Graph search planning and distributed trajectory optimization method and system for ground-air multi-robot formation

Abstract

The application discloses a kind of ground-air multi-robot formation's graph search planning and distributed trajectory optimization method and system, first, the state space is discretized to position and angle, the discrete point obtained is mapped to obtain different formation configuration, the undirected graph of effective configuration is constructed, the path sequence from start configuration to target configuration is searched on undirected graph using graph search planning method;Then using differential flatness, the dynamics model of quadcopter and mobile robot is reduced dimension, respectively for each flat output trajectory planning, considering energy consumption and formation error index, build optimal function, convert the trajectory planning of robot into optimal problem;Finally, distributed segmented solution is made, and the optimized trajectory is obtained after iteration.The method of the application effectively reduces the computing pressure of single robot, improves the computing efficiency, and is more suitable for large-scale cluster robot system planning method.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of robot trajectory planning, and particularly relates to a graph search planning and distributed trajectory optimization method and system for ground-air multi-robot formation. BACKGROUND

[0002] With the rapid development of computer science and artificial intelligence technology, robot technology is increasingly mature and is applied to more and more fields. However, in the actual application process, due to the complexity of the task and the variability of the environment, tasks that cannot be completed by a single robot often occur, and people pay more and more attention to the development of multi-robot systems. In recent years, the unmanned aerial vehicle industry has ushered in new opportunities for development, and ground-air collaborative operation has become a research hotspot. The ground-air multi-robot system is composed of quadcopters and ground mobile robots, which fully utilizes the advantages of each other, makes up for each other's shortcomings, and collaboratively completes complex tasks.

[0003] For the ground-air multi-robot system collaborative formation, trajectory planning technology is one of the key technologies for its intelligentization, which can ensure that the robot can autonomously complete the task in many occasions, especially in some emergency and dangerous occasions. The collaborative trajectory planning of the multi-robot system not only needs to consider the obstacle avoidance problem when planning, but also needs to consider the collision avoidance problem between multiple robots. In some specific tasks, multiple robots are required to maintain a specific formation, which brings challenges to the collaborative trajectory planning of the robots. In the process of practical application, there are further requirements for the time, energy consumption and other factors of the robot executing the task, and 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 path search and trajectory optimization. Path search is to determine a collision-free path sequence from the starting point to the ending point in the map based on the existing environmental information, including sampling-based methods, search-based methods and biological heuristic algorithms. The sampling-based method includes the rapid exploration random tree algorithm (RRT), the probabilistic roadmap algorithm (PRM), etc., which is suitable for high-dimensional complex constraint problems, but usually cannot guarantee that the obtained path is optimal. The graph search-based algorithm mainly includes Dijkstra algorithm, A* algorithm, D* algorithm, etc., which can obtain the optimal path, but has low computational efficiency and consumes a large amount of memory in large-scale or high-dimensional problems. Biological heuristic algorithms include ant colony algorithm, genetic algorithm, etc., which have good adaptability and exploration ability in complex dynamic environments, but also have problems such as complex parameter adjustment, easy to fall into local optimum, and large demand for computing resources.

[0005] Trajectory optimization is to ensure that the robot can smoothly track the planned path, the initial geometric trajectory parameterization can be divided into hard constraint method and soft constraint method. The hard constraint method divides the state space, converts the trajectory optimization problem into a constrained convex optimization problem for solving, the constraint is more strict, suitable for high safety and high reliability occasions, but lacks flexibility and difficulty in solving. The soft constraint method directly constructs the collision constraint, the dynamics constraint and other constraints in the form of penalty function in the objective function, converts the trajectory planning problem into an unconstrained optimization problem, provides flexibility and adaptability, and is suitable for dynamic complex environment, but the quality of the solution is relatively low. The method of the application is designed for the hard constraint trajectory optimization method, and a QP problem with obstacle avoidance, motion and other constraints is solved by a distributed method. SUMMARY

