dwa动态窗口算法代码

% -------------------------------------------------------------------------  
%  
% File : DynamicWindowApproachSample.m  
%  
% Discription : Mobile Robot Motion Planning with Dynamic Window Approach  
%  
% Environment : Matlab  
%  
% Author : Atsushi Sakai  
%  
% Copyright （c）: 2014 Atsushi Sakai  
%  
% License : Modified BSD Software License Agreement  
% -------------------------------------------------------------------------  
  
function [] = DynamicWindowApproachSample（）  
   
close all;  
clear all;  
   
disp（‘Dynamic Window Approach sample program start!!‘）  
  
x=[0 0 pi/2 10 2]‘;% 机器人的初期状态[x（m）y（m）yaw（Rad）v（m/s）w（rad/s）]  
goal=[1010];% 目标点位置 [x（m）y（m）]  
% 障碍物位置列表 [x（m） y（m）]  
% obstacle=[0 2;  
%           4 2;  
%           4 4;  
%           5 4;  
%           5 5;  
%           5 6;  
%           5 9  
%           8 8  
%           8 9  
%           7 9];  
obstacle=[0 2;  %整体
          4 2;  
          4 4;  
          2 4;  %54
          3 2;  %5 5 
          5 6;  
          5 9  
          7 8  %8 8
          9 7  
          7 10 
          6 5.5  
          6 3  
          4 8  %6 8 
          6 7  
          9 4  %7 4
          9 8  
     %    9 11  
          9 6
          0 8
          3 4
          8.5 10
          ];   
        
obstacleR=0.5;% 冲突判定用的障碍物半径  
global dt; dt=0.1;% 时间[s]  
  
% 机器人运动学模型  
% 最高速度m/s]最高旋转速度[rad/s]加速度[m/ss]旋转加速度[rad/ss]  
% 速度分辨率[m/s]转速分辨率[rad/s]]  
Kinematic=[1.0toRadian（20.0）0.2toRadian（50.0）0.01toRadian（1）];  
  
% 评价函数参数 [headingdistvelocitypredictDT]  
evalParam=[0.10.20.23.0];  
area=[-1 11 -1 11];% 模拟区域范围 [xmin xmax ymin ymax]  
  
% 模拟实验的结果  
result.x=[];  
tic;  
% movcount=0;  
% Main loop  
for i=1:5000  
    % DWA参数输入  
    [utraj]=DynamicWindowApproach（xKinematicgoalevalParamobstacleobstacleR）;  
    x=f（xu）;% 机器人移动到下一个时刻  
      
    % 模拟结果的保存  
    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）‘*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）;
    %******β
    xlabel（‘x（m）‘）;
    ylabel（‘y（m）‘）;
    title（‘DWA算法（α=0.1，β=0.2，r=0.2）‘）
    set（gca‘xtick‘[-1:1:11]）;
    set（gca‘ytick‘[-1:1:11]）;
    %*******
    grid on;  
    drawnow;  
    %movcount=movcount+1;  
    %mov（movcount） = getframe（gcf）;%   
end  
toc  
%movie2avi（mov‘movie.avi‘）;  
   
  
function [utrajDB]=Dynami