Multi-unmanned vehicle formation trajectory planning method based on search and optimization

Abstract

The invention belongs to the technical field of robot path planning, and particularly relates to a multi-unmanned vehicle formation trajectory planning method based on search and optimization. The method specifically comprises the steps of 1, setting an expected formation configuration, calculating a starting point and a target point of each unmanned vehicle, and setting an initial formation orientation and an orientation of the formation reaching the target point; 2, based on the starting point and the target point, path searching and path point processing are carried out on the virtual navigator and all the unmanned vehicles in the formation according to the grid map, and a safety corridor is constructed; and step 3, under the constraint of the safe corridor, based on the set initial formation orientation and the orientation of the formation reaching the target point, taking the trajectory optimization of the virtual navigator as the reference basis of the trajectory optimization of each unmanned vehicle, constructing a multi-unmanned vehicle formation cooperative trajectory optimization function, and performing multi-unmanned vehicle formation cooperative trajectory optimization.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot path planning, and in particular relates to a multi-unmanned vehicle formation trajectory planning method based on search and optimization. Background Art

[0002] In recent years, with the development of robotics technology and the deepening of theoretical research on multi-agent systems, multi-unmanned vehicle formations have been widely used in industrial production and military fields. Unmanned vehicle trajectory planning is one of the important links in the autonomous execution of tasks by unmanned vehicles, and plays an important role in many fields such as military, agriculture, and logistics. Compared with a single unmanned vehicle, the collaborative operation of a multi-unmanned vehicle cluster is more efficient and has a wider range of applications. Formation is one of the important ways for multi-unmanned vehicle clusters to collaborate, which greatly improves the efficiency, safety, and flexibility of multi-unmanned vehicles in completing tasks. Therefore, it is very important to study the trajectory planning method of multi-unmanned vehicle formations.

[0003] In recent years, multi-unmanned vehicle trajectory planning has been widely studied by scholars at home and abroad. Currently, there are conflict-based methods, machine learning-based methods, etc., which can solve the generation of conflict-free trajectories for multiple unmanned vehicles. However, there are few related studies on multi-unmanned vehicle trajectory planning in formation collaboration scenarios. Therefore, in response to such problems, the present invention proposes a multi-unmanned vehicle formation collaborative trajectory planning method from the perspective of search and optimization, which can generate smooth, collision-free formation trajectories for multiple unmanned vehicles that meet kinematic constraints. Summary of the invention

[0004] Aiming at the trajectory planning problem of multiple unmanned vehicle formations, the present invention proposes a trajectory planning method for multiple unmanned vehicle formations based on search and optimization from the perspective of search and optimization, which can generate smooth, collision-free formation trajectories of multiple unmanned vehicles that meet kinematic constraints.

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

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

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

[0008] Step 2: Based on the starting point and the target point, according to the grid map, the virtual navigator and each unmanned vehicle in the formation will search for paths and process path points, and build a safe corridor;

[0009] Step 3: Under the constraint of the safety corridor, based on the setting of 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 a multi-unmanned vehicle formation collaborative trajectory optimization function is constructed to perform multi-unmanned vehicle formation collaborative trajectory optimization.

[0010] Furthermore, in step 2, the present invention adds a newly added formation heuristic item k(n) to the A* path search algorithm to obtain an improved A* path search algorithm, wherein the k(n) is calculated as the distance k(n) between the node to be expanded and the reference path point closest to the current node n, thereby constructing a safe corridor.

[0011] Furthermore, the A* path search algorithm of the present invention is:

