简介：KKSwarm是由易科机器人实验室与阿木实验室联合打造的开源机器人集群项目，面向多机器人协同、ROS开发、高校教学与科研实验场景，适合具备ROS基础的中高级开发者与研究人员，提供可复现、可二次开发的集群系统代码与实验环境。压缩包共343个文件，约177.57MB，以124个C/C++头文件和82个C++源文件为核心，辅以19个Simulink模型、launch与yaml配置文件、msg通信定义、Shell/Python脚本、gif演示及PDF文档，覆盖编队控制、目标点导航、纯追踪、强化学习策略、相机初始化与可视化等模块。已有82人学习下载。项目采用模块化设计，目录按核心代码、仿真模型、配置与脚本组织，便于读者理解机器人集群的通信、协调、导航与任务分配机制，并据此搭建仿真或实物实验平台，开展编队、协同搬运、搜索救援等任务的教学演示与算法验证，也适合作为开源社区贡献和功能扩展的起点。

1. 开源机器人集群项目的第一道门槛：先看清它到底解决什么问题

开源机器人集群项目落地时，最容易被低估的不是单机控制，而是多机之间的时间同步和通信抖动。某开源机器人集群项目把仿真、编队控制、任务调度和可视化串成一条链路，适合拿来做多机一致性、领航跟随、局部避障的算法验证。它不是上来就讲集群智能玄学，而是先把每台机器人的命名空间、话题和服务拆清楚，再让多台模型在同一张地图里跑起来。读者如果正卡在“单机能跑、多机就乱”的阶段，这份资源的价值在于给出一套可复现的工程骨架，而不是只给几个公式。

2. 架构拆解：从单机节点到多机集群的通信与控制分层

2.1 目录结构：仿真、控制、通信、配置四层怎么分

拿到一份机器人集群资源，我一般先看目录，不先看算法。单机项目可以靠一个launch文件从头跑到尾，多机项目不行，因为每台机器人都要有独立的命名空间、参数文件和话题前缀，目录一层乱，后面排查就是黑匣子。某开源机器人集群项目的工程结构通常会把“仿真世界”“控制节点”“通信桥接”“配置模板”拆成四块，下面是一份常见的目录骨架，包名用通用代称，读者按自己的工程改名即可。

# 通用集群工程目录骨架 swarm_ws/ ├── src/ │ ├── swarm_bringup/ # 启动脚本、launch、多机编排 │ │ ├── launch/ │ │ │ ├── single_robot.launch.xml │ │ │ └── swarm_sim.launch.xml │ │ └── config/ │ │ ├── robot_ids.yaml # 机器人ID与命名空间映射 │ │ └── qos_profiles.yaml │ ├── formation_control/ # 编队控制、一致性协议 │ │ ├── src/ │ │ │ └── formation_node.cpp │ │ └── config/ │ │ └── formation_params.yaml │ ├── safety_layer/ # 局部避障、速度平滑、急停 │ │ └── src/ │ │ └── avoid_node.py │ └── task_manager/ # 多机任务状态机、超时恢复 │ └── src/ │ └── task_fsm.py ├── simulation/ │ ├── worlds/ │ │ └── swarm_empty.world │ └── models/ │ └── robot_template/ └── scripts/ ├── spawn_multi.sh └── record_all.sh

这个结构的关键不在文件夹名字，而在“配置和代码分离”。robot_ids.yaml负责ID、命名空间、初始位姿的映射，qos_profiles.yaml负责通信质量策略，formation_params.yaml负责控制增益。常见做法是把每台机器人的差异全部收敛到YAML里，代码里只读参数，不写死robot_1、robot_2。我见过不少翻车案例，都是因为控制节点里硬编码了邻居名字，换一台机器人就要改代码，最后配置散落在五个文件里，调参调到怀疑人生。多机项目的第一条血泪经验：凡是和机器人编号有关的东西，都不要进源码。

目录分层的另一个好处是启动顺序清晰。仿真世界先起，模型生成器再起，通信桥接和控制节点最后起。如果顺序反了，控制节点会先订阅到空话题，等模型出来以后又不会重新建立连接，表面看节点都在，实际数据链路是断的。把启动文件放在swarm_bringup里统一编排，比每个包各自写一个launch更可控。参数文件按机器人数量生成，不要手写十份，脚本读一遍robot_ids.yaml就能展开。

