ADMM For Multi-Aircraft Coordination Problem
According to ADMM lecture, the consensus ADMM algorithm can be written as
\[\begin{align} x _ { i } ^ { k + 1 } & : = \operatorname { arg } \min _ { x _ { i } } ( f _ { i } ( x _ { i } ) + \lambda _ { i } ^ { k \intercal} ( x _ { i } - z ^ { k } ) + \frac { \rho } { 2 } \| x _ { i } - z ^ { k } \| _ { 2 } ^ { 2 } ) \\ z ^ { k + 1 } & : = \frac { 1 } { N } \sum _ { i = 1 } ^ { N } ( x _ { i } ^ { k + 1 } + \frac { 1 } { \rho } \lambda _ { i } ^ { k } )\\ \lambda _ { i } ^ { k + 1 } & : = \lambda _ { i } ^ { k } + \rho ( x \_ { i } ^ { k + 1 } - z ^ { k + 1 } ) \end{align}\]In our case, we will apply consensus ADMM algorithm to a the multi-aircraft coordination formulation problem. Assume \(x_{f, i}^k\) is the private terminal state of \(i\)-th agent at \(k\)-th ADMM iteration, \(z^k\) is the public common terminal state at \(k\)-th ADMM iteration. For the first step, the ADMM algorithm will optimize \(x_i\). In our case, this is to optimize the sub problem similar to problem:
\[\begin{aligned} \min_{U_i} ~ & U_i^{\intercal} H_i U_i + h_i U_i + c_i + \lambda _ { i } ^ { k \intercal} ( x _ {f, i } - z ^ { k } ) + \frac { \rho } { 2 } \| x _ { i } - z ^ { k } \| _ { 2 } ^ { 2 } \\ \text{s.t.} \quad & F_{i, u} U_i = - F_{i, x} x_i(0) + x_{f, i} \\ & - \frac{u_{\text{max}}}{T_{\text{final}}} \leq U_i \leq \frac{u_{\text{max}}}{T_{\text{final}}} \\ \end{aligned}\]Define \(V_i = \left[\begin{array}{c} U_i \\ x*{f, i}\end{array} \right]\). The cost function can be rewritten as
\[\begin{aligned} J_{\text{ADMM}, i} & = U_i^{\intercal} H_i U_i + h_i U_i + c_i + \lambda _ { i } ^ { k \intercal} ( x _ {f, i } - z ^ { k } ) + \frac { \rho } { 2 } \| x _ { f, i } - z ^ { k } \| _ { 2 } ^ { 2 } \\ & = U_i^{\intercal} H_i U_i + h_i U_i + c_i + \lambda _ { i } ^ { k \intercal} x _ {f, i } - \lambda _ { i } ^ { k \intercal} z ^ { k } + \frac { \rho } { 2 } (x _ { f, i } - z ^ { k })^{\intercal} (x _ { f, i } - z ^ { k }) \\ & = V_i^{\intercal} H_{i, v} V_i + h_{i,v} V_i + C_i \end{aligned}\]in which
\[\begin{aligned} H_{i,v} & := \left[ \begin{array}{cc} H_i & \\ & \frac{\rho}{2} I \end{array}\right] \\ h_{i,v} & := \left[ h_i,~ \lambda_i - \rho z^{k\intercal} \right] \\ C_{i,v} & := c_i + \frac{\rho}{2} z^{k \intercal} z^k \end{aligned}\]\(I\) is a 4-by-4 identity matrix.
Therefore, the problem could be written as
\[\begin{aligned} \min_{V_i} ~ & V_i^{\intercal} H_{i, v} V_i + h_{i,v} V_i + C_i \\ \text{s.t.} \quad & F_{i, v} V_{i} = - F_{i, x} x_i(0) \\ & V_{lb} \leq V_{i} \leq V_{ub} \end{aligned}\]in which the equality constraints: \(F_{i,v} = [F_{i, u}~, -I ]\) The upper and lower bound of \(V\) is set as follows: for entries corresponding to \(U_i\), the upper bound is \(\frac{u_{\text{max}}}{T_{\text{final}}}\) and lower bound is \(-\frac{u_{\text{max}}}{T_{\text{final}}}\); for entries corresponding to \(x_{f, i}\) the upper bound is \(\infty\) and lower bound is \(-\infty\).
Next, update the common terminal state $z$ follows
\[z ^ { k + 1 } = \frac { 1 } { 4 } \sum _ { i = 1 } ^ { 4 } ( x _ {f,i} ^ { k + 1 } + \frac { 1 } { \rho } \lambda _ { i } ^ { k } )\]Finally, update the dual variable \(\lambda^{k}\)
\[\lambda _ { i } ^ { k + 1 } = \lambda _ { i } ^ { k } + \rho ( x _ { f, i } ^ { k + 1 } - z ^ { k + 1 } )\]