Convex model predictive control for collision avoidance

This manuscript proposes a model predictive control for collision avoidance for the regu-lation problem of deterministic linear systems, which provides a priori guarantees of strong system theoretic properties, such as positive invariance and asymptotic stability, and high computational efﬁciency. Notion of safe distance sets is introduced, and also utilized as a novel approach to ensure collision avoidance via suitably deﬁned convex constraints. The proposed convex model predictive control for collision avoidance is obtained by employ-ing interactive strategic-tactical structure for overall decision-making. The strategic stage of the overall algorithm employs direct algebraic manipulations in order to construct safe distance sets that ensure collision avoidance. The tactical stage of the overall algorithm employs strictly convex quadratic programs for the optimization of local ﬁnite horizon predicted control processes. The dynamically compatible interaction of strategic and tactical stages of the overall algorithm is ensured by construction, which guarantees structural and computational beneﬁts. These novel and unique features effectively enable both real time implementation and real life utilization of model predictive control for collision avoidance.


INTRODUCTION
Collision avoidance is a classical control problem whose theoretical, computational and practical aspects have received a considerable amount of attention [1][2][3][4][5][6][7][8][9][10][11][12][13][14][15][16][17][18][19]. This strong interest in collision avoidance control problem should not come as a surprise given its fundamental relevance for classical vehicles within automotive, aerospace and space applications as well as contemporary intelligent robotic systems and unmanned vehicles. The most systematic approach to control synthesis for collision avoidance is to deploy model predictive control (MPC). Indeed, MPC [20,21] offers a systematic approach to handle static and dynamic constraints while optimizing performance of the considered system. Naturally, MPC is highly effective when its implementation can be executed via convex optimization. Unfortunately, MPC under collision-avoidance constraints is an intrinsically nonconvex problem [1-4, 9, 12]. This intrinsic nonconvexity of collision avoidance control problem diminishes considerably realistic utility of MPC due to prohibitive computational efforts required for the actual implementation of this advanced control methodology. A shift of paradigm in MPC for obstacle avoidance has been recently reported in [22]. In this predecessor article, separation theorem was utilized in order to obtain suitable separating hyperplanes and construct related "separating closed polyhedra", with the help of which nonconvex obstacle avoidance constraints were converted to computationally more convenient (convex) polyhedral constraints. The convex MPC for obstacle avoidance proposed in [22] was reduced to solving two strictly convex quadratic programming (QP) problems per iteration of the actual control process (except for the initialization of the actual control process at its very beginning). The solution of the first of these two strictly convex QP problems enabled, via direct algebraic manipulations, construction of sequences of, separating-hyperplanes-based, "separating closed polyhedra". The use of the related "separating closed polyhedra" enabled optimization of an admissible finite horizon predicted control process via the second of the two above mentioned strictly convex QP problems. The strictly convex QP problems for the construction of "separating closed polyhedra" and optimization of admissible finite horizon predicted control processes were dynamically compatible and consistent, which resulted in convex MPC for obstacle avoidance with a priori guarantees of strong system theoretic properties and considerable computational efficiency. The underlying principles of this recent approach [22] to convex MPC for obstacle avoidance provide silhouettes, and motivate development, of a conceptually new approach to MPC for collision avoidance via convex optimization. The further development reported in this article, however, requires a considerable degree of generalizations from conceptual and implementational points of view due to the inevitable differences of obstacle and collision avoidance problems.
To gain clearer insights into the intrinsic features of collision avoidance problem as well as to provide concrete basis for associated interactive strategic-tactical decision-making, the real time air-traffic control is perhaps the most illustrative real life application. At an airport, a number of airplanes arrive and aim to land safely. The airplanes might have different dynamics as well as different actual constraints, but they will surely have different locations allocated for their landing at a given and fixed time period. This can be seen as a collection of dynamically decoupled systems with associated stage constraints, which are typically different (but can possibly be identical as the airplanes share common airspace) and with different terminal constraint sets and different terminal states. Obviously for safety reasons, the airplanes should not collide during the approach and landing stages of their flights. This naturally induces collision-avoidance constraints on the collection of systems, requiring that each system operates at a safe distance relative to all other systems. The air-traffic control center provides detailed and regularly updated paths to each of the airplanes in order to land safely, while each of the airplanes optimizes its flight path according to the directions received from the air-traffic control center. This reveals an interactive strategic-tactical decision-making, in which the air-traffic control center, given the knowledge of all relevant details for each of the airplanes, provides strategic instructions to each of the airplanes, which are tactically implemented by each of the airplanes through local, simultaneous and independent optimization of their flight paths resulting in safe landing. Naturally, this modeling related discussion applies transparently, with possibly minor modifications, to a variety of real life applications including, inter alia, smart traffic control of general autonomous vehicles as well as motion and mission planning of intelligent robotic systems and smart autonomous vehicles.
In this article, we consider a collection of linear discrete time systems subject to system-wise independent stage and terminal polyhedral constraints and system-wise dependent, but collection-wise independent, collision-avoidance constraints. More precisely, we are concerned with MPC within such a setting with system-wise prescribed strictly convex quadratic stage and terminal cost functions. As already pointed out, such optimal control and, consequently, MPC problems are inherently nonconvex even in the theoretically most flexible centralized setting. To address this challenge, the objectives of this paper are (i) to propose an interactive strategic-tactical decision-making architecture, (ii) to introduce the notion of safe distance sets in order to ensure collision-avoidance constraints through (convex) polyhedral constraints, and (iii) to design a dynamically compatible and consistent MPC.
The resulting convex MPC for collision avoidance has the benefits of a priori guarantees of strong system theoretic properties and computational efficiency.
The article structure is as follows. Section 2 details setting, discusses plausible decision-making structures, and outlines traditional, nonconvex optimization based, approach to centralized MPC for collision avoidance. Section 3 introduces a novel notion of safe distance sets motivated by Voronoi diagrams, which enables utilization of polyhedral constraints to ensure satisfaction of nonconvex collision-avoidance constraints. Section 3 also provides algebraic details enabling the construction of safe distance sets. Section 4 provides the formulation of interactive strategic-tactical decision-making resulting in local convex finite horizon optimal control for collision avoidance, and it outlines a prototype algorithm for convex MPC for collision avoidance. Section 5 establishes main system theoretic properties of the proposed algorithm. Section 6 discusses implementational aspects; this includes construction of terminal constraint sets, modification of the main algorithm to allow for optimality improving subiterations, and outline of plausible alternatives for initialization step. Section 6 also provides a detailed illustration of the proposed convex MPC for collision avoidance. Section 7 delivers concluding remarks and comments on extensions.
Basic Nomenclature: The sets of reals and non-negative integers are denoted by ℝ and ℕ. Given a, b ∈ ℕ such that A polyhedron is the intersection of a finite number of open and/or closed half-spaces and a polytope is a closed and bounded polyhedron. We distinguish row vectors from column vectors only when necessary, and we employ the same symbol for a variable x and its vectorized form. The scalars appearing in algebraic expressions represent a matrix/vector of compatible dimensions. Proofs of some of the technical results are given in Appendices.

