A Search-and-Optimization-Based Method for Multi-Autonomous Vehicle Phasing Trajectory Planning

Abstract

This invention belongs to the field of robot path planning technology, specifically relating to a multi-unmanned vehicle (UAV) platoon trajectory planning method based on search and optimization. The specific process is as follows: Step 1: Set the desired platoon configuration, calculate the starting point and target point of each UAV, and set the initial platoon orientation and the orientation at the target point. Step 2: Based on the starting point and target point, and according to the grid map, perform path search and waypoint processing for the virtual navigator and each UAV in the platoon, and construct a safe corridor. Step 3: Under the constraint of the safe corridor, based on the set initial platoon orientation and the orientation at the target point, and using the trajectory optimization of the virtual navigator as a reference benchmark for the trajectory optimization of each UAV, construct a multi-UAV platoon cooperative trajectory optimization function, and perform multi-UAV platoon cooperative trajectory optimization.

Description

Technical Field

[0001] This invention belongs to the field of robot path planning technology, specifically relating to a multi-unmanned vehicle platoon trajectory planning method based on search and optimization. Background Technology

[0002] In recent years, with the development of robotics technology and the deepening of theoretical research on multi-agent systems, multi-unmanned vehicle (UAV) swarms have been widely used in industrial production and military fields. UAV trajectory planning is a crucial step in the autonomous execution of tasks by UAVs, playing a vital role in various fields such as military, agriculture, and logistics. Compared to individual UAVs, multi-UAV swarm collaboration is more efficient and has a wider range of applications. Swarming is one of the important methods for multi-UAV swarm collaboration, significantly improving the efficiency, safety, and flexibility of multi-UAV task completion. Therefore, researching multi-UAV swarm trajectory planning methods is extremely important.

[0003] In recent years, multi-vehicle trajectory planning has been extensively studied by scholars both domestically and internationally. Currently, methods based on conflict and machine learning exist, capable of generating conflict-free trajectories for multiple autonomous vehicles. However, research on multi-vehicle trajectory planning in platooning scenarios is scarce. Therefore, this invention addresses this issue by proposing a multi-vehicle platooning cooperative trajectory planning method from a search and optimization perspective. This method can generate smooth, collision-free trajectories for multiple autonomous vehicles that satisfy kinematic constraints. Summary of the Invention

[0004] This invention addresses the trajectory planning problem of multi-unmanned vehicle platoons. From the perspective of search and optimization, it proposes a multi-unmanned vehicle platoon trajectory planning method based on search and optimization, which can generate smooth, collision-free platoon trajectories of multiple unmanned vehicles that satisfy kinematic constraints.

[0005] The technical solution for implementing the present invention is as follows:

[0006] A multi-unmanned vehicle platooning trajectory planning method based on search and optimization, the specific process of which is as follows:

[0007] Step 1: Set the desired formation configuration, calculate the starting point and target point of each unmanned vehicle, and set the initial formation orientation and the orientation when the formation reaches the target point;

[0008] Step 2: Based on the starting point and the target point, and according to the grid map, perform path search and waypoint processing for the virtual navigator and each unmanned vehicle in the formation, and construct a safe corridor;

[0009] Step 3: Under the constraints of the safe corridor, based on the initial formation orientation and the orientation of the formation when it reaches the target point, and using the trajectory optimization of the virtual navigator as the reference benchmark for the trajectory optimization of each unmanned vehicle, construct a multi-unmanned vehicle formation collaborative trajectory optimization function to perform multi-unmanned vehicle formation collaborative trajectory optimization.

[0010] Furthermore, in step two of this invention, a new formation heuristic term k(n) is added to the A* path search algorithm to obtain an improved A* path search algorithm. The k(n) is calculated as the distance k(n) between the node to be expanded and the nearest reference path point to the current node n, thus constructing a safe corridor.

[0011] Furthermore, the A* path search algorithm described in this invention is as follows:

[0012] f(n)=g(n)+λ h ·h(n)+λ k ·k(n),

