Ground-air multi-robot formation graph search planning and distributed trajectory optimization method and system

Abstract

The invention discloses a graph search planning and distributed trajectory optimization method and system for ground-air multi-robot formation, and the method comprises the steps: carrying out the discretization of the position and angle of a state space, mapping obtained discrete points to obtain different formation configurations, constructing an undirected graph of an effective configuration, and employing a graph search planning method to obtain a distributed trajectory of the formation; searching a path sequence from the initial configuration to the target configuration on the undirected graph; performing dimension reduction on the kinetic models of the four-rotor aircraft and the mobile robot by using differential flatness, performing trajectory planning on each flat output, comprehensively considering energy consumption and formation error indexes, constructing an optimal function, and converting trajectory planning of the robot into an optimal problem; and finally, carrying out distributed segmentation solution, and obtaining an optimized trajectory after iteration. The method effectively reduces the calculation pressure of a single robot, improves the calculation efficiency, and is more suitable for a robot system planning method of a large-scale cluster.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot trajectory planning, and in particular relates to a graph search planning and distributed trajectory optimization method and system for a ground-to-air multi-robot formation. Background Art

[0002] With the rapid development of computer science and artificial intelligence technology, robotics technology has become increasingly mature and has been applied to more and more fields. However, in actual applications, due to the complexity of tasks and the variability of the environment, there are often tasks that a single robot cannot complete, and people are paying more and more attention to the development of multi-robot systems. In recent years, the drone industry has ushered in new development opportunities, and ground-to-air collaborative operations have become a current research hotspot. The ground-to-air multi-robot system consists of a quadcopter and a ground mobile robot, making full use of their respective advantages, learning from each other's strengths and weaknesses, and coordinating formations to complete complex tasks.

[0003] For the collaborative formation of multi-robot systems in the air and ground, trajectory planning technology is one of the key technologies for its intelligentization, which can ensure that robots can complete tasks autonomously in many situations, especially in some emergency and dangerous situations. The collaborative trajectory planning of multi-robot systems not only needs to consider the obstacle avoidance problem, but also the collision avoidance problem between multiple robots. In some specific tasks, multiple robots are also required to maintain a specific formation, which brings challenges to the collaborative trajectory planning of robots. In the process of actual application, there are usually further requirements for factors such as the time and energy consumption of the robot's execution of tasks. At the same time, in order to ensure that the planned trajectory can be tracked by the robot, the smoothness of the trajectory and the controllability of the speed also need to meet the requirements.

[0004] Trajectory planning usually includes two parts: path search and trajectory optimization. Path search is to determine a collision-free path sequence from the starting point to the end point in the map based on the existing environmental information, including sampling-based methods, search-based methods and biological heuristic algorithms. Sampling-based methods include the rapid exploration random tree algorithm (RRT) and the probabilistic roadmap algorithm (PRM), which are suitable for high-dimensional complex constraint problems, but usually cannot guarantee that the obtained path is optimal. The main graph search-based algorithms include the Dijkstra algorithm, the A* algorithm, the D* algorithm, etc., which can obtain the optimal path, but have low computational efficiency and consume large amounts of memory in large-scale or high-dimensional problems. Biological heuristic algorithms include the ant colony algorithm and the genetic algorithm, which have good adaptability and exploration capabilities for complex dynamic environments, but also have problems such as complex parameter adjustment, easy to fall into local optimality, and large computing resource requirements.

[0005] Trajectory optimization is to ensure that the robot can smoothly track the planned path. The initial geometric trajectory is parameterized and can be divided into hard constraint methods and soft constraint methods. The hard constraint method divides the state space and converts the trajectory optimization problem into a constrained convex optimization problem for solution. The constraints are more stringent and suitable for high safety and high reliability occasions, but the lack of flexibility makes the solution difficult. The soft constraint method directly constructs collision constraints, dynamic constraints, etc. in the objective function in the form of penalty functions, converts the trajectory planning problem into an unconstrained optimization problem, provides flexibility and adaptability, and is suitable for dynamic and complex environments, but the quality of the solution is relatively low. The method of the present invention is designed for a trajectory optimization method with hard constraints, and solves the QP problem with constraints such as obstacle avoidance and motion through a distributed method. Summary of the invention

