Yang X, Wang W, Huang P. Distributed optimal consensus with obstacle avoidance algorithm of mixed-order UAVs–USVs–UUVs systems[J]. ISA transactions, 2020, 107: 270-286.

Xuekuan Yang, Wei Wang, Ping Huang, Distributed optimal consensus with obstacle avoidance algorithm of mixed-order UAVs–USVs–UUVs systems, ISA Transactions, Volume 107, 2020, Pages 270-286.

(https://www.sciencedirect.com/science/article/pii/S0019057820303098)

Abstract: This article addresses the formation control with obstacle avoidance for a mixed-order linear multi-agent in the ocean environment via a distributed optimal control approach. The considered system is comprised of unmanned aerial vehicles (UAVs), unmanned surface vehicles (USVs) and unmanned underwater vehicles (UUVs). A consensus control law for horizontal direction and height direction is then designed uniformly, and the stability is ensured using block Kronecker product and matrix transformation theory. A linear quadratic regulator method is further designed to achieve distributed optimization. Moreover, for obstacle avoidance in the UUVs, a non-quadratic penalty function is constructed based on the seamount threat model by adopting an inverse optimal control approach. Simulation results confirm the efficiency of the consensus and obstacle avoidance.

Keywords: Mixed-order multi-agent systems (MOMAS); Consensus; Optimal formation control; Obstacle avoidance

Distributed Optimal Consensus with Obstacle Avoidance Algorithm of Mixed-Order UAVs-USVs-UUVs Systems

1 Introduction
2 Problem Formulation
- 2.1 Problem description
- 2.2 System description
- - 2.2.1 High-Order UAV Dynamics Model
  - 2.2.2 Second-Order USV and UUV
  - 2.2.3 State Space Model: Consider the Expressing MOMAS as a State Space
3 Distributed Optimal Consensus and Obstacle Avoidance Control
- 3.1 Consensus Protocol
- 3.2 Optimal Formation Control
- 3.3 Optimal Control with Underwater Obstacle Avoidance
4 Simulations
- 4.1 Consistency Simulation
- 4.2 Optimal Formation Contorl with Obstacle Avoidance Simulation
5 Conclusion
- Lemma

1 Introduction

2 Problem Formulation

2.1 Problem description

2.2 System description

2.2.1 High-Order UAV Dynamics Model

2.2.2 Second-Order USV and UUV

2.2.3 State Space Model: Consider the Expressing MOMAS as a State Space

3 Distributed Optimal Consensus and Obstacle Avoidance Control

3.1 Consensus Protocol

3.2 Optimal Formation Control

3.3 Optimal Control with Underwater Obstacle Avoidance

4 Simulations

4.1 Consistency Simulation

4.2 Optimal Formation Contorl with Obstacle Avoidance Simulation

5 Conclusion

一篇很不错的无人机，水面帆船，水下潜艇三者协同控制的

本文采用的无人机动力学模型为：
x¨=gθy¨=−gϕz¨=−fz/m−gϕ¨=Mϕ/Ixθ¨=Mθ/Iyψ¨=Mψ/Iz\begin{aligned} &\ddot{x} = g \theta \\ &\ddot{y} = -g \phi \\ &\ddot{z} = -f_z/m - g \\ &\ddot{\phi} = M_\phi / I_x \\ &\ddot{\theta} = M_\theta / I_y \\ &\ddot{\psi} = M_\psi / I_z \\ \end{aligned}x¨=gθy¨=−gϕz¨=−fz/m−gϕ¨=Mϕ/Ixθ¨=Mθ/Iyψ¨=Mψ/Iz

关于各个符号的具体释义参考原文献。

控制输入为：u=[uz,uϕ,uθ,uψ]Tu=[u_z, u_\phi, u_\theta, u_\psi]^Tu=[uz,uϕ,uθ,uψ]T。

转换成状态空间的形式：
x˙=Ax+Bu\dot{x} = A x + B ux˙=Ax+Bu

对于共有 mmm 个UAV而言，x=[p1,p2,⋯,pm,v1,v2,⋯,vm,α1,α2,⋯,αm,β1,β2,⋯,βm]Tx = [p1, p2, \cdots, p_m,~~~ v_1, v_2, \cdots, v_m,~~~ \alpha_1, \alpha_2, \cdots, \alpha_m,~~~ \beta_1, \beta_2,\cdots, \beta_m]^Tx=[p1,p2,⋯,pm, v1,v2,⋯,vm, α1,α2,⋯,αm, β1,β2,⋯,βm]T。

对于共有 111 个UAV而言，x=[p1,v1,α1,β1]Tx = [p1, v_1, \alpha_1, \beta_1]^Tx=[p1,v1,α1,β1]T。

由于 UAV 的运动空间为3维空间，因此各个节点的位置状态和速度状态又分为 x,y,zx,y,zx,y,z 三个方向，数学表示为 pi=[xi,yi,zi],vi=[vix,viy,viz]p_i = [x_i, y_i, z_i], v_i = [v_i^x, v_i^y, v_i^z]pi=[xi,yi,zi],vi=[vix,viy,viz]。

状态矩阵分别如下：
AA=[03×3I3×303×303×303×303×3I3×303×303×303×303×3I3×303×303×303×303×3]⊗Im\begin{aligned}A_A= \left[\begin{matrix} 0_{3\times3} & I_{3\times3} & 0_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & 0_{3\times3} & I_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3}\\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \\ \end{matrix}\right] \otimes I_m \end{aligned}AA=⎣⎢⎢⎡03×303×303×303×3I3×303×303×303×303×3I3×303×303×303×303×3I3×303×3⎦⎥⎥⎤⊗Im

