--- title: "Switched Linear Multi-Robot Navigation Using Hierarchical Model Predictive Control" authors: - "Chao Huang" - "Xin Chen" - "Yifan Zhang" - "Shengchao Qin" - "Yifeng Zeng" - "Xuandong Li" doi: "10.24963/ijcai.2017/605" url: "https://ijcai.org/proceedings/2017/605" pdf: "https://ijcai.org/proceedings/2017/0605.pdf" venue: "Proceedings of the Twenty-Sixth International Joint Conference on Artificial Intelligence" year: 2017 pages: "4331-4337" track: "Main track" keywords: - "Robotics and Vision: Multi-Robot Systems" - "Robotics and Vision: Motion and Path Planning" - "Planning and Scheduling: Distributed/Multi-agent Planning" --- # Switched Linear Multi-Robot Navigation Using Hierarchical Model Predictive Control Chao Huang, Xin Chen, Yifan Zhang, Shengchao Qin, Yifeng Zeng, Xuandong Li *Proceedings of the Twenty-Sixth International Joint Conference on Artificial Intelligence*, pages 4331-4337. DOI: [10.24963/ijcai.2017/605](https://doi.org/10.24963/ijcai.2017/605) ## Abstract Multi-robot navigation control in the absence of reference trajectory is rather challenging as it is expected to ensure stability and feasibility while still offer fast computation on control decisions. The intrinsic high complexity of switched linear dynamical robots makes the problem even more challenging. In this paper, we propose a novel HMPC based method to address the navigation problem of multiple robots with switched linear dynamics. We develop a new technique to compute the reachable sets of switched linear systems and use them to enable the parallel computation of control parameters. We present theoretical results on stability, feasibility and complexity of the proposed approach, and demonstrate its empirical advance in performance against other approaches. ## Full text ## 1 Introduction A switched linear system is a special type of hybrid systems, consisting of several modes equipped with linear dynamics (differential/difference equations), and a switching rule specifying how to switch between them. It provides an expressive model for designing robots which embrace complex behaviors [Faust et al., 2016; Bogomolov et al., 2014; Lahijanian et al., 2014]. For example, a robot with many gears which deliver different torques and speeds can be directly modeled by a switched linear system. The multi-robot navigation problem attracts much attention for being not only academically challenging, but also of practical importance. It drives a group of robots from their initial positions to goal positions without any reference trajectory. A successful navigation should provide three guarantees: converging to the goal positions finally (stability), avoiding collision all the time (feasibility), and fast computing (efficiency). There have been a number of works treating the navigation problems as path planning problems, where heuristic-search based methods are the most popular and well-developed solutions [Wagner and Choset, 2011; Karaman and Frazzoli, 2011; Janovsk´y et al., 2014]. For robots with discrete state space such as graph, A* based algorithms can quickly find the optimal path while ensuring the stability, and thus are well studied [Koenig and Likhachev, 2002; Wagner and Choset, 2011]. Before adopting it to continuous states, the continuous state space must be discretized in advance. Those methods that deal with continuous state space directly receive much attention in recent years. Optimal reciprocal collision avoidance (ORCA) approach [van den Berg et al., 2011] is proposed to address the collision avoidance problem of robots. It is then combined with the RRT* algorithm [Karaman and Frazzoli, 2011], an extension of RRT (rapidly exploring random tree), for robot navigation in a continuous state space [Janovsk´y et al., 2014]. However neither of them ensures theoretical stability, therefore may easily lead robots to a situation where robots stick at one point as a result of infinite loops or deadlock [Janovsk´y et al., 2014]. To ease treatment, all the above methods assume that once the specified positions are given, all robots can easily compute the corresponding inputs and arrive the positions accurately, thus do not take the concrete dynamics of robots into consideration. ∗Corresponding authors: Xin Chen, Shengchao Qin Unfortunately, as is pointed in [Pecora et al., 2012], for robots with complex behaviors, treating the navigation problem as two separated steps: path panning and control inputs computation may cause severe collision issues. That is, due to the absence of robot behavior in path planning, even though the planned path is theoretically safe, in practice it might lead to collisions as the target positions in the given trajectory may be beyond the robots' ability. Therefore, for switched linear robots, due to their intrinsic complexity, the strategy of separation does not fit for handling the navigation problems. Different from path planning approaches which only consider the trajectory, the switched linear robot navigation control requires to compute control inputs of all robots with consideration of robot dynamics. Model Predictive Control (MPC) is the most popular method whose basic idea is to first compute an input sequence of the next several steps using certain path finding strategy, and then choose the sequence head as the current input. MPCs fall into three categories: centralized MPC (CMPC), distributed MPC (DMPC) and hierarchical MPC (HMPC). CMPC provides theoretical guarantee on feasibility and stability, but suffers from a high computation complexity [Dunbar and Murray, 2002]. DMPCs can hardly ensure the stability under (non-convex) collision avoidance specification [Keviczky et al., 2004; Kuwata and How, 2011; Mercang¨oz and Doyle, 2007]. Recently, HMPCs are proposed which balance the merits of both CMPC and DMPC schemes. In HMPC, a central controller determines the next state of every robot while for each robot, there is a distributed controller computing its control parameter driving it to the given state. Among them, the approach proposed in [Cirillo et al., 2014] cannot ensure the dynamic feasibility generated by the central controller and the method proposed in [Huang et al., 2016] replaces constraints of dynamics with reachable sets of robots so that the goal states can be efficiently produced by the central controller, ensuring both stability and feasibility for simple linear dynamical robots. In this paper, starting from the reachable set based HMPC, we aim to address the navigation problem of robots whose behaviors are described by switched linear systems in continuous space. Here, the key challenge is how to compute the exact reachable set of robots in a very short time: the accuracy of the reachable sets assures the equivalence between the transformed optimization problem and the original one; if the overhead incurred by the computation of reachable sets is more than the time saving benefitted from parallel computation, it would render HMPC to be less useful. The reachable set computation of general switched systems is a well-known undecidable problem requiring heavy computation efforts [Alur et al., 1995; Henzinger et al., 1995]. We give a novel method to compute the reachable set function by slightly restricting the multi-robot system with a reasonable assumption. The reachable set function is computed in advance off-line, and the concrete reachable set is directly online derived by substituting the current state. Our main contributions are listed as follows: - We propose a novel HMPC based method to address the navigation problem of switched linear multi-robot using our newly developed technique to compute the reachable set function of switched linear systems. - We theoretically ensure the feasibility and stability of the proposed method and discuss its complexity; - We demonstrate the performance of the new HMPC based method in several navigation scenarios and show its advantages empirically by varied comparison. ## 2 ProblemFormulation Consider a switched linear dynamical multi-robot system S containing N robots on a plane. Each robot has independent dynamics and inputs along the x-axis and y-axis. Assuming the sampling interval of the i-th robot is Ti, robots synchronizes with each other at the end of every collaboration cycle, which is the least common multiple of robots' sampling intervals. Before formally defining the systems and problem, we first introduce the assumption throughout this paper. Assumption 1. Each robot stay stationary at a collaboration instant. Notably in current works of multi-robot navigation control, in particular heuristic-search methods, forcing robots to stop is a common choice when confronted with conflicting situations [Wei et al., 2014]. Now, we model such a multi-robot system. For the i-th robot, the collaboration cycle is Ki times its local control cycle. A state of the i-th robot is denoted as qi = [xi, ˙xi, yi, ˙yi]T , where xi, ˙xi, yi, ˙yi denote the position and velocity along the x-axis and the y-axis, respectively. Let qi,x = [xi, ˙xi]T and qi,y = [yi, ˙yi]T be the states along two axes. The dynamics of each robot along two axes are independent. We can model the dynamic along each axis separately as a switched linear system. Let Mi,x and Mi,y be the number of permissible linear dynamical modes along x-axis and y-axis. For the x-axis, the dynamic is modeled as the linear discrete-time time-varying state equality below: where k denotes the k-th collaboration instant, ki∈{0, 1, ...,Ki-1} represents the ki-th local sampling instant of the i-th robot, the pair (mi,x(k, ki), ui,x(k, ki)) is referred as the hybrid control along the x-axis, where: Switching Control mi,x(k, ki) ∈Mi,x≜{1, · · · ,Mi,x} determines the dynamic mode; Continuous Control ui,x(k, ki) ∈ Ui,x≜[-Ui,x,Ui,x] determines the continuous evolution. Ai,x is the 2×2 coefficient matrix of state qi,x while Bi,x is the 2 × 1 coefficient matrix of ui,x determined by mi,x(k, ki). Equation (1b) defines the beginning state of one collaboration cycle as the ending state of the last collaboration cycle. Intuitively, Equation (1) indicates that the i-th robot can switch to one of the permissible dynamic modes along the x-axis at each sampling instant, then pick an appropriate input under the mode. Following the dynamic model along the x-axis, we model the one along the y-axis in the same way. Let ui = [ui,x, ui,y]T denote the continuous control of the ith robot, mi = (mi,x,mi,y) denote the switching control. The complete dynamic of the i-th robot is: where Ai=Ai,x⊕Ai,y, Bi(mi(k, ki))=Bi,x(mi,x(k, ki)) ⊕Bi,y (mi,y(k, ki)), and the direct sum operator ⊕ for two matrices A and B is defined as:A ⊕ B ≜ The constraint of switching control is: which means that the i-th robot has Mi,x·Mi,y dynamic modes. The switching control sequence of the ith robot in the k-th collaboration cycle is denoted by ˜mi(k) ≜mi(k),mi(k, 1), ...,mi(k,Ki-1). The constraint of continuous control is: where Ui = [Ui,x, Ui,y]T . The continuous control sequence of the i-th robot in the k-th collaboration cycle is denoted by ˜ui(k) ≜ ui(k), ui(k, 1), ..., ui(k,Ki-1). We refer to the pair (mi(k, ki), ui(k, ki)) as the hybrid control of the i-th robot. For a multi-robot system S, let q(k) ≜ [q1(k), ..., qN(k)] denote the global state at the k-th collaboration instant, ˜m(k) ≜ ˜m1(k), · · · , ˜mN(k) and ˜u(k) ≜ ˜u1(k), · · · , ˜uN(k) denote the global switching and continuous control sequence in the k-th collaboration cycle respectively. Assumption 1 is described as collaboration specification: We define the collision avoidance specification requiring that the (infinity norm) distance between any two robots must not be less than a given safety distance dsafe: The infinite norm of a vector x = [x1, ..., xn]T is defined as ‖x‖∞ ≜ max (|x1|, ..., |xn|). Following [Huang et al., 2016], we define two core concepts for MPC, namely feasibility and stability. Definition 1. (Feasibility) A MPC controller is feasible iff for any feasible initial state, the actual state {q(t), t≥0} computed by the controller at every collaboration instant satisfies the collision avoidance specification h(q(t))≤0. Definition 2. (Stability) A MPC scheme is stable iff starting from any feasible initial state, the state sequence {q(t)}∞t=0 computed by the MPC controller converges to the goal state. Subsequently we formulate the navigation problem: Problem 1. Given a multi-robot system S, where - the i-th robot has the kinematic model described as in (3a) and (3b) with switching control constraint (4) and continuous control constraint (5); - a stationary initial state q(0) = [q1(0), ..., qN(0)]T of S, where the initial velocity of each robot is zero; - a stationary goal state q′ = [q′1, ..., q′N]T of S, where the goal velocity of each robot is zero; - the collaboration specification and collision avoidance specification are described as in (6) and (7) respectively. Determine a control strategy to generate the hybrid control sequence {( ˜mi(k), ˜ui(k))}∞k=0 for the i-th robot, i=1, ...,N, satisfying feasibility and stability. Remark 1. In this work, we consider the obstacle-free navigation scenario. Note that for each robot, all other robots are treated as moving obstacles, and the collision avoidance is described by coordination constraints. With trivial extension, stationary obstacles can be treated similarly. Therefore, this setup does not lose the generality and practicality. ## 3 HMPC Framework In this section, we briefly introduce the basic idea of the HMPC control framework on switched linear multi-robot navigation. At a collaboration instant t, the HMPC scheme consists of two successive steps: Compute the control goals: The central controller collects the current state q(t) and minimizes the state cost in a finite horizon H: where the notation (·)(k|t) denotes the predictive value at the (t+k)-th collaboration instant computed at time t, Ri(qi(k|t)) is the state set which the i-th robot can reach at the next collaboration instants from the state qi(k|t), lH and ˜Qf are userdefined terminal cost and terminal constraint set to ensure stability. lq and lu denote the state cost and continuous control cost respectively, which are designed to be positive definite: Let q∗ (1|t), ..., q∗ (H|t) denote the optimal solution. The first sample q∗ (1|t) will be used as the desired states q(t+1) at the instant t+1, that is q(t+1) = q∗ (1|t). For the vector q(t+1) = [q1(t+1), ..., qN(t+1)]T , its i-th element qi(t+1) specifies the control goal of the i-th robot at the collaboration instant t+1. Compute the hybrid controls: The i-th robot receives the control goal qi(t+1) and then derives the explicit control: Note that this optimization problem is non-convex. It can be convert to mixed integer programming and solved by optimization solvers according to [Richards and How, 2005]. The optimal solution ( ˜m∗i (t), ˜u∗i (t)) is the actual hybrid control sequence for the i-th robot in the next collaboration cycle. This procedure will be repeated at the next collaboration instant t + 1 based on new measurements of the state q(t + 1) until J ∗ c (q(t)) meets a certain convergence criteria. Observing HMPC framework, the key lies on how to efficiently compute the reachable set for switched linear systems. ## 4 Computation of Reachable Set Function In this section, We propose a novel method for computing the reachable set function Ri(qi(t)) for a switched linear system. Recall the formal definition of a reachable set function. Definition 3. The reachable set function Ri(qi(t)) of the i-th robot is defined as the set comprising all states that the i-th robot can reach in a collaboration cycle from the state qi(t). In particular, RPi(qi(t)) denotes the reachable position set function, which consists all the positions in Ri(qi(t)). Consider the switching control sequence ˜mi(t) in the t-th collaboration cycle. Since mi(t, ki) has Mi,x·Mi,y choices at any ki-th local sampling instant, ˜mi(t) can have (Mi,x·Mi,y)Ki cases. And the reachable set under a certain switching control sequence is a polytope [Gritzmann and Sturmfels, 1993]. Hence a naive way to compute Ri(qi(t)) is to directly compute the union of the (Mi,x·Mi,y)Ki convex polytopes. However, this approach faces a serious problem: if the representation of reachable set function includes the intermediate variables, its computation becomes time consuming and the optimization (9) solved in central controller degenerates to a CMPC. It indicates two key requirements of reachable set function. - Requirement 1: The computation of reachable set should be efficient by reachable set function; - Requirement 2: The representation of reachable set function should hide the intermediate variables between two collaboration instants. In the following, we introduce our approach for computing the reachable set function Ri(qi(t)) under Assumption 1. Since the collaboration specification (6) demands ˙xi(t+1) = ˙yi(t+1)=0, if we obtain the reachable position set RPi(qi(t)), Ri(qi(t)) can be easily computed as Ri(qi(t))={[x, ˙x, y, ˙y]T :[x, y]T ∈ RPi(qi(t))∧( ˙xi=˙yi=0)}. Hence the key is to compute RPi(qi(t)). Since the dynamic along the x-axis and y-axis is independent, we separately consider the dynamics along two axes. Let RPi,x(qi(t)) and RPi,y(qi(t)) denote the reachable position set along x-axis and y-axis, respectively. For the dynamic along the x-axis, we have the following conclusion. Lemma 1. The reachable position set RPi,x(qi(t)) along the x-axis is a closed interval. Proof. For a switching control sequence ˜mi(t) = {mi,x(t, ki)}Ki-1 ki=0 along with x-axis, let RPi,x(qi(t),mi(t)) be the reachable position set along the x-axis with ˜mi(t). Since RPi,x(qi(t), ˜mi(t)) is a convex polytope [Gritzmann and Sturmfels, 1993] and is 1-dimensional, RPi,x(qi(t), ˜mi(t)) is a closed interval. Furthermore, recall that Obviously, {(mi,x(t, ki), 0)}Ki-1 ki=0 is a feasible hybrid input sequence to make ˙xi(t+1)= ˙yi(t+1)=0 for any switching control sequence ˜mi(t). In this case, qi,x(t+1)= (Ai,x)Ki qi,x(t), i.e. (Ai,x)Ki qi,x(t) ∈ RPi,x(qi(t), ˜mi(t)). So the reachable set under each switching control sequence shares a common element (Ai,x)Ki qi,x(t), i.e. (Ai,x)Ki qi,x(t) ∈ ⋂ ˜mi (t) RPi,x(qi(t), ˜mi(t)). Then Lemma 1 can be proved by the property of intervals. When qi,x(t) is the origin, following Lemma 1, let RPi,x(0) ≜ [minxi,maxxi] be the corresponding reachable position set along x-axis, where minxi and maxxi are the nearest and farthest distance the i-th robot can reach along the x-axis. According to Assumption 1, we have It immediately conducts the following corollary on the reachable position set from the above equation. Similarly, we can obtain the same conclusion on the dynamic along the y-axis. Let RPi,y(0) = [minyi,maxyi]. These facts indicate that the reachable set Ri(qi(t)) is a hyperrectangle: Theorem 1. The reachable set function Ri(qi(t)) of the i-th robot is: minxi,maxxi,minyi,maxyi can be computed by solving the corresponding optimization problems respectively. For example, we can obtain min xi by solving a MIP problem: Let us see if the reachable set function satisfies the two requirements. Since the reachable set function Ri(qi(t)) can be obtained by simply substituting qi(t) value, Requirement 1 is achieved. Note that all the intermediate variables are hidden in the Ri(qi(t)) , thus Requirement 2 is also satisfied. ## 5 AlgorithmAnalysis We discuss the two important properties of the algorithm: feasibility and stability. We also compare the complexity of the proposed method with CMPC on switched linear systems. As the goal states given by the central controller at each collaboration cycle will be exactly reached by all robots, the feasibility of HMPC is determined by the optimal problem settled by the central controller. The optimal problem encodes the status of all robots as well as the constraints arising from the complete collision avoidance specification, so the feasibility is automatically assured by the solution. Theorem 2 (Feasibility). Our HMPC scheme is feasible. The terminal constraint set and cost function methods are widely used to ensure the stability of traditional MPC approaches [Mayne et al., 2000]. They differ from each other on the setting of stable parameters (namely, a terminal cost lH, a terminal constraint set ˜Qf and a controller function κ(·)). Since CMPC deals with the multi-robot system as a whole, these various approaches can be easily applied on CMPC. So instead of giving the concrete parameter setting, we show the stability of our HMPC is equivalent to the stability of a specific linear CMPC. Thus users can choose their favorite methods to set parameters to ensure the stability. Theorem 3 (Stability). Given a multi-robot system where each robot is modeled as (3a) and (3b), if a set of parameters lH, ˜Qf , κf(·) ensures the stability of a specific linear CMPC, the same setting also ensures the stability of our HMPC. Proof. We construct the following linear CMPC problem: where In ∈ Rn denotes the n-dimension identity matrix, u′ is the 4N × 2 input variable in (10). Provided that lH, ˜Qf , κf(·) ensures the stability for (10), i.e. the state sequence computed by (10) converges to the goal state. We have to prove the state sequence {q(t)}∞t=0 computed by (8) is exactly the same with the one computed by (10). Note that (8) has the same cost function with (10). The only difference between the constraints parts of (8) and (10) is the state function and input constraints, which actually provide the same feasible range of states. So given any q(t), the optimal solutions of (8) and (10) are the same, which leads to the conclusion that the state sequence {q(t)}∞t=0 computed by (8) will be identical to the one computed by (10). Notice that each robot is ensured by (9) to reach its control goal computed by the central controller in each collaboration cycle. Stability provided by the central controller in turn guarantees that the HMPC scheme is stable. Due to the parallel computation of robots' concrete inputs, HMPC's efficiency is mainly determined by the optimization problem in the central controller. Thus a straight forward way to analyze theoretical efficiency is to quantitatively compare the complexity of programming problems in the central controllers of our HMPC and CMPC. Note that non-convex collision avoidance specification should be rewritten with BigM method by introducing integer slack variables. Also as we mentioned above, the non-convex switched linear dynamic (3a) and (3b) should be treated by replacing the original switching control variable mi(k, ki) with a Mi,x ×Mi,y binary slack matrix variable, and the original continuous control variable ui(k, ki) with a Mi,x ×Mi,y cell matrix variable where each element is a 2 × 1 real vector [Richards and How, 2005]. It means that the optimization problems in HMPC and CMPC are mixed integer programming (MIP). There is no available theory of MIP complexity analysis, we analyze their complexity by comparing the number of (scalar) variables and (scalar) constraints instead. As integer variables have great impact on the MIP complexity, we consider continuous and integer variables separately. Proposition 1 (Complexity). Comparing to the optimization problem in CMPC, the one in our HMPC's central controller is reduced by H · ∑Ni=1 (2KiMi,xMi,y +4Ki -4) continuous variables, H · ∑Ni=1 KiMi,xMi,y integer variables and H · ∑Ni=1 (2KiMi,xMi,y + 5Ki - 4) constraints. Proof. Collaboration specification and collision avoidance specification are treated the same in both control frameworks, thus we do not take them into account. For a robot i, at each sampling instant, there are Mi,x·Mi,y integer switching control variables, 2Mi,x·Mi,y continuous control variables, 4 continuous state variables, 2Mi,x·Mi,y+1 control constraints and 4 state equation constraints, which should be all considered in CMPC. The intermediate variables and constraints in a collaboration cycle are reduced in our HMPC with the additional 4 reachable set constraints. Since the optimization problems in our HMPC and CMPC's central controller consider N robots in H collaboration cycles, the proposition is true. Proposition 1 demonstrates that our HMPC greatly reduces the number of variables and constraints comparing to CMPC, which is empirically supported by the following experiments. ## 6 Experimental Results In order to explore the practicability of our HMPC based method on switched linear multi-robot systems, we have implemented other MPC schemes:centralized MPC (CMPC), basic DMPC (BDMPC), sequential DMPC (SDMPC), and iterative DMPC (IDMPC). CMPC [Dunbar and Murray, 2002] deploys a central controller to compute the concrete inputs of all the robots. On the contrary, DMPCs distribute the computation to many robots and differ from each other in terms of the policy of distribution: robots compute concurrently in BDMPC [Keviczky et al., 2004], sequentially in SDMPC [Kuwata and How, 2011], and iteratively in IDMPC [Mercang¨oz and Doyle, 2007]. We derive the dynamic model of the i-th robot (Eq. 3) by discretizing a switched linear continuous system: where at the sampling frequency: The continuous control constraint is Ui = [2, 2]T and the safety distance is dsafe = 0.6. Programming problems embedded in MPCs are solved by the CVX, a Matlab-based package [CVX Research, 2012; Grant and Boyd, 2008]. Figure 1: Robot trajectories using HMPC ![Image](https://ijcai.org/proceedings/2017/fulltext/605/image_000000_e677eb8b52e2525e08c794521babf653446466c7b321a5aea1682af45a278309.png) ## 6.1 Effectiveness We compare the effectiveness by simulating MPCs with different numbers of robots N=3, 6, 9, 12, 15, 18, 21, 24. Our HMPC finishes all the simulations as CMPC. Due to the space limitation, we show the trajectories computed by our HMPC in 6/12-robot cases as Figure 1. In Figure 1 (and Figure 2), the x-axis and y-axis construct the plane which all the robots move in, and represent the positions of robots. Blue stars denote the initial positions, black crosses denote the goal positions, and dots in different colors denote the trajectories of different robots. In this configuration, robots have to cross the trajectories of others before they reach the goal positions. If all robots move directly towards to their corresponding goal positions, collision will inevitably happen. Observing the trajectories computed by our HMPC, the robots in each scenario do not collide (satisfying feasibility) and finally converge to the goal states (satisfying stability), which conforms to the theoretical conclusion in Sec. 5. However, DMPCs suffer from heavy failure issues, where several typical failed scenarios are shown in Figure 2. Note that the simulation is forced to stop, once the distance between any two robots is detected smaller than the safety distance. Reasons of failures fall into three main categories: Incorrect Prediction: In Figure 2a, the 2rd and 6th robots collide, even though they are neighbors. Instead of knowing the actual behaviors of its neighbors, in BDMPC each robot computes its action by prediction. These neighbors often behave differently from what the i-th robot predicts. As a result, it is of high possibility that two adjacent robots make incorrect predictions on each other's behavior and thus collide. Divergent Iteration: In Figure 2b, the robots get stuck in an infinite loop as the iteration process does not converge when using IDMPC. Before it was forcibly terminated, the iteration had tried over 100 times and spent more than four hours. It indicates that IDMPC may need additional treatment when encountering navigation problems with non-convex constraints. Aggressive Action: In Figure 2c, the 10th robot cannot get a feasible solution. In SDMPC, robots compute their actions sequentially in a fixed order. The aggressive actions taken by the robots with high priority may push the robots with low priority out of feasible regions. ## 6.2 Efficiency Figure 3a shows the computation time of different MPCs in the scenarios in Sec. 6.1. For each scenario, the average computation time at each collaboration instant is plotted, if a MPC scheme succeeds. We study the efficiency improvement ac- ![Image](https://ijcai.org/proceedings/2017/fulltext/605/image_000001_81cbe7f53a885ee87e7c3897f2bf1eb76f6557c2dac57eefa12871a16f5c9e30.png) (a) BDMPC in 6-robot case (b) IDMPC in 6-robot case (c) SDMPC in 12-robot case Figure 2: Typical failures of DMPCs ![Image](https://ijcai.org/proceedings/2017/fulltext/605/image_000002_a1d1f3374898037ac9cbe7e1a2b1d21cfe7c2c54a65ae84909a67ada4eb4aff3.png) (a) Computation time vs. number of robots (b) Increasing rate of computation time vs. dynamic modes Figure 3: Efficiency Comparison company with the increasement of robots. Notably, compared to CMPC, our HMPC improves the efficiency by at least 66.4% for all cases. This result complies with the theoretical conclusion in Sec. 5. The great reduction of constraints and variables makes our HMPC much more efficient in practice. We then explore the efficiency improvement when the number of dynamic modes grows up. Figure 3b shows the result. The y-axis reflects the increasing rate of the average computation time at each collaboration instant, which is calculated by dividing the extra computation time in the case of four-mode robots by the computation time in the case of one-mode robots. The x-axis shows the number of robots involved. The average increasing rate of our HMPC is 61.8%, while that of CMPC is over 300%. It indicates that our HMPC is less sensitive to the growth of systems' complexity than CMPC. It is due to that in our HMPC the hyper-rectangle representation of reachable set hides the integer variables in the central controller introduced by multiple modes. Thus the complexity of the problem in the central controller increases smoothly with the robot dynamic modes. ## 7 Conclusion In this paper, we propose a novel HMPC based approach for the navigation problem of switched linear dynamical robots with a new reachable set computation technique. We discuss feasibility, stability and complexity of the approach theoretically and shows the effectiveness and efficiency empirically. ## Acknowledgement Chao Huang was supported by the China Scholarship Council (No.201606190185). This research was supported by The National Key Research and Development Program of China (No.2016YFB1000802), National Natural Science Foundation of China (No.61632015, No.61561146394, No.61373033) and also by Collaborative Innovation Center of Novel Software Technology and Industrialization. ## References 1. [Alur et al., 1995] Rajeev Alur, Costas Courcoubetis, Nico- las Halbwachs, Thomas A Henzinger, P-H Ho, Xavier Nicollin, Alfredo Olivero, Joseph Sifakis, and Sergio Yovine. The algorithmic analysis of hybrid systems. The- oretical computer science, 138(1):3-34, 1995. 2. [Bogomolov et al., 2014] Sergiy Bogomolov, Daniele Mag- azzeni, Andreas Podelski, and Martin Wehrle. Planning as model checking in hybrid domains. In AAAI, pages 2228- 2234, 2014. 3. [Cirillo et al., 2014] Marcello Cirillo, Federico Pecora, Hen- rik Andreasson, Tansel Uras, and Sven Koenig. Integrated motion planning and coordination for industrial vehicles. In ICAPS, 2014. 4. [CVX Research, 2012] Inc. CVX Research. CVX: Matlab software for disciplined convex programming, version 2.0. http://cvxr.com/cvx, aug 2012. 5. [Dunbar and Murray, 2002] William B. Dunbar and Richard M. Murray. Model predictive control of co- ordinated multi-vehicle formations. In CDC, volume 4, pages 4631-4636, 2002. 6. [Faust et al., 2016] Aleksandra Faust, Hao-Tien Chiang, Nathanael Rackley, and Lydia Tapia. Avoiding moving obstacles with stochastic hybrid dynamics using PEARL: preference appraisal reinforcement learning. In ICRA, pages 484-490, 2016. 7. [Grant and Boyd, 2008] Michael C. Grant and Stephen P. Boyd. Graph implementations for nonsmooth convex pro- grams. In Recent Advances in Learning and Control, Lec- ture Notes in Control and Information Sciences, pages 95- 110. 2008. 8. [Gritzmann and Sturmfels, 1993] Peter Gritzmann and Bernd Sturmfels. Minkowski addition of polytopes: com- putational complexity and applications to gröbner bases. SIAM Journal on Discrete Mathematics, 6(2):246-269, 1993. 9. [Henzinger et al., 1995] Thomas A Henzinger, Peter W Kopke, Anuj Puri, and Pravin Varaiya. What's decidable about hybrid automata? In STOC, pages 373-382. ACM, 1995. 10. [Huang et al., 2016] Chao Huang, Xin Chen, Yifan Zhang, Shengchao Qin, Yifeng Zeng, and Xuandong Li. Hierar- chical model predictive control for multi-robot navigation. In IJCAI, pages 3140-3146, 2016. 11. [Janovský et al., 2014] Pavel Janovský, Michal Cáp, and Jirí Vokrínek. Finding coordinated paths for multiple holo- nomic agents in 2-d polygonal environment. In AAMAS, pages 1117-1124, 2014. 12. [Karaman and Frazzoli, 2011] Sertac Karaman and Emilio Frazzoli. Sampling-based algorithms for optimal motion planning. Int. J. Rob. Res., 30(7):846-894, June 2011. 13. [Keviczky et al., 2004] Tamás Keviczky, Francesco Borrelli, and Gary J. Balas. A study on decentralized receding hori- zon control for decoupled systems. In ACC, volume 6, pages 4921-4926. IEEE, 2004. 14. [Koenig and Likhachev, 2002] Sven Koenig and Maxim Likhachev. D* lite. In AAAI/IAAI, pages 476-483, 2002. 15. [Kuwata and How, 2011] Yoshiaki Kuwata and Jonathan P. How. Cooperative distributed robust trajectory optimiza- tion using receding horizon milp. IEEE Transactions on Control Systems Technology, 19(2):423-431, 2011. 16. [Lahijanian et al., 2014] Morteza Lahijanian, Lydia E Kavraki, and Moshe Y Vardi. A sampling-based strategy planner for nondeterministic hybrid systems. In ICRA, pages 3005-3012. IEEE, 2014. 17. [Mayne et al., 2000] David Q. Mayne, James B. Rawlings, Christopher V Rao, and Pierre OM Scokaert. Constrained model predictive control: Stability and optimality. Auto- matica, 36(6):789-814, 2000. 18. [Mercangöz and Doyle, 2007] Mehmet Mercangöz and Francis J. Doyle. Distributed model predictive control of an experimental four-tank system. Journal of Process Control, 17(3):297-308, 2007. 19. [Pecora et al., 2012] Federico Pecora, Marcello Cirillo, and Dimitar Dimitrov. On mission-dependent coordination of multiple vehicles under spatial and temporal constraints. In IROS, pages 5262-5269. IEEE, 2012. 20. [Richards and How, 2005] Arthur Richards and Jonathan How. Mixed-integer programming for control. In ACC, pages 2676-2683. IEEE, 2005. 21. [van den Berg et al., 2011] Jur van den Berg, Stephen J. Guy, Ming Lin, and Dinesh Manocha. Reciprocal n-Body Col- lision Avoidance, pages 3-19. 2011. 22. [Wagner and Choset, 2011] Glenn Wagner and Howie Choset. M*: A complete multirobot path planning algorithm with performance bounds. In IROS, pages 3260-3267, Sept 2011. 23. [Wei et al., 2014] Changyun Wei, Koen V Hindriks, and Catholijn M Jonker. Multi-robot cooperative pathfind- ing: A decentralized approach. In IEA/AIE, pages 21-31. Springer, 2014.