RRT路径规划算法

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

　　

　　RRT是一种多维空间中有效率的规划方法。它以一个初始点作为根节点，通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中找到一条由从初始点到目标点的路径。基本RRT算法如下面伪代码所示：

 function BuildRRT(qinit, K, Δq)
     T.init(qinit)
     for k =  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(, 1.0)
     if  < 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_new which 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算法一样，都是采样随机点然后进行扩展。然后扩展完第一棵树的新节点