BA=[03×303×303×3I3×3]⊗Im\begin{aligned}B_A= \left[\begin{matrix} 0_{3\times3} \\ 0_{3\times3} \\ 0_{3\times3} \\ I_{3\times3} \\ \end{matrix}\right] \otimes I_m \end{aligned}BA=⎣⎢⎢⎡03×303×303×3I3×3⎦⎥⎥⎤⊗Im

而 αi=[gθi,−gϕi,0]\alpha_i = [g \theta_i, -g \phi_i, 0]αi=[gθi,−gϕi,0]， βi=[gςi,−gξi,0]\beta_i = [g \varsigma_i, -g \xi_i, 0]βi=[gςi,−gξi,0]。

针对单个节点，先将状态空间表示式展开：
[x˙y˙z˙v˙xv˙yv˙zgθ˙−gϕ˙0gς˙−gξ˙0]=[000100000000000010000000000001000000000000100000000000010000000000001000000000000100000000000010000000000001000000000000000000000000000000000000][xyzvxvyvzgθ−gϕ0gς−gξ0]+[000000000000000000000000000100010001]U\begin{aligned} \left[\begin{matrix} \dot{x} \\ \dot{y} \\ \dot{z} \\ \\ \dot{v}^x \\ \dot{v}^y \\ \dot{v}^z \\ \\ g \dot{\theta} \\ -g \dot{\phi} \\ 0 \\ \\ g \dot{\varsigma} \\ -g \dot{\xi} \\ 0 \end{matrix}\right]= \left[\begin{matrix} 0 & 0 & 0 && 1 & 0 & 0 && 0 & 0 & 0 && 0 & 0 & 0 \\ 0 & 0 & 0 && 0 & 1 & 0 && 0 & 0 & 0 && 0 & 0 & 0 \\ 0 & 0 & 0 && 0 & 0 & 1 && 0 & 0 & 0 && 0 & 0 & 0 \\ \\ 0 & 0 & 0 && 0 & 0 & 0 && 1 & 0 & 0 && 0 & 0 & 0 \\ 0 & 0 & 0 && 0 & 0 & 0 && 0 & 1 & 0 && 0 & 0 & 0 \\ 0 & 0 & 0 && 0 & 0 & 0 && 0 & 0 & 1 && 0 & 0 & 0 \\ \\ 0 & 0 & 0 && 0 & 0 & 0 && 0 & 0 & 0 && 1 & 0 & 0 \\ 0 & 0 & 0 && 0 & 0 & 0 && 0 & 0 & 0 && 0 & 1 & 0 \\ 0 & 0 & 0 && 0 & 0 & 0 && 0 & 0 & 0 && 0 & 0 & 1 \\ \\ 0 & 0 & 0 && 0 & 0 & 0 && 0 & 0 & 0 && 0 & 0 & 0 \\ 0 & 0 & 0 && 0 & 0 & 0 && 0 & 0 & 0 && 0 & 0 & 0 \\ 0 & 0 & 0 && 0 & 0 & 0 && 0 & 0 & 0 && 0 & 0 & 0 \\ \end{matrix}\right] \left[\begin{matrix} x \\ y \\ z \\ \\ v^x \\ v^y \\ v^z \\ \\ g \theta \\ -g \phi \\ 0 \\ \\ g \varsigma \\ -g \xi \\ 0 \end{matrix}\right]+ \left[\begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \\ 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ \end{matrix}\right] U \end{aligned}⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡x˙y˙z˙v˙xv˙yv˙zgθ˙−gϕ˙0gς˙−gξ˙0⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡000000000000000000000000000000000000100000000000010000000000001000000000000100000000000010000000000001000000000000100000000000010000000000001000⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡xyzvxvyvzgθ−gϕ0gς−gξ0⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤+⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡000000000100000000000010000000000001⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤U

