Mobile terminal path planning method and system based on distance field

Abstract

The invention discloses a mobile terminal path planning method and system based on a distance field, and the method comprises the steps: firstly building an ESDF map, and calculating a gradient value of each grid point in the ESDF map; establishing a state vector and a discretization state transition equation of the mobile terminal based on the vehicle model; and finally, calculating the next state of the mobile terminal moving from the current node by using an improved # imgabs0 # algorithm until an optimal path from the starting node to the terminal node is formed. Wherein the ESDF cost is calculated by using the ESDF value and the gradient value of the node, and the cost function of the improved # imgabs1 # algorithm is the sum of the actual cost, the heuristic estimation cost and the ESDF cost. According to the distance field and gradient information of the ESDF map, collision detection and expansion cost calculation in the # imgabs2 algorithm are combined, and the path planning efficiency is improved while the path safety is guaranteed.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a method and system for mobile terminal path planning based on a distance field. Background Art

[0002] The application scenarios of outdoor mobile terminals have been gradually expanding in recent years, such as park logistics, ports, outdoor unmanned sweepers, inspections, scenic sightseeing, etc. Its technologies cover multi-sensor fusion, environmental perception, map construction, global positioning, path planning, intelligent decision-making, navigation and obstacle avoidance, cluster collaboration, etc., which makes outdoor mobile terminals show higher intelligence and lower maintenance costs.

[0003] The path planning algorithm is a core component of the mobile terminal navigation module. The algorithm plans a safe and shortest global path from the starting point and the end point that meets the dynamic constraints of the mobile terminal based on the known map information. It is mainly used in the free navigation and replanning mode in the mobile terminal field, so that the mobile terminal can navigate to the specified location according to the set target. The global route is a local obstacle avoidance reference route, so its quality will indirectly lead to the obstacle avoidance effect, so the path planning problem is a core issue in the field of mobile terminal navigation.

[0004] Path planning algorithms are divided into two categories: path sampling and path search. Path search is widely used in engineering implementation due to its completeness. algorithm( ) is a path planning algorithm optimized for vehicle dynamics. It improves on the traditional A algorithm to solve efficiency problems and motion constraints in high-dimensional continuous space. The algorithm has been improved in some aspects, but it also has some disadvantages and limitations. When processing large maps, it needs to process a large number of nodes and constraints, especially collision constraints, which leads to low computational efficiency and The path quality given by the algorithm is low and is generally closer to obstacles, causing the mobile terminal to frequently trigger obstacle avoidance during the tracking process. Summary of the invention

[0005] Purpose of the invention: The purpose of the present invention is to provide a distance field-based mobile terminal path planning method and system that accelerates path planning efficiency and reduces computational burden.

[0006] Technical solution: The mobile terminal path planning method based on distance field described in the present invention comprises the following steps:

[0007] Establish ESDF map and calculate the gradient value of each grid point in ESDF map;

[0008] Establish the state vector and discretized state transfer equation of the mobile terminal based on the vehicle model;

[0009] Take advantage of improved The algorithm calculates the next state of the mobile terminal moving from the current node until the optimal path from the starting node to the end node is formed; the ESDF cost is calculated using the ESDF value and gradient value of the node, and the improved The cost function of the algorithm is the sum of the actual cost, the heuristic estimated cost and the ESDF cost.

[0010] Furthermore, the ESDF cost is ,in , is the weight factor, For Node The ESDF value, For Node The gradient value of and To adjust the parameters.

[0011] Furthermore, the ESDF map is established by multi-threaded concurrent construction of the ESDF map. When the calculation process is decoupled, the distance field of the information point inside the obstacle is not calculated.

[0012] Furthermore, the establishing of the state vector and the discretized state transfer equation of the mobile terminal based on the vehicle model includes:

[0013] State Vector , the motion primitive is , for the time step , the discretized state transfer equation is:

[0014] ;

[0015] ;

[0016] ;

[0017] ;

[0018] ;

[0019] in: is the node coordinate of the mobile terminal in the global coordinate system, is the heading angle of the mobile terminal, is the mobile terminal speed, is the steering angle of the mobile terminal, is the mobile terminal acceleration, is the angular velocity of the mobile terminal.

[0020] Furthermore, the mobile terminal satisfies motion constraints when moving to the next state, and the motion constraints include speed constraints, acceleration constraints, steering angle constraints, steering angle velocity constraints and minimum turning radius constraints.

[0021] Furthermore, the collision denial constraint is satisfied when the mobile terminal moves to the next state; whether the mobile terminal collides is detected by coarse detection and fine detection; the coarse detection is to determine whether the ESDF value of the node is greater than the safety threshold, if it is greater than the safety threshold, the node is safe, otherwise, fine detection is performed according to the outline of the mobile terminal to determine whether the node is safe or collided.

