基于 ROS2 Nav2 的移动机器人 TTC 动态避障系统

一、项目介绍

本文介绍一个基于 ROS2 Humble + Nav2 的移动机器人自主导航与动态障碍物避障项目。项目地址：

https://github.com/gghzz/ros2-nav2-ttc-dynamic-avoidance

本项目以 FishBot 差速移动机器人为实验平台，在 Gazebo 仿真环境中完成了移动机器人自主导航系统的搭建，包括 AMCL 定位、SmacPlanner2D 全局路径规划、DWB 局部控制、global/local costmap 配置以及 RViz 可视化验证。

在普通 Nav2 导航系统的基础上，本项目针对移动机器人在复杂环境中容易出现的几个问题进行了优化：

• 窄通道无法进入；

• 路径贴边，通行稳定性较差；

• 动态障碍物横穿时存在碰撞风险；

• 多个速度控制节点同时作用，导致速度链路混乱；

• 动态障碍物靠近时机器人缺少提前减速和停车等待机制。

为了解决这些问题，项目设计了三个自研节点：

dynamic_scan_filter.py
intersection_supervisor.py
intersection_speed_gate.py

并结合 Nav2 自带的 collision_monitor，构建了一套完整的动态障碍物提取、TTC 风险判断、速度门控和最终安全保护机制。

---

二、项目整体架构

系统整体架构如下：

Gazebo 仿真环境
        ↓
FishBot 差速移动机器人
        ↓
/scan /odom /tf
        ↓
Nav2 导航系统
        ↓
dynamic_scan_filter
        ↓
intersection_supervisor
        ↓
intersection_speed_gate
        ↓
collision_monitor
        ↓
fishbot_diff_drive_controller

系统可以分为以下几层：

感知层：
    LaserScan / Map / TF / Odom

动态障碍物提取层：
    dynamic_scan_filter

风险判断与决策层：
    intersection_supervisor

速度门控层：
    intersection_speed_gate

安全保护层：
    collision_monitor

执行层：
    fishbot_diff_drive_controller

普通 Nav2 主要负责全局路径规划、局部轨迹跟踪和目标点导航，而本项目重点是在 Nav2 基础上增加动态障碍物风险判断与速度控制逻辑，使机器人不仅能“到达目标点”，还能在动态障碍物出现时做出减速、停车、等待和恢复通行等行为。

---

三、为什么需要重新设计速度链路

在普通 Nav2 系统中，controller_server 或 velocity_smoother 通常会直接发布 /cmd_vel 给底盘控制器。

但是在加入动态避障、速度门控、安全监控等模块之后，如果多个节点都直接发布 /cmd_vel，就容易出现速度控制混乱的问题。例如：

• Nav2 认为机器人可以继续前进；

• 动态障碍物判断节点认为应该减速；

• collision_monitor 认为应该停车；

• 底盘同时收到多个来源的速度命令。

为了解决这个问题，本项目重新设计了速度控制链路：

Nav2 controller_server / behavior_server
        ↓
/cmd_vel_raw

velocity_smoother
        ↓
/cmd_vel

intersection_speed_gate
        ↓
/cmd_vel_risk

collision_monitor
        ↓
/cmd_vel_safe

fishbot_diff_drive_controller

各个话题的含义如下：

话题	含义
/cmd_vel_raw	Nav2 原始速度输出
/cmd_vel	velocity_smoother 平滑后的速度
/cmd_vel_risk	经过 TTC 风险判断后门控的速度
/cmd_vel_safe	collision_monitor 输出的最终安全速度

最终底盘控制器只订阅：

/cmd_vel_safe

这样可以保证底盘只接收最终经过风险判断和安全保护后的速度命令，避免多个节点抢占 /cmd_vel 导致控制混乱。

---

四、dynamic_scan_filter：动态障碍物提取节点

dynamic_scan_filter.py 的作用是从普通激光雷达数据中提取动态障碍物点。

普通 /scan 中既包含墙体、柱子、地图边界等静态障碍物，也包含行人、移动物体等动态障碍物。如果直接把所有激光点都交给动态避障模块处理，机器人可能会把墙体也误认为动态障碍物，导致频繁减速或停车。

因此，本项目设计了 dynamic_scan_filter 节点，用静态地图过滤激光雷达数据。

处理流程如下：

/scan + /map + TF
        ↓
将 LaserScan 点转换到 map 坐标系
        ↓
查询该点在静态地图中的占用状态
        ↓
过滤墙体、固定障碍物和地图边界
        ↓
保留疑似动态障碍物点
        ↓
发布 /dynamic_scan

该节点的核心作用是：

从原始激光雷达数据中分离出动态障碍物，使后续风险判断模块只关注移动障碍物。

