Multi-Robot Robust Motion Planning based on Model Predictive Priority Contouring Control with Double-Layer Corridors

Featured Application: This work is used for multi-robot autonomous systems. Abstract: Disturbance poses a major challenge for the safety and real-time performance of robust robot motion planning. To address the disturbance while improving the real-time performance of multi-robot robust motion planning, a model predictive priority contouring control method is proposed. First, an improved conﬂict-based search (ICBS) planner is utilized to plan reference paths. The low-level planner of the conﬂicted-based search (CBS) planner is replaced by the hybrid A* planner and reference paths are adopted as an initial guess of model predictive priority contouring control. Second, double-layer corridors are proposed to provide safety guarantees, which include static-layer corridors and dynamic-layer corridors. The static-layer corridors are generated based on reference paths and the dynamic-layer corridors are generated based on the relative positions and velocities of robots. The double-layer corridors are applied as safety constraints of model predictive priority contouring control. Third, a prioritization mechanism is devised to improve computational efﬁciency. Priorities are assigned according to each robot’s task completion percentage. Based on the assigned priority, multiple robots are grouped, and each group executes the model predictive priority contouring control algorithm to acquire trajectories. Finally, our method is compared with the centralized method and the soft constraint-based DMPC. Simulations verify the effectiveness and real-time performance of our approach.


Introduction
Multi-robot systems play a vital role in next-generation factories, urban search and rescue, and package delivery, and they are anticipated to be applied in space exploration [1,2]. One of the key ingredients to a multi-robot system is the motion planning module. The motion planning module should find a set sequence of valid configurations that move multiple robots from starting points to goal points safely [3]. However, in unstructured environments, the multi-robot motion planning algorithm being both optimal, complete, real-time, and flexible is still a valuable and challenging problem to be solved, especially in the presence of disturbances.
As mentioned in previous publications, the multi-robot motion planning (MMP) problem is generally defined as a multi-objective optimization problem with different types of constraints [4,5]. The methods for solving MMP problems are mainly classified into centralized and decentralized methods [3].
Centralized methods plan the motion trajectories of robots jointly [6]. In [7], Guni Sharon et al. proposed a two-level algorithm called conflict-based search (CBS) to find optimal paths for multiple robots. The core of the algorithm is to maintain a constraint tree to resolve conflicts and plan the optimal path for each robot with constraints. In [8], Jiaoyang

•
The construction method of double-layer corridors is devised, and the chance constraints introduced by disturbances are transformed into linear deterministic safety constraints.

•
Model predictive priority contouring control with the double-layer corridors is proposed to generate multi-robot trajectories, which ensures security while greatly improving computational efficiency. • Extensive evaluation of the method through a great number of simulations.
The rest of the paper is organized as follows. In Section 2, the problem of multi-robot robust motion planning is stated. In Section 3, double-layer corridors are devised. In Section 4, the model predictive priority contouring control with double-layer corridors is proposed. In Section 5, simulations are present. Finally, Section 6 concludes our work.

Problem Statement of Multi-Robot Robust Motion Planning
The purpose of multi-robot robust motion planning is to ensure the safety of multirobot motion planning even disturbed. Suppose that there are N mobile robots in the environment. The mobile robots may have different sizes d, different maximum velocity v max , maximum acceleration a max , maximum angular velocity w max , and maximum angular acceleration α max .

Robot Model
In this paper, differential wheeled robots are studied. The planning process is carried out in the SE(2) space and state x m,k includes 2-D position (x m,k , y m,k ) and orientation ϕ m,k .
The robot model f (·) can be written as Equation (1).
x m,k+1 = x m,k + v m,k ∆t cos(ϕ m,k ) (1a) y m,k+1 = y m,k + v m,k ∆t sin(ϕ m,k ) (1b) ϕ m,k+1 = ϕ m,k + w m,k ∆t (1c) In Equation (1), v m,k and w m,k are the velocity and angular velocity of robot m at time k respectively. ∆t represents the time interval between two control inputs. Based on the robot model (1), the control input of robot m at time k is defined as u m,k = [v m,k , w m,k ].
It is worth mentioning that Equation (1) is the ideal kinematic model of the robot. Under pure rolling conditions, the motion of the robot is exactly in accordance with Equation (1). However, imperfections are unavoidable in real devices [24]. Complex wheel-ground interactions and system noises cause disturbances to the robots. Moreover, these disturbances are modeled as Gaussian random variables, characterized by their mean and covariance matrices. Thus, the model of a given robot m at time k are defined as Equation (2).
x m,k+1 = f (x m,k , u m,k ) + N noise m,k In Equation (2), x m,k ∈ R n x and u m,k ∈ R n u are the state and control input of robot m at the time k respectively. N noise m,k ∈ R n x is an unknown disturbance with Gaussian probability distribution. Considering the disturbance level in the real environment, N noise m,k is set to diag(0.1 m, 0.1 m, 0.5 deg). What is more, the simulations are carried out under different level disturbances. Related simulations are in Section 5.1.

