简介一套基于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 无人机模型参数质量和电机响应为什么决定编队收敛编队控制器的输入是位置误差输出是位置期望或者速度期望本质上是给飞控层当外环。所以模型参数的物理意义必须搞清楚否则调参就是玄学。影响编队收敛的模型参数主要看四个我一般会先在模型文件里确认它们的取值。参数典型取值区间对编队收敛的影响机体质量 m0.81.5 kg位置环增益的量级由它决定质量越大同样增益下响应越慢偏航惯量 Izz0.0050.02 kg·m²决定航向响应速度编队转弯时跟随机航向滞后主要看它电机时间常数 τ0.050.2 s推力变化速率τ 越大高速追击时位置超调越明显最大推力与重力比2.03.0决定机动余量急停和避碰时控制器饱和的边界就是它理解这四个参数之后调增益就不会乱来。位置环增益 Kp 的量级大致围绕 m 换算出的自然频率来定Kp 太小编队像拉橡皮筋领航机转弯跟随机拖一个很大的弧形Kp 太大跟随机会在目标点附近振荡机身高频抖动。还有个容易被忽略的点是电机时间常数SITL 固件里电机响应被建模为一阶惯性环节τ 越大你的编队控制器越要把目标点“提前量”加进去否则转弯时内侧跟随机容易冲过头。2.3 通信拓扑选型三种结构的差异和适用场景编队通信拓扑直接决定你的节点怎么写。集中式是最简单的一个地面站节点统一计算所有无人机目标位姿再分别发布问题也明显单点故障地面站一挂整个编队失去目标指令。领航-跟随是这类仿真工程里最常见的做法领航机用自己的控制律飞航迹同时持续发布位姿每架跟随机订阅领航机位姿在本地算出自己的目标位置不需要知道其他跟随机在干什么。这种结构的通信图是星形适合 35 架的小编队代码量小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, anonymousTrue) # 期望偏移写在领航机机体坐标系下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_size1) 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 nsuavN所有话题自动带上命名空间前缀。launch !-- 三架无人机各自用命名空间隔离模型位姿通过参数传入 -- group nsuav0 include file$(find multi_uav_gazebo)/launch/spawn_uav.launch arg namemodel valuequadrotor_base / arg namex value0.0 / arg namey value0.0 / arg namez value1.0 / /include /group group nsuav1 include file$(find multi_uav_gazebo)/launch/spawn_uav.launch arg namemodel valuequadrotor_base / arg namex value2.0 / arg namey value0.0 / arg namez value1.0 / /include /group group nsuav2 include file$(find multi_uav_gazebo)/launch/spawn_uav.launch arg namemodel valuequadrotor_base / arg namex value4.0 / arg namey value0.0 / arg namez value1.0 / /include /group !-- 编队控制节点本示例用领航-跟随 -- node nameformation_controller pkgmulti_uav_formation typeformation_follower.py outputscreen / /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, anonymousTrue) # 队形表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_size1) 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 五个必调参数与调参顺序编队仿真的调参顺序比参数本身重要同时动三个参数发散了你根本不知道是谁的锅。我固定的套路是先间距再位置增益再阻尼最后才碰频率。参数默认值作用调参建议编队间距2.0 m队形尺寸决定避碰安全距离不小于旋翼直径 4 倍过小气流干扰会触发振荡位置环增益 Kp1.02.0目标跟踪的快慢从 0.5 起步每次翻倍出现超调就回退速度阻尼 Kv约为 2√Kp抑制位置超调跟 Kp 联动出现振荡先加 Kv 而不是减 Kp控制频率2050 Hz指令平滑度低于内环位置环带宽的一半宁低勿高use_sim_timetrue时钟同步多机仿真必须为 true单机可 false调间距最容易理解间距太小编队还没开始移动三架飞机的避碰势场就在互相打架目标位置永远在抖动。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_size10 导致消息堆积改 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 显示三个节点都 runningGazebo 世界里却只有两架飞机。这是多机仿真的经典问题模型名或端口冲突。同一份模型文件 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_pose6.2 故障注入与回放最便宜的可靠性训练半实物验证阶段我最推荐做故障注入。写一个简单的脚本随机 kill 掉领航机的消息发布或者用 tc 命令给某个话题所在链路加延迟观察编队的降级行为是否符合预期。这个验证在纯仿真阶段就该做一遍因为编队的价值恰恰体现在“有一架飞机出问题时其他飞机怎么办”。仿真里测出降级逻辑的漏洞成本是重启一次仿真半实物阶段发现成本是一架失控的飞机。我的习惯是每次编队实验都先录 rosbag不管是仿真还是半实物。很多偶发的编队异常现场肉眼根本看不出来回放时逐帧看话题数据才能定位是哪个节点先出了问题。rosbag 回放配合rqt_bag的同步视图基本能还原 90% 的编队异常现场。经历过一次在实飞前因为回放才发现领航机掉线保护没生效的事故之后我的规矩就是不带 rosbag 的编队实验不做这个习惯帮我省下的排查时间远比录包的成本多。希望帮到你。本文还有配套的精品资源点击获取
简介一套基于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 无人机模型参数质量和电机响应为什么决定编队收敛编队控制器的输入是位置误差输出是位置期望或者速度期望本质上是给飞控层当外环。所以模型参数的物理意义必须搞清楚否则调参就是玄学。影响编队收敛的模型参数主要看四个我一般会先在模型文件里确认它们的取值。参数典型取值区间对编队收敛的影响机体质量 m0.81.5 kg位置环增益的量级由它决定质量越大同样增益下响应越慢偏航惯量 Izz0.0050.02 kg·m²决定航向响应速度编队转弯时跟随机航向滞后主要看它电机时间常数 τ0.050.2 s推力变化速率τ 越大高速追击时位置超调越明显最大推力与重力比2.03.0决定机动余量急停和避碰时控制器饱和的边界就是它理解这四个参数之后调增益就不会乱来。位置环增益 Kp 的量级大致围绕 m 换算出的自然频率来定Kp 太小编队像拉橡皮筋领航机转弯跟随机拖一个很大的弧形Kp 太大跟随机会在目标点附近振荡机身高频抖动。还有个容易被忽略的点是电机时间常数SITL 固件里电机响应被建模为一阶惯性环节τ 越大你的编队控制器越要把目标点“提前量”加进去否则转弯时内侧跟随机容易冲过头。2.3 通信拓扑选型三种结构的差异和适用场景编队通信拓扑直接决定你的节点怎么写。集中式是最简单的一个地面站节点统一计算所有无人机目标位姿再分别发布问题也明显单点故障地面站一挂整个编队失去目标指令。领航-跟随是这类仿真工程里最常见的做法领航机用自己的控制律飞航迹同时持续发布位姿每架跟随机订阅领航机位姿在本地算出自己的目标位置不需要知道其他跟随机在干什么。这种结构的通信图是星形适合 35 架的小编队代码量小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, anonymousTrue) # 期望偏移写在领航机机体坐标系下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_size1) 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 nsuavN所有话题自动带上命名空间前缀。launch !-- 三架无人机各自用命名空间隔离模型位姿通过参数传入 -- group nsuav0 include file$(find multi_uav_gazebo)/launch/spawn_uav.launch arg namemodel valuequadrotor_base / arg namex value0.0 / arg namey value0.0 / arg namez value1.0 / /include /group group nsuav1 include file$(find multi_uav_gazebo)/launch/spawn_uav.launch arg namemodel valuequadrotor_base / arg namex value2.0 / arg namey value0.0 / arg namez value1.0 / /include /group group nsuav2 include file$(find multi_uav_gazebo)/launch/spawn_uav.launch arg namemodel valuequadrotor_base / arg namex value4.0 / arg namey value0.0 / arg namez value1.0 / /include /group !-- 编队控制节点本示例用领航-跟随 -- node nameformation_controller pkgmulti_uav_formation typeformation_follower.py outputscreen / /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, anonymousTrue) # 队形表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_size1) 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 五个必调参数与调参顺序编队仿真的调参顺序比参数本身重要同时动三个参数发散了你根本不知道是谁的锅。我固定的套路是先间距再位置增益再阻尼最后才碰频率。参数默认值作用调参建议编队间距2.0 m队形尺寸决定避碰安全距离不小于旋翼直径 4 倍过小气流干扰会触发振荡位置环增益 Kp1.02.0目标跟踪的快慢从 0.5 起步每次翻倍出现超调就回退速度阻尼 Kv约为 2√Kp抑制位置超调跟 Kp 联动出现振荡先加 Kv 而不是减 Kp控制频率2050 Hz指令平滑度低于内环位置环带宽的一半宁低勿高use_sim_timetrue时钟同步多机仿真必须为 true单机可 false调间距最容易理解间距太小编队还没开始移动三架飞机的避碰势场就在互相打架目标位置永远在抖动。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_size10 导致消息堆积改 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 显示三个节点都 runningGazebo 世界里却只有两架飞机。这是多机仿真的经典问题模型名或端口冲突。同一份模型文件 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_pose6.2 故障注入与回放最便宜的可靠性训练半实物验证阶段我最推荐做故障注入。写一个简单的脚本随机 kill 掉领航机的消息发布或者用 tc 命令给某个话题所在链路加延迟观察编队的降级行为是否符合预期。这个验证在纯仿真阶段就该做一遍因为编队的价值恰恰体现在“有一架飞机出问题时其他飞机怎么办”。仿真里测出降级逻辑的漏洞成本是重启一次仿真半实物阶段发现成本是一架失控的飞机。我的习惯是每次编队实验都先录 rosbag不管是仿真还是半实物。很多偶发的编队异常现场肉眼根本看不出来回放时逐帧看话题数据才能定位是哪个节点先出了问题。rosbag 回放配合rqt_bag的同步视图基本能还原 90% 的编队异常现场。经历过一次在实飞前因为回放才发现领航机掉线保护没生效的事故之后我的规矩就是不带 rosbag 的编队实验不做这个习惯帮我省下的排查时间远比录包的成本多。希望帮到你。本文还有配套的精品资源点击获取
平方复杂度无法被常规优化消除｜TaoToken 统一 Key 通道实测)
