RRT-Connect narrow channel path planning method based on optimization of intermediate node interpolation method and cross method shaped like Chinese character'mi '

Abstract

The invention discloses an RRT-Connect narrow channel path planning method based on optimization of an intermediate node interpolation method and a cross method shaped like a Chinese character'mi '. Belongs to the field of robot path planning. Comprising the steps that in the searching process, an intermediate node interpolation method is introduced, two intermediate nodes are inserted between two nodes colliding with an obstacle, and sampling is conducted in a narrow channel entrance area in a targeted mode; on the basis of a bridge test algorithm, a *-shaped crossing method is provided, a *-shaped line segment is formed by taking a node as a center, and the node in a narrow channel is accurately identified; judging whether the node collides with an obstacle or not by combining a vector geometry principle and an extended boundary concept; and eliminating redundant nodes in the path by adopting a greedy strategy, and smoothing the path by using a quadratic B-spline curve to obtain an optimized path. The method effectively solves the problems that an original RRT-Connect algorithm is too random in sampling under a narrow channel, low in search efficiency, poor in path quality and the like.

Description

Technical Field

[0001] The present invention relates to the technical field of robot path planning, and in particular to an RRT-Connect narrow channel path planning method based on optimization of an intermediate node interpolation method and a crossover method. Background Art

[0002] Mobile robots have shown great application potential in many fields such as industry, agriculture and commercial services due to their excellent flexibility and mobility. As a core component of mobile robot technology, path planning technology is committed to finding a safe path from the starting point to the end point. When evaluating the performance of mobile robots, path cost and planning speed are two indispensable considerations. Therefore, achieving efficient path planning is crucial to promoting the development of mobile robot technology.

[0003] However, although the RRT-Connect algorithm has made remarkable achievements in the field of path planning, it still has many shortcomings when facing complex environments: (1) The uniform global random sampling strategy adopted by the algorithm makes it easy for the search tree to explore blindly and fall into the local optimal solution; (2) Since narrow channels account for a small proportion of the overall environment space, it is difficult for the random tree to effectively extend into the narrow channels, resulting in low search efficiency; (3) The search tree expansion is too random, resulting in the generation of many redundant nodes and poor path quality.

[0004] In order to solve the problem of sampling in narrow channels, existing literature has proposed a variety of strategies for combining and optimizing bridge detection algorithms to improve their performance in narrow areas. For example, RRT-Connect is combined with an improved bridge detection algorithm to improve the efficiency of building bridges inside narrow channels, but the problems of wide sample space and low sampling success rate have not been completely solved. Therefore, the present invention improves the bridge test algorithm and the RRT-Connect algorithm, and proposes an RRT-Connect narrow channel path planning method based on the optimization of the intermediate node interpolation method and the cross-shaped method. Summary of the invention

[0005] The purpose of the present invention is to provide an RRT-Connect narrow channel path planning method based on the optimization of the intermediate node interpolation method and the cross-shaped method. The present invention can solve the problems of high sampling randomness, low search efficiency and poor path quality of the original RRT-Connect algorithm in the complex environment of narrow channels.

[0006] The technical solution adopted by the present invention is as follows:

[0007] According to one aspect of the present invention, there is provided an RRT-Connect narrow channel path planning method based on an intermediate node interpolation method and a cross-shaped cross method optimization, comprising the following steps:

[0008] Initialize the state space, set the starting point and end point, and construct a bidirectional random search tree and , with the starting point and the end point as and The root node of

[0009] Evaluate and The number of nodes, select the tree with fewer nodes as the starting point for the search, and search first by default ;

[0010] In the expansion phase, randomly sampled nodes are obtained and , when both collide with obstacles, the offset is set based on the middle node interpolation method (The value range is ),exist and Generate two intermediate nodes between and , check whether they collide. If they do, reselect random sampling nodes. and If at least one node in the process does not collide, reselect the parent node and reconnect it, and use the intermediate node that does not collide as the new node. Add to search tree;

