【机器人规划】DWA解析

参考文献

《The Dynamic Window Approach To Collision Avoidance》
《The Dynamic Window Approach To Collision Avoidance》
《State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in ComplexEnvironments》
DWA局部路径规划算法论文阅读：The Dynamic Window Approach to Collision Avoidance。
AtsushiSakai/PythonRobotics
七种代价函数的选取

ros功能包

机器人获得目的地信息后，首先全局路径规划规划出一条大致可行的路线，然后局部路径规划器根据这条路线及costmap的信息规划出机器人在局部时做出具体行动策略，ROS中主要是使用了DWA算法。在ROS中每当move_base处于规划状态就调用DWA算法计算出一条最佳的速度指令，发送给机器人运动底盘执行。
ros导航功能包-古月居
ros-planning/navigation

ROS采用了Gerkey的论文：《planning and control in unstructured Terrain》

---

简介

动态窗口是由一系列的约束构成，其中约束主要包括差动机器人的非完整约束、环境障碍物约束与受结构与电机影响的动力学约束。

• 非完整约束就是普通汽车，只能前进，转向完整约束是可以360度前进的那种
• 车辆不能直接横着向螃蟹一样走，表明这个方向受到一定约束。但是却能通过加减速和方向盘的配合让车辆转过来，表明又存在一定自由度。一句话，约束了又感觉没有约束，所以叫非完整约束

优点：

• 计算复杂度低：考虑到速度和加速度的限制，只有安全的轨迹会被考虑，且每次采样的时间较短，因此轨迹空间较小
• 可以实现避障：可以实时避障，但是避障效果一般
• 适用于两轮差分和全向移动模型

缺点：

• 前瞻性不足：只模拟并评价了下一步，如在机器人前段遇见“C”字形障碍时，不能很好的避障
• 动态避障效果差：模拟运动轨迹短，动态避障效果差
• 非全局最优路径：每次都选择下一步的最佳路径，而非全局最优路径
• 不适用于阿克曼模型

机器人运动学模型

以两轮移动机器人模型，分两个方面进行分析。

1 非全向移动机器人（v,w）

2 全向移动机器人（v_x，v_y）

速度空间(v,w)

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

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

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

在上述三条约束条件的限制下，速度空间（v,w）会有一定的范围，另外会随着电机的线加速度、角加速度进行变换，速度空间会动态变化，我们将其称为动态窗口。在满足约束条件的情况下，进行采样（v,w），可以得到相应的轨迹：

评价函数

在速度空间（v,w）中采样，根据运动学模型推测对应的轨迹，接下来引入评价函数，对轨迹进行打分，选取最优的轨迹。

1 平滑处理

解读The Dynamic Window Approach to Collision Avoidance

DWA（动态窗口）算法是用于局部路径规划的算法，已经在ROS中实现，在move_base堆栈中：http://wiki.ros.org/dwa_local_planner

DWA算法第一次提出应该是1997年，发在了《IEEE Robotics and Automation Magazines》上

路径规划算法主要包括全局路径规划和局部路径规划。局部路径规划主要用于动态环境下的导航和避障，对于无法预测的障碍物DWA算法可以较好地解决。DWA算法的优点是计算负复杂度较低，由于考虑到速度和加速度的限制，只有安全的轨迹会被考虑，且每次采样的时间较短，因此轨迹空间较小。采样的速度即形成了一个动态窗口。

对于轨迹的评价函数主要包括三个方面：与目标的接近程度、机器人前进的速度、与下一个障碍物的距离。简而言之就是在局部规划出一条路径，希望与目标越来越近，且速度较快，与障碍物尽可能远。评价函数权衡以上三个部分得到一条最优路径。

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

• 该方法是由一个移动机器人的运动动力学推导出来的
• 考虑到机器人的惯性（代码中计算了刹车距离），这对于具有扭矩限制机器人在高速行驶时很重要。
• 在动态杂乱环境中速度可以较快，对于速度较快的机器人以及低电动机转矩的机器人较为实用。

相关工作

该部分作者说明了全局路径规划的优点在于计算时可以离线进行，但是目前ROS中全局路径也在导航过程中不断变化。缺点在于不能适应环境变化以及计算复杂度太高，尤其是环境不断变化时。局部路径规划的缺点在于不能保证得到最优解，容易陷入局部最优（如U形障碍环境）。优点在于计算速度快，适合环境不断变化。

对比了其他的局部路径规划算法的优缺点，势场法计算速度很快，但是在狭窄区域会产生震荡，如果目标点在两个很近的障碍物之间，则可能找不到路径。

同步传动机器人运动学方程

为了使运动学方程更加接近实际，将模型的速度设为随时间变化的分段函数，在该假设下，机器人轨迹可看做许多的圆弧积分组成，采用该方法使得障碍物碰撞检测很方便，因为圆弧与障碍物的交点很好求。

$令x(t)$,$y(t)$,$\theta (t)$分别表示机器人在$t$时刻的x坐标、y坐标以及朝向角，$x(t_0)$和$x(t_n)$分别表示机器人在$t_0$和$t_1$时刻的x坐标，令$v(t)$表示机器人的平移速度。

