Robot path planning method based on parallel sampling point optimization RRT algorithm

Abstract

The invention discloses a robot path planning method based on a parallel sampling point optimization RRT algorithm. The robot path planning method mainly aims at the problems that the RRT algorithm is low in random tree search efficiency, the path is zigzag and discontinuous, and the algorithm adaptability to different environments is low. The method comprises the following steps: firstly, randomly sampling a plurality of nodes by adopting a multipoint parallel weighted heuristic sampling strategy, establishing an evaluation function introducing a distance factor and a corner factor, and selecting an optimal sampling node for expansion; a dual-tree guiding mechanism is constructed, information of newly-generated nodes is fully utilized, invalid sampling of a local space is reduced, and rapid fusion of the two trees is achieved; determining an extension step length according to the surrounding environment information of the node to obtain a collision-free path with low cost from a starting point to a target point; and then global and local rewiring optimization is performed on the initial path, a B spline curve is introduced for fitting smoothing, and finally an optimized path satisfying robot curvature constraint is generated. According to the method, the planning efficiency and the path quality can be improved, and the method has relatively high stability and robustness for different environments.

Description

Technical Field

[0003] The present invention relates to the technical field of robot autonomous navigation, and in particular to a robot path planning method based on a parallel sampling point optimization RRT algorithm. Background Art

[0005] In recent years, advances in artificial intelligence technology have led to the application of more and more autonomous mobile robots in various fields, such as transportation, manufacturing, and medical industries. Path planning is a key technology and main challenge for mobile robots. The path planning problem is to plan a global collision-free path from the starting point to the target point that meets the robot's motion constraints in a space with obstacles based on certain evaluation criteria (planning time and path length, etc.).

[0006] According to different theories, robot path planning algorithms can be divided into three categories: (1) Graph-based planning algorithms, such as Dijkstra and A* algorithms, can find the optimal path, but the computational complexity grows exponentially in complex or high-resolution environments; (2) Artificial potential field-based planning algorithms, which seek paths by minimizing potential energy functions. This method may fall into local minima, resulting in path planning failure; (3) Sampling-based planning algorithms, such as the rapidly-exploring random tree (RRT) algorithm, which generates random sampling points to explore unknown space, connects a set of sampled points in the obstacle-free space, and constructs a feasible path.

[0007] The RRT algorithm avoids geometric modeling of space, has a fast search speed, and is probabilistically complete. It is widely used in path planning problems such as mobile robots, drones, and robotic arms. However, the randomness and blindness of sampling will cause a large number of invalid sampling nodes, resulting in an increase in the number of iterations. The searched path contains a large number of redundant nodes, the path is tortuous and discontinuous, and the planned path does not have asymptotic optimality. For map environments of different scales, there is still much room for improvement in the algorithm planning efficiency and path quality. Summary of the invention

[0009] The purpose of the present invention is to provide a robot path planning method based on a parallel sampling point optimization RRT algorithm in view of the deficiencies in the prior art, utilize a multi-point parallel weighted heuristic sampling strategy to reduce invalid sampling nodes, and combine an evaluation function in the sampling process to reduce the blindness of random sampling and improve the sampling quality. A dynamic step extension strategy and a dual-tree guidance mechanism are adopted to make the random tree expansion flexible and directional, accelerate the convergence of the algorithm, and after the initial path of the algorithm is generated, rewire the path for optimization, reduce the path length and curvature, and improve the path quality.

[0010] To achieve the above object, the present invention provides the following technical solutions:

[0011] A robot path planning method based on parallel sampling point optimization RRT algorithm includes the following steps:

[0012] Step 1, define X free and X obs are free space and obstacle space respectively, x init is the starting node of the robot’s motion in the state space, x goal For the target node, create the starting tree T1 and the end tree T2, and set the algorithm parameters;

[0013] Step 2: For the starting tree T1, randomly sample multiple nodes in parallel in the state space. The number of parallel sampling points is determined by the map complexity. The parallel sampling points are screened, and the sampling points that fall into obstacles are discarded. The distance and angle metrics are normalized, and an evaluation function containing distance constraints and angle constraints is established. The evaluation function is used to select the optimal node for expansion of the remaining sampling points, and the coordinates of the optimal node and the expansion direction of the new node are determined.

[0014] Step 3: Detect the obstacle distribution information around the nearest node, dynamically select the expansion step, take the nearest node as the starting endpoint, extend the step toward the optimal node, and obtain a new expansion node in the starting tree T1;

[0015] Step 4: Connect the new extended node and the nearest node to perform midpoint collision detection, discretize the path segments, and after obtaining all discrete path points, determine whether they are on the obstacle. If so, return to step 2. If not, add the new node to the random tree T1.

