局部路径规划 DWA 算法完全解析（理论推导+代码实现，包你看懂！）
转载请注明出处，谢谢
理论基础
前面学习的全局路径规划方法，Dijkstra、Best-First-Search、A*算法都属于状态采样（State Sampling）方法，而 DWA 局部路径规划则属于典型的动作采样（action sampling）方法
DWA 算法（Dynamic Window Approach）的原理主要是以一定的分辨率在速度空间 (v, w) 中采样多组速度，并模拟出这些速度在一定时间的运动轨迹，得到可行轨迹后通过评价函数对轨迹进行评分，评分后选取最优轨迹对应的 (v, w) 驱动机器人运动
速度空间
速度空间 (v, w) 即机器人的速度范围，机器人的速度受到各种因素的限制
- 移动机器人受自身最大速度最小速度的限制
Vs 为机器人能够到达的所有矢量速度的集合；机器人受到最大最小线速度和角速度影响
- 移动机器人受电机性能的影响
由于加速度有一个范围限制，所以最大加速度或最大减速度一定时间内能达到的速度 ，才会被保留，表达式如下：
- 移动机器人受障碍的影响
为了能在碰到障碍物前停下来，在最大减速度的条件下，速度满足以下条件：
其中dist(v,w)为（v,w）对应的轨迹上离障碍物最近的距离
在上述 3 个约束条件下，速度空间 (v, w) 会有一定的范围，注意速度空间不是固定不变的，而是时变的，t 时刻的速度空间与 t + dt 时刻的速度空间是不同的，故将其称为动态窗口
速度采样
得到 t 时刻的速度空间 (v, w) 后，以一定的分辨率对速度 v 和角速度 w 进行采样
设 t 时刻小车 x 轴线速度取值范围 5~20m/s,为简化数学模型，设 y轴线速度为0m/s，小车的角速度取值范围 0.1~1rad/s,小车的线速度的速度分辨率为0.1m/s，角速度的速度分辨率为0.05rad/s
根据离散的方法，离散 x 轴线速度和角速度如下
运动模型
之所以要分析机器人的运动学模型，是因为要根据采样的速度 (v, w) 模拟机器人的运动轨迹
DWA 算法适用于差速运动模型和全向运动模型，并不适用于阿克曼模型
差速运动模型
机器人只能向前运动或者旋转，图中有世界坐标系和机器人坐标系，下式中 v(t) 指机器人坐标系中 x 方向的速度
全向移动模型
在全向移动模型中，多了机器人坐标系下 y 方向的速度
令 Vy(t) = 0，则退化为差速运动模型，因此 ROS 自带的局部路径规划器使用上式计算
轨迹预测
在确定机器人的采样速度和运动模型后，可以对机器人的状态进行预测和更新
机器人下一时刻位移距离
机器人下一时刻坐标的变化
采样时刻机器人坐标与坐标变化求和，得到下一时刻机器人坐标
设 ∆t 为 0.1s，实际上预测机器人前向几秒内的所有状态，假设前向预测时间为 3s，则会预测 t ~ t + 3 s内的所有状态，即 t ~ t + ∆t ~ t + 2*∆t ~ …. ~ t + 3，相当于会预测出 t 时刻位置前 30 个点的位置，如下图所示
评价函数
对速度空间进行采样后，根据机器人运动学模型能够预测出多条轨迹，需要对这些轨迹进行评价，选取最优的轨迹，机器人根据最优轨迹对应的速度进行运动
DWA 算法对轨迹的评价函数一般如下所示
heading(v,w)为方位角评价函数：评价机器人在当前的设定的速度下，轨迹末端朝向与目标点之间的角度差距dist(v,w)主要的意义为机器人处于预测轨迹末端点位置时与地图上最近障碍物的距离，对于靠近障碍物的采样点进行惩罚，确保机器人的避障能力，降低机器人与障碍物发生碰撞的概率velocity(v,w)为当前机器人的线速度，为了促进机器人快速到达目标- α、β、γ、σ 为权重系数
很好理解：
1、希望我的前进方向对准终点
2、希望不发生任何碰撞
3、希望速度尽量快
4、除此之外，还要保证最短刹车距离是安全的
原文中提到方位角评价函数对轨迹规划的影响较大，太大易陷入局部最优解，太小能更好地避障，但路径有点长，效率有限低
当然也可以对评价函数进行优化，添加更多的评价函数指标
注意需要对评价函数的结果进行归一化处理，将每一项除以每一项的总和，如下式
这样可以避免不同类别得分基数相差太大造成的结果误差
至此，DWA 算法的完整流程
MATLAB 仿真
程序的思维导图如下，这里只讲解重要的部分，绘图及障碍物设置部分暂时略过
这里的 for 循环有次数限制，最多循环 5000 次
for i = 1:5000
% DWA参数输入 返回控制量 u = [v(m/s),w(rad/s)] 和 轨迹
[u,traj] = DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);%算出下发速度u/当前速度u
x = f(x,u);% 机器人移动到下一个时刻的状态量 根据当前速度和角速度推导 下一刻的位置和角度
abc = abc+1;
% 历史轨迹的保存
result.x = [result.x; x']; %最新结果 以行的形式 添加到result.x，保存的是所有状态参数值，包括坐标xy、朝向、线速度、角速度，其实应该是只取坐标就OK
% 是否到达目的地
if norm(x(POSE_X:POSE_Y)-goal')<0.25 % norm函数来求得坐标上的两个点之间的距离
disp('==========Arrive Goal!!==========');break;
end
%====Animation====
hold off; % 关闭图形保持功能。 新图出现时，取消原图的显示。
ArrowLength = 0.5; % 箭头长度
DynamicWindowApproach() 函数
DynamicWindowApproach() 函数最重要的函数，通过该函数返回最优控制量 u
%% DWA算法实现
% model 机器人运动学模型 最高速度[m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss], 速度分辨率[m/s],转速分辨率[rad/s]]
% 输入参数：当前状态、模型参数、目标点、评价函数的参数、障碍物位置、障碍物半径
% 返回参数：控制量 u = [v(m/s),w(rad/s)] 和 轨迹集合 N * 31 （N：可用的轨迹数）
% 选取最优参数的物理意义：在局部导航过程中，使得机器人避开障碍物，朝着目标以较快的速度行驶。
function [u,trajDB] = DynamicWindowApproach(x,model,goal,evalParam,ob,R)
% Dynamic Window [vmin,vmax,wmin,wmax] 最小速度 最大速度 最小角速度 最大角速度速度
Vr = CalcDynamicWindow(x,model); % 根据当前状态 和 运动模型 计算当前的参数允许范围
% 评价函数的计算 evalDB N*5 每行一组可用参数 分别为 速度、角速度、航向得分、距离得分、速度得分
% trajDB 每5行一条轨迹 每条轨迹都有状态x点串组成
[evalDB,trajDB]= Evaluation(x,Vr,goal,ob,R,model,evalParam); %evalParam 评价函数参数 [heading,dist,velocity,predictDT]
if isempty(evalDB)
disp('no path to goal!!');
u=[0;0];return;
end
% 各评价函数正则化
evalDB = NormalizeEval(evalDB);
% 最终评价函数的计算
feval=[];
for id=1:length(evalDB(:,1))
feval = [feval;evalParam(1:3)*evalDB(id,3:5)']; %根据评价函数参数 前三个参数分配的权重 计算每一组可用的路径参数信息的得分
end
evalDB = [evalDB feval]; % 最后一组；加最后一列，每一组速度的最终得分
[maxv,ind] = max(feval);% 选取评分最高的参数 对应分数返回给 maxv 对应下标返回给 ind
u = evalDB(ind,1:2)';% 返回最优参数的速度、角速度
CalcDynamicWindow() 函数
在该函数中首先调用 CalcDynamicWindow() 函数计算当前时刻的速度空间 Vr
%% 计算动态窗口
% 返回 最小速度 最大速度 最小角速度 最大角速度速度
function Vr = CalcDynamicWindow(x,model)
V_SPD = 4;%机器人速度
W_ANGLE_SPD = 5;%机器人角速度
MD_MAX_V = 1;%
MD_MAX_W = 2;%
MD_ACC = 3;%
MD_VW = 4;%
global dt;
% 车子速度的最大最小范围 依次为：最小速度 最大速度 最小角速度 最大角速度速度
Vs=[0 model(MD_MAX_V) -model(MD_MAX_W) model(MD_MAX_W)];
% 根据当前速度以及加速度限制计算的动态窗口 依次为：最小速度 最大速度 最小角速度 最大角速度速度
Vd = [x(V_SPD)-model(MD_ACC)*dt x(V_SPD)+model(MD_ACC)*dt ...
x(W_ANGLE_SPD)-model(MD_VW)*dt x(W_ANGLE_SPD)+model(MD_VW)*dt];
% 最终的Dynamic Window
Vtmp = [Vs;Vd]; %2 X 4 每一列依次为：最小速度 最大速度 最小角速度 最大角速度速度
disp(Vtmp);
Vr = [max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))];
这里相当于对 Vs 和 Vd 求了交集，如下图所示，Va 在后面的代码中有考虑
Evaluation() 函数
% 评价函数的计算 evalDB N*5 每行一组可用参数 分别为 速度、角速度、航向得分、距离得分、速度得分
% trajDB 每5行一条轨迹 每条轨迹都有状态x点串组成
[evalDB,trajDB]= Evaluation(x,Vr,goal,ob,R,model,evalParam); %evalParam 评价函数参数 [heading,dist,velocity,predictDT]
%% 评价函数 内部负责产生可用轨迹
% 输入参数 ：当前状态、参数允许范围（窗口）、目标点、障碍物位置、障碍物半径、评价函数的参数
% 返回参数：
% evalDB N*5 每行一组可用参数 分别为 速度、角速度、航向得分、距离得分、速度得分
% trajDB 每5行一条轨迹 每条轨迹包含 前向预测时间/dt + 1 = 31 个轨迹点（见生成轨迹函数）
function [evalDB,trajDB] = Evaluation(x,Vr,goal,ob,R,model,evalParam)
evalDB = [];
trajDB = [];
for vt = Vr(1):model(5):Vr(2) %根据速度分辨率遍历所有可用速度： 最小速度和最大速度 之间 速度分辨率 递增
for ot=Vr(3):model(6):Vr(4) %根据角度分辨率遍历所有可用角速度： 最小角速度和最大角速度 之间 角度分辨率 递增
% 轨迹推测; 得到 xt: 机器人向前运动后的预测位姿; traj: 当前时刻 到 预测时刻之间的轨迹（由轨迹点组成）
[xt,traj] = GenerateTrajectory(x,vt,ot,evalParam(4)); %evalParam(4),前向模拟时间;
% 各评价函数的计算
heading = CalcHeadingEval(xt,goal); % 前项预测终点的航向得分 偏差越小分数越高
[dist,Flag] = CalcDistEval(xt,ob,R); % 前项预测终点 距离最近障碍物的间隙得分 距离越远分数越高
vel = abs(vt); % 速度得分 速度越快分越高
stopDist = CalcBreakingDist(vel,model); % 制动距离的计算
if dist > stopDist && Flag == 0 % 如果可能撞到最近的障碍物 则舍弃此路径 （到最近障碍物的距离 大于 刹车距离 才取用）
evalDB = [evalDB;[vt ot heading dist vel]];
trajDB = [trajDB;traj]; % 每5行 一条轨迹
disp(evalDB);
disp(trajDB);
end
end
end
Evaluation() 这个函数也很重要，它肩负了较多的职责，包括
- 对速度空间进行离散化采样
for vt = Vr(1):model(5):Vr(2) %根据速度分辨率遍历所有可用速度： 最小速度和最大速度 之间 速度分辨率 递增
for ot=Vr(3):model(6):Vr(4) %根据角度分辨率遍历所有可用角速度： 最小角速度和最大角速度 之间 角度分辨率 递增
- 进行轨迹推测，预测出当前时刻到预测时刻之间的轨迹
调用 GenerateTrajectory() 函数进行轨迹预测
%% 单条轨迹生成、轨迹推演函数
% 输入参数： 当前状态、vt当前速度、ot角速度、evaldt 前向模拟时间、机器人模型参数（没用到）
% 返回参数;
% x : 机器人模拟时间内向前运动 预测的终点位姿(状态);
% traj: 当前时刻 到 预测时刻之间 过程中的位姿记录（状态记录） 当前模拟的轨迹
% 轨迹点的个数为 evaldt / dt + 1 = 3.0 / 0.1 + 1 = 31
%
function [x,traj] = GenerateTrajectory(x,vt,ot,evaldt)
global dt;
time = 0;
u = [vt;ot];% 输入值
traj = x; % 机器人轨迹
while time <= evaldt
time = time+dt; % 时间更新
x = f(x,u); % 运动更新 前项模拟时间内 速度、角速度恒定
traj = [traj x]; % 每一列代表一个轨迹点 一列一列的添加
end
在 f() 函数中通过状态空间表达式的形式更新机器人的状态
%% Motion Model 根据当前状态推算下一个控制周期（dt）的状态
% u = [vt; wt];当前时刻的速度、角速度 x = 状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
function x = f(x, u)
global dt;
F = [1 0 0 0 0
0 1 0 0 0
0 0 1 0 0
0 0 0 0 0
0 0 0 0 0];
B = [dt*cos(x(3)) 0
dt*sin(x(3)) 0
0 dt
1 0
0 1];
x= F*x+B*u;
- 计算评价函数
调用 CalcHeadingEval() 计算方位角评价函数
%% heading的评价函数计算
% 输入参数：当前位置、目标位置
% 输出参数：航向参数得分 当前车的航向和相对于目标点的航向 偏离程度越小 分数越高 最大180分
function heading = CalcHeadingEval(x,goal)
theta = toDegree(x(3));% 机器人朝向
goalTheta = toDegree(atan2(goal(2)-x(2),goal(1)-x(1)));% 目标点相对于机器人本身的方位
if goalTheta > theta
targetTheta = goalTheta-theta;% [deg]
else
targetTheta = theta-goalTheta;% [deg]
end
heading = 180 - targetTheta;
调用 CalcDistEval() 计算障碍物距离评价函数
%% 障碍物距离评价函数 （机器人在当前轨迹上与最近的障碍物之间的距离，如果没有障碍物则设定一个常数）
% 输入参数：位姿、所有障碍物位置、障碍物半径
% 输出参数：当前预测的轨迹终点的位姿距离所有障碍物中最近的障碍物的距离 如果大于设定的最大值则等于最大值
% 距离障碍物距离越近分数越低
function [dist,Flag] = CalcDistEval(x,ob,R)
dist=100;
for io = 1:length(ob(:,1))
disttmp = norm(ob(io,:)-x(1:2)')-R; %到第io个障碍物的距离 - 障碍物半径 ！！！有可能出现负值吗
if disttmp <0
Flag = 1;
break;
else
Flag = 0;
end
if dist > disttmp % 大于最小值 则选择最小值
dist = disttmp;
end
end
% 障碍物距离评价限定一个最大值，如果不设定，一旦一条轨迹没有障碍物，将太占比重
if dist >= 3*R %最大分数限制
dist = 3*R;
end
调用 abs() 计算速度评价函数
vel = abs(vt); % 速度得分 速度越快分越高
调用 CalcBreakingDist() 函数计算制动距离，并比较制动距离与最近障碍物距离，判断是否要舍弃该路径，这里相当于考虑了速度空间中的 Va
%% 计算制动距离
%根据运动学模型计算制动距离, 也可以考虑成走一段段圆弧的累积 简化可以当一段段小直线的累积
function stopDist = CalcBreakingDist(vel,model)
global dt;
MD_ACC = 3;%
stopDist=0;
while vel>0 %给定加速度的条件下 速度减到0所走的距离
stopDist = stopDist + vel*dt;% 制动距离的计算
vel = vel - model(MD_ACC)*dt;%
end
NormalizeEval() 函数
计算得到各轨迹的评价函数后，调用 NormalizeEval() 函数对各评价函数正则化
%% 归一化处理
% 每一条轨迹的单项得分除以本项所有分数和
function EvalDB=NormalizeEval(EvalDB)
% 评价函数正则化
if sum(EvalDB(:,3))~= 0
% disp(EvalDB(:,3));
% disp(sum(EvalDB(:,3)));
EvalDB(:,3) = EvalDB(:,3)/sum(EvalDB(:,3)); %矩阵的数除 单列矩阵的每元素分别除以本列所有数据的和
end
if sum(EvalDB(:,4))~= 0
EvalDB(:,4) = EvalDB(:,4)/sum(EvalDB(:,4));
end
if sum(EvalDB(:,5))~= 0
EvalDB(:,5) = EvalDB(:,5)/sum(EvalDB(:,5));
end
然后就选取评分最高的路径，并返回该路径对应的控制量
% 最终评价函数的计算
feval=[];
for id=1:length(evalDB(:,1))
feval = [feval;evalParam(1:3)*evalDB(id,3:5)']; %根据评价函数参数 前三个参数分配的权重 计算每一组可用的路径参数信息的得分
end
evalDB = [evalDB feval]; % 最后一组；加最后一列，每一组速度的最终得分
[maxv,ind] = max(feval);% 选取评分最高的参数 对应分数返回给 maxv 对应下标返回给 ind
u = evalDB(ind,1:2)';% 返回最优参数的速度、角速度
获得最优控制量后，在主循环中更新机器人为下一时刻状态，并将历史轨迹保存，同时根据当前点与目标点间的欧氏距离判断是否到达目标点
x = f(x,u);% 机器人移动到下一个时刻的状态量 根据当前速度和角速度推导 下一刻的位置和角度
abc = abc+1;
% 历史轨迹的保存
result.x = [result.x; x']; %最新结果 以行的形式 添加到result.x，保存的是所有状态参数值，包括坐标xy、朝向、线速度、角速度，其实应该是只取坐标就OK
% 是否到达目的地
if norm(x(POSE_X:POSE_Y)-goal')<0.25 % norm函数来求得坐标上的两个点之间的距离
disp('==========Arrive Goal!!==========');break;
end
仿真效果
% 评价函数参数 [heading,dist,velocity,predictDT]
% 航向得分的比重、距离得分的比重、速度得分的比重、向前模拟轨迹的时间
evalParam = [0.06, 0.1 ,0.1, 3.0];
>> dwa_V_1_0
Dynamic Window Approach sample program start!!
==========Arrive Goal!!==========
历时 46.357604 秒。
323
将航向得分比重改为 0.04，结果如下
>> dwa_V_1_0
Dynamic Window Approach sample program start!!
==========Arrive Goal!!==========
历时 44.884120 秒。
312
将航向得分比重改为 0.08，结果如下
>> dwa_V_1_0
Dynamic Window Approach sample program start!!
==========Arrive Goal!!==========
历时 52.595727 秒。
336
可以看到当航向得分比重较小时规划的路径更安全，但相对长度也更长；当航向得分比重较大时规划的路径更短，但靠近障碍物时会“犹豫”，有一定撞到障碍物的风险
参考
ROS 2D导航原理系列（六）|局部路径规划-DWA算法
DWA Planner | Husky Robot | Motion Planning for Robots
DWA算法总结
DWA动态窗口法的原理及应用:The Dynamic Window Approach to Collision Avoidance
The Dynamic Window Approach to Collision Avoidance
《The_dynamic_window_approach_to_collision_avoidance》
《基于动态窗口法和改进A*算法的多车路径规划》
《面向车场日巡检机器人的路径规划研究_李清天》
《移动机器人的SLAM与自主路径规划研究》