这个算法分为两个阶段：
第一阶段：将Xnew用最小的成本连接到树上。
选取Xnew的方法与RRT是相同的，然后以Xnew为圆心，选取半径r画圆。得到的圆形区域内的所有点，都选入集合Znear。在上图中，这些点都用绿色表示,叫做Xnear。然后，迭代计算所有Xnew->Xnear+Xnear->Xinit的成本，选最小的那个Xnear与Xnew链接。到此第一阶段结束。
第二阶段：检查通过Xnew到达集合Znear的其他点的成本是否比原来更小，是的话，链接新线。
能看到上图，Xnew向另外两个Xnear连虚线。从Xinit经过Xnew到达Xnear，如果比从Xinit到Xnear的成本更低，就采用新边链接，删除旧边。
相关文章：