Multi-robot probability trajectory generation method based on distributed model predictive control

Abstract

The invention discloses a multi-robot probability trajectory generation method based on distributed model predictive control, and relates to the technical field of multi-robot systems, and the method constructs collision avoidance constraints based on a safety corridor with time perception. Uncertainty introduced by state estimation noise and motion interference is fully considered in construction of collision avoidance constraint, probability collision avoidance constraint is converted into deterministic constraint of mean value and covariance of robot states, and robustness of collision avoidance is ensured; meanwhile, in order to solve the problem of deadlock possibly occurring in a multi-robot system, a warning tape mechanism is introduced into probability constraint for avoiding collision and is combined with a right hand rule, and robot trajectory planning is dynamically adjusted; finally, the probabilistic collision constraint avoiding and deadlock preventing mechanisms are integrated into a distributed model predictive control framework, so that a locally optimal collision-free trajectory is generated.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-robot systems, and particularly to a multi-robot probabilistic trajectory generation method based on distributed model predictive control. Background Art

[0002] Multi-robot systems are widely used in practical applications for advanced tasks such as target tracking, facility inspection, and search and rescue. In order to safely perform tasks in diverse environments, generating collision-free trajectories is crucial. Existing trajectory generation algorithms can generally be classified into the following types: search-based methods, sampling-based methods, potential field-based methods, learning-based methods, and optimization-based methods.

[0003] In recent years, optimization-based methods have received increasing attention due to their powerful modeling capabilities and scalability. Such methods typically rely on the Model Predictive Control (MPC) framework to generate robot paths by constructing collision avoidance constraints. In the literature "C.E. Luis and A.P. Schoellig, "Trajectory generation for multiagent point-to-point transitions via distributed model predictive control," IEEE Robotics and Automation Letters, vol. 4, no. 2, pp. 375–382, 2019.", the authors introduced a demand-driven collision avoidance technique with soft constraints in the distributed model predictive control framework, which improves scalability and success rate compared to centralized methods. In "D. Zhou, Z. Wang, S. Bandyopadhyay, and M. Schwager, "Fast, on-line collision avoidance for dynamic vehicles using buffered voronoi cells," IEEE Robotics and Automation Letters, vol. 2, no. 2, pp. 1047–1054, 2017.", an improved distributed model predictive control formulation based on extended buffered Voronoi cells was proposed to ensure collision avoidance. In "J. Tordesillas and J.P. How, "Mader: Trajectory planner in multiagent and dynamic environments," IEEE Transactions on Robotics, vol. 38, no. 1, pp. 463–476, 2021.", an asynchronous planning technique was developed that utilizes trajectory intervals represented by external polyhedra and introduces separating planes as decision variables within the model predictive control optimization framework.The authors of the literature "T. Jin, X. Wang, H. Ji, J. Di, and H. Yan, 'Collision avoidance for multiple quadrotors using elastic safety clearance based model predictive control,' in 2022 International Conference on Robotics and Automation (ICRA). IEEE, 2022, pp. 265–271." proposed a model predictive control based on elastic safety clearance to enhance collision avoidance for multi-robots. In "C. Toumieh and A. Lambert, 'Decentralized multi-agent planning using model predictive control and time-aware safe corridors,' IEEE Robotics and Automation Letters, vol. 7, no. 4, pp. 11 110–11117, 2022.", the safe corridor has a time concept and is incorporated into the model predictive control problem to constrain the position relationship between robots and static obstacles and other robots in the known environment. However, existing trajectory generation algorithms often assume that the states of robots are exactly known, which is not true in the real environment. In practical applications, state estimation noise and motion disturbances may introduce uncertainties in the trajectory generation process of multi-robot systems. Considering these uncertainties to ensure safe and robust collision avoidance is crucial.

[0004] The literature "C. Toumieh and A. Lambert, 'Decentralized multi-agent planning using model predictive control and time-aware safe corridors,' IEEE Robotics and Automation Letters, vol. 7, no. 4, pp. 11110–11117, 2022." proposed the concept of Time-Aware Safe Corridor (TASC), constructed a time-aware corridor to handle collisions with static obstacles and other robots simultaneously, and solved the trajectory planning problem based on the model predictive control framework. The TASC algorithm in it is divided into two stages. In the first stage, the algorithm generates a global path for each robot and constructs a safe corridor around this global path. This global path only considers the obstacle avoidance problem between the robot and static obstacles, without considering collisions between robots. In the second stage, the global path and the safe corridor are used to generate a local reference trajectory and a local safe corridor, and the model predictive control problem is solved iteratively to generate a local optimal trajectory.

