Path planning method based on Bi-RT-RRT* logistics transportation vehicle

Abstract

The invention discloses a logistics transport vehicle path planning method based on a Bi-RT-RRT * algorithm. The method comprises the following steps: respectively growing two trees from an initial state and a target state; when the two trees meet each other, the reverse tree is used as heuristic information to guide the forward tree to grow towards a target state, so that a feasible path for reaching a target position is quickly and effectively generated; on the basis of a path reconnection strategy of a target node, path quality is optimized, and path length is shortened; and then optimizing the path quality by using cubic B-spline interpolation, calculating the minimum turning radius of the vehicle by using an Ackerman steering formula according to the maximum turning angle of the front wheel, and optimizing the curvature radius to enable the radius of each section of arc of the path to be greater than the minimum turning radius so as to meet the turning angle constraint of the vehicle. The method is short in planning time and optimized in path quality in an unknown dynamic environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent vehicle path planning, and more specifically, relates to a Bi-RT-RRT*-based logistics transportation vehicle path planning method. Background Art

[0002] In recent years, autonomous driving technology has made significant progress. Research on autonomous navigation in diverse environments, such as ports, mining areas, campuses, and closed logistics parks, has garnered increasing attention. The logistics industry is rapidly developing, and logistics parks, as key hubs for cargo distribution, storage, and transshipment, are crucial for operational efficiency and safety. Logistics transport vehicles are the primary means of transportation for logistics distribution within these parks. When introducing intelligently driven logistics transport vehicles into these dynamic environments, where humans and intelligently driven vehicles coexist, efficient and stable path planning algorithms are crucial.

[0003] Over the past few decades, extensive research has been conducted and numerous methods have been proposed to solve motion planning problems. These methods can be roughly divided into three categories: sampling-based methods, graph-based methods, and potential field-based methods. Specifically, while graph-based planning methods such as A* and Dijkstra can utilize established grid maps to obtain optimal paths, they require discretizing the state space, resulting in time costs and memory usage that increase exponentially with the dimensionality of the state space. Potential field-based planning methods, such as the artificial potential field (APF), combine the attractive force of target points with the repulsive force of obstacles to form a potential field to guide the movement of autonomous vehicles. However, these methods can suffer from local minima, leading to planning termination or failure. Sampling-based planning algorithms, such as the probabilistic roadmap (PRM) and the rapidly exploring random tree (RRT) algorithm, are widely used due to their ability to efficiently handle path planning problems in complex environments. However, the poor quality of the paths they generate and their lack of replanning capabilities make them unsuitable for unknown dynamic environments.

[0004] To improve the performance of sampling-based methods, researchers have proposed many variations. For example, the RRT* algorithm introduces a path optimization mechanism based on RRT. By continuously optimizing the path cost while expanding the tree, it generates shorter and smoother paths, but this is more time-consuming. The RRT-connect algorithm, compared to the RRT algorithm, accelerates the path search process by simultaneously expanding the tree from both the starting point and the end point and searching for a connecting path between the two trees, resulting in a faster path finding. However, in practical applications, merging two trees not only requires ensuring that the vehicle adheres to kinematic principles but also requires solving a two-point boundary value problem (TBVP). For autonomous vehicles with nonholonomic constraints, deriving a closed-form solution is difficult, and obtaining a numerical solution can be time-consuming. Furthermore, logistics parks are dynamic and complex spaces with moving vehicles and personnel. Traditional motion planning algorithms primarily focus on static environmental factors and often ignore the impact of dynamic obstacles, leading to planning failures in dynamic environments. The RT-RRT* algorithm, proposed in 2015, has advanced this field by responding to environmental changes in real time. It can quickly adapt to changes and generate more optimal paths in dynamic environments. However, when encountering unknown obstacles that cause the expansion tree search to be interrupted, it needs to regrow on the existing tree to establish a new connection path, which increases the planning time. Summary of the Invention