[0006] The present invention is aimed at the problem that it is difficult to maintain the formation in the collaboration of multiple robots in the prior art, and the centralized trajectory planning algorithm has low solution efficiency. It provides a graph search planning and distributed trajectory optimization method and system for ground-to-air multi-robot formations. First, the position and angle of the state space are discretized, and the obtained discrete points are mapped to obtain different formation configurations, and an undirected graph of effective configurations is constructed. The graph search planning method is used to search the path sequence from the starting configuration to the target configuration on the undirected graph; then the dynamic model of the quadcopter and the mobile robot is reduced in dimension by differential flatness, and trajectory planning is performed for each flat output respectively. The optimal function is constructed by comprehensively considering energy consumption and formation error indicators, and the robot's trajectory planning is converted into an optimal problem; finally, a distributed segmented solution is performed, and the optimized trajectory is obtained after iteration. The method of the present invention effectively reduces the computing pressure of a single robot, improves computing efficiency, and is more suitable for the robot system planning method of large-scale clusters.

[0007] In order to achieve the above object, the technical solution adopted by the present invention is: a graph search planning and distributed trajectory optimization method for a ground-to-air multi-robot formation, comprising the following steps:

[0008] S1. Acquisition of formation path sequence: Discretize the position and angle of the state space, map the obtained discrete points to obtain different formation configurations, construct an undirected graph of effective configurations, and use the graph search planning method to search for the path sequence from the starting configuration to the target configuration on the undirected graph;

[0009] S2. Conversion of trajectory planning problem: Use differential flatness to reduce the dimension of the dynamic model of the quadrotor and mobile robot, perform trajectory planning for each flat output, and construct the optimal function by minimizing energy consumption and formation error function to convert the robot's trajectory planning into an optimal problem;

[0010] S3, distributed segmented solution: solve the optimal problem of step S2. When solving for the first time, set the formation item to zero to obtain the initial value of the solution. Subsequent calculations use the previous solution as the trajectory of the neighboring robots. Each robot independently calculates its own trajectory, and the optimized trajectory is obtained after iteration.

[0011] As an improvement of the present invention, the step S1 of obtaining the formation path sequence specifically includes the following steps:

[0012] S11: Select appropriate discretization factors △x, △y, △θ to discretize the state space and obtain discrete points (x i ,y i ,θ i ), map it to the configuration space, and obtain the corresponding formation configuration c i ={p i ,θ i ,s}, where p i =[x i ,y i ,0] T Corresponding to the position of the formation center, θ i The corresponding angle between the formation coordinate system and the positive direction of the x-axis of the world coordinate system, s is the formation shape defined as h i =[h ix ,h iy ,h iz ] T is the formation vector in the formation coordinate system;

[0013] S12: The formation configuration c obtained in step S11 is i ={p i ,θ i ,s} is used as the vertex of the undirected graph, and the distance between adjacent configurations is used as the weight of the edge of the undirected graph to generate an undirected graph;

[0014] S13: Using a graph search algorithm on the undirected graph, a set of formation path sequences is obtained.

[0015] As an improvement of the present invention, the determination method of the safe formation configuration in step S12 is: assuming that the radius of the j-th cylindrical obstacle is r oj , height is h oj , the center position p oj =[x oj ,y oj ] T , if ||p i -p oj ||>r oj +d safe , then the corresponding configuration c iis a safe formation configuration, whereas a dangerous formation configuration where a collision may occur. safe To ensure the safe distance of the formation;

[0016] When two configurations c i ={p i ,θ i ,s} and c j ={p j ,θ j ,s} satisfies ||p i -p j || 2 <ξ,|θ i -θ j |<ζ, then the formation configuration is called c i and c j is the adjacent configuration, where ξ and ζ are constants.

