\fail

Improved Hierarchical ADMM for Nonconvex Cooperative Distributed Model Predictive Control

Xiaoxue Zhang, Jun Ma, Zilong Cheng, Sunan Huang, Clarence W. de Silva, and Tong Heng Lee X. Zhang, Z. Cheng, and T. H. Lee are with the Integrative Sciences and Engineering Programme, NUS Graduate School, National University of Singapore, 119077 (e-mail: [email protected]; [email protected]; [email protected]).J. Ma is with the Department of Mechanical Engineering, University of California, Berkeley, CA 94720 USA (e-mail: [email protected]).S. Huang is with the Temasek Laboratories, National University of Singapore, Singapore, 117411 (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 may no longer be accessible.

Abstract

Distributed optimization is often widely attempted and innovated as an attractive and preferred methodology to solve large-scale problems effectively in a localized and coordinated manner. Thus, it is noteworthy that the methodology of distributed model predictive control (DMPC) has become a promising approach to achieve effective outcomes, e.g., in decision-making tasks for multi-agent systems. However, the typical deployment of such distributed MPC frameworks would lead to the involvement of nonlinear processes with a large number of nonconvex constraints. To address this important problem, the development and innovation of a hierarchical three-block alternating direction method of multipliers (ADMM) approach is presented in this work to solve this nonconvex cooperative DMPC problem in multi-agent systems. Here firstly, an additional slack variable is introduced to transform the original large-scale nonconvex optimization problem. Then, a hierarchical ADMM approach, which contains outer loop iteration by the augmented Lagrangian method (ALM) and inner loop iteration by three-block semi-proximal ADMM, is utilized to solve the resulting transformed nonconvex optimization problem. Additionally, it is analytically shown and established that the requisite desired stationary point exists for convergence in the algorithm. Finally, an approximate optimization stage with a barrier method is then applied to further significantly improve the computational efficiency, yielding the final improved hierarchical ADMM. The effectiveness of the proposed method in terms of attained performance and computational efficiency is demonstrated on a cooperative DMPC problem of decision-making process for multiple unmanned aerial vehicles (UAVs).

Index Terms:

Alternating direction method of multipliers (ADMM), distributed optimization, model predictive control (MPC), multi-agent system, collision avoidance.

I Introduction

With the rapid development of communication and computation technologies, cooperative decision making of large-scale multi-agent systems has become a promising trend in the development of automated systems [1, 2, 3]. Nevertheless, the significant challenge involved in cooperative decision making, in handling the coupling conditions among the interconnected agents with an appropriate guarantee of efficient computation still remains. In the existing literature, a significant amount of effort has been put into distributed control and coordinated networks for large-scale interconnected multi-agent systems, with the objective of optimizing the global cost while satisfying the required safety constraints collectively. Essentially for such approaches, the control and optimization algorithms deployed are distributed in structure, and the deployment utilizes only the local observations and information [4, 5]. With the appropriate communication technology in place, the agents are developed to suitably make decisions while attempting to satisfy all of the constraints or requirements invoked [6, 7]. On the other hand, a remarkable alternate development that utilizes an approach termed as distributed model predictive control (DMPC) has also been shown to display great promise to be effectively used to handle the input and state constraints with multiple objectives; and possibly also applicable to this important problem of cooperative decision making of large-scale multi-agent systems [8]. In addition, via the DMPC framework, there is great potential that the computational burden can be significantly relieved by decomposing the centralized system into several interconnected subsystems (with also a similar situation of availability of knowledge of necessary information among the connected agents).

For the DMPC problem with interconnected agents, the promising optimization approach of the alternating direction method of multipliers (ADMM) algorithm has the potential for very effective utilization. In this context, it can be noted that a large number of research works have demonstrated that the ADMM methodology has the capability to rather effectively determine the optimal solution to many challenging optimization problems, such as distributed optimization [9], decentralized control [10, 11], and statistical learning problems [12, 13]. The key idea of the ADMM approach is to utilize a decomposition coordination principle, wherein local sub-problems with smaller sizes are solved, and then the solutions of these subproblems can be coordinated to obtain the solution to the original problem. Notably, the ADMM solves the sub-problems by optimizing blocks of variables, such that efficient distributed computation can be attained. The convergence of the ADMM with two blocks has been investigated for convex optimization problems in [14, 15], and typical applications of such two-block ADMM approaches in distributed convex optimization problems are reported extensively in [16, 17]. Several researchers have also made rather noteworthy extensions from this two-block convex ADMM to multi-block convex ADMM in [18, 19]. However, although seemingly evident as a possibility with great potential, there has not been much substantial work on innovating the ADMM framework for nonconvex DMPC. Classical ADMM is known only to admit a linear convergence rate for convex optimization problems.

For nonconvex optimization problems, certain additional conditions and assumptions need to be considered to guarantee the required convergence [20, 21, 22, 23, 24, 25, 26]. More specifically, the work in [20] studies the two-block ADMM with one of the blocks defined as the identity matrix; and it also states that the objective function and the domain of the optimization variable need to be lower semi-continuous. Yet further, a nonconvex Bregman-ADMM is proposed in [21] by introducing a Bregman divergence term to the augmented Lagrangian function to promote the descent of certain potential functions. Moreover, the work in [22] proposes two proximal-type variants of ADMM with $\epsilon$ -stationarity conditions to solve the nonconvex optimization problem with affine coupling constraints under the assumption that the gradient of the objective function is Lipschitz continuous. Additionally, in [23], a multi-block ADMM is presented to minimize a nonconvex and nonsmooth objective function subject to certain coupling linear equality constraints; and the work in [24] presents an ADMM-type algorithm for nonconvex optimization problem in the application environment of modern machine learning problems, though requiring the assumption that the penalty parameter should be large enough. Besides, an augmented Lagrangian based alternating direction inexact newton (ALADIN) method is introduced in [27] to solve the optimization problems with a separable nonconvex objective function and coupled affine constraints.

Noting all these additional conditions and assumptions that typically can arise, it is the situation where the ADMM approach cannot yet be directly applied to solve a large-scale constrained nonconvex program without further relaxation or reformulation. Also, the ADMM with more than two blocks is typically implemented with sequential optimization; and while a direct parallelization of a multi-block ADMM formulation is also sometimes considered, the aforementioned approach has the disadvantage that it is typically less likely to converge [28, 29]. In order to address all these limitations, a hierarchical three-block ADMM approach is utilized in our paper to solve the nonconvex optimization problem that arises for decision making in multi-agent systems. Firstly, an additional slack variable is innovatively introduced to transform the original large-scale nonconvex optimization problem, such that the intractable nonconvex coupling constraints are suitably related to the distributed agents. Then, the approach with a hierarchical ADMM that contains the outer loop iteration by the augmented Lagrangian method (ALM), and inner loop iteration by three-block semi-proximal ADMM, is utilized to solve the resulting transformed nonconvex optimization problem. Next, the approximate optimization with a barrier method is then applied to improve the computational efficiency. Finally, a multi-agent system involving decision making for multiple unmanned aerial vehicles (UAVs) is utilized to demonstrate the effectiveness of the proposed method in terms of attained performance and computational efficiency.

The remainder of this paper is organized as follows. Section II describes a multi-agent system that contains multiple interconnected agents equipped with sensors and communication devices, and also formulates a large-scale nonconvex optimization problem for decision making. Section III gives a more compact form of the optimization problem, which is further transformed by introducing slack variables. In section IV, the hierarchical ADMM is used to solve the transformed optimization problem. Next, an improved version of such hierarchical ADMM is presented to accelerate the computation process based on the barrier method in Section V. Then, in Section VI, a multi-UAV system is used as an example to show the effectiveness and computational efficiency of the hierarchical ADMM and improved hierarchical ADMM. Finally, pertinent conclusions of this work are given in Section VII.

II Problem Formulation

The following notations are used in the remaining content. $\mathbb{R}^{a\times b}$ denotes the set of real matrices with $a$ rows and $b$ columns, $\mathbb{R}^{a}$ means the set of $a$ -dimensional real column vectors. The symbol $X\succ 0$ and $X\succeq 0$ mean that the matrix $X$ is positive definite and positive semi-definite, respectively. $x>y$ and $x\geq y$ mean that vector $x$ is element-wisely greater and no less than the vector $y$ . $A^{\top}$ and $x^{\top}$ denote the transpose of the matrix $A$ and vector $x$ . $I_{a}$ represents the $a$ -dimensional identity matrix; $1_{a}$ and $1_{(a,b)}$ denote the $a$ -dimensional all-one vector and the $a$ -by- $b$ all-one matrix, respectively; $0_{a}$ and $0_{(a,b)}$ represent the $a$ -dimensional all-zero vector and the $a$ -by- $b$ all-zero matrix, respectively. The operator $\langle X,Y\rangle$ denotes the Frobenius inner product, i.e., $\langle X,Y\rangle=\operatorname{Tr}(X^{\top}Y)$ for all $X,Y\in\mathbb{R}^{a\times b}$ ; the operator $\|X\|$ is the Frobenius norm of matrix $X$ . $\otimes$ denotes the Kronecker product and $\odot$ denotes the Hadamard product (Schur product). $\operatorname{blockdiag}(X_{1},X_{2},\cdots,X_{n})$ denotes a block diagonal matrix with diagonal entries $X_{1},X_{2},\cdots,X_{n}$ ; $\operatorname{diag}(a_{1},a_{2},\cdots,a_{n})$ denotes a diagonal matrix with diagonal entries $a_{1},a_{2},\cdots,a_{n}$ . $\mathbb{Z}^{a}$ and $\mathbb{Z}_{a}^{b}$ mean the sets of positive integers $\{1,2,\cdots,a\}$ and $\{a,a+1,\cdots,b\}$ , respectively. $\mathbb{Z}_{+}$ denotes the set of positive integers $\{1,2,\cdots\}$ . The operator $\{x_{i}\}_{\forall i\in\mathbb{Z}_{a}^{b}}$ means the concatenation of the vector $x_{i}$ for all $i\in\mathbb{Z}_{a}^{b}$ , i.e., $\{x_{i}\}_{\forall i\in\mathbb{Z}_{a}^{b}}=(x_{a},x_{a+1},\cdots,x_{b})=\begin{bmatrix}x_{a}^{\top}&x_{a+1}^{\top}&\cdots&x_{b}^{\top}\end{bmatrix}^{\top}$ .

II-A Modelling of Interconnected Agents

In order to represent the constraint or information topology of multiple interconnected agents, an undirected graph $\mathcal{G}(\mathcal{V},\mathcal{E})$ can be utilized. The node set $\mathcal{V}=\{1,2,\cdots,N\}$ denotes the agents, and the edge set $\mathcal{E}$ denotes the coupling constraints (information flow) between two interconnected agents, which is defined as

\begin{cases}(i,j)\in\mathcal{E}(t),&\;d_{\mathrm{safe}}\leq\|p_{i}-p_{j}\|\leq d_{\mathrm{cmu}}\\ (i,j)\notin\mathcal{E}(t),&\;\text{otherwise},\end{cases}

(1)

where $N$ and $M$ are the number of agents and coupling constraints in the multi-agent system, respectively, $p_{i},p_{j}$ are the position vectors of the $i$ th agent and $j$ th agent, $d_{\mathrm{cmu}}$ and $d_{\mathrm{safe}}$ mean the maximum communication distance and minimum safe distance between two agents, respectively. Therefore, based on the communication topology, an adjacency matrix of the network (denoted by $\mathcal{D}$ ) can be obtained, which is a square symmetric matrix. The elements of $\mathcal{D}$ indicate whether pairs of vertices are adjacent/connected or not in the graph. Therefore, the neighbor nodes of the $i$ th agent are the corresponding indexes of nonzero elements of the $i$ th row in $\mathcal{D}$ , which is denoted by $\nu(i)=\{j|(i,j)\in\mathcal{E},\forall j\in\mathcal{V}\}$ .

II-B Problem Description

Each connected agent in the set $\mathcal{V}$ has its origin and target state, which means each agent needs to achieve its task by coordinating with other connected agents in $\mathcal{V}$ . Moreover, the information of the connected agents (neighbors) is necessary for each agent to make decisions, due to the requirement of the communication topology (1). Since the communication between connected agents occurs during the whole process, the present state information of neighbors needs to be conveyed to the $i$ th agent to make decisions. Thus, the cooperative task can be formulated as a cooperative DMPC problem for each agent $i\in\mathcal{V}$ . Then, given the initial state $x_{i}(t)$ , the DMPC problem for the $i$ th agent at timestamp $t$ can be written as

$\displaystyle\min_{u_{i},x_{i}}\quad$	$\displaystyle\sum_{i\in\mathcal{V}}J_{i}(x_{i},u_{i})$	(2a)
$\displaystyle\operatorname{s.t.}\quad$	$\displaystyle x_{i}(\tau+1)=A_{i}x_{i}(\tau)+B_{i}u_{i}(\tau)$
	$\displaystyle\underline{u}_{i}\leq u_{i}(\tau)\leq\overline{u}_{i}$
	$\displaystyle\underline{x}_{i}\leq x_{i}(\tau+1)\leq\overline{x}_{i}$
	$\displaystyle(x_{i}(\tau+1),x_{j}(\tau+1))\in\mathbb{C}$
	$\displaystyle\forall j\in\nu(i),\forall\tau\in\mathbb{Z}_{t}^{t+T-1},$

where the subscript $(\cdot)_{i}$ means the corresponding variable of the $i$ th agent, $x_{i}\in\mathbb{R}^{n}$ and $u_{i}\in\mathbb{R}^{m}$ denote the state and control input vectors of the $i$ th agent, respectively. $A_{i}\in\mathbb{R}^{n\times n},B_{i}\in\mathbb{R}^{n\times m}$ are the state matrix and control input matrix, respectively. $t$ denotes the initial time stamp and $T$ is the prediction horizon. Notice that (2a) is the objective function for the state and control variables; (II-B) denotes the kinematics of the agents; (II-B) and (II-B) denote the bound constraints of state and control input vectors, $\overline{x}_{i}$ ( $\underline{x}_{i}$ ) and $\overline{u}_{i}$ ( $\underline{u}_{i}$ ) are the upper (lower) bound of state and control input vectors, respectively; (II-B) denotes the coupling constraints of two interconnected agents (the $i$ th agent and the $j$ th agent with $j\in\nu(i)$ ) and $\mathbb{C}$ denotes the set of the coupling constraints. The cost function of the $i$ th agent $J_{i}(x_{i},u_{i})$ is a summation of the stage cost term $\ell_{i}$ and the terminal cost term $\ell_{T,i}$ , i.e.,

	$\displaystyle\quad J_{i}(x_{i},u_{i})$
	$\displaystyle=\ell_{i}\left(x_{i}(t+1),\cdots,x_{i}(t+T-1),u_{i}(t),\cdots,u_{i}(t+T-1)\right)$
	$\displaystyle+\ell_{T,i}(x_{i}(t+T)),\yesnumber$

where the initial state $x_{i}(t)$ is known. The stage cost function $\ell_{i}$ is defined as

	$\displaystyle\ell_{i}(x_{i},u_{i})$	$\displaystyle=$	$\displaystyle\sum_{\tau=t+1}^{t+T-1}\langle x_{i}(\tau)-x_{i,\text{ref}}(\tau),\>Q_{i}\left(x_{i}(\tau)-x_{i,\text{ref}}(\tau)\right)\rangle$
			$\displaystyle+\sum_{\tau=t}^{t+T-1}\langle u_{i}(\tau)\>,\>R_{i}u_{i}(\tau)\rangle,\yesnumber$

where $x_{i,\mathrm{ref}}$ is the reference state the $i$ th agent tries to follow, and $Q_{i}\in\mathbb{R}^{n\times n}$ and $R_{i}\in\mathbb{R}^{m\times m}$ are the diagonal weighting matrices for the state and control input vectors, respectively. The terminal cost function $\ell_{T,i}$ is

	$\displaystyle\ell_{T,i}(x_{i}(t+T))$	$\displaystyle=$	$\displaystyle\langle x_{i}(t+T)-x_{i,\text{ref}}(t+T)\>,\>$
			$\displaystyle\>\>Q_{i,T}\left(x_{i}(t+T)-x_{i,\text{ref}}(t+T)\right)\rangle,\yesnumber$

where $Q_{i,T}$ is the diagonal weighting matrix for the terminal state vector.

In a specific multi-agent system, $\mathbb{C}$ can be specified as

	$\displaystyle\mathbb{C}=\Big{\{}(x_{i}(\tau),x_{j}(\tau))\>\Big{\|}\>d_{\mathrm{safe}}\leq\left\\|L_{p}(x_{i}(\tau)-x_{j}(\tau))\right\\|\leq d_{\mathrm{cmu}},$
	$\displaystyle\quad\forall i\in\mathcal{V},\forall j\in\nu(i),\forall\tau\in\mathbb{Z}_{t+1}^{t+T}\Big{\}},\yesnumber$

where $L_{p}$ is the locating matrix to extract the position vector $p_{i}$ from the state vector $x_{i}$ , i.e., $p_{i}(\tau)=L_{p}x_{i}(\tau)$ .

Assumption 1.

$\nu(i)$ in the prediction horizon $[t+1,t+T]$ is time-invariant.

III Problem Transformation

III-A Problem Reformulation

First of all, the decision variable is set as

	$\displaystyle z_{i}$	$\displaystyle=$	$\displaystyle\big{(}u_{i}(t),x_{i}(t+1),u_{i}(t+1),x_{i}(t+2),\cdots,$
			$\displaystyle\quad u_{i}(t+T-1),x_{i}(t+T)\big{)}\in\mathbb{R}^{T(m+n)}.\yesnumber$

It is straightforward that $L_{p}x_{i}=p_{i}\in\mathbb{R}^{n_{p}},\forall i\in\mathcal{V}$ , where $n_{p}$ is the dimension of position in state vector $p_{i}$ and $n_{p}\leq n$ , and $\|p_{i}(\tau)-p_{j}(t)\|^{2}=\left\langle p_{i}(\tau)-p_{j}(t)\>,\>p_{i}(\tau)-p_{j}(t)\right\rangle$ . For all $j\in\nu(i)$ and $\tau\in\mathbb{Z}_{t+1}^{t+T}$ , the coupling constraints $d_{\mathrm{safe}}\leq\|p_{i}(\tau)-p_{j}(\tau)\|\leq d_{\mathrm{cmu}}$ , $\forall j\in\nu(i)$ , $\forall i\in\mathcal{V}$ , $\forall\tau\in\mathbb{Z}_{t+1}^{t+T}$ can be equivalently expressed as

	$\displaystyle d_{\mathrm{safe}}^{2}\leq\left(p_{i}(t+1)-p_{j}(t)\right)^{\top}\left(p_{i}(t+1)-p_{j}(t)\right)\leq d_{\mathrm{cmu}}^{2},$
	$\displaystyle\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\forall j\in\nu(i),$
	$\displaystyle\vdots$
	$\displaystyle d_{\mathrm{safe}}^{2}\leq\left(p_{i}(t+T)-p_{j}(t)\right)^{\top}\left(p_{i}(t+T)-p_{j}(t)\right)\leq d_{\mathrm{cmu}}^{2},$
	$\displaystyle\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\forall j\in\nu(i),\yesnumber$

where $r_{i}$ is the number of the neighbours of the $i$ th agent, i.e., $r_{i}=|\nu(i)|$ where $|\cdot|$ is the cardinality operator for the set $\nu(i)$ . This means the $i$ th agent has $r_{i}$ neighbors, and there are $r_{i}$ inequalities for each $\tau$ . Thus, the coupling constraint $d_{\mathrm{safe}}\leq\|p_{i}(\tau)-p_{j}(\tau)\|\leq d_{\mathrm{cmu}}$ , $\forall j\in\nu(i)$ , $\forall i\in\mathcal{V}$ , $\forall\tau\in\mathbb{Z}_{t+1}^{t+T}$ denotes $Tr_{i}$ inequalities for all $\tau$ and $j$ .

Define a known vector $[p_{j}]=\left\{p_{j}(t)\right\}_{\forall j\in\nu(i)}\in\mathbb{R}^{r_{i}n_{p}}$ . Notice that the position vector of all neighbors of the $i$ th agent $p_{j}(t)$ are known due to the existence of communication. Thus, the coupling constraints can be rewritten as

	$\displaystyle\mathcal{M}\left(z_{i};[p_{j}]\right)\mathcal{P}\left(z_{i};[p_{j}]\right)$	$\displaystyle\leq$	$\displaystyle d_{\mathrm{cmu}}^{2}$
	$\displaystyle\mathcal{M}\left(z_{i};[p_{j}]\right)\mathcal{P}\left(z_{i};[p_{j}]\right)$	$\displaystyle\geq$	$\displaystyle d_{\mathrm{safe}}^{2},\yesnumber$

with the mapping $\mathcal{P}:\mathbb{R}^{T(m+n)}\rightarrow\mathbb{R}^{Tr_{i}n_{p}}$ characterized by

\displaystyle\mathcal{P}\left(z_{i};[p_{j}]\right)=(I_{T}\otimes 1_{r_{i}}\otimes I_{n_{p}})M_{p}z_{i}-1_{T}\otimes[p_{j}],\yesnumber

where the matrix $M_{p}\in\mathbb{R}^{Tn_{p}\times T(m+n)}$ is a matrix to extract all position vectors $\left(p_{i}(t+1),p_{i}(t+2),\cdots,p_{i}(t+T)\right)$ from the decision variable $z_{i}$ , and $M_{p}=I_{T}\otimes\begin{bmatrix}{0}_{(n_{p},m)}&I_{n_{p}}&{0}_{(n_{p},(n-n_{p}))}\end{bmatrix}$ . Besides, the mapping $\mathcal{M}:\mathbb{R}^{T(m+n)}\rightarrow\mathbb{R}^{Tr_{i}\times Tr_{i}n_{p}}$ is given by

	$\displaystyle\mathcal{M}\left(z_{i};[p_{j}]\right)=\left(I_{Tr_{i}}\otimes 1_{n_{p}}^{\top}\right)\left(I_{Tr_{i}n_{p}}\odot\left({\mathcal{P}\left(z_{i};[p_{j}]\right)}{1}_{Tr_{i}n_{p}}^{\top}\right)\right).$
	$\displaystyle\yesnumber$

Therefore, the large-scale cooperative optimization problem (2a) can be reformulated as

$\displaystyle\min\limits_{z_{i}}$	$\displaystyle\sum_{i\in\mathcal{V}}J_{i}(z_{i})$
$\displaystyle\operatorname{s.t.}$	$\displaystyle G_{i}z_{i}=g_{i}$	(7)
	$\displaystyle F_{z,i}z_{i}\leq f_{z,i}$
	$\displaystyle{\mathcal{M}\left(z_{i};[p_{j}]\right)\mathcal{P}\left(z_{i};[p_{j}]\right)}\leq d_{\mathrm{cmu}}^{2}$
	$\displaystyle{\mathcal{M}\left(z_{i};[p_{j}]\right)\mathcal{P}\left(z_{i};[p_{j}]\right)}\geq d_{\mathrm{safe}}^{2}$
	$\displaystyle\forall j\in\nu(i),\forall\tau\in\mathbb{Z}_{t+1}^{t+T},$

where $J_{i}(z_{i})=\langle z_{i}-z_{i,\text{ref}}\>,\>H_{i}(z_{i}-z_{i,\text{ref}})\rangle$ ; $H_{i}=$ $\operatorname{blockdiag}(R_{i},Q_{i},R_{i},Q_{i},\cdots,R_{i},Q_{i},R_{i},Q_{T,i})$ is the weighting matrix for the optimization variable $z_{i}$ ; $z_{i,\text{ref}}$ is the reference vector, i.e., $z_{i,\text{ref}}=\big{(}0_{m},x_{i,\text{ref}}(t+1),$ $0_{m},x_{i,\text{ref}}(t+2),\cdots,0_{m},x_{i,\text{ref}}(t+T)\big{)}$ . Due to the fact that the agents’ dynamic models are linear time-invariant during the optimization horizon $[t,t+T]$ , we can rewrite (II-B) as the equality constraint $G_{i}z_{i}=g_{i}$ in (III-A), where

	$\displaystyle G_{i}$	$\displaystyle=$	$\displaystyle I_{T}\otimes\begin{bmatrix}-B_{i}&I_{n}\end{bmatrix}\;+\operatorname{blockdiag}\Big{(}0_{(n,m)},$
			$\displaystyle\underbrace{\begin{bmatrix}-A_{i}&0_{(n,m)}\end{bmatrix},\cdots,\begin{bmatrix}-A_{i}&0_{(n,m)}\end{bmatrix}}_{T-2},\begin{bmatrix}-A_{i}&0_{(n,m+n)}\end{bmatrix}\Big{)}$

and $g_{i}=\Big{[}\left(A_{i}x_{i}(t)\right)^{\top}\quad\underbrace{0_{n}^{\top}\quad 0_{n}^{\top}\quad\cdots\quad 0_{n}^{\top}}_{T-1}\Big{]}^{\top}$ with a known state vector $x_{i}(t)$ at current timestamp $t$ . Notice that $G_{i}\in\mathbb{R}^{Tn\times T(m+n)}$ and $g_{i}\in\mathbb{R}^{Tn}$ . Additionally, (II-B) and (II-B) can be rewritten as $F_{z,i}z_{i}\leq f_{z,i}$ in (III-A), where

\displaystyle F_{z,i}=\begin{bmatrix}I_{T(m+n)}\\ -I_{T(m+n)}\end{bmatrix},f_{z,i}=\begin{bmatrix}{1_{T}\otimes\begin{bmatrix}\overline{u}_{i}^{\top}&\overline{x}_{i}^{\top}\end{bmatrix}^{\top}}\\ {1_{T}\otimes\begin{bmatrix}-\underline{u}_{i}^{\top}&-\underline{x}_{i}^{\top}\end{bmatrix}^{\top}}\end{bmatrix}.

Remark 1.

It is well-known that increasing the prediction horizon $T$ can enlarge the domain of attraction of the MPC controller. However, this will increase the computational burden. Weighting the terminal cost can also enlarge the domain of attraction of the MPC controller without the occurrence of the terminal constraints; thus, the stabilizing weighting factor of a given initial state can be included, which has been proved in [30] that the asymptotic stability of the nonlinear MPC controller can be achieved.

By introducing an auxiliary variable $z_{f,i}\in\mathbb{R}^{2T(m+n)}$ , we have

	$\displaystyle\min\limits_{z_{i}}$		$\displaystyle\sum_{i\in\mathcal{V}}J_{i}(z_{i})+\delta_{\mathbb{R}_{+}^{2T(m+n)}}(z_{f,i})$
	$\displaystyle\operatorname{s.t.}$		$\displaystyle\mathcal{A}_{i}z_{i}+\mathcal{B}_{i}z_{f,i}-h_{i}=0$
			$\displaystyle{\mathcal{M}\left(z_{i};[p_{j}]\right)\mathcal{P}\left(z_{i};[p_{j}]\right)}\leq d_{\mathrm{cmu}}^{2}$
			$\displaystyle{\mathcal{M}\left(z_{i};[p_{j}]\right)\mathcal{P}\left(z_{i};[p_{j}]\right)}\geq d_{\mathrm{safe}}^{2}$
			$\displaystyle\forall j\in\nu(i),\yesnumber$

where $\mathcal{A}_{i}=\begin{bmatrix}G_{i}\\ F_{z,i}\end{bmatrix}$ , $\mathcal{B}_{i}=\begin{bmatrix}0_{(Tn,2T(m+n))}\\ I_{2T(m+n)}\end{bmatrix}$ , and $h_{i}=\begin{bmatrix}g_{i}\\ f_{z,i}\end{bmatrix}$ , $\delta_{\mathbb{R}_{+}^{2T(m+n)}}(z_{f,i})$ is an indicator function defined as

\displaystyle\delta_{\mathbb{R}_{+}^{2T(m+n)}}(z_{f,i})=\begin{cases}0,&\text{if }z_{f,i}\in\mathbb{R}_{+}^{2T(m+n)}\\ \infty,&\text{otherwise}.\end{cases}

(8)

Since the second inequality in (III-A) is nonlinear and nonconvex, we define a set

\displaystyle\mathcal{Z}_{i}=\left\{z_{i}\in\mathbb{R}^{T(m+n)}\middle|\begin{array}[]{l}\mathcal{M}(z_{i})\mathcal{P}(z_{i})\leq d_{\mathrm{cmu}}^{2}\\ \mathcal{M}(z_{i})\mathcal{P}(z_{i})\geq d_{\mathrm{safe}}^{2}\end{array}\right\}.

Then, the problem (III-A) can be simplified as

	$\displaystyle\min\limits_{z_{i}}$		$\displaystyle J_{i}(z_{i})+\delta_{\mathcal{Z}_{i}}(z_{i})+\delta_{\mathbb{R}_{+}^{2T(m+n)}}(z_{f,i})$
	$\displaystyle\operatorname{s.t.}$		$\displaystyle\mathcal{A}_{i}z_{i}+\mathcal{B}_{i}z_{f,i}-h_{i}=0,\yesnumber$

where $\delta_{\mathcal{Z}_{i}}(z_{i})$ is an indicator function of $z_{i}$ onto the set $\mathcal{Z}_{i}$ .

III-B Optimization Problem Transformation

Due to the fact that the direct use of the conventional ADMM to the nonconvex optimization problem (III-A) directly cannot ensure the convergence, problem transformation is significant in the constrained nonconvex optimization problem [22]. Thus, a slack variable $s_{i}\in\mathbb{R}^{2Tm+3Tn}$ is introduced to the problem (III-A), and then the transformed optimization problem is given by

	$\displaystyle\min\limits_{z_{i}}$		$\displaystyle J_{i}(z_{i})+\delta_{\mathcal{Z}_{i}}(z_{i})+\delta_{\mathbb{R}_{+}^{2T(m+n)}}(z_{f,i})$
	$\displaystyle\operatorname{s.t.}$		$\displaystyle\mathcal{A}_{i}z_{i}+\mathcal{B}_{i}z_{f,i}-h_{i}+s_{i}=0$
			$\displaystyle s_{i}=0.\yesnumber$

After introducing $s_{i}$ , the linear coupling constraints in (III-A) change from 2 blocks ( $z_{i}$ and $z_{f,i}$ ) to 3 blocks ( $z_{i}$ , $z_{f,i}$ and $s_{i}$ ). Since the block about $s_{i}$ is an identity matrix $I_{2Tm+3Tn}$ whose image is the whole space, there always exists an $s_{i}$ such that $\mathcal{A}_{i}z_{i}+\mathcal{B}_{i}z_{f,i}-h_{i}+s_{i}=0$ is satisfied, given any $z_{i}$ and $z_{f,i}$ . In addition, the constraint $s_{i}=0$ can be treated separately from the equality constraint $\mathcal{A}_{i}z_{i}+\mathcal{B}_{i}z_{f,i}-h_{i}+s_{i}=0$ . Therefore, we can consider the constraints in the problem (III-B) by separating them into two groups of constraints. The first group considers the equality constraint and the nonconvex constraint involved by the indicator function $\delta_{\mathcal{Z}_{i}}(z_{i})$ and $\delta_{\mathbb{R}_{+}^{2T(m+n)}}(z_{f,i})$ , and the other group deals with the constraint $s_{i}=0$ . As for the first sub-problem which does not include the constraint $s_{i}=0$ , some existing techniques of the ADMM can be employed. On the other hand, the constraint $s_{i}=0$ can be transformed by dualizing the constraint $s_{i}=0$ with $\lambda_{i}$ and adding a quadratic penalty $\frac{\beta_{i}}{2}\|s_{i}\|^{2}$ . Then the problem (III-B) can be rewritten as

	$\displaystyle\min\limits_{z_{i},z_{f,i},s_{i}}$		$\displaystyle J_{i}(z_{i})+\delta_{\mathcal{Z}_{i}}(z_{i})+\delta_{\mathbb{R}_{+}^{2T(m+n)}}(z_{f,i})$
			$\displaystyle+\langle\lambda_{i},s_{i}\rangle+\frac{\beta_{i}}{2}\\|s_{i}\\|^{2}$
	$\displaystyle\operatorname{s.t.}$		$\displaystyle\mathcal{A}_{i}z_{i}+\mathcal{B}_{i}z_{f,i}-h_{i}+s_{i}=0,\yesnumber$

where $\lambda_{i}\in\mathbb{R}^{2Tm+3Tn}$ is the dual parameter and $\beta_{i}\in\mathbb{R}_{+}$ is the penalty parameter. Then, the ALM is used to solve the second sub-problem.

The augmented Lagrangian function of problem (III-B) is

$\displaystyle\mathcal{L}_{\rho_{i}}$	$\displaystyle=$	$\displaystyle J_{i}(z_{i})+\delta_{\mathcal{Z}_{i}}(z_{i})+\delta_{\mathbb{R}_{+}^{2T(m+n)}}(z_{f,i})+\langle\lambda_{i},s_{i}\rangle$
		$\displaystyle+\frac{\beta_{i}}{2}\\|s_{i}\\|^{2}+\langle y_{i},\mathcal{A}_{i}z_{i}+\mathcal{B}_{i}z_{f,i}-h_{i}+s_{i}\rangle$
		$\displaystyle+\frac{\rho_{i}}{2}\\|\mathcal{A}_{i}z_{i}+\mathcal{B}_{i}z_{f,i}-h_{i}+s_{i}\\|^{2},\yesnumber$

where $y_{i}\in\mathbb{R}^{2Tm+3Tn}$ is the dual parameter of constraint $\mathcal{A}_{i}z_{i}+\mathcal{B}_{i}z_{f,i}-h_{i}+s_{i}=0$ and $\rho_{i}\in\mathbb{R}$ is the penalty parameter. Then, the first-order optimality conditions at a stationary solution $(z_{i}^{k},z_{f,i}^{k},s_{i}^{k},y_{i}^{k})$ are given by

	$\displaystyle 0\in\nabla J_{i}(z_{i}^{k})+\mathbb{N}_{\mathcal{Z}_{i}}(z_{i}^{k})+\mathcal{A}_{i}^{\top}y_{i}^{k}$		(10a)
	$\displaystyle 0\in\mathcal{B}_{i}^{\top}y_{i}^{k}+\mathbb{N}_{\mathbb{R}_{+}^{2T(m+n)}}\left(z_{f,i}^{k}\right)$
	$\displaystyle\lambda_{i}^{k}+\beta^{k}s_{i}^{k}+y_{i}^{k}=0$
	$\displaystyle\mathcal{A}_{i}z_{i}^{k}+\mathcal{B}_{i}z_{f,i}^{k}-h_{i}+s_{i}^{k}=0,$

where $\mathbb{N}_{\mathcal{Z}_{i}}(z_{i}^{k})$ and $\mathbb{N}_{\mathbb{R}_{+}^{2T(m+n)}}\left(z_{f,i}^{k}\right)$ denote the normal cone of the set $\mathcal{Z}_{i}$ and $\mathbb{R}_{+}^{2T(m+n)}$ with respect to $z_{i}^{k}$ and $z_{f,i}^{k}$ , respectively, and the superscript $(\cdot)^{k}$ means the variable in the $k$ th iteration.

Remark 2.

A solution that satisfies the optimality conditions (10a) may not necessarily satisfy the primal feasibility of the constraint $s_{i}=0$ . However, ALM can prompt the slack variable $s_{i}$ to zero through updating its dual parameter $\lambda_{i}$ .

Based on Remark 2, the update of $\lambda_{i}$ helps to prompt $s_{i}$ to zero with $\lambda_{i}^{k+1}=\lambda_{i}^{k}+\beta_{i}^{k}s_{i}^{k}$ .

IV Hierarchical ADMM

For the problem (III-B), we treat it into two groups. The first one is to solve the problem (III-B) and update $z_{i},z_{f,i},s_{i},y_{i}$ in sequence with keeping $\lambda_{i}$ and $\beta_{i}$ constant, and the other group is to update the parameter $\lambda_{i}$ and $\beta_{i}$ . In order to clarify the two groups, the iteration number of the first group (inner iteration) is indexed by $r$ , and the one of the second group (outer iteration) is $k$ .

1. Update $z_{i}$ :

$\displaystyle z_{i}^{k,r+1}$	$\displaystyle=$	$\displaystyle\underset{z_{i}}{\operatorname{argmin}}\;\mathcal{L}_{\rho_{i}^{k}}\left(z_{i},z_{f,i}^{k,r},s_{i}^{k,r};y_{i}^{k,r},\lambda_{i}^{k},\rho_{i}^{k},\beta_{i}^{k}\right)$
	$\displaystyle=$	$\displaystyle\underset{z_{i}\in\mathcal{Z}_{i}}{\operatorname{argmin}}\;J_{i}(z_{i})+\frac{\rho_{i}^{k}}{2}\left\\|\mathcal{A}_{i}z_{i}+\mathcal{B}_{i}z_{f,i}^{k,r}\right.$
		$\displaystyle\left.\quad\quad+s_{i}^{k,r}-h_{i}+\frac{y_{i}^{k,r}}{\rho_{i}^{k}}\right\\|^{2}.\yesnumber$

2. Update $z_{f,i}$ :

$\displaystyle z_{f,i}^{k,r+1}$	$\displaystyle=$	$\displaystyle\underset{z_{f,i}}{\operatorname{argmin}}\;\mathcal{L}_{\rho_{i}^{k}}\left(z_{i}^{k,r+1},z_{f,i},s_{i}^{k,r};y_{i}^{k,r},\lambda_{i}^{k},\rho_{i}^{k},\beta_{i}^{k}\right)$
	$\displaystyle=$	$\displaystyle\underset{z_{f,i}}{\operatorname{argmin}}\;\delta_{\mathbb{R}_{+}^{2T(m+n)}}(z_{f,i})+\frac{\rho_{i}^{k}}{2}\left\\|\mathcal{A}_{i}z_{i}^{k,r+1}\right.$
		$\displaystyle\left.\quad+\mathcal{B}_{i}z_{f,i}+s_{i}^{k,r}-h_{i}+\frac{y_{i}^{k,r}}{\rho_{i}^{k}}\right\\|^{2}.\yesnumber$

In order to obtain a close-form solution to the problem (IV), we define $\mathcal{T}_{i}^{k}\in\mathbb{S}^{2T(m+n)}$ as a given positive semidefinite linear operator. Then, it follows that

	$\displaystyle z_{f,i}^{k,r+1}$	$\displaystyle=$	$\displaystyle\arg\min\limits_{z_{f,i}}\delta_{\mathbb{R}_{+}^{2T(m+n)}}(z_{f,i})+\frac{1}{2}\left\\|z_{f,i}-z_{f,i}^{k,r}\right\\|^{2}_{\mathcal{T}_{i}^{k}}$
			$\displaystyle+\frac{\rho_{i}^{k}}{2}\left\\|\mathcal{A}_{i}z_{i}^{k,r+1}+\mathcal{B}_{i}z_{f,i}+s_{i}^{k,r}-h_{i}+\frac{y_{i}^{k,r}}{\rho_{i}^{k}}\right\\|^{2}.\yesnumber$

Then, the optimality condition to the sub-problem of $z_{f,i}$ updating is given by

	$\displaystyle 0$	$\displaystyle\in$	$\displaystyle\partial\delta_{\mathbb{R}_{+}^{2T(m+n)}}(z_{f,i})+\rho^{k}\mathcal{B}_{i}^{T}\mathcal{B}_{i}z_{f,i}+\mathcal{T}_{i}^{k}\left(z_{f,i}-z_{f,i}^{k,r}\right)$
			$\displaystyle+\rho^{k}\mathcal{B}_{i}^{T}\left(\mathcal{A}_{i}z_{i}^{k,r+1}+s_{i}^{k,r}-h_{i}+\frac{y_{i}^{k,r}}{\rho_{i}^{k}}\right).\yesnumber$

Setting $\mathcal{T}_{i}^{k}=\alpha_{i}^{k}I_{2T(m+n)}-\rho_{i}^{k}\mathcal{B}_{i}^{T}\mathcal{B}_{i}$ where $\alpha_{i}^{k}$ is the largest eigenvalue of the matrix $\rho_{i}^{k}\mathcal{B}_{i}^{T}\mathcal{B}_{i}$ , we have

	$\displaystyle 0$	$\displaystyle\in$	$\displaystyle\left(\partial\delta_{\mathbb{R}_{+}^{2T(m+n)}}+\alpha_{i}^{k}I_{2T(m+n)}\right)z_{f,i}-\left(\alpha_{i}^{k}I-\rho_{i}^{k}\mathcal{B}_{i}^{T}\mathcal{B}_{i}\right)z_{f,i}^{k,r}$
			$\displaystyle+\rho^{k}\mathcal{B}_{i}^{T}\left(\mathcal{A}_{i}z_{i}^{k,r+1}+s_{i}^{k,r}-h_{i}+\frac{y_{i}^{k,r}}{\rho_{i}^{k}}\right).\yesnumber$

Then, it follows that

$\displaystyle z_{f,i}^{k,r+1}$	$\displaystyle=$	$\displaystyle\left({\alpha_{i}^{k}}^{-1}\partial\delta_{\mathbb{R}_{+}^{2T(m+n)}}+I_{2T(m+n)}\right)^{-1}$
		$\displaystyle\Bigg{(}\left(I_{2T(m+n)}-{\alpha_{i}^{k}}^{-1}\rho_{i}^{k}\mathcal{B}_{i}^{T}\mathcal{B}_{i}\right)z_{f,i}^{k,r}$
		$\displaystyle\;-{\alpha_{i}^{k}}^{-1}\rho^{k}\mathcal{B}_{i}^{T}\left(\mathcal{A}_{i}z_{i}^{k,r+1}+s_{i}^{k,r}-h_{i}+\frac{y_{i}^{k,r}}{\rho_{i}^{k}}\right)\Bigg{)}.$
$\displaystyle\yesnumber$

Lemma 1.

The projection operator $\operatorname{Proj}_{\mathcal{C}}(\cdot)$ with respect to the convex set $\mathcal{C}$ can be expressed as

\displaystyle\operatorname{Proj}_{\mathcal{C}}(\cdot)=(I+\alpha\partial\delta_{\mathcal{C}})^{-1},

(11)

where $\partial$ is the sub-differential operator, and $\alpha\in\mathbb{R}$ is any arbitrary number.

Proof.

Define a finite dimensional Euclidean space $\mathcal{X}$ equipped with an inner product and its induced norm such that $\mathcal{C}\subset\mathcal{X}$ . For any $x\in\mathcal{X}$ , there exists $z\in\mathcal{X}$ such that $z\in(I+\alpha\partial\delta_{\mathcal{C}})^{-1}(x)$ . Then, it follows that

\displaystyle x

\displaystyle\in

\displaystyle(I+\alpha\partial\delta_{\mathcal{C}})(z)=z+\alpha\partial\delta_{\mathcal{C}}.\yesnumber

The projection operator $\operatorname{Proj}_{\mathcal{C}}(z)$ is

\displaystyle\operatorname{Proj}_{\mathcal{C}}(z)

\displaystyle=

\displaystyle\underset{z}{\operatorname{argmin}}\left\{\delta_{\mathcal{C}}(x)+\frac{1}{2\alpha}\|z-x\|^{2}\right\}.

(12)

Due to the fact that the optimization problem (12) is strictly convex, the sufficient and necessary condition of (12) is

\displaystyle 0\in\alpha\partial\delta_{\mathcal{C}}(x)+z-x.

(13)

Since (13) is equivalent to (12) and the projection onto a convex set is unique, the operator $(I+\alpha\partial\delta_{\mathcal{C}})^{-1}$ is a point-to-point mapping. This completes the proof of Lemma 1. ∎

Based on Lemma 1, $z_{f,i}$ can be determined as

	$\displaystyle z_{f,i}^{k,r+1}$	$\displaystyle=\operatorname{Proj}_{\mathbb{R}_{+}^{2T(m+n)}}\Bigg{\{}\left(I_{2T(m+n)}-{\alpha_{i}^{k}}^{-1}\rho_{i}^{k}\mathcal{B}_{i}^{T}\mathcal{B}_{i}\right)z_{f,i}^{k,r}$
		$\displaystyle-{\alpha_{i}^{k}}^{-1}\rho^{k}\mathcal{B}_{i}^{T}\left(\mathcal{A}_{i}z_{i}^{k,r+1}+s_{i}^{k,r}-h_{i}+\frac{y_{i}^{k,r}}{\rho_{i}^{k}}\right)\Bigg{\}},\yesnumber$

where the projection operator $\operatorname{Proj}_{\mathbb{R}_{+}^{2T(m+n)}}(z)$ means the projection of $z$ onto a closed convex set $\mathbb{R}_{+}^{2T(m+n)}$ .

Lemma 2.

With $\mathcal{C}=\mathbb{R}_{+}^{n}$ , then for any $z\in\mathbb{R}^{n}$ , the projection of $z$ onto $\mathcal{C}$ is given by

\displaystyle\operatorname{Proj}_{\mathcal{C}}(z)=\max\{z,0\}.

(14)

According to Lemma 2, $z_{f,i}^{k,r+1}$ can be easily obtained.

3. Update $s_{i}$ :

$\displaystyle s_{i}^{k,r+1}$	$\displaystyle=$	$\displaystyle\underset{s_{i}}{\operatorname{argmin}}\;\mathcal{L}_{\rho_{i}^{k}}\left(z_{i}^{k,r+1},z_{f,i}^{k,r+1},s_{i};y_{i}^{k,r},\lambda_{i}^{k},\rho_{i}^{k},\beta_{i}^{k}\right)$
	$\displaystyle=$	$\displaystyle-\frac{\rho_{i}^{k}}{\rho_{i}^{k}+\beta_{i}^{k}}\left(\mathcal{A}_{i}z_{i}^{k,r+1}+\mathcal{B}_{i}z_{f,i}^{k,r+1}-h_{i}+\frac{y_{i}^{k,r}}{\rho_{i}^{k}}\right)$
		$\displaystyle\;-\frac{\lambda_{i}^{k}}{\rho_{i}^{k}+\beta_{i}^{k}}.\yesnumber$

4. Update $y_{i}$ :

$\displaystyle y_{i}^{k,r+1}$	$\displaystyle=$	$\displaystyle\underset{y_{i}}{\operatorname{argmin}}\;\mathcal{L}_{\rho_{i}^{k}}\left(z_{i}^{k,r+1},z_{f,i}^{k,r+1},s_{i}^{k,r+1};y_{i},\lambda_{i}^{k},\rho_{i}^{k},\beta_{i}^{k}\right)$
	$\displaystyle=$	$\displaystyle y_{i}^{k,r}+\rho_{i}^{k}\left(\mathcal{A}_{i}z_{i}^{k,r+1}+\mathcal{B}_{i}z_{f,i}^{k,r+1}+s_{i}^{k,r+1}-h_{i}\right).$
$\displaystyle\yesnumber$

Assumption 2.

Given $\lambda_{i}^{k}$ , $\beta_{i}^{k}$ and $\rho_{i}^{k}$ , the $z_{i}$ -update can find a stationary solution $z_{i}^{r}$ such that

$\displaystyle 0$	$\displaystyle\in$	$\displaystyle\partial_{z_{i}}\mathcal{L}_{\rho_{i}^{k}}(z_{i}^{r},z_{f,i}^{r-1},s_{i}^{r-1},y_{i}^{r-1})$
$\displaystyle\mathcal{L}_{\rho_{i}^{k}}(z_{i}^{r},z_{f,i}^{r-1},s_{i}^{r-1},y_{i}^{r-1})$	$\displaystyle\leq$	$\displaystyle\mathcal{L}_{\rho_{i}^{k}}(z_{i}^{r-1},z_{f,i}^{r-1},s_{i}^{r-1},y_{i}^{r-1}),$
$\displaystyle\yesnumber$

for all $a\in\mathbb{Z}_{+}$ .

Lemma 3.

For all $a\in\mathbb{Z}_{+}$ , the following conditions holds:

	$\displaystyle\left\langle\mathcal{B}_{i}^{\top}y_{i}^{r-1}+\rho_{i}\mathcal{B}_{i}^{\top}\left(\mathcal{A}_{i}z_{i}^{r}+\mathcal{B}_{i}z_{f,i}^{r}+s_{i}^{r-1}-h_{i}\right),\right.$
	$\displaystyle\left.(z_{f,i}-z_{f,i}^{r})\right\rangle\geq 0,\;\forall z_{f,i}\in\mathbb{R}_{+}^{2T(m+n)}$
	$\displaystyle\lambda_{i}+\beta_{i}s_{i}^{r}+y_{i}^{r}=0.\yesnumber$

Proof.

This lemma can be straightforwardly derived based on the optimality conditions of $z_{i}$ - and $z_{f,i}$ -updates, i.e., (III-B) and (III-B), and thus the proof is omitted. ∎

Theorem 1.

With $\rho_{i}^{k}\geq\sqrt{2}\beta_{i}^{k}$ , the inner iteration of problem (III-B) converges to the stationary point $(z_{i}^{k},z_{f,i}^{k},s_{i}^{k},y_{i}^{k})$ of the transformed problem (III-B).

Proof.

With Assumption 2, we have

	$\displaystyle\mathcal{L}_{\rho_{i}^{k}}\left(z_{i}^{r-1},z_{f,i}^{r-1},s_{i}^{r-1},y_{i}^{r-1}\right)\geq\mathcal{L}_{\rho_{i}^{k}}\left(z_{i}^{r},z_{f,i}^{r-1},s_{i}^{r-1},y_{i}^{r-1}\right).$
	$\displaystyle\yesnumber$

Besides, we also have

			$\displaystyle\mathcal{L}_{\rho_{i}^{k}}\left(z_{i}^{r},z_{f,i}^{r-1},s_{i}^{r-1},y_{i}^{r-1}\right)-\mathcal{L}_{\rho_{i}^{k}}\left(z_{i}^{r},z_{f,i}^{r},s_{i}^{r-1},y_{i}^{r-1}\right)$
		$\displaystyle=$	$\displaystyle\Big{\langle}\mathcal{B}_{i}^{\top}y_{i}^{r-1}+\rho_{i}^{k}\mathcal{B}_{i}^{\top}\left(\mathcal{A}_{i}z_{i}^{r}+\mathcal{B}_{i}z_{f,i}^{r}+s_{i}^{r-1}-h_{i}\right),$
			$\displaystyle\quad z_{f,i}^{r-1}-z_{f,i}^{r}\Big{\rangle}+\frac{\rho_{i}^{k}}{2}\left\\|\mathcal{B}_{i}(z_{f,i}^{r-1}-z_{f,i}^{r})\right\\|^{2}$
		$\displaystyle\geq$	$\displaystyle\frac{\rho_{i}^{k}}{2}\left\\|\mathcal{B}_{i}(z_{f,i}^{r-1}-z_{f,i}^{r})\right\\|^{2}.\yesnumber$

Note that the equality in (IV) holds because of the following claim:

	$\displaystyle\left\\|\mathcal{A}_{i}z_{i}^{r}+s_{i}^{r-1}-h_{i}+\mathcal{B}_{i}z_{f,i}^{r-1}\right\\|^{2}-\left\\|\mathcal{A}_{i}z_{i}^{r}+s_{i}^{r-1}-h_{i}+\mathcal{B}_{i}z_{f,i}^{r}\right\\|^{2}$
	$\displaystyle=2\left\langle\mathcal{A}_{i}z_{i}^{r}+s_{i}^{r-1}-h_{i}+\mathcal{B}_{i}z_{f,i}^{r},\mathcal{B}_{i}z_{f,i}^{r-1}-\mathcal{B}_{i}z_{f,i}^{r}\right\rangle$
	$\displaystyle\quad+\left\\|\mathcal{B}_{i}z_{f,i}^{r-1}-\mathcal{B}_{i}z_{f,i}^{r}\right\\|^{2}.\yesnumber$

In addition, the inequality in (IV) can be obtained from Lemma 3. Now, the descent of $z_{i}$ and $z_{f,i}$ updates has been proved. Next, we will show the descent of $s_{i}$ and $y_{i}$ updates.

			$\displaystyle\mathcal{L}_{\rho_{i}^{k}}\left(z_{i}^{r},z_{f,i}^{r},s_{i}^{r-1},y_{i}^{r-1}\right)-\mathcal{L}_{\rho_{i}^{k}}\left(z_{i}^{r},z_{f,i}^{r},s_{i}^{r},y_{i}^{r}\right)$
		$\displaystyle=$	$\displaystyle\left\langle\lambda_{i}^{k},(s_{i}^{r-1}-s_{i}^{r})\right\rangle+\frac{\beta_{i}^{k}}{2}\left(\left\\|s_{i}^{r-1}\\|^{2}-\\|s_{i}^{r}\right\\|^{2}\right)$
			$\displaystyle\>+\left\langle y_{i}^{r},s_{i}^{r-1}-s_{i}^{r}\right\rangle+\frac{\rho_{i}^{k}}{2}\left\\|s_{i}^{r-1}-s_{i}^{r}\right\\|^{2}$
			$\displaystyle\>-\rho_{i}^{k}\left\\|\mathcal{A}_{i}z_{i}^{r}+\mathcal{B}_{i}z_{f,i}^{r}+s_{i}^{r}-h_{i}\right\\|^{2}$
		$\displaystyle\geq$	$\displaystyle\left(\frac{\rho_{i}^{k}}{2}-\frac{{\beta_{i}^{k}}^{2}}{\rho_{i}^{k}}\right)\left\\|s_{i}^{r-1}-s_{i}^{r}\right\\|^{2}.\yesnumber$

Remarkably, the first equality holds due to (IV) and the following derivation:

			$\displaystyle-\rho_{i}^{k}\langle\mathcal{A}_{i}z_{i}^{r}+\mathcal{B}_{i}z_{f,i}^{r}-h_{i}+s_{i}^{r},\mathcal{A}_{i}z_{i}^{r}+\mathcal{B}_{i}z_{f,i}^{r}-h_{i}+s_{i}^{r-1}\rangle$
			$\displaystyle+\frac{\rho_{i}^{k}}{2}\Big{(}\left\\|\mathcal{A}_{i}z_{i}^{r}+\mathcal{B}_{i}z_{f,i}^{r}-h_{i}+s_{i}^{r-1}\right\\|^{2}$
			$\displaystyle\quad-\left\\|\mathcal{A}_{i}z_{i}^{r}+\mathcal{B}_{i}z_{f,i}^{r}-h_{i}+s_{i}^{r}\right\\|^{2}\Big{)}$
		$\displaystyle=$	$\displaystyle\frac{\rho_{i}^{k}}{2}\left\\|s_{i}^{r-1}-s_{i}^{r}\right\\|^{2}-\rho_{i}^{k}\left\\|\mathcal{A}_{i}z_{i}^{r}+\mathcal{B}_{i}z_{f,i}^{r}-h_{i}+s_{i}^{r}\right\\|^{2}.\yesnumber$

To prove the inequality in (IV), we define a function $f(s_{i})=\langle\lambda_{i}^{r-1},s_{i}\rangle+\frac{\beta_{i}}{2}\|s_{i}\|^{2}$ . The gradient of $f(s_{i})$ is $\nabla f(s_{i}^{r})=\lambda_{i}^{r-1}+\beta_{i}s_{i}^{r}=-y_{i}^{r}$ due to (III-B). It follows that $f(s_{i}^{r-1})-f(s_{i}^{r})+(y_{i}^{r})^{\top}(s_{i}^{r-1}-s_{i}^{r})\geq 0$ , as $f(s_{i})$ is convex. Therefore, the inequality in (IV) holds.

Under the condition $\rho_{i}\geq\sqrt{2}\beta_{i}$ , the descent of $\mathcal{L}_{\rho_{i}^{k}}$ with updating $(z_{i}^{k},z_{f,i}^{k},s_{i}^{k},y_{i}^{k})$ can be proved, which means the augmented Lagrangian function is sufficient descent.

Since the function $f(s_{i})$ , a part of the augmented Lagrangian function, is Lipschitz differentiable with $\beta_{i}$ , and $\lambda_{i}$ is bounded by $[\underline{\lambda_{i}},\overline{\lambda_{i}}]$ , it is obvious that $\mathcal{L}_{\rho_{i}^{k}}\left(z_{i}^{r},z_{f,i}^{r},s_{i}^{r},y_{i}^{r}\right)$ is lower bounded [28]. Thus, the solution of the inner loop of this transformed problem (III-B) will converge to the stationary point $(z_{i}^{k},z_{f,i}^{k},s_{i}^{k},y_{i}^{k})$ . This completes the proof of Theorem 1. ∎

5. Update $\lambda_{i}$ and $\beta_{i}$ : Then, in the outer iterations, the dual variable $\lambda_{i}^{k}$ is updated. In order to drive the slack variable $s_{i}$ to zero, the penalty parameter $\beta_{i}^{k}$ is multiplied by a ratio $\gamma>1$ if the currently computed $s_{i}^{k}$ from the inner iterations does not decrease enough from the previous outer iteration $s_{i}^{k-1}$ , i.e., $\|s_{i}^{k}\|>\omega\|s_{i}^{k-1}\|,\omega\in(0,1)$ .

	$\displaystyle\lambda_{i}^{k+1}$	$\displaystyle=$	$\displaystyle\operatorname{Proj}_{[\underline{\lambda}_{i},\overline{\lambda}_{i}]}(\lambda_{i}^{k}+\beta_{i}^{k}s_{i}^{k})$
	$\displaystyle\beta^{k+1}$	$\displaystyle=$	$\displaystyle\begin{cases}\gamma\beta_{i}^{k},&\\|s_{i}^{k}\\|>\omega\\|s_{i}^{k-1}\\|\\ \beta_{i}^{k},&\\|s_{i}^{k}\\|\leq\omega\\|s_{i}^{k-1}\\|,\end{cases}\yesnumber$

where the projection operator $\operatorname{Proj}_{[\underline{\lambda}_{i},\overline{\lambda}_{i}]}(\lambda_{i}^{k}+\beta_{i}^{k}s_{i}^{k})$ means the projection of $\lambda_{i}^{k}+\beta_{i}^{k}s_{i}^{k}$ onto a closed convex set $[\underline{\lambda}_{i},\overline{\lambda}_{i}]$ .

Lemma 4.

Given a hypercube set $[a,b]$ with $a,b\in\mathbb{R}^{n}$ and $a<b$ , for any $z\in\mathbb{R}^{n}$ , the projection of $z$ onto the hypercube $[a,b]$ is given by

\displaystyle~{}\operatorname{Proj}_{[a,b]}(z)

\displaystyle=

\displaystyle\begin{cases}z_{(i)},&a_{(i)}\leq z_{(i)}\leq b_{(i)}\\ a_{(i)},&z_{(i)}<a_{(i)}\\ b_{(i)},&z_{(i)}>b_{(i)},\\ \end{cases}

(15)

where the subscript $\cdot_{(i)}$ means the $i$ th component in vector $z$ , $a$ and $b$ .

Theorem 2.

Suppose that the point $(z_{i}^{k},z_{f,i}^{k},s_{i}^{k},y_{i}^{k})$ satisfies the optimality condition (10a). For any $\epsilon_{1},\epsilon_{2},\epsilon_{3}>0$ , after finite outer iterations $k$ , appropriate stationary point $(z_{i}^{k},z_{f,i}^{k},s_{i}^{k},y_{i}^{k})$ can be found such that the optimality condition of the original problem (III-A) before introducing the slack variable $s_{i}$ is also satisfied [28], i.e.,

	$\displaystyle 0\in\nabla J_{i}(z_{i}^{k})+\mathbb{N}_{\mathcal{Z}_{i}}(z_{i}^{k})+\mathcal{A}_{i}^{\top}y_{i}^{k}$		(16a)
	$\displaystyle 0\in\mathcal{B}_{i}^{\top}y_{i}^{k}+\mathbb{N}_{\mathbb{R}_{+}^{2T(m+n)}}\left(z_{f,i}^{k}\right)$
	$\displaystyle\mathcal{A}_{i}z_{i}^{k}+\mathcal{B}_{i}z_{f,i}^{k}-h_{i}=0.$

Proof.

Assume that $(z_{i}^{k},z_{f,i}^{k},s_{i}^{k},y_{i}^{k})$ converges to the stationary point $(z_{i}^{\star},z_{f,i}^{\star},s_{i}^{\star},y_{i}^{\star})$ . We have $z_{i}^{\star}\in\mathcal{Z}_{i},z_{f,i}\in\mathbb{R}_{+}^{2T(m+n)}$ and $\mathcal{A}_{i}z_{i}^{\star}+\mathcal{B}_{i}z_{f,i}^{\star}-h_{i}+s_{i}^{\star}=0$ .

For the case where $\beta_{i}^{k}$ is bounded, we have $z_{i}^{k}\rightarrow 0$ and $z_{i}^{\star}=0$ . For the case where $\beta_{i}^{k}$ is not bounded, based on the optimality condition (10a), it follows that

\displaystyle\frac{{\lambda_{i}}^{k}}{\beta_{i}^{k}}+s_{i}^{k,r}+\frac{{y_{i}}^{k,r}}{\beta_{i}^{k}}=0.

(18)

Then, we have $s_{i}^{\star}=0$ by taking the limitation of (18) to zero, as $y_{i}^{k}\rightarrow y_{i}^{\star}$ and $\lambda_{i}^{k}\in[\underline{\lambda}_{i},\overline{\lambda}_{i}]$ . Since we have $s_{i}^{\star}=0$ in both of the two cases, the optimality condition with respect to $y_{i}$ , i.e., (2), can be satisfied. Besides, the optimality condition with respect to $z_{i}$ and $z_{f,i}$ , i.e., (16a) and (2), can also be satisfied by taking $k\rightarrow 0$ on (10a) and (III-B), respectively. This completes the proof of Theorem 2. ∎

Remark 3.

Our algorithm explicitly demonstrates how to apply the hierarchical ADMM in the multi-agent system with the presence of nonconvex collision-avoidance constraints. The closed-form solution of every variable is provided. In this approach, a proximal term is introduced to the two-level algorithm presented in [28] and a more compact convergence condition is proposed, i.e., $\rho_{i}\geq\sqrt{2}\beta_{i}$ . Therefore, the Theorem 1 in our paper proves the convergence of the inner iteration under the condition $\rho_{i}\geq\sqrt{2}\beta_{i}$ . Theorem 2 is proved under different assumptions, compared with the algorithm proposed in [28]. Thus, we believe our algorithm and theorems provide more details about the hierarchical ADMM algorithms for the multi-gent systems.

The stopping criterion of this ADMM algorithm is given by

	$\displaystyle\left\\|\rho_{i}^{k}\mathcal{A}_{i}^{T}\left(\mathcal{B}_{i}z_{f,i}^{r}+s_{i}^{r}-\mathcal{B}_{i}z_{f,i}^{r+1}-s_{i}^{r+1}\right)\right\\|\leq\epsilon_{1}^{k}$		(19a)
	$\displaystyle\left\\|\rho_{i}^{k}\mathcal{B}_{i}^{T}\left(s_{i}^{r}-s_{i}^{r+1}\right)\right\\|\leq\epsilon_{2}^{k}$
	$\displaystyle\left\\|\mathcal{A}_{i}z_{i}^{r+1}+\mathcal{B}_{i}z_{f,i}^{r+1}-h_{i}+s_{i}^{r+1}\right\\|\leq\epsilon_{3}^{k},\textbf{}$

where $\epsilon_{e}^{k}\rightarrow 0,\forall e\in\{1,2,3\}$ .

To summarize the above developments, the pseudocode of this algorithm is presented in Algorithm 1. After obtaining each agent’s control input and executing the control input, the agents will communicate with their neighbors to obtain the updated position information of their neighbors. Then, this process will be repeated until finishing the designed task.

Algorithm 1 Hierarchical ADMM for Multi-Agent Decision Making

Initialize

z_{i}^{0}\in\mathbb{R}^{T(m+n)},z_{f,i}^{0}\in\mathbb{R}^{2T(m+n)},s_{i}^{0}\in\mathbb{R}^{2Tm+3Tn}

; initialize the dual variable

\lambda_{i}^{0}\in[\underline{\lambda}_{i},\overline{\lambda}_{i}]

, where

\underline{\lambda}_{i},\overline{\lambda}_{i}\in\mathbb{R}^{2Tm+3Tn}

, and

\overline{\lambda}_{i}>\underline{\lambda}_{i}

; initialize the penalty parameter

\beta_{i}^{0}>0

, and the constant

\omega\in[0,1)

and

\gamma>1

; initialize the sequence of tolerance

\{\epsilon_{1}^{k},\epsilon_{2}^{k},\epsilon_{3}^{k}\}\in\mathbb{R}_{+}

with

\lim_{k\rightarrow\infty}\epsilon_{e}^{k}=0,\forall e\in\{1,2,3\}

. Given the terminated tolerances

\epsilon_{\text{term},e},e=\{1,2,3\}

, if

\epsilon_{e}^{k}\leq\epsilon_{\text{term},e}

\epsilon_{e}^{k}=\epsilon_{\text{term},i}

for

e=\{1,2,3\}

Set the outer iteration

k=0

while Stopping criterion is violated do

Given

\lambda_{i}^{k}

and

\beta_{i}^{k}

, initialize

z_{i}^{k,0},z_{f,i}^{k,0},s_{i}^{k,0}

and

y_{i}^{k,0}

such that

\lambda_{i}^{k}+\beta_{i}^{k}s_{i}^{k,0}+y_{i}^{k,0}=0

; initialize tolerance

(\epsilon_{1}^{k},\epsilon_{2}^{k},\epsilon_{3}^{k})

;

\rho_{i}^{k}\geq\sqrt{2}\beta_{i}^{k}

Set the inner iteration

r=0

while Stopping criterion is violated do

Update

z_{i}^{k,r+1}

by (IV) via nonlinear programming solvers.

Update

z_{f,i}^{k,r+1}

s_{i}^{k,r+1}

y_{i}^{k,r+1}

by (IV), (IV), (IV), respectively.

r=r+1

end while

Set

z_{i}^{k+1},z_{f,i}^{k+1},s_{i}^{k+1}

y_{i}^{k+1}

to be

z_{i}^{k,r+1},z_{f,i}^{k,r+1},s_{i}^{k,r+1}

y_{i}^{k,r+1}

, respectively.

Update

\lambda_{i}^{k+1}

and

\beta_{i}^{k+1}

by (IV).

k=k+1

end while

V Improved Hierarchical ADMM

In the hierarchical ADMM approach, the minimization of $z_{i}$ in each iteration is computationally expensive, and it requires nonlinear programming (NLP) solvers. Therefore, the improved minimization of $z_{i}$ is required to improve the computational efficiency by approximating and solving this inexact minimization problem.

The original $z_{i}$ -minimization problem can be expressed as

	$\displaystyle\underset{z_{i}}{\operatorname{min}}$		$\displaystyle J_{i}(z_{i})+\frac{\rho_{i}^{k}}{2}\left\\|\mathcal{A}_{i}z_{i}+\mathcal{B}_{i}z_{f,i}^{k,r}+s_{i}^{k,r}-h_{i}+\frac{y_{i}^{k,r}}{\rho_{i}^{k}}\right\\|^{2}$
	$\displaystyle\operatorname{s.t.}$		$\displaystyle\phi\left(z_{i};[p_{j}]\right)\leq 0$
			$\displaystyle\psi\left(z_{i};[p_{j}]\right)\leq 0,\yesnumber$

where

	$\displaystyle\phi\left(z_{i};[p_{j}]\right)$	$\displaystyle=$	$\displaystyle\mathcal{M}\left(z_{i};[p_{j}]\right)\mathcal{P}\left(z_{i};[p_{j}]\right)-d_{\mathrm{cmu}}^{2}$
	$\displaystyle\psi\left(z_{i};[p_{j}]\right)$	$\displaystyle=$	$\displaystyle d_{\mathrm{safe}}^{2}-\mathcal{M}\left(z_{i};[p_{j}]\right)\mathcal{P}\left(z_{i};[p_{j}]\right).$

The KKT conditions of this $z_{i}$ -minimization problem (V) are

$\displaystyle 0$	$\displaystyle=$	$\displaystyle\nabla J_{i}(z_{i})+{\rho_{i}^{k}}\mathcal{A}_{i}^{\top}\left(\mathcal{A}_{i}z_{i}+\mathcal{B}_{i}z_{f,i}^{k,r}+s_{i}^{k,r}-h_{i}+\frac{y_{i}^{k,r}}{\rho_{i}^{k}}\right)$
		$\displaystyle+\sum\limits_{\theta=1}^{\theta_{\phi}}\mu_{\theta,i}\nabla\phi_{\theta}\left(z_{i};[p_{j}]\right)+\sum\limits_{\theta=1}^{\theta_{\psi}}\kappa_{\theta,i}\nabla\psi_{\theta}\left(z_{i};[p_{j}]\right)$
$\displaystyle 0$	$\displaystyle=$	$\displaystyle\mu_{\theta,i}\phi_{\theta}\left(z_{i};[p_{j}]\right),\>\forall\theta=1,2,\cdots,\theta_{\phi}$
$\displaystyle 0$	$\displaystyle=$	$\displaystyle\kappa_{\theta,i}\psi_{\theta}\left(z_{i};[p_{j}]\right),\>\forall\theta=1,2,\cdots,\theta_{\psi}.\yesnumber$

Notice that the hierarchical ADMM algorithm requires complete minimization of $z_{i}$ in each inner iteration. However, this is neither desirable due to the computational cost nor practical, as any NLP solver finds only a point that approximately satisfies the KKT optimality conditions. In the hierarchical ADMM algorithm, we propose the use of the interior-point method to solve the $z_{i}$ minimization problem (V). The interior-point method employs double-layer iterations to find the solution. In the outer iteration, a barrier technique is used to convert the inequality constraints into an additional term in the objective; the stationary points of resulting barrier problems converge to true stationary points as the barrier parameter converges to 0. In the inner iteration, a proper search method is used to obtain the optimum of the barrier problem. Since both the interior-point algorithm and the hierarchical ADMM algorithm have a double-layer structure, we consider matching these two layers.

In order to accelerate the computation process of solving (V), the barrier method can be employed to transform the inequality constraints to additional terms in the objective function. For example, in the $k$ -th outer loop, the function $J_{i}(z_{i})$ can be added with a barrier term $-b_{i}^{k}\sum_{\theta_{\phi}}\ln(-\phi(z_{i}))$ based on the barrier method, where $b_{i}^{k}$ is the barrier parameter with $\lim\limits_{k\rightarrow\infty}b_{i}^{k}=0$ . Hence, the barrier augmented Lagrangian in the $k$ -th outer loop can be written as

\displaystyle{\hat{\mathcal{L}}}_{b_{i}^{k}}=\mathcal{L}_{\rho_{i}^{k}}-b_{i}^{k}\sum\limits_{\theta=1}^{\theta_{\phi}}\ln(-\phi_{\theta}(z_{i})).

(22)

If the $z_{i}$ -minimization step returns $z_{i}^{k,r+1}$ by minimizing $\hat{\mathcal{L}}_{b_{i}}$ with respect to $z_{i}$ , then the inner iterations lead to the decrease of $\hat{\mathcal{L}}_{b^{k}_{i}}$ , which also matches the KKT conditions of the original optimization problem. Therefore, the outer iterations can find an approximate stationary point of the original problem with the decrease of the barrier parameter $b_{i}^{k}$ . Note that only $\psi(z_{i};[p_{j}])$ is used as the inequality constraint for the purpose of demonstration.

Therefore, it remains to minimize $\hat{\mathcal{L}}_{b_{i}}$ , which is given by

	$\displaystyle\hat{\mathcal{L}}_{b_{i}^{k}}$	$\displaystyle=$	$\displaystyle J_{i}(z_{i})+\frac{\rho_{i}^{k}}{2}\left\\|\mathcal{A}_{i}z_{i}+\mathcal{B}_{i}z_{f,i}^{k,r}+s_{i}^{k,r}-h_{i}+\frac{y_{i}^{k,r}}{\rho_{i}^{k}}\right\\|^{2}$
			$\displaystyle-\left\langle b_{i}^{k}1_{Tr_{i}},\ln\left(h\left(z_{i};[p_{j}]\right)-d_{\mathrm{safe}}^{2}\right)\right\rangle,\yesnumber$

where $h\left(z_{i};[p_{j}]\right)={\mathcal{M}\left(z_{i};[p_{j}]\right)\mathcal{P}\left(z_{i};[p_{j}]\right)}$ .

Lemma 5.

The gradient of $h\left(z_{i};[p_{j}]\right)$ with respect to $z_{i}$ is a linear mapping with respect to $z_{i}$ .

Proof.

Define $W=I_{Tr_{i}}\otimes 1_{n_{p}}\in\mathbb{R}^{Tr_{i}n_{p}\times Tr_{i}}$ , $\omega=(I_{T}\otimes 1_{r_{i}}\otimes I_{n_{p}})M_{p}z_{i}-1_{T}\otimes[p_{j}]\in\mathbb{R}^{Tr_{i}n_{p}}$ , $\Omega=\operatorname{diag}(\omega)\in\mathbb{R}^{Tr_{i}n_{p}\times Tr_{i}n_{p}}$ . Then,

\displaystyle h\left(z_{i};[p_{j}]\right)=W^{\top}(I_{Tr_{i}n_{p}}\odot(\omega 1_{Tr_{i}n_{p}}^{\top}))\omega=W^{\top}\Omega\omega.

Since $\Omega=I_{Tr_{i}n_{p}}\odot(\omega 1_{Tr_{i}n_{p}}^{\top})=I_{Tr_{i}n_{p}}\odot(\mathbf{1}_{Tr_{i}n_{p}}\omega^{\top})$ , we have

\displaystyle\textup{d}h\left(z_{i};[p_{j}]\right)

\displaystyle=

\displaystyle 2W^{\top}\Omega(I_{T}\otimes 1_{r_{i}}\otimes I_{n_{p}})M_{p}\textup{d}z_{i}.\yesnumber

It follows

			$\displaystyle\frac{\textup{d}h\left(z_{i};[p_{j}]\right)}{\textup{d}z_{i}}$
		$\displaystyle=$	$\displaystyle 2(I_{Tr_{i}}\otimes 1_{n_{p}})^{\top}\Big{[}I_{Tr_{i}n_{p}}\odot\Big{(}\big{(}(I_{T}\otimes 1_{r_{i}}\otimes I_{n_{p}})M_{p}z_{i}$
			$\displaystyle\;-1_{T\times 1}\otimes[p_{j}]\big{)}1_{Tr_{i}n_{p}}^{T}\Big{)}\Big{]}\cdot(I_{T}\otimes 1_{r_{i}}\otimes I_{n_{p}})M_{p}.\yesnumber$

Therefore, Lemma 5 is proved. ∎

Set $\Upsilon_{i}=-\psi\left(z_{i};[p_{j}]\right)$ and define $\Xi_{i}$ such that $\Xi_{i}\odot\Upsilon_{i}=1_{Tr_{i}}$ . Then, the gradient of the barriered augmented Lagrangian function $\hat{\mathcal{L}}_{b_{i}^{k}}$ is

		$\displaystyle\nabla\hat{\mathcal{L}}_{b_{i}^{k}}$
	$\displaystyle=$	$\displaystyle 2H_{i}(z_{i}-z_{i,ref})+{\rho_{i}^{k}}\mathcal{A}_{i}^{\top}\Bigg{(}\mathcal{A}_{i}z_{i}+\mathcal{B}_{i}z_{f,i}^{k,r}+s_{i}^{k,r}-h_{i}$
		$\displaystyle+\frac{y_{i}^{k,r}}{\rho_{i}^{k}}\Bigg{)}-2b_{i}^{k}\operatorname{Tr}\left(1_{Tr_{i}}^{\top}\Xi_{i}\odot W^{\top}\Omega(I_{T}\otimes 1_{r_{i}}\otimes I_{n_{p}})M_{p}\right).$
$\displaystyle\yesnumber$

A closed-form algebraic solution of problem (V) is challenging to determine. However, since the gradient $\nabla\hat{\mathcal{L}}_{b_{i}^{k}}$ is available, some well-known gradient-based numerical methods, e.g., Barzilai-Borwein, Polak-Ribiere, and Fletcher-Reeves, can be used to calculate the optimal $z_{i}$ . Therefore, the $z_{i}$ -update step in Algorithm 1 can be replaced by solving an unconstrained optimization problem (V) with the known gradient (V).

Usually, iterative algorithms for NLP need to be called when solving the $z_{i}$ minimization problem (V), and always searching for a highly accurate solution in each ADMM iteration will result in an excessive computational cost. It is thus desirable to solve the optimization subproblem (V) in ADMM inexactly when the dual variables are yet far from the optimum, i.e., to allow $z_{i}^{k,r+1}$ to be chosen such that

\displaystyle d_{x}^{k+1}\in\partial_{z_{i}}\mathcal{L}_{\rho_{i}}.

(23)

In this paper, we use externally shrinking and summable sequence of the absolute errors, i.e.,

\displaystyle\|d_{z}^{k}\|\leq\epsilon_{4}^{k},\sum_{k=1}^{\infty}\epsilon_{4}^{k}\leq\infty.

(24)

such as a summable absolute error sequence can effectively reduce the total number of subroutine iterations throughout the ADMM algorithm. Such a criterion is a constructive one, rendered to guarantee the decrease of a quadratic distance between the intermediate solutions $(z_{i}^{k},z_{f,i}^{k},s_{i}^{k},y_{i}^{k})$ and the optimum $(z_{i}^{*},z_{f,i}^{k},s_{i}^{k},y_{i}^{k})$ .

The above approximate NLP solution is realizable by NLP solvers where the tolerances of the KKT conditions are allowed to be specified by the user, e.g., the IPOPT solver. The pseudocode of the detailed algorithm is shown in Algorithm 2.

Algorithm 2 Improved Hierarchical ADMM for Multi-Agent Decision Making

Initialize

z_{i}^{0}\in\mathbb{R}^{T(m+n)},z_{f,i}^{0}\in\mathbb{R}^{2T(m+n)},s_{i}^{0}\in\mathbb{R}^{2Tm+3Tn}

; initialize the dual variable

\lambda_{i}^{0}\in[\underline{\lambda}_{i},\overline{\lambda}_{i}]

, where

\underline{\lambda}_{i},\overline{\lambda}_{i}\in\mathbb{R}^{2Tm+3Tn}

, and

\overline{\lambda}_{i}>\underline{\lambda}_{i}

; initialize the penalty parameter

\beta_{i}^{0}>0

, and the constant

\omega\in[0,1)

and

\gamma>1

; initialize the sequence of tolerance

\{\epsilon_{1}^{k},\epsilon_{2}^{k},\epsilon_{3}^{k},epsilon_{4}^{k},\epsilon_{b}\}\in\mathbb{R}_{+}

with

\lim_{k\rightarrow\infty}\epsilon_{e}^{k}=0,\forall e\in\{1,2,3,4,b\}

. Given the terminated tolerances

\epsilon_{\text{term},e},e=\{1,2,3,4,b\}

, if

\epsilon_{e}^{k}\leq\epsilon_{\text{term},e}

\epsilon_{e}^{k}=\epsilon_{\text{term},i}

for

e=\{1,2,3,4,b\}

Set the outer iteration

k=0

while Stopping criterion is violated or

b_{i}^{k}\geq\epsilon_{b}

Given

\lambda_{i}^{k}

and

\beta_{i}^{k}

, initialize

z_{i}^{k,0},z_{f,i}^{k,0},s_{i}^{k,0}

and

y_{i}^{k,0}

such that

\lambda_{i}^{k}+\beta_{i}^{k}s_{i}^{k,0}+y_{i}^{k,0}=0

; initialize tolerance

(\epsilon_{1}^{k},\epsilon_{2}^{k},\epsilon_{3}^{k})

;

\rho_{i}^{k}\geq\sqrt{2}\beta_{i}^{k}

Set the inner iteration

r=0

while Stopping criterion is violated do

Update

z_{i}^{k,r+1}

by minimize (22).

Update

z_{f,i}^{k,r+1}

s_{i}^{k,r+1}

y_{i}^{k,r+1}

by (IV), (IV), (IV), respectively.

r=r+1

end while

Set

z_{i}^{k+1},z_{f,i}^{k+1},s_{i}^{k+1}

y_{i}^{k+1}

to be

z_{i}^{k,r+1},z_{f,i}^{k,r+1},s_{i}^{k,r+1}

y_{i}^{k,r+1}

, respectively.

Update

\lambda_{i}^{k+1}

and

\beta_{i}^{k+1}

by (IV).

k=k+1

end while

In this method, a nonlinear programming problem without constraints is solved with inequality constraints handled by a fixed barrier constant throughout inner iterations. The barrier constant decreases across outer iterations. In the hierarchical ADMM, the NLP solver needs to solve an inequality-constrained NLP in each inner iteration. When the NLP solver, i.e., interior-point algorithm, is called for NLP with inequalities, the barrier constant will go through an iteration inside the solver, and thus every inner iteration will have more loops of internal iteration. By this barrier Lagrangian construction, we integrate the inner loop of the IPOPT with the inner loop of the hierarchical ADMM algorithm. In addition, in the improved hierarchical ADMM, each inner iteration only solves NLP, and an approximate solution is reached without the need of convergence. The accuracy, namely the tolerances of the errors from the KKT conditions, can be controlled when using the interior-point algorithm. Therefore, these tolerances can be set adaptively throughout the iterations (for instance, the tolerances of the 1st, 5th, and 20th iteration can be set to 0.1, 0.01, and 0.001, respectively). By this adaptive tolerance setting, we integrate the outer loop of the IPOPT with the inner loop of the hierarchical ADMM. Therefore, the improved hierarchical ADMM gives less iteration number and computation time than the hierarchical ADMM.

VI Illustrative Example

In order to show the effectiveness of the hierarchical ADMM and improved hierarchical ADMM, a multi-UAV system is presented as an application test platform for demonstration of the proposed decision-making framework. The simulations are performed in Python 3.7 environment with processor Intel Xeon(R) E5-1650 v4 @ 3.60GHz. All nonlinear problems are solved by the open source IPOPT solver.

VI-A Dynamic Model

For each quadcopter system, the system model can be expressed in the following state-space representation:

\displaystyle\dot{x}={\begin{bmatrix}{\dot{p}}\\ {\dot{v}}\\ {\dot{\zeta}}\\ {\dot{\omega}}\end{bmatrix}}={\begin{bmatrix}{R(\phi,\theta,\psi)}^{\top}{v}\\ {-{\omega}\times v+g{R(\phi,\theta,\psi)}e}\\ W(\phi,\theta,\psi){\omega}\\ J^{-1}(-{\omega}\times{J}{\omega})\end{bmatrix}+{\tilde{B}}{({u}_{\mathrm{eq}}+{u})}},\yesnumber

where $x\in\mathbb{R}^{12}$ and $u\in\mathbb{R}^{4}$ are state and control input vector for the agents, respectively. In this dynamics model, rotor thrusts of each rotor are chosen as control input vector, i.e., $u=\begin{bmatrix}F_{1}&F_{2}&F_{3}&F_{4}\end{bmatrix}^{\top}$ . ${p}=\begin{bmatrix}p_{x}&p_{y}&p_{z}\end{bmatrix}^{\top}$ is a vector comprising the position in the $x$ -, $y$ -, and $z$ -dimension. Additionally, the velocity vector is represented by ${v}=\begin{bmatrix}v_{x}&v_{y}&v_{z}\end{bmatrix}^{\top}$ . $\zeta=\begin{bmatrix}\phi&\theta&\psi\end{bmatrix}^{\top}$ denotes a vector in terms of the roll, pitch, and yaw angles. The angular velocity vector is represented by $\omega=\begin{bmatrix}\omega_{x}&\omega_{y}&\omega_{z}\end{bmatrix}^{\top}$ . Also, ${e}=\begin{bmatrix}0&0&1\end{bmatrix}^{\top}$ , $u_{\mathrm{eq}}=\begin{bmatrix}{mg}/{4}&{mg}/{4}&{mg}/{4}&{mg}/{4}\end{bmatrix}^{\top}$ is the control input to balance the gravity of the agents, $g$ is the gravitational acceleration, $m$ is the mass of the quadcopter, $J=\textup{diag}(J_{x},J_{y},J_{z})$ denotes the moment of inertia of the quadcopter, $R(\phi,\theta,\psi)$ and $W(\phi,\theta,\psi)$ denote the rotation matrices of the quadcopter. In addition, ${\tilde{B}}$ is the control input matrix in the above state-space representation.

Here, (VI-A) is nonlinear and time-variant, which can be represented as a pseudo-linear form by using state-dependent coefficient factorization [31]. The state-space expression is

\displaystyle{x_{t+1}}=(\tilde{A}_{t}\Delta t+I_{12})x_{t}+\tilde{B}\Delta t(u_{\textup{eq}}+u_{t}),

(25)

where $\Delta t$ is the sampling time interval. Since ${\tilde{A}}_{t}$ and $\tilde{B}$ are dependent on the current state $x$ , this state-space representation is in a pseudo-linear form, and then we can suitably consider the system matrices to be constant during the prediction horizon. More details of the parameter settings in (VI-A) and (25) are presented in [32].

VI-B Optimization and Simulation Results

The dynamic model of each agent in the multi-UAV system is shown in (25). All the agents are set evenly distributed in a circle with the radius of $2$ m and $p_{z}=0$ m, and each agent needs to reach its corresponding point in this same sphere. Therefore, a large number of potential collision possibilities are set up in the simulation task, and our objective is to avoid these designed potential collisions among these agents and make all agents reach their desired destinations. Here, a distributed MPC with a quadratic objective function is designed to realize the path tracking and collision avoidance tasks. Set the number of agents $N=8$ . The safety distance $d_{\text{safe}}$ is $0.2$ m. The dimensions are $n=12,m=4,n_{p}=3$ . In addition, the agents are connected with all of the remaining agents. The adjacency matrix of the communication network $\mathcal{D}$ is the difference between the upper triangular matrix of the square one matrix $1_{(N,N)}$ and the identity matrix $I_{N}$ , i.e., $\mathcal{D}=U(1_{(N,N)})-I_{N}$ . In this adjacency matrix $\mathcal{D}$ , each entry $\mathcal{D}_{ij}$ represents whether there is the connection between the $i$ th agent and the $j$ th agent. If $\mathcal{D}_{ij}=1$ , there is a communication connection between the two agents; otherwise, there is no connection between the two agents. For example, the $i$ th agent will get communication with the $i+1,i+2,\cdots,N$ agents. The velocity of the agents is confined into [-5, 5] m/s, and the upper and lower bounds of angles $(\phi,\psi,\theta)$ in three dimensions are $(\pi,\frac{\pi}{2},\pi)$ and $(-\pi,-\frac{\pi}{2},-\pi)$ . The upper bound of control input variable is $\overline{u}=(1.96,1.96,1.96,1.96)$ N and its lower bound is $\underline{u}=-\overline{u}$ . Set the prediction horizon $T=25$ with the sampling time $\Delta t=0.05$ s. The weighting matrices $R_{i}=0_{(m,m)}$ and $Q_{i}=\operatorname{blockdiag}(I_{n_{p}},0_{(n-n_{p},n-n_{p})})$ for all $i\in\mathcal{V}$ . Each element in $\overline{\lambda}_{i}$ and $\underline{\lambda}_{i}$ are set as $0.01$ and $-0.01$ , respectively. $\omega$ and $\gamma$ are set as 0.9 and 1.1. The initial tolerances $\epsilon_{1}^{0},\epsilon_{2}^{0},\epsilon_{3}^{0}$ are defined as $10^{-2},10^{-2},10^{-1},$ respectively, and $\epsilon_{i}^{k}=\epsilon_{i}^{k-1}/5^{k-1},\forall i\in\{1,2,3\}$ . The terminated tolerances for outer iterations $\epsilon_{\text{term},1}$ , $\epsilon_{\text{term},2}$ , $\epsilon_{\text{term},3}$ are $10^{-6},10^{-6},10^{-5}$ , respectively. The maximum iteration number is set to $10^{4}$ .

The reference trajectories and the resulted trajectories computed by the improved hierarchical ADMM are shown in Fig. 1 and Fig. 2. The initial positions of the 8 agents are distributed uniformly in a circle, whose radius equals 2 m, and the z-axis is 0. In this simulation task, the 8 agents need to move towards their corresponding points, which are the central symmetric points of their initial position, respectively. For example, the agents with initial position (0,2,0) m and ( $\sqrt{2},\sqrt{2},0$ ) m needs to reach (0,-2,0) m and ( $-\sqrt{2},-\sqrt{2},0$ ) m. In the two figures, each agent is represented by a different color. These lines in different colors denote the resulted trajectories of the 8 agents, respectively. From the two figures, it is obvious that all agents can effectively avoid their neighbors and maintain the safety distance. On the other hand, for the hierarchical ADMM, a similar performance of collision avoidance can be attained.

Refer to caption — Figure 1: Resulted trajectories of 8 agents in 3D view.

In order to show the bound of the control inputs, we only choose the control input results of the 1st agent $u=\begin{bmatrix}F_{1}&F_{2}&F_{3}&F_{4}\end{bmatrix}$ as an example for the clarity of this figure, as shown in Fig. 3. The 4 dimensions of the control inputs are bounded in the predefined range $[-1.96,1.96]$ N.

The distances between each pair of the 8 agents are shown in Fig. 4. The solid gray line in this figure represents the safety distance all of the agents need to maintain, and the other colored lines denote the distance of each pair of agents with respect to time. It shows that the distance between agents are no smaller than the safety distance.

The comparison of computational efficiency between our proposed hierarchical ADMM and improved hierarchical ADMM can be illustrated, according to Fig. 5 and Fig. 6. Fig. 5 demonstrates the comparison of iteration numbers the inner loop and outer loop require in both hierarchical ADMM and improved hierarchical ADMM. It is straightforward to observe that the improved hierarchical ADMM can effectively reduce the iteration numbers of both the inner loop and outer loop, compared with the hierarchical ADMM. The comparison of computation time for all inner loop iterations between hierarchical ADMM and improved hierarchical ADMM is shown in Fig. 6. From this figure, it can be observed that the computation time is significantly reduced by using the improved version.

TABLE I: Comparison of proposed algorithms (hierarchical ADMM and improved hierarchical ADMM) with the centralized MPC and penalty dual decomposition method [33]

	Centralized MPC	Penalty Dual Decomposition [33]	Hierarchical ADMM	Improved Hierarchical ADMM
Average outer iterations	-	4.26	2.12	1.03
Average inner iterations	-	17.36	6.38	1.12
$\\|\mathcal{A}_{i}z_{i}+\mathcal{B}_{i}z_{f,i}-h_{i}\\|$	2.21E-5	3.32E-5	2.54E-5	1.47E-5
Cost value	253.12	315.35	294.23	245.36
Stopping criterions	-	1E-6, 7.52E-2, 2.32E-3	1E-6, 9.38E-2, 3.5E-3	1E-6, 3.28E-2, 2.64E-3
Time	1.47s	0.22s	0.16s	0.09s

In order to show the effectiveness of the proposed improved hierarchical ADMM, the penalty dual decomposition method [33] and the centralized MPC method are used as the comparison methods. The centralized MPC method means we solve the centralized MPC problem by using a NLP solver, and the penalty dual decomposition method is also a double-loop iterative algorithm [33]. The inner loop of the penalty dual decomposition method is used to solve a nonconvex nonsmooth augmented Lagrangian problem via block-coordinate-descent-type methods, and the outer loop of this algorithm is to update the dual variables and/or the penalty parameter [33]. Table I illustrates the advantages of our proposed algorithms (hierarchical ADMM and improved hierarchical ADMM) over the penalty dual decomposition method and the centralized MPC method.

In order to demonstrate the performance in terms of computational efficiency, the comparison of computation time per step (including all outer loop iterations and inner loop iterations) under the hierarchical ADMM and the improved hierarchical ADMM, and the computation time per step under the centralized MPC (solved by the IPOPT solver) are shown in Fig. 7. From this figure, the computational efficiency of the ADMM for solving large-scale problems can be clearly observed. Moreover, the improved hierarchical ADMM is capable of further improving the efficiency, as compared with the hierarchical ADMM. With the increasing number of agents, the number of nonlinear constraints increases rapidly, which leads to difficulty in solving the problem. Therefore, the computation time is increasing with the increase in the number of agents $N$ , as shown in Fig. 8. However, when the centralized MPC solver is used, the growth rate of the computation time is much higher than the ones when the hierarchical ADMM and improved hierarchical ADMM are used, as shown in Fig. 8, which also demonstrates the efficiency of the two proposed methods. Besides, the efficiency of the improved hierarchical ADMM can also be proved in this figure.

VII Conclusion

In this paper, a large-scale nonconvex optimization problem for multi-agent decision making is investigated and solved by the hierarchical ADMM approach. For the multi-agent system, an appropriate DMPC problem is formulated and presented to handle the constraints and achieve the collective objective. Firstly, an innovation using appropriate slack variables is deployed, yielding a resulting transformed optimization problem, which is used to deal with the nonconvex characteristics of the problem. Then, the hierarchical ADMM is applied to solve the transformed problem in parallel. Next, the improved hierarchical ADMM is proposed to increase the computational efficiency and decrease the number of inner iterations. Additionally, it is analytically shown that the appropriate desired stationary point exists for the procedures of the hierarchical stages for algorithmic convergence. Finally, comparative simulations are conducted using a multi-UAV system as the test platform, which further illustrates the effectiveness and efficiency of the proposed methodology.

References

[1] X. He, T. Huang, J. Yu, C. Li, and Y. Zhang, “A continuous-time algorithm for distributed optimization based on multiagent networks,” IEEE Transactions on Systems, Man, and Cybernetics: Systems, vol. 49, no. 12, pp. 2700–2709, 2017.
[2] H. Hong, W. Yu, G. Wen, and X. Yu, “Distributed robust fixed-time consensus for nonlinear and disturbed multiagent systems,” IEEE Transactions on Systems, Man, and Cybernetics: Systems, vol. 47, no. 7, pp. 1464–1473, 2016.
[3] M. Ye, G. Wen, S. Xu, and F. L. Lewis, “Global social cost minimization with possibly nonconvex objective functions: An extremum seeking-based approach,” IEEE Transactions on Systems, Man, and Cybernetics: Systems, 2020.
[4] J. Ma, S.-L. Chen, C. S. Teo, A. Tay, A. Al Mamun, and K. K. Tan, “Parameter space optimization towards integrated mechatronic design for uncertain systems with generalized feedback constraints,” Automatica, vol. 105, pp. 149–158, 2019.
[5] S. Yang, Q. Liu, and J. Wang, “Distributed optimization based on a multiagent system in the presence of communication delays,” IEEE Transactions on Systems, Man, and Cybernetics: Systems, vol. 47, no. 5, pp. 717–728, 2016.
[6] ——, “A multi-agent system with a proportional-integral protocol for distributed constrained optimization,” IEEE Transactions on Automatic Control, vol. 62, no. 7, pp. 3461–3467, 2016.
[7] D. Yuan, S. Xu, and H. Zhao, “Distributed primal–dual subgradient method for multiagent optimization via consensus algorithms,” IEEE Transactions on Systems, Man, and Cybernetics, Part B (Cybernetics), vol. 41, no. 6, pp. 1715–1724, 2011.
[8] E. Camponogara and L. B. De Oliveira, “Distributed optimization for model predictive control of linear-dynamic networks,” IEEE Transactions on Systems, Man, and Cybernetics-Part A: Systems and Humans, vol. 39, no. 6, pp. 1331–1338, 2009.
[9] M. Hong, “A distributed, asynchronous, and incremental algorithm for nonconvex optimization: an ADMM approach,” IEEE Transactions on Control of Network Systems, vol. 5, no. 3, pp. 935–945, 2017.
[10] J. Ma, Z. Cheng, X. Zhang, M. Tomizuka, and T. H. Lee, “On symmetric Gauss-Seidel ADMM algorithm for ${H}_{\infty}$ guaranteed cost control with convex parameterization,” arXiv preprint arXiv:2001.00708, 2020.
[11] ——, “Optimal decentralized control for uncertain systems by symmetric Gauss-Seidel semi-proximal ALM,” IEEE Transactions on Automatic Control, DOI: 10.1109/TAC.2021.3052768, 2021.
[12] Y. Hu, E. C. Chi, and G. I. Allen, “ADMM algorithmic regularization paths for sparse statistical machine learning,” in Splitting Methods in Communication, Imaging, Science, and Engineering. Springer, 2016, pp. 433–459.
[13] J. Ding, Y. Gong, C. Zhang, M. Pan, and Z. Han, “Optimal differentially private ADMM for distributed machine learning,” arXiv preprint arXiv:1901.02094, 2019.
[14] B. He and X. Yuan, “On the ${O}(1/n)$ convergence rate of the Douglas-Rachford alternating direction method,” SIAM Journal on Numerical Analysis, vol. 50, no. 2, pp. 700–709, 2012.
[15] R. D. Monteiro and B. F. Svaiter, “Iteration-complexity of block-decomposition algorithms and the alternating direction method of multipliers,” SIAM Journal on Optimization, vol. 23, no. 1, pp. 475–507, 2013.
[16] E. Wei and A. Ozdaglar, “Distributed alternating direction method of multipliers,” in 2012 IEEE 51st IEEE Conference on Decision and Control (CDC). IEEE, 2012, pp. 5445–5450.
[17] A. Makhdoumi and A. Ozdaglar, “Convergence rate of distributed ADMM over networks,” IEEE Transactions on Automatic Control, vol. 62, no. 10, pp. 5082–5095, 2017.
[18] B. He, M. Tao, and X. Yuan, “Alternating direction method with Gaussian back substitution for separable convex programming,” SIAM Journal on Optimization, vol. 22, no. 2, pp. 313–340, 2012.
[19] T. Lin, S. Ma, and S. Zhang, “On the global linear convergence of the ADMM with multiblock variables,” SIAM Journal on Optimization, vol. 25, no. 3, pp. 1478–1497, 2015.
[20] G. Li and T. K. Pong, “Global convergence of splitting methods for nonconvex composite optimization,” SIAM Journal on Optimization, vol. 25, no. 4, pp. 2434–2460, 2015.
[21] H. Wang and A. Banerjee, “Bregman alternating direction method of multipliers,” in Advances in Neural Information Processing Systems, 2014, pp. 2816–2824.
[22] B. Jiang, T. Lin, S. Ma, and S. Zhang, “Structured nonconvex and nonsmooth optimization: algorithms and iteration complexity analysis,” Computational Optimization and Applications, vol. 72, no. 1, pp. 115–157, 2019.
[23] Y. Wang, W. Yin, and J. Zeng, “Global convergence of ADMM in nonconvex nonsmooth optimization,” Journal of Scientific Computing, vol. 78, no. 1, pp. 29–63, 2019.
[24] X. Wang, J. Yan, B. Jin, and W. Li, “Distributed and parallel ADMM for structured nonconvex optimization problem,” IEEE Transactions on Cybernetics, 2019.
[25] Z. Cheng, J. Ma, X. Zhang, C. W. de Silva, and T. H. Lee, “ADMM-based parallel optimization for multi-agent collision-free model predictive control,” arXiv preprint arXiv:2101.09894, 2021.
[26] J. Ma, Z. Cheng, X. Zhang, M. Tomizuka, and T. H. Lee, “Alternating direction method of multipliers for constrained iterative LQR in autonomous driving,” arXiv preprint arXiv:2011.00462, 2020.
[27] B. Houska, J. Frasch, and M. Diehl, “An augmented lagrangian based algorithm for distributed nonconvex optimization,” SIAM Journal on Optimization, vol. 26, no. 2, pp. 1101–1127, 2016.
[28] K. Sun and X. A. Sun, “A two-level distributed algorithm for nonconvex constrained optimization,” arXiv preprint arXiv:1902.07654, 2019.
[29] W. Tang and P. Daoutidis, “Fast and stable nonconvex constrained distributed optimization: The ELLADA algorithm,” arXiv preprint arXiv:2004.01977, 2020.
[30] D. Limón, T. Alamo, F. Salas, and E. F. Camacho, “On the stability of constrained mpc without terminal constraint,” IEEE Transactions on Automatic Control, vol. 51, no. 5, pp. 832–836, 2006.
[31] X. Zhang, J. Ma, S. Huang, Z. Cheng, and T. H. Lee, “Integrated planning and control for collision-free trajectory generation in 3D environment with obstacles,” in Proceedings of International Conference on Control, Automation and Systems, 2019, pp. 974–979.
[32] X. Zhang, J. Ma, Z. Cheng, S. Huang, S. S. Ge, and T. H. Lee, “Trajectory generation by chance-constrained nonlinear MPC with probabilistic prediction,” IEEE Transactions on Cybernetics, DOI: 10.1109/TCYB.2020.3032711, 2020.
[33] Q. Shi and M. Hong, “Penalty dual decomposition method for nonsmooth nonconvex optimization-Part I: Algorithms and convergence analysis,” IEEE Transactions on Signal Processing, vol. 68, pp. 4108–4122, 2020.

			$\displaystyle\mathcal{L}_{\rho_{i}^{k}}\left(z_{i}^{r},z_{f,i}^{r},s_{i}^{r-1},y_{i}^{r-1}\right)-\mathcal{L}_{\rho_{i}^{k}}\left(z_{i}^{r},z_{f,i}^{r},s_{i}^{r},y_{i}^{r}\right)$
		$\displaystyle=$	$\displaystyle\left\langle\lambda_{i}^{k},(s_{i}^{r-1}-s_{i}^{r})\right\rangle+\frac{\beta_{i}^{k}}{2}\left(\left\\|s_{i}^{r-1}\\|^{2}-\\|s_{i}^{r}\right\\|^{2}\right)$
			$\displaystyle\>+\left\langle y_{i}^{r},s_{i}^{r-1}-s_{i}^{r}\right\rangle+\frac{\rho_{i}^{k}}{2}\left\\|s_{i}^{r-1}-s_{i}^{r}\right\\|^{2}$
			$\displaystyle\>-\rho_{i}^{k}\left\\|\mathcal{A}_{i}z_{i}^{r}+\mathcal{B}_{i}z_{f,i}^{r}+s_{i}^{r}-h_{i}\right\\|^{2}$
		$\displaystyle\geq$	$\displaystyle\left(\frac{\rho_{i}^{k}}{2}-\frac{{\beta_{i}^{k}}^{2}}{\rho_{i}^{k}}\right)\left\\|s_{i}^{r-1}-s_{i}^{r}\right\\|^{2}.\yesnumber$

			$\displaystyle-\rho_{i}^{k}\langle\mathcal{A}_{i}z_{i}^{r}+\mathcal{B}_{i}z_{f,i}^{r}-h_{i}+s_{i}^{r},\mathcal{A}_{i}z_{i}^{r}+\mathcal{B}_{i}z_{f,i}^{r}-h_{i}+s_{i}^{r-1}\rangle$
			$\displaystyle+\frac{\rho_{i}^{k}}{2}\Big{(}\left\\|\mathcal{A}_{i}z_{i}^{r}+\mathcal{B}_{i}z_{f,i}^{r}-h_{i}+s_{i}^{r-1}\right\\|^{2}$
			$\displaystyle\quad-\left\\|\mathcal{A}_{i}z_{i}^{r}+\mathcal{B}_{i}z_{f,i}^{r}-h_{i}+s_{i}^{r}\right\\|^{2}\Big{)}$
		$\displaystyle=$	$\displaystyle\frac{\rho_{i}^{k}}{2}\left\\|s_{i}^{r-1}-s_{i}^{r}\right\\|^{2}-\rho_{i}^{k}\left\\|\mathcal{A}_{i}z_{i}^{r}+\mathcal{B}_{i}z_{f,i}^{r}-h_{i}+s_{i}^{r}\right\\|^{2}.\yesnumber$

	$\displaystyle\left\\|\rho_{i}^{k}\mathcal{A}_{i}^{T}\left(\mathcal{B}_{i}z_{f,i}^{r}+s_{i}^{r}-\mathcal{B}_{i}z_{f,i}^{r+1}-s_{i}^{r+1}\right)\right\\|\leq\epsilon_{1}^{k}$		(19a)
	$\displaystyle\left\\|\rho_{i}^{k}\mathcal{B}_{i}^{T}\left(s_{i}^{r}-s_{i}^{r+1}\right)\right\\|\leq\epsilon_{2}^{k}$
	$\displaystyle\left\\|\mathcal{A}_{i}z_{i}^{r+1}+\mathcal{B}_{i}z_{f,i}^{r+1}-h_{i}+s_{i}^{r+1}\right\\|\leq\epsilon_{3}^{k},\textbf{}$