A star A* 算法，MATLAB 好用

可运行的A star A* MATLAB 算法，包括地图显示。

%% a* algorithm
% seminar TU KL
% Author: Francisco J. Garcia R.
% based on: 
%    R. Kala （2014） Code for Robot Path Planning using A* algorithm 
%    Indian Institute of Information Technology Allahabad Available at: http://rkala.in/codes.html

%% Load Map and initial parameters
figure（1）
mapOriginal=im2bw（imread（‘Maps/a_map2.bmp‘））; % input map read from a bmp file.

resolutionX=100;
resolutionY=100;

mapResized=imresize（mapOriginal[resolutionX resolutionY]）;
map=mapResized; 

% Conection matrix - define admisible movement of robot
conn=[1 1 1;
      1 2 1;
      1 1 1];

display_process=true; % display processing of nodes

% grow boundary by 1 unit pixel - take into account size of robot
for i=1:size（mapResized1）
    for j=1:size（mapResized2）
        if mapResized（ij）==0
            if i-1>=1 map（i-1j）=0; end
            if j-1>=1 map（ij-1）=0; end
            if i+1<=size（map1） map（i+1j）=0; end
            if j+1<=size（map2） map（ij+1）=0; end
            if i-1>=1 && j-1>=1 map（i-1j-1）=0; end
            if i-1>=1 && j+1<=size（map2） map（i-1j+1）=0; end
            if i+1<=size（map1） && j-1>=1 map（i+1j-1）=0; end
            if i+1<=size（map1） && j+1<=size（map2） map（i+1j+1）=0; end
        end
    end
end

image（（map==0）.*0 + （map==1）.*255 + （mapResized-map）.*150）;
colormap（gray（256））

disp（‘select source in the image‘）;
[xy] = ginput（1）;
source=[double（int8（y）） double（int8（x））];    % source position in Y X format

disp（‘select goal in the image‘）;
[xy] = ginput（1）;
goal = [double（int8（y）） double（int8（x））];    % goal position in Y X format

if length（find（conn==2））~=1 error（‘no robot specified in connection matrix‘）; end

%% Compute path

%structure of a node is taken as positionY positionX historic cost heuristic cost total cost parent index in closed list （-1 for source） 
Q=[source 0 heuristic（sourcegoal） 0+heuristic（sourcegoal） -1]; % the processing queue of A* algorihtm open list
closed=ones（size（map））; % the closed list taken as a hash map. 1=not visited 0=visited
closedList=[]; % the closed list taken as a list
pathFound=false;
tic;
counter=0;

size（Q）

while size（Q1）>0
     [A I]=min（Q[]1）;
     n=Q（I（5）:）; % smallest cost element to process
     Q=[Q（1:I（5）-1:）;Q（I（5）+1:end:）]; % delete element under processing
     if n（1）==goal（1） && n（2）==goal（2） % goal test
         pathFound=true;break;
     end
     [rxryrv]=find（conn==2）; % robot position at the connection matrix
     [mxmymv]=find（conn==1）; % array of possible moves
     for mxi=1:size（mx1） %iterate through all moves
         newPos=[n（1）+mx（mxi）-rx n（2）+my（mxi）-ry]; % possible new node
         if checkPath（n（1:2）newPosmap） %if path from n to newPos is collission-free
              if closed（newPos（1）newPos（2））~=0 % not already in closed
                  historicCost=n（3）+historic（n（1:2）newPos）;
                  heuristicCost=heuristic（newPosgoal）;
                  totalCost=historicCost+heuristicCost;
                 

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2017-09-30 14:03  Maps\
     文件      251078  2017-09-30 14:03  Maps\a_map1.bmp
     文件      251078  2017-09-30 14:03  Maps\a_map2.bmp
     文件      251078  2017-09-30 14:03  Maps\a_map3.bmp
     文件      251078  2017-09-30 14:03  Maps\a_map4.bmp
     文件      251078  2017-09-30 14:03  Maps\a_map5.bmp
     文件      308346  2017-09-30 14:03  Maps\map1.bmp
     文件        4685  2016-11-28 19:46  a_star.m
     文件        3256  2019-02-12 10:30  a_start_compute_path.m
     文件         978  2019-02-12 10:30  checkPath.m
     文件         727  2019-02-12 10:30  feasiblePoint.m
     文件         542  2019-02-12 10:30  heuristic.m
     文件         535  2019-02-12 10:30  historic.m