[0017] As another improvement of the present invention, the weight of the edge of the undirected graph in step S13 is α||p i -p j || 2 +β|θ i -θ j |, the feasible path sequence is in Then we can inversely solve The corresponding path point of the i-th robot for

[0018]

[0019] As another improvement of the present invention, in step S2, differential flatness is used to obtain the flat output x, y, z, ψ of the quadrotor aircraft and the flat output x, y of the mobile robot. The trajectory of the drone is expressed as

[0020]

[0021] Where i∈V A ={1,2,…,N 1},N 1 is the number of drones, j∈{1,2,…,M-1}, M is the number of path points obtained by planning, t j-1 ,t j represents the start and end time of the jth trajectory, are the trajectory polynomial coefficient vectors of the i-th UAV at the j-th stage x, y, z, ψ, respectively, and n is the trajectory polynomial coefficient;

[0022] The trajectory of the unmanned vehicle is expressed as

[0023]

[0024] Where i∈V G = {N 1 +1,N 1 +2,…,N 1 +N 2},N 2 is the number of unmanned vehicles, j∈{1,2,…,M-1}, M is the number of path points obtained by planning, t j-1 ,t j represents the start and end time of the jth trajectory, are the trajectory polynomial coefficient vectors of the i-th unmanned vehicle at the j-th stage x, y, and n is the trajectory polynomial coefficient.

[0025] As another improvement of the present invention, the optimal function in step S2 is specifically:

[0026]

[0027] Where M is the number of path points obtained by planning, j∈{1,2,…,M-1}, t j-1 ,t j represents the start and end time of the jth trajectory, k 1 >0,k 2 >0 is the weight coefficient. The optimal function consists of two parts: It is designed based on the idea of ​​minimizing jerk, aiming to reduce the energy consumption of the system by making the motion smoother and more stable. is the fourth-order derivative of the trajectory polynomial. It is designed based on formation error, aiming to minimize the formation error and thus enable the ground-to-air multi-robot system to maintain formation.

[0028]

[0029] In the formula, a ik represents the weight coefficient between the i-th robot and the k-th robot. If they are connected, then a ik =1, otherwise a ik =0,h ix Represents the formation vector component of the ith robot in the x direction.

[0030] As another improvement of the present invention, the trajectory planning is converted into a QP problem for solution, specifically:

[0031]

[0032] in,

[0033]

[0034] Among them, r and c represent the row index and column index of the matrix, t j-1 ,t j represents the start and end time of the jth trajectory, a ik represents the weight coefficient between the i-th robot and the k-th robot. If they are connected, then a ik =1, otherwise a ik =0,h ix Represents the formation vector component of the ith robot in the x direction.

[0035] In order to achieve the above-mentioned purpose, the present invention also adopts the technical solution of: a graph search planning and distributed trajectory optimization system for a ground-to-air multi-robot formation, including a computer program, which, when executed by a processor, implements the steps of any of the methods described above.

[0036] Compared with the prior art, the present invention has the following technical advantages and effects:

[0037] (1) This paper proposes a new formation planning method, which effectively solves the planning problem in the collaborative formation process of multi-robot systems.

[0038] (2) The present invention proposes a distributed trajectory optimization method, which can effectively reduce the computing pressure of a single robot, improve computing efficiency, and is more suitable for large-scale cluster robot system planning methods.

[0039] (3) The idea of ​​formation trajectory planning proposed in the present invention can also be applied to other collaborative formation scenarios, has strong versatility, and has broad application prospects. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 A flowchart of the steps of the graph search planning and distributed trajectory optimization method for the ground-to-air multi-robot formation of the present invention;

[0041] Figure 2 This is a schematic diagram of a formation configuration according to Embodiment 1 of the present invention;

[0042] Figure 3 This is a schematic diagram of searching and planning path points for a ground-to-air multi-robot system according to Embodiment 1 of the present invention, wherein

[0043] Figure 3 (a) is the path point of the quadrotor drone;

