4、动态窗口法（DWA）原理：速度空间搜索、轨迹预测、评价函数设计
动态窗口法，圈内人喜欢叫它 DWA。说实话，这是我在实际项目里用得最多的局部规划算法之一。为什么？因为它简单、直观，而且效果真的不错。你想想看，一个机器人要在复杂环境里跑起来，最怕什么？怕撞墙、怕绕不出去、怕速度忽快忽慢让人头晕。DWA 就是专门解决这些问题的。
我个人习惯把 DWA 拆成三个核心模块来理解：速度空间搜索、轨迹预测、评价函数设计。咱们一个一个聊。
4.1 速度空间搜索：到底哪些速度能用？
先问一个问题：机器人当前能用的速度有多少种？理论上无限多，对吧？但实际上，受限于电机性能、刹车距离、障碍物位置，真正能用的速度其实就那么一小撮。这一小撮，就是所谓的「动态窗口」。
我刚开始做 DWA 时犯过一个低级错误——把整个速度空间都搜了一遍。结果呢？计算量爆炸，机器人反应慢得像树懒。后来才明白，动态窗口的核心思想就是「只考虑当前时刻可达的速度」。
具体来说，速度空间搜索要过三关：
- 第一关：电机极限。机器人能跑多快？加速度多大？这是硬约束。比如你家的扫地机器人，最大线速度可能就 0.5 m/s，角速度也就 1.0 rad/s。超出这个范围，电机根本转不到。
- 第二关：刹车距离。这是安全底线。假设机器人以速度 v 往前冲，突然发现前面有障碍物，它能不能刹住？如果刹不住，这个速度就不能用。我记得有一次在实验室测试，忘了考虑刹车距离，机器人直接怼墙上了……嗯，从那以后我再也不敢跳过这一步。
- 第三关：动态窗口。这是最核心的一步。考虑电机响应时间，在当前速度下，Δt 时间内能加速或减速到多少？说白了，就是「下一时刻我能跑到什么速度」。
这三关筛下来，剩下的速度组合通常只有几十到几百个。你想想看，从无限到有限，计算量一下子就降下来了。
核心要点：速度空间搜索不是搜所有速度，而是搜「当前时刻真正能用」的速度。三关筛选：电机极限 → 刹车距离 → 动态窗口。
4.2 轨迹预测：选一个速度，跑一段看看
好，现在我们有了一堆候选速度 (v, ω)。接下来要干嘛？模拟一下，如果机器人用这个速度跑一小段时间，会走出什么样的轨迹。
轨迹预测其实不复杂。我常用的模型是圆弧模型：假设机器人在 Δt 时间内做匀速圆周运动。线速度 v 决定圆弧的「大小」，角速度 ω 决定圆弧的「弯曲程度」。如果 ω = 0，那就是直线；如果 v = 0，那就是原地旋转。
具体怎么算？看代码：
def predict_trajectory(x, y, theta, v, w, dt, steps):
trajectory = []
for _ in range(steps):
x += v * cos(theta) * dt
y += v * sin(theta) * dt
theta += w * dt
trajectory.append((x, y, theta))
return trajectory
这段代码很简单，但够用。每个候选速度都会生成一条轨迹。然后呢？把这些轨迹画出来，看看哪条不撞墙、哪条离目标近、哪条跑得顺。
这里有个坑，我踩过：预测步数不能太多，也不能太少。步数太多，计算量大，而且预测越远越不准；步数太少，来不及反应障碍物。我个人习惯设 3-5 秒的预测时长，步长 0.1 秒，也就是 30-50 步。这个参数可以根据机器人实际速度调一调。
小技巧：如果机器人速度很快，可以适当增加预测步数；如果环境障碍物密集，可以减小步长提高分辨率。没有万能参数，多试几次就找到感觉了。
4.3 评价函数设计：怎么选「最好」的那条轨迹？
轨迹预测完了，每个候选速度都对应一条轨迹。问题来了：哪条最好？这就需要评价函数来打分。
DWA 的评价函数通常包含三个子项：
| 子项 | 含义 | 我的经验 |
|---|---|---|
| 方位角评价 | 轨迹终点朝向与目标方向的偏差 | 权重别太大，否则机器人容易「头对头」卡住 |
| 障碍物评价 | 轨迹与最近障碍物的距离 | 这个最重要，安全第一 |
| 速度评价 | 轨迹对应的速度大小 | 鼓励快跑，但不能牺牲安全 |
最终得分是这三个子项的加权和：
score = α * heading_cost + β * obstacle_cost + γ * velocity_cost
α、β、γ 是权重系数。怎么调？说实话，没有标准答案。我一般先设 β 最大（安全优先），然后根据实际表现微调 α 和 γ。如果机器人老是在原地打转，就加大 α；如果跑得太慢，就加大 γ。
我曾经在一个仓储机器人项目里，把 β 设得太小，结果机器人贴着障碍物走，差点把货架撞翻。后来我把 β 调大了两倍，机器人就老实多了。嗯，这个教训挺深刻的。
注意：三个子项的量纲不同，一定要先归一化再加权。否则速度项可能因为数值大而主导整个评价，导致机器人只顾快跑不顾安全。
4.4 避坑指南：我踩过的那些坑
做 DWA 这么多年，我总结了几条血泪教训：
- 速度采样分辨率要合适。太粗了，找不到好路径；太细了，计算量爆炸。我一般线速度采 10-20 个点，角速度采 20-30 个点，总共 200-600 个候选速度，够用。
- 别忘了考虑机器人形状。很多教程把机器人当质点处理，但实际机器人是有大小的。我建议用多个圆或者矩形包络来检测碰撞，别偷懒。
- 动态窗口要实时更新。每控制周期都要重新计算一次动态窗口，因为机器人的状态一直在变。别想着算一次用到底，那是找死。
- 评价函数权重不是一次调好的。不同场景需要不同权重。比如狭窄走廊里，障碍物权重应该更大；开阔地带，速度权重可以大一些。我习惯做几组预设权重，运行时根据环境自动切换。
最后说一句，DWA 不是万能的。它在动态环境里表现不错，但在 U 形陷阱、狭窄通道等场景下容易卡住。这时候就需要结合全局规划或者重规划机制来弥补。不过那是后面章节的内容了，咱们到时候再细聊。
一句话总结：DWA 的核心就是「在动态窗口里搜速度 → 预测轨迹 → 打分选最优」。简单、高效、实用，是局部路径规划的经典之作。