资源简介

用 matlab 仿真A*路径规划算法,有子函数可修改。解压后运行a_star.m即可。生成迷宫中的最短的路径,以及路径搜索的过程。

资源截图

代码片段和文件信息

%% 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;
                 

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

     文件       4685  2016-11-28 22:46  astar\a_star.m

     文件       3256  2017-09-30 10:35  astar\a_start_compute_path.m

     文件        978  2017-09-30 17:04  astar\checkPath.m

     文件        727  2017-09-30 17:04  astar\feasiblePoint.m

     文件        542  2017-09-30 17:04  astar\heuristic.m

     文件        535  2017-09-30 17:04  astar\historic.m

     文件     251078  2017-09-30 17:03  astar\Maps\a_map1.bmp

     文件     251078  2017-09-30 17:03  astar\Maps\a_map2.bmp

     文件     251078  2017-09-30 17:03  astar\Maps\a_map3.bmp

     文件     251078  2017-09-30 17:03  astar\Maps\a_map4.bmp

     文件     251078  2017-09-30 17:03  astar\Maps\a_map5.bmp

     文件     308346  2017-09-30 17:03  astar\Maps\map1.bmp

     目录          0  2017-09-30 17:03  astar\Maps

     目录          0  2017-09-30 17:04  astar

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

              1574459                    14


评论

共有 条评论