%?-------------------------------------------------------------------------??
%??
%?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?10?2]‘;%?機(jī)器人的初期狀態(tài)[x（m）y（m）yaw（Rad）v（m/s）w（rad/s）]??
goal=[1010];%?目標(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;??
??????????2?4;??%54
??????????3?2;??%5?5?
??????????5?6;??
??????????5?9??
??????????7?8??%8?8
??????????9?7??
??????????7?10?
??????????6?5.5??
??????????6?3??
??????????4?8??%6?8?
??????????6?7??
??????????9?4??%7?4
??????????9?8??
?????%????9?11??
??????????9?6
??????????0?8
??????????3?4
??????????8.5?10
??????????];???
????????
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.0toRadian（20.0）0.2toRadian（50.0）0.01toRadian（1）];??
??
%?評(píng)價(jià)函數(shù)參數(shù)?[headingdistvelocitypredictDT]??
evalParam=[0.10.20.23.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ù)輸入??
????[utraj]=DynamicWindowApproach（xKinematicgoalevalParamobstacleobstacleR）;??
????x=f（xu）;%?機(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）;
????%******β
????xlabel（‘x（m）‘）;
????ylabel（‘y（m）‘）;
????title（‘DWA算法（α=0.1，β=0.2，r=0.2）‘）
????set（gca‘xtick‘[-1:1:11]）;
????set（gca‘ytick‘[-1:1:11]）;
????%*******
????grid?on;??
????drawnow;??
????%movcount=movcount+1;??
????%mov（movcount）?=?getframe（gcf）;%???
end??
toc??
%movie2avi（mov‘movie.avi‘）;??
???
??
function?[utrajDB]=Dynami