matlab练习程序（快速搜索随机树RRT）

RRT快速搜索随机树英文全称Rapid-exploration Random Tree，和PRM类似，也是一种路径规划算法。

和PRM类似，算法也需要随机撒点，不过不同的是，该算法不是全局随机撒点，而是一次撒一个点，然后判断当前搜索树与随机点距离，然后找到搜索树距离随机点最近的节点，向该随机点方向扩展。这里随机点有一定的概率是终点，所以搜索树最终是能够到达终点的。

算法流程如下：

1. 首先确定地图与起始结束点位置，设置搜索树，这里定义了一个随机点列表和一个随机点索引前驱列表代表搜索树。

2. 随机撒一个点，该点有可能是最终点，也有可能是全局中的一个随机点，设为nextp。

3. 找到搜索树中距离nextp最近的节点，从该节点向nextp方向扩展step距离，生成新的路径。

4. 判断新生成的路径是否通过障碍物或者该路径已经被搜索过，如果都没有则该路径加入到搜索树中，否则重新生成随机点。

5. 不断循环直到搜索树最终节点距离终点小于一定阈值，搜索结束，根据前驱列表画出搜索路径。

matlab代码如下：

main.m：

clear all;
close all;
clc;

img = imread('map.png');    %空间地图
imshow(img);
hold on;

[h,w]=size(img);
p=ginput();                 %选取起始与结束位置
plot(p(:,),p(:,),'r.');

pc = p(,:);                %随机节点列表
step = ;                  %随机扩展步长
parent = ;                 %所有节点前驱，初始节点前驱为自己

while norm(pc(end,:)-p(,:))>step           %搜索到距离结束节点一定距离停止

    if rand()<0.3                           %按30%概率随机搜索，%概率朝着结束位置搜索
        nextp = [rand()*h rand()*w];
    else
        nextp = p(,:);
    end

    diff = repmat(nextp,length(pc(:,)),)-pc;          %计算节点树与待搜索节点距离
    [~,ind] = min(sqrt(diff(:,).^+diff(:,).^));     %找到距离带搜索节点最小的节点树节点

    direct = atan2(nextp()-pc(ind,),nextp()-pc(ind,));
    sin_dir = sin(direct);
    cos_dir = cos(direct);

    newp = pc(ind,:) + step*[sin_dir cos_dir];          %向着待搜索节点方向扩展节点树

    isobs = check_obs(img,newp,pc(ind,:));              %判断该路径是否有障碍物

    if isobs==                                         %有障碍物重新搜索
        continue;
    end

    diff = repmat(newp,length(pc(:,)),)-pc;           %判断该路径是否已搜索过，如果已搜索过，则重新搜索
    if min(sqrt(diff(:,).^+diff(:,).^))<sqrt(step)
        continue;
    end

    pc=[pc;newp];                                       %将新节点加入节点树
    parent = [parent;ind];                              %设置新节点的前驱

    line([pc(ind,) pc(parent(ind),)],[pc(ind,) pc(parent(ind),)]);
end

line([pc(ind,) p(,)],[pc(ind,) p(,)],'color','r');
ind = length(pc);
while ind~=
    ind = parent(ind);                                  %不断搜索当前节点的父节点
    line([pc(ind,) pc(parent(ind),)],[pc(ind,) pc(parent(ind),)],'color','r');
end

check_obs.m：

function isobs = check_obs(img,p1,p2)
[h w]=size(img);
d = norm(p1-p2);
direct = atan2(p1()-p2(),p1()-p2());
sin_dir = sin(direct);
cos_dir = cos(direct);
for r=:d
    p = floor(p2 + r*[sin_dir cos_dir]);

    y = p();
    x = p();
    if y>= && y<=h && x>= && x<=w
        if img(y,x) ==
            isobs = ;
            return;
        end
    end
end
isobs = ;
end

结果如下：

原图：

算法结果：