• 大小: 1.44MB
    文件类型: .zip
    金币: 1
    下载: 0 次
    发布日期: 2023-10-15
  • 语言: Matlab
  • 标签: simulink  path  planning  

资源简介

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个文件信息

评论

共有 条评论