结合本文提出的控制协议：
ui=−r1∑j=1naij(P~i−P~j)−r2∑j=1naij(V~i−V~j)−r3αi−r4βiu_i = -r_1 \sum_{j=1}^n a_{ij} (\tilde{P}_i - \tilde{P}_j) -r_2 \sum_{j=1}^n a_{ij} (\tilde{V}_i - \tilde{V}_j) - r_3 \alpha_i - r_4 \beta_iui=−r1j=1∑naij(P~i−P~j)−r2j=1∑naij(V~i−V~j)−r3αi−r4βi

为方便理解，可以先简化协议成如下形式（不考虑 Desired Position & Velocity）：
ui=−r1∑j=1naij(Pi−Pj)−r2∑j=1naij(Vi−Vj)−r3αi−r4βiu_i = -r_1 \sum_{j=1}^n a_{ij} ({P}_i - {P}_j) -r_2 \sum_{j=1}^n a_{ij} ({V}_i - {V}_j) - r_3 \alpha_i - r_4 \beta_iui=−r1j=1∑naij(Pi−Pj)−r2j=1∑naij(Vi−Vj)−r3αi−r4βi

文献中给出的

[UAUSUV]=[U11U12U13U21U22U23U31U32U33]×X~=[−r1LAA−r2LAA−r3I−r4I−r1LAS−r2LAS00−r1LSA−r2LSA00−r1LSS−r2LSS−r1LSU−r2LSU0000−r1LUS−r2LUS−r1LUU−r2LUU]\begin{aligned} \left[\begin{matrix} U_A \\ U_S \\ U_V \\ \end{matrix}\right] &= \left[\begin{matrix} U_{11} & U_{12} & U_{13} \\ U_{21} & U_{22} & U_{23} \\ U_{31} & U_{32} & U_{33} \\ \end{matrix}\right]\times\tilde{X}\\ &= \left[\begin{matrix} -r_1 L_{AA} & -r_2L_{AA} & -r_3I & -r_4I && -r_1L_{AS} & -r_2L_{AS} && 0 & 0 \\ \\ -r_1 L_{SA} & -r_2L_{SA} & 0 & 0 && -r_1L_{SS} & -r_2L_{SS} && -r_1L_{SU} & -r_2L_{SU} \\ \\ 0 & 0 & 0 & 0 && -r_1L_{US} & -r_2L_{US} && -r_1L_{UU} & -r_2L_{UU} \\ \end{matrix}\right] \end{aligned}⎣⎡UAUSUV⎦⎤=⎣⎡U11U21U31U12U22U32U13U23U33⎦⎤×X~=⎣⎢⎢⎢⎢⎡−r1LAA−r1LSA0−r2LAA−r2LSA0−r3I00−r4I00−r1LAS−r1LSS−r1LUS−r2LAS−r2LSS−r2LUS0−r1LSU−r1LUU0−r2LSU−r2LUU⎦⎥⎥⎥⎥⎤

[UAUSUV]=[U11U12U13U21U22U23U31U32U33]×X~\begin{aligned} \left[\begin{matrix} U_A \\ U_S \\ U_V \\ \end{matrix}\right]= \left[\begin{matrix} U_{11} & U_{12} & U_{13} \\ U_{21} & U_{22} & U_{23} \\ U_{31} & U_{32} & U_{33} \\ \end{matrix}\right]\times\tilde{X} \end{aligned}⎣⎡UAUSUV⎦⎤=⎣⎡U11U21U31U12U22U32U13U23U33⎦⎤×X~

状态反馈输入 UUU：
U=[UAUSUV]=[U11⊗I3U12⊗T1U13⊗I3U21⊗T2U22⊗I2U23⊗T2U31⊗I3U32⊗T1U33⊗I3]×X~\begin{aligned}U&= \left[\begin{matrix} U_A \\ U_S \\ U_V \\ \end{matrix}\right]= \left[\begin{matrix} U_{11}\otimes I_3 & U_{12}\otimes T_1 & U_{13}\otimes I_3 \\ U_{21}\otimes T_2 & U_{22}\otimes I_2 & U_{23}\otimes T_2 \\ U_{31}\otimes I_3 & U_{32}\otimes T_1 & U_{33}\otimes I_3 \\ \end{matrix}\right]\times\tilde{X} \end{aligned}U=⎣⎡UAUSUV⎦⎤=⎣⎡U11⊗I3U21⊗T2U31⊗I3U12⊗T1U22⊗I2U32⊗T1U13⊗I3U23⊗T2U33⊗I3⎦⎤×X~

