機器人局部避障的動态視窗法(dynamic window approach)

首先在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)  //如果能夠及時刹車，該對速度可接收
	如果這組速度可接受，接下來利用評價函數對其評價，找到最優的速度組
           

來源:http://adrianboeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html
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
           

（轉載請注明作者和出處：http://blog.csdn.net/heyijia0327 未經允許請勿用于商業用途）

參考：

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.http://adrianboeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html

運動模型：

4. http://adrianboeing.blogspot.com.au/2010/09/circular-motion-in-2d-for-graphics-and.html

5.https://www.cs.princeton.edu/courses/archive/fall11/cos495/COS495-Lecture5-Odometry.pdf

6.http://rossum.sourceforge.net/papers/DiffSteer/

最後貼出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]]
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),'*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;
    %movcount=movcount+1;
    %mov(movcount) = getframe(gcf);% 
end
toc
%movie2avi(mov,'movie.avi');
 

function [u,trajDB]=DynamicWindowApproach(x,model,goal,evalParam,ob,R)

% 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;%僷僗偺埵抲偲忈奞暔偲偺僲儖儉岆嵎傪寁嶼
    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;