[0011] node Continue to generate random sampling nodes Based on the bridge test algorithm, the cross-shaped method is proposed to explore the interior of the narrow channel. Centered on the a priori map channel width and robot size, with a certain step length , generate eight "M"-shaped line segments extending in different directions, and detect their collisions one by one. If there is at least one pair of line segments in opposite directions that collide with obstacles at the same time, it can be determined that the node Located in a narrow passage. The search tree starts with The nearest node As a starting point, The direction extends until it encounters an obstacle or the two trees are connected;

[0012] If the two trees are connected, then the path is backtracked to get the initial path, otherwise the search is repeated;

[0013] After obtaining the initial path, a greedy strategy is used to remove redundant nodes on the path, and a secondary Spline smoothing paths.

[0014] As a preferred solution of the present invention, the intermediate node interpolation method is:

[0015] set up The coordinates are , The coordinates are , preset offset , at the node and nodes Generate two intermediate nodes between and . and The coordinates of are calculated according to the following formula:

[0016] (1)

[0017] (2)

[0018] Offset Determine the intermediate node and Relative to the original node and The specific location of and Perform collision detection, if the middle node and At least one of the nodes has not collided with an obstacle, and the algorithm infers that the non-collided node is located in the entrance area of ​​the narrow passage. Based on this inference, the algorithm will guide the search tree to generate more nodes in this area. and If both nodes collide with obstacles, it means that the current path direction is not feasible. At this time, the algorithm will abandon these two nodes and continue to randomly generate new node pairs for collision detection and path exploration until a feasible path is found or the preset exploration termination condition is reached.

[0019] As a preferred embodiment of the present invention, the cross method is:

[0020] After the random tree finds the narrow channel entrance area through the intermediate node interpolation method, a random node is selected. As the center, the coordinates are , with step length Generate "M"-shaped cross line segments in eight directions: right, lower right, lower, lower left, left, upper left, upper, and upper right, that is, , , , , , , , The parametric equations of each line segment are:

[0021] (3)

[0022] The algorithm will perform collision detection on these eight "M" shaped intersection line segments. If any of the following conditions is met:

[0023] (4)

[0024] That is, there is at least one pair of line segments with opposite directions (e.g. and ,or and etc.), and collides with obstacles at the same time, it can be determined that the node It is indeed located in a narrow passage, and the algorithm will continue exploring in this direction.

[0025] As a preferred solution of the present invention, the collision detection method is:

[0026] Combining vector geometry and the concept of extended boundaries, the collision detection between line segments and obstacles is performed. If the line segments formed by connecting nodes intersect with the extended boundaries of obstacles, it means that a collision has occurred. If the line segments formed by connecting nodes do not intersect with the extended boundaries of obstacles, it means that no collision has occurred.

[0027] As a preferred solution of the present invention, greedy strategy pruning and secondary The strategy for spline smoothing paths is:

[0028] From the starting point Start by connecting the nodes after it, and check whether the line segments connected by these nodes collide with the obstacle boundary. If there is no collision, it will be used as a path, continue to extend to the next node and repeat this check step. However, if a line segment is found to collide with an obstacle, it will be traced back to the starting point node by node. , check whether the connection line collides, until a non-collision node is found, and record it as Repeat this process until you find a direct connection Finally, the new path node is combined with , After the redundant points are deleted, for each pair of adjacent nodes in the path, the previous node, the current node and the next node are used as control points. , , For each pair of control points, by changing the parameter (The value range is ) values, multiple points on the curve can be calculated. Connect the calculated points on the curve in order to construct a quadratic The calculation formula of the spline smooth path is:

[0029] (5)

[0030] Compared with the prior art, the advantages and beneficial effects of the present invention are as follows:

[0031] (1) The intermediate node interpolation method is introduced. By presetting the offset between the two collision nodes randomly generated by the RRT-Connect algorithm, the intermediate node is generated and its collision with the obstacle is checked. This strategy effectively narrows the sampling range and guides the search tree to explore deeper into the narrow channel. The simulation results show that compared with the original RRT-Connect algorithm, the intermediate node interpolation method reduces the running time by more than 24.68% and the total number of nodes by more than 38.34%. This method significantly reduces unnecessary node generation and sampling operations, avoids the algorithm from falling into the local optimal solution, and effectively improves the operation efficiency of the algorithm.

