ROS+Gazebo五台差速小车领航跟随编队仿真实战

简介：这份开源机器人集群项目KKSwarm由易科机器人实验室与阿木实验室联合打造，面向机器人集群研究者、高校师生与开发者，聚焦多机器人协同通信、导航、编队和任务分配等实验场景，可作为集群算法验证、课程实验与二次开发的工程底座。资源包共343个文件，约177.57MB，以124个C++头文件、82个C++源文件为核心，配合19个Simulink模型，以及xml、msg、yaml、launch等配置与消息定义，另有Python脚本、PDF和Markdown文档，覆盖代码实现、仿真建模、部署配置与使用说明等环节。已有82人学习，适合希望快速了解项目结构与技术路线的读者。其价值在于提供较完整的集群项目源码、仿真模型和配置样例，读者可据此搭建实验环境、梳理模块关系、复现协同控制流程，并在通信、编队与任务分配等方向开展算法验证或二次开发，降低从零搭建框架的成本。

1. 五台小车在仿真里像一串糖葫芦拐弯，这项目就是干这个的

第一次看到机器人集群的演示，是五台差速小车在 Gazebo 里绕着一个障碍物走出 S 形：头车加速、后车跟紧，整条队列像一串糖葫芦一样同步拐弯。这个开源机器人集群项目，就是把这个场景拆成一套能自己跑起来的工程——多机器人模型、领航-跟随编队、相对定位、速度分配、仿真与实机共用一套控制接口，全部摊在 ROS 工作空间里。它适合两类人：一类是刚学完 ROS 基础、想找一套能跑起来的多机工程练手的；另一类是要在原型阶段验证编队算法、但不想从零搭多机环境的。整套东西不是玩具 demo，单个机器人的底盘控制、多机命名空间隔离、仿真时间同步这些细节都做全了，不然五台车根本不可能待在同一个世界里而不互相干扰。

2. 架构与技术栈：一套 ROS 工作空间怎么管住五台机器人

拿到这类项目，我第一个看的往往不是控制器算法，而是包结构和话题的命名方式。集群项目里最容易出问题的点，八成集中在多机命名空间没有完全隔离开：五台车都订阅了同一个/cmd_vel，结果一动就全都往东走。这个项目在架构上把这层隔离做得很干净，整体是标准的 ROS1 工作空间加 Gazebo 仿真，通信靠话题，状态靠 TF，控制回路按 50 Hz 定时器跑。

2.1 目录分层：把描述、控制、仿真、启动拆开

工作空间里的包这样划分，是我比较认可的拆法，职责边界清楚，二次开发时不容易改一个文件动全身：

swarm_ws/src/ ├── swarm_msgs/ # 自定义消息：编队状态、相对位姿、拓扑配置 ├── swarm_description/ # xacro 机器人模型、Gazebo 差速插件、传感器 ├── swarm_bringup/ # launch 文件、参数 yaml、拓扑配置 ├── swarm_control/ # 领航者节点、跟随者节点、底层速度转换 ├── swarm_sim/ # 仿真世界、多机器人 spawn 脚本 └── swarm_tools/ # 轨迹录制、编队误差统计脚本

描述包只负责“车长什么样”，控制包只负责“车该怎么走”，仿真包只负责“世界和出生点”，启动包负责把前面几样拼起来。这种分法的好处是上真机时只换swarm_sim和描述里的硬件接口，swarm_control里的控制器代码基本不用动。

机器人模型用 xacro 写成宏，每个实例传入前缀，所有 link、joint、frame 都带上这个前缀。这是多机 TF 不冲突的根本：

