基于 ROS2 Nav2 的多机器人排队作业与共享资源调度系统

自律少年王风帆

475人浏览 · 2026-05-11 11:11:52

自律少年王风帆 · 2026-05-11 11:11:52 发布

基于 ROS2 Nav2 的多机器人排队作业与共享资源调度系统

一、项目背景

在移动机器人应用场景中，单机器人自主导航已经能够通过 ROS2 Nav2 实现较为完整的定位、路径规划、局部避障和速度控制。但是在真实工程场景中，很多任务并不是单机器人独立完成的，而是多个机器人同时运行，并且需要访问某些共享资源。

例如：

工厂 AGV 共享上料工位
仓储机器人排队取货
服务机器人排队进入狭窄通道
多机器人共享充电桩
多个机器人访问同一个工作站

在这些场景中，如果每台机器人只依赖自身的 Nav2 导航系统，很容易出现以下问题：

多台机器人同时前往同一个工作站
机器人在工作站入口发生阻塞
后车等待位置不合理，影响前车离开
出站机器人和进站机器人路径冲突
某台机器人导航失败后导致整个队列卡死

因此，除了单机器人路径规划能力之外，还需要一个上层的多机器人调度决策层，用于管理任务顺序、等待队列、共享资源占用和异常恢复。

本项目就是围绕这个问题展开的：基于 ROS2 Humble、Nav2、Gazebo 与 RViz，设计并实现一个多机器人排队作业与共享资源调度系统。

项目地址：

https://github.com/gghzz/ros2-nav2-multi-robot-queue-scheduling

二、项目简介

本项目面向多机器人共享工作站作业场景，基于 ROS2 Humble、Nav2、Gazebo Classic 与 RViz2 搭建双机器人协同调度系统。

系统底层通过 namespace 隔离 robot1 和 robot2 的传感器、里程计、TF、速度控制话题以及 Nav2 action server，使两台机器人能够独立完成定位、全局规划、局部控制和目标点导航。

在上层调度部分，设计了 queue_manager 调度节点，将机器人任务流程抽象为：

前往等待点
队列等待
自动补位
进入工作站
作业执行
前往出口
任务完成

同时引入工作站资源锁和出口通道资源锁，解决多机器人同时访问共享资源时可能产生的工作站抢占、等待点阻塞、通道冲突和队列卡死问题。

三、系统整体架构

整个系统可以分为三层：

1. 仿真与机器人模型层

该层主要负责在 Gazebo 中加载两台 FishBot 移动机器人，包括机器人模型、传感器、差速控制器和 Gazebo 插件。

两台机器人分别为：

robot1
robot2

每台机器人都具有独立的：

激光雷达 scan
里程计 odom
TF 坐标变换
cmd_vel 速度控制
ros2_control 控制器

2. 单机器人导航层

每台机器人都运行独立的 Nav2 导航系统，包括：

AMCL 定位
global_costmap
local_costmap
planner_server
controller_server
bt_navigator
NavigateToPose action server

其中 robot1 的导航接口为：

/robot1/navigate_to_pose

robot2 的导航接口为：

/robot2/navigate_to_pose

这样可以保证 queue_manager 能够分别向不同机器人发送导航目标，而不会出现话题混用或控制冲突。

3. 多机器人调度层

调度层是本项目的核心，由自定义的 queue_manager 节点实现。

它不直接控制机器人底盘速度，而是通过 Nav2 的 NavigateToPose action 给不同机器人发送阶段性目标点。

queue_manager 主要负责：

维护机器人状态机
分配等待点
维护等待队列
控制机器人自动补位
判断工作站是否被占用
判断出口通道是否被占用
处理导航失败恢复
发布调度状态
生成实验日志

这种设计方式使得底层导航和上层调度解耦：

Nav2 负责“怎么走”
queue_manager 负责“谁先走、什么时候走、走到哪里”

四、多机器人 namespace 隔离

多机器人系统中最关键的问题之一是 namespace 隔离。

如果两台机器人都使用默认话题：

/scan
/odom
/cmd_vel
/tf

就会导致传感器数据、里程计数据和控制命令混在一起，Nav2 无法判断当前控制的是哪台机器人。

因此，本项目为 robot1 和 robot2 分别设置了独立 namespace。

robot1 相关话题包括：

/robot1/scan
/robot1/odom
/robot1/cmd_vel
/robot1/tf
/robot1/navigate_to_pose

robot2 相关话题包括：

/robot2/scan
/robot2/odom
/robot2/cmd_vel
/robot2/tf
/robot2/navigate_to_pose

