动态窗口法的理解和一些细节

机器人局部路径规划—动态窗口法

动态窗口法（Dynamic Window Approach，DWA）是一类经典的机器人局部路径规划算法。它的过程主要分为两部分：

1. 速度空间$(v,\omega )$的计算。考虑到实际的运动限制可以得到一个速度范围：
   V = { ( v , ω ) ∣ v 1 ≤ v ≤ v 2 ,    ω 1 ≤ ω ≤ ω 2 } V = \{ (v,\omega)|v_1\leq v \leq v_2, \; \omega_1\leq \omega \leq \omega_2\} V={(v,ω)∣v1​≤v≤v2​,ω1​≤ω≤ω2​}
   这个范围由多种因素决定，如机器人的速度限制、电机的加速度限制、机器人的最小转动角度等。

2. 速度空间$(v,\omega )$的评价。根据给定的评价公式，对速度空间的每一组$(v_i,{\omega }_i)$进行评价，然后选择一组最佳速度作为当前速度。

值得注意的是，在使用DWA方法进行路径规划时，一般应用于以下场景：

• 机器人和目的地的坐标已知，但障碍物坐标未知；
• 有传感器可以检测到障碍物，并且可以得到机器人与障碍物的距离和角度.

接下来，我会详细介绍DWA的算法流程。

1.速度空间计算

在计算速度空间时，为了更加贴近现实，我们一般会考虑一些运动限制。在DWA中，主要考虑了三种运动限制：

• 机器人移动的最大速度限制；
• 电机的最大加速度限制；
• 遇到障碍物时能否在碰撞前停止.

1.1.移动最大速度限制

显然，机器人的速度是不可能无限增加的，所以应该考虑其最大运动速度。于是，我们可以得到如下公式：
$$v_s\in [v_{min},v_{max}]$$

$${\omega }_s\in [{\omega }_{min},{\omega }_{max}]$$

一般最小线速度取零，最小角速度和最大角速度互为相反数。但考虑到为了增加评价函数的区分度，可以考虑把最小线速度设置为1。因为当线速度为0时，评价函数就无法发挥作用。从下图的上下限可以看出，其最大速度限制设置为：
$$v_{min}=0,v_{max}=90,{\omega }_{min}=-90,{\omega }_{max}=90$$

如果不考虑其它运动限制，在设置最大速度限制时，上图的坐标系上的所有点都是我们需要考虑的速度，这些速度构成了一个速度空间。

1.2.移动最大加速度限制

由于电机的转矩有限，因此存在一个最大的加速度限制。假设机器人当前速度为$(v_c,{\omega }_c)$，在一个有限的时间周期$\Delta t$，机器人的速度范围应为：
$$v_d\in [v_c-{\dot{v}}_a\Delta t,v_c+{\dot{v}}_b\Delta t]$$

$${\omega }_d\in [{\omega }_c-{\dot{\omega }}_a\Delta t,{\omega }_c+{\dot{\omega }}_b\Delta t]$$

上式中，$(v_a,v_b,{\omega }_a,{\omega }_b)$分别代表线速度最大减速度、线速度最大加速度、角速度最大减速度以及角速度最大加速度。

在一般情况下，都是默认最大加速度和最大减速度的绝对值相同。下图的白色矩形区域就是考虑加速度限制后可以取到的速度空间，也就是论文提到的动态窗口，该矩形的中心就是机器人当前的运动速度。

1.3.允许速度（Admissible Velocities）

当机器人检测到障碍物时，应该留有一段刹车距离用于减速。当机器人与最近障碍物的距离小于刹车距离时，机器人就会与障碍物发生碰撞，这是我们不愿看到的情况。因此，我们应该考虑一个允许速度，也就是可以安全停止下来的速度。

显然，这个安全停止速度应该与机器人与最近障碍物的距离成正比。当距离越小时，这个安全停止速度也应该相应减小。它们应该满足如下不等式：
$$v_a\le \sqrt{2\cdot dist\cdot {\dot{v}}_a}$$

