RRT算法原理介绍：RRT搜索树与树的生长相类似，即不断生长的同时又向四周扩散。算法以路径起点Xstart作为随机树T的根节点，树中节点xj用集合V存储，节点间的连接用连接边集E存储，所有节点xj满足属于集合Xfree的条件。路径规划中，从空闲区域选取随机节点来引导树的生长方向，随机节点数依照具体情况而定，直到到达目标点才完成整个搜索，标出根节点与目标节点的连接线。
RRT算法伪代码如下：
Build_RRT(xstart, xfinish, K)

1. T.start(xstart);
2. for k = 1 to K do
3. Xrand<-RANDOM();
4. Xnear<-NEAREST(Xrand, T);
5. if OBS_NOT_FREE(Xnew, Xrand)
6. continue;
7. Xnew<-NEW_START(Xnew, Xrand, u);
8. V<-T.add_vertex(Xnew);
9. E<-T.add_edge(Xnear, Xnew);
10. if dis[Xnew, xfinish]<u
11. Return T;
    图1 RRT算法扩展图

算法在第1-2步处进行初始化，设置最大迭代次数后进入循环。
第3-7步产生新节点，在空间区域中，随机节点Xrand属于集合RANDOM；NEAREST函数利用欧氏距离选择节点中距离Xrand最近的节点，产生Xnear节点。如果Xrand与Xnear连线上存在障碍物，那么判断之后就会停止当前循环，进入下一次循环。NEW_START函数从Xnear与Xrand两节点的连线方向上扩展固定步长u，从而得到新节点Xnew。节点扩展过程如图1所示。
第8-9步将产生的新节点以及新产生的边添加至原随机树。
第10-11步判断条件，若新节点与目标节点之间的欧式距离小于固定步长或者新节点已经到达最终目标节点，则返回随机树T；另一方面，若未达到目标节点但达到了最大迭代次数K，此时算法会终止，连接到目标节点。
随机树确定后，标记目标点返回，循环遍历父节点，探索至树的根节点。此时，算法从随机树中找到一条连接根节点和目标节点且无碰撞障碍物的可行路径。

MATLAB仿真：

Time = 33.240944 s
Path Length = 1.8452e+03