[0016] Step 5, for the terminal tree T2, randomly sample multiple nodes in parallel in the sampling space, and use the newly generated extended node in the starting tree T1 as the local target point of the terminal tree T2, provide heuristics for selecting the optimal node for the terminal tree T2, determine the coordinates of the optimal node, and perform intermediate point collision detection and dynamically select step size expansion through steps 3 and 4.

[0017] Step 6: Repeat steps 2 to 5 until the number of newly generated nodes of the starting tree T1 and the ending tree T2 is less than the connection threshold, connect the two points, and trace back to the starting point and the target point with the newly generated nodes of the two trees as the starting endpoints to obtain the initial path. Perform global path replanning and local path replanning on the initial path, filter out non-critical nodes, and improve the quality of the initial path.

[0018] Step 7, for the optimized path obtained in step 6, control points are constructed between adjacent nodes of the path, the control point construction method is selected by setting a threshold, and the path of the constructed control points is fit-optimized using a cubic B-spline curve to generate a smooth path that meets the robot's maximum curvature constraint and has a high degree of fit.

[0019] Specifically, define the free space, obstacle space, starting point and target point as described in step 1, and create two random trees. The process is as follows:

[0020] Step 1.1: Import map data and perform binary processing on the map;

[0021] Step 1.2: Set the starting point coordinate x of the robot movement init and the end point coordinate x goal , construct two random trees with the starting point and the target point as the root nodes respectively, and define the connection threshold of the two random trees.

[0022] Furthermore, the algorithm parameter values ​​in step one include a distance factor, an angle factor, a safety distance and a random tree expansion step size.

[0023] Specifically, the process of multi-point parallel weighted heuristic sampling of the starting tree T1 and selecting the optimal node for expansion in step 2 is as follows:

[0024] Step 2.1: Sample multiple nodes in parallel in the state space. The number of parallel sampling points is determined by the map complexity. After a lot of experimental research and analysis, it is found that the best planning effect is to sample three nodes in a simple environment and five nodes in a complex environment. Determine whether the parallel sampling node is within the obstacle range, discard the sampling point that falls into the obstacle, and find the node x in the random tree branch that is closest to the remaining sampling points after screening. near ;

[0025] Step 2.2: Determine x near The Euclidean distance d to the target point n , the Euclidean distance D from the starting point to the target point n , three adjacent nodes x parent 、x near and x rand Angle T n , determine the Euclidean distances A(x) and N(x) from the remaining sampling points to the starting point and the target point after screening, and establish the evaluation function F1(x) of the starting tree T1;

[0026] Step 2.3: Calculate the evaluation function values ​​of the remaining sampling points after screening, select the sampling point with the smallest evaluation function, and determine the coordinates of the optimal node and the new node x new direction of expansion.

[0027] Specifically, the process of detecting the obstacle distribution information around the nearest node and dynamically selecting the expansion step size described in step 3 is as follows:

[0028] Step 3.1: Draw a graph with the nearest node x nearest is the center of the circle, the safety distance R safe_dis1 and R safe_dis2 A circle with a radius of , extract the points in each area on the circle;

[0029] Step 3.2: Check whether the point set on the circle is outside the state space and on the obstacle, and dynamically select the expansion step size L step1 , L step2 and L step3 , taking the nearest node as the starting endpoint, extending the step length toward the optimal node, and obtaining a new extended node in the starting tree T1;

[0030] Specifically, the process of connecting the optimal node and the nearest node to perform midpoint collision detection in step 4 is as follows:

[0031] Step 4.1: Connect the new extended node with the nearest node to obtain all discrete path points at the minimum resolution of the map;

[0032] Step 4.2: After obtaining all discrete path points, determine whether they are on the obstacle. If so, return to step 2 and re-sample in parallel in space. Otherwise, add the new expansion node to the random tree T1.

[0033] Specifically, the newly generated extended node in the starting tree T1 in step 5 is used as the local target point of the terminal tree T2, and the process of providing a heuristic for selecting the optimal node for the terminal tree T2 is as follows:

[0034] Step 5.1: Sample multiple nodes in parallel in the state space. The number of parallel sampling points is determined by the map complexity. In a simple environment, three nodes are sampled in parallel, and in a complex environment, five nodes are sampled in parallel. Determine whether the parallel sampling node is within the obstacle range, discard the sampling points that fall into the obstacle, and find the node x in the random tree branch that is closest to the remaining sampling points after screening. near ;

[0035] Step 5.2: Determine the newly generated node x of the starting tree T1 T1new The Euclidean distance L to the target point n , x near to x T1new The Euclidean distance l n , three adjacent nodes x parent 、x near and x rand Angle T n , determine the remaining sampling points after screening to x T1newThe Euclidean distance S(x) and B(x) of the target point are used to establish the evaluation function F2(x) of the terminal tree T2. The evaluation function values ​​of the remaining sampling points after screening are calculated respectively, and the sampling point with the smallest evaluation function is selected from them. The expansion step size is dynamically selected according to step 3. According to step 4, the intermediate point collision detection is performed to determine whether it is on the obstacle. If it is on the obstacle, return to step 2. If not, the new node is added to the random tree T2.

