Robot motion planning method based on improved jump point search

Abstract

The invention discloses an improved robot motion planning method based on Jump Point Search (JPS), and relates to a robot motion planning method based on JPS. According to the method, the search direction and the path planning process of the JPS algorithm are optimized, the path direction guidance of BFS (breadth-First Search) is combined, and the search direction priorities are sorted, so that the overall search efficiency and the path security are remarkably improved. Specifically, according to the method, a cosine value is used for measuring the similarity degree of each search direction and a BFS path, and the directions are divided into high priorities and secondary priorities. In the hop point searching process, the heuristic function value only evaluates the extended hop points in the high priority direction, and selects the hop point corresponding to the minimum value, so that the extension in the redundant direction is reduced, and the searching efficiency is improved. In addition, the invention also designs a set of path optimization strategy which mainly comprises two parts, namely a'security node updating strategy 'and a'redundant node eliminating strategy'. Aiming at the dangerous boundary path points, a security node updating strategy is utilized, and adjacent nodes are replaced by common security neighbor points, so that paths with potential safety hazards are prevented from being formed; according to the redundant node elimination strategy, the number of control nodes is reduced and the overall path length is shortened by detecting the direct connectivity between continuous nodes.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and particularly to a robot motion planning method based on an improved jump point search strategy. Background Art

[0002] With the wide application of intelligent robots in fields such as industry, logistics, medical treatment, agriculture, and services, the problem of robot path planning has become a key technical challenge.

[0003] In practical applications, path planning algorithms need to meet the requirements of both high efficiency and safety. Especially in complex or dynamic environments, traditional search algorithms often have problems such as low search efficiency, low path safety, and many redundant nodes. As an efficient grid path planning algorithm, the Jump Point Search (JPS) algorithm can reduce search nodes based on the A* algorithm, but it still has limitations in terms of insufficient path safety and room for improvement in search efficiency when dealing with complex scenarios.

[0004] Therefore, how to improve the JPS algorithm to enhance path planning efficiency, reduce redundant nodes, and optimize path safety has become a hot and difficult issue in current research. In response to the above problems, the present invention proposes a robot motion planning method based on improved JPS, providing a new solution for the efficient path planning of intelligent robots in complex scenarios. Summary of the Invention

[0005] The present invention proposes a robot motion planning method based on JPS. By optimizing the search direction and combining a path optimization strategy, it introduces the Breadth-First Search (BFS) to improve the search direction of the jump point search, and combines a safe node update strategy and a redundant node elimination strategy, significantly enhancing the efficiency and safety of path planning. This method is applicable to the global path planning of robots in complex or dynamic scenarios, and provides an efficient, safe, and reliable solution for intelligent robot motion. The main technical solutions of the present invention include the following aspects:

[0006] 1. A path search strategy based on direction priority is proposed, integrating the direction guidance of BFS into the JPS algorithm. When expanding jump points, it preferentially expands jump points in the direction of the BFS path to evaluate the optimal jump point. Specifically: by dividing the dot product value by the product of the lengths of two vectors, the cosine value of the included angle can be obtained. This cosine value represents the similarity of the two directions, and the algorithm will preferentially consider the direction with a smaller angle, that is, the moving direction consistent with or close to the BFS path direction.

[0007]

[0008] By performing path searches on high-priority directions, unnecessary search directions can be avoided and redundant jumps can be reduced. Especially in large-scale grids or complex terrains, the number of search nodes can be significantly reduced, thereby improving path search efficiency.

[0009] The JPS algorithm selects the next expansion node based on the value of the heuristic function F(n). The expression of the heuristic function is

[0010] F(n)=G(n)+H(n)

[0011] Only jump points in high-priority directions are evaluated, and the optimal jump point is selected and added to the CloseList list.

[0012] 2. A path optimization strategy is proposed, which includes the following two parts:

[0013] (1) Safety node update strategy: During the path planning process, the safety of path nodes is evaluated based on environmental information and dynamic obstacle detection results, and the path is updated to avoid dangerous areas to ensure the safety of the planned path.

