Robot real-time online path planning method based on bidirectional search and node direction

Abstract

The invention discloses a robot real-time online path planning method based on bidirectional search and node directions, and belongs to the technical field of path planning. The online path planning method solves the problems that an existing online path planning method is low in convergence speed and poor in planned path smoothness in a complex environment. According to the invention, by introducing a bidirectional search sampling method, the efficiency of path planning in a complex scene can be significantly improved, and when the robot moves on a planned path, whether a better path exists at each position at other time can be judged in real time according to a historical search result. By introducing a weighted node selection strategy, the problem of poor smoothness of a planned path can be improved, and the quality of path planning in a complex scene is remarkably improved. The method can be applied to real-time online path planning of the robot.

Description

Technical Field

[0001] The invention belongs to the technical field of path planning, and in particular relates to a real-time online path planning method for a robot based on bidirectional search and node direction. Background Art

[0002] Motion planning is an important research field in the robotics industry (Gan Fubao, Wang Zhongyang, Lian Yinxing, et al. Mobile robot path planning method based on improved gray wolf optimization algorithm [J]. Sensors and Microsystems, 2024, 43(08): 110-113.). Its purpose is to find a safe and collision-free path from the starting state to the target state under given environment and constraints. It is widely used in complex environments such as industrial automation, drone navigation and search and rescue (CHEN L, TENG S, LIB, et al. Milestones in Autonomous Driving and Intelligent Vehicles: Survey of Surveys [J]. IEEE Transactions on Intelligent Vehicles, 2023, 8(2): 1046-1056.). In order to improve the safety and computational efficiency of path planning, researchers have proposed and improved many planning algorithms (Yan Chunlai, Xu Cong, Liu Yongshi, et al. Local path planning algorithm for intelligent vehicles in complex scenarios [J]. Sensors and Microsystems, 2024, 43(06): 161-163+168.). For example, based on the grid map, taking Dijkstra (DIJKSTRA E WA note on two problems in connexion with graphs [J]. Numerische Mathematik, 1959, 1: 269-271.) and A* (GAN X, HUO Z, LI W. DP-A*: For Path Planning of UGV and Contactless Delivery [J]. IEEE Transactions on Intelligent Transportation Systems, 2024, 25 (1): 907-919.) as examples, the configuration space is first discretized into a graph consisting of points and edges, and then the optimal path from the starting point to the target point is searched on the graph. Among them, the core idea of ​​Dijkstra is breadth-first search, which can find the optimal path. A* adopts a heuristic search strategy, which greatly reduces the search time and resource allocation.D* (LIU H, WEI X, WANG G, et al. A StarSpot Extraction Method Based on SSA-UNet for Star Sensors Under Dynamic Conditions [J]. IEEE Sensors Journal, 2023, 23 (7): 7410-7419.) is a dynamic version of A* that dynamically adjusts the path when the environment changes by incrementally updating the path based on heuristic search. The above method is an algorithm based on graph search, which usually has poor scalability and is difficult to adapt well according to the dimension of the state space and the size of the problem (KAMAT J, ORTIZ-HARO J, TOUSSAINT M, et al. BITKOMO: Combining Sampling and Optimization for Fast Convergence in Optimal Motion Planning [C] / / 2022 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS), 2022: 4492-4497.).

