Multi-robot path planning method based on affine formation and trajectory optimization

Abstract

A multi-robot path planning method based on affine formation and trajectory optimization belongs to the technical field of multi-robot path planning and formation control, and comprises the following steps: firstly, using a multi-robot path planner CL-CBS to plan a safe and feasible initial path for each robot to provide an initial solution for a subsequent trajectory optimization problem; and then calculating a stress matrix of the target formation to obtain a formation path, and finally, designing an objective function and constraints of a trajectory optimization problem, and solving through a solver to realize the target of the invention. According to the method, the multiple robots can keep a certain formation through track optimization, and meanwhile, the method is safe and free of collision and has dynamics feasibility.

Description

Technical Field

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

[0002] In recent years, with the development of science and technology, robots have been widely used in various industries. For industrial robots, automated production lines have greatly improved the efficiency of factories; in the field of national defense, the addition of drones, robot dogs and special robots has improved combat efficiency. Compared with single-robot systems, multi-robot systems complete work content through teamwork, such as cargo transportation, environmental information detection, coordinated roundups and patrols. Multi-robot systems have shown many advantages, including higher work efficiency, a wider range of task processing capabilities, greater system redundancy and better robustness, and therefore have attracted much attention from the industry. In the field of multi-robot systems, path planning and formation control are very hot topics. When these systems are deployed in complex environments to perform tasks, they must not only maintain a coordinated formation, but also be able to avoid obstacles on the path. This ability is crucial to ensure that the multi-robot system can successfully reach the target location and successfully complete the task. Therefore, multi-robot path planning and formation coordination are the key foundations for ensuring the efficient operation of the entire system.

[0003] The problem of multi-robot formation control originates from the group behavior of organisms in nature, such as the collective migration behavior of bird flocks, the defensive behavior of fish formations, and the collective collaborative foraging behavior of ants. In the field of robotics, multi-robot formation refers to the formation of multiple mobile robots into a desired geometric shape according to certain distance and motion state constraints during the movement process. In recent years, scholars have proposed a variety of formation control methods such as the leader-follower method, the virtual structure method, the behavior-based method, the artificial potential field method, etc. According to the different graph Laplace matrices used in the formation control law, it can be divided into the following four types: conventional Laplace matrix, symbolic Laplace matrix, complex Laplace matrix, and generalized Laplace matrix. Affine formation control is a formation control method based on the symbolic Laplace matrix. Affine transformation is a linear transformation that keeps the shape and scale of the figure unchanged. It can include operations such as translation, rotation, scaling, and shearing, such as Figure 1 An important feature of affine transformation is that it maintains the collinearity and parallelism of all lines and planes in the figure, that is, after affine transformation, the originally collinear points remain collinear, and the originally parallel lines and planes remain parallel, which has good formation maneuverability and flexibility.

[0004] Lin et al. first proposed the concept of affine formation in 2015, established the theoretical basis of affine formation, defined the affine formation problem as asymptotically forming a formation that can maintain collinearity and distance ratio with the target formation, and gave the requirements of system stability on the formation topology under the conditions of undirected graphs and directed graphs. In 2018, Zhao further analyzed the affine formation control problem from the perspective of undirected graphs, and proposed several formation control laws for different conditions, including first-order and second-order integrator models under static, uniform and variable speed conditions, and carried out rigorous proofs. Zhao's affine formation control law adopts the leader-follower method, but the path of its leader is artificially planned, and there is no corresponding path planning algorithm. By planning the path of the leader and using the formation control law to control the follower, the follower will be unable to avoid obstacles and the follower's trajectory will not conform to dynamics. In summary, how to enable the multi-robot system to maintain a certain formation in an obstacle environment while being safe and collision-free and having dynamic feasibility is an important issue affecting the multi-robot formation. Summary of the invention

[0005] In order to overcome the shortcomings of the existing technology, the present invention proposes a multi-robot path planning method based on affine formation and trajectory optimization for the multi-robot formation control problem in an obstacle environment. First, a multi-robot path planner CL-CBS is used to plan a safe and feasible initial path for each robot to provide an initial solution for the subsequent trajectory optimization problem, and then the stress matrix of the target formation is calculated to obtain the formation path. Finally, the objective function and constraints of the trajectory optimization problem are designed and solved by a solver to achieve the goal of the present invention. This method uses trajectory optimization to enable multiple robots to maintain a certain formation while being safe and collision-free, and having dynamic feasibility.

[0006] The technical solution adopted by the present invention to solve its technical problem is:

[0007] A multi-robot path planning method based on affine formation and trajectory optimization comprises the following steps:

