寻找路径的一种比较好用的算法 DStar算法

寻找路径的方法，对这种方法感兴趣的可以参考一下

function path = 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


%路径规划

    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
        Obs = min（size（map） round（ginput（1）））;
        Obs = max（[11] Obs）;
        x = Obs（1）;y=Obs（2）;
        Obj.x = x; Obj.y = y;
        map（y x）=v;
        X = [x-0.5 x+0.5 x+0.5 x-0.5];
        Y = [y-0.5 y-0.5 y+0.5 y+0.5];
        fill（XYc‘EdgeColor‘c）;drawnow;
        choice = questdlg（‘请选择‘‘状态变化‘ ‘SetObstacle‘ ‘CancelObstacle‘‘NoChange‘‘NoChange‘）;
    end

    Var.R = S;
    path = Var.R;
    while Var.R.x~=G.x | Var.R.y~=G.y
       hR = plot（Var.R.x Var.R.y ‘rs‘）;drawnow; pause（0.2）;
       InconsistentX = CheckBySensor（Var.R 5 map）;
       if ~isempty（InconsistentX）
            if ~isequal（Var.Rcur Var.R）
                Var.Dcur = Var.Dcur + Get_gVal（Var.Rcur Var.R）;
                Var.Rcur = Var.R;
            end
            for i=1 : length（InconsistentX）
   

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      17359  2007-12-15 01:48  FocusdDstar.m

----------- ---------  ---------- -----  ----

                17577                    2