企业官网建设流程全解析

简介：基于ROS的多无人机编队仿真资料，面向机器人领域学生、科研人员及无人机算法开发者，围绕多机协同编队、路径规划与避障等核心问题提供一整套可运行的仿真资源。压缩包共1152个文件，约127.95MB，涵盖launch启动配置、cpp/h算法实现、sdf/world仿真模型、urdf/xacro机器人描述、yaml参数、bag数据记录以及rviz可视化配置，能够直接用于Gazebo环境下的多无人机仿真与调试。已有370人学习查看。内容涉及PX4/ArduPilot底层控制、ROS通信机制、几何路径规划、激光雷达避障、视觉里程计定位、扩展卡尔曼滤波多传感融合以及编队保持算法，系统展示了从单机控制到多机协同的完整技术链路。同时保留了完善的软件架构组织方式，便于研究launch文件批量启动、topic/service通信和参数管理实践。适合希望深入理解ROS集成环境下无人机编队算法落地细节、并快速搭建实验场景的开发者参考。

1. 基于ROS的多无人机编队仿真：拿到压缩包后先看这四层再启动

拿到一个“基于ROS的多无人机编队仿真”压缩包，绝大多数人的第一步是解压、编译、跑launch，然后盯着Gazebo里三架无人机看能不能飞。但这类项目最坑的地方恰恰是能跑不代表对——三架飞机同时出现在画面里，可能只是各自在空转，编队控制器根本没接手。多无人机编队仿真解决的是“不上真机就把集群算法验证掉”的需求，要同时搞定四层东西：仿真场景、机载飞控、编队控制器、任务调度。适合正在做集群算法验证、从单机转多机或者做课题选型的开发者。想把这个压缩包变成自己能改能调的框架，不要急着双击启动脚本，先按下面四层拆一遍，再决定从哪一层开始动刀。

2. 编队仿真的四层结构：世界、机载、控制分工与文件体检

2.1 仿真场景层：世界文件与无人机模型的分工

基于ROS的多无人机编队仿真里，Gazebo世界文件和无人机模型文件是两种完全不同的东西。世界文件（.world）负责地面、光照、重力、物理引擎参数，它不关心飞机长什么样；无人机则是单独的URDF/SDF模型，由launch文件通过spawn_entity插件逐个放进世界。编队仿真中常见的组织方式是一个世界文件配多个spawn调用，每个spawn通过xacro参数传入无人机的命名空间和初始坐标，所以场景层的核心工作其实是“摆位置”和“分命名空间”，而不是建模。

一个值得提前避开的坑：如果无人机模型的xacro宏里用的是固定link名，比如base_link，三架飞机加载进来后TF树会互相覆盖，编队控制器读到的位姿时而是uav0的、时而是uav1的，表现就是飞机乱跳。规范的工程应该把每个link、joint和TF变换都加上命名空间前缀，这个前缀通常由参数ns传入。检查模型文件时，第一件事就是grep有没有硬编码的link名。

在终端里做一次文件体检很直接：

# 检查launch里有几个spawn，坐标有没有重叠 grep -n "spawn" src/my_formation/launch/*.launch | grep uav # 检查模型文件里link名是否带命名空间参数 grep -n "link name" src/my_formation/models/uav.xacro | head -20 # 确认包路径存在且没有被重复定义 rospack find my_formation

这三条命令能快速看出一个编队仿真包是“真多机”还是“三份单机硬凑”。如果模型link名固定、spawn坐标全在原点，后面的调试基本会变成灾难。我习惯在动任何代码前先跑完这三条命令，省下的时间远大于执行命令本身的花费。

2.2 机载层：轻量飞控仿真与SITL怎么选

第二层是机载飞控仿真。多无人机编队仿真通常有两条路线：轻量模型方案和SITL方案。轻量方案的代表是hector_quadrotor这类Gazebo插件，模型简单、起飞快、CPU占用低，适合验证队形算法；SITL方案接入PX4或ArduPilot的仿真固件，飞控链路更接近真机，但每一架飞机都是一个独立飞控进程，三机编队时内存和CPU开销明显上升。

我的选择逻辑很直接：编队算法验证阶段用轻量仿真，等编队逻辑稳定后再把控制器接到SITL上跑一遍系统联调。直接上SITL会放大排错成本，因为飞控参数和编队参数混在一起，问题定位要跨两个系统；反过来只用轻量仿真、从来不上SITL，又容易把“控制器能用”误判成“飞控衔接没问题”，真机移植时被姿态控制差异卡住。