[0008] Step 1: Use the multi-robot path planner CL-CBS to generate an initial path from the starting point to the end point for each robot, which is used to generate the formation path and as the initial solution to the trajectory optimization problem; CL-CBS has a two-layer search framework. In the upper-layer search, a vehicle body conflict tree is constructed to resolve the detected conflicts, so that the upper-layer search tree only needs to consider whether there is a conflict between the paths of the planning scheme, without considering the path planning problem; and in the lower-layer search, a spatiotemporal hybrid A* search algorithm is used to plan the best path for each robot that satisfies its own constraints;

[0009] Step 2: construct the multi-robot target formation as an undirected graph and calculate the corresponding stress matrix Ω;

[0010] Step 3: Generate the path of the leader first, and then use the formation control law to find the path of the follower as the formation path. The affine formation control law is based on the leader-follower method, and at least three leaders are required in two-dimensional space. Select at least three robots as leaders and the rest as followers.

[0011] Step 4: First, build a safe corridor based on the initial path and obstacle information, then design and solve the trajectory optimization problem to obtain the target path.

[0012] Further, the process of step one is as follows:

[0013] Step 1.1: Input the starting state and final state of each robot into the lower search. The robot state is represented by z = [x, y, θ] T , (x, y) represents the position of the robot, θ represents the yaw angle of the robot; When the lower-level search is used for the first time, the influence between robots will be ignored and an optimal path will be planned for each robot. The path of each robot is a series of discrete path points z(t) with time stamps, where the path points should satisfy the kinematic model of formula (1). The kinematic model of each robot is defined as,

[0014]

[0015] The control input of the robot is expressed as u = [v, ω] T , v represents the linear velocity of the robot, ω represents the angular velocity of the robot. z(t) represents the state of the robot at time t, u(t) represents the control input of the robot at time t, Indicates that each robot is subject to its maximum speed limit, that is, the robot's linear speed cannot exceed v max , the angular velocity cannot exceed ω max ;

[0016] Step 1.2: The upper search will check in chronological order whether there is a collision between two robots. Assume that the first collision is (a i ,a j ,t), which represents robot a i and robot a j The paths collide at time t, and the body conflict tree generates two child nodes to resolve this conflict. These two nodes will add a constraint to the two robots respectively to prevent them from passing through the location at time t.

[0017] Step 1.3: Re-plan the path of the robot with the newly added constraints to generate a path that satisfies the constraints. The paths of other robots remain unchanged. Then sort the nodes according to the sum of the path lengths of all robots. The nodes with the shortest length are prioritized for conflict detection until a result without conflict is found.

[0018] Furthermore, in step 2, the connection between the robots is represented by an undirected graph Indicates that is the vertex of the graph, n is the number of robots, ε is the edge of the graph, and the neighbor set of vertex i is Stress {ω ij} (i,j)∈ is the scalar weight on each edge, where ω ij =ω ji ,By hiding the target formation in the balance stress weight, the robot will eventually converge to the target formation, and the balance stress satisfies,

[0019]

[0020] where p i =[x i ,y i ] T It's a robot i The position of the robot a is represented by equation (2). i The forces on the surface are balanced, the resultant force is 0, and the stress matrix Ω and stress ω ij The relationship is,

[0021]

[0022] There are generally two methods to calculate the stress matrix, dynamic programming solution and linear matrix inequality solution. Using the linear matrix inequality solution method, first construct the matrix E as:

[0023]

[0024] Where n is the number of robots, d is the spatial dimension, d = 2, and m is the image The number of edges, Representation diagram The correlation matrix of is the i-th column of H. Represented as the relative position between the target formation robots, the augmented matrix Defined as,

[0025]

[0026] make is the stress vector of the target formation, given by Ω=H T diag(w)H and Know By diag(w)h i =diag(h i )w, the two positions are swapped, and we get That is, Ew = 0. So we define is a basis of Null(E), obtained by calculating the singular value decomposition of E, in is a set of undetermined coefficients;

[0027] Next is the coefficient of determination Assumptions The singular value decomposition of Let U = [U1, U2], where U1 is the first d+1 columns of U; because The rank of is d+1, so U1 is The column space of The null space of Therefore, ΩU1=0, so the rank of Ω in the first d+1 dimensions is 0, so the remaining nd-1 dimensions need to be full rank, that is, Ω in The null space of is positive definite, so Will Substituting in,

[0028]

[0029] By solving the linear matrix inequality above, we can get The stress matrix is ​​obtained and solved using Matlab's LMI toolbox.

[0030] The process of step three is as follows:

[0031] Step 3.1: Generate the paths of the three leaders. Since the relative position change between the leaders will affect the change of the overall formation, in order to maintain the formation, the l-β method is used to calculate the paths of the other two leaders a2 and a3 from the path of one leader a1. The yaw angle is consistent with a1. l is the distance and β is the angle. The values ​​of these two are set according to the requirements of the formation.

