傳統的路徑規划算法有人工勢場法、模糊規則法、遺傳算法、神經網絡、模擬退火算法、蟻群優化算法等。但這些方法都需要在一個確定的空間內對障礙物進行建模，計算復雜度與機器人自由度呈指數關系，不適合解決多自由度機器人在復雜環境中的規划。基於快速擴展隨機樹（RRT / rapidly exploring random tree）的路徑規划算法，通過對狀態空間中的采樣點進行碰撞檢測，避免了對空間的建模，能夠有效地解決高維空間和復雜約束的路徑規划問題。該方法的特點是能夠快速有效地搜索高維空間，通過狀態空間的隨機采樣點，把搜索導向空白區域，從而尋找到一條從起始點到目標點的規划路徑，適合解決多自由度機器人在復雜環境下和動態環境中的路徑規划。與PRM類似，該方法是概率完備且不最優的。
RRT是一種多維空間中有效率的規划方法。它以一個初始點作為根節點，通過隨機采樣增加葉子節點的方式，生成一個隨機擴展樹，當隨機樹中的葉子節點包含了目標點或進入了目標區域，便可以在隨機樹中找到一條由從初始點到目標點的路徑。基本RRT算法如下面偽代碼所示：
1 function BuildRRT(qinit, K, Δq) 2 T.init(qinit) 3 for k = 1 to K 4 qrand = Sample() -- chooses a random configuration 5 qnearest = Nearest(T, qrand) -- selects the node in the RRT tree that is closest to qrand 6 if Distance(qnearest, qgoal) < Threshold then 7 return true 8 qnew = Extend(qnearest, qrand, Δq) -- moving from qnearest an incremental distance in the direction of qrand 9 if qnew ≠ NULL then 10 T.AddNode(qnew) 11 return false 12 13 14 function Sample() -- Alternatively,one could replace Sample with SampleFree(by using a collision detection algorithm to reject samples in C_obstacle 15 p = Random(0, 1.0) 16 if 0 < p < Prob then 17 return qgoal 18 elseif Prob < p < 1.0 then 19 return RandomNode()
初始化時隨機樹T只包含一個節點：根節點qinit。首先Sample函數從狀態空間中隨機選擇一個采樣點qrand（4行）；然后Nearest函數從隨機樹中選擇一個距離qrand最近的節點qnearest（5行）；最后Extend函數通過從qnearest向qrand擴展一段距離，得到一個新的節點qnew（8行）。如果qnew與障礙物發生碰撞，則Extend函數返回空，放棄這次生長，否則將qnew加入到隨機樹中。重復上述步驟直到qnearest和目標點qgaol距離小於一個閾值，則代表隨機樹到達了目標點，算法返回成功（6~7行）。為了使算法可控，可以設定運行時間上限或搜索次數上限（3行）。如果在限制次數內無法到達目標點，則算法返回失敗。
為了加快隨機樹到達目標點的速度，簡單的改進方法是：在隨機樹每次的生長過程中，根據隨機概率來決定qrand是目標點還是隨機點。在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 ）。可以看到，同樣是隨機樹，但是這棵樹並沒很好地探索空間。
根據上面的偽代碼，可以用MATLAB實現一個簡單的RRT路徑規划（參考這里）。輸入一幅像素尺寸為500×500的地圖，使用RRT算法搜索出一條無碰撞路徑：
%% RRT parameters map=im2bw(imread('map1.bmp')); % input map read from a bmp file. for new maps write the file name here source=[10 10]; % source position in Y, X format goal=[490 490]; % 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','k'); line(path(:,2),path(:,1));
其它M文件：
%% 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
RRT算法也有一些缺點，它是一種純粹的隨機搜索算法對環境類型不敏感，當C-空間中包含大量障礙物或狹窄通道約束時，算法的收斂速度慢，效率會大幅下降：
RRT 的一個弱點是難以在有狹窄通道的環境找到路徑。因為狹窄通道面積小，被碰到的概率低。下圖展示的例子是 RRT 應對一個人為制作的很短的狹窄通道，有時RRT很快就找到了出路，有時則一直被困在障礙物里面：
上述基礎RRT算法中有幾處可以改進的地方：
- how to sample from C-Space (line 4). 如何進行隨機采樣
- how to define the “nearest” node in T (line 5). 如何定義“最近”點
- how to plan the motion to make progress toward sample (line 8). 如何進行樹的擴展
Even a small change to the sampling method, for example, can yield a dramatic change in the running time of the planner. A wide variety of planners have been proposed in the literature based on these choices and other variations. 根據以上幾個方向，多種RRT改進算法被提出。
-
Defining the Nearest Node
查找最近點的基礎是計算C-Space中兩點間的距離。計算距離最直觀的方法是使用歐氏距離，但對很多C-Space來說這樣做的直觀意義並不明顯。Finding the “nearest” node depends on a definition of distance. A natural choice for the distance between two points is simply the Euclidean distance. For other spaces, the choice is less obvious. 舉個例子，如下圖所示，對於一個car-like robot來說其C-space為R2×S1. 虛線框分別代表三種不同的機器人構型：第一個構型繞其旋轉了20°，第二個在它后方2米處，最后一個在側方位1米處。那么哪一種距離灰色的目標“最近”呢？汽車型機器人的運動約束導致其不能直接進行橫向運動和原地轉動。因此，對於這種機器人來說從第二種構型移動到目標位置“最近”。
從上面的例子可以看出來，定義一個距離需要考慮以下兩點：
- combining components of different units (e.g., degrees, meters, degrees/s,meters/s) into a single distance measure;
- taking into account the motion constraints of the robot
結合不同單位的一個簡單辦法是使用加權平均計算距離，不同分量的重要程度用權值大小表示（The weights express the relative importance of the different components）。尋找最近點在計算機科學和機器人學等領域中是一個非常普遍的問題，已經有各種用於加速計算的方法，比如K-d樹、hash算法等。
-
The Sampler
The reason that the tree ends up covering the entire search space (in most cases) is because of the combination of the sampling strategy, and always looking to connect from the nearest point in the tree. The choice of where to place the next vertex that you will attempt to connect to is the sampling problem. In simple cases, where search is low dimensional, uniform random placement (or uniform random placement biased toward the goal) works adequately. In high dimensional problems, or when motions are very complex (when joints have positions, velocities and accelerations), or configuration is difficult to control, sampling strategies for RRTs are still an open research area.
-
The Local Planner
The job of the local planner is to find a motion from qnearest to some point qnew which is closer to qrand. The planner should be simple and it should run quickly.
- A straight-line planner. The plan is a straight line to qnew, which may be chosen at qrand or at a fixed distance d from qnearest on the straight line to qrand. This is suitable for kinematic systems with no motion constraints.
Bidirectional RRT / RRT Connect
基本的RRT每次搜索都只有從初始狀態點生長的快速擴展隨機樹來搜索整個狀態空間，如果從初始狀態點和目標狀態點同時生長兩棵快速擴展隨機樹來搜索狀態空間，效率會更高。為此，基於雙向擴展平衡的連結型雙樹RRT算法，即RRT_Connect算法被提出。
該算法與原始RRT相比，在目標點區域建立第二棵樹進行擴展。每一次迭代中，開始步驟與原始的RRT算法一樣，都是采樣隨機點然后進行擴展。然后擴展完第一棵樹的新節點𝑞𝑛𝑒𝑤后，以這個新的目標點作為第二棵樹擴展的方向。同時第二棵樹擴展的方式略有不同（15~22行），首先它會擴展第一步得到𝑞′𝑛𝑒𝑤，如果沒有碰撞，繼續往相同的方向擴展第二步，直到擴展失敗或者𝑞′𝑛𝑒𝑤=𝑞𝑛𝑒𝑤表示與第一棵樹相連了，即connect了，整個算法結束。當然每次迭代中必須考慮兩棵樹的平衡性，即兩棵樹的節點數的多少（也可以考慮兩棵樹總共花費的路徑長度），交換次序選擇“小”的那棵樹進行擴展。這種雙向的RRT技術具有良好的搜索特性，比原始RRT算法的搜索速度、搜索效率有了顯著提高，被廣泛應用。首先，Connect算法較之前的算法在擴展的步長上更長，使得樹的生長更快；其次，兩棵樹不斷朝向對方交替擴展，而不是采用隨機擴展的方式，特別當起始位姿和目標位姿處於約束區域時，兩棵樹可以通過朝向對方快速擴展而逃離各自的約束區域。這種帶有啟發性的擴展使得樹的擴展更加貪婪和明確，使得雙樹RRT算法較之單樹RRT算法更加有效。
參考這里可以用MATLAB實現一個簡單的RRT Connect路徑規划：
RRT_Connect.m
%%%%% parameters
map=im2bw(imread('map2.bmp')); % input map read from a bmp file. for new maps write the file name here
source=[10 10]; % source position in Y, X format
goal=[490 490]; % goal position in Y, X format
stepsize=20; % size of each step of the RRT
disTh=20; % nodes closer than this threshold are taken as almost the same
maxFailedAttempts = 10000;
display=true; % display of RRT
tic;
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
RRTree1 = double([source -1]); % First RRT rooted at the source, representation node and parent index
RRTree2 = double([goal -1]); % Second RRT rooted at the goal, representation node and parent index
counter=0;
tree1ExpansionFail = false; % sets to true if expansion after set number of attempts fails
tree2ExpansionFail = false; % sets to true if expansion after set number of attempts fails
while ~tree1ExpansionFail || ~tree2ExpansionFail % loop to grow RRTs
if ~tree1ExpansionFail
[RRTree1,pathFound,tree1ExpansionFail] = rrtExtend(RRTree1,RRTree2,goal,stepsize,maxFailedAttempts,disTh,map); % RRT 1 expands from source towards goal
if ~tree1ExpansionFail && isempty(pathFound) && display
line([RRTree1(end,2);RRTree1(RRTree1(end,3),2)],[RRTree1(end,1);RRTree1(RRTree1(end,3),1)],'color','b');
counter=counter+1;M(counter)=getframe;
end
end
if ~tree2ExpansionFail
[RRTree2,pathFound,tree2ExpansionFail] = rrtExtend(RRTree2,RRTree1,source,stepsize,maxFailedAttempts,disTh,map); % RRT 2 expands from goal towards source
if ~isempty(pathFound), pathFound(3:4)=pathFound(4:-1:3); end % path found
if ~tree2ExpansionFail && isempty(pathFound) && display
line([RRTree2(end,2);RRTree2(RRTree2(end,3),2)],[RRTree2(end,1);RRTree2(RRTree2(end,3),1)],'color','r');
counter=counter+1;M(counter)=getframe;
end
end
if ~isempty(pathFound) % path found
if display
line([RRTree1(pathFound(1,3),2);pathFound(1,2);RRTree2(pathFound(1,4),2)],[RRTree1(pathFound(1,3),1);pathFound(1,1);RRTree2(pathFound(1,4),1)],'color','green');
counter=counter+1;M(counter)=getframe;
end
path=[pathFound(1,1:2)]; % compute path
prev=pathFound(1,3); % add nodes from RRT 1 first
while prev > 0
path=[RRTree1(prev,1:2);path];
prev=RRTree1(prev,3);
end
prev=pathFound(1,4); % then add nodes from RRT 2
while prev > 0
path=[path;RRTree2(prev,1:2)];
prev=RRTree2(prev,3);
end
break;
end
end
if display
disp('click/press any key');
waitforbuttonpress;
end
if size(pathFound,1)<=0, error('no path found. maximum attempts reached'); end
pathLength=0;
for i=1:length(path)-1, pathLength=pathLength+distanceCost(path(i,1:2),path(i+1,1:2)); end
fprintf('processing time=%d \nPath Length=%d \n\n', toc,pathLength);
imshow(map);
rectangle('position',[1 1 size(map)-1],'edgecolor','k');
line(path(:,2),path(:,1));
rrtExtend.m
function [RRTree1,pathFound,extendFail] = rrtExtend(RRTree1,RRTree2,goal,stepsize,maxFailedAttempts,disTh,map)
pathFound=[]; %if path found, returns new node connecting the two trees, index of the nodes in the two trees connected
failedAttempts=0;
while failedAttempts <= maxFailedAttempts
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
[A, I] = min( distanceCost(RRTree1(:,1:2),sample) ,[],1); % find the minimum value of each column
closestNode = RRTree1(I(1),:);
%% 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
[A, I2] = min( distanceCost(RRTree2(:,1:2),newPoint) ,[],1); % find closest in the second tree
if distanceCost(RRTree2(I2(1),1:2),newPoint) < disTh, % if both trees are connected
pathFound=[newPoint I(1) I2(1)];extendFail=false;break;
end
[A, I3] = min( distanceCost(RRTree1(:,1:2),newPoint) ,[],1); % check if new node is not already pre-existing in the tree
if distanceCost(newPoint,RRTree1(I3(1),1:2)) < disTh, failedAttempts=failedAttempts+1;continue; end
RRTree1 = [RRTree1;newPoint I(1)];extendFail=false;break; % add node
end
其它M文件：
%% 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
參考：
Rapidly-exploring Random Trees (RRTs)
Code for Robot Path Planning using Rapidly-exploring Random Trees
Sampling-based Algorithms for Optimal Motion Planning
馮林，賈菁輝. 基於對比優化的RRT路徑規划改進算法.計算機工程與應用
The open online robotics education resource
Rapidly Exploring Random Tree (RRT) Path Planning
Implementing Rapidly exploring Random Tree (RRT) OpenRave Plugin on A 7-DOF PR2 Robot Arm