[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 the child node, the actual cost from the starting node to the current node, and the estimated value of the heuristic function, respectively. h and λ k are the weights of h(n) and k(n) respectively.

[0014] Furthermore, the specific process of step 2 of the present invention is:

[0015] First, based on the starting point and target point of each unmanned vehicle set in step 1, use the A* algorithm to search for a path as the reference path;

[0016] Secondly, search for paths for each unmanned vehicle. Each time the parent node searches for a child node, it traverses the path points in the reference path and selects the one with the shortest Euclidean distance between the parent node and all the path points in the reference path as the reference point for the child node expansion.

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

[0018] Then, after densifying each path, the number of waypoints in 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 of the present invention is:

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

[0022] (2) Taking the optimized trajectory of the virtual navigator as a benchmark, under the constraints of the safety corridor and the formation orientation at the initial and target points, a multi-unmanned vehicle formation collaborative trajectory optimization function is constructed to perform multi-unmanned vehicle formation collaborative trajectory optimization; the specific process is as follows:

[0023] First, according to the expected formation configuration matrix F des Calculate the vector A formed by the line connecting each unmanned vehicle and the center point of the formation;

[0024] Secondly, the initial formation direction is recorded as φ1, the direction of the formation when it reaches the target point is recorded as φ2, the number of path points is recorded as H, and the direction matrix is ​​created as D, whose number of elements is H; the first element D1 and the last element D H Assign values ​​of φ1 and φ2 respectively, and the other elements of D are taken in the path searched by the virtual navigator according to the principle of equal spacing;

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

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

[0027] Furthermore, the present invention constructs a multi-unmanned vehicle formation collaborative trajectory optimization problem and performs multi-unmanned vehicle formation collaborative trajectory optimization. The specific process is:

[0028] Firstly, the optimization objective function is set to include the smoothness term and the deviation term from the discrete path, and the formation cost term is constructed by using the rotation matrix and added to the objective function to construct the trajectory optimization problem of the unmanned vehicle formation.

[0029] Secondly, set constraints including start and end point constraints, vehicle kinematic constraints, safety corridor constraints, and vehicle-to-vehicle collision avoidance constraints;

[0030] Finally, the path searched by the front end is used as the initial solution, and numerical calculation methods are used to solve the optimization problem of the unmanned vehicles in the formation one by one.

[0031] Furthermore, the collaborative trajectory optimization problem of multiple unmanned vehicle formations described in the present invention is:

[0032]

[0033] in, is the difference between the control inputs of two adjacent trajectory points of the i-th unmanned vehicle, is the deviation of the jth trajectory point of the i-th unmanned vehicle from its reference path; is the formation cost of the i-th unmanned vehicle at the j-th trajectory point, where is the position of the virtual navigator at the jth trajectory point, is the position of the i-th unmanned vehicle at the j-th trajectory point, represents the position of the i-th unmanned vehicle at the j-th trajectory point, H is the number of trajectory points, P and Q are the positive definite weight matrices of the smooth term and the bias term, T is the weight coefficient of the formation term, and R safe is the collision radius of the unmanned vehicle, represents the safety corridor corresponding to the jth trajectory point of the i-th unmanned vehicle, represents the range of control input of the i-th unmanned vehicle.

[0034] Beneficial effects:

[0035] First, the present invention proposes a collaborative trajectory planning method for a multi-unmanned vehicle formation, which combines front-end path search with back-end trajectory optimization to generate a smooth, collision-free formation trajectory of multiple unmanned vehicles that satisfies kinematic constraints.

[0036] Second, based on the A* algorithm, the present invention proposes an improved A* algorithm, and adds a deviation term from the formation center point to the heuristic function of the 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 back-end multi-unmanned vehicle trajectory optimization.

[0037] Third, the present invention constructs the trajectory optimization problem of multiple unmanned vehicles and verifies its solvability and effectiveness. The optimized trajectory of the virtual navigator is used to obtain the formation angle information, and the expected relative positions between multiple unmanned vehicles are calculated through the rotation matrix, realizing the overall rotation of the formation. In addition, the proposed method can change the number of unmanned vehicles in the formation and the formation formation by only changing the expected position distribution matrix, and has good scalability. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0039] Figure 1 Schematic diagram of the overall architecture of the invention.

[0040] Figure 2 Define methods for desired formation configurations and formation rotation angles;

[0041] Figure 3 To improve the A* algorithm flow chart;

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

[0043] Figure 5 Generate schematic diagrams for safe corridors;

[0044] Figure 6 This is a schematic diagram of trajectory optimization results; DETAILED DESCRIPTION

[0045] The embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0046] It should be noted that the following embodiments and features in the embodiments may be combined with each other in the absence of conflict; and, based on the embodiments in the present disclosure, all other embodiments obtained by ordinary technicians in the field without making any creative work are within the scope of protection of the present disclosure.

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

