RRT路径规划算法

博客转载自：http://www.cnblogs.com/21207-iHome/p/7210543.html

传统的路径规划算法有人工势场法、模糊规则法、遗传算法、神经网络、模拟退火算法、蚁群优化算法等。但这些方法都需要在一个确定的空间内对障碍物进行建模，计算复杂度与机器人自由度呈指数关系，不适合解决多自由度机器人在复杂环境中的规划。基于快速扩展随机树（RRT / rapidly exploring random tree）的路径规划算法，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径，适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。与PRM类似，该方法是概率完备且不最优的。

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 ）。可以看到，同样是随机树，但是这棵树并没很好地探索空间。

根据上面的伪代码，可以用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为R²×S¹.虚线框分别代表三种不同的机器人构型：第一个构型绕其旋转了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 q_nearest to some point q_newwhich is closer to q_rand. The planner should be simple and it should run quickly.

A straight-line planner. The plan is a straight line to q_new, which may be chosen at q_rand or at a fixed distance d from q_nearest on the straight line to q_rand. 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