基于ROS 和PX4 的无人机编队协同飞行模型的仿真研究

路梦源，王天一，2*，陈新昌，张宇卓，宫泽奇，张星山

（1. 中国农业大学工学院，北京市，100083； 2. 中国农业大学农业无人机系统研究院，北京市，100193；3. 河南农业大学机电工程学院，河南郑州，450053； 4. 中国科学院空天信息创新研究院，北京市，100094）

0 引言

近年来，无人机应用价值日益凸显，其研究潜力也被广泛认可［1-2］。无人机编队概念的提出及其相关技术的发展不仅有效解决了单体无人机信息感知处理能力弱的不足，更是将无人机的应用领域进一步扩大［3-4］。常用的无人机开发研究工具有PIXHAWK（PX4）与Gazebo；技术有即时定位与地图构建（simultaneous localization and mapping， SLAM）。作为领域内常用的开源飞控软件，PX4 具有高性能和低成本的特点；而Gazebo 是用于机器人仿真的开源三维动力学仿真软件，以Ubuntu 操作系统为平台，集成在ROS 开发环境中；SLAM 作为真正实现全自主移动机器人的关键技术，也被引入到无人机编队的飞行导航中，基于激光雷达的Hector SLAM 算法，凭借着在动态小场景下具有稳定的测距性能、受光强的影响小、操作简单、鲁棒性强等优点逐渐成为研究热点［5］。LIU等［6］提出一种分布式编队控制算法，让无人机在二维和三维空间中以逐渐稳定的方式确定编队的形状；成成［7］设计一种虚拟长机状态估计算法，编队个体从通信拓扑中获取虚拟长机状态并作为飞行导引，提供速度、航向和高度指令，提升了编队队形的精确度；赵伟伟［8］针对无人机编队在复杂环境中避障时存在一致性差、决策困难等问题，提出共享障碍物信息的无人机编队协同避障算法，并将编队避障算法和智能体强化学习算法相结合，同时提升了避障性能和一致性性能；张迪等［9］针对执行器故障时对无人机编队飞行所产生的影响，提出基于动态规划的最优容错控制率，并利用RBF 神经网络逼近最优性能指标函数，最终实现对无人机编队的高精度飞行控制。

无人化智慧农场作为无人机编队的重要应用场景之一，对无人机编队的作业方式、能力有定制化需求。但使用真实无人机进行编队飞行控制、定点导航算法的调试成本较大，且常见仿真平台的接口适配度不高［10-11］，如JMavSim 平台无法添加传感器、X-Plane 和FlightGear 平台均无法进行深层次的飞行调控。因此，亟需一种通用性较高且机器人操作系统（robot operating system， ROS）接口统一的无人机编队飞行仿真开发环境，以便高效可靠地模拟无人机的农田作业状况，优化无人机的编队控制算法［12-14］。

在此背景下，肖昆［15］团队基于ROS 系统、PX4 飞控和Gazebo 仿真软件通讯模块开发了XTDone 无人机通用开源仿真平台，并内置多种类型的无人机、传感器模型，封装部分控制和状态评估算法。胡新雨等［16］介绍XTDrone 平台的基本概况、体系架构和主要功能，并阐述无人机集群协同搜寻系统的设计细节和部分关键技术。为进一步探究该平台中无人机编队的飞行控制和自主导航仿真试验的可行性，本文首先梳理各软件和功能包在ROS 中的通信方式和交互逻辑，并以此为基础搭建基于XTDone 开源平台的无人机编队飞行控制的仿真环境，建立无人机个体及编队模型。通过MAVROS（micro air vehicle Link ROS interface）协议实现各机与ROS、飞控系统的通信，进而对接收无人机多种位姿信息的关键脚本文件进行结构重置和参数优化，最后通过基于二维激光SLAM 建图的自主导航试验模拟无人化智慧农场中的生产作业任务。

1 仿真环境内部的交互

1.1 ROS 与PX4 飞控之间的交互

在SITL 平台中，MAVROS 节点通过MAVLink协议与PX4 飞控软件实现通信连接，并通过发布、订阅话题或服务的方式将期望的位置、速度和姿态信息传输至PX4 飞控，而后者根据这些信息控制仿真无人机的运动状态，并将实际的状态信息通过/mavros/state话题发布出来，供ROS 系统中的其他节点接收使用，从而实现ROS 与PX4 飞控之间的数据传输和控制命令的传递，如图1 所示。

图1 ROS、PX4 和Gazebo 之间的信息交互Figure 1 Information exchange between ROS， PX4，and Gazebo