[0044] Figure 3 (b) is the waypoint of the mobile robot;

[0045] Figure 4This is a schematic diagram of the trajectory obtained by distributed trajectory optimization of the ground-to-air multi-robot system in Example 1 of the present invention, wherein:

[0046] Figure 4 (a) is the trajectory of the quadrotor drone;

[0047] Figure 4 (b) is the trajectory of the mobile robot;

[0048] Figure 5 This is a schematic diagram of the trajectory planning of the traditional ground-to-air multi-robot system collaborative formation in the test case.

[0049] Figure 5 (a) is the trajectory of the quadrotor drone;

[0050] Figure 5 (b) is the trajectory of the mobile robot. DETAILED DESCRIPTION

[0051] The present invention will be further explained below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0052] Example 1

[0053] Aiming at the trajectory planning problem of ground-to-air multi-robot system in collaborative formation task, a graph search planning and distributed trajectory optimization method for ground-to-air multi-robot system formation is proposed. Figure 1 As shown, the method comprises the following steps:

[0054] Step S1: Select a suitable discretization factor, discretize the position and angle of the state space, and map the obtained discrete points from the state space to the configuration space to obtain different formation configurations. These configurations are used as vertices of the undirected graph, and the distances between the configurations are used as the weights of the edges of the undirected graph to generate an undirected graph; the configurations that collide with obstacles are marked as invalid configurations, and the configurations that do not collide are valid configurations. A graph search algorithm is used on the undirected graph to obtain a set of formation path sequences.

[0055] This embodiment takes a ground-to-air multi-robot system consisting of a group of two quad-rotor drones and three mobile robots as an example.

[0056] S11: Select appropriate discretization factors △x, △y, △θ to discretize the state space, and obtain the discrete points (x i ,y i ,θ i ), map it to the configuration space, and obtain the corresponding formation configuration c i ={p i ,θ i ,s}, where p i=[x i ,y i ] T Corresponding to the position of the formation center, θ i The corresponding angle between the formation coordinate system and the positive direction of the x-axis of the world coordinate system, s is the formation shape defined as h i =[h ix ,h iy ,h iz ] T is the formation vector in the formation coordinate system. The schematic diagram of the formation configuration of this embodiment is shown in the attached figure. Figure 2 As shown in the figure, taking a group of ground-to-air multi-robots including 3 mobile robots and 2 quadrotors as an example, the center position of the formation is p i =[x i ,y i ] T With the configuration angle θ i As shown in the figure, when the formation shape is determined, by p i and θ i Able to uniquely determine a formation configuration.

[0057] S12: For each formation configuration obtained, check its validity. The radius of the j-th cylindrical obstacle is r oj , height is h oj , the center position p oj =[x oj ,y oj ] T , if ||p i -p oj ||>r oj +d safe , then the corresponding configuration c i To ensure a safe formation without collision, otherwise it is a dangerous formation with collision. safe To ensure the safe distance of the formation. i ={p i ,θ i ,s} and c j ={p j ,θ j ,s} satisfies

[0058] ||p i -p j || 2 <ξ

[0059] |θ i -θ j ||<ζ

[0060] The formation configuration c i and c jis the adjacent configuration, where ξ and ζ are constants.

[0061] The safe formation configuration in the configuration obtained by mapping the discretized points is taken as the vertex, and edges are generated between adjacent configurations to establish an undirected graph. The weight of the edge is recorded as α||p i -p j || 2 +β|θ i -θ j |.

[0062] S13: From the starting configuration c 0 To the target configuration c f The Dijkstra-based graph search algorithm is used to search for a feasible path sequence.

[0063] The feasible path sequence is in Then we can inversely solve The corresponding path point of the i-th robot for

[0064]

[0065] The robot discrete path points obtained in this embodiment are shown in the attached figure. Figure 3 As shown, attached Figure 3 (a) is the path sequence of the quadrotor aircraft obtained by the graph search planning algorithm, Figure 3 (b) is the path sequence of the mobile robot obtained by the graph search planning algorithm. It can be seen from the figure that the path sequence obtained under the algorithm of the present invention can enable the ground-to-air multi-robot system to complete the obstacle avoidance task while maintaining formation and successfully reach the target point from the starting point.