[0006] The present application is aimed at the problems in the prior art that it is difficult to maintain the formation in multi-robot cooperation, and the centralized trajectory planning algorithm has low solving efficiency. A graph search planning and distributed trajectory optimization method and system for ground-air multi-robot formation are provided. Firstly, the state space is discretized in position and angle, different formation configurations are mapped from the discrete points, an undirected graph of effective configurations is constructed, and a path sequence from the initial configuration to the target configuration is searched on the undirected graph by using a graph search planning method. Then, the dynamics model of the quadrotor aircraft and the mobile robot is reduced in dimension by using differential flatness, trajectory planning is performed on each flat output respectively, an optimal function is constructed by comprehensively considering the energy consumption and formation error index, and the trajectory planning of the robot is converted into an optimal problem. Finally, distributed segmented solving is performed, and the optimized trajectory is obtained after iteration. The method of the application effectively reduces the computing pressure of a single robot, improves the computing efficiency, and is more suitable for large-scale cluster robot system planning method.

[0007] To achieve the above object, the technical scheme adopted by the application is as follows:

[0008] S1, formation path sequence acquisition: the state space is discretized in position and angle, different formation configurations are mapped from the discrete points, an undirected graph of effective configurations is constructed, and a path sequence from the initial configuration to the target configuration is searched on the undirected graph by using a graph search planning method.

[0009] S2, trajectory planning problem conversion: the dynamics model of the quadrotor aircraft and the mobile robot is reduced in dimension by using differential flatness, trajectory planning is performed on each flat output respectively, an optimal function is constructed by minimizing the energy consumption and formation error function, and the trajectory planning of the robot is converted into an optimal problem.

[0010] S3, distributed segment solving: solving the optimal problem of step S2, setting the formation term to zero in the initial solving, obtaining the initial value of the solution, taking the solution of the previous iteration as the trajectory of the neighbor robot, each robot independently calculates its own trajectory, and the optimized trajectory is obtained after iteration.

[0011] As an improvement of the application, the step S1 formation path sequence acquisition specifically comprises the following steps:

[0012] S11: selecting a suitable discretization factor △x, △y, △θ for discretizing the state space to obtain discrete points (x i ,y i ,θ i ), which are mapped to the configuration space to obtain the corresponding formation configuration c i ={p i ,θ i ,s}, wherein p i =[x i ,y i ,0] T corresponds to the position of the formation center, θ i corresponds to the angle between the formation coordinate system and the positive direction of the x-axis of the world coordinate system, and 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: taking the safe formation configuration in the formation configuration c i ={p i ,θ i ,s} obtained in step S11 as the vertex of the undirected graph, and the distance between adjacent configurations 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 to obtain a set of formation path sequences.

[0015] As an improvement of the application, the judgment method of the safe formation configuration in step S12 is: assuming that the radius of the jth cylindrical obstacle is r oj , the height is h oj , and the center position is p oj =[x oj ,y oj ] T , if ||p i -p oj ||>r oj +d safe , then the corresponding configuration c iFor safety formation configuration, otherwise it is dangerous formation configuration where d safe To ensure the safety distance of the formation;

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

[0017] As another improvement of the present application, the weight of the edge of the undirected graph in the step S13 is α||p i -p j ||2+β|θ i -θ j |, and the feasible path sequence is Where Then the path point of the corresponding ith robot can be obtained by inverse solution is

[0018]

[0019] As another improvement of the present application, the step S2 uses differential flatness to obtain the flat outputs x, y, z, ψ of the quadcopter and the flat outputs x, y of the mobile robot, and the trajectory of the UAV is represented as

[0020]

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

[0022] The trajectory of the UAV vehicle is represented as

[0023]

[0024] Where, i∈VG 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 denotes the start and end time of the jth trajectory, respectively, the trajectory polynomial coefficient vector of the ith unmanned vehicle at the jth stage x, y, n is the trajectory polynomial coefficient.

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

[0026]