Setting
We consider a collection of r ∈ ℕ linear discrete time systems given, for each i ∈ ℕ [1:r] , by where x i ∈ ℝ n i and u i ∈ ℝ m i are the current state and control of the i th system, x + i is the successor state of the i th system, while the matrix pairs (A i , B i ) ∈ ℝ n i ×n i × ℝ n i ×m i , i ∈ ℕ [1:r] are of compatible dimensions. The considered collection of linear discrete time systems is accompanied with a collection of fixed point pairs (x i , u i ), i ∈ ℕ [1:r] , which satisfy, for all i ∈ ℕ [1:r] , (2. 2) The collection of linear discrete time systems is also accompanied with a collection of auxiliary variables z i ∈ ℝ n , i ∈ ℕ [1:r] specified, for all i ∈ ℕ [1:r] , by The states and controls x i and u i of the i th system are subject to stage constraints The terminal state of the i th system is subject to terminal constraints taking the form The variables z i associated with the i th systems are subject to collision-avoidance constraints (i) Stage constraint set  i is a closed polyhedral subset of ℝ n i +m i containing the corresponding fixed point pair (x i , u i ) in its interior, and its irreducible representation is given by The matrix pair (Y x i , Y u i ) ∈ ℝ p i ×n i × ℝ p i ×m i is known. (ii) Terminal constraint set  i is a closed polyhedral subset of ℝ n i containing the controlled fixed point x i in its interior, and its irreducible representation is given by where z i := C i x i + D i u i (and z j := C j x j + D j u j ).
The stage and terminal cost functions, i (⋅, ⋅) and V f i (⋅), are given, for each i ∈ ℕ [1:r] and all x i ∈ ℝ n i and all u i ∈ ℝ m i , by The terminal control laws f i (⋅), associated with the i th systems, are specified, for each i ∈ ℕ [1:r] , by (2.11) and they induce the related i th terminal dynamics (i) The cost weighting matrices P i ∈ ℝ n i ×n i , Q i ∈ ℝ n i ×n i and R i ∈ ℝ m i ×m i are symmetric and positive definite, that is, where All of these assumptions are introduced for stability, and these assumptions are positive invariance and fairly standard in the MPC for the considered setting [20,21].