怎么判断手里的压缩包属于哪一类？看依赖和launch文件。launch里出现px4、mavros、sitl这些关键字，就是SITL方案；如果只有gazebo_ros、quadrotor这类插件，就是轻量方案。编队仿真的目标是验证算法而不是复刻真机，所以机载层的选择标准只有一个——和你要验证的算法耦合度。耦合的是队形保持逻辑，就用轻量模型；耦合的是链路延迟和丢包影响，就必须上SITL。

2.3 编队控制层：三种主流控制策略的取舍

编队控制层是整个压缩包的灵魂，也是“基于ROS的多无人机编队仿真”这个标题真正值钱的部分。基于ROS的项目里常见的策略有三种：领航-跟随者、虚拟结构、虚拟领航者。三者的核心差异在于“谁来决定队形中心”。

很多编队仿真包默认采用领航-跟随者，因为实现直观：跟随者节点订阅领航机的位姿，加上一个固定偏移，算出自己的期望位置，再发速度指令给飞控。但它有天然弱点——队形误差会沿链传播，领航机一旦抖动，末尾飞机会放大误差。虚拟领航者则把所有跟随者都接到同一个数学轨迹点上，误差不传播，实现也只需要多维护一条轨迹生成器。

选型时不要只看论文里写得漂亮。实际工程里，领航-跟随者调试最容易、翻车概率最低；虚拟结构对话题同步要求极高，仿真里一个节点堵了半秒，整个队形就崩。新人做编队仿真，我建议从领航-跟随者起步，跑通后改成虚拟领航者，再考虑虚拟结构。

2.4 任务层：命令下发与状态机的完整链路

最后一层是任务调度。编队不是让三架飞机同时保持一个队形就结束了，还需要“起飞、编队、变换、降落”这样按阶段推进的任务状态机。这一层通常由一个task_manager节点负责，它发布当前任务编号和参数，编队控制器收到后切换内部的期望队形或目标高度。

完整的数据链路是：task_manager → 编队控制器 → 单机位姿控制器 → 飞控插件 → Gazebo物理引擎。任何一个环节的名字空间错位都可能断链。排查链路有两种手段：看话题图和看日志。rostopic info能查某个话题是谁发布谁订阅；rqt_graph能直观显示节点间的连接。多机编队里如果话题图上没有带前缀的交叉连线，说明命名空间隔离是有效的。

这一层最容易出的问题不是代码错，而是逻辑错。task_manager下发“切队形”指令时，编队控制器可能还在等待第一架飞机起飞完成。成熟的工程会在任务节点里加ready握手——各机先报告自己到达悬停点，task_manager再下发下一步指令。没有这一步，编队初始化的瞬态过程会被当成队形误差，控制器立刻纠正，表现就是起飞后飞机先乱窜一阵。

2.5 xacro参数化模型：多机不串线的起点

前面反复说到link名带前缀，这里给一个最简的xacro宏写法，也是编队仿真包最常见的模型组织方式：

<xacro:macro name="uav_model" params="ns x y yaw"> <!-- 所有link名都拼上ns，避免三机TF互相覆盖 --> <link name="${ns}_base_link"> ... </link> <joint name="${ns}_base_joint" type="fixed"> <parent link="${ns}_base_link"/> <child link="${ns}_imu_link"/> </joint> </xacro:macro>

宏里面所有link、joint的名字都用${ns}作为前缀，三架飞机加载后互不干扰。调用时只需在launch里传不同的ns值。如果压缩包里的模型使用了这种写法，说明作者对多机命名空间有基本认知；如果全是固定名，那就要做好自己动手改造的准备。这也是我判断一个编队仿真包能不能直接用的首要指标。

3. 跑通第一个多机launch：命名空间隔离与三机同时起飞

3.1 最小的多机launch怎么写

一个能跑起来的多机launch至少有三个部分：启动Gazebo、重复拉起单机spawn、给每架飞机独立命名空间。常见的实现是在group ns标签内include同一个spawn launch，这样同一份模型配置可以被多架飞机复用。最小可用写法如下：