[0027] wherein M is the number of path points obtained by planning, j∈{1,2,…,M-1}, t j-1 ,t j denotes the start and end time of the jth trajectory, k1>0, k2>0 is the weight coefficient. The optimal function consists of two parts, which is designed based on the idea of minimizing jerk, aiming to reduce the energy consumption of the system by making the motion more smooth and stable, wherein is the fourth derivative of the trajectory polynomial. which is designed based on the formation error, aiming to keep the formation of the ground-air multi-robot system by minimizing the formation error, wherein

[0028]

[0029] In the formula, a ik denotes the weight coefficient between the ith robot and the kth robot, if connected, a ik =1, otherwise a ik =0, h ix denotes the formation vector component of the ith robot in the x direction.

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

[0031]

[0032] wherein,

[0033]

[0034] wherein r and c represent the row index and column index of the matrix, t j-1 ,t j denotes the start and end time of the jth trajectory, a ikdenotes the weight coefficient between the ith robot and the kth robot, a ik = 1, otherwise a ik = 0, h ix denotes the formation vector component of the ith robot in the x direction.

[0035] To achieve the above object, the technical scheme adopted by the present application is: a graph search planning and distributed trajectory optimization system for ground-air multi-robot formation, comprising a computer program, which, when executed by a processor, implements the steps of the method according to any one of the above.

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

[0037] (1) The present application proposes a new formation planning method, which effectively solves the planning problem in the cooperative formation process of a multi-robot system.

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

[0039] (3) The formation trajectory planning idea proposed by the present application can also be applied to other cooperative formation scenarios, has strong universality, and has broad application prospects. BRIEF DESCRIPTION OF DRAWINGS

[0040] Figure 1 is a step flow chart of the graph search planning and distributed trajectory optimization method for ground-air multi-robot formation of the present application;

[0041] Figure 2 is a schematic diagram of the formation configuration of embodiment 1 of the present application;

[0042] Figure 3 is a graph search planning path point schematic diagram of the ground-air multi-robot system of embodiment 1 of the present application, wherein

[0043] Figure 3 (a) is the path point of a quadcopter unmanned aerial vehicle;

[0044] Figure 3 (b) is the path point of a mobile robot;

[0045] Figure 4 is a trajectory schematic diagram obtained by the distributed trajectory optimization of the ground-air multi-robot system of embodiment 1 of the present application, wherein

[0046] Figure 4 (a) is the trajectory of a quadcopter unmanned aerial vehicle;

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

[0048] Figure 5 This is a schematic diagram of the trajectory planning for 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 described below with reference to 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-air multi-robot system in collaborative formation task, a graph search planning and distributed trajectory optimization method for ground-air multi-robot system formation is proposed. Figure 1 As shown, the method includes 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 an 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 marked as 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 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 the obtained 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 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, taking a group of ground-to-air multi-robots including 3 mobile robots and 2 quadrotors as an example, the formation center position p i =[x i ,y i ] T and 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 j is the adjacent configuration, where ξ and ζ are constants.

[0061] The safe formation configuration in the configuration obtained by mapping the discretized points is used 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 initial configuration c0 to the target configuration c f The feasible path sequence is searched by using a Dijkstra-based graph search algorithm.

[0063] The feasible path sequence is denoted as Wherein The inverse solution can be obtained as The path point of the i-th robot is is

[0064]

[0065] The discrete path points of the robots obtained in this embodiment are shown in the accompanying drawings Figure 3 (a) is the path sequence of the quadrotor aircraft obtained by the graph search planning algorithm, and Figure 3 (b) is the path sequence of the mobile robot obtained by the graph search planning algorithm, and it can be seen from the drawings that the path sequence obtained by the algorithm of the present application can make the ground-air multi-robot system complete the obstacle avoidance task while maintaining the formation, and smoothly reach the target point from the starting point. Figure 3