[0005] In response to the problems of long planning time and poor path quality in unknown dynamic environments in existing methods, the purpose of the present invention is to provide a logistics transportation vehicle path planning method based on bidirectional real-time rapid expansion tree (Bi-RT-RRT*). In this algorithm, two trees are grown from the starting state and the target state respectively. When the two trees meet each other, the reverse tree will serve as heuristic information to guide the forward tree to grow towards the target state, thereby quickly and efficiently generating a feasible path to the target location. In addition, in order to optimize the path quality, a path reconnection strategy based on the target node is proposed to optimize the path quality and shorten the path length. Then, the path quality is optimized using cubic B-spline curve interpolation, and the minimum turning radius of the vehicle is calculated using the Ackerman steering formula based on the maximum front wheel turning angle to optimize the curvature radius so that the radius of each arc segment of the path is greater than the minimum turning radius to meet the vehicle turning angle constraint.

[0006] The purpose of the present invention is achieved through the following technical solutions:

[0007] A logistics transport vehicle path planning method based on the Bi-RT-RRT* algorithm includes the following steps:

[0008] Step 1: Set the path planning parameters and initialize Qfstart, Sfgoal, Qfgoal and the expansion tree;

[0009] Among them, Qfstart is the queue for storing the forward tree for rewiring nodes, Sfgoal is the queue for storing the initial feasible path nodes, and Qfgoal is the queue for storing the branch nodes of the expansion tree;

[0010] The path planning parameters include: starting point coordinates, target point coordinates, agent position coordinates, expansion step size, target bias sampling probability, sampling map and maximum number of iterations;

[0011] Step 2: Use the coordinates of the starting node and the target node to build a forward and reverse bidirectional tree;

[0012] Step 3: Expand the new node using the target bias strategy, reconnect the newly generated nodes, and update the forward and reverse bidirectional trees;

[0013] Step 4: Determine whether the updated forward tree and backward tree meet. If so, proceed to step 6. If not, determine whether the number of sampling points is greater than the maximum number of iterations. If the total number of sampling points is greater than or equal to the maximum number of iterations, output that the path planning failed. If the total number of sampling points is less than the maximum number of iterations, proceed to step 5.

[0014] Step 5: Use the nodes generated in step 3 to update the forward tree and reverse tree again, and then return to step 3;

[0015] Step 6: Pass the node information of the reverse tree to the forward tree, so that the reverse tree acts as heuristic information to guide the forward tree to continue growing towards the target state, thereby generating an initial feasible path from the starting point to the target position;

[0016] Step 7: The backward tree stops growing and is initialized, and the forward tree continues to grow towards the target position and explore the unknown environment;

[0017] Step 8: After obtaining an initial target path, a rewiring process is used to reduce the path length and optimize the path quality;

[0018] Step 9: The rerouted path is smoothed by cubic B-spline interpolation, and then the curvature is constrained by adjusting the interpolation points of the B-spline curve to meet the vehicle turning angle constraint;

[0019] Step 10: Output the final path.

[0020] Furthermore, the step 2 is specifically as follows: using the coordinates of the starting node and the target node respectively to establish a forward tree extending from the starting node and a reverse tree extending from the target node;

[0021] Furthermore, the target bias strategy is adopted for expanding the new node in step 3, and the newly generated nodes are reconnected. The positive and negative bidirectional trees are updated specifically as follows:

[0022] Step 31: Before expanding the new node, the obstacle is expanded, that is, a safety distance constraint is added to avoid collision with obstacles when the vehicle is used as a mass point for path planning;

[0023]

[0024] Among them L safe is the safety distance of the obstacle expansion, L is the length of the vehicle outline;

[0025] Step 32: Introduce biased sampling probability p bias (0≤p bias ≤1), when expanding a new node, there is p bias The probability of the target point or target area is biased, and the random number r (0≤r≤1) is generated by the rand function. If r <p bias , bias sampling point x sample The sampling is biased towards the target area, that is, random sampling is generated in the target point or target area; if r≥p bias , then random sampling is generated in the free area of the global space, which can increase the chance of exploring the target direction while ensuring the probability completeness of the algorithm;

[0026]

[0027] where X goal is the target point or target area, X free is the global free area, p bias is the bias sampling probability;

