Dstar动态路径规划算法

D*算法又称为动态A*算法，在未知环境或有动态障碍物出现时，采用A*算法需要丢弃初始规划完成的open表和close表，重新进行规划。造成规划时间的增加，D*算法的核心思想是先用dijkstra或A*从目标点向初始点进行反向搜索，然后机器人从起点向目标点移动，当遇到动态障碍物时，只进行局部的更改即可，效率明显提高。本仿真基于matlab进行D*算法的动画演示。

function Dstar
clc;
clear;
%% 初始化界面
 n = 10;   % field size n x n tiles  20*20的界面
%wallpercent = 0.3;  % this percent of field is walls   15%的界面作为阻碍物（墙）
cmap = [1 1 1; ...%  1 - white - 空地
        0 0 0; ...% 2 - black - 障碍 
        1 0 0; ...% 3 - red - 已搜索过的地方
        0 0 1; ...% 4 - blue - 下次搜索备选中心 
        0 1 0; ...% 5 - green - 起始点
        1 1 0;...% 6 - yellow -  到目 标点的路径 
       1 0 1];% 7 - -  目标点 
colormap（cmap）; 
global field;
field = ones（n）; 
startposind =12;   %sub2ind用来将行列坐标转换为线性坐标，这里是必要的，因为如果把startposind设置成[xy]的形式，访问field（[xy]）的时候
goalposind =77;    %它并不是访问x行y列元素，而是访问线性坐标为x和y的两个元素
% field（ceil（n^2.*rand（floor（n*n*wallpercent）1） ）） = Inf;
field（81:3） = 2; 
field（2:53:5）=2;
 %   field（810）=Inf;
% startposind = sub2ind（[nn]ceil（n.*rand）ceil（n.*rand））;   %sub2ind用来将行列坐标转换为线性坐标，这里是必要的，因为如果把startposind设置成[xy]的形式，访问field（[xy]）的时候
%goalposind = sub2ind（[nn]ceil（n.*rand）ceil（n.*rand））;    %它并不是访问x行y列元素，而是访问线性坐标为x和y的两个元素
field（startposind ）=5;
field（goalposind ）=7;
global costchart;
costchart = NaN*ones（n）;      %costchart用来存储各个点的实际代价，NaN代表不是数据（不明确的操作）
costchart（goalposind） = 0;     %起点的实际代价
% 生成n*n的元胞
global fieldpointers;
fieldpointers = zeros（n）;      %fieldpointers用来存储各个点的来源方向
%  起点设置为“S“终点设置为“G“
%fieldpointers{startposind} = ‘S‘; fieldpointers{goalposind} = ‘G‘;
% 墙的方向设置为0
%fieldpointers（field == 2） = {0};  
global setOpen;
global setOpenCosts_h;
global setOpenCosts_k
global setClosed;
global setClosedCosts;
global movementdirections
setOpen = （goalposind）; setOpenCosts_h = （0）; %setOpenHeuristics = （Inf）;
setOpenCosts_k=（0）;
setClosed = []; setClosedCosts = [];%初始化起点的open表和close表
movementdirections = {‘L‘‘R‘‘U‘‘D‘};
%counterIterations = 1;
tic
while true %ismember（AB）返回与A同大小的矩阵，其中元素1表示A中相应位置的元素在B中也出现，0则是没有出现
    if（max（ismember（setOpenstartposind））） 
       break;
    end   
    if isempty（setOpen）
      break; 
    end         %当OPEN表为空，代表可以经过的所有点已经查询完毕 
    process_state（2nstartposindgoalposind）                 
end

%% 显示路线运动过程中不会出现动态障碍物
if max（ismember（setOpenstartposind））    %当找到目标点时
  disp（‘已找到路径!‘）;  %disp： Display array， disp（X）直接将矩阵显示出来，不显示其名字，如果X为string，就直接输出文字X
     posind = startposind;
     while （fieldpointers（posind（1）） ~= 0） 
         posind = [fieldpointers（posind（1）） posind];     
     end 
      posind=flip（posind）;    %翻转矩阵
     for k = 2:length（posind） - 1 
        field（posind（k）） =6;
        image（1.5 1.5 field）;
        field（startposind ）=5;
        field（goalposind ）=7;
        grid on;
        set（gca‘gridline‘‘-‘‘gridcolor‘‘y‘‘linewidth‘2‘GridAlpha‘0.5）;
        set（gca‘xtick‘1:1:11‘ytick‘1:1:11）;
        axis image;
        drawnow;
        title（‘基于D*算法的路径规划 ‘‘fontsize‘16）
    end      
elseif isempty（setOpen）
      disp（‘路径不存在!‘）; 
end
%% 运动过程中出现障碍物
  figure（2）;
  cmap = [1 1 1; ...%  1 - white - 空地
        0 0 0; ...% 2 - black - 障碍 
        1 0 0; ...% 3 - red - 已搜索过的地方
       

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件       11974  2019-04-25 09:10  Dstar.m