[0036] Specifically, the process of performing global path replanning and local path replanning on the initial path in step 6 is as follows:

[0037] Step 6.1: Repeat steps 2 to 5 until the newly generated nodes of the starting tree T1 and the ending tree T2 coincide or are less than the set connection threshold, connect the two points, and trace back to the starting point and the target point with the newly generated nodes of the two trees as the starting endpoints to obtain the initial path.

[0038] Step 6.2: Use the end point as the starting endpoint of the line segment to trace back the initial path, connect the subsequent path nodes in sequence, and check whether the connected path segment collides with the obstacle. If no collision occurs, delete all path nodes between the two nodes; if a collision occurs, use the child node of the node as the starting endpoint of the new line segment, repeat the above operation, and trace back to the starting point;

[0039] Step 6.3: Interpolate the newly generated path segment, with the interpolation distance as the minimum step length, and obtain a path with denser nodes for local path replanning. Take the starting point as the starting endpoint of the line segment, traverse the subsequent nodes in turn, and check whether the line segment collides with obstacles. If there is no collision, delete all path nodes between the two nodes. Otherwise, perform the above operation again with the parent node of the node as the starting endpoint of the line segment until traversing to the target point.

[0040] Specifically, the process of constructing control points between adjacent nodes in step 7, selecting a control point construction method by setting a threshold, and fitting the path of the constructed control points using a B-spline curve is as follows:

[0041] Step 7.1: Construct control points between adjacent nodes, set a threshold to select the control point construction method, and determine whether the length of the path segment between two adjacent nodes is less than the threshold. If it is less than the threshold, construct a control point at the midpoint of the path segment. Otherwise, take the two nodes as the starting points and take the normal step length to construct a control point.

[0042] Step 7.2: Take the constructed control points and the optimized path nodes as input, use the B-spline harmonic function and curve formula to obtain the discretized trajectory points of the B-spline curve, and connect them to obtain the smooth path after the spline curve is optimized.

[0043] The probability completeness and time complexity analysis of the RRT algorithm based on parallel sampling point optimization in the present invention are as follows:

[0044] For the algorithm of the present invention, the multi-point parallel weighted heuristic sampling strategy is to add parallel sampling points on the basis of random sampling, guide the random tree to grow towards the target point, and reduce the invalid sampling of the random tree. Therefore, it will not have a negative impact on the convergence of the algorithm, and at least the same path solution as the RRT algorithm can be found. The bidirectional guidance mechanism can speed up the speed of dual-tree fusion and will not affect the probability completeness. The probability completeness of the RRT and Bi-RRT algorithms has been proved, so the algorithm has probability completeness.

[0045] The difference between the algorithm of the present invention and the RRT algorithm is mainly reflected in multi-point parallel weighted heuristic sampling, dynamic step extension and bidirectional guidance mechanism. For the multi-point parallel weighted heuristic sampling strategy, the ParallelSampling function randomly samples multiple nodes for evaluation function comparison, and its time complexity is the same as the RandomSampling function and the ComparisonDistance function. The evaluation function only performs some simple calculations, and the time cost can be ignored. For the dynamic step extension strategy, the time cost of the RapidExtend function mainly depends on the CheckPath function, and its time complexity is proportional to the CheckConstraint function. When the number of sampling times tends to infinity, the proportionality coefficient is the ratio of the area of ​​the obstacle area to the area of ​​the obstacle-free area. The ratio is a known constant. The CheckPath function is executed at most twice in each random sampling, which depends on the state space and the safety distance. Therefore, the time complexity of the RapidExtend function is similar to that of the CheckConstraint function. The time complexity of the nstraint function is the same. For the bidirectional guidance mechanism, the starting tree structure is the same as the RRT single tree structure. The time complexity of the Nearest function in the end tree is the same as the time complexity of the ComparisonDistance function. The time complexity of the GuideSampling function is the same as the time complexity of the ParallelSampling function. The time complexity of the RRT algorithm depends on the RandomSampling function, the ComparisonDistance function and the CheckConstraint function. The algorithm of the present invention adds and modifies some functions based on the RRT algorithm. The time complexity of this function is the same as the ComparisonDistance function and the CheckConstraint function. Therefore, the time complexity of the algorithm of the present invention is the same as the time complexity of the RRT algorithm, and the time complexity of the RRT algorithm is the same as the Bi-RRT algorithm, so the time complexity of the algorithm of the present invention is the same as the Bi-RRT algorithm.

[0046] Compared with the prior art, the present invention has the following beneficial effects:

[0047] The present invention provides a robot path planning method based on parallel sampling point optimization RRT algorithm, adds parallel sampling points to perform multi-point optimization expansion, establishes an evaluation function to screen and select the parallel sampling points for expansion in order to guide random tree expansion and reduce path turning points, takes into account both path length and path smoothness, constructs a dual-tree guidance mechanism, improves the fusion speed of bidirectional random trees, prevents the generation of invalid nodes in local space, adopts different adaptive expansion steps according to the environmental information around the explored nodes, avoids the negative effects caused by excessive exploration and collision area expansion, and avoids the situation of insufficient algorithm stability under different environments, proposes a path rewiring optimization method, reduces the initial path length and the number of nodes, and the final path after B-spline smoothing optimization meets the dynamic characteristics of the mobile robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. The drawings in the following description are merely embodiments of the present invention and are only used to explain the present invention, and cannot be understood as limiting the present invention.

[0050] Figure 1 The present invention is a flow chart of a robot path planning method based on a parallel sampling point optimization RRT algorithm.

[0051] Figure 2 This is a schematic diagram of the multi-point parallel weighted heuristic sampling strategy of the present invention. Figure 2 (a) is a schematic diagram of parallel sampling. Figure 2 (b) in the figure is a schematic diagram of point selection expansion.

[0052] Figure 3 This is a schematic diagram of the dynamic step size expansion of the present invention. Figure 3 (a) is a schematic diagram of the expansion of the obstacle-dense area. Figure 3 (b) is a schematic diagram of the expansion of the obstacle sparse area.

[0053] Figure 4 Schematic diagram of the dual-tree guidance mechanism of the present invention.

[0054] Figure 5 This is a schematic diagram of the initial path global rewiring optimization of the present invention.

[0055] Figure 6 This is a schematic diagram of the local rewiring optimization of the path of the present invention.

[0056] Figure 7 This is a schematic diagram of the optimization using B-spline curve fitting after the path rewiring optimization of the present invention.

[0057] Figure 8 This is a specific simulation result path diagram of the present invention in four environments. Figure 8 (a) in the figure is a random map planning path diagram. Figure 8 (b) is the corridor map planning path map. Figure 8 (c) in the figure is the warehouse map planning path diagram. Figure 8 (d) in the figure is the path diagram of maze map planning.

[0058] Fig. 9 The following are specific simulation result data of the present invention in four environments.

[0059] Fig.10 This is a comparison diagram of the initial path optimization of the present invention. Fig.10 (a) is a comparison chart of random map path optimization. Fig.10 (b) is a comparison chart of corridor map path optimization. Fig.10 (c) is a comparison chart of warehouse map path optimization. Fig.10 (d) in the figure is a comparison chart of maze map path optimization.

[0060] Fig.11 Modeling the environment and generating effect diagrams for the present invention, Fig.11 (a) is a real outdoor environment picture. Fig.11 (b) is the free space extraction effect diagram. Fig.11 (c) in the figure is a grayscale map. Fig.11 (d) in the figure is the path generation effect diagram. DETAILED DESCRIPTION

[0062] The technical solution of the present invention is described in detail below in conjunction with the accompanying drawings.

[0063] like Figure 1 As shown, a robot path planning method based on parallel sampling point optimization RRT algorithm includes the following steps:

[0064] Step 1, define X free and X obs are free space and obstacle space respectively, x init is the starting node of the robot’s motion in the state space, x goal For the target node, create the starting tree T1 and the end tree T2, and set the algorithm parameters;

[0065] Step 2: For the starting tree T1, randomly sample multiple nodes in parallel in the state space. The number of parallel sampling points is determined by the map complexity. The parallel sampling points are screened, and the sampling points falling into obstacles are discarded. The distance metric and the angle metric are normalized, and an evaluation function containing distance constraints and angle constraints is established. The evaluation function is used to select the optimal node for expansion of the remaining sampling points, and the coordinates of the optimal node and the expansion direction of the new node are determined.

[0066] Step 3: Detect the obstacle distribution information around the nearest node, dynamically select the expansion step, take the nearest node as the starting endpoint, extend the step toward the optimal node, and obtain a new expansion node in the starting tree T1;

[0067] Step 4: Connect the new extended node and the nearest node to perform midpoint collision detection, discretize the path segments, and after obtaining all discrete path points, determine whether they are on the obstacle. If so, return to step 2. If not, add the new node to the random tree T1.

[0068] Step 5, for the terminal tree T2, randomly sample multiple nodes in parallel in the sampling space, and use the newly generated extended node in the starting tree T1 as the local target point of the terminal tree T2, provide heuristics for selecting the optimal node for the terminal tree T2, determine the coordinates of the optimal node, and perform intermediate point collision detection and dynamically select step size expansion through steps 3 and 4.

[0069] Step 6: Repeat steps 2 to 5 until the number of newly generated nodes of the starting tree T1 and the ending tree T2 is less than the connection threshold, connect the two points, and trace back to the starting point and the target point with the newly generated nodes of the two trees as the starting endpoints to obtain the initial path. Perform global path replanning and local path replanning on the initial path, filter out non-critical nodes, and improve the quality of the initial path.

