Rapid optimal path planning method for mobile robot based on IBIPF-RRT

Abstract

The invention discloses a rapid optimal path planning method for a mobile robot based on IBIPF-RRT. The rapid optimal path planning method comprises the following steps: step S1, a sampling stage; the path between the two target points in the walking process of the robot is limited in an oval area, and the average value of the paths obtained through multiple times of operation of the RRT * algorithm is taken as the long axis of an oval; s3, a parent node and child node updating stage: a step S3 of performing expansion by using a predefined IPsteer function to obtain an expansion point qnew; checking whether the qnew and the qnew collide with each other or not, and restarting sampling if the collision occurs; and S4, searching a set Qnew of adjacent points in a certain range by taking the extension point qnew as a center if the collision does not exist, and S4, optimizing the connection stage. The invention provides a bidirectional search strategy of the shortest path, path recombination is carried out after the path is planned, and the problem of local optimization caused by a dual-tree search strategy can be solved, so that the overall optimization after connection is ensured.

Description

Technical Field

[0001] This invention relates to the field of robot path planning, and in particular to a fast optimal path planning method for mobile robots based on IBIPF-RRT. Background Technology

[0002] With the widespread application of intelligent robot technology in industrial automation, intelligent logistics, and special operations, path planning, as its core key technology, faces increasingly severe challenges in both real-time performance and optimality. While the traditional Rapid Expanding Random Tree Star Schema (RRT*) algorithm possesses theoretical advantages in probabilistic completeness and asymptotic optimality, it reveals three significant drawbacks in practical engineering applications: First, its completely random sampling strategy leads to low planning efficiency in complex environments; second, the fixed step size setting results in insufficient throughput in narrow passage scenarios; and finally, the generated paths often suffer from curvature discontinuities, making it difficult to meet the motion control requirements of modern mobile robots. These limitations severely restrict its practical application effectiveness in highly dynamic environments (such as warehousing and logistics, disaster relief, etc.).

[0003] Current mainstream improvement schemes mainly follow three paths: Informed-RRT, based on sampling optimization, improves efficiency by 2-3 times by limiting the elliptic sampling domain, but the success rate of replanning in dynamic obstacle environments drops sharply to 58%; F-RRT, a hybrid potential field method, performs well in structured environments, but has inherent problems such as complex parameter tuning and high failure rate in U-shaped trap environments; DRRT-Transformer, based on deep learning, can achieve planning speeds in the hundreds of milliseconds, but requires thousands of sets of labeled data and several times the hardware cost.

[0004] To address these technical bottlenecks, this invention proposes an improved Bidirectional Informed-RRT Fast Random Search Tree (IBIPF-RRT) algorithm. In the sampling phase, the sampling space is optimized by referencing the Informed-RRT algorithm. In the expansion phase, its adaptive information potential field mechanism automatically adjusts parameters through a dynamic potential field matrix, enabling rapid exploration with large steps in open areas and fine-tuning with small steps in narrow passages. In the connection optimization phase, a unique bidirectional shortest path optimization architecture innovatively combines a forward tree with an improved P-RRT sampling strategy and a reverse tree incorporating potential field gradient information. This dual-tree collaboration significantly reduces the number of iterations and ensures optimal overall path quality. Therefore, the IBIPF-RRT algorithm proposed in this invention enables mobile robots to achieve better environmental adaptability and faster, superior path planning capabilities in complex environments. Summary of the Invention

[0005] The technical problem this invention aims to solve is: addressing the issues of poor environmental adaptability and long algorithm consumption in mobile robot path planning. Based on the F-RRT* algorithm, it optimizes the sampling space during the sampling phase and introduces an adaptive gravity strategy during the expansion phase. In the connection optimization phase, a bidirectional search strategy using shortest path optimization is employed, and after a path is found, the shortest path optimization strategy is used again, thereby ensuring the path quality and timeliness of the robot's movement in complex environments.

[0006] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is: a fast optimal path planning method for mobile robots based on IBIPF-RRT, comprising the following steps:

[0007] Step S1: Sampling phase;

[0008] The path between two target points during robot movement is restricted to an elliptical region, and the average of the paths obtained from multiple runs of the RRT* algorithm is taken as the major axis of the ellipse. The SampleInformed sampling function is used to sample within the preset elliptical region to obtain a random point qrand and the nearest point qnearest in the known tree.

[0009] Step S2: Extension phase;

[0010] An expansion point qnew is obtained by extending the robot using the predefined IP_steer function. The expansion function employs an adaptive step-size expansion strategy, determining the variable step size based on the number and size of obstacles. Furthermore, an improved gravitational field and gravity calculation strategy are introduced, enabling the mobile robot to maintain navigation performance while avoiding getting stuck in obstacle areas by attenuating the gravitational field when entering hazardous areas.

[0011] Step S3: Parent node and child node update phase;