Collision Avoidance
The workspace of the robot is defined as C ⊆ R 2 . Let p m,k = [x m,k , y m,k ] ∈ R 2 denote the position of robot m and q o,k = [x o,k , y o,k ] ∈ R 2 denote the position of obstacle o. Hence, the collision condition of robot m with obstacle o at time k is given as follows.
In Equation (3), d m represents the minimum safe distance of robot m.
For any two robots m and n, if their position p m,k and p n,k satisfy the following equation there is a collision between robot m and n. Due to the disturbances, the position of the robot is a Gaussian random variable. Chance safety constraints are given as Equations (5) and (6).
In Equation (5), δ o is the probability threshold for robot-obstacle collision. In Equation (6), δ r is the probability threshold for inter-robot collision

Multi-Robot Robust Motion Planning Problem
The multi-robot motion planning (MMP) problem is generally defined as a multiobjective optimization problem with different types of constraints [4,5]. Thus, the general form of multi-robot robust motion planning is modeled as a multi-objective optimization problem in this paper. Section 2.1 shows the robot model constraint, and Section 2.2 gives safety constraints. What is more, the control input of each robot is constrained by its maximum velocity, maximum acceleration, maximum angular velocity, and maximum angular acceleration. Meanwhile, the position and yaw at the initial time constrain the initial state of each robot. Based on the above discussion, the multi-robot robust motion planning problem can be defined as follows.
In Equation (7), m ∈ {1, 2, . . . , N}. Equation (7a) is the cost function. In this paper, we define J as a cost function that forces the robot to stay near the reference path and track it synchronously. The exact form is given in Equations (7e,f) provide control input constraints. The initial state constraint is denoted by Equation (7g).
The key difficulty in solving this problem is how to deal with the safety constraints (7c,d). The safety constraints (7c,d) are nonconvex constraints represented by the integral of a multivariate Gaussian distribution. These types of constraints are called chance constraints. They are intractable to handle in the usual case. To make the problem easy to solve, double-layer corridors are devised. Then, the problem can be transformed into a deterministic optimization problem.

Double-Layer Corridor
In this section, double-layer corridors are proposed to provide safety guarantees. Double-layer corridors include static-layer corridors and dynamic-layer corridors. Staticlayer corridors ensure obstacle collision avoidance and dynamic-layer corridors ensure inter-robot collision avoidance.