[0070] Step 7, for the optimized path obtained in step 6, control points are constructed between adjacent nodes of the path, the control point construction method is selected by setting a threshold, and the path of the constructed control points is fit-optimized using a cubic B-spline curve to generate a smooth path that meets the robot's maximum curvature constraint and has a high degree of fit.

[0071] Specifically, define the free space, obstacle space, starting point and target point as described in step 1, and create two random trees. The process is as follows:

[0072] Step 1.1: Import map data and perform binary processing on the map;

[0073] Step 1.2: Set the starting point coordinate x of the robot movement init and the end point coordinate x goal , construct two random trees with the starting point and the target point as the root nodes respectively, and set the connection threshold of the two random trees.

[0074] Furthermore, the algorithm parameter values ​​in step one include a distance factor, an angle factor, a safety distance and a random tree expansion step size.

[0075] Specifically, Figure 2 As shown, the process of multi-point parallel weighted heuristic sampling of the starting tree T1 and selecting the optimal node for expansion in step 2 is as follows:

[0076] Step 2.1: Sample multiple nodes in parallel in the state space. The number of parallel sampling points is determined by the map complexity. After a lot of experimental research and analysis, it is found that the best planning effect is to sample three nodes in a simple environment and five nodes in a complex environment. Determine whether the parallel sampling node is within the obstacle range, discard the sampling point that falls into the obstacle, and find the node x in the random tree branch that is closest to the remaining sampling points after screening. near ,like Figure 2 As shown in (a), five nodes are randomly sampled, the evaluation function of each randomly sampled point is calculated, and the node with the smallest evaluation function is selected for expansion.

[0077] Step 2.2: Distance and angle belong to different dimensions and need to be normalized to unify data from different sources into one reference system. The normalization method is as follows:

[0078]

[0079] Where d ( x randi ,x start) For x randi and x start The Euclidean distance between two points, D ( x randi, x goal) For x randi and x goal The Euclidean distance between two points, S d (d ( x randi ,x start) ) and S D (D ( x randi, x goal) ) is the normalized function corresponding to the distance; θ ( x parent ,x near ,x randi) is the angle between adjacent nodes, S A (θ ( x parent ,x near ,x randi) ) is the normalized function corresponding to the angle;

[0080] Establish the evaluation function F1(x) of the starting tree T1, expressed as:

[0081]

[0082] Where D n is the Euclidean distance from the starting point to the target point, d n is the Euclidean distance from the current node to the target point, T n is the angle between three adjacent nodes, α is the distance factor, and k is the rotation factor, such as Figure 2 As shown in (b), x rand2 With x goal 、x start The Euclidean distance between rand1 With x goal 、x start The distance between them is long, but the angle θ2 between the adjacent path segment vectors is smaller than θ1. Therefore, considering the cost of distance and angle, we choose rand2 With x near Generate paths between them;

[0083] Step 2.3: Calculate the evaluation function values ​​of the remaining sampling points after screening, select the sampling point with the smallest evaluation function, and determine the coordinates of the optimal node and the new node x new direction of expansion.

[0084] Specifically, Figure 3 As shown, the process of detecting the obstacle distribution information around the nearest node in step 3 and dynamically selecting the expansion step size is as follows:

[0085] Step 3.1: Draw a graph with the nearest node x nearest is the center of the circle, and the safety distance is R safe_dis The circle with radius is used to extract points in each area on the circle. The formula for extracting the point set is expressed as:

[0086]

[0087] Where R safe_dis For the safety distance, select safe_dis1 and safe_dis2 as the safety distance.

[0088] Step 3.2: Check whether the point set on the circle is outside the state space and on the obstacle, and dynamically select the expansion step size L step1 , L step2 and L step3, taking the nearest node as the starting endpoint, extending the step length to the optimal node, and obtaining a new extended node in the starting tree T1. First, safe_dis2 is used as the safety distance, and the CheckPath function is used to detect whether the point set on the circle is outside the state space and on the obstacle. If not, a large step length L is selected. step3 To expand; on the contrary, use safe_dis1 as the safe distance, use the CheckPath function again to detect the point set on the circle, and select the normal step length L step2 Or small step length L step1 to expand.

[0089] Specifically, the process of connecting the optimal node and the nearest node to perform midpoint collision detection in step 4 is as follows:

[0090] Step 4.1: Connect the new extended node with the nearest node, obtain all discrete path points at the minimum resolution of the map, and adaptively discretize the intermediate points x i It can be expressed as:

[0091]

[0092] Where Δp is the map resolution, k is the number of the current discrete path point, and after all discrete path points are obtained, it is determined whether they are on the obstacle. The above collision detection method can be applied to various irregular obstacle environments.

