• 最近正在學習DWA，按照DynamicWindowApproachSample.m 提供的matlab模擬程式碼簡單梳理下，但是這並不是ros裡用的，只是一個簡單的模擬。
• DWA的原理：假設一個機器人要從當前點通過一定的速度和方向到達目標點，在速度(v, w)空間中取樣多組軌跡，為這些軌跡使用評價函式進行評價，選取最優的軌跡對應的速度(v_best, w_best)來驅動機器人,最終到達目標點。

程式碼分析：

1. 首先定義一些初始狀態

   % 機器人的初始狀態，包括初始位置，朝向（這裡可以理解為yaw角）速度，角速度
   % [x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
    
   
   x=[0 0 pi/2 0 0]';
   
   % 目標點位置 [x(m),y(m)]
   goal=[10,10];
   
   % 障礙物位置列表 [x(m) y(m)]
   obstacle=[0 2;4 2;4 4;5 4;5 5;5 6; 6 7;7 4;9 8;9 11;9 6];
   
   % 衝突判定用的障礙物半徑
   obstacleR=0.5;
   
   % 時間間隔[s]，主要用於運動模型
   global dt; dt=0.1;
   
   %% 機器人運動學模型引數
   % 最高速度[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];
   
   % 模擬區域範圍 [xmin xmax ymin ymax]
   area=[-1 11 -1 11];
   
   % 模擬實驗的結果
   result.x=[];

2. 進入模擬主迴圈
   迴圈主要函式為DynamicWindowApproach，輸入機器人當前的狀態x，運動模型引數，評價函式引數，障礙物，障礙-物半徑，輸出最優的控制量u(v, w)以及模擬軌跡，控制量傳遞給運動模型後機器人開始運動，最終到達目標點，後面繪圖部分不重要，不予解釋了。

   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
   
       %% ====繪圖部分===
       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),'*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; 
   end

   運動模型 x=f(x,u)指的是在世界座標系下，機器人從a點運動到b點遵循的運動規則，w指的是世界座標系
   
   
   用矩陣的形式表示這個模型如下：

function x = f(x, u)
% 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;

1. DynamicWindowApproach函式，這個是DWA函式的模型，輸出最優的線速度和角速度
   首先會根據機器人模型引數中的最高速度，最高旋轉速度，加速度，旋轉加速度等引數計算出機器人在dt時間內速度和角速度可以到達的最大最小範圍，這個範圍可以根據情況調整；
   之後將這個視窗範圍內的速度（包含角速度）進行取樣，輸入到評價函式Evaluation中，計算出每個取樣速度對應的評價值evalDB，在對這些評價值進行歸一化操作，分別乘以評價值對應的評價引數（cost = a*heading+b*dist+c*velocity），得到最優的評價值後找到其對應的u(v, w)，輸出這個最佳的速度和角速度

   function [u,trajDB]=DynamicWindowApproach(x,model,goal,evalParam,ob,R)
   
   % 動態視窗 [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)';

2. CalcDynamicWindow

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

3. Evaluation函式，評價函式的計算，輸入初始x的狀態，動態視窗，障礙物，衝突判定用的障礙物半徑，機器人運動模型引數以及評價函式引數，通過兩個for迴圈計算出需要評價的引數heading dist vel

   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)
           % GenerateTrajectory軌跡推測
           % 得到 xt: 機器人向前運動後的預測位姿; traj: 當前時刻到預測時刻之間的軌跡
           % evalParam(4),前向模擬時間;
           [xt,traj]=GenerateTrajectory(x,vt,ot,evalParam(4),model);
           %% 各評價函式的計算
           % 當前機器人和目標點的朝向
           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

4. GenerateTrajectory函式

   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

5. CalcHeadingEval函式，heading的評價函式計算

   function heading=CalcHeadingEval(x,goal)
   
   % 機器人朝向
   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
   
   % theta越小，評價越高
   heading = 180 - targetTheta;

6. CalcDistEval函式，障礙物距離評價函式

   function dist=CalcDistEval(x,ob,R)
       dist=100;
       for io=1:length(ob(:,1))
           disttmp=norm(ob(io,:)-x(1:2)')-R;
           % 離障礙物最小的距離
           if dist>disttmp
               dist=disttmp;
           end
       end
   
       % 障礙物距離評價限定一個最大值，如果不設定，一旦一條軌跡沒有障礙物，將太佔比重
       if dist>=2*R
           dist=2*R;
       end