focusddstar.m

通过研讨Stentz的Focussed D*文章

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;
    [m,n] = size(map);
    X.x=[];X.y=[];
    Var.B(m,n) = X; % B(X)  : b(X) ,backpointers 
    Var.r(m,n) =X;
    Var.Tag = zeros(m,n);   % Tag(X) = 0(NEW),1(OPEN), or 2(CLOSED)
    Var.H = -1 * Inf*ones(m,n);        % H(X)  : estimate of the sum of the arc costs from X to G
    Var.K = -1 * Inf*ones(m,n);        % 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(m,n);        % f(X)  : state estimate cost from S through X to G
    Var.fB = -1 * Inf*ones(m,n);       % 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)<Inf & Var.Tag(S.y, S.x)~=2  
        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([1,1], 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(X,Y,c,'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)
                X = InconsistentX(i);
                val = MODIFY_COST(X, 0, 0);
            end
            while ~isempty(val) & LESS(val,COST(Var.R))
                val =PROCESS_STATE2(map, 1);
            end
       end
      Var.R = Var.B(Var.R.y, Var.R.x);
      path = [path Var.R];
      delete(hR);
    end

    
      

 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
function val = PROCESS_STATE(map, flag)
    global Var;
    persistent h;
    if ishandle(h) delete(h);end
    
    if nargin < 2
        flag = 0;
    end
    
    [Ymax, Xmax]  = size(map);
    X = GET_MIN_STATE();
    if isempty(X)
        val = [];
        return;
    end
    K_old = Var.K(X.y, X.x); %K_old = GET_KMIN(); 
    DELETE(X);
    
    if flag ==1
        h = plot(X.x, X.y, 'ro');drawnow;
        legend('Start','Goal','OPEN','CLOSED');
    end
    Ylist = GetNeighbour8(X);    

    if K_old < Var.H(X.y, X.x)    %Reduce H(X) by lowest_cost neighbor if possible
        for j = 1 : 8
            Y = Ylist(j);
             if Y.x<1 | Y.y<1 | Y.x>Xmax | Y.y>Ymax
                continue;
             elseif Var.Tag(X.y, X.x)~=0 & Var.H(Y.y, Y.x)<=K_old & Var.H(X.y, X.x)>Var.H(Y.y, Y.x)+C(Y, X, map)
                Var.B(X.y, X.x) = Y;
                Var.H(X.y, X.x) = Var.H(Y.y, Y.x) + C(Y, X,map);
            end
        end
    end
    
    if K_old==Var.H(X.y, X.x)
        % Process each neighbor of X
        for j = 1 : 8
            Y = Ylist(j);
            if Y.x<1 | Y.y<1 | Y.x>Xmax | Y.y>Ymax 
                continue;
            end
            
            R = Var.B(Y.y, Y.x);
            cxy = C(X,Y,map);
                    
            if Var.Tag(Y.y, Y.x)==0 ... % a NEW state
                    | ((R.x==X.x & R.y==X.y) & Var.H(Y.y, Y.x)~=(Var.H(X.y, X.x)+cxy)) ...%Y指向了X但代价不一致
                    | (~(R.x==X.x & R.y==X.y) & Var.H(Y.y, Y.x)>(Var.H(X.y, X.x)+cxy))    %Y没有指向X但X可能是路径点
                Var.B(Y.y, Y.x) = X;
                INSERT(Y, Var.H(X.y,X.x)+cxy);
            end
        end
    else            %K_old>Var.H(X.y, X.x)
        for j = 1 : 8
            Y = Ylist(j);
            if Y.x<1 | Y.y<1 | Y.x>Xmax | Y.y>Ymax 
                continue;
            end

            % Propagate cost to NEW state
            R = Var.B(Y.y, Y.x);
            cxy = C(X,Y,map);
          
            if Var.Tag(Y.y, Y.x)==0 ... % a NEW state
                    | ((R.x==X.x & R.y==X.y) & Var.H(Y.y, Y.x)~=Var.H(X.y, X.x)+cxy)  %Y指向了X但代价不一致
                Var.B(Y.y, Y.x) = X;
                INSERT(Y, Var.H(X.y,X.x)+cxy);
            else
                %Y已在CLOSED列表中，但Y没有指向X，而X可能是Y的下一路径点
                if ~(R.x==X.x & R.y==X.y) & Var.H(Y.y, Y.x)>Var.H(X.y, X.x)+cxy & Var.Tag(Y.y, Y.x)==2
                   INSERT(X, Var.H(X.y,X.x));
                elseif ~(R.x==X.x & R.y==X.y) ...
                      & Var.H(X.y, X.x)>Var.H(Y.y, Y.x)+cxy ...
                      & Var.Tag(Y.y, Y.x)== 2 & Var.H(Y.y, Y.x)>K_old
                  INSERT(Y,Var.H(Y.y, Y.x));
                end
            end   
        end
    end

    val = GET_MIN_VAL();
    return;
    
    
%C(X,Y):  c(X,Y)--The cost of traversing an arc from state Y to state X
%maybe bidirectional(c(X,Y)==C(Y,X)) or unidirectional(c(X,Y)~=C(Y,X))
function cost = C(A, B, map)
    
    H = abs(map(A.y, A.x)-map(B.y, B.x));
    if H > 0.5;
        cost = Inf;
    else
        cost=sqrt((A.x-B.x)^2+(A.y-B.y)^2+H^2);
    end
        
%G(X,Y):g(X0, X1)-G(X, Var.Rcur)
function g = Get_gVal(X, Y)
    di = abs(X.x-Y.x);
    dj = abs(X.y-Y.y);
%     g = di + dj; return; %manhattan distance
    g = max(di,dj);return;% Chess Distance
%     g = sqrt(di^2 + dj^2);return %Euclid distance
%     Cmin = 1.0;
%     if di >= dj
%         g = Cmin*(di+0.5*dj);
%     else
%         g = Cmin*(dj+0.5*di);
%     end
    g = max(di,dj)+0.4142*min(di,dj);



                
function val = PROCESS_STATE1(map, flag)
    global Var;
    persistent h;
    if (nargin < 2)  flag = 0; end
    if(ishandle(h)) delete(h); end
    [Ymax, Xmax]  = size(map);
    X = GET_MIN_STATE();
    if isempty(X.x) | isempty(X.y)
        val = [];
        return;
    end
    K_old = Var.K(X.y, X.x);DELETE(X);

    if (flag==1)  h=plot(X.x, X.y, 'ro');drawnow;  end
%    
    Ylist = GetNeighbour8(X);
    for j = 1 : 8
        Y = Ylist(j); 
        if Y.x<1 | Y.y<1 | Y.x>Xmax | Y.y>Ymax
            continue;
        end
        Ynext = Var.B(Y.y, Y.x);
        Cxy = C(X,Y,map);
        
        %************comment of IF*****************************************
        %if Y is a NEW node, then insert Y into OPEN
        %or when Y points to X but H(y) dosen't equal to H(x) plus
        %Cost(X,y), this means X state or map(X) is changed, then re-insert
        %Y into OPEN. Or Y dosen't point to X, but X may be is on the
        %optimal next point of Y.
        if Var.Tag(Y.y, Y.x)==0 ... % a NEW state
                | (isequal(Ynext, X) & Var.H(Y.y, Y.x)~=Var.H(X.y, X.x)+Cxy) ...%Y指向了X但代价不一致
                | (~isequal(Ynext, X) & Var.H(Y.y, Y.x)>Var.H(X.y, X.x)+Cxy)
            Var.B(Y.y, Y.x) = X;
            INSERT(Y, Var.H(X.y,X.x)+Cxy);
        else
            if ~isequal(Ynext, X) & Var.H(X.y, X.x)>Var.H(Y.y, Y.x)+Cxy%Cxy=Cyx
                Var.B(X.y, X.x) = Y;
                INSERT(X, Var.H(Y.y, Y.x)+Cxy);
            end
        end
    end
  
    val = GET_MIN_VAL();
    return; 
    
function val = PROCESS_STATE2(map, flag)
    global Var;
    persistent h;
    if (nargin < 2)  flag = 0; end
    if(ishandle(h)) delete(h); end
    [Ymax, Xmax]  = size(map);
    X = GET_MIN_STATE();
    if isempty(X.x) | isempty(X.y)
        val = [];
        return;
    end
    K_old = Var.K(X.y, X.x);DELETE(X);