动态窗口法 (DWA, Dynamic Window)

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

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

DWA算法思路

1. 构建速度空间

空间，每一个点代表以固定半径运动，控制时间为

下一个点的位置：

速度空间里这个点为0，代表上述轨迹中没有障碍物；为1则代表有障碍，不能抵达。

不碰撞的条件：

如何计算 ？？

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

2. 速度窗口空间

某时刻小车正在运动，速度是。

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

2. 可行速度空间

其中，是最大速度上限

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

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

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. 速度控制指令

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

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)