[0028] Step 33: When expanding a new node, if x nreast (nearest node) and x new There are no obstacles between the straight lines (new nodes), and when x nreast and x new When the Euclidean distance between new Add to the extension tree;

[0029] Step 3 and 4: Reconnect the current node when the node (xi) can obtain a lower connection cost c by passing from another node that is not its parent node i When rewiring is performed;

[0030] c i =Cost(x near )+d E (x near ,x i );

[0031]

[0032] where ci is x near Expand to x i Total cost of the node, Cost(x near ) is x i The initial cost at the node, d E (x near ,x i ) is x near ,x i The Euclidean distance cost between two nodes;

[0033] Step 35: When the root node of the expansion tree changes and the root node list is not empty, reconnect the nodes except the root node using the connection cost between nodes, i.e. c new <c old Reconnect and update the expansion tree;

[0034] c old =Cost(x near );

[0035] c new =Cost(x r )+dE(x r ,x near );

[0036] where x r is the second item in the root node list, c old is the cost of reconnecting to the node before reconnecting, c new The cost of the node after reconnection during the reconnection process;

[0037] Furthermore, in step 4, it is determined whether the updated forward tree and backward tree meet. If so, step 6 is executed. If not, it is determined whether the number of sampling points is greater than the maximum number of iterations. If the total number of sampling points is greater than or equal to the maximum number of iterations, the path planning failure is output. If the total number of sampling points is less than the maximum number of iterations, step 5 is executed as follows:

[0038] Step 41: When the Euclidean distance between the latest nodes of the forward tree and the reverse tree is less than one expansion step, the two trees meet;

[0039] Step 42: If the two trees meet, the reverse tree node information is passed to the forward tree through the swap function, so that the reverse tree acts as heuristic information to guide the forward tree to continue growing towards the target state, thereby generating an initial feasible path from the starting point to the target position; if there is no meeting, determine whether the number of sampling points is greater than the maximum number of iterations; if the total number of sampling points is greater than or equal to the maximum number of iterations, output path planning failure; if the total number of sampling points is less than the maximum number of iterations, expand the sampling of new nodes again and update the forward and reverse trees;

[0040] Furthermore, the step seven is specifically as follows: after the two trees meet, the reverse tree stops growing and is initialized, the forward tree trunk grows toward the target position, and the branches continue to explore the unknown environment;

[0041] Furthermore, after obtaining an initial target path in step eight, a rewiring process is adopted to reduce the path length and optimize the path quality, specifically as follows:

[0042] Step 81: Determine whether the number of valid nodes in the initial target path is greater than 2. If so, execute step 82; otherwise, terminate the path reconnection and directly output the path.

[0043] Step 82: From the starting point (end point) of this path optimization reconnection, try to connect to the ith node (i is the total number of valid nodes in the initial target path of this path optimization, a = 2, 3, 4, ..., i-1) in sequence; before connecting, check whether there is a collision between the current node and the ith node; if there is no collision, directly connect the two nodes; otherwise, connect the end point and the node before the collision occurred;

[0044] Step 83: Repeat step 82 from the starting point of this path optimization reconnection (the node before the last optimization collision occurred) until the optimization reconnects to the starting point of the initial target path, and output the path after optimization reconnection and the path valid node coordinate information list P i (i=0,1,2,...,n);

[0045] Furthermore, in step nine, the rerouted path is interpolated with a cubic B-spline curve to smooth the path, and then the curvature is constrained by adjusting the interpolation points of the B-spline curve to meet the vehicle turning angle constraint. Specifically,

[0046] Step 91: Use the valid node list output after path optimization reconnection to perform interpolation processing. First, calculate the Euclidean distance between adjacent nodes, and then calculate the total Euclidean distance between the starting point and the end point. Based on the ratio of the Euclidean distance between adjacent nodes to the total distance, make the interpolation point interval proportional to the physical distance, thereby reducing the occurrence of curvature mutations during the interpolation process;

[0047] d k =||P k -P k-1 ||(k=1,2,...,n);

[0048]

[0049] N = 10 × D;