U11⊗I3=[−r1LAA00−r2LAA00−r3I00−r4I000−r1LAA00−r2LAA00−r3I00−r4I000−r1LAA00−r2LAA00−r3I00−r4I]\begin{aligned}U_{11} \otimes I_3= \left[\begin{matrix} -r_1 L_{AA} & 0 & 0 & -r_2 L_{AA} & 0 & 0 & -r_3 I & 0 & 0 & -r_4 I & 0 & 0 \\ 0 & -r_1 L_{AA} & 0 & 0 & -r_2 L_{AA} & 0 & 0 & -r_3 I & 0 & 0 & -r_4 I & 0 \\ 0 & 0 & -r_1 L_{AA} & 0 & 0 & -r_2 L_{AA} & 0 & 0 & -r_3 I & 0 & 0 & -r_4 I \\ \end{matrix}\right] \end{aligned}U11⊗I3=⎣⎡−r1LAA000−r1LAA000−r1LAA−r2LAA000−r2LAA000−r2LAA−r3I000−r3I000−r3I−r4I000−r4I000−r4I⎦⎤

U21⊗T2=[−r1LSA00−r2LSA00−r3I00−r4I000−r1LSA00−r2LSA00−r3I00−r4I0]\begin{aligned}U_{21} \otimes T_2= \left[\begin{matrix} -r_1 L_{SA} & 0 & 0 & -r_2 L_{SA} & 0 & 0 & -r_3 I & 0 & 0 & -r_4 I & 0 & 0 \\ 0 & -r_1 L_{SA} & 0 & 0 & -r_2 L_{SA} & 0 & 0 & -r_3 I & 0 & 0 & -r_4 I & 0 \\ \end{matrix}\right] \end{aligned}U21⊗T2=[−r1LSA00−r1LSA00−r2LSA00−r2LSA00−r3I00−r3I00−r4I00−r4I00]

U31⊗I3=[000000000000000000000000000000000000]\begin{aligned}U_{31} \otimes I_3= \left[\begin{matrix} 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ \end{matrix}\right] \end{aligned}U31⊗I3=⎣⎡000000000000000000000000000000000000⎦⎤

U12⊗T1=[−r1LAS0−r2LAS00−r1LAS0−r2LAS0000]\begin{aligned}U_{12} \otimes T_1= \left[\begin{matrix} -r_1 L_{AS} & 0 & -r_2 L_{AS} & 0 \\ 0 & -r_1 L_{AS} & 0 & -r_2 L_{AS} \\ 0 & 0 & 0 & 0 \\ \end{matrix}\right] \end{aligned}U12⊗T1=⎣⎡−r1LAS000−r1LAS0−r2LAS000−r2LAS0⎦⎤

U22⊗I2=[−r1LSS0−r2LSS00−r1LSS0−r2LSS]\begin{aligned}U_{22} \otimes I_2= \left[\begin{matrix} -r_1 L_{SS} & 0 & -r_2 L_{SS} & 0 \\ 0 & -r_1 L_{SS} & 0 & -r_2 L_{SS} \\ \end{matrix}\right] \end{aligned}U22⊗I2=[−r1LSS00−r1LSS−r2LSS00−r2LSS]

U32⊗T1=[−r1LUS0−r2LUS00−r1LUS0−r2LUS0000]\begin{aligned}U_{32} \otimes T_1= \left[\begin{matrix} -r_1 L_{US} & 0 & -r_2 L_{US} & 0 \\ 0 & -r_1 L_{US} & 0 & -r_2 L_{US} \\ 0 & 0 & 0 & 0 \\ \end{matrix}\right] \end{aligned}U32⊗T1=⎣⎡−r1LUS000−r1LUS0−r2LUS000−r2LUS0⎦⎤

U13⊗I3=[000000000000000000]\begin{aligned}U_{13} \otimes I_3= \left[\begin{matrix} 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ \end{matrix}\right] \end{aligned}U13⊗I3=⎣⎡000000000000000000⎦⎤