Decision-making structures
While dynamical constraints (2.1), stage and terminal constraints (2.4) and (2.5) as well as stage and terminal cost functions (2.10) of the considered collection of linear systems are effectively independent for each of the systems, the collision-avoidance constraints (2.6) introduce, in general case, dependence between all systems. This dependence calls for characterization of the structure of the decision-making for control synthesis as well as compatibility of informational and dynamical flows so as to enable plausibility of utilized control functions for underlying decision-making. It is possible to consider a wide spectrum of structural architectures for the related decision-making lying between two extreme settings, namely centralized and decentralized decision-making. The centralized decision-making is performed with a single entity and globally. The global decision maker 1 has perfect knowledge of systems, constraints and costs as well as states of all systems when computing control actions for each of the systems. (The global decision maker computes control actions for each of the systems simultaneously.) The global decision maker is allowed to deploy control functions for the i th system. Equally importantly, the global decision maker is able to address the collision-avoidance constraints (2.6) by taking the states, and related auxiliary variables, of all systems directly into account. The centralized decision-making is a theoretically exact structure, and it yields best results. Since all systems, constraints and cost functions are considered globally and simultaneously, the centralized decision-making is effectively a dimension-wise enlarged and computationally more complex decision-making process, which somewhat offsets its structural exactness and flexibility.
The decentralized decision-making is performed with multiple entities and locally. In particular, a local decision-maker is assigned to each of the systems. Any of the local decision makers has perfect knowledge only of his system and its constraints and cost as well as its states when computing control action for the related system. (The local decision makers compute control actions for their systems simultaneously.) The i th local decision maker is allowed to deploy control functions u i (x i ) in order to generate control actions u i = u i (x i ) for the i th system. Since each of the local decision makers considers only his system and its constraints and cost, the decentralized decision-making is effectively decomposed into a number of computationally more convenient decision-making processes, which are distributed among local decision makers. However, collision-avoidance constraints (2.6) are traditionally addressed The interactive strategic-tactical decision-making in a somewhat conservative way, as these constraints require a nontrivial mechanism to ensure their satisfaction.
Evidently, the main challenge is to devise a structural architecture for decision-making which incorporates best of both extreme cases. In this sense, it is highly desirable to allow for systematic handling of the collision-avoidance constraints (2.6) as available in centralized decision-making as well as to achieve computational convenience of decentralized decision-making. This chore, in fact, leads to a number of research questions including design of communication networks, protocols, and their dynamics, compatibility of related informational and dynamical flows as well as customized distributed computations for local decision-making. The decision-making structure in this article adapts this philosophy, and it is illustrated in Figure 1. The utilized architecture for decision-making is composed of a strategist and a number of tacticians, each of which is assigned to a particular system. At the conceptual level, the strategist is in charge of initializing the actual control process as well as providing guidance to tacticians, which are in charge of locally optimizing behavior of the controlled system. Technical analysis of the utilized decision-making structure, which is for obvious reasons referred to as the interactive strategic-tactical decision-making, is elaborated on in detail in what follows.

Centralized finite horizon optimal control
Centralized MPC employs a centralized finite horizon N optimal control (FHNOC) so as to simultaneously optimize finite horizon N predicted control processes d (i,N ) for each of the considered systems. A finite horizon N predicted control process d (i,N ) of the i th system is simply a pair of state and control sequences The collection of finite horizon N predicted control process d N is, for any given composed state x := (x 1 , x 2 , … , x i , … , x r ) ∈ ℝ n C with n C = ∑ r i=1 n i , subject to system-wise independent: dynamical consistency constraints (2.18) and to system-wise dependent, but collection-wise independent, collision-avoidance constraints N ) ). The set N (x) of admissible collections of finite horizon N control processes d N is given, for any x ∈ ℝ n C , by (2.20) The cost function V (i,N ) (⋅) is associated with the i th system, and it is given as the sum of the stage costs For any given composed state x ∈ ℝ n C , the global decision maker aims at simultaneous optimization of cost functions V (i,N ) (⋅), i ∈ ℕ [1:r] via selection of an admissible collection of finite horizon N control processes d N ∈ N (x). Clearly, the optimality of centralized decision-making depends on the preference employed by the global decision maker. In this sense, the global decision maker can consider several optimality criteria including, but not exclusively limited to, multiobjective, pareto, and equilibria optimality. A computationally simplifying, and practically reasonable, solution that the global decision maker can employ is to consider a suitable scalar-valued function of cost functions Within the intended scope of this manuscript, the latter approach is discussed briefly so as to set the stage for what follows. Thus, the cost function V N (⋅) associated with a collection of finite horizon N control processes d N is simply given by With this concession, the centralized FHNOC problem N (x) takes form, for any given composed state x ∈ ℝ n C , The domain of the value function V 0 N (⋅) and its optimizer map d 0 N (⋅) is the N -step controllable set N to a terminal constraint set  :=  1 ×  2 × … ×  r given by (2.24) In general, under Assumptions 1-3, the sets N (x), x ∈ N are nonempty and closed but nonconvex, while the centralized FHNOC problem N (x) is a well-posed optimization problem, which is, in fact, a problem of minimization of a strictly convex quadratic function over a nonempty, closed and nonconvex set.

Centralized model predictive control
Centralized MPC utilizes solution of the centralized FHNOC problem N (x) so as to implement control laws N i (⋅) satisfying, for all x ∈ N , where u 0 (i,0) (x) denotes the set of optimal control actions u (i,0) at x, which is not necessarily single valued due to nonconvexity of collision-avoidance constraints (2.19). The control laws N i (⋅) induce model predictive controlled dynamics taking the form, for each i ∈ ℕ [1:r] , Under Assumptions 1-3, the composed controlled fixed point state x := (x 1 , x 2 , … , x r ) is asymptotically stable in the strong sense for the collection of model predictive controlled ) with the domain of attraction being the entire N -step controllable set N (in fact, exponentially stable if N is bounded), which is also a positively invariant set in the strong sense for the collection of model predictive controlled Unfortunately, even in this simplified version of the centralized decision-making, due to the size and, more prohibitively, nonconvexity of the centralized FHNOC problem N (x), the computational effort of traditional approach to centralized MPC for collision avoidance is overwhelming for its practical utilization.

Main objective
It is of importance to alleviate prohibitive computational burden to enable realistic implementation of MPC for collision avoidance. This aspect is addressed entirely in this article by documenting a computationally efficient, convex optimization based, reformulation of traditional approach to MPC for collision avoidance.

Safe distance sets
The ideas underpinning Voronoi diagrams are utilized in order to derive the notion, and enable computationally efficient construction, of safe distance sets, as discussed next. A basic overview of Voronoi diagrams is recalled in Appendix A, and a detailed study of it can be found in [23,24].
, are referred to as the safe distance sets. The justification of the term safe distance sets and relevant properties of the safe distance sets  i are provided by the following.

