双向bidirectional RRT

从起始点和末端点同时搜索的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 Bidirectional 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=[20 20]; % source position in Y X format
goal=[450 450]; % 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
RRTree1=double（[source -1]）; % First RRT rooted at the source representation node and parent index
RRTree2=double（[goal -1]）; % Second RRT rooted at the goal representation node and parent index
counter=0;
tree1ExpansionFail=false; % sets to true if expansion after set number of attempts fails
tree2ExpansionFail=false; % sets to true if expansion after set number of attempts fails
while ~tree1ExpansionFail || ~tree2ExpansionFail  % loop to grow RRTs
    if ~tree1ExpansionFail 
        [RRTree1pathFoundtree1ExpansionFail]=rrtExtend（RRTree1RRTree2goalstepsizemaxFailedAttemptsdisThmap）; % RRT 1 expands from source towards goal
        if ~tree1ExpansionFail && isempty（pathFound） && display
            line（[RRTree1（end2）;RRTree1（RRTree1（end3）2）][RRTree1（end1）;RRTree1（RRTree1（end3）1）]‘color‘‘b‘）;
            counter=counter+1;M（counter）=getframe;
        end
    end
    if ~tree2ExpansionFail 
        [RRTree2pathFoundtree2ExpansionFail]=rrtExtend（RRTree2RRTree1sourcestepsizemaxFailedAttemptsdisThmap）; % RRT 1 expands from goal towards source
        if ~isempty（pathFound） pathFound（3:4）=pathFound（4:-1:3）; end % path found
        if ~tree2ExpansionFail && isempty（pathFound） && display
            line（[RRTree2（end2）;RRTree2（RRTree2（end3）2）][RRTree2（end1）;RRTree2（RRTree2（end3）1）]‘color‘‘r‘）;
            counter=counter+1;M（counter）=getframe;
        end
    end
    if ~isempty（pathFound） % path found
         if display
            line（[RRTree1（pathFound（13）2）;pathFound（12）;RRTree2（pathFound（14）2）][RRTree1（pathFound（13）1）;pathFound（11）;RRTree2（pathFound（14）1）]‘color‘‘green‘）;
            counter=counter+1;M（counter）=getframe;
        end
        path=[pathFound（11:2）]; % compute path
        

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-05-21 10:22  bidirectional RRT\
     目录           0  2018-05-21 10:22  bidirectional RRT\bidirectional RRT\
     文件        3801  2018-01-24 17:22  bidirectional RRT\bidirectional RRT\astart.m
     文件      194852  2014-06-06 15:35  bidirectional RRT\bidirectional RRT\bidirectional RRT.pdf
     文件        1024  2014-06-06 15:35  bidirectional RRT\bidirectional RRT\checkPath.m
     文件         603  2014-06-06 15:35  bidirectional RRT\bidirectional RRT\distanceCost.m
     文件         768  2014-06-06 15:35  bidirectional RRT\bidirectional RRT\feasiblePoint.m
     文件      251078  2014-06-06 15:35  bidirectional RRT\bidirectional RRT\map1.bmp
     文件      251078  2014-06-06 15:35  bidirectional RRT\bidirectional RRT\map2.bmp
     文件      251078  2014-06-06 15:35  bidirectional RRT\bidirectional RRT\map3.bmp
     文件      251078  2014-06-06 15:35  bidirectional RRT\bidirectional RRT\map4.bmp
     文件      251078  2014-06-06 15:35  bidirectional RRT\bidirectional RRT\map5.bmp
     文件      251080  2017-11-08 11:19  bidirectional RRT\bidirectional RRT\map6.bmp
     文件        2114  2014-06-06 15:35  bidirectional RRT\bidirectional RRT\rrtExtend.m