[0093] Step 4.2: After obtaining all discrete path points, determine whether they are on the obstacle. If so, return to step 2 and re-sample in parallel in space. Otherwise, add the new expansion node to the random tree T1.

[0094] Specifically, Figure 4 As shown, the newly generated extended node in the starting tree T1 in step 5 is used as the local target point of the terminal tree T2, and the process of providing a heuristic for selecting the optimal node for the terminal tree T2 is as follows:

[0095] Step 5.1: Sample multiple nodes in parallel in the state space. The number of parallel sampling points is determined by the map complexity. In a simple environment, three nodes are sampled in parallel, and in a complex environment, five nodes are sampled in parallel. Determine whether the parallel sampling node is within the obstacle range, discard the sampling points that fall into the obstacle, and find the node x in the random tree branch that is closest to the remaining sampling points after screening. near ;

[0096] Step 5.2: Determine the newly generated node x of the starting tree T1 T1new The Euclidean distance L to the target point n , x near to x T1new The Euclidean distance l n , three adjacent nodes xparent 、x near and x rand Angle T n , determine the remaining sampling points after screening to x T1new The Euclidean distance S(x) and B(x) of the target point are used to establish the evaluation function F2(x) of the terminal tree T2, which is expressed as:

[0097]

[0098] Where, L n From the target point to x T1new The Euclidean distance, l n From the current node to x T1new The Euclidean distance, ε is the distance factor, m is the rotation factor, Figure 4 The blue dashed line in the middle is the expansion direction of the random tree of the traditional Bi-RRT algorithm, and the green dashed line is the expansion direction of the dual-tree guidance mechanism. The evaluation function values ​​of the remaining sampling points after screening are calculated respectively, and the sampling point with the smallest evaluation function is selected from them. The expansion step size is dynamically selected according to step 3, and the collision detection of the intermediate points is performed according to step 4.

[0099] Specifically, the process of performing global path replanning and local path replanning on the initial path in step 6 is as follows:

[0100] Step 6.1: Repeat steps 2 to 5 until the newly generated nodes of the starting tree T1 and the ending tree T2 coincide or are less than the set connection threshold, connect the two points, and use the newly generated nodes of the two trees as the starting endpoints to trace back to the starting point and the target point to obtain the initial path;

[0101] Step 6.2: If Figure 5 As shown, the initial path is traced back with the end point as the starting endpoint of the line segment, and the subsequent path nodes are connected in sequence to check whether the connected path segment collides with the obstacle. If no collision occurs, all path nodes between the two nodes are deleted; if a collision occurs, the child node of the node is used as the starting endpoint of the new line segment, and the above operation is repeated until the end of the traceback to the starting point;

[0102] Step 6.3: If Figure 6 As shown in the figure, the newly generated path segment is interpolated, and the interpolation distance is the minimum step size. A path with denser nodes is obtained for local path replanning. The starting point is used as the starting endpoint of the line segment, and the subsequent nodes are traversed in sequence to check whether the line segment collides with the obstacle. If no collision occurs, all path nodes between the two nodes are deleted. Otherwise, the above operation is performed again with the parent node of the node as the starting endpoint of the line segment until the target point is traversed.

[0103] Specifically, Figure 7As shown, in step 7, control points are constructed between adjacent nodes, a control point construction method is selected by setting a threshold, and a B-spline curve is used to fit the path of the constructed control points as follows:

[0104] Step 7.1: Construct control points between adjacent nodes, set a threshold to select the control point construction method, and determine whether the length of the path segment between two adjacent nodes is less than the threshold. If it is less than the threshold, construct a control point at the midpoint of the path segment. Otherwise, take the two nodes as the starting points and take the normal step length to construct a control point.

[0105] Step 7.2: Take the constructed control points and the optimized path nodes as input, use the B-spline harmonic function and curve formula to obtain the discretized trajectory points of the B-spline curve, and connect them to obtain the smooth path after the spline curve is optimized.

[0106] Conduct simulation analysis in different environment maps:

[0107] like Figure 8 As shown, the random tree expansion of the algorithm of the present invention is purposeful and directional. The starting tree and the end tree guide each other, reducing invalid sampling in the local space, obtaining a better path with very few sampling nodes, and having better path planning performance. The planned path is concise and smooth and suitable for different environments.

[0108] like Fig. 9 As shown, the algorithm of the present invention has the shortest average planning time and the smallest standard deviation in different environmental maps, and has more stable planning performance. It can efficiently complete planning tasks in different environments. The algorithm is highly robust, with the least average number of sampling nodes and the shortest average path length in all test environments. That is, the algorithm can use fewer sampling nodes to explore the space and find a better path solution. The average number of nodes in the final path obtained by planning is significantly reduced compared to other algorithms, with an average of only five nodes. Fewer path nodes can reduce the number of turns of the robot and avoid mechanical wear of the robot.

