Yan Z, Jiang A, Lai C. Adaptive Formation Control of Unmanned Underwater Vehicles with Collision Avoidance under Unknown Disturbances[J]. Journal of Marine Science and Engineering, 2022, 10(4): 516.

文章目录

1. Introduction
2. Preliminaries and Problem Formulation
- 2.1. Feedback linearization of UUV model
- 2.2. Graph theory
- 2.3. Artificial potential field
3. Adaptive Formation Control Scheme Design
- 3.1. Adaptive sliding mode disturbance observer design
- 3.2. Adaptive formation control with collision avoidance under unknown disturbance
4. Experimental Results and Simulation
- 4.1. Distrubance observer simulation result
- 4.2. Collision Avoidance Simulation Result

1. Introduction

2. Preliminaries and Problem Formulation

2.1. Feedback linearization of UUV model

UUV的模型为

η˙=J(η)vMv˙=τˉ+ωˉ−D(v)v−C(v)(1)\begin{aligned} \dot{\eta} &= J(\eta) v \\ M \dot{v} &= \bar{\tau} + \bar{\omega} - D(v) v - C(v) \end{aligned} \tag{1}η˙Mv˙=J(η)v=τˉ+ωˉ−D(v)v−C(v)(1)

其中
η=[xyzθψ]\eta = \left[\begin{matrix} x & y & z & \theta & \psi \end{matrix}\right]η=[xyzθψ]，
v=[uvwqr]v = \left[\begin{matrix} u & v & w & q & r \end{matrix}\right]v=[uvwqr]。

线性简化后的UUV模型为

p˙i=viv˙i=τi+ω(4)\begin{aligned} \dot{p}_i &= v_i \\ \dot{v}_i &= \tau_i + \omega \end{aligned} \tag{4}p˙iv˙i=vi=τi+ω(4)

其中
pi=[xiyiziθiψi]p_i = \left[\begin{matrix} x_i & y_i & z_i & \theta_i & \psi_i \end{matrix}\right]pi=[xiyiziθiψi]，
vi=[uiviwiqiri]v_i = \left[\begin{matrix} u_i & v_i & w_i & q_i & r_i \end{matrix}\right]vi=[uiviwiqiri]，
τi\tau_iτi 是控制输入，
ω\omegaω 是未知干扰。

假设存在了一个虚拟领航者

p˙l=vlv˙l=gl(t)(5)\begin{aligned} \dot{p}_l &= v_l \\ \dot{v}_l &= g_l(t) \end{aligned} \tag{5}p˙lv˙l=vl=gl(t)(5)

误差变量为

epi=pi−pl−εievi=vi−vl(6)\begin{aligned} e_{pi} &= p_i - p_l - \varepsilon_i \\ e_{vi} &= v_i - v_l \end{aligned} \tag{6}epievi=pi−pl−εi=vi−vl(6)

其中
εi\varepsilon_iεi 表示与虚拟领航者之间的期望距离。

2.2. Graph theory

2.3. Artificial potential field

与传统的人工势场法类似，有安全距离 rsr_srs，碰撞距离 rcr_crc，两个UUV之间的距离为 ∥dij∥\|d_ij\|∥dij∥。

当 ∥dij∥>rs\|d_{ij}\| > r_s∥dij∥>rs，安全，
当 rs≥∥dij∥>2rcr_s \ge\|d_{ij}\| > 2r_crs≥∥dij∥>2rc，需要避障函数，
当 2rc≥∥dij∥2 r_c \ge\|d_{ij}\|2rc≥∥dij∥，撞上了。

因此，为了保证别撞上，有人工势场函数 δij(d)\delta_{ij}(d)δij(d) 和动作函数 ς(d)\varsigma (d)ς(d) 定义如下

δij(d)=∫rsdς(s)dsς(d)={−βˉid2,d∈(2rc,rs)0,d∈[rs,∞)(8-9)\begin{aligned} \delta_{ij}(d) &= \int_{r_s}^{d} \varsigma(s) \text{d}s \\ \varsigma(d) &= \left\{\begin{aligned} &-\frac{\bar{\beta}_i}{d^2}, & d \in (2r_c, r_s) \\ & 0 , & d \in [r_s, \infty) \end{aligned}\right. \end{aligned} \tag{8-9}δij(d)ς(d)=∫rsdς(s)ds=⎩⎨⎧−d2βˉi,0,d∈(2rc,rs)d∈[rs,∞)(8-9)

其中 βˉi\bar{\beta}_iβˉi 是一个设计参数。

斥力为

τica=βi∑j∈Nic−∇xiδij(d)=−βi∑j∈Nicς(∥dij∥)dij∥dij∥(10)\begin{aligned} \tau_{i}^{ca} &= \beta_i \sum_{j \in N_i^c} - \nabla_{x_i} \delta_{ij}(d) \\ &= -\beta_i \sum_{j \in N_i^c} \varsigma(\|d_{ij}\|) \frac{d_{ij}}{\|d_{ij}\|} \end{aligned} \tag{10}τica=βij∈Nic∑−∇xiδij(d)=−βij∈Nic∑ς(∥dij∥)∥dij∥dij(10)

3. Adaptive Formation Control Scheme Design

3.1. Adaptive sliding mode disturbance observer design

由于干扰 ω\omegaω 是非线性且未知的，因此我们先想办法估算出来这个干扰。这里我们使用滑模的方法。

给定一个辅助状态估计误差 e0e_0e0

eo=z−v(13)\begin{aligned} e_o = z - v \\ \end{aligned} \tag{13}eo=z−v(13)

其中
zzz 是辅助状态向量，满足下述动态变化。

z˙=τ+vs(14)\begin{aligned} \dot{z} = \tau + v_s \\ \end{aligned} \tag{14}z˙=τ+vs(14)

其中
vsv_svs 是待设计的切换项。

将式 (4) (14) 代入到式 (13) 的微分中，有
e˙o=z˙−v˙=τ+vs−(τ+ω)=vs−ω(15)\begin{aligned} \dot{e}_o &= \dot{z} - \dot{v} \\ &= \tau + v_s - ( \tau+\omega ) \\ &= v_s - \omega \end{aligned} \tag{15}e˙o=z˙−v˙=τ+vs−(τ+ω)=vs−ω(15)

为了保证误差变为0，我们设计如下切换项
vs=−Λ1eo−Λeomn−Ksgn(eo)(16)\begin{aligned} \blue{ v_s = -\Lambda_1 e_o - \Lambda e_o ^{\frac{m}{n}} - K \text{sgn}(e_o) \\ } \end{aligned} \tag{16}vs=−Λ1eo−Λeonm−Ksgn(eo)(16)

3.2. Adaptive formation control with collision avoidance under unknown disturbance

编队控制方案为

τif=−μ1∑j∈Niaij(epi−epj)−μ2∑j∈Niaij(evi−evj)−θi(c1epi+c2evi)−vs(23)\begin{aligned} \tau_i^f = -\mu_1 \sum_{j \in N_i} a_{ij} (e_{pi} - e_{pj}) - \mu_2 \sum_{j \in N_i} a_{ij} (e_{vi} - e_{vj}) - \theta_i (c_1 e_{pi} + c_2 e_{vi}) - v_s \\ \end{aligned} \tag{23}τif=−μ1j∈Ni∑aij(epi−epj)−μ2j∈Ni∑aij(evi−evj)−θi(c1epi+c2evi)−vs(23)

τi=τif+τica(24)\begin{aligned} \tau_i = \tau_i^f + \tau_i^{ca} \\ \end{aligned} \tag{24}τi=τif+τica(24)