四旋翼无人机的编队飞行控制系统设计

徐 俊，王茂森，戴劲松，王大鹏

（南京理工大学 机械工程学院，江苏 南京 210094）

1 引言

近年来无人机（UAV）编队飞行已经成为智能控制领域的一个研究热点。无人机编队飞行，即多架无人机进行某种队形排列组织模式和任务分配的组织模式以适应任务的要求，它既包括队形的产生，保持和变化，也包括飞行任务的规划和组织[1]。多UAV编队飞行可以弥补单个无人机执行任务效率低，安全冗余差等缺点，可以执行通讯中继、高精度定位对地攻击、多角度3维立体成像等复杂任务[2]。通过多无人机的协作，无人机组群飞行有效的延伸无人机的任务执行范围，增强了任务的执行效能和冗余保障，其综合性能与单机相比成几何级的增强，应用范围和任务使命都有了拓展。

国内目前对无人机编队飞行的研究基本上聚集在协同控制方法的研究和实验室仿真阶段。例如文献[3]对多飞行平台分散化协同控制系统结构以及分散化的规划等技术问题进行了深入研究。文献[4]提出了一种分布式编队控制器的分散化设计方法，并进行了仿真实验。在提出了一种基于GPS坐标控制的编队算法并对算法进行了实验验证。搭建了稳定的多节点无线通讯链路，解决了数据冲撞以及干扰问题。

最后对无人机群规划的路径和实际飞行路径进行了比较以及对各部分硬软件进行测试，从而验证本系统的精确性和稳定性和可行性。

2 编队飞行控制系统总体设计

编队飞行控制系统设计主要分为一下几个部分：一是无人机端的飞行控制系统设计，二是无线通讯链路的搭建。三是编队算法的实现。

2.1 控制系统任务规划

地面站主要负责整个系统的统筹管理，包括队形目标函数的产生，各个目标航点的计算，对各架无人机的状态包括姿态、高度、速度、位置等的监测。

2.2 控制系统的总体结构

让编队模块任务在电脑端的地面站处理之后，无人机端的任务负担得到减小，可以让更多的资源分配给姿态检测、控制、导航、通讯等更紧急的任务。从而保证这些任务执行的实时性。

系统的整体结构，如图1所示。系统由多架无人机、地面站组成，系统之间的通信由ZigBee实现各个节点之间的组网，通讯协议采用MAVLink协议。地面站根据编队需要的阵形，计算出在当前位置下各个无人机的GPS坐标和高度，通过无线发送给各架无人机，无人机接收到自身的目标航点并飞向该航点。同时，每架无人机广播自身的GPS坐标，并接受其他无人机的GPS坐标，进行防撞检测。

图1 控制系统结构Fig.1 Control System Structure

由于地面站对整个系统的无人机进行监管和调度，其职能类似于空管，能够全局调度各架无人机，从而保证了编队系统的安全性和灵活性。

3 单架无人机的系统设计

微型四旋翼是一个具有六自由度和4个控制输入的欠阻尼系统，具有多变量、非线性、强耦合和干扰敏感的特性[5]。为了实现编队指令的正确执行，无人机端的任务非常多，要同时进行大量的计算，比如传感器数据采集和处理、姿态计算、姿态控制、导航、通讯、指令解析等。

3.1 无人机的硬件设计

为了保证单机系统的可靠性，无人机端的传感器都选择了不同型号的两种同样同样功能的传感器进行数据冗余，增强了系统的抗干扰性和鲁棒性。

同时为了提高无人机执行任务的实时性，无人机控制系统采用两颗处理器芯片，主控制器和协处理器之间通过串口进行数据通讯。协处理器主要负责遥控器信号的接收解析和对电机的PWM输出控制。系统大部分传感器通过SPI总线和主控制器连接，保证了底层数据传输的实时性。单机系统的硬件框架图，如图2所示。

图2 单机系统硬件框图Fig.2 UAV Control System Hardware Structure

3.2 单架无人机的软件设计

传统的无人机系统程序采用while（1）循环或者中断的执行方式，这种方式往往实时性弱、可维护性差。为了大运算量、多任务的有序执行，系统采用了NuttX操作系统，能为多旋翼飞行器控制系统的开发提供极大的方便。

无人机端所有的任务通过任务进程实现。每个进程设定一个调度的时间间隔，该进程则按照这个时间间隔运行，同时给每个进程设定最大运行时间，若本次进程执行时间预计超过该时间则放弃该次运行。

程序框架图，如图3所示。每个进程有两个执行控制量，如（2.5，180）表示该进程没2.5ms调度一次，当该进程执行的预估时间大于180ms时则放弃该次执行，释放占有的CPU资源。

图3 程序执行流程图Fig.3 Program Execution Flow Chart

4 编队系统的通讯链路设计

无人机编队系统的通讯是链接各架无人机和地面站之间的桥梁，各架无人机自身坐标点的发送、地面站指令的发送都需要一个稳定实现全双工的无线通讯链路。