[0109] like Fig.10 As shown in the figure, there are a lot of redundant nodes in the initial path, which leads to a long path length. After global rewiring optimization, the path nodes and path length are greatly reduced, but the optimized path is still composed of some nodes of the initial path, and there are unnecessary twists and turns. After local rewiring optimization, the path length is shortened again and the path nodes are deleted. Fig.10 From (a) to (d), it can be seen that the path is shorter and smoother after rewiring optimization, and the path quality is significantly improved, making it more suitable for mobile robot driving.

[0110] like Fig.11As shown, the free space area is extracted from the real environment map, and the real environment map is modeled into a grayscale map. After the planned path is generated, the movement command is sent to the robot, allowing the robot to move along the planned path until it reaches the target point area.

[0111] The above specifically describes the preferred implementation method of the present invention, but the invention is not limited to the described embodiments. Those skilled in the art may make various equivalent modifications or substitutions without violating the spirit of the present invention. These equivalent modifications or substitutions are all included in the scope defined by the claims of this application.

Claims

1. A robot path planning method based on parallel sampling point optimization RRT algorithm, characterized in that: The following steps are involved: Step 1, define X free and X obs are free space and obstacle space respectively, x init is the starting node of the robot’s motion in the state space, x goal For the target node, create the starting tree T1 and the end tree T2, and set the algorithm parameters; Step 2: For the starting tree T1, randomly sample multiple nodes in parallel in the state space. The number of parallel sampling points is determined by the map complexity. The parallel sampling points are screened, and the sampling points falling into obstacles are discarded. The distance metric and the angle metric are normalized, and an evaluation function containing distance constraints and angle constraints is established. The evaluation function is used to select the optimal node for expansion of the remaining sampling points, and the coordinates of the optimal node and the expansion direction of the new node are determined. Step 3: Detect the obstacle distribution information around the nearest node, dynamically select the expansion step, take the nearest node as the starting endpoint, extend the step toward the optimal node, and obtain a new expansion node in the starting tree T1; Step 4: Connect the new extended node and the nearest node to perform midpoint collision detection, discretize the path segments, and after obtaining all discrete path points, determine whether they are on the obstacle. If so, return to step 2. If not, add the new node to the random tree T1. Step 5: For the terminal tree T2, multiple nodes are randomly sampled in parallel in the state space. The newly generated extended node in the starting tree T1 is used as the local target point of the terminal tree T2 to provide a heuristic for selecting the optimal node for the terminal tree T2. The coordinates of the optimal node are determined, and intermediate point collision detection and dynamic step size expansion are performed through steps 3 and 4.

2. Step 6: Repeat steps 2 to 5 until the newly generated nodes of the starting tree T1 and the ending tree T2 coincide or are less than the connection threshold, connect the two points, and trace back to the starting point and the target point with the newly generated nodes of the two trees as the starting endpoints to obtain the initial path. Perform global path replanning and local path replanning on the initial path, filter out non-critical nodes, and improve the quality of the initial path. Step 7, for the optimized path obtained in step 6, control points are constructed between adjacent nodes of the path, the control point construction method is selected by setting a threshold, and the path of the constructed control points is fit-optimized using a cubic B-spline curve to generate a smooth path that meets the robot's maximum curvature constraint and has a high degree of fit.

3. A robot path planning method based on parallel sampling point optimization RRT algorithm according to claim 1, characterized in that: Define the free space, obstacle space, starting point and target point as described in step 1, and create two random trees. The specific process is as follows: Step 1.1: Import map data and perform binary processing on the map; Step 1.2: Set the starting point coordinate x of the robot movement init and the end point coordinate x goal , set the algorithm parameter values ​​including distance factor, angle factor, safety distance and random tree expansion step, build two random trees with the starting point and target point as root nodes respectively, and set the connection threshold of the two random trees.

4. The robot path planning method based on parallel sampling point optimization RRT algorithm according to claim 1 is characterized in that: The specific process of multi-point parallel weighted heuristic sampling of the starting tree T1 and selecting the optimal node for expansion in step 2 is as follows: Step 2.1: Sample multiple nodes in parallel in the state space. The number of parallel sampling points is determined by the map complexity. After a lot of experimental research and analysis, it is found that the best planning effect is to sample three nodes in a simple environment and five nodes in a complex environment. Determine whether the parallel sampling node is within the obstacle range, discard the sampling point that falls into the obstacle, and find the node x in the random tree branch that is closest to the remaining sampling points after screening. near ; Step 2.2: Determine x near The Euclidean distance d to the target point n , the Euclidean distance D from the starting point to the target point n , three adjacent nodes x parent 、x near and x rand Angle T n , determine the Euclidean distances A(x) and N(x) from the remaining sampling points to the starting point and the target point after screening, and establish the evaluation function F1(x) of the starting tree T1; Step 2.3: Calculate the evaluation function values ​​of the remaining sampling points after screening, select the sampling point with the smallest evaluation function value for expansion, and determine the coordinates of the optimal node and the new node x new direction of expansion.