[0003] Sampling-based methods have attracted great attention due to their efficiency (KYAW PT, LE AV, ELARA MR, et al. Energy-Efficient Path Planning of Reconfigurable Robots in Complex Environments [J]. IEEE Transactions on Robotics, 2022, 38 (4): 2481-2494.). Fast random search trees and their variants are widely used in robot motion planning because they can quickly find feasible paths. The literature (KARAMANS, FRAZZOLI E. Sampling-based algorithms for optimal motion planning [J]. The International Journal of Robotics Research, 2011, 30 (7): 846-894.) proposed RRT*, which introduced neighbor node search and node rewiring strategies based on RRT. When the number of iterations is large enough, the path planned by RRT* is optimal. In order to speed up the convergence of the algorithm, the literature (KLEMM S, J, HERMANN A, et al. RRT*-Connect: Faster, asymptotically optimal motion planning [C] / / 2015 IEEE International Conference on Robotics and Biomimetics (ROBIO), Zhuhai, China, 2015: 1670-1677.) proposed RRT-Connect using a bidirectional search strategy, which builds two trees with the start state and the goal state as root nodes and searches simultaneously. The literature (GAMMELL JD, SRINIVASAS S, BARFOOT T D. Informed RRT*: Optimal sampling-based path planning focused via direct sampling of anadmissible ellipsoidal heuristic [C] / / 2014 IEEE / RSJ International Conference on Intelligent Robots and Systems, 2014: 2997-3004.) proposed an informed sampling Informed RRT*, which, after finding an initial path, focuses on sampling in the elliptical area that may contain the optimal path, reducing unnecessary sampling and accelerating the convergence of the algorithm. In order to achieve real-time path planning and obstacle avoidance in dynamic environments, the literature (NADERI K, J, P.RT-RRT*:A real-time path planning algorithmbased on RRT[C] / / ACM SIGGRAPH Conf.Motion Games,2015:113-118.) first proposed Real-Time RRT* (RT-RRT*), introduced an online reconnection strategy for the tree, allowing the tree root to move with the agent without discarding the previously sampled path. On this basis, the literature (ARMSTRONG DW, JONASSON A.AM-RRT*:InformedSampling-based Planning with Assisting Metric[C] / / 2021IEEE InternationalConference on Robotics and Automation(ICRA),2020:10093-10099.) used auxiliary distance metrics to improve the sampling method of RT-RRT*, proposed Assisted Metric RRT*(AM-RRT*), and realized online path planning in complex and dynamic environments. However, this online path planning method still has problems such as slow convergence speed and poor smoothness of the planned path in complex environments. Therefore, the planning performance needs to be improved, especially in terms of search time and path quality. Summary of the invention

[0004] The purpose of the present invention is to solve the problems of slow convergence speed and poor smoothness of the planned path in existing online path planning methods in complex environments, and to propose a robot real-time online path planning method based on bidirectional search and node direction.

[0005] The technical solution adopted by the present invention to solve the above technical problems is: a real-time online path planning method for a robot based on bidirectional search and node direction, the method specifically comprising the following steps:

[0006] Step 1: Start searching and expanding from the starting point and the target point of the robot at the same time, and merge the starting tree and the target tree obtained by searching and expanding to obtain the planned path from the starting point to the target point, and continue searching and expanding the starting tree;

[0007] Step 2: The robot moves from the starting point to the target point according to the planned path;

[0008] (1) If the robot is moving from the starting point to the target point along the planned path, according to the historical search expansion results in the global map, there are other paths from the current point on the planned path to the target point. i, i = 1, 2, ..., m, m is the total number of other paths from the current point to the target point;

[0009] Then compare t i and the size of t, where t i Is the robot on path l i The time required for the robot to move from the current point to the target point on the planned path is t, and the time required for the robot to move from the current point to the target point on the planned path is t. The path corresponding to the minimum time is taken as the newly selected path, and the robot moves toward the target point along the newly selected path and continues to execute step 3.

[0010] (2) If, in the process of the robot continuously moving from the starting point to the target point along the planned path, according to the historical search expansion results in the global map, there is no other path from the current point position on the planned path to the target point, then the robot continues to move along the planned path to the target point and continues to execute step 3;

[0011] Step 3: As the robot moves toward the target point on the path, it repeats the process of step 2.

[0012] Furthermore, in step 1, the method of searching and expanding from the starting point of the robot is the same as the method of searching and expanding from the target point of the robot;

[0013] Take the search expansion from the robot's starting point as an example:

[0014] Step 1: define the tree obtained by searching and expanding from the starting point of the robot as the starting tree, and initialize it at time t=0;

[0015] Step 1 and 2: Obtain point set X through sampling s , the starting tree is in the point set X s Random sampling is performed within to obtain the state x rand , and then find the state x from the starting tree rand The closest node x est ;