[0014] For each pair of adjacent points in the path (p i , p i+1 ) Check if there is a common safe point p c ,satisfy:

[0015]

[0016] In order for the path to avoid hitting obstacles, the following conditions need to be met: Where: p i The i-th hop in the path. p i+1 The i+1th hop in the path. c Indicates the common safety points checked in the path. This safe node update strategy replaces adjacent points with common safe neighbor points, which can significantly reduce the collision risk of the path near corners or obstacles, thereby improving the robustness and safety of path planning.

[0017] (2) Redundant node elimination strategy: After the path is generated, the algorithm is used to identify and eliminate redundant nodes in the path to improve planning efficiency. To eliminate redundant nodes, during the path generation process, check whether each point in equation (9) meets the following conditions: By generating points on the straight line, check whether the following points collide with obstacles:

[0018]

[0019] Where: (x i-1 ,yi-1 ) is the current check p i-1 coordinates

[0020] Step 28: Remove redundant nodes and update the path point sequence.

[0021] 3. Global path planning method based on improved JPS: A global path planning method suitable for complex scenarios is proposed. Combining a path search strategy with direction priority and a path optimization strategy, it can generate a safe path with global optimization performance. This method shows strong adaptability in a dynamic environment, can adjust the path in real time according to environmental changes, and effectively solves the global path planning problem in complex dynamic scenarios.

[0022] The beneficial effects of the present invention are as follows: By combining the path direction guidance of BFS, the direction priorities are sorted (the magnitude of the cosine value represents the direction similarity with the BFS path), divided into high priority and sub-priority. When performing jump point search, the heuristic function values in the high-priority directions are evaluated to select the jump point with the minimum value, reducing jump point expansion and improving search efficiency. A path optimization strategy is designed, including safe node update and redundant node elimination. The dangerous boundary path points are identified and updated. The safe node update strategy replaces adjacent points with common safe neighbor points to avoid dangerous paths. The redundant node elimination strategy checks whether consecutive points on the path can be directly connected, thereby reducing the number of control path points and the total path length. This global path planning method applicable to complex scenarios effectively enhances the motion planning ability of intelligent robots in complex scenarios. Description of the Drawings

[0023] Figure 1 Flow schematic diagram of the robot motion planning method based on the improved jump point search method provided by the embodiment of the present invention;

[0024] Figure 2 It is an improved implementation process diagram based on the jump point search algorithm provided by another embodiment of the present invention;

[0025] Figure 3 It is a path effect comparison diagram generated by the improved JPS algorithm provided by another embodiment of the present invention. Detailed Embodiment

[0026] In practical applications, path planning algorithms need to meet the requirements of both high efficiency and safety. Especially in complex or dynamic environments, traditional path planning algorithms often suffer from problems such as low search efficiency, low path safety, and many redundant nodes. As an efficient grid path planning algorithm, the JPS algorithm can reduce the search nodes based on the A* algorithm, but it still has limitations in terms of insufficient path safety and room for improvement in search efficiency when dealing with complex scenarios. How to improve the JPS algorithm to enhance the efficiency of path planning, reduce redundant nodes, and improve path safety has become a hot and difficult issue in current research. In view of the above problems, the present invention proposes a robot motion planning method based on improved JPS, providing a new solution for the efficient path planning of intelligent robots in complex scenarios.

[0027] A robot motion planning method based on improved jump point search, comprising the following steps:

[0028] Step 1: Collect the working environment data of the robot and establish a working environment map for the mobile robot; use lidar, camera sensors, etc. to collect real-time data such as obstacles and wall boundaries around the robot.

[0029] Step 2: Discretize the grid map Discretize the continuous environment data into a two-dimensional grid map. Set the grid resolution Δx and Δy, and calibrate the obstacle information for each grid cell. Using the grid method, the vehicle driving environment model can be described as:

[0030]

[0031] where (x i , y i ) represents the position coordinates of the i-th grid, x i is the coordinate of this grid in the horizontal direction, and y i is the coordinate of this grid in the vertical direction. mod( ) represents the remainder operation, ceil( ) represents the ceiling operation, and these operations are usually used to map the actual coordinates to the grid index in the grid map. N x is the number of rows of the grid map, N y is the number of columns of the grid map, and a is the side length of a grid, representing the physical space length occupied by each grid in the actual map.