Static-Layer Corridor
A series of partially overlapping convex sets are often used to model the safe region of robots to minimize the number of collision avoidance constraints [11]. These convex sets are defined as static-layer corridors in this paper. The collision avoidance between robot and obstacle is guaranteed by static-layer corridors. To construct static-layer corridors, the minimum safe distance between the robot and the obstacle needs to be obtained. Assume that the position of the robot m at time k is a multivariate Gaussian random variable p m,k ∼ N ( p m,k , Σ m,k ) and obstacle o is located at q o . Then, p m,k − q o is also a Gaussian random variable, i.e., p m,k − q o ∼ N ( p m,k − q o , Σ m,k ). Thus, the collision probability is given by Equation (8).
Equation (8) is the integral over the circle of the multivariate Gaussian probability density function. Referring to [25], the circle collision region C mo,k is enlarged into a half space C mo,k . C mo,k is defined as Equation (9).
In Equation (9), Lemma 2. Let Z ∼ N (µ, Σ) be a multivariate Gaussian random variable and a probability threshold δ ∈ (0, 0.5). There is a deterministic equivalent form to which is shown as Equation (14).
Based on Lemma 1, Equation (10) can be rewritten as follows.
After that, according to Lemma 2, Equation (16) can be acquired According to Equation (16), the minimum safe distance between the robot and the obstacle can be obtained in real-time.
A series of partially overlapping convex sets are utilized to model the safe region of robots. The series of partially overlapping convex sets are defined as static-layer corridors According to Equation (16), the minimum safe distance between the robot and the obstacle can be obtained in real-time.
A series of partially overlapping convex sets are utilized to model the safe region of robots. The series of partially overlapping convex sets are defined as static-layer corridors s S . For robot m, the static-layer corridors Equation (17) guarantees that the intersection of the kth static-layer corridor and the obstacle region is empty. Moreover, Equation (18) enforces that the kth static-layer corridor and the ( + 1)th static-layer corridor are sequentially connected.
The distance between the boundary of a static-layer corridor and the obstacle is set as s m d , which is shown in Figure 2 In Equation (19),  For the kth static-layer corridor S s m,k of robot m, it satisfies Equation (17) and Equation (18).
Equation (17) guarantees that the intersection of the kth static-layer corridor and the obstacle region is empty. Moreover, Equation (18) enforces that the kth static-layer corridor and the (k + 1)th static-layer corridor are sequentially connected.
The distance between the boundary of a static-layer corridor and the obstacle is set as d s m , which is shown in Figure 2. d s m is set to an initial value at the beginning and d s m updates during the robot's operation.
In Equation (19), inv(η m,k ) represents the generalized inverse matrix of η m,k . In Equation (20), p M is the position of point M and p N is the position of point N. M is a point on the boundary of static-layer corridor and N is its vertical point on the obstacle area. If robot m stays inside the static-layer corridor, then Equation (16) satisfies. When the robot satisfies Equation (16), the robot cannot collide with the obstacle.
Thus, it can be seen that if the state of robot m satisfies Equation (21), there is no obstacle collision.

Dynamic-Layer Corridor
Collision avoidance between robots is ensured by dynamic-layer corridors. Similar to the static-layer corridor, the minimum safe distance between the robots needs to be obtained.
Let the position of the robot m and n be multivariate Gaussian random variables Similarly, according to Lemma 1 and Lemma 2, Equation (23) can be acquired. (23) In Equation (23), The minimum safe distance between robot m and robot n is acquired based on Equation (23). However, ignoring the velocity of the robots may result in the collision avoidance condition (23) not being satisfactory. Generally, the problem is addressed by the velocity obstacle method [12][13][14][15]. However, the constraint obtained by the velocity obstacle method is nonconvex. Jungwon Park et. al adopted the constant velocity assumption to predict short trajectories of robots and simplified the constraint by ensuring that the predicted trajectories do not collide [26]. Therefore, let m and n be the velocities of robot m and robot n, respectively. After a very short time

Dynamic-Layer Corridor
Collision avoidance between robots is ensured by dynamic-layer corridors. Similar to the static-layer corridor, the minimum safe distance between the robots needs to be obtained.
Let the position of the robot m and n be multivariate Gaussian random variables p m,k ∼ N ( p m,k , Σ m,k ), p n,k ∼ N ( p n,k , Σ n,k ). Then p m,k − p n,k is also a Gaussian random variable, i.e., p m,k − p n,k ∼ N ( p m,k − p n,k , Σ m,k + Σ n,k ). The collision probability is given by Equation (22).
In Equation (23), η mn,k = ( p m,k − p n,k )/ p m,k − p n,k and d mn = d m + d n .
The minimum safe distance between robot m and robot n is acquired based on Equation (23). However, ignoring the velocity of the robots may result in the collision avoidance condition (23) not being satisfactory. Generally, the problem is addressed by the velocity obstacle method [12][13][14][15]. However, the constraint obtained by the velocity obstacle method is nonconvex. Jungwon Park et al. adopted the constant velocity assumption to predict short trajectories of robots and simplified the constraint by ensuring that the predicted trajectories do not collide [26]. Therefore, let v m and v n be the velocities of robot m and robot n, respectively. After a very short time ∆t s , the position of robot m is p m ∼ N ( p m , Σ m ), p m = p m + v m ∆t s , and the position of robot n is p n ∼ N ( p n , Σ n ), p n = p n + v n ∆t s . p m − p n , which is needed to satisfy Equation (24).
In this paper, the set S d mn is defined as the dynamic-layer corridor for robot m and robot n. The set S d mn satisfies Equation (25).
Equation (25) guarantees that the intersection of the dynamic-layer corridor and the inter-collision region of robot m and robot n is empty. For any robot m, it can be seen that if the state of robot m satisfies Equation (26), there is no collision between robot m and robot n.