[0032] The position of leader a2 is calculated by equation (7). The path calculation of another leader a3 is similar. The path of leader a1 is the path of a1 in the initial path of step 1.

[0033]

[0034] Step 3.2: Generate the follower path using the formation control law. After obtaining the leader path generated in step 3.1, apply equation (8) to obtain the follower path.

[0035]

[0036] where v = [v x ,v y ] T represents the speed of the robot on the x-axis and y-axis, represents the vertex representing the follower;

[0037] The leader path generated in step 3.1 consists of discrete path points, and the time interval between each path point is 1 second. The leader's speed is calculated from this, and the follower's path is calculated in turn based on the calculated follower speed. In this way, the formation path of all robots is obtained, and this path is used as the objective function in the trajectory optimization problem.

[0038] In step 4, path point parameterization is used, and [x, y, θ, v, ω] is used as the optimization variable. The process is as follows:

[0039] Step 4.1: Construct a safe corridor and restrict the robot position within the safe corridor. The corridor does not intersect with obstacles, so the robot in the corridor is absolutely safe. The initial path of each robot obtained by CL-CBS is expressed as There are M+1 path points in total. The i-th line segment in the path consists of I i = < z i-1 →z i > means that a convex polyhedron S is generated around each line segment in the path i To form a safe corridor, in order to ensure safety, the following conditions should be met first:

[0040]

[0041] Where R(z) represents the area occupied by the robot, the body shape function R is determined by factors such as the size of the robot, O represents the obstacle space, and the collection of these convex polyhedrons constitutes the safety corridor, which is expressed by SC(π)={S i |i=1,…,M} means that the safety corridors need to be connected in sequence, so the following conditions must be met:

[0042]

[0043] Each safety corridor is centered on the line segment between two points and expands in the positive and negative directions of the x-axis and y-axis until it encounters an obstacle or exceeds the edge;

[0044] Step 4.2: Construct a trajectory optimization problem. After obtaining the path points and safe corridors, start trajectory optimization. The goal of trajectory optimization is to make the trajectory smooth, dynamically feasible and able to maintain the formation. A nonlinear optimization problem is designed, and the nonlinear optimization solver IPOPT is used to solve the problem to obtain the local optimal solution. IPOPT uses the interior point method to solve the following general nonlinear programming problems:

[0045]

[0046] In formula (11), x∈R n is the optimization variable, with a lower limit of x L , the upper limit is x U ; Function f is the objective function; Function g is a general nonlinear constraint, and its lower limit is g L , the upper limit is g U , by setting g L =g U To formulate equality constraints;

[0047] The nonlinear optimization problem constructed is:

[0048]

[0049] in represents the formation path point of the robot numbered j generated in step 3 at time i, N represents the number of robots, s j is the starting state of the robot numbered j, g j is the final state of the robot numbered j, d safe is the safe distance between robots, and To optimize the variables, their initial values ​​are provided by the initial path obtained in step 1; the objective function consists of three parts: the first part is to make the trajectory maintain the formation and punish the trajectory that is far away from the formation path; the second part is to make the trajectory smooth and as straight as possible; the third part makes the difference between two consecutive control inputs smaller, so that the speed change is as small as possible; in addition, 5 constraints are set, the first one is the restriction of the starting point and the end point; the second one ensures that the path point satisfies the kinematic model and ensures that the kinematics is feasible; the third one is the safety corridor restriction, so that the position of the trajectory point is within the safety corridor to ensure that the robot does not collide with obstacles; the fourth one limits the control input of each robot within the allowable range; the last one is the distance limit between robots to prevent collisions between robots;

[0050] Step 4.3: Process the path data and substitute it into the solution to generate the final path. Delete some of the path points at the end of the path generated by step 1, that is, the part with the Reeds–Shepp path, and fill the deleted path so that the number of path points for each robot is the same. The specific method of filling is to repeat the last path point until the number of path points for each path is the same, which means that the robot stays in place.

[0051] The technical concept of the present invention is: the multi-robot affine formation can achieve the target formation through the formation control law, but it avoids obstacles by artificially planning the trajectory, and the present invention enables the multi-robots to maintain a certain formation through an optimization-based method, while being safe and collision-free, and having dynamic feasibility. The present invention uses the l-β method to generate the leader path so that the overall formation remains stable, and at the same time, by constructing a safe corridor and designing optimization problems, the safety and dynamic feasibility of the robot's path are guaranteed. The advantage of using this method is that in an environment with obstacles, the multi-robot system can maintain a certain formation while avoiding obstacles.

[0052] The beneficial effects of the present invention are mainly manifested in that trajectory optimization is used to enable multiple robots to maintain a certain formation while being safe, collision-free, and dynamically feasible. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 It is a schematic diagram of affine transformation, where (a) is the original image, (b) is rotation, (c) is scaling, and (d) is shearing.

