简介这份开源机器人集群项目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 nameswarm_robot paramsprefix !-- 每个 link 名都带前缀否则五台车会抢同一个 base_link -- link name${prefix}base_link visualgeometrybox size0.20 0.16 0.08//geometry/visual collisiongeometrybox size0.20 0.16 0.08//geometry/collision inertial mass value1.2/ inertia ixx0.001 iyy0.002 izz0.002 ixy0 ixz0 iyz0/ /inertial /link joint name${prefix}left_wheel_joint typecontinuous parent link${prefix}base_link/ child link${prefix}left_wheel_link/ axis xyz0 1 0/ /joint !-- Gazebo 差速插件也挂前缀话题这是隔离的关键一环 -- gazebo plugin name${prefix}diff_drive filenamelibgazebo_ros_diff_drive.so leftJoint${prefix}left_wheel_joint/leftJoint rightJoint${prefix}right_wheel_joint/rightJoint wheelSeparation0.16/wheelSeparation wheelDiameter0.065/wheelDiameter commandTopic${prefix}cmd_vel/commandTopic odometryTopic${prefix}odom/odometryTopic odometryFrame${prefix}odom/odometryFrame publishOdomTFtrue/publishOdomTF /plugin /gazebo /xacro:macro参数说明几个点wheelSeparation和wheelDiameter必须和 URDF 里视觉模型的尺寸对得上对不上就会出现“仿真里走得挺准录出来的里程计一拐弯就偏”的情况odometryFrame加前缀是必须的否则所有车都往全局 tf 里塞同一个odomframe后发布的消息会直接覆盖前面的。2.2 通信拓扑多机命名空间与话题设计话题按机器人编号分命名空间全局只保留编队层面的状态量。这是这个项目里我最先抄的一段设计话题消息类型方向说明/robot_N/odomnav_msgs/Odometry插件发布带前缀的里程计/robot_N/scansensor_msgs/LaserScan插件发布单线激光避障用/robot_N/cmd_velgeometry_msgs/Twist控制器发布速度指令唯一入口/swarm/formation自定义FormationState领航者发布队形定义与领航者位姿/tf、/tf_statictf2_msgs/TFMessage全局共享所有 frame 带前缀一棵树全局 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_TYPERelease source devel/setup.bash说明几点-DCMAKE_BUILD_TYPERelease不是可有可无的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 valuetrue/ include file$(find gazebo_ros)/launch/empty_world.launch arg nameworld_name value$(find swarm_sim)/worlds/swarm_empty.world/ arg namepaused valuefalse/ /include !-- 逐台拉起ns 前缀和初始位姿一一对应 -- group nsrobot_1 param namerobot_description command$(find xacro)/xacro $(find swarm_description)/urdf/robot.xacro prefix:robot_1_/ node namespawn pkggazebo_ros typespawn_model args-urdf -param robot_description -model robot_1 -x 0 -y 0 -z 0.06/ node namestate_pub pkgrobot_state_publisher typerobot_state_publisher/ /group !-- robot_2 ~ robot_5 结构相同x/y/z 按队形错开 -- group nsrobot_2 param namerobot_description command$(find xacro)/xacro $(find swarm_description)/urdf/robot.xacro prefix:robot_2_/ node namespawn pkggazebo_ros typespawn_model args-urdf -param robot_description -model robot_2 -x -0.8 -y 0 -z 0.06/ node namestate_pub pkgrobot_state_publisher typerobot_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_vel3.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_realworld 文件里把max_step_size压回 0.001real_time_update_rate保持 1000。四、五台车 spawn 出来叠在一起或者只有一台出现现象spawn_model报错或者被吞掉场景里只有一台车。原因-model后面的名字重复Gazebo 拒绝同名模型或者 spawn 的 z 太小陷进地面再或者五台车在同一时刻 spawn仿真还没稳定。解决model 名带前缀z 给 0.06~0.1spawn 之间加个延时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 commandload/挂到节点命名空间下节点统一用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 名字控制器代码不用编译第二次。改造点动的文件验证方式换队形拓扑topology.yamleval_formation.py看队列间距误差换控制器follower_node.cppbag 回放对比均值与超调上真机PoseProvider实现 参数先单机跑直线再跑三车编队从那以后我每次改完控制器参数都先把 bag 录下来回放跑一遍误差脚本再上真机——五台车在场地里追尾收拾起来比在仿真里查半天麻烦太多了。希望帮到你。本文还有配套的精品资源点击获取 
长河君 · 2026/11/4 8:23:12 · 阅读 1.2k 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 nameswarm_robot paramsprefix !-- 每个 link 名都带前缀否则五台车会抢同一个 base_link -- link name${prefix}base_link visualgeometrybox size0.20 0.16 0.08//geometry/visual collisiongeometrybox size0.20 0.16 0.08//geometry/collision inertial mass value1.2/ inertia ixx0.001 iyy0.002 izz0.002 ixy0 ixz0 iyz0/ /inertial /link joint name${prefix}left_wheel_joint typecontinuous parent link${prefix}base_link/ child link${prefix}left_wheel_link/ axis xyz0 1 0/ /joint !-- Gazebo 差速插件也挂前缀话题这是隔离的关键一环 -- gazebo plugin name${prefix}diff_drive filenamelibgazebo_ros_diff_drive.so leftJoint${prefix}left_wheel_joint/leftJoint rightJoint${prefix}right_wheel_joint/rightJoint wheelSeparation0.16/wheelSeparation wheelDiameter0.065/wheelDiameter commandTopic${prefix}cmd_vel/commandTopic odometryTopic${prefix}odom/odometryTopic odometryFrame${prefix}odom/odometryFrame publishOdomTFtrue/publishOdomTF /plugin /gazebo /xacro:macro参数说明几个点wheelSeparation和wheelDiameter必须和 URDF 里视觉模型的尺寸对得上对不上就会出现“仿真里走得挺准录出来的里程计一拐弯就偏”的情况odometryFrame加前缀是必须的否则所有车都往全局 tf 里塞同一个odomframe后发布的消息会直接覆盖前面的。2.2 通信拓扑多机命名空间与话题设计话题按机器人编号分命名空间全局只保留编队层面的状态量。这是这个项目里我最先抄的一段设计话题消息类型方向说明/robot_N/odomnav_msgs/Odometry插件发布带前缀的里程计/robot_N/scansensor_msgs/LaserScan插件发布单线激光避障用/robot_N/cmd_velgeometry_msgs/Twist控制器发布速度指令唯一入口/swarm/formation自定义FormationState领航者发布队形定义与领航者位姿/tf、/tf_statictf2_msgs/TFMessage全局共享所有 frame 带前缀一棵树全局 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_TYPERelease source devel/setup.bash说明几点-DCMAKE_BUILD_TYPERelease不是可有可无的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 valuetrue/ include file$(find gazebo_ros)/launch/empty_world.launch arg nameworld_name value$(find swarm_sim)/worlds/swarm_empty.world/ arg namepaused valuefalse/ /include !-- 逐台拉起ns 前缀和初始位姿一一对应 -- group nsrobot_1 param namerobot_description command$(find xacro)/xacro $(find swarm_description)/urdf/robot.xacro prefix:robot_1_/ node namespawn pkggazebo_ros typespawn_model args-urdf -param robot_description -model robot_1 -x 0 -y 0 -z 0.06/ node namestate_pub pkgrobot_state_publisher typerobot_state_publisher/ /group !-- robot_2 ~ robot_5 结构相同x/y/z 按队形错开 -- group nsrobot_2 param namerobot_description command$(find xacro)/xacro $(find swarm_description)/urdf/robot.xacro prefix:robot_2_/ node namespawn pkggazebo_ros typespawn_model args-urdf -param robot_description -model robot_2 -x -0.8 -y 0 -z 0.06/ node namestate_pub pkgrobot_state_publisher typerobot_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_vel3.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_realworld 文件里把max_step_size压回 0.001real_time_update_rate保持 1000。四、五台车 spawn 出来叠在一起或者只有一台出现现象spawn_model报错或者被吞掉场景里只有一台车。原因-model后面的名字重复Gazebo 拒绝同名模型或者 spawn 的 z 太小陷进地面再或者五台车在同一时刻 spawn仿真还没稳定。解决model 名带前缀z 给 0.06~0.1spawn 之间加个延时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 commandload/挂到节点命名空间下节点统一用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 名字控制器代码不用编译第二次。改造点动的文件验证方式换队形拓扑topology.yamleval_formation.py看队列间距误差换控制器follower_node.cppbag 回放对比均值与超调上真机PoseProvider实现 参数先单机跑直线再跑三车编队从那以后我每次改完控制器参数都先把 bag 录下来回放跑一遍误差脚本再上真机——五台车在场地里追尾收拾起来比在仿真里查半天麻烦太多了。希望帮到你。本文还有配套的精品资源点击获取