[0012] Check if qnearest and qnew collide. If a collision occurs, restart sampling. If no collision occurs, find a set of neighboring nodes Qnear, centered on the expansion point qnew, within a certain range. Based on this set, find a parent node qnear that, when combined with the expansion point qnew, has the shortest path. Then, use a triangle pruning strategy to backtrack on the parent node qnear to find an ancestor node qnearest. This ancestor node is located on the qnear tree and, when combined with the expansion point qnew, has a shorter path. However, due to obstacles, the path formed by the ancestor node qnearest and the expansion point qnew may still have room for optimization when passing through obstacles due to larger gaps. Therefore, use the createnode function to create a new node qcreate. If qcreate is created, the new node becomes the parent node of the expansion point qnew; otherwise, the ancestor node qnearest becomes the parent node of the expansion point qnew. Child nodes are also updated (reware) in the same way.

[0013] Step S4: Optimize the connection phase;

[0014] Based on two target points, a bidirectional search strategy for shortest path optimization is designed to plan the path between the two target points. First, the distance between the two expanded trees and whether there are obstacles between the two trees are determined. If the distance is less than a certain value and there are no obstacles in between, the expansion stops, and the path is obtained. Otherwise, the random trees T1 and T2 are swapped.

[0015] In summary, due to the adoption of the above technical solution, the beneficial effects of the present invention are:

[0016] 1. This invention proposes a bidirectional search strategy for the shortest path. After planning the path, the path is reorganized, which can make up for the local optimum problem caused by the dual-tree search strategy, thereby ensuring the overall optimum after connection.

[0017] 2. Based on the F-RRT* algorithm, this invention improves the sampling space in the sampling stage, which greatly shortens the planning time and effectively reduces the upper limit of path length.

[0018] 3. Based on the F-RRT* algorithm, this invention optimizes the search strategy in the expansion phase. By introducing an adaptive phase gravity strategy, it solves the problem that F-RRT* often fails to plan an optimal path when facing complex environments.

[0019] 4. The bidirectional search strategy for the shortest path proposed in this invention greatly shortens the planning time while ensuring path quality. Attached Figure Description

[0020] To facilitate understanding by those skilled in the art, the present invention will be further described below with reference to the accompanying drawings.

[0021] Figure 1 This is a flowchart of the key steps of the IBIPF-RRT algorithm.

[0022] Figure 2 This is a schematic diagram of path optimization.

[0023] Figure 3 This is a schematic diagram for shortest path optimization.

[0024] Figure 4 This is a schematic diagram of path reorganization optimization after a bidirectional search strategy. Detailed Implementation

[0025] The technical solution of the present invention will be clearly and completely described below with reference to the embodiments. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. All other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0026] Example 1

[0027] The following describes in detail a fast optimal path planning method for mobile robots based on IBIPF-RRT, comprising the following steps:

[0028] Step S1: Sampling phase;

[0029] The path between two target points during robot movement is restricted to an elliptical region, and the average of the paths obtained from multiple runs of the RRT* algorithm is taken as the major axis of the ellipse. The SampleInformed sampling function is used to sample within the preset elliptical region to obtain a random point qrand and the nearest point qnearest in the known tree.

[0030] Step S2: Extension phase;

[0031] An expansion point qnew is obtained by extending the robot using the predefined IP_steer function. The expansion function employs an adaptive step-size expansion strategy, determining the variable step size based on the number and size of obstacles. Furthermore, an improved gravitational field and gravity calculation strategy are introduced, enabling the mobile robot to maintain navigation performance while avoiding getting stuck in obstacle areas by attenuating the gravitational field when entering hazardous areas.

[0032] Step S3: Parent node and child node update phase;

[0033] Check if qnearest and qnew collide. If a collision occurs, restart sampling. If no collision occurs, find a set of neighboring nodes Qnear, centered on the expansion point qnew, within a certain range. Based on this set, find a parent node qnear that, when combined with the expansion point qnew, has the shortest path. Then, use a triangle pruning strategy to backtrack on the parent node qnear to find an ancestor node qnearest. This ancestor node is located on the qnear tree and, when combined with the expansion point qnew, has a shorter path. However, due to obstacles, the path formed by the ancestor node qnearest and the expansion point qnew may still have room for optimization when passing through obstacles due to larger gaps. Therefore, use the createnode function to create a new node qcreate. If qcreate is created, the new node becomes the parent node of the expansion point qnew; otherwise, the ancestor node qnearest becomes the parent node of the expansion point qnew. Child nodes are also updated (reware) in the same way.

[0034] Step S4: Optimize the connection phase;

[0035] Based on two target points, a bidirectional search strategy for shortest path optimization is designed to plan the path between the two target points. First, the distance between the two expanded trees and whether there are obstacles between the two trees are determined. If the distance is less than a certain value and there are no obstacles in between, the expansion stops, and the path is obtained. Otherwise, the random trees T1 and T2 are swapped.

[0036] like Figure 2 As shown, two random trees are planned and expanded from qstart (yellow dot) and qgoal (red dot). The blue lines and dots represent the nodes and branches of random tree T1, and the green lines and dots represent the nodes and branches of random tree T2. When the two random trees are a certain distance apart, the search is completed, and a connection is made to plan a path connecting the initial point and the target point (e.g., ...). Figure 2 (As shown by the middle red line).

