简介：一套基于ROS的多无人机编队仿真项目源码，面向无人机控制、算法和智能机器方向的研究者、竞赛队伍及ROS进阶学习者。项目中覆盖PX4/ArduPilot等底层控制接口、多机通信、路径规划、避障、视觉定位与编队协同等关键模块，并配套Gazebo等仿真场景、可录制的bag数据与可视化工具，可帮助读者从零搭建多机协同实验环境，理解工程实现与算法融合的完整链路，并掌握传感器融合、软件架构设计要点。压缩包共1152个文件、约128MB，以launch启动配置、world/sdf场景描述、config参数文件、cpp/py算法源码、dae/stl三维模型及rviz可视化配置为主要类型，目录按功能模块划分，便于按需查阅和二次开发。目前已有370人学习下载，适合希望快速上手多无人机仿真、深入掌握ROS开发流程与无人机编队算法的开发者。

1. 多无人机编队仿真：为什么 ROS 是绕不开的底座

第一次做多无人机编队仿真的人，普遍觉得“基于ROS的多无人机编队仿真”不过是单机仿真多开几个进程。真正把三架飞机同时放进仿真器的人都知道，坑全藏在坐标系、时钟和话题隔离里：三架飞机都往同一个话题发指令，后发覆盖先发；时间基准不统一，目标位置持续漂移。这套方案的价值，是用 ROS 的话题机制和 TF 树，把“多机协同”的系统工程问题收敛成模块化开发。它适合正在验证编队算法收敛性、需要快速拿到可视化结果的从业者，也适合从单机过渡到多机、想先摸清系统边界的一线工程师。下面按架构选型、算法落地、启动调参、踩坑排查的顺序往下拆。

2. 仿真环境拆解：飞控、仿真器、中间件与通信拓扑选型

2.1 四层架构各干什么：先分清锅在谁头上

拿到这类仿真工程，第一件事不是去改算法，而是先把系统分成四层。最底层是飞控层，负责姿态和位置的内环控制。在仿真里，常见做法是接入开源飞控的 SITL 模式，也就是把飞控固件当作一个进程跑起来，靠 MAVLink 与仿真器交换传感器数据和电机指令。这一层决定了无人机对外部指令的响应特性，你的编队节点发一个目标位置，飞控层要经过姿态环、位置环的串级控制才能把飞机拉过去，这个响应快慢直接决定编队能不能跟上。

第二层是仿真器层，提供重力、空气阻力、碰撞和传感器噪声。仿真器里的 IMU 有零偏，气压计有随机噪声，GPS 有延迟，这些噪声会在多机场景里被放大——两架无人机都在做位置闭环，各自的位置估计都有几厘米的偏差，编队间距如果卡得很紧，这几厘米就可能变成碰撞。第三层是中间件层，也就是 ROS 本身，负责节点间的话题通信、TF 坐标变换和仿真时间统一。第四层才是你要写的编队算法层，它只负责算目标位置或者目标速度。

分清这四层最大的好处是排错快。编队发散时，先看是算法算错了目标，还是话题没收到数据，还是飞控内环根本就没跟上指令。我最常看到的情况是，新手拿着仿真器自带的简化四旋翼模型调出来的编队参数，换到 SITL 模型上直接发散，因为简化模型推力响应是瞬时的，真实固件的电机模型有延迟，同一套增益在两个模型上表现完全不同。

2.2 无人机模型参数：质量和电机响应为什么决定编队收敛

编队控制器的输入是位置误差，输出是位置期望或者速度期望，本质上是给飞控层当外环。所以模型参数的物理意义必须搞清楚，否则调参就是玄学。影响编队收敛的模型参数主要看四个，我一般会先在模型文件里确认它们的取值。

理解这四个参数之后，调增益就不会乱来。位置环增益 Kp 的量级大致围绕 m 换算出的自然频率来定，Kp 太小，编队像拉橡皮筋，领航机转弯，跟随机拖一个很大的弧形；Kp 太大，跟随机会在目标点附近振荡，机身高频抖动。还有个容易被忽略的点是电机时间常数，SITL 固件里电机响应被建模为一阶惯性环节，τ 越大，你的编队控制器越要把目标点“提前量”加进去，否则转弯时内侧跟随机容易冲过头。

2.3 通信拓扑选型：三种结构的差异和适用场景

