RRT-基于采样的约束运动规划的引入

基于采样的运动规划算法是一种高效的运动规划方法，其放弃了在高维状态中通过网格搜索产生的分辨率上的完整性和最优解，实现高维状态空间中快速找到可行解的能力。在机器人研究和应用中主要有两种基础算法：一个是单次查询的快速探索随机树（Rapidly exploring Random Tree, RRT）和概率路线图（probabilistic roadmap, PRM）。本篇文章主要是为了介绍快速随机搜索随机树的流程和MATLAB代码实现，为基于采样的约束运动规划提供理论和代码基础。

参考：

参考文献：

[1] K. M. Lynch, F. C. Park. Modern robotics. Massachusetts: Cambridge University Press, 2017
[2] Z. Kingston, M. Moll, L. E. Kavraki. Sampling-Based Methods for Motion Planning with Constraints. Annual Review of Control, Robotics, and Autonomous Systems. 2018, 1(1): 159-185

参考代码：

https://github.com/Mesywang/Motion-Planning-Algorithms
https://github.com/Nirucool/Robotics-Motion-Planning
https://petercorke.com/toolboxes/robotics-toolbox/#Downloading_the_Toolbox

RRT

RRT算法的伪代码如下：

Algorithm 1 RRT algorithm.
1: initialize search tree T with qstart
2: while T is less than the maximum tree size do
3:   qsamp ←sample from Q
4:   qnearest ←nearest node in T to qsamp
5:   employ a local planner to find a motion from qnearest to qnew in
the direction of qsamp
6:   if the motion is collision-free then
7:     add qnew to T with an edge from qnearest to qnew
8:     if ||qnew - qgoal||<ε then
9:        return SUCCESS and the motion to qnew
10:    end if
11:  end if
12: end while
13: return FAILURE

根据伪代码可知对于RRT算法来说，其主要分为如下模块：生成新样本点的采样器，用以确定最近点的距离度量方式；局部规划器用以进行碰撞检测和生成新的节点。

采样器

最常见的采样器是在整个C空间进行随机采样，随着时间的增长，这一采样器理论上能够覆盖整个采样空间，这也为RRT方法提供了概率上的完整性；然而这种随机采样方法，会使得采样效率比较低，搜索产生目标路径所需的节点数量较多。
为了提高采样效率，可以使得采样器以一定的概率采样到目标位形。另一种可行的方案是利用示教学习的方法学习到可行轨迹和最优轨迹所在的采样子空间。如下图，为两种不同采样方法在同一场景和相同参数下的采样点分布和最终路径。

度量方法

度量方法用于确定搜索树中距离采样点中的最近点，通常所采样空间中的欧式距离已经能够解决大部分问题。

局部规划器

局部规划器用于生成新的节点并判断是否存在碰撞，因此往往在不同的应用场景需要使用不同的碰撞算法；而路线插值方法直接使用直线方法。

主要场景和代码

本文以及后续文章主要使用的是参考链接中的场景—二维路径规划和机器人路径规划场景。二维路径规划中的黑色图块为障碍物，搜索空间为整个图像的空间；机器人规划场景中机器人模型采用的是圆柱体模型，由机器人工具包生成，球形物体为障碍，位置和半径均是随机生成。

代码

ImpRgb = imread('map.png');
Imp = rgb2gray(ImpRgb);
imshow(Imp);
xL = size(Imp,1);   % 地图x轴长度
yL = size(Imp,2);   % 地图y轴长度
space=[0 xL;0 yL;];
规划参数 
x_I = 1; y_I = 1;           % 设置初始点
x_G = 750; y_G = 750;       % 设置目标点
Thr = 30;                   % 设置目标点阈值
Delta = 40;                 % 设置扩展步长
hold on
plot(x_I, y_I, 'mo', 'MarkerSize',10, 'MarkerFaceColor','m');
plot(x_G, y_G, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');   % 绘制起点和目标点
树初始化
tree.v(1).configuration=[x_I;y_I];
tree.v(1).father=1;
tree.v(1).distance=0;
开始构建搜索树
count=1;
for iter=1:3000
    %step1 随机采样
    q_rand=sampler('goal',space,[x_G,y_G],0.1);
    
    %step2 寻找树中的最近点
    for j=1:size(tree.v,2)
       dis(j)=dismetric(q_rand,tree.v(j).configuration) ;
    end
    [minDis,minIndex]=min(dis);
    q_near=tree.v(minIndex).configuration;
    
    %step3 生成新的节点x_new
    q_new=extend_dis(q_near,q_rand,Delta);
        
    %局部规划器与碰撞检测
    if ~collisionChecking(q_near,q_new,Imp)
        continue;   % 有障碍物
    end
    count = count+1;
    
    %将目标点添加到搜索树上
    tree.v(count).configuration=q_new;
    tree.v(count).father=minIndex;
    tree.v(count).distance=Delta+tree.v(minIndex).distance;
    
    %step4 是否到达目标点
    dis2goal=norm([x_G;y_G]-q_new);
    if(dis2goal < Thr)
        count=count+1;
        goalIndex=count;
        tree.v(count).configuration=[x_G;y_G];
        tree.v(count).father=count-1;
        tree.v(count).distance=dis2goal+tree.v(count-1).distance;
        break
    end

    %绘图
    plot([q_near(1), q_new(1)], [q_near(2), q_new(2)], 'b', 'Linewidth', 2);
    plot(q_new(1), q_new(2), 'ko', 'MarkerSize', 4, 'MarkerFaceColor','k');
%     pause(0.02);     % 暂停0.02s，使得RRT扩展过程容易观察
end


路径查询
if iter < 2000
    path=FindTreePath(tree,goalIndex);
    for j = 2:size(path,1)
        plot([path(j,1); path(j-1,1)], [path(j,2); path(j-1,2)], 'g', 'Linewidth', 4);
    end
else
    disp('Error, no path found!');
end

其中寻找可行路径的代码为

configuration=tree.v(goalIndex).configuration;
path(1,:)=configuration(:)';
j=2;
pathIndex=tree.v(goalIndex).father;
while 1
    configuration=tree.v(pathIndex).configuration;
    path(j,:)=configuration(:)';
    pathIndex=tree.v(pathIndex).father;
    if pathIndex == 1
        break
    end
    j=j+1;
end
path(j+1,:)=tree.v(1).configuration;