基于PX4飞控的多旋翼无人机编队跟踪系统设计

姚洪斌，张 民，胡淦敏

(南京航空航天大学自动化学院，南京 211100)

0 引言

近年来，无人机编队飞行已经成为控制领域的一个硏究热点。无人机编队飞行是指2架或者2架以上的无人机，接到任务指令后，可以根据当时任务阶段自主进行队形的集结、变化、保持、重构等功能变化，同时也包括飞行任务的规划和分配，整个无人机群在执行任务过程中体现出高度协同性和一致性。但在实际工程应用中发现，单架无人机会因为续航能力有限导致任务效率较低，严重制约了其应用场景和性能优势。通过多无人机的相互协作，无人机群飞行可以有效地延伸无人机的任务执行范围，提升执行任务的完成度和成功率，综合性能与单机相比有了极大的提高，应用范围和任务场景也不断拓展。国内目前对无人机编队飞行的研究基本上聚焦在协同控制方法的研究和实验室仿真阶段。国外E.Frew等利用李雅普诺夫矢量场并基于视线角的制导策略，实现多无人机协同跟踪目标，最终使无人机稳定收敛在极限环上做周期运动。国内杨祖强等建立tau制导矢量场，设计了基于standoff的多机协同跟踪算法，利用相位调整并结合人工势场法避碰，以达到多机协同跟踪目的。另外，固定翼飞机虽然具有高速巡航能力，但是对于起降跑道和平飞高度要求较高。多旋翼无人机具有垂直起降、空中悬停、低空低速飞行等独特飞行技能，且具有很强的灵活性，被广泛应用于地图航拍、农业喷洒、侦查测绘等领域。

本文提出了一种基于PX4飞控的通过长机领航的四旋翼无人机编队系统，将基于距离变化率的协同定距跟踪制导律成功应用于多旋翼无人机平台，设计的协同制导律公式简洁，所需要的传感器参数信息大大减少，且整个系统的复杂度不随飞行器数量的增加而增加，最终可以使多架无人机自主形成预设编队并保持编队，能够对地面目标进行跟踪，从而提高任务效率，最大化发挥多旋翼无人机的性能。同时，应用机器人操作系统(Robot Operating System，ROS)搭建编队软件系统，实现了多无人机之间的信息交互，最后在Gazebo仿真环境中进行测试，并分析了无人机群的飞行数据，从而验证了本系统的稳定性和可行性。

1 问题描述

1.1 制导策略与任务需求

无人机协同跟踪是无人机编队在探测范围内发现地面目标后，编队成员之间可通过链路通信获得相邻无人机的状态信息和地面目标的信息，进而对地面目标进行有效的协同跟踪。无人机飞行控制系统由内回路和外回路构成。本文直接利用PX4开源飞控系统底层架构作为内回路控制无人机姿态，同时基于设计的编队制导算法进行位置控制和速度控制作为外回路的输入，以响应外回路给出的制导指令。目标几乎以恒定的速度在相当平坦的地面上移动,并且在默认条件下无人机的速度高于目标速度。理想情况下，所有无人机在执行跟踪任务过程中，都会保持在同一水平高度上，所以这里仅考虑固定高度上的二维制导问题。制导策略如图1所示。

图1 制导策略示意图Fig.1 Schematic diagram of guidance strategy

任务目标可理解为地面目标与无人机同时处于同一个二维平面上，多架无人机在跟踪制导律作用下逐渐收敛到构成以目标为圆心、半径为的圆编队进行稳定跟踪。在跟踪编队初步成形后，协同制导律开始作用，最终架无人机平均分布于轨迹圆上。

1.2 无人机建模

建立无人机模型的系统状态空间，其描述形式为

(1)

=[,,,,,,,,,,,]

其中，为状态向量；后面各项依次为滚转角、俯仰角、偏航角、滚转角速度、俯仰角速度、偏航角速度、机体坐标系3个坐标轴的空速分量、横向位移、侧向位移、高度；作为控制向量，且作为执行机构的操纵量，经过底层飞控的控制器，最后都通过电机产生的力矩作为输出。在制导问题研究中，将无人机状态向量简化为=[,,,,,]，其运动模型为

(2)

(3)

2 编队算法

当跟踪地面目标时，无人机的飞行状态分为顺时针飞行和逆时针飞行。在本文中，为了便于分析，采用前者的飞行方式。本文使用如式(4)所示的无人机跟踪地面目标制导律

(4)

由图2中的无人机间运动关系，得方程

(5)

(6)

其中，2架相邻无人机与-1间距离用,-1(±在范围内，∈)表示，相邻2架无人机轨迹所对圆心角用,-1表示，由几何关系可得,-1=,-1=--1。

编队的协同制导律形式为

图2 相邻无人机运动关系示意图Fig.2 Schematic diagram of motion relationship between adjacent UAVs

=

(7)

控制结构框图如图3所示，以跟随无人机为例，跟随无人机自身产生的控制指令经过处理，在无人机编队系统中输入的分别是各个跟随无人机实时采集到的位置信息和偏航角信息，经过飞控底层得到的领航无人机信息作为编队协同律的输入，两个控制律的输出融合后共同发送给飞控，输出为底层飞控姿态控制器计算的控制量。

图3 编队控制结构框图Fig.3 Block diagram of formation control structure

3 编队系统设计

