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