从零开始掌握局部路径规划算法DWA（动态窗口法）

1. 概述

DWA(Dynamic Window Approach)是一种基于速度空间采样的局部避障路径规划算法，特别适用于像轮式机器人这样在动态环境中需要实时避障和导航的场景。

在SLAM或机器人导航中，全局规划器（如A*、Dijkstra）会给出一条从起点到终点的全局最优路径，然而，真实环境是动态变化的，可能会出现全局地图中未记录的障碍物（如行人、临时放置的椅子），或者由于机器人位姿估计误差导致其偏离了全局路径。 DWA的任务就是在全局路径的指导下，实时地计算出一个安全、平滑且尽可能贴合全局路径的局部速度指令（线速度和角速度），使机器人能避开障碍物，并朝着目标前进。

2. 核心原理

DWA之所以叫“动态窗口”，是因为采样空间不是固定的，而是根据机器人当前的动态性能（如当前速度、加速度）实时计算出的一个窗口，其核心思想可以概括为三个步骤：

1. 离散采样： 机器人在当前状态下，由于其物理限制（电机性能、惯性等），不可能瞬间达到任意速度，DWA会在一个可行的速度空间 (v, ω)（线速度和角速度）中采集大量候选速度对。
2. 轨迹推演： 对于每一个候选速度对 (v, ω)，模拟出机器人在未来一个短时间内（通常几秒钟）的运动轨迹。
3. 评价择优： 使用一个设计好的评价函数对所有这些模拟出的轨迹进行打分，选择得分最高的那条轨迹所对应的速度指令发送给机器人执行。

3. DWA算法的详细步骤

步骤一：建立机器人的运动学模型 DWA通常使用差分轮式机器人的运动学模型，给定一个速度指令 (v, ω) 和一段时间 Δt，可以推演下一时刻的位姿 (x, y, θ)。 假设在短时间内 (v, ω) 是恒定的，则轨迹是圆弧。

• 离散模型：

• 轨迹生成： 对一个候选速度 (v, ω)，通过多次迭代上述模型，就能得到一条未来轨迹，轨迹上的每个点都是一个未来的位姿。

步骤二：速度空间采样与动态窗口的生成 这是DWA算法的精髓， DWA通过一系列约束，将速度空间 (v, ω)缩小到一个很小的“动态窗口”内进行采样，极大地减少了计算量。

• 速度约束 (Vₘ)：机器人自身物理结构决定的最大最小速度。 v_min <= v <= v_max ω_min <= ω <= ω_max
• 加速度约束 (Vₐ) - 形成动态窗口：假设当前速度为 (v_c, ω_c)，最大加速度为 (a_v, a_ω)，最大减速度为 (d_v, d_ω)。 下一个周期内可达的线速度范围：[v_c - d_v * Δt, v_c + a_v * Δt] 下一个周期内可达的角速度范围：[ω_c - d_ω * Δt, ω_c + a_ω * Δt] 这个由加速度约束产生的窗口就是“动态窗口”，它随着当前速度的变化而变化。
• 安全约束 (Vₛ)：为了确保机器人不会撞上障碍物，必须要求机器人能够在其轨迹上及时停下来，要求机器人在碰到障碍物之前，其速度必须能减为0。 对于每个候选速度 (v, ω)，计算其到达最近障碍物的距离 dist(v, ω)，然后根据最大减速度 d_v 计算所需停车距离：v² / (2 * d_v)。 只有满足 v <= sqrt(2 * dist(v, ω) * d_v) 和 ω <= sqrt(2 * dist(v, ω) * d_ω) 的速度才是安全的。
• 最终的采样空间 V ：是上述三个约束空间的交集，DWA只在这个交集空间 Vₛ 内进行均匀或自适应采样。 V = Vₘ ∩ Vₐ ∩ Vₛ

步骤三：轨迹推演 对于动态窗口 V 内采样的每一个速度对 (v, ω)，使用步骤一中的运动学模型，推演出一条未来一段时间 T（称为模拟时间或前瞻时间）内的轨迹。

模拟时间 T 是一个关键参数： T 太短：机器人“目光短浅”，可能发现不了远一点的危险。 T 太长：计算量变大，且环境可能已发生变化，预测不准。

步骤四：评价函数与最优速度选择 这是DWA的决策核心，评价函数 G(v, ω) 用于量化每条轨迹的优劣，通常它是一个多目标加权函数，包含以下几个核心组成部分：

1. 朝向目标：衡量轨迹终点位姿的方向与目标点方向的接近程度，差值越小，得分越高。 score_heading = 180° - |θₜ - θₒ|，θₜ是终点朝向，θₒ是目标点方向，这驱使机器人朝向目标。
2. 安全距离：衡量轨迹与最近障碍物之间的距离，距离越远，安全系数越高，得分越高。 score_dist = min(在轨迹上所有点与障碍物的距离)。如果轨迹撞到障碍物，此分数为0甚至负无穷，会被直接剔除。
3. 速度：鼓励机器人以更快的速度前进，提高效率，速度绝对值越大，得分越高。 score_velocity = |v|
4. 贴合全局路径：为了不让机器人完全“自由探索”，需要用一个项来引导它贴合全局路径，可以计算轨迹终点与全局路径上最近点的距离，距离越小，得分越高。
5. 最终的评价函数是这些子项的加权和： G(v, ω) = α * score_heading + β * score_dist + γ * score_velocity + δ * score_path，其中 α, β, γ, δ 是权重系数，需要根据实际应用场景进行调整。例如，在障碍物密集区域，β（安全权重）应该设得更高；在开阔区域，γ（速度权重）可以设得更高。
6. 选择速度： 计算所有候选速度 (v, ω) 对应的 G(v, ω)，选择得分最高的那个速度对作为本控制周期的输出指令。

步骤五：循环执行 将选出的最优速度 (v_best, ω_best) 发送给机器人底层驱动执行，然后进入下一个控制周期，重新感知周围环境（获取最新的传感器数据），重复步骤二到步骤四。这样形成了一个“感知-规划-执行”的闭环，使机器人能够应对动态变化的环境。

4. 总结

在实际应用中，DWA常与其他的规划思想结合，例如与Timed-Elastic-Band (TEB) 等优化方法结合，或者引入更复杂的评价机制，以克服其局部最优的缺点。

• 计算高效：通过动态窗口大幅缩小采样空间，适合实时计算。
• 考虑动力学：考虑了机器人的加速度约束，生成的轨迹更平滑、更可行。
• 避障效果好：对动态和未知障碍物反应灵敏。
• 容易陷入局部最优：例如在狭窄走廊或U型障碍物中，可能因为“目光短浅”（模拟时间T短）而原地振荡。
• 参数调优复杂：权重参数 (α, β, γ, δ) 和模拟时间 T 对性能影响很大，需要精心调节。
• 非全局最优：是局部贪心算法，可能为了避开眼前小障碍而偏离全局最优路径很远。