机器人局部避障的动态窗口法(dynamic window approach) （转）

源：机器人局部避障的动态窗口法(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.****.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):  Atsushi Sakai
%
% License : Modified BSD Software License Agreement
% -------------------------------------------------------------------------

function [] = DynamicWindowApproachSample()

close all;
clear all;

disp('Dynamic Window Approach sample program start!!')

x=[  pi/  ]';% 机器人的初期状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
goal=[,];% 目标点位置 [x(m),y(m)]
% 障碍物位置列表 [x(m) y(m)]
% obstacle=[ ;
%            ;
%            ;
%            ;
%            ;
%            ;
%
%
%
%            ];
obstacle=[ ;
           ;
           ;
           ;
           ;
           ;

           ];

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

% 评价函数参数 [heading,dist,velocity,predictDT]
evalParam=[0.05,0.2,0.1,3.0];
area=[-  - ];% 模拟区域范围 [xmin xmax ymin ymax]

% 模拟实验的结果
result.x=[];
tic;
% movcount=;
% Main loop
for i=:
    % DWA参数输入
    [u,traj]=DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);
    x=f(x,u);% 机器人移动到下一个时刻

    % 模拟结果的保存
    result.x=[result.x; x'];

    % 是否到达目的地
    if norm(x(:)-goal')<0.5
        disp('Arrive Goal!!');break;
    end

    %====Animation====
    hold off;
    ArrowLength=0.5;%
    % 机器人
    quiver(x(),x(),ArrowLength*cos(x()),ArrowLength*sin(x()),'ok');hold on;
    plot(result.x(:,),result.x(:,),'-b');hold on;
    plot(goal(),goal(),'*r');hold on;
    plot(obstacle(:,),obstacle(:,),'*k');hold on;
    % 探索轨迹
    if ~isempty(traj)
        for it=:length(traj(:,))/
            ind=+(it-)*;
            plot(traj(ind,:),traj(ind+,:),'-g');hold on;
        end
    end
    axis(area);
    grid on;
    drawnow;
    %movcount=movcount+;
    %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=[;];return;
end

% 各评价函数正则化
evalDB=NormalizeEval(evalDB);

% 最终评价函数的计算
feval=[];
for id=:length(evalDB(:,))
    feval=[feval;evalParam(:)*evalDB(id,:)'];
end
evalDB=[evalDB feval];

[maxv,ind]=max(feval);% 最优评价函数
u=evalDB(ind,:)';% 

function [evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam)
%
evalDB=[];
trajDB=[];
for vt=Vr():model():Vr()
    for ot=Vr():model():Vr()
        % 轨迹推测; 得到 xt: 机器人向前运动后的预测位姿; traj: 当前时刻 到 预测时刻之间的轨迹
        [xt,traj]=GenerateTrajectory(x,vt,ot,evalParam(),model);  %evalParam(),前向模拟时间;
        % 各评价函数的计算
        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(:,))~=
    EvalDB(:,)=EvalDB(:,)/sum(EvalDB(:,));
end
if sum(EvalDB(:,))~=
    EvalDB(:,)=EvalDB(:,)/sum(EvalDB(:,));
end
if sum(EvalDB(:,))~=
    EvalDB(:,)=EvalDB(:,)/sum(EvalDB(:,));
end

function [x,traj]=GenerateTrajectory(x,vt,ot,evaldt,model)
% 轨迹生成函数
% evaldt：前向模拟时间; vt、ot当前速度和角速度;
global dt;
time=;
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=;
while vel>
    stopDist=stopDist+vel*dt;% 制动距离的计算
    vel=vel-model()*dt;%
end

function dist=CalcDistEval(x,ob,R)
% 障碍物距离评价函数

dist=;
for io=:length(ob(:,))
    disttmp=norm(ob(io,:)-x(:)')-R;%僷僗偺埵抲偲忈奞暔偲偺僲儖儉岆嵎傪寁嶼
    if dist>disttmp% 离障碍物最小的距离
        dist=disttmp;
    end
end

% 障碍物距离评价限定一个最大值，如果不设定，一旦一条轨迹没有障碍物，将太占比重
if dist>=*R
    dist=*R;
end

function heading=CalcHeadingEval(x,goal)
% heading的评价函数计算

theta=toDegree(x());% 机器人朝向
goalTheta=toDegree(atan2(goal()-x(),goal()-x()));% 目标点的方位

if goalTheta>theta
    targetTheta=goalTheta-theta;% [deg]
else
    targetTheta=theta-goalTheta;% [deg]
end

heading=-targetTheta;

function Vr=CalcDynamicWindow(x,model)
%
global dt;
% 车子速度的最大最小范围
Vs=[ model() -model() model()];

% 根据当前速度以及加速度限制计算的动态窗口
Vd=[x()-model()*dt x()+model()*dt x()-model()*dt x()+model()*dt];

% 最终的Dynamic Window
Vtmp=[Vs;Vd];
Vr=[max(Vtmp(:,)) min(Vtmp(:,)) max(Vtmp(:,)) min(Vtmp(:,))];

function x = f(x, u)
% Motion Model
% u = [vt; wt];当前时刻的速度、角速度
global dt;

F = [    

         ];

B = [dt*cos(x())
    dt*sin(x())
     dt

     ];

x= F*x+B*u;

function radian = toRadian(degree)
% degree to radian
radian = degree/*pi;

function degree = toDegree(radian)
% radian to degree
degree = radian/pi*;