2.2 启动链路：一个集群仿真从launch到话题订阅的完整路径

多机启动最容易犯的错，是把单机launch复制十遍，改个名字就以为能跑。真正的集群启动链路要处理三件事：仿真世界只加载一次、机器人模型按命名空间生成、控制节点按ID订阅邻居话题。下面是一段通用的launch骨架，语言用XML，包名和节点名都是占位，读者替换成自己的即可。

<!-- swarm_sim.launch.xml：多机启动骨架 --> <launch> <!-- 1. 启动仿真世界，只启动一次 --> <include file="$(find-pkg-share swarm_bringup)/launch/world.launch.xml"/> <!-- 2. 按机器人数量循环生成模型，参数来自robot_ids.yaml --> <node pkg="swarm_bringup" exec="spawn_swarm" name="spawn_swarm" output="screen"> <param name="robot_ids_file" value="$(find-pkg-share swarm_bringup)/config/robot_ids.yaml"/> <param name="spawn_z" value="0.15"/> </node> <!-- 3. 每个机器人一个控制节点，使用命名空间隔离 --> <group> <push-ros-namespace namespace="robot_1"/> <node pkg="formation_control" exec="formation_node" name="formation_node" output="screen"> <param from="$(find-pkg-share formation_control)/config/formation_params.yaml"/> <param name="robot_id" value="1"/> <param name="neighbor_ids" value="[2,3]"/> </node> </group> <group> <push-ros-namespace namespace="robot_2"/> <node pkg="formation_control" exec="formation_node" name="formation_node" output="screen"> <param from="$(find-pkg-share formation_control)/config/formation_params.yaml"/> <param name="robot_id" value="2"/> <param name="neighbor_ids" value="[1,3]"/> </node> </group> <!-- 4. 任务管理器和记录节点放在全局命名空间 --> <node pkg="task_manager" exec="task_fsm" name="task_fsm" output="screen"/> <node pkg="swarm_bringup" exec="record_all" name="record_all" output="screen"/> </launch>

这段launch的逻辑说明：第一步只加载一次世界，避免每台机器人重复加载物理引擎；第二步用生成器节点读YAML批量生成模型，spawn_z控制初始高度，防止模型陷进地面；第三步用push-ros-namespace把每台机器人的话题隔离到/robot_1/...、/robot_2/...，控制节点的robot_id和neighbor_ids从参数注入；第四步把任务管理和记录节点放在全局命名空间，方便统一观测。参数说明里最容易被忽略的是neighbor_ids，它决定一致性协议的邻居集合，写错不会报错，只会让队形慢慢漂移，这种问题最难查。

启动链路跑通的标准不是“launch没报错”，而是三件事同时成立：仿真里能看到N台模型、每台机器人的控制节点都能收到自己的状态话题、邻居话题的订阅数不为零。我一般会在启动后先跑一遍ros2 node list和ros2 topic info，确认命名空间没有串。多机项目里，节点名重复和话题名重复是两种不同的故障，前者会让launch直接失败，后者会让数据悄悄串台，后者更危险。

2.3 核心控制节点：编队控制器与邻居状态机

编队控制的核心不是公式多复杂，而是邻居状态怎么收、时间戳怎么对齐、输出限幅怎么加。某开源机器人集群项目常见做法是用领航跟随加一致性修正：每台机器人维持一个期望相对位置，同时根据邻居的位置误差做小增益修正。下面是一段Python控制节点骨架，展示参数读取、邻居订阅和速度输出，代码里保留了关键注释。

