path-planning 路径规划d*算法

function  [path V] = FocusdDstar（map StartPos GoalPos）
%map——二维数组图 0: passable 1: obstacle
% startPos: Original Position
% endPos:   Target Postion


% The routine is programmed according to article:
% Anthony Stentz. “The D* Algorithm for Real-Time Planning of Optimal
% Traverse“ CMU-RI-TR-94-37 1994


%路径规划
clc;clear all;
m=30;n=30;
for i = 1:m+2
    if i == 1
        for j = 1:n+2
            map（ij） = 1;
        end
    elseif i == m+2
        for j = 1:n+2
            map（ij） = 1;
        end
    else
        for j = 1:n+2
            if （（j == 1）||（j == n+2））
                map（ij） = 1;
            else
                map（ij） = 0;
            end
        end
    end
end
%%障碍
for j=2:10
    map（5j）=1;
end
for j=2:15
     map（24j）=1;
end
for j=9:24
     map（10j）=1;
end
for j=20:31
    map（15j）=1;
end
for j=5:20
   map（20j）=1;
end
for j=18:27
    map（28j）=1;
end
for i=2:6  
    map（i18）=1;
end
for i=17:20
     map（i5）=1;
end
for i=23:25 
    map（i20）=1;
end
for i=13:17
    map（i13）=1;
end
StartPos=[33];
GoalPos=[2922];

    clear global Var;
    clear global ori_map;
    persistent hLine;
    if ishandle（hLine） 
        delete（hLine）; end
    global ori_map;
    global Var;
    [mn] = size（map）;
    X.x=[];X.y=[];
    Var.B（mn） = X; % B（X）  : b（X） backpointers 
    Var.r（mn） =X;
    Var.Tag = zeros（mn）;   % Tag（X） = 0（NEW）1（OPEN） or 2（CLOSED）
    Var.H = -1 * Inf*ones（mn）;        % H（X）  : estimate of the sum of the arc costs from X to G
    Var.K = -1 * Inf*ones（mn）;        % K（X）  : state key function value = min（H（X）Pre（X）） By which the states on the OPEN list are sorted
    Var.f = -1 * Inf*ones（mn）;        % f（X）  : state estimate cost from S through X to G
    Var.fB = -1 * Inf*ones（mn）;       % fb（X） : state bias estimate cost of X
    Var.OPEN = [];
    
   
    G.x = GoalPos（1）; G.y = GoalPos（2）;
    Var.H（G.y G.x） = 0;
    
    S.x = StartPos（1）; S.y = StartPos（2）;
    path = S;
    Var.R = S;
    Var.Rcur = S;
    Var.Dcur = 0;
   
 
    INSERT（G Var.H（G.y G.x））;
    val = 0; 
    while ~isempty（val） && val（1）        val = PROCESS_STATE2（map）;
    end

    if Var.Tag（S.y S.x） ~= 2
        warndlg（‘There is no path existed!‘）;
        path = [];
        return;
    end
    
    cur = Var.B（S.y S.x）;
    while cur.x~=G.x || cur.y~=G.y
        path = [path cur];
        cur = Var.B（cur.y cur.x）;
    end
    path = [path cur];
        
    hold on;
    hLine = plot（[path（:）.x] [path（:）.y] ‘b.:‘）;

    ori_map = map;
    choice = questdlg（‘请选择‘‘状态变化‘ ‘SetObstacle‘ ‘CancelObstacle‘‘NoChange‘‘NoChange‘）;
    while ~isequal（choice ‘NoChange‘）
        switch choice
           case ‘SetObstacle‘
               v = 0;
               c = ‘k‘;
           case ‘CancelObstacle‘
               v = 255;
               c = ‘g‘;
        end