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

结果如下：

原图：

算法结果：

巴特西

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

最新文章

热门文章