機(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í)注意：為了簡化每組速度對(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]]??