[0016] Step 13: From node x est To state x rand Direction guides the generation of new node x new , and find the new node x on the starting tree new The neighboring node set X near , that is, find the new node x on the starting tree new The distance is less than the threshold r s All nodes of

[0017] If the following conditions (1) or (2) are met, then from the neighboring node set X near Select the new node xnew The node with the smallest distance will be selected as the new node x new The parent node of the new node x new Add to the starting tree; then execute steps 1 and 4;

[0018] Condition (1): Neighboring node set X near The number of nodes in is less than the preset threshold k max ;

[0019] Condition (2): From node x est To state x rand The distance is greater than the preset threshold s max ;

[0020] If condition (1) and condition (2) are not met, then directly execute step 14;

[0021] Step 14: Determine whether time t is equal to n·t s , where n is a positive integer, t s is the set time step;

[0022] If t equals n·t s , then determine whether the starting tree and the target tree meet the merging condition. If they do, execute step 15;

[0023] If t is not equal to n·t s , then return to step 1 and 2;

[0024] Step 15: After merging the starting tree and the target tree, backtrack through the merged tree through the BackTrack function to obtain a planned path from the starting point to the target point.

[0025] Furthermore, the point set X is obtained by sampling s The method is:

[0026]

[0027] Among them, X s is the point set obtained by sampling, p is a random number defined between [0,1], x goal is the target point, T s is the starting tree, X free It is the area where passage is allowed, and InformedArea is the elliptical informed sampling area.

[0028] Further, the neighboring node set X near Select the node x that matches the new node new The node with the smallest distance is as follows:

[0029] For the neighboring node set X near The non-starting point n in i , n i With the new node x new The distance is calculated as:

[0030] The node n i The coordinates of i ,y i ], node n on the starting tree i The parent node is n j , parent node n j The coordinates of j ,y j ], new node x new The coordinates of new ,y new ], then node n i With the new node x new The distance d w_c for:

[0031] d w_c =d δ +d e

[0032] Among them, d e is node n i With the new node x new The Euclidean distance, d δ is node n i With the new node x new Direction similarity;

[0033] For the neighboring node set X near The starting point in the new node x new Distance d′ w_c The calculation method is:

[0034] d′ w_c =d′ e

[0035] Where d′ e is the starting point and the new node x new The Euclidean distance of

[0036] Calculate the neighboring node set X respectively near Each node in the new After the distance, select the node with the smallest distance in the neighboring node set X near The corresponding node in .

[0037] Furthermore, the node n i With the new node x new The Euclidean distance de for:

[0038]

[0039] Furthermore, the node n i With the new node x new The direction similarity d δ for:

[0040]

[0041] Where d1 is node n i The direction vector of the new node x new The direction vector of .

[0042] Furthermore, the node n i The direction vector d1 is:

[0043] d1=[x i -x j ,y i -y j ].

[0044] Furthermore, the new node x new The direction vector d2 is:

[0045] d2=[x new -x i ,y new -y i ].

[0046] Furthermore, the merging condition in step 14 is: the distance between the starting tree and the target tree is not greater than a threshold τ.

[0047] Furthermore, when the robot moves toward the target point, if it needs to reach a new target point, the robot's current position is used as the starting point and the new target point is used as the target point, and a new search is performed to expand the path from the starting point to the target point.

[0048] The beneficial effects of the present invention are:

[0049] The present invention can significantly improve the efficiency of path planning in complex scenarios by introducing a two-way search sampling method, and when the robot moves on the planned path, it can judge in real time whether there are other more optimal paths at each location based on historical search results. By introducing a weighted node selection strategy, the problem of poor smoothness of the planned path can be improved, and the quality of path planning in complex scenarios can be significantly improved.

[0050] Comparative experiments were conducted in different scenarios, and the experimental results showed that the proposed method performed well in blank, trap, maze and office scenarios, especially significantly outperforming RT-RRT* and AM-RRT* in search time, with an average reduction of 74.3% and 64.7%. This proves the potential of the proposed method in practical applications, especially in situations that require fast response and efficient path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 It is a schematic diagram of bidirectional tree growth;

[0052] Figure 2 This is a schematic diagram of the result of merging two-way trees;

