【Paper】2018_多无人机协同编队控制算法研究_林倩玉

原文地址：[1]林倩玉. 多无人机协同编队控制算法研究[D].哈尔滨工业大学,2018.

第 3 章 多无人机协同编队控制算法设计

3.1 引言

---

3.2 多无人机系统运动的状态控制

---

3.2.1 航向控制

航向角的动态模型是
$${\dot{\psi }}_i=\frac{1}{{\tau }_{\psi }}({\psi }_i^c-{\psi }_i)\text{\hspace{1em}(3-1)}$$

${\psi }_i$：当前航向角
${\psi }_i^c$：航向指令

航向角的一致性控制算法如下：
{ ψ i c = ψ i + 1 1 + n i u i u i = − ∑ j = 1 n a i j ( ψ i − ψ j ) (3-2) \left\{

$$\begin{aligned} &\psi^c_i = \psi_i + \frac{1}{1+n_i} u_i \\ &u_i = -\sum_{j=1}^n a_{ij} (\psi_i - \psi_j) \end{aligned}$$ \right.\tag{3-2} ⎩⎪⎪⎪⎨⎪⎪⎪⎧​​ψic​=ψi​+1+ni​1​ui​ui​=−j=1∑n​aij​(ψi​−ψj​)​(3-2)

$n_i$ 是无人机 $i$ 的邻居无人机的个数。

指定航向角的一致性算法为：
{ ψ i c = ψ i + 1 1 + n i u i u i = − b i ( ψ i − ψ ∗ ) − ∑ j = 1 n a i j ( ψ i − ψ j ) (3-3) \left\{

$$\begin{aligned} &\psi^c_i = \psi_i + \frac{1}{1+n_i} u_i \\ &u_i =\red{-b_i (\psi_i - \psi^*)} -\sum_{j=1}^n a_{ij} (\psi_i - \psi_j) \end{aligned}$$ \right.\tag{3-3} ⎩⎪⎪⎪⎨⎪⎪⎪⎧​​ψic​=ψi​+1+ni​1​ui​ui​=−bi​(ψi​−ψ∗)−j=1∑n​aij​(ψi​−ψj​)​(3-3)

${\psi }^{\ast }$ 为指定航向角。

---

3.2.2 速度控制

速度动态模型为：
$${\dot{v}}_i={\alpha }_v(v_i^c-v_i)\text{\hspace{1em}(3-5)}$$

速度一致性控制算法为：
{ v i c = v i + τ v u i u i = − ∑ j = 1 n a i j ( v i − v j ) (3-6) \left\{

$$\begin{aligned} &v^c_i = v_i + \tau_v u_i \\ &u_i = -\sum_{j=1}^n a_{ij} (v_i - v_j) \end{aligned}$$ \right.\tag{3-6} ⎩⎪⎪⎨⎪⎪⎧​​vic​=vi​+τv​ui​ui​=−j=1∑n​aij​(vi​−vj​)​(3-6)

指定编队收敛时的飞行速度控制算法为：
{ v i c = v i + τ v u i u i = − c i ( v i − v ∗ ) − ∑ j = 1 n a i j ( v i − v j ) (3-7) \left\{

$$\begin{aligned} &v^c_i = v_i + \tau_v u_i \\ &u_i = \red{-c_i (v_i - v^*)} -\sum_{j=1}^n a_{ij} (v_i - v_j) \end{aligned}$$ \right.\tag{3-7} ⎩⎪⎪⎨⎪⎪⎧​​vic​=vi​+τv​ui​ui​=−ci​(vi​−v∗)−j=1∑n​aij​(vi​−vj​)​(3-7)

$c_i>0$ 时控制系数。

---

3.2.3 高度控制

无人机高度动态模型为：
$${\ddot{z}}_i=-\frac{1}{{\tau }_{\dot{z}}}{\dot{z}}_i-\frac{1}{{\tau }_z}(z_i^c-z_i)\text{\hspace{1em}(3-8)}$$

高度一致性控制算法：
{ z i c = z i + τ z τ z ˙ z ˙ i + τ z u i u i = − k z ˙ i − ∑ j = 1 n a i j [ ( v i − v j ) + γ ( z ˙ i − z ˙ j ) ] (3-10) \left\{

$$\begin{aligned} &z^c_i = z_i + \frac{\tau_z}{\tau_{\dot{z}}} \dot{z}_i + \tau_z u_i \\ &u_i = -k\dot{z}_i -\sum_{j=1}^n a_{ij} [(v_i - v_j) + \gamma (\dot{z}_i - \dot{z}_j)] \end{aligned}$$ \right.\tag{3-10} ⎩⎪⎪⎪⎨⎪⎪⎪⎧​​zic​=zi​+τz˙​τz​​z˙i​+τz​ui​ui​=−kz˙i​−j=1∑n​aij​[(vi​−vj​)+γ(z˙i​−z˙j​)]​(3-10)

有外部参考输入下的算法：
{ z i c = z i + τ z τ z ˙ z ˙ i + τ z u i u i = − d i ( z i − z ∗ ) − k z ˙ i − ∑ j = 1 n a i j [ ( v i − v j ) + γ ( z ˙ i − z ˙ j ) ] (3-12) \left\{

$$\begin{aligned} &z^c_i = z_i + \frac{\tau_z}{\tau_{\dot{z}}} \dot{z}_i + \tau_z u_i \\ &u_i = \red{-d_i (z_i - z^*)} -k\dot{z}_i -\sum_{j=1}^n a_{ij} [(v_i - v_j) + \gamma (\dot{z}_i - \dot{z}_j)] \end{aligned}$$ \right.\tag{3-12} ⎩⎪⎪⎪⎨⎪⎪⎪⎧​​zic​=zi​+τz˙​τz​​z˙i​+τz​ui​ui​=−di​(zi​−z∗)−kz˙i​−j=1∑n​aij​[(vi​−vj​)+γ(z˙i​−z˙j​)]​(3-12)

---

3.3 多无人机系统编队的队形控制算法

---

3.3.1 平面队形控制

当编队稳定时，其横侧向平面内有：
$$(x_i^{{}^{\prime }}-x_{iF}^{{}^{\prime }})-(x_j^{{}^{\prime }}-x_{jF}^{{}^{\prime }})=0$$$$(y_i^{{}^{\prime }}-y_{iF}^{{}^{\prime }})-(y_j^{{}^{\prime }}-y_{jF}^{{}^{\prime }})=0$$

即：
$$(x_i^{{}^{\prime }}-x_{iF}^{{}^{\prime }})-(x_j^{{}^{\prime }}-x_{jF}^{{}^{\prime }})=x_i^{{}^{\prime }}-x_j^{{}^{\prime }}-x_{ij}^{{}^{\prime }r}=0$$$$(y_i^{{}^{\prime }}-y_{iF}^{{}^{\prime }})-(y_j^{{}^{\prime }}-y_{jF}^{{}^{\prime }})=y_i^{{}^{\prime }}-y_j^{{}^{\prime }}-y_{ij}^{{}^{\prime }r}=0$$

$x_{ij}^{{}^{\prime }r},y_{ij}^{{}^{\prime }r}$ 分别表示无人机之间的预定机间距离。

多无人机协同编队的控制算法如下：
{ v i c = v i − ∑ j = 1 n a i j [ τ v ( v i − v j ) + k i v ( x i ′ − x j ′ − x i j ′ r ) ] ψ i c = ψ i − ∑ j = 1 n a i j [ 1 1 + n i ( ψ i − ψ j ) + k i ψ ∑ j ∈ N i ( y i ′ − y j ′ − y i j ′ r ) ] (3-16) \left\{

$$\begin{aligned} &v^c_i = v_i - \sum_{j=1}^n a_{ij} [\tau_v(v_i-v_j) + k^v_i(x^{'}_i -x^{'}_j - x^{'r}_{ij})] \\ &\psi^c_i = \psi_i - \sum_{j=1}^n a_{ij} [\frac{1}{1+n_i} (\psi_i - \psi_j) + k^{\psi}_i \sum_{j\in N_i} (y^{'}_i-y^{'}_j-y^{'r}_{ij})] \end{aligned}$$ \right.\tag{3-16} ⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧​​vic​=vi​−j=1∑n​aij​[τv​(vi​−vj​)+kiv​(xi′​−xj′​−xij′r​)]ψic​=ψi​−j=1∑n​aij​[1+ni​1​(ψi​−ψj​)+kiψ​j∈Ni​∑​(yi′​−yj′​−yij′r​)]​(3-16)

$k_i^v,k_i^{\psi }$ 分别用于调整无人机间隔的控制增益。

上式是进行队形控制的最基本算法。如果要指定编队的某些飞行状态，可由上式衍生出以下三种队形控制算法：

1）指定飞行航向，${\psi }_i\to {\psi }^{\ast }$
{ v i c = v i − ∑ j = 1 n a i j [ τ v ( v i − v j ) + k i v ( x i ′ − x j ′ − x i j ′ r ) ] ψ i c = ψ i − b i ( ψ i − ψ ∗ ) − ∑ j = 1 n a i j [ 1 1 + n i ( ψ i − ψ j ) + k i ψ ∑ j ∈ N i ( y i ′ − y j ′ − y i j ′ r ) ] (3-17) \left\{

$$\begin{aligned} &v^c_i = v_i - \sum_{j=1}^n a_{ij} [\tau_v(v_i-v_j) + k^v_i(x^{'}_i -x^{'}_j - x^{'r}_{ij})] \\ &\psi^c_i = \psi_i - \red{b_i(\psi_i - \psi^*)} - \sum_{j=1}^n a_{ij} [\frac{1}{1+n_i} (\psi_i - \psi_j) + k^{\psi}_i \sum_{j\in N_i} (y^{'}_i-y^{'}_j-y^{'r}_{ij})] \end{aligned}$$ \right.\tag{3-17} ⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧​​vic​=vi​−j=1∑n​aij​[τv​(vi​−vj​)+kiv​(xi′​−xj′​−xij′r​)]ψic​=ψi​−bi​(ψi​−ψ∗)−j=1∑n​aij​[1+ni​1​(ψi​−ψj​)+kiψ​j∈Ni​∑​(yi′​−yj′​−yij′r​)]​(3-17)

2）指定飞行速度

3）指定飞行速度和飞行航向

---

3.3.2 纵向队形控制

{ z i c = z i + τ z τ z ˙ z ˙ i + τ z u i u i = − k z ˙ i − ∑ j = 1 n a i j [ ( z i − z j − z i j r ) + γ ( z ˙ i − z ˙ j ) ] (3-20) \left\{

$$\begin{aligned} &z^c_i = z_i + \frac{\tau_z}{\tau_{\dot{z}}} \dot{z}_i + \tau_z u_i \\ &u_i = -k\dot{z}_i -\sum_{j=1}^n a_{ij} [(\red{z_i - z_j - z^r_{ij}}) + \gamma (\dot{z}_i - \dot{z}_j)] \end{aligned}$$ \right.\tag{3-20} ⎩⎪⎪⎪⎨⎪⎪⎪⎧​​zic​=zi​+τz˙​τz​​z˙i​+τz​ui​ui​=−kz˙i​−j=1∑n​aij​[(zi​−zj​−zijr​)+γ(z˙i​−z˙j​)]​(3-20)

---

3.3.3 三维空间队形控制算法

3.4 多无人机协同编队的队形保持

3.5 多无人机协同编队的队形变化

当编队稳定收敛时，编队成员之间的机间距离满足式（3-22）时，则认为队形稳定。

{ lim ⁡ t → ∞ ( x i ′ ( t ) − x j ′ ( t ) ) = x i j ′ ( t ) lim ⁡ t → ∞ ( y i ′ ( t ) − y j ′ ( t ) ) = y i j ′ ( t ) lim ⁡ t → ∞ ( z i ′ ( t ) − z j ′ ( t ) ) = z i j ′ ( t ) lim ⁡ t → ∞ ( v i ( t ) − v j ( t ) ) = 0 lim ⁡ t → ∞ ( ψ i ( t ) − ψ j ( t ) ) = 0 (3-22) \left\{

$$\begin{aligned} &\lim_{t \rightarrow \infty} ( x_i^\prime(t) - x_j^\prime(t) ) = x_{ij}^\prime(t) \\ &\lim_{t \rightarrow \infty} ( y_i^\prime(t) - y_j^\prime(t) ) = y_{ij}^\prime(t) \\ &\lim_{t \rightarrow \infty} ( z_i^\prime(t) - z_j^\prime(t) ) = z_{ij}^\prime(t) \\ &\lim_{t \rightarrow \infty} ( v_i(t) - v_j(t) ) = 0 \\ &\lim_{t \rightarrow \infty} ( \psi_i(t) - \psi_j(t) ) = 0 \end{aligned}$$ \right.\tag{3-22} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧​​t→∞lim​(xi′​(t)−xj′​(t))=xij′​(t)t→∞lim​(yi′​(t)−yj′​(t))=yij′​(t)t→∞lim​(zi′​(t)−zj′​(t))=zij′​(t)t→∞lim​(vi​(t)−vj​(t))=0t→∞lim​(ψi​(t)−ψj​(t))=0​(3-22)

---

3.6 多无人机协同编队算法仿真验证

3.6.1 状态控制仿真验证

高度有错误，修改后调出来了。
同时还给出了高度爬升时的速度曲线。

下图结果对应程序 Fig3_6_1_20220812.m

---

3.6.2 队形控制仿真验证

论文中的仿真结果图如下：

---

下图结果对应程序 Fig3_6_2_20220813_formation

3.6.4 队形变换仿真验证

两个问题

1 高度曲线

高度曲线之前一直没有调整出来，最近又看了一下，发现了问题。

问题就出现在下图中箭头位置，应该是 $z_i-z_i^c$，但是论文中给的错了。

2 状态初值

位置状态的初始值如果设置不好的话，容易造成不一致的情况。

论文中给的 $y_4=-73$，仿真出来的结果刚好差了一个周期。
结果如下图，刚好从 $\frac{\pi }{4}=0.7854$ 跑到了 $\frac{\pi }{4}+2\pi =7.0686$。

解决办法就是调下一下初始值就行了。

---

需要程序代码可加+V：Zhao-Jichao