4.1 通讯链路的硬件选择

针对编队系统多对多通讯的特点，选用了具有自组网功能的ZigBee模块。同时ZigBee模块在2.4GHz工作频率下能提供250kbps的数据吞吐率，远高于实际编队系统中的数据吞吐率，因此ZigBee模块能够满足编队系统的无线数据传输要求。

4.2 编队系统的通讯协议设计

通讯链路使用MAVLink（Micro Air Vehicle Link）通讯协议，MAVLink是一种只包含头文件信息调度库的轻量通讯协议，主要用于地面站（GCS）和微型无人运载工具间的通信[6]。

为了使MAVLink适应编队系统，对MAVLink的协议进行了定制，将一号机的MAVLink消息帧起始位标志设置为0xFE，二号机的MAVLink消息帧起始位修改为0xFD，将三号机的MAVLink消息帧起始位标志修改为0xFC。这样在编队系统中每架无人机可以在多个消息帧中识别出自己的消息。

4.3 通讯链路的防冲撞处理

在把每架无人机的MAVLink消息帧的ID区分开之后理论上各个无人机都能在众多的消息中识别出发给自己的消息，但多架无人机一起发送的时候，会发生数据冲撞，接受端会收到乱码。因此同一时刻只能由一个发送设备占用信道，系统采用了类似于令牌环形网络的数据通讯机制。

以三架无人机为例说明通讯链路的传输流程：将地面站作为参与环形网络的一员，网络的发送按照约定的顺序发送消息，一号机先发送消息，在消息的中增加自身的发送OK状态标志位，一号机发送完毕之后网络中的所有设备都接收到一号机的消息，获取了一号机的状态信息。每架无人机将消息发送OK的标志位和约定的发送时序进行比对，发现是自身约定的时序时则开始发送否则不发送。此时二号机接收到消息发现是约定的时序，二号机开始发送消息，并将消息中的OK状态标志位修改为UAV2。接线来是三号机发送，这样依次发送构成一个环形网络，且保证了每次发送的时候只有一个设备占用信道。

为了提高环形网络的鲁棒性，每个设备通讯程序中引入了一个发送周期计时模块，每次发送完毕计时清零并开始下一次计时，当在设定的计时时间内没有接收到某架无人机消息是则从最后一次接受到消息的OK标志位按照约定的顺序向后递推2个设备，此时该设备发送消息，并且该设备之后在循环中将监听的设备改为最后一次接收到的设备，这样就保证通讯系统不会因为某个设备的损坏而通信中断。

5 队形编队算法设计

考虑到地面站能对所有的无人机进行统一调度并且笔记本电脑的强大的计算能力和方便的开发环境，因此本系统将编队算法实现放在地面站软件中。

5.1 经纬度坐标和距离之间的转换

无人机端获取位置的方式是依靠GPS模块。在实际位置估算中结合加速度计判断自身状态，能以20cm的精度稳定自身位置。高精度的GPS为接下来的控制算法提供了基础[7]。

在编队算法的实现过程中，运算的单位是米，即阵形是以米为单位控制的，因此需要把计算结果的米的单位转化成GPS坐标发送给无人机执行。

已知当前位置经纬度为lon1、lat1，目标航点经纬度为lon2、lat2，将经度差dlon转换成经度方向的距离差dLon转换公式如下

将纬度差dLat转换成纬度方向的距离差dLat需要有一个换算，因为每个纬度对应的纬线的圆周长是不一样的，假如当前的纬度值为lat，则换算公式为：

计算出在本地（118．852108E，32．026415W）一米的长度在经度方向上近似等于0．000010°，在纬度方向上一米近似等于0．000009°。

5.2 自由空间的编队飞行算法

自由空间即没有约束条件的三维空间，无人机在此空间可以自由飞行。下面说明算法坐标系到无人机实际运行的GPS坐标点之间的转化。

算法坐标系中的坐标可以看成是相对于起始点的距离偏移量，因此转化到经纬度坐标系时只要知道起始点的经纬度，再在起始点经纬度叠加上相对距离转化成经纬度的偏移量就得到了该点在经纬度坐标系中的GPS坐标。可以得到无人机的经度坐标值Lon和其纬度坐标值Lat。

式（4）为经纬度值和算法求出的坐标值之间的转化关系公式：

从算法坐标系我们通过式（5）可以求出无人机的经纬度值，按照0．5m为单位可以得到一系列的沿着轨迹的航点，将这些航点依次发送给一架无人机，无人机沿着这些航点飞行便使得该架无人机按照我们设定的轨迹飞行。

控制了一架无人机之后还需要控制其余的无人机，当多架无人机按照某一个阵形飞行时，各架无人机之间的相对位置可以看着固定不变的[8]。

