一致性算法和领航跟随法结合的多机编队控制*

刘志江 宋 友 李国宾 屈 瑞 邓 丽

1. 北京航空航天大学软件学院，北京 100191 2. 中国科学院国家空间科学中心复杂航天系统电子信息技术重点实验室，北京 100190

无人机编队飞行可以使得多台无人机协作完成更复杂多样的飞行任务，在军事领域、民用领域、科研领域等各个领域都有广泛应用[1-2]，成为了近些年的热门研究领域[3]。无人机编队飞行控制技术主要有基于领航跟随法的编队飞行控制方法、基于行为的编队飞行控制方法、基于虚拟结构的编队飞行控制方法、基于一致性算法的编队飞行控制方法。

Radaideh等在有环境风影响的条件下对领航跟随法进行了优化[4]。Hao等基于组的层次结构实现了大规模无人机编队控制[5]。Chen等使用downwash模型研究了无人机编队中无人机产生的气流对相邻无人机的影响[6]。Balch等最早提出了基于行为的编队飞行控制算法[7]。Sun等通过引入虚拟电子为多智能体分布式集群控制提供了一个通用框架[8]。刘金龙等基于自适应通信拓扑结构，设计了一种防碰撞的分布式协同控制器[9]。张令等通过模仿寒鸦配对飞行行为机制，减小了无人机编队飞行控制的通信负载[10]。王平等提出了一种基于混合算法的多无人机航迹规划方法[11]。Huang等提出了一种基于并行触发机制的控制策略，以解决存在障碍物和受限条件下的飞行稳定性问题[12]。Askari等基于逆动力学提出了一种新的基于虚拟结构的编队控制方法，适用于编队中无人机数量较多的情况[13]。任伟等在2007年首次将一致性理论引入到多智能体编队控制系统中[14]。田静等基于一致性理论，提高了编队系统通信能力的鲁棒性[15]。董朝阳等针对时变编队问题提出了抗干扰编队控制[16]。

但是，上述编队飞行控制方法都不能同时兼顾好多无人机在执行编队飞行任务时在时间、空间、姿态多维度的协同配合。多机协同探测的编队飞行控制中，风力等级、电磁环境和网络延迟等因素都会对无人机编队飞行过程中时间、空间、姿态的协同配合带来影响。

本文采用一致性算法和领航跟随法相结合的无人机编队控制方法。通过多无人机开展飞行实验，得到并分析无人机飞行数据，验证了采用一致性算法和领航跟随法相结合的无人机编队控制方法控制无人机执行编队飞行任务时在时间、空间、姿态的协同性上都满足协作要求。使无人机在执行多机协同探测编队飞行任务时对准角度误差小于通信天线的通信波束角，为使用无人机模拟卫星来测试星载有效载荷设备提供了支持。

1.1 编队队形和协作要求

为了测试星载有效载荷设备的性能，使用无人机模拟卫星，携带星载有效载荷设备执行编队飞行任务。无人机从机携带的有效载荷设备对地面进行探测的同时，与无人机主机携带的有效载荷设备进行通信并将探测数据实时传输给主机携带的有效载荷设备。有效载荷设备上的窄波束通信天线，具备高速率的数据传输能力，但天线波束较窄，对通信天线对准提出了高的要求。通信天线搭载在无人机上，因此进一步对无人机在执行编队飞行任务过程中时间、空间、姿态上的协同配合提出了高的要求。

在空间维度，多台无人机距离地面的高度相同，多机协同探测时以相同的角速度绕同一圆心以不同半径进行圆形绕飞。在姿态维度，无人机的机头方向始终指向主机的机尾，对准角度误差不能超过无人机携带的窄波束通信天线的通信波束角。在时间维度，无人机到达协同探测开始点后，同时开始探测和无人机间的通信传输工作。图1为无人机编队进行圆形绕飞的队形设计图。无人机A为主机，无人机B、C为从机。从机机头方向实时对准主机的机尾，将探测数据传输至主机携带的有效载荷设备。无人机B、C的机头方向和无人机A的机尾的对准角度误差需要小于有效载荷设备的通信波束角(6°)。无人机的运动轨迹为：

图1 队形设计图

(1)

式中，x,y,z是无人机在惯性坐标系下的坐标，ri是无人机的绕飞半径，h是无人机的绕飞高度，α是无人机绕飞角速度，t是时间。