<!-- swarm_description/urdf/robot.xacro --> <xacro:macro name="swarm_robot" params="prefix"> <!-- 每个 link 名都带前缀，否则五台车会抢同一个 base_link --> <link name="${prefix}base_link"> <visual><geometry><box size="0.20 0.16 0.08"/></geometry></visual> <collision><geometry><box size="0.20 0.16 0.08"/></geometry></collision> <inertial> <mass value="1.2"/> <inertia ixx="0.001" iyy="0.002" izz="0.002" ixy="0" ixz="0" iyz="0"/> </inertial> </link> <joint name="${prefix}left_wheel_joint" type="continuous"> <parent link="${prefix}base_link"/> <child link="${prefix}left_wheel_link"/> <axis xyz="0 1 0"/> </joint> <!-- Gazebo 差速插件也挂前缀话题，这是隔离的关键一环 --> <gazebo> <plugin name="${prefix}diff_drive" filename="libgazebo_ros_diff_drive.so"> <leftJoint>${prefix}left_wheel_joint</leftJoint> <rightJoint>${prefix}right_wheel_joint</rightJoint> <wheelSeparation>0.16</wheelSeparation> <wheelDiameter>0.065</wheelDiameter> <commandTopic>${prefix}cmd_vel</commandTopic> <odometryTopic>${prefix}odom</odometryTopic> <odometryFrame>${prefix}odom</odometryFrame> <publishOdomTF>true</publishOdomTF> </plugin> </gazebo> </xacro:macro>

参数说明几个点：wheelSeparation和wheelDiameter必须和 URDF 里视觉模型的尺寸对得上，对不上就会出现“仿真里走得挺准，录出来的里程计一拐弯就偏”的情况；odometryFrame加前缀是必须的，否则所有车都往全局 tf 里塞同一个odomframe，后发布的消息会直接覆盖前面的。

2.2 通信拓扑：多机命名空间与话题设计

话题按机器人编号分命名空间，全局只保留编队层面的状态量。这是这个项目里我最先抄的一段设计：

全局 TF 树是一个需要提前想清楚的选择。常见做法是全局一棵 TF 树，所有 frame 带机器人前缀；另一种是每台车各起一个robot_state_publisher加一个独立 tf 树，再用网关合并。这个项目走的是前者，好处是跟随者节点一次lookupTransform("world", "robot_2_base_link", ...)就能拿到世界坐标系下的位姿，不用自己做坐标串联。

有一点要注意：/tf是全局的，但每台车的robot_state_publisher只能发布自己那棵子树。如果 launch 里图省事只起了一个robot_state_publisher，那另外四台车的轮子关节就不会动，视觉上看不出问题，但一想接robot_state_publisher做正运动学就会出问题。

2.3 控制闭环：从 cmd_vel 到轮速再到里程计

仿真里 Gazebo 插件替你把cmd_vel转成轮速，但真机上这层得自己写，所以项目里保留了一份参考实现。差速模型本身不复杂，容易错的是符号：

// swarm_control/src/diff_drive.cpp // 把上层 cmd_vel 拆成左右轮的目标角速度（rad/s） void diffDrive(const geometry_msgs::Twist& cmd, double& wl, double& wr) { const double v = cmd.linear.x; // 车体线速度 m/s const double w = cmd.angular.z; // 车体角速度 rad/s const double R = wheel_radius_; // 轮半径，0.0325 m const double L = wheel_separation_; // 轮距，0.16 m // 差速模型：左轮线速度 = v - L/2*w，右轮 = v + L/2*w wl = (v - 0.5 * L * w) / R; wr = (v + 0.5 * L * w) / R; // 限幅，防止上层给了过大的速度把电机指令打饱和 wl = std::clamp(wl, -max_wheel_w_, max_wheel_w_); wr = std::clamp(wr, -max_wheel_w_, max_wheel_w_); }

里程计回收则是对轮速做积分。这段代码是踩坑重灾区，尤其角度归一化：

