多差速无人车编队控制：ROS/C++源码实现与调参实战

简介：面向ROS开发者与机器人编队研究者的完整项目源码包，以C++实现多差速无人车编队控制，覆盖Gazebo仿真与Rviz可视化场景。资源共30个文件，压缩包约51KB，包含4个launch启动脚本、9个xacro机器人模型文件、4个C++源文件与头文件，以及rviz配置、yaml参数、世界地图和README说明，目录结构清晰，便于按模块学习与二次开发。项目已有437人学习下载。这套代码源自作者本科毕业设计，程序经过运行验证，并附带详细注释与使用说明；针对多机器人仿真环境、差速底盘模型和NMPC编队控制器均有实现，可帮助读者理解从运动建模、控制器设计到Gazebo仿真部署的完整链路，适合自动化、计算机、人工智能等专业学生用于毕设、课程设计或初期项目演示，也可作为后续扩展编队算法的基础。

1. 多差速无人车编队控制：这套C++/ROS源码解决的是什么事

一套用 C++ 写在 ROS 里的多差速无人车编队控制源码，拿到手先别急着编译，先想清楚它替你省了什么：仓库里两辆差速底盘要排队进出库，比赛里三台小车要顶着三角队形绕圈，或者你只是要在 Gazebo 里验证多车协同——编队控制就是把“几辆车之间相对位置别散掉”这件事，从手工遥控变成底盘自己实时修正。这套源码按领航者-跟随者思路组织，把差速模型解算、编队控制律、话题通信三层拆开并加了详细注释，适合已经能单机跑通 /cmd_vel 和 /odom、却还不清楚怎么让多辆车联动起来的 ROS 开发者。后面展开的三件事最重要：坐标系怎么统一、控制频率怎么保证、速度限幅怎么做。

2. 差速模型与编队控制律：先把“跟随”这件事算清楚

编队控制和单机导航最大的区别，是它要把“队友在哪”写进控制律。多差速无人车是典型的非完整约束系统：车身不能横移，只能朝当前朝向走，所以任何编队算法落地前，都得先把差速底盘的位姿表达和速度解算写对。这一章把从轮速到位姿、从位姿到相对误差、从误差到速度指令的完整链路拆一遍，后面读源码时你会发现每条注释都在对应其中一步。

2.1 差速底盘的位姿表示与速度解算：从轮速到 v 和 ω

差速底盘有两个独立驱动轮，外加一个或两个万向支撑轮。它的位姿用三个量描述就够了：x、y 和车头朝向 θ。底盘不能横向滑动的约束，决定了瞬时速度只有两个自由度：前进线速度 v，以及绕车身中心的角速度 ω。把左右轮速度换算成 v、ω，是这套源码里最基础的一段 C++：

// 差速底盘运动学解算：由左右轮线速度推出车体速度 double wheel_base = 0.30; // 轮距，单位 m，按车架实测 double v = 0.5 * (wheel_v_right + wheel_v_left); double w = (wheel_v_right - wheel_v_left) / wheel_base;

wheel_v_left 和 wheel_v_right 都是线速度，单位 m/s；wheel_base 是左右轮轮心距，这一点要在注释里标清楚。很多底盘手册给的是轮距外沿，直接套公式会让 ω 偏小约一个轮宽，实车转弯半径变大，编队过弯时整体外偏。仿真里这两行可以直接用；真车上它前面还隔着一层“编码器脉冲 → 轮线速度”的换算，底盘驱动节点通常已经替你做过，这段代码负责的是反过来：从轮速积分为位姿。

里程计积分是下一段：

// 在世界系下的位姿递推，dt 为里程计消息间隔 x += v * std::cos(theta) * dt; y += v * std::sin(theta) * dt; theta += w * dt;

注意 theta 这行没有做角度规整。车辆转了几圈后，theta 会超过 ±π，虽然 cos/sin 精度不受影响，但如果你拿它直接算航向差，会在 π 附近出现瞬间跳变，编队会突然抽动一下。所以源码里通常有一个 normalizedAngle 函数把角度规整到 [-π, π]，或者用 tf2 的 getAngle 做同样的事。这一步非常关键，也是很多人第一次跑编队觉得“算法有 bug”的隐藏来源。

2.2 领航-跟随者编队策略与坐标系变换：把“队形”写成数学约束

