文章目录
- 前言
- 一、输入输出
-
- 输入
- 输出
- 二、逻辑
-
- 第一步：构建“动态窗口”
- 第二步：采样与轨迹推演
- 第三步：多维度评分
- 第四步：选择最优轨迹并输出
- 第五步：循环
- 总结
前言
DWA算法在机器人控制领域经常用到，尤其是nav2开源算法对其进行了升级成了DWB。 本文讲解一下这个DWA控制算法。
很多人把DWA划分在局部路径规划中，但我还是习惯把它当成一个控制算法来讲。
DWA是为了差速模型而设计的，不适用于阿克曼等模型。
一、输入输出
老规矩，还是先讲输入输出。
输入
DWA的输入主要来自三个部分： 当前机器人状态 (Odometry) 全局路径参考 (Global Plan) 局部环境感知 (Local Costmap)
输出
线速度 角速度
二、逻辑
提醒一下，全程要记得输入输出。 从输入到输出，DWA的计算可以分为五步。
第一步：构建“动态窗口”
既然是动态窗口控制算法，那首先就要解释清楚，这个动态窗口指的是速度窗口。 这个速度窗口，是机器人当前时刻，速度能达到的速度范围。 这个速度窗口有两个，一个是机器人的线速度，一个是机器人的角速度。 计算依据： 机器人的物理极限：v_min ~ v_max，ω_min ~ ω_max 当前速度：v_current、ω_current 加速度极限：acc_lim_x、acc_lim_theta 控制周期：dt（比如0.1秒）
计算公式：
v ∈ [max(v_min, v_current – acc_lim_x * dt), min(v_max, v_current + acc_lim_x * dt)]
ω ∈ [max(ω_min, ω_current – acc_lim_theta * dt), min(ω_max, ω_current + acc_lim_theta * dt)]
这个线速度和角速度范围就是动态窗口。
第二步：采样与轨迹推演
前面有了速度的动态窗口，DWA会均匀采样多组速度组合 (v, ω)。 例如：线速度采样 vx_samples = 20 个点，角速度采样 vtheta_samples = 20 个点 总共就有 20 × 20 = 400 组速度组合 对于每一组 (v, ω)，DWA都会做轨迹推演： 假设机器人以这个恒定速度行驶一段模拟时间 sim_time（如1.7秒） 利用运动学模型（如差速轮模型），计算未来这段时内的轨迹点序列 这些轨迹点会落在局部代价地图上
这样子就出现了400条预测轨迹。
第三步：多维度评分
上一步计算出来了400条预测轨迹，这一步就需要给这些预测轨迹打分。 打分公式：
G(v, ω) = α · heading(v, ω) + β · dist(v, ω) + γ · velocity(v, ω)
评分项 含义 权重 调参效果 heading（朝向） 轨迹终点朝向与目标方向的一致性 α α越大，机器人越“执着”转向目标 dist（安全） 轨迹与最近障碍物的距离，越远分越高 β β越大，机器人越“胆小”，避障更激进 velocity（速度） 鼓励机器人跑得快 γ γ越大，机器人越“急性子”
关键机制：如果某条轨迹上的点与障碍物距离小于安全阈值，该轨迹会被直接判负分或淘汰，确保安全性优先。
而nav2中的DWB（DWA升级版），在这一项中新增了评分项： PathAlign：轨迹是否贴合全局路径 GoalDist：轨迹终点离目标点有多近
第四步：选择最优轨迹并输出
前面把400条预测轨迹都算出了分数，那这一步就选出分数最高的那条路。 然后这条路对应的(v*, ω*)就是输出的线速度和角速度指令了。
第五步：循环
进入下一个循环，继续实时计算。
总结
DWA是差速机器人运动控制中非常经典的一种算法。
