傳統的路徑規劃算法有人工勢場法、模糊規則法、遺傳算法、神經網絡、模拟退火算法、蟻群優化算法等。但這些方法都需要在一個确定的空間内對障礙物進行模組化，計算複雜度與機器人自由度呈指數關系，不适合解決多自由度機器人在複雜環境中的規劃。基于快速擴充随機樹（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 View Code
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 %% 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的運動規劃算法綜述
路徑規劃算法初步認識
PRM路徑規劃算法
V-rep學習筆記：機器人路徑規劃
基于Mathematica的機器人仿真環境（機械臂篇）
馮林，賈菁輝. 基于對比優化的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
Robotics Toolbox