编队通信拓扑直接决定你的节点怎么写。集中式是最简单的，一个地面站节点统一计算所有无人机目标位姿再分别发布，问题也明显，单点故障，地面站一挂整个编队失去目标指令。领航-跟随是这类仿真工程里最常见的做法：领航机用自己的控制律飞航迹，同时持续发布位姿；每架跟随机订阅领航机位姿，在本地算出自己的目标位置，不需要知道其他跟随机在干什么。这种结构的通信图是星形，适合 3～5 架的小编队，代码量小，RViz 里追踪问题也直观。

分布式一致性则是每架飞机只和邻居交换状态，编队形状是协商出来的，不依赖某个领航节点，鲁棒性最好，但工程复杂度上了一个台阶。你需要维护邻居状态缓存、处理消息乱序、考虑图的代数连通度，调参时还得同时照顾好几条链路的增益。我的建议很直接：如果目标是快速验证一个队形保持算法，别犹豫，用领航-跟随；如果目标里有“任意一架飞机掉线后编队还能重组”，那再上一致性协议。选型这一步走错，后面所有代码都要推倒重写。

提示：多机仿真里“话题隔离”是选型之外的另一条命。每架无人机的状态话题、指令话题必须落在各自命名空间下，否则后启动的节点会覆盖先启动的，这个坑在第四章展开说。

3. 编队控制算法落地：领航-跟随与一致性控制的代码实现

3.1 领航-跟随：最小可行方案与偏航旋转的实现

领航-跟随最核心的一段代码，是把“跟随机相对领航机的期望偏移”从领航机体坐标系转到世界坐标系。新手最容易漏掉的就是这个旋转：直接把偏移加到领航机世界坐标上，平飞看起来没问题，领航机一转向，队形就像被拖在地上一样整体歪掉。

#!/usr/bin/env python3 # 领航-跟随编队：跟随机订阅领航机位姿，在机体系下计算目标点 import rospy import numpy as np from geometry_msgs.msg import PoseStamped class FormationFollower: def __init__(self): rospy.init_node('formation_follower', anonymous=True) # 期望偏移写在领航机机体坐标系下（x 前、y 右、z 上） # 这里取三角队形的右后位，方便领航机转弯时验证旋转逻辑 self.offset = np.array([-1.5, 1.5, 0.0]) self.leader_pose = None # 订阅领航机位姿，发布目标位置给本机的位置控制器 rospy.Subscriber('/uav0/ground_truth', PoseStamped, self.leader_cb) self.target_pub = rospy.Publisher('/uav1/cmd_pose', PoseStamped, queue_size=1) def leader_cb(self, msg): self.leader_pose = msg.pose def yaw_of(self, q): # 只从四元数提取偏航角，避免欧拉角万向锁 return np.arctan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z)) def loop(self): rate = rospy.Rate(30) # 30 Hz，低于内环位置环带宽 while not rospy.is_shutdown(): if self.leader_pose is None: rate.sleep() continue yaw = self.yaw_of(self.leader_pose.orientation) # 偏航旋转矩阵：把领航机体系偏移转到世界系 R = np.array([[np.cos(yaw), -np.sin(yaw), 0.0], [np.sin(yaw), np.cos(yaw), 0.0], [0.0, 0.0, 1.0]]) p = self.leader_pose.position target = np.array([p.x, p.y, p.z]) + R @ self.offset msg = PoseStamped() msg.header.stamp = rospy.Time.now() msg.pose.position.x = target[0] msg.pose.position.y = target[1] msg.pose.position.z = target[2] msg.pose.orientation.w = 1.0 self.target_pub.publish(msg) rate.sleep() if __name__ == '__main__': try: FormationFollower().loop() except rospy.ROSInterruptException: pass

这段代码里有两个关键决策。一是旋转矩阵只用偏航角，不用完整姿态，因为水平编队不需要跟随领航机的横滚和俯仰，用了反而会把姿态噪声引入目标点。二是发布频率选 30 Hz 而不是越高越好，位置控制环带宽通常只有几个赫兹，发得太快消息队列会堆积，控制效果反而变差。offset 数组就是你的队形定义，改成[-1.5, -1.5, 0]就是左后位，三架飞机的队形就是靠给每架跟随机配不同的 offset 搭出来的。

3.2 一致性控制：邻居协商出队形，代数连通度决定收敛

如果选分布式方案，控制律就变成一致性协议。核心思想是每架无人机只跟邻居交换位置状态，控制输入等于带权重的邻居相对位置误差之和，当所有飞机的相对误差趋于零，队形就形成了。

