由于四旋翼无人机体积小，结构简单，飞行方便，适用场合广泛等特点受到广泛欢迎，四旋翼无人机有欠驱动，非线性，强耦合等控制难点，近年来出现大量针对四旋翼无人机的控制算法，提出了诸如比例-积分-微分，反步法，自抗扰，自适应等方法^[1-2]。反步法是以Lyapunov理论为基础，由前向后递推的设计方法。文献[3-5]利用积分反步法实现单个无人机的轨迹跟踪。文献[6]则基于反步法设计控制算法实现了多无人机时变编队的轨迹跟踪。滑模控制(SMC)是将被控系统的状态量拉到预先设计的滑模面并保持在滑模面上运动，能够有效针对模型不确定性，实现轨迹跟踪控制。文献[7]通过滑模控制实现了对单个无人机的位置控制与姿态控制，利用Lyapunov理论证明了稳定性并且通过仿真验证了所提方案的有效性。文献[8]提出了一种由反步法和滑模控制组合的方法，并用Lyapunov稳定性理论证明在所提算法的作用下单个无人机系统的稳定性。由于多旋翼无人机主要适用于环境受限，空间狭隘的场合，对控制精度有较高的要求。近年来，由于单个无人机在执行任务过程中存在一些局限性，因此多无人机协同控制的研究逐渐火热化。多无人机协同编队控制是多无人机自主协同控制方法中的重要技术，可用多无人机来完成一些复杂的任务，如自然灾害监视，军事探索，集群表演，航空测绘，植被保护等。文献[9-13]利用滑模控制实现对多个飞行器的控制，在多无人机执行任务时，需保证多无人机保持一定的队形执行任务。编队控制的方法主要有Leader-Follower法，基于行为法，虚拟结构法以及基于一致性的方法等。文献[14-17]采用Leader-Follower方法实现多机器人编队控制，根据Leader层的信息来实现对Follower层的控制。文献[18-19]提出基于一致性的控制策略实现了多自主体的编队控制，利用可通信的邻居自主个体的信息来更新自身状态，最终所有自主体状态达成一致。文献[20-21]采用仿生鸽群算法实现编队控制，利用图论和势场函数理论对编队中的拓扑结构和群体中的主从关系进行定义，实现整个队伍的控制。

四旋翼无人机在执行大范围工作时，有时需要多个无人机协同完成相同的工作。本文通过Leader-Follower策略设计了多无人机协同编队，基于滑模控制(SMC)设计了编队控制器使多个无人机实现期望编队，并且，通过积分反步法(IBS)设计了顶层Leader无人机的运行轨迹，从而实现整个无人机队伍的轨迹跟踪。然后，对所设计的控制算法进行了数值仿真实验，并通过视觉定位系统(VPS)对所设计算法进行了飞行实验，通过飞行实验验证了算法的可行性和实用性。文献[16]仅研究了基于Leader-Follower的多无人机编队控制，相比之下，本文还对Leader无人机采用反步法设计了轨迹跟踪，在实现多无人机编队的同时也实现了轨迹跟踪。与文献[8]中实现无人机轨迹跟踪所采用的反步法相比，本文的反步法引入了积分项，使得整个无人机队伍能够更加准确的跟踪上预先设定的轨迹。此外，除数值仿真实验外，还利用平台对所设计的基于Leader-Follower的多无人机编队轨迹跟踪算法进行了飞行实验验证，实验平台包括：搭载Matlab软件和ROS机器人操作系统的计算机、视觉定位系统和Parrot bebop 2无人机。

1 四旋翼模型建立

首先，构建四旋翼无人机模型^[22]，根据质心运动和牛顿-欧拉方程得到四旋翼无人机的平衡方程为