Model Predictive Priority Contouring Control with Double-Layer Corridors
The model predictive priority contouring control with double-layer corridors is proposed in this section. The whole framework is shown in Figure 3. First of all, reference path planning is performed using the ICBS planner. The reference paths are adopted as an initial guess of model predictive priority contouring control. Then, the construction process of double-layer corridors is described in detail. The double-layer corridors are applied as safety constraints of model predictive priority contouring control. At last, a prioritization mechanism is devised to improve the computational efficiency of model predictive contouring control. The improved algorithm is defined as model predictive priority contouring control. (24) In Equation (24), In this paper, the set d mn S is defined as the dynamic-layer corridor for robot m and robot n. The set d mn S satisfies Equation (25). (25) Equation (25) guarantees that the intersection of the dynamic-layer corridor and the inter-collision region of robot m and robot n is empty. For any robot m, it can be seen that if the state of robot m satisfies Equation (26), there is no collision between robot m and robot n.

Model Predictive Priority Contouring Control with Double-Layer Corridors
The model predictive priority contouring control with double-layer corridors is proposed in this section. The whole framework is shown in Figure 3. First of all, reference path planning is performed using the ICBS planner. The reference paths are adopted as an initial guess of model predictive priority contouring control. Then, the construction process of double-layer corridors is described in detail. The double-layer corridors are applied as safety constraints of model predictive priority contouring control. At last, a prioritization mechanism is devised to improve the computational efficiency of model predictive contouring control. The improved algorithm is defined as model predictive priority contouring control.

Reference Path Planning
The multi-robot motion planning with disturbances is nonlinear, nonconvex, and NPhard. It is difficult to obtain the solution in real-time. Thus, the ICBS planner is adopted to find discrete and collision-free reference paths for multiple robots. The solution is regarded as an initial guess for the multi-robot motion planning with disturbances. A more accurate solution is acquired by the method proposed in Section 4.3.4.
In this paper, the environment is modeled by a voxel grid map, and each grid has eight neighbors. Thus, the whole map can be regarded as an undirected graph G = {V, E}.
The vertex ver∈V is the free grid of the map and the edge (ver i , ver j ) represents that the gird ver j is a neighbor of the grid ver i . The ICBS planner works in two levels. At the high level, the constraint tree is built to resolve potential conflicts. At the low level, the hybrid A* planner is utilized to replace the A* planner. The kinematics of the robot is considered during the planning process and discrete paths are planned for robots with the constraints of the constraint tree. The procedure is as follows.
First, the hybrid A* planner searches paths for each robot with no constraints. Second, the constraint tree checks for conflicts between the paths of every two robots. Referring to [7], two types of conflicts are applied when planning reference paths. The first one is a vertex conflict. If robot m and robot n occupy the same grid at time k, vertex conflict occurs. This kind of conflict is defined by Equation (27). The second type of conflict is edge conflict. If robot n moves from ver i to ver j and robot m moves from ver j to ver i between time k and k+1, there is an edge conflict (28).
Third, assuming a conflict arises, two nodes will be added to the constraint tree. The first one adds constraint for robot m to avoid the collision. The second node adds constraint for robot n to avoid the collision. Fourth, each time a new node is generated in the constraint tree, the hybrid A* planner re-plans the path for the robot with the new constraint. Finally, repeat the above process until all reference paths {(x initial m,0 , y initial m,0 ), (x initial m,1 , y initial m,1 ), . . . , (x initial m,K , y initial m,K )}, ∀m ∈ {1, . . . , N} are planned.