这样可以降低静态障碍物对动态避障逻辑的干扰，也为后续 TTC 风险判断提供更干净的数据输入。

---

五、intersection_supervisor：TTC 风险判断节点

intersection_supervisor.py 是本项目的核心决策节点。

它主要负责根据动态障碍物的位置、运动方向以及机器人当前状态，判断是否存在碰撞风险，并输出对应的导航决策。

该节点主要订阅：

/dynamic_scan
/tf
/odom

主要输出：

/intersection_decision

输出状态包括：

NORMAL_NAVIGATION
SLOW_DOWN
YIELD_WAIT
PASS_INTERSECTION
RECOVERY

各状态含义如下：

状态	含义
NORMAL_NAVIGATION	正常导航
SLOW_DOWN	动态障碍物存在潜在风险，机器人减速观察
YIELD_WAIT	预测存在碰撞风险，机器人停车等待
PASS_INTERSECTION	判断可以通过，机器人继续前进
RECOVERY	长时间等待后的恢复状态

该节点不是简单地判断“前方有没有障碍物”，而是进一步判断障碍物是否会进入机器人通行区域，以及机器人和障碍物之间的距离是否正在缩小。

---

六、TTC 动态避障思想

TTC 的全称是 Time To Collision，即预计发生碰撞所需的时间。

在移动机器人动态避障中，TTC 可以用来判断：

如果机器人和动态障碍物继续按照当前运动趋势运动，多久之后可能发生冲突？

本项目中主要使用了两类 TTC 思想。

---

1. 动态障碍物到机器人通行路径的 TTC

第一类 TTC 用于判断动态障碍物多久后会进入机器人通行路径。

可以简单理解为：

TTC_path = distance_to_robot_path / obstacle_velocity_to_path

也就是：

TTC_path = 动态障碍物到机器人主通道的距离 / 动态障碍物朝向主通道的速度

例如在交叉路口场景中，机器人沿 x 方向前进，动态障碍物从侧面沿 y 方向横穿。

如果动态障碍物距离机器人通行区域很近，并且正在快速靠近主通道，那么系统会认为存在风险，输出：

SLOW_DOWN

或者：

YIELD_WAIT

机器人就会提前减速或停车等待，而不是等到障碍物已经非常接近时才被动刹车。

---

2. 机器人与动态障碍物之间的相对 TTC

仅判断动态障碍物是否离开机器人路径还不够。

因为在实际运动中，可能出现这种情况：

动态障碍物看起来已经远离主路径，
但机器人自身正在向障碍物靠近。

如果此时直接放行，仍然可能发生碰撞。

因此，本项目进一步引入了相对 TTC 思想：

relative_position = obstacle_position - robot_position
relative_velocity = obstacle_velocity - robot_velocity

系统会判断机器人和动态障碍物之间的距离是在增大还是减小。

核心判断逻辑是：

动态障碍物远离路径线 ≠ 一定安全

只有同时满足：

1. 动态障碍物已经通过机器人必经路径；
2. 动态障碍物与机器人之间的距离正在增大；

系统才认为机器人可以安全通过。

---

七、intersection_speed_gate：速度门控节点

intersection_speed_gate.py 用于将 supervisor 的高层决策真正作用到底盘速度上。

该节点主要订阅：

/cmd_vel
/intersection_decision

主要发布：

/cmd_vel_risk

控制逻辑如下：

NORMAL_NAVIGATION / PASS_INTERSECTION:
    原样输出速度

SLOW_DOWN:
    按比例降低线速度和角速度

YIELD_WAIT / RECOVERY:
    输出 0 速度

也就是说，intersection_supervisor 负责判断“应该怎么做”，而 intersection_speed_gate 负责真正修改机器人速度。

例如：

当系统输出：

SLOW_DOWN

速度门控节点会降低机器人当前速度，使机器人进入减速观察状态。

当系统输出：

YIELD_WAIT

速度门控节点会直接输出 0 速度，使机器人停车等待。

当系统输出：

PASS_INTERSECTION

速度门控节点恢复正常速度输出，机器人继续执行 Nav2 导航任务。

---

八、collision_monitor：最后一层安全保护

除了自研 TTC 风险判断和速度门控节点，本项目还引入了 Nav2 的 collision_monitor 作为最后一层安全保护。

在本项目中，collision_monitor 的输入和输出为：

输入速度：/cmd_vel_risk
输入障碍物：/dynamic_scan
输出速度：/cmd_vel_safe

它的作用不是做复杂的预测和决策，而是在极近距离风险下进行兜底保护。

可以理解为：

intersection_supervisor：提前判断风险
intersection_speed_gate：根据风险修改速度
collision_monitor：最后负责安全裁决

这样系统既具备提前预测能力，又保留了近距离紧急保护能力。

---

九、通过承诺区设计

