机器人局部避障的动态窗口法的运动轨迹分析和对比

机器人局部路径规划方法有很多，ROS中主要采用的是动态窗口法（但是和原论文中的dwa方法不一样，具体见最后）。动态窗口法主要是在速度（v,w）空间中采样多组速度，并模拟机器人在这些速度下一定时间（sim_period）内的轨迹。在得到多组轨迹以后，对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人运动。该算法突出点在于动态窗口这个名词，它的含义是依据移动机器人的加速性能限定速度采样空间在一个可行的动态范围内。

 

 

 

 

 

为了更加直观的感受速度如何采样以及如何排除会碰到障碍的速度，将速度采样的伪代码列车如下：
Sample velocities code demo:

首先在V_m∩V_d的范围内采样速度：  

allowable_v = generateWindow(robotV, robotModel)  

allowable_w  = generateWindow(robotW, robotModel)  

然后根据能否及时刹车剔除不安全的速度：  

for each v in allowable_v  

for each w in allowable_w  

dist = find_dist(v,w,laserscan,robotModel)  

breakDist = calculateBreakingDistance(v)//刹车距离  

if (dist > breakDist)  //如果能够及时刹车，该对速度可接收  

如果这组速度可接受，接下来利用评价函数对其评价，找到最优的速度组  

同时注意：为了简化每组速度对应轨迹的计算，改算法假设机器人在往前模拟轨迹的这段时间（sim_period)内速度不变，直到下一时刻采样给定新的速度命令。

动态窗口采样的轨迹如下图所示：

 

 

 

总结起来三者构成的评价函数的物理意义是：在局部导航过程中，使得机器人避开障碍，朝着目标以较快速度行驶，缺一不可。

流程清楚以后，dwa算法的demo如下：

BEGIN DWA(robotPose,robotGoal,robotModel)  

laserscan = readScanner()  

allowable_v = generateWindow(robotV, robotModel)  

allowable_w  = generateWindow(robotW, robotModel)  

for each v in allowable_v  

for each w in allowable_w  

dist = find_dist(v,w,laserscan,robotModel)  

breakDist = calculateBreakingDistance(v)  

if (dist > breakDist)  //can stop in time  

heading = hDiff(robotPose,goalPose, v,w)   

//clearance与原论文稍不一样  

clearance = (dist-breakDist)/(dmax - breakDist)   

cost = costFunction(heading,clearance, abs(desired_v - v))  

if (cost > optimal)  

best_v = v  

best_w = w  

optimal = cost  

set robot trajectory to best_v, best_w  

END  

引申到ROS:

在ROS的dwa应用中，好像只用了窗口采样速度，到故障物的最小距离以及刹车距离都没有计算，如果某条轨迹上有障碍，那直接丢弃这条轨迹。并且ROS中的评价函数也不是用的这个，采用的是Gerkey的论文《planning and control in unstructured Terrain》中的评价函数。

参考：
dwa：
1.Fox.《The Dynamic Window Approach To CollisionAvoidance》
2.MarijaSeder. 《dynamic window based approach tomobile robot motion control in the presence of moving obstacles》
3.

运动模型：
4.
5. https://www.cs.princeton.edu/courses/archive/fall11/cos495/COS495-Lectur...
6.

最后贴出matlab仿真代码：

% -------------------------------------------------------------------------  

%  

% File : DynamicWindowApproachSample.m  

%  

% Discription : Mobile Robot Motion Planning with Dynamic Window Approach  

%  

% Environment : Matlab  

%  

% Author : Atsushi Sakai  

%  

% Copyright (c): 2014 Atsushi Sakai  

%  

% License : Modified BSD Software License Agreement  

% -------------------------------------------------------------------------  

 

function [] = DynamicWindowApproachSample()  

 

close all;  

clear all;  

 

disp('Dynamic Window Approach sample program start!!')  

 

x=[0 0 pi/2 0 0]';% 机器人的初期状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]  

goal=[10,10];% 目标点位置 [x(m),y(m)]  

% 障碍物位置列表 [x(m) y(m)]  

% obstacle=[0 2;  

%           4 2;  

%           4 4;  

%           5 4;  

%           5 5;  

%           5 6;  

%           5 9  

%           8 8  

%           8 9  

%           7 9];  

obstacle=[0 2;  

4 2;  

4 4;  

5 4;  

5 5;  

5 6;  

5 9  

8 8  

8 9  

7 9  

6 5  

6 3  

6 8  

6 7  

7 4  

9 8  

9 11  

9 6];  

 

obstacleR=0.5;% 冲突判定用的障碍物半径  

global dt; dt=0.1;% 时间[s]  

 

% 机器人运动学模型  

% 最高速度m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss],  

% 速度分辨率[m/s],转速分辨率[rad/s]]  

Kinematic=[1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];  

 