1.2 ROS 与Gazebo 之间的交互

由ROS 系统启动并加载Gazebo 仿真环境中的无人机编队和传感器等模型，当PX4 飞控成功连接到MAVROS 节点实现数据传输后，便可通过ROS 的基本通信方式结合Gazebo 提供的ROS API 与仿真环境进行交互：在仿真环境中发布、获取模型的状态信息，以及暂停、恢复仿真环境中的物理运算等。例如：话题通信由“/gazebo_msgs/ModelStates”指令发布所有模型的状态信息，包括位置、速度、角度、角速度等；由“gazebo/get_model_state”指令获取模型状态信息，包括坐标、姿态、线速度、角速度等；服务通信由“gazebo/pause_physics”指令用于暂停仿真环境的物理运算，可以调试和预设场景；由“/gazebo/unpause_physics”指令恢复仿真环境的物理运算［17］。

2 无人机编队飞行试验工作准备

2.1 模型构建

ROS 中已预设多种描述机器人模型的标签，如robot 根标签、link 连杆标签、joint 关节标签、sensor 标签及其他子级标签［18］。在模型建立时，需遵循URDF（unified robot description format）语言格式、XML（extensible markup language）语法完整性、机器人模型规范等要求［19］。分别在多个URDF 文件中使用link 标签于描述无人机的基础坐标系、外壳、前后旋翼、电机、支撑轴及其他传感器，再由joint 标签描述各个组成部分间关节连接的旋转、平移、限位和父子级等关系信息，并通过sensor 标签对IMU（inertial measurement unit）、相机、激光雷达、GNSS（global navigation satellite system）等传感器的名称、类型、位置、方向等信息进行描述。由Gazebo 将集成的URDF 文件解析并生三维无人机模型，并设定蓝色旋翼所在的一侧为无人机的前进正方向，如图2（a）所示，方便对无人机的位姿变化进行实时观察。在验证单一模型能够成功被解析后，使用Xacro 文件用宏的方式来对多个URDF文件及文件中robot 标签元素进行组合调用，以创建无人机编队，如图2（b）所示。

图2 多旋翼无人机个体和编队三维模型Figure 2 Multi-rotary-wing UAV individual and formation 3D-model

2.2 多机通信及飞行控制逻辑

编队中每一个无人机模型均具有独立的MAVROS 节点和PX4 节点。其中MAVROS 节点会提供ROS 接口，用于该节点与无人机的通信和控制；而PX4 SITL 节点则模拟无人机的硬件和软件环境，包括传感器、控制器和飞行控制算法等［18］。

由键盘接收用户的控制指令，并通过预设定的话题节点（例如"/keyboard"）将指令信息传输至编队各无人机的MAVROS 节点，再由控制算法将指令信息分析并转换为对应的飞行姿态控制指令，并依靠MAVLink 协议传递至各自的PX4 SITL 节点，最终无人机各执行机构遵循分配的指令，在Gazebo 中执行相应的动作，实现无人机编队的飞行控制。此外，各机在执行指令后，也需要将自身的飞行位置、速度、姿态角等状态信息反馈发布到ROS 话题中，以供其他节点进行处理和分析。

编队中各机均有独立的数据交互模块，处理无人机飞行时自身位置、目标位置、飞行速度等数据信息，并通过自身的MAVROS 功能包和全局的TF（Transformation）库实现多机飞行控制信息的交互。0号机内部及其与其他无人机整体的的数据交互如图3所示。1～4 号机、5～8 号机内部的数据信息在分析处理后均汇总至0 号机处进行数据交换，以便进行下一环节的指令分配和数据分发。

图3 无人机编队中各机飞行控制信息交互关系Figure 3 Flight control information exchange among drones in a swarm

2.3 通信优化

XTDone 平台中无人机编队的通信连接通过“multi_UAV_communication. sh”编 译 脚 本 和“multi_UAV_communication.py”的执行脚本实现。前者是对生成的无人载具的类型、数量和队列进行保存记录。后者则分别实现机器类型和ID 的获取；建立ROS 订阅者、发布者和服务通信，定义飞行的基础参数，实现飞行参数的获取和输出；通过ROS 主要线程节点构建和发布目标运动信息；由多个定义的回调函数接收不同坐标系和运动类型的控制指令，并通过悬浮状态转换函数判断和切换飞行模式，各模块间位姿信息的传输如图4 所示。

图4 接收、处理模块间的位姿信息传输Figure 4 Transmission of pose information between receiving and processing modules

2.3.1 编译脚本优化

