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

动态窗的口法（Dynamic Window Approach，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]]  
Kinem