$${\omega }_a\le \sqrt{2\cdot dist\cdot {\dot{\omega }}_a}$$

计算安全速度的难点在于机器人与障碍物的最短距离如何计算。因为角速度的存在，机器人并不是直线运动的，它的运动轨迹是一条弧线。因此在计算与障碍物的距离时，我们求的应该是弧长，而不是直线距离。
下图的淡灰色区域就是可允许速度，而深灰色区域则是不可允许速度。

在计算三种速度限制后，我们得到了三个速度范围，它们的交集就是我们在当前状态下可以取到的速度范围，也就是上图中的红色区域。
$$V_r=V_s\bigcap V_d\bigcap V_a$$

2.速度空间评价

在得到速度空间后，根据评价函数，我们就通过遍历每一组速度$(v_i,{\omega }_i)$对其做出评价，然后得到一组当前最优速度$(v_{best},{\omega }_{best})$。由于在计算机中处理数据是离散的，因此需要设置速度增量，即
$$\Delta v=0.01m/s,\Delta \omega =1^{\circ }/s$$
假设计算得到的速度空间为
$$v\in [0,3],\omega \in [-20^{\circ },20^{\circ }]$$
根据设置的速度增量，我们可以得到如下速度序列
$$\text{the list of v}:[0,0.01,...,3.00],n=101$$

$$\text{the list of}\omega :[-20^{\circ },-19^{\circ },...,20^{\circ }],n=41$$

经过上述计算后，我们可以得到$101\times 41$组速度。

在得到每组速度后，我们还需要做一些预备工作，即对每组速度生成在给定时间周期内的轨迹预测。

2.1.轨迹预测

假设有一组速度为$(v_i,{\omega }_i)$，预测时间周期为$t$，我们需要计算出在该周期内的机器人运动轨迹。与上相同，我们需要先设置一个时间增量$\Delta t$，这样我们就有了一个时间序列$[0,\Delta t,2\Delta t,...,t]$。

假设机器人当前位姿为$(x_0,y_0,{\theta }_0)$，根据下述递推公式就可以得到预测的轨迹点。但要说明的一点是，此处的轨迹预测公式是假设机器人是直线运动的，这是一种近似的运动模型。当距离很小时，我们可以用直线去近似弧线。