[0048] Steps 1 and 2 are the front-end path search process, and the process is as follows: Figure 1 As shown; Step three is the optimization problem construction and solution process, the process is as follows Figure 3 shown.

[0049] The problem addressed by the present invention is as follows: Assume that there are several obstacles and N unmanned vehicles in a known two-dimensional mission scene, and each unmanned vehicle is assigned a number i, and its mission starting point is s i , the target point is g i , the trajectory planning task of unmanned vehicle i is to find a feasible trajectory so that it can go from s i Move to g without collision i The discrete path point set obtained by the unmanned vehicle i through path search is recorded as in, is a 2×1 column vector, which indicates the position of the jth path point of the unmanned vehicle from the starting point, and L indicates the number of path points contained in this path. i is the initial solution, and the set of trajectory points of unmanned vehicle i obtained by trajectory optimization is recorded as in is a 3×1 column vector, representing the position of the unmanned vehicle at the jth trajectory point from the starting point, t k = kΔt represents the time corresponding to the kth trajectory point, Δt is the unit time, H represents the trajectory q i The number of track points to include.

[0050] The present application embodiment provides a multi-unmanned vehicle formation trajectory planning method based on search and optimization, and the specific process is as follows:

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

[0052] The expected formation configuration is the formation shape that each unmanned vehicle is expected to form and maintain, which is defined by the relative positions of each unmanned vehicle.

[0053] First, select an unmanned vehicle (called unmanned vehicle No. 1) as the coordinate origin, and the expected positions of other unmanned vehicles relative to unmanned vehicle No. 1 can be expressed by a two-dimensional vector. By analogy, we can get an expected formation configuration matrix F with a dimension of N×2 des , where N is the number of unmanned vehicles.

[0054] Then, the starting point and target point of the formation center are defined in the two-dimensional grid map, and the starting point s of each unmanned vehicle can be calculated according to the expected formation configuration matrix. i and the target point g i .

[0055] Then, define the orientation angle of the formation at the starting point and the target point (with 0 degrees to the right of the horizon and the angle increasing in the counterclockwise direction). Figure 1 The expected formation configuration construction method of three unmanned vehicles and the meaning of the rotation angle are described. The solid line represents the initial position of the formation, and the dotted line represents the target position of the formation; 1, 2, and 3 represent unmanned vehicles, and 0 represents the virtual navigator (the center point of the formation). The expected formation configuration matrix F des =[0,0; 2,4; 4,0], the initial formation heading angle is The target heading angle is the angle θ between the formation heading and the horizontal direction.

[0056] Step 2: Based on the starting point and the target point, according to the grid map, the virtual navigator and each unmanned vehicle in the formation will search for paths and process path points, and build a safe corridor;

[0057] The starting point and target point coordinates of the formation selected in step 1 are used as the starting point and target point coordinates of the virtual navigator. Use the A* algorithm to search for a feasible path for the virtual navigator. The sub-node expansion formula of the A* algorithm is

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

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

[0060] When multiple unmanned vehicles are searching for paths, if they use the A* algorithm separately, when they encounter obstacles, multiple unmanned vehicles may bypass them from different directions, making the initial solution provided by the front-end search for the back-end trajectory optimization very poor, resulting in low efficiency or even no solution for the back-end optimization problem. To solve this problem, the present invention designs an improved A* algorithm for formation path search. The subnode expansion formula of the improved A* algorithm is

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

[0062] Among them, the meanings of f(n), g(n) and h(n) are consistent with those of the A* algorithm, and k(n) is the newly added formation heuristic item, which is calculated as the distance between the node to be expanded and the nearest reference path point to the current node n. h and λ k are the weights of h(n) and k(n) respectively.

[0063] The algorithm first searches for a path at the center point of the formation, and then uses it as a benchmark to complete the path search for each vehicle. Compared with the original A* algorithm's unrelated search, the improved A* algorithm can make the paths searched by each unmanned vehicle bypass obstacles from the same direction, providing a good initial solution for back-end trajectory optimization. Based on the searched path, the number of path points for each vehicle is densified and normalized, and then spatial expansion is performed point by point to generate a safe corridor as one of the hard constraints of the back-end optimization problem.