[0053] Figure 3 It is a schematic diagram of the node selection strategy;

[0054] Figure 4 It is a blank scene;

[0055] Figure 5 It’s a trap scenario;

[0056] Figure 6 It’s a maze scene;

[0057] Figure 7 It’s an office scene;

[0058] Figure 8 is the path length ratio and search time of the three algorithms in the blank scenario;

[0059] Fig. 9 is the path length ratio and search time of the three algorithms in the trap scenario;

[0060] Fig.10 is the path length ratio and search time of the three algorithms in the maze scenario;

[0061] Fig.11 is the path length ratio and search time of the three algorithms in the office scenario;

[0062] Fig.12 It is a fused collision-free path;

[0063] Fig.13 This is a schematic diagram after adding an obstacle;

[0064] Fig.14 This is a schematic diagram after adding two obstacles;

[0065] Fig.15 is a feasible path to avoid obstacles. DETAILED DESCRIPTION

[0066] Specific implementation method 1: This implementation method is a real-time online path planning method for a robot based on bidirectional search and node direction, and the method is specifically as follows:

[0067] Step 1: Start searching and expanding from the starting point and the target point of the robot at the same time, and merge the starting tree and the target tree obtained by searching and expanding to obtain the planned path from the starting point to the target point, and continue searching and expanding the starting tree;

[0068] Step 2: The robot moves from the starting point to the target point according to the planned path;

[0069] (1) If the robot is moving from the starting point to the target point along the planned path, according to the historical search expansion results in the global map, there are other paths from the current point on the planned path to the target point. i , i = 1, 2, ..., m, m is the total number of other paths from the current point to the target point;

[0070] Then compare t i and the size of t, where t i Is the robot on path l i The time required for the robot to move from the current point to the target point on the planned path is t, and the time required for the robot to move from the current point to the target point on the planned path is t. The path corresponding to the minimum time is taken as the newly selected path, and the robot moves toward the target point along the newly selected path and continues to execute step 3.

[0071] (2) If, in the process of the robot continuously moving from the starting point to the target point along the planned path, according to the historical search expansion results in the global map, there is no other path from the current point position on the planned path to the target point, then the robot continues to move along the planned path to the target point and continues to execute step 3;

[0072] Step 3: When the robot moves toward the target point on the path, the process of step 2 is repeated (the only difference is that if a new path is selected in the previous step, the judgment is made based on the new path this time).

[0073] The robot first moves toward the target point on the planned path. At each position on the path, it determines whether there are other paths from the current position to the target point. If there are other paths, each other path and the planned path are used as an alternative path. The time required from the current position to the target point on each alternative path is compared, and the path with the least time is selected from them. The robot moves from the current position to the target point on the selected path. If there is no other path from the current position to the target point, it continues to move on the planned path. If the robot does not choose other paths, it repeats the above judgment process after moving to the new position. If the robot chooses other paths, it also repeats the above judgment process after moving to the new position. The only difference is that when the robot chooses other paths, the judgment is made from the new position on the other paths. What needs to be compared is the movement time required for each path from the new position to the target point, that is, the original planned path is no longer involved in the comparison.

[0074] Moreover, if there are dynamic obstacles in the global map and the robot may encounter obstacles while moving along the path, the robot will autonomously avoid the obstacles and re-plan the path with the current position as the starting point or select a path that can reach the target point based on the historical search expansion results (since the starting tree continues to expand after the starting tree and the target tree are merged, and the historical expansion results are retained in the global map, they can be used for subsequent path selection).

[0075] The search process of the method of the present invention is as follows Figure 1 and Figure 2 As shown in the figure, starting from the starting point, the starting tree search process gradually expands the tree structure to explore the reachable state. At the same time, the target tree starts to search and expand from the end point to explore the reverse reachable state. During the search and expansion process, the starting point is assigned to the root of the starting tree, and the target point is set to the target point of the starting tree. Similarly, the target point is set to the root of the target tree, and the root of the starting tree is set to the target point of the target tree.

