动态窗口法（Dynamic Window Approach，DWA）是一类经典的机器人局部路径规划算法。它的过程主要分为两部分：
速度空间
(
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}
这个范围由多种因素决定，如机器人的速度限制、电机的加速度限制、机器人的最小转动角度等。
速度空间 ( v , ω ) (v,\omega) (v,ω)的评价。根据给定的评价公式，对速度空间的每一组 ( v i , ω i ) (v_i,\omega_i) (vi,ωi)进行评价，然后选择一组最佳速度作为当前速度。
值得注意的是，在使用DWA方法进行路径规划时，一般应用于以下场景：
接下来，我会详细介绍DWA的算法流程。
在计算速度空间时，为了更加贴近现实，我们一般会考虑一些运动限制。在DWA中，主要考虑了三种运动限制：
显然，机器人的速度是不可能无限增加的，所以应该考虑其最大运动速度。于是，我们可以得到如下公式：
v
s
∈
[
v
m
i
n
,
v
m
a
x
]
v_s \in [v_{min}, v_{max}]
vs∈[vmin,vmax]
ω s ∈ [ ω m i n , ω m a x ] \omega_s \in [\omega_{min},\omega_{max}] ωs∈[ωmin,ωmax]
一般最小线速度取零，最小角速度和最大角速度互为相反数。但考虑到为了增加评价函数的区分度，可以考虑把最小线速度设置为1。因为当线速度为0时，评价函数就无法发挥作用。从下图的上下限可以看出，其最大速度限制设置为：
v
m
i
n
=
0
,
v
m
a
x
=
90
,
ω
m
i
n
=
−
90
,
ω
m
a
x
=
90
v_{min}=0, \; v_{max}=90, \; \omega_{min}=-90, \; \omega_{max}=90
vmin=0,vmax=90,ωmin=−90,ωmax=90
如果不考虑其它运动限制，在设置最大速度限制时，上图的坐标系上的所有点都是我们需要考虑的速度，这些速度构成了一个速度空间。
由于电机的转矩有限，因此存在一个最大的加速度限制。假设机器人当前速度为
(
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)分别代表线速度最大减速度、线速度最大加速度、角速度最大减速度以及角速度最大加速度。
在一般情况下，都是默认最大加速度和最大减速度的绝对值相同。下图的白色矩形区域就是考虑加速度限制后可以取到的速度空间，也就是论文提到的动态窗口，该矩形的中心就是机器人当前的运动速度。
当机器人检测到障碍物时，应该留有一段刹车距离用于减速。当机器人与最近障碍物的距离小于刹车距离时，机器人就会与障碍物发生碰撞，这是我们不愿看到的情况。因此，我们应该考虑一个允许速度，也就是可以安全停止下来的速度。
显然，这个安全停止速度应该与机器人与最近障碍物的距离成正比。当距离越小时，这个安全停止速度也应该相应减小。它们应该满足如下不等式：
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
计算安全速度的难点在于机器人与障碍物的最短距离如何计算。因为角速度的存在，机器人并不是直线运动的，它的运动轨迹是一条弧线。因此在计算与障碍物的距离时，我们求的应该是弧长，而不是直线距离。
下图的淡灰色区域就是可允许速度，而深灰色区域则是不可允许速度。
在计算三种速度限制后，我们得到了三个速度范围，它们的交集就是我们在当前状态下可以取到的速度范围，也就是上图中的红色区域。
V
r
=
V
s
⋂
V
d
⋂
V
a
V_r=V_s \bigcap V_d \bigcap V_a
Vr=Vs⋂Vd⋂Va
在得到速度空间后，根据评价函数，我们就通过遍历每一组速度
(
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
,
3
]
,
ω
∈
[
−
2
0
∘
,
2
0
∘
]
v \in [0, 3],\;\omega \in [-20^{\circ},20^{\circ}]
v∈[0,3],ω∈[−20∘,20∘]
根据设置的速度增量，我们可以得到如下速度序列
the list of v
:
[
0
,
0.01
,
.
.
.
,
3.00
]
,
n
=
101
\text{the list of v}:[0,0.01,...,3.00], \; n=101
the list of v:[0,0.01,...,3.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组速度。
在得到每组速度后，我们还需要做一些预备工作，即对每组速度生成在给定时间周期内的轨迹预测。
假设有一组速度为 ( 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)，根据下述递推公式就可以得到预测的轨迹点。但要说明的一点是，此处的轨迹预测公式是假设机器人是直线运动的，这是一种近似的运动模型。当距离很小时，我们可以用直线去近似弧线。
除了上述预测公式外，还有更精确的轨迹公式。事实上，当线速度和角速度不变时，机器人的运动轨迹应该是一个半径为
∣
v
ω
∣
|\frac{v}{\omega}|
∣ωv∣的圆。这也是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−ωvsinθ+ωvsin(θ+ωΔt)y+ωvcosθ−ωvcos(θ+ωΔt)θ+ωΔt⎦⎤(ω=0)
上述公式成立的条件是角速度不为零。当角速度为零时，公式与第一个预测公式相同。
另外需要注意的是，在整个预测的时间周期 ( t 1 , t n ) (t_1,t_n) (t1,tn)，我们默认机器人的速度不变。但在计算安全停止速度时，预测的轨迹点的线速度应该是减少的。
对时间序列的每个值进行计算，我们就可以得到一系列的轨迹点了。但不是每一个轨迹点都是有用的，我们需要对每个轨迹点进行判断。假设某个轨迹点与障碍物碰撞，则我们应该舍弃这个以及后面的所有轨迹点，选择上一个轨迹点作为最终预测的轨迹点。
速度评价函数主要由三个部分组成，分别为方向角评价、障碍物距离评价以及速度评价。
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))
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θ∣
d i s t ( v i , ω i ) dist(v_i,\omega_i) dist(vi,ωi)用于表示机器人当前位置与最近的障碍物之间的距离。如果轨迹上无障碍物，则设定一个常数。根据函数描述可以得知，当机器人与障碍物的距离越大，则该函数评价应该越高。因此，该函数可以直接用距离作为评价函数。
另外值得注意的是，在计算距离时还应该考虑机器人的半径。
当角速度不为零时，我们应该计算的距离是弧线长度，而不是长度
c
c
c。这个弧长的计算也比较简单，根据运动模型，我们知道：当角速度和线速度不变时，机器人的运动轨迹为一个标准的圆形。该圆的半径为（角速度单位为弧度每秒）：
r
=
∣
v
ω
∣
(
ω
≠
0
)
r=|\frac{v}{\omega}| \; (\omega \neq 0)
r=∣ωv∣(ω=0)
设机器人当前的位姿为
(
x
,
y
,
θ
)
(x,y,\theta)
(x,y,θ)（这里并不是预测点的位姿），则该圆的圆心坐标为：
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−ωvsinθyc=y+ωvcosθ
然后通过余弦公式可以计算出角度
γ
\gamma
γ，最后应用弧长公式就可以得到弧长：
s
=
r
⋅
γ
s=r \cdot \gamma
s=r⋅γ
（补充于2021.5.30： 最近看了CVM算法，发现它和DWA的算法过程十分相似，区别只在于障碍物距离的计算方式不同。CVM论文中的很大篇幅都在描述如何计算障碍物距离，而且它考虑的是弧线长度。值得一提的是，CVM将曲率c（角速度与线速度的比值）作为讨论对象，即在遍历速度时，CVM遍历的是角速度与线速度的比值。）
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)做出评价。
当机器人运动到某个位置时，首先计算速度空间，然后对每一组速度进行轨迹预测并给出速度评价，最后取评价最高的一组速度作为当前速度。就这样，机器人不断进行速度的计算、评价和选择，就可以越来越接近目标而不碰撞障碍物了。最后给出DMA运行的图像，蓝点表示预测的轨迹点。
在经过实际仿真后，我发现在较为复杂的环境下，DWA方法并不是很有效。特别是当目标位置与当前位置相隔一层障碍物时，此时的评价函数的三个参数很难调，很难同时让角度评价和距离评价的效果都很好。
在上图中，机器人与目标点相隔一层障碍物，只有一个缺口可以通过。此时，如果设置角度评价参数较高，那么机器人很可能会在左侧区域打转；如果设置障碍物距离参数较高，即使机器人很靠近目标时，可能仍然会错过。或许可以根据具体情况自适应地调整参数，而不是让参数固定不变。
