RRT路徑規劃算法
傳統的路徑規劃算法有人工勢場法、模糊規則法、遺傳算法、神經網絡、模擬退火算法、蟻羣優化算法等。但這些方法都須要在一個肯定的空間內對障礙物進行建模，計算複雜度與機器人自由度呈指數關係，不適合解決多自由度機器人在複雜環境中的規劃。基於快速擴展隨機樹（RRT / rapidly exploring random tree）的路徑規劃算法，經過對狀態空間中的採樣點進行碰撞檢測，避免了對空間的建模，可以有效地解決高維空間和複雜約束的路徑規劃問題。該方法的特色是可以快速有效地搜索高維空間，經過狀態空間的隨機採樣點，把搜索導向空白區域，從而尋找到一條從起始點到目標點的規劃路徑，適合解決多自由度機器人在複雜環境下和動態環境中的路徑規劃。與PRM相似，該方法是機率完備且不最優的。php
RRT是一種多維空間中有效率的規劃方法。它以一個初始點做爲根節點，經過隨機採樣增長葉子節點的方式，生成一個隨機擴展樹，當隨機樹中的葉子節點包含了目標點或進入了目標區域，即可以在隨機樹中找到一條由從初始點到目標點的路徑。基本RRT算法以下面僞代碼所示：html
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時，隨機樹朝一個隨機方向生長。node
上述算法的效果是隨機採樣點會「拉着」樹向外生長，這樣能更快、更有效地探索空間（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算法搜索出一條無碰撞路徑：spring
%% 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文件：express
%% 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-空間中包含大量障礙物或狹窄通道約束時，算法的收斂速度慢，效率會大幅降低：api
RRT 的一個弱點是難以在有狹窄通道的環境找到路徑。由於狹窄通道面積小，被碰到的機率低。下圖展現的例子是 RRT 應對一我的爲製做的很短的狹窄通道，有時RRT很快就找到了出路，有時則一直被困在障礙物裏面：網絡
上述基礎RRT算法中有幾處能夠改進的地方：app
- 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改進算法被提出。less
-
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
- 1. RRT路徑規劃算法（matlab實現）
- 2. ROS(indigo)RRT路徑規劃
- 3. octomap中3d-rrt路徑規劃
- 4. 【規劃】RRT*算法圖解
- 5. 關於RRT路徑規劃算法的改進？
- 6. 基於採樣的路徑規劃算法——RRT
- 7. 無人駕駛路徑規劃之RRT算法
- 8. A*算法-路徑規劃
- 9. 路徑規劃算法之Djistra算法
- 10. 【規劃】RRT Planer
- 更多相關文章...
- • Eclipse Java 構建路徑 - Eclipse 教程
- • XML 語法規則 - XML 教程
- • 算法總結-廣度優先算法
- • 算法總結-深度優先算法
-
每一个你不满意的现在，都有一个你没有努力的曾经。
- 1. RRT路徑規劃算法（matlab實現）
- 2. ROS(indigo)RRT路徑規劃
- 3. octomap中3d-rrt路徑規劃
- 4. 【規劃】RRT*算法圖解
- 5. 關於RRT路徑規劃算法的改進？
- 6. 基於採樣的路徑規劃算法——RRT
- 7. 無人駕駛路徑規劃之RRT算法
- 8. A*算法-路徑規劃
- 9. 路徑規劃算法之Djistra算法
- 10. 【規劃】RRT Planer