Sequence of safe distance sets
Consider sequences of points [1:r] and all k ∈ ℕ N , by where, in light of (A.2), for all i ∈ ℕ [1:r] , all j ∈ ℕ [1:r] ⧵ {i} and all k ∈ ℕ N , (3.5) are also referred to as the safe distance sets. A direct extension of Proposition 1, provides a formal justification of the term safe distance sets and summarizes relevant properties of  (i,k) .

Proposition 2. Take any collection of scalars
. Take also any collection of sequences of points (3.4) and (3.5). (3.7)

Safe distance sets for collision avoidance
For any i ∈ ℕ [1:r] , the sequence of sets { (i,k) } N k=0 is referred to as the sequence of safe distance sets for the i th system, while each of the sets  (i,k) is called safe distance set for the i th system at time k. In light of Proposition 2(iv), the collection of the sequences of the safe distance sets for the i th system, that is, the collection {{ (i,k) } N k=0 : i ∈ ℕ [1:r] } can be utilized to ensure satisfaction of collision-avoidance constraints (2.6). In particular, under postulates of Proposition 2, collision-avoidance constraints (2.19) can be replaced by Unlike direct form of the collision-avoidance constraints (2.19), constraints based on safe distance sets (3.8) are affine and system-wise independent constraints. Evidently, knowledge of a sequence of the safe distance sets { (i,k) } N k=0 , enables the i th tactician to optimize locally and independently finite horizon N control process d (i,N ) for the i th system. As formally shown in what follows, this can be achieved by solving a strictly convex QP problem. When the strategist provides sequences of the safe distance sets { (i,k) } N k=0 , i ∈ ℕ [1:r] , the tacticians can optimize locally, independently and simultaneously, finite horizon N control processes d (i,N ) , i ∈ ℕ [1:r] for their systems. Within MPC paradigm, once the tacticians optimize finite horizon N control processes d (i,N ) , i ∈ ℕ [1:r] , they can provide the strategist with the collection of sequences of points {{s (i,k) ∈ ℝ n } N k=0 : i ∈ ℕ [1:r] } satisfying postulates of Proposition 2. As formally shown in what follows, this requires direct and simple algebraic calculations. The strategist can then update sequences of the safe distance sets [1:r] , and the whole process can be repeated. This brief summary provides insights into the philosophy of the interactive strategic-tactical decision-making, which is the architectural structure employed in this article and which is formally elaborated on next.

Strategic decision-making
At the very beginning of the actual control process, the strategist is tasked with its initialization. Since the initialization stage is performed only once, it is discussed in more detail in Section 6.3. The main effective chore of the strategist is the construction of the sequences of the safe distance sets r] throughout the actual control process; these sequences are communicated to tacticians so as to make them available for their decision-making. At the current composed state x = (x 1 , x 2 , … , x r ), for the construction of the sequences of the safe distance sets { (i,k) (x)} N k=0 , i ∈ ℕ [1:r] , the strategist receives sequences {s (i,k) (x)} N k=0 from each i th tactician, whose collection {{s (i,k) (x)} N k=0 : i ∈ ℕ [1:r] } satisfies postulates of Proposition 2. The strategist utilizes this collection to construct safe distance sets  (i,k) (x) by using direct algebraic operations specified in (3.4) and (3.5). In particular, the strategist constructs, for all i ∈ ℕ [1:r] and all k ∈ ℕ N , safe distance sets  (i,k) (x) represented as where the j th rows E (i, j,k) (x) of the matrices E (i,k) (x) and j th entries The values of (i, j,k) (x) and (i, j,k) (x) are obtained by using relations (3.5), in which each s (i,k) is replaced by s (i,k) (x). Thus, each of the safe distance sets  (i,k) (x) is entirely characterized by the related matrix-vector pair (E (i,k) (x), e (i,k) (x)), which is constructed by simple and direct algebraic operations. At the current composed state x = (x 1 , x 2 , … , x r ), the strategist then communicates the sequences of the safe distance sets { (i,k) (x)} N k=0 , i ∈ ℕ [1:r] to the i th tacticians, that is, it provides the related sequences of the matrix-vector pairs {(E (i,k) (x), e (i,k) (x))} N k=0 , i ∈ ℕ [1:r] to the tacticians.