[0013] Where f(n), g(n), and h(n) represent the total cost of expanding child nodes, the actual cost from the starting node to the current node, and the estimated value of the heuristic function, respectively, and λ h and λ k These are the weights of h(n) and k(n), respectively.

[0014] Furthermore, the specific process of step two in this invention is as follows:

[0015] First, based on the starting point and target point of each unmanned vehicle set in step one, the A* algorithm is used to search for a path as the baseline path.

[0016] Secondly, for each autonomous vehicle search path, when the parent node searches for child nodes each time, the path points in the baseline path are traversed, and the path point with the shortest Euclidean distance between the parent node and all path points in the baseline path is selected as the baseline point for the expansion of the child node.

[0017] Next, at the reference point, the child nodes of the parent node are expanded according to the improved A* algorithm expansion rule;

[0018] Then, after densification of each path, the number of waypoints of each path is normalized.

[0019] Finally, the path is traversed point by point to generate a safe corridor.

[0020] Furthermore, the specific process of step three in this invention is as follows:

[0021] (1) Optimize the trajectory of a single unmanned vehicle for the virtual navigator;

[0022] (2) Using the optimized trajectory of the virtual navigator as a benchmark, and under the constraints of the safe corridor and the formation orientation at the initial and destination points, a multi-unmanned vehicle formation cooperative trajectory optimization function is constructed to optimize the multi-unmanned vehicle formation cooperative trajectory; the specific process is as follows:

[0023] First, based on the desired formation configuration matrix F des Calculate the vector A formed by the lines connecting each unmanned vehicle to the center point of the formation;

[0024] Secondly, let the initial formation orientation be denoted as φ1, the orientation at the target point be denoted as φ2, the number of waypoints be denoted as H, and the direction matrix be denoted as D with H elements; then, the first element D1 and the last element D2 of D are set together. H φ1 and φ2 are assigned values ​​respectively, and the other elements of D are selected from the path searched by the virtual navigator according to the principle of equal spacing;

[0025] Next, based on the vector A and the direction matrix D, the time-varying expected formation vector matrix A between the multiple unmanned vehicles in motion state is calculated. j ;

[0026] Finally, under the constraint of the safety corridor, based on the desired formation vector matrix, a multi-unmanned vehicle formation cooperative trajectory optimization problem is constructed, and multi-unmanned vehicle formation cooperative trajectory optimization is performed.

[0027] Furthermore, the present invention describes the construction of a multi-unmanned vehicle platoon cooperative trajectory optimization problem, and the specific process of performing multi-unmanned vehicle platoon cooperative trajectory optimization is as follows:

[0028] First, the objective function is set to include a smoothing term and a deviation term from the discrete path. Then, a formation cost term is constructed using a rotation matrix and added to the objective function to construct the trajectory optimization problem of the autonomous vehicle formation.

[0029] Secondly, the constraints include start and end point constraints, vehicle kinematic constraints, safety corridor constraints, and inter-vehicle collision avoidance constraints;

[0030] Finally, using the path found by the front end as the initial solution, numerical calculation methods are used to solve the optimization problem for each unmanned vehicle in the formation.

[0031] Furthermore, the multi-unmanned vehicle platoon cooperative trajectory optimization problem described in this invention is as follows:

[0032]

[0033] in, It is the difference between the control inputs of two adjacent trajectory points of the i-th autonomous vehicle. It is the deviation between the j-th trajectory point of the i-th autonomous vehicle and its reference path; The formation cost of the i-th autonomous vehicle at the j-th trajectory point is given by: Let J be the pose of the virtual navigator at the j-th trajectory point. Let i be the pose of the i-th autonomous vehicle at the j-th trajectory point. Let H represent the position of the j-th trajectory point of the i-th autonomous vehicle, H be the number of trajectory points, P and Q be the positive definite weight matrices of the smoothing and deviation terms respectively, T be the weight coefficient of the formation term, and R be the weight of the formation term. safe The collision radius of the autonomous vehicle. This represents the safe corridor corresponding to the j-th trajectory point of the i-th autonomous vehicle. This represents the range of control inputs for the i-th autonomous vehicle.