[0076] like Figure 1 As shown in , the reachable paths searched by the two are represented as dark and light colors respectively. Once they meet, the starting tree merges the target tree into itself and quickly builds a path to the target tree based on the information of the target tree. The target tree stops growing and is initialized, as shown in Figure 2 shown.

[0077] Specific implementation method 2: This implementation method is different from specific implementation method 1 in that, in step 1, the method of searching and expanding from the starting point of the robot is the same as the method of searching and expanding from the target point of the robot;

[0078] Take the search expansion from the robot's starting point as an example:

[0079] Step 1: define the tree obtained by searching and expanding from the starting point of the robot as the starting tree, and initialize it at time t=0;

[0080] Step 1 and 2: Obtain point set X through sampling s , the starting tree is at the point set x s Random sampling is performed within to obtain the state x rand , and then find the state x from the starting tree rand The closest node x est ;

[0081] When the first search expansion is performed, since there is only the starting point in the starting tree, at this time, it is consistent with the state x rand The closest node x est is the starting point, state x rand The distance from the starting point in the starting tree does not take into account the direction, and the state x rand The distance to other nodes in the starting tree needs to take into account the direction;

[0082] Step 13: From node x est To state x rand Direction guides the generation of new node x new , and find the new node x on the starting tree mew The neighboring node set X near , that is, find the new node x on the starting tree new The distance of the new node x new The distance from the starting point in the original tree does not take into account the direction, and the new node x new The distance to other nodes in the starting tree (needs to consider the direction) is less than the threshold r s All nodes (r s The value can be set according to the actual situation);

[0083] If the following conditions (1) or (2) are met, then from the neighboring node set X near Select the node x that matches the new node new The node with the smallest distance will be selected as the new node x new The parent node of the new node x new Add to the starting tree (that is, in the starting tree, add the new node x new The parent node and the new node x new Connect); then execute step 1-4;

[0084] Condition (1): Neighboring node set X near The number of nodes in is less than the preset threshold k max ;

[0085] Condition (2): From node x est To state x randThe distance is greater than the preset threshold s max ;

[0086] If condition (1) and condition (2) are not met, then directly execute step 14;

[0087] Step 14: Determine whether time t is equal to n·t s , where n is a positive integer, t s is the set time step;

[0088] If t equals n·t s , then determine whether the starting tree and the target tree meet the merging condition. If they do, execute step 15;

[0089] If t is not equal to n·t s , then return to step 1 and 2;

[0090] Step 15: After merging the starting tree and the target tree, backtrack through the merged tree through the BackTrack function to obtain a planned path from the starting point to the target point.

[0091] The other steps and parameters are the same as those in the first embodiment.

[0092] Specific implementation method three: This implementation method is different from specific implementation methods one or two in that the point set X is obtained by sampling s The method is:

[0093]

[0094] Among them, X s is the point set obtained by sampling, p is a random number defined between [0,1], x goal is the target point, T s is the starting tree, X free It is the area where passage is allowed, and InformedArea is the elliptical informed sampling area.

[0095] The other steps and parameters are the same as those in the first or second embodiment.

[0096] Specific implementation method 4: This implementation method is different from any one of the specific implementation methods 1 to 3 in that the neighboring node set X near Select the node x that matches the new node new The node with the smallest distance is as follows:

[0097] For the neighboring node set X near The non-starting point n in i , n i With the new node x new The distance is calculated as:

[0098] The node n i The coordinates of i ,y i ], node n on the starting tree i The parent node is n j , parent node n j The coordinates of j ,y j ], new node x new The coordinates of new ,y new ], then node n i With the new node x new The distance d w_c for:

[0099] d w_c =d δ +d e

[0100] Among them, d e is node n i With the new node x new The Euclidean distance, d δ is node n i With the new node x new Direction similarity;

[0101] For the neighboring node set X near The starting point in the new node x new Distance d′ w_c The calculation method is:

[0102] d′ w_c =d′ e

[0103] Where d′ e is the starting point and the new node x new The Euclidean distance of

[0104] Calculate the neighboring node set X respectively near Each node in the new After the distance, select the node with the smallest distance in the neighboring node set x near The corresponding node in .

