ADMM-Based Parallel Optimization for Multi-Agent Collision-Free Model Predictive Control
Zilong Cheng, Jun Ma, Xiaoxue Zhang, Clarence W. de Silva, Tong Heng Lee
11 ADMM-Based Parallel Optimizationfor Multi-Agent Collision-FreeModel Predictive Control
Zilong Cheng, Jun Ma, Xiaoxue Zhang, Clarence W. de Silva,
Fellow, IEEE, and Tong Heng Lee
Abstract —This paper investigates the multi-agent collision-free control problem for medium and large scale systems.For such multi-agent systems, it is the typical situation whereconventional methods using either the usual centralized modelpredictive control (MPC), or even the distributed counterpart,would suffer from substantial difficulty in balancing optimalityand computational efficiency. Additionally, the non-convex char-acteristics that invariably arise in such collision-free control andoptimization problems render it difficult to effectively derive areliable solution (and also to thoroughly analyze the associatedconvergence properties). To overcome these challenging issues,this work establishes a suitably novel parallel computation frame-work through an innovative mathematical problem formulation;and then with this framework and formulation, the alternatingdirection method of multipliers (ADMM) algorithm is presentedto solve the sub-problems arising from the resulting parallelstructure. Furthermore, an efficient and intuitive initializationprocedure is developed to accelerate the optimization process,and the optimum is thus determined with significantly improvedcomputational efficiency. As supported by rigorous proofs, theconvergence of the proposed ADMM iterations for this non-convex optimization problem is analyzed and discussed in detail.Finally, a multi-agent system with a group of unmanned aerialvehicles (UAVs) serves as an illustrative example here to demon-strate the effectiveness and efficiency of the proposed approach.
Index Terms —Multi-agent system, collision avoidance, dis-tributed computation, model predictive control (MPC), alter-nating direction method of multipliers (ADMM), non-convexoptimization.
I. I
NTRODUCTION
The study on multi-agent collision-free control problems hasgreatly progressed in recent times, and there already exist asignificant corpus of substantial works with that central ideaon addressing the control objective of a group of agents to berealized appropriately [1]–[6]. It is also certainly evident thatit would be highly desirable to achieve the control objectivesin these medium and large scale collision-free control prob-lems suitably effectively, and various approaches have been
Z. Cheng, X. Zhang, and T. H. Lee are with the NUS Graduate Schoolfor Integrative Sciences and Engineering, National University of Singapore,Singapore 119077 (e-mail:[email protected]; [email protected];[email protected]).J. Ma is with the Department of Mechanical Engineering, University ofCalifornia, Berkeley, CA 94720 USA (e-mail:[email protected]).Clarence W. de Silva is with the Department of Mechanical Engineering,University of British Columbia, Vancouver, BC, Canada V6T 1Z4 (e-mail:[email protected]).This work has been submitted to the IEEE for possible publication.Copyright may be transferred without notice, after which this version mayno longer be accessible. proposed in the existing literature [7], [8]. One commonlyused method focuses on the implementation of distributedalgorithms [9]–[11], with the purpose of formulating a localoptimization problem from the global optimization target. Itis pertinent to note that, with the distributed algorithms, eachagent can explore the information of the neighbor agents andmake the control decision independently. Therefore, from theviewpoint of the entire system, all agents can be deployed ina distributed manner, which facilitates effective computationin a parallel fashion. Additionally, in this context, the dis-tributed model predictive control (MPC) algorithm has shownpromising effectiveness and efficiency in dealing with thevarious dynamic constraints and objective functions in thesemedium and large-scale collision-free control systems [12]–[14]. However, in such distributed MPC problems, even thoughthe framework exists where the optimum can certainly bereached via coordinating each distributed local optimizationproblem, there is nevertheless no guarantee that this globaloptimality is always attained. Tremendous further efforts havethus been dedicated to minimizing the objective gap betweenthe distributed local optimization problems and the globaloptimization problem [15]–[17]; and additionally too, it is alsorevealed in these studies that the performance of the distributedMPC system could suffer significantly when compared to thecentralized MPC system approach.To cater to the aforementioned drawbacks, a significantamount of newer effort has been put into improving thecomputational efficiency of the optimization schemes for thesemedium and large scale centralized systems, and several morerecent works have demonstrated the attainment of satisfy-ing performance. One particularly noteworthy direction thatresearchers have explored focuses on effective and novelproblem formulation/re-formulation methodologies. In this ap-proach, an appropriate methodology is developed where it isshown that for a group of specific centralized MPC problems,the optimization problems can be readily formulated in aparticular structure, and which can be solved rather efficientlyby various effective existing solvers. For example, in [18],the dual decomposition technique is utilized to decentralizethe multi-agent MPC problem, and then the accelerated prox-imal gradient (APG) method is used to solve the convertedoptimization problem in a parallel manner. Another possibleway would be to aim to improve the computational efficiencyof the MPC solver. Along this line, it is certainly a well-known notion that the traditional MPC problem is essentiallya constrained quadratic optimization problem; and thus various a r X i v : . [ c s . M A ] J a n vident and potential improvements in computational effi-ciency are still possible (if the information in the optimizationproblem that has not been otherwise already excavated canbe further sufficiently explored and utilized). Examples ofsuch efforts are the work in [19], where a new quadratic pro-gramming algorithm is proposed to deal with these large-scaleMPC problems; and also in [20], where a partial enumerationmethod is proposed to solve the large-scale MPC problemsthat are usually intractable with the classical algorithms.Additionally, recent studies on the newer routines pos-sible in the alternating direction method of multipliers(ADMM) [21]–[24] point to new and promising possibili-ties for solving the medium and large scale MPC problem.The principal idea of the ADMM iterations is to decom-pose a large-scale optimization problem into several sub-problems. With a suitable decomposition approach, the sub-problems can be efficiently solved in a parallel framework.The ADMM algorithm has been successfully applied in manyareas such as neural networks [25], image processing [26],optimal control [27], [28], and general optimization problemsolver [29]. Some similar works have been proposed in theexisting literature. In [30], the authors provide a frameworkfor nonlinear process systems based on local subsystem modelinformation in the MPC problem, and in [31], a distributedMPC method via ADMM is proposed for power networkssystem. However, in both works, no non-convex constraintsare considered in the ADMM iterations. Therefore, utilizingsuitably adroitly the idea from the ADMM framework, thispaper proposes a methodology to solve the centralized modelpredictive collision-free control problem in an efficient andeffective parallel manner. The merits present in both thedistributed method and the centralized counterpart are thussuitably combined, while their major shortcomings are effec-tively avoided.The main contributions of this paper are listed as fol-lows: (1) Through an appropriate and suitably innovativemathematical problem formulation, a parallel computationframework is constructed, which facilitates the deploymentof advanced optimization methods to solve the centralizedmodel predictive collision-free control problem efficiently. (2)With the parallelization structure, the ADMM algorithm isinvoked to solve each sub-problem, which leads to significantimprovement of the computational efficiency. (3) Despitethe non-convexity of the optimization problem, an efficientand intuitive initialization procedure is developed to furtheraccelerate the optimization process, where the optimum canbe straightforwardly determined in some specific cases. Fur-thermore, the convergence of the proposed iterations for thenon-convex optimization problem is analyzed with rigorousproofs.The remainder of this paper is organized as follows. SectionII formulates the multi-agent collision-free control problems.The dynamic constraints, and also the collision avoidanceconstraints, are characterized; and subsequently, a non-convexoptimization problem is presented. Section III proposes the useof the ADMM algorithm to solve the formulated optimizationproblem in a parallel manner. In section IV, the convergenceanalysis of the proposed non-convex ADMM iterations is given. In Section V, a multi-agent system with a group ofunmanned aerial vehicles (UAV) is introduced as an exampleto test out (and thus validate) the proposed methodology; andcomparisons between the proposed method and the existingmethods are carried out with supporting simulations. Finally,pertinent conclusions of this work are given in Section VI.II. P ROBLEM S TATEMENT
A. Notations
The following notations are used in the remaining text. diag( · ) denotes the block diagonal operator. A (cid:62) denotes thetranspose of the matrix A . R m × n ( R n ) denotes the real matrixwith m rows and n columns (real column vector with thedimension n ). S n denotes the symmetric matrix space with thedimension n . S n + ( S n ++ ) denotes the real positive semi-definite(positive definite) symmetric matrix with the dimension n .Vector combination notation with respect to column is denotedby ( · ) , i.e., a = ( a , a , · · · , a N ) = (cid:2) a (cid:62) , a (cid:62) , · · · , a (cid:62) N (cid:3) (cid:62) .Operator combination notation is also represented by ( · ) , suchas the operator f including f , f , · · · , f N can be denotedby f = ( f , f , · · · , f N ) . I n represents the identity matrixwith a dimension of n × n . The operator Tr( A ) denotes thetrace of the square matrix A . The operator (cid:104) A, B (cid:105) denotesthe Frobenius inner product, i.e., (cid:104)
A, B (cid:105) = Tr (cid:0) A (cid:62) B (cid:1) for all A, B ∈ R m × n . The norm operator based on the inner productoperator is defined by (cid:107) x (cid:107) = (cid:112) (cid:104) x, x (cid:105) for all x ∈ R m × n . Theoperator (cid:107) x (cid:107) denotes the (cid:96) norm of the vector x . (cid:23) denotesthe generalized inequality symbol (element-wisely greater orequal than). B. Dynamic Constraints
In the MPC problem, a precise system model is usuallynecessary for improving the control performance. Thus, thedynamic constraint is the essential part of an MPC controller.Here, for all τ = t, t + 1 , · · · , t + T − , the linear time-variant(LTI) system dynamics can be expressed by x ( τ + 1) = Ax ( τ ) + Bu ( τ ) , (1)where A ∈ R n × n , B ∈ R n × m are the system state matrix andcontrol input matrix, respectively; x ( τ ) ∈ R n , u ( τ ) ∈ R m arethe system state vector and control input vector, respectively; t is the initial timestamp; T is the prediction horizon. Weassume that the weighting parameters in terms of the systemstate variables and system input variables are time-invariant.To track a given reference signal, the tracking optimizationproblem of each individual agent with a given quadraticobjective function can be formulated as min (cid:0) x ( t + T ) − r ( t + T ) (cid:1) (cid:62) ˆ Q T (cid:0) x ( t + T ) − r ( t + T ) (cid:1) + t + T − (cid:88) τ = t (cid:16)(cid:0) x ( τ ) − r ( τ ) (cid:1) (cid:62) ˆ Q (cid:0) x ( τ ) − r ( τ ) (cid:1) + u ( τ ) (cid:62) ˆ Ru ( τ ) (cid:17) subject to x ( τ + 1) = Ax ( τ ) + Bu ( τ ) τ = t, t + 1 , · · · , t + T − , (2)where ˆ Q ∈ S n + and ˆ R ∈ S m + are the weighting matrices for thesystem state variables and system input variables, respectively; r ( τ ) is the reference signal at the time τ .2ince the initial system state variable x ( t ) cannot be influ-enced by the optimization result, the optimization variables x and u in terms of the system state variables and system inputvariables, respectively, are defined as x = (cid:0) x ( t + 1) , x ( t + 2) , · · · , x ( t + T ) (cid:1) ∈ R nT u = (cid:0) u ( t ) , u ( t + 1) , · · · , u ( t + T − (cid:1) ∈ R mT , (3)and the initial state variable is redefined as x t = x ( t ) toeliminate the confusion of notation in the remaining text. Thenit follows that x = Gx t + Hu, (4)where G = (cid:16) A, A , · · · , A T − , A T (cid:17) ∈ R nT × n H = B · · · AB B · · · A B AB B · · · ... ... ... . . . ... A T − B A T − B A T − B · · · B ∈ R nT × mT . (5)Then the MPC optimization problem (2) can be converted intoa compact form: min x,u ( x − r ) (cid:62) Q ( x − r ) + u (cid:62) Ru subject to x = Gx t + Hu, (6)where r = (cid:0) r ( t + 1) , r ( t + 2) , · · · , r ( t + T ) (cid:1) ∈ R nT Q = diag (cid:16) ˆ Q, ˆ Q, · · · , ˆ Q (cid:124) (cid:123)(cid:122) (cid:125) T − , ˆ Q T (cid:17) ∈ R nT × nT R = diag (cid:16) ˆ R, ˆ R, · · · , ˆ R (cid:124) (cid:123)(cid:122) (cid:125) T (cid:17) ∈ R mT × mT . (7)To deal with the over-actuation problem and formulate theoptimization problem in a more general form, we introducethe Lasso term into the objective function. Besides, in thecommon practice, the box constraints with respect to thesystem state variables and system input variables are alsotaken into consideration in the MPC problem. Then it followsthat the optimization problem (6) can be generalized andrepresented by min x,u ( x − r ) (cid:62) Q ( x − r ) + u (cid:62) Ru + α (cid:107) u (cid:107) subject to x = Gx t + Hux ∈ X , u ∈ U , (8)where X and U denote the box constraints of the system statevariables and system input variables, respectively. The boxconstraints can be denoted by X = { x ∈ R nT | x (cid:22) x (cid:22) x } ,where x ∈ R nT , x ∈ R nT denote the minimum value andmaximum value of the system state variables, respectively; U = { u ∈ R mT | u (cid:22) u (cid:22) u } , where u ∈ R mT , u ∈ R mT denote the minimum value and maximum value of the system input variables, respectively; α is the weighting parameter withrespect to the lasso term.To further simplify the optimization problem, we introducethe definition of the indicator function. Definition 1.
The indicator function with respect to a cone B is defined as δ B ( B ) = (cid:26) if B ∈ B ∞ otherwise . (9)By using the indicator function, we can convert the opti-mization problem to min x,u ( x − r ) (cid:62) Q ( x − r ) + δ X ( x ) + u (cid:62) Ru + δ U ( u ) + α (cid:107) u (cid:107) subject to x = Gx t + Hu. (10)After the formulation of the optimization problem in termsof an individual agent, we generalize the single agent MPCproblem to a multi-agent optimization problem with N agents.Each agent is required to satisfy the aforementioned dynamicconstraint and box constraints. Therefore, the multi-agentMPC optimization problem is represented by min x i ,u i N (cid:88) i =1 (cid:16) ( x i − r i ) (cid:62) Q i ( x i − r i ) + δ X i ( x i )+ u (cid:62) i R i u i + δ U i ( u i ) + α i (cid:107) u i (cid:107) (cid:17) subject to x i = G i x ti + H i u i ∀ i = 1 , , · · · , N, (11)where the subscript i of each variable and parameter denotesthat the corresponding variable and parameter belong to the i th agent. C. Collision Avoidance Constraint
To deal with the multi-agent collision-free control problem,we introduce the collision avoidance constraints [32], [33]into the optimization problem. In order to solve the collisionavoidance problem in an effective and efficient manner, mixedinteger programming (MIP) is utilized [34]–[36], which canbe handled by many existing numerical methods.The MIP collision avoidance problem is indeed a relaxationof the original collision avoidance problem. The intuitiveexplanation of the MIP collision avoidance constraints issimilar to relaxing a ball constraint to a box constraint,i.e., finding a minimal box that contains the given ball. Forexample, in the 2D plane, consider two agents with theposition ( x , y ) and ( x , y ) , respectively. The safe distanceis given as d , and the collision avoidance constraint can bedescribed as ( x − x ) + ( y − y ) ≥ d . If a minimal boxconstraint is considered as the collision avoidance constraint,then the constraints can be described in a linear form, that is ( x − x ) ≥ d − εc , ( x − x ) ≥ d − εc , ( y − y ) ≥ d − εc , ( y − y ) ≥ d − εc , and (cid:80) j =1 c j ≤ , where ε is chosenas a large number, and c , c , c , c are four binary variablesbelonging to the set { , } . The relaxed constraint ensures thateach agent is definitely outside the minimal box constraint of3he other agent. If the binary variable value is , it means thecorresponding linear inequality constraint is trivial. To ensureat least one nontrivial linear inequality constraint exists, i.e.,two agents keep a safe distance, the summation of all binaryvariable values must be less than the total number of the binaryvariables.To formulate the collision avoidance problem in a gen-eral case, for all positive integers i = 1 , , · · · , N , j =2 , , · · · , N, j > i , τ = t, t + 1 , · · · , t + T − , and (cid:96) = 1 , , · · · , , we have the following MIP constraints: E τ(cid:96) ( x i − x j ) ≥ d − εc ijτ(cid:96) (12a) (cid:88) (cid:96) =1 c ijτ(cid:96) ≤ , (12b)where the linear operator E τ(cid:96) , ∀ (cid:96) = 1 , , extracts the positionstate variables in three dimensions x, y, z in the Cartesiancoordinates at the timestamp τ , respectively; the linear op-erator E τ(cid:96) , ∀ (cid:96) = 2 , , extracts the position state variablesin three dimensions x, y, z in the Cartesian coordinates at thetimestamp τ with a negative sign, respectively; d denotes thesafe distances in all three dimensions x, y, z , respectively; ε isa large number given in the MIP problem; c ijτ(cid:96) represents theinteger optimization variables, which introduce the non-convexconstraints into the optimization problem. The inequality con-straint (12b) provides the condition that there is no such a casewhere all the inequality constraints in (12a) are activated.Then we define the vector c ∈ R N ( N − T as a vector in-cluding the c ijτ(cid:96) for all i = 1 , , · · · , N , j = 1 , , · · · , N, j >i , τ = t, t + 1 , · · · , t + T − , (cid:96) = 1 , , · · · , in the ascendingorder, that is c = (cid:16) c t , · · · , c t , · · · ,c t + T − , · · · , c t + T − , · · · ,c ( N − Nt , · · · , c ( N − Nt , · · · ,c ( N − N ( t + T − , · · · , c ( N − N ( t + T − (cid:17) . (13)Then the optimization problem is equivalent to min x i ,u i ,c N (cid:88) i =1 (cid:16) ( x i − r i ) (cid:62) Q i ( x i − r i ) + δ X i ( x i )+ u (cid:62) i R i u i + δ U i ( u i ) + α i (cid:107) u i (cid:107) (cid:17) + δ C ( c )subject to x i = G i x ti + H i u i ,E τ(cid:96) ( x i − x j ) − d + εc ijτ(cid:96) ≥ (cid:88) (cid:96) =1 c ijτ(cid:96) − ≤ ∀ i = 1 , , · · · , N ∀ j = 2 , , · · · , N, j > i ∀ τ = t, t + 1 , · · · , t + T − ∀ (cid:96) = 1 , , · · · , , (14)where C denotes a non-convex cone where only two elements , exist, i.e., C = { , } N ( N − T . To simplify the nota- tions, define the vector y = ( x , u , x , u , · · · , x N , u N , c ) ∈ R ( m + n +3 N − T N , and the sets Y and Y as Y = y = x u x u ... x N u N c (cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12) H i u i − x i + G i x ti = 0 ∀ i = 1 , , · · · , N Y = y = x u x u ... x N u N c (cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12) E τ(cid:96) ( x i − x j ) − d + εc ijτ(cid:96) ≥ (cid:88) (cid:96) =1 c ijτ(cid:96) − ≤ ∀ i = 1 , , · · · , N ∀ j = 2 , , · · · , N, j > i ∀ τ = t, t + 1 , · · · , t + T − ∀ (cid:96) = 1 , , · · · , . (15) Remark 1.
To realize the parallel computation, we separatethe independent constraints and the coupling constraints in themulti-agent system by defining two sets Y and Y . The set Y contains only the constraint belonging to the individual agent,while the set Y contains only the coupling constraints.To denote the optimization problem in a compact form, wedefine the function f i as f i ( x i , u i ) = ( x i − r i ) (cid:62) Q i ( x i − r i ) + δ X i ( x i )+ u (cid:62) i R i u i + δ U i ( u i ) + α i (cid:107) u i (cid:107) . (16)Then we define the operator f : R ( m + n +3 N − T N → R N +1 and g : R ( m + n +3 N − T N → R N +1 , which are represented by f ( y ) = (cid:0) f , f , · · · , f N , (cid:1) ( y ) ,g ( z ) = (cid:0) , , · · · , (cid:124) (cid:123)(cid:122) (cid:125) N , δ C (cid:1) ( z ) , (17)where ( · ) denotes the zero operator in the Hilbert space.Finally, the multi-agent optimization problem is formulated as min y,z (cid:62) f ( y ) + δ Y ( y ) + (cid:62) g ( z ) + δ Y ( z )subject to y − z = 0 , (18)where z ∈ R ( m + n +3 N − T N is the consensus variable; denotes the vector with an appropriate dimension and allentries as .III. O PTIMIZATION FOR P ARALLEL
MPCTo solve the problem by ADMM, we introduce the aug-mented Lagrangian function, which can be denoted by L σ ( y, z ; λ ) = (cid:62) f ( y ) + δ Y ( y )+ (cid:62) g ( z ) + δ Y ( z ) + (cid:104) λ, y − z (cid:105) + σ (cid:107) y − z (cid:107) = (cid:62) f ( y ) + δ Y ( y ) + (cid:62) g ( z ) + δ Y ( z )+ σ (cid:107) y − z + σ − λ (cid:107) − σ (cid:107) λ (cid:107) , (19)4here λ = (cid:0) λ , λ , · · · , λ N , λ c (cid:1) ∈ R ( m + n +3 N − T N is theLagrangian multiplier; σ is the penalty parameter of ADMM.Then it is straightforward to see that the ADMM algorithmcan be denoted by y k +1 = argmin y L σ (cid:16) y, z k ; λ k (cid:17) (20a) z k +1 = argmin z L σ (cid:16) y k +1 , z ; λ k (cid:17) (20b) λ k +1 = λ k + σ (cid:16) y k +1 − z k +1 (cid:17) , (20c)where the superscript k denotes the optimization variable inthe k th iteration. The stopping criterion for the iterations canbe chosen as (cid:15) pri ≤ ˆ (cid:15) , where (cid:15) pri is the primal residual error (cid:15) pri = (cid:107) y − z (cid:107) . A. First Sub-Problem in ADMM
The first sub-problem (20a) in the ADMM iterations canbe calculated in a parallel manner, which means that for eachagent i , we have (cid:16) x k +1 i , u k +1 i (cid:17) = argmin x i ,u i (cid:26) f i ( x i , u i ) + σ (cid:13)(cid:13)(cid:13) ( x i , u i ) − z ki + σ − λ ki (cid:13)(cid:13)(cid:13) (cid:12)(cid:12)(cid:12)(cid:12) H i u i − x i + G i x ti = 0 (cid:27) , (21)where z is separated and denoted by z = ( z , z , · · · , z N , z c ) and z ∈ R ( m + n ) T × ( m + n ) T ×··· ( m + n ) T × N ( N − T . It followsthat for each agent i , the following optimization problem issolved: min ( x i − r i ) (cid:62) Q i ( x i − r i ) + u (cid:62) i R i u i + α i (cid:107) u i (cid:107) + σ (cid:13)(cid:13)(cid:13) ( x i , u i ) − z ki + σ − λ ki (cid:13)(cid:13)(cid:13) subject to H i u i − x i + G i x ti = 0 x i (cid:22) x i (cid:22) x i u i (cid:22) u i (cid:22) u i . (22)It is straightforward to see that the the sub-problem (22)is a constrained quadratic optimization problem with a lassoterm, which can be solved by many existing approaches, suchas semi-proximal ADMM. However, in this paper, we areprone to solve the dual problem of the proposed optimizationproblem, because it can be formulated as a typical inequalityconstrained quadratic optimization problem, which can besolved apparently faster than the primal problem.For simplicity, define β i = z ki − σ − λ ki , ξ i = ( x i , u i ) , and the following notations: Q i = diag( Q i , R i ) + σ I ( m + n ) T ∈ S ( m + n ) T q i = − (cid:16) Q i r i , mT (cid:17) − σβ i ∈ R ( m + n ) T P = [0 mT × nT I mT ] ∈ R mT × ( m + n ) T M i = [ − I nT , H i ] ∈ R nT × ( n + m ) T m i = − G i x ti ∈ R nT ξ i = ( x i , u i ) ∈ R ( m + n ) T ξ i = ( x i , u i ) ∈ R ( m + n ) T N i = (cid:0) I ( m + n ) T , − I ( m + n ) T (cid:1) ∈ R m + n ) T × ( m + n ) T n i = (cid:16) ξ i , − ξ i (cid:17) ∈ R m + n ) T . (23)Then the problem (22) is converted to min ξ (cid:62) i Q i ξ i + q (cid:62) i ξ i + α i (cid:107) p i (cid:107) subject to M i ξ i = m i N i ξ i (cid:22) n i P ξ i = p i . (24)Notably, the vectors m i and n i are constant vectors but thevector p i is a variable in the optimization problem. Assumption 1.
Problem (24) is strictly feasible.Under Assumption 1, the Slater’s condition is satisfied.Therefore, strong duality always holds for the proposed op-timization problem, and the optimal solution to the linearquadratic optimization problem (24) can be obtained by solv-ing the corresponding dual problem, due to the difficulty todeal with the primal problem directly.Then it follows that the dual problem of the proposedoptimization problem can be derived. To find the dual problem,the Lagrangian function with respect to the sub-problem isdenoted by L i ( ξ i , p i ; µ i , µ i , µ i ) = ξ (cid:62) i Q i ξ i + q (cid:62) i ξ i + α i (cid:107) p i (cid:107) + (cid:10) µ i , M i ξ i − m i (cid:11) + (cid:10) µ i , N i ξ i − n i (cid:11) + (cid:10) µ i , P ξ i − p i (cid:11) , (25)where µ i ∈ R nT , µ i ∈ R m + n ) T , and µ i ∈ R mT arethe Lagrangian dual variables. Then the dual problem can bedenoted by max µ i ,µ i ,µ i min ξ i ,p i L i ( ξ i , p i ; µ i , µ i , µ i )subject to µ i (cid:23) . (26)In the following text, the parameter of the Lagrangianfunction is ignored for the simplicity of the notation. Then theminimization of the Lagrangian function L i can be separatedinto two parts: the minimization with respect to ξ i and theminimization with respect to p i : min ξ i ,p i L i = min ξ i (cid:110) ξ (cid:62) i Q i ξ i + q (cid:62) i ξ i + (cid:10) µ i , M i ξ i (cid:11) + (cid:10) µ i , N i ξ i (cid:11) + (cid:10) µ i , P ξ i (cid:11)(cid:111) + min p i (cid:110) α i (cid:107) p i (cid:107) − (cid:10) µ i , p i (cid:11)(cid:111) − (cid:10) µ i , m i (cid:11) − (cid:10) µ i , n i (cid:11) . (27)5ince the first part in (27) is an unconstrained optimizationproblem, and thus its optimum can be determined by settingthe gradient to be zero, that is Q i ˆ ξ i + q i + M (cid:62) i µ i + N (cid:62) i µ i + P (cid:62) µ i = 0 , (28)where ˆ ξ i denotes the optimal ξ i in the optimization problem.Therefore, we have ˆ ξ i = − Q − i (cid:16) q i + M (cid:62) i µ i + N (cid:62) i µ i + P (cid:62) µ i (cid:17) . (29)Notably, because the matrices Q i and R i are positive semi-definite, the matrix Q i is non-singular.To find the minimization solution of the second part in (27),we introduce the definition of the conjugate function. Definition 2.
The conjugate function of a function f is definedas f ∗ ( y ) = sup {(cid:104) x, y (cid:105) − f ( x ) } . (30)Define h ( p i ) = α i (cid:107) p i (cid:107) . Then it is straightforward to seethat the second part in (27) is the conjugate function of h , which can be denoted by h ∗ ( µ i ) . It is well-known theconjugate function of a norm function is an indicator functionwith respect to the corresponding dual norm ball, that is h ∗ ( µ i ) = (cid:26) (cid:107) µ i (cid:107) ∞ ≤ α i ∞ otherwise . (31)Then we have min ξ i ,p i L i = − (cid:16) q i + M (cid:62) i µ i + N (cid:62) i µ i + P (cid:62) µ i (cid:17) (cid:62) Q − i (cid:16) q i + M (cid:62) i µ i + N (cid:62) i µ i + P (cid:62) µ i (cid:17) − h ∗ ( µ i ) . (32)Subsequently, the dual problem (26) is given by min µ i ,µ i ,µ i (cid:16) q i + M (cid:62) i µ i + N (cid:62) i µ i + P (cid:62) µ i (cid:17) (cid:62) Q − i (cid:16) q i + M (cid:62) i µ i + N (cid:62) i µ i + P (cid:62) µ i (cid:17) + (cid:10) µ i , m i (cid:11) + (cid:10) µ i , n i (cid:11) subject to µ i (cid:23) (cid:107) µ i (cid:107) ∞ ≤ α i . (33)Define the following vectors and matrices: µ i = ( µ i , µ i , µ i ) ∈ R m + n ) T Ω i = (cid:16) M i , N i , P (cid:17) ∈ R m + n ) T × ( m + n ) T ω i = (cid:16) m i , n i , mT (cid:17) ∈ R m + n ) T Φ i = (cid:110) µ i = ( µ i , µ i , µ i ) (cid:12)(cid:12)(cid:12) µ i (cid:23) , (cid:107) µ i (cid:107) ∞ ≤ α i (cid:111) . (34)It follows that the optimization problem can be denoted in acompact form: min µ i (cid:16) Ω (cid:62) i µ i + q i (cid:17) (cid:62) Q − i (cid:16) Ω (cid:62) i µ i + q i (cid:17) + (cid:10) ω i , µ i (cid:11) subject to µ i ∈ Φ . (35)It is obvious that the optimization problem (35) is aninequality constrained quadratic optimization problem, which has been widely explored and can be solved much moreefficiently than the primal optimization problem (24).After the solution to the dual problem is obtained, theoptimal ξ can be calculated by ξ ∗ i = − Q − i (cid:16) Ω (cid:62) i µ ∗ i + q i (cid:17) , (36)where µ ∗ i denotes the value of the corresponding vector whenthe stopping criterion is reached.After that, each agent sends the latest optimal value x k +1 i and u k +1 i to the other agents, and in each agent, the followingupdating rule is performed: c k +1 = z kc − σ − λ kc . (37) B. Second Sub-Problem in ADMM
The second sub-problem (20b) in the main ADMM itera-tions can be represented by min z δ C ( z c ) + σ (cid:13)(cid:13)(cid:13) z − y k +1 − σ − λ k (cid:13)(cid:13)(cid:13) subject to E τ(cid:96) ( z xi − z xj ) + εz c,ijτ(cid:96) − d ≥ − (cid:88) (cid:96) =1 z c,ijτ(cid:96) ≥ ∀ i = 1 , , · · · , N ∀ j = 2 , , · · · , N, j > i ∀ τ = t, t + 1 , · · · , t + T − ∀ (cid:96) = 1 , , · · · , , (38)where the location of z c,ijτ(cid:96) in the vector z is the same as thelocation of c ijτ(cid:96) in the vector y .Define the new linear operator E ijτ(cid:96) : R [ m + n +3 N − T N → R as E ijτ ( z ) = − (cid:88) (cid:96) =1 z c,ijτ(cid:96) E ijτ(cid:96) ( z ) = E τ(cid:96) ( z xi − z xj ) + εz c,ijτ(cid:96) ∀ i = 1 , , · · · , N ∀ j = 2 , , · · · , N, j > i ∀ τ = t, t + 1 , · · · , t + T − ∀ (cid:96) = 1 , , · · · , , (39)and the linear operator E : R [ m + n +(13 / N − (13 / T N → R (7 / N ( N − T that contains all the linear operator E ijτ(cid:96) for all i = 1 , , · · · , N , j = 2 , , · · · , N, j > i , τ = t, t + 1 , · · · , t + T − , (cid:96) = 0 , , · · · , in the ascending order. Also, define thevector E c ∈ R (7 / N ( N − T as E c = (cid:16) , − d, − d, · · · , − d (cid:124) (cid:123)(cid:122) (cid:125) , · · · , , − d, − d, · · · , − d (cid:124) (cid:123)(cid:122) (cid:125) (cid:124) (cid:123)(cid:122) (cid:125) There are (1 / N ( N − T repetition (cid:17) . (40)Further define the constant γ k in the k th iteration as γ k = y k +1 + σ − λ k , (41)6nd then the optimization problem can be represented by min z (cid:13)(cid:13)(cid:13) z − γ k (cid:13)(cid:13)(cid:13) + δ C ( z c )subject to E ( z ) + E c (cid:23) . (42)Remarkably, (42) is a constrained least square optimizationproblem with integer optimization variables, and feasible so-lutions can be readily obtained by many existing methods. C. Initialization of Optimization Variables
Since the problem (18) is a non-convex optimization prob-lem, it suffers from NP-hardness to find the global optimum.However, some intuitive initialization procedures are straight-forward to be implemented to accelerate the optimization pro-cess. To introduce the initialization procedures, the followingmatrices are introduced: E ijτ(cid:96) ( z ) = E τ(cid:96) ( z xi − z xj ) ∀ i = 1 , , · · · , N ∀ j = 2 , , · · · , N, j > i ∀ τ = t, t + 1 , · · · , t + T − ∀ (cid:96) = 1 , , · · · , . (43)Then define the linear operator E : R [ m + n +3 N − T N → R N ( N − T , which contains all the linear operator E ijτ(cid:96) for all i = 1 , , · · · , N , j = 2 , , · · · , N, j > i , τ = t, t + 1 , · · · , t + T − , (cid:96) = 1 , · · · , in the ascending order. Notably, the linearoperator E gives the information of the distances betweendifferent agents.In order to determine the initial optimization variables, wecan use the linear operator E to find a feasible vector z c . Tobe more specific, if the distance between a pair of agents in adirection is greater or equal than the safe distance d , then thecorresponding entry in the vector z c will be , and otherwiseit will be . With this technique, the initial parameter can bedetermined easily. Remarkably, Remark 2.
Based on the proposed initialization procedures,the optimum can be reached for the sub-problem (20b) directly,under the condition that each pair of agents maintains asafe distance. This is because the vector z c is chosen as afeasible solution with the proposed initialization procedures.It is also pertinent to note that if a safe distance is notmaintained initially, the proposed initialization procedures willfail because the initial choice of the vector z c will not befeasible. Remark 3.
The control law of the proposed model pre-dictive controller is the optimal solution to the optimizationproblem (18) in a receding time window, and the controllerparameters are always changed at each timestamp. To be morespecific, a receding horizon control (RHC) structure based onthe solution of the MPC problem is utilized in the proposedalgorithm.
Remark 4.
In terms of the stability of the closed-loop system,there are many existing literature that reports the results on stability of the typical model predictive controller such as [15],[37]–[39].The proposed parallel optimization method is summarizedas Algorithm 1.
Algorithm 1
ADMM for Parallel MPC Problem
Input: weighting matrices for each agent Q i and R i ; boxconstraint parameter vectors for each agent x i , x i , u i , u i ;reference signal for each agent r i ; initial system statevector for each agent x i ; penalty parameter σ in theaugmented Lagrangian function; prediction horizon T ;number of total timestamps T t ; maximum ADMM iter-ation number K ; stopping criterion ˆ (cid:15) . for t = 0 , , · · · , T t do Choose the initial optimization variable y . for k = 0 , , · · · , K do Update the optimization variable y k +1 by (20a) in aparallel manner. Update the consensus variable z k +1 by (20b). Update the Lagrangian multiplier λ k +1 by (20c). Calculate (cid:15) pri . if (cid:15) pri ≤ ˆ (cid:15) then Update the optimal control input vector for eachagent u ∗ i from the vector z k +1 . break end if end for Apply the control input signal at the first timestamp in u ∗ i to the system dynamic model for each agent. Calculate the initial state vector for each agent x t +1 i . end for return feasible trajectory for each agent x i , x i , . . . , x T t i .IV. C ONVERGENCE A NALYSIS
In this section, the proof of the convergence with theproposed ADMM iterations is given. Also, a new ADMM iter-ations scheme is applicable to solve the proposed optimizationproblem. Analysis and relationship between the two schemesare discussed after the convergence proof.
A. Convergence Analysis for the Proposed ADMM Iterations
To analyze the convergence of the optimization prob-lem (18) and simplify the notations, we define the objectivefunction of the optimization problem (18) as φ ( y, z ) and thenseparate φ ( y, z ) into two parts ϕ ( y ) , ψ ( z ) , where φ ( y, z ) = (cid:62) f ( y ) + δ Y ( y ) + (cid:62) g ( z ) + δ Y ( z ) ϕ ( y ) = (cid:62) f ( y ) + δ Y ( y ) ψ ( z ) = (cid:62) g ( z ) + δ Y ( z ) (44)Before we analyze the convergence of the proposed non-convex ADMM, the following definition of the coercive func-tion is given. Definition 3.
If a function φ ( y, z ) is coercive over a set F ,then φ ( y, z ) → ∞ if ( y, z ) ∈ F and (cid:107) ( y, z ) (cid:107) → ∞ .7hen we propose the following lemma to claim that theobjective function φ ( y, z ) is coercive in terms of the equalityconstraint. Lemma 1.
Define a feasible set F = { ( y, z ) ∈ R ( m + n +3 N − T N × R ( m + n +3 N − T N | y − z = 0 } . In thefeasible set F , the objective function φ ( y, z ) is coercive. Proof.
Since the function f ( y ) = (cid:0) f , f , · · · , f N , (cid:1) ( y ) andeach function f i ( x i , u i ) contain the linear quadratic termwith a positive definite weighting matrix, it is straightfor-ward that the function f ( y ) is coercive over the domain R ( m + n +3 N − T N . Then it follows that φ ( y, z ) is coerciveover the set F because y = z . This completes the proof ofLemma 1.The following lemma proposes the property of the Lip-schitz sub-minimization paths in terms of the optimizationproblem (18). Lemma 2.
The following statements hold for (46):(a) For any fixed y , the problem argmin z { φ ( y, z ) | z = u } hasa unique minimizer, and u (cid:55)→ argmin z { φ ( y, z ) | z = u } is a Lipschitz continuous map.(b) For any fixed z , the problem argmin y { φ ( y, z ) | y = u } hasa unique minimizer, and u (cid:55)→ argmin y { φ ( y, z ) | y = u } is a Lipschitz continuous map.(c) The two maps in Statement (a) and (b) have a universalLipschitz constant ¯ M , and ¯ M > . Proof.
Since in the equality constraint y = z , each coefficientmatrix of the optimization variable has full column rank, thenull spaces of both coefficient matrices are trivial. Therefore,the two maps in Statement (a) and (b) are linear operators.This completes the proof of Lemma 2.To explore the property of the funtion ψ ( z ) , we introducethe definition of the semi-continuous function. Definition 4.
A function φ is semi-continous at x if for all y such that y < φ ( x ) , there exists a neighbourhood X of x such that y < φ ( x ) for all x in X . A function φ is semi-continuous if φ is semi-continuous at every point in the domainof φ . Lemma 3.
The function ψ ( z ) is lower semi-continuous. Proof.
According to the definition of the function g ( z ) , it is anindicator function of the closed discrete set { , } N − NT .Therefore, ψ ( z ) is an indicator function of a closed set. It iscommon that the indicator function of a closed set is lowersemi-continuous. This completes the proof of Lemma 3.Next, the definition of the restricted prox-regularity is firstlyintroduced to generalized the definition of the Lipschitz differ-entiable function. Then the definition of the Lipschitz differen-tiable function is presented and generalized to be applied to theproposed optimization problem. Since the indicator functionis included in the function ϕ ( z ) , it leads to the difficultly inanalyzing the properties of the function ϕ ( z ) . Definition 5.
Define the lower semi-continuous function ϕ ( z ) : R ( m + n +3 N − T N → R ∪ {∞} , M > , and define the set H = { z ∈ dom( ϕ ) | (cid:107) d (cid:107) > M, ∀ d ∈ ∂ϕ ( z ) } . Then ϕ ( z ) is restricted prox-regular if for any M > , and bounded set T ⊆ dom( ϕ ) , there exists (cid:36) > such that φ ( z )+ (cid:36) (cid:107) x − z (cid:107) ≥ ϕ ( x ) + (cid:104) d, z − x (cid:105) , ∀ z ∈ T \ H , x ∈ T, d ∈ ∂ϕ ( z ) , (cid:107) d (cid:107) ≤ M . Definition 6.
A function ϕ ( z ) is Lipschitz differentiable if thefunction ϕ ( z ) is differentiable and the gradient is Lipschitzcontinuous, and a function ϕ ( z ) is Lipschitz differentiable ifthe function ϕ ( z ) is restricted prox-regular. Lemma 4.
The function ϕ ( z ) is Lipschitz differentiable withthe constant L ϕ . Proof.
For each i , the function f i ( x i , u i ) contains the linearquadratic term f lini , indicator function of the set f indi , and alasso term f lasi . Then it follows that ∂f lini = 2 Q i ( x i − r i ) + 2 R i u i , (45)which is Lipschitz continuous and the Lipschitz constant isthe largest eigenvalue of the matrix Q i . The lasso term f lasi = α i (cid:107) u i (cid:107) is a well-known Lipschitz differentiable func-tion with the Lipschitz constant α i . The indicator function ofthe compact set f indi = δ X i ( x i )+ δ U i ( u i ) satisfies Definition 5,and therefore it is restricted prox-regular. Then it followsthat f indi is Lipschitz continuous according to Definition 6.Similarly, the indicator function δ Y ( y ) is also restricted prox-regular. Since the sum of Lipschitz continuous functions isalso Lipschitz continuous, the function ϕ ( z ) is thus Lipschitzcontinuous. Then It completes the proof of Lemma 4. Theorem 1.
The ADMM iterations (20) converge to the limitpoint for any sufficiently large σ . Equivalently, given anystarting point ( y , z , λ ) , the proposed ADMM iterations (20)provide a bounded sequence, which converges to the limitpoint ( y ∗ , z ∗ , λ ∗ ) . The converged limit point ( y ∗ , z ∗ , λ ∗ ) isa stationary point of the augmented Lagrangian function L σ ( y, z ; λ ) , that is ∈ ∂ L σ ( y, z ; λ ) . Proof.
From Lemma 1-4 and the fact that the equality con-straint y = z is feasible, the convergence assumptionsproposed in [40] are satisfied. This completes the proof ofTheorem 1. Remark 5.
From Theorem 1, it is obvious that a sufficientlylarge penalty parameter in the augmented Lagrangian functioncan guarantee the local convergence. However, if σ is chosenas a very large value, the convergence of the proposed algo-rithm will be significantly impeded. B. Discussion on the Convergence of ADMM Iterations inOther Forms
In this section, we propose a new structure of the ADMMiterations in terms of the parallel model predictive collision-free control problem, and the relationship between the newstructure and the former structure is given. Before we intro-duce the new ADMM iterations, Theorem 2 is proposed toclaim the equivalence of the two optimization problems.8 heorem 2.
The optimization problem (18) is equivalent tothe following optimization problem: min (cid:62) f ( y ) + (cid:62) g ( z ) + δ Y ( z )subject to H y − G z = F , (46)where the matrices H ∈ R (2 n + m +3 N − NT × ( n + m +3 N − and G ∈ R (2 n + m +3 N − NT × ( n + m +3 N − are defined as H = (cid:18)(cid:104) diag (cid:0) [ − I nT , H ] , · · · , [ − I nT , H N ] (cid:124) (cid:123)(cid:122) (cid:125) N (cid:1) , N ( N − T × N ( N − T (cid:105) , I ( n + m +3 N − NT (cid:19) G = (cid:16) nNT × ( n + m +3 N − NT , I ( n + m +3 N − NT (cid:17) F = (cid:16) − G x t , − G x t , · · · , − G N x tN , N − NT (cid:17) . (47) Proof.
The indicator function δ Y ( y ) of dynamic constraintsin the first part of the problem (18) is combined into the matrix H . The equivalent constraint y = z is also included into thematrices H and G by introducing two identity matrices. Thiscompletes the proof of Theorem 2.Define the augmented Lagrangian function ˆ L σ with respectto the optimization problem (46) as ˆ L σ (cid:16) y, z ; ˆ λ (cid:17) = (cid:62) f ( y ) + (cid:62) g ( z ) + δ Y ( z )+ σ (cid:107) H y − G z − F + σ − ˆ λ (cid:107) − σ (cid:107) ˆ λ (cid:107) . (48)where ˆ λ k ∈ R (2 n + m +3 N − NT denotes the Lagrangian mul-tiplier. Then the ADMM iterations in terms of the augmentedLagrangian function (48) are given by y k +1 = argmin y ˆ L σ (cid:16) y, z k ; ˆ λ k (cid:17) z k +1 = argmin z ˆ L σ (cid:16) y k +1 , z ; ˆ λ k (cid:17) ˆ λ k +1 = ˆ λ k + σ (cid:16) H y k +1 − G z k +1 − F (cid:17) . (49)Even though Theorem 2 shows that the two optimizationproblems are equivalent, it is not trivial to prove that the cor-responding ADMM iterations are equivalent. Actually, the op-timization variables in the iterations (20) are the approximatesolutions to the optimization variables in the iterations (49) bytransferring the dynamic constraints and box constraints intothe objective function.It is obvious that the ADMM iterations (49) is also straight-forward to apply, and the convergence rate should also be thefirst-order convergence due to the nature of the ADMM algo-rithm. However, it is noteworthy to mention that the formerADMM iterations have less optimization variables and thusit can save the memory during the algorithm implementation.Experimental results also demonstrate faster convergence withthe former ADMM iterations. Therefore, the ADMM iterationsof the problem (18) are applied in this paper.V. I LLUSTRATIVE E XAMPLE
In this section, an illustrative example is introduced topresent the performance of the proposed parallel model pre-dictive collision-free control method.
A. UAV Modeling
To clearly illustrate the UAV modeling, the notations in thissection are not related to the notations used in the previoussections. Assume that aerodynamic and gyroscopic effects arenot considered in the dynamic model. Then it follows that thedynamic model of the quadcopter can be denoted by ˙ p ( t ) = R ( φ, θ, ψ ) (cid:62) v ( t )˙ v ( t ) = −(cid:104) ω ( t ) , v ( t ) (cid:105) + gR ( φ, θ, ψ ) e + 1 m eT t ˙ ζ ( t ) = W ( φ, θ, ψ ) ω ( t )˙ ω ( t ) = J − (cid:16) (cid:104)− ω ( t ) , Jω ( t ) (cid:105) + τ (cid:17) , (50)where p = ( p x , p y , p z ) is the position state vector; R ( φ, θ, ψ ) and W ( φ, θ, ψ ) are the rotation matrices; v = ( v x , v y , v z ) is the velocity state vector; ω = ( ω x , ω y , ω z ) is the angularvelocity state vector; g is the acceleration of gravity; m is thetotal mass of the helicopter; e = (0 , , is the standard basisvector; T t denotes the total thrust; ζ = ( φ, θ, ψ ) is the anglestate vector; τ = ( τ x , τ y , τ z ) represents the torques of thequadcopter in each dimension; J = diag ( J x , J y , J z ) denotesthe moment of the inertia of the quadcopter. Notice that therotation matrices are represented by R ( φ, θ, ψ ) = c θ c ψ c θ s ψ − s θ s θ c ψ s φ − s ψ c φ s θ s ψ s φ + c ψ c φ c θ s φ s θ c ψ c φ + s ψ s φ s θ s ψ c φ − c ψ s φ c θ c φ W ( φ, θ, ψ ) = s φ t θ c φ t θ c φ − s φ s φ sc θ c φ sc θ . (51)where the functions sin( · ) , cos( · ) , tan( · ) , sec( · ) are repre-sented by c , s , t , sc for the sake of simplicity.Assume that the dynamics of the rotor are ignored. Thenthe vertical forces are directly considered as the system inputs,i.e., u = ( F , F , F , F ) , and a moment perpendicular to theplane of the propeller rotation is necessary to be considered.Then we have the following equation: T t τ τ τ = − − − − − L LL − L − c c − c c F F F F , (52)where L is the distance between the rotor and the center of thequadcopter; c is the distance between the center of the angularmomentum and the center of the vertical force. To denote thedynamic model in a compact manner, we define the systemstate vector as x = ( p, v, ζ, ω ) . To ensure the stable point isaround the original point, define u = u eq + δu , where u eq =( mg/ , mg/ , mg/ , mg/ is used to offset the gravity.Then it follows that the dynamic model of the quadcopter canbe represented by ˙ p ˙ v ˙ ζ ˙ ω = R ( φ, θ, ψ ) (cid:62) v −(cid:104) ω, v (cid:105) + gR ( φ, θ, ψ ) eW ( φ, θ, ψ ) ωJ − (cid:0) − (cid:104) ω, Jω (cid:105) (cid:1) + B ( u eq + δu ) , (53)9 z Fig. 1. Initial and final positions of the agents in the yz -plane. where the matrix B is given by B = − × (cid:2) m m m m (cid:3) × LJ x − LJ x − LJ y LJ y γJ z − γJ z γJ z − γJ z , (54)where γ is the ratio of the rotor angular momentum.To guarantee the feasibility of the MPC problem, the boxconstraints on the system state variables and system inputvariables are required to be considered. In the proposed model,the system input δu is restricted in a specific range, which is δu min ≤ δu ≤ δu max , (55)and the box constraints on the system state variables are givenby − π ≤ φ ≤ π − π ≤ ψ ≤ π − π ≤ θ ≤ π . (56)Since the system (53) is nonlinear and time-variant, a state-dependent coefficient factorization method [41] is used tohandle and address the nonlinear dynamics. The resultingstate-space expression can be expressed in a linear time variant(LTV) dynamic model. Since a RHC law is utilized, and thelinear dynamic matrices A and B are dependent on the currentstate x , we can suitably consider the system matrices to beconstant during the prediction horizon. B. Simulation Result
In this section, a multi-UAV system with 21 agents( N = 21 ) is utilized to demonstrate the performance ofthe proposed optimization method. The agents are requiredto track the given reference considering all the given con-straints, which means that the system state variables and Fig. 2. Trajectories of the agents in the 3D view.Fig. 3. Trajectories of the agents in the 2D view. control input variables must be restricted in a feasible rangeand the safe distance must be guaranteed for each pairof agents. The parameters of the system model and theoptimization problem are given as follows: The limitationsof the control input variables are chosen as δu min =( − . N , − . N , − . N , − . N ) and δu max =(1 . N , . N , . N , . N ) ; the prediction horizon ischosen as T = 25 ; the sampling time is chosen as ∆ t = 0 . s; the safety distance between each pair of agents is chosenas d = 0 . m; for the agent i , the weighting matrices arechosen as Q i = diag( I , × ) and R i = 0 × , which meansthat only the position error is considered in the quadraticobjective function; the stopping criterion of the proposedADMM iterations is chosen as ˆ (cid:15) = 0 . . The simulationsare conducted in the environment of Python 3.7 with twoprocessors Intel(R) Xeon(R) CPU E5-2695 v3 @ 2.30GHz.Both the constrained quadratic optimization problem and theMIP problem are solved by GUROBI.Fig. 1 shows the initial and final positions of each agentin the yz -plane. There are four key points for each agent to10 Fig. 4. Control inputs of the agents. track. The initial points, which are different for each agent,are chosen in a square with an equal interval of m inthe yz -plane and − on the x -axis. The second point andthe third point, which are the same for all the agents, arechosen as ( − , − , − and (1 , , , respectively. If there isno collision avoidance constraint, all the agents will pass thepoints ( − , − , − and (1 , , along the reference. The finalpoints are chosen as the points with the same position as theinitial points in the yz -plane and on the x -axis.In Fig. 2, the trajectory of each agent is shown in the3D view. It demonstrates that each agent can track the keypoint successfully and avoid the collision from each other.Fig. 3 presents the 2D views of the trajectory, where theleft sub-figure shows the top view of the trajectory, and theright one is the side view of the trajectory. Due to the non-holonomic property and the system input constraints of theUAV system, the trajectory of each agent is much smootherand more realistic compared to the given reference trajectory.Both Fig. 2 and Fig. 3 clearly illustrate the effectiveness ofthe proposed method. The control input variables for eachagent are depicted in Fig. 4, where δu , δu , δu , and δu are the control input signals for the four rotors of each agent,respectively. It is clear to see that all the control inputs arerestricted to the feasible specific range. Fig. 5. Distances between each pair of agents.TABLE IR
ESULTS OF C OMPARISONS
Computaion Timeper Timestamp (s) Computaion Timefor ADMM First Iteartion (s)Proposed Method 43.4681 2.1558PQ-MIP 88.6464 5.4371NC-MIP 181.8962 N.A.NC-QCQP 441.7536 N.A.
From Fig. 5, the distances between each pair of agentsare given. It shows that the safe distance is slightly violatedbecause the stopping criterion we use in the example cannotguarantee the high precision of the solution. The choice of thestopping criterion balances the optimization efficiency and theoptimization precision. Therefore, we can reduce the violationby using a more strict stopping criterion. Alternatively, we canchoose a larger safe distance to deal with this problem.Table I documents the comparisons of computational ef-ficiency using different optimization approaches, where thecomputational time per step and the computational time for thefirst ADMM iteration are given. In the primal quadratic MIP(PQ-MIP) method, we replace the dual optimization problemsby the primal optimization problems, which are successfullysolved by MOSEK. With the non-convex MIP (NC-MIP)optimization method, the non-convex optimization problemswith mixed integer constraints are solved in a centralizedmanner by GUROBI. With the non-convex quadratically con-strained quadratic program (NC-QCQP) optimization method,the original collision avoidance optimization problems withnon-convex quadratic distance constraints are solved directlyby the non-convex solver CasADi. As clearly observed fromTable I, our proposed method demonstrates the significantsuperiority over other methods in terms of the computationalefficiency. VI. C
ONCLUSION
In this work, the multi-agent collision-free control prob-lem for medium and large scale systems is investigated. Toparallelize the resulting proposed optimization problem in themodel predictive collision-free control scheme, the ADMMtechnique is adroitly utilized to decompose the global problem11nto sub-problems. Specifically, in the first ADMM iteration,the dual problem of the optimization problem is formulatedto enhance the optimization efficiency; and in the secondADMM iteration, a non-convex mixed integer quadratic pro-gramming problem is formulated and an intuitive initializationmethod is presented to accelerate the computational process.Furthermore, rigorous proofs pertaining to the convergenceof the proposed non-convex ADMM iterations are furnished.Finally, a multi-agent system with a group of unmanned aerialvehicles (UAVs) is given as an illustrative example to validatethe effectiveness of the proposed method. Comparison resultsclearly demonstrate the promising effectiveness, efficiency andpracticability of the proposed methodologies here (involvingan innovative parallel computation framework which is con-structed, and which facilitates the deployment of advancedoptimization methods to solve the multi-agent collision-freecontrol problem efficiently).R
EFERENCES[1] F. Xiao, L. Wang, J. Chen, and Y. Gao, “Finite-time formation controlfor multi-agent systems,”
Automatica , vol. 45, no. 11, pp. 2605–2611,2009.[2] B. Ning, Q.-L. Han, and Z. Zuo, “Distributed optimization for multi-agent systems: An edge-based fixed-time consensus approach,”
IEEETransactions on Cybernetics , vol. 49, no. 1, pp. 122–132, 2017.[3] X. Tan, J. Cao, and X. Li, “Consensus of leader-following multiagentsystems: A distributed event-triggered impulsive control strategy,”
IEEETransactions on Cybernetics , vol. 49, no. 3, pp. 792–801, 2018.[4] A. Liu, W.-A. Zhang, L. Yu, H. Yan, and R. Zhang, “Formation controlof multiple mobile robots incorporating an extended state observer anddistributed model predictive approach,”
IEEE Transactions on Systems,Man, and Cybernetics: Systems , 2018.[5] P. Hang, C. Lv, C. Huang, J. Cai, Z. Hu, and Y. Xing, “An integratedframework of decision making and motion planning for autonomousvehicles considering social behaviors,” arXiv preprint arXiv:2005.11059 ,2020.[6] P. Hang, C. Lv, Y. Xing, C. Huang, and Z. Hu, “Human-like decisionmaking for autonomous driving: A noncooperative game theoretic ap-proach,”
IEEE Transactions on Intelligent Transportation Systems , 2020.[7] F. Li, Y. Ding, M. Zhou, K. Hao, and L. Chen, “An affection-baseddynamic leader selection model for formation control in multirobot sys-tems,”
IEEE Transactions on Systems, Man, and Cybernetics: Systems ,vol. 47, no. 7, pp. 1217–1228, 2016.[8] Z. Zhang, C. Guo, and L. Martínez, “Managing multigranular linguisticdistribution assessments in large-scale multiattribute group decisionmaking,”
IEEE Transactions on Systems, Man, and Cybernetics: Sys-tems , vol. 47, no. 11, pp. 3063–3076, 2016.[9] H. Fukushima, K. Kon, and F. Matsuno, “Distributed model predictivecontrol for multi-vehicle formation with collision avoidance constraints,”in
Proceedings of the 44th IEEE Conference on Decision and Control .IEEE, 2005, pp. 5480–5485.[10] S. Chen, D. W. Ho, L. Li, and M. Liu, “Fault-tolerant consensus of multi-agent system with distributed adaptive protocol,”
IEEE Transactions onCybernetics , vol. 45, no. 10, pp. 2142–2155, 2014.[11] B. Zhu, K. Guo, and L. Xie, “A new distributed model predictivecontrol for unconstrained double-integrator multiagent systems,”
IEEETransactions on Automatic Control , vol. 63, no. 12, pp. 4367–4374,2018.[12] X. Kong, X. Liu, L. Ma, and K. Y. Lee, “Hierarchical distributed modelpredictive control of standalone wind/solar/battery power system,”
IEEETransactions on Systems, Man, and Cybernetics: Systems , vol. 49, no. 8,pp. 1570–1581, 2019.[13] F. Ke, Z. Li, H. Xiao, and X. Zhang, “Visual servoing of constrainedmobile robots based on model predictive control,”
IEEE Transactions onSystems, Man, and Cybernetics: Systems , vol. 47, no. 7, pp. 1428–1438,2016.[14] H. Wei, C. Shen, and Y. Shi, “Distributed Lyapunov-based model pre-dictive formation tracking control for autonomous underwater vehiclessubject to disturbances,”
IEEE Transactions on Systems, Man, andCybernetics: Systems , 2019. [15] A. N. Venkat, J. B. Rawlings, and S. J. Wright, “Stability and optimalityof distributed model predictive control,” in
Proceedings of the 44th IEEEConference on Decision and Control . IEEE, 2005, pp. 6680–6685.[16] X. Zhang, J. Ma, Z. Cheng, S. Huang, S. S. Ge, and T. H. Lee,“Trajectory generation by chance constrained nonlinear MPC withprobabilistic prediction,”
IEEE Transactions on Cybernetics , 2020.[17] N. Mahdavi, J. H. Braslavsky, M. M. Seron, and S. R. West, “Modelpredictive control of distributed air-conditioning loads to compensatefluctuations in solar power,”
IEEE Transactions on Smart Grid , vol. 8,no. 6, pp. 3055–3065, 2017.[18] P. Giselsson, M. D. Doan, T. Keviczky, B. De Schutter, and A. Rantzer,“Accelerated gradient methods and dual decomposition in distributedmodel predictive control,”
Automatica , vol. 49, no. 3, pp. 829–833, 2013.[19] R. A. Bartlett, L. T. Biegler, J. Backstrom, and V. Gopal, “Quadratic pro-gramming algorithms for large-scale model predictive control,”
Journalof Process Control , vol. 12, no. 7, pp. 775–795, 2002.[20] G. Pannocchia, J. B. Rawlings, and S. J. Wright, “Fast, large-scale modelpredictive control by partial enumeration,”
Automatica , vol. 43, no. 5,pp. 852–860, 2007.[21] H. Wang, Y. Gao, Y. Shi, and R. Wang, “Group-based alternatingdirection method of multipliers for distributed linear classification,”
IEEE Transactions on Cybernetics , vol. 47, no. 11, pp. 3568–3582, 2016.[22] W. Deng and W. Yin, “On the global and linear convergence of thegeneralized alternating direction method of multipliers,”
Journal ofScientific Computing , vol. 66, no. 3, pp. 889–916, 2016.[23] M. Hong and Z.-Q. Luo, “On the linear convergence of the alternatingdirection method of multipliers,”
Mathematical Programming , vol. 162,no. 1-2, pp. 165–199, 2017.[24] X. Luo, M. Zhou, S. Li, L. Hu, and M. Shang, “Non-negativityconstrained missing data estimation for high-dimensional and sparse ma-trices from industrial applications,”
IEEE Transactions on Cybernetics ,vol. 50, no. 5, pp. 1844–1855, 2019.[25] G. Taylor, R. Burmeister, Z. Xu, B. Singh, A. Patel, and T. Goldstein,“Training neural networks without gradients: A scalable ADMM ap-proach,” in
International Conference on Machine Learning , 2016, pp.2722–2731.[26] H. Tan, X. Zeng, S. Lai, Y. Liu, and M. Zhang, “Joint demosaicingand denoising of noisy bayer images with ADMM,” in . IEEE, 2017,pp. 2951–2955.[27] J. Ma, Z. Cheng, X. Zhang, M. Tomizuka, and T. H. Lee, “Optimaldecentralized control for uncertain systems by symmetric Gauss-Seidelsemi-proximal ALM,” arXiv preprint arXiv:2001.00306 , 2020.[28] ——, “On symmetric Gauss-Seidel ADMM algorithm for H ∞ guar-anteed cost control with convex parameterization,” arXiv preprintarXiv:2001.00708 , 2020.[29] B. O’donoghue, E. Chu, N. Parikh, and S. Boyd, “Conic optimizationvia operator splitting and homogeneous self-dual embedding,” Journal ofOptimization Theory and Applications , vol. 169, no. 3, pp. 1042–1068,2016.[30] W. Tang and P. Daoutidis, “Distributed nonlinear model predictivecontrol through accelerated parallel admm,” in . IEEE, 2019, pp. 1406–1411.[31] P. Braun, T. Faulwasser, L. Grüne, C. M. Kellett, S. R. Weller, andK. Worthmann, “Hierarchical distributed admm for predictive controlwith applications in power networks,”
IFAC Journal of Systems andControl , vol. 3, pp. 10–22, 2018.[32] X. Zhang, J. Ma, S. Huang, Z. Cheng, and T. H. Lee, “Integratedplanning and control for collision-free trajectory generation in 3Denvironment with obstacles,” in . IEEE, 2019, pp. 974–979.[33] J. Ma, Z. Cheng, X. Zhang, A. A. Mamun, C. W. de Silva, and T. H.Lee, “Data-driven predictive control for multi-agent decision makingwith chance constraints,” arXiv preprint arXiv:2011.03213 , 2020.[34] A. Richards and J. P. How, “Aircraft trajectory planning with collisionavoidance using mixed integer linear programming,” in
Proceedings ofthe 2002 American Control Conference , vol. 3. IEEE, 2002, pp. 1936–1941.[35] A. Alonso-Ayuso, L. F. Escudero, and F. J. Martín-Campo, “Collisionavoidance in air traffic management: A mixed-integer linear optimizationapproach,”
IEEE Transactions on Intelligent Transportation Systems ,vol. 12, no. 1, pp. 47–57, 2010.[36] R. Deits and R. Tedrake, “Efficient mixed-integer planning for UAVsin cluttered environments,” in . IEEE, 2015, pp. 42–49.[37] M. Morari, C. E. Garcia, J. H. Lee, and D. M. Prett,
Model PredictiveControl . Englewood Cliffs: Prentice Hall, 1993.
38] D. Q. Mayne, J. B. Rawlings, C. V. Rao, and P. O. Scokaert, “Con-strained model predictive control: Stability and optimality,”
Automatica ,vol. 36, no. 6, pp. 789–814, 2000.[39] B. T. Stewart, A. N. Venkat, J. B. Rawlings, S. J. Wright, and G. Pan-nocchia, “Cooperative distributed model predictive control,”
Systems &Control Letters , vol. 59, no. 8, pp. 460–469, 2010.[40] Y. Wang, W. Yin, and J. Zeng, “Global convergence of ADMM innonconvex nonsmooth optimization,”
Journal of Scientific Computing ,vol. 78, no. 1, pp. 29–63, 2019.[41] X. Zhang, S. Huang, W. Liang, Z. Cheng, K. K. Tan, and T. H. Lee,“HLT*: Real-time and any-angle path planning in 3D environment,” in
IECON 2019-45th Annual Conference of the IEEE Industrial ElectronicsSociety , vol. 1. IEEE, 2019, pp. 5231–5236., vol. 1. IEEE, 2019, pp. 5231–5236.