为实现无人机编队飞行控制，本文采用了一致性算法和领航跟随法相结合的无人机编队控制方法。具体流程如图2所示，以无人机A为示例，飞行初始调整时，无人机飞行路径为A0→A1→A2→A3。首先从地面出发点A0垂直上升至A1点(20 m高度)。而后，飞向A2点，在A2点旋转调姿，使得无人机机头指向绕飞圆心；
而后保持该姿态指向到达协同探测的开始点A3。无人机姿态变化如图2中俯视图所示。以此类推，无人机B、C同时完成飞行初始调整。

图2 编队队形组织策略流程图

多机协同探测时，所有无人机收到执行任务的控制指令后，以相同的绕飞圆心为中心进行圆形绕飞。绕飞时，无人机A是领航机，无人机B和C是跟随机。在保持绕飞高度、半径不变的同时，跟随机B和C始终保持在绕飞圆心O和领航机A组成的以O为顶点的射线上。本阶段任务要求无人机B和C窄波束通信天线(机头方向安装)实时指向无人机A携带的窄波束通信天线(机尾方向安装)，在探测的同时将数据实时传输至无人机A携带的有效载荷设备，即对准角度误差小于通信波束角，需要无人机编队在时间、空间、姿态上的协同配合。

任务完成返航时，无人机接收控制系统的返航指令，按照A3→A1→A0的飞行路径安全返航。

1.2 无人机运动学模型

假设p∈R3是无人机在惯性坐标系下的位置，v∈R3是无人机在惯性坐标系下的速度，φ,θ,ψ是无人机在机体坐标系下的欧拉角。R∈R3×3是机体坐标系转换到惯性坐标系的旋转矩阵

(2)

式中，s表示正弦函数sin(·)，c表示余弦函数cos(·)。无人机的运动学模型为：

(3)

式中，e3=(0 0 1)T，m是无人机的质量，ω=(ω1ω2ω3)T∈R3是以无人机机体为参考的机体坐标系下姿态变化角速度，g是重力加速度，f是无人机的旋翼产生的升力的和，S(ω)是ω的反对称矩阵。

(4)

1.3 控制器设计

设编队中无人机数量为N。针对式(3)所述的无人机运动学模型，设计控制输入μ(t)=[μ1(t)μ2(t) …μN(t)]T，μi(t)是无人机i的控制输入，i=1,2,…,N。一致性算法公式为：

(5)

对于N架无人机组成的编队，使用G表示其通信拓扑结构，使用L∈RN×N表示G的拉普拉斯矩阵。lij是L的元素。

(6)

关于参数γ，当-L的所有N-1个非零特征值均为负实部时

(7)

否则

(8)

基于一致性算法和领航跟随法相结合的无人机编队控制方法研发的无人机协同编队飞行控制系统由电脑总控端和手持端两部分组成，电脑总控端和手持端通过无线网络通信，手持端通过无线电波直接控制与其连接的无人机。手持端的主要功能是向电脑总控端转发无人机的控制指令，接收无人机状态数据并向总控端转发信息。总控端对所有无人机进行编队控制。涉及的指令和数据交互过程如下。

指令交互过程包括9个环节、17次指令交互，如图3所示。全部9个环节分别是：1)总控端上传编队飞行任务参数配置文件。2)总控端→手持端→无人机，发送编队飞行任务参数配置，内容包括探测起始点(经度、纬度、高度)、绕飞圆心(经度、纬度)、绕飞半径、绕飞角速度、绕飞圈数等。3)无人机→手持端→总控端，反馈编队飞行任务参数配置成功信息。4)总控端→手持端→无人机，若所有无人机任务参数均配置成功，发送“开始执行”指令。若出现部分无人机状态反馈异常，可选择对该无人机重新发送编队飞行任务参数配置，或选择部分无人机继续执行编队飞行任务，或停止任务。5)无人机→手持端→总控端，各无人机到达协同探测的开始点后，发送“到达协同探测起始点”指令。总控端等待所有无人机均已到达协同起始点，任务就绪。6)总控端→手持端→无人机，发送“执行任务”指令，无人机开始执行编队飞行任务，并实时将其状态数据反馈给总控端。7)无人机→手持端→总控端，各无人机完成编队飞行任务后，发送“任务完成”指令至总控端。8)总控端→手持端→无人机，总控端待收到“任务完成”指令后，发送“返航”指令到各无人机。若无人机在飞行过程中经状态数据判断，出现异常，总控端也可以随时向其发送“结束任务，返航”指令。9)无人机→手持端→总控端，无人机成功返航至指定点后，发送无人机“已返航”指令至总控端，编队飞行任务结束。