[0034] Beneficial effects:

[0035] First, this invention proposes a multi-unmanned vehicle platooning cooperative trajectory planning method, which combines front-end path search with back-end trajectory optimization to generate smooth, collision-free platooning trajectories for multiple unmanned vehicles that satisfy kinematic constraints.

[0036] Secondly, this invention proposes an improved A* algorithm based on the A* algorithm. A deviation term from the formation center point is added to the heuristic function of A* search, so that the paths searched by multiple unmanned vehicles pass through obstacles from the same direction, which effectively improves the effect of multiple unmanned vehicles searching for paths at the same time and is conducive to improving the efficiency of backend multi-unmanned vehicle trajectory optimization.

[0037] Third, this invention constructs a multi-unmanned vehicle (UAV) platoon trajectory optimization problem and verifies its solvability and effectiveness. The platoon turning angle information is obtained using the optimized trajectory of a virtual navigator, and the expected relative positions between the multiple UAVs are calculated through a rotation matrix, achieving overall formation rotation. Furthermore, the proposed method only requires changing the expected position distribution matrix to alter the number of UAVs and the platoon formation, demonstrating good scalability. Attached Figure Description

[0038] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0039] Figure 1 This is a schematic diagram of the overall architecture of the invention.

[0040] Figure 2 Methods for defining desired formation configurations and formation rotation angles;

[0041] Figure 3 To improve the flowchart of the A* algorithm;

[0042] Figure 4 A comparison diagram of the improved A* algorithm and the A* algorithm; (a) simulation results of the classic A* search, (b) simulation results of the improved A* search;

[0043] Figure 5 Generate a schematic diagram for the safety corridor;

[0044] Figure 6 This is a schematic diagram of the trajectory optimization results; Detailed Implementation

[0045] The embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0046] It should be noted that, in the absence of conflict, the following embodiments and features can be combined with each other; and, based on the embodiments of this disclosure, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this disclosure.

[0047] It should be noted that various aspects of embodiments within the scope of the appended claims are described below. It will be apparent that the aspects described herein can be embodied in a wide variety of forms, and any particular structure and / or function described herein is merely illustrative. Based on this disclosure, those skilled in the art will understand that one aspect described herein can be implemented independently of any other aspect, and two or more of these aspects can be combined in various ways. For example, any number of aspects set forth herein can be used to implement the device and / or practice the method. Additionally, this device and / or method can be implemented using structures and / or functionalities other than one or more of the aspects set forth herein.

[0048] Steps one and two are the front-end path search process, and the flow is as follows: Figure 1 As shown; Step 3 is to optimize the problem construction and solution process, the flow of which is as follows: Figure 3 As shown.

[0049] The problem addressed by this invention is as follows: Assume a known two-dimensional task scenario contains several obstacles and N unmanned vehicles. Each unmanned vehicle is assigned a number i, and its task starting point is s. i The target point is g i The trajectory planning task of the autonomous vehicle i is to find a feasible trajectory so that it can move from s i Move to g without collision i The set of discrete path points obtained by autonomous vehicle i through path search is denoted as . in, Let p be a 2×1 column vector representing the position of the j-th path point of the autonomous vehicle starting from the starting point, and L represent the number of path points contained in this path. Let p be a discrete set of path points. i Let be the initial solution, and let be the set of trajectory points of the unmanned vehicle i obtained through trajectory optimization. in Let t be a 3×1 column vector representing the pose of the autonomous vehicle at the j-th trajectory point from the starting point. k =kΔt represents the time corresponding to the kth trajectory point, where Δt is the unit time, and H represents the trajectory q. i The number of trajectory points included.

[0050] This application provides a multi-unmanned vehicle platooning trajectory planning method based on search and optimization, the specific process of which is as follows:

[0051] Step 1: Set the desired formation configuration, calculate the starting point and target point of each unmanned vehicle, and set the initial formation orientation and the orientation when the formation reaches the target point;

[0052] The desired formation configuration is the desired formation shape of the autonomous vehicles, defined by the relative positions of each vehicle.