[0032] (2) A cross-shaped method is proposed. This method takes the node as the center and extends line segments in eight directions. By detecting the collision between these line segments and obstacles, it accurately determines whether the node is located in a narrow channel and guides the algorithm through the narrow channel accordingly. Although the improvement effect of the cross-shaped method is not as significant as expected in some maps, compared with the original RRT-Connect algorithm, its total number of nodes is still reduced by more than 26.05%, and the node utilization rate is increased by more than 7.80%. This method enables the algorithm to quickly locate and flexibly search narrow channel areas, so as to quickly find feasible paths. Not only does it significantly improve the search efficiency, but it also effectively optimizes the cost and time cost of the path, achieving an overall improvement in the algorithm performance.

[0033] (3) In order to further optimize the path, this paper adopts the greedy strategy and quadratic Spline curve smoothing technology. Compared with the traditional RRT-Connect algorithm, the optimized path not only has a shorter length, but also has significantly improved smoothness.

[0034] After the above improvements, the algorithm in this paper has achieved significant improvement in overall performance compared with the RRT-Connect algorithm. Its path length is reduced by at least 4.69%, the running time is reduced by more than 46.53%, and the node utilization rate is increased to more than 30.22%. These optimization measures effectively limit the sampling space, shorten the calculation time, reduce redundant paths, and significantly improve the quality of the path. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 Flow chart of the RRT-Connect narrow channel path planning method based on the optimization of the intermediate node interpolation method and the cross-shaped method described in the present invention

[0036] Figure 2 Generate a reserved node diagram for the intermediate nodes;

[0037] Figure 3 Generate abandoned node diagrams for intermediate nodes;

[0038] Figure 4 This is a schematic diagram of the cross method;

[0039] Figure 5 Schematic diagram of node removal for collision detection;

[0040] Figure 6 Keep node diagrams for collision detection;

[0041] Figure 7 Schematic diagram for deleting redundant points on the initial path;

[0042] Figure 8 This is a schematic diagram of path smoothing;

[0043] Fig. 9 This is a schematic diagram of a simulation comparison experiment result of a 100*100 map and an RRT-Connect algorithm of a narrow channel path planning method of an improved RRT-Connect algorithm according to an embodiment of the present invention; DETAILED DESCRIPTION

[0044] The technical solutions in the embodiments of the present invention will be described in detail below in conjunction with the accompanying drawings in the embodiments of the present invention. The described embodiments are only part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by ordinary persons in the art without creative work should fall within the scope of protection of the present invention.

[0045] The embodiments of the present invention are all based on simulation verification under Pycharm2023, and the innovation and effectiveness of the present invention are verified by experiments. A large number of experiments have proved the superiority of the improved algorithm over the traditional algorithm.

[0046] A RRT-Connect narrow channel path planning method based on the optimization of the intermediate node interpolation method and the cross-shaped method mainly includes the following steps:

[0047] Initialize the state space, set the starting point and end point, and construct a bidirectional random search tree and , with the starting point and the end point as and The root node of

[0048] Evaluate and The number of nodes, select the tree with fewer nodes as the starting point for the search, and search first by default ;

[0049] In the expansion phase, the RRT-Connect algorithm randomly generates two new nodes. and If both nodes collide with obstacles, the algorithm and Insert intermediate nodes between them in order to find a feasible path channel. The coordinates are , The coordinates are ; By presetting an offset ( The value range is ), at the node and nodes Generate two intermediate nodes between and ; Intermediate node and The coordinate formula is:

[0050] (1)

[0051] (2)

[0052] Offset Determine the intermediate node and Relative to the original node and The specific location of the intermediate nodes. Then, collision detection is also performed on the two intermediate nodes to evaluate whether they can be used as potential valid path points. and At least one of them did not collide with the obstacle. and If at least one node in the process does not collide, reselect the parent node and reconnect it, and use the intermediate node that does not collide as the new node. Join the search tree. If the middle node and All of them collided with obstacles, indicating that the current path direction is not feasible, and the node is abandoned;