[0064] The specific steps of the improved A* algorithm are as follows: First, according to the expansion rule of formula (1), use the A* algorithm to search for a path as the reference path. Then, search for paths for each unmanned vehicle. Each time the parent node searches for a child node, traverse the path points in the reference path and select the one with the shortest Euclidean distance between the parent node and all the path points in the reference path as the reference point for the child node expansion. Then, according to the expansion rule of formula (2), the child node of the parent node is expanded, that is, on the basis of the A* algorithm, the distance term from the child node to the reference node is added. The flowchart of the improved A* search algorithm is shown in the figure. Figure 3 As shown, its effect is compared with that of the A* algorithm. Figure 4 shown.

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

[0066] The densification process inserts a custom number of waypoints by equidistant interpolation of the coordinates of every two points in the path. This operation is used to ensure the continuity of the safety corridor.

[0067] The waypoint number normalization process selects the path with the largest number of waypoints among the discrete paths searched by the virtual navigator and each unmanned vehicle as the reference path, and continuously copies the last waypoint of other paths and adds it to the end of the path until the number of waypoints is the same as that of the reference path, so that the number of paths of each path is normalized. This operation is used to ensure that the timestamps of each unmanned vehicle remain consistent during the back-end trajectory optimization process.

[0068] The safe corridor is generated by the expansion method. For example, The initial safe corridor space is detected in all the grids in the rectangular range of the four directions (+x, +y, -x, -y) of the safe corridor space. If there are blank grids in a certain direction, the grids in that direction are added to the safe corridor set; if there are obstacle grids, the space expansion in that direction is stopped. Repeat the above steps until there is no direction to continue expanding, at which point the path point is obtained. The corresponding maximum rectangular safety corridor is recorded as is a row vector containing four elements, whose four elements are the row and column numbers corresponding to the upper, lower, left and right sides of the safety corridor in the grid map. Figure 5 shown.

[0069] Step 3: Optimize the trajectory of the virtual navigator as a reference for the trajectory optimization of each unmanned vehicle and use it to construct the formation cost, and solve the trajectory optimization problem for each unmanned vehicle in turn.

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

[0071] In order to obtain the reference trajectory of multiple unmanned vehicle formations and to design the cost function of multiple unmanned vehicle formations, the trajectory of a single unmanned vehicle is first optimized for the virtual navigator. The optimization problem is as follows:

[0072]

[0073] Wherein, formula (3a) is the cost function of the optimization problem. is the difference between the control inputs of two adjacent trajectory points (jth and j-1th trajectory points) of the i-th unmanned vehicle, expressed as is the deviation of the jth trajectory point of the i-th unmanned vehicle from its reference path, expressed as The smooth term and the deviation term are expressed in the form of quadratic form, where the smooth term Make the control input change between every two trajectory points of the unmanned vehicle as small as possible, and 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 are the positive definite weight matrices for the smoothing term and the bias term, respectively.

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

[0075] First, according to the expected formation configuration matrix F des Calculate the vector formed by the line connecting each unmanned vehicle and the center point of the formation.

[0076] by Figure 2 Taking the three unmanned vehicle formation as an example, the expected formation configuration matrix F des =[0,0; 2,4; 4,0], then the coordinates of the formation center in the formation coordinate system are F des The average value of each column, that is Then calculate the vectors connecting each unmanned vehicle and the center point of the formation to form the expected formation vector matrix It is worth noting that when the multi-unmanned vehicle formation is driving along the trajectory point, the formation needs to change in real time as the movement state of the formation changes. When the formation is driving along a straight line, the formation remains unchanged; when the formation turns as a whole, the formation needs to be transformed according to the turning angle, that is, the above-mentioned expected relative position vector needs to be rotated using a two-dimensional rotation matrix, the method is as follows:

[0077] Assume that the initial formation direction is set to φ1, the formation direction when it reaches the target point is set to φ2, and the number of path points is H. Create a direction matrix denoted as D, whose number of elements is H, which is used to store the rotation angle of the formation. H Assign values ​​of φ1 and φ2 respectively, and the other elements of D are taken in the path searched by the virtual navigator according to the principle of equal spacing. Taking the jth trajectory point as an example, assuming that there is D j =θ j , then the two-dimensional rotation matrix of the relative position of the formation at this time is