[0054] Figure 2 It is a system structure diagram of a multi-robot path planning method based on affine formation and trajectory optimization.

[0055] Figure 3 It is a diagram of leader path calculation.

[0056] Figure 4 This is a schematic diagram of the safety corridor.

[0057] Figure 5 It is a schematic diagram of the formation.

[0058] Figure 6 This is the experimental result of the algorithm of the present invention.

[0059] Figure 7 It is the experimental result of CL-CBS algorithm.

[0060] Figure 8 It is the experimental result of CL-CBS-II algorithm.

[0061] Fig. 9: is a formation error comparison diagram of the algorithm, wherein (a) represents the result of the method of the present invention when the number of obstacles is 10, (b) represents the result of the CL-CBS algorithm when the number of obstacles is 10, (c) represents the result of the method of the present invention when the number of obstacles is 20, (d) represents the result of the CL-CBS algorithm when the number of obstacles is 20, (e) represents the result of the method of the present invention when the number of obstacles is 30, and (f) represents the result of the CL-CBS algorithm when the number of obstacles is 30. DETAILED DESCRIPTION

[0062] The present invention will be further described below in conjunction with the accompanying drawings.

[0063] Reference Figures 1 to 9 , a multi-robot path planning method based on affine formation and trajectory optimization, comprising the following steps:

[0064] Step 1: Use the multi-robot path planner CL-CBS to generate an initial path from the starting point to the end point for each robot, which is used to generate the formation path and as the initial solution to the trajectory optimization problem. The algorithm has the ability to plan kinematically feasible paths for a large number of robots with Ackerman motion models in a continuous workspace. CL-CBS has a two-layer search framework. In the upper-level search, a body conflict tree is constructed to resolve the detected conflicts, so that the upper-level search tree only needs to consider whether there is a conflict between the paths of the planning scheme, without considering the path planning problem; in the lower-level search, a time-space hybrid A* search algorithm is used to plan the best path for each robot that meets its own constraints. The specific implementation steps are as follows:

[0065] Step 1.1: Input the starting state and final state of each robot into the lower search. The robot state is represented by z = [x, y, θ] T , (x, y) represents the position of the robot, and θ represents the yaw angle of the robot. When the lower-level search is used for the first time, it will ignore the influence between robots and plan an optimal path for each robot. The path of each robot is a series of discrete path points z(t) with timestamps, where the path points should satisfy the kinematic model of equation (1). The kinematic model of each robot is defined as

[0066]

[0067] The control input of the robot is expressed as u = [v, ω] T , v represents the linear velocity of the robot, ω represents the angular velocity of the robot. z(t) represents the state of the robot at time t, u(t) represents the control input of the robot at time t, Indicates that each robot is subject to its maximum speed limit, that is, the robot's linear speed cannot exceed v max , the angular velocity cannot exceed ωmax .

[0068] Step 1.2: The upper search will check in chronological order whether there is a collision between two robots. Assume that the first collision is (a i ,a j ,t), which represents robot a i and robot a j The paths collided at time t, and the body conflict tree generates two child nodes to resolve this conflict. These two nodes will add a constraint to the two robots respectively to prevent them from passing through the location at time t.

[0069] Step 1.3: Re-plan the path of the robot with the newly added constraints to generate a path that satisfies the constraints. The paths of other robots remain unchanged. Then the nodes are sorted according to the sum of the path lengths of all robots, and the nodes with the shortest length are prioritized for conflict detection until a result without conflict is found.

[0070] Step 2: construct the multi-robot target formation as an undirected graph and calculate the corresponding stress matrix Ω. The connection between robots is represented by an undirected graph Indicates that is the vertex of the graph, n is the number of robots, and ε is the edge of the graph. The neighbor set of vertex i is Stress {ω ij} (i,j)∈ε is the scalar weight on each edge, where ω ij =ω ji By hiding the target formation in the balance stress weight, the robot can finally converge to the target formation, and the balance stress satisfies

[0071]

[0072] where p i =[x i ,y i ] T It's a robot i The position of the robot a can be understood as i The forces on the body are balanced and the resultant force is 0. The stress matrix Ω and stress ω ij The relationship is

[0073]

[0074] There are generally two methods for calculating the stress matrix, dynamic programming solution and linear matrix inequality solution. The present invention uses the linear matrix inequality solution method. First, construct the matrix E as

[0075]

[0076] Where n is the number of robots, d is the spatial dimension, in this invention, d=2, and m is the The number of edges. Representation diagram The correlation matrix of is the i-th column of H. Represented as the relative position between the target formation robots, the augmented matrix Defined as

[0077]