[0105] The other steps and parameters are the same as those in Specific Embodiments 1 to 3.

[0106] The improved candidate node selection strategy in this implementation is as follows: Figure 3 As shown in Figure 1, this is the weighted nearest node selection strategy. This strategy not only considers the distance between nodes when selecting candidate nodes, but also introduces the directionality between nodes (ξ1, ξ2, ξ3). Using the weighted distance dw_c To measure the sampling point x new The comprehensive distance to the surrounding tree nodes can generate a smoother and more efficient path, reduce unnecessary turns and adjustments, and improve the overall path planning quality. near There may not be both starting points and non-starting points in the graph, but this does not affect the method of this embodiment. It is only necessary to calculate the neighboring node set X. near The node type in the calculation can be used.

[0107] Specific implementation mode 5: This implementation mode is different from any one of the specific implementation modes 1 to 4 in that the node n i With the new node x new The Euclidean distance d e for:

[0108]

[0109] The other steps and parameters are the same as those in Specific Embodiments 1 to 4.

[0110] Specific implementation method 6: This implementation method is different from any one of the specific implementation methods 1 to 5 in that the node n i With the new node x new The direction similarity d δ for:

[0111]

[0112] Where d1 is node n i The direction vector of the new node x new The direction vector of .

[0113] The other steps and parameters are the same as those in Specific Implementation Methods 1 to 5.

[0114] Specific implementation method 7: This implementation method is different from any one of the specific implementation methods 1 to 6 in that the node n i The direction vector d1 is:

[0115] d1=[x i -x j ,y i -y j ]

[0116] The other steps and parameters are the same as those in Specific Embodiments 1 to 6.

[0117] Specific implementation eight: This implementation differs from any one of specific implementations one to seven in that the new node x new The direction vector d2 is:

[0118] d2=[x new-x i ,y new -y i ]

[0119] The other steps and parameters are the same as those in Specific Embodiments 1 to 7.

[0120] Specific implementation method 9: This implementation method is different from any one of specific implementation methods 1 to 8 in that the merging condition in step 14 is that the distance between the starting tree and the target tree is not greater than a threshold value τ.

[0121] The other steps and parameters are the same as those in Specific Embodiments 1 to 8.

[0122] For any node on the starting tree, this implementation first calculates the Euclidean distance between the node and each node on the target tree, then traverses each node on the starting tree, and finally selects the minimum distance among all distances, compares the minimum distance with the threshold τ, and the thresholds in the present invention can be set according to actual conditions.

[0123] Specific implementation ten: This implementation differs from any one of specific implementations one to nine in that, during the process of the robot moving toward the target point, if it needs to reach a new target point, the robot's current position is used as the starting point and the new target point is used as the target point, and a new search is performed to expand the path from the starting point to the target point.

[0124] The other steps and parameters are the same as those in Specific Embodiments 1 to 9.

[0125] Experimental simulation and analysis

[0126] In order to evaluate the efficiency of the method (Fast AM-RRT*) of the present invention, a detailed comparative test was conducted between the method of the present invention and RT-RRT* and AM-RRT*. All simulation experiments were performed on an Intel(R) Core(TM) i9-12900H 2.5GHz CPU16GBRAM device, and the simulation platform was PyCharm 2022. The experimental scenario is as follows Figures 4 to 7 As shown in the figure, the size of the office scene is 200×200m, and the size of the other scenes is 100×100m. In order to reduce accidental errors and improve data reliability, each experimental scene was repeated 30 times and the average value was taken. In addition, for consistency, for all algorithms, their t s , k max and r s They are set to 0.15s, 20m and 5m respectively. For the method of the present invention, the parameter τ is set to 30m. For each scene, the robot passes through target points 1 to 5 in sequence starting from S, and the time taken by each algorithm to plan each path and the path length from the previous target point to the next target point are calculated.

[0127] The experimental results are as follows Figures 8 to 11 As shown, the path length ratio refers to the ratio of the path length generated by the algorithm to the theoretical shortest path length. The closer the path length ratio is to 1, the better the path generated by the algorithm. Compared with RT-RRT* and AM-RRT*, the method of the present invention has obvious improvements in path length and search time. On average, the path length and search time of the method of the present invention are reduced by 11.2% and 74.3% respectively compared with RT-RRT*, and the path length and search time of AM-RRT* are reduced by 4.8% and 64.7% respectively.