<launch> <arg name="world" default="$(find my_formation)/worlds/empty.world"/> <include file="$(find gazebo_ros)/launch/empty_world.launch"> <arg name="world_name" value="$(arg world)"/> </include> <!-- 三架飞机共用同一个模型宏，只传不同命名空间和坐标 --> <group ns="uav0"> <include file="$(find my_formation)/launch/spawn_uav.launch"> <arg name="ns" value="uav0"/> <arg name="x" value="0.0"/> <arg name="y" value="0.0"/> <arg name="yaw" value="0.0"/> </include> </group> <group ns="uav1"> <include file="$(find my_formation)/launch/spawn_uav.launch"> <arg name="ns" value="uav1"/> <arg name="x" value="2.5"/> <arg name="y" value="0.0"/> <arg name="yaw" value="0.0"/> </include> </group> <group ns="uav2"> <include file="$(find my_formation)/launch/spawn_uav.launch"> <arg name="ns" value="uav2"/> <arg name="x" value="0.0"/> <arg name="y" value="2.5"/> <arg name="yaw" value="0.0"/> </include> </group> </launch>

这里最关键的标签是group ns="uav0"，它会把内部所有相对话题名和相对参数名自动加上uav0前缀。spawn_uav.launch里的模型描述和spawn_entity都会注册到uav0命名空间下。这段launch里没有把三架飞机写成循环，因为roslaunch原生不支持for循环，我通常会为更多飞机的场景使用动态生成launch的脚本，但三机这种固定规模直接手写反而更清晰。

3.2 spawn子launch的写法与参数传递

spawn_uav.launch接收ns、x、y、yaw四个参数，用xacro生成模型描述后，再通过spawn_entity把模型放进Gazebo。代码如下：

<launch> <arg name="ns"/> <arg name="x" default="0.0"/> <arg name="y" default="0.0"/> <arg name="yaw" default="0.0"/> <param name="robot_description" command="$(find xacro)/xacro '$(find my_formation)/models/uav.xacro' ns:=$(arg ns) x:=$(arg x) y:=$(arg y) yaw:=$(arg yaw)"/> <node name="spawn_model" pkg="gazebo_ros" type="spawn_entity" output="screen" respawn="false" args="-topic robot_description -entity $(arg ns) -x $(arg x) -y $(arg y) -Y $(arg yaw)"/> </launch>

参数说明：ns是命名空间前缀，x、y是初始坐标，yaw是初始朝向角。spawn_entity的-entity参数是模型在Gazebo里的实体名，必须唯一，否则Gazebo会拒绝加载并报already exists。把spawn写成一个子launch的好处是后续加第四架、第五架飞机时不需要复制模型配置，只需在总launch里加一个group。

这里有个容易踩的坑：spawn_entity节点必须在group ns下启动，否则它订阅的robot_description是全局话题，加载出来的模型实体名如果重复，新版Gazebo会直接拒绝spawn。所以group ns的位置很关键，它管的是节点而不是模型文件。

3.3 启动后先查命名空间和TF，再谈控制

多机launch跑通后，先别急着发起飞指令。确认三件事：rostopic list里的所有话题都带uav前缀；rosrun tf view_frames生成的TF树上每个link都带前缀；每个控制器节点订阅的是带前缀的位姿话题。我见过很多项目卡在模型link名固定导致TF互相覆盖，表面上看三架飞机都出现在画面里，实际只有最后一架加载的模型在响应指令。

此时我会用三条命令做启动后体检：

# 列出所有与uav相关的话题，确认是否带命名空间前缀 rostopic list | grep uav # 查看某一架飞机的位姿是否在更新，数字在变说明模型正常 rostopic echo -n 3 /uav0/ground_truth/pose # 生成TF树PDF，重点看base_link是否带uav前缀 rosrun tf view_frames

如果发现话题不带前缀，十有八九是节点启动时没用相对命名空间，或者代码里的话题名用了绝对路径（以/开头）。这是多机仿真里最常见的误用，后面第5章会专门展开。先把这一步确认完，再往下做编队控制才不会白忙。

3.4 三机同时起飞的最小命令与条件

假设每架飞机都订阅cmd_vel话题，控制器需要把期望速度发到uav0/cmd_vel、uav1/cmd_vel、uav2/cmd_vel。最小的起飞验证可以直接用rostopic pub，但编队仿真里一般会写一个起飞脚本来控制时序，避免三机同时操作带来的相位差。

rostopic pub -r 10 /uav0/cmd_vel geometry_msgs/Twist \ '{linear: {x: 0.0, y: 0.0, z: 2.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}'

这个命令以10Hz发布z轴速度2.0，让uav0垂直上升。三架飞机分别用不同命名空间发布，就能同时起飞。但注意：直接用rostopic pub起飞只能用来验证话题通不通，不能作为编队任务的正式手段。原因是它没有高度反馈，飞机升到预定高度后不会悬停，会一直加速上升直到模型飞出物理世界。