[0053] First, select one autonomous vehicle (referred to as vehicle 1) as the origin of the coordinate system. The desired positions of the other autonomous vehicles relative to vehicle 1 can then be represented by a two-dimensional vector. By repeating this process, a desired formation configuration matrix F with dimensions N×2 can be obtained. des , where N is the number of driverless cars.

[0054] Subsequently, the starting point and target point of the formation center are defined in the two-dimensional grid map. Based on the desired formation configuration matrix, the starting point s of each autonomous vehicle can be calculated. i and target point g i .

[0055] Then, define the orientation angles of the formation at the starting and target points (0 degrees to the right horizontally, and the angles increase counterclockwise). Figure 1 This paper describes the method for constructing the desired formation configuration of three autonomous vehicles and the meaning of the rotation angles. Solid lines represent the initial formation positions, and dashed lines represent the target formation positions; 1, 2, and 3 represent the autonomous vehicles, and 0 represents the virtual navigator (formation center point). The desired formation configuration matrix F is... des =[0,0; 2,4; 4,0], initial formation orientation angle is The target orientation angle is the angle θ between the formation orientation and the horizontal direction.

[0056] Step 2: Based on the starting point and the target point, and according to the grid map, perform path search and waypoint processing for the virtual navigator and each unmanned vehicle in the formation, and construct a safe corridor;

[0057] Use the starting and target coordinates of the formation selected in Step 1 as the starting and target coordinates of the virtual navigator. Use the A* algorithm to search for a feasible path for the virtual navigator. The child node expansion formula for the A* algorithm is as follows:

[0058] f(n)=g(n)+h(n) (1)

[0059] Where f(n), g(n), and h(n) represent the total cost of expanding child nodes, the actual cost from the starting node to the current node, and the estimated value of the heuristic function, respectively.

[0060] When multiple autonomous vehicles (UAVs) perform path search, if each UAV uses the A* algorithm separately, obstacles may be encountered where multiple UAVs attempt to bypass them from different directions. This results in a poor initial solution provided by the front-end search for the back-end trajectory optimization, leading to low efficiency or even no solution for the back-end optimization problem. To address this issue, this invention designs an improved A* algorithm for platoon path search. The child node expansion formula of the improved A* algorithm is as follows:

[0061] f(n)=g(n)+λ h ·h(n)+λ k ·k(n), (2)

[0062] Where f(n), g(n), and h(n) have the same meanings as in the A* algorithm, and k(n) is a newly added formation heuristic term, calculated as the distance between the node to be expanded and the nearest reference path point to the current node n. λ h and λ k These are the weights of h(n) and k(n), respectively.

[0063] The algorithm first performs a path search on the formation center point, and then uses it as a reference to complete the path search for each vehicle. Compared with the original A* algorithm's unrelated search, the improved A* algorithm enables the paths found by each autonomous vehicle to bypass obstacles from the same direction, providing a good initial solution for the subsequent trajectory optimization. Based on the searched paths, the number of path points for each vehicle is densified and normalized, and then spatial expansion is performed point by point to generate safe corridors, which serve as one of the hard constraints of the subsequent optimization problem.

[0064] The specific steps of the improved A* algorithm are as follows: First, according to the expansion rule of equation (1), use the A* algorithm to search for a path as the base path. Then, for each autonomous vehicle search path, when the parent node searches for child nodes each time, traverse the path points in the base path and select the one with the shortest Euclidean distance between the parent node and all path points in the base path as the base point for child node expansion. Subsequently, expand the child nodes of the parent node according to the expansion rule of equation (2), that is, add the distance term from the child node to the base node on the basis of the A* algorithm. The flowchart of the improved A* search algorithm is as follows. Figure 3 As shown, its performance is compared with that of the A* algorithm. Figure 4 As shown.

[0065] The virtual navigator and the discrete paths searched by each autonomous vehicle are a set of waypoints. After obtaining the discrete paths, each path is first densified, then the number of waypoints on each path is normalized, and finally, the paths are traversed point by point to generate safe corridors.

