动态窗口法 (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)