[0032] Step 3: Divide the working environment map of the mobile robot into grids, determine the starting point and the ending point, and set the positions of the starting point Pstart(xs, ys) and the ending point Ptarget(xt, yt) in the discretized map. Perform data preprocessing and filtering, and use Kalman filtering to perform noise reduction and normalization processing on the original data to ensure the stability and reliability of the data.

[0033] Step 4: Use the starting point as the parent node, expand the child nodes of the parent node and add them to the list of alternative path nodes;

[0034] Step 5: Calculate the cost of the child nodes in the direction of the list of alternative path nodes using the improved direction priority in the JPS algorithm, and select the child node with the minimum cost according to the cost calculation and move it into the path node list of the JPS algorithm.

[0035] Step 6: Obstacle detection and marking. Judge according to the echo intensity or the point cloud density threshold, and mark the cells occupied by obstacles in the grid.

[0036] Step 7: Calculate and preferentially generate a reference feasible path through the BFS algorithm for guiding subsequent jump point expansion. Specifically as follows:

[0037] Step 8: Initialization:

[0038] Step 8.1: Queue: Let Q be the queue, and initially put the starting point into it

[0039] Q ← [s]

[0040] Step 8.1: Visited set: Let V visited be the set of visited nodes, and initialize it to

[0041] V visited ← [s]

[0042] Step 8.1: Distance array: Let dist(v) represent the shortest distance from S to node V, and initialize it to

[0043] dist(s) = 0, dist(v) = ∞ (for other nodes)

[0044] Step 9: When the queue Q is not empty, repeat the following steps: Take out the element u at the head of the queue, u = Q.pop - front(). Check the target: If u = t, the search ends, and return dist(t) and the path (if the predecessor nodes are recorded). For each neighbor v connected to u, if then mark it as visited: V visited ← V visted ∪ {v}

[0045] Step 10: Update the distance: dist(v) = dist(u) + 1, and enqueue v: Q.push_back(v)

[0046] Step 11: Assume that the BFS optimized path is a set of ordered points {P1, P2,..., P n}, define the path segment from P i to P i+1 as the vector During the JPS search process, the expansion direction of the current node is dynamically adjusted based on the optimized BFS path. This direction adjustment depends primarily on the direction of the current node and the nearest straight line segment on the optimized path. Specifically, the algorithm draws a direction vector from the current node to the nearest straight line segment on the BFS path that does not cross obstacles, which serves as the target direction.

[0047]

[0048] The scale factor t of the projection point on the vector vi:

[0049]

[0050] Where: (x n ,y n ) The coordinates of the current node, calculate the projection point P according to the value of t proj If t≤0, the projection point is at P i Outside, then P proj =P i =(x i ,y i ). If 0<t<1: the projection point is on line segment P i and P i+1 Between, then Pproj=P i +t·v i =(x i +t·(x i+1 -x i ), y i +t·(y i+1 -y i )). t≥1: The projection point is at P i+1 Outside, then P proj =P i =(x i ,y i ).

[0051] Step 12: During the JPS search, the algorithm dynamically adjusts the expansion direction of the current node based on the optimized BFS path. This direction adjustment depends primarily on the direction of the current node and the nearest straight line segment on the optimized path. Specifically, the algorithm draws a direction vector from the current node to the nearest straight line segment on the BFS path that does not cross obstacles, which serves as the target direction.

[0052]

[0053] Step 13: Record the minimum distance and the corresponding line segment. After traversing all line segments, select the line segment that makes d i The smallest line segment P closest and P closest+1 .

[0054] : Step 14: During the jump point search process, the algorithm calculates the direction from the current node to the nearest straight line segment in the BFS optimized path that does not cross obstacles, thereby obtaining the target direction vector. Three search directions with the highest priority are found, allowing the algorithm to explore different path branches to find the farthest valid jump point; the remaining search directions are of sub-optimal priority, ensuring the completeness of the search algorithm. In this way, the algorithm preferentially expands along the BFS path direction during each jump point search, significantly reducing unnecessary searches and improving the efficiency and intuitiveness of path planning.

