三种不同路径规划的仿真

A*算法 RRY算法 遗传算法三种算法的机器人路径规划方法仿真，可直接运行 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  2016-11-28 14:46  Path_Planning_Simulator-master\
     文件         378  2016-11-28 14:46  Path_Planning_Simulator-master\.gitattributes
     文件        4700  2016-11-28 14:46  Path_Planning_Simulator-master\Bidirectional_RRT.m
     文件        1413  2016-11-28 14:46  Path_Planning_Simulator-master\LoadMap.m
     目录           0  2016-11-28 14:46  Path_Planning_Simulator-master\Maps\
     文件      251078  2016-11-28 14:46  Path_Planning_Simulator-master\Maps\a_map1.bmp
     文件      251078  2016-11-28 14:46  Path_Planning_Simulator-master\Maps\a_map2.bmp
     文件      251078  2016-11-28 14:46  Path_Planning_Simulator-master\Maps\a_map3.bmp
     文件      251078  2016-11-28 14:46  Path_Planning_Simulator-master\Maps\a_map4.bmp
     文件      251078  2016-11-28 14:46  Path_Planning_Simulator-master\Maps\a_map5.bmp
     文件      308346  2016-11-28 14:46  Path_Planning_Simulator-master\Maps\map1.bmp
     文件        5439  2016-11-28 14:46  Path_Planning_Simulator-master\Potential_field_function.m
     文件         362  2016-11-28 14:46  Path_Planning_Simulator-master\README.md
     文件       36632  2016-11-28 14:46  Path_Planning_Simulator-master\RobotSimulation.fig
     文件       19465  2016-11-28 14:46  Path_Planning_Simulator-master\RobotSimulation.m
     文件        4685  2016-11-28 14:46  Path_Planning_Simulator-master\a_star.m
     文件        3272  2016-11-28 14:46  Path_Planning_Simulator-master\a_start_compute_path.m
     文件        3454  2016-11-28 14:46  Path_Planning_Simulator-master\bidirectional_RRT_compute_final_path.m
     文件        5889  2016-11-28 14:46  Path_Planning_Simulator-master\callback_interrupt.m
     文件         978  2016-11-28 14:46  Path_Planning_Simulator-master\checkPath.m
     文件         441  2016-11-28 14:46  Path_Planning_Simulator-master\distToEdge.m
     文件         552  2016-11-28 14:46  Path_Planning_Simulator-master\distanceCost.m
     文件         603  2016-11-28 14:46  Path_Planning_Simulator-master\distanceCost_RRT.m
     文件        5477  2016-11-28 14:46  Path_Planning_Simulator-master\ex_guide_timergui.fig
     文件        7074  2016-11-28 14:46  Path_Planning_Simulator-master\ex_guide_timergui.m
     文件         727  2016-11-28 14:46  Path_Planning_Simulator-master\feasiblePoint.m
     文件         542  2016-11-28 14:46  Path_Planning_Simulator-master\heuristic.m
     文件         535  2016-11-28 14:46  Path_Planning_Simulator-master\historic.m
     文件         521  2016-11-28 14:46  Path_Planning_Simulator-master\minDistToEdges.m
     文件        1362  2016-11-28 14:46  Path_Planning_Simulator-master\plotRobot.m
     文件        3218  2016-11-28 14:46  Path_Planning_Simulator-master\potential_field.m
............此处省略4个文件信息