[0066] Step S2: using differential flatness to reduce the dimension of the dynamics model of the quadrotor aircraft and the mobile robot, and planning the trajectory of each flat output. An optimal function is constructed by using the minimum energy consumption and formation error function, and the obstacle avoidance and collision avoidance constraints of the robot are converted into position boundary constraints by using the idea of safety corridor. Combined with the continuity constraints of the trajectory, the trajectory planning of the robot is converted into an optimal problem.

[0067] Firstly, the flat outputs x, y, z, and ψ of the quadrotor aircraft and the flat outputs x and y of the mobile robot are obtained by using differential flatness, and the trajectory of the unmanned aerial vehicle can be expressed as

[0068]

[0069] Wherein, i∈V A ={1, 2, …, N1}, N1 is the number of unmanned aerial vehicles, j∈{1, 2, …, M-1}, M is the number of path points obtained by planning, t j-1 , t j represent the start and end times of the j-th trajectory, are the trajectory polynomial coefficient vectors of the i-th unmanned aerial vehicle in the j-th stage x, y, z, and ψ, 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 = {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 denote the start and end time of the jth trajectory, are the trajectory polynomial coefficient vectors of the ith unmanned vehicle in the jth stage x, y, respectively, and n is the trajectory polynomial coefficient.

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

[0074]

[0075] Satisfy the following constraint conditions:

[0076] Continuity constraint

[0077]

[0078] Boundary conditions

[0079]

[0080] where, and are the x and y components of the jth path point of the ith robot obtained by planning in step S1, 2r is the designed safety corridor width, v min ,v max ,a min ,a max are the boundary values of the allowable speed and acceleration, and the values are appropriately taken to ensure that the problem has a solution.

[0081] Step S3: Distributed segmented solution of the optimal problem. When solving for the first time, set the formation term to zero to obtain the initial value of the solution. The previous solution is used as the trajectory of the neighbor robot in subsequent calculations, and each robot independently calculates its own trajectory, and the desired trajectory is continuously approached through iteration.

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

[0083]

[0084] where,

[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; tj-1 ,t j Indicates 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 i-th robot in the x direction.

[0087] The solution obtained by the mth solution is When m=0, let k2=0, ignoring the formation error term, and get the initial value When m≥1, select appropriate k1, k2 and solve it iteratively, 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 collaborative formation planning method for multi-robot systems 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 robots; 4) for the follower trajectory, using the artificial potential field algorithm to achieve the obstacle avoidance goal.

[0091] The formation planning algorithm proposed in the present invention has the following characteristics compared with the traditional method under the same conditions as in Example 1:

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

[0093] (2)The average trajectory length of the present application is 5.0331 meters, and the average trajectory length of the traditional method is 5.8135 meters, the trajectory optimization of the present application considers the minimization of energy consumption of the trajectory, so that the trajectory can be shorter, and the traditional method uses artificial potential field method to realize obstacle avoidance, which will make the trajectory longer in the process of obstacle avoidance and turning.

[0094] (3)The maximum formation error of the present application 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, the traditional method can completely maintain the formation shape without obstacles, but the formation error will increase sharply in the process of obstacle avoidance, and it is difficult to meet the requirements in more stringent working conditions.

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

[0096] It should be noted that the above content only illustrates the technical idea of the present application, and cannot limit the protection scope of the present application, for ordinary skilled in the art, without departing from the principle of the present application, a number of improvements and refinements can be made, which fall within the scope of protection of the claims of the present application.

Claims

1. A method for graph search planning and distributed trajectory optimization for ground-air multi-robot formation, characterized in that, The method comprises the following steps: S1, obtaining a formation path sequence: discretizing the position and angle of the state space, mapping the obtained discrete points to different formation configurations, constructing an undirected graph of effective configurations, and searching for a path sequence from a starting configuration to a target configuration on the undirected graph by using a graph search planning method; wherein the configurations that do not collide with obstacles are effective configurations, the configurations are vertices of the undirected graph, the distance between adjacent configurations is the weight of the edge of the undirected graph, and the undirected graph is generated; S2, converting a trajectory planning problem: reducing the dimension of the dynamics model of the quadrotor aircraft and the mobile robot by using differential flatness, performing trajectory planning on each flat output respectively, constructing an optimal function by using a minimum energy consumption and formation error function, and converting the trajectory planning of the robot into an optimal problem; S3, distributed segmented solving: solving the optimal problem of step S2, setting the formation term to zero to obtain an initial value of the solution in the initial solving, using the trajectory of the previous solution as the trajectory of the neighbor robot in the subsequent calculation, independently calculating the trajectory of each robot, and obtaining an optimized trajectory after iteration.

2. The graph search planning and distributed trajectory optimization method for ground multi-robot formation of claim 1, wherein: The step S1 of obtaining a formation path sequence specifically comprises the following steps: S11: select a suitable discretization factor Discretize the state space to get discrete points Map them into the configuration space to get the corresponding formation configuration Where The position of the corresponding formation center, The angle between the positive direction of the x-axis of the corresponding formation coordinate system and the world coordinate system, The formation vector in the formation coordinate system is defined as , The formation vector in the formation coordinate system is defined as S12: generate a safety platoon configuration in step S11 as a vertex of an undirected graph, and a distance between adjacent configurations as a weight of an edge of the undirected graph. S12: generate an undirected graph with the safety platoon configuration in step S11 as a vertex of the undirected graph, and a distance between adjacent configurations as a weight of an edge of the undirected graph. S13: obtaining a group of formation path sequences by using a graph search algorithm on the undirected graph.

3. The graph search planning and distributed trajectory optimization method for ground multi-robot formation of claim 2, wherein: The judgment mode of the safe formation configuration in step S12 is: assuming that the radius of the first cylindrical obstacle is , the height is , the center position is , if , the corresponding configuration is a safe formation configuration, otherwise, it is a dangerous formation configuration in which a collision will occur, wherein is a safe distance to ensure the formation. When two configurations and satisfy then the configurations and are called adjacent configurations, where and are constant values.

4. The graph search planning and distributed trajectory optimization method for ground multi-robot formation of claim 3, wherein: The weight of the edge of the undirected graph in the step S13 is The feasible path sequence is Wherein Then the inverse solution can be obtained as The corresponding path point of the first robot is 。 5. The graph search planning and distributed trajectory optimization method for ground multi-robot formation of claim 1, wherein: The step S2 uses differential flatness to obtain the flat output of the quadrotor aircraft and the flat output of the mobile robot The trajectory of the aircraft is represented as ； wherein, , is the number of aircraft, , is the number of planned waypoints, denotes the start and end time of the th segment trajectory, , , , are the trajectory polynomial coefficient vectors of the th aircraft in the th phase, are the trajectory polynomial coefficients;​ The trajectory of the robot is represented as ； in, , is the number of robots, , is the number of path points obtained by planning, Indicates the The start and end time of the segment trajectory, , They are The robot in Stages The trajectory polynomial coefficient vector of are the coefficients of the trajectory polynomial.

6. The graph search planning and distributed trajectory optimization method for ground multi-robot formation of claim 5, wherein: The optimal function in the step S2 is specifically: ； wherein, is the number of planned waypoints, , denotes the start and end time of the segment trajectory, is a weight coefficient, is the fourth derivative of the trajectory polynomial; ； In the formula, represents the weight coefficient between the first robot and the second robot, if connected, otherwise , , represents the platoon vector component of the first robot in the direction.

7. The graph search planning and distributed trajectory optimization method for ground multi-robot formation of claim 6, wherein: The trajectory planning is converted into a QP problem for solving, specifically: ； wherein, ； ； ； ； ； in, and represents the row and column indices of the matrix, Indicates the The start and end time of the segment trajectory, Indicates the The robot and The weight coefficients between robots, if connected, then ,on the contrary , Indicates the A robot in The component of the formation vector in the direction.

8. A graph search planning and distributed trajectory optimization system for ground-air multi-robot formations, comprising a computer program, characterized in that: The computer program is executed by the processor to realize the steps of the method in any one of claims 1-7.

Images