除了上述预测公式外，还有更精确的轨迹公式。事实上，当线速度和角速度不变时，机器人的运动轨迹应该是一个半径为$|\frac{v}{\omega }|$的圆。这也是DWA论文中采用的运动模型。该预测公式如下：
[ x ′ y ′ θ ′ ] = [ x − v ω sin ⁡ θ + v ω sin ⁡ ( θ + ω Δ t ) y + v ω cos ⁡ θ − v ω cos ⁡ ( θ + ω Δ t ) θ + ω Δ t ]    ( ω ≠ 0 ) \left[

$$\begin{matrix} x'\\ y'\\ \theta' \end{matrix}$$ \right]= \left[ $$\begin{matrix} x-\frac{v}{\omega}\sin\theta+\frac{v}{\omega}\sin(\theta+\omega \Delta t)\\ y+\frac{v}{\omega}\cos\theta-\frac{v}{\omega}\cos(\theta+\omega \Delta t)\\ \theta+\omega \Delta t \end{matrix}$$ \right] \; (\omega \neq0) ⎣⎡​x′y′θ′​⎦⎤​=⎣⎡​x−ωv​sinθ+ωv​sin(θ+ωΔt)y+ωv​cosθ−ωv​cos(θ+ωΔt)θ+ωΔt​⎦⎤​(ω​=0)
上述公式成立的条件是角速度不为零。当角速度为零时，公式与第一个预测公式相同。

另外需要注意的是，在整个预测的时间周期$(t_1,t_n)$，我们默认机器人的速度不变。但在计算安全停止速度时，预测的轨迹点的线速度应该是减少的。

对时间序列的每个值进行计算，我们就可以得到一系列的轨迹点了。但不是每一个轨迹点都是有用的，我们需要对每个轨迹点进行判断。假设某个轨迹点与障碍物碰撞，则我们应该舍弃这个以及后面的所有轨迹点，选择上一个轨迹点作为最终预测的轨迹点。

2.2.速度评价

速度评价函数主要由三个部分组成，分别为方向角评价、障碍物距离评价以及速度评价。

$$G(v_i,{\omega }_i)=\sigma (\alpha \cdot heading(v_i,{\omega }_i)+\beta \cdot dist(v_i,{\omega }_i)+\gamma \cdot velocity(v_i,{\omega }_i))$$

2.2.1.方向角评价

$heading(v_i,{\omega }_i)$用于评价机器人在给定角速度下运动的角度与目标角度之间的差值。显然，根据函数描述，该函数值越小，方向角评价应该越高。
$$heading(v_i,{\omega }_i)=180^{\circ }-|target-cur\theta |$$

2.2.2.障碍物距离评价

$dist(v_i,{\omega }_i)$用于表示机器人当前位置与最近的障碍物之间的距离。如果轨迹上无障碍物，则设定一个常数。根据函数描述可以得知，当机器人与障碍物的距离越大，则该函数评价应该越高。因此，该函数可以直接用距离作为评价函数。

另外值得注意的是，在计算距离时还应该考虑机器人的半径。

当角速度不为零时，我们应该计算的距离是弧线长度，而不是长度$c$。这个弧长的计算也比较简单，根据运动模型，我们知道：当角速度和线速度不变时，机器人的运动轨迹为一个标准的圆形。该圆的半径为（角速度单位为弧度每秒）：
$$r=|\frac{v}{\omega }|(\omega \ne 0)$$
设机器人当前的位姿为$(x,y,\theta )$（这里并不是预测点的位姿），则该圆的圆心坐标为：
x c = x − v ω sin ⁡ θ y c = y + v ω cos ⁡ θ

$$\begin{aligned} x_c = x - \frac{v}{\omega}\sin\theta \\ y_c = y + \frac{v}{\omega}\cos\theta \end{aligned}$$ xc​=x−ωv​sinθyc​=y+ωv​cosθ​
然后通过余弦公式可以计算出角度$\gamma$，最后应用弧长公式就可以得到弧长：
$$s=r\cdot \gamma$$

（补充于2021.5.30： 最近看了CVM算法，发现它和DWA的算法过程十分相似，区别只在于障碍物距离的计算方式不同。CVM论文中的很大篇幅都在描述如何计算障碍物距离，而且它考虑的是弧线长度。值得一提的是，CVM将曲率c（角速度与线速度的比值）作为讨论对象，即在遍历速度时，CVM遍历的是角速度与线速度的比值。）

2.2.3.速度评价

$velocity(v_i,{\omega }_i)$表示当前的机器人速度。对于路径规划而言，显然速度越快越好，因此可直接把当前线速度作为速度评价值，即
$$velocity(v_i,{\omega }_i)=|v_i|$$
在计算三种评价函数后，还需要分别做归一化处理。最后代入上述给出的评价函数，就可以对速度$(v_i,{\omega }_i)$做出评价。

3.总结

当机器人运动到某个位置时，首先计算速度空间，然后对每一组速度进行轨迹预测并给出速度评价，最后取评价最高的一组速度作为当前速度。就这样，机器人不断进行速度的计算、评价和选择，就可以越来越接近目标而不碰撞障碍物了。最后给出DMA运行的图像，蓝点表示预测的轨迹点。

在经过实际仿真后，我发现在较为复杂的环境下，DWA方法并不是很有效。特别是当目标位置与当前位置相隔一层障碍物时，此时的评价函数的三个参数很难调，很难同时让角度评价和距离评价的效果都很好。

在上图中，机器人与目标点相隔一层障碍物，只有一个缺口可以通过。此时，如果设置角度评价参数较高，那么机器人很可能会在左侧区域打转；如果设置障碍物距离参数较高，即使机器人很靠近目标时，可能仍然会错过。或许可以根据具体情况自适应地调整参数，而不是让参数固定不变。