[0005] The main disadvantages of the prior art are as follows:

[0006] 1. Insufficient handling of uncertainties: Most existing trajectory generation algorithms assume that the robot state is known. However, in reality, state estimation errors and motion disturbances are inevitable, and ignoring them may lead to failure in collision avoidance.

[0007] 2. Deadlock problem: Existing distributed algorithms are prone to deadlock in multi-robot systems, especially in symmetric configurations or in the absence of central coordination, resulting in robots being unable to continue moving towards the goal.

[0008] Currently, the problems of uncertainties and deadlocks existing in multi-robot systems cannot be solved. Summary of the Invention

[0009] In view of this, the present invention provides a multi-robot probabilistic trajectory generation method based on distributed model predictive control, which can solve the problems of uncertainties and deadlocks existing in multi-robot systems during trajectory generation.

[0010] To achieve the above object, the technical solution of the present invention is: A multi-robot probabilistic trajectory generation method based on distributed model predictive control, characterized in that it is used to generate a trajectory such that each robot can safely reach the target point from the starting point in the presence of uncertainties. The method specifically includes the following steps:

[0011] In a multi-robot system, first, a global path from the starting point to the target point is generated for each robot; a safety corridor is established around the global path; the global path is used to avoid static obstacles and does not consider collision avoidance with other robots.

[0012] The robot moves along the global path, and continuously samples position points to form a local reference trajectory; it moves from the starting point along the global path at the sampling speed, and the time interval between adjacent sampling points is h.

[0013] Before all robots reach the target point, a probabilistic trajectory generation iterative process is executed:

[0014] In each iteration, N points are sampled from the global path as the reference trajectory for N discrete positions in model predictive control; considering uncertainty and potential collisions with other robots, P polyhedrons are extracted from the safety corridor to form a local safety corridor, and considering the uncertainty introduced by state estimation noise and motion disturbances, a collision avoidance probability constraint is constructed in combination with the local safety corridor; robots exchange the trajectory information of their own plans as shared information with each other.

[0015] In each iteration, each robot updates its trajectory according to the shared information, combines the shared information, the collision avoidance constraint, and the probability constraint of avoiding deadlocks to construct a distributed model predictive control problem, and uses a solver to solve the distributed model predictive control problem to generate a locally optimal collision-free trajectory and update its own trajectory.

[0016] Furthermore, in each iteration, a distributed model predictive control problem is constructed by combining the shared information, the collision avoidance constraint, and the probability constraint of avoiding deadlocks, and a solver is used to solve the distributed model predictive control problem to generate a locally optimal collision-free trajectory and update its own trajectory. Specifically:

[0017] In each iteration, the initial state of the distributed model predictive control problem is set to the state of the previous iteration; if the solver fails to find a feasible solution within the time step h, the initial state of the next iteration is adjusted to the next state in the trajectory successfully solved in the previous time; if the solver still fails continuously when reaching the final state and the target point has not been reached, the algorithm is considered to have failed.

[0018] Furthermore, the multi-robot system consists of n robots, and each robot i is modeled as a rigid sphere with a radius of r i .

[0019] The non-linear discrete kinematic model of robot i is described as follows:

[0020]

[0021] where the superscript k represents the k-th moment, and the subscript i represents robot i; denotes the state of robot \(i\) at the \(k\) - th moment, including the position and the velocity denotes the control input at the \(k\) - th moment, which is the acceleration of robot \(i\) at the \(k\) - th moment; under the influence of uncertainties, the state of the robot follows a Gaussian distribution, and the initial state is defined as a Gaussian random variable with a mean of and a covariance of The process noise follows a Gaussian distribution with a mean of 0 and a covariance of \(W\) i k where \(W\) i k is a diagonal covariance matrix; is:

[0022]

[0023] where \(h\) is the discrete time interval.

[0024] In a multi - robot system, the velocities and accelerations of the robots follow the following constraints:

[0025]

[0026] where \(v\) max is the upper limit of the absolute value of the velocity, and \(a\) max is the upper limit of the absolute value of the acceleration.