这样每台机器人都拥有独立的 Nav2 实例，可以分别完成定位、规划和控制。

五、排队作业场景设计

本项目设计了一个共享工作站排队场景，包含：

P1：队首等待点
P2：第二等待点
P3：预留等待点
WORKSTATION：共享工作站
EXIT_LEFT：robot1 出口点
EXIT_RIGHT：robot2 出口点

默认调度流程为：

robot1 -> P1 -> WORKSTATION -> EXIT_LEFT
robot2 -> P2 -> P1 -> WORKSTATION -> EXIT_RIGHT

也就是说：

robot1 先前往 P1 等待点；
robot2 前往 P2 等待点；
robot1 进入工作站后，P1 被释放；
robot2 从 P2 自动补位到 P1；
robot1 作业完成并离开工作站；
robot2 获得工作站资源后进入工作站；
两台机器人依次完成任务。

这个流程模拟了真实排队场景中的“前车离开，后车补位”。

六、queue_manager 状态机设计

为了描述每台机器人的任务执行过程，本项目为每个机器人设计了状态机。

主要状态包括：

IDLE
GOING_TO_WAIT
WAITING_IN_QUEUE
MOVING_TO_FRONT
GOING_TO_WORKSTATION
WORKING
GOING_TO_EXIT
FINISHED
RECOVERING
FAILED

各状态含义如下：

状态	含义
IDLE	机器人空闲，尚未分配任务
GOING_TO_WAIT	正在前往初始等待点
WAITING_IN_QUEUE	已到达等待点，正在队列中等待
MOVING_TO_FRONT	从后方等待点向队首等待点补位
GOING_TO_WORKSTATION	正在前往工作站
WORKING	正在工作站执行作业
GOING_TO_EXIT	作业完成，正在前往出口
FINISHED	任务完成
RECOVERING	导航失败后正在恢复
FAILED	超过最大重试次数，任务失败

基本状态转移流程如下：

IDLE
  ↓
GOING_TO_WAIT
  ↓
WAITING_IN_QUEUE
  ↓
GOING_TO_WORKSTATION
  ↓
WORKING
  ↓
GOING_TO_EXIT
  ↓
FINISHED

对于 robot2，当 robot1 离开 P1 后，会出现补位过程：

WAITING_IN_QUEUE
  ↓
MOVING_TO_FRONT
  ↓
WAITING_IN_QUEUE

七、P2 到 P1 自动补位机制

自动补位是本项目的一个核心功能。

传统串行方式可能是：

robot1 完成全部任务后
robot2 才开始移动

这种方式虽然安全，但效率较低，也不符合真实排队逻辑。

本项目采用半并行排队策略：

robot1 去 P1
robot2 去 P2
robot1 进入工作站后
robot2 从 P2 自动补位到 P1

这样 robot2 不需要一直停在起点，而是可以提前进入等待区。当 robot1 释放工作站资源后，robot2 能够更快进入工作站，提高任务连续性。

八、共享资源锁设计

多机器人访问同一个工作站时，如果只依赖 Nav2 局部避障，并不能从根本上解决资源冲突问题。

例如：

robot1 正在从 WORKSTATION 前往 EXIT
robot2 同时从 P1 前往 WORKSTATION

这两个路径可能在工作站入口附近交叉，容易产生拥堵或碰撞风险。

因此，本项目将工作站和出口通道抽象为有限共享资源，并设计了两个资源锁：

WORKSTATION_LOCK
EXIT_LANE_LOCK

1. WORKSTATION_LOCK

工作站锁用于控制机器人是否可以进入工作站。

规则如下：

只有队首机器人可以申请工作站锁
工作站锁为空时，机器人才能进入工作站
机器人到达出口后，才释放工作站锁

2. EXIT_LANE_LOCK

出口通道锁用于控制机器人是否可以占用出站通道。

规则如下：

机器人从工作站离开前，需要申请出口通道锁
机器人到达出口后，释放出口通道锁
下一台机器人只有在出口通道释放后才允许进入工作站

这样可以避免“前车出站、后车进站”同时发生造成冲突。

九、失败恢复机制

多机器人系统不能假设每一次导航都成功。

在实际运行中，可能出现以下问题：

目标点不可达
局部规划失败
机器人被障碍物阻塞
TF 暂时异常
Nav2 action 返回失败

如果不处理这些情况，某台机器人可能会永久占用资源锁，导致整个队列卡死。

因此，本项目增加了失败恢复机制。

当机器人导航失败时，系统不会立即终止任务，而是进入：

RECOVERING

恢复流程如下：

