深蓝路径规划A* hw2 Matlab代码

       初步了解A*路径规划算法，写了一个很粗糙的Matlab版本，有很大的优化空间，主要有两个优化的地方，其中一个为未记录前进过程中的路径点，导致最后一步重新对过程中进行了遍历寻找父节点，其次为未对开闭集合进行逻辑判断，从而创建了两个数组进行存储，导致不必要的定义。

function path = A_star_search(map,MAX_X,MAX_Y)
%%
%This part is about map/obstacle/and other settings
    %pre-process the grid map, add offset
    size_map = size(map,1);
    Y_offset = 0;
    X_offset = 0;
    
    %Define the 2D grid map array.
    %Obstacle=-1, Target = 0, Start=1
    MAP=2*(ones(MAX_X,MAX_Y));
    
    %Initialize MAP with location of the target
    xval=floor(map(size_map, 1)) + X_offset;
    yval=floor(map(size_map, 2)) + Y_offset;
    xTarget=xval;
    yTarget=yval;
    MAP(xval,yval)=0;
    
    %Initialize MAP with location of the obstacle
    for i = 2: size_map-1
        xval=floor(map(i, 1)) + X_offset;
        yval=floor(map(i, 2)) + Y_offset;
        MAP(xval,yval)=-1;
    end 
    
    %Initialize MAP with location of the start point
    xval=floor(map(1, 1)) + X_offset;
    yval=floor(map(1, 2)) + Y_offset;
    xStart=xval;
    yStart=yval;
    MAP(xval,yval)=1;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %LISTS USED FOR ALGORITHM
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %OPEN LIST STRUCTURE
    %--------------------------------------------------------------------------
    %IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
    %--------------------------------------------------------------------------
    OPEN=[];
    %CLOSED LIST STRUCTURE
    %--------------
    %X val | Y val |
    %--------------
    % CLOSED=zeros(MAX_VAL,2);
    CLOSED=[];

    %Put all obstacles on the Closed list
    k=1;%Dummy counter
    for i=1:MAX_X
        for j=1:MAX_Y
            if(MAP(i,j) == -1)
                CLOSED(k,1)=i;
                CLOSED(k,2)=j;
                k=k+1;
            end
        end
    end
    CLOSED_COUNT=size(CLOSED,1);
    %set the starting node as the first node
    xNode=xval;
    yNode=yval;
    OPEN_COUNT=1;
    goal_distance=distance(xNode,yNode,xTarget,yTarget);
    path_cost=0;
    OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,xNode,yNode,goal_distance,path_cost,goal_distance);
    OPEN(OPEN_COUNT,1)=0;
    CLOSED_COUNT=CLOSED_COUNT+1;
    CLOSED(CLOSED_COUNT,1)=xNode;
    CLOSED(CLOSED_COUNT,2)=yNode;
    NoPath=1;

%%
%This part is your homework
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% START ALGORITHM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    gn = zeros(10,10);
    gn(1,1) = 1;
    OPEN_COUNT = 0;
    FIND_COUNT = 0;
    cnt = 1
    while(cnt) %you have to dicide the Conditions for while loop exit 
%此步中OPEN和FIND进行了两个定义，显得很麻烦，后续也可进行优化   exp_array=expand_array(xStart,yStart,gn(xStart,yStart),xTarget,yTarget,CLOSED,MAX_X,MAX_Y)
        if OPEN_COUNT >=1
            OPEN_LIST(min,:)=[]
            OPEN_COUNT = OPEN_COUNT-1
        end
        [exp_array_x,exp_array_y] = size(exp_array);
        for i=1:exp_array_x
            if gn(exp_array(i,1),exp_array(i,2))==0 || gn(exp_array(i,1),exp_array(i,2)) > gn(xStart,yStart)+1
                gn(exp_array(i,1),exp_array(i,2)) = gn(xStart,yStart)+1
                OPEN_COUNT = OPEN_COUNT + 1;
                FIND_COUNT = FIND_COUNT + 1;
                OPEN_LIST(OPEN_COUNT,:)= insert_open(exp_array(i,1),exp_array(i,2),xStart,yStart,exp_array(i,3),exp_array(i,4),exp_array(i,5))
                FIND_COUNT_LIST(FIND_COUNT,:)= insert_open(exp_array(i,1),exp_array(i,2),xStart,yStart,exp_array(i,3),exp_array(i,4),exp_array(i,5))
            end
        end
        min = min_fn(OPEN_LIST,OPEN_COUNT,xTarget,yTarget)
        xStart = OPEN_LIST(min,2)
        yStart = OPEN_LIST(min,3)
        if xStart==9 && yStart==9
            cnt = 0
        end
     %
     %finish the while loop
     %
    end %End of While Loop
    
    %Once algorithm has run The optimal path is generated by starting of at the
    %last node(if it is the target node) and then identifying its parent node
    %until it reaches the start node.This is the optimal path
    
    %
    %How to get the optimal path after A_star search?
    %please finish it
    %
   %下面为通过最后一步，一步一步向上遍历找父节点，写的很麻烦，可以对此进行优化 
   path = [];
   [OPEN_LIST_x,OPEN_LIST_y] = size(FIND_COUNT_LIST);
   min = min_fn(FIND_COUNT_LIST,FIND_COUNT,xTarget,yTarget)
   k_cnt = 1
   gn_x = FIND_COUNT_LIST(min,4)
   gn_y = FIND_COUNT_LIST(min,5)
   while gn_x ~=1 || gn_y ~=1
       path(k_cnt,1) = FIND_COUNT_LIST(min,2) - 0.5
       path(k_cnt,2) = FIND_COUNT_LIST(min,3) - 0.5
       gn_x = FIND_COUNT_LIST(min,4)
       gn_y = FIND_COUNT_LIST(min,5)
       for j=1:OPEN_LIST_x
           if FIND_COUNT_LIST(j,2) == gn_x && FIND_COUNT_LIST(j,3) == gn_y
               min = j
               break
           end
       end
       k_cnt = k_cnt + 1
   end
   path(k_cnt,1)= [1]
   path(k_cnt,2)= [1]
end

附结果图