Tactical decision-making
Each of the i th tacticians, optimizes locally, independently and simultaneously finite horizon N control process d (i,N ) , which is subject to dynamical consistency, stage and terminal as well as term-wise inclusion in safe distance sets constraints. Thus, for each i ∈ ℕ [1:r] and each state x i and related sequence of safe distance sets { (i,k) (x)} N k=0 , these constraints reduce to a set of affine equalities and inequalities, as detailed next systemwise, that is, for each i ∈ ℕ [1:r] . The dynamical consistency is expressed explicitly as The stage constraints are given explicitly as Likewise, the terminal constrains are given explicitly as The term-wise inclusion into safe distance sets (3.8), as already elaborated on in Section 3.3, ensures collection-wise collisionavoidance constraints (2.19). These inclusion constraints take the explicit form given by where u (i,N ) : N −1) ) for the i th system is given, for each i ∈ ℕ [1:r] and any x i ∈ ℝ n i and related sequence of safe distance sets { (i,k) (x)} N k=0 , by and, by definition, it satisfies represent constraints that can be constructed by the i th tactician without guidance of the strategist (the set ℂ (i,N ) (x i )) and with guidance of the strategist (the set (i,N ) (x)). In view of relations (4.3)-(4.6), the set (i,N ) (x) of admissible finite horizon N control processes for the i th system is a closed polyhedral set. Furthermore, for each i ∈ ℕ [1:r] and any x i ∈ ℝ n i and related sequence of safe distance sets { (i,k) (x)} N k=0 , the local decisionmaking process that is solved by the i th tactician is a FHNOC problem (i,N ) (x), which takes the form of a computationally efficient strictly convex QP problem (4.10) Each i th tactician employs MPC (i,N ) (⋅), which is simply given by where u 0 (i,0) (x) is, in this case, unique. The related i th model predictive controlled dynamics are given by As shown in what follows, the composed domain of the proposed convex MPC for collision avoidance, is, in fact, the N -step controllable set N to a terminal constraint set  specified in (2.24) provided that the initialization step is performed as discussed in Section 6.3. The dependence of the sets (i,N ) (⋅) of admissible horizon N control processes d (i,N ) , the value function V 0 (i,N ) (⋅) and its optimizer function d 0 (i,N ) (⋅) and the MPC laws (i,N ) (⋅) on the composed state x = (x 1 , x 2 , … , x r ) is indirect, and it is induced by the dependence of the sequences of the safe distance sets { (i,k) (x)} N k=0 on the composed state x = (x 1 , x 2 , … , x r ). Evidently, the local decision-making processes can be performed without direct knowledge of the composed state x = (x 1 , x 2 , … , x r ). More precisely, the strictly convex quadratic programs (i,N ) (x) can be solved by the i th tacticians as long as they have the knowledge of the current state x i ∈ ℝ n i of the i th system and related sequence of safe distance sets { (i,k) (x)} N k=0 . Thus, it is a subtle point that tacticians have an indirect access to, and make locally an indirect use of, global information (i.e. the composed state x = (x 1 , x 2 , … , x r )), both of which are enabled due to their interaction with, and guidance of, the strategist; namely, the strategist provides related sequence of safe distance sets { (i,k) (x)} N k=0 to each of the i th tacticians. Summa summarum, each i th tactician performs local and independent optimization of an admissible finite horizon N control process d (i,N ) = (x (i,N ) , u (i,N −1) ), implements control action u 0 (i,0) (x) to the i th system, constructs the sequence , and communicates the sequence {s (i,k) (x + )} N k=0 to the strategist in order to enable him to perform his subsequent decision-making at

Interactive strategic-tactical decision-making
The interactive strategic-tactical decision-making represents effectively a dynamically consistent composition of the strategic and tactical decision-making, and it results in a computationally highly efficient convex MPC for collision avoidance. The overall algorithm constructs, at any time instant, the collection {{ (i,k) } N k=0 : i ∈ ℕ [1:r] } of the sequences of the safe distance sets for the i th system by direct algebraic manipulations, and then optimizes independently and simultaneously the predicted finite horizon N control processes by solving resulting strictly convex QP problems (i,N ) (x). Initialization.