[0055] v target = P closest+1 - P closest = (x closest+1 - x closest , y closest+1 - y closest )

[0056] Step 13:: By dividing the dot product value by the product of the lengths of the two vectors, the cosine value of the included angle can be obtained. This cosine value represents the similarity between the two directions: the algorithm will preferentially consider the direction with a smaller angle, that is, the moving direction that is consistent or close to the BFS path direction. Using BFS for direction guidance, the breadth-first search (BFS) is used on the grid map to calculate the distance from each cell to the target.

[0057]

[0058] In the formula: {v move，1 , v move，2 , v move，3} represents the 3 optimal moving directions selected. arccos represents calculating the included angle between the directions. v target · v move represents calculating the inner product between the target direction and the moving direction. |v target | · |v move | represents the modulus (length) of the vector.

[0059] : Step 15: Initialize the open list and the cost function, establish an open list to store the jump points to be expanded, start from the starting point, and expand the jump points according to the rules in Step 8. Terminate the expansion in the current direction when encountering an obstacle or the map boundary.

[0060] Step 16: The JPS algorithm selects the next node to expand according to the value of the heuristic function F(n), and the expression of the heuristic function is:

[0061] F(n) = G(n) + H(n)

[0062] Where: F(n) consists of two parts. G(n) refers to the actual distance from the starting position to the current hop point Nc through a series of intermediate hop points, as shown in Equation (5); H(n) refers to the estimated distance from the hop point N to the target point Ng(xg, yg). The closer H(n) is to the true distance between two points in space, the higher the efficiency of the heuristic function. Therefore, the diagonal distance is selected, as shown in Equation (6).

[0063]

[0064]

[0065] Step 17: Prioritize expanding the hop points that match the target direction. Sort the candidate hop points in the open list according to the matching degree with the target direction (inner product calculation), and prioritize expanding the ones with higher matching degrees:

[0066]

[0067] Step 18: Update the cost and parent node records. Update the cumulative cost after each expansion of a hop point node n':

[0068] g(n') = g(n) + Δd

[0069] Step 19: And record its parent node for subsequent path backtracking.

[0070] Step 20: Judge the search termination condition. When the distance between a certain expanded hop point and the target is less than the preset threshold ∈, it is considered that the reachable target has been found and the search is terminated.

[0071] Step 21: Backtrack to generate a preliminary path. Backtrack along the parent node chain from the target node to obtain a preliminary path sequence P = {P1, P2,..., P m}

[0072] Step 22: Verify the connectivity of the preliminary path. Use line detection (Bresenham algorithm) to verify whether there are obstacles blocking between adjacent hop points on the preliminary path.

[0073] Step 23: A tool for further optimizing and simplifying the path during path planning. By reducing the turns and the number of points on the path, the path becomes more direct and efficient, while maintaining the safety and feasibility of the path. Its main function is to continue to simplify the path by checking whether consecutive points on the path can be directly connected.

[0074] Step 24: Implement a path planning safety node update strategy. Remove redundant nodes from the path data, greatly reducing the control points and the total length of the path, ensuring the safety of the planned path. The path optimization strategy is as follows:

[0075] Step 25: Update the safety nodes, check adjacent node pairs in the path, determine whether they are dangerous path points, and try to find their common neighbors. Only when these common neighbors are far from obstacles will the original two nodes be replaced by a safer neighbor point.

[0076] Step 26: For each pair of adjacent points (p i , p i+1 ) in the path, check whether there exists a common safety point p c , satisfying:

[0077]

[0078] To avoid the path hitting obstacles, the following conditions need to be satisfied: Where: p i The i-th hop point in the path. p i+1 The (i + 1)-th hop point in the path. p c Represents the common safety point detected in the path. Let be the free space set, that is, the area where the path can move. This safety node update strategy can significantly reduce the collision risk of the path near corners or obstacles by replacing adjacent points with common safe neighbor points, thereby improving the robustness and safety of path planning.

[0079] Step 27: Eliminate redundant nodes. During the path generation process, check each point in the path to satisfy the following conditions: By generating points on a straight line, check whether the following points collide with obstacles:

[0080]

[0081] Where: (x i-1 , y i-1 ) is the coordinate of the currently checked p i-1

[0082] Step 28: Remove redundant nodes and update the path point sequence.

[0083] Step 29: Smooth the path using a third-order B-spline curve. Smooth the remaining discrete path using a third-order B-spline curve. Given control points {P i , P i+1 , P i+2 , P i+3}, the third-order B-spline curve formula is:

[0084] B(t) = (1 - t) 3 P i + 3(1 - t) 2 tP i+1 + 3(1 - t)t2 P i+2 +t 3 P i+3 , t∈[0,1]

[0085] Step 30: Generate the final safe path sequence. This curve can effectively reduce sharp turns in the path and improve motion continuity and smoothness.

[0086] Step 31: Calculate the total length of the preliminary path. The total length of the path is calculated using the following formula:

[0087]

[0088] Step 32: Evaluate the path security and calculate P for each node on the path i Distance to the nearest obstacle:

[0089]

[0090] Step 33: Re-verify the updated path, re-verify the connectivity and safety of the path updated through the safety node to ensure that the path meets the motion requirements.

[0091] Step 34: Calculate the distance and angle between consecutive nodes. i , P i+1 , P i+2 ), calculate the Euclidean distance and angle: Δdi=(xi+1-xi) 2 +(yi+1-yi) 2