在“multi_UAV_communication. sh”文件中，源代码通过常量定义的方式来确定无人载具的类型和数量，并通过9 个单独的while 循环来判断和调用多个Python 脚本并行生成对应的模型，由于资源占用过多，易引发系统运行缓慢甚至崩溃等问题；代码结构分配不够明晰，则容易导致数据混淆的问题，不利于后期系统维护。将类型和数量信息储存进预设数组，重复的代码封装成名为“start_vehicle_communication”的函数调用，并使用嵌套循环来遍历数组以逐步生成编队模型，以减少代码的复用性，提高稳定性。

2.3.2 执行脚本优化

在“multi_UAV_communication. py”文件中，由定义的construct_target 的函数接收PositionTarget 传输的位置、速度、加速度或力、偏航角和偏航角速率等信息用于构造无人机的目标位姿信息。该函数中多PositionTarget 变量的重复性高，且未对异常信息进行检测处理，不符合规范的参数输入，易导致程序出错。因此，通过更为简洁的Vector 三维向量消息类型，将接收储存位姿信息中的每个元素都封装成独立对象，并添加数据异常处理函数。以此提高代码的可读性和可维护性，减少错误发生的可能性。例如，当motion_type 参数不是有效的MotionType 枚举值或当位置、速度或加速度的元素数量不等于3 时，程序可捕获并处理输入参数错误等异常情况，忽略错误的目标位姿信息。

通过调用多维数组，封装循环函数，简化数据类型，增添过滤机制等方法对无人机编队通信连接中的部分脚本进行结构重置和局部优化（图5），提升代码的易读性、稳定性和可维护性。

图5 通信脚本优化逻辑Figure 5 Communication script optimization logic

3 编队控制与导航仿真试验

编队协同飞行控制是指多架无人机之间通过通信协议和算法实现协同飞行的过程。在无人化智慧农场中，无人机编队可通过协同飞行控制实现对农田的精准施药、施肥与灌溉等近地作业。在无人机编队中，编队飞行控制可以实现无人机之间的位置、速度、姿态等信息的传递和共享，从而实现编队内部的协同飞行和任务分配。另外，为实现对农田进行精准作业，无人机编队需具备良好的自主导航能力。自主导航是指无人机通过搭载各类传感器和导航设备，如GNSS 模块、IMU 装置、激光雷达等，对周围环境进行感知和识别，进一步通过算法和控制策略，实现无人机的路径规划和自主飞行。自主导航技术可以帮助无人机实现避障、定位、优化路径等行为，从而实现高效、安全、精准的田间作业。编队控制与自主导航的融合为无人机编队实现复杂的编队内、编队间协同近地作业任务提供可能性，为无人机在农业生产领域的应用提供了更大的灵活性和扩展性。因此，除验证编队飞行控制的正常运行外，还加入了自主导航测试的环节，以便更加精准地模拟无人机编队在无人化智慧农场中的作业情况。

3.1 编队飞行控制

在“multirotor_keyboard_control.py”脚本文件中声明无人机的飞行参数：最大线速度（MAX_LINEAR）=1、最大角速度（MAX_ANG_VEL）=0.3、线速度步长（LINEAR_STEP_SIZE）=0.01、角速度的步长（ANG_VEL_STEP_SIZE）=0.01，以保证飞行的平稳性。定义速度控制（cmd_vel_mask）和领航控制（ctrl_leader）两个布尔变量的初始值为False，为无人机起飞前速度信息的修正和飞行时队形的保持提供基准信息。

将无人机手柄式遥控器的功能分配和操作逻辑映射至键盘按键。其中，左手控制w、x、a、d 键，控制无人机的前后左右飞行移动；右手控制i、“，”、j、l 键，控制无人机的升降转向；r、v、n 键为自定义功能按钮，分别实现返航、起飞、降落；s 和k 键分别是左右和右手的悬停待命按钮；t、b、g 键为模式切换按钮，分别管控进入/退出任务、离线/在线控制、全部/个体连接等模式间的切换。

对基于ROS/Gazebo 仿真环境和PX4 飞控的无人机编队开展的XTdone 平台仿真试验流程如下。

1）通过在ubuntu 系统的Terminal 中运行generator脚本文件，生成可由Gazebo 编译的3×3 队列的0～8号多旋翼无人机编队模型文件，设定各无人机的间距均为3 个标准单位，编队个体的编号及相对位置信息如图6 所示。

图6 无人机编队的编号顺序及相对位置Figure 6 The numbering sequence and relative positions of the UAV formation

