機器人局部路徑規劃方法有很多，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]]??