[0066] Step S2: Use differential flatness to reduce the dimension of the dynamic model of the quadrotor and mobile robot, and perform trajectory planning for each flat output. Use the minimized energy consumption and formation error function to construct the optimal function, use the idea of ​​safe corridors to transform the robot's obstacle avoidance and collision constraints into position boundary constraints, and combine the trajectory continuity constraints to transform the robot's trajectory planning into an optimal problem.

[0067] First, by using differential flatness, we can obtain the flat output x, y, z, ψ of the quadrotor and the flat output x, y of the mobile robot. The trajectory of the drone can be expressed as

[0068]

[0069] Where i∈V A ={1,2,…,N 1},N 1is the number of drones, j∈{1,2,…,M-1}, M is the number of path points obtained by planning, t j-1 ,t j represents the start and end time of the jth trajectory, are the trajectory polynomial coefficient vectors of x, y, z, and ψ of the ith UAV at the jth stage, respectively, and n is the trajectory polynomial coefficient.

[0070] Similarly, the trajectory of the unmanned vehicle can be expressed as

[0071]

[0072] Where i∈V G = {N 1 +1,N 1 +2,…,N 1 +N 2},N 2 is the number of unmanned vehicles, j∈{1,2,…,M-1}, M is the number of path points obtained by planning, t j-1 ,t j represents the start and end time of the jth trajectory, are the trajectory polynomial coefficient vectors of the i-th unmanned vehicle at the j-th stage x, y, and n is the trajectory polynomial coefficient.

[0073] Consider the following optimal function (taking the x direction as an example, due to the differential flatness, each direction can be solved independently):

[0074]

[0075] The following constraints are met:

[0076] Continuity Constraints

[0077]

[0078] Boundary conditions

[0079]

[0080] in, and are the x and y components of the j-th path point of the ith robot planned in step S1, 2r is the designed safety corridor width, and v min ,v max ,a min ,a max are the boundary values ​​of the allowable velocity and acceleration, and the values ​​are appropriate to ensure that the problem has a solution.

[0081] Step S3: Distributed segmented solution of the optimal problem. When solving the problem for the first time, the formation item is set to zero to obtain the initial value of the solution. In subsequent calculations, the previous solution is used as the trajectory of the neighboring robots. Each robot independently calculates its own trajectory and continuously approaches the desired trajectory through iteration.

[0082] In order to solve the problem in a distributed manner, the problem is converted into a QP problem for solution, and we get

[0083]

[0084] in,

[0085]

[0086] Where r and c represent the row index and column index of the matrix, i.e. the rth row and the cth column, starting from 0; t j-1 ,t j represents the start and end time of the jth trajectory, a ik represents the weight coefficient between the i-th robot and the k-th robot. If they are connected, then a ik =1, otherwise a ik =0;h ix Represents the formation vector component of the ith robot in the x direction.

[0087] The solution obtained by the mth solution is When m = 0, let k 2 =0, ignoring the formation error term, and obtaining the initial value When m ≥ 1, choose the appropriate k 1 ,k 2 , iterative solution, where After the above steps, the trajectory of each robot is obtained as shown in the attached figure. Figure 4 As shown, attached Figure 4 (a) is the trajectory of the quadrotor aircraft obtained by the distributed trajectory planning algorithm. Figure 4 (b) is the trajectory of the mobile robot obtained by the distributed trajectory planning algorithm. It can be seen from the figure that under the distributed trajectory planning algorithm proposed in the present invention, the trajectory of each robot in the ground-to-air multi-robot system composed of a quadrotor aircraft and a mobile robot is smooth and collision-free, and can maintain formation during movement.

[0088] Test Case