# formation_node.py：编队控制节点骨架 import math import rclpy from rclpy.node import Node from geometry_msgs.msg import Twist, PoseStamped from std_msgs.msg import Float32 class FormationNode(Node): def __init__(self): super().__init__('formation_node') # 机器人自身ID和邻居ID由launch参数注入 self.robot_id = self.declare_parameter('robot_id', 1).value self.neighbor_ids = self.declare_parameter('neighbor_ids', [2, 3]).value self.k_formation = self.declare_parameter('k_formation', 0.8).value self.k_damping = self.declare_parameter('k_damping', 0.3).value self.max_speed = self.declare_parameter('max_speed', 0.6).value self.own_pose = None self.neighbor_poses = {} self.last_time = self.get_clock().now() # 自身状态订阅，话题带命名空间 self.create_subscription( PoseStamped, 'self_pose', self.on_own_pose, 10) # 邻居状态订阅，每个邻居一个话题 for nid in self.neighbor_ids: topic = f'/robot_{nid}/self_pose' self.create_subscription( PoseStamped, topic, lambda msg, n=nid: self.on_neighbor_pose(n, msg), 10) self.cmd_pub = self.create_publisher(Twist, 'cmd_vel', 10) self.timer = self.create_timer(0.05, self.control_loop) # 20Hz def on_own_pose(self, msg): self.own_pose = msg def on_neighbor_pose(self, nid, msg): self.neighbor_poses[nid] = msg def control_loop(self): if self.own_pose is None: return # 期望队形：邻居在自身右侧/左侧的固定偏移，实际项目从YAML读 desired_offsets = {nid: (0.0, 0.8 * (nid - self.robot_id)) for nid in self.neighbor_ids} vx, vy = 0.0, 0.0 for nid, (dx, dy) in desired_offsets.items(): if nid not in self.neighbor_poses: continue nx = self.neighbor_poses[nid].pose.position.x ny = self.neighbor_poses[nid].pose.position.y ex = (nx - self.own_pose.pose.position.x) - dx ey = (ny - self.own_pose.pose.position.y) - dy # 小增益一致性修正，避免大增益引起震荡 vx += self.k_formation * ex vy += self.k_formation * ey # 阻尼项抑制来回摆 vx -= self.k_damping * vx vy -= self.k_damping * vy # 输出限幅，防止单次指令过大 speed = math.hypot(vx, vy) if speed > self.max_speed: vx = vx / speed * self.max_speed vy = vy / speed * self.max_speed cmd = Twist() cmd.linear.x = vx cmd.linear.y = vy self.cmd_pub.publish(cmd) def main(): rclpy.init() node = FormationNode() rclpy.spin(node) node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()

这段代码的逻辑说明：robot_id和neighbor_ids决定订阅谁的话题，k_formation是队形修正增益，k_damping是阻尼项，max_speed是输出限幅。控制循环跑20Hz，先检查自身状态是否到达，再遍历邻居，计算相对位置误差，累加修正速度，最后限幅发布。参数说明里最需要盯的是k_formation和k_damping：增益太大，队形会像弹簧一样来回震荡；阻尼太大，机器人反应迟钝，遇到障碍物来不及修正。我一般先把max_speed压到0.3左右，确认队形能收敛，再逐步放开。

这个节点还有一个工程细节：邻居话题用/robot_{nid}/self_pose硬拼字符串，实际项目最好从robot_ids.yaml读命名空间前缀，避免ID和话题名两套规则。控制节点不要自己维护全局ID表，ID表只在一个配置文件里维护，节点启动时读一次。多机项目里，配置漂移比算法错误更常见，今天改一个话题名，明天改一个命名空间，最后没人知道哪份配置是准的。

2.4 通信层：多机话题命名与QoS

多机集群的通信层要做两件事：话题命名统一、QoS策略匹配。话题命名不统一，节点能看到但收不到数据；QoS不匹配，发布端和订阅端一个用可靠传输、一个用尽力而为，连接能建立但消息会丢。下面是一份QoS配置骨架，语言用YAML，读者可以把它加载到控制节点和记录节点里。

# qos_profiles.yaml：多机通信QoS配置骨架 pose_qos: reliability: reliable # 位置状态用可靠传输，避免关键帧丢失 durability: volatile history_depth: 10 deadline_ms: 100 # 超过100ms没更新视为邻居失联 cmd_qos: reliability: best_effort # 速度指令用尽力而为，降低延迟 durability: volatile history_depth: 5 lifespan_ms: 200 # 指令过期自动丢弃，防止旧指令卡住 neighbor_heartbeat: reliability: reliable history_depth: 3 lease_ms: 500 # 心跳租约，超时触发邻居状态清理

