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

Abstract

The application discloses 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; then, calculating a stress matrix of a target formation to obtain a formation path; and finally, designing a target function and a constraint of the trajectory optimization problem, and solving the problem by using a solver to achieve the target of the application. The method can make the multi-robot keep a certain formation, and can be safe and collision-free and have kinetic feasibility by using the trajectory optimization.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of multi-robot path planning and formation control, and particularly relates to a multi-robot path planning method based on affine formation and trajectory optimization. BACKGROUND

[0002] In recent years, with the development of science and technology, robots are widely used in various industries. For industrial robots, automated production lines greatly improve the efficiency of factories; in the field of national defense, the addition of unmanned aerial vehicles, robotic dogs and special robots improves the combat efficiency. Compared with single robot systems, multi-robot systems can complete work content such as cargo transportation, environmental information detection, cooperative hunting and patrol through team cooperation. Multi-robot systems have many advantages, including higher work efficiency, more extensive task processing capability, greater system redundancy and better robustness, and therefore are of great concern in the industry. In the field of multi-robot systems, path planning and formation control are very popular topics. When these systems are deployed in complex environments to perform tasks, they not only need to maintain a coordinated formation, but also need to be able to avoid obstacles on the path, and this ability is crucial to ensure that the multi-robot system can smoothly reach the target location and successfully complete the task. Therefore, path planning and formation coordination of multi-robots are the key foundation to ensure the efficient operation of the entire system.

[0003] Multi-robot formation control problem originates from the group behavior of creatures in nature, such as the collective migration behavior of bird flocks, the formation defense behavior of fish schools and the collective foraging behavior of ant colonies, etc. In the field of robots, multi-robot formation refers to the process in which multiple mobile robots form a desired geometric shape according to certain distance and motion state constraints during movement. In recent years, scholars have proposed various formation control methods such as leader-follower method, virtual structure method, behavior-based method and artificial potential field method. According to the different graph Laplacian matrices applied by the formation control law, it can be divided into the following four types: regular Laplacian matrix, signed Laplacian matrix, complex Laplacian matrix and generalized Laplacian matrix. Affine formation control is a formation control method based on signed Laplacian matrix. Affine transformation is a linear transformation that preserves the shape and scale of a figure, which can include translation, rotation, scaling and shearing operations, as shown in FIG. 1. An important feature of affine transformation is that it preserves the collinearity and parallelism of all lines and planes in the figure, i.e. after affine transformation, the originally collinear points are still collinear, and the originally parallel lines and planes remain parallel, which has good formation maneuvering flexibility. Figure 1