[0078] make is the stress vector of the target formation, given by Ω=H T diag(w)H and Know By diag(w)h i =diag(h i )w, the two positions are swapped, and we get That is, Ew = 0. So we define is a basis of Null(E), which can be obtained by calculating the singular value decomposition of E, and we can get in is a set of undetermined coefficients.

[0079] Next is the coefficient of determination Assumptions The singular value decomposition of Let U = [U1, U2], where U1 is the first d+1 columns of U. The rank of is d+1, so U1 is The column space of The null space of . Therefore, ΩU1=0, so the rank of Ω in the first d+1 dimensions is 0, so the remaining nd-1 dimensions need to be full rank, that is, Ω in is positive definite on the null space of . So we have Will Substitute

[0080]

[0081] By solving the linear matrix inequality above, we can get The stress matrix is ​​obtained, and the present invention uses the LMI toolbox of Matlab to solve it.

[0082] Step 3: Generate the path of the leader first, and then use the formation control law to find the path of the follower as the formation path. The affine formation control law is based on the pilot-follower method, and at least three leaders are required in a two-dimensional space. The present invention selects three robots as leaders and the remaining robots as followers. The specific implementation steps are as follows:

[0083] Step 3.1: Generate the paths of the three leaders. Since the relative position change between the leaders will affect the change of the overall formation, in order to maintain the formation, the present invention uses the l-β method to calculate the paths of the other two leaders a2 and a3 from the path of one leader a1, and the yaw angle is consistent with a1. Figure 3 As shown in Figure 2, l is the distance and β is the angle. The values ​​of these two are set according to the requirements of the formation.

[0084] The position of leader a2 can be calculated by equation (7), and the path calculation of another leader a3 is similar. The path of leader a1 is the path of a1 in the initial path of step 1.

[0085]

[0086] Step 3.2: Generate the follower path using the formation control law. After obtaining the leader path generated in step 3.1, apply equation (8) to obtain the follower path.

[0087]

[0088] where v = [v x ,v y ] T represents the speed of the robot on the x-axis and y-axis, Denotes the vertex representing the follower. The leader path generated in step 3.1 consists of discrete path points, and the time interval between each path point is 1 second. From this, the speed of the leader can be calculated, and vice versa, the path of the follower can be calculated based on the calculated follower speed. In this way, the formation path of all robots is obtained. This path cannot be used directly because it does not consider factors such as obstacle avoidance. However, it strictly follows the formation formation and can be used as the objective function in the trajectory optimization problem. On the premise of ensuring that the robot can avoid obstacles, it is necessary to ensure that the entire robot team moves in formation as much as possible.

[0089] Step 4: First build a safe corridor based on the initial path and obstacle information, then design the trajectory optimization problem and solve it to obtain the target path. In order to optimize the trajectory, you need to choose a way to parameterize the trajectory. Common methods include: path point parameterization, using a series of path points to represent the trajectory; spline curve parameterization, using Bezier curves, B-spline curves, etc. to represent the trajectory; function parameterization, using an explicit or implicit function f(x,y)=0 to represent the trajectory. The present invention uses path point parameterization and takes [x,y,θ,v,ω] as optimization variables. The specific implementation steps are as follows:

[0090] Step 4.1: Construct a safe corridor. In order to avoid collision between the robot and obstacles, the present invention can limit the robot position to the safe corridor. The corridor does not intersect with obstacles, so the robot in the corridor is absolutely safe. The initial path of each robot obtained by CL-CBS is expressed as There are M+1 path points in total. The i-th line segment in the path consists of I i = < z i-1 →z i > indicates that the present invention generates a convex polyhedron S around each line segment in the path i To form a safe corridor, in order to ensure safety, the following conditions should be met first:

[0091]

[0092] Where R(z) represents the area occupied by the robot, and the body shape function R is determined by factors such as the size of the robot. O represents the obstacle space. The set of these convex polyhedrons constitutes the safety corridor, which is represented by SC(π) = {S i |i=1,…,M}. It should be noted that the safety corridor needs to maintain sequential connection, so the following conditions must be met:

[0093]

[0094] The construction of safe corridors Figure 4 As shown in the figure, the solid circle is an obstacle, the hollow circle is a path point, and the dotted box is a safety corridor. Each safety corridor is centered on the line segment between the two points and expands in the positive and negative directions of the x-axis and y-axis until it encounters an obstacle or exceeds the margin.

[0095] Step 4.2: Construct a trajectory optimization problem. After obtaining the path points and safe corridors, trajectory optimization can be started. The goal of trajectory optimization is to make the trajectory smooth, dynamically feasible and able to maintain the formation. The present invention designs a nonlinear optimization problem and uses the nonlinear optimization solver IPOPT to solve the problem to obtain the (local) optimal solution. IPOPT uses the interior point method to solve the following general nonlinear programming problems:

[0096]

