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(:,1),p(:,2),'r.');

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

while norm(pc(end,:)-p(2,:))>step           %搜索到距离结束节点一定距离停止
    
    if rand()<0.3                           %按30%概率随机搜索，70%概率朝着结束位置搜索
        nextp = [rand()*h rand()*w];
    else
        nextp = p(2,:);
    end
    
    diff = repmat(nextp,length(pc(:,1)),1)-pc;          %计算节点树与待搜索节点距离
    [~,ind] = min(sqrt(diff(:,1).^2+diff(:,2).^2));     %找到距离带搜索节点最小的节点树节点
    
    direct = atan2(nextp(1)-pc(ind,1),nextp(2)-pc(ind,2));
    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==1                                         %有障碍物重新搜索
        continue;
    end
     
    diff = repmat(newp,length(pc(:,1)),1)-pc;           %判断该路径是否已搜索过，如果已搜索过，则重新搜索
    if min(sqrt(diff(:,1).^2+diff(:,2).^2))<sqrt(step)
        continue;
    end
    
    pc=[pc;newp];                                       %将新节点加入节点树
    parent = [parent;ind];                              %设置新节点的前驱
    
    line([pc(ind,1) pc(parent(ind),1)],[pc(ind,2) pc(parent(ind),2)]);
end

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

check_obs.m：

function isobs = check_obs(img,p1,p2)  
[h w]=size(img);
d = norm(p1-p2);
direct = atan2(p1(1)-p2(1),p1(2)-p2(2));
sin_dir = sin(direct);
cos_dir = cos(direct);
for r=0:d
    p = floor(p2 + r*[sin_dir cos_dir]);
    
    y = p(2);
    x = p(1);
    if y>=1 && y<=h && x>=1 && x<=w      
        if img(y,x) ==0
            isobs = 1;
            return;
        end
    end
end
isobs = 0;
end

结果如下：

原图：

算法结果：