[0078]

[0079] Multiply the desired formation vector matrix A by the rotation matrix R j , we can get the time-varying expected formation vector matrix A between multiple unmanned vehicles in motion j =R j A.

[0080] Secondly, the collaborative trajectory optimization problem of multiple unmanned vehicle formations can be constructed as follows

[0081]

[0082] Wherein, formula (5a) is the cost function of the optimization problem. is the difference between the control inputs of two adjacent trajectory points (the jth and j-1th trajectory points) of the i-th unmanned vehicle; is the deviation of the jth trajectory point of the i-th unmanned vehicle from its reference path; is the formation cost of the i-th unmanned vehicle at the j-th trajectory point, where is the position of the virtual navigator at the jth trajectory point. The smooth term, deviation term, and formation term are all expressed in the form of quadratic form, where the smooth term Make sure that the control input between each two trajectory points does not change too much, and the deviation term Make sure that the deviation between the trajectory point and the reference path point is not too much, and the formation item Make multiple unmanned vehicles maintain the desired formation as much as possible. In addition, H is the number of trajectory points, P 2×2 , Q 2×2 are the positive definite weight matrices of the smoothing term and the deviation term, respectively, and T is the weight coefficient of the formation term (a constant). safe is the collision radius of the unmanned vehicle.

[0083] Formula (5b)-(5f) are the constraints of the optimization problem. Their meanings are as follows: Formula (4b) represents the constraints of the starting point and the target point, that is, the first and last points of the generated trajectory need to meet the pre-set starting and ending point position coordinate requirements. Formula (5c) represents the kinematic model constraint, that is, the state update needs to satisfy the state update equation of the unmanned vehicle kinematic model. Formula (5d) represents the safety corridor constraint, that is, the coordinates of all trajectory points need to be within the range of their corresponding safety corridors. Formula (5e) represents the minimum / maximum control input constraint, that is, the control input (linear velocity and angular velocity) at each trajectory point needs to be within the set control input minimum / maximum range. Formula (5f) represents the vehicle collision avoidance constraint, that is, at the same time, the distance between any two unmanned vehicles in the formation needs to be greater than the minimum safety distance (here twice the unmanned vehicle collision radius R is taken). safe ) to prevent multiple unmanned vehicles from colliding with each other during driving.

[0084] By solving the above nonlinear multi-constrained optimization problem through numerical solution methods such as Sequential Quadratic Programming (SQP), we can obtain the smooth, collision-free trajectory of each unmanned vehicle that maintains the formation.

[0085] The effectiveness of the method proposed in the present invention is verified by an example. Take N = 3, and the initial formation rotation angle is Target position formation rotation angle Expected formation configuration F des =[0,0; 2,4; 4,0], 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 using the SQP algorithm is as follows Figure 6 It can be seen that the trajectory planning method proposed in the present invention can obtain smooth, collision-free trajectories of multiple unmanned vehicles that maintain the formation.

[0086] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed by the present invention should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.

Claims

1. A multi-unmanned vehicle formation trajectory planning method based on search and optimization, characterized in that: The specific process is: Step 1: Set the desired formation configuration, calculate the starting point and target point of each unmanned vehicle, set the initial formation orientation and the orientation of the formation when it reaches the target point; Step 2: Based on the starting point and the target point, according to the grid map, the virtual navigator and each unmanned vehicle in the formation will search for paths and process path points, and build a safe corridor; Step 3: Under the constraint of the safety corridor, based on the set initial formation orientation and the orientation of the formation at the target point, the trajectory optimization of the virtual navigator is used as a reference benchmark for the trajectory optimization of each unmanned vehicle, and the formation cost is constructed based on this, and the trajectory optimization problem is solved for each unmanned vehicle in turn.

2. The multi-unmanned vehicle formation trajectory planning method based on search and optimization according to claim 1 is characterized in that: In step 2, a newly added formation heuristic term k(n) is added to the A* path search algorithm to obtain an improved A* path search algorithm, wherein k(n) is calculated as the distance k(n) between the node to be expanded and the reference path point closest to the current node n, and a safe corridor is constructed.