Construction of Static-Layer Corridor
The goal for the construction of the static-layer corridors of robot m is to find a group of free convex polygons S s m := S s m,0 , S s m,1 , . . . , ∀m to take up as much free workspace as possible. Since the environment is modeled by a voxel grid map, S s m is a convex hull of free voxel grids. The static-layer corridors are constructed by an axis-search method. The procedure is shown in Figure 4.
In Figure 4, black grids represent the obstacle region. The blue curve is the reference path of robot m. As Figure 4a shows, the static-layer corridor is initialized by a grid in the reference path. The grid is marked by a blue square. Each static-layer corridor expands to the maximum axis-aligned area. These static-layer corridors are represented by the yellow rectangle in Figure 4b. The steps to construct static-layer corridors are as follows.
To begin with, the grids occupied by the reference path points {(x initial m,0 , y initial m,0 ), (x initial m,1 , y initial m,1 ), . . . , (x initial m,K , y initial m,K )}, ∀m ∈ {1, . . . , N} are applied to initialize the staticlayer corridors. Next, static-layer corridors expand step by step in all axis-aligned directions until they cover the maximum free workspace. Further, the duplicated static-layer corridors are checked. Static-layer corridors occupying exactly the same area are reserved for only one. Complete the above process for each robot. Finally, the boundaries of the static-layer corridors are updated based on Equation (19) while the multi-robot system is running.

Construction of Dynamic-Layer Corridor
The purpose of constructing dynamic corridors is to avoid collisions between robots. The construction process of the dynamic-layer corridor is shown in Figure 5.
In Figure 5, the green circle represents robot m and the convex area marked with green dotted lines is the dynamic-layer corridor of robot m. For robot m, it treats other robots as obstacles with velocity. Other robots are indicated by black circles in Figure 5. For robot m, it cannot collide with robots that are too far away. To reduce the consumption of computing resources, reciprocal avoidance distance L is introduced. For any two robots m and n, if their relative distance mn L − pp , then ignores the dynamic-layer corridor construction between them. The steps to construct dynamic-layer corridors are as follows.
Firstly, calculate their relative distances for any robot m and n. Secondly, construct dynamic-layer corridors for robots whose relative distance is less than L. Finally, the boundaries of the dynamic-layer corridors are updated based on Equation (25) while the multi-robot system is running.

Construction of Dynamic-Layer Corridor
The purpose of constructing dynamic corridors is to avoid collisions between robots. The construction process of the dynamic-layer corridor is shown in Figure 5.

Model Predictive Priority Contouring Control
A prioritization mechanism is devised to improve the computational efficiency of model predictive contouring control. The improved algorithm is defined as model predictive priority contouring control. What is more, the reference paths are processed as the initial guess and double-layer corridors are employed as safety constraints. In Figure 5, the green circle represents robot m and the convex area marked with green dotted lines is the dynamic-layer corridor of robot m. For robot m, it treats other robots as obstacles with velocity. Other robots are indicated by black circles in Figure 5. For robot m, it cannot collide with robots that are too far away. To reduce the consumption of computing resources, reciprocal avoidance distance L is introduced. For any two robots m and n, if their relative distance p m − p n > L, then ignores the dynamic-layer corridor construction between them. The steps to construct dynamic-layer corridors are as follows.
Firstly, calculate their relative distances for any robot m and n. Secondly, construct dynamic-layer corridors for robots whose relative distance is less than L. Finally, the boundaries of the dynamic-layer corridors are updated based on Equation (25) while the multi-robot system is running.

Model Predictive Priority Contouring Control
A prioritization mechanism is devised to improve the computational efficiency of model predictive contouring control. The improved algorithm is defined as model predictive priority contouring control. What is more, the reference paths are processed as the initial guess and double-layer corridors are employed as safety constraints.

Initial Guess
Let (x initial m,0 , y initial m,0 ), (x initial m,1 , y initial m,1 ), . . . , (x initial m,K , y initial m,K ) denote the reference path point of robot m. Processing is carried out here to obtain an initial guess for yaw, velocity, and angular velocity.
In Equation (29), ϕ initial m,k is the reference yaw of robot m. In Equation (30), v initial m,k is the reference velocity of robot m and ∆t denotes the time interval between two control signals. In Equation (31), w initial m,k is the reference angular velocity of robot m.

Constraints
To obtain safe dynamically feasible trajectories, the proposed method should satisfy robot dynamic constraints, kinematic constraints, and safety constraints.
The dynamic constraints and kinematic constraints of the robot include robot model constraints, state constraints, and control input constraints. The robot model constraint is shown as Equation (2). The state constraints and control input constraints are shown as Equation (7e,f). The safety constraints (7c,d) are nonlinear nonconvex chance constraints. In Section 3, static-layer corridors and dynamic-layer corridors are proposed to ensure safety. Static-layer corridors provide obstacle avoidance guarantees and dynamic-layer corridors provide inter-robot collision avoidance guarantees. Thus, safety constraints (7c,d) are transformed into static-layer corridors constraints (21) and dynamic-layer corridors constraints (26).

