从零开始的OMPL库算法学习（2）RRT-connect算法

从零开始的OMPL库算法学习（2）RRT-connect算法

简介

RRT-connect算法是基于RRT算法的一种算法，它从起始点和终点同时生长两棵快速扩展随机树来搜索状态空间，效率会更高。其伪代码如图所示：

每一次迭代中，开始步骤与原始的RRT算法一样，都是采样随机点然后进行扩展。然后扩展完第一棵树的新节点𝑞𝑛𝑒𝑤后，以这个新的目标点作为第二棵树扩展的方向。同时第二棵树扩展的方式略有不同（15~22行），首先它会扩展第一步得到𝑞′𝑛𝑒𝑤，如果没有碰撞，继续往相同的方向扩展第二步，直到扩展失败或者𝑞′𝑛𝑒𝑤=𝑞𝑛𝑒𝑤表示与第一棵树相连了，即connect了，整个算法结束。每次选择较小的那棵树进行拓展，搜索速度、搜索效率有了显著提高。