动态窗口法 (DWA, Dynamic Window)

DWA是一种直接给出速度指令的避障规划，基本思想是在速度空间中搜索适当的平移速度和旋转速度指令。

在Python语言下实现DWA的初步算法设计：

DWA算法思路

1. 构建速度空间

$(v,\omega)$ 空间，每一个点代表以固定半径运动，控制时间为 $\Delta t$
$运动圆弧半径r=|\frac v\omega|, ~转过角度\Delta \theta=\omega\Delta t\\$
下一个点的位置：
$\left[\begin{matrix} x'\\y'\\\theta' \end{matrix}\right] = \left[\begin{matrix} x-r\sin\theta+r\sin(\theta+\omega\Delta t)\\ y+r\cos\theta-r\cos(\theta+\omega\Delta t)\\ \theta+\omega\Delta t \end{matrix}\right]$
速度空间里这个点为0，代表上述轨迹中没有障碍物；为1则代表有障碍，不能抵达。

不碰撞的条件：
$V_a=\{(v,\omega)|v\le\sqrt{2d(v,w)\cdot a_b},\omega\le\sqrt{2d(v,\omega)\cdot\beta_b}\}\\ a_b:刹车的平移加速度\\ \beta_b:刹车的角加速度\\ d(v,\omega):圆弧r上到该点最近的一个障碍物$
如何计算 $d(v,\omega)$ ？？

初步想法是在位形空间map[][]里面画出圆形轨迹，然后算出 $(x',y')$ 坐标。沿着转动方向逐个点往前找，直到找到障碍物
圆形轨迹离散化：先把 $(\theta,\theta+\omega\Delta )$ 离散化，算出 $(x',y')$ 之后再取整得到rx，ry

2. 速度窗口空间

某时刻小车正在运动，速度是 $(v,\omega)$ 。

最大加速度限制的速度窗口空间：

$V_d=\{ (v,\omega)|v\in [v_1,v_2],\omega\in[\omega_1,\omega_2] \}\\ \cases{ v_1=v-a_{\max}\Delta t\\ v_2=v+a_{\max}\Delta t\\ \omega_1=\omega-\beta_\max\Delta t\\ \omega_2=\omega+\beta_\max\Delta t }$

2. 可行速度空间

$V_r=V_a\bigcap V_d\bigcap V_s$

其中 $V_s=\{(v,\omega)|v\in[-v_{\max},v_{\max}],\omega\in[-\omega_\max,\omega_\max] \}$ ，是最大速度上限

有个问题：好像没有考虑小车启动改变速度的过程？

到这一步为止的函数代码：

import math

def give_Vr(self, map, cx, cy, ctheta, cv, cw):
    ## 传参：cx, cy, ctheta是当前位置状态， cv, cw是当前速度状态
    ## map[x][y]=0表明没有障碍物，map[x][y]=1则有
    ## 固定的参数：间隔时间t，最大速度v_max，w_max，最大加速度a_max, b_max，最大减速度a_b,b_b
    ## 先求出Vd和Vs的交集Vds，v1<=v<=v2,w1<=w,=w2
    Vr=set()  ## Vr是一个不重复元素集，里面包含所有可行的(v,w,d,theta),d和theta在下一个函数里面有用
    
    v1=max(-v_max, cv-a_max*t)
    v2=min(v_max, cv+a_max*t)
    w1=max(-w_max, cw-b_max*t)
    w2=min(w_max, cw+b_max*t)
    
    v=v1
    w=w1
    v_step=  ## 这两个setp是(v,w)空间里取点的步长
    w_step=
    Vds=set()
    while v<=v2:
        while w<=w2:
            Vds.add((v,w))
            w+=w_step
        v+=v_step
    
    for (v,w) in Vds
        ## 检查v,w是否在Va空间内，关键是计算d(v,w)
        
        ## 先画出圆弧形轨迹
        Cirle=set()
        Circle.add((cx,cy))
        r=v/w
        i=0.05  ## 弧度变化间隔0.05rad
        while i<=w*t:
            tx=int(cx-r*math.sin(ctheta)+r*math.sin(ctheta+i))
            ty=int(cy+r*math.cos(ctheta)-r*math.cos(ctheta+i))
            
            if map[tx][ty]==0:
                Circle.add((tx,ty))
            else:
                ## 圆弧路径轨迹上有障碍物,直接判断(v,w)点不行
                flag=1
                break
                
            i+=0.05
            
        if flag==1:
            continue  ## 圆弧路上有障碍，不通
        
        x=int(cx-r*math.sin(ctheta)+r*math.sin(ctheta+w*t))
        y=int(cy+r*math.cos(ctheta)-r*math.cos(ctheta+w*t))
        theta=ctheta+w*t## x,y,theta是经过(v,w)运动后达到的位置状态
        
        ## 计算d(v,w)
        if w>0:  ## 顺时针运动
            i=0.05
            while w*t+i<math.pi:
                tx=int(x-r*math.sin(theta)+r*math.sin(theta+i))
                ty=int(y+r*math.cos(theta)-r*math.cos(theta+i))
                if map[tx][ty]==1:  ## 在圆弧上找到了障碍物
                    d=  ## 这里用曼哈顿距离还是欧拉距离？
                    break
                else:
                    i+=0.05
        else:  ## 逆时针运动
            i=-0.05
            while w*t+i<math.pi:
                tx=int(x-r*math.sin(theta)+r*math.sin(theta+i))
                ty=int(y+r*math.cos(theta)-r*math.cos(theta+i))
                if map[tx][ty]==1: 
                    d=  ## 同上？
                    break
                else:
                    i-=0.05
        ## 检查是否在Va内
        if v*v<2*d*a_b and w*w<2*d*b_b:
            Vr.add((v,w,d,theta))
        
    return Vr

4. 速度控制指令

$evaluation(v,\omega)=\alpha\cdot heading(v,\omega)+\beta\cdot dist(v,\omega)+\gamma\cdot velocity(v,\omega)\\ \begin{aligned} &\alpha+\beta+\gamma=1(\alpha\ge0,\beta\ge0,\gamma\ge0)\\ &heading:朝向目标点\\ &dist:远离障碍物\\ &velocity:速度最大化 \end{aligned}$

在可行速度空间 $V_r$ 里面挑选出权重函数最优的速度指令

import numpy

def give_vw(self,map, cx, cy, ctheta, cv, cw, gx, gy):
    Vr=give_Vr(map, cx, cy, ctheta, cv, cw)
    
    Eva=set()  ## Eva里面单元的格式为(e,v,w)，e是指令的分值
    alpha,beta=  ## 设定权重系数
    gamma=1-alpha-beta
    
    for (v,w,d,theta) in Vr:
        heading=abs(numpy.arctan((gy-cy)/(gx-cx))-theta)  ## heading函数是方向的角度差
        e=alpha*heading+beta*d+gamma*(-v)  ## e值越小越好
        Eva.add((e,v,w))
    
    (er,vr,wr)=min(Eva)
    
    return (vr,wr)

避障规划：动态窗口法(dynamic window approach)的算法思路