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

参考博客：机器人局部避障的动态窗口法(dynamic window approach)

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

1. 速度空间 ( v , ω ) (v,\omega) (v,ω)的计算。考虑到实际的运动限制可以得到一个速度范围：
   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 , ω ) (v,\omega) (v,ω)的评价。根据给定的评价公式，对速度空间的每一组 ( v i , ω i ) (v_i,\omega_i) (vi​,ωi​)进行评价，然后选择一组最佳速度作为当前速度。

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

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

接下来，我就自己的理解来描述DWA的算法流程。如果有错误，还望各位指出。

1.速度空间计算

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

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

1.1.移动最大速度限制

显然，机器人的速度是不可能无限增加的，所以应该考虑其最大运动速度。于是，我们可以得到如下公式：
v m ∈ [ v m i n , v m a x ] v_m \in [v_{min}, v_{max}] vm​∈[vmin​,vmax​]

ω m ∈ [ ω m i n , ω m a x ] \omega_m \in [\omega_{min},\omega_{max}] ωm​∈[ωmin​,ωmax​]

一般最小线速度取零，最小角速度和最大角速度互为相反数。

1.2.移动最大加速度限制

由于电机的转矩有限，因此存在一个最大的加速度限制。假设机器人当前速度为 ( v c , ω c ) (v_c,\omega_c) (vc​,ωc​)，在一个有限的时间周期 Δ t \Delta t Δt，机器人的速度范围应为：
v d ∈ [ v c − v ˙ a Δ t ,    v c + v ˙ b Δ t ] v_d \in [v_c-\dot{v}_a \Delta t, \;v_c+\dot{v}_b \Delta t] vd​∈[vc​−v˙a​Δt,vc​+v˙b​Δt]

ω d ∈ [ ω c − ω ˙ a Δ t ,    ω c + ω ˙ b Δ t ] \omega_d \in [\omega_c-\dot{\omega}_a \Delta t, \;\omega_c+\dot{\omega}_b \Delta t] ωd​∈[ωc​−ω˙a​Δt,ωc​+ω˙b​Δt]

上式中， ( v a , v b , ω a , ω b ) (v_a,v_b,\omega_a,\omega_b) (va​,vb​,ωa​,ωb​)分别代表线速度最大减速度、线速度最大加速度、角速度最大减速度以及角速度最大加速度。

在一般情况下，都是默认最大加速度和最大减速度的绝对值相同。

1.3.安全停止速度

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

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

ω a ≤ 2 ⋅ d i s t ⋅ ω ˙ a \omega_a \leq \sqrt{2 \cdot dist \cdot \dot{\omega}_a} ωa​≤2⋅dist⋅ω˙a​ ​

已知的上述三种限制后，我们该如何使用呢？常用的做法是先由前两种条件计算出一个速度空间，然后根据第三个条件排除掉不满足的速度，这样就可以得到一个比较符合现实的速度空间了。

2.速度空间评价

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

the list of      ω : [ − 2 0 ∘ , − 1 9 ∘ , . . . , 2 0 ∘ ] ,    n = 41 \text{the list of}\;\;\omega:[-20^{\circ},-19^{\circ},...,20^{\circ}], \; n=41 the list ofω:[−20∘,−19∘,...,20∘],n=41

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

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

2.1.轨迹预测

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

假设机器人当前位姿为 ( x 0 , y 0 , θ 0 ) (x_0,y_0,\theta_0) (x0​,y0​,θ0​)，根据下述递推公式就可以得到预测的轨迹点。

需要注意的是，在整个时间周期t内，我们默认机器人的速度不变。
因此在计算上述安全停止速度时，实际使用的公式并不是上面给出的那个公式。

对时间序列的每个值进行计算，我们就可以得到一系列的轨迹点了。选择最后一个轨迹点，这就是我们预测的最终位置，而这个点的位姿可以用于下面速度的评价公式中。

2.2.速度评价

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

G ( v i , ω i ) = σ ( α ⋅ h e a d i n g ( v i , ω i ) + β ⋅ d i s t ( v i , ω i ) + γ ⋅ v e l o c i t y ( v i , ω i ) ) 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)) G(vi​,ωi​)=σ(α⋅heading(vi​,ωi​)+β⋅dist(vi​,ωi​)+γ⋅velocity(vi​,ωi​))

2.2.1.方向角评价

h e a d i n g ( v i , ω i ) heading(v_i,\omega_i) heading(vi​,ωi​)用于评价机器人在给定角速度下运动的角度与目标角度之间的差值。显然，根据函数描述，该函数值越小，方向角评价应该越高。
h e a d i n g ( v i , ω i ) = 18 0 ∘ − ∣ t a r g e t − c u r θ ∣ heading(v_i,\omega_i)=180^{\circ}-|target-cur\theta| heading(vi​,ωi​)=180∘−∣target−curθ∣

2.2.2.障碍物距离评价

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

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

假设机器人当前位置为 ( x c , y c ) (x_c,y_c) (xc​,yc​)，半径为 R R R，距离最近的障碍物位置为 ( x o , y o ) (x_o,y_o) (xo​,yo​)，则评价函数为
d i s t ( v i , ω i ) = ( x c − x 0 ) 2 + ( y c − y 0 ) 2 − R dist(v_i,\omega_i)=\sqrt{(x_c-x_0)^2+(y_c-y_0)^2}-R dist(vi​,ωi​)=(xc​−x0​)2+(yc​−y0​)2 ​−R

2.2.3.速度评价

v e l o c i t y ( v i , ω i ) velocity(v_i,\omega_i) velocity(vi​,ωi​)表示当前的机器人速度。对于路径规划而言，显然速度越快越好，因此可直接把当前线速度作为速度评价值，即
v e l o c i t y ( v i , ω i ) = ∣ v i ∣ velocity(v_i,\omega_i)=|v_i| velocity(vi​,ωi​)=∣vi​∣
在计算三种评价函数后，还需要分别做归一化处理。
最后把三种评价结果代入上述给出的评价函数，就可以对速度 ( v i , ω i ) (v_i,\omega_i) (vi​,ωi​)做出评价。

3.结束

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