$ \left\{ {\begin{array}{*{20}{l}} {L = {I_x}\frac{{{\rm{d}}p}}{{{\rm{d}}t}} - ({I_y} - {I_z})qr}\\ {M = {I_y}\frac{{{\rm{d}}q}}{{{\rm{d}}t}} - ({I_z} - {I_x})rp}\\ {N = {I_z}\frac{{{\rm{d}}r}}{{{\rm{d}}t}} - ({I_x} - {I_y})pq} \end{array}} \right. $	（1）

式中:L、M、N分别为机体坐标系下三轴的合外力矩; p、q、r分别为机体坐标系下3个角速度; I_x、I_y、I_z分别为绕机体系下三轴的转动惯量。

$ \left\{ \begin{array}{l} \ddot x = \frac{{({\rm{cos}}\psi {\rm{sin}}\theta {\rm{cos}}\phi + {\rm{sin}}\phi {\rm{sin}}\psi )\sum\limits_{i = 4}^4 {{F_i}} - {K_x}\dot x}}{m}\\ \ddot y = \frac{{({\rm{sin}}\psi {\rm{sin}}\theta {\rm{cos}}\phi - {\rm{sin}}\phi {\rm{cos}}\psi )\sum\limits_{i = 4}^4 {{F_i}} - {K_y}\dot y}}{m}\\ \ddot z = \frac{{({\rm{cos}}\theta {\rm{cos}}\phi )\sum\limits_{i = 4}^4 {{F_i}} - {K_z}\dot z}}{m} \end{array} \right. $

（2）

式中：F_i(i=1, 2, 3, 4)为4个旋翼的升力；K_x、K_y、K_z为空气阻力系数；m为机体质量；x、y、z分别为地面坐标系中沿三轴方向机体的位移，根据坐标系转化可以得到p、q、r与欧拉角速度的关系为

$ \left\{ {\begin{array}{*{20}{l}} {\dot \phi = \frac{{p{\rm{cos}}\theta + q{\rm{sin}}\phi {\rm{sin}}\theta + r{\rm{cos}}\phi {\rm{sin}}\theta }}{{{\rm{cos}}\theta }}}\\ {\dot \theta = q{\rm{cos}}\phi + r{\rm{sin}}\phi }\\ {\dot \psi = \frac{{q{\rm{sin}}\phi + r{\rm{cos}}\phi }}{{{\rm{cos}}\theta }}} \end{array}} \right. $	（3）

式中：ϕ为滚转角；θ为俯仰角；ψ为偏航角。

根据四旋翼升力组合4组控制量为

$ \left\{ {\begin{array}{*{20}{l}} {{u_1} = \sum\limits_{i = 1}^4 {{F_1}} = {C_L}(\varOmega _1^2 + \varOmega _2^2 + \varOmega _3^2 + \varOmega _4^2)}\\ {{u_2} = l({F_4} - {F_2}) = U{C_L}(\varOmega _4^2 - \varOmega _2^2)}\\ {{u_3} = l({F_3} - {F_1}) = l{C_L}(\varOmega _3^2 - \varOmega _1^2)}\\ {{u_4} = d( - \varOmega _1^2 + \varOmega _2^2 - \varOmega _3^2 + \varOmega _4^2)} \end{array}} \right. $	（4）

式中：u₁为四旋翼飞行器受到的升力总和，用来控制位置；u₂, u₃, u₄为四旋翼飞行器受到的转动力矩，分别控制滚转角，俯仰角和偏航角；C_L为旋翼的升力系数; l为四旋翼飞行器质心到旋翼旋转轴之间的距离; d为反扭距系数, Ω₁, Ω₂, Ω₃, Ω₄为四旋翼4个轴的电极转速。最后，将控制量代入系统方程式(1)和式(2)中，可以得到系统的模型为

$ \left\{ \begin{array}{l} \begin{array}{*{20}{l}} {\dot x = {v_x}}\\ {{{\dot v}_x} = \frac{{{u_1}({\rm{cos}}\psi {\rm{sin}}\theta {\rm{cos}}\phi + {\rm{sin}}\phi {\rm{sin}}\psi )}}{m}}\\ {\dot y = {v_y}}\\ {{{\dot v}_y} = \frac{{{u_1}({\rm{sin}}\psi {\rm{sin}}\theta {\rm{cos}}\phi - {\rm{sin}}\phi {\rm{cos}}\psi )}}{m}} \end{array}\\ \begin{array}{*{20}{l}} {\dot z = {v_z}}\\ {{{\dot v}_z} = \frac{{{u_1}({\rm{cos}}\theta {\rm{cos}}\phi )}}{m} - g}\\ {\dot \phi = {v_\phi }}\\ {{{\dot v}_\phi } = \frac{{({I_y} - {I_z})\dot \theta \dot \psi + {u_2}}}{{{I_x}}}}\\ {\dot \theta = {v_\theta }} \end{array}\\ \begin{array}{*{20}{l}} {{{\dot v}_\theta } = \frac{{({I_z} - {I_x})\dot \phi \dot \psi + {u_3}}}{{{I_y}}}}\\ {\dot \psi = {v_\psi }}\\ {{{\dot v}_\psi } = \frac{{({I_x} - {I_y})\dot \phi \dot \theta + {u_4}}}{{{I_z}}}} \end{array} \end{array} \right. $

（5）

2 编队控制设计 2.1 Leader无人机轨迹跟踪设计

首先，运用backstepping算法设计Leader无人机的轨迹跟踪控制器^[23]，如图 1所示。由式(5)中高度子系统可得Leader无人机的高度子系统微分方程为

$ {\mathit{\boldsymbol{\dot z}} = {v_z}} $	（6）

$ {{{\dot v}_z} = \frac{{{u_1}({\rm{cos}}\theta {\rm{cos}}\phi )}}{m} - g} $	（7）

参考轨迹用(x_r, y_r, z_r)表示，由此可得到Leader无人机实时位置与参考轨迹在z方向上的误差：

$ {e_{z1}} = {z_{\rm{r}}} - z $	（8）

定义误差e_z1的积分项为

$ {e_{z2}} = k\int_0^t {({z_{\rm{r}}} - z)} {\rm{d}}\tau $	（9）

这里k为常数。选取Lyapunov函数V₁为

$ {V_1} = \frac{1}{2}e_{z1}^2 + \frac{1}{2}e_{z2}^2 $	（10）

对式(10)进行求导，可得

$ \begin{array}{l} {{\dot V}_1} = {e_{z1}}{{\dot e}_{z1}} + {e_{z2}}{{\dot e}_{z2}} = {e_{z1}}({{\dot e}_{z1}} + k{e_{z2}}) = \\ {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {e_{z1}}(k{e_{z2}} + {{\dot z}_{\rm{r}}} - \alpha ) \end{array} $	（11）

设计α为Leader无人机在z轴方向上跟踪上参考轨迹的虚拟控制量：

$ \alpha = k{e_{z2}} + {\dot z_{\rm{r}}} + \lambda {e_{z1}},\lambda > 0 $	（12）

由式(11)和式(12)可以得到，α的值能够保证$ \dot{V}_{1} \leqslant 0$，使得Lyapunov函数V₁的导数负定，可保证系统稳定。

Leader无人机在z轴方向上的速度与虚拟控制量α误差为

$ {e_{z3}} = \alpha - {v_z} = k{e_{z2}} + {\dot z_{\rm{r}}} + \lambda {e_{z1}} - \dot z $	（13）

由式(13)可以得到

$ {\dot e_{z1}} = {\dot z_r} - \dot z = {e_{z3}} - \lambda {e_{z1}} - k{e_{z2}} $	（14）

对式(13)求导可得到

$ {\dot e_{z3}} = {k^2}{e_{z1}} + {\ddot z_{\rm{r}}} + \lambda {\dot e_{z1}} - {\dot v_z} $	（15）

将式(7)、式(14)代入到式(15)，可以得到

$ \begin{array}{l} {{\dot e}_{z3}} = {k^2}{e_{z1}} + {{\ddot z}_{\rm{r}}} + \lambda ({e_{z3}} - \lambda {e_{z1}} - k{e_{z2}}) - \\ {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \frac{{{u_1}{\rm{cos}}\theta {\rm{cos}}\phi }}{m} + g \end{array} $

（16）

定义一个Lyapunov函数V₂为

$ {V_2} = \frac{1}{2}e_{z1}^2 + \frac{1}{2}e_{z2}^2 + \frac{1}{2}e_{z3}^2 $	（17）

对式(17)进行求导可得

$ \begin{array}{*{20}{c}} {{{\dot V}_2} = {e_{z1}}({{\dot e}_{z1}} + k{e_{z2}}) + {e_{z3}}{{\dot e}_{z3}} = {e_{z1}}({e_{z3}} - \lambda {e_{z1}}) + }\\ {{e_{z3}}{{\dot e}_{z3}} = - \lambda e_{z1}^2 + {e_{z3}}({e_{z1}} + {{\dot e}_{z3}})} \end{array} $	（18）

要使式(18)负定，可使得

$ \begin{array}{*{20}{l}} {{e_{z1}} + {{\dot e}_{z3}} = - {\lambda _1}{e_{z3}} = {e_{z1}} + {k^2}{e_{z1}} + {{\ddot z}_{\rm{r}}} + \lambda ({e_{z3}} - }\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \lambda {e_{z1}} - k{e_{z2}}) - \frac{{{u_1}{\rm{cos}}\theta {\rm{cos}}\phi }}{m} + g} \end{array} $

（19）

此时$\dot{V}_{2}=-\lambda e_{z 1}^{2}-\lambda_{1} e_{z 3}^{2} \leqslant 0 $，可使得系统稳定，由式(19)可得

$ \begin{array}{l} {u_1} = \\ {\kern 1pt} {\kern 1pt} {\kern 1pt} \frac{{m[(\lambda + {\lambda _1}){e_{z3}} + (1 + {k^2} - {\lambda ^2}){e_{z1}} + {{\ddot z}_{\rm{r}}} - k\lambda {e_{z2}} + g]}}{{{\rm{cos}}\theta {\rm{cos}}\phi }} \end{array} $	（20）

由于四旋翼无人机的位置控制是一个欠驱动控制系统，仅靠控制量u₁是难以完成的，因此引入中间变量u_x和u_y:

$ \left\{ {\begin{array}{*{20}{l}} {{u_y} = {\rm{sin}}\psi {\rm{sin}}\theta {\rm{cos}}\phi + {\rm{sin}}\phi {\rm{cos}}\psi }\\ {{u_x} = {\rm{cos}}\psi {\rm{sin}}\theta {\rm{cos}}\phi + {\rm{sin}}\phi {\rm{sin}}\psi } \end{array}} \right. $	（21）

与Leader无人机高度子系统推导过程类似，可以推导出u_x和u_y。

y表示Leader无人机在y方向上的实时位置，y_r为其在y方向上的参考位置，定义变量：

$ \left\{ {\begin{array}{*{20}{l}} {{e_{y1}} = {y_{\rm{r}}} - y}\\ {{e_{y2}} = {k_2}\int_0^t {({y_{\rm{r}}} - y)} {\rm{d}}\tau }\\ {{e_{y3}} = {k_2}{e_{y2}} + {{\dot y}_{\rm{r}}} + {\lambda _2}{e_{y1}} - \dot y} \end{array}} \right. $	（22）

经过类似式(6)~式(20)的推理可以得到

$ \begin{array}{l} {u_y} = {\rm{sin}}\psi {\rm{sin}}\theta {\rm{cos}}\phi + {\rm{sin}}\phi {\rm{cos}}\psi = \\ {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \frac{m}{{{u_1}\left[ {(1 + k_2^2 - \lambda _2^2){e_{y1}} + ({\lambda _3} + {\lambda _2}){e_{y3}} + {{\ddot y}_3} - {k_2}{\lambda _2}{e_y}} \right]}} \end{array} $

（23）

式中：k₂, λ₂, λ₃≥0。

x表示Leader无人机在x方向上的实时位置，x_r为其在x方向上的参考位置，定义变量：

$ \left\{ {\begin{array}{*{20}{l}} {{e_{x1}} = {x_r} - x}\\ {{e_{x2}} = {k_3}\int_0^t {({x_r} - x)} {\rm{d}}\tau }\\ {{e_{x3}} = {k_3}{e_{x2}} + {{\dot x}_{\rm{r}}} + {\lambda _4}{e_{x1}} - \dot x} \end{array}} \right. $	（24）

类似地，可以解出

$ \begin{array}{*{20}{l}} {{u_x} = {\rm{cos}}\psi {\rm{sin}}\theta {\rm{cos}}\phi + {\rm{sin}}\phi {\rm{sin}}\psi = }\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \frac{m}{{{u_1}[(1 + k_3^2 - \lambda _4^2){e_{x1}} + ({\lambda _5} + {\lambda _4}){e_{x3}} + {{\ddot x}_3} - {k_3}{\lambda _4}{e_{x2}}]}}} \end{array} $	（25）

式中：k₃, λ₄, λ₅≥0。

通过反解模块可以求出俯仰角和横滚角的值，即

$ \left\{ {\begin{array}{*{20}{l}} {{\phi _{\rm{d}}} = {\rm{arcsin}}({u_x}{\rm{sin}}\psi - {u_y}{\rm{cos}}\psi )}\\ {{\theta _{\rm{d}}} = {\rm{arcsin}}\frac{{{u_x} - {\rm{sin}}{\phi _{\rm{d}}}{\rm{sin}}\psi }}{{{\rm{cos}}{\phi _{\rm{d}}}{\rm{cos}}\psi }}} \end{array}} \right. $	（26）

通过中间变量，将四旋翼的水平位置控制与姿态控制联系起来，其他控制量进行姿态控制从而实现控制飞机的目的，图 1所示的姿态控制量u₂、u₃、u₄采用串级PID控制设计，姿态角速度作为内环，姿态角作为外环，外环回路的输出信号作为内环回路的设定值，由内环控制器的输出作用于执行器，采用串级PID控制更好的抑制了外界干扰。于是，通过控制量u₁、u_x、u_y实现高度和水平位置的控制，通过控制量u₂、u₃、u₄控制姿态角实现飞机的姿态控制。

2.2 编队设计

无人机之间通过信息传递实现编队控制，针对如图 2所示的通信拓扑结构，o代表Leader无人机且根据2.1节算法设计轨迹跟踪，无人机j和无人机k为Leader无人机o的直接Follower，无人机i为Leader无人机j的直接Follower，无人机l为Leader无人机k的直接Follower。通过对无人机o的轨迹设计并且基于Leader-Follower控制策略可以实现对整个队伍的轨迹设计及编队控制，下面以Leader无人机j和Follower无人机i为例给出所设计的Leader-Follower编队控制算法。

无人机的运动学模型表示为

$ \left\{ {\begin{array}{*{20}{l}} {{{\dot x}_i} = {v_i}{\rm{cos}}{\psi _i}}\\ {{{\dot y}_i} = {v_i}{\rm{sin}}{\psi _i}}\\ {{{\dot \psi }_i} = {w_i}} \end{array}} \right. $	（27）

式中：x_i、y_i为当前无人机i的位置；v_i为其前向速度；ψ_i为其前向速度与x轴之间的夹角；ω_i为其角速度。

无人机i与无人机j当前时刻实际的前向距离和侧向距离分别用d_ij^x、d_ij^y表示，如图 3所示，计算公式为^[24]

$ \left\{ {\begin{array}{*{20}{l}} {d_{ij}^x = ({x_i} - {x_j}){\rm{cos}}{\psi _j} + ({y_i} - {y_j}){\rm{sin}}{\psi _j} + }\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {d_{\rm{f}}}{\rm{cos}}({\psi _i} - {\psi _j})}\\ {d_{ij}^y = ({x_i} - {x_j}){\rm{sin}}{\psi _j} - ({y_i} - {y_j}){\rm{cos}}{\psi _j} - }\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {d_{\rm{f}}}{\rm{sin}}({\psi _i} - {\psi _j})} \end{array}} \right. $

（28）

式中：d_f为一个比较小的无向距离且d_f>0。

令d_x^d、d_y^d表示无人机i与无人机j之间在d_ij^x方向与d_ij^y方向的期望距离，则无人机i与j实际距离与期望编队距离之间的误差可表示为

$ \left\{ {\begin{array}{*{20}{l}} {e_{ij}^x = d_{ij}^x - d_x^{\rm{d}}}\\ {e_{ij}^y = d_{ij}^y - d_y^{\rm{d}}}\\ {e_{ij}^\psi = {\psi _i} - {\psi _j}} \end{array}} \right. $	（29）

对式(29)进行求导可以得到

$ \left\{ {\begin{array}{*{20}{l}} {\dot e_{ij}^x = - {v_j} - e_{ij}^y{w_j} + {v_i} \cos e_{ij}^\psi - {d_{\rm{f}}}{w_i}\sin e_{ij}^\psi }\\ {\dot e_{ij}^y = e_{ij}^x{w_j} - {v_i}{\rm{sin}}e_{ij}^\psi - {d_{\rm{f}}}{w_i} \cos e_{ij}^\psi } \end{array}} \right. $	（30）

这里，将(e_ij^x, e_ij^y)看作状态变量e_ij，(v_i, ω_i)看作控制器u。

针对i、j 2架无人机之间控制方程设计控制器：

$ {\mathit{\boldsymbol{\dot e}}_{ij}} = \mathit{\boldsymbol{A}}({\mathit{\boldsymbol{e}}_{ij}}) + {\mathit{\boldsymbol{B}}_{ij}}\mathit{\boldsymbol{u}} $	（31）

式中：状态向量e_ij=[e_ij^x, e_ij^y]^T, 编队的控制输入为$ \boldsymbol{u}=\left[\begin{array}{ll} v_{i}, & \omega_{i} \end{array}\right]^{\mathrm{T}} ; \boldsymbol{A}\left(\boldsymbol{e}_{i j}\right)=\left[\begin{array}{c} -v_{j}-e_{i j}^{y} w_{j} \\ e_{i j}^{x} w_{j} \end{array}\right]$; $\boldsymbol{B}_{i j}=\left[\begin{array}{cc} \operatorname{cos}e_{i j}^{\psi} & -d_{\mathrm{f}} \sin e_{i j}^{\psi} \\ -\sin e_{i j}^{\psi} & -d_{\mathrm{f}} \operatorname{cos}e_{i j}^{\psi} \end{array}\right] $。

然后，基于滑模控制设计协同编队控制律u_i。

设计无人机系统的切换函数为

$ {\mathit{\boldsymbol{s}}_i} = \mathit{\boldsymbol{c}} \times {\mathit{\boldsymbol{e}}_{ij}},i = 1,2, \cdots ,n $	（32）

式中：c∈R^2×2且可逆；n为Follower无人机的个数。

设计无人机i的滑模趋近律为

$ {\mathit{\boldsymbol{\dot s}}_i} = - {\varepsilon _1} {\rm{sgn}} ({\mathit{\boldsymbol{s}}_i}) $	（33）

式中：ε₁>0为滑模运动收敛到滑模面的速度，通过求解可得到协同的控制律u_i为

$ {u_i} = - {(\mathit{\boldsymbol{c}}{\mathit{\boldsymbol{B}}_{ij}})^{ - 1}}({\varepsilon _1} {\rm{sgn}} ({\mathit{\boldsymbol{s}}_i}) + \mathit{\boldsymbol{cA}}({\mathit{\boldsymbol{e}}_{ij}})) $	（34）

设计Lyapunov函数V₃为

$ {V_3} = \frac{1}{2}\mathit{\boldsymbol{s}}_i^{\rm{T}}{\mathit{\boldsymbol{s}}_i} $	（35）

对Lyapunov函数V₃求导可得

$ {\dot V_3} = \mathit{\boldsymbol{s}}_i^{\rm{T}}{\mathit{\boldsymbol{\dot s}}_i} = \mathit{\boldsymbol{s}}_i^{\rm{T}}( - {\varepsilon _1} {\rm{sgn}} ({\mathit{\boldsymbol{s}}_i})) = - {\varepsilon _1}\left\| {{\mathit{\boldsymbol{s}}_i}} \right\| \le 0 $	（36）

由此可知协同编队控制律式(34)能够使得多无人机系统满足Lyapunov稳定条件，从而实现期望编队。

通过2.1节算法设计控制律式(20)、式(23)、式(25)实现对无人机o的轨迹跟踪，无人机o作为Leader无人机实现对它的直接Follower无人机的控制，通过2.2节编队控制算法设计协同编队控制律(34)实现对无人机j和无人机k的控制，Follower无人机j和Follower无人机k与Leader无人机o在x，y方向上分别保持固定的距离。无人机i作为无人机j的直接Follower，通过协同编队控制律(34)，实现对无人机i的控制，Follower无人机i在x，y方向上与无人机j保持固定的距离；同样地，无人机l作为无人机k的直接Follower，应用协同控制律(34)能够使得Follower无人机l在x, y方向上与无人机k保持固定的距离。

3 仿真与实验

为了验证设计算法的可行性，采用仿真与实验2种方法进行验证，仿真与实验过程中涉及的位置、速度、角速度单位分别为m、m/s、rad/s。对于Leader无人机设定参考轨迹为

$ \left\{ {\begin{array}{*{20}{l}} {{x_{\rm{r}}} = 0.5{\rm{cos}}(0.02t)}\\ {{y_{\rm{r}}} = 0.5{\rm{sin}}(0.02t)}\\ {{z_{\rm{r}}} = 1} \end{array}} \right. $	（37）

在仿真过程中采用如图 4(a)所示的通信拓扑，Leader无人机跟踪如式(37)所示的轨迹，UAV 1和UAV 2为Leader无人机的直接Follower，在x方向设置位置误差都为0.8 m，在y方向设置位置误差分别为±1.5 m，UAV 3为UAV 1的直接Follower，在x方向上设置位置误差为1.5 m，在y方向设置位置误差为-1.5 m，UAV 4为UAV 2的直接Follower，在x方向设置位置误差为1.5 m，在y方向设置位置误差为1.5 m。在实验过程中，由于实验场地和实验设备等条件限制，采用3架无人机进行实验，通信拓扑如图 4(b)所示，实验设置Leader无人机的参考轨迹为式(37)所示，Leader无人机与Follower 1无人机和Follower 2无人机在x方向上位置差值都设置为0.8 m，在y方向上位置差值分别设置为±0.8 m。

3.1 仿真测试

仿真与实验过程中涉及的参数如表 1所示。通过Matlab仿真验证设计算法，图 5为在x、y平面设定的参考轨迹(37)以及Leader无人机运行的实际轨迹。图 6为x方向、y方向、z方向上Leader无人机实际轨迹与参考轨迹(37)的误差。在x、y方向上，Leader无人机的起始位置与参考轨迹存在较大误差，但是持续时间比较短。在z方向上，起始位置与参考轨迹也存在误差，z方向误差值比较稳定，主要是因为在轨迹的变化过程中z设定为定值。

从编队仿真结果来看，图 7为5架无人机组成的队形示意图。图 8为每一个Follower无人机与其直接Leader无人机在x方向之间的距离，UAV 1和UAV 2与Leader无人机之间的位置差值都是0.8 m，接着UAV 1作为UAV 3的直接Leader, UAV 3与UAV 1在x方向上位置差值为1.5 m。UAV 2作为UAV 4的直接Leader，UAV 4与UAV 2在x方向上位置差值为1.5 m。图 9为每一个Follower无人机与其直接Leader无人机之间在y方向上的距离，UAV 1和UAV 2与Leader无人机之间的位置差值分别为-1 m和1 m，UAV 3和UAV 1之间的位置差值为-1.5 m，UAV 4与UAV 2之间的距离为1.5 m，5架无人机构成了一个三角编队队形。

3.2 实验测试

本系统是基于视觉定位系统(VPS)和MATLAB/Simulink+ROS(Robot Operating System)平台来实现对Parrot bebop 2无人机的室内定位以及相关指令来完成任务。Parrot Bebop 2四旋翼无人机由机体结构，动力系统，飞行控制器，通信模块组成。视觉定位系统通过识别四旋翼无人机上的标记点，实时解算出无人机位置与姿态信息。实验平台(如图 10所示)借助ROS系统获取到四旋翼无人机的实时信息，通过控制算法计算，控制命令发送，该系统可以保证控制算法的实时性要求。

本次实验在x、y方向上分别平移跟踪轨迹如式(37)所示，图 11(a)显示的是所设定的Leader无人机的参考轨迹，图 11 (b)为根据IBS算法式(20)、式(23)、式(25)实现的Leader无人机的实际轨迹，根据实验图像看出Leader无人机能够完全跟踪上设定的参考轨迹。图 11(c)和图 11(d)分别为在编队控制算法(34)作用下Follower无人机UAV 1和UAV 2的实际轨迹。

图 12(a)和图 12(b)分别为每个时刻Leader无人机与UAV 1无人机在x方向与y方向上位置变化，由图 12(a)可以看出随着时间的推移，在x方向上UAV 1无人机与Leader无人机之间总是保持一定的距离，这个距离为所设置的位置差值0.8 m，在图 12(b)中看出，在y方向上UAV 2无人机与Leader无人机之间总是保持-0.8 m。

图 13(a)和图 13(b)分别为Leader无人机与UAV 2无人机在x方向与y方向上位置变化，图 13(a)表示在x方向上UAV 2无人机与Leader无人机之间的位置差值保持为0.8 m，图 13(b)表示在y方向上UAV 2无人机与Leader无人机的位置差值保持为0.8 m。图 14和图 15为实验过程中实时解算出UAV 1无人机的速度v₁和偏航角速度w₂，图 16和图 17为实时解算出UAV 2无人机的速度v₂和偏航角速度w₂。从实验结果来看，此次设计的算法能够实现多无人机编队轨迹跟踪，从速度图像能够看出，速度值一直围绕零波动，从位置图像看实现了编队队形，误差保持一致，但是存在误差，主要存在以下原因：滑模控制器的抖振缺点，数据处理，数据传输过程中存在误差，以及在测试过程中存在外界干扰(风力影响)。

4 结论

本文利用滑模控制方法实现了多无人机协同编队，主要贡献部分：

1) 通过建立四旋翼无人机模型，利用积分反步法设计了Leader无人机的轨迹跟踪算法。

2) 通过Leader-Follower策略设计了多无人机协同编队控制律，使得多无人机能够以期望编队运行。

3) 利用MATLAB仿真软件验证了所设计控制律的可行性，从仿真效果来看，所设计控制律能够实现编队要求。

4) 通过实验方法验证算法的实际可行性，根据实验结果来看，无人机能够实现编队，并且能够跟踪上参考轨迹。