2）在新的命令窗口分别通过launch 文件一键启动ROS core、PX4 飞控、Gazebo、MAVROS 节点，实现编队模型类型和数量的校验，各话题、服务通信节点的启动，预设数据（飞行速度、俯仰—横翻角度、传感器位置、广播频率、GPS 经纬度等）的核对，初始位置、速度数据与GPS 信息的重置融合，以及地面站、航点任务接收模块的预连接，最终打印输出编队内各机的通信连接结果，如图7 所示。

图7 编队内各无人机通信连接结果Figure 7 Communication connection results between unmanned aerial vehicles within the formation

3）编译并运行XTDone 环境下的0～8 号机通信连接和键盘控制脚本。通过预设的键盘按钮控制整个无人机编队分别向x、y、z坐标轴正方向飞行移动1 m，如图8 所示。

图8 无人机编队的空中悬停Figure 8 Autonomous formation flight and hovering of UAVs

4）通过ROS 指令，将无人机编队中的个体起飞及悬停位置信息打印输出（表1）。

表1 无人机编队飞行前后队形坐标对比Table 1 Experimental results and comparison of autonomous navigation

由飞行前后坐标的散点图（图9）可知，飞行前后无人机编队个体间的相对位置未发生大幅变化，队形仍可保持为正方形。由表1中分析期望坐标和悬停坐标之间的相对误差数据可知，误差值均在0.73～0.89 m间波动，且波动幅度较小。

图9 无人机编队飞行前后的目标与实际位置示意图Figure 9 Illustration of targets and actual positions before and after drone formation flight

为进一步描述无人机在执行定点导航任务时所能达到的准确程度，在此引入飞行精度的这一评估标准。其主要评估要素分别为位置偏差（position deviation）和姿态偏差（attitude deviation），但该部分试验并未出现大幅度无人机姿态变化，因此将位置偏差作为主要偏差进行分析计算，飞行精度

式中：CEP——飞行精度，%；

∆E——预期位置与实际位置的空间距离，m；

D——总预期飞行路径长度，m。

由计算结果可知，无人机编队单位飞行距离内的飞行精度约为76%。由2018 年1 月1 日实施《农用植保无人机技术要求及测试方法》（T/JSAMIA 2—2017）行业标准可知：无人机最大飞行速度为30～60 km/h 时，水平航迹与预设航线误差、垂直航迹与预设航线误差均应不大于10 m。等比换算可知：该无人机编队飞行速度为30 km/h 时，本次试验的平均航点误差约为6.5 m，飞行精度高于行业标准要求。考虑到无人机编队间数据传输的延迟和丢失的不可避免性，可推断本次试验中各通信节点的数据传输精度以及领航控制信息的发布精度处于正常水平，这进一步证明了无人机编队飞行作业在无人化智慧农场投入应用的可行性。

3.2 基于二维激光SLAM 的封闭空间地图构建

基于激光雷达的Hector SLAM 建图算法使用激光数据进行栅格地图构建和无人载具定位。通过高斯牛顿方法直接估算无人载具姿态信息，并使用占据栅格地图（occupancy grid map，OGM）表示环境，将无人载具所处的环境划分为空闲、占据或未知3 种状态的栅格，通过激光雷达扫描数据对栅格地图一一进行标记和状态更新，并将区域的坐标信息映射到全局地图中保存整合，生成全局地图［20］。

基于无人机编队模型实现飞行控制，操控无人机对二维激光SLAM 建图和自主导航仿真试验进行研究。QGroundControl（QGC）作为一种用于控制无人机开源的地面站软件，为用户提供了一个图形界面，可实时显示飞行器的状态、传感器数据和遥控器信息，并提供一些基本的飞行模式（如手动、自稳、定点悬停、航线追踪等）。启动QGC 并等待其与9 架无人机完成通信连接，在Gazebo 中构建“8”字形三维封闭地图，并由地图中部的正方形空心支柱和地图边缘的不规则凹凸结构模拟无人化农场中的飞行障碍。控制无人机在地图中飞行和激光扫描，将二维激光SLAM（Hector SLAM）输出的2D 位姿和高度值通过优化后的通信脚本（multi_communication. py）转化为MAVROS 话题，以此在ROS Visualization（RViz）中构建出完整二维地图，如图10 所示。

图10 基于二维激光SLAM 的地图构建Figure 10 Map construction based on 2D laser SLAM

将生成的二维线性地图与3D 地图的俯视图进行比对（图11），随机选取7 个特征检验点，并利用平行等长的参考线进行距离进行特征和距离对比。观察发现，除第2 处特征点外其余全部特征点均较大程度的吻合原地图的结构特征和位置距离。分析结果可初步证明该仿真平台下的激光SLAM 工具能构建具有一定参考价值的二维地图。