[0092] Corner changes:

[0093] Step 35: Convert the final smoothed and optimized path generated in step 28 into motion control instructions (such as linear velocity, angular velocity, steering angle, etc.). The instruction format must conform to the message format under ROS (for example, geometry_msgs / Twist).

[0094] Step 36: ROS node development. Developing a path planning node in the ROS environment mainly includes:

[0095] (1) Data acquisition module: subscribes to sensor data such as lidar and cameras;

[0096] (2) Path planning module: implements the above-mentioned improved JPS, third-order B-spline smoothing and path optimization algorithms;

[0097] (3) Control instruction publishing module: publishes the generated motion instructions to the robot chassis control topic.

[0098] Step 37: Simulation and offline testing. Test the developed ROS nodes on simulation platforms such as Gazebo or RViz. Verify the continuity, safety, and execution effect of the path, and debug the parameters if necessary to ensure meeting the expected requirements.

[0099] Step 38: Deploy the algorithm to the robotic vehicle. Deploy the well-tested ROS nodes to the actual robotic vehicle to build a complete closed-loop system (from environmental perception to path planning and then to motion control). Start each node through the ROS launch file and use the ROS monitoring tool to track the state and path execution of the robot in real time.

[0100] Step 39: Simulation and offline testing. Conduct simulation tests on the algorithm in the Gazebo or RViz environment, verify the continuity, safety, and execution effect of the path planning, and debug the parameters until meeting the expected requirements.

[0101] Step 40: Deploy the algorithm to the robotic vehicle. Deploy the tested and verified ROS nodes to the actual robotic vehicle to build a complete closed-loop, from environmental perception, path planning to motion control. Use the ROS launch file to start the relevant nodes and use the ROS monitoring tool to monitor the state of the robot and the path execution in real time.

Claims