[0027] Furthermore, considering the uncertainties introduced by state - estimation noise and motion disturbances, a collision - avoidance probability constraint is constructed by combining with a local safety corridor, specifically:

[0028]

[0029]

[0030] where denotes the probability that the state of robot \(i\) at the \(k\) - th moment belongs to the collision - avoidance safety state set \(\Delta\) o is the collision - probability threshold between the robot and the static obstacle, and \(erf\) -1 (1 - 2\(\Delta\) o ) is the quantile of the standard normal distribution corresponding to the collision probability \(\Delta\) o , is the multi - face coefficient matrix of the safety corridor at the \(k\) - th moment, is the constant - term vector of the polyhedron, is the transpose matrix of is the position mean of robot \(i\) at time \(k\). is the state covariance matrix of robot \(i\) at time \(k\). represents the state of robot \(i\) at time \(k\). belongs to the collision avoidance safety state set the probability of, \(\Delta\) r is the collision probability threshold between robots, erf -1 (1 - 2\(\Delta\) r ) is the collision probability \(\Delta\) r corresponding standard normal distribution quantile is the unit vector of is the transpose of and are the optimized trajectory points of robot \(i\) and robot \(j\) in the previous iteration \(l - 1\), \(r\) ij is the shortest safe distance between the two robots and are the state covariance matrices of robot \(i\) and robot \(j\) at time \(k\) respectively.

[0031] Furthermore, the probability constraint to avoid deadlock is specifically:

[0032]

[0033] where is the unit vector of is the transpose of is the position mean of robot \(i\) at the terminal time \(N\). and are the optimized trajectory points of robot \(i\) and robot \(j\) at the terminal time \(N\) in the previous iteration \(l - 1\), \(r\) ij is the shortest safe distance between the two robots, \(\varepsilon\) ij is the warning distance between the two robots, erf -1 (1 - 2\(\Delta\) r ) is the collision probability \(\Delta\) r corresponding standard normal distribution quantile and are the state covariance matrices of robot \(i\) and robot \(j\) at time \(k\) respectively.

[0034] Add a penalty term \(J\) to solve the deadlock problem w :

[0035] \(J\) w =\(\rho\) ij (\(\varepsilon\) ij -\(\varepsilon\) max ) 2,

[0036] where ρ ij > 0 is a parameter related to the repulsive force, and by adjusting this parameter, robot i is moved away from robot j.

[0037] Furthermore, the constructed distributed model predictive control problem is specifically:

[0038]

[0039] where is the objective function of the distributed model predictive control problem, is the mean value of the state of robot i at time k, is its control input at time k, is the state sequence, u i is the control input sequence, x ref,i is the reference state sequence, ρ ij is the adjustment parameter in the deadlock avoidance mechanism, ε ij is the warning distance between robot i and robot j, N is the length of the optimization time domain, is the initial state, is the mean value of the initial state.

[0040] Beneficial effects:

[0041] 1: The multi-robot probabilistic trajectory generation method based on distributed model predictive control provided by the present invention aims to: generate trajectories such that each robot can safely reach the target point from the starting point in the presence of uncertainties. The algorithm constructs collision avoidance constraints based on a time-aware safety corridor, and its core improvement lies in fully considering the uncertainties introduced by state estimation noise and motion disturbances in the construction of collision avoidance constraints, converting the probabilistic collision avoidance constraints into deterministic constraints on the mean and covariance of the robot state, ensuring the robustness of collision avoidance; at the same time, to solve the possible deadlock problem in the multi-robot system, the algorithm introduces a warning band mechanism in the probabilistic collision avoidance constraints, combines it with the "right-hand rule", and dynamically adjusts the robot trajectory planning; finally, integrates these probabilistic collision avoidance constraints and the deadlock prevention mechanism into the distributed model predictive control framework, thereby generating locally optimal collision-free trajectories. Description of the drawings

[0042] Figure 1 is a schematic diagram of the deadlock avoidance mechanism; Figure 1 In (a), when the forces reach an equilibrium state, robot 1 is deadlocked. Figure 1 In (b), after introducing the right-hand rule and the warning band, decreases, increases, and the resultant force at this time Not zero, deadlock is released;

[0043] Figure 2 is the simulation effect diagram in the embodiment of the present invention;