% 评价函数参数 [heading,dist,velocity,predictDT]  

evalParam=[0.05,0.2,0.1,3.0];  

area=[-1 11 -1 11];% 模拟区域范围 [xmin xmax ymin ymax]  

 

% 模拟实验的结果  

result.x=[];  

tic;  

% movcount=0;  

% Main loop  

for i=1:5000  

% DWA参数输入  

[u,traj]=DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);  

x=f(x,u);% 机器人移动到下一个时刻  

 

% 模拟结果的保存  

result.x=[result.x; x'];  

 

% 是否到达目的地  

if norm(x(1:2)-goal')<0.5  

disp('Arrive Goal!!');break;  

end  

 

%====Animation====  

hold off;  

ArrowLength=0.5;%   

% 机器人  

quiver(x(1),x(2),ArrowLength*cos(x(3)),ArrowLength*sin(x(3)),'ok');hold on;  

plot(result.x(:,1),result.x(:,2),'-b');hold on;  

plot(goal(1),goal(2),'*r');hold on;  

plot(obstacle(:,1),obstacle(:,2),'*k');hold on;  

% 探索轨迹  

if ~isempty(traj)  

for it=1:length(traj(:,1))/5  

ind=1+(it-1)*5;  

plot(traj(ind,:),traj(ind+1,:),'-g');hold on;  

end  

end  

axis(area);  

grid on;  

drawnow;  

%movcount=movcount+1;  

%mov(movcount) = getframe(gcf);%   

end  

toc  

%movie2avi(mov,'movie.avi');  

 

 

function [u,trajDB]=DynamicWindowApproach(x,model,goal,evalParam,ob,R)  

 

% Dynamic Window [vmin,vmax,wmin,wmax]  

Vr=CalcDynamicWindow(x,model);  

 

% 评价函数的计算  

[evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam);  

 

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);% 最优评价函数  

u=evalDB(ind,1:2)';%   

 

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),model);  %evalParam(4),前向模拟时间;  

% 各评价函数的计算  

heading=CalcHeadingEval(xt,goal);  

dist=CalcDistEval(xt,ob,R);  

vel=abs(vt);  

% 制动距离的计算  

stopDist=CalcBreakingDist(vel,model);  

if dist>stopDist %   

evalDB=[evalDB;[vt ot heading dist vel]];  

trajDB=[trajDB;traj];  

end  

end  

end  

 

function EvalDB=NormalizeEval(EvalDB)  

% 评价函数正则化  

if sum(EvalDB(:,3))~=0  

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  

 

function [x,traj]=GenerateTrajectory(x,vt,ot,evaldt,model)  

% 轨迹生成函数  

% evaldt：前向模拟时间; vt、ot当前速度和角速度;   

global dt;  

time=0;  

u=[vt;ot];% 输入值  

traj=x;% 机器人轨迹  

while time<=evaldt  

time=time+dt;% 时间更新  

x=f(x,u);% 运动更新  

traj=[traj x];  

end  

 

function stopDist=CalcBreakingDist(vel,model)  

% 根据运动学模型计算制动距离,这个制动距离并没有考虑旋转速度，不精确吧！！！  

global dt;  

stopDist=0;  

while vel>0  

stopDist=stopDist+vel*dt;% 制动距离的计算  

vel=vel-model(3)*dt;%   

end  

 

function dist=CalcDistEval(x,ob,R)  

% 障碍物距离评价函数  

 

dist=100;  

for io=1:length(ob(:,1))  

disttmp=norm(ob(io,:)-x(1:2)')-R;%僷僗偺埵抲偲忈奞暔偲偺僲儖儉岆嵎傪寁嶼  

if dist>disttmp% 离障碍物最小的距离  

dist=disttmp;  

end  

end  

 

% 障碍物距离评价限定一个最大值，如果不设定，一旦一条轨迹没有障碍物，将太占比重  

if dist>=2*R  

dist=2*R;  

end  

 

function heading=CalcHeadingEval(x,goal)  

% heading的评价函数计算  

 

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;  

 

function Vr=CalcDynamicWindow(x,model)  

%  

global dt;  

% 车子速度的最大最小范围  

Vs=[0 model(1) -model(2) model(2)];  

 

% 根据当前速度以及加速度限制计算的动态窗口  

Vd=[x(4)-model(3)*dt x(4)+model(3)*dt x(5)-model(4)*dt x(5)+model(4)*dt];  

 

% 最终的Dynamic Window  

Vtmp=[Vs;Vd];  

Vr=[max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))];  

 

function x = f(x, u)  

% Motion Model  

% u = [vt; wt];当前时刻的速度、角速度  

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;  

 

function radian = toRadian(degree)  

% degree to radian  

radian = degree/180*pi;  

 

function degree = toDegree(radian)  

% radian to degree  

degree = radian/pi*180;