二、DWA算法簡介
DWA算法全稱為dynamic window approach，其原理主要是在速度空間（v,w）中采樣多組速度，并模拟這些速度在一定時間内的運動軌迹，再通過一個評價函數對這些軌迹打分，最優的速度被選擇出來發送給下位機。
1 原理分析
2 速度采樣
機器人的軌迹運動模型有了，根據速度就可以推算出軌迹。
是以隻需采樣很多速度，推算軌迹，然後評價這些軌迹好不好就行了。
（一）移動機器人受自身最大速度最小速度的限制
（二） 移動機器人受電機性能的影響:由于電機力矩有限,存在最大的加減速限制,是以移動機器人軌迹前向模拟的周期sim_period内,存在一個動态視窗,在該視窗内的速度是機器人能夠實際達到的速度:
（三） 基于移動機器人安全的考慮:為了能夠在碰到障礙物前停下來, 是以在最大減速度條件下, 速度有一個範圍。
三、源代碼
% -------------------------------------------------------------------------
%
% File : DynamicWindowApproachSample.m
%
% Discription : Mobile Robot Motion Planning with Dynamic Window Approach
%
% Environment : Matlab
% -------------------------------------------------------------------------
function [] = DynamicWindowApproachSample()
close all;
clear all;
disp('Dynamic Window Approach sample program start!!')
%% 機器人的初期狀态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
% x=[0 0 pi/2 0 0]'; % 5x1矩陣 列矩陣 位置 0，0 航向 pi/2 ,速度、角速度均為0
x = [0 0 pi/10 0 0]';
% 下标宏定義 狀态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
POSE_X = 1; %坐标 X
POSE_Y = 2; %坐标 Y
YAW_ANGLE = 3; %機器人航向角
V_SPD = 4; %機器人速度
W_ANGLE_SPD = 5; %機器人角速度
goal = [10,10]; % 目标點位置 [x(m),y(m)]
% 障礙物位置清單 [x(m) y(m)]
obstacle=[0 2;
2 4;
2 5;
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]]
Kinematic = [1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];
%定義Kinematic的下标含義
MD_MAX_V = 1;% 最高速度m/s]
MD_MAX_W = 2;% 最高旋轉速度[rad/s]
MD_ACC = 3;% 加速度[m/ss]
MD_VW = 4;% 旋轉加速度[rad/ss]
MD_V_RESOLUTION = 5;% 速度分辨率[m/s]
MD_W_RESOLUTION = 6;% 轉速分辨率[rad/s]]
% 評價函數參數 [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 循環運作 5000次 指導達到目的地 或者 5000次運作結束
for i = 1:5000
% DWA參數輸入 傳回控制量 u = [v(m/s),w(rad/s)] 和 軌迹
[u,traj] = DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);
x = f(x,u);% 機器人移動到下一個時刻的狀态量 根據目前速度和角速度推導 下一刻的位置和角度
% 曆史軌迹的儲存
result.x = [result.x; x']; %最新結果 以列的形式 添加到result.x
% 是否到達目的地
if norm(x(POSE_X:POSE_Y)-goal')<0.5 % norm函數來求得坐标上的兩個點之間的距離
disp('Arrive Goal!!');break;
end
%====Animation====
hold off; % 關閉圖形保持功能。 新圖出現時，取消原圖的顯示。
ArrowLength = 0.5; % 箭頭長度
% 機器人
% quiver(x,y,u,v) 在 x 和 y 中每個對應元素對組所指定的坐标處将向量繪制為箭頭
quiver(x(POSE_X), x(POSE_Y), ArrowLength*cos(x(YAW_ANGLE)), ArrowLength*sin(x(YAW_ANGLE)), 'ok'); % 繪制機器人目前位置的航向箭頭
hold on; %啟動圖形保持功能，目前坐标軸和圖形都将保持，從此繪制的圖形都将添加在這個圖形的基礎上，并自動調整坐标軸的範圍
plot(result.x(:,POSE_X),result.x(:,POSE_Y),'-b');hold on; % 繪制走過的所有位置 所有曆史資料的 X、Y坐标
plot(goal(1),goal(2),'*r');hold on; % 繪制目标位置
%plot(obstacle(:,1),obstacle(:,2),'*k');hold on; % 繪制所有障礙物位置
DrawObstacle_plot(obstacle,obstacleR);
% 探索軌迹 畫出待評價的軌迹
if ~isempty(traj) %軌迹非空
for it=1:length(traj(:,1))/5 %計算所有軌迹數 traj 每5行資料 表示一條軌迹點
ind = 1+(it-1)*5; %第 it 條軌迹對應在traj中的下标
plot(traj(ind,:),traj(ind+1,:),'-g');hold on; %根據一條軌迹的點串畫出軌迹 traj(ind,:) 表示第ind條軌迹的所有x坐标值 traj(ind+1,:)表示第ind條軌迹的所有y坐标值
end
end
axis(area); %根據area設定目前圖形的坐标範圍，分别為x軸的最小、最大值，y軸的最小最大值
grid on;
drawnow; %重新整理螢幕. 當代碼執行時間長，需要反複執行plot時，Matlab程式不會馬上把圖像畫到figure上，這時，要想實時看到圖像的每一步變化情況，需要使用這個語句。
%movcount = movcount+1;
%mov(movcount) = getframe(gcf);% 記錄動畫幀
end
toc %輸出程式運作時間 形式：時間已過 ** 秒。
%movie2avi(mov,'movie.avi'); %錄制過程動畫 儲存為 movie.avi 檔案
%% 繪制所有障礙物位置
% 輸入參數：obstacle 所有障礙物的坐标 obstacleR 障礙物的半徑
function [] = DrawObstacle_plot(obstacle,obstacleR)
r = obstacleR;
theta = 0:pi/20:2*pi;
for id=1:length(obstacle(:,1))
x = r * cos(theta) + obstacle(id,1);
y = r *sin(theta) + obstacle(id,2);
plot(x,y,'-m');hold on;
end
% plot(obstacle(:,1),obstacle(:,2),'*m');hold on; % 繪制所有障礙物位置
%% DWA算法實作
% model 機器人運動學模型 最高速度m/s],最高旋轉速度[rad/s],加速度[m/ss],旋轉加速度[rad/ss], 速度分辨率[m/s],轉速分辨率[rad/s]]
% 輸入參數：目前狀态、模型參數、目标點、評價函數的參數、障礙物位置、障礙物半徑
% 傳回參數：控制量 u = [v(m/s),w(rad/s)] 和 軌迹集合 N * 31 （N：可用的軌迹數）
% 選取最優參數的實體意義：在局部導航過程中，使得機器人避開障礙物，朝着目标以較快的速度行駛。
function [u,trajDB] = DynamicWindowApproach(x,model,goal,evalParam,ob,R)
% Dynamic Window [vmin,vmax,wmin,wmax] 最小速度 最大速度 最小角速度 最大角速度速度
Vr = CalcDynamicWindow(x,model); % 根據目前狀态 和 運動模型 計算目前的參數允許範圍
% 評價函數的計算 evalDB N*5 每行一組可用參數 分别為 速度、角速度、航向得分、距離得分、速度得分
% trajDB 每5行一條軌迹 每條軌迹都有狀态x點串組成
[evalDB,trajDB]= Evaluation(x,Vr,goal,ob,R,model,evalParam); %evalParam 評價函數參數 [heading,dist,velocity,predictDT]
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);% 選取評分最高的參數 對應分數傳回給 maxv 對應下标傳回給 ind
u = evalDB(ind,1:2)';% 傳回最優參數的速度、角速度
%% 評價函數 内部負責産生可用軌迹
% 輸入參數 ：目前狀态、參數允許範圍（視窗）、目标點、障礙物位置、障礙物半徑、評價函數的參數
% 傳回參數：
% evalDB N*5 每行一組可用參數 分别為 速度、角速度、航向得分、距離得分、速度得分
% trajDB 每5行一條軌迹 每條軌迹包含 前向預測時間/dt + 1 = 31 個軌迹點（見生成軌迹函數）
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]; % 每5行 一條軌迹
end
end
end
四、運作結果
五、matlab版本及參考文獻
1 matlab版本
2014a
2 參考文獻