多差速车编队最常用的拓扑是领航者-跟随者：第一辆车是 leader，它有独立的速度指令源（导航节点或键盘）；其余车是 follower，它们不直接接任务速度，而是把“跟前车保持某个固定相对位姿”当成控制目标。这个描述落到源码里就是坐标系变换：以 leader 车头朝向为基准，把期望的相对位置偏移写成编队约束。

先解释为什么必须有这步旋转：两辆车都在世界系（odom/map）下有坐标，如果直接算世界系下的坐标差，leader 一转弯，follower 的期望点就偏了。正确做法是先让 leader 位姿乘上编队偏移，得到 follower 的期望世界坐标：

double dx = 0.8, dy = 0.0; // 期望相对位置：纵队，前车正后方 0.8m double x_des = leader_x + dx * cos(leader_yaw) - dy * sin(leader_yaw); double y_des = leader_y + dx * sin(leader_yaw) + dy * cos(leader_yaw); double theta_des = leader_yaw;

dy 不为零时，期望点在 leader 侧后方，排出来的就是斜队。把这段看懂，队形就能随意改了：纵队 dy=0，三角队形对后车分别设 dy=±0.6。工程上没有必要每个人都手写旋转矩阵，常见做法是直接用 tf2 查两车相对位姿：lookupTransform("leader_base_link", "follower_base_link") 返回的 transform 就是这句话的结果。我一般建议先手写一遍这个旋转，再去用 tf2，因为 tf 树的 frame_id 一旦写错，起源根本定位不准；手写过一遍，定位错误会快很多。

这一步最容易出 bug 的是符号：期望偏移是在 leader 系下定义的，而误差必须在 follower 自身坐标系下分解，才能直接喂给控制律。两步之间的坐标系标注——“到底哪一步在 leader 系、哪一步在 world 系”——是这套源码注释里最该用力的地方。两车纵队跑起来后，如果 follower 在直线段左右来回摆，优先查的就是这一步的符号。

2.3 控制律设计与参数：比例通道怎么设不震荡

拿到相对误差后，控制目标变成三件事：follower 在前进方向上追上期望距离，在侧向上压掉横向偏差，在航向上跟上 leader 的朝向。对差速底盘，通常分两个通道——线速度管纵向，角速度管航向和横向的耦合修正：

double v_cmd = v_leader + kx * ex; // 纵向通道：追距离 double w_cmd = kth * eth + ky * ey; // 角速度通道：航向 + 横向 v_cmd = clamp(v_cmd, -v_max, v_max); // 限幅必须放在发布之前 w_cmd = clamp(w_cmd, -w_max, w_max);

v_leader 是前车当前线速度，从 /cmd_vel 或 /odom 差分得到；ex 是距离误差；ey 是横向偏移；eth 是航向误差。w_cmd 同时含 eth 和 ey 两项，因为差速车要同时消除航向差和横向偏移，只用一个通道做不到。

参数初值可以参考下表，再按实车手感微调：

这三个增益量纲不同，kx 是 1/s，ky 和 kth 混合了角度与长度，别用同一个基准去对比，直接按现象调。

提示：调参顺序应该是先提控制频率，再压限幅，最后动增益。频率不够时动增益，很容易越过临界阻尼变成持续振荡；限幅不设，实车上碰撞风险会被几何放大。

另外注意：差速底盘是欠驱动系统——三个位姿量，两个控制量，想同时把 ex、ey、eth 严格压到零做不到。判断编队收敛的标准，是剩余误差在一个轮径（约 0.1m）以内，且弯道横向偏差不超过 0.3m。这也是这份源码注释里通常会写明的验收口径。

3. 源码工程结构：C++ 节点怎么组织、消息怎么流

拿到一套带使用说明和详细注释的编队源码，先别急着从头逐行读。正确顺序是：先看目录结构，再看话题表，然后看 CMakeLists 和 package.xml 确认能编译，最后沿着主循环把代码过一遍。这一章就是这条阅读路线的地图。

3.1 工作空间与包结构：一套可交付的工程怎么摆

一个能直接交给别人用的 ROS 功能包，目录至少长这样（以 catkin 工作空间为例）：

使用说明合格的 README，第一段会告诉你三件事：用什么 ROS 版本编译过、launch 怎么起、参数在哪里改。我一般建议把 ROS 1 和 ROS 2 的差异单独写一节，因为 add_executable 换成 ament_target_dependencies 后，新人照抄是编不过的。源码工程用 VSCode 打开最舒服，C/C++ 插件配好后函数跳转和变量重命名都能跨文件工作。这套源码最值得看的不是某行炫技，而是 controller.h 里控制律类的接口——它决定了改参数能不能立刻生效。