Construct sequences of safe distance sets {
for each i ∈ ℕ 5. Optimize predicted finite horizon N control process d (i,N ) by solving strictly convex QP problem (i,N ) (x) specified in (4.10). 6. Implement value of the control law (i,N ) (x) := u 0 (i,0) (x), and obtain x + i = A i x i + B i (i,N ) (x). 7. Generate related sequence of points {s (i,k) (x + )} N k=0 by utilizing (4.13). 8. Communicate the sequence of points {s (i,k) (x + )} N k=0 to the strategist. 9. Set x i = x + i and go to step 2.
Except for initialization, which is discussed in more detail in Section 6.3, the algorithm requires, at any time instant of the actual control process, simple algebraic operations for the strategic decision-making and solutions to a collection of strictly convex QP problems for the tactical decision-making. This is in a stark contrast to the centralized MPC formulation outlined in Section 2.4, which requires a solution to single nonconvex optimization problem (taking the form of the minimization of a quadratic function over a nonconvex closed set). This effectively enhances computational aspects considerably and, in fact, enables a real time implementation of MPC for collision avoidance. The algorithm is marginally more complex compared to the conventional decentralized (and centralized) MPC without collision-avoidance constraints, but the analysis of related system theoretic properties does require a more careful consideration as discussed next.

SYSTEM THEORETIC PROPERTIES
To discuss feasibility, positive invariance, stability, attractivity and consistent improvement properties, we need to slightly generalize standard arguments. More precisely, we need to account adequately for the interaction of the strategic and tactical decision-making processes.

Feasibility and positive invariance
First thing to observe is that, for any composed state x = (x 1 , x 2 , … , x r ) lying in the N -step controllable set N given by (2.24), the actual control process can be initiated. Namely, for any composed state x ∈ N , there exists a collection of admissible finite horizon N control pro-

.1) and (4.2) with properties established in Proposition 2, is feasible.
For each i ∈ ℕ [1:r] , any admissible finite horizon N control process ({x (i,k) . In plain words, each of the tactical decision-making problems, that is, each of the strictly convex QP problems (i,N ) (x) specified in (4.10), is also feasible for any composed state x ∈ N . The unique solutions of the strictly convex QP problems [1:r] . Since the control applied to each of the i th systems in the actual control process is (i,N ) (x) = u 0 (i,0) (x), that is, [1:r] , the feasibility arguments above can be repeated recursively throughout the iterations of the proposed algorithm. To see this point clearly, consider finite horizon N control pro- Assumption 3(v). By definition, the collection of sequences {{s (i,k) (x + )} N k=0 : i ∈ ℕ [1:r] } satisfies postulates of Proposition 2, that is, for each i ∈ ℕ [1:r] , each j ∈ ℕ [1:r] ⧵ {i} and each k ∈ ℕ N , it holds that ‖s (i,k) (x + ) − s ( j,k) (x + )‖ ≥ (i, j ) ; this also ensures that ‖z (i,k) (x + ) − z ( j,k) (x + )‖ ≥ (i, j ) with z (i,k) (x + ) := s (i,k) (x + ) (and z ( j,k) (x + ) := s ( j,k) (x + )) so that collection-wise collision-avoidance constraints (2.19) are satisfied. All in all, the arguments preceding Propositions 3 and 4 can be repeated at x + , which effectively verifies the highly desired positive invariance property of the interactive strategic-tactical decision-making process, that is, convex MPC for collision avoidance. Assumptions 1, 2 and 3 hold. The N -step controllable set N given by (2.24) is a positively invariant set for the composed model predictive controlled dynamics, components of which are given, for each i ∈ ℕ [1:r] , by

Stability and attractivity
The optimized costs at a given composed state x ∈ N are V 0 (i,N ) (x), i ∈ ℕ [1:r] and the above utilized collection of admissible finite horizon N control processes r] } yields the desired cost decrease. As discussed in the previous subsection, the strategic and tactical decision-making processes are feasible. Furthermore, each of the considered finite horizon N control processes ({x (i,k) , and it is guaranteed to be an admissible finite horizon N control process for the optimal control problem (i,N ) (x + ). Thus, each of the optimized costs V 0 (i,N ) (x + ), i ∈ ℕ [1:r] at the composed successor state [1:r] , satisfies the cost decrease condition.

Proposition 6. Suppose Assumptions 1, 2 and 3 hold.
Furthermore, under postulated assumptions, for some strictly positive and finite scalars c (i,1) , i ∈ ℕ [1:r] , and for some strictly positive and finite scalars c (i,2) , i ∈ ℕ [1:r] . With relation (5.1) of Proposition 5 in mind, relations (5.2), (5.3) and (5.4) suffice to conclude asymptotic stability of the composed controlled fixed point x := (x 1 , x 2 , … , x r ) for the composed model predictive controlled dynamics, components of which are given, for each i ∈ ℕ [1:r] , by , with the domain of attraction being the N -step controllable set N . We also observe that, under our assumptions, when N is bounded the upper bounds of (5.4) can be extended to N (possibly relative to different scalars c (i, 3) , i ∈ ℕ [1:r] such that c (i,2) ≤ c (i,3) < ∞ for each i ∈ ℕ [1:r] ), in which case the composed controlled fixed point x = (x 1 , x 2 , … , x r ) is exponentially stable.
, with the domain of attraction being the N -step controllable set N given by (2.24). (The composed controlled fixed point x = (x 1 , x 2 , … , x r ) is actually exponentially stable when N is bounded.)

Consistent improvement
For any composed state x ∈ N , there exists a collection of admissible finite horizon N control processes r] }, and any such collection of admissible finite horizon N control processes can be extended by defining ) as well as u (i,N ) (x) = f i (x (i,N ) (x)) for each i ∈ ℕ [1:r] in order to obtain a collection of admissible finite horizon N + 1 control process Thus, under Assumptions 1-3 and in view of properties established in preceding subsections, the related Nstep controllable sets are monotonically nondecreasing, that is where, by convention, V 0 (i,N ) (x) = ∞ for each i ∈ ℕ [1:r] and all x ∈ N +1 ⧵ N and V 0 (i,0) (x) = V f i (x i ) for each i ∈ ℕ [1:r] and all x ∈ 0 =  1 ×  2 × … ×  r . In layman's terms, increasing the prediction horizon length N comes with the potential for the enlargement of the domain of attraction and improvement of the optimized cost.