这份配置的逻辑说明：pose_qos给位置状态用可靠传输，深度10，deadline_ms用于判断邻居是否失联；cmd_qos给速度指令用尽力而为，lifespan_ms让过期指令自动失效；neighbor_heartbeat用心跳租约，超过lease_ms没收到就清理邻居状态。参数说明里，deadline_ms不要设得太小，仿真里物理步长和渲染会带来抖动，设成50ms以下很容易误判断联；lifespan_ms也不要太大，否则机器人急停后还会执行旧指令。常见做法是把QoS配置放在一个YAML里，控制节点和记录节点共用，避免一边可靠一边尽力而为。

通信层还有一个容易被忽略的点：DDS域。多机仿真在同一台机器上跑，如果域ID和别的项目冲突，会出现“节点列表里混进陌生节点”的怪现象。我一般给每个项目分配独立的域ID，写进启动脚本，不依赖默认值。话题命名、QoS、域ID三件事固定下来，多机通信才有可复现性。

3. 跑起来：仿真环境、依赖与多机启动的实操步骤

3.1 环境准备：中间件、仿真器与飞控桥接的常见组合

机器人集群项目的环境准备，最怕一上来就装一堆东西，最后不知道哪个依赖没对上。常见组合是机器人中间件加仿真器，再加一个飞控桥接节点。中间件负责节点通信，仿真器负责物理世界和传感器，桥接节点负责把飞控状态转成中间件话题。下面是一段环境检查脚本，语言用bash，读者按自己的系统改包管理器。

# check_env.sh：环境检查骨架 set -e # 1. 检查中间件命令是否存在 command -v ros2 >/dev/null || { echo "缺少机器人中间件命令"; exit 1; } # 2. 检查仿真器命令是否存在 command -v gazebo >/dev/null || echo "警告：未找到仿真器命令，检查安装路径" # 3. 检查Python依赖 python3 -c "import yaml, numpy" || { echo "缺少Python依赖"; exit 1; } # 4. 检查工作空间是否已构建 [ -f install/setup.bash ] || { echo "工作空间未构建"; exit 1; } # 5. 输出当前DDS域ID，确认没有和别的项目冲突 echo "当前域ID: ${ROS_DOMAIN_ID:-未设置}"

这段脚本的逻辑说明：先检查中间件命令，再检查仿真器命令，然后检查Python依赖，最后确认工作空间构建状态和域ID。参数说明里，ROS_DOMAIN_ID没有设置时容易和默认项目撞车，建议在启动脚本里显式导出。仿真器命令找不到不一定要立刻退出，有些环境用源码安装，命令名不同，先给警告再人工确认。我一般把这段脚本放在scripts/下，每次换机器先跑一遍，比凭记忆装依赖靠谱。

环境准备阶段不要追求一次装全。先把单机模型跑起来，再装多机需要的桥接和记录工具。多机项目依赖多，一次装全容易把版本冲突掩盖掉。常见做法是先用最小依赖跑通一个空世界加一台机器人，确认中间件、仿真器、桥接节点三者能通，再往上加机器人数量。

3.2 单机验证：先让一台机器人稳定动起来

多机之前必须先单机。单机验证的目标不是“能起飞”，而是“状态话题、控制话题、仿真模型三者时间对齐”。下面是一段单机启动和检查命令，语言用bash，命令里的包名是占位。

# 单机验证：启动、检查话题、发一条低速指令 source install/setup.bash export ROS_DOMAIN_ID=42 # 启动单机仿真 ros2 launch swarm_bringup single_robot.launch.xml & sleep 5 # 检查模型是否生成 ros2 topic list | grep -E "self_pose|cmd_vel" # 检查控制节点是否在线 ros2 node list | grep formation_node # 发一条低速前进指令，观察模型是否响应 ros2 topic pub /robot_1/cmd_vel geometry_msgs/msg/Twist \ "{linear: {x: 0.2, y: 0.0, z: 0.0}, angular: {z: 0.0}}" -r 5

这段命令的逻辑说明：先设置域ID，启动单机launch，等待模型生成，然后检查话题和节点，最后用ros2 topic pub以5Hz发低速指令。参数说明里，sleep 5不是随便写的，仿真器加载世界和模型需要时间，太早检查话题会误判；-r 5控制发布频率，频率太高会压住控制节点。单机验证时，我一般先看/robot_1/self_pose有没有持续更新，再看cmd_vel有没有被控制节点覆盖。如果手动发的指令立刻被控制节点改掉，说明控制链路是通的，这是好事。