// 用左右轮累计转角积分出位姿，发布 odom -> base_link void publishOdom(const ros::Time& now) { const double dt = (now - last_time_).toSec(); if (dt <= 0.0) return; // 仿真暂停时 dt 会是 0，跳过 const double dl = (wheel_angle_[0] - last_angle_[0]) * wheel_radius_; const double dr = (wheel_angle_[1] - last_angle_[1]) * wheel_radius_; const double ds = 0.5 * (dl + dr); // 车体位移 const double dth = (dr - dl) / wheel_separation_; // 车体转角 x_ += ds * std::cos(theta_ + 0.5 * dth); // 中点积分，比直接 cos(theta_) 准 y_ += ds * std::sin(theta_ + 0.5 * dth); theta_ += dth; theta_ = std::atan2(std::sin(theta_), std::cos(theta_)); // 归一化到 [-pi, pi] // ... 填充 nav_msgs::Odometry 并 publish last_angle_[0] = wheel_angle_[0]; last_angle_[1] = wheel_angle_[1]; last_time_ = now; }

参数和逻辑说明：wheel_angle_来自编码器或仿真插件的关节状态；last_time_要用消息时间戳而不是ros::Time::now()，否则仿真时间和你本地时间对不上。中点积分这一步很多人会省，小车低速直行时看不出差别，一旦频繁转向，累积误差会差出一大截。

3. 环境搭建与首跑：在仿真里让五台车排成一列

这套工程对系统版本比较敏感，ROS 版本和 Gazebo 版本错一个，gazebo_ros插件就可能加载不上，或者加载上了但不发话题。我一般按下面的顺序来，能少走不少弯路。

3.1 依赖与编译：Ubuntu 与 ROS 版本对齐

仿真类项目建议锁定 Ubuntu 20.04 + ROS Noetic + Gazebo 11 这个组合，插件和消息都能直接对上：

# 基础工具与 ROS 桌面版，仿真必备的几个包单独装 sudo apt update sudo apt install -y ros-noetic-desktop-full \ ros-noetic-gazebo-ros-pkgs \ ros-noetic-gazebo-ros-control \ ros-noetic-teleop-twist-keyboard \ ros-noetic-tf2-tools # 编译，仿真控制对实时性有要求，Release 比默认的 Debug 稳 cd ~/swarm_ws catkin_make -DCMAKE_BUILD_TYPE=Release source devel/setup.bash

说明几点：-DCMAKE_BUILD_TYPE=Release不是可有可无的，Debug 下控制节点跑不满 50 Hz，五台车一起动时会明显卡顿；ros-noetic-tf2-tools装上，后面排查 TF 问题用tf2_echo会方便很多。编译完先rospack find swarm_control确认路径能被解析到，别急着 launch。

3.2 多机 spawn：一个 launch 拉起五台车

多机启动的 launch 文件是整套工程里最值得反复读的一处，因为它同时决定了话题隔离和出生位姿两件事：

<launch> <!-- 全局仿真时钟，缺了它 TF 必然报 extrapolation --> <param name="/use_sim_time" value="true"/> <include file="$(find gazebo_ros)/launch/empty_world.launch"> <arg name="world_name" value="$(find swarm_sim)/worlds/swarm_empty.world"/> <arg name="paused" value="false"/> </include> <!-- 逐台拉起，ns 前缀和初始位姿一一对应 --> <group ns="robot_1"> <param name="robot_description" command="$(find xacro)/xacro '$(find swarm_description)/urdf/robot.xacro' prefix:=robot_1_"/> <node name="spawn" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot_1 -x 0 -y 0 -z 0.06"/> <node name="state_pub" pkg="robot_state_publisher" type="robot_state_publisher"/> </group> <!-- robot_2 ~ robot_5 结构相同，x/y/z 按队形错开 --> <group ns="robot_2"> <param name="robot_description" command="$(find xacro)/xacro '$(find swarm_description)/urdf/robot.xacro' prefix:=robot_2_"/> <node name="spawn" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot_2 -x -0.8 -y 0 -z 0.06"/> <node name="state_pub" pkg="robot_state_publisher" type="robot_state_publisher"/> </group> </launch>

说明：-model后面的名字必须唯一，Gazebo 里模型名重复会被直接拒绝，而且错误信息只出现在 Gazebo 终端，容易漏看；-z 0.06是让车从略微悬空的位置落下来，给0.01会被判定为穿模弹飞。实际项目里通常不会手写五遍<group>，而是写个 Python 脚本按编号生成，或者用<include>配<arg>复用同一份模板——手写五遍的坏处是改一个参数要改五处。