[0050] N k =u k ×N;

[0051] where d k is the Euclidean distance between adjacent points, rounded to one decimal place, D is the Euclidean distance between the starting point and the end point, u k is the ratio of the cumulative distance from the starting point to the point to the total distance, N is the total number of interpolation points, N k is the number of interpolation points between the starting point and this point;

[0052] Step 92: Use cubic B-spline curve to interpolate the processing path;

[0053] The expression of the cubic B-spline curve is:

[0054]

[0055] where N i,p (u) is the P-order B-spline curve interpolation basis function, N i,3 (u) is the cubic B-spline curve interpolation basis function;

[0056] Step 93: Calculate the minimum turning radius of the vehicle using the maximum front wheel turning angle according to the Ackerman steering formula;

[0057] θ max =arctan(L / R min )

[0058] where θ max is the maximum turning angle of the front wheel, L is the vehicle wheelbase, R min is the minimum turning radius of the vehicle;

[0059] Step 94: Calculate the curvature of each point of the cubic B-spline curve and apply constraints;

[0060]

[0061] Where κ(u) is the curvature of the cubic B-spline curve, C'(u) is the first-order derivative of the cubic B-spline curve, C"(u) is the second-order derivative of the cubic B-spline curve, and N' i,3 (u k ) is the first-order derivative of the cubic B-spline curve basis function, N″ i,3 (u k ) is the second-order derivative of the cubic B-spline curve basis function, κ max is the maximum curvature of the cubic B-spline curve;

[0062] Step 95: Construct an optimization objective function to minimize the second-order differential energy of the control points, force adjacent control points to be approximately collinear, and reduce local curvature;

[0063]

[0064] Step 96: Construct the gradient information of the curvature constraint through automatic differentiation and combine it with the interior point method to solve the nonlinear programming problem.

[0065] Compared with the prior art, the present invention has the following advantages:

[0066] The present invention incorporates a target bias strategy into the generation of random points in the expansion tree to guide the growth of nodes toward the target location. Furthermore, a bidirectional expansion strategy is introduced during the sampling process, where two trees are grown from the initial state and the target state, respectively, to accelerate environmental exploration and improve the efficiency of path planning. When the two trees meet, the reverse tree serves as heuristic information to guide the forward tree toward the target state, thereby quickly and efficiently generating a feasible path to the target location. Furthermore, a target node-based path reconnection strategy is employed to optimize path quality and shorten path length. Finally, cubic B-spline interpolation is used to smooth the path, and the curvature is constrained by adjusting the interpolation points of the B-spline curve to meet the vehicle's turning angle constraint. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] Figure 1 A schematic diagram of a process flow of a Bi-RT-RRT* logistics transportation vehicle path planning method provided in an embodiment of the present invention;

[0068] Figure 2 It is a diagram of the forward and reverse tree node expansion and encounter process;

[0069] Figure 3 Optimize the reconnection process graph for the initial target path node. DETAILED DESCRIPTION

[0070] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for describing the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be derived from these drawings without inventive efforts. The present invention will be further described in detail below with reference to the drawings.

[0071] A logistics transport vehicle path planning method based on Bi-RT-RRT* algorithm is shown in the attached Figure 1 As shown in the flow chart;

[0072] Step 1: Set the path planning parameters and initialize Qfstart, Sfgoal, Qfgoal and the expansion tree; Qfstart is a queue for storing forward tree nodes for rewiring, Sfgoal is a queue for storing initial feasible path nodes, and Qfgoal is a queue for storing expansion tree branch nodes; the path planning parameters include: starting point coordinates, target point coordinates, agent position coordinates, expansion step size, target bias sampling probability, sampling map and maximum number of iterations;

[0073] Step 2: Use the coordinates of the starting node and the target node to build a forward and reverse bidirectional tree; Figure 2 As shown in (a), the coordinates of the starting node and the target node are used to establish a forward tree extending from the starting node and a reverse tree extending from the target node respectively;

[0074] Step 3: Expand the new node using the target bias strategy, reconnect the newly generated nodes, and update the forward and reverse bidirectional trees; specifically:

[0075] Step 31: Before expanding the new node, the obstacle is expanded, that is, a safety distance constraint is added to avoid collision with obstacles when the vehicle is used as a mass point for path planning;

[0076]

[0077] Among them L safe is the safety distance of the obstacle expansion, L is the length of the vehicle outline;

[0078] Step 32: Introduce biased sampling probability p bias (0≤p bias ≤1), when expanding a new node, there is p bias The probability of the target point or target area is biased, and the random number r (0≤r≤1) is generated by the rand function. If r <p bias , bias sampling point x sample The sampling is biased towards the target area, that is, random sampling is generated in the target point or target area; if r≥p bias , then random sampling is generated in the free area of the global space, which can increase the chance of exploring the target direction while ensuring the probability completeness of the algorithm;

[0079]

[0080] where X goal is the target point or target area, X free is the global free area, p bias is the bias sampling probability;

[0081] Step 33: When expanding a new node, if x nreast and xnew There is no obstacle between the straight lines, and when x nreast and x new When the Euclidean distance between new Add to the extension tree;

[0082] Step 3 and 4: Reconnect to the current node. When the node (x i ) can obtain a lower connection cost c by passing from another node that is not its parent node i When rewiring is performed;

[0083] c i =Cost(x near )+d E (x near ,x i );

[0084]

[0085] where c i is x near Expand to x i Total cost of the node, Cost(x near ) is x i The initial cost at the node, d E (x near ,x i ) is x near ,x i The Euclidean distance cost between two nodes;

[0086] Step 35: When the root node of the expansion tree changes and the root node list is not empty, reconnect the nodes except the root node using the connection cost between nodes, i.e. c new <c old Reconnect and update the expansion tree;

[0087] c old =Cost(x near );

[0088] c new =Cost(x r )+dE(x r ,x near );

[0089] where x r is the second item in the root node list, c old is the cost of reconnecting to the node before reconnecting, c new The cost of the node after reconnection during the reconnection process;

[0090] Step 4: Determine whether the updated forward tree and backward tree meet. If so, execute step 6. If not, determine whether the number of sampling points is greater than the maximum number of iterations. If the total number of sampling points is greater than or equal to the maximum number of iterations, output path planning failure. If the total number of sampling points is less than the maximum number of iterations, execute step 5. Specifically:

[0091] Step 41: If Figure 2 As shown in (b), when the Euclidean distance between the latest nodes of the forward tree and the reverse tree is less than one expansion step, the two trees meet;

[0092] Step 42: If Figure 2 As shown in (c), if the two trees meet, the reverse tree node information is passed to the forward tree through the swap function, and the reverse tree is used as heuristic information to guide the forward tree to continue growing towards the target state, thereby generating an initial feasible path from the starting point to the target position, as shown in Figure 2 (d) If there is no encounter, determine whether the number of sampling points is greater than the maximum number of iterations; if the total number of sampling points is greater than or equal to the maximum number of iterations, output path planning failure; if the total number of sampling points is less than the maximum number of iterations, expand the sampling of new nodes again and update the forward and reverse trees;

[0093] Step 5: Use the nodes generated in step 3 to update the forward tree and reverse tree again, and then return to step 3;

[0094] Step 6: Pass the node information of the reverse tree to the forward tree, so that the reverse tree acts as heuristic information to guide the forward tree to continue growing towards the target state, thereby generating an initial feasible path from the starting point to the target position;

[0095] Step 7: The reverse tree stops growing and initializes, while the forward tree continues to grow toward the target position and explore the unknown environment. Specifically, after the two trees meet, the reverse tree stops growing and initializes, while the forward tree trunk grows toward the target position and the branches continue to explore the unknown environment.

[0096] Step 8: After obtaining an initial target path, a rewiring process is used to reduce the path length and optimize the path quality; specifically:

[0097] Step 81: Determine whether the number of valid nodes in the initial target path is greater than 2. If so, execute step 82; otherwise, terminate the path reconnection and directly output the path.

[0098] Step 82: If Figure 3As shown in (a), starting from the starting point (end point) of this path optimization reconnection, try to connect to the ith node (i is the total number of valid nodes in the initial target path of this path optimization, a = 2, 3, 4, ..., i-1) in sequence; before connecting, check whether there is a collision between the current node and the ith node; if there is no collision, directly connect the two nodes; otherwise, connect the end point and the previous node where the collision occurred, as shown in Figure 3 (e) as shown;

[0099] Step 83: If Figure 3 As shown in (b)-(d), repeat step 82 from the starting point of this path optimization reconnection (the previous node where the last optimization collision occurred) until the optimization reconnects to the starting point of the initial target path, and output the path after optimization reconnection and the path valid node coordinate information list P i (i=0,1,2,...,n);

[0100] Step 9: Smooth the rerouted path using cubic B-spline interpolation, and then constrain the curvature by adjusting the interpolation points of the B-spline curve to meet the vehicle corner constraint; specifically:

[0101] Step 91: Use the valid node list output after path optimization reconnection to perform interpolation processing. First, calculate the Euclidean distance between adjacent nodes, and then calculate the total Euclidean distance between the starting point and the end point. Based on the ratio of the Euclidean distance between adjacent nodes to the total distance, make the interpolation point interval proportional to the physical distance, thereby reducing the occurrence of curvature mutations during the interpolation process;

[0102] d k =||P k -P k-1 ||(k=1,2,...,n);

[0103]

[0104] N = 10 × D;

[0105] N k =u k ×N;

[0106] where d k is the Euclidean distance between adjacent points, rounded to one decimal place, D is the Euclidean distance between the starting point and the end point, u k is the ratio of the cumulative distance from the starting point to the point to the total distance, N is the total number of interpolation points, N k is the number of interpolation points between the starting point and this point;

[0107] Step 92: Use cubic B-spline curve to interpolate the processing path;

[0108] The expression of the cubic B-spline curve is:

[0109]

[0110]

[0111] where N i,p (u) is the P-order B-spline curve interpolation basis function, N i,3 (u) is the cubic B-spline curve interpolation basis function;

[0112] Step 93: Calculate the minimum turning radius of the vehicle using the maximum front wheel turning angle according to the Ackerman steering formula;

[0113] θ max =arctan(L / R min );

[0114] where θ max is the maximum turning angle of the front wheel, L is the vehicle wheelbase, R min is the minimum turning radius of the vehicle;

[0115] Calculate the curvature of the cubic B-spline curve and impose constraints;

[0116]

[0117] Where κ(u) is the curvature of the cubic B-spline curve, C'(u) is the first-order derivative of the cubic B-spline curve, C"(u) is the second-order derivative of the cubic B-spline curve, and N' i,3 (u k ) is the first-order derivative of the cubic B-spline curve basis function, N″ i,3 (u k ) is the second-order derivative of the cubic B-spline curve basis function, κ max is the maximum curvature of the cubic B-spline curve;

[0118] Step 95: Construct an optimization objective function to minimize the second-order differential energy of the control points, force adjacent control points to be approximately collinear, and reduce local curvature;

[0119]

[0120] Step 96: Construct the gradient information of the curvature constraint through automatic differentiation and solve the nonlinear programming problem in combination with the interior point method;

[0121] Step 10: Output the final path.

[0122] The present invention grows two trees from a starting state and a target state, respectively. When the two trees meet, the reverse tree serves as heuristic information to guide the forward tree to grow toward the target state, thereby quickly and efficiently generating a feasible path to the target location. Furthermore, a path reconnection strategy based on the target node is used to optimize path quality and shorten path length. Path quality is then optimized using cubic B-spline interpolation. The vehicle's minimum turning radius is then calculated using the Ackermann steering formula based on the maximum front wheel turning angle, and the curvature radius is optimized so that the arc radius of each path segment is greater than the minimum turning radius to meet the vehicle's turning angle constraint.

Claims

1. A logistics transport vehicle path planning method based on Bi-RT-RRT* algorithm, characterized in that: The following steps are involved: Step 1: Set the path planning parameters and initialize Qfstart, Sfgoal, Qfgoal and the expansion tree; Among them, Qfstart is the queue for storing the forward tree for rewiring nodes, Sfgoal is the queue for storing the initial feasible path nodes, and Qfgoal is the queue for storing the branch nodes of the expansion tree; The path planning parameters include: starting point coordinates, target point coordinates, agent position coordinates, expansion step size, target bias sampling probability, sampling map and maximum number of iterations; Step 2: Use the coordinates of the starting node and the target node to build a forward and reverse bidirectional tree; Step 3: Expand the new node using the target bias strategy, reconnect the newly generated nodes, and update the forward and reverse bidirectional trees; Step 4: Determine whether the updated forward tree and backward tree meet. If so, proceed to step 6. If not, determine whether the number of sampling points is greater than the maximum number of iterations. If the total number of sampling points is greater than or equal to the maximum number of iterations, output that the path planning failed. If the total number of sampling points is less than the maximum number of iterations, proceed to step 5. Step 5: Use the nodes generated in step 3 to update the forward tree and reverse tree again, and then return to step 3; Step 6: Pass the node information of the reverse tree to the forward tree, so that the reverse tree acts as heuristic information to guide the forward tree to continue growing towards the target state, thereby generating an initial feasible path from the starting point to the target position; Step 7: The backward tree stops growing and is initialized, and the forward tree continues to grow towards the target position and explore the unknown environment; Step 8: After obtaining an initial target path, a rewiring process is used to reduce the path length and optimize the path quality; Step 9: The rerouted path is smoothed by cubic B-spline interpolation, and then the curvature is constrained by adjusting the interpolation points of the B-spline curve to meet the vehicle turning angle constraint; Step 10: Output the final path.

2. The logistics transport vehicle path planning method based on the Bi-RT-RRT* algorithm according to claim 1 is characterized in that: In the step 2, a forward tree extending from the starting node and a reverse tree extending from the target node are established using the coordinates of the starting node and the target node respectively.

3. The logistics transport vehicle path planning method based on the Bi-RT-RRT* algorithm according to claim 1 is characterized in that: The step three is specifically as follows: Step 31: Before expanding the new node, expand the obstacle and add a safety distance constraint to avoid collision with the obstacle when the vehicle is used as a mass point for path planning; Among them L safe is the safety distance of the obstacle expansion, L is the length of the vehicle outline; Step 32: Introduce biased sampling probability p bias , 0≤p bias ≤1, when expanding a new node, there is p bias The probability of the target point or target area is biased, and the random number r is generated by the rand function, 0≤r≤1. <p bias , bias sampling point x sample Sampling is biased towards the target area, and samples are randomly generated within the target point or target area; If r≥p bias , then random sampling is generated in the free area of the global space, which increases the chance of exploring the target direction while ensuring the probability completeness of the algorithm; where X goal is the target point or target area, X free is the global free area, p bias is the bias sampling probability; Step 33: When expanding a new node, if the nearest node x nreast and the new node x new There is no obstacle between them, and when x nreast and x new When the Euclidean distance between new Add to the extension tree; Step 3 and 4: Reconnect to the current node. When node x i Rewiring is performed when a lower connection cost ci is obtained by passing from another node that is not its parent node; c i =Cost(x near )+d E (x near ,x i ); where c i is x near Expand to x i Total cost of the node, Cost(x near ) is x i The initial cost at the node, d E (x near ,x i ) is x near ,x i The Euclidean distance cost between two nodes; Step 35: When the root node of the expansion tree changes and the root node list is not empty, reconnect the nodes except the root node using the connection cost between nodes, i.e. c new <c old Reconnect and update the expansion tree; c old =Cost(x near ); c new =Cost(x r )+dE(x r ,x near ); where x r is the second item in the root node list, c old is the cost of reconnecting to the node before reconnecting, c new It is the cost of the node after reconnection during the process of reconnecting the node.

4. The logistics transport vehicle path planning method based on the Bi-RT-RRT* algorithm according to claim 1 is characterized in that: Step 4 is as follows: Step 41: When the Euclidean distance between the latest nodes of the forward tree and the reverse tree is less than one expansion step, the two trees meet; Step 42: If the two trees meet, the reverse tree node information is passed to the forward tree through the swap function, so that the reverse tree acts as heuristic information to guide the forward tree to continue growing towards the target state, thereby generating an initial feasible path from the starting point to the target position; if there is no meeting, determine whether the number of sampling points is greater than the maximum number of iterations; If the total number of sampling points is greater than or equal to the maximum number of iterations, the output path planning fails; if the total number of sampling points is less than the maximum number of iterations, the new sampling nodes are expanded again and the forward and reverse trees are updated.

5. The logistics transport vehicle path planning method based on the Bi-RT-RRT* algorithm according to claim 1 is characterized in that: The step seven is specifically as follows: after the two trees meet, the reverse tree stops growing and is initialized, the forward tree trunk grows toward the target position, and the branches continue to explore the unknown environment.

6. The logistics transport vehicle path planning method based on the Bi-RT-RRT* algorithm according to claim 1 is characterized in that: The step eight is specifically as follows: Step 81: Determine whether the number of valid nodes in the initial target path is greater than 2. If so, execute step 82; otherwise, terminate the path reconnection and directly output the path. Step 82: From the starting point of this path optimization reconnection, try to connect to the ith node in sequence, where i is the total number of valid nodes in the initial target path of this path optimization, and a = 2, 3, 4, ..., i-1. Before connecting, check whether there is a collision between the current node and the ith node. If there is no collision, directly connect the two nodes; otherwise, connect the end point to the node before the collision occurred. Step 83: Repeat step 82 from the starting point of this path optimization reconnection until the optimization reconnection reaches the starting point of the initial target path, and output the path after optimization reconnection and the path valid node coordinate information list P i (i=0,1,2,...,n).

7. The logistics transport vehicle path planning method based on the Bi-RT-RRT* algorithm according to claim 6 is characterized in that: The step nine is specifically as follows: Step 91: Use the valid nodes output after path optimization reconnection to perform interpolation processing. First, calculate the Euclidean distance between adjacent nodes, and then calculate the total Euclidean distance between the starting point and the end point. Based on the ratio of the Euclidean distance between adjacent nodes to the total distance, make the interpolation point interval proportional to the physical distance to reduce the occurrence of curvature mutation during the interpolation process. d k =||P k -P k-1 ||(k=1,2,...,n); N = 10 × D; N k =u k ×N; where d k is the Euclidean distance between adjacent points, rounded to one decimal place, D is the Euclidean distance between the starting point and the end point, u k is the ratio of the cumulative distance from the starting point to the point to the total distance, N is the total number of interpolation points, N k is the number of interpolation points between the starting point and this point; Step 92: Use cubic B-spline curve to interpolate the processing path; The expression of the cubic B-spline curve is: where N i,p (u) is the P-order B-spline curve interpolation basis function, N i,3 (u) is the cubic B-spline curve interpolation basis function; Step 93: Calculate the minimum turning radius of the vehicle using the maximum front wheel turning angle according to the Ackerman steering formula; θ max =arctan(L / R min ); where θ max is the maximum turning angle of the front wheel, L is the vehicle wheelbase, R min is the minimum turning radius of the vehicle; Step 94: Calculate the curvature of each point of the cubic B-spline curve and apply constraints; Where κ(u) is the curvature of the cubic B-spline curve, C'(u) is the first-order derivative of the cubic B-spline curve, C"(u) is the second-order derivative of the cubic B-spline curve, and N' i,3 (u k ) is the first-order derivative of the cubic B-spline curve basis function, N″ i,3 (u k ) is the second-order derivative of the cubic B-spline curve basis function, κ max is the maximum curvature of the cubic B-spline curve; Step 95: Construct an optimization objective function to minimize the second-order differential energy of the control points, force adjacent control points to be approximately collinear, and reduce local curvature; Step 96: Construct the gradient information of the curvature constraint through automatic differentiation and combine it with the interior point method to solve the nonlinear programming problem.

Images