DWA/TEB 局部避障
一句话读懂：实验 3 里小车会"看地图走大路"，但路上突然滚来一个球、放下一只箱子怎么办？这节课专门练"开车时的随机应变"——用 DWB 局部规划器，让小车在行驶中实时躲开动态障碍，绕不过去还会自己转圈、后退重新想办法。
01问题引入：静态地图挡不住滚动的球
本项目的全局路径（ThetaStar）给出的是一条"战略路线"，真正把轮子转起来的是局部规划器（controller_server 里的 DWB）。它每 50 ms（控制器 20 Hz）就重新采样"接下来 1.5 秒怎么走"，一旦发现当前轨迹前有障碍，就地换一条——所以障碍物根本不需要出现在静态地图里。
02原理通俗讲解：先想"刹得住"，再打分挑最优
① 速度必须在 acc_lim（acc_lim_x 1.0 m/s²、acc_lim_theta 1.5 rad/s²）约束下"下一拍可达"；② 以它刹车必须能在撞上障碍前停住。过了两道闸门的 (vx, ω) 才进入采样范围——这就是"刹得住"的数学表达。
vx_samples: 20 个线速度 × vtheta_samples: 40 个角速度 = 上限 800 条候选轨迹（vy_samples 为 0，麦轮横移未启用）；每条用 sim_time: 1.5 推演未来 1.5 秒，在局部代价地图上逐点查代价。
DWB 把 DWA 模块化成可插拔 critics，本项目启用了 7 个：RotateToGoal / Oscillation / BaseObstacle / GoalAlign / PathAlign / PathDist / GoalDist——每项先归一化再乘 scale，总分最高的轨迹被执行。
TEB 把路径看成一根弹性带，用图优化同时调整轨迹点位置和到达时间（贴路径+避障+时间最短+平滑四目标）。DWA 快而糙只看眼前 1.5 秒；TEB 平滑可加时间约束但算力高。本项目默认 DWB，TEB 未安装，属知识扩展。
避障全链路：雷达 → voxel_layer 标记障碍 → 局部代价地图 5 Hz 更新 → controller（20 Hz）发现当前轨迹撞了 → 重采样绕行轨迹 → 绕过后按 PathDist 重新贴回全局路径。全程无需重新规划全局路径——就像高速上遇事故，现场变道绕过再并回原车道。
03关联知识点
只考虑当前刹得住的速度组合——这是它"安全"的根本；acc_lim 直接决定窗口大小。
PathDist / GoalDist / BaseObstacle / RotateToGoal 等 critics 加权打分，plugin 为 dwb_core::DWBLocalPlanner。
3 m×3 m 滚动窗口 + voxel 层（z_voxels 16）+ inflation 层（0.18 m），5 Hz 更新，动态避障全靠它。
0=空闲；1-252=离障碍越近代价越高（按 cost_scaling_factor 4.0 衰减）；254=障碍本体；255=致命不可入。
全局路径是"目标线路"（长，行为树 1 Hz 触发重规划）；局部轨迹是"接下来 1.5 秒怎么走"（短，20 Hz 重算）——别混。
障碍堵死通道时，controller 一直报失败，行为树转入恢复：清代价地图 → Spin 1.57 rad → Wait 5 s，重试 6 次仍不行则 ABORTED。（本车麦轮可横移，未配 backup）
04实验目标
- 理解 DWA/DWB 速度窗口思想与评价函数（critics）加权机制 —— 原理理解
- 理解 TEB 时间弹性带原理及其与 DWA 的差异 —— 原理理解
- 掌握 local_costmap（滚动窗口 / voxel / inflation）配置与 RViz 可视化 —— 流程掌握
- 独立完成"正前方放纸箱"动态避障三重复试验 —— 独立完成
- 通过单变量调参改善避障表现并记录"参数→现象"对照表 —— 独立完成
05实验环境与准备
| 类别 | 配置 |
|---|---|
| 硬件 | 具身智能实验箱 B2 / 小车 C1 + COIN-D6 雷达；地图（EXP-02）+ 定位（EXP-04）可用 |
| 系统 | Ubuntu 22.04 + ROS 2 Humble |
| 核心库 | Nav2 dwb_controller、nav2_costmap_2d |
| 障碍物 | 纸箱 / 泡沫块（低矮安全，便于测试） |
| 主入口 | navigation/car1_navigation/config/nav2_params.yaml（controller_server / local_costmap / behavior_server 段） |
06动画讲解：采样轨迹打分与代价地图膨胀
图 5-1：蓝虚线是全局路径；灰色小车按 sim_time 1.5 推演出多条候选轨迹，穿过膨胀圈（inflation 0.18）的轨迹被 BaseObstacle 打上天文数字代价直接淘汰，"绕行且与原路对齐"的绿轨迹总分最高被选中——绕过后 PathDist 又把它拉回全局路径。障碍本体 cost 254（黑块），周围灰蓝渐变是膨胀出的 1-252 代价区。
07互动讲解：膨胀半径 × 代价权重调参台
| critic | 作用 | 本项目 scale |
|---|---|---|
| PathDist | 离全局路径的横向偏差 | 32.0 |
| PathAlign | 轨迹方向与全局路径对齐程度 | 32.0 |
| GoalDist | 离目标点的距离 | 24.0 |
| GoalAlign | 轨迹方向与目标朝向对齐 | 24.0 |
| BaseObstacle | 轨迹是否撞障碍（查代价地图原始 cost） | 0.02 |
| RotateToGoal | 到目标时转正的效率 | 32.0 |
| Oscillation | 惩罚来回震荡（防抖动） | 未单独设 scale，靠默认参数生效 |
上表 7 项与本项目 nav2_params.yaml 的 critics 列表一一对应；顺序即打分顺序，顺序本身不影响总分但影响短路的判定时机。
08实操步骤：纸箱前方 1 m 动态避障三重复试验
# ① 启动导航（含定位），map 指向 EXP-02 保存的地图名 ros2 launch car1_navigation navigation.launch.py map:=map_01 # ② 命令行下发穿过房间的目标（也可用 RViz 工具栏 Nav2 Goal） ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose \ "{pose: {header: {frame_id: 'map'}, pose: {position: {x: 2.0, y: 1.0}, orientation: {w: 1.0}}}}" # ③ 实时观察速度变化（减速→绕行→回升；平滑后的名字是 controller/cmd_vel） ros2 topic echo /controller/cmd_vel # ④ RViz 添加 Display → By topic → /local_costmap/costmap（Color 显示） # 放纸箱后应看到障碍处黑圈（254）与周围 1-252 渐变 # ⑤ 调参：改 nav2_params.yaml 的 controller_server / local_costmap 段后必须整栈重启 ros2 launch car1_navigation navigation.launch.py map:=map_01
09重点与易错点
| 坑 | 现象 | 解决 |
|---|---|---|
| 以为 DWA 考虑所有速度 | 误解安全来源 | 只考虑刹得住的（acc_lim 决定窗口），这是安全根本 |
| 避障时撞上箱子 | 擦到纸箱 | 箱子离机器人太近（进局部窗口才被发现）或速度太快刹不住；放远一点 / 降 max_vel_x |
| 绕过后"迷路"乱转 | 回不到原路径 | PathDist 权重不足；调大 PathDist.scale / PathAlign.scale |
| 行驶抖动左右摆 | 原地哆嗦 | 调大 Oscillation 惩罚；降低 sim_time；降速 |
| 贴障碍太近 | 擦边走 | 调大 inflation_radius（默认 0.18 已比半宽 0.135 多留 4.5 cm，再大窄道会判不可行） |
| 改参没重启 | 调了没效果 | controller_server 参数启动时读取，改后必须重启 navigation.launch |
| 一次改多个参数 | 无法归因 | 单变量对比，每轮记录"当前参数→当前现象" |
| 窄通道过不去 | 50 cm 通不过 | 车宽 0.27 m + 两侧各 0.18 m 膨胀 ≈ 需要 0.63 m 净宽——属正常不可行，换宽通道或改小 inflation_radius |
- 全程人站开阔侧、手远离小车路径，随时准备按急停
- 0.5 m 突现测试留够刹车距离：acc_lim_x 1.0 决定减速能力，小车不是无限刹车性能
- 每次调参前记录"参数→现象"，便于对比与回退