[0089] The method of the present invention is compared with the traditional multi-robot system collaborative formation planning method. Figure 5 As shown, Figure 5 (a) is the trajectory diagram of the quadrotor drone. Figure 5 (b) is the trajectory diagram of the mobile robot.

[0090] The main steps of the traditional multi-robot system collaborative formation planning method include: 1) using a graph search algorithm to plan a path from the starting point to the target point for the leader robot; 2) combining the Minimum Snap algorithm to smooth the path planned by the leader robot; 3) using the leader-follower method to obtain the trajectory of the follower robot; 4) for the follower's trajectory, the obstacle avoidance goal is achieved based on the artificial potential field algorithm.

[0091] The conventional method is planned under the same conditions as in Example 1. Compared with the conventional method, the formation planning algorithm proposed in the present invention has the following characteristics:

[0092] (1) The solution proposed by the present invention takes 0.936 seconds, while the traditional method takes 1.016 seconds. The method of the present invention can consider whether all robots in the formation will collide when building the map and remove the configuration that will collide, thus saving search time.

[0093] (2) The average trajectory length of the present invention is 5.0331 meters, and the average trajectory length of the traditional method is 5.8135 meters. The trajectory optimization of the present invention takes into account the minimized energy consumption of the trajectory, so that the trajectory can be shorter. The traditional method uses the artificial potential field method to achieve obstacle avoidance, which will make the trajectory longer during the obstacle avoidance process and the turning process.

[0094] (3) The maximum formation error of the present invention is 0.1296 meters, and the average formation error is 0.0045 meters. The maximum formation error of the traditional method is 0.5271 meters, and the average formation error is 0.0019 meters. Although the traditional method can completely maintain the formation when there are no obstacles, the formation error will increase sharply during the obstacle avoidance process, and it is difficult to meet the requirements under working conditions with more stringent formation requirements.

[0095] In summary, the method of the present invention discretizes the position and angle of the state space, maps the obtained discrete points to obtain different formation configurations, and constructs an undirected graph of effective configurations; secondly, the graph search planning method is used to search for the path sequence from the starting configuration to the target configuration on the undirected graph; finally, based on the jerk minimization method and combined with the formation error, a distributed trajectory optimization method is designed. The present invention proposes a method for distributed trajectory planning of ground-to-air multi-robot system collaborative formation, which effectively reduces the computational pressure of a single robot, improves computational efficiency, and is more suitable for large-scale cluster robot system planning methods.

[0096] It should be noted that the above content only illustrates the technical idea of ​​the present invention and cannot be used to limit the protection scope of the present invention. For ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications all fall within the protection scope of the claims of the present invention.

Claims

1. A graph search planning and distributed trajectory optimization method for ground-air multi-robot formations, characterized in that: The steps include: S1. Acquisition of formation path sequence: Discretize the position and angle of the state space, map the obtained discrete points to obtain different formation configurations, construct an undirected graph of effective configurations, and use the graph search planning method to search for the path sequence from the starting configuration to the target configuration on the undirected graph; S2. Conversion of trajectory planning problem: Use differential flatness to reduce the dimension of the dynamic model of the quadrotor and mobile robot, perform trajectory planning for each flat output, and construct the optimal function by minimizing energy consumption and formation error function to convert the robot's trajectory planning into an optimal problem; S3, distributed segmented solution: solve the optimal problem of step S2. When solving for the first time, set the formation item to zero to obtain the initial value of the solution. Subsequent calculations use the previous solution as the trajectory of the neighboring robots. Each robot independently calculates its own trajectory, and the optimized trajectory is obtained after iteration.

2. The graph search planning and distributed trajectory optimization method for ground-to-air multi-robot formation as claimed in claim 1, characterized in that: The step S1 of obtaining the formation path sequence specifically includes the following steps: S11: Select appropriate discretization factors △x, △y, △θ to discretize the state space and obtain discrete points (x i ,y i ,θ i ), map it to the configuration space, and obtain the corresponding formation configuration c i ={p i ,θ i ,s}, where p i =[x i ,y i ,0] T Corresponding to the position of the formation center, θ i The corresponding angle between the formation coordinate system and the positive direction of the x-axis of the world coordinate system, s is the formation shape defined as is the formation vector in the formation coordinate system; S12: The formation configuration c obtained in step S11 is i ={p i ,θ i ,s} is used as the vertex of the undirected graph, and the distance between adjacent configurations is used as the weight of the edge of the undirected graph to generate an undirected graph; S13: Using a graph search algorithm on the undirected graph, a set of formation path sequences is obtained.