3. The multi-unmanned vehicle formation trajectory planning method based on search and optimization according to claim 2 is characterized in that: The A* path search algorithm is: f(n)=g(n)+λ h ·h(n)+λ k ·k(n), Where f(n), g(n) and h(n) represent the total cost of expanding the child node, the actual cost from the starting node to the current node, and the estimated value of the heuristic function, respectively. h and λ k are the weights of h(n) and k(n) respectively.

4. The multi-unmanned vehicle formation trajectory planning method based on search and optimization according to claim 3 is characterized in that: The specific process of step 2 is as follows: First, based on the starting point and target point of each unmanned vehicle set in step 1, use the A* algorithm to search for a path as the reference path; Secondly, search for paths for each unmanned vehicle. Each time the parent node searches for a child node, it traverses the path points in the reference path and selects the one with the shortest Euclidean distance between the parent node and all the path points in the reference path as the reference point for the child node expansion. Again, at the reference point, the child nodes of the parent node are expanded according to the improved A* algorithm expansion rule; Then, after densifying each path, the number of waypoints in each path is normalized; Finally, the path is traversed point by point to generate a safe corridor.

5. The multi-unmanned vehicle formation trajectory planning method based on search and optimization according to claim 1 is characterized in that: The specific process of step three is: (1) Optimize the trajectory of a single unmanned vehicle for the virtual pilot; (2) Taking the optimized trajectory of the virtual navigator as a benchmark, under the constraints of the safety corridor and the formation orientation at the initial and target points, a multi-unmanned vehicle formation collaborative trajectory optimization function is constructed to perform multi-unmanned vehicle formation collaborative trajectory optimization; the specific process is as follows: First, according to the expected formation configuration matrix F des Calculate the vector A formed by the line connecting each unmanned vehicle and the center point of the formation; Secondly, the initial formation direction is recorded as φ1, the direction of the formation when it reaches the target point is recorded as φ2, the number of path points is recorded as H, and the direction matrix is ​​created as D, whose number of elements is H; the first element D1 and the last element D H Assign values ​​of φ1 and φ2 respectively, and the other elements of D are taken in the path searched by the virtual navigator according to the principle of equal spacing; Again, based on the vector A and the direction matrix D, the time-varying expected formation vector matrix A between multiple unmanned vehicles in motion is calculated. j ; Finally, under the constraint of the safety corridor, based on the expected formation vector matrix, a multi-unmanned vehicle formation collaborative trajectory optimization problem is constructed, and multi-unmanned vehicle formation collaborative trajectory optimization is performed.

6. The multi-unmanned vehicle formation trajectory planning method based on search and optimization according to claim 5 is characterized in that: The above-mentioned process of constructing the collaborative trajectory optimization problem of multiple unmanned vehicle formations and performing collaborative trajectory optimization of multiple unmanned vehicle formations is as follows: Firstly, the optimization objective function is set to include the smoothness term and the deviation term from the discrete path, and the formation cost term is constructed by using the rotation matrix and added to the objective function to construct the trajectory optimization problem of the unmanned vehicle formation. Secondly, set constraints including start and end point constraints, vehicle kinematic constraints, safety corridor constraints, and vehicle-to-vehicle collision avoidance constraints; Finally, the path searched by the front end is used as the initial solution, and numerical calculation methods are used to solve the optimization problem of the unmanned vehicles in the formation one by one.

7. The multi-unmanned vehicle formation trajectory planning method based on search and optimization according to claim 6 is characterized in that: The collaborative trajectory optimization problem of multiple unmanned vehicle formations is: in, is the difference between the control inputs of two adjacent trajectory points of the i-th unmanned vehicle, is the deviation of the jth trajectory point of the i-th unmanned vehicle from its reference path; is the formation cost of the i-th unmanned vehicle at the j-th trajectory point, where is the position of the virtual navigator at the jth trajectory point, is the position of the i-th unmanned vehicle at the j-th trajectory point, represents the position of the i-th unmanned vehicle at the j-th trajectory point, H is the number of trajectory points, P and Q are the positive definite weight matrices of the smooth term and the bias term, T is the weight coefficient of the formation term, and R safe is the collision radius of the unmanned vehicle, represents the safety corridor corresponding to the jth trajectory point of the i-th unmanned vehicle, represents the range of control input of the i-th unmanned vehicle.

Images