机器人 / 系统课时
局部采样轨迹绕开移动障碍
局部采样轨迹绕开移动障碍
这一课用 ROS 2 Jazzy 的 Nav2 DWB 控制器,让差速机器人在障碍物横穿路线时,从多条短时轨迹中选出当前可执行的速度。你将配置采样范围、足迹碰撞检查和路径评分,并验证减速、绕行与恢复循迹的行为。
学习目标
- 说清候选速度、短时轨迹、碰撞淘汰与加权评分的关系。
- 为最高线速度 0.25 m/s 的差速底盘配置 DWB 和激光局部代价图。
- 用可重复场景检查控制指令更新,并判断何时必须停车。
每个控制周期如何选动作
DWB(Dynamic Window Based Controller)接收当前位置、里程计速度、全局路径和局部代价图。假设控制频率为 20 Hz,每 0.05 秒重新计算一次;每次按速度边界采样线速度 和角速度 ,再按加速度限制向前模拟 1.5 秒的机器人运动轨迹。
处理顺序是:采样候选速度;用底盘运动学预测各条机器人轨迹;把沿途机器人足迹放到当前代价图检查碰撞;对剩余轨迹计算障碍物、路径偏离及目标偏差等代价;选择总分较低的候选速度。只下发当前周期的速度指令,而不是一次执行完整的 1.5 秒轨迹。

配置差速底盘的采样和评分
以下适用于 ROS 2 Jazzy 的 Nav2。假设机器人半径 0.22 米,前进速度上限 0.25 m/s,角速度上限 1.0 rad/s,激光雷达发布 /scan,底盘以 base_link 为基准、局部参考系为 odom。先确认 TF、里程计和激光数据均正常。
片段示例:把下列两个顶层配置合并到已有的 nav2_params.yaml,保留其他 Nav2 服务的参数;它们不能单独替代完整的导航配置。
controller_server:
ros__parameters:
# 20 Hz 控制与 DWB 插件选择
controller_frequency: 20.0
controller_plugins: ["FollowPath"]
progress_checker_plugins: ["progress_checker"]
goal_checker_plugins: ["goal_checker"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.2
movement_time_allowance: 10.0
goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.15
yaw_goal_tolerance: 0.2
stateful: true
FollowPath:
# 速度上限与底盘可实现的加减速度
plugin: "dwb_core::DWBLocalPlanner"
min_vel_x: 0.0
max_vel_x: 0.25
min_vel_y: 0.0
max_vel_y: 0.0
max_vel_theta: 1.0
min_speed_xy: 0.0
max_speed_xy: 0.25
min_speed_theta: 0.0
acc_lim_x: 0.5
decel_lim_x: -0.5
acc_lim_y: 0.0
decel_lim_y: 0.0
acc_lim_theta: 1.0
decel_lim_theta: -1.0
# 差速底盘横向速度固定为零
vx_samples: 12
vy_samples: 5
vtheta_samples: 20
sim_time: 1.5
linear_granularity: 0.05
angular_granularity: 0.025
# 足迹碰撞与路径、目标评分共同选取动作
critics: ["RotateToGoal", "Oscillation", "ObstacleFootprint", "PathAlign", "GoalAlign", "PathDist", "GoalDist"]
ObstacleFootprint.scale: 0.02
PathAlign.scale: 16.0
GoalAlign.scale: 12.0
PathDist.scale: 16.0
GoalDist.scale: 12.0
PathAlign.forward_point_distance: 0.1
GoalAlign.forward_point_distance: 0.1
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0
publish_local_plan: true
publish_trajectories: true
publish_evaluation: true
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 10.0
publish_frequency: 2.0
global_frame: odom
robot_base_frame: base_link
rolling_window: true
width: 4.0
height: 4.0
resolution: 0.05
robot_radius: 0.22
plugins: ["obstacle_layer", "inflation_layer"]
obstacle_layer:
# 标记新障碍,也沿空闲激光射线清除旧障碍
plugin: "nav2_costmap_2d::ObstacleLayer"
observation_sources: scan
scan:
topic: /scan
data_type: LaserScan
marking: true
clearing: true
obstacle_max_range: 2.5
raytrace_max_range: 3.0
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
inflation_radius: 0.35
cost_scaling_factor: 3.0vx_samples: 12 与 vtheta_samples: 20 给出最多约 240 种不同的差速目标速度组合(不计横向零速的重复采样);加速度限制、速度边界与重复轨迹会使有效候选更少。ObstacleFootprint 使用机器人足迹检查障碍,PathDist 和 GoalDist 让避障后仍倾向回到全局路径。marking 与 clearing 必须同时关注:只标记不清除,走开的障碍可能长期留在代价图中。
算一次绕行候选的代价
假设行人突然进入直线路径,当前代价图已标记障碍。为看清选择机制,暂时只计算 ObstacleFootprint、PathDist、GoalDist 三项,按上面的权重 0.02、16、12 加权,下表是教学假设数据,不是 DWB 实际调试输出。
| 候选目标速度(, ) | 三项原始分:障碍 / 路径 / 目标 | 简化总分 |
|---|---|---|
| 直行(0.25,0) | 足迹发生碰撞 | 淘汰 |
| 左绕(0.18,0.30) | 40 / 0.10 / 0.15 | 4.20 |
| 右绕(0.12,-0.30) | 55 / 0.14 / 0.18 | 5.50 |
左绕总分为 ,因此在这个简化比较中优先于右绕。实际 DWB 还会叠加 PathAlign、GoalAlign、Oscillation 等评分项,且每次都以最新代价图重算,不能把表格分数当成固定决策规则。
启动后的验证顺序
加载新参数并启动现有 Nav2 bringup,确认 /controller_server 处于活动状态。在终端查询采样和控制周期,命令应与配置一致:
ros2 param get /controller_server controller_frequency
ros2 param get /controller_server FollowPath.vx_samples预期输出:
Double value is: 20.0
Integer value is: 12如果节点运行在命名空间中,应将 /controller_server 改成实际完整节点名。然后在 RViz 2 左侧 Displays 选择 Add → By display type → Map,把 Topic 指向 /local_costmap/costmap,同时显示全局路径与机器人模型。用仿真障碍物或受控测试物横穿路径,不要直接以人员进行首次实机测试。
按顺序验收:障碍进入激光视野后出现在局部代价图;机器人改变速度或安全等待;障碍移开并被清除后恢复跟踪;全程没有足迹碰撞。不能因为最终到达目标,就忽略中途是否发生过近距离擦碰或危险加速。
移动障碍前容易失败的边界
如果机器人仍冲向横穿障碍,先检查 /scan 时间戳和 odom → base_link 变换,再检查代价图是否以约 10 Hz 更新、clearing 是否生效,最后才调 Critic 权重。碰撞已经被判定为不可行的轨迹,不应靠提高 GoalDist.scale 强行保留。
速度上限 0.25 m/s、可实现减速度 0.5 m/s²、感知到制动延迟按 0.2 秒估算时,停车所需距离约为 米,尚未计入定位误差、轮胎打滑和障碍自身移动。实际应额外留裕量,并用独立的安全停车机制处理突然闯入;DWB 的轨迹评分不能代替硬件急停或安全监控。