Cost Function
The design of the cost function is particularly important to ensure the smooth operation of multi-robot systems. The cost function constructed in this paper mainly includes the deviation term and the input smoothing term.
In Equation (32), l 1 , l 2, and l 3 are weight factors. J d a is the cost of deviating from the reference paths. J s_v ensures smooth velocity changes and J s_w ensures smooth angular velocity changes.
Referring to [27], the cost J d is defined as the sum of contour error e c m,k and lag error e l m,k . The contour error and the lag error are shown in Figure 6.
tion of multi-robot systems. The cost function constructed in this paper mainly includes the deviation term and the input smoothing term.
In Equation (32), l1, l2, and l3 are weight factors. d J a is the cost of deviating from the reference paths.  In Equation (33), (x m,k , y m,k ) represents the kth trajectory point of robot m. N is the number of robots and K is the number of reference path points.
The input smoothing term J s_v and J s_w are given as follows.
In Equation (34), v m,k is the velocity of robot m at time k and ∆t represents the time interval between two control signals. In Equation (35), w m,k is the angular velocity of robot m at time k.

Prioritization Mechanism
Based on the previous discussion, the multi-robot robust motion planning of Section 2 can be rewritten in the following form.
This paper uses an open-source solver, qpOASES [28], to solve (36). Although the constraints are simplified as much as possible, it is still difficult to solve in real-time for largescale robotic systems. Therefore, a prioritization mechanism is devised to decouple the problem (36). The schematic diagram is shown in Figure 7. In Figure 7, high-priority robots are indicated by black circles, and they are regarded as obstacles by low-priority robots. First, let Prank be the result of the robot priority ranking. In the beginning, the priority is assigned according to the robot ID. Second, S robots with the highest priority are selected to form a group according to the priority ranking, and the (36) composed of these S robots are solved using model predictive contouring control. Third, these S robots are removed from the task ranking queue Prank, and the process is repeated until all robot trajectories are acquired. During the solution process, the high-priority robots and their trajectories are treated as obstacles for the low-priority robots. Fourth, the first group control signal in the control sequence is used as input to each robot. Fifth, the priority of the robots is re-assigned according to the value of Equation (37) from the largest to the smallest. Finally, the above process is repeated until all robots reach their goals. The whole process is shown as Algorithm 1.  For each robot, the reference path is defined as its task to accomplish. The priority of each robot is constructed in terms of the percentage of uncompleted tasks. Each robot's reference path points are constructed as a task queue. When a robot travels past a reference path point, it is removed from the queue and the percentage of uncompleted tasks is calculated as follows.
In Equation (37), Pm represents the percentage of tasks that the robot m did not complete. H m represents the number of path points left in the task queue of robot m. H m, ref represents the initial number of reference path points in the task queue of robot m.
First, let P rank be the result of the robot priority ranking. In the beginning, the priority is assigned according to the robot ID. Second, S robots with the highest priority are selected to form a group according to the priority ranking, and the (36) composed of these S robots are solved using model predictive contouring control. Third, these S robots are removed from the task ranking queue P rank , and the process is repeated until all robot trajectories are acquired. During the solution process, the high-priority robots and their trajectories are treated as obstacles for the low-priority robots. Fourth, the first group control signal in the control sequence is used as input to each robot. Fifth, the priority of the robots is re-assigned according to the value of Equation (37) from the largest to the smallest. Finally, the above process is repeated until all robots reach their goals. The whole process is shown as Algorithm 1.

Algorithm 1: Model Predictive Priority Contouring Control.
Initial guess preprocessing; InitializeP rank While not all robots reach the goal While P rank = ϕ Select S robots with the highest priority in P rank ; Solve (36) composed of these S robots Remove those S robots from P rank foreach robot Calculate the percentage of the uncompleted tasks P m; Append P m to P rank end Sort P rank end end