#!/usr/bin/env python3 # 一致性编队：每架无人机与邻居交换位置，按图权重收敛到期望队形 import rospy import numpy as np from std_msgs.msg import Float64MultiArray class ConsensusNode: def __init__(self): self.state = np.zeros(3) # 本机当前位置，由飞控位置反馈填充 self.cache = {} # 邻居ID -> 最近一帧的位置状态 self.gain = 0.5 # 一致性增益，先小后大 # 邻接表在工程里从 yaml 配置读取，写成定点格式方便换队形 self.neighbors = {1: 1.0, 2: 0.8} self.target_offset = np.array([0.0, -2.0, 0.0]) # 期望队形偏移 rospy.Subscriber('/uav0/state', Float64MultiArray, self.state_cb) rospy.Subscriber('/uav1/state', Float64MultiArray, self.neighbor_cb) def control(self): # 一致性协议：u = gain * Σ w_ij * ((x_j - offset_j) - (x_i - offset_i)) u = np.zeros(3) for nid, w in self.neighbors.items(): if nid in self.cache: err = (self.cache[nid] - self.target_offset) - self.state u += self.gain * w * err return u

一致性协议比领航-跟随难调，根源在收敛速度由通信图的代数连通度决定。三架飞机排成一条线，中间那架是瓶颈，它的邻居只有两个，信息传播要经过它中转，整个编队收敛就慢；换成三角形全连通拓扑，代数连通度变大，收敛速度明显提升。实际调参时如果发现编队半天收不拢，先别急着加增益，先检查邻接表是不是把中间节点孤立了。另外注意 cache 的更新策略，ROS 消息天然可能乱序，一定要取“最近一帧”而不是“先到先得”，否则控制输入会在两三个旧状态之间来回跳，编队会抖。

3.3 队形变换与避碰：权重切换与人工势场的组合

队形变换的实现有两类思路。一类是直接改期望偏移，从直线队形的 offset 线性插值到三角队形的 offset；另一类是改一致性协议里的邻接权重，断开某些边、增强某些边，编队就自然重排。我一般用前者，因为插值的轨迹可控，而权重切换的过渡过程依赖收敛速度，不好预测。

避碰则建议用人工势场，在编队控制器输出的目标位置上叠加一个排斥项。势场只作用于位置，不作用于速度，否则会和位置环的阻尼打架，造成持续振荡。实现时加一个切换阈值和迟滞区间：两机距离小于安全半径时，排斥项接管目标点；距离恢复到大安全半径之后才切回编队目标。没有迟滞的话，飞机会在阈值附近来回切换，看起来就像在编队位置附近“哆嗦”。

4. 启动一整个编队：launch 文件、话题检查与五个必调参数

4.1 一条 launch 拉起三架无人机：命名空间与模型生成

多机仿真的 launch 文件和单机最大的区别就是命名空间。如果不做隔离，三架飞机的状态话题都叫/ground_truth，后面启动的节点会覆盖前面的数据，RViz 里看到的全是同一架飞机。工程上的标准做法是把每架飞机包进一个<group ns="uavN">，所有话题自动带上命名空间前缀。

<launch> <!-- 三架无人机各自用命名空间隔离，模型位姿通过参数传入 --> <group ns="uav0"> <include file="$(find multi_uav_gazebo)/launch/spawn_uav.launch"> <arg name="model" value="quadrotor_base" /> <arg name="x" value="0.0" /> <arg name="y" value="0.0" /> <arg name="z" value="1.0" /> </include> </group> <group ns="uav1"> <include file="$(find multi_uav_gazebo)/launch/spawn_uav.launch"> <arg name="model" value="quadrotor_base" /> <arg name="x" value="2.0" /> <arg name="y" value="0.0" /> <arg name="z" value="1.0" /> </include> </group> <group ns="uav2"> <include file="$(find multi_uav_gazebo)/launch/spawn_uav.launch"> <arg name="model" value="quadrotor_base" /> <arg name="x" value="4.0" /> <arg name="y" value="0.0" /> <arg name="z" value="1.0" /> </include> </group> <!-- 编队控制节点：本示例用领航-跟随 --> <node name="formation_controller" pkg="multi_uav_formation" type="formation_follower.py" output="screen" /> </launch>

这个 launch 里每个 include 的 x、y 参数是无人机在仿真世界里的出生点。初始间距至少要留出两倍机身长度，不要为了画面好看把三架飞机挤在一起，否则起飞阶段的内环收敛过程就可能撞机。启动方式很简单，两条命令：

source devel/setup.bash roslaunch multi_uav_formation formation_3uav.launch

启动后先别急着发编队指令，花 30 秒做三个检查。第一，rostopic list | grep uav看话题是不是按 uav0、uav1、uav2 分好了；第二，rosrun tf view_frames生成 TF 树，确认三架飞机各自有独立的坐标系；第三，用rqt_graph看编队节点有没有同时发布到三架飞机。这三个检查做完再动算法，能省掉后面一半排查时间。

