• 大小: 216KB
    文件类型: .zip
    金币: 1
    下载: 0 次
    发布日期: 2021-06-14
  • 语言: Matlab
  • 标签: MATLAB  RRT  

资源简介

用matlab编写的RRT算法,代码简单,可以完美运行,对初学者有所帮助

资源截图

代码片段和文件信息

% ?Rahul Kala IIIT Allahabad Creative Commons Attribution-ShareAlike 4.0 International License. 
% The use of this code its parts and all the materials in the text; creation of derivatives and their publication; and sharing the code publically is permitted without permission. 

% Please cite the work in all materials as: R. Kala (2014) Code for Robot Path Planning using Rapidly-exploring Random Trees Indian Institute of Information Technology Allahabad Available at: http://rkala.in/codes.html

map=im2bw(imread(‘map1.bmp‘)); % input map read from a bmp file. for new maps write the file name here
source=[10 10]; % source position in Y X format
goal=[490 490]; % goal position in Y X format
stepsize=20; % size of each step of the RRT
disTh=20; % nodes closer than this threshold are taken as almost the same
maxFailedAttempts = 10000;
display=true; % display of RRT

%%%%% parameters end here %%%%%

tic;
if ~feasiblePoint(sourcemap) error(‘source lies on an obstacle or outside map‘); end
if ~feasiblePoint(goalmap) error(‘goal lies on an obstacle or outside map‘); end
if display imshow(map);rectangle(‘position‘[1 1 size(map)-1]‘edgecolor‘‘k‘); end
RRTree=double([source -1]); % RRT rooted at the source representation node and parent index
failedAttempts=0;
counter=0;
pathFound=false;
while failedAttempts<=maxFailedAttempts  % loop to grow RRTs
    if rand < 0.5 
        sample=rand(12) .* size(map); % random sample
    else
        sample=goal; % sample taken as goal to bias tree generation to goal
    end
    [A I]=min( distanceCost(RRTree(:1:2)sample) []1); % find closest as per the function in the metric node to the sample
    closestNode = RRTree(I(1)1:2);
    theta=atan2(sample(1)-closestNode(1)sample(2)-closestNode(2));  % direction to extend sample to produce new node
    newPoint = double(int32(closestNode(1:2) + stepsize * [sin(theta)  cos(theta)]));
    if ~checkPath(closestNode(1:2) newPoint map) % if extension of closest node in tree to the new point is feasible
        failedAttempts=failedAttempts+1;
        continue;
    end
    if distanceCost(newPointgoal)    [A I2]=min( distanceCost(RRTree(:1:2)newPoint) []1); % check if new node is not already pre-existing in the tree
    if distanceCost(newPointRRTree(I2(1)1:2))    RRTree=[RRTree;newPoint I(1)]; % add node
    failedAttempts=0;
    if display 
        line([closestNode(2);newPoint(2)][closestNode(1);newPoint(1)]);
        counter=counter+1;M(counter)=getframe;
    end
end
if display && pathFound 
    line([closestNode(2);goal(2)][closestNode(1);goal(1)]);
    counter=counter+1;M(counter)=getframe;
end
if display 
    disp(‘click/press any key‘);
    waitforbuttonpress; 
end
if ~pathFound error(‘no path found. maximum attempts reached‘); end
path=[goal];
prev=I(1);
while prev>0
    path=[

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        3331  2014-06-06 15:35  RRT\astart.m
     文件        1012  2014-06-06 15:35  RRT\checkPath.m
     文件         591  2014-06-06 15:35  RRT\distanceCost.m
     文件         756  2014-06-06 15:35  RRT\feasiblePoint.m
     文件      251078  2014-06-06 15:35  RRT\map1.bmp
     文件      251078  2014-06-06 15:35  RRT\map2.bmp
     文件      251078  2014-06-06 15:35  RRT\map3.bmp
     文件      251078  2014-06-06 15:35  RRT\map4.bmp
     文件      251078  2014-06-06 15:35  RRT\map5.bmp
     文件      210658  2014-06-06 15:35  RRT\RRT.pdf

评论

共有 条评论