[matlab] 7.快速搜索隨機樹（RRT---Rapidly-exploring Random Trees) 路徑規划

RRT是一種多維空間中有效率的規划方法。它以一個初始點作為根節點，通過隨機采樣增加葉子節點的方式，生成一個隨機擴展樹，當隨機樹中的葉子節點包含了目標點或進入了目標區域，便可以在隨機樹中找到一條由從初始點到目標點的路徑。RRT方法是概率完備且不最優的。

function BuildRRT(qinit, K, Δq)
    T.init(qinit)
    for k = 1 to K
        qrand = Sample()  -- chooses a random configuration
        qnearest = Nearest(T, qrand) -- selects the node in the RRT tree that is closest to qrand
        if  Distance(qnearest, qgoal) < Threshold then
            return true
        qnew = Extend(qnearest, qrand, Δq)  -- moving from qnearest an incremental distance in the direction of qrand
        if qnew ≠ NULL then
            T.AddNode(qnew)
    return false


function Sample() -- Alternatively,one could replace Sample with SampleFree(by using a collision detection algorithm to reject samples in C_obstacle
    p = Random(0, 1.0)
    if 0 < p < Prob then
        return qgoal
    elseif Prob < p < 1.0 then
        return RandomNode()

　　初始化時隨機樹T只包含一個節點：根節點q_init。首先Sample函數從狀態空間中隨機選擇一個采樣點q_rand（4行）；然后Nearest函數從隨機樹中選擇一個距離q_rand最近的節點q_nearest（5行）；最后Extend函數通過從q_nearest向q_rand擴展一段距離，得到一個新的節點q_new（8行）。如果q_new與障礙物發生碰撞，則Extend函數返回空，放棄這次生長，否則將q_new加入到隨機樹中。重復上述步驟直到q_nearest和目標點q_gaol距離小於一個閾值，則代表隨機樹到達了目標點，算法返回成功（6~7行）。為了使算法可控，可以設定運行時間上限或搜索次數上限（3行）。如果在限制次數內無法到達目標點，則算法返回失敗。
　　為了加快隨機樹到達目標點的速度，簡單的改進方法是：在隨機樹每次的生長過程中，根據隨機概率來決定q_rand是目標點還是隨機點。在Sample函數中設定參數Prob，每次得到一個0到1.0的隨機值p，當0<p<Prob的時候，隨機樹朝目標點生長行；當Prob<p<1.0時，隨機樹朝一個隨機方向生長。

　　上述算法的效果是隨機采樣點會“拉着”樹向外生長，這樣能更快、更有效地探索空間（The effect is that the nearly uniformly distributed samples “pull” the tree toward them, causing the tree to rapidly explore C-Space）。隨機探索也講究策略，如果我們從樹中隨機取一個點，然后向着隨機的方向生長，那么結果是什么樣的呢？見下圖（Left: A tree generated by applying a uniformly-distributed random motion from a randomly chosen tree node does not explore very far. Right: A tree generated by the RRT algorithm using samples drawn randomly from a uniform distribution. Both trees have 2000 nodes ）。可以看到，同樣是隨機樹，但是這棵樹並沒很好地探索空間。

 

 

下面是具體的代碼

%% RRT parameters
map=im2bw(imread('map.bmp')); % input map read from a bmp file. for new maps write the file name here
source=[490 490]; % source position in Y, X format
goal=[10 900]; % goal position in Y, X format
stepsize = 20;  % size of each step of the RRT
threshold = 20; % nodes closer than this threshold are taken as almost the same
maxFailedAttempts = 10000;
display = true; % display of RRT

if ~feasiblePoint(source,map), error('source lies on an obstacle or outside map'); end
if ~feasiblePoint(goal,map), error('goal lies on an obstacle or outside map'); end
if display,imshow(map);rectangle('position',[1 1 size(map)-1],'edgecolor','k'); end

tic;  % tic-toc: Functions for Elapsed Time

RRTree = double([source -1]); % RRT rooted at the source, representation node and parent index
failedAttempts = 0;
counter = 0;
pathFound = false;

while failedAttempts <= maxFailedAttempts  % loop to grow RRTs
%% chooses a random configuration
    if rand < 0.5
        sample = rand(1,2) .* size(map);   % random sample
    else
        sample = goal; % sample taken as goal to bias tree generation to goal
    end
    
%% selects the node in the RRT tree that is closest to qrand
    [A, I] = min( distanceCost(RRTree(:,1:2),sample) ,[],1); % find the minimum value of each column
    closestNode = RRTree(I(1),1:2);
    
%% moving from qnearest an incremental distance in the direction of qrand
    theta = atan2(sample(1)-closestNode(1),sample(2)-closestNode(2));  % direction to extend sample to produce new node
    newPoint = double(int32(closestNode(1:2) + stepsize * [sin(theta)  cos(theta)]));
    if ~checkPath(closestNode(1:2), newPoint, map) % if extension of closest node in tree to the new point is feasible
        failedAttempts = failedAttempts + 1;
        continue;
    end
    
    if distanceCost(newPoint,goal) < threshold, pathFound = true; break; end % goal reached
    
    [A, I2] = min( distanceCost(RRTree(:,1:2),newPoint) ,[],1); % check if new node is not already pre-existing in the tree
    if distanceCost(newPoint,RRTree(I2(1),1:2)) < threshold, failedAttempts = failedAttempts + 1; continue; end 
    
    RRTree = [RRTree; newPoint I(1)]; % add node
    failedAttempts = 0;
    if display, line([closestNode(2);newPoint(2)],[closestNode(1);newPoint(1)]);counter = counter + 1; M(counter) = getframe; end % Capture movie frame 
end

% getframe returns a movie frame, which is a structure having two fields
if display && pathFound, line([closestNode(2);goal(2)],[closestNode(1);goal(1)]); counter = counter+1;M(counter) = getframe; end

if display, disp('click/press any key'); waitforbuttonpress; end
if ~pathFound, error('no path found. maximum attempts reached'); end

%% retrieve path from parent information
path = [goal];
prev = I(1);
while prev > 0
    path = [RRTree(prev,1:2); path];
    prev = RRTree(prev,3);
end

pathLength = 0;
for i=1:length(path)-1, pathLength = pathLength + distanceCost(path(i,1:2),path(i+1,1:2)); end % calculate path length
fprintf('processing time=%d \nPath Length=%d \n\n', toc, pathLength); 
imshow(map);rectangle('position',[1 1 size(map)-1],'edgecolor','r');
line(path(:,2),path(:,1));

%% distanceCost.m
function h=distanceCost(a,b)
    h = sqrt(sum((a-b).^2, 2));

%% checkPath.m    
function feasible=checkPath(n,newPos,map)
feasible=true;
dir=atan2(newPos(1)-n(1),newPos(2)-n(2));
for r=0:0.5:sqrt(sum((n-newPos).^2))
    posCheck=n+r.*[sin(dir) cos(dir)];
    if ~(feasiblePoint(ceil(posCheck),map) && feasiblePoint(floor(posCheck),map) && ... 
            feasiblePoint([ceil(posCheck(1)) floor(posCheck(2))],map) && feasiblePoint([floor(posCheck(1)) ceil(posCheck(2))],map))
        feasible=false;break;
    end
    if ~feasiblePoint(newPos,map), feasible=false; end
end

%% feasiblePoint.m
function feasible=feasiblePoint(point,map)
feasible=true;
% check if collission-free spot and inside maps
if ~(point(1)>=1 && point(1)<=size(map,1) && point(2)>=1 && point(2)<=size(map,2) && map(point(1),point(2))==1)
    feasible=false;
end

 

結果如圖