4.2 编队控制节点的骨架代码：目标位姿计算与发布

上一章的跟随机代码只覆盖一架跟随机，工程里你会写一个支持多机的控制器，根据无人机 ID 加载不同的 offset 和话题名。骨架逻辑是同一个：订阅领航机位姿，按 ID 查 offset，算目标，发布到对应命名空间的指令话题。

#!/usr/bin/env python3 # 多机编队控制器：按无人机 ID 加载队形偏移，向多架跟随机发布目标 import rospy import numpy as np from geometry_msgs.msg import PoseStamped rospy.init_node('formation_controller', anonymous=True) # 队形表：uav1 左后位，uav2 右后位，uav3 正后位 # 工程里这里建议从 yaml 读取，方便换队形而不用改代码 FORMATION = { 'uav1': np.array([-1.5, 1.5, 0.0]), 'uav2': np.array([-1.5, -1.5, 0.0]), 'uav3': np.array([-2.5, 0.0, 0.0]), } leader_pose = None def leader_cb(msg): global leader_pose leader_pose = msg.pose rospy.Subscriber('/uav0/ground_truth', PoseStamped, leader_cb) pubs = {key: rospy.Publisher('%s/cmd_pose' % key, PoseStamped, queue_size=1) for key in FORMATION}

这段初始化代码的作用是建立“队形表”和“发布表”的映射。队形表用字典而不是硬编码的多个变量，加第四架飞机只需要在字典里多一行。发布表同理，用字典推导式给每架飞机建一个独立的 publisher。这样编队控制器的扩展成本降到了最低，这也是多机工程和单机脚本的重要区别。

4.3 启动后必做的三个检查：TF 树、话题频率与仿真时钟

多机仿真一个非常隐蔽的坑是仿真时钟。Gazebo 里默认用/clock发布仿真时间，所有节点的 ros::Time 必须跟随它走，否则编队节点用系统时间算间隔，飞控用仿真时间做控制，两边的时间戳对不上，跟随机就会表现出“间歇性抽搐”。检查方法是看节点的rospy.get_rostime()和系统时间差多少，差得远就是use_sim_time没设。

话题频率检查用rostopic hz /uav1/cmd_pose，正常情况下频率应该稳定在发布频率附近。如果频率忽高忽低，多半是消息队列满了或者发布端被阻塞，queue_size 改成 1 只取最新能缓解大部分问题。TF 树检查则要看有没有 frame_id 冲突，三架飞机的 base_link 都叫同一个名字的话，TF 树会直接报错，这个问题第五章专门讲。

4.4 五个必调参数与调参顺序

编队仿真的调参顺序比参数本身重要，同时动三个参数，发散了你根本不知道是谁的锅。我固定的套路是：先间距，再位置增益，再阻尼，最后才碰频率。

调间距最容易理解，间距太小，编队还没开始移动，三架飞机的避碰势场就在互相打架，目标位置永远在抖动。Kp 和 Kv 的组合本质上是在调一个二阶系统，Kp 决定自然频率，Kv 决定阻尼比，理想状态是临界阻尼，跟随机快速到达目标点且不振荡。控制频率这块很多人有误区，觉得越高越好，实际上一套 10 Hz 的位置控制器，你用 100 Hz 发目标，消息队列里堆的全是过期目标，内环跟踪的是旧数据，效果反而更差。

5. 避坑指南：多无人机编队仿真的五个翻车现场与排查

5.1 现象：跟随机起飞后持续漂移，目标点追不上

编队节点发布的目标位置看起来在动，跟随机却越飞越偏，甚至绕圈。原因几乎都是仿真时钟不一致：编队节点用的系统时间，飞控内环用的仿真时间，两者误差随时间累积，位置控制器的目标点永远是“旧”的。解决方法是检查所有涉及时间戳的配置，use_sim_time统一为 true，代码里禁止用time.time()，全部换成rospy.Time.now()。改完重启后先跑盘旋航线验证，如果漂移消失就确认是时钟问题。

5.2 现象：三架起飞后互相碰撞，避碰逻辑没生效

编队指令还没发，三架飞机在起飞爬升阶段就撞到一起。常见原因有两个：出生点间距小于避碰半径，起飞时内环收敛的位置波动就足以触发碰撞；或者避碰势场的排斥力上限低于编队控制器的拉力，势场被“压住”了。解决思路是先拉开出生点间距到安全距离两倍以上，再确认势场最大排斥力大于编队控制力，否则障碍物一逼近，避碰根本抢不过编队指令。我习惯在代码里加一条调试话题，把势场力和编队力的幅值打印出来，一眼就能看出谁在主导。