[0044] Figure 3 is the motion trajectory and speed diagram of the robot. Specific implementation manner

[0045] The present invention will be described in detail below with reference to the accompanying drawings and by way of examples.

[0046] The present invention provides a multi-robot probabilistic trajectory generation method based on distributed model predictive control, which is used to generate trajectories such that each robot can safely reach the target point from the starting point in the presence of uncertainties. The method specifically includes the following steps:

[0047] Step 1: In a multi-robot system, first generate a global path from the starting point to the target point for each robot; establish a safety corridor around the global path; the global path is used to avoid static obstacles and does not consider collision avoidance with other robots.

[0048] In the embodiment of the present invention, first, the following modeling is performed on the multi-robot system. In a three-dimensional space, the multi-robot system consists of n robots, and each robot i is modeled as a rigid sphere with a radius of r i . To consider the uncertainties brought by state estimation noise and motion disturbances, it is assumed that these uncertainties follow a Gaussian distribution. At this time, the nonlinear discrete kinematic model of robot i is described as follows:

[0049]

[0050] Among them, represents the state of robot i at the k-th moment, including the position and the speed represents the control input at the k-th moment, is the acceleration. Under the influence of uncertainties, the state of the robot follows a Gaussian distribution, and the initial state is defined as a Gaussian random variable with a mean of and a covariance of The process noise follows a Gaussian distribution with a mean of 0 and a covariance of W i k , and W i k is a diagonal covariance matrix. Generally approximated as:

[0051]

[0052] where h is the time interval.

[0053] In a multi-robot system, physical and safety constraints ensure the effective and safe operation of the system. The movement of a robot is physically limited by its inherent parameters and conditions, and the speed and acceleration need to follow the constraints:

[0054]

[0055] where v max is the upper limit of the absolute value of the speed, and a max is the upper limit of the absolute value of the acceleration;

[0056] Safety constraints include obstacle avoidance constraints and collision avoidance constraints between robots. For an obstacle o represented by a closed ellipsoid (a o , b o , c o , c o ) at position p, the collision check needs to calculate the minimum distance between the spherical surface of robot i and the ellipsoid of obstacle o. Then, the safety condition is defined as:

[0057]

[0058] where is the obstacle avoidance safety set between robot i and obstacle o at time k,

[0059] r i is the radius of robot i.

[0060] To further ensure safety, robots must avoid colliding with each other. Any pair of robots i and j should satisfy:

[0061]

[0062] where is the collision avoidance safety set between robot i and robot j at time k, and r ij = r i + r j is the shortest safety distance between the two robots.

[0063] Step 2: The robot moves along the global path, continuously sampling position points to form a local reference trajectory; it moves from the starting point along the global path at the sampling speed, and the time interval between adjacent sampling points is h;

[0064] Step 3: Before all robots reach the target point, execute the probabilistic trajectory generation iteration process:

[0065] In each iteration, N points are sampled from the global path as the reference trajectory of N discrete positions in model predictive control; considering uncertainties and potential collisions with other robots, P polyhedrons are extracted from the safety corridor to form a local safety corridor, and considering the uncertainties introduced by state estimation noise and motion disturbances, a collision avoidance probability constraint is constructed in combination with the local safety corridor; robots transmit the trajectory information of their own plans to each other as shared information;

[0066] In each iteration, each robot updates its trajectory according to the shared information, constructs a distributed model predictive control problem by combining the shared information, collision avoidance constraints, and probability constraints for avoiding deadlocks, solves the distributed model predictive control problem using a solver, generates a locally optimal collision-free trajectory, and updates its own trajectory.

[0067] In the embodiments of the present invention, the safety constraints include obstacle avoidance constraints and collision avoidance constraints between robots, and the method for constructing the safety constraints is as follows:

[0068] A safety corridor with time awareness is used to construct safety constraints. The safety corridor effectively separates the robot from obstacles by demarcating a safety area independent of environmental complexity, thereby simplifying the optimal control problem and reducing redundant obstacle avoidance constraints. These safety areas consist of a series of overlapping convex polyhedrons, P convex polyhedrons, and any trajectory generated within these convex polyhedrons is considered a safe trajectory. In the absence of random disturbances, it is only necessary to limit the predicted trajectory within these convex polyhedrons to ensure the satisfaction of the collision avoidance constraint.