图3 指令及数据交互过程图

数据交互过程主要为无人机传输至手持端的无人机状态数据，和手持端转发至主控端的无人机状态数据。无人机状态数据具体包括无人机的时间、位置(经度、纬度、高度)、姿态(俯仰角、偏航角、横滚角)、速度、飞行模式、任务执行状态。多台无人机的状态数据均通过无线电波传输至手持端，再由手持端通过无线网络传输至总控端，在总控端实时显示各无人机状态数据，绘制无人机实时飞行轨迹3D图和2D图，并保存任务数据。

上述实施过程，采用一致性算法和领航跟随法相结合的无人机编队控制方法，为多无人机执行编队飞行任务时在时间、空间、姿态多维度协同配合提供了保障。

3.1 实验设置

在完成仿真实验后，进行了外场飞行实验。实验使用3台如图4所示的大疆M600 Pro无人机，分别携带探测载荷低频干涉成像谱仪，进行编队飞行，完成干涉成像实验。编队需求：3台无人机绕圆心飞行，保持无人机A-B-C实时共线，且A-B、A-C的对准角度误差小于有效载荷设备的通信波束角(6°)。

图4 大疆M600 Pro

任务参数配置：绕飞圆心(经度116.8990753°，纬度40.3543671°，高度100 m)，无人机起飞和返航速度为2 m/s，无人机A绕飞半径为30 m，无人机B绕飞半径为60 m，无人机C绕飞半径为100 m，各无人机绕飞角速度均为2(°)/s，绕飞圈数为1圈，实验当天风力等级在2～4级之间波动。从地面拍摄的飞行实验实景图如图5所示。

图5 飞行实验实景图

3.2 实验结果分析

根据无人机飞行数据绘制的无人机飞行轨迹3D图像(包含飞行初始调整、多机协同探测两个子行为)如图6所示。3台无人机垂直上升20 m；
飞向预备点；
调整姿态并飞向协同探测开始点；
执行多机协同探测任务。从该轨迹图可以看出，三台无人机按照预期的飞行轨迹完成了编队飞行任务。

图6 无人机编队飞行轨迹图

飞行数据分析结果如图7～10所示。俯仰角误差图如图7所示，俯仰角误差在0°到1.1°范围内波动。俯仰角误差对有效载荷设备对准角度误差的影响较小。横滚角误差如图8所示，无人机在绕飞过程中机头方向指向绕飞圆心，即无人机一直在横向飞行，因此在绕飞起始时和绕飞结束时横滚角误差波动较大。无人机B的绕飞半径比无人机C的绕飞半径小，因此大部分时候无人机A/B的横滚角误差比无人机A/C的横滚角误差小。横滚角误差对有效载荷设备对准角度的影响几乎可以忽略不记。偏航角误差如图9所示，偏航角误差在0°到3°范围内波动。造成无人机偏航角误差的原因主要有绕飞起始时间不同步和环境风的影响。偏航角误差对有效载荷设备对准角度误差的影响较大。

图7 俯仰角误差

图8 横滚角误差

图9 偏航角误差

无人机对准角度误差如图10所示，在0°到4°范围内波动，始终小于有效载荷设备的通信波束角(6°)，满足有效载荷设备对准角度的精度要求。

图10 对准角度误差

结果表明，实验中，在网络时延、2～4级环境风力等因素影响下，一致性算法和领航跟随法相结合的无人机编队控制方法有效保障了多机协同探测的编队飞行任务的实施。

无人机协同编队飞行可以使得多台无人机协作完成更复杂多样的飞行和探测任务，尤其是分布式合成孔径等机载应用。本文采用一致性算法和领航跟随法相结合的无人机编队控制方法，使多无人机执行编队飞行任务时在时间、空间、姿态多维度协同配合。后续，将继续提升无人机闭环控制能力。