当前位置:   article > 正文

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

动态窗口法

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

动态窗口法(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,ω)v1vv2,ω1ωω2}
    这个范围由多种因素决定,如机器人的速度限制、电机的加速度限制、机器人的最小转动角度等。

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

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

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

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

1.速度空间计算

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

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

1.1.移动最大速度限制

显然,机器人的速度是不可能无限增加的,所以应该考虑其最大运动速度。于是,我们可以得到如下公式:
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

在这里插入图片描述

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

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[vcv˙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.允许速度(Admissible Velocities)

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

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

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

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

在这里插入图片描述

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

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 , 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组速度。

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

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),根据下述递推公式就可以得到预测的轨迹点。但要说明的一点是,此处的轨迹预测公式是假设机器人是直线运动的,这是一种近似的运动模型。当距离很小时,我们可以用直线去近似弧线。

在这里插入图片描述

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

xyθ
\right]= \left[
xvωsinθ+vωsin(θ+ωΔt)y+vωcosθvωcos(θ+ωΔt)θ+ωΔt
\right] \; (\omega \neq0) xyθ=xωvsinθ+ωvsin(θ+ωΔt)y+ωvcosθωvcos(θ+ωΔt)θ+ωΔt(ω=0)
上述公式成立的条件是角速度不为零。当角速度为零时,公式与第一个预测公式相同。

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

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

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)=180targetcurθ

在这里插入图片描述

2.2.2.障碍物距离评价

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 ⁡ θ

xc=xvωsinθyc=y+vωcosθ
xc=xωvsinθyc=y+ωvcosθ
然后通过余弦公式可以计算出角度 γ \gamma γ,最后应用弧长公式就可以得到弧长:
s = r ⋅ γ s=r \cdot \gamma s=rγ

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

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运行的图像,蓝点表示预测的轨迹点。
在这里插入图片描述

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

在这里插入图片描述

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

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/不正经/article/detail/413187
推荐阅读
相关标签
  

闽ICP备14008679号