图11 二维线性地图与3D 地图俯视图的比对Figure 11 Comparison between 2D linear maps and 3D top view maps

3.3 基于全局地图的无人机自主飞行导航

全局路径规划的本质是在特定场景中，以多个条件为基准获得一条最优路径。Dijkstra、A*、人工势场法、快速探索随机树等均为路径规划领域经典算法［21］。在本次仿真试验中，global_planner 模块支持Dijkstra 和A*两种算法，理论上两种算法效果应近似，且A*算法计算复杂度更低。而对比结果表明，ROS global_planner 模块中的A*算法在寻求可行解进行路径规划时，其路线顺滑程度方面不如Dijkstra 算法的求解结果如图12 所示。

图12 Dijkstra、A*算法求解路径对比图Figure 12 Comparison of Dijkstra and A* algorithms for pathfinding

图12（b）中所规划的路线多次出现大幅度拐点和小范围连续断点（蓝色圆圈标示），而图12（a）中所呈现的路线较为平直顺滑。因此，本次试验最终选用Dijkstra 算法作为导航试验的路径规划算法［22］。

在RViz 中重新读取二维激光SLAM 生成的pgm格式地图文件，并将该地图作为自主导航系统中的全局地图。利用脚本文件启动ROS 自身的move_base 功能包和封装Dijkstra 路径规划算法，在RViz 中使用2D Nav Goal 功能在地图上随机、连续标定10 个目标航点，通过话题节点间的数据传输发布飞行位姿信息指令，控制无人机按照航点坐标顺序飞往目标位置，如图13 所示，记录无人机到达各目标航点时的x、y、z坐标，并将其与预定航点坐标进行对比分析，结果如表2。

表2 自主导航试验结果Table 2 Dominant navigation experiment results

图13 基于全局地图的自主飞行导航Figure 13 Autonomous flight navigation based on global map

由表2 中的误差分析和图14 的二维飞行路线对比可知：随着累计飞行距离的增加，累计飞行误差也不断增加。累计飞行距离和累计飞行误差的线性回归方程为，y=0.025 3x+0.351 2，其中R2（R-squared）为0.830 9（图15）。表明累计飞行距离和累计飞行误差存在较强的线性关系，而累计飞行误差会随着飞行距离的增加而平稳增长。试验结果证明：该仿真环境下无人机编队飞行控制的稳定度和自主导航定位的精准度均处在较高水准，可为无人化智慧农场中农田巡查、作物施肥等生产工作提供技术支撑。

图14 目标点与实际到达的二维路线对比图Figure 14 Comparison graph of the desired and actual two-dimensional routes to the target point

图15 累计飞行距离与累计误差的散点—拟合直线图Figure 15 Scatter plot of accumulated flight distance and accumulated error with a fitted line

无人机在“8”字形封闭空间内激光建图和自主导航功能的实现，证明了其在复杂环境中具备基本的作业能力，也表现出对于其在无人化智慧农场实际应用中的重要意义。在大尺度作业场景中，可利用无人机编队快速进行全局地图的建立，并配合其他无人载具实现高通量田间作业，提升农田生产效率和作业质量，减少资源浪费和环境污染。

4 结论

本研究搭建了基于XTDone 开源仿真平台的多旋翼无人机编队飞行仿真环境，并在进行通信代码的优化后，通过ROS 系统与PX4 飞控在Gazebo 仿真软件中完成了无人机编队模型的飞行控制；根据农田近地作业场景需求，利用基于激光雷达的Hector SLAM 算法实现无人机编队自主飞行定位与二维地图实时构建；同时，基于全局地图的Dijkstra 路径规划算法实现无人机编队的定点飞行导航。在编队仿真飞行试验中，无人机编队单位飞行距离内的平均飞行误差为0.257 2 m、飞行精度约为76%，累计飞行距离和飞行误差间的变异程度R2约为0.830 9。试验结果表明无人机编队在该仿真环境下表现出较为可靠的飞行精度和稳定性，能够为进一步开展无人机编队技术在无人化智慧农场领域的研究和应用提供技术支撑与参考依据。

本研究搭建了仿真环境并实现无人机编队飞行控制和自主导航，但并未进行无人机编队个体间的任务分配与复杂任务下的协同作业探究，而无人编队协同飞行、任务分配及自主导航技术正逐步为农、牧生产提供日益精准的近低空感知与作业服务。在今后的探索中，将进一步研究无人机编队的任务决策与协同工作，以推动无人机编队在农田的自动化监测与管理方面的应用。