[0069] (1) Obstacle avoidance constraint between the robot and the obstacle: In each iteration of the present invention, a local safety corridor composed of P convex polyhedrons is selected as the constraint area for the robot's movement. The local safety corridor includes the convex polyhedron where the robot is currently located and the subsequent P - 1 convex polyhedrons. To ensure that the robot avoids collisions in path planning, it is required that the positions at each pair of consecutive discrete times k and k + 1 must be located within at least one polyhedron of the local safety corridor. Therefore, the safety state constraint for robot i at time k can be expressed as:

[0070]

[0071] where, is in is the obstacle avoidance safety state set between robot i and obstacle o at time k, is the position of robot i at time k, the local safety corridor consists of P convex polyhedrons, and each convex polyhedron is represented by where is the coefficient matrix of convex polyhedron p at time k, which defines the coefficients of the constraint conditions, is the constant term vector of the convex polyhedron p at time k, which defines the bounds on the right side of the inequality for each constraint. is a Boolean variable. represents and whether they are in the same polyhedron p. The constraint ensures that the positions at two consecutive times k and k + 1 are simultaneously within at least one polyhedron.

[0072] (2) Collision avoidance constraint between robots: In each iteration, the collision avoidance constraint is established based on the optimized trajectory generated in the previous iteration. For each discrete position in the planning, the robot positions generated in the previous iteration are used to construct a hyperplane, thereby forming a collision avoidance constraint to ensure a safe distance between robots. In each iteration l, the hyperplane normal vector of robot i with respect to robot j at time k and the corresponding collision-free condition are defined as follows:

[0073]

[0074] where is the variable to be optimized in the current iteration, and are the optimized trajectory points of robot i and robot j in the previous iteration l - 1, respectively. is the hyperplane normal vector of robot i with respect to robot j at time k, the set of collision avoidance safety states between robot i and robot j at time.

[0075] In the embodiments of the present invention, considering the uncertainty introduced by state estimation noise and motion interference, a collision avoidance probability constraint is constructed in combination with a local safety corridor, specifically:

[0076] In system modeling, it has been assumed that the uncertainty follows a Gaussian distribution. The robot state is modeled using an unbounded probability distribution, and the collision probability must be kept below a specified threshold. Therefore, the collision avoidance constraints in (7) and (9) are reformulated in a probabilistic manner as follows:

[0077]

[0078] where Δ o and Δ r are the collision probability thresholds between the robot and the static obstacle and between the robots, respectively. represents the state of the i-th robot at time k belonging to the set of collision avoidance safety states with probability.

[0079] Theorem 1: Assume that a random vector x follows a Gaussian distribution is the mean vector, Σ is the covariance matrix, and its linear collision probability is:

[0080]

[0081] where x is a random vector, a is a constant vector, and b is a constant. Then the probability constraint Pr(a T x ≤ b) ≤ Δ is equivalent to:

[0082]

[0083] where erf(·) is the Gaussian error function,

[0084] To facilitate the calculation, the probability constraint is reformulated in a deterministic form. According to Theorem 1, the probability constraints (10), (11) can be transformed into deterministic constraints on the mean and covariance of the robot state:

[0085]

[0086] where, represents the probability that the state of robot i at time k belongs to the obstacle avoidance safety state set Δ o is the collision probability threshold between the robot and the static obstacle, erf -1 (1 - 2Δ o ) is the standard normal distribution quantile corresponding to the collision probability Δ o , is the polyhedral coefficient matrix of the safety corridor at time k, is the constant term vector of the polyhedron, is 's transpose matrix, is the position mean of robot i at time k, is the state covariance matrix of robot i at time k. represents the probability that the state of robot i at time k belongs to the collision avoidance safety state set Δ r is the collision probability threshold between robots, erf -1 (1 - 2Δ r ) is the standard normal distribution quantile corresponding to the collision probability Δ r , is 's unit vector, is 's transpose, and are the optimized trajectory points of robot i and robot j in the previous iteration l - 1, rij is the shortest safe distance between two robots, and are the state covariance matrices of robot i and robot j at time k, respectively.

[0087] To evaluate the probabilistic constraints (12) and (13), the uncertainty covariance of the state needs to be calculated at each time step. The present invention uses an EKF-type approach to update the propagation covariance to achieve real-time performance:

[0088]