3.2 话题设计与消息类型：谁发布、谁订阅、谁仲裁

多差速编队里每个节点其实都很简单，复杂的是话题归属。下面是一份典型的两车纵队话题表：

这张表看起来平淡，但藏着一个几乎所有新手都会踩的坑：命令话题只有一个，往上发布它的节点却可能有键盘节点、导航节点、编队节点。ROS 话题没有优先级，后到的消息直接覆盖先到的，底盘节点在多个移动指令话题之间“取舍”不过来，车就会忽停忽走。

我处理这个问题的方式是加一个仲裁节点 cmd_vel_arbiter：它订阅多个指令源，按当前状态机选一路发布真正的底盘指令。导航进行中放行导航的 /cmd_vel，导航完成切到编队通道，键盘遥控永远最高优先级。这个仲裁逻辑在源码里通常只有三十行，但对整车行为的影响比控制律还大。

3.3 CMakeLists.txt 与 package.xml：C++ 依赖最小集

编队控制节点用到的 ROS 依赖不复杂：roscpp、geometry_msgs（Twist）、nav_msgs（Odometry）、tf2/tf2_ros（坐标系变换）。一个能编译的 CMakeLists 最少是这个样子：

cmake_minimum_required(VERSION 3.0.2) project(formation_control) add_compile_options(-std=c++14) find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs nav_msgs tf2 tf2_ros ) catkin_package() include_directories(include ${catkin_INCLUDE_DIRS}) add_executable(formation_node src/formation_node.cpp) add_dependencies(formation_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(formation_node ${catkin_LIBRARIES})

对应 package.xml 里要把 roscpp、geometry_msgs、nav_msgs、tf2、tf2_ros 写进 build_depend 和 exec_depend。转 ROS 2 时，把 catkin 换成 ament_cmake，target_link_libraries 换成 ament_target_dependencies，依赖名也有细微差别。

这段最容易翻车的是漏掉 add_dependencies：如果你用了自定义消息却忘加这行，编译期可能正常，运行时节点一启动就报消息类型未注册。这类问题在编队这种多节点场景特别隐蔽，因为它不是编译错误，而是启动顺序问题。

3.4 让注释真的能救命：读这份源码的正确路线

一份带“详细注释”的编队源码，注释重点应该放在五个地方：节点职责、话题方向、坐标系约定、单位、限幅。我读这类代码的顺序是：先找 main，顺着初始化走一遍找到三个回调，然后从回调向控制函数看数据流。一个合格的节点文件头注释大致长这样：

/** * @brief formation_node：领航者/跟随者二合一节点 * 通过 role 参数区分角色；follower 订阅 leader 的 /odom * 坐标系约定：所有位姿统一转到 odom 系后再算相对误差 * 速度单位 m/s，角速度 rad/s；控制频率由定时器决定 * 输出 /cmd_vel 前必须先过限幅，防止实车碰撞 */

这五六行把最容易模糊的三件事——坐标系、单位、频率——固定下来了。没有这个头注释，代码里全是裸的 double，三个月后你自己都分不清当时写的是角度还是长度。主循环的数据流是固定的：订阅回调里拿里程计 → 算相对位姿误差 → 走控制律得 v_cmd/w_cmd → 限幅 → 发布 Twist。源码里每个函数只干一件事，注释也按这个边界写，后面跑起来出问题，你才能按“哪一段最可疑”去加打印。

4. 跑通编队的最小步骤：launch 文件、参数调参与仿真验证

编队控制最怕的不是写不出来，是跑不起来的时候不知道问题出在坐标系还是控制律。所以先花最少的时间拿到一个能看的现象：两辆车在仿真里排成纵队、间距不散。有了这个基线，再去碰真车和多机。

4.1 环境准备：先让两辆差速车出现在 Gazebo 里

如果你是从零开始装 ROS，Ubuntu 系统安装 ROS 这个步骤能磨掉不少人半天，常见做法是先用鱼香ROS一键安装把 ROS 本体装好，再回到 catkin 工作空间继续。装好之后，先确认环境和编译工具链：

source /opt/ros/noetic/setup.bash # ROS 1 示例；ROS 2 对应 humble mkdir -p ~/formation_ws/src cd ~/formation_ws && catkin_make # ROS 1 source devel/setup.bash

catkin_make 是最核心的一步：它在 src 下递归找 CMakeLists，把功能包编成可执行文件。如果报找不到 formation_control 包，先检查 package.xml 是否在 src/formation_control/ 下，而不是散在 src 下。

有了 ROS 环境，接下来需要两辆差速小车出现在仿真里。常见做法是复用一套差速车 URDF/xacro，通过命名空间实例化两份；如果你想先验证编队，更省事的是在 Gazebo 里放两个差分驱动轮式机器人模型，每个模型单独挂一个 /cmd_vel 底盘插件。很多现成的差速小车仿真包都带这个能力，配上 robot_state_publisher 就能在 RViz 里看到完整 tf。

注意：Gazebo 给每个仿真实体生成的 model name 不能重复，命名空间最好和模型名一一对应，否则话题不错位、tf 也会错位，这类问题在后端查起来非常绕。

4.2 launch 文件与命名空间：一辆车一个 group

多车编队里，launch 文件决定了很多隐性 bug。最简单的两车纵队写法是用 group 包 namespace：

<launch> <group ns="leader"> <node name="formation_node" pkg="formation_control" type="formation_node" output="screen"> <param name="role" value="leader"/> </node> </group> <group ns="follower1"> <node name="formation_node" pkg="formation_control" type="formation_node" output="screen"> <param name="role" value="follower"/> <param name="leader_ns" value="leader"/> </node> </group> </launch>

这样 follower 节点里的所有相对话题名会自动变成 /follower1/odom、/follower1/cmd_vel。订阅 leader 时，节点内部通过 ros::param 读取 leader_ns 拼成 /leader/odom，而不是硬编码——这是让代码能复用 N 辆车的关键约定。

如果你把两辆车分别跑在两台工控机上，还需要补一步 ROS 多机通信配置：ROS 1 里所有机器指向同一台 ROS_MASTER_URI，ROS 2 里设同一个 ROS_DOMAIN_ID，机器之间能互访话题端口。这一步的典型错误是只配了 IP 忘了开放端口，follower 节点起得来但收不到 leader 的 odom，现象和话题串线几乎一样。跑起来之后先用 rqt_graph 检查话题图：应该看到 leader 的 /odom 连到 follower1 的 formation_node，formation_node 再连到 follower1 的 /cmd_vel。如果 follower1 的 odom 也出现在连线里但没人订阅，说明节点角色或命名空间写岔了。

4.3 参数调参与队形观察：用什么现象判断收敛

参数都放 config/formation.yaml，launch 加载到参数服务器后再由节点读取。最省事的调参顺序是：先确认控制频率不低于 20Hz，再把限幅压到“车肯定不会冲出跑道”的量级，最后才动三个增益 kx、ky、kth。

判断收敛看三个现象：直线段两车距离是否稳定在设定间距；转弯后横向偏差能否在三秒内拉回；航向差是否出现肉眼可见的摆头。我惯用的验证命令是录下 leader 和 follower 的里程计轨迹：

rostopic echo -p /leader/odom/pose/pose > leader_pose.csv rostopic echo -p /follower1/odom/pose/pose > follower1_pose.csv

Ctrl+C 停止录制后，两条 csv 的时间戳通常对不齐，这是正常的，因为两辆车的话题频率和触发时刻不同。处理方法是按时间戳做线性插值再比对，别直接在原 csv 上逐行相减。

仿真里调通了，实车要重新标定轮距和轮径、重新调限幅，因为仿真模型的轮距是精确值，实车底盘手册给的数据往往和实际有出入。这也是为什么源码把参数全放在 yaml 而不是写死在代码里——实车调参不需要重新编译。

5. 编队控制避坑指南：新手最常翻车的五个问题

编队控制是一个“不踩坑不成活”的方向。我整理了仿真和实车上反复遇到的五类问题，每一条都按现象、原因、解决来讲，其中大部分和 ROS 的坐标系、话题、频率有关，和控制算法的数学反而关系不大。

5.1 现象：队形越拉越远，最后直接跟丢

编队跑五分钟，follower 和 leader 的间距从设定的 0.8m 逐渐拉到 2m、5m，最后在某次转弯中跟丢。

原因是里程计长期积分漂移，尤其是航向 θ 的漂移。差速底盘在仿真里几乎不漂，实车上编码器会因打滑和轮径误差累积，θ 偏一度，跑十米就偏出 0.17m。

解决：实车上给航向加融合，常见做法是编码器加 IMU 做航向融合，或者把轮径、轮距标定到毫米级。如果只是在仿真里复现，第一步先看 Gazebo 的噪声参数，把 odom 噪声降到最低，验证你的控制律而不是验证标定。

5.2 现象：队列前后震荡，像弹簧停不下来

follower 距离 leader 忽近忽远，幅度在半米内来回，偶尔还能看到明显的加减速交替。

原因是控制频率和增益不匹配：控制频率低于 10Hz 时，误差在每个周期里积累太多，比例增益稍放大就过冲，形成极限环震荡。

解决：把控制频率提到 30~50Hz，再把 kx 从 1.0 降到 0.5，给 v_cmd 加一阶低通做加速度平滑，震荡基本消失。不要只动增益，频率和限幅要先满足。

5.3 现象：话题串线，follower 收到自己发的数据

follower 的 odom 明明是自己在发布，但节点里取到的像是 leader 的位姿，两台车的数据互相污染。

原因是所有节点都挂在全局命名空间，话题名重名覆盖。用 rosbag 回放或 ROS 网桥时，这种问题会被放大。

解决：每个车一个 group namespace，代码里所有话题都写相对名，用 ros::param 取 leader_ns 配出完整话题名。跑起来后用 rostopic info 确认发布者列表里只有你期望的那一个，并且用 rostopic echo 单独看数据源对不对。

5.4 现象：底盘节点被多个指令源拉扯，车忽停忽走

导航节点、键盘节点、编队节点都在往同一个 /cmd_vel 发指令，车一会走导航、一会走键盘，实际在快速切换指令。

原因是 ROS 话题没有优先级，后到覆盖先到。这里要明确：多个节点发布移动指令话题时，底盘节点不是去“取舍”，而是应该单独划出一个仲裁节点决定哪条指令当前生效。

解决：加一个 cmd_vel_arbiter 节点，订阅各指令源，用状态机选路：键盘遥控优先级最高，导航次之，编队最后。没有仲裁节点的多指令多车，跑起来必然是一个玄学现场。

5.5 现象：follower 原地画圈或直冲 leader

控制律没有明显问题，但 follower 一启动就转圈，或者径直朝 leader 撞过去。

原因是 frame_id 不一致。follower 的里程计发布在 /follower1/odom，但 tf 树里 base_link 的父帧写成了 /odom，编队节点在算相对位姿时，可能缺少一次 transform 查询缓存，或者直接混用了 /map 和 /odom。

解决：先统一坐标系约定，比如全部用 /odom，在代码里一次性 lookupTransform，不要混用。用 rostopic echo 查看两个 odom 消息的 header.frame_id，确认它们在同一棵 tf 树下，再写一个五行的 tf2 listener 打印相对变换，误差一般在 0.5 秒内就会现形。

6. 从两车纵队到 N 车链式编队：误差传导与验证技巧

两车纵队调通之后，最自然的扩展是链式编队：每辆 follower 把前一辆车当成 leader，像火车车厢一样一截咬一截。这样做的好处是每辆车只需要和最近的前车通信，不需要全局编队解算；坏处是误差逐级传导，第 N 辆车的误差是前面 N-1 辆误差的叠加，越靠后越晃。我的做法是在控制律里加前馈：第 i 辆车的线速度基准取第 i-1 辆车的当前速度，而不是拿第一辆车的速度硬追。这样弯道时后车能提前减速，链式队形不会在中段发生“折纸”现象。

验证方法可以量化，不要只看眼睛。把 5.3 节录下的两条 csv 按时间戳插值对齐后，算两车实际相对位姿和期望相对位姿的差，重点是三个指标：纵向间距均值误差、横向偏差均值、航向差均值。纵向控制在 0.1~0.2m 内算合格，横向控制在 0.3m 内算合格。写成脚本每天跑一遍，比你盯着 RViz 三小时有效得多。我自己的习惯是每次改参后只跑直线加两个弯道的固定路线，录 csv、出指标，再决定要不要继续调增益。

我在这条路上最大的教训是限幅永远优先于增益。那次实车测试我忘了给 w_cmd 限幅，只调大了 kth，车在过弯时原地转了一整圈才被我按停。从那以后，我把仲裁、命名空间、tf 约定和限幅写成一页 checklist，每换一辆车前先过一遍，再碰任何增益参数。希望帮到你。

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