在动态障碍物横穿场景中，有一个非常重要的问题：

机器人如果已经进入冲突区域，此时突然停车，可能反而更危险。

例如机器人已经进入交叉口，如果这时突然急停，可能正好停在动态障碍物的运动路径上，导致被侧向撞击。

因此，本项目设计了“通过承诺区”的思想。

逻辑如下：

机器人尚未进入动态障碍物运动区域：
    可以减速或停车等待

机器人运动过程中已经进入动态障碍物运动区域：
    进入 PASS_INTERSECTION，继续通过

机器人启动时已经位于动态障碍物运动区域：
    先进行相对 TTC 判断，确认安全后再通过

这个设计可以避免机器人在冲突区域中途急停，提高动态场景中的通行稳定性。

---

十、窄通道通行优化

除了动态障碍物避障，本项目还针对 Nav2 在窄通道中的通行问题进行了优化。

在 Nav2 中，机器人是否能稳定进入窄通道，通常与以下因素有关：

robot_radius / footprint
inflation_radius
cost_scaling_factor
DWB critic 权重
路径平滑参数

如果 inflation_radius 设置过大，窄通道两侧墙体的膨胀层可能会连接在一起，导致通道在 costmap 中被认为不可通行。

如果 BaseObstacle.scale 过大，局部控制器会过于保守，导致机器人不敢进入狭窄区域。

如果 PathAlign.scale 或 PathDist.scale 设置不合理，机器人可能出现贴边、抖动或者偏离全局路径的问题。

本项目的优化思路是：

1. 合理减小 inflation_radius，避免窄通道被膨胀层堵死；
2. 调整 cost_scaling_factor，使障碍物代价衰减更加合理；
3. 优化 DWB 中 PathDist、PathAlign、BaseObstacle 等 critic 权重；
4. 结合路径平滑，减少局部轨迹抖动；
5. 在安全和可通行性之间取得平衡。

最终实现机器人能够稳定进入窄通道，并降低贴边和局部控制抖动问题。

---

十一、项目运行环境

本项目运行环境如下：

Ubuntu 22.04
ROS2 Humble
Nav2
Gazebo Classic
RViz2
Python 3
CMake / colcon

---

十二、编译方法

进入 ROS2 工作空间：

cd ~/ros2_ws
colcon build --symlink-install
source install/setup.bash

如果只编译导航包：

colcon build --packages-select fishbot_navigation2 --symlink-install
source install/setup.bash

---

十三、启动方法

1. 启动 Gazebo 仿真环境

ros2 launch fishbot_description gazebo_sim.launch.py

2. 启动 Nav2 与动态避障系统

ros2 launch fishbot_navigation2 navigation2.launch.py

---

十四、关键话题检查

1. 检查自研节点是否启动

ros2 node list | grep dynamic
ros2 node list | grep intersection
ros2 node list | grep collision

期望看到：

/dynamic_scan_filter
/intersection_supervisor
/intersection_speed_gate
/collision_monitor

---

2. 检查速度链路

ros2 topic info /cmd_vel_raw -v
ros2 topic info /cmd_vel -v
ros2 topic info /cmd_vel_risk -v
ros2 topic info /cmd_vel_safe -v

期望速度链路如下：

/cmd_vel_raw:
  publisher: controller_server / behavior_server
  subscriber: velocity_smoother

/cmd_vel:
  publisher: velocity_smoother
  subscriber: intersection_speed_gate

/cmd_vel_risk:
  publisher: intersection_speed_gate
  subscriber: collision_monitor

/cmd_vel_safe:
  publisher: collision_monitor
  subscriber: fishbot_diff_drive_controller

---

3. 检查动态避障决策

ros2 topic echo /intersection_decision

可能输出：

data: "NORMAL_NAVIGATION"
data: "SLOW_DOWN"
data: "YIELD_WAIT"
data: "PASS_INTERSECTION"

---

十五、实验场景设计

本项目主要设计了以下几类实验场景。

1. 基础自主导航实验

验证机器人是否能够在 Gazebo 中完成：

定位
全局路径规划
局部轨迹跟踪
目标点到达

该实验用于验证 Nav2 基础导航链路是否正常。

---

2. 窄通道通行实验

通过调整 costmap inflation layer 和 DWB critic 权重，验证机器人是否能够稳定进入窄通道。

重点观察：

机器人是否能进入通道
是否贴边
是否出现局部抖动
是否频繁重新规划
是否因为膨胀层过大导致通道不可通行

---

3. 动态障碍物减速实验

当动态障碍物靠近机器人通行区域，但尚未达到停车风险阈值时，系统输出：

SLOW_DOWN

此时 intersection_speed_gate 降低机器人速度，使机器人减速观察。

---

4. TTC 停车等待实验

