局部路径规划 DWA 算法完全解析（理论推导+代码实现，包你看懂！）

---

转载请注明出处，谢谢

---

前面学习的全局路径规划方法，Dijkstra、Best-First-Search、A*算法都属于（State Sampling）方法，而 DWA 局部路径规划则属于典型的（action sampling）方法

DWA 算法（Dynamic Window Approach）的原理主要是以一定的在 (v, w) 中采样多组速度，并模拟出这些速度在一定时间的，得到可行轨迹后通过对轨迹进行评分，评分后选取最优轨迹对应的 (v, w) 驱动机器人运动

速度空间 (v, w) 即机器人的速度范围，机器人的速度受到各种因素的限制

• 移动机器人受自身最大速度最小速度的限制

Vs 为机器人能够到达的所有矢量速度的集合；机器人受到最大最小线速度和角速度影响

• 移动机器人受电机性能的影响

由于加速度有一个范围限制，所以最大加速度或最大减速度一定时间内能达到的速度 ，才会被保留，表达式如下：

• 移动机器人受障碍的影响

为了能在碰到障碍物前停下来，在最大减速度的条件下，速度满足以下条件：

其中dist(v,w)为（v,w）对应的轨迹上离障碍物最近的距离

在上述 3 个约束条件下，速度空间 (v, w) 会有一定的范围，注意速度空间不是固定不变的，而是时变的， 时刻的速度空间与 时刻的速度空间是不同的，故将其称为

得到 t 时刻的速度空间 (v, w) 后，以一定的分辨率对速度 v 和角速度 w 进行采样

设 t 时刻小车 x 轴线速度取值范围 5~20m/s,为简化数学模型，设 y轴线速度为0m/s，小车的角速度取值范围 0.1~1rad/s,小车的线速度的速度分辨率为0.1m/s，角速度的速度分辨率为0.05rad/s

根据离散的方法，离散 x 轴线速度和角速度如下

之所以要分析机器人的运动学模型，是因为要根据采样的速度 (v, w) 模拟机器人的运动轨迹

DWA 算法适用于差速运动模型和全向运动模型，并不适用于阿克曼模型

差速运动模型

机器人只能向前运动或者旋转，图中有世界坐标系和机器人坐标系，下式中 v(t) 指机器人坐标系中 x 方向的速度

全向移动模型

在全向移动模型中，多了机器人坐标系下 y 方向的速度

令 Vy(t) = 0，则退化为差速运动模型，因此 ROS 自带的局部路径规划器使用上式计算

在确定机器人的采样速度和运动模型后，可以对机器人的状态进行预测和更新

机器人下一时刻位移距离

机器人下一时刻坐标的变化

采样时刻机器人坐标与坐标变化求和，得到下一时刻机器人坐标

设 ?t 为 0.1s，实际上预测机器人前向几秒内的所有状态，假设前向预测时间为 3s，则会预测 s内的所有状态，即 ~ ~ ~ …. ~ ，相当于会预测出 t 时刻位置前 30 个点的位置，如下图所示

对速度空间进行采样后，根据机器人运动学模型能够预测出多条轨迹，需要对这些轨迹进行评价，选取最优的轨迹，机器人根据最优轨迹对应的速度进行运动

DWA 算法对轨迹的评价函数一般如下所示

• 为方位角评价函数：评价机器人在当前的设定的速度下，轨迹末端朝向与目标点之间的角度差距
• 主要的意义为机器人处于预测轨迹末端点位置时与地图上最近障碍物的距离，对于靠近障碍物的采样点进行惩罚，确保机器人的避障能力，降低机器人与障碍物发生碰撞的概率
• 为当前机器人的线速度，为了促进机器人快速到达目标
• α、β、γ、σ 为权重系数

很好理解：

1、希望我的前进方向对准终点

2、希望不发生任何碰撞

3、希望速度尽量快

4、除此之外，还要保证最短刹车距离是安全的

原文中提到对轨迹规划的影响较大，太大易陷入局部最优解，太小能更好地避障，但路径有点长，效率有限低

当然也可以对评价函数进行优化，添加更多的评价函数指标

注意需要对评价函数的结果进行处理，将每一项除以每一项的总和，如下式

这样可以避免不同类别造成的结果误差

至此，DWA 算法的完整流程

程序的思维导图如下，这里只讲解重要的部分，绘图及障碍物设置部分暂时略过

这里的 for 循环有次数限制，最多循环 5000 次

函数最重要的函数，通过该函数返回最优控制量 u

CalcDynamicWindow() 函数

在该函数中首先调用 函数计算当前时刻的 Vr

这里相当于对 和 求了交集，如下图所示， 在后面的代码中有考虑

Evaluation() 函数

这个函数也很重要，它肩负了较多的职责，包括

• 对速度空间进行离散化采样

• 进行轨迹推测，预测出当前时刻到预测时刻之间的轨迹

调用 函数进行轨迹预测

在 函数中通过状态空间表达式的形式更新机器人的状态

• 计算评价函数

调用 计算方位角评价函数

调用 计算障碍物距离评价函数

调用 计算速度评价函数

调用 函数计算制动距离，并比较制动距离与最近障碍物距离，判断是否要舍弃该路径，这里相当于考虑了速度空间中的

NormalizeEval() 函数

计算得到各轨迹的评价函数后，调用 函数对各评价函数正则化

然后就选取评分最高的路径，并返回该路径对应的控制量

获得最优控制量后，在主循环中更新机器人为下一时刻状态，并将历史轨迹保存，同时根据当前点与目标点间的判断是否到达目标点

将航向得分比重改为 ，结果如下

将航向得分比重改为 ，结果如下

可以看到当航向得分比重较小时规划的路径更安全，但相对长度也更长；当航向得分比重较大时规划的路径更短，但靠近障碍物时会“犹豫”，有一定撞到障碍物的风险

ROS 2D导航原理系列（六）|局部路径规划-DWA算法

DWA Planner | Husky Robot | Motion Planning for Robots

DWA算法总结

DWA动态窗口法的原理及应用:The Dynamic Window Approach to Collision Avoidance

The Dynamic Window Approach to Collision Avoidance

《The_dynamic_window_approach_to_collision_avoidance》

《基于动态窗口法和改进A*算法的多车路径规划》

《面向车场日巡检机器人的路径规划研究_李清天》

《移动机器人的SLAM与自主路径规划研究》