1. The robot path planning method based on improved jump point search (JPS) according to claim 1, wherein The method includes the following steps: Step 1: By introducing the Breadth-First Search (BFS) algorithm into the JPS path search process, preferentially expand the path nodes that are consistent with the target path direction to avoid unnecessary jump points during the search process. Assume that the BFS-optimized path is a set of ordered points {P1, P2,..., P n} Step 2: During the JPS search process, dynamically adjust the expansion direction of the current node according to the optimized BFS path. This direction adjustment mainly depends on the direction of the nearest straight line segment on the optimized path to the current node. Specifically, the algorithm draws a direction vector from the current node to the nearest straight line segment on the BFS path that does not pass through obstacles as the target direction. Step 3: Record the minimum distance and the corresponding line segment. After traversing all line segments, select the line segment P i that minimizes d closest and P closest+1 Step 4: By dividing the dot product value by the product of the lengths of the two vectors, the cosine value of the included angle can be obtained. This cosine value represents the similarity between the two directions: the algorithm will give priority to the direction with a smaller angle, that is, the moving direction that is the same as or close to the BFS path direction. Using BFS for direction guidance, the breadth-first search (BFS) is used to calculate the distance from each cell to the target on the grid map. Where: {v move，1 , v move，2 , v move，3} represents three optimal moving directions selected. arccos represents calculating the angle between directions. v target ·v move represents calculating the inner product between the target direction and the moving direction. |v target |·|v move | represents the modulus (length) of the vector. Step 5: Add a path planning safety node update strategy to ensure the safety of the planned path. Add a redundant node elimination strategy to remove redundant nodes from the path data, greatly reducing the control points and the total path length. There are sharp inflection points at the jump points, and the obtained path is a continuous line segment with serious curvature mutation. Add a cubic B-spline curve path smoothing strategy to increase the stability of the intelligent robot during actual operation, which is in line with the actual path planning operation of the intelligent robot. Through the above strategies, redundant path points can be effectively reduced, and the efficiency of path search can be improved. Especially in large-scale grids or complex terrains, unnecessary calculations and the number of path node expansions can be significantly reduced, thereby improving the overall efficiency of path search. During the path search process, first analyze the obstacles and target positions in the current environment through the BFS algorithm, and combine the guidance information of the path direction of BFS to preferentially expand the direction that is the same as or close to the target path.

2. The improved hop point search robot path planning method according to claim 1, wherein The method further includes a path optimization strategy, which is characterized in that: after the path search is completed, the generated path is further optimized. The path optimization strategy includes the following three aspects: a safety node update strategy, a redundant node elimination strategy, and a path smoothing strategy. Safety node update strategy: During the path generation process, identify dangerous paths. By judging whether there are two points in the four directions around an obstacle at the same time to determine whether it is a dangerous path. When there is a dangerous path, update the safety node, check the adjacent node pairs in the path, and try to find their common neighbors. Only when these common neighbors are far from the obstacle will the original two nodes be replaced by a safer neighbor point. Redundant node elimination strategy: After the path is generated, use the redundant node elimination algorithm to identify the redundant nodes in the path and remove these redundant parts. After removing the redundant nodes, the path structure can be optimized, the total number of path points can be reduced, the path length can be shortened, and the efficiency of path planning can be improved.

3. The method according to claim 2 further comprises implementing a secure node update policy to check, for each pair of adjacent points (p i , p i+1 ) in the path, whether there exists a common secure point p c , such that: To avoid the path hitting obstacles, the following conditions need to be met: Where: p i The i-th hop point in the path. p i+1 The (i + 1)-th hop point in the path. p c Represents the common safety point detected in the path. Is the set of free space, that is, the area where the path can move. This safety node update strategy can significantly reduce the collision risk of the path near corners or obstacles by replacing adjacent points with common safe neighbor points, thus improving the robustness and safety of path planning.

4. The method according to claim 3 further includes a tool for implementing the redundant node elimination strategy to further optimize and simplify the path during the path planning process. By reducing the turns and points on the path, the path becomes more direct and efficient, while maintaining the safety and feasibility of the path. The main function is to continue to simplify the path by checking whether consecutive points on the path can be directly connected. n = max(|x i+1 - x i-1 |, |y i+1 - y i-1 |) During the process of path generation, check that each point satisfies the following conditions: By generating points on a straight line, check whether the following points collide with obstacles: where (x i-1 , y i-1 ) is the coordinate of the currently inspected p i-1 ​ Consider an initial path containing the following nodes: A→B→C→D→E→F→F. If A and C can be directly connected without hitting an obstacle, then node B is redundant and can be directly deleted. In this way, the final path may only contain A→C→F, avoiding unnecessary turns and redundant intermediate nodes. where: isDirectlyConnected(pi-1, pi+1, map) indicates whether a direct connection can be made without interruption or hitting an obstacle. represents the finally optimized path, p i the point to be currently checked.

Images