单机阶段还要确认坐标系。仿真里的模型位姿、控制节点读的位姿、桥接节点转发的位姿，三者要在同一个坐标系下。常见翻车是控制节点读的是世界坐标系，桥接节点转发的是机体坐标系，单机看不太出来，多机编队时队形会整体旋转。单机验证时把机器人转个90度，看self_pose的朝向变化是否符合预期，这一步别省。

3.3 多机扩展：命名空间、端口、DDS域与启动参数

单机稳定后，多机扩展不是简单把数量改成N。每台机器人要有独立命名空间、独立参数、独立初始位姿，仿真器还要处理端口和物理步长。下面是一段多机启动脚本骨架，语言用bash，展示数量、域ID、记录开关三个参数怎么传。

# start_swarm.sh：多机启动骨架 #!/usr/bin/env bash set -e ROBOT_NUM=${1:-3} # 机器人数量，默认3 export ROS_DOMAIN_ID=${2:-42} # DDS域ID，默认42 RECORD=${3:-false} # 是否录制，默认false # 根据数量生成robot_ids.yaml，避免手写十份配置 python3 scripts/gen_robot_ids.py --num "$ROBOT_NUM" \ --out src/swarm_bringup/config/robot_ids.yaml # 启动集群仿真 ros2 launch swarm_bringup swarm_sim.launch.xml \ robot_num:="$ROBOT_NUM" \ record_enable:="$RECORD"

这段脚本的逻辑说明：ROBOT_NUM控制机器人数量，ROS_DOMAIN_ID隔离通信域，RECORD控制是否开启记录。参数说明里，gen_robot_ids.py负责把数量展开成ID和初始位姿，初始位姿要按圆形或队列排开，别让模型初始重叠，重叠会导致物理引擎把模型弹飞。record_enable用布尔字符串，launch里做条件判断，避免每次手动改launch。多机启动后，先看ros2 node list里有没有N个控制节点，再看每台机器人的self_pose是否都在更新，最后看邻居话题的订阅数。

DDS域ID在多机扩展时特别重要。同一台机器上跑两个集群项目，域ID一样会互相看到对方节点，话题名再一样就直接串台。我一般把域ID写进启动脚本，不依赖环境默认值。端口方面，仿真器图形界面和物理引擎各有端口，数量多时优先关掉不必要的渲染，减少CPU占用。

3.4 观测与记录：rosbag、rqt与日志三件套

多机调试不能靠盯屏幕。观测和记录要覆盖三层：节点层看谁在线，话题层看数据频率，日志层看异常。下面是一段记录命令骨架，语言用bash，展示按正则录话题和限制缓存。

# record_swarm.sh：多机记录骨架 #!/usr/bin/env bash set -e BAG_DIR=${1:-bags/swarm_$(date +%Y%m%d_%H%M%S)} mkdir -p "$BAG_DIR" # 录制所有机器人的位姿、速度指令和任务状态 ros2 bag record \ -o "$BAG_DIR" \ --max-cache-size 200000000 \ -e "/robot_[0-9]+/(self_pose|cmd_vel)" \ -e "/task_state" # 记录完成后打印话题统计，方便检查是否漏录 ros2 bag info "$BAG_DIR"

这段命令的逻辑说明：BAG_DIR按时间戳建目录，--max-cache-size限制缓存，防止内存被吃满，-e用正则匹配多机话题，/task_state单独录任务状态。参数说明里，正则里的[0-9]+要确认和命名空间一致，如果机器人编号带前缀，正则也要改；max-cache-size不要设太大，多机高频话题很容易把内存打满。记录完成后用ros2 bag info看消息数和时长，消息数为零说明正则没匹配上，别等回放时才发现。

观测工具方面，rqt看话题连接和节点图，日志看控制节点有没有超时警告。我一般把日志级别调到info，关键状态打印一行，别打太多，否则多机日志会刷屏。多机记录的目标是能回放复现，不是录得越多越好，位姿、指令、任务状态这三类够用了。

4. 核心模块拆解：编队控制、避障与任务调度怎么落到代码