[0128] In the blank scene, the path length ratios of RT-RRT*, AM-RRT* and the method of the present invention are all close to 1, and the search time is basically below 0.2s. In the absence of obstacles, all three algorithms can generate near-optimal paths in a short time. However, due to the use of a two-way search strategy, the search time of the method of the present invention is reduced by 34.9% and 20% respectively compared with the other two algorithms. In the trap scene, the path length of the method of the present invention is reduced by an average of 9.9% and 6.7% compared with RT-RRT* and AM-RRT*. In terms of time, due to the setting of the trap, the robot spends more time from the starting point to the target point 1 and from the target point 2 to 3. In this scenario, the method of the present invention benefits from the two-way search strategy, and the time consumption is reduced by 80% and 73.4% compared with RT-RRT* and AM-RRT*, respectively. In the complex maze scene, due to the large number of obstacles and strong randomness, the path length ratios and time consumption of the three algorithms are significantly different. Overall, the method of the present invention is still better than RT-RRT* and AM-RRT*.

[0129] The office scene combines the characteristics of the trap environment and the maze environment, and the path length ratios of the three algorithms all show certain fluctuations. Especially from target point 2 to 3 and target point 4 to 5, the path ratios of the three algorithms all reached above 1.25. In addition, since target points 2 and 3 are both in the corner of the office and are difficult to cover during the search, the time spent on these two targets basically reached more than 5 seconds. Even in such a complex scenario, the method of the present invention reduces the path length by 12.2% and 1.7% respectively, and the search time by 71.6% and 61.8% respectively compared to RT-RRT* and AM-RRT*.

[0130] In order to test the dynamic obstacle avoidance performance of the algorithm proposed in this invention, a test was conducted in a maze environment where obstacles can be added at any time. Obstacles are simulated by black solid circles, and can be added to any position on the map according to user input. Fig.12 As shown, the starting tree and the target tree are fused to construct a collision-free path, and the paths are added one by one. Fig.13 and Fig.14The method of the present invention can use the information of the surrounding trees to quickly construct another feasible path to avoid the obstacle. The final feasible path is as follows: Fig.15 shown.

[0131] The above calculation examples of the present invention are only used to explain the calculation model and calculation process of the present invention in detail, and are not intended to limit the implementation methods of the present invention. For ordinary technicians in the relevant field, other different forms of changes or modifications can be made based on the above description. It is impossible to list all the implementation methods here. All obvious changes or modifications derived from the technical solution of the present invention are still within the scope of protection of the present invention.

Claims

1. A robot real-time online path planning method based on bidirectional search and node direction, characterized in that: The method specifically comprises the following steps: Step 1: Start searching and expanding from the starting point and the target point of the robot at the same time, and merge the starting tree and the target tree obtained by searching and expanding to obtain the planned path from the starting point to the target point, and continue searching and expanding the starting tree; Step 2: The robot moves from the starting point to the target point according to the planned path; (1) If the robot is moving from the starting point to the target point along the planned path, according to the historical search expansion results in the global map, there are other paths from the current point on the planned path to the target point. i , i = 1, 2, ..., m, where m is the total number of other paths from the current point to the target point; Then compare t i and the size of t, where t i Is the robot on path l i The time required for the robot to move from the current point to the target point on the planned path is t, and the time required for the robot to move from the current point to the target point on the planned path is t. The path corresponding to the minimum time is taken as the newly selected path, and the robot moves toward the target point along the newly selected path and continues to execute step 3. (2) If, as the robot moves continuously from the starting point to the target point along the planned path, according to the historical search expansion results in the global map, there is no other path from the current point position on the planned path to the target point, the robot continues to move toward the target point along the planned path and continues to execute step 3; Step 3: As the robot moves toward the target point on the path, it repeats the process of step 2.