导航失败
↓
进入 RECOVERING 状态
↓
等待 retry_delay 秒
↓
重新发送上一个目标
↓
如果成功，继续原任务流程
↓
如果超过 max_retry_count，标记为 FAILED
↓
释放其占用的资源锁

这样可以保证即使某台机器人失败，也不会永久阻塞整个系统。

十、状态话题与 RViz 可视化

为了方便观察系统运行状态，本项目增加了状态话题和 RViz Marker 可视化。

1. 状态话题

状态话题为：

/queue_manager/status

可以通过以下命令查看：

ros2 topic echo /queue_manager/status

该话题会输出：

当前等待队列
robot1 当前状态
robot2 当前状态
工作站锁占用情况
出口通道锁占用情况
任务执行状态

2. RViz Marker 可视化

Marker 话题为：

/queue_manager/markers

在 RViz 中添加：

Add -> By topic -> /queue_manager/markers -> MarkerArray

可以看到：

P1 / P2 / P3 等待点
WORKSTATION 工作站
EXIT_LEFT / EXIT_RIGHT 出口点
机器人当前调度状态
资源锁占用信息
队列状态文本

这样可以非常直观地展示多机器人调度过程。

十一、CSV 实验日志统计

为了评估调度效果，本项目增加了 CSV 实验日志功能。

程序运行后会自动生成：

queue_result.csv

主要字段包括：

robot_name,
start_time,
wait_arrival_time,
front_wait_arrival_time,
work_start_time,
work_finish_time,
exit_time,
total_time,
queue_wait_time,
front_move_time,
retry_count,
failure_reason,
recovery_status,
status

这些数据可以用于统计：

每台机器人总任务耗时
排队等待时间
补位耗时
是否发生失败恢复
最终任务状态

这使得项目不只是“能跑”，而是具备了一定的实验评估能力。

十二、配置文件化设计

为了避免将点位和参数写死在 Python 源码中，本项目将调度参数抽取到了：

queue_config.yaml

示例配置如下：

queue_order:
  - robot1
  - robot2

front_wait_slot: P1

wait_slots:
  P1: [1.20, -0.65, 0.00]
  P2: [0.00, -0.65, 0.00]
  P3: [-1.20, -0.65, 0.00]

workstation:
  pose: [3.00, -0.20, 0.00]
  work_duration: 5.0

robots:
  - name: robot1
    initial_pose: [-3.50, 0.50, 0.00]
    wait_slot: P1
    exit_pose: [0.35, -2.00, -1.57]

  - name: robot2
    initial_pose: [-3.50, -0.50, 0.00]
    wait_slot: P2
    exit_pose: [1.25, -2.00, -1.57]

recovery:
  max_retry_count: 2
  retry_delay: 3.0

logging:
  result_csv_path: queue_result.csv

通过配置文件可以方便修改：

机器人数量
等待点位置
工作站位置
出口点位置
作业时间
失败重试次数
CSV 日志路径

这使项目具有更好的扩展性和工程可维护性。

十三、运行方法

1. 编译工作空间

cd ~/ros2test/duoji/chapt7_ws
source /opt/ros/humble/setup.bash
colcon build --symlink-install
source install/setup.bash

2. 启动 Gazebo 双机器人仿真

ros2 launch fishbot_description gazebo_sim.launch.py

3. 启动 robot1 Nav2

ros2 launch fishbot_navigation2 nav2_robot1.launch.py

4. 启动 robot2 Nav2

ros2 launch fishbot_navigation2 nav2_robot2.launch.py

5. 启动 queue_manager

ros2 run queue_manager multi_robot_queue_demo

十四、关键检查命令

检查 Nav2 action server：

ros2 action list | grep navigate

期望看到：

/robot1/navigate_to_pose
/robot2/navigate_to_pose

检查 robot1 话题：

ros2 topic list | grep robot1

检查 robot2 话题：

ros2 topic list | grep robot2

检查状态话题：

ros2 topic echo /queue_manager/status

检查 Marker 话题：

ros2 topic list | grep queue_manager

十五、项目难点总结

1. 多机器人 namespace 配置

多机器人系统中，必须保证每台机器人的传感器、里程计、TF 和控制话题相互隔离，否则 Nav2 会出现话题混用和 TF 错乱问题。

2. 多实例 Nav2 启动

每台机器人都需要独立启动一套 Nav2，包括 AMCL、costmap、planner、controller 和 bt_navigator，并生成独立的 action server。

3. 调度层与导航层解耦

queue_manager 不直接控制速度，而是通过 Nav2 action 发送目标点，保持了系统结构清晰。

4. 共享资源冲突处理

