【自动驾驶轨迹规划之DWA算法】
欢迎大家关注我的B站：
郑同学在哔哩哔哩平台上的个人中心和介绍页面展示了他独特的视频风格。
目录
1 DWA算法简介
2 运动学模型
2.1直线轨迹模型
2.2 圆弧直线模型
2.3 车辆二自由度模型
3 DWA 算法整体框架
3.1 速度搜索空间
3.2 代价函数
3.3 搜索流程
4 DWA算法matlab实现
4.1 动画
4.2 核心算法部分
4.3 随机生成障碍物
4.4 dist 函数
4.5 distance 函数
1 DWA算法简介
该改进型DWA算法最初于1997年提出，并发表在《IEEE Robotics and Automation Magazines》期刊上, 具体内容可参考原始论文https://www.csie.ntu.edu.tw/~b92025/paper/Dyanmic_Window.pdf。对于难以预测的动态障碍物问题而言, DWA算法表现出了出色的动力学避障能力。其优势在于兼顾速度与加速度限制, 仅会考虑无碰撞路径, 因此, 采样的速度即形成了一个动态窗口. 在此生成的动态窗口中存在多条可行轨迹, 通过自行设计一个评价标准筛选出最优路径, 并反复迭代优化以寻求当前最优路径.
2 运动学模型
2.1直线轨迹模型
在机器人或智能车辆运行的短时间内（简称短时间内），轨迹可被视为匀速直线运动；然而该模型极为简单（具有显著的简化性），但由于采样时间过短导致计算复杂度较高（运算速度较慢）。下图展示了世界坐标系与机器人坐标系的示意图
若机器人不是全向运动的：
该机器人仅限于进行直线前进和旋转动作。其行进速度方向与机器人航向角始终保持一致状态，并且其运动过程遵循以下差分方程模型：其中控制参数包括控制线速度的加速度项以及控制角速度的转矩项。
若机器人是全向运动的：
那么机器人在任何方向上都有速度，并不总是与航向角一致。因此，在机器人坐标系中可以将速度分解为横向和纵向两个分量。其中关键的控制变量包括：用于调整前进的速度（即横向加速度），用于调整高度或深度（即纵向加速度），以及用于改变转向方向的能力（即角加速度）。运动学差分方程如后所示：
该微分方程可视为第一个微分方程的调整版本，在引入用于表示机器人纵向速度在世界坐标系中投影的行为项后完成。其中，在基于ROS的轨迹推演框架中使用了此计算模型，并且，在base_local_planner的轨迹采样程序中也采用了此模型作为核心算法。
2.2 圆弧直线模型
对于时间极其短暂的情况，在将运动简化为匀速直线运动时存在明显偏差。这一做法不仅与机器人及车辆动力学理论相悖，在动态系统建模中我们采用了更为精确的动力学模型描述以确保计算结果的准确性与可靠性。
建立关于加速度和角加速度的运动学模型，并设定起始状态中的线性初速、起始状态中的角线性初速以及起始方位角度参数值后，则可用于补充上述数学表达式
同样地，在y轴方向上确定位置关系时（即y坐标值），我们可以通过该公式可以看出机器人可以由控制变量与初始状态来进行未来状态的控制。考虑到机器人内部结构的限制（即其加速度的变化具有不连续性），因此将时间段t0到tn划分为多个小时间段（即时间片），从而使得积分计算可以转化为对各个小时间段内的运动量求和。具体来说，在每一个时间片内（即切片），当分割的数量n足够大时，在每个切片中我们可以假设加速度与角加速度是恒定的。
然而该公式仍然无法确定机器人具体的行驶方向。同样地，在计算障碍物与路径的交叉点时也存在困难。为了简化计算过程，在n足够大的情况下,角加速度和加速度可近似看作对角速度和速度不起作用*
简化为
在y轴上对应的方程可知，在wi取值为零时会呈现直线轨迹；而当wi不为零时则呈现圆弧形轨迹。
通过上述公式能够计算得到机器人的运动轨迹。具体而言，则可通过多段圆弧和平直线组合的方式进行拟合。值得注意的是，在DWA算法的经典论文中也采用了类似的运动学模型。
2.3 车辆二自由度模型
模型的假设：
（1）车辆行驶于平坦路面，忽略车辆在垂直于地面方向上的运动；
（2）忽略车辆受到的空气阻力以及地面侧向摩擦力；
（3）车辆与地面始终保持良好的滚动摩擦；
（4）车辆为刚体，忽略车身悬架结构的影响。
该模型假设左右前后车轮在运动中产生的角速度和角加速度变化具有相似性，并通过将前驱驱动的前后车轮沿纵向中心线统一处理为一个等效的虚拟车轮来简化分析过程。由此得名bicycle运动学模型。运动学微分方程组如下
3 DWA 算法整体框架
3.1 速度搜索空间
主要轨迹是以圆弧形式存在。其中的采样速度(v,w)必须位于允许范围内，并通过这些参数来确定该轨迹。这些参数值形成一个初始的速度搜索区域。
（2）设为动态调整的速度上限，并确保车辆能够及时减速至零，并成功避让前方所有可能的障碍。
dist(v,w)为机器人轨迹上与最近的障碍物的距离。
考虑到加速度具有一定的限制范围，在一定时间段内达到的最大加速度或负方向的最大加速度所达到的速度 从而被保留下来。从而形成了动态窗口。
这里的(v0,w0)是当前的速度。
综上所述，搜索空间为
下图是搜索空间下的采样轨迹 示例
3.2 代价函数
该段落中的heading参数代表的是方位角与实际机器人至目标点连线所形成的角度偏差值，并其主要作用在于矫正机器人的航行方向以便迅速抵达目标点位置。而这一误差值通常可以通过余弦定理或反正弦函数来进行计算。
（2）这里的dist表示机器人轨迹上与最近的障碍物的距离。其目标是远离障碍物。在计算过程中通常会设定一个最大阈值，在超过该距离时将dist设为该最大阈值以避免让机器人趋向于远离障碍物的情况发生。
（3）在本系统中，变量vel表示机器人的移动速度。主要目标是使机器人能够迅速且准确地抵达预定位置。同时防止由于避开障碍物而导致移动速度过低的情况。在计算过程中，默认采用的是绝对值形式。
在多场景环境下，三个重要指标（即权重分配方案）需根据具体情况微调以实现最佳效果。同时通过权重归一化处理可有效解决量纲问题。
为了优化系统性能，在路径规划中实现三个部分的权重更加均匀分布 ，从而确保轨迹与障碍物之间维持合理的安全距离。
当机器人陷入局部最优状态（即当前路径无法通向目标点），通过使其原地旋转 ，可有效避免陷入僵局并寻找到新的可行路径。
在路径规划过程中需确定一个安全裕度区间，在路径与障碍物之间预留必要的空白区域。值得注意的是，在速度提升的过程中这一空白区域也会随之按线性比例扩大以保证系统的安全性。
下图为评价函数的示例
3.3 搜索流程
DWA算法的基本原理首先涉及构建基于3.1节所述三种约束条件下的搜索空间。随后采用固定或可调节的分辨率对速度分量(v,w)进行采样操作。对于每个采样点(v,w)，都会计算其对应的代价函数值。在此基础上选择在时间间隔t内具有最低总代价的最佳轨迹，并将机器人按照所选最佳轨迹对应的速度模式，在时间段t内持续运动t/n的时间段（其中n由用户自行设定）。这一过程之后会不断重复上述过程……直至机器人与目标点之间的距离小于预设阈值。
4 DWA算法matlab实现
4.1 动画
4.2 核心算法部分
超参数自行调整，运动学模型 可以根据本篇文章介绍的自行调整
clc
clear all
%如下初始化范围均可自行调整
a=[-1,1];%加速度范围
b=[-20,20];%角加速度范围
vfan=[0.2,0.5];%速度范围
wfan=[-20,20];%角速度范围
v=0.4;%初始速度
o=pi/4;%初始化航向角
dt=0.1;%时间间隔
t=0;
wxx=0;%初始角速度
x=0;y=0;%机器人实时坐标
B=[90,90];%目标点坐标
k1=[0];k2=[0];%储存路径
[f,n1]=ob(5);%随机生成障碍物，个数自行调整，个数过多不易生成
while distance(x,y,B(1),B(2))>2 %自行设定与目标点的截止距离
va=sqrt(2*a(2)*dist(x,y,f,n1));%撞上障碍物的最大速度
wa=sqrt(2*b(2)*dist(x,y,f,n1));
max=0;
%速度在加速或减速可达的范围内
if va<v+dt*a(2)
vmax=va;
vmin=v+dt*a(1);
else
vmax=v+dt*a(2);
vmin=v+dt*a(1);
end
if vmax>vfan(2)
vmax=vfan(2);
end
if vmin<vfan(1)
vmin=vfan(1);
end
if wa<wxx+dt*a(2)
wmax=wa;
wmin=wxx+dt*b(1);
else
wmax=wxx+dt*b(2);
wmin=wxx+dt*b(1);
end
if wmax>wfan(2)
wmax=wfan(2);
end
if wmin<wfan(1)
wmin=wfan(1);
end
for v=vmin:0.3:vmax %采样频率自行调整
for w=wmin:0.3:wmax%采样频率自行调整
Fxx=x;Fyy=y;oxx=o;%保存起点位置
max1=10;%根据障碍物大小进行调整
min1=10000;
while t<12 %评估轨迹时的时间间隔，可自行调整
Fx=x;Fy=y;
x=x+v*dt*cos(o);%位置更新
y=y+v*dt*sin(o);
if dist(x,y,f,n1)<min1 %dist等于整条轨迹中离障碍物最近的距离
min1= dist(x,y,f,n1);
end
if min1>=max1 %在距离障碍物距离大于max1时，dist恒等于max1
max1=max1;
else
max1=min1;
end
o=o+w*dt*0.1;%航向角更新
t=t+dt;
%plot([Fx,x],[Fy,y],'b-');hold on;xlim([0,100]);ylim([0,100]);
%这一语句是来测试一个速度不同角速度下的采样轨迹是否合理
end
pinjia1=abs(o-atan((90-y)/(90-x)));%航向角偏差
if pinjia1>pi
pinjia1=(pi-(2*pi-pinjia1))*70;%转化到0~pi范围并乘权重
else
pinjia1=(pi-pinjia1)*70;
end
pinjia2=max1*190;%可调整参数
pinjia3=abs(v)*90;%可调整参数
pinjia=pinjia1+pinjia2+pinjia3;
if pinjia>max
max=pinjia;
vfinal=v;wfinal=w; %不断选取最优评价函数的(v,w)
end
t=0;
x=Fxx;y=Fyy;o=oxx;%起点位置不能丢失，要保存
end
end
t=0;x=Fxx;y=Fyy;o=oxx;
while t<1 %实际移动时的时间间隔
Fx=x;Fy=y;
x=x+vfinal*dt*cos(o);
y=y+vfinal*dt*sin(o);
o=o+wfinal*dt*0.1;
t=t+dt;
%plot([Fx,x],[Fy,y],'b-');hold on;xlim([0,100]);ylim([0,100]);
end
%plot(x,y,'ro');hold on;xlim([0,100]);ylim([0,100]);
v=vfinal;wxx=wfinal;
k1=[k1,x];
k2=[k2,y];
end
%显示动画
f1=size(k1);
for i=1:f1(2)
plot(k1(i),k2(i),'ro');
pause(0.0001);hold off;%动画速度自行调整
aplha=0:pi/40:2*pi;
for j=1:n1
r=f(3*j);
x=f(3*j-1)+r*cos(aplha);
y=f(3*j-2)+r*sin(aplha);
plot(x,y,'b-');hold on
plot(0,0,'go');hold on;
plot(90,90,'ko');hold on;
hold on;
end
end
AI助手4.3 随机生成障碍物
function [f,n1]=ob(n)
f=[];%储存障碍物信息
n1=n;%返回障碍物个数
p=0;
for i=1:n
k=1;
while(k)
D=[rand(1,2)*60+15,rand(1,1)*1+3];%随机生成障碍物的坐标与半径，自行调整
if(distance(D(1),D(2),90,90)>(D(3)+5)) %与目标点距离一定长度，防止过多阻碍机器人到达目标点
k=0;
end
for t=1:p %障碍物之间的距离不能过窄，可自行调整去测试
if(distance(D(1),D(2),f(3*t-2),f(3*t-1))<=(D(3)+f(3*t)+20))
k=1;
end
end
end
%画出障碍物
aplha=0:pi/40:2*pi;
r=D(3);
x=D(1)+r*cos(aplha);
y=D(2)+r*sin(aplha);
plot(x,y,'b-');
axis equal;
hold on;
xlim([0,100]);ylim([0,100]);
f=[f,D];
p=p+1;%目前生成的障碍物个数
end
hold all;
AI助手4.4 dist 函数
function f=dist(a,b,D,n)
min=10000;%
for i=1:n
if abs(distance(a,b,D(3*i-2),D(3*i-1))-D(3*i))<min
min=distance(a,b,D(3*i-2),D(3*i-1))-D(3*i);
end
end
f=min;
AI助手4.5 distance 函数
function f=distance(x,y,x1,y1)
f=sqrt((x-x1)^2+(y-y1)^2);
AI助手