正式的做法是写一个带闭环的小节点：读位姿、算期望、发指令。第4章会给出一个可控的编队控制节点实现。这里先记住一个原则：任何多机操作都要走“读位姿→算期望→发指令”的闭环，而不是裸发速度，否则你根本不知道飞机在哪个状态。

4. 编队控制器参数落地：从队形偏移到增益的完整配置

4.1 领航-跟随者的控制公式与节点职责

领航-跟随者实现起来很直观，但要把参数调到队形稳定，得先搞清楚控制量之间的关系。设领航机位置为p_leader，跟随者i的期望偏移是offset_i，则期望位置为：

p_i_des = p_leader + R(θ_leader) * offset_i

其中θ_leader是领航机朝向角，offset_i是机体坐标系下的偏移。多数编队仿真只做二维编队，高度单独控制，所以控制器只处理x、y两个方向，算出的期望位置再经过P控制器生成cmd_vel的线速度。

如果队形定义在地球坐标系下，公式里的R(θ_leader)可以去掉，直接用p_leader加偏移，实现更简单，但领航机旋转时队形不会跟着转，这要看你的场景需求。编队仿真包里常见的默认设定是固定偏移，也就是队形不随领航机朝向旋转，适合验证最基本的编队保持；带旋转矩阵的版本适合做队形变换和转弯协同。

4.2 编队参数文件：队形、间距、增益怎么调

大多数工程的参数都集中在yaml文件里，这是调参的主战场。一个典型的formation_params.yaml如下：

formation: type: "leader_follower" leader_topic: "/uav0/ground_truth/pose" height: 1.0 offsets: - [0.0, 0.0] # uav0：领航机，偏移为0 - [2.0, 0.0] # uav1：领航机右方2米 - [1.0, 1.732] # uav2：与uav1构成等边三角形 kp_xy: 1.5 kp_z: 1.2 max_vel: 2.0 publish_rate: 50

参数含义如下表：

调参有个基本原则：先调高度，再调水平。因为高度方向的响应最直观，z方向稳不住，水平方向再准也没意义。把kp_z调到飞机能平稳悬停在目标高度后，再去动kp_xy。改增益时每次只改一个参数，记录下当前值，别一次动三个，否则出现问题你根本不知道是哪个参数引起的。

4.3 用Python写一个最小编队控制节点

假设要控制uav1跟随uav0，最简单的一个节点长这样：

#!/usr/bin/env python3 import rospy from geometry_msgs.msg import PoseStamped, Twist class FollowerController: def __init__(self, ns, offset, leader_topic): self.ns = ns self.offset = offset # 期望相对领航机的偏移 [dx, dy] self.leader_sub = rospy.Subscriber(leader_topic, PoseStamped, self.leader_cb) self.pose_sub = rospy.Subscriber(ns + "/ground_truth/pose", PoseStamped, self.pose_cb) self.cmd_pub = rospy.Publisher(ns + "/cmd_vel", Twist, queue_size=1) self.leader_pose = None self.my_pose = None def leader_cb(self, msg): self.leader_pose = msg.pose def pose_cb(self, msg): self.my_pose = msg.pose if self.leader_pose is None: return # 期望位置 = 领航位置 + 固定偏移 des_x = self.leader_pose.position.x + self.offset[0] des_y = self.leader_pose.position.y + self.offset[1] # P控制：误差乘以增益得到速度 err_x = des_x - self.my_pose.position.x err_y = des_y - self.my_pose.position.y kp = 1.5 cmd = Twist() cmd.linear.x = kp * err_x cmd.linear.y = kp * err_y cmd.linear.z = 1.2 * (1.2 - self.my_pose.position.z) self.cmd_pub.publish(cmd) if __name__ == "__main__": rospy.init_node("follower_controller") ns = rospy.get_param("~namespace", "uav1") offset = rospy.get_param("~offset", [2.0, 0.0]) leader_topic = rospy.get_param("~leader_topic") rospy.loginfo("follower %s started with offset %s", ns, offset) FollowerController(ns, offset, leader_topic) rospy.spin()

这段代码的核心是绝对位置的P控制：期望位置减去当前位置得到误差，乘以增益kp输出速度。因为代码里所有话题都用参数拼出来的相对话题名，同一份代码可以启动多份，分别对应uav1、uav2。启动时通过launch传不同的namespace和offset，三架飞机就各自干各自的活，互不干扰。

