平面改进RRT算法路径规划

在平面多个障碍物的复杂情况下，可以绕开多个平面墙，找到目标点

%% Function collisionCheck
% 
% *Description:* Returns whether a collision occurs between an edge and a set
% of obstacles. Also gives the point of intersection. （P1=nodeP2=parent node）

function [collisionPInt]=collisionCheck（P1P2obs）
collision=0;
global collcount;
collcount=collcount+1;
%default is that it is safe then if a collision is found it is set to 1
%and we return

if size（obs1）==0
    PInt=inf;
    return;
end
% Calculate intercept point of a line and line
% ---------------------------------------------


for i=1:size（obs3）
  
    pt1.x = P1（1）;
    pt1.y = P1（2）;
    pt2.x = P2（1）;
    pt2.y = P2（2）;
    pt3.x = obs（11i）;
    pt3.y = obs（12i）;
    pt4.x = obs（21i）;
    pt4.y = obs（22i）;
    
      denom=（（pt4.y - pt3.y）*（pt2.x - pt1.x））-（（pt4.x - pt3.x）*（pt2.y - pt1.y））;
      nume_a=（（pt4.x - pt3.x）*（pt1.y - pt3.y））-（（pt4.y - pt3.y）*（pt1.x - pt3.x））;
      nume_b=（（pt2.x - pt1.x）*（pt1.y - pt3.y））-（（pt2.y - pt1.y）*（pt1.x - pt3.x））;
 
        if（denom==0）
            if（nume_a == 0 & nume_b == 0）
                collision=1;
                PInt=inf;
                return;
            end
            collision=0;
        end
 
        ua = nume_a / denom;
        ub = nume_b / denom;
 
        if（ua >= 0.0 & ua <= 1.0 & ub >= 0.0 & ub <= 1.0）
            % Get the intersection point.
            PInt（1） = pt1.x + ua*（pt2.x - pt1.x）;
            PInt（2） = pt1.y + ua*（pt2.y - pt1.y）;
            collision=1;
            return;
        end
        collision=0;
        PInt=Inf;

end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-05-21 09:52  2维RRT避障路径规划\
     文件         476  2009-11-04 14:33  2维RRT避障路径规划\PointInHole.m
     文件         622  2009-11-04 11:40  2维RRT避障路径规划\PointInQuad.m
     文件        5052  2018-01-22 16:40  2维RRT避障路径规划\RRT.m
     文件        5054  2010-12-10 15:58  2维RRT避障路径规划\RRTv1.m
     文件        1591  2009-11-06 21:51  2维RRT避障路径规划\collisionCheck.m
     文件        4784  2009-11-04 11:40  2维RRT避障路径规划\connect.m
     文件        3795  2009-11-06 21:59  2维RRT避障路径规划\connect2.m
     文件      179254  2010-12-10 16:19  2维RRT避障路径规划\deltaq=10.bmp
     文件      236278  2010-12-10 16:17  2维RRT避障路径规划\deltaq=20.bmp
     文件        1292  2009-11-04 22:26  2维RRT避障路径规划\displayTree.m
     文件        1710  2009-11-04 11:40  2维RRT避障路径规划\dist2edges.m
     文件         495  2010-12-10 18:43  2维RRT避障路径规划\getSegmentPoint.m
     文件        1160  2009-11-09 15:26  2维RRT避障路径规划\initSearch.m
     文件      236278  2010-12-10 17:27  2维RRT避障路径规划\jammed.bmp
     文件     1187638  2010-12-10 17:30  2维RRT避障路径规划\maze.bmp
     文件        1279  2009-11-10 15:18  2维RRT避障路径规划\monexplore.m
     文件        1678  2010-12-10 16:44  2维RRT避障路径规划\monexplore2.asv
     文件        1667  2010-12-10 16:55  2维RRT避障路径规划\monexplore2.m
     文件         489  2009-11-04 12:37  2维RRT避障路径规划\nearestNeighbour.m
     文件      179254  2010-12-10 16:56  2维RRT避障路径规划\neurones.bmp
     文件         343  2009-11-09 16:24  2维RRT避障路径规划\newPoint.m
     文件         245  2010-12-10 17:24  2维RRT避障路径规划\obstacles.asv
     文件         186  2010-12-10 17:55  2维RRT避障路径规划\obstacles.txt
     文件         472  2009-11-05 14:08  2维RRT避障路径规划\obstacles2.txt
     文件        2279  2009-11-04 11:50  2维RRT避障路径规划\obstacles_array.m
     文件        1458  2009-11-05 11:40  2维RRT避障路径规划\obstacles_array2.m
     文件         894  2009-11-04 11:40  2维RRT避障路径规划\sameSide.m
     文件         355  2010-12-10 18:41  2维RRT避障路径规划\smoothPath.asv
     文件         429  2010-12-10 18:43  2维RRT避障路径规划\smoothPath.m
     文件         492  2010-12-10 18:00  2维RRT避障路径规划\smoothing.mat
............此处省略2个文件信息