2. A robot real-time online path planning method based on bidirectional search and node direction according to claim 1, characterized in that: In the step 1, the method of searching and expanding from the starting point of the robot is the same as the method of searching and expanding from the target point of the robot; Take the search expansion from the robot's starting point as an example: Step 1: define the tree obtained by searching and expanding from the starting point of the robot as the starting tree, and initialize it at time t=0; Step 1 and 2: Obtain point set X through sampling s , the starting tree is in the point set X s Random sampling is performed within to obtain the state x rand , and then find the state x from the starting tree rand The closest node x est ; Step 13: From node x est To state x rand Direction guides the generation of new node x new , and find the new node x on the starting tree new The neighboring node set X near , that is, find the new node x on the starting tree new The distance is less than the threshold r s All nodes of If the following conditions (1) or (2) are met, then from the neighboring node set X near Select the node x that matches the new node new The node with the smallest distance will be selected as the new node x new The parent node of the new node x new Add to the starting tree; Then execute step 14; Condition (1): Neighboring node set X near The number of nodes in is less than the preset threshold k max ; Condition (2): From node x est To state x rand The distance is greater than the preset threshold s max ; If condition (1) and condition (2) are not met, then directly execute step 1-4; Step 14: Determine whether time t is equal to n·t s , where n is a positive integer, t s is the set time step; If t equals n·t s , then determine whether the starting tree and the target tree meet the merging condition. If they do, execute step 15; If t is not equal to n·t s , then return to step 1 and 2; Step 15: After merging the starting tree and the target tree, backtrack through the merged tree through the BackTrack function to obtain a planned path from the starting point to the target point.

3. A robot real-time online path planning method based on bidirectional search and node direction according to claim 2, characterized in that: The point set X is obtained by sampling s The method is: Among them, X s is the point set obtained by sampling, p is a random number defined between [0,1], x goal is the target point, T s is the starting tree, X free It is the area where passage is allowed, and InformedArea is the elliptical informed sampling area.

4. A robot real-time online path planning method based on bidirectional search and node direction according to claim 3, characterized in that: The neighboring node set X near Select the new node x new The node with the smallest distance, the specific process is: For the neighboring node set X near The non-starting point n in i , n i With the new node x new The distance is calculated as: The node n i The coordinates are [x i ,y i ], node n on the starting tree i The parent node is n j , parent node n j The coordinates are [x j ,y j ], new node x new The coordinates are [x new ,y new ], then node n i With the new node x new The distance d w_c for: d w_c =d δ +d e Among them, d e is node n i With the new node x new The Euclidean distance, d δ is node n i With the new node x new Direction similarity; For the neighboring node set X near The starting point in the new node x new Distance d′ w_c The calculation method is: d′ w_c =d′ e Among them, d′ e is the starting point and the new node x new The Euclidean distance of Calculate the neighboring node set X respectively near Each node in the new After the distance, select the node with the smallest distance in the neighboring node set X near The corresponding node in .

5. A robot real-time online path planning method based on bidirectional search and node direction according to claim 4, characterized in that: The node n i With the new node x new The Euclidean distance d e for:

6. A robot real-time online path planning method based on bidirectional search and node direction according to claim 5, characterized in that: The node n i With the new node x new The direction similarity d δ for: Where d1 is node n i The direction vector of the new node x new The direction vector of .

7. A robot real-time online path planning method based on bidirectional search and node direction according to claim 6, characterized in that: The node n i The direction vector d1 is: d1=[x i -x j ,y i -y j ]。 8. A robot real-time online path planning method based on bidirectional search and node direction according to claim 7, characterized in that: The new node x new The direction vector d2 is: d2=[x new -x i ,y new -y i ]。 9. A robot real-time online path planning method based on bidirectional search and node direction according to claim 8, characterized in that: The merging condition in step 1-4 is: the distance between the starting tree and the target tree is not greater than the threshold τ.

10. A robot real-time online path planning method based on bidirectional search and node direction according to claim 9, characterized in that: During the movement of the robot toward the target point, if a new target point needs to be reached, the current position of the robot is used as the starting point and the new target point is used as the target point, and a new search is performed to expand the path from the starting point to the target point.

Images