4.1 编队控制：领航跟随与一致性协议

编队控制模块是多机项目的核心，但代码不需要写成论文。领航跟随负责给整体一个方向，一致性协议负责让邻居之间保持相对位置。下面是一段领航跟随加一致性修正的代码骨架，语言用Python，展示领航者选择和邻居误差合成。

# leader_follower.py：领航跟随控制骨架 import math class LeaderFollower: def __init__(self, robot_id, leader_id, offset, kp, max_v): self.robot_id = robot_id self.leader_id = leader_id self.offset = offset # (dx, dy) 期望相对领航者偏移 self.kp = kp # 位置误差增益 self.max_v = max_v # 最大速度 def compute(self, own_pose, leader_pose, neighbor_errors): # 领航者位置加偏移，得到自身期望位置 target_x = leader_pose[0] + self.offset[0] target_y = leader_pose[1] + self.offset[1] ex = target_x - own_pose[0] ey = target_y - own_pose[1] # 一致性修正：邻居误差取平均，小增益叠加 if neighbor_errors: ex += 0.2 * sum(e[0] for e in neighbor_errors) / len(neighbor_errors) ey += 0.2 * sum(e[1] for e in neighbor_errors) / len(neighbor_errors) vx = self.kp * ex vy = self.kp * ey speed = math.hypot(vx, vy) if speed > self.max_v: vx = vx / speed * self.max_v vy = vy / speed * self.max_v return vx, vy

这段代码的逻辑说明：leader_id指定领航者，offset是期望相对位置，kp是位置误差增益，max_v限制速度。compute先算自身到期望位置的误差，再把邻居误差平均后以0.2的小增益叠加，最后限幅。参数说明里，offset要和队形设计一致，别让两个机器人抢同一个位置；kp从0.5左右起调，太大容易震荡；一致性修正项不要超过0.3，否则领航者指令会被邻居拉偏。常见做法是把领航者ID和偏移写进YAML，代码只读参数。

这个模块的测试方法很简单：先固定领航者不动，让跟随者从不同初始位置收敛到期望偏移，看超调和稳态误差；再让领航者走直线，看队形保持情况。多机项目里，编队控制不是孤立的，避障模块会临时改速度，任务调度会切换队形，所以控制输出要留一个“外部修正”接口，别把速度发布写死在控制循环里。

4.2 避障模块：局部代价地图与速度平滑

避障模块要解决的是“编队队形”和“安全距离”的冲突。常见做法是用局部代价地图做排斥，再对速度做低通平滑。下面是一段避障节点骨架，语言用Python，展示障碍物距离计算和速度修正。

# avoid_node.py：局部避障骨架 import math class AvoidLayer: def __init__(self, safe_dist=0.8, avoid_gain=1.2, smooth=0.7): self.safe_dist = safe_dist # 安全距离 self.avoid_gain = avoid_gain # 排斥增益 self.smooth = smooth # 速度平滑系数 self.last_v = (0.0, 0.0) def apply(self, desired_v, obstacles): vx, vy = desired_v for ox, oy in obstacles: dx = -ox # 障碍物相对自身的反向向量 dy = -oy dist = math.hypot(dx, dy) if 0.0 < dist < self.safe_dist: # 距离越近，排斥越强 strength = self.avoid_gain * (1.0 / dist - 1.0 / self.safe_dist) vx += strength * dx / dist vy += strength * dy / dist # 低通平滑，避免速度突变 vx = self.smooth * self.last_v[0] + (1 - self.smooth) * vx vy = self.smooth * self.last_v[1] + (1 - self.smooth) * vy self.last_v = (vx, vy) return vx, vy

这段代码的逻辑说明：safe_dist决定开始避障的距离，avoid_gain决定排斥强度，smooth决定速度平滑程度。obstacles传入的是相对自身的位置，距离小于安全距离时按反比例增加排斥速度，最后用低通平滑。参数说明里，safe_dist不要小于机器人半径加余量，否则避障触发太晚；avoid_gain太大容易让机器人抖，太小又避不开；smooth越大越平滑，但响应越慢，常见取值0.6到0.8。避障输出要限幅，别让排斥项把速度推到控制节点上限之外。