Construction of terminal constraint sets
For each i ∈ ℕ [1:r] , the terminal affine state feedbacks can be constructed directly based on relation (2.14) by using linear algebra. An optimal choice is to select matrices K i and P i as the solution of the infinite horizon linear quadratic regulator problem for the system ( , that is, to get these as the solution to the infinite horizon linear quadratic regulator for the quadruple (A i , B i , Q i , R i ). For each i ∈ ℕ [1:r] , given a terminal affine state feedback u i = u i + K i (x i − x i ), the construction of the terminal constraint set  i reduces to the construction of a positively invariant set for the terminal affine dynamics subject to overall state constraints In (6.2),  (i,0) represents stage constraints under terminal affine state feedback Likewise, in (6.2),  (i,0) represents terminal inclusion in the safe distance sets constraints that, by Proposition 1, ensure collection-wise collision-avoidance constraints, and which are constructed relative to the collection of points 0) is the related terminal safe distance set given by where the j th rows E (i, j,0) of the matrices E (i,0) and j th entries , and the values of (i, j,) and (i, j ) are obtained by using relations (A.2), in which each s i is replaced by z i = C i x i + D i u i . Hence, Under Assumption 2, the overall state constraint  (i,0) is a closed polyhedral subset of ℝ n i containing the controlled fixed point x i in its interior. Thus, the construction of the terminal constraint set reduces to a well-understood problem of positive invariance for strictly stable affine dynamics subject to proper polyhedral state constraints, for which efficient computational procedures exist and can be employed directly [25]. An optimal choice for the terminal constraint set is the maximal positively invariant set for , which is obtained as the limit of the standard set iteration and which is, under our assumptions, guaranteed to be finitely determined as well as a bounded closed polyhedral set in ℝ n i containing the controlled fixed point x i in its interior, provided that either terminal dynamics matrix A i + B i K i and matrix composed by concatenation of form an observable matrix pair or  (i,0) is bounded. When K i and P i solve the infinite horizon linear quadratic regulator problem for the system ( , and the terminal constraint set is the maximal positively invariant set for x + i = (A i + B i K i )x i + o i subject to constraints x ∈  (i,0) , the convex MPC for collision avoidance algorithm can be safely terminated when x = (x 1 , x 2 , … , x r ) ∈  1 ×  2 × … ×  r , as, in this case, (i,N ) (x) = u i + K i (x i − x i ) for each i ∈ ℕ [1:r] and all x ∈  1 ×  2 × … ×  r . Furthermore, under such construction, each of tactical decision-making process, that is, each optimization problem (i,N ) (x), needs not be solved when x i ∈  i , since then (i,N ) (x) = u i + K i (x i − x i ).

Optimality improving subiteration
The strategic decision-making is effectively reduced to the construction of the sequences of safe distance sets { (i,k) (x)} N k=0 , which requires merely simple algebraic operations. Furthermore, the tactical decision-making is effectively reduced to solving the optimization problems (i,N ) (x), which are highly structured strictly convex QP problems and, thus, can be solved very efficiently [26]. These two facts allow for a subiteration with the main aim to improve optimality in global sense. This can be attained by modifying steps 2 and 5. of the proposed prototype algorithm for convex MPC for collision avoidance.
Step 2 could be replaced by 2(a). Set j = 0 and, for each i ∈ ℕ [1:r]  Likewise, the step 5. could be replaced by 5(a). Optimize predicted finite horizon N control process d (i,N ) by solving strictly convex QP problem (i,N ) (x) specified in (4.10). 5(b). Set j = j + 1, and V 0 (i,N, j ) (x) = V 0 (i,N ) (x). 5(c). If termination condition holds, set V 0 (i,N ) (x) = V 0 (i,N, j ) (x) and go to step 6. of the main algorithm. Otherwise, update sequence of points , to the strategist and go to step 2(b).
Sensible termination condition for the outlined subiteration could be j ≥ j max for a prescribed maximal number of subiterations j max , or a desired improvement of the optimized costs [1:r] , or combination of the two, for example j ≥ j max or ∀i ∈ ℕ [1:r] Note that, within our setting, such a subiteration produces monotonically nonincreasing sequences of optimized costs, that is, ∀i ∈ ℕ [1:r] , ∀ j ∈ ℕ, V 0 (i,N, j +1) (x) ≤ V 0 (i,N, j ) (x). Since the optimized costs V 0 i,N (⋅), i ∈ ℕ [1:r] are nonnegative and, thus, lower bounded, the sequences of the optimized costs V 0 (i,N, j ) (x) are guaranteed to be convergent sequences.

Initialization step
The most direct, but computationally most intensive, way to initialize the algorithm is to solve a feasibility stage of the optimization problem N (x) specified in (2.23). This approach is acceptable since feasibility stage of the optimization problem (2.23) is considerably simpler than the related optimality stage. More importantly, this step is performed only once at the very beginning of the actual control process. Thus, its computational burden is, in fact, negligible relative to accumulated computational effort of the proposed algorithm. Namely, as already pointed out, all of the remaining steps require throughout actual control process direct algebraic operations (for the strategic decision-making processes) and solutions to computa-tionally highly efficient, standard and strictly convex quadratic programs (for the tactical decision-making processes). An alternative is to devise a dedicated algorithm for the initialization step based on successive relaxation of the collisionavoidance constraints; The conceptual prototype formulation can be obtained along the following lines. 0(a). Start with a collection of finite horizon N control pro- , each of which satisfies stage and terminal constraints. 0(b). Obtain a collection of scalars (i, j ) ∈ [0, (i, j ) ], i ∈ ℕ [1:r] , j ∈ ℕ [1:r] ⧵ {i} for which possibly relaxed collisionavoidance constraints ‖z (i,k) − z ( j,k) ‖ ≥ (i, j ) for each relevant i, j and k hold. 0(c). If collision-avoidance constraints (2.19) hold (i.e. (i, j ) ≥ (i, j ) for each relevant i and j ) go to the step 1. of the main algorithm. Otherwise, construct sequences of safe distance sets { (i,k) } N k=0 with respect to the relaxed collisionavoidance constraints from the step 0(b). (If degeneracy is observed, perturb utilized sequences so as to handle it.) 0(d). Update a collection of finite horizon N control processes ) by constructing each of d (i,N ) (x) to satisfy stage and terminal constraints as well as inclusion into relaxed safe distance sets  (i,k) constraints and for which the related sequence {z (i,k) } N k=0 lies term-wise as deep as possible inside of the corresponding sequence of relaxed safe distance sets  (i,k) (i.e. z (i,k) is as deep as possible inside of  (i,k) for each k). This can be done by solving a linear programming problem. 0(e). Go to step 0(b).
The outlined conceptual prototype algorithm for initialization step is, by construction, guaranteed to be consistently improving. Its concrete formulation is relatively direct. Naturally, its more detailed analysis, including various modifications and enhancements as well as convergence properties and handling of singular geometries, deserves a study in its own right that, however, lies beyond the intended scope and page limitations of this article.