[0037] Then, the planned path connecting the initial point and the target point is optimized using the shortest path algorithm. For example... Figure 3 As shown, q n-1 and q n+1 There are two target points, q n-1 q n and q n q n+1 This is the planned path connecting the two objective points. In the shortest path optimization process, we first find nodes q that have bends. n Then from q n-1 Start by taking three adjacent nodes and connecting them forward, at q n-1 qn and q n q n+1 The above are respectively from q n-1 and q n+1 To q n Iterate until a collision occurs, then the connection is successful, as shown by the blue line. In this case, connect q. n(1) q n(2) Then press q. n Q n(1) q n(2) Replace, and then continue from q n-1 Start by connecting three points until you reach a point where it's impossible to iterate directly and connect them all; in this case, shortest path optimization is successful. Then, consider q... n After taking the tape, first select q. n-1 q n(1) and q n(2) The iteration continues, and since all connected paths collide, q is selected. n(1) q n(2) q n+1 The process is iterated, and the purple path in the diagram represents the result of the latter iteration. At this point, q... n(2) Use q n(4) and q n(3) The substitution is performed, and then the above iterative process is repeated. The final shortest path result is as follows: Figure 3 As shown in the red path.

[0038] Optimize the shortest path for the planned path connecting the initial point and the target point. For example... Figure 4 As shown, q n-1 and q n+1 There are two target points, q n-1 q n and q n q n+1 This is the planned path connecting the two objective points. In the shortest path optimization process, we first find nodes q that have bends. n Then from q n-1 Start by taking three adjacent nodes and connecting them forward, at q n-1 q n and q n q n+1 The above are respectively from q n-1 and q n+1 To q n Iterate until no collision occurs, then the connection is successful, as shown by the blue line. In this case, connect q. n(1) q n(2) Then press q. n Q n(1) q n(2) Replace, and then continue from q n-1Start by connecting three points until you reach a point where it's impossible to iterate directly and connect them all; in this case, shortest path optimization is successful. Then, consider q... n After taking the tape, first select q. n-1 q n(1) and q n(2) The iteration continues, and since all connected paths collide, q is selected. n(1) q n(2) q n+1 Perform iterations. Figure 4 The purple path in the middle is generated by the latter iteration. Now, q... n(2) Use q n(4) and q n(3) The substitution is performed, and then the above iterative process is repeated. The final shortest path result is as follows: Figure 4 As shown in the red path, the result of optimizing the generated path using this shortest path optimization bidirectional search strategy is as follows: Figure 4 As shown, during the optimization process, a blue path is first formed, and then it is gradually optimized into a red dashed line.

[0039] The preferred embodiments of the present invention disclosed above are merely illustrative of the invention. These preferred embodiments do not exhaustively describe all details, nor do they limit the invention to any specific implementation. Clearly, many modifications and variations can be made based on the content of this specification. This specification selects and specifically describes these embodiments to better explain the principles and practical applications of the invention, thereby enabling those skilled in the art to better understand and utilize the invention. The invention is limited only by the claims and their full scope and equivalents.

Claims

1. A fast optimal path planning method for mobile robots based on IBIPF-RRT, characterized in that, Includes the following steps: Step S1: Sampling phase; The path between two target points during robot movement is restricted to an elliptical region, and the average of the paths obtained from multiple runs of the RRT* algorithm is taken as the major axis of the ellipse. The SampleInformed sampling function is used to sample within the preset elliptical region to obtain a random point qrand and the nearest point qnearest in the known tree. Step S2: Extension phase; An expansion point qnew is obtained by extending the robot using the predefined IP_steer function. The expansion function employs an adaptive step-size expansion strategy, determining the variable step size based on the number and size of obstacles. Furthermore, an improved gravitational field and gravity calculation strategy are introduced, enabling the mobile robot to maintain navigation performance while avoiding getting stuck in obstacle areas by attenuating the gravitational field when entering hazardous areas. Step S3: Parent node and child node update phase; Check if qnearest and qnew collide. If a collision occurs, restart sampling. If no collision occurs, find a set of neighboring nodes Qnear, centered on the expansion point qnew, within a certain range. Based on this set, find a parent node qnear that, when combined with the expansion point qnew, has the shortest path. Then, use a triangle pruning strategy to backtrack on the parent node qnear to find an ancestor node qnearest. This ancestor node is located on the qnear tree and, when combined with the expansion point qnew, has a shorter path. However, due to obstacles, the path formed by the ancestor node qnearest and the expansion point qnew may still have room for optimization when passing through obstacles due to larger gaps. Therefore, use the createnode function to create a new node qcreate. If qcreate is created, the new node becomes the parent node of the expansion point qnew; otherwise, the ancestor node qnearest becomes the parent node of the expansion point qnew. Child nodes are also updated (reware) in the same way. Step S4: Optimize the connection phase; Based on two target points, a bidirectional search strategy for shortest path optimization is designed to plan the path between the two target points. First, the distance between the two expanded trees and whether there are obstacles between the two trees are determined. If the distance is less than a certain value and there are no obstacles in between, the expansion stops, and the path is obtained. Otherwise, the random trees T1 and T2 are swapped.

Images