3. The graph search planning and distributed trajectory optimization method for ground-to-air multi-robot formation as claimed in claim 2, characterized in that: The determination method of the safe formation configuration in step S12 is: assuming that the radius of the j-th cylindrical obstacle is r oj , height is h oj , the center position p oj =[x oj ,y oj ] T , if ||p i -p oj ||>r oj +d safe , then the corresponding configuration c i is a safe formation configuration, whereas a dangerous formation configuration where a collision may occur. safe To ensure the safe distance of the formation; When two configurations c i ={p i ,θ i ,s} and c j ={p j ,θ j ,s} satisfies ||p i -p j ||2<ξ,|θ i -θ j |<ζ, then the formation configuration is called c i and c j is the adjacent configuration, where ξ and ζ are constants.

4. The graph search planning and distributed trajectory optimization method for ground-to-air multi-robot formation as claimed in claim 3, characterized in that: The weight of the edge of the undirected graph in step S13 is α||p i -p j ||2+β|θ i -θ j |, the feasible path sequence is in Then we can inversely solve The corresponding path point of the i-th robot for 5. The graph search planning and distributed trajectory optimization method for ground-to-air multi-robot formation as claimed in claim 1, characterized in that: In step S2, differential flatness is used to obtain the flat outputs x, y, z, ψ of the quadrotor aircraft and the flat outputs x, y of the mobile robot. The trajectory of the drone is expressed as Where i∈V A ={1,2,…,N1}, N1 is the number of drones, j∈{1,2,…,M-1}, M is the number of path points obtained by planning, t j-1 ,t j represents the start and end time of the jth trajectory, are the trajectory polynomial coefficient vectors of the i-th UAV at the j-th stage x, y, z, ψ, respectively, and n is the trajectory polynomial coefficient; The trajectory of the unmanned vehicle is expressed as Where i∈V G ={N1+1,N1+2,…,N1+N2}, N2 is the number of unmanned vehicles, j∈{1,2,…,M-1}, M is the number of path points obtained by planning, t j-1 ,t j represents the start and end time of the jth trajectory, are the trajectory polynomial coefficient vectors of the i-th unmanned vehicle at the j-th stage x, y, and n is the trajectory polynomial coefficient.

6. The graph search planning and distributed trajectory optimization method for ground-to-air multi-robot formation as claimed in claim 5, characterized in that: The optimal function in step S2 is specifically: Where M is the number of path points obtained by planning, j∈{1,2,…,M-1}, t j-1 ,t j represents the start and end time of the jth trajectory, k1>0, k2>0 are weight coefficients, is the fourth-order derivative of the trajectory polynomial; In the formula, a ik represents the weight coefficient between the i-th robot and the k-th robot. If they are connected, then a ik =1, otherwise a ik =0,h ix Represents the formation vector component of the ith robot in the x direction.

7. The graph search planning and distributed trajectory optimization method for ground-to-air multi-robot formation according to claim 6, characterized in that: The trajectory planning is transformed into a QP problem for solution, specifically: in, Among them, r and c represent the row index and column index of the matrix, t j-1 ,t j represents the start and end time of the jth trajectory, a ik represents the weight coefficient between the i-th robot and the k-th robot. If they are connected, then a ik =1, otherwise a ik =0,h ix Represents the formation vector component of the ith robot in the x direction.

8. A graph search planning and distributed trajectory optimization system for ground-to-air multi-robot formations, including a computer program, characterized in that: When the computer program is executed by a processor, the steps of any of the above methods are implemented.

Images