$$x(t_n)=x(t_0)+{\int }_{t_0}^{t_n}v(t)\ast cos\theta (t)dt\text{\hspace{1em}(1)}$$

$$y_{(}{t}_n)=y(t_0)+{\int }_{t_0}^{t_n}v(t)\ast sin\theta (t)dt\text{\hspace{1em}(2)}$$

等式(1)和等式(2)取决于机器人的速度，但机器人速度不能直接设定。机器人速度$v(t)$取决于初始时刻$t_0$的速度以及时间$t_0$、机器人在时间间隔$\hat{t}\in [t_0,t]$的平移加速度$\dot{v}(\hat{t})$。同样的，$\theta (t)$也是初始转向角$\theta (t_0)$函数（设$t_0$时刻的初始旋转速度为$w(t_0)$，$\hat{t}\in [t_0,t]$的旋转加速度为$\dot{w}(\hat{t})$，则式(1)可写为：

$$x(t_n)=x(t_0)+{\int }_{t_0}^{t_n}\left(v(t_0)+{\int }_{t_0}^t\dot{v}(\hat{t})d\hat{t}\right)\ast cos\left(\theta (t_0)+{\int }_{t_0}^t\left((w(t_0)+{\int }_{t_0}^{\bar{t}}\dot{w}(\tilde{t})d\tilde{t}\right)d\tilde{t}\right)dt\text{\hspace{1em}(3)}$$

此时机器人的轨迹由初始时刻的状态以及加速度决定，可以认为这些状态是可控的，同时由于机器人内部结构原因，其加速度也不是一直变化（类似于连续函数），因此可以将$t_0$到$t_n$看作是很多个时间片，积分可以转换为求和，假设有n个时间片，在每个时间片$[t_i,t_{i+1}]$，机器人的加速度$\dot{v_i}$和$\dot{w}$保持不变，设${\Delta }_t^i=t-t_i$则式(3)可以转化为：

$$x(t_n)=x(t_0)+\sum _{i=0}^{n-1}{\int }_{t_i}^{t_{i+1}}\left(v(t_i)+\dot{v_i}\ast {\Delta }_t^i\right)\ast cos\left(\theta (t_i)+w(t_i)\ast {\Delta }_t^i+\frac{1}{2}\dot{w_i}\ast ({\Delta }_t^i{)}^2\right)dt\text{\hspace{1em}(4)}$$

等式(4)虽然与机器人的动力控制相关，但是不能决定机器人具体的驾驶方向，对于障碍物与机器人轨迹的交点也很难求出，继续进行简化，既然时间间隔都很小，因此可以假设在每一个时间片内速度保持不变，则$v(t_i)+\dot{v_i}\ast {\Delta }_t^i$可近似为$v_i\in [v(t_i),v(t_{i+1})]$，同理，$\theta (t_i)+w(t_i)\ast {\Delta }_t^i+\frac{1}{2}\dot{w_i}\ast ({\Delta }_t^i{)}^2$近似为$\theta (t_i)+w_i\ast {\Delta }_t^i$，其中$w(i)\in [w(t_i),w(t_{i+1}]$，则式(4)可写为：

$$x(t_n)=x(t_0)+\sum _{i=0}^{n-1}{\int }_{t_i}^{t_{i+1}}{v}_i\ast cos\left(\theta (t_i)+w_i\ast (\hat{t}-t_i)\right)d\hat{t}\text{\hspace{1em}(5)}$$

解这个积分方程，简化为：

$$x(t_n)=x(t_0)+\sum _{i=0}^{n-1}{F}_x^i(t_{i+1})\text{\hspace{1em}(6)}$$

其中：

$$F_x^i(t_i)=\{\begin{array}{l}\frac{v_i}{w_i}\left(sin\theta (t_i)-sin\left(\theta (t_i)+w_i\ast (t-t_i)\right)\right),w_i\ne 0\\ v_{i}cos\left(\theta (t_i)\right)\ast t,w_i=0\end{array}\text{\hspace{1em}(7)}$$

同理，对应的y坐标公式为：

$$y(t_n)=y(t_0)+\sum _{i=0}^{n-1}{F}_y^i(t_{i+1})\text{\hspace{1em}(8)}$$

$$F_y^i(t_i)=\{\begin{array}{l}-\frac{v_i}{w_i}\left(cos\theta (t_i)-cos\left(\theta (t_i)+w_i\ast (t-t_i)\right)\right),w_i\ne 0\\ v_{i}sin\left(\theta (t_i)\right)\ast t,w_i=0\end{array}\text{\hspace{1em}(9)}$$

（推导了一遍总感觉式(7）和式(9)的第一项前面少了一个负号）

当$w_i=0$时，机器人行走轨迹为一条直线，当$w_i\ne =0$时，机器人轨迹为圆弧，设

$$M_x^i=-\frac{v_i}{w_i}\ast sin\theta (t_i)\text{\hspace{1em}(10)}$$

$$M_y^i=\frac{v_i}{w_i}\ast cos\theta (t-i)\text{\hspace{1em}(11)}$$

则：

$$(F_x^i-M_x^i{)}^2+(F_x^i-M_x^i{)}^2=(\frac{v_i}{w_i}{)}^2\text{\hspace{1em}(12)}$$

式(12)说明第i段圆弧轨迹的圆心为$(M_x^i,M_y^i)$，半径为$\frac{v_i}{w_i}$。

根据上述公式可以求出机器人的轨迹，即通过一系列分段的圆弧和直线来拟合轨迹。

误差上界：

将机器人轨迹进行分段会在控制点之间产生线性误差，即$t_{i+1}-t_i$之间的误差，设x坐标和y坐标的误差分别为$E_x^i$和$E_y^i$，$\Delta t_i=t_{i+1}-t_i$，由于$v_i\in [v(t_i),v(t_{i+1})]$，易知最大误差$E_x^i,E_y^i\le |v(t_{i+1})-v(t_i)|\ast \Delta t_i$，在$\Delta t_i$内是线性的。注意该上界误差仅仅可用于机器人内部预测，而实际机器人位置一般通过里程计测量。

动态窗口

动态窗口法在速度空间中进行速度采样，并对随机采样的速度进行限制，减小采样数目，在使用代价函数进行评价。

主要算法步骤如下

1.速度搜索空间，根据以下三点进行速度空间降采样

• 圆弧轨迹：动态窗口法仅仅考虑圆弧轨迹，该轨迹由采样速度$(v,w)$决定，这些速度构成一个速度搜索空间。
• 允许速度：如果机器人能够在碰到最近的障碍物之前停止，则该采样速度将被评估。
• 动态窗口：由于机器人加速度的限制，因此只有在加速时间内能达到的速度才会被保留。

2.求最优值
　　　代价函数：

$$G(v,w)=\sigma \left(\alpha \ast heading(v,w)+\beta \ast dist(v,w)+\gamma \ast vel(v,w)\right)\text{\hspace{1em}(13)}$$

最大值即使最优值最大：

（a）Target heading:heading用于评价机器人与目标位置的夹角，当机器人朝着目标前进时，该值取最大。

（b）Clearance:dist 用于表示与机器人轨迹相交的最近的障碍物距离。

（c） Velocity:vel 表示机器人的前向移动速度，支持快速移动。

其中\sigma 使得三个部分的权重更加平滑，使得轨迹与障碍物之间保持一定的间隙。

安全的速度是指机器人能够在撞掉障碍物之前停下，$dist(v,w)$为机器人轨迹上与障碍物的最近距离，设刹车时的加速度为${\dot{v}}_b$和${\dot{w}}_b$，则$V_a$为机器人不与障碍物碰撞的速度集合：

$$V_a=\left\{v,w|v\le \sqrt{2\ast dist(v,w)\ast {\dot{v}}_b}\cap w\le \sqrt{2\ast dist(v,w)\ast {\dot{w}}_b}\right\}\text{\hspace{1em}(14)}$$

考虑到机器人的动力加速度，搜索空间降采样到动态窗口，只保留以当前加速度可到达的速度，设$t$为时间间隔，$(v_a,w_a)$为实际速度，则动态窗口的速度集合为$V_d$:

V d = { v , w ∣ v ∈ [ v a − v ˙ ∗ t , v a + v ˙ ∗ t ] ∩ w ∈ [ w a − w ˙ ∗ t , w a + w ˙ ∗ t ] } (15) V_d=\left\{v,w | v\in [v_a-\dot v*t,v_a+\dot v*t] \cap w\in [w_a-\dot w*t,w_a+\dot w*t ] \right\}\tag{15} Vd​={v,w∣v∈[va​−v˙∗t,va​+v˙∗t]∩w∈[wa​−w˙∗t,wa​+w˙∗t]}(15)

该集合以外的速度都不能在该时间间隔内达到。

综上，最终的搜索空间：

$$V_r=V_s\cap V_a\cap V_d\text{\hspace{1em}(16)}$$

最终的代价函数(13)是基于速度$V_r$计算的。其中：

Target Heading:表示机器人与目标点的对齐程度，用$180-\theta$表示，$\theta$为机器人与目标夹角，夹角越大，代价值越小。

Clearance：表示与机器人轨迹相交的最近障碍物的距离，如果障碍物与机器人轨迹不相交，则设为一个较大的值。

Velocity：机器人某条轨迹的速度$v$。

平滑处理：

评价函数的三个部分都被正则化到$[0,1]$上，实验中设置了$\alpha =2$，$\beta =0.2$，$\gamma =0.2$，平滑处理可以使机器人与障碍物之间有一定的间隙（裕度）。

实现细节：

• 当机器人陷入局部最优时（即不存在路径可以通过），使其原地旋转，直到找到可行路径。
• 安全裕度：在路径规划时，设定一安全裕度，即在路径和障碍物之间保留一定间隙，且该间隙随着速度增大线性增长。

参数设定：

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