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

結果如下：

原圖：

算法結果：