單路徑： function [] = DynamicWindowApproachSample()   close all; clear all;   disp('Dynamic Window Approach sample program start!!')

x=[0 0 pi/2 0 0]';   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.3;      % 衝突判定用的障礙物半徑 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),'hb','linewidth',4,'markersize',0.5);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)% DWA引數輸入

% 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;%僷僗偺埵抲偲?釆Q暔偲偺僲儖儉岆?穫饘v嶼     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;

多路徑：