对于共享工作站，不能只依赖局部避障，还需要从任务调度层判断资源是否可用。

5. 失败恢复与防卡死

如果机器人失败后不释放资源锁，会导致后续机器人永久等待，因此需要设计失败恢复和资源释放机制。

十六、项目亮点

本项目的主要亮点包括：

1. 实现 ROS2 多机器人 namespace 隔离
2. 搭建双机器人独立 Nav2 导航系统
3. 设计 queue_manager 多机器人调度节点
4. 实现 P2 到 P1 自动补位机制
5. 引入工作站锁和出口通道锁
6. 支持导航失败自动重试
7. 支持异常机器人释放资源
8. 支持 RViz Marker 可视化
9. 支持 CSV 实验日志统计
10. 支持 YAML 配置文件化

相比单纯调用 Nav2 发目标点，本项目更强调多机器人任务调度、行为决策和共享资源管理。

十七、项目适用场景

该项目可以扩展应用到：

工厂 AGV 排队上料
仓储机器人排队取货
服务机器人共享通道通行
多机器人共享充电桩
多机器人共享工作站
移动机器人任务调度教学实验

十八、后续优化方向

目前项目完成的是双机器人共享单工作站调度，后续可以继续扩展：

扩展到三台及以上机器人
支持多个工作站
增加任务优先级
增加动态任务插入与取消
增加机器人间实时距离约束
增加死锁检测与解除机制
接入真实机器人多机 ROS2 网络
增加 Web 调度监控界面
将资源锁扩展为更细粒度的区域锁

十九、项目总结

本项目基于 ROS2 Humble 和 Nav2 搭建了一个多机器人排队作业与共享资源调度系统。底层通过 namespace 隔离实现多机器人独立导航，上层通过 queue_manager 状态机实现等待队列、自动补位、工作站资源锁、出口通道锁、失败恢复、状态发布和日志统计。

通过该项目，我进一步理解了 Nav2 在多机器人系统中的使用方式，也加深了对机器人任务调度、行为决策、共享资源管理和系统工程化的认识。

该项目不仅是一个多机器人仿真 demo，更是一个面向复杂场景的调度决策系统雏形，为后续扩展多机器人协同、任务分配和智能调度提供了基础。

二十、GitHub 地址

项目已开源到 GitHub：

https://github.com/gghzz/ros2-nav2-multi-robot-queue-scheduling

欢迎交流、学习和改进。

AtomGit开源社区

AtomGit 是由开放原子开源基金会联合 CSDN 等生态伙伴共同推出的新一代开源与人工智能协作平台。平台坚持“开放、中立、公益”的理念，把代码托管、模型共享、数据集托管、智能体开发体验和算力服务整合在一起，为开发者提供从开发、训练到部署的一站式体验。

更多推荐

「2026国际AI黑客松」选手招募

AtomGit开源社区

稀缺真机｜AtomGit × 启元机器人具身智能原创内容征文活动

AtomGit开源社区

南京大学、京东等开源 VideoJev： LVBench 73.4%，每题约 1.5 秒

AtomGit开源社区

所有评论(0)

查看更多评论

自律少年王风帆

@2303_81021740

已为社区贡献1条内容

基于 ROS2 Nav2 的多机器人排队作业与共享资源调度系统

自律少年王风帆

基于 ROS2 Nav2 的多机器人排队作业与共享资源调度系统

一、项目背景

二、项目简介

三、系统整体架构

1. 仿真与机器人模型层

2. 单机器人导航层

3. 多机器人调度层

四、多机器人 namespace 隔离

五、排队作业场景设计

六、queue_manager 状态机设计

七、P2 到 P1 自动补位机制

八、共享资源锁设计

1. WORKSTATION_LOCK

2. EXIT_LANE_LOCK

九、失败恢复机制

十、状态话题与 RViz 可视化

1. 状态话题

2. RViz Marker 可视化

十一、CSV 实验日志统计

十二、配置文件化设计

十三、运行方法

1. 编译工作空间

2. 启动 Gazebo 双机器人仿真

3. 启动 robot1 Nav2

4. 启动 robot2 Nav2

5. 启动 queue_manager

十四、关键检查命令

十五、项目难点总结

1. 多机器人 namespace 配置

2. 多实例 Nav2 启动

3. 调度层与导航层解耦

4. 共享资源冲突处理

5. 失败恢复与防卡死

十六、项目亮点

十七、项目适用场景

十八、后续优化方向

十九、项目总结

二十、GitHub 地址

所有评论(0)

温馨提示：您尚未绑定手机号

自律少年王风帆