5. The robot path planning method based on parallel sampling point optimization RRT algorithm according to claim 1, characterized in that: The specific process of detecting the obstacle distribution information around the nearest node and dynamically selecting the expansion step size described in step 3 is as follows: Step 3.1: Draw a graph with the nearest node x nearest is the center of the circle, the safety distance R safe_dis1 and R safe_dis2 A circle with a radius of , extract the points in each area on the circle; Step 3.2: Check whether the point set on the circle is outside the state space and on the obstacle, and dynamically select the expansion step length L step1 , L step2 and L step3 , with the nearest node x nearest As the starting endpoint, extend the step length toward the optimal node to obtain a new extended node in the starting point tree T1.

6. The robot path planning method based on parallel sampling point optimization RRT algorithm according to claim 1, characterized in that: The specific process of connecting the optimal node and the nearest node to perform midpoint collision detection in step 4 is as follows: Step 4.1: Connect the new extended node with the nearest node to obtain all discrete path points at the minimum resolution of the map; Step 4.2: After obtaining all discrete path points, determine whether they are on the obstacle. If so, return to step 2 and re-sample in parallel in space. Otherwise, add the new expansion node to the random tree T1.

7. The robot path planning method based on parallel sampling point optimization RRT algorithm according to claim 1 is characterized in that: The specific process of using the newly generated extended node in the starting tree T1 in step 5 as the local target point of the terminal tree T2 to provide a heuristic for selecting the optimal node for the terminal tree T2 is as follows: Step 5.1: Sample multiple nodes in parallel in the state space. The number of parallel sampling points is determined by the map complexity. In a simple environment, three nodes are sampled in parallel, and in a complex environment, five nodes are sampled in parallel. Determine whether the parallel sampling node is within the obstacle range, discard the sampling points that fall into the obstacle, and find the node x in the random tree branch that is closest to the remaining sampling points after screening. near ; Step 5.2: Determine the newly generated node x of the starting tree T1 T1new The Euclidean distance L to the target point n , x near to x T1new The Euclidean distance l n , three adjacent nodes x parent 、x near and x rand Angle T n , determine the remaining sampling points after screening to x T1new The Euclidean distance S(x) and B(x) of the target point are used to establish the evaluation function F2(x) of the terminal tree T2. The evaluation function values ​​of the remaining sampling points after screening are calculated respectively, and the sampling point with the smallest evaluation function is selected from them. The expansion step size is dynamically selected according to step 3. According to step 4, the intermediate point collision detection is performed to determine whether it is on the obstacle. If it is on the obstacle, return to step 2. If not, the new node is added to the random tree T2.

8. The robot path planning method based on parallel sampling point optimization RRT algorithm according to claim 1 is characterized in that: The specific process of performing full path replanning and local path replanning on the initial path described in step 6 is as follows: Step 6.1: Repeat steps 2 to 5 until the newly generated nodes of the starting tree T1 and the ending tree T2 coincide or are less than the set connection threshold, connect the two points, use the newly generated nodes of the two trees as the starting and ending points, trace back to the starting point and the target point, and obtain the initial path; Step 6.2: Use the end point as the starting endpoint of the line segment to trace back the initial path, connect the subsequent path nodes in sequence, and check whether the connected path segment collides with the obstacle. If no collision occurs, delete all path nodes between the two nodes; if a collision occurs, use the child node of the node as the starting endpoint of the new line segment, repeat the above operation, and trace back to the starting point; Step 6.3: Interpolate the newly generated path segment, with the interpolation distance as the minimum step length, and obtain a path with denser nodes for local path replanning. Take the starting point as the starting endpoint of the line segment, traverse the subsequent nodes in turn, and check whether the line segment collides with obstacles. If there is no collision, delete all path nodes between the two nodes. Otherwise, perform the above operation again with the parent node of the node as the starting endpoint of the line segment until traversing to the target point.

9. The robot path planning method based on parallel sampling point optimization RRT algorithm according to claim 1, characterized in that: The specific process of constructing control points between adjacent nodes described in step 7, selecting the control point construction method by setting a threshold, and fitting the path of the constructed control points using a B-spline curve is as follows: Step 7.1: Construct control points between adjacent nodes, set a threshold to select the control point construction method, and determine whether the length of the path segment between two adjacent nodes is less than the threshold. If it is less than the threshold, construct a control point at the midpoint of the path segment. Otherwise, take the two nodes as the starting points and take the normal step length to construct a control point. Step 7.2: Take the constructed control points and the optimized path nodes as input, use the B-spline harmonic function and curve formula to obtain the discretized trajectory points of the B-spline curve, and connect them to obtain the smooth path after the spline curve is optimized.

Images