[0022] The mobile terminal path planning system based on distance field of the present invention comprises:

[0023] An ESDF map building unit is used to build an ESDF map and calculate the gradient value of each grid point in the ESDF map;

[0024] A state transfer modeling unit, used to establish a state vector and a discretized state transfer equation of the mobile terminal based on a vehicle model;

[0025] Path planning unit for utilizing improved The algorithm calculates the next state of the mobile terminal moving from the current node until the optimal path from the starting node to the end node is formed; the ESDF cost is calculated using the ESDF value and gradient value of the node, and the improved The cost function of the algorithm is the sum of the actual cost, the heuristic estimated cost and the ESDF cost.

[0026] Furthermore, the ESDF cost is ,in , is the weight factor, For Node The ESDF value, For Node The gradient value of and To adjust the parameters.

[0027] The electronic device described in the present invention includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the computer program is loaded into the processor, the mobile terminal path planning method based on the distance field is implemented.

[0028] The computer-readable storage medium of the present invention stores a computer program, and when the computer program is executed by a processor, the method for mobile terminal path planning based on distance field is implemented.

[0029] Beneficial effect: Compared with the prior art, the present invention proposes to combine the distance field and gradient information of the ESDF map with The collision detection and extended cost calculation in the algorithm accelerate the efficiency of path planning while ensuring path safety. In order to prevent the high computational burden caused by online generation of ESDF, this solution will pre-generate ESDF maps to reduce planning expenses, so that the algorithm can run stably and efficiently on mobile terminals. Its advantages include the following:

[0030] (1) Concurrently build ESDF maps to reduce subsequent resource overhead

[0031] The ESDF map is built in parallel using multiple threads, which reduces the ESDF map construction time and better meets the needs of engineering implementation. The pre-built map separates construction and use, thereby reducing the subsequent online computing burden and is more suitable for running on platforms with limited computing resources such as mobile terminals.

[0032] (2) Improved collision detection efficiency leads to improved path search speed

[0033] Through the ESDF map, fast collision detection is achieved. The detection scheme from coarse to fine makes a huge improvement in collision detection in open environments. Under the premise of ensuring accuracy, it can achieve lower computational overhead and higher detection efficiency than the traditional pixel-by-pixel detection scheme. Experiments show that compared with traditional The algorithm reduces search time by 40% on average.

[0034] (3) Path quality optimization

[0035] The introduction of ESDF gradient information makes the path naturally tend to stay away from obstacles. The generated path has a better safety margin, is smoother, and is more suitable for actual robot execution. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 This is a schematic diagram of accepting an original grid map in an embodiment of the present invention.

[0037] Figure 2 It is a schematic diagram of ESDF map in an embodiment of the present invention.

[0038] Figure 3 FIG. 4 is a flow chart of a collision detection method in an embodiment of the present invention. DETAILED DESCRIPTION

[0039] The technical solution of the present invention is further described below in conjunction with the accompanying drawings.

[0040] The mobile terminal of this embodiment is a mobile robot, and the computing execution subject is a robot computing unit, which specifically includes the following steps.

[0041] Step 1: Map preprocessing.

[0042] Step 1.1, receiving original raster map data.

[0043] Grid map is a commonly used map representation method in path planning. It discretizes the environment into a series of grid areas. Each grid area contains specific attribute information, the most common of which is occupancy status. This step will accept occupancy grid map data (nav_msgs / OccupancyGrid) based on the ROS general message type. The message data contains map attributes (map length and width (width, height), resolution (resolution), map origin coordinates (origin)) and map data. This information is provided by the map service node, and only reception is done here, such as Figure 1 shown.

[0044] Step 1.2: Concurrently build the ESDF map using multiple threads.

[0045] The ESDF (Euclidean Signed Distance Field) map is a Euclidean signed distance field map, which assigns a distance value to each spatial point. This value represents the distance from the point to the surface of the nearest object. If a point is inside an object, the assigned distance value is a negative number; if the point is outside the object, it is a positive number. It can easily query the distance and gradient information between the current position point and the obstacle, thereby improving the computational efficiency of the trajectory planning algorithm. This embodiment uses a global construction method to convert the raster map into an ESDF map. Therefore, it is necessary to accelerate the offline construction to improve the construction efficiency, use multi-threaded concurrency to accelerate the construction process, and optimize related calculations, such as decoupling the calculation process, not calculating the distance field of the information point inside the obstacle, etc. The generated graph is as follows: Figure 2 The specific implementation steps are as follows:

[0046] (1) Using the two-dimensional separability principle, the 2D distance transform is decomposed into two 1D transforms;