注意这个节点没有对速度做限幅，编队控制器一个常见翻车点就是误差太大时输出一个大速度指令，飞机直接飞没影。正式使用前要加上速度限幅逻辑，或者直接用max_vel参数对输出做限制。另外，P控制会有静态误差，领航机匀速运动时跟随者会滞后一个固定距离，这是P控制器的固有特性。想消除滞后可以加速前馈项，把领航机的速度叠加到输出上，这个优化留给有需要的场景。

4.4 三机编队的launch组装与启动顺序

把三个控制器实例装进launch，最稳妥的方式是在编队launch里为每个控制器节点单独传参：

<node name="follower_uav1" pkg="my_formation" type="follower_controller.py" output="screen"> <param name="namespace" value="uav1"/> <param name="offset" value="[2.0, 0.0]"/> <param name="leader_topic" value="/uav0/ground_truth/pose"/> <remap from="cmd_vel" to="cmd_vel"/> </node>

这里的核心是用param把命名空间显式传给节点，而不是依赖相对命名空间自动解析。原因是Python代码里roslib的命名空间自动解析有时会因为节点启动顺序导致话题名错位，显式传入后问题就消失了。启动顺序上，先启动仿真环境，等待模型加载，再启动控制器，最后启动任务节点。等模型加载好再启动控制器，能在日志里少看到好几条服务等待报错。

5. 多无人机编队仿真排查：四个高频翻车点与解决方案

5.1 现象：起飞后机体重叠或剧烈漂移

第一次三机起飞时，经常看到两架飞机从同一个坐标冒出来，或者某架飞机不受控制地侧滑。先别急着怀疑代码，大概率是spawn坐标设置重复或者模型link名冲突。我用过的项目里，坐标重叠是最高频的错误，因为很多模板默认把飞机放在原点，复制三份时忘了改x、y参数。

原因有两类：一是launch里的x、y重复，二是模型xacro里link名固定导致TF互相覆盖。区分方法是看rostopic echo各机的位姿数据——如果数据坐标各不相同但画面重叠，说明是加速了物理模型原因；如果数据只有一架飞机在更新，其余话题根本没动，说明是模型加载失败。

解决：先grep spawn参数确认坐标不重复，再用rostopic echo -n 3 /uavN/ground_truth/pose查看各机位置是否独立更新。如果坐标正确但视觉上重叠，多半是物理碰撞没生效，Gazebo默认在某些情况下不会让模型互相排开，需要在世界文件里开启碰撞检测。调试时建议临时把三机间距设到4米以上，从物理隔离开始排查，逐步缩小间距。

5.2 现象：所有无人机都在飞但只有一架响应指令

三架飞机都悬停在空中，rostopic pub给uav1/cmd_vel没反应，却发现/cmd_vel在反复被发布。这个问题我遇到过好几次，症状是话题列表里没有带前缀的cmd_vel，所有控制话题都挤在全局命名空间。

原因几乎都是代码里用了绝对话题名。比如在Python里写rospy.Publisher("/cmd_vel")，开头的/会绕过所有命名空间，group ns对这段代码完全失效。为此，三架飞机的控制器会订阅同一个全局cmd_vel，只有最后启动的那架抢到了话题，其他自然没反应。当然还是存在期望的命令实现差异。

解决：把代码里的话题名统一改成相对路径，也就是去掉开头的/，写成cmd_vel。在ROS里，相对话题名会自动拼接上当前节点的命名空间，例如节点在uav1/下，cmd_vel就会解析为/uav1/cmd_vel。检查方法是rostopic list | grep cmd_vel，如果看到一个不带前缀的主题名，按这个思路去找代码中的绝对路径。

5.3 现象：仿真时间与系统时间错乱，队形误差发飘

控制器算出来的队形误差不断增大，画面里飞机却一直稳稳悬停，误差曲线和实际表现对不上。多数时候不是控制算法问题，而是时间源不一致。Gazebo默认使用仿真时间，而ROS节点默认使用系统时间，如果launch里没有显式开启use_sim_time，话题时间戳就会出现跳变，累积误差会让控制器误判状态。

还有一个隐蔽场景：笔记本电脑在编译项目时分发热量大，系统时间被NTP校正，时间戳突然回退几十毫秒，编队控制器就会误以为位置突变，输出一个很大的速度指令。这个现象在真机数传链路里也存在，仿真里更容易忽略。

解决：在launch文件里加一行use_sim_time true，让所有节点统一用仿真时间：