[0053] Select new node Finally, based on the bridge test algorithm, the cross-shaped method is proposed to explore the interior of the narrow channel. Select a random node As the center, the coordinates are , according to the prior map channel width and robot size, with a step length Generate "M"-shaped cross line segments in eight directions: right, lower right, lower, lower left, left, upper left, upper, and upper right, that is, , , , , , , , The parametric equations of each line segment are:

[0054] (3)

[0055] The algorithm will perform collision detection on these eight "M" shaped intersection line segments. If any of the following conditions is met:

[0056] (4)

[0057] That is, there is at least one pair of line segments with opposite directions (e.g. and ,or and etc.), and collides with obstacles at the same time, it can be determined that the node is indeed in a narrow passage. The search tree starts with The nearest node As a starting point, The direction extends until it encounters an obstacle or the two trees are connected;

[0058] Combining vector geometry and the concept of extended boundaries, collision detection between line segments and obstacles is performed. If the line segment formed by connecting nodes intersects with the boundary of the obstacle, it means that a collision has occurred; if the line segment formed by connecting nodes does not intersect with the boundary of the obstacle, it means that no collision has occurred.

[0059] A greedy strategy is used to remove redundant nodes on the path, starting from the starting point Start by establishing connections with the nodes after it, and check whether the line segments connected by these nodes collide with the obstacle boundary. If there is no collision, it will be used as a path directly, continue to extend to the next node and repeat this check step. Once a line segment is found to collide with an obstacle, trace back to the starting point node by node. , check whether the connection line collides, until a non-collision node is found, and record it as Repeat this process until you find a direct connection Finally, the new path node is connected to , Connect them to get the optimized path;

[0060] Secondary application Spline curve smoothing and path optimization. For each pair of adjacent nodes in the path, the previous node, the current node, and the next node are used as control points. , , For each pair of control points, by changing the parameter (The value range is ) values, multiple points on the curve can be calculated. Connect the calculated points on the curve in order to construct a quadratic Spline smoothing path. The calculation formula of spline curve is:

[0061] (5)

[0062] In order to verify the feasibility and reliability of the present invention in complex environments such as narrow passages, the algorithm of the present invention is compared with the original RRT-Connect algorithm. PyCharm2023 was used to perform simulation verification on a computer with 64-bit Windows 11 (Home and Student Edition), Intel i5-8250U processor, and 16GB memory. The simulation experiment is based on a 100×100 two-dimensional map, which distinguishes obstacles (black) and passable areas (white), and sets the starting point (blue) and target point (green). All fixed step size algorithms are used uniformly, and the maximum number of iterations is 10,000. The average initial path length and the average running time are calculated through 30 simulation experiments, and the node utilization, that is, the average ratio of the number of valid nodes to the total number of nodes, is evaluated. The simulation results are shown in the figure. Figure 8 shown.

[0063] Depend on Figure 8 It can be seen that when solving the path planning of a mobile robot in a large-scale complex environment, the algorithm of the present invention has fewer nodes, shorter paths, better path smoothing effects, and better operating efficiency.

[0064] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention may be modified or replaced by equivalents without departing from the spirit and scope of the technical solutions of the present invention, which should all be included in the scope of the claims of the present invention.

Claims