例如当无人机编队按照等腰三角形阵形飞行，一号机的轨迹确定，二号机相对于1号机的位置差△X1和△Y是保持不变，在此基础上叠加上位移差得到二号机的GPS坐标和三号机的GPS坐标，这样得到阵形中各无人机的坐标。

假设在算法坐标系中，主机（一号机）的飞行轨迹方程，如式（5）所示。根据式（4）我们可以计算出在GPS坐标系中主机的轨迹方程。

若按三角形编队飞行，三号机相对于一号机往西偏移7m，往南方向偏移7m；二号机相对于一号机往东偏移7m，往南偏移7m。1号机GPS坐标为（118．852108E，32．026415W）。在算法坐标系和GPS坐标系中的理论队形飞行轨迹，如图4所示。

图4 算法坐标系和GPS坐标系编队路径图Fig．4 Algorithm Coordinate System and GPS Coordinate System Formation Path Graph

6 编队飞行系统实验与分析

编队系统以三架四旋翼无人机为测试对象，测试系统的实际飞行的效果。实验内容包括通讯功能测试和编队飞行测试，测试主机的GPS坐标为（118．852108E，32．026415W）。

为了验证编队飞行系统的可靠性，使用三架四旋翼无人机进行编队飞行试验，设置1号机为主机，三号机相对于一号机往西偏移7m；二号机相对于一号机往东偏移7m。二号机和三号机跟随一号机飞行，三架四旋翼无人机组成一字形队并排形飞行。

一号机的飞行的四段轨迹方程依次为式（6）～式（9）所示。

在各架无人机飞行过程中，记录无人机实时位置得到实际的航线。实验现场拍摄图片，如图5所示。实际飞行曲线和理论轨迹曲线的对比，如图6所示。

图5 一字型编队飞行Fig.5 Linear Formation Flight

图6 理论路径和实际路径对比图Fig.6 Comparison of Theoretical and Practical Paths

可以发现实际的飞行轨迹能够较好的沿着理论曲线飞行，说明了编队算法的正确性和可行性。

7 总结

设计四旋翼无人机的编队飞行控制系统，包括单机飞控系统、多节点通讯链路、编队算法，并对各部分进行了测试，得到了令人满意的运行效果，验证其可行性。同时单机采用的NuttX操作系统在保证实时性的同时也为以后的功能扩展提供了良好的平台。实现三架无人机编队飞行为以后十架甚至几十架无人机编队提供了扎实的基础。

［1］樊琼剑，杨忠，方挺.多无人机协同编队飞行控制的研究现状［J］.航空学报，2009，30（4）：683-691.（Fan Qiong-jian，Yang Zhong，Fang Ting.Research status of coordinated formation flight control for multi-UAVs［J］.Aeronautical Journal，2009，30（4）：683-69.）

［2］洪晔，缪存孝，雷旭升.基于长机—僚机模式的无人机编队方法及飞行实验研究［J］.机器人，2010，32（4）：505-509.（Hong Ye，Miu Chun-xiao，Lei Xu-shen.Study and test based on UAV formation method of the master-slave modle ［J］.Robot，2010，32（4）：505-509.）

［3］周锐，吴雯漫，罗广文．自主多无人机的分散化协同控制［J］．航空学报，2008，29（B05）：26-32.（Zhou Rui，Wu Wen-man，Luo Guang-wun.The decentralized coordination control of multiple autonomous UAVs［J］.Aeronautical Journal，2008，29（B05）：26-32.）

［4］何真，陆宇平．无人机编队队形保持控制器的分散设计方法［J］．航空学报，2008，29（B05）：55-60.（He Zhen，Lu Yu-ping.Design method of decentralized controller for formation keeping machine ［J］.Aeronautical Journal，2008，29（B05）：55-60.）

［5］聂博文，马宏绪，王剑.微小型四旋翼飞行器的研究现状与关键技术［J］.电光与控制，2007，14（6）：113-117.（Nie Bo-wen，Ma Hong-xu，Wang Jian.Research status and key technology of micro four rotor aircraft［J］.Electronics Optics&Control，2007，14（6）：113-117）

［6］孟颖，张贵阳，魏晓马.基于APM飞控制作的MAVLink通信协议分析［J］.电子世界，2014（23）：101-103.（Meng Ying，Zhang Gui-yang，Wei Xiao-ma.Analysis of MAVLink communication protocol based on the production of APM flight control［J］.Electronic World，2014（23）：101-103.）

［7］Park，Chulwoo；Kim，Hyoun Jin；Kim，Youdan.Real-time leader-follower UAV formation flight based on modified nonlinear guidence［J］.29th Congress of the International Council of the Aeronautical Sciences，ICAS 2014，9（7）.

［8］高玉东，郗晓宁，王威.GPS技术用于编队卫星状态整体确定方案设计［J］.宇航学报，2006，27（2）：205-209.（Gao Yu-dong，Xi Xiao-bo，Wang Wei.Design of GPS technology for formation satellite state integral determination［J］.Aeronautical Journal，2006，27（2）：205-209.）