整体编队实现采用图4所示的分层结构表示。该结构总共4层，分别为进行外部控制的应用层、负责通信的通信层、融合数据的飞控层和各传感器模型的接口层。

图4 编队系统层次结构Fig.4 Hierarchy of formation system

应用层通过规划整体编队队形，使机群中的每个成员达到期望的位置和速度，并通过上文中的编队算法计算出每架无人机在其机体坐标系下的期望姿态。通信层可以利用系统程序设计基于开源ROS的分布式框架，为无人机提供通信服务。首先组建一个局域网，将应用层计算出的期望速度、滚转角、俯仰角等信息以及各无人机成员的自身状态信息作为话题节点，在飞控层与应用层之间传递。飞控层利用开源的PX4飞控负责无人机的信息融合，飞行控制栈是导航、制导、控制算法的集合，可以提供姿态解算、控制量分配、电机舵机控制等，使无人机在稳定飞行的同时速度可以快速收敛到期望的值，保证无人机飞行的姿态稳定且提供无人机速度和高度保持，从而控制无人机达到期望的跟踪队形。接口层主要是由无人机模型和各传感器模型构成。4个层次共同组成了无人机编队控制系统。本文主要在通信层和应用层搭建Gazebo仿真环境，利用ROS结合c++进行程序设计。

GAZEBO是一款能够提供高保真度的物理模拟的图形界面(见图5)，并且可以自定义增加场景，即使在复杂的环境下，仍然能够准确地模拟出机器人的行为，是一款对用户和程序都非常友好的物理模型仿真软件。本文系统使用这款软件对现实场景进行还原，并在其中进行无人机编队算法的验证。在GAZEBO仿真环境中，通过配置四旋翼无人机尺寸、质量和相关传感器、控制器等模型参数，得到基于PX4的iris四旋翼仿真模型，并使其与实际无人机基本保持一致，对其编号以模拟多架无人机在实际飞行环境中的效果。其次，修改仿真时运行的 launch 文件，通过标签的方式添加无人机的属性，改变无人机的个数、起始位置等，每个无人机都具有各自的用户数据报协议(User Datagram Protocol，UDP)端口，并且与其mavros 包的UDP通信端口一一对应，设置好后即可结合其他ROS包中的launch文件进行编队控制实验。启动后，可以在仿真环境中观察到多架无人机形成队形且保持队形进行预期活动。

图5 GAZEBO仿真界面Fig.5 GZEBO simulation interface

ROS在此无人机编队系统中，可作为无人机起飞、切换模式等命令的接收者，也可以作为经过内部相关程序包计算后的数据处理者以及制导信息的发布者。通过微型空中飞行器链路通信协议(Micro Air Vehicle Link，MAVLink)和其可扩展通信节点mavros，在编队中的无人机会向服务器发布自己的相关状态信息，同时，若其想要接收相邻无人机的相关状态，可以通过订阅话题的方式进行获取，与其建立通信关系，即可达成目的。内部 ROS 接收来自发布节点以及各个传感器的信息，计算无人机的制导信息，并将此信息传递给飞控，进而控制无人机飞行姿态，其中涉及的订阅/发布的ROS话题如表1所示。同时，设计了能够观察各机详细数据和进行指令切换的小型地面站，如图6所示。

表1 订阅/发布的ROS话题

图6 小型地面站Fig.6 Small ground station

4 飞行仿真验证

通过仿真实验，无人机的跟踪轨迹如图7所示。

图7 无人机协同跟踪目标轨迹Fig.7 UAV cooperative tracking target trajectory

4架无人机最终能通过外部指令切换飞行模式在各自位置起飞，每架无人机起飞的水平间隔为8m，起飞后先平飞，之后搜索到目标后进入跟踪阶段，在5m的高度对目标进行跟踪，跟踪过程中4架无人机在跟踪圆轨迹中保持均匀分布，飞行过程仿真曲线如图8所示。

(a) 无人机对目标距离

(b) 无人机高度变化

(c) 滚转角变化曲线

(d) 俯仰角变化曲线

(e) 滚转角速率变化

(f) 俯仰角速率变化

(g) 无人机速度变化

(h) 无人机之间距离变化

观察过程仿真曲线结果可知：无人机能够根据制导律给出的指令很好地跟踪地面移动目标，对目标距离稳定在10m，无人机在跟踪过程中在2.5～2.7m/s的速度范围内变化，高度偏差都在±0.1m以内，跟踪过程保证了4架无人机之间的距离保持在15m左右；同时，姿态也能根据指令及时调整，俯仰角和滚转角都能达到良好的控制效果，验证了本文无人机编队系统可以有效且稳定地跟踪目标。

5 结论

本文采用开源的PX4 四旋翼无人机飞控，围绕编队协同跟踪问题开展工作，主要结论为：

1)利用领航者-跟随者编队方式与ROS分布式模型的优点，设计了编队协同控制系统，ROS系统将控制底层与上层应用相互分离，隐藏各自细节，具有良好的互操作性。

2)将基于距离变化率的制导律结合PX4飞控的位置和速度控制，在GAZEBO环境中搭建多架四旋翼实际模型，对编队控制系统的效果及功能进行编队仿真验证。

3)仿真结果表明，本文提出的编队跟踪策略和算法可以有效地跟踪地面目标，具有工程应用价值。

4)同时本系统也具有良好的拓展性，在后续实验中可以通过对无人机和机载设备选型，进而对本文提出的编队算法进行实物验证。