【局部路径规划】DWA动态窗口法 Dynamic Window Approach

The Dynamic Window Approach to Collision Avoidance

---

---

1. 航迹推演：

1) 位置推算

将速度增量=加速度对时间积分、角度增量=角速度对时间积分、角速度增量=角加速度对时间积分带入，得X向位置为

2) 短时间间隔内加速度/角加速度恒定假设

短时间内机器人所能执行的加速度/角加速度命令数量是有限的，当积分区间足够短时，可以认为这段时间内机器人的加速度和角加速度恒定，同时对上式离散化表示，得

其中

这里认为$t_i$到$t_{i+1}$时间段内加速度恒定。

3) 短时间间隔内速度/角速度恒定假设

当$t_i$到$t_{i+1}$的间隔充分小时，下式

可以用$t_i$到$t_{i+1}$之间的任意一个速度来近似，这是基于机器人短时间内平滑运动的假设

类似地，同样可以将角增量

近似为

其中， ${\omega }_i$为$t_i$到$t_{i+1}$之间的任意一个角速度

于是航迹推演公式可改写为

进一步地，对积分符号及内部式子运算，结果可表示如下

这里我推导的结果和给出的结果有点差别，看别的博主写到这篇文章时也有提到推导结果不一样
我推导的当角速度≠0时，$F_x^i(t)$的值前面有负号而$F_y^i(t)$的值前面没有负号
而当角速度=0时，要将多项式的最后一项t改为(t-ti)

4) 结果

经过上述假设，作者将一段机器人的完整轨迹划分成无数段短时间内速度、角速度恒定的轨迹，当角速度=0，轨迹为线段，当角速度≠0，由于恒定假设，机器人运动的轨迹是圆弧，其半径为速度/角速度，于是就可以用无数段圆弧/线段来拟合整段轨迹。

5) 拟合误差评估

当角速度为0时，沿轴向的位置偏差最大，由于是选定短时间内的任意一个速度来代替，所以速度的误差一定不超过短时间内的最大速度-最小速度，于是有位置误差

2. 动态窗口

给定情境如下：可以看出，如果要机器人到达目标位置，可以右转通过两面墙之间的缝隙。

DWA通过以下2个约束，大大缩小了搜索空间，提高了计算效率。

1) Admissable Velocities

首先，通过障碍物来限制。当前方有障碍物时，要保证当前的速度不要过大，以至于以最大加速度制停时都无法在障碍物前停下。

对速度搜索范围做如下限制：

其中dist表示以该速度和角速度拟合出的圆弧上，机器人到障碍物的距离。

上图矩形区域为原本的搜索空间Vs，经过上述约束（Va），可以去除灰色部分，得到剩余部分Vs∩Va

2) Dynamic Window

进一步地，限制搜索空间为短时间内可达的速度和角速度

搜索空间将进一步缩小，如下

最终的搜索范围是Vs∩Va∩Vd，Vd也就是图上的小矩形，表示速度的可达范围，即动态窗口。

3. 评估机制

经过2，确定了速度的搜索空间，并可以通过1来用线段/圆弧模拟短时间内速度的轨迹，接下来对轨迹进行评价，找到最优的轨迹，它所对应的速度即为DWA规划的结果。

用于评价的函数object function为：

函数由三部分组成，对应三个考察角度，对应不同的权重

1) 朝向评价 angle(v, ω) — 目标的“引力”

轨迹末端机器人的朝向与机器人与目标连线之间的夹角，这个夹角越小越好

对应的angle函数值分布如下，值为0的部分是经过第一个约束Va去除掉的，不考虑。并且，为了更加直观，这里作者给出的搜索空间是整个Vs而不是动态窗口Vd

可以看出，右上角的angle函数值最大，说明这个函数希望机器人以一定的前向速度、右转，到达目标。

2) 障碍物评价 dist(v, ω) — “斥力”

这个函数衡量的是当前机器人沿轨迹与最近障碍物之间的距离。如果轨迹上没有障碍物，函数值设置为一个很大的常数。
这个函数希望机器人选择没有障碍物、或者与障碍物距离较远的轨迹。

作者给出了距离的计算方法，找到轨迹（圆弧）与障碍物相交的点，获得夹角γ，γ×r即为机器人与障碍物在圆弧上的距离。

3) 速度评价 — 驱动

当机器人原地不动，自转到一个和目标对齐的朝向时，它很好地满足了前两项评价标准，然而很明显不是想要的结果，为了避免机器人的这种“惰性”，引入这一项评价，速度和角速度的幅度越大，函数值越大。这个函数希望机器人在合理范围内尽可能以更大的速度和角速度移动。

4) 加权与平滑

作者给出的权重范例为α=γ=0.2，β=2.0，加权值为

对加权值进行平滑（σ函数），得到最终的Objective Function如下

那么搜索空间内对应着Object Function最大值的速度和角速度即为一次DWA规划的结果。

4. 特点

1) 决策结果与当前速度的关系

下图给出了在较大速度Vd1和较小速度Vd2的情况下生成的动态窗口，以及经过评估后所得到的规划结果，用×表示。

联系前面作者给出的情境可以发现，当速度较大时，规划结果并不会使机器人右转穿过缝隙而到达目标，这是因为此时如果紧急右转，由于速度较大，无法保证在撞到墙之前制停，所以算法是希望机器人加速前进在前方绕路的。

而当速度较小，就可以满足上述条件，所以此时算法希望机器人减速右转，通过缝隙抵达目标。

2) 决策结果与最大加速度的关系

同理，最大加速度也将限制生成的动态窗口的大小，从而影响决策结果，这里不赘述。

5. 其他说明

• DWA法同样会有local minima问题，作者提出陷入local minima时，机器人采取原地自转策略，直到找到脱离的轨迹。
• 算法依赖于传感器获得的障碍物信息，当障碍物有一定高度或者不触地时，可能造成规划结果不准确。
• 当机器人移动速度过快，需要能够感知更远处信息的传感器满足规划需求，也可以选用不同类型传感器进行信息融合。