跑起来之后，用一句命令验证隔离是否生效：

# 看话题有没有按机器人编号分开 rostopic list | grep cmd_vel # 期望输出：/robot_1/cmd_vel ... /robot_5/cmd_vel，而不是一个全局 /cmd_vel

3.3 参数梳理：yaml 里真正要改的键

参数全部集中在启动包里，改队形间距、改跟踪增益都只动一个文件：

# swarm_bringup/config/swarm.yaml swarm: robot_num: 5 formation: line # line / wedge / column 三种预置队形 leader_spacing: 0.8 # 队形间距，单位 m control: kp_linear: 0.8 # 位置误差 -> 线速度 kp_angular: 1.5 # 航向误差 -> 角速度 max_linear: 0.4 # m/s，上限压住，别让跟随者冲过头 max_angular: 1.2 # rad/s control_rate: 50.0 # Hz，跟 Gazebo 物理步长匹配

真正需要调的其实就三个：leader_spacing、kp_linear、max_linear。间距太大跟不住，太小容易追尾；kp_linear调大收敛快但会震荡，调小则像慢半拍；max_linear是我见过最容易忽略的一个——跟随者速度上限如果等于甚至大于领航者，转弯时后车一定会往前冲一截。

4. 核心模块拆解：一个领航-跟随编队的完整链路

编队控制这一层，项目走的是虚拟结构加领航-跟随的混合思路：领航者维护一条随时间推进的参考轨迹，跟随者按固定偏移量算出各自的期望位姿，再用比例控制把误差消掉。链路不长，但每一段都有讲究。

4.1 领航者轨迹：从参考线到带时间戳的位姿

领航者不直接发cmd_vel，而是发布一串带时间戳的目标位姿，跟着这条路走的好处是后面所有跟随者都只依赖位姿，不依赖领航者的控制细节：

// swarm_control/src/leader_node.cpp // 沿参考折线按弧长匀速推进，发布带时间戳的目标位姿 void LeaderNode::tick(const ros::TimerEvent& e) { // 用 current_real 而不是 now()，仿真暂停时不会把时间轴带偏 const double dt = (e.current_real - e.last_real).toSec(); s_ += cfg_.speed * dt; // 沿路径累计弧长 if (s_ > path_.totalLength()) s_ = path_.totalLength(); const auto pt = path_.sample(s_); // 按弧长插值出 (x, y, yaw) geometry_msgs::PoseStamped msg; msg.header.stamp = e.current_real; msg.header.frame_id = "world"; msg.pose.position.x = pt.x; msg.pose.position.y = pt.y; tf2::Quaternion q; q.setRPY(0.0, 0.0, pt.yaw); msg.pose.orientation = tf2::toMsg(q); leader_pub_.publish(msg); }

逻辑说明：这里用弧长s_推进而不是用绝对时间，改领航速度时整条队形的节奏会一起变，不会出现“头车快了、后车还在按老节奏走”的断层。e.current_real是ros::TimerEvent里带的真实触发时间，仿真里用它算 dt 比ros::Time::now()稳。

4.2 跟随者解算：相对位姿怎么变成速度指令

跟随者是整套工程里改得最多的一段。核心动作是把期望位姿和当前位姿的误差投影到车体坐标系，再分别喂给线速度和角速度：