[0097] In formula (11), x∈R n is the optimization variable, with a lower limit of x L , the upper limit is x U ; Function f is the objective function; Function g is a general nonlinear constraint, and its lower limit is g L , the upper limit is g U , you can set g L =g U To formulate equality constraints.

[0098] The nonlinear optimization problem constructed in the present invention is:

[0099]

[0100] in represents the formation path point of the robot numbered j generated in step 3 at time i, N represents the number of robots, s j is the starting state of the robot numbered j, g j is the final state of the robot numbered j, d safe is the safe distance between robots. and is an optimization variable, and its initial value is provided by the initial path obtained in step 1. The objective function consists of three parts: the first part is to make the trajectory maintain the formation and punish the trajectory that is far away from the formation path; the second part is to make the trajectory smooth and as straight as possible; the third part makes the difference between two consecutive control inputs smaller, so that the speed change is as small as possible. In addition, the present invention sets 5 constraints, the first one is the restriction of the starting point and the end point; the second one ensures that the path point satisfies the kinematic model to ensure that the kinematics is feasible; the third one is the safety corridor restriction, so that the position of the trajectory point is within the safety corridor to ensure that the robot does not collide with obstacles; the fourth one limits the control input of each robot within the allowable range; the last one is the distance limit between robots to prevent collisions between robots.

[0101] Step 4.3: Process the path data and substitute it into the solution to generate the final path. Because in the CL-CBS algorithm, when the robot is within a certain range close to the end point, the lower-level search will stop expanding, but will call the algorithm to generate a Reeds–Shepp path from the current point to the end point as the final path, which is used to solve the problem that the end state cannot be accurately reached when using a fixed step size to expand the path. Therefore, the present invention deletes such a path, and the paths used in subsequent steps are all processed paths, and finally CL-CBS is used to plan the last part of the path. The specific method is to delete some of the latter path points in the path generated by step one, that is, the part with the Reeds–Shepp path, and fill the deleted path so that the number of path points for each robot is the same. The specific method of filling is to repeat the last path point until the number of path points for each path is the same, which is understood as the robot staying in place.

[0102] In order to verify the performance of the method of the present invention, a simulation experiment was conducted. There are seven robots in the experiment, of which number 0, 1, and 2 are leaders, and the remaining four are followers. The formation is as follows: Figure 5 shown.

[0103] The map size is set to 150*50, and the obstacles are randomly generated. However, the obstacles that are too close to the starting point and the end point are modified to ensure that the robot can reach the end point from the starting point. A total of nine maps are generated and divided into three groups, with 10, 20 and 30 obstacles respectively. The starting states of the seven robots are numbered in order: [30,25,0], [24,31,0], [24,19,0], [18,31,0], [18,19,0], [12,31,0], [12,19,0]. The ending states are [130,25,0], [124,31,0], [124,19,0], [118,31,0], [118,19,0], [112,31,0], [112,19,0].

[0104] On each map, the algorithm of the present invention, the CL-CBS algorithm, and the CL-CBS-II algorithm, in which the leader path is planned by CL-CBS and the follower path is generated by the formation control law, are used respectively. First, the experimental results are shown. Figure 6 , Figure 7 , Figure 8 The running results of the three algorithms under 20 obstacles are shown, where the solid circle is the obstacle, the dotted line is the robot's path, and the dotted rectangle is the end point.

[0105] From the above experimental results, for the CL-CBS-II algorithm, which uses CL-CBS to plan the path for the leader and generates the path for the followers by the formation control law, it can be seen that the followers have no obstacle avoidance ability and do not consider kinematics and dynamics at all. For the CL-CBS algorithm, since it does not consider the formation, although it can be seen that all robots can reach the end point without collision, the target formation will not be formed between the robots during the whole process. For the algorithm of the present invention, it can be seen that the seven robots can not only reach the end point safely, but also maintain a certain formation.

[0106] Then the path lengths generated by the three algorithms were calculated, and the path lengths of the seven robots were added together. The calculation results are shown in Table 1. The table shows the results of nine experiments, and the calculation results retain 4 decimal places. It can be seen from the table that in most cases, the path length of the algorithm of the present invention will be greater than the CL-CBS algorithm and less than the CL-CBS-II algorithm. This is because the robots sometimes do not take the shortest path in order to maintain the formation. However, from the results, the increase in path length is not very high, with a maximum increase of 4.76%. In some cases, due to the trajectory optimization of this algorithm, there will be a situation where the path length is less than the CL-CBS algorithm.

[0107] Table 1 Total path length

[0108]

[0109]

