Dynamic Window Approach_机器人局部避障的动态窗口法

• 最近正在学习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