【MATLAB】快速扩展随机树（RRT）路径搜索算法实现

目录

1 概述

2 MATLAB代码

3 运行结果

4 github链接

5 参考

---

1 概述

RRT是一种基于概率搜索的寻路算法，与PRM类似，但PRM是一次性随机撒下所有点，然后去构造图，而RRT每次随机采样一个点，然后向前扩展一段距离，形成一个新的节点，不断重复此过程，最终形成一棵树，树的起点就是设定的路径起始点，最后由终点回溯形成一条可行路径。

算法伪代码如下：

 具体实现步骤：

首先随机选择一个点qrand，找最近的节点qnear，并向前扩展一段得到qnew，判断qnew和qnear连线是否与障碍物发生碰撞，若不碰撞，则将qnew和其父节点qnear加入Ei，新生成的结果如下图所示：

 然后重复上一步骤，继续寻找下一个点：

 直到找到目标点：

 

2 MATLAB代码

主要由三部分构成：

构造RRT算法：

function [GEd,qw]=build_RRT(qset, NumNodes, dq, QLIST)
qinit=qset(:,1); qgoal=qset(:,2);
G=qinit; GE=[];
% 障碍物个数
Obstacle_Num = size(QLIST,1);
%% 绘制障碍物模型
for i=1:Obstacle_Num
    Q = [QLIST(i,1:4);QLIST(i,5:8)];
    fill(Q(1,:),Q(2,:),'k');
    hold on
end


fig_num=1;
for k= 1:NumNodes
    qrand=[rand*20; rand*20];
    C(:,k)=qrand;
    qnear= Nearest_Vertex( C(:,k) , G);
    qnew= qnear + dq*(C(:,k)-qnear)/norm(C(:,k)-qnear);
    if Obstacle_Num==0
        G=[G,qnew];
        GE= [GE, [qnear;qnew]];
        plot([qnear(1),qnew(1)],[qnear(2),qnew(2)])
        pause(0.1);
    else
        for i=1:Obstacle_Num
            Q = [QLIST(i,1:4);QLIST(i,5:8)];
            if isequal(1,isIntersection( qnear,qnew,Q ) )
                break;
            elseif i==Obstacle_Num
                % 如果到最后一个障碍物都不碰撞，则将新节点及其父节点保存进GE
                G=[G,qnew];
                GE= [GE, [qnear;qnew]];
                % 保存gif，与算法无关
                plot([qnear(1),qnew(1)],[qnear(2),qnew(2)])
                pause(0.1);