Simulation Analysis
The method proposed in this paper is implemented by C++ and runs under the Ubuntu 16.04, ROS kinetic. The computer hardware configuration is Intel i7-9750H 2.60 GHz, 32 GB RAM. In this paper, a grid map is used to represent the whole environment and the environment is an area of 4 m * 4 m. The grid size is set to 0.25 m. The center of the area is set as the coordinate origin. Right is the positive direction of the x-axis and upward is the positive direction of the y-axis. The time interval is 0.2 s. The probability threshold δ o and δ r are both 5%. The safety distance is 0.15m. The maximum velocity v max of each robot is 1 m/s, and the maximum angular velocity w max is 1 rad/s. The maximum acceleration a max of each robot is 2 m/s 2 , and the maximum angular acceleration α max is 1 rad/s 2 . The prediction horizon length of model predictive priority contouring control is 8. Batch size S is 3. The minimum distance between robot and obstacle and the minimum distance between robots during the operation of the multi-robot system are recorded. The solid line in Figure 9a indicates the minimum distance between obstacle and robot. The solid line in Figure 9b presents the minimum distance between robots. It can be seen that each robot can arrive at the goals safely at the end. The minimum distance between robot and obstacle and the minimum distance between robots during the operation of the multi-robot system are recorded. The solid line in Figure 9a indicates the minimum distance between obstacle and robot. The solid line in Figure 9b presents the minimum distance between robots. It can be seen that each robot can arrive at the goals safely at the end.

T = 16s, (f) T = 20s
The minimum distance between robot and obstacle and the minimum distance between robots during the operation of the multi-robot system are recorded. The solid line in Figure 9a indicates the minimum distance between obstacle and robot. The solid line in Figure 9b presents the minimum distance between robots. It can be seen that each robot can arrive at the goals safely at the end.
(a) (b) Figure 9. Comparison of minimum distance under different level disturbances, (a) the minimum distance between robots, (b) the minimum distance between robot and obstacle.
To verify the effectiveness of the proposed method. The simulation is done under different level disturbances. In this paper, the proposed method is evaluated in terms of safety, task success rate, operational efficiency, and computational efficiency. The simulation is done fifteen times under each condition, and the results are shown in Table 2.  To verify the effectiveness of the proposed method. The simulation is done under different level disturbances. In this paper, the proposed method is evaluated in terms of safety, task success rate, operational efficiency, and computational efficiency. The simulation is done fifteen times under each condition, and the results are shown in Table 2. In terms of safety, it can be seen from Table 2 that as the level of disturbance increases, the minimum distance between robot and obstacle increases, and the minimum distance between robots increases. Moreover, it can be seen from Figure 9 that there is no collision between robots and no collision between robot and obstacles during the entire operation of the robots. In terms of task success rate, our method achieves a 100% task success rate at various levels of disturbances. In terms of operational efficiency, the average velocity of the multi-robot system decreases, and task time increases as the level of disturbance increases. However, the difference is not very large. In terms of computational efficiency, it can be seen from Table 2 that the difference in computational time consumption is not significant.

Method Comparison and Analysis
To further verify the effectiveness, our method is compared with the centralized method [6] and the soft constraint-based DMPC method [17].
The starts and goals are the same as in Table 1. Meanwhile, the robot operating environment and parameter settings are the same as in Section 5.1. The covariance matrix is set to N noise = diag(0.1 m, 0.1 m, 0.5 deg). Fifteen independent repeated simulations are carried out. The different methods are evaluated in terms of task success rate, safety, operational efficiency, and computational efficiency. Results are shown in Table 3. It is important to note that task time refers to the time spent on the mission in the simulation while calculation time refers to the time spent by the CPU in the real world. In terms of task success rate, our method and centralized method achieve a 100% task success rate. There is a failure when the soft constraint-based DMPC is utilized. The task success rate of soft constraint-based DMPC is 93.3%. Thus, the soft constraint-based DMPC is more sensitive to disturbances and our method is more robust.
In terms of safety, the successful results of all methods are compared. Figure 10a presents the minimum distance between robots. In addition, Figure 8b illustrates the minimum distance between robot and obstacle. The minimum distance between robot and obstacle for our method, the centralized method, and the soft constraint-based DMPC are 0.40 m, 0.38 m, and 0.40 m, respectively. Moreover, the minimum distances between the robots for our method, the centralized method, and the soft constraint-based DMPC are 0.50 m, 0.51 m, and 0.44 m, respectively. 0.40m, 0.38m, and 0.40m, respectively. Moreover, the minimum distances between the robots for our method, the centralized method, and the soft constraint-based DMPC are 0.50m, 0.51m, and 0.44m, respectively.
(a) (b) Figure 10. Comparison of the minimum distance between different methods. (a) the minimum distance between robots, (b) the minimum distance between robot and obstacle.
In terms of operational efficiency, the average velocity of our method is 15% higher than that of the centralized method, and the task time of our method is 6.80% less than that of the centralized method. Moreover, the average velocity of our method is 25% higher than that of the soft constraint-based DMPC, and the task time of our method is 12.1% less than that of the soft constraint-based DMPC.
In terms of computational efficiency, the calculation time per horizon of our method is longer than that of the soft constraint-based DMPC. However, the difference is not significant. Moreover, it can be seen that our method is much more computationally efficient than the centralized method.