[0110] Finally, the experiment also calculated the follower formation error of the algorithm of the present invention and the CL-CBS algorithm, and compared them under different numbers of obstacles. Since the follower path of the CL-CBS-II algorithm is directly generated by the formation control law, there is no formation error. The results are shown in Table 2. The formation error is the distance difference between the actual position of the follower and the ideal position calculated by the formation control law. It can be seen from Table 2 that the formation error of the algorithm of the present invention will increase when encountering an obstacle, but after passing the obstacle, the formation error will drop to 0, while the formation error of the CL-CBS algorithm will not drop to 0 after passing the obstacle.

[0111] The contents described in the embodiments of this specification are merely enumerations of implementation forms of the inventive concept and are for illustrative purposes only. The protection scope of the present invention should not be considered to be limited to the specific forms described in this embodiment, and the protection scope of the present invention also extends to equivalent technical means that can be thought of by ordinary technicians in this field based on the inventive concept.

Claims

1. A multi-robot path planning method based on affine formation and trajectory optimization, characterized in that: The method comprises the following steps: Step 1: Use the multi-robot path planner CL-CBS to generate an initial path from the starting point to the end point for each robot, which is used to generate the formation path and as the initial solution to the trajectory optimization problem; CL-CBS has a two-layer search framework. In the upper-layer search, a vehicle body conflict tree is constructed to resolve the detected conflicts, so that the upper-layer search tree only needs to consider whether there is a conflict between the paths of the planning scheme, without considering the path planning problem; and in the lower-layer search, a spatiotemporal hybrid A* search algorithm is used to plan the best path for each robot that satisfies its own constraints; Step 2: construct the multi-robot target formation as an undirected graph and calculate the corresponding stress matrix Ω; Step 3: Generate the path of the leader first, and then use the formation control law to find the path of the follower as the formation path. The affine formation control law is based on the leader-follower method, and at least three leaders are required in two-dimensional space. Select at least three robots as leaders and the rest as followers. Step 4: First, build a safe corridor based on the initial path and obstacle information, then design and solve the trajectory optimization problem to obtain the target path.

2. The multi-robot path planning method based on affine formation and trajectory optimization as claimed in claim 1, characterized in that: The process of step one is as follows: Step 1.1: Input the starting state and final state of each robot into the lower-level search. The robot state is represented by z = [x, y, θ] T , (x, y) represents the position of the robot, θ represents the yaw angle of the robot; When the lower-level search is used for the first time, the influence between robots will be ignored and an optimal path will be planned for each robot. The path of each robot is a series of discrete path points z(t) with time stamps, where the path points should satisfy the kinematic model of formula (1). The kinematic model of each robot is defined as, The control input of the robot is expressed as u = [v, ω] T , v represents the linear velocity of the robot, ω represents the angular velocity of the robot, z(t) represents the state of the robot at time t, u(t) represents the control input of the robot at time t, Indicates that each robot is subject to its maximum speed limit, that is, the robot's linear speed cannot exceed v max , the angular velocity cannot exceed ω max ; Step 1.2: The upper search will check in chronological order whether there is a collision between two robots. Assume that the first collision is (a i ,a j ,t), which represents robot a i and robot a j The paths collide at time t, and the body conflict tree generates two child nodes to resolve this conflict. These two nodes will add a constraint to the two robots respectively to prevent them from passing through the location at time t. Step 1.3: Re-plan the path of the robot with the newly added constraints to generate a path that satisfies the constraints. The paths of other robots remain unchanged. Then sort the nodes according to the sum of the path lengths of all robots. The nodes with the shortest length are prioritized for conflict detection until a result without conflict is found.

3. The multi-robot path planning method based on affine formation and trajectory optimization as claimed in claim 1 or 2, characterized in that: In the step 2, the connection between robots is represented by an undirected graph Indicates that is the vertex of the graph, n is the number of robots, ε is the edge of the graph, and the neighbor set of vertex i is Stress {ω ij } (i,j)∈ is the scalar weight on each edge, where ω ij =ω ji ,By hiding the target formation in the balance stress weight, the robot will eventually converge to the target formation, and the balance stress satisfies, where p i =[x i ,y i ] T It's a robot i The position of the robot a is represented by equation (2). i The forces on the body are balanced, the resultant force is 0, and the stress matrix Ω and stress ω ij The relationship is, There are generally two methods to calculate the stress matrix, dynamic programming solution and linear matrix inequality solution. Using the linear matrix inequality solution method, first construct the matrix E as: Where n is the number of robots, d is the spatial dimension, d = 2, and m is the image The number of edges, Representation diagram The correlation matrix of is the i-th column of H, Represented as the relative position between the target formation robots, the augmented matrix Defined as, make is the stress vector of the target formation, given by Ω=H T diag(w)H and Know By diag(w)h i =diag(h i )w, the two positions are swapped, and we get That is, Ew=0, so we define is a basis of Null(E), obtained by calculating the singular value decomposition of E, in is a set of undetermined coefficients; Next is the coefficient of determination Assumptions The singular value decomposition of Let U = [U1, U2], where U1 is the first d+1 columns of U; because The rank of is d+1, so U1 is The column space of The null space of Therefore, ΩU1=0, so the rank of Ω in the first d+1 dimensions is 0, so the remaining nd-1 dimensions need to be full rank, that is, Ω in is positive definite on the null space, so Will Substituting in, By solving the linear matrix inequality above, we can get The stress matrix is ​​obtained and solved using Matlab's LMI toolbox.