当系统预测机器人和动态障碍物可能发生冲突时，输出：

YIELD_WAIT

此时机器人停止前进，等待动态障碍物通过。

---

5. 障碍物离开后恢复通行实验

当动态障碍物已经通过机器人必经路径，并且与机器人之间的距离正在增大时，系统输出：

PASS_INTERSECTION

机器人恢复运动，继续执行原本的 Nav2 导航任务。

---

十六、项目效果

经过系统搭建和调试，本项目实现了以下效果：

1. 机器人能够完成基础 Nav2 自主导航；
2. 机器人能够稳定进入部分窄通道；
3. 动态障碍物靠近时机器人能够提前减速；
4. TTC 判断存在风险时机器人能够停车等待；
5. 动态障碍物离开后机器人能够恢复前进；
6. 底盘只接收最终安全速度 /cmd_vel_safe；
7. 避免多个速度源同时控制底盘；
8. 提高动态障碍物横穿场景下的通行稳定性。

相比只依赖 Nav2 原始局部规划器，本项目增加了动态障碍物预测和决策逻辑，使机器人在动态场景中具备更明确的“减速、等待、通过”行为。

---

十七、项目亮点总结

本项目的主要亮点如下：

1. 不只是调用 Nav2，而是在 Nav2 基础上进行了动态避障链路扩展；
2. 设计 dynamic_scan_filter，从激光雷达数据中提取动态障碍物；
3. 引入 TTC 思想，判断动态障碍物是否会进入机器人通行区域；
4. 引入相对 TTC，判断机器人和障碍物之间是否正在靠近；
5. 设计 intersection_supervisor，实现动态障碍物风险决策；
6. 设计 intersection_speed_gate，将决策结果作用到速度控制链路；
7. 重新设计 /cmd_vel_raw → /cmd_vel → /cmd_vel_risk → /cmd_vel_safe 多级速度链路；
8. 使用 collision_monitor 作为最后一层安全保护；
9. 针对窄通道问题优化 inflation layer 和 DWB critic 参数；
10. 区分“进入冲突区前停车”和“已经进入冲突区后继续通过”两种情况。

---

十八、当前项目局限性

目前系统主要面向结构化动态障碍物横穿场景，适合单个或少量规则运动障碍物的风险判断。

当前仍存在一些可以继续优化的方向：

1. 对多个动态障碍物的跟踪能力还可以进一步增强；
2. 当前预测模型相对简单，可以加入更完整的轨迹预测模块；
3. 动态障碍物预测结果还可以进一步融合到局部轨迹评价函数中；
4. 可以进一步对比 DWB、RPP、MPPI 在动态障碍物场景下的表现；
5. 可以增加 rosbag 实验记录；
6. 可以加入更完整的定量评估指标，例如通行时间、停车次数、最小安全距离等；
7. 可以在真实机器人平台上进一步验证参数鲁棒性。

---

十九、后续优化方向

后续可以从以下几个方向继续完善：

1. 增加多目标动态障碍物跟踪

当前主要关注结构化横穿场景，后续可以加入多目标跟踪模块，对多个动态障碍物分别估计位置、速度和运动方向。

---

2. 增加轨迹预测模块

可以在现有 TTC 判断基础上，进一步加入短时轨迹预测，根据障碍物历史位置预测未来运动轨迹。

---

3. 与局部规划器深度融合

目前速度门控主要通过修改速度命令实现风险控制。后续可以把动态障碍物预测结果融合进局部规划器的轨迹评价函数，使局部规划器在采样轨迹时主动避开高风险区域。

---

4. 加入定量实验指标

可以增加以下指标：

通行成功率
平均通行时间
平均停车次数
最小安全距离
动态障碍物触发次数
路径长度
速度变化平滑程度

这样可以让项目从“现象验证”进一步提升为“实验评估”。

---

二十、总结

本文介绍了一个基于 ROS2 Humble 与 Nav2 的移动机器人 TTC 动态避障系统。

项目在完成基础自主导航的基础上，针对窄通道通行、动态障碍物横穿、多速度源抢占控制等问题进行了系统优化。通过设计 dynamic_scan_filter、intersection_supervisor 和 intersection_speed_gate 三个自研节点，实现了动态障碍物提取、TTC 风险判断、相对 TTC 判断和速度门控控制。

同时，项目重新设计了机器人速度控制链路，使底盘只接收最终安全速度 /cmd_vel_safe，并结合 collision_monitor 构建最后一层安全保护。

该项目不仅能够展示 ROS2、Nav2、costmap、DWB、TF、LaserScan 等移动机器人导航基础能力，也体现了对动态障碍物避障、行为决策和系统集成的理解，适合作为移动机器人路径规划、ROS2 工程开发和机器人算法岗位的项目展示。