// swarm_control/src/follower_node.cpp void FollowerNode::onLeader(const geometry_msgs::PoseStamped::ConstPtr& leader) { // 1. 期望位姿 = 领航者位姿 × 编队偏移（偏移来自 yaml，按 robot_id 查） tf2::Transform T_leader; tf2::fromMsg(leader->pose, T_leader); const auto T_offset = formation_.offsetOf(robot_id_); const auto T_desired = T_leader * T_offset; // 2. 当前位姿，仿真从 p3d 插件拿，真机换成 amcl 或里程计 geometry_msgs::TransformStamped cur_tf; try { cur_tf = tf_buffer_.lookupTransform("world", frame_id_, ros::Time(0), ros::Duration(0.1)); } catch (const tf2::TransformException& ex) { ROS_WARN_THROTTLE(1.0, "tf 还没准备好: %s", ex.what()); return; // 拿不到位姿就发零速度，别让上一条指令继续生效 } tf2::Transform T_cur; tf2::fromMsg(cur_tf.transform, T_cur); // 3. 误差投到车体坐标系，x 是前后、y 是左右、theta 是航向 const auto T_err = T_desired.inverse() * T_cur; const double ex = T_err.getOrigin().x(); const double ey = T_err.getOrigin().y(); const double eth = tf2::getYaw(T_err.getRotation()); // 4. 比例控制 + 限幅 geometry_msgs::Twist cmd; cmd.linear.x = clamp(kp_linear_ * ex, -max_linear_, max_linear_); cmd.angular.z = clamp(kp_angular_ * eth, -max_angular_, max_angular_); // 差速底盘没有横移能力，ey 只能靠转向慢慢消，这里用航向前馈补一点 cmd.angular.z += clamp(-0.5 * ey, -0.3, 0.3); cmd_pub_.publish(cmd); }

参数说明和几个关键点：lookupTransform的第二参用ros::Time(0)拿最新可用变换，能省掉时间同步的一部分麻烦；T_desired.inverse() * T_cur的顺序决定了误差是“期望坐标系下的当前位姿”，别写反，写反之后车会朝着反方向跑，现象很吓人但原因极简单；ey那一项是给差速底盘的补偿，全向底盘直接把它塞进linear.y更直接。所有速度都要限幅，上层任何一处算错都不至于让车飞出去。

4.3 编队误差评估：别只看车有没有撞上

仿真里五台车跑得整整齐齐，不代表控制效果好——可能是间距设得大、速度设得慢掩盖了问题。项目里带了个离线统计脚本，从 rosbag 回放里算误差：

# swarm_tools/scripts/eval_formation.py # 从录好的 bag 里回放 /tf，统计每台跟随车相对期望位姿的误差 import rosbag import tf.transformations as tft errors = [] with rosbag.Bag('swarm_run.bag') as bag: for topic, msg, _ in bag.read_messages(topics=['/tf']): for tr in msg.transforms: # 只挑 robot_N 到 world 的变换，按 frame 名筛 if tr.child_frame_id.startswith('robot_') and tr.header.frame_id == 'world': errors.append(tr.transform.translation.x) # 示例：取 x 方向偏差 errors.sort() n = len(errors) if n: print('样本 %d, 均值 %.3f m, p95 %.3f m, 超调 %.3f m' % (n, sum(errors) / n, errors[int(0.95 * n)], errors[-1]))

这个脚本本身很简单，价值在于它把“看起来还行”变成了可比较的数字。同一组队形参数，均值误差差 20%、p95 差一倍，肉眼基本看不出来，但在真机上就是一次追尾和一次顺利通过的区别。

5. 避坑与排查：多机仿真里最容易翻车的五件事

这套工程跑通不难，难的是跑稳。下面几条是我在同类项目上反复踩过的，按现象、原因、解决分三段写清楚。

一、发一条速度指令，五台车一起动

现象：rostopic pub /cmd_vel之后，五台车同时响应。原因：话题没加命名空间，Gazebo 差速插件的commandTopic被写成了全局/cmd_vel，或者 launch 里没用<group ns>包住。解决：插件的commandTopic写成${prefix}cmd_vel，节点里用ros::NodeHandle("~")加ros::this_node::getNamespace()拼话题名；验证时rostopic info /robot_2/cmd_vel看订阅者是不是只有一台。

二、TF 报 “Lookup would require extrapolation into the future”

