路径规划算法C++实现（三）--DWA

一、算法介绍

DWA算法全称为dynamic window approach，其原理主要是在速度空间（v,w）中采样多组速度，并模拟出这些速度在一定时间内的运动轨迹，并通过评价函数对这些轨迹进行评价，选取最优轨迹对应的速度驱动机器人运动。

动态窗口法 DWA 的实现包含两个步骤：
（1）对机器人速度进行约束限制，形成动态窗口进行速度采样；
（2）根据速度采样点求出待评价轨迹，最大化评价函数选取最优速度命令。
动态窗口是由一系列的约束构成，其中约束主要包括差动机器人的非完整约束、环境障碍物约束与受结构与电机影响的动力学约束。

由于使用C++在Qt上对DWA的可视化效果较差，故使用python。
C++工程源码文件
程序参考PythonRobotics-master

原始DWA

二、机器人运动学模型

下图为两轮差动机器人运动模型示意图，v和w分别代表机器人在世界坐标系下的平移速度与转动速度。
在一次采样时间内，机器人的移动距离较小，所以可以把两次采样间运动的轨迹看作是一条直线，所以得到以下公式。

三、DWA算法采样空间

下图为传统DWA算法的速度矢量空间示意图，图中横坐标为机器人角速度，纵坐标为机器人线速度，整个区域为Vs ，所有白色区域Va 为避免碰撞的可行区域，灰色区域的速度表示执行该速度可能造成机器人碰撞障碍物，Vd 为考虑电机扭矩在控制周期内限制的机器人可达速度范围，Vr 为上述三个集合的交集，即最终确定的动态窗口。滑动窗口主要由以下三个方面限制，移动机器人受自身最大速度最小速度的限制、移动机器人受电机性能的影响（加速度）、移动机器人受障碍的影响。

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

（2）移动机器人受电机性能的影响

（3）移动机器人受障碍的影响

综上所述，DWA的动态窗口为以上三个速度集合的交集，即：

在速度空间Vr中，等间距采集离散点(v,w)，并使用采样点，根据机器人运动模型对机器人在之后T时间内进行仿真。（T与上文中△t不同，在仿真时间T内，默认机器人的速度v和角速度w不变，均为采样点的速度）仿真图如下图所示：

四、DWA算法评价函数

根据采样的速度得到相应轨迹后，需要对轨迹进行评价，进而选择当前状态下的最佳轨迹。评价函数的定义如下：

heading(v,w)

heading(v,w) 主要促进机器人在移动过程中使其航向角不断的朝向目标点。
方位角示意图如下所示：

distance(v,w)

distance(v,w) 主要的意义为机器人处于预测轨迹末端点位置时与地图上最近障碍物的距离，对于靠近障碍物的采样点进行惩罚，确保机器人的避障能力，降低机器人与障碍物发生碰撞的概率。
设定L的目的是，一旦轨迹上没有障碍物，将函数输出为一较大的固定值，避免在评价函数中占比重过大。

velocity(v,w)

为了促进机器人快速到达目标而采用的评价函数，具体定义如下：

五、DWA算法优化

DWA算法未考虑到待评价轨迹的方向性，运动的平滑性及到达目标点的快速性等缺点。因此可增加评价函数来优化DWA算法，如提增加方向评价子函数、平滑速度评价子函数以及加速度评价子数，以提高机器人运动方向的稳定性，运动的平滑性，并且减少了机器人到达终点的时间。
具体参考以下论文
面向家庭环境的移动机器人局部路径规划算法研究
例如增加离终点额距离评价函数后，可视化如下：

优化评价函数DWA