一、动态窗口法基本概念

 1.1  速度采样空间

1.2  评价函数

二、基于Matlab的机器人局部避障仿真

---

一、动态窗口法基本概念

        动态窗口方法(DynamicWindowApproach) 是一种可以实现实时避障的局部规划算法，通过将轮式机器人的位置约束转化为速度约束，根据约束进行速度采样，并由一系列的选定速度的动作生成轨迹，结合评价函数选择评分最高的轨迹，实现执行最优速度的问题。

        DWA算法的流程图可表述为：

 1.1  速度采样空间

        初始速度搜索空间：在任意t时刻，机器人速度(v，ω) 的二维空间中可以形成无穷多组运动轨迹，即构成一个初始的速度搜索空间。但是受以下机器人的运动学约束和环境约束等可以使采样速度缩小至更加精确的区域：

        ①机器人本身的速度极限：

        ②机器人驱动的电机性能及摩擦等影响，即匀加（减）速运动的启停动作：

         ③避障，需要保证以最大加速度能在最近的障碍物前停下来（速度从v，ω 减为0）：

         式中，dist（v，ω ）为当前采样速度（v，ω ） 下距离障碍物最小的间距值。

         综上，速度搜索空间为以上三个速度集合的交集，也称为该机器人的动态窗口，即Vr=Vs∩Vd∩Va 。

1.2  评价函数

        设置为如下评价函数，用以确定当前速度状态下的最佳路径：

（1）heading（v，ω） 为机器人方位角评价函数，使机器人能够不断地对准下一个参考点。

        设定heading（v，ω）=180°-θ ，θ 为机器人当前位置与目标点连线在全局坐标系下的位姿角。

（2）distance（v，ω）为机器人距离障碍物的评价函数，用以提高机器人与避障能力。

        设定   ，其中d为机器人与障碍物的最近距离；L为预先设定的避障阈值，当轨迹上没有障碍物时，为防止避障评价指标对函数G（v，ω） 的影响过大，distance（v，ω） 将输出一个数值较大的定值，提高寻找最短路径的执行效率。

（3）velocity（v，ω） 为速度评价函数。

        设定velocity（v，ω）=|vg| ，vg 为当前待评价的轨迹线速度，速度值越大，得分越高。

（4）σ，α，β，γ 均为评价子函数的系数。

        综上，引入评价函数使机器人能够完成避障的局部规划，并以较快的速度朝着目标点位置移动。

二、基于Matlab的机器人局部避障仿真

        Matlab代码如下：

 
close all;  
clear all;  
  
  
x=[0 0 0 0 0]';% 机器人的初期状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]  
goal=[10,10];% 目标点位置 [x(m),y(m)]  
 
obstacle=[2 2;4 5; 5 9;8 6;7 9];%设置五个障碍物
obstacleR=0.5;% 冲突判定用的障碍物排斥半径  
global dt; dt=0.1;% 时间[s]  
T = 1000;
traj_X = zeros(1,T);%记录所走轨迹的x坐标
traj_Y = zeros(1,T);%记录所走轨迹的y坐标
  
% 机器人运动学模型  
% 最高速度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;  

% Main loop  
 Fig=figure;
 filename = 'test.gif';      % 输出路径+保存的文件名.gif
for i=1:T
    % 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),'or');hold on;  

    r = obstacleR;
    for id=1:length(obstacle(:,1))         %绘制矩形障碍物（黄色表示障碍物）
        rectangle('Position',[obstacle(id,1)-r/2,obstacle(id,2)-r/2,r,r],'Linewidth',2,'LineStyle','-','EdgeColor','y');
    end

    % 探索轨迹  
    if ~isempty(traj)  
        for it=1:length(traj(:,1))/5  
            ind=1+(it-1)*5;  
            traj_X(i) = traj(1,31);
            traj_Y(i) = traj(2,31);
            plot(traj(ind,:),traj(ind+1,:),'-g');hold on;  
        end  
    end  
    axis(area);  
    grid on;  
    xlabel('x / m')
    ylabel('y / m')
    title('动态窗口法生成避障规划轨迹')
    drawnow;  
 
    
    frame = getframe(Fig); 
    im = frame2im(frame); 
    [imind,cm] = rgb2ind(im,256);
    if i == 1
        imwrite(imind,cm,filename,'gif','WriteMode','overwrite', 'Loopcount',inf);
        %Loopcount只是在i==1的时候才有用
   else
        imwrite(imind,cm,filename,'gif','WriteMode','append','DelayTime',0.05);
        %DelayTime:帧与帧之间的时间间隔
    end
   save Trajectory.mat traj_X traj_Y
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)';%   
end
  
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  
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  
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  
end
  
function stopDist=CalcBreakingDist(vel,model)  
% 根据运动学模型计算制动距离  
global dt;  
stopDist=0;  
while vel>0  
    stopDist=stopDist+vel*dt;% 制动距离的计算  
    vel=vel-model(3)*dt;%   model(3)为加速度
end  
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  
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;  
end
  
function Vr=CalcDynamicWindow(x,model)  
%  计算动态窗口，即速度交集Vr
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))];  
end
  
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;  
end
  
function radian = toRadian(degree)  
% 角度转为弧度制 
radian = degree/180*pi;  
end
  
function degree = toDegree(radian)  
% 弧度转为角度
degree = radian/pi*180; 
end

        仿真结果如下图：