4. The multi-robot path planning method based on affine formation and trajectory optimization as claimed in claim 1 or 2, characterized in that: The process of step three is as follows: Step 3.1: Generate the paths of the three leaders. Since the relative position change between the leaders will affect the change of the overall formation, in order to maintain the formation, the l-β method is used to calculate the paths of the other two leaders a2 and a3 from the path of one leader a1. The yaw angle is consistent with a1. l is the distance and β is the angle. The values ​​of these two are set according to the requirements of the formation. The position of leader a2 is calculated by equation (7). The path calculation of another leader a3 is similar. The path of leader a1 is the path of a1 in the initial path of step 1. Step 3.2: Generate the follower path using the formation control law. After obtaining the leader path generated in step 3.1, apply equation (8) to obtain the follower path. where v = [v x ,v y ] T represents the speed of the robot on the x-axis and y-axis, represents the vertex representing the follower; The leader path generated in step 3.1 consists of discrete path points, and the time interval between each path point is 1 second. The leader's speed is calculated from this, and the follower's path is calculated based on the calculated follower speed. In this way, the formation path of all robots is obtained, and this path is used as the objective function in the trajectory optimization problem.

5. The multi-robot path planning method based on affine formation and trajectory optimization as claimed in claim 1 or 2, characterized in that: In step 4, path point parameterization is used, and [x, y, θ, v, ω] is used as the optimization variable. The process is as follows: Step 4.1: Construct a safe corridor and restrict the robot position within the safe corridor. The corridor does not intersect with obstacles, so the robot in the corridor is absolutely safe. The initial path of each robot obtained by CL-CBS is expressed as There are M+1 path points in total. The i-th line segment in the path consists of I i = < z i-1 →z i > means that a convex polyhedron S is generated around each line segment in the path i To form a safe corridor, in order to ensure safety, the following conditions should be met first: Where R(z) represents the area occupied by the robot, the body shape function R is determined by factors such as the size of the robot, O represents the obstacle space, and the collection of these convex polyhedrons constitutes the safety corridor, which is expressed by SC(π)={S i |i=1,…,M} means that the safety corridors need to be connected in sequence, so the following conditions must be met: Each safety corridor is centered on the line segment between two points and expands in the positive and negative directions of the x-axis and y-axis until it encounters an obstacle or exceeds the edge; Step 4.2: Construct a trajectory optimization problem. After obtaining the path points and safe corridors, start trajectory optimization. The goal of trajectory optimization is to make the trajectory smooth, dynamically feasible and able to maintain the formation. A nonlinear optimization problem is designed, and the nonlinear optimization solver IPOPT is used to solve the problem to obtain the local optimal solution. IPOPT uses the interior point method to solve the following general nonlinear programming problems: In formula (11), x∈R n is the optimization variable, with a lower limit of x L , the upper limit is x U ; Function f is the objective function; Function g is a general nonlinear constraint, and its lower limit is g L , the upper limit is g U , by setting g L =g U To formulate equality constraints; The constructed nonlinear optimization problem is: in represents the formation path point of the robot numbered j generated in step 3 at time i, N represents the number of robots, s j is the starting state of the robot numbered j, g j is the final state of the robot numbered j, d safe is the safe distance between robots, and To optimize the variables, their initial values ​​are provided by the initial path obtained in step 1; The objective function consists of three parts: the first part is to keep the trajectory in formation and penalize the trajectory that is far away from the formation path; The second part is to make the trajectory smooth and as straight as possible; the third part makes the difference between two consecutive control inputs smaller, so that the speed change is as small as possible; in addition, 5 constraints are set. The first one is the limit of the starting point and the end point; the second one ensures that the path points meet the kinematic model to ensure that the kinematics is feasible; the third one is the safety corridor limit, so that the position of the trajectory point is within the safety corridor to ensure that the robot does not collide with obstacles; the fourth one limits the control input of each robot within the allowable range; the last one is the distance limit between robots to prevent collisions between robots; Step 4.3: Process the path data and substitute it into the solution to generate the final path. Delete some of the path points at the end of the path generated by step 1, that is, the part with the Reeds–Shepp path, and fill the deleted path so that the number of path points for each robot is the same. The specific method of filling is to repeat the last path point until the number of path points for each path is the same, which means that the robot stays in place.

Images