[0089] where, is the state transition matrix at time k, is the state uncertainty covariance, and W i k is the process noise.

[0090] In the embodiments of the present invention, to avoid the deadlock problem that often occurs in multi-robot navigation, a probabilistic constraint for avoiding deadlock is proposed. Specifically, the present invention formulates the distributed model predictive control problem of robots as a finite-horizon optimization problem, and adds an additional warning band to the terminal constraint of the inter-robot collision avoidance probabilistic constraint (13). This added warning band serves as a protection to facilitate the solution of potential deadlocks.

[0091] The collision avoidance constraint with a warning band added at the terminal becomes:

[0092]

[0093] where N is the length of the optimization horizon, and ε ij ∈[0, ε max represents the warning distance between robot i and robot j, and ε max ∈(0.2r i , 0.5r i ) is a parameter representing the maximum width of the warning band. Then, according to Theorem 1, the corresponding probabilistic constraint for avoiding deadlock is:

[0094]

[0095] where, is the unit vector of , is the transpose of , is the mean position of robot i at the terminal time N, and are the optimized trajectory points of robot i and robot j at the terminal time N in the previous iteration l - 1, respectively, and r ijis the shortest safe distance between two robots, ε ij is the warning distance between two robots, erf -1 (1 - 2Δ r ) is the collision probability Δ r corresponding standard normal distribution quantile, and are the state covariance matrices of robot i and robot j at time k, respectively;

[0096] In addition, a penalty term J w :

[0097] J w = ρ ij (ε ij - ε max ) 2 (17)

[0098] where ρ ij > 0 is a parameter related to the repulsive force, and by adjusting this parameter, robot i is moved away from robot j. In each iteration l, the calculation method of the ρ ij parameter is as follows:

[0099]

[0100] where ρ0 > 0 and Δλ > 0 are predefined parameters, and θ ij ∈[-π, π) is the angle between the projection of the line segment to its target position p target,i and the projection of the line segment to on the xy plane. Initially, the present invention sets When the following conditions are met, a potential deadlock is detected: and

[0101] When a deadlock occurs, the forces at the terminal positions reach equilibrium and remain static indefinitely. Adjust the parameter ρ ij according to the right-hand rule ij to change the repulsive force. Initially, ρ i = ρ0 and no repulsive force is applied. If a deadlock is detected, the value of λ ij will increase. When robot j is on the right side of robot i (i.e., θ < 0 and ij > 0 and ), the repulsive force exerted by robot j decreases, causing robot i to move closer to robot j. Conversely, if robot j is on the left (i.e., θ Figure 1 As shown in is the resultant force, F 1 is the attraction force of the target on the robot 1, and is the repulsive force from neighboring robots. Figure 1 (a) When the forces reach an equilibrium state, the robot 1 is deadlocked. Figure 1 (b) After introducing the right-hand rule and the warning band, decreases, increases, and at this time the resultant force is not zero, and the deadlock is released.

[0102] In the embodiments of the present invention, the probabilistic trajectory generation algorithm integrates the probabilistic constraints (12), (13) and the deadlock avoidance mechanisms (16), (17) in a distributed model predictive control framework to generate a locally optimal trajectory. The algorithm first generates a global path and a safety corridor around the path for each robot. Then, the global path and the safety corridor are used to generate a local reference trajectory and a local safety corridor, and the distributed model predictive control problem in the loop is solved at a constant rate.

[0103]

[0104]

[0105] As shown in Algorithm 1, the algorithm first generates a global path Φ 0,i from the initial position p target,i to the target position p i for each robot i, and establishes a safety corridor (lines 4-5) around this path. The global path Φ i is mainly used to avoid static obstacles and does not consider collision avoidance with other robots.

[0106] The robot advances along the global path Φ i and continuously samples position points to form a local reference trajectory. At the sampling speed v samp it moves along the global path Φ i from the starting position p 0,i with the time interval between adjacent sampling points being h. In each iteration, N points are sampled from Φ i as the reference trajectory x ref,i of N discrete positions in the model predictive control (line 12). At this time, the present invention takes into account uncertainties and potential collisions with other robots. The present invention extracts P polyhedrons from the safety corridor and uses them to form a local safety corridor (Line 13), which helps to construct the collision avoidance probability constraint as in formula (12). In addition, robots will transmit the trajectory information they plan to each other (Line 11). In each iteration, each robot updates its trajectory based on the shared information to avoid potential conflicts with the trajectories of other robots and combines this interaction information to optimize its own path to ensure that the collision avoidance probability constraint (13) is satisfied. Finally, the local trajectory Γ is solved through distributed model predictive control i (Line 14), in which the collision avoidance probability constraints (12), (13) and the deadlock solutions (16), (17) are combined.

[0107] The distributed model predictive control runs in a loop at a fixed time interval h until each robot reaches its target position p target,i . In each iteration l, the initial state of the distributed model predictive control problem is set to the state of the previous iteration If the solver fails to find a feasible solution within the time step h, the initial state of the next iteration will be adjusted to the next state in the trajectory successfully solved in the previous iteration. If the solver still fails continuously when reaching the state and the target p target,i has not been reached, the algorithm is considered to have failed (Line 15).

[0108] For the distributed model predictive control problem, its objective function is defined as minimizing the error between the actual trajectory of robot i and the reference trajectory x ref,i , while also including a penalty term for reducing the control input and a penalty term related to the warning band:

[0109]

[0110] where R x , R u , R N are the weight matrices of the tracking error, the control input, and the terminal state respectively. The bold symbol represents the sequence of the state mean changing with time, x ref,i represents the sequence of the reference trajectory changing with time, u i represents the sequence of the control input changing with time. The superscript · k represents the value at time k, the reference state corresponding to robot i at time, N is the terminal time, ε ij ε ij ∈[0, ε max represents the warning distance between robot i and robot j, ε max ∈(0.2r i , 0.5ri ) is a parameter representing the maximum width of the warning zone. Then, the distributed model predictive control problem of robot i is defined as:

[0111]

[0112] where is the objective function of the distributed model predictive control problem, is the mean value of the state of robot i at time k, is its control input at time k, is the state sequence, u i is the control input sequence, x ref,i is the reference state sequence, ρ ij is the adjustment parameter in the deadlock avoidance mechanism, ε ij is the warning distance between robot i and robot j, N is the optimization time horizon length, is the initial state, is the mean value of the initial state.

[0113] To ensure that the robot can still execute its trajectory safely when the future model predictive control optimization fails, the terminal velocity and the terminal acceleration are both set to zero.

[0114] If (21) has no solution, the trajectory generation fails.

[0115] Simulation experiments were carried out for this algorithm. Assume that the mean value of the uncertainty is 0 and the covariance matrix is Σ x = diag(0.05m, 0.05m, 0.05m, 0.03m / s, 0.03m / s, 0.03m / s) 2 . Simulation experiments were carried out in an environment of 13m×13m×5m with 10 randomly generated obstacles, and 8 robots symmetrically exchanged positions pairwise about a center. More than 100 experiments were carried out at three uncertainty levels of 0.25Σ x , Σ x and 4Σ x , and the success rates were 98.37%, 96.33%, and 93.75% respectively. The simulation results are as shown in Figure 2 and Figure 3 .

[0116] In summary, the above is only the preferred embodiment of the present invention and is not used to limit the protection scope of the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A multi-robot probabilistic trajectory generation method based on distributed model predictive control, characterized in that: In the presence of uncertainty, a trajectory is generated so that each robot can safely reach a target point from a starting point. The method specifically comprises the following steps: In a multi-robot system, a global path from a starting point to a target point is first generated for each robot; a safety corridor is established around the global path; the global path is used to avoid static obstacles and does not consider collision avoidance with other robots; The robot moves along the global path, continuously sampling position points to form a local reference trajectory; it moves along the global path from the starting point at the sampling speed, and the time interval between adjacent sampling points is h; The probabilistic trajectory generation process is iterated until all robots reach the goal point: In each iteration, N points are sampled from the global path as reference trajectories of N discrete positions in model predictive control; considering uncertainty and potential collisions with other robots, P polyhedrons are extracted from the safety corridor to form a local safety corridor, and considering the uncertainty introduced by state estimation noise and motion interference, collision avoidance probability constraints are constructed in combination with the local safety corridor; robots transmit their own planned trajectory information to each other as shared information; In each iteration, each robot updates its trajectory according to the shared information, and constructs a distributed model predictive control problem by combining the shared information, the collision avoidance constraint and the deadlock avoidance probability constraint. The distributed model predictive control problem is solved by the solver, a locally optimal collision-free trajectory is generated, and its own trajectory is updated.

2. The multi-robot probabilistic trajectory generation method based on distributed model predictive control as claimed in claim 1, characterized in that: In each iteration, the shared information, the collision avoidance constraint and the deadlock avoidance probability constraint are combined to construct a distributed model predictive control problem, and the distributed model predictive control problem is solved by a solver to generate a local optimal collision-free trajectory and update its own trajectory. Specifically: At each iteration, the initial state of the distributed model predictive control problem is set to the state of the previous iteration; if the solver fails to find a feasible solution within the time step h, the initial state of the next iteration is adjusted to the next state in the trajectory of the last successful solution; if the solver continues to fail when reaching the final state and the target point has not been reached, the algorithm is considered to have failed.

3. The multi-robot probabilistic trajectory generation method based on distributed model predictive control as claimed in claim 1, characterized in that: The multi-robot system consists of n robots, each robot i is modeled as a robot with a radius of r. i A rigid sphere; The nonlinear discrete kinematic model of robot i is described as follows: Among them, the superscript k represents the kth moment, and the following table i represents robot i; Represents the state of robot i at the kth moment, including the position and speed represents the control input at the kth moment, is the acceleration of robot i at the kth moment; under the influence of uncertainty, the state of the robot obeys the Gaussian distribution, and the initial state Defined as a Gaussian random variable with mean The covariance is Process noise Subject to mean 0 and covariance W i k Gaussian distribution, W i k is a diagonal covariance matrix; Approximately: Where h is the discrete time interval; In a multi-robot system, the velocity and acceleration of the robots follow the following constraints: Among them, v max is the upper limit of the absolute value of the speed, a max is the upper limit of the absolute value of acceleration.

4. The multi-robot probabilistic trajectory generation method based on distributed model predictive control as claimed in claim 1, characterized in that: The uncertainty introduced by the state estimation noise and motion interference is considered, and the collision avoidance probability constraint is constructed in combination with the local safety corridor, specifically: in, represents the state of robot i at time k Belongs to the obstacle avoidance safety state set The probability of o is the collision probability threshold between the robot and the static obstacle, erf -1 (1-2Δ o ) is the collision probability Δ o The corresponding standard normal distribution quantile, is the polyhedral system matrix of the safe corridor at time k, is the constant term vector of the polyhedron, for The transposed matrix of is the mean position of robot i at time k, is the state covariance matrix of robot i at time k; represents the state of robot i at time k Belongs to the collision avoidance safety state set The probability of r is the collision probability threshold between robots, erf -1 (1-2Δ r ) is the collision probability Δ r The corresponding standard normal distribution quantile, for The unit vector of for The transpose of and are the optimized trajectory points of robot i and robot j in the previous iteration l-1, r ij is the shortest safe distance between two robots, and are the state covariance matrices of robot i and robot j at time k respectively.

5. The method for generating probabilistic trajectories of multiple robots based on distributed model predictive control as claimed in claim 4, characterized in that: The probability constraint for avoiding deadlock is specifically: in, for The unit vector of for The transpose of is the mean position of N robots i at the terminal moment, and are the optimized trajectory points of robot i and robot j at the terminal time N in the previous iteration l-1, r ij is the shortest safe distance between two robots, ε ij is the warning distance between the two robots, erf -1 (1-2Δ r ) is the collision probability Δ r The corresponding standard normal distribution quantile, and are the state covariance matrices of robot i and robot j at time k respectively; Add a penalty term J to solve the deadlock problem w : J w =ρ ij (e ij -e max ) 2 , Among them, ρ ij >0 is a parameter related to the repulsive force, which can be adjusted to make robot i stay away from robot j.

6. The method for generating probabilistic trajectories of multiple robots based on distributed model predictive control as claimed in claim 5, characterized in that: The distributed model predictive control problem constructed is as follows: in, is the objective function of the distributed model predictive control problem, is the state mean of robot i at time k, is its control input at time k, is the state sequence, u i is the control input sequence, x ref,i is the reference state sequence, ρ ij To avoid deadlock in the tuning parameters of the mechanism, ε ij is the warning distance between robot i and robot j, N is the optimized time domain length, is the initial state, is the mean value of the initial state.

Images