现象：跟随者节点一启动就刷警告，车原地不动。原因：/use_sim_time没设成true，或者设了但节点比 Gazebo 先起来，/clock还没开始发。解决：launch 第一行就设/use_sim_time；节点里lookupTransform前先用ros::Time::now()判断时钟是否已经推进，拿不到就跳过这一帧，别硬发指令。

三、车走起来抖，位置越跑越偏

现象：直行还好，一转弯就开始抖，久了位置明显漂。原因有两个：控制回调和物理步长不匹配，PID 的 dt 用了固定值而不是实际间隔；或者 world 里max_step_size太大，默认 0.001 是合适的，有人为了跑快改成 0.01 就会抖。解决：控制走 50 Hz 定时器，dt 用current_real - last_real；world 文件里把max_step_size压回 0.001，real_time_update_rate保持 1000。

四、五台车 spawn 出来叠在一起，或者只有一台出现

现象：spawn_model报错或者被吞掉，场景里只有一台车。原因：-model后面的名字重复，Gazebo 拒绝同名模型；或者 spawn 的 z 太小陷进地面；再或者五台车在同一时刻 spawn，仿真还没稳定。解决：model 名带前缀，z 给 0.06~0.1，spawn 之间加个延时（launch 里用<arg>控制启动顺序，或者用 Python 脚本串行调用 spawn 服务）。

五、改了 yaml，参数就是不生效

现象：swarm.yaml里把max_linear改成 0.2，车跑起来还是原来的速度。原因：launch 里既用了<param>又用了<rosparam>，加载顺序把 yaml 覆盖掉了；或者节点用的是私有 NodeHandle 读~max_linear，而 yaml 挂在全局命名空间下。解决：统一用<rosparam file="$(find swarm_bringup)/config/swarm.yaml" command="load"/>挂到节点命名空间下，节点统一用ros::NodeHandle("~")读；改完启动后立刻rosparam get /robot_1/control/max_linear确认实际生效值。

排查这类问题，我一般先跑三条命令定位：rosnode info /xxx_node看订阅发布对不对，rostopic hz /robot_1/odom看频率有没有掉，tf2_echo world robot_1_base_link看坐标变换有没有断。三条都过了再去看代码，能省掉一半的无用功。

6. 二次开发：从改队形到上真机，只动这三处

这套工程最容易上手的地方，是它把“编队怎么定义”和“车怎么走”拆开了。想换队形不用改控制器代码，改配置就行；想把比例控制换成 PID，控制器接口不动，换实现即可；想上真机，抽一层硬件接口就够。

先说换队形。原来固定的是领航者加固定偏移，改成图拓扑只改一个 yaml：

# swarm_bringup/config/topology.yaml # 有向图：key 是跟随者，value 是它参考的上游，空表示直接跟领航者 topology: robot_2: robot_1 robot_3: robot_1 robot_4: robot_2 robot_5: robot_3

跟随者节点按拓扑文件查上游，用上游位姿乘相对偏移得出自己的期望位姿，这一段从“单领航”变成“多级跟随”，控制器的其余部分一行不用动。再往下走就是一致性算法，把偏移约束换成相对位置约束，那是另一个话题了。

再说换控制器，改动集中在两个文件：把onLeader里的比例项换成带积分和微分的实现，积分项记得限幅，不然低速时积分饱和会让车慢慢往前蹭。验证方法固定下来：改完参数先录 30 秒 bag，跑eval_formation.py看均值和 p95 两个数，和改之前对比，数字变好才算数。

最后说上真机。仿真和真机的分水岭在“位姿从哪来”：仿真里从 Gazebo 插件拿，真机得从里程计或激光定位拿。我的做法是抽一个PoseProvider接口，仿真实现订阅插件话题，真机实现订阅amcl或轮式里程计，控制器只认接口不认来源。这样上真机时唯一要改的就是配置文件里换一个 provider 名字，控制器代码不用编译第二次。

从那以后，我每次改完控制器参数，都先把 bag 录下来回放跑一遍误差脚本再上真机——五台车在场地里追尾，收拾起来比在仿真里查半天麻烦太多了。希望帮到你。

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