[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 gradually forming a formation that can maintain collinearity and distance ratio with the target formation, and gave the requirements of system stability for the formation topology under the conditions of undirected graph and directed graph. In 2018, Zhao further analyzed the affine formation control problem from the perspective of undirected graph, and proposed several formation control laws for different conditions, including first-order and second-order integrator models in static, uniform and variable speed conditions, and strict proof was carried out. The affine formation control law of Zhao adopts the leader-follower method, but the path of the 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 followers, the followers may not be able to avoid obstacles and the trajectories of the followers may not be consistent with the dynamics. In summary, how to make the multi-robot system maintain a certain formation in the obstacle environment while being safe and collision-free and having dynamic feasibility is an important problem affecting multi-robot formation. SUMMARY

[0005] In order to overcome the shortcomings of the prior art, for the multi-robot formation control problem in the obstacle environment, the present application proposes a multi-robot path planning method based on affine formation and trajectory optimization. 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. Then, the stress matrix of the target formation is calculated to obtain the formation path. Finally, the objective function and constraint of the trajectory optimization problem are designed, and the solver is used to solve the problem to achieve the goal of the present application. This method uses trajectory optimization to enable the multi-robot to maintain a certain formation while being safe and collision-free and having dynamic feasibility.

[0006] The technical scheme adopted by the present application to solve its technical problems is:

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

[0008] Step one, using a multi-robot path planner CL-CBS to generate an initial path from the starting point to the ending point for each robot, which is used to generate a formation path and as an initial solution for the trajectory optimization problem; CL-CBS has two layers of search framework. In the upper layer search, a vehicle body conflict tree is constructed to solve the detected conflicts, so that only the path conflict between the planning schemes needs to be considered in the upper layer search tree, and the path planning problem does not need to be considered. In the lower layer search, a space-time hybrid A* search algorithm is used to plan the best path for each robot that meets its own constraints;

[0009] Step two, the multi-robot target formation is constructed as an undirected graph, and the corresponding stress matrix Ω is calculated;

[0010] Step three, the path of the leader is generated first, and then the path of the follower is calculated as the formation path using the formation control law. The affine formation control law is based on the leader-follower method, and at least 3 leaders are needed in two-dimensional space. At least 3 robots are selected as leaders, and the rest are followers;

[0011] Step four: first, construct a safe corridor according to the initial path and obstacle information, and then design a trajectory optimization problem and solve it to obtain the target path.

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

[0013] Step 1.1: input the initial state and final state of each robot into the lower-level search. The robot state is represented as z = [x, y, θ] T , where (x, y) represents the position of the robot, and θ represents the yaw angle of the robot. The lower-level search will ignore the influence between robots when planning the optimal path for each robot for the first time. 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,

[0014]

[0015] where the control input of the robot is represented as u = [v, ω] T , v represents the linear velocity of the robot, and ω represents the angular velocity of the robot. z(t) represents the state of the robot at time t, and u(t) represents the control input of the robot at time t, , which means that each robot is subject to its maximum speed limit, i.e. the linear velocity of the robot cannot exceed v max , and the angular velocity cannot exceed ω max ;

[0016] Step 1.2: the upper-level search will check in chronological order whether a collision occurs between two robots. Suppose the first collision is (a i , a j , t), which means that the paths of robot a i and robot a j collide at time t. The body conflict tree will generate two child nodes to solve this conflict, which will add a constraint to each of the two robots to avoid passing through that location at time t;

[0017] Step 1.3: Re-plan the path for the robot with the new constraints to generate a path that satisfies the constraints, while keeping the paths of other robots unchanged; then sort the nodes according to the sum of the path lengths of all robots, and prioritize the node with the shortest path length for conflict detection until a conflict-free result is found.

[0018] Furthermore, in step two, the connections between robots are represented by an undirected graph. It means that among them Let i be a vertex of the graph, n be the number of robots, ε be the number of edges in the graph, and the set of neighbors of vertex i be ε. Stress {ω ij} (i,j)∈ It is the scalar weight of each edge, where ω ij =ω ji By hiding the target formation within the equilibrium stress weights, the robots ultimately converge to the target formation, satisfying the equilibrium stress.

[0019]

[0020] Where p i =[x i ,y i ] T It is robot A i The position, equation (2) can be understood as all actions acting on robot a i The forces on the surface reach equilibrium, the resultant force is 0, and the stress matrix Ω and stress ω ij The relationship is,

[0021]

[0022] There are generally two methods for calculating the stress matrix: dynamic programming and linear matrix inequality. Using the linear matrix inequality method, we first construct matrix E as follows:

[0023]

[0024] Where n is the number of robots, d is the spatial dimension (d=2), and m is the graph. The number of sides, Representation diagram The correlation matrix, It is the i-th column of H. The augmented matrix represents the relative positions of the target formation robots. Defined as,

[0025]

[0026] make It 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, interchanging the positions of both, we get i.e. Ew = 0. Then define is a basis of Null(E), which can be obtained by computing the singular value decomposition of E, we have where is a set of undetermined coefficients;

[0027] Next, we determine the coefficients Suppose the singular value decomposition of is Let U = [U1, U2], where U1 is the first d+1 columns of U; since the rank of is d+1, U1 is the column space of , U2 is the null space of ; since , ΩU1 = 0, so Ω has rank 0 in the first d+1 dimensions, the remaining n-d-1 dimensions need to be full rank, i.e. Ω is positive definite on the null space of , we have Substituting into the above equation, we have

[0028]

[0029] By solving the linear matrix inequality above, we get and the stress matrix is obtained, which is solved by using the LMI toolbox of Matlab.

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

[0031] Step 3.1: generate the path of three leaders, since the relative position between leaders will affect the change of the overall formation, in order to maintain the formation, the path of the other two leaders a2 and a3 is calculated from the path of a leader a1 using the l-β method, the yaw angle is consistent with a1; l is the distance, β is the angle, the values of the two are set according to the requirements of the formation formation;

[0032] The position of leader a2 is calculated by formula (7), and the path of another leader a3 is also calculated similarly, the path of leader a1 is the path of a1 in the initial path of step one,

[0033]

[0034] Step 3.2: generate the follower path using the formation control law, after obtaining the leader path generated in step 3.1, the follower path is obtained by applying formula (8),

[0035]

[0036] where v = [v x ,v y ] T denotes the velocity of the robot on x and y axes, denotes the vertex representing the follower;

[0037] Step 3.1 The leader path generated in step 3 is composed of discrete path points, each with a time interval of 1 second, from which the velocity of the leader is calculated, and in turn the velocity of the follower is calculated according to the calculated path of the follower, so that the formation path of all robots is obtained, which is used as the objective function in the trajectory optimization problem.

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

[0039] Step 4.1: Build a safety corridor to limit the robot position within the safety corridor, and the corridor does not intersect with the obstacle, then the robot in the corridor is absolutely safe; the initial path of each robot obtained by CL-CBS is represented as M+1 path points in total. The i i =<z i-1 →z i th segment in the path generates a convex polyhedron S i to form a safety 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, and the body shape function R is determined by factors such as the size of the robot, and O represents the obstacle space, the set of these convex polyhedrons constitutes a safety corridor, which is represented as SC(π) = {S i |i = 1, …, M}, it should be noted that the safety corridor needs to be sequentially connected, so the following conditions should be met,

[0042]

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

[0044] Step 4.2: Constructing the trajectory optimization problem. After obtaining the waypoints and safety corridors, trajectory optimization begins. The goal of trajectory optimization is to make the trajectory smooth, dynamically feasible, and able to maintain formation. A nonlinear optimization problem is designed and solved using the nonlinear optimization solver IPOPT to obtain the local optimum. IPOPT uses the interior-point method to solve general nonlinear programming problems of the following form.

[0045]

[0046] In equation (11), x∈R n To optimize the variables, the lower bound is x. L The upper limit is x U The function f is the objective function; the function g is a general nonlinear constraint with a lower limit of g. L The upper limit is g U By setting g L =g U To define equality constraints;

[0047] The constructed nonlinear optimization problem is as follows:

[0048]

[0049] in This represents the formation path point of robot j generated in step three at time i, N represents the number of robots, and s represents the number of robots. j This refers to the initial state of robot j, g j It is the termination state of robot j, d safe To maintain a safe distance between robots, and To optimize the variables, their initial values ​​are provided by the initial path obtained in step one. The objective function consists of three parts: the first part is to maintain the trajectories in formation, penalizing trajectories that stray far from the formation path; the second part is to smooth the trajectories, making them as straight as possible; the third part is to minimize the difference between two consecutive control inputs, minimizing the change in velocity. Additionally, five constraints are imposed: the first is a constraint on the start and end points; the second ensures that the path points satisfy the kinematic model, guaranteeing kinematic feasibility; the third is a safety corridor constraint, ensuring that the position of the trajectory points is within the safety corridor, preventing the robot from colliding with obstacles; the fourth restricts the control input of each robot to within the allowable range; and the last is a distance constraint between robots, preventing collisions between them.

[0050] Step 4.3: process the path data and substitute into the solution to generate the final path, delete some path points in the path generated in step one, that is, the part with the Reeds-Shepp path, and fill in the deleted path to make the number of path points of each robot the same, and the specific method of filling in is to repeat the last path point until the number of path points of each path is the same, and it is understood that the robot stays in place.

[0051] The technical concept of the present application is that: the multi-robot affine formation can realize the target formation through the formation control law, but it avoids obstacles through artificial planning of the trajectory, while the present application can make the multi-robot keep a certain formation, and also be safe and collision-free, and have dynamics feasibility through the method based on optimization. The present application uses the l-beta method to generate the leader path, so that the overall formation keeps stable, and through the construction of a safety corridor and the design of an optimization problem, the safety and dynamics feasibility of the path of the robot are ensured. The advantage of using this method is that in the presence of obstacles, the multi-robot system can keep a certain formation while avoiding obstacles.

[0052] The beneficial effects of the present application mainly manifest in: through the use of trajectory optimization, the multi-robot can keep a certain formation, and also be safe and collision-free, and have dynamics feasibility. BRIEF DESCRIPTION OF DRAWINGS

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

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

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

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

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

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

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

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

[0061] Figure 9is a comparison chart of formation error of the algorithm, wherein (a) represents the result of the method of the application 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 application 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 application 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 application will be further described below with reference to the drawings.

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

[0064] Step one, using a multi-robot path planner CL-CBS to generate an initial path for each robot from the starting point to the end point, which is used to generate a formation path and as an initial solution of the trajectory optimization problem. The algorithm has the ability to plan kinematically feasible paths for a large number of Ackerman motion model robots in continuous work space. CL-CBS has a two-layer search framework. In the upper layer search, a vehicle body conflict tree is constructed to solve the detected conflicts, so that only the inter-path conflict of the planning scheme needs to be considered in the upper layer search tree, and the path planning problem does not need to be considered. In the lower layer search, a space-time 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 the ending state of each robot into the lower layer search. The robot state is represented as z=[x,y,θ] T , (x, y) represents the position of the robot, and θ represents the yaw angle of the robot. The lower layer search will ignore the influence between robots for the first time to plan an optimal path for each robot. The path of each robot is a series of discrete path points z(t) with time stamp, wherein the kinematic model of formula (1) should be satisfied between path points. The kinematic model of each robot is defined as

[0066]

[0067] wherein the control input of the robot is represented as u=[v,ω] T , v represents the linear velocity of the robot, and ω 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, , which means that each robot is limited by its maximum speed, i.e. the linear velocity of the robot cannot exceed v max , and the angular velocity cannot exceed ωmax .

[0068] Step 1.2: The upper layer search will check whether the collision between two robots occurs in time sequence. Assuming that the first conflict is (a i ,a j ,t), which indicates that the paths of robot a i and robot a j collide at time t, the body conflict tree generates two child nodes to solve the conflict. The two nodes add a constraint to the two robots to avoid passing through the location at time t.

[0069] Step 1.3: The path of the newly added constraint robot is re-planned to generate a path that satisfies the constraint, and the path of the other robot remains unchanged. Then the nodes are sorted according to the sum of the path lengths of all robots, and the node with the shortest length is given priority for conflict detection until a conflict-free result is found.

[0070] Step 2: The multi-robot target formation is constructed as an undirected graph, and the corresponding stress matrix Ω is calculated. The relationship between robots is represented by an undirected graph , where V 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 The stress {ω ij} (i,j)∈ε is a scalar weight on each edge, where ω ij = ω ji . By hiding the target formation in the balanced stress weight, the robots can finally converge to the target formation, and the balanced stress satisfies

[0071]

[0072] where p i = [x i ,y i ] T is the position of robot a i . Equation (2) can be understood as the balance of all forces acting on robot a i , and the resultant force is 0. The relationship between the stress matrix Ω and the stress ω ij is

[0073]

[0074] There are generally two methods to calculate the stress matrix, dynamic programming solution and linear matrix inequality solution, and 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 graph. The number of sides. Representation diagram The correlation matrix, It is the i-th column of H. The augmented matrix represents the relative positions of the target formation robots. Defined as

[0077]

[0078] make It 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 If the two are interchanged, we get: That is, Ew = 0. Therefore, we define... It is a basis of Null(E), which can be obtained by calculating the singular value decomposition of E. in It is a set of undetermined coefficients.

[0079] The next step is to determine the coefficients. Assumption The singular value decomposition is 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, U2 is The zero space. Because... Therefore, ΩU1 = 0, so Ω has a rank of 0 in the first d+1 dimensions. Thus, the remaining nd-1 dimensions need to have full rank, meaning Ω has a rank of 0 in the first d+1 dimensions. It is positive definite on the null space. Therefore, we can obtain Will Substitution

[0080]

[0081] By solving the linear matrix inequality in the above equation, we can obtain... This yields the stress matrix, which is then solved using the LMI toolbox in Matlab.

[0082] Step three, first generate the leader's path, then use the formation control law to get the follower's path as the formation path. Affine formation control law is based on the leader-follower method, and at least 3 leaders are needed in two-dimensional space. The invention selects 3 robots as leaders and the rest as followers. The specific implementation steps are as follows:

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

[0084] The position of leader a2 can be calculated by formula (7), and the path of another leader a3 is calculated similarly. The path of leader a1 is the path of a1 in the initial path of step one.

[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 formula (8) to obtain the follower path.

[0087]

[0088] Where v = [v x ,v y ] T represents the speed of the robot on the x and y axes, represents the vertex representing the follower. The leader path generated in step 3.1 consists of discrete path points, each with a time interval of 1 second. The speed of the leader can be calculated, and the follower's path can be calculated based on the calculated follower's speed. In this way, the formation path of all robots is obtained. This path cannot be used directly because it does not take into account factors such as obstacle avoidance, but it strictly follows the formation formation and can be used as the objective function in the trajectory optimization problem. Under the premise of ensuring that the robot can avoid obstacles, the entire robot team can advance as much as possible according to the formation.

[0089] Step four: First, construct the safety corridor according to the initial path and obstacle information, then design the trajectory optimization problem and solve it to get the target path. In order to optimize the trajectory, a way to parameterize the trajectory needs to be chosen. Common methods are: path point parameterization, which represents the trajectory with a series of path points; spline curve parameterization, which represents the trajectory with Bezier curve, B-spline curve, etc.; function parameterization, which represents the trajectory with an explicit or implicit function f(x, y) = 0. The present invention uses path point parameterization, taking [x, y, θ, v, ω] as optimization variables, and the specific implementation steps are as follows:

[0090] Step 4.1: Construct the safety corridor. In order to avoid collision between the robot and the obstacle, the present invention can limit the robot's position within the safety corridor, which does not intersect with the obstacle, so that the robot in the corridor is absolutely safe. The initial path of each robot obtained by CL-CBS is represented as There are a total of M+1 path points. The i-th line segment in the path is represented by I i = <z i-1 →z i >, the present invention generates a convex polyhedron S i around each line segment in the path to form a safety 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 the size and other factors 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 be sequentially connected, so the following conditions should be met:

[0093]

[0094] The construction of the safety corridor is shown in Figure 4 . The solid circles are obstacles, the hollow circles are path points, and the dashed boxes are safety corridors. Each safety corridor is centered on the line segment between two points and is expanded in the positive and negative directions of the x and y axes until it encounters an obstacle or exceeds the margin.

[0095] Step 4.2: Construct the trajectory optimization problem. After obtaining the path points and safety corridor, trajectory optimization can begin. The goal of trajectory optimization is to make the trajectory smooth, dynamically feasible, and maintain formation. The present invention designs a nonlinear optimization problem and uses the nonlinear optimization solver IPOPT to solve it to get the (local) optimal solution of the problem. IPOPT uses the interior point method to solve the general nonlinear programming problem in the following form:

[0096]

[0097] x∈R n x is the optimization variable, the lower bound is x L , the upper bound is x U ; f is the objective function; g is the general nonlinear constraint, the lower bound is g L , the upper bound is g U , the equality constraint can be set by setting g L = g U .

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

[0099]

[0100] wherein represents the formation path point of the robot numbered j at time i generated by step three, N represents the number of robots, s j is the starting state of the robot numbered j, g j is the terminal state of the robot numbered j, d safe is the safety distance between robots. and are optimization variables, the initial values of which are provided by the initial path obtained in step one. The objective function is composed of three parts: the first part is to keep the trajectory in formation, and the trajectory far from the formation path is punished; 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 small, so that the change in speed is as small as possible. In addition, the present application sets five constraints: the first is the limitation of the starting point and the terminal point; the second guarantees that the path point satisfies the kinematics model, ensuring the kinematics feasible; the third is the safety corridor limitation, so that the position of the trajectory point is within the safety corridor, ensuring that the robot does not collide with the obstacle; the fourth limits the control input of each robot within the allowed range; and the last is the distance limitation between robots, which prevents the collision between robots.

[0101] Step 4.3: Process the path data and substitute into the solution to generate the final path. Since the lower layer search in the CL-CBS algorithm will stop expanding when the robot is within a certain range of the end point, and instead call the algorithm to generate a Reeds-Shepp path from the current point to the end point as the final path, to solve the problem of not being able to accurately reach the termination state using fixed step expansion. Therefore, the invention deletes such paths, and the paths used in subsequent steps are processed paths, and finally the last part of the path is planned using CL-CBS. The specific method is to delete some path points in the path generated in step one, that is, the part with the Reeds-Shepp path, and to fill the deleted path so that the number of path points of each robot is the same. The specific method of filling is to repeat the last path point until the number of path points of each path is the same, and it is understood that the robot stays in place.

[0102] To verify the performance of the method of the invention, a simulation experiment is performed. The experiment has a total of seven robots, of which numbers 0, 1, and 2 are leaders, and the remaining four are followers, and the formation is as shown in Figure 5 .

[0103] The size of the map is set to 150*50, and the obstacles therein are randomly generated, but the obstacles that are too close to the start and end points are modified to ensure that the robot can reach the end point from the start point. The experiment generates a total of nine maps and is divided into three groups, with the number of obstacles being 10, 20, and 30, respectively. The starting states of the seven robots are [30, 25, 0], [24, 31, 0], [24, 19, 0], [18, 31, 0], [18, 19, 0], [12, 31, 0], and [12, 19, 0] in order of numbering. The termination states are [130, 25, 0], [124, 31, 0], [124, 19, 0], [118, 31, 0], [118, 19, 0], [112, 31, 0], and [112, 19, 0].

[0104] On each map, the algorithm of the invention, the CL-CBS algorithm, and the CL-CBS-II algorithm using CL-CBS to plan the path for the leader and the path for the follower 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 circles are obstacles, the dashed lines are the paths of the robots, and the dashed rectangles are the end points.

[0105] From the above experimental results, for the CL-CBS-II algorithm in which the leader's path is planned using CL-CBS and the follower's path is generated by the formation control law, it can be seen that the follower has no obstacle avoidance ability at all and does not take into account the kinematics and dynamics. For the CL-CBS algorithm, since it does not take into account the formation, although it can be seen that all robots can reach the destination without collision, the robots do not form the target formation during the entire process. For the algorithm of the present application, it can be seen that the seven robots not only can safely reach the destination, but also can maintain a certain formation.

[0106] Then the path lengths generated by the three algorithms are calculated, the path lengths of the seven robots are all added up, and the calculation results are shown in Table 1, which shows the results of nine experiments, and the calculation results are kept to four decimal places. From the table, it can be seen that in most cases, the path length of the algorithm of the present application will be greater than that of the CL-CBS algorithm and less than that of the CL-CBS-II algorithm, because in order to maintain the formation between the robots, sometimes the shortest path is not taken, but from the results, the increase in path length is not very high, with a maximum increase of 4.76%, and in some cases, due to trajectory optimization by the algorithm of the present application, the path length will be less than that of the CL-CBS algorithm.

[0107] Table 1 Total path length

[0108]

[0109]

[0110] Finally, the follower formation error of the algorithm of the present application and the CL-CBS algorithm is calculated, and the comparison is made under different numbers of obstacles, and the CL-CBS-II algorithm does not have formation error because the follower path is directly generated by the formation control law, and 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. From Table 2, it can be seen that when the algorithm of the present application encounters obstacles, the formation error will increase, but after passing through the obstacles, the formation error will drop to 0, while the formation error of the CL-CBS algorithm will not drop to 0 after passing through the obstacles.

[0111] The content described in the embodiments of the present specification is only a list of implementation forms of the inventive concept, and is only for illustrative purposes. The protection scope of the present application should not be regarded as being limited to the specific forms described in the embodiments, and the protection scope of the present application also extends to equivalent technical means that can be thought of by those skilled in the art according to the inventive concept.

Claims

1. An affine formation and trajectory optimization based multi-robot path planning method, characterized in that, The method comprises the following steps: Step one, using a multi-robot path planner CL-CBS to generate an initial path for each robot from the starting point to the end point, for generating formation path and as the initial solution of trajectory optimization problem; CL-CBS has two layers of search framework, in the upper layer search, the body conflict tree is constructed to solve the detected conflict, so that only the path conflict between the planning scheme needs to be considered in the upper layer search tree, without considering the path planning problem; and in the lower layer search, a space-time hybrid A* search algorithm is used to plan the best path for each robot that meets its own constraints; Step two, the multi-robot target formation is constructed as an undirected graph, and the corresponding stress matrix is calculated ; Step three, first generate the leader's path, and then use the formation control law to obtain the follower's path as the formation path, the affine formation control law is based on the leader-follower method, and at least 3 leaders are needed in two-dimensional space, at least 3 robots are selected as leaders, and the rest of the robots are used as followers; Step four: first construct a safe corridor according to the initial path and obstacle information, and then design a trajectory optimization problem and solve it to obtain the target path; The process of step one is as follows: Step 1.1: Input the initial and final states of each robot into the lower-level search. The robot state is represented as follows: , Indicates the robot's position. The yaw angle of the robot is represented; the lower-level search ignores the influence between robots when it is used for the first time and plans an optimal path for each robot. The path of each robot is a series of discrete path points with timestamps, and the path points should all satisfy the kinematic model of equation (1). The kinematic model of each robot is defined as follows: （1）； where the control input of the robot is denoted as , the linear velocity of the robot is denoted as the angular velocity of the robot is denoted as the state of the robot at time is denoted as the control input of the robot at time is denoted as it is denoted that each robot is subject to its maximum speed, i.e., the linear velocity of the robot cannot exceed and the angular velocity cannot exceed ; Step 1.2: The upper layer search will check if there is a collision between two robots in time sequence, assuming the first collision is , which means the path of robot and robot collide at time , the body collision tree will generate two child nodes to solve this collision, which will add a constraint to each of the two robots to avoid passing through the same place at time ; Step 1.3: re-planning the path of the robot with new constraints to generate a path that meets the constraints, and 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 node with the shortest length is prioritized for conflict detection until a result without conflict is found.

2. The method of claim 1, wherein, In the second step, the connection between robots is represented by an undirected graph , where is the vertex of the graph, is the number of robots, is the edge of the graph, the neighbor set of vertex is , the stress is the scalar weight on each edge, where , the final convergence of robots to the target formation is achieved by hiding the target formation in the balanced stress weight, the balanced stress satisfies, （2）； wherein is the position of the robot , formula (2) is understood as all forces acting on the robot reach a balance, the resultant is 0, the stress matrix is in relation to the stress ​ （3）； There are two methods to calculate the stress matrix, dynamic programming solution and linear matrix inequality solution. Using linear matrix inequality solution method, first construct the matrix for （4）； in For the number of robots, For spatial dimensions, , For the image The number of sides, Representation diagram The correlation matrix, yes The List, The augmented matrix represents the relative positions of the target formation robots. Defined as, （5）； Let be the stress vector of the target formation, by and , know ; by , the positions of the two are interchanged, get , that is , so define is a set of bases, by calculating singular value decomposition, you can get , where is a set of undetermined coefficients; The next step is to determine the coefficients , assuming is the singular value decomposition of , let where are the first columns of ; since has rank , it follows that is the column space of , and is the null space of ; since , it follows that , so is of rank in the first dimensions, so the remaining dimensions need to be full rank, i.e. is positive definite on the null space of , giving , and substituting gives （6）； The stress matrix is obtained by solving the linear matrix inequality of the above equation The stress matrix is obtained by solving the linear matrix inequality of the above equation 3. The method of claim 1, wherein, The process of step three is as follows: Step 3.1: Generate the path of 3 leaders, since the relative position between leaders will affect the change of the whole formation, in order to maintain the formation, the path of the other two leaders is calculated by the path of one leader , the yaw angle is consistent with ; is the distance, is the angle, the values of the two are set according to the requirements of the formation formation;​​ Leaders The position is calculated using equation (7), and the other leader The path calculation is similar, leader The path is the same as the initial path in step one. The path, （7）； Step 3.2: generate the follower's path using the formation control law, after obtaining the leader's path generated in step 3.1, apply formula (8) to obtain the follower's path, （8）； wherein representing a robot axis and speed on the axis, , representing a vertex representing a follower; The leader's path generated in step 3.1 consists of discrete path points, and the time interval of each path point is 1 second, so the leader's speed is calculated, and the follower's speed is calculated in reverse, so the formation path of all robots is obtained, which is used as the objective function in the trajectory optimization problem.

4. The method of claim 1, wherein, In step four, the path point parameterization is used to As optimization variables, the process is as follows: Step 4.1: Construct a safety corridor, limit the robot position within the safety corridor, the corridor does not intersect with obstacles, then the robot in the corridor is absolutely safe; the initial path of each robot obtained by CL-CBS is represented as , a total of path points, the th line segment in the path is represented by , a convex polyhedron is generated around each line segment in the path to form a safety corridor, in order to ensure safety, the following conditions should be met first, （9）； wherein represents the area occupied by the robot, the body shape function is determined by the size of the robot and other factors, represents the obstacle space, the collection of convex polyhedrons constitutes the safety corridor, using represents, the safety corridor needs to be kept sequentially connected, so the following conditions need to be met, （10）； Each safe corridor is centered on a line segment between two points, and is expanded in the positive and negative directions of the x and y axes until it encounters an obstacle or exceeds the margin; 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 maintain formation; a nonlinear optimization problem is designed, and a nonlinear optimization solver IPOPT is used to solve the problem to obtain a local optimal solution, IPOPT uses an interior point method to solve the following general nonlinear programming problem, （11）； In formula (11) The lower limit for the optimization variable is The upper limit is ; the function is the objective function; the function is a general nonlinear constraint, the lower limit of which is The upper limit is The equality constraint is formulated by setting ; The constructed nonlinear optimization problem is: （12）； wherein represents the formation path point of the robot numbered at time , represents the number of robots, is the initial state of the robot numbered , is the final state of the robot numbered , is the safety distance between robots, and are optimization variables, the initial values of which are provided by the initial path obtained in step one; The objective function consists of three parts: the first part is to punish the trajectory that deviates from the formation path to keep the trajectory in formation; The second part is to make the trajectory smooth and straight. The third part makes the difference between two continuous control inputs small, so that the change of speed is as small as possible. Five constraints are added. The first constraint is the start and end point limit. The second constraint ensures that the path points meet the kinematic model and ensure the kinematic feasibility. The third constraint is the safety corridor limit, which ensures that the trajectory points are within the safety corridor and the robot does not collide with obstacles. The fourth constraint limits the control input of each robot within the allowed range. The last constraint is the distance limit between robots to prevent collision between robots. Step 4.3: Process the path data and substitute it into the solution to generate the final path. Delete some path points from the path generated in step one, i.e. the part with the Reeds-Shepp path, and fill in the deleted path to make the number of path points of each robot the same. The specific method of filling is to repeat the last path point until the number of path points of each path is the same. Understand that the robot stays in place without moving.

Images