<param name="/use_sim_time" value="true"/>

同时，代码里不要用time.time()这类系统时间函数去算控制周期，统一用rospy.Time.now()。验证方法是在控制器节点日志里看时间戳，如果相邻两帧时间差稳定在0.02秒（对应50Hz），说明时间源已经统一。

5.4 现象：控制器收不到位姿，话题有发布但无订阅

编队控制器启动后日志一直打“waiting for pose”，rostopic hz /uav0/ground_truth/pose显示10Hz在发布，但控制器就是拿不到数据。这个问题通常是节点启动顺序导致的服务抖动，或者代码里的订阅回调没触发。

原因多半是模型还没加载完，控制器就启动了，订阅器在Gazebo插件发布位姿前就订阅了，之后插件启动时话题重新注册，订阅连接断开。轻量仿真的插件在节点启动后可能需要几秒才完成模型装配，这段时间里话题虽然存在，但数据是空的。

解决：在launch里给控制器节点加上respawn，或者用roslaunch的延迟机制等待模型加载；更稳的方案是控制器代码里加一个就绪判断——先等待subscription.getNumPublishers()大于0再进入控制循环。我一般在编队工程里都会在task_manager里实现ready握手，所有节点报告就绪后才开始任务，这样能把启动顺序问题一次解决掉。另外检查队列长度，把Subscriber的queue_size从默认值调大到10，避免高频位姿被丢弃。

6. 仿真验收一步到位：用rosbag回放评估编队误差

6.1 用rosbag记录三机轨迹，别用眼睛判断

编队调完参，不要靠肉眼判断队形是否保持。Gazebo画面里看起来差距不大的两架飞机，误差可能已经超过半米。我的做法是跑任务的同时用rosbag记录三架飞机的位姿，结束后离线回放，再脚本计算队形误差，用数据说话。

记录命令很简单：

rosbag record -O formation_test.bag \ /uav0/ground_truth/pose \ /uav1/ground_truth/pose \ /uav2/ground_truth/pose

跑完任务后Ctrl+C结束记录，再用rosbag info确认数据完整性。注意bag文件不要录全量话题，只录编队控制相关的那几个，否则一顿饭的功夫能录出好几个GB，后续处理也受罪。回放时如果只想算误差不想再打开Gazebo，可以直接用rostopic echo从bag里抽数据，配合Python脚本分析。

6.2 用Python脚本计算队形误差并输出曲线

假设队形是等边三角形，期望偏移分别是[0,0]、[2.0,0]、[1.0,1.732]，计算uav1相对uav0的实际偏移与期望值的差：

import rosbag import numpy as np bag = rosbag.Bag("formation_test.bag") p0 = None errors = [] for topic, msg, t in bag.read_messages(topics=["/uav0/ground_truth/pose", "/uav1/ground_truth/pose"]): if topic == "/uav0/ground_truth/pose": p0 = msg.pose.position # 领航机位置 elif topic == "/uav1/ground_truth/pose" and p0 is not None: dx = msg.pose.position.x - p0.x - 2.0 # 减去期望偏移 dy = msg.pose.position.y - p0.y - 0.0 err = np.sqrt(dx*dx + dy*dy) errors.append((t.to_sec(), err)) for t, err in errors[::50]: # 每50帧取一个点，避免输出过多 print(f"{t:.2f}s 误差 {err:.3f}m")

这个脚本的核心逻辑非常简单：领航机位置加上期望偏移得到期望位置，实际相对位置减去期望位置就是误差。误差在多长时间内收敛到0.1米以内，收敛后有没有周期性振荡，这两个指标直接决定编队参数是否合格。如果误差一直收敛不了，回看第4章的kp_xy和max_vel参数；如果出现振荡，说明增益偏大。

6.3 一个被低估的调参技巧：先慢后快，再用热调参

编队控制调参有个血泪经验：拿到一个新仿真包，先把max_vel降到0.5，所有增益减半跑一遍，确认队形能收敛，再把速度逐步提上去。很多人一上来就按照论文参数填进去，结果飞机乱飞，根本分不清是控制器问题还是参数问题。先慢后快能有效隔离问题来源。

等参数基本稳定后，再用rqt_reconfigure做在线热调，不用每次改yaml都重启仿真，省下的时间非常可观。我的习惯是：先把参数跑出可接受的误差范围，再用bag验证误差曲线，确认没有振荡后再固化到yaml里。这套流程帮我少走了很多弯路，也避免了多次重新仿真等待。希望帮到你。

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