避障和编队要分优先级。我一般让避障层只修正速度，不直接改目标点，这样编队控制器还能继续收敛。避障触发时，任务状态机可以打一个“避障中”的标记，回放时能看出队形变化是避障导致的，不是控制发散。

4.3 任务调度：多机状态机与超时恢复

多机任务调度不是简单的顺序执行，要考虑某台机器人掉线、某台机器人避障超时、领航者失效。下面是一段任务状态机骨架，语言用Python，展示状态切换和超时恢复。

# task_fsm.py：多机任务状态机骨架 import time class TaskFSM: def __init__(self, timeout_s=2.0): self.state = "IDLE" self.timeout_s = timeout_s self.last_update = time.time() def update(self, robot_states): now = time.time() # 任意机器人失联超过阈值，进入降级 if any(not s["alive"] for s in robot_states.values()): if now - self.last_update > self.timeout_s: self.state = "DEGRADED" # 所有机器人在线且队形误差小，进入编队 if all(s["alive"] for s in robot_states.values()): if self.state == "DEGRADED": self.state = "RECOVER" elif all(s["formation_error"] < 0.15 for s in robot_states.values()): self.state = "FORMATION" self.last_update = now return self.state

这段代码的逻辑说明：timeout_s是失联判定时间，robot_states里每台机器人有alive和formation_error。任意机器人失联超过阈值进入DEGRADED，全部在线后进入RECOVER，队形误差小于0.15进入FORMATION。参数说明里，timeout_s要大于心跳周期两到三倍，太小会误判；formation_error阈值根据队形精度要求调，别设得太苛刻，否则状态机一直在恢复。状态机输出要能驱动控制节点切换增益，降级时降低速度、扩大安全距离。

任务调度模块的测试方法是注入故障：手动停掉一台机器人的控制节点，看状态机是否在超时后进入降级；再恢复节点，看是否能回到编队。多机项目里，状态机不是装饰，它决定了故障时集群是整体停住还是继续降级运行。

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

5.1 现象：节点能看到但收不到邻居消息

现象很典型：ros2 node list里每台机器人的控制节点都在，ros2 topic list也能看到邻居话题，但控制节点日志里邻居状态一直是空，队形不动。原因通常有两个：命名空间拼错，或者QoS不匹配。命名空间拼错时，发布端在/robot_2/self_pose，订阅端订的是/robot_02/self_pose，话题列表里两个都存在，节点不会报错。QoS不匹配时，发布端用尽力而为，订阅端要求可靠传输，连接能建立但消息到不了。解决方法是先用ros2 topic info -v看订阅数和QoS，再核对robot_ids.yaml里的ID格式，ID统一用不带前导零的整数，命名空间统一用robot_前缀。我一般会写一个小脚本，启动后自动打印每台机器人的话题和订阅数，少一次人工核对就少一次翻车。

5.2 现象：多机启动后CPU飙升、仿真变慢

机器人数量加到五六台以后，仿真开始卡，控制频率从20Hz掉到5Hz，队形跟着抖。原因不一定是算法重，常见的是仿真器渲染和物理步长没调。每台机器人模型都带摄像头或激光雷达时，渲染开销会线性上涨；物理步长设得太小，CPU也会被吃满。解决方法是先关掉不必要的传感器渲染，把物理步长调到项目能接受的精度，控制节点频率单独用定时器控制，不跟仿真步长绑定。多机调试阶段我一般只开位姿和速度话题，传感器数据等算法稳定后再加。CPU飙升时先看top里是仿真器进程还是控制节点进程，别一上来就怀疑控制算法。

5.3 现象：编队队形整体漂移或旋转

队形能保持形状，但整体慢慢漂移，或者走直线时队形整体旋转。原因多半是坐标系不一致或时间戳不同步。控制节点读的是世界坐标系，桥接节点转发的是机体坐标系，单机看不出来，多机编队时相对位置会整体偏。时间戳不同步也会导致漂移，邻居状态用旧时间戳做修正，等于用过去的位置控制现在。解决方法是统一坐标系，所有位姿话题明确标注frame_id，控制节点做坐标变换后再算误差；时间戳用消息头里的时间，别用本地接收时间。我一般在控制节点里加一行日志，打印自身和邻居的时间戳差值，差值超过100ms就警告，这种小日志能省很多排查时间。