1. A RRT-Connect narrow channel path planning method based on the optimization of the intermediate node interpolation method and the cross-shaped method, characterized in that: The following steps are involved: (1) Initialize the state space, set the starting point and end point, and construct a bidirectional random search tree and , with the starting point and the end point as and The root node of (2) Evaluation and The number of nodes, select the tree with fewer nodes as the starting point for the search, and search first by default ; (3) In the expansion phase, randomly sampled nodes are obtained and , when both collide with obstacles, the offset is set based on the middle node interpolation method (The value range is ),exist and Generate two intermediate nodes between and , check whether they collide. If they do, reselect random sampling nodes. and If at least one node in the process does not collide, reselect the parent node and reconnect it, and use the intermediate node that does not collide as the new node. Add to search tree; (4) Node Continue to generate random sampling nodes Based on the bridge test algorithm, the cross-shaped method is proposed to explore the interior of the narrow channel. Centered on the a priori map channel width and robot size, with a certain step length , generate eight "M"-shaped line segments extending in different directions, and detect their collisions one by one. If there is at least one pair of line segments in opposite directions that collide with obstacles at the same time, the node can be determined. Located in a narrow passage, the search tree starts with The nearest node As a starting point, The direction extends until it encounters an obstacle or the two trees are connected; (5) If the two trees are connected, then trace back the path to get the initial path, otherwise return to step (2) and repeat the operation; (6) After obtaining the initial path, a greedy strategy is used to remove redundant nodes on the path and a secondary Spline smoothing paths.

2. According to claim 1, a RRT-Connect narrow channel path planning method based on the optimization of the intermediate node interpolation method and the cross-shaped method is characterized in that: In step (3), the intermediate node interpolation method is: set up The coordinates are , The coordinates are , preset offset , at the node and nodes Generate two intermediate nodes between and , and The coordinates of are calculated according to the following formula: (1) (2) Offset Determine the intermediate node and Relative to the original node and The specific location of and Perform collision detection, if the middle node and At least one of the nodes has not collided with the obstacle. The algorithm infers that the non-collided node is located in the entrance area of ​​the narrow passage. Based on this inference, the algorithm will guide the search tree to generate more nodes in this area. and If both nodes collide with obstacles, it means that the currently attempted path direction is not feasible. At this time, the algorithm will abandon these two nodes and continue to randomly generate new node pairs for collision detection and path exploration until a feasible path is found or the preset exploration termination condition is reached.

3. According to claim 1, a RRT-Connect narrow channel path planning method based on the optimization of the intermediate node interpolation method and the cross-shaped method is characterized in that: In step (4), the cross-hatching method is: After the random tree finds the narrow channel entrance area through the intermediate node interpolation method, a random node is selected. As the center, the coordinates are , with step length Generate "M"-shaped cross line segments in eight directions: right, lower right, lower, lower left, left, upper left, upper, and upper right, that is, , , , , , , , , the parametric equations of each line segment are: (3) The algorithm will perform collision detection on these eight "M"-shaped intersection line segments if any of the following conditions are met: (4) That is, there is at least one pair of line segments with opposite directions (e.g. and ,or and etc.), and collides with obstacles at the same time, it can be determined that the node It is indeed located in a narrow passage, and the algorithm will continue exploring in this direction.

4. According to claim 1, a RRT-Connect narrow channel path planning method based on the optimization of the intermediate node interpolation method and the cross-shaped method is characterized in that: In steps (3)-(4), the collision detection method is: Combining vector geometry with the concept of extended boundaries, collision detection between line segments and obstacles is performed. If the line segment formed by connecting the nodes intersects with the extended boundary of the obstacle, it means that a collision has occurred. If the line segment formed by connecting the nodes does not intersect with the extended boundary of the obstacle, it means that no collision has occurred.

5. According to claim 1, a RRT-Connect narrow channel path planning method based on the optimization of the intermediate node interpolation method and the cross-shaped method is characterized in that: In step (6), the greedy strategy pruning and the quadratic The strategy for spline smoothing paths is: From the starting point At the beginning, it establishes connections with the nodes behind it one by one, and checks whether the line segments connected by these nodes collide with the obstacle boundary one by one. If there is no collision, it is directly used as a path, continues to extend to the next node and repeats this check step. However, once it is found that a line segment collides with an obstacle, it traces back to the starting point node by node. , check whether the connection line collides, until a non-collision node is found, and record it as Repeat this process until you find a direct connection Node, finally, the new path node and , After the redundant points are deleted, for each pair of adjacent nodes in the path, the previous node, the current node and the next node are used as control points. , , , for each pair of control points, by changing the parameter (The value range is ) value, multiple points on the curve can be calculated, and the calculated points on the curve are connected in sequence to construct a quadratic The calculation formula of the spline smooth path is: (5)。

Images