3、局部路径规划：DWA与TEB算法对比
局部路径规划，说白了就是机器人“眼前这几步怎么走”。全局规划告诉你从A到B，但路上突然冒出个纸箱、一个人，或者走廊变窄了，这时候就得靠局部规划来救场。
目前最主流的两个方案，就是DWA（动态窗口法）和TEB（时间弹性带）。我这些年做过的项目里，这两套算法几乎包圆了所有场景。今天咱们就把它们掰开揉碎了讲清楚。
3.1 DWA与TEB的核心差异
先给个直观对比。DWA的思路是“我看看当前速度下，未来几秒能走到哪，选一条最好的”。TEB的思路是“我先把整条路径拉成一条橡皮筋，然后优化它的形状和时间”。
我刚开始接触TEB时总觉得它太复杂，后来在工厂项目里遇到一个U型弯道，DWA死活过不去，换成TEB一把就过了。嗯，从那以后我再也不敢小看TEB了。
| 对比维度 | DWA | TEB |
|---|---|---|
| 核心思想 | 速度空间采样 + 轨迹预测 | 路径拓扑优化 + 时间分配 |
| 优化对象 | 线速度、角速度 | 路径点序列 + 时间间隔 |
| 计算方式 | 离散采样 + 暴力枚举 | 图优化（g2o / ceres） |
| 实时性 | 高（2-10ms） | 中（10-50ms） |
| 复杂环境 | 一般 | 优秀（窄通道、U型弯） |
| 调参难度 | 中等 | 较高 |
选哪个？我的经验是：差速底盘、室内走廊、要求高实时性，用DWA。全向底盘、复杂障碍物、需要精确通过狭窄区域，用TEB。
核心结论：DWA是“短视但果断”，TEB是“远视但犹豫”。没有好坏，只有场景适配。
3.2 动态窗口法核心参数
DWA的“动态窗口”是什么意思？就是只考虑当前速度附近的一个小范围。你想想看，机器人不可能瞬间从0加速到1m/s，所以DWA只在这个“可达速度窗口”里采样。
我调DWA踩过最大的坑，就是加速度限制设得太宽松。有一次在实验室里跑，机器人急停时直接滑出去半米，差点撞到设备。后来我把加速度参数砍了一半，问题就解决了。
3.2.1 速度空间参数
- max_vel_x / min_vel_x：最大/最小线速度。室内一般设0.5~1.0 m/s，别贪快。
- max_vel_theta：最大角速度。差速底盘建议1.0~2.0 rad/s，太快容易甩尾。
- acc_lim_x / acc_lim_theta：线加速度和角加速度。这是关键，设大了急停不稳，设小了反应迟钝。
我的调参习惯：先把加速度设到理论值的60%，跑起来看急停效果，再慢慢往上加。安全第一。
3.2.2 轨迹评价参数
DWA每采样一组速度，就生成一条轨迹，然后打分。分数由三个子项加权求和：
cost = heading_cost * weight_heading
+ dist_cost * weight_dist
+ vel_cost * weight_vel- weight_heading：朝向目标权重。值越大，机器人越“直着走”。我一般设3.0~5.0。
- weight_dist：障碍物距离权重。值越大，机器人越“躲着走”。设2.0~4.0。
- weight_vel：速度权重。值越大，机器人越“快着走”。设0.5~1.5。
这三个权重的平衡，说白了就是“快、准、稳”的三角博弈。我曾经在一个仓储项目里，为了追求速度把weight_vel拉到3.0，结果机器人贴着障碍物飞驰，吓得我赶紧改回来。
3.2.3 仿真时间参数
- sim_time：轨迹预测时长。默认1.5~2.0秒。太长计算量大，太短看不到前方障碍。
- sim_granularity：轨迹离散步长。默认0.025米，一般不用动。
- vx_samples / vtheta_samples：速度采样数量。默认6~10个，多了精度高但慢。
注意：sim_time设得太大，机器人会“看到”远处的障碍然后提前减速，导致走走停停。我建议从1.5秒开始调，根据实际反应微调。
3.3 TEB轨迹优化参数
TEB的核心是把路径点序列看作一条“弹性带”，每个点之间有一个时间间隔。优化目标就是让这条带子最短、最平滑、最安全，同时满足动力学约束。
我第一次用TEB时，被它的参数数量吓到了——几十个参数，光看文档就看了两天。但后来发现，真正需要调的其实就那几个。
3.3.1 轨迹长度与分辨率
- min_samples / max_samples：路径点数量下限/上限。默认10~500。点太少路径粗糙，点太多计算爆炸。
- global_plan_overwrite_orientation：是否覆盖全局规划的朝向。建议true，否则机器人可能“横着走”。
- allow_init_with_backwards_motion：是否允许倒车起步。我一般关掉，除非做泊车场景。
3.3.2 优化权重参数
TEB的代价函数比DWA复杂得多，但核心权重就这几个：
| 参数名 | 作用 | 推荐值 | 我的经验 |
|---|---|---|---|
| weight_optimaltime | 时间最优权重 | 1.0~5.0 | 设大了机器人猛冲，设小了慢悠悠 |
| weight_obstacle | 障碍物避让权重 | 50~100 | 这个值要足够大，否则会撞 |
| weight_kinematics_forward_drive | 正向行驶约束 | 1000~5000 | 差速底盘设大点，防止倒车 |
| weight_velocity | 速度约束权重 | 10~50 | 限制最大速度用 |
| weight_acceleration | 加速度约束权重 | 10~50 | 防止急加速急减速 |
避坑指南：我曾经把weight_obstacle设成10，结果机器人直接穿过障碍物。后来我才意识到，TEB的障碍物权重必须比DWA大一个数量级，因为它是在连续空间里优化，不像DWA那样离散采样有“安全边界”。
3.3.3 优化器参数
- no_inner_iterations / no_outer_iterations：内层/外层迭代次数。默认5/5，复杂场景可以提到10/10。
- teb_autosize：是否自动调整路径点数量。建议true，省心。
- dt_ref / dt_hysteresis：参考时间间隔/迟滞。默认0.3/0.1秒。设小了路径点密集，设大了路径粗糙。
嗯，这里要注意：迭代次数不是越多越好。我试过把内外迭代都设成20，结果每个周期要算200ms，机器人反应明显变慢。后来我固定在外5内5，效果已经够用了。
3.4 DWA与TEB的选型决策流程
说了这么多，到底怎么选？我画了一张决策图，帮你快速判断。
3.5 实战调参建议
最后给几条实在的建议。这些是我在项目里摔出来的经验：
- 先跑起来，再调优。别一开始就追求完美参数。先用默认参数让机器人能动，然后观察它的“毛病”——是太慢？太抖？还是太近？
- 一次只改一个参数。我见过太多人同时改五六个参数，然后不知道问题出在哪。改一个，跑一圈，记下来。
- 用仿真先验证。Gazebo或Stage里先跑一遍，确认参数不会导致危险行为，再上真车。
- 记录参数组合。我习惯用YAML文件管理不同场景的参数集，比如warehouse.yaml、office.yaml、corridor.yaml。切换场景时直接加载，不用重新调。
小技巧：如果你不确定某个参数的作用，把它设成极端值（比如0或10000），看机器人怎么反应。这比读文档直观多了。
好了，DWA和TEB的核心内容就这些。记住，参数是死的，场景是活的。多试、多记、多总结，你也能成为局部规划调参的老手。