5.4 现象：仿真模型加载失败或模型重叠弹飞

启动后仿真器里模型数量不对，或者模型一出生就重叠弹飞。原因通常是模型路径没配好，或者初始位姿生成脚本把多台机器人放在同一个点。模型加载失败时先看仿真器日志里的路径报错，确认模型目录在环境变量里；模型重叠时检查robot_ids.yaml里的初始位姿，圆形排布要留够半径，队列排布要留够间距。解决方法是生成初始位姿时加最小间距校验，间距小于机器人直径加安全余量就报错退出。多机项目里，初始位姿不是随便填的，重叠导致物理引擎把模型弹飞，后面所有控制都白搭。我一般把初始位姿生成脚本单独测一遍，打印每台机器人的间距，确认没问题再启动仿真。

5.5 现象：记录文件里关键话题缺失

回放时发现没有邻居位姿，或者任务状态没录上。原因通常是记录命令的正则没匹配到命名空间，或者QoS不匹配导致记录节点订阅不上。记录节点默认QoS可能和发布端不一致，高频话题还会被缓存策略丢掉。解决方法是记录前先ros2 topic list核对话题名，正则里把命名空间写全；记录节点显式配置QoS，和发布端保持一致；缓存大小按话题频率算，别用默认值。我一般会在记录脚本里先录10秒测试包，用ros2 bag info看消息数，确认关键话题都有数据再开始正式录。多机项目回放是排查的主要手段，记录缺话题等于丢了后悔药。

6. 二次开发：把仿真集群改成自己的算法验证平台

6.1 替换控制律：从参数化控制器到自定义插件

二次开发最直接的路是替换控制律，但不要一上来重写整个节点。某开源机器人集群项目的控制节点通常把参数暴露在YAML里，先把k_formation、k_damping、max_speed调成自己算法需要的范围，确认输入输出接口不变，再替换control_loop里的计算部分。常见做法是把控制律抽成一个独立类，输入自身位姿、邻居位姿、期望队形，输出速度指令，节点只负责订阅和发布。这样替换算法不用动通信层，测试也能单独跑。我一般会保留原控制律作为基线，新算法和基线用同一组回放数据对比，看收敛时间和稳态误差，别只看仿真里“看起来能跑”。

6.2 验证方法：分层回放与指标表

验证不要只靠盯屏幕。分层回放先看通信层，确认邻居话题频率和时间戳正常；再看控制层，确认速度指令没有突变；最后看任务层，确认状态机切换符合预期。下面是一张简单的指标表，回放时逐项填。

这张表不是形式，回放时每项都要有数据。指标不合格时先查通信层，再查控制层，最后查任务层，顺序反了会把通信问题当成算法问题调。

6.3 一个具体技巧：用统一ID映射避免多机配置散落

多机二次开发最容易失控的是配置。机器人数量一多，话题名、参数文件、初始位姿、邻居列表散在各处，改一个ID要动五个文件。我一般会做一个统一ID映射表，所有模块只认映射表里的逻辑ID，物理命名空间由启动脚本展开。下面是一段映射表骨架，语言用YAML。

# robot_ids.yaml：统一ID映射骨架 robots: - id: 1 namespace: robot_1 initial_pose: [0.0, 0.0, 0.1] neighbors: [2, 3] - id: 2 namespace: robot_2 initial_pose: [1.0, 0.0, 0.1] neighbors: [1, 3] - id: 3 namespace: robot_3 initial_pose: [0.5, 1.0, 0.1] neighbors: [1, 2]

这段映射表的逻辑说明：id是逻辑编号，namespace是话题前缀，initial_pose是初始位姿，neighbors是邻居列表。启动脚本读这张表生成launch参数，控制节点读这张表生成订阅话题。参数说明里，neighbors要双向一致，1的邻居里有2，2的邻居里也要有1，否则一致性协议会单向修正，队形会偏。我吃过一次亏，邻居列表只配了单向，仿真里看起来能跑，回放时发现一台机器人一直在追另一台，队形整体漂。从那以后我每次改robot_ids.yaml都强制走一遍双向校验脚本，确认邻居关系对称、初始位姿间距够、命名空间不重复，再启动仿真。希望帮到你。

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