Academic illustration
An academic illustration of the proposed convex MPC for collision avoidance is provided by a multi-agent system example taken from [14]. The system is a collection of six agents given by All of the scalars (i, j ) defining collision-avoidance constraints are identically equal to with concrete value = 0.25. The stage and terminal cost function are defined by (2.10) with Q i = 100I and R i = 100I for i = 1, 2, … , 6. Each of the terminal cost weighting matrices P i and terminal control feedback matrix gains K i are obtained as the solution of the infinite horizon unconstrained optimal control for the system ( , where  (i,0) represents stage constraints under terminal affine dynamics and  (i,0) represents terminal inclusion in the safe distance sets constraints; These are constructed following the procedure described in Section 6.1. The terminal constraint sets  i , i = 1, 2, … , 6 are polytopes with representations specified by (2.8); Each  i is described by 11 affine inequalities. The length of the prediction horizon is N = 7. The termination condition parameters are j max = 10 and i = 0.1 for i = 1, 2, … , 6.
We first demonstrate the efficiency of basic convex MPC proposed in Section 4.3. The first stage, that is, construction of sequences of safe distance sets, of the convex MPC for  Figure 2 by using different levels of gray scale shading, darkness of which increases with the increase of time steps in prediction. The second stage of the convex MPC for collision avoidance is illustrated in Figure 3. The figure is composed of six subfigures, which show optimized position sequences for the considered current composed state x = (x 1 , x 2 , … , x 6 ). Thus, Figure 3 illustrates the optimization of the finite horizon N predicted control process at the current states and related sequences of safe distance sets. The black dash-dotted line depicts the feasible position sequences, while the black regular line depicts the corresponding optimized position sequences. For this example, the feasible and optimized position sequences coincide due to the choice of the terminal cost functions and terminal constraint sets. Namely, since the predicted terminal states of each of agents lie in the interior of the terminal constraint sets, both feasible and optimized finite horizon N state sequences are truncation of length N + 1 of the related infinite horizon optimal state sequences. Such behavior is not necessarily generic, but it does illustrate benefits, and motivates consideration, of the utilized selections of the terminal cost functions and constraint sets.
Figures 4 and 5 depict model predictive controlled position sequences and the corresponding input sequences over time of the considered collection of agents, respectively. As expected in light of the analysis of system theoretic properties of convex MPC for collision avoidance provided in Section 5, these input sequences satisfy stage input constraints, and the state sequences satisfy stage state constraints as well as collision-avoidance constraints, and also converge to the specified controlled fixed points in a stable and exponentially fast manner.
We next compare the basic convex MPC proposed in Section 4.3 and improved convex MPC proposed in Section 6.2 in terms of control performance and computational times. For the considered current composed state, model predictive controlled position sequences generated by these two MPC methods are shown in Figure 6. From a local enlargement of the plot, for this example, it can be seen that position sequences do not coincide. By considering the summation of the costs of all agents over 20 time steps as the control performance index, the performance index of improved convex MPC is reduced by 241.0463, which demonstrates that the subiterations in the improved algorithm play an important role. The computational times are recorded in seconds for online optimization. For each time instant of simulated control processes, the worst-case, average and best-case computational times are 0.0364, 0.0248, 0.0217 s, respectively, for basic convex MPC, and 0.9820, 0.1425, 0.0837 s, respectively, for improved convex MPC, which demonstrates clearly a very high degree of computational efficiency of these two algorithms.

CONCLUSIONS AND EXTENSIONS
This article has reported convex MPC for collision avoidance. The unique feature of the developed algorithm is the utilization of safe distance sets in order to handle nonconvexity induced by collision-avoidance constraints. This feature enables design of convex MPC for collision avoidance, which is computationally efficient, since, except for the initialization step, its implementation reduces to performing simple algebraic operations for the construction of safe distance sets and solving standard and strictly convex QP problems for the optimization of local predicted control processes.
In terms of decision-making architectures, the notion of safe distance sets can be utilized directly as a technique for the design of convex centralized MPC for collision avoidance. The interactive strategic-tactical decision-making structure can be also entirely decentralized. More precisely, the strategist can be removed from the proposed structure at the cost of ensuring information exchange of collection of systems in all-to-all manner. With this modification, tacticians can, in an entirely decentralized fashion, construct sequences of safe distance sets for their systems and optimize their local finite horizon N control processes. The potential drawback of such a modification would be reflected in an increased demand for information exchange and utilization of appropriate communication protocols as well as in an increased local computational effort. Likewise, such modifications can be relatively directly customized for the setting of collection of systems with a priori designed static or dynamic communication networks. In terms of robustness, a highly relevant extension is the development of convex robust MPC for collision avoidance. This extension can be obtained by combining algorithm proposed herein with computationally efficient tube MPC methods [27][28][29][30][31][32]. In terms of class of systems, an equally relevant extension is MPC of nonlinear systems for collision avoidance, which would require a combination of direct algebraic operations and sequential convex QP. In terms of control problems, the considered convex MPC for stabilization can be extended to tracking of a class of admissible references. This is a relatively standard extension in MPC [20,21].

PERMISSION STATEMENT TO REPRODUCE THE MATERIALS FROM THE OTHER SOURCES
None.