機(jī)器人局部路徑規(guī)劃方法有很多，ROS中主要采用的是動(dòng)態(tài)窗口法（但是和原論文中的dwa方法不一樣，具體見最后）。動(dòng)態(tài)窗口法主要是在速度（v,w）空間中采樣多組速度，并模擬機(jī)器人在這些速度下一定時(shí)間（sim_period）內(nèi)的軌跡。在得到多組軌跡以后，對(duì)這些軌跡進(jìn)行評(píng)價(jià)，選取最優(yōu)軌跡所對(duì)應(yīng)的速度來驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)。該算法突出點(diǎn)在于動(dòng)態(tài)窗口這個(gè)名詞，它的含義是依據(jù)移動(dòng)機(jī)器人的加速性能限定速度采樣空間在一個(gè)可行的動(dòng)態(tài)范圍內(nèi)。

?

?

?

?

?

為了更加直觀的感受速度如何采樣以及如何排除會(huì)碰到障礙的速度，將速度采樣的偽代碼列車如下：
Sample velocities code demo:

首先在V_m∩V_d的范圍內(nèi)采樣速度：??

allowable_v?=?generateWindow(robotV,?robotModel)??

allowable_w??=?generateWindow(robotW,?robotModel)??

然后根據(jù)能否及時(shí)剎車剔除不安全的速度：??

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)??//如果能夠及時(shí)剎車，該對(duì)速度可接收??

如果這組速度可接受，接下來利用評(píng)價(jià)函數(shù)對(duì)其評(píng)價(jià)，找到最優(yōu)的速度組??

同時(shí)注意：為了簡(jiǎn)化每組速度對(duì)應(yīng)軌跡的計(jì)算，改算法假設(shè)機(jī)器人在往前模擬軌跡的這段時(shí)間（sim_period)內(nèi)速度不變，直到下一時(shí)刻采樣給定新的速度命令。

動(dòng)態(tài)窗口采樣的軌跡如下圖所示：

?

?

?

總結(jié)起來三者構(gòu)成的評(píng)價(jià)函數(shù)的物理意義是：在局部導(dǎo)航過程中，使得機(jī)器人避開障礙，朝著目標(biāo)以較快速度行駛，缺一不可。

流程清楚以后，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應(yīng)用中，好像只用了窗口采樣速度，到故障物的最小距離以及剎車距離都沒有計(jì)算，如果某條軌跡上有障礙，那直接丟棄這條軌跡。并且ROS中的評(píng)價(jià)函數(shù)也不是用的這個(gè)，采用的是Gerkey的論文《planning and control in unstructured Terrain》中的評(píng)價(jià)函數(shù)。

參考：
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.

運(yùn)動(dòng)模型：
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]';%?機(jī)器人的初期狀態(tài)[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]??

goal=[10,10];%?目標(biāo)點(diǎn)位置?[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;%?時(shí)間[s]??

?

%?機(jī)器人運(yùn)動(dòng)學(xué)模型??

%?最高速度m/s],最高旋轉(zhuǎn)速度[rad/s],加速度[m/ss],旋轉(zhuǎn)加速度[rad/ss],??

%?速度分辨率[m/s],轉(zhuǎn)速分辨率[rad/s]]??

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

?

%?評(píng)價(jià)函數(shù)參數(shù)?[heading,dist,velocity,predictDT]??

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

area=[-1?11?-1?11];%?模擬區(qū)域范圍?[xmin?xmax?ymin?ymax]??

?

%?模擬實(shí)驗(yàn)的結(jié)果??

result.x=[];??

tic;??

%?movcount=0;??

%?Main?loop??

for?i=1:5000??

%?DWA參數(shù)輸入??

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

x=f(x,u);%?機(jī)器人移動(dòng)到下一個(gè)時(shí)刻??

?

%?模擬結(jié)果的保存??

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

?

%?是否到達(dá)目的地??

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

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

end??

?

%====Animation====??

hold?off;??

ArrowLength=0.5;%???

%?機(jī)器人??

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);??

?

%?評(píng)價(jià)函數(shù)的計(jì)算??

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

?

if?isempty(evalDB)??

disp('no?path?to?goal!!');??

u=[0;0];return;??

end??

?

%?各評(píng)價(jià)函數(shù)正則化??

evalDB=NormalizeEval(evalDB);??

?

%?最終評(píng)價(jià)函數(shù)的計(jì)算??

feval=[];??

for?id=1:length(evalDB(:,1))??

feval=[feval;evalParam(1:3)*evalDB(id,3:5)'];??

end??

evalDB=[evalDB?feval];??

?

[maxv,ind]=max(feval);%?最優(yōu)評(píng)價(jià)函數(shù)??

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)??

% 軌跡推測(cè);?得到?xt:?機(jī)器人向前運(yùn)動(dòng)后的預(yù)測(cè)位姿;?traj:?當(dāng)前時(shí)刻?到?預(yù)測(cè)時(shí)刻之間的軌跡 ?

[xt,traj]=GenerateTrajectory(x,vt,ot,evalParam(4),model);??%evalParam(4),前向模擬時(shí)間;??

%?各評(píng)價(jià)函數(shù)的計(jì)算??

heading=CalcHeadingEval(xt,goal);??

dist=CalcDistEval(xt,ob,R);??

vel=abs(vt);??

%?制動(dòng)距離的計(jì)算??

stopDist=CalcBreakingDist(vel,model);??

if?dist>stopDist?%???

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

trajDB=[trajDB;traj];??

end??

end??

end??

?

function?EvalDB=NormalizeEval(EvalDB)??

%?評(píng)價(jià)函數(shù)正則化??

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)??

%?軌跡生成函數(shù)??

%?evaldt：前向模擬時(shí)間;?vt、ot當(dāng)前速度和角速度;???

global?dt;??

time=0;??

u=[vt;ot];%?輸入值??

traj=x;%?機(jī)器人軌跡??

while?time<=evaldt??

time=time+dt;%?時(shí)間更新??

x=f(x,u);%?運(yùn)動(dòng)更新??

traj=[traj?x];??

end??

?

function?stopDist=CalcBreakingDist(vel,model)??

%?根據(jù)運(yùn)動(dòng)學(xué)模型計(jì)算制動(dòng)距離,這個(gè)制動(dòng)距離并沒有考慮旋轉(zhuǎn)速度，不精確吧?。。??

global?dt;??

stopDist=0;??

while?vel>0??

stopDist=stopDist+vel*dt;%?制動(dòng)距離的計(jì)算??

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

end??

?

function?dist=CalcDistEval(x,ob,R)??

%?障礙物距離評(píng)價(jià)函數(shù)??

?

dist=100;??

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

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

if?dist>disttmp%?離障礙物最小的距離??

dist=disttmp;??

end??

end??

?

%?障礙物距離評(píng)價(jià)限定一個(gè)最大值，如果不設(shè)定，一旦一條軌跡沒有障礙物，將太占比重??

if?dist>=2*R??

dist=2*R;??

end??

?

function?heading=CalcHeadingEval(x,goal)??

%?heading的評(píng)價(jià)函數(shù)計(jì)算??

?

theta=toDegree(x(3));%?機(jī)器人朝向??

goalTheta=toDegree(atan2(goal(2)-x(2),goal(1)-x(1)));%?目標(biāo)點(diǎn)的方位??

?

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)];??

?

%?根據(jù)當(dāng)前速度以及加速度限制計(jì)算的動(dòng)態(tài)窗口??

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];當(dāng)前時(shí)刻的速度、角速度??

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;??