U23⊗T2=[−r1LSU00−r1LSU000−r1LSU00−r1LSU0]\begin{aligned}U_{23} \otimes T_2= \left[\begin{matrix} -r_1 L_{SU} & 0 & 0 & -r_1 L_{SU} & 0 & 0 \\ 0 & -r_1 L_{SU} & 0 & 0 & -r_1 L_{SU} & 0 \\ \end{matrix}\right] \end{aligned}U23⊗T2=[−r1LSU00−r1LSU00−r1LSU00−r1LSU00]

U33⊗I3=[−r1LUU00−r2LUU000−r1LUU00−r2LUU000−r1LUU00−r2LUU]\begin{aligned}U_{33} \otimes I_3= \left[\begin{matrix} -r_1 L_{UU} & 0 & 0 & -r_2 L_{UU} & 0 & 0 \\ 0 & -r_1 L_{UU} & 0 & 0 & -r_2 L_{UU} & 0 \\ 0 & 0 & -r_1 L_{UU} & 0 & 0 & -r_2 L_{UU} \\ \end{matrix}\right] \end{aligned}U33⊗I3=⎣⎡−r1LUU000−r1LUU000−r1LUU−r2LUU000−r2LUU000−r2LUU⎦⎤

Lemma

If Re(uˉ)<0\text{Re}(\bar{u})<0Re(uˉ)<0, r1≥0r_1 \ge 0r1≥0, α^≥0\hat{\alpha} \ge 0α^≥0,
fuˉ(λ)=λ2+(α^−r2uˉ)λ+β^−r1uˉf_{\bar{u}}(\lambda) = \lambda^2 + (\hat{\alpha} - r_2 \bar{u})\lambda+\hat{\beta}-r_1\bar{u} fuˉ(λ)=λ2+(α^−r2uˉ)λ+β^−r1uˉ
the roots of above equation lie on the left half complex plane if and only if:
r1>β^Re(uˉ)Re(uˉ)2+Im(uˉ)2r_1 > \frac{\hat{\beta} \text{Re} (\bar{u})}{\text{Re}(\bar{u})^2 + \text{Im} (\bar{u})^2} r1>Re(uˉ)2+Im(uˉ)2β^Re(uˉ)

r2>α^Re(uˉ)+r_2 > \frac{\hat{\alpha}}{\text{Re}(\bar{u})} + \frac{}{} r2>Re(uˉ)α^+

【Paper】2020_Distributed optimal consensus with obstacle avoidance algorithm of mixed-order UAVs相关推荐

【Paper】2019_Distributed Optimal Control of Energy Storages in a DC Microgrid with Communication Dela
M. Shi, X. Chen, J. Zhou, Y. Chen, J. Wen and H. He, "Distributed Optimal Control of Energy Sto ...
【Paper】2015_Leader–follower consensus of linear multi-agent systems with unknown external disturbanc
[Cao W, Zhang J, Ren W. Leader–follower consensus of linear multi-agent systems with unknown externa ...
【Paper】2021_Observer-based distributed consensus for multi-agent systems with directed networks and
Shuzhen Yu, Zhiyong Yu, Haijun Jiang, Xuehui Mei, Observer-based distributed consensus for multi-age ...
【Paper】2010_Distributed optimal control of multiple systems
Dong W. Distributed optimal control of multiple systems[J]. International Journal of Control, 2010, ...
【Paper】2020_Event-triggered bipartite consensus over cooperation-competition networks under DoS atta
Hu, A., Park, J.H., Cao, J. et al. Event-triggered bipartite consensus over cooperation-competition ...
【Paper】2021_Distributed Consensus Tracking of Networked Agent Systems Under Denial-of-Service Attack
Y. Wan, G. Wen, X. Yu and T. Huang, "Distributed Consensus Tracking of Networked Agent Systems ...
【Paper】2019_Consensus Control of Multiple AUVs Recovery System Under Switching Topologies and Time D
Zhang W, Zeng J, Yan Z, et al. Consensus control of multiple AUVs recovery system under switching to ...
【Paper】2003_Consensus Problems in Networks of Agents with Switching Topology and Time-Delays
此篇文章主要在于仿真代码,关于文献的解释请参考 [Paper]2003_Murr_Consensus Problems in Networks of Agents with Switching Top ...
【Paper】2017_水下潜航器编队海洋勘测的协调控制方法研究
友情链接:[paper]2019_Consensus Control of Multiple AUVs Recovery System Under Switching Topologies and T ...

【Paper】2020_Distributed optimal consensus with obstacle avoidance algorithm of mixed-order UAVs