5.3 现象：跟随机高频抖动，机身出现持续振荡

抖动多发生在编队转弯或加速阶段。原因是增益给过了，或者控制频率和消息频率不匹配。先看是不是 Kp 太大导致的极限环振荡，把 Kp 降一半观察；如果抖动依旧，检查发布端是不是用 queue_size=10 导致消息堆积，改 1 只取最新。还有一个容易忽略的点：仿真器里如果用了降噪传感器模型，位置反馈自带低通滤波，增益加再高实际跟踪带宽也上不去，这时候加增益只会让系统更接近不稳定边界。血泪教训：位置抖动的排查顺序永远是增益、队列、传感器滤波，别一上来就怀疑算法。

5.4 现象：RViz 里 TF 树错乱，三架飞机的坐标互相串

三者都显示在同一位置，或者坐标系跳来跳去。几乎可以断定是 frame_id 冲突：无人机模型的 URDF 里 base_link 写死了，三份模型 publish 出相同 frame_id 的 TF，后启动的覆盖先启动的。解决方法是给每架飞机的 URDF 加命名空间前缀，或者在 robot_state_publisher 里设 tf_prefix。检查时用rostopic echo /tf看 frame_id 是不是带着 uav0、uav1 前缀，不带就是没隔离干净。

5.5 现象：第三架无人机没出现在仿真器里，但节点都启动了

launch 显示三个节点都 running，Gazebo 世界里却只有两架飞机。这是多机仿真的经典问题：模型名或端口冲突。同一份模型文件 spawn 三次，如果模型名没有按命名空间区分，Gazebo 会静默失败。解决办法是给每架飞机的 spawn 传不同 model 名和初始位姿，检查 gzserver 的启动日志，里面会明确提示是模型加载失败还是端口被占用。千万别只盯着 ROS 侧的日志，多机仿真的模型生成问题八成出在仿真器侧。

6. 从仿真到半实物：用同一套编队节点做硬件在环验证

6.1 关键差异：时钟、时延与断连处理

编队算法在仿真里跑通了，下一步不是上真机，而是硬件在环。做法是把仿真器替掉，让飞控实机或者 SITL 固件直接接入，你的编队节点一行不改，因为它的接口是话题，它根本不关心数据从哪里来。但这个切换有三个差异必须处理。

时钟变了。仿真里 use_sim_time 给你一个干净的统一时间，硬件场景没有仿真时钟，必须回到系统时间，同时给所有输入加“新鲜度检查”。我实现的逻辑是：编队节点记录领航机消息的最后到达时间，超过 200 ms 没收到新数据，就判定领航机失控，跟随机立刻切换到悬停模式，同时上报编队降级状态。这个超时保护是半实物验证里最重要的代码，没有它，一次通信抖动就能让整个编队散架。时延和丢包也一样，仿真里消息丢失概率接近零，无线链路则完全不同，跟随机的缓存必须保留最近一帧而不是空的等待。

# 录制多机编队全过程，回放时用 rqt 逐帧排查异常 rosbag record -O formation_take1.bag \ /uav0/ground_truth /uav1/ground_truth /uav2/ground_truth \ /uav0/cmd_pose /uav1/cmd_pose /uav2/cmd_pose

6.2 故障注入与回放：最便宜的可靠性训练

半实物验证阶段我最推荐做故障注入。写一个简单的脚本，随机 kill 掉领航机的消息发布，或者用 tc 命令给某个话题所在链路加延迟，观察编队的降级行为是否符合预期。这个验证在纯仿真阶段就该做一遍，因为编队的价值恰恰体现在“有一架飞机出问题时，其他飞机怎么办”。仿真里测出降级逻辑的漏洞，成本是重启一次仿真；半实物阶段发现，成本是一架失控的飞机。

我的习惯是每次编队实验都先录 rosbag，不管是仿真还是半实物。很多偶发的编队异常，现场肉眼根本看不出来，回放时逐帧看话题数据才能定位是哪个节点先出了问题。rosbag 回放配合rqt_bag的同步视图，基本能还原 90% 的编队异常现场。经历过一次在实飞前因为回放才发现领航机掉线保护没生效的事故之后，我的规矩就是：不带 rosbag 的编队实验不做，这个习惯帮我省下的排查时间远比录包的成本多。希望帮到你。

本文还有配套的精品资源，点击获取