Computational Time Consumption Comparison and Analysis
In this section, the computational performance and the quality of the results of our method are further compared with other methods. Simulations are performed under different numbers of robots. The cost is utilized to evaluate the quality of the results and calculation time per horizon is utilized to evaluate the computational efficiency. Since the soft constraint-based DMPC does not guarantee a 100% success rate, our method is compared with the centralized method. The robot operating environment and parameter settings are the same as in Section 5.1. The covariance matrix is set to In terms of operational efficiency, the average velocity of our method is 15% higher than that of the centralized method, and the task time of our method is 6.80% less than that of the centralized method. Moreover, the average velocity of our method is 25% higher than that of the soft constraint-based DMPC, and the task time of our method is 12.1% less than that of the soft constraint-based DMPC.
In terms of computational efficiency, the calculation time per horizon of our method is longer than that of the soft constraint-based DMPC. However, the difference is not significant. Moreover, it can be seen that our method is much more computationally efficient than the centralized method.

Computational Time Consumption Comparison and Analysis
In this section, the computational performance and the quality of the results of our method are further compared with other methods. Simulations are performed under different numbers of robots. The cost is utilized to evaluate the quality of the results and calculation time per horizon is utilized to evaluate the computational efficiency. Since the soft constraint-based DMPC does not guarantee a 100% success rate, our method is compared with the centralized method. The robot operating environment and parameter settings are the same as in Section 5.1. The covariance matrix is set to N noise = diag(0.1m, 0.1m, 0.5deg). The results are shown in Figure 11.
is longer than that of the soft constraint-based DMPC. However, the difference is not significant. Moreover, it can be seen that our method is much more computationally efficient than the centralized method.

Computational Time Consumption Comparison and Analysis
In this section, the computational performance and the quality of the results of our method are further compared with other methods. Simulations are performed under different numbers of robots. The cost is utilized to evaluate the quality of the results and calculation time per horizon is utilized to evaluate the computational efficiency. Since the soft constraint-based DMPC does not guarantee a 100% success rate, our method is compared with the centralized method. The robot operating environment and parameter settings are the same as in Section 5.1. The covariance matrix is set to (0.1 ,0.1 ,0.5 ) noise N diag m m deg = . The results are shown in Figure 11.  As can be seen from Figure 11a, the computational efficiency of our method decreases slightly as the number of robots increases while the computational efficiency of the centralized method decreases rapidly. Figure 11b illustrates that the cost of our method is a little higher than that of the centralized method. According to the experimental data, the computational efficiency of our method is on average 3.50 times higher than that of the centralized method, and the cost is on average 23.9% higher than that of the centralized method.

Conclusions
A model predictive priority contouring control is proposed to deal with the problem of multi-robot robust motion planning in this paper. Firstly, the ICBS planner is utilized to obtain the initial guess for model predictive priority contouring control. The lowlevel planner of the CBS planner is replaced by the hybrid A* planner. Second, doublelayer corridors are proposed to deal with chance safety constraints. Static-layer corridors ensure obstacle avoidance and dynamic-layer corridors guarantee inter-robot collision avoidance. The double-layer corridors are applied as safety constraints of model predictive priority contouring control. Third, a prioritization mechanism is devised to improve the computational efficiency of model predictive contouring control. The priority of the robot is assigned based on the percentage of uncompleted tasks. Finally, simulations show the effectiveness and real-time performance of our approach in multi-robot robust motion planning. In future work, the authors plan to extend the proposed method to a maze-like environment. Moreover, interaction with unknown dynamic obstacles will be considered in our future work.