## 3 Double-Layer Corridor
### Model Predictive Priority Contouring Control
#### 4.3.4 Prioritization Mechanism  
Based on the previous discussion, the multi-robot robust motion planning of Section 2 can be rewritten in the following form.  
\[J=l_{1}J_{d}+l_{2}J_{s\_{=}v}+l_{3}J_{s\_{=}w} \tag{36a}\] \[s.t.:\] (36b) \[\mathbf{x}_{m,k}\in S^{s}_{m},\forall m\] (36c) \[\mathbf{x}_{m,k}\in S^{d}_{m,k},\forall m \tag{36d}\]  
Figure 6: The contour error and the lag error.  
\[\left|v_{m,k}\right| \leq v_{\max}^{m}\left|w_{m,k}\right|\leq w_{\max}^{m},\forall m \tag{36e}\] \[\left|v_{m,k+1}-v_{m,k}\right| \leq a_{\max}^{m}\left|w_{m,k+1}-w_{m,k}\right|\leq a_{\max}^{m},\forall m\] (36f) \[\mathbf{x}_{m,0}=\mathbf{x}_{m}(0),\forall m \tag{36g}\]  
In Equation (36), \(m\in\{1,2,\dots,N\}\). Equation (36a) represents the cost function: the details of its items are discussed previously. Equation (36b) is robot model constraints. Equation (36c) is obstacle avoidance constraint. Equation (36d) is inter-collision avoidance constraint. Equation (36e,f) represent control input constraints. The initial state constraints are denoted by Equation (36g).  
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 large-scale 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.  
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.  
\[P_{m}=\frac{H_{m}}{H_{m,ref}} \tag{37}\]  
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_{mmk}\) 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.  
Figure 7: Model predictive priority contouring control with double-layer corridors.