[0047] (2) Scan in the column direction and calculate the vertical distance from the midpoint of each column to the nearest obstacle;

[0048] (3) Scan in the row direction, using the characteristics of the lower parabola intersection to construct the minimum envelope. By maintaining a monotonic queue to record the valid parabola segments, the final distance of each point is determined by the lowest parabola segment it is in;

[0049] (4) Combine the two scan results to calculate the final Euclidean distance.

[0050] Step 1.3, calculate the gradient field.

[0051] The gradient field is calculated based on the ESDF map, and the gradient calculation formula is: ;

[0052] in: express The gradient of the direction, express Direction gradient.

[0053] Step 1.4, save the map data.

[0054] Save the map as offline data so that it can be directly called next time for use, avoiding waste of resources caused by frequent generation.

[0055] Step 2: Search space construction.

[0056] Step 2.1, construct the state space based on the vehicle dynamics model.

[0057] The vehicle dynamics model is simplified to the vehicle model here, and the state vector is expressed as ,in: is the position of the robot's rear axle center in the global coordinate system, is the robot heading angle (with The angle in the positive direction of the axis), is the robot speed, is the steering angle of the robot’s front wheels.

[0058] This embodiment is based on a discretized state space, therefore, The direction discrete step length is set to , The direction discrete step length is set to , the heading angle step length is (usually , is the heading angle resolution), the velocity discrete set: , a discrete set of steering angles: , the robot sets specific physical constraints according to the actual chassis parameters:

[0059] Speed ​​Constraints: ;

[0060] Acceleration constraints: ;

[0061] Steering angle constraint: ;

[0062] Steering angular velocity constraint: ;

[0063] Minimum turning radius constraint .

[0064] Step 2.2, define the state transfer equation based on the control quantity.

[0065] According to the previous motion state, the robot dynamics model and control primitives can be combined to describe the evolution from the current state to the next state, where the motion primitive is , is the robot acceleration, is the robot angular velocity; for the time step , the discretized state transfer equation is:

[0066] ;

[0067] ;

[0068] ;

[0069] ;

[0070] ;

[0071] During the state transfer process, motion constraints and environmental constraints need to be set, including speed constraints, acceleration constraints, minimum turning radius constraints, and collision rejection constraints, that is, collision detection.

[0072] Step 3, Improvement The algorithm performs path search.

[0073] Step 3.1, initialize the start and end states.

[0074] The node occupied by the current position of the mobile terminal is set as the starting node, the target point is set as the end node, and the cost value is calculated.

[0075] Based on the original cost function and combined with the ESDF gradient information, the cost function of the present invention is as follows:

[0076] ;

[0077] in:

[0078] is the actual cost from the starting point to the current node; is the heuristic estimate from the current node to the target point; is the weight factor; is the cost term based on ESDF, and its calculation formula is:

[0079] ;

[0080] in: and To adjust the parameters, is the ESDF value at the current position, is the gradient value at the current position.

[0081] Step 3.2, build a priority queue.

[0082] Construct open_list and closed_list priority queues, where open_list is used to store nodes to be expanded and closed_list is used to record visited nodes. Add the starting node to open_list and set is 0, and the starting node is calculated The node information is stored.

[0083] Step 3.3, fast collision detection based on ESDF map.

[0084] For the collision detection of access nodes, a combination of coarse detection and fine detection is used to achieve accelerated processing. The coarse detection is based on the comparison of the ESDF value of the access node with the safety threshold. If it is greater than the safety threshold, the node is considered safe. If it is less than the safety threshold, further fine detection is performed based on the node information (posture information) combined with the robot contour information. The specific process is as follows: Figure 3 In this embodiment, the fine detection method is to perform AABB algorithm detection in combination with the robot contour, wherein AABB stands for Axis-Aligned Bounding Box.

[0085] Step 3.4, main loop search.

[0086] While the priority queue is not empty, the loop executes the following steps:

[0087] (1) Node selection: Take the node with the smallest f_cost from the open_list as the current node and check whether the termination condition (reaching the vicinity of the target point) is met. If the target is reached, exit the loop and start path reconstruction.

[0088] (2) State expansion: Generate a set of possible motion primitives based on the current node state, apply the vehicle kinematic model to each motion primitive to calculate the next state, check the constraints, and perform collision detection using the method in step 3.3.

[0089] (3) Node cost calculation: Calculate the cost value of each visited node combined with the ESDF map gradient information and store it as node attribute information.

[0090] (4) Node processing: Add the current node to closed_set. For the next valid node, check whether it is in closed_set. If it is not in closed_set, update the node information and set the parent node information.

[0091] (5) Path reconstruction: Starting from the target point, trace back the path through the parent node, record the completed node information and complete the path preservation.

[0092] The mobile terminal path planning system based on distance field of the present invention comprises:

[0093] An ESDF map building unit is used to build an ESDF map and calculate the gradient value of each grid point in the ESDF map;

[0094] A state transfer modeling unit, used to establish a state vector and a discretized state transfer equation of the mobile terminal based on a vehicle model;

[0095] Path planning unit for utilizing improved The algorithm calculates the next state of the mobile terminal moving from the current node until the optimal path from the starting node to the end node is formed; the ESDF cost is calculated using the ESDF value and gradient value of the node, and the improved The cost function of the algorithm is the sum of the actual cost, the heuristic estimated cost and the ESDF cost.

[0096] Furthermore, the ESDF cost is ,in , is the weight factor, For Node The ESDF value, For Node The gradient value of and To adjust the parameters.

[0097] The electronic device described in the present invention includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the computer program is loaded into the processor, the mobile terminal path planning method based on the distance field is implemented.

[0098] The computer-readable storage medium of the present invention stores a computer program, and when the computer program is executed by a processor, the method for mobile terminal path planning based on distance field is implemented.

[0099] The computer-readable storage media may include RAM, ROM, EEPROM, CD-ROM or other optical disk storage, magnetic disk storage or other magnetic storage devices, flash memory, or any other medium that can be used to store the desired program code in the form of instructions or data structures and that can be accessed by a computer.

[0100] The processor is used to execute the computer program stored in the memory to implement each step of the method involved in the above embodiment.

Claims

1. A mobile terminal path planning method based on distance field, characterized in that: The steps include: Establish ESDF map and calculate the gradient value of each grid point in ESDF map; Establish the state vector and discretized state transfer equation of the mobile terminal based on the vehicle model; Take advantage of improved The algorithm calculates the next state of the mobile terminal moving from the current node until the optimal path from the starting node to the end node is formed; the ESDF cost is calculated using the ESDF value and gradient value of the node, and the improved The cost function of the algorithm is the sum of the actual cost, the heuristic estimated cost and the ESDF cost.

2. The method for mobile terminal path planning based on distance field according to claim 1, characterized in that: The ESDF cost is ,in , is the weight factor, For Node The ESDF value, For Node The gradient value of and To adjust the parameters.

3. The method for mobile terminal path planning based on distance field according to claim 1, characterized in that: The ESDF map is established by multi-threaded concurrent construction of the ESDF map. When the calculation process is decoupled, the distance field of the information point inside the obstacle is not calculated.

4. The method for mobile terminal path planning based on distance field according to claim 1, characterized in that: The establishing of the state vector and the discretized state transfer equation of the mobile terminal based on the vehicle model comprises: State Vector , the motion primitive is , for the time step , the discretized state transfer equation is: ； ； ； ； ； in: is the node coordinate of the mobile terminal in the global coordinate system, is the heading angle of the mobile terminal, is the mobile terminal speed, is the steering angle of the mobile terminal, is the mobile terminal acceleration, is the angular velocity of the mobile terminal.

5. The method for mobile terminal path planning based on distance field according to claim 1, characterized in that: The mobile terminal satisfies motion constraints when moving to the next state, and the motion constraints include speed constraints, acceleration constraints, steering angle constraints, steering angular velocity constraints and minimum turning radius constraints.

6. The method for mobile terminal path planning based on distance field according to claim 1, characterized in that: The mobile terminal satisfies the collision rejection constraint when it moves to the next state; whether the mobile terminal collides is detected by coarse detection and fine detection; the coarse detection is to determine whether the ESDF value of the node is greater than the safety threshold. If it is greater than the safety threshold, the node is safe, otherwise, fine detection is performed according to the outline of the mobile terminal to determine whether the node is safe or collided.

7. A mobile terminal path planning system based on distance field, characterized in that: include: An ESDF map building unit is used to build an ESDF map and calculate the gradient value of each grid point in the ESDF map; A state transfer modeling unit, used to establish a state vector and a discretized state transfer equation of the mobile terminal based on a vehicle model; Path planning unit for utilizing improved The algorithm calculates the next state of the mobile terminal moving from the current node until the optimal path from the starting node to the end node is formed; the ESDF cost is calculated using the ESDF value and gradient value of the node, and the improved The cost function of the algorithm is the sum of the actual cost, the heuristic estimated cost and the ESDF cost.

8. The mobile terminal path planning system based on distance field according to claim 7, characterized in that: The ESDF cost is ,in , is the weight factor, For Node The ESDF value, For Node The gradient value of and To adjust the parameters.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the computer program is loaded into a processor, the method for mobile terminal path planning based on distance field according to any one of claims 1 to 6 is implemented.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the method for mobile terminal path planning based on distance field according to any one of claims 1 to 6 is implemented.

Images