欢迎访问 生活随笔！
TAG聚合

生活随笔

当前位置： 首页 > 编程资源 > 编程问答 >内容正文

编程问答

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

发布时间：2025/4/5 编程问答 34 豆豆
生活随笔 收集整理的这篇文章主要介绍了 【Paper】2020_Distributed optimal consensus with obstacle avoidance algorithm of mixed-order UAVs 小编觉得挺不错的,现在分享给大家,帮大家做个参考.

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×3​03×3​03×3​03×3​​I3×3​03×3​03×3​03×3​​03×3​I3×3​03×3​03×3​​03×3​03×3​I3×3​03×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×3​03×3​03×3​I3×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​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​000000000000​000000000000​000000000000​​100000000000​010000000000​001000000000​​000100000000​000010000000​000001000000​​000000100000​000000010000​000000001000​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​xyzvxvyvzgθ−gϕ0gς−gξ0​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​+⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​000000000100​000000000010​000000000001​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​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​=−r1​j=1∑n​aij​(P~i​−P~j​)−r2​j=1∑n​aij​(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​=−r1​j=1∑n​aij​(Pi​−Pj​)−r2​j=1∑n​aij​(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}⎣⎡​UA​US​UV​​⎦⎤​​=⎣⎡​U11​U21​U31​​U12​U22​U32​​U13​U23​U33​​⎦⎤​×X~=⎣⎢⎢⎢⎢⎡​−r1​LAA​−r1​LSA​0​−r2​LAA​−r2​LSA​0​−r3​I00​−r4​I00​​−r1​LAS​−r1​LSS​−r1​LUS​​−r2​LAS​−r2​LSS​−r2​LUS​​​0−r1​LSU​−r1​LUU​​0−r2​LSU​−r2​LUU​​⎦⎥⎥⎥⎥⎤​​

[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}⎣⎡​UA​US​UV​​⎦⎤​=⎣⎡​U11​U21​U31​​U12​U22​U32​​U13​U23​U33​​⎦⎤​×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​=⎣⎡​UA​US​UV​​⎦⎤​=⎣⎡​U11​⊗I3​U21​⊗T2​U31​⊗I3​​U12​⊗T1​U22​⊗I2​U32​⊗T1​​U13​⊗I3​U23​⊗T2​U33​⊗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​=⎣⎡​−r1​LAA​00​0−r1​LAA​0​00−r1​LAA​​−r2​LAA​00​0−r2​LAA​0​00−r2​LAA​​−r3​I00​0−r3​I0​00−r3​I​−r4​I00​0−r4​I0​00−r4​I​⎦⎤​​

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​=[−r1​LSA​0​0−r1​LSA​​00​−r2​LSA​0​0−r2​LSA​​00​−r3​I0​0−r3​I​00​−r4​I0​0−r4​I​00​]​

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​=⎣⎡​000​000​000​000​000​000​000​000​000​000​000​000​⎦⎤​​

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​=⎣⎡​−r1​LAS​00​0−r1​LAS​0​−r2​LAS​00​0−r2​LAS​0​⎦⎤​​

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​=[−r1​LSS​0​0−r1​LSS​​−r2​LSS​0​0−r2​LSS​​]​

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​=⎣⎡​−r1​LUS​00​0−r1​LUS​0​−r2​LUS​00​0−r2​LUS​0​⎦⎤​​

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​=⎣⎡​000​000​000​000​000​000​⎦⎤​​

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​=[−r1​LSU​0​0−r1​LSU​​00​−r1​LSU​0​0−r1​LSU​​00​]​

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​=⎣⎡​−r1​LUU​00​0−r1​LUU​0​00−r1​LUU​​−r2​LUU​00​0−r2​LUU​0​00−r2​LUU​​⎦⎤​​

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+(α^−r2​uˉ)λ+β^​−r1​uˉ
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的全部内容，希望文章能够帮你解决所遇到的问题。

如果觉得生活随笔网站内容还不错，欢迎将生活随笔推荐给好友。