动态窗的口法（DWA）与Matlab中gif动态图的制作

动态窗口方法是由Dieter Fox，Wolfram Burgard和Sebastian Thrun在1997年开发的用于移动机器人在线避障的一种策略。动态窗口法根据机器人模型自身的有限速度和加速度约束，将笛卡尔坐标（x,y）转换成一组机器人速度集合（v是机器人的直线速度，w为机器人的角速度）构成的速度矢量空间。在这个速度矢量空间中对移动机器人的运动速度进行控制搜索，就可以将移动机器人局部路径规划问题转换成约束条件下的最优解选择问题。
DWA由两个主要部分组成，一是利用速度矢量空间为移动机器人生成有效的搜索空间，Fox等人将搜索空间仅限于安全的圆形区域或扇形区域，然后在短时间内达到并且没有碰撞。二是在搜索空间中选择最佳解决方案。设计评价函数对速度矢量空间中轨迹进行评价，选择出一条当前最优轨迹，使机器人与任何障碍物保持最大间隙。具体的算法步骤如下：
1）离散采样移动机器人的运行状态（dx,dy,θ），即多组速度对（v,w）；
2）对获得的每一组采样速度进行模拟，以预测这些速度集在短时间内的运动轨迹；
3）使用到障碍物的距离，到目标的距离，速度等标准来评估每个获得的轨迹，并从集合中删除与障碍物碰撞的轨迹；
4）选择最佳轨迹并在机器人模型中配置合适的设定速度；
5）执行选定的轨迹并重复，直到机器人到达目标点。

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,8];% 目标点位置 [x(m),y(m)]  
% 障碍物位置列表 [x(m) y(m)]  
obstacle=[0 2;1 5; 1 8;2 3; 2 6; 3 8; 4 7;4 2; 4 4; 5 4; 5 6; 5 9 ; 8 8 ;8 9 ;7 9 ;6 5 ;6 3; 6 8;7 4 ;9 8 ;9 10  ;9 6; 2 2;6 4;10 7];                
obstacleR=0.2;% 冲突判定用的障碍物半径  
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  
 Fig=figure;
 filename = 'test.gif';      % 输出路径+保存的文件名.gif
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),'or');hold on;  
    for j=1:22
    plot(obstacle(j,1),obstacle(j,2),'*k');hold on;  
    end
    if i>20
     plot(obstacle(23,1),obstacle(23,2),'sk');hold on; 
    end
    if i > 200
       plot(obstacle(24,1),obstacle(24,2),'sk');hold on; 
    end
    if i>350
         plot(obstacle(25,1),obstacle(25,2),'sk');hold on; 
    end
    % 探索轨迹  
    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);%   
    
    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
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; 

MATLAB getframe用法

F = getframe(H,Rect)

H：图像句柄
Rect：一个四元素向量，Rect=[a b c d]，其中a表示截取区域到图像左端的像素数，b表示截取区域到图像底端的像素数，c表示截取区域水平像素数，d表示截取区域垂直向像素数。
F：是一个结构体,包含两个元素：cdata和colormap
cdata：Rect中参数选择的区域的数据，一个三维数组，其中第三维的长度是3，即截取到的是区域中RGB的值。