[0066] The densification process involves interpolating the coordinates of every two points in the path at equal intervals and inserting a custom number of waypoints. This operation is used to ensure the continuity of the safe corridor.

[0067] The waypoint number normalization process selects the path with the most waypoints among the discrete paths searched by the virtual navigator and each autonomous vehicle as the baseline path. The last waypoint of other paths is continuously copied and added to the end of the path until the number of waypoints is the same as that of the baseline path, so that the number of waypoints of each path is normalized. This operation is used to ensure that the timestamps of each autonomous vehicle are consistent during the back-end trajectory optimization process.

[0068] The safe corridor is generated using the dilatation method. It is based on path points. For example, taking For the initial safe corridor space, examine all grid cells within a rectangular area in the four directions (+x, +y, -x, -y). If all grid cells in a certain direction are empty, add those cells to the safe corridor set; if they are obstacle grid cells, stop expanding the space in that direction. Repeat the above steps until no direction can be expanded further, at which point the pathpoint is obtained. The corresponding largest rectangular safety corridor is denoted as It is a row vector containing four elements, where each element represents the row and column number of the top, bottom, left, and right edges of the safety corridor in the raster map, respectively. The safety corridor generation effect is as follows: Figure 5 As shown.

[0069] Step 3: Optimize the trajectory of the virtual navigator as a reference benchmark for the trajectory optimization of each autonomous vehicle, and construct the formation cost based on it. Solve the trajectory optimization problem for each autonomous vehicle in turn.

[0070] (1) Optimize the trajectory of the virtual navigator for a single unmanned vehicle.

[0071] To obtain a reference trajectory for multi-vehicle platooning and to design the platooning cost function, we first optimize the trajectory of a single autonomous vehicle for the virtual navigator. The optimization problem is as follows:

[0072]

[0073] Equation (3a) is the cost function for this optimization problem. It is the difference in control input between two adjacent trajectory points (the j-th and (j-1)-th trajectory points) of the i-th autonomous vehicle, denoted as: Let $\mathbf{j}$ be the deviation between the $i$-th autonomous vehicle's $j$-th trajectory point and its reference path, denoted as $\mathbf{j}$. The smoothing term and the deviation term are represented using quadratic forms, where the smoothing term... To minimize the change in control input between any two trajectory points of the autonomous vehicle, the deviation term Ensure that the deviation between the trajectory point and the reference path point is as small as possible. 2×2 Q 2×2 These are the positive definite weight matrices for the smoothing term and the deviation term, respectively.

[0074] (2) Using the optimized trajectory of the virtual navigator at the formation center as a benchmark, construct the formation cost for each unmanned vehicle in the formation. Specifically:

[0075] First, based on the desired formation configuration matrix F des Calculate the vector formed by the lines connecting each unmanned vehicle to the center point of the formation.

[0076] by Figure 2 Taking a platoon of three autonomous vehicles as an example, the desired platoon configuration matrix F des = [0,0; 2,4; 4,0], then the coordinates of the formation center point in the formation coordinate system are F. des The average of each column, i.e. Then, the vectors connecting each unmanned vehicle to the center point of the formation are calculated to form the desired formation vector matrix. It is worth noting that during the platooning of multiple unmanned vehicles traveling along the trajectory points, the formation needs to change in real time as the platoon's motion changes. When the platoon is traveling in a straight line, the formation remains unchanged; when the platoon turns, the formation needs to be transformed according to the turning angle. That is, the aforementioned desired relative position vector needs to be rotated using a two-dimensional rotation matrix, as follows:

[0077] Assume the initial formation orientation is set to φ1, the orientation at the target point is set to φ2, and the number of waypoints is H. Create a direction matrix denoted as D, with H elements, to store the formation's rotation angles. Then, combine the first element D1 and the last element D2 of D. H φ1 and φ2 are assigned values ​​respectively, and the other elements of D are selected from the path searched by the virtual navigator according to the principle of equal spacing. Taking the j-th trajectory point as an example, assume that D has... j =θ j Then the two-dimensional rotation matrix of the relative positions of the formation at this time is:

[0078]

[0079] Left-multiply the desired formation vector matrix A by the rotation matrix R. j This allows us to obtain the time-varying expected formation vector matrix A among the multiple unmanned vehicles in motion. j =R j A.

[0080] Secondly, the following problem can be constructed: multi-unmanned vehicle platoon cooperative trajectory optimization.

[0081]

[0082] Equation (5a) is the cost function for this optimization problem. It is the difference between the control inputs of the i-th autonomous vehicle and two adjacent trajectory points (the j-th and j-1-th trajectory points); It is the deviation between the j-th trajectory point of the i-th autonomous vehicle and its reference path; The formation cost of the i-th autonomous vehicle at the j-th trajectory point is given by: Let be the pose of the virtual navigator at the j-th trajectory point. The smoothing term, deviation term, and formation term are all represented using quadratic forms, where the smoothing term... To ensure that the change in control input between any two trajectory points is not too large, the deviation term To ensure that the deviation between the trajectory points and the reference path points is not too large, the formation item This ensures that the multiple autonomous vehicles maintain the desired formation as much as possible. Furthermore, H represents the number of trajectory points, and P... 2×2 Q 2×2 R are the positive definite weight matrices for the smoothing and deviation terms, respectively, and T is the weight coefficient of the formation term (which is a constant). safe This represents the collision radius of the autonomous vehicle.

[0083] Equations (5b)-(5f) represent the constraints of this optimization problem. Their meanings are as follows: Equation (4b) represents the starting point and target point constraints, meaning the first and last points of the generated trajectory must satisfy the pre-defined starting and ending point coordinate requirements. Equation (5c) represents the kinematic model constraints, meaning the state update must satisfy the state update equation of the autonomous vehicle's kinematic model. Equation (5d) represents the safety corridor constraints, meaning the coordinates of all trajectory points must be within their corresponding safety corridor range. Equation (5e) represents the minimum / maximum control input constraints, meaning the control input (linear velocity and angular velocity) at each trajectory point must be within the set minimum / maximum control input range. Equation (5f) represents the vehicle collision avoidance constraints, meaning that at any given moment, the distance between any two autonomous vehicles in the formation must be greater than the minimum safe distance (here, twice the autonomous vehicle collision radius R). safe This is to prevent multiple vehicles from colliding with each other while in motion.

[0084] By solving the above nonlinear multi-constraint optimization problem using numerical methods such as Sequential Quadratic Programming (SQP), the smooth, collision-free trajectories of each unmanned vehicle that maintain formation can be obtained.

[0085] The effectiveness of the method proposed in this invention is verified through an example below. Taking N=3, the initial formation rotation angle is... Target position formation rotation angle Desired formation configuration F des = [0,0; 2,4; 4,0], the collision radius R of the unmanned vehicle safe =0.5, weight matrix P = [100, 0; 0, 100], Q = [0.1, 0; 0, 0.1], R = 3. The solution effect using the SQP algorithm is as follows: Figure 6 As shown, the trajectory planning method proposed in this invention can obtain smooth, collision-free trajectories for multiple unmanned vehicles while maintaining their formation.

[0086] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A multi-unmanned vehicle platooning trajectory planning method based on search and optimization, characterized in that, The specific process is as follows: Step 1: Set the desired formation configuration, calculate the starting point and target point of each unmanned vehicle, and set the initial formation orientation and the orientation when the formation reaches the target point; Step 2: Based on the starting point and the target point, and according to the grid map, perform path search and waypoint processing for the virtual navigator and each unmanned vehicle in the formation, and construct a safe corridor; Step 3: Under the constraint of the safe corridor, based on the initial formation orientation and the orientation of the formation when it reaches the target point, the trajectory optimization of the virtual navigator is used as the reference benchmark for the trajectory optimization of each unmanned vehicle, and the formation cost is constructed accordingly. The trajectory optimization problem is solved for each unmanned vehicle in turn. In step two, The path search algorithm adds a new swarm heuristic. Improved Path search algorithm, the The calculation method is to compare the distance between the node to be expanded and the distance from the current node. Distance to the nearest baseline path point Construct a safe corridor; the aforementioned The path search algorithm is as follows: in, , and These represent the total cost of expanding child nodes, the actual cost from the starting node to the current node, and the estimated value of the heuristic function, respectively. and They are respectively and The weight.

2. The multi-unmanned vehicle platooning trajectory planning method based on search and optimization according to claim 1, characterized in that, The specific process of step two is as follows: First, based on the starting and target points of each autonomous vehicle set in step one, using... The algorithm searches for a path as the baseline path; Secondly, for each autonomous vehicle search path, when the parent node searches for child nodes each time, the path points in the baseline path are traversed, and the path point with the shortest Euclidean distance between the parent node and all path points in the baseline path is selected as the baseline point for the expansion of the child node. Again, at the aforementioned reference point, according to the improved The algorithm expands the child nodes of the parent node according to the expansion rules; Then, after densification of each path, the number of waypoints of each path is normalized. Finally, the path is traversed point by point to generate a safe corridor.

3. The multi-unmanned vehicle platooning trajectory planning method based on search and optimization according to claim 1, characterized in that, The specific process of step three is as follows: (1) Optimize the trajectory of a single unmanned vehicle for the virtual navigator; (2) Using the optimized trajectory of the virtual navigator as a benchmark, and under the constraints of the safe corridor and the formation orientation at the initial and destination points, a multi-unmanned vehicle formation cooperative trajectory optimization function is constructed to optimize the multi-unmanned vehicle formation cooperative trajectory; the specific process is as follows: First, based on the desired formation configuration matrix Calculate the vector formed by the lines connecting each unmanned vehicle to the center point of the formation. ; Secondly, the initial formation orientation is denoted as The orientation of the formation when it reaches the target point is denoted as The number of path points is denoted as Create a direction matrix denoted as The number of its elements is ;Will The first element and the last element Assignment respectively and , Other elements are assigned values ​​in the path searched by the virtual navigator according to the principle of equal spacing; Again, based on the vector and direction matrix Calculate the time-varying expected formation vector matrix between multiple unmanned vehicles under motion conditions. ; Finally, under the constraint of the safety corridor, based on the desired formation vector matrix, a multi-unmanned vehicle formation cooperative trajectory optimization problem is constructed, and multi-unmanned vehicle formation cooperative trajectory optimization is performed.

4. The multi-unmanned vehicle platooning trajectory planning method based on search and optimization according to claim 3, characterized in that, The problem of constructing a multi-unmanned vehicle platoon cooperative trajectory optimization is addressed by performing multi-unmanned vehicle platoon cooperative trajectory optimization, specifically as follows: First, the objective function is set to include a smoothing term and a deviation term from the discrete path. Then, a formation cost term is constructed using a rotation matrix and added to the objective function to construct the trajectory optimization problem of the autonomous vehicle formation. Secondly, the constraints include start and end point constraints, vehicle kinematic constraints, safety corridor constraints, and inter-vehicle collision avoidance constraints; Finally, using the path found by the front end as the initial solution, numerical calculation methods are used to solve the optimization problem for each unmanned vehicle in the formation.

5. The multi-unmanned vehicle platooning trajectory planning method based on search and optimization according to claim 4, characterized in that, The problem of optimizing the cooperative trajectory of a multi-unmanned vehicle platoon is as follows: in, It is the first The difference in control input between two adjacent trajectory points of an autonomous vehicle. It is the first The first driverless car The deviation of each trajectory point from its reference path; For the first The first driverless car in The formation cost at each trajectory point, among which For the first virtual navigator pose at each trajectory point For the first The first driverless car pose at each trajectory point Indicates the first The first driverless car The position of each trajectory point The number of trajectory points, , These are the positive definite weight matrices for the smoothing term and the bias term, respectively. The weighting coefficients for the formation item are... The collision radius of the autonomous vehicle. Indicates the first The first driverless car The safety corridor corresponding to each trajectory point Indicates the first The range of control inputs for an autonomous vehicle.

Images