An improved bit* based global path planning method for mobile robots
Patent Information
- Application Number
- CN202211333294.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-10-28
- Publication Date
- 2026-11-03
- Estimated Expiration
- 2042-10-28
AI Technical Summary
The existing BIT* algorithm suffers from problems such as low path safety, long planning time, too many corners, and insufficient path smoothness during path planning. This is mainly due to the fact that the cost estimation in its heuristic method is only related to the path distance and the step size is fixed.
The BIT* algorithm is improved by using obstacle inflation, adaptive dynamic step size, environmental information factor to improve the estimation function, and quadratic reverse pruning method to enhance path safety and planning efficiency.
It shortens the path length, reduces the number of turning points, improves path smoothness and planning speed, and increases the safety factor.
Smart Images
Figure CN115903789B_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of mobile robot path planning technology, specifically relating to a global path planning method for mobile robots based on an improved BIT*. Background Technology
[0002] Since their inception, autonomous mobile robots have been continuously evolving to serve humanity. With the development of robotics technology, robots can now operate not only in simple, repetitive environments such as factories and warehouses, but also gradually enter spaces where human activity takes place. Today, intelligent mobile robots are increasingly used in many fields such as industry, healthcare, logistics, and agriculture, significantly improving production efficiency and gradually becoming an indispensable part of human life. The application prospects of mobile robots will undoubtedly bring enormous economic and social benefits. Autonomous movement is one of the most basic and important functions of mobile robots, and path planning is a key supporting technology. Path planning mainly refers to planning an optimal or near-optimal collision-free path from the starting point to the target point from the application scenario, based on prior knowledge and under certain constraints or performance conditions.
[0003] RRT* is a common path planning algorithm that guides the growth of an expanded tree through random sampling, eventually exploring the area around the target point. However, because it relies on random sampling, the sampling points lack order and a fixed step size, leading to excessively long convergence times and tortuous paths with numerous inflection points. The BIT* algorithm, proposed in recent years, is a global path planning algorithm that builds upon RRT* by employing batch sampling and sorting the samples, while also incorporating heuristics from search algorithms, significantly improving its convergence speed.
[0004] However, the estimated cost in its heuristic method is only related to the path distance, ignoring the safety of the path and the validity of the nodes. This may still lead to the generation of unused paths and increase the planning time. At the same time, the fixed step size still causes the algorithm to converge slowly and the path to be less smooth. Summary of the Invention
[0005] To address the aforementioned problems, this invention provides a global path planning method for mobile robots based on an improved BIT*. By designing a map obstacle expansion method to avoid potential collisions, the safety of the generated path is improved. An adaptive dynamic step size method and a heuristic method that increases map information factors are designed to improve the efficiency of path planning. A secondary reverse pruning method is designed to significantly reduce the number of inflection points with a small number of iterations.
[0006] To solve the above problems, the technical solution of the present invention is as follows:
[0007] A global path planning method for mobile robots based on improved BIT* includes the following steps:
[0008] S1. Obtain an environmental map and inflate the obstacles in the map based on the size of the mobile robot;
[0009] S2. Determine the starting point of the mobile robot, the position coordinates of the target point, the baseline growth step size r, the maximum number of iterations, and construct an extension tree with the starting point of the mobile robot as the root node;
[0010] S3. Check if the priority queues for tree edges and tree nodes are empty. If they are empty, proceed to S4; otherwise, proceed to S6.
[0011] S4. Based on the current feasible path length and the position coordinates of the starting point and the target point, generate a sampling ellipse domain, perform pruning and sampling operations, and add the results to the queues of tree nodes and tree edges.
[0012] S5. Sort the queues of tree nodes and tree edges according to the minimum estimated cost, and generate a priority queue for tree nodes and a priority queue for tree edges.
[0013] S6. After determining the minimum estimated cost, obtain the tree node with the minimum estimated cost from the priority queue of tree nodes and expand it.
[0014] S7. After judging the minimum estimated cost, estimated cost, and actual cost, obtain the tree edge with the minimum estimated cost in the tree edge priority queue as the optimal edge to be expanded and expand it.
[0015] S8. Determine if the terminal point of the optimal edge to be expanded is in the tree node list. If so, perform a rewiring operation and then empty the priority queues of tree edges and tree nodes; otherwise, expand the edge.
[0016] S9. Determine if the iteration count has been reached. If yes, output the current path node; otherwise, execute S4.
[0017] S10. Perform secondary reverse pruning on the current path node and output the executable path.
[0018] Furthermore, the map obstacle inflation is performed in the following manner:
[0019] D ex =E rob / α s (E obs >0,1>α s >0)
[0020] Among them, D ex E is the distance by which the obstacle expands. rob α is the maximum distance from the edge of the mobile robot to its geometric center.s For security expansion factor.
[0021] Furthermore, the minimum estimated cost mentioned in S5, S6, and S7 is:
[0022]
[0023] Where V value The minimum estimated cost in the priority queue of tree nodes, gT(v), represents the true cost from the starting point of the current tree to node v, i.e., the path length. QV E represents a priority queue of tree nodes. value h refers to the minimum estimated cost in the tree edge priority queue. estimated (v) represents the estimated cost from point v to the target point, which is determined by the following formula:
[0024] h estimated (v) = calc dist (v,x_goal)+βd estimated (v)
[0025] calc dist (v, x_goal) returns the Euclidean norm:
[0026]
[0027] In the formula, x_goal represents the target point, v x and v y Let x_goal represent the x-coordinate and y-coordinate of point v, respectively. x and x_goal y ... estimated (v) represents the distance between point v and the nearest obstacle.
[0028] Furthermore, in step S6, a priority queue of tree nodes is obtained. Using an adaptive dynamic step size as the radius of the expanding circle, tree nodes within the expanded circle of the priority queue are acquired. The adaptive dynamic step size is determined by the following formula:
[0029]
[0030] Where δ is the distance factor of the extended circle, ε is the area factor of the extended circle, and s cover Let s be the area coinciding between the extended circle with a radius equal to the base step size and the obstacle. r Expand the area of the circle based on this.
[0031] Furthermore, the secondary reverse pruning in S10 includes the following steps:
[0032] S10.1 Obtain the list of nodes on the current path and assign the target point to node;
[0033] S10.2, Execute the collision detection function:
[0034]
[0035] Where node is the starting node for the current judgment, node par For its parent node, node grand for node par For the parent node, if a collision occurs, execute S10.2; otherwise, execute S10.3.
[0036] S10.2, move node par Assigning the value to node adds node to the new path list T. new Then execute S10.4;
[0037] S10.3, move node grand Assign to node par ;
[0038] S10.4 Determine the node grand If empty, output T. new Otherwise, execute S10.2.
[0039] Compared with the prior art, the beneficial effects of the present invention are as follows:
[0040] The traditional BIT* algorithm suffers from drawbacks in path planning, such as low path safety, long planning time, and uneven paths due to excessive corners, as its heuristic cost estimation is only related to path distance and fixed step size. This is addressed by improving BIT* in four ways: 1) Obstacle inflation based on the mobile robot's dimensions treats the inflated obstacle area as a solid obstacle, giving it unsampling and unreachable characteristics, thus improving path safety; 2) Introducing a dynamic adaptive step size: when the expansion tree node is far from the obstacle, a step size that increases based on the distance to the nearest obstacle is used to accelerate convergence; when the expansion tree node is close to the obstacle, a step size that shortens the overlap area between the expansion circle and the obstacle is used based on the baseline step size, ensuring the algorithm's passability in narrow areas; 3) Adding an environmental information factor to the cost estimation function, associating the sampling point ranking with the environmental map, improving sampling efficiency; 4) Adding a quadratic inverse pruning function to reduce the number of path inflection points. This results in shorter path search time, fewer path inflections, and shorter paths during path planning, leading to a higher safety factor. Attached Figure Description
[0041] Figure 1This is a flowchart of the method of the present invention;
[0042] Figure 2 A schematic diagram of pruning and sampling in a path planning algorithm;
[0043] Figure 3 This is a schematic diagram of the adaptive dynamic step size of the present invention;
[0044] Figure 4 This is a schematic diagram illustrating the start of the secondary reverse pruning of the path in this invention;
[0045] Figure 5 This is a schematic diagram of the secondary reverse pruning process of the present invention;
[0046] Figure 6 This is a schematic diagram illustrating the completion of the secondary reverse pruning of the path according to the present invention;
[0047] Figure 7 This is a schematic diagram showing the comparative test results of the mobile robot path planning method of the present invention and the path planning method in the prior art under environmental condition 1.
[0048] Figure 8 This is a schematic diagram showing the comparative test results of the mobile robot path planning method of the present invention and the path planning method in the prior art under environmental condition 2.
[0049] Figure 9 This is a schematic diagram showing the comparative test results of the mobile robot path planning method of the present invention and the path planning method in the prior art under environmental condition 3. Detailed Implementation
[0050] The specific embodiments of the present invention will be further described below with reference to the accompanying drawings.
[0051] This implementation presents a global path planning method for mobile robots based on an improved BIT* algorithm, the flowchart of which is shown below. Figure 1 As shown. The method is implemented through the following steps:
[0052] Step 1: Use SLAM technology to acquire environmental map information. This map information can be captured by sensors, such as LiDAR or inertial navigation; the specific method is not limited here. Inflate obstacles in the map. The method is as follows:
[0053] D ex =E rob / α s (E obs >0,1>α s >0.5), where D ex E is the distance by which the obstacle expands. r o bE represents the maximum distance from the edge of the mobile robot to its geometric center, which is taken as E in the experiment. rob =1, α s The safety expansion factor was set to 0.8 in the experiment. α s The larger the value of α, the closer the planned path is to the allowable distance from the obstacle. s The smaller the value, the farther the allowed distance.
[0054] Step 2: Determine the starting point x_start of the mobile robot, the position coordinates x_goal of the target point, and the basic growth step size r (in the experiment, r = 4) and the maximum number of iterations k. Construct a scalar tree with the starting point of the mobile robot as the root node;
[0055] Step 3: Determine the priority queue for tree edges. QE Tree node priority queue QV Is it empty? If it is empty, proceed to step four; otherwise, proceed to step five.
[0056] Step 4, as follows Figure 2 As shown, a sampling elliptical domain is generated with a major axis length of C. best , representing the optimal path length in the current state, is set to infinity in the first iteration, and the minor axis length is . Where C min This represents the Eulerian distance between the start and end points. Perform a pruning operation, removing nodes whose estimated cost is greater than the given cost from the existing nodes; here, node H is removed. Also remove connections whose estimated cost is greater than the given cost from the existing connections; here, connections AH and DH are removed. Perform a sampling operation.
[0057] Step 5: Determine if the minimum estimated cost in the tree node priority queue is less than the minimum estimated cost in the tree edge priority queue. If it is less, proceed to Step 6; otherwise, proceed to Step 7. The minimum estimated cost is determined by the following formula:
[0058]
[0059] Where V value The minimum estimated cost in the priority queue of tree nodes, gT(v), represents the true cost from the starting point of the current tree to node v, i.e., the path length. QV E represents a priority queue of tree nodes. value h refers to the minimum estimated cost in the tree edge priority queue. estimated (v) represents the estimated cost from point v to the target point, which is determined by the following formula:
[0060] h estimated (v) = calc dist (v,x_goal)+βd estimated (v)
[0061] calc dist (v, x_goal) returns the Euclidean norm:
[0062]
[0063] In the formula, x_goal represents the target point, v x and v y Let x_goal represent the x-coordinate and y-coordinate of point v, respectively. x and x_goal y ... estimated (v) represents the distance between point v and the nearest obstacle.
[0064] Step 6: Obtain tree nodes within the expansion circle's radius from the priority queue of tree nodes, using the adaptive dynamic step size as the expansion circle radius. Then, select the tree node with the lowest estimated cost for expansion. The adaptive dynamic step size is determined by the following formula:
[0065]
[0066] Where δ is the distance factor of the extended circle, which is taken as 0.8 in the experiment, ε is the area factor of the extended circle, which is taken as 2 in the experiment, and s cover Let s be the area coinciding between the extended circle with a radius equal to the base step size and the obstacle. r Expand the area of the circle based on the given area. For example... Figure 3 As shown, at point A, there are no obstacles within the basic step-length expansion circle, d estimated (v) represents the length of line segment AI. As the adaptive step size increases, the range of nodes acquired increases, A1 is directly expanded, and the growth speed of the expanded tree increases accordingly. At point C, the basic step size expansion circle connects to the obstacle, and the step size is the basic step size. At point G, there is an obstacle within the basic step size expansion circle, s cover To determine the area of the raster shaded region, the node range is reduced. Points that originally needed to be expanded to G1 can now be expanded to G2, thus improving the quality of the expanded tree growth. Then return to step five.
[0067] Step 7: Obtain the optimal edge to be expanded from the tree edge priority queue that has the lowest estimated cost;
[0068] Step 8: Determine whether the estimated cost of the optimal edge to be expanded is less than the current path cost. If it is less, proceed to step 9; otherwise, proceed to step 10.
[0069] Step 9: Determine whether the actual cost of the optimal edge to be expanded is less than the current path cost. If it is less, proceed to step 11; otherwise, proceed to step 12.
[0070] Step 10: Empty all priority queues for tree edges and tree nodes, then proceed to Step 12;
[0071] Step 11: Determine if the terminal point of the optimal edge to be expanded is in the tree node list. If it is, perform the rewiring operation; otherwise, expand the edge and then proceed to Step 12.
[0072] Step 12: Determine if the iteration count has been reached. If yes, output the current path node; otherwise, return to step 3.
[0073] Step Thirteen, as Figure 4 As shown, a second reverse pruning is performed on the current path nodes. The second reverse pruning includes the following steps: obtaining the list of current path nodes, assigning the target point B to node; and executing the collision detection function.
[0074]
[0075] Where node is the current starting node B for judgment, node par For its parent node G, node grand for node par A collision occurs with parent node F; assign G to node and add G to the new path list T. new In; as Figure 5 As shown, continue performing collision detection (collision(G,E)) if no collision occurs; change the parent node of G to E; perform collision detection (collision(G,D)) if no collision occurs; change the parent node of G to E; perform collision detection (collision(G,D)); as... Figure 6 As shown, if a collision is detected by performing collision(G,C), D is assigned to node, and D is added to the new path list T. new In the middle; perform a collision check (collision(D,A)) and if no collision occurs; change the parent node of D to A, check if the parent node of A is empty, and output T. new (ADGB).
[0076] Implementation results:
[0077] Based on the above steps, this invention designed nine sets of comparative experiments, and the experimental results are as follows: Figure 7 , Figure 8As shown in Figure 9, the planned path is represented by black lines, path nodes by black dots, and the final planned path by a thick solid line. Based on the number and area of obstacles in the environment, three experimental environments were set: sparse, dense, and U-shaped. An acceptable planned path length was set as the termination condition. A comprehensive comparison was made between RRT*, BIT*, and the improved BIT* algorithm. Experimental results show that the improved BIT* algorithm, formed by introducing obstacle expansion, adaptive dynamic step size, environmental information factor to improve the estimation function, and secondary reverse pruning, shortens the path length by 3%–10%, improves the path planning time by an average of 21%–26%, and reduces the number of inflection points by 58%–66%. This effectively solves the problems of low path safety, long planning time, and excessive corners in the traditional BIT* algorithm. The comparison results are shown in Table 1.
[0078] Table 1. Average results of 50 experiments on the three algorithms under three different environments.
[0079]
Claims
1. A global path planning method for mobile robots based on an improved BIT*, characterized in that, Includes the following steps: S1: Obtain an environmental map and inflate obstacles in the map based on the size of the mobile robot; S2: Determine the starting point, target point coordinates, and baseline growth step size of the mobile robot. The maximum number of iterations is determined, and an extended tree is constructed with the starting point of the mobile robot as the root node. S3: Check if the priority queues for tree edges and tree nodes are empty. If they are empty, execute S4; otherwise, execute S6. S4: Based on the current feasible path length and the position coordinates of the starting point and the target point, generate a sampling ellipse domain, perform pruning and sampling operations, and add the results to the queues of tree nodes and tree edges; S5: Sort the queues of tree nodes and tree edges according to the minimum estimated cost, and generate a priority queue for tree nodes and a priority queue for tree edges; S6: After determining the minimum estimated cost, obtain the tree node with the minimum estimated cost in the priority queue of tree nodes and expand it; S7: After judging the minimum estimated cost, estimated cost, and actual cost, obtain the tree edge with the minimum estimated cost in the tree edge priority queue as the optimal edge to be expanded and expand it. S8: Determine if the terminal point of the optimal edge to be expanded is in the tree node list. If so, perform a rewiring operation and then empty the priority queues of tree edges and tree nodes; otherwise, expand the edge. S9: Determine if the iteration count has been reached. If yes, output the current path node; otherwise, execute S4. S10: Perform secondary reverse pruning on the current path node and output an executable path.
2. The global path planning method for mobile robots based on improved BIT* according to claim 1, characterized in that, The map obstacle inflation in S1 is performed in the following manner: ； in, The distance the obstacle expands. This represents the maximum distance from the edge of the mobile robot to its geometric center. For security expansion factor.
3. The global path planning method for mobile robots based on improved BIT* according to claim 1, characterized in that, The minimum estimated cost described in S5, S6, and S7 is: ； in The minimum estimated cost in the priority queue of tree nodes. This represents the distance from the starting point of the current tree to the point... The true cost, i.e., path length. Represents a priority queue of tree nodes. Represents a tree edge priority queue. The minimum estimated cost in the tree edge priority queue. To return to the Euclidean norm, Point Estimated cost to the target point Point The estimated cost to the target point is determined by the following formula: ； In the formula To return to the Euclidean norm, Represents environmental information factors, Point Distance to the nearest obstacle.
4. The global path planning method for mobile robots based on improved BIT* according to claim 1, characterized in that, The method described in S6 for obtaining the priority queue of tree nodes involves using an adaptive dynamic step size as the radius of the expanding circle to obtain tree nodes within the range of the expanding circle in the priority queue. The adaptive dynamic step size is determined by the following formula: ； in, Based on step size, To extend the circular distance factor, To expand the area factor of a circle, Let be the area of the extended circle with radius equal to the base step size that coincides with the obstacle. Expand the area of the circle based on this.
5. The global path planning method for mobile robots based on improved BIT* according to claim 1, characterized in that, The secondary reverse pruning described in S10 includes the following steps: S10.1: Obtain the current path node list and assign the target point to it. ; S10.2: Execute the collision detection function: ； in, This is the starting node for the current judgment. Its parent node, for For the parent node, if a collision occurs, execute S10.2; otherwise, execute S10.
3. S10.2: Will Assign to ,Will Add to new path list Then execute S10.4; S10.3: Will Assign to ; S10.4: Judgment Is it empty? If it is empty, output the value. Otherwise, execute S10.2.