DWA算法(一) 论文 | 沉默杀手

DWA算法的优点是计算的复杂度较低，由于考虑到速度和加速度的限制，只有安全的轨迹会被考虑，且每次采样的时间较短，因此轨迹空间较小。采样的速度即形成了一个动态窗口

DWA的弊端时很明显的，他对heading的实现是一个软约束，并不能够很好的在终点达到期望的heading.

该论文相对于之前的创新点在于：

从移动机器人的运动动力学推导出来的
考虑到机器人的惯性，也就是刹车距离，对于限制机器人在高速行驶时很重要
在动态杂乱环境中速度可以较快

建立的假设：
a. 运动学模型的速度设为随时间变化的分段函数，因此机器人轨迹可看做许多的圆弧积分组成
b. 加速度也不是一直变化，假设有n个时间片，在每个时间片内的线加速度和角加速度不变
c. 省略 $\Delta t$ 的二次项

DWA的整体轨迹评价函数主要是三个方面：

与目标的接近程度
机器人前进的速度
与下一个障碍物的距离

也就是在局部规划出一条路径，希望与目标越来越近，且速度较快，与障碍物尽可能远。评价函数权衡以上三个部分得到一条最优路径。这样一看确实和CMU张楫团队提出的路径算法挺像

根据机器人的速度加速度假设和运动学模型，可以得出

$(F_x^{i} - M_x^{i})^2 + (F_y^{i} - M_y^{i})^2 = (\frac{v_i}{w_i})^2$

其中的 $F_x, F_y, M_x, M_y$ 都可以用 $v_i, w_i$ 表示，这个式子就是圆在平面的公式，根据此公式可以求出圆的轨迹，即通过一系列的分段圆弧和直线来拟合轨迹。

速度空间降采样

圆弧轨迹：动态窗口法仅仅考虑圆弧轨迹，该轨迹由采样速度 (v,w) 决定，这些速度构成一个速度搜索空间 $V_s$
允许速度：如果机器人能够在碰到最近的障碍物之前停止，则该采样速度参与打分，机器人不与障碍物碰撞的速度集合记做 $V_a$
动态窗口：由于机器人加速度的限制，只保留以当前加速度可到达的速度，速度集合记做 $V_d$

最终的搜索空间 $V_r = V_s \cap V_a \cap V_d$

有点cartographer分支定界算法的感觉

目标函数

说是使用最优化方法，但是我看目标函数方程十分简单，不用使用什么优化算法，求最大值即可

$G(v,w) = \sigma(\alpha \cdot heading(v,w)+\beta \cdot dist(v,w) + \gamma \cdot vel(v,w)\ )$

heading用于评价机器人与目标位置的夹角，当机器人朝着目标前进时，该值取最大
dist 用于表示与机器人轨迹相交的最近的障碍物距离
vel 表示机器人的前向移动速度，支持快速移动

实现细节：
当机器人陷入局部最优时（即不存在路径可以通过），使其原地旋转，直到找到可行路径。但这样看着会很奇怪，这就是一个缺点
安全裕度：在路径规划时，设定一安全裕度，即在路径和障碍物之间保留一定间隙，且该间隙随着速度增大线性增长。

$\alpha$ 占比重太大，机器人运动自由度大，窄的区域不容易通过， $\alpha$ 占比重太小，机器人轨迹则不够平滑。因此 $\alpha$ 越大，越适合在窄区域， $\alpha$ 越小，越适合在宽区域。

在不涉及TEB的情况下，ROS中经常使用的局部规划器，有base_local_planner 和 dwa_local_planner，简单来看，两者属并列关系，其实就是选择使用 TrajectoryPlannerROS 还是 DWAPlannerROS，我们用ROS就只看DWAPlannerROS。两种配置如下:

1
2
3

<arg name="base_local_planner" default="base_local_planner/TrajectoryPlannerROS"/>

<arg name="base_local_planner" default="dwa_local_planner/DWAPlannerROS"/>

其实base_local_planner也是包含dwa选项的，不过通常还是使用独立的DWA模块。 base_local_planner::TrajectoryPlanner是对DWA的实现和Trajectory Rollout。可以向dwa_local_planner添加自己的代价函数或者trajectory generators

Matlab对DWA算法的仿真
DWA泊车算法的实现
 Dynamic Window Approach_机器人局部避障的动态窗口法

参考：DWA算法的论文