rrt-connect算法路径规划

rrt-connect算法是一种基于随机区域树(Rapidly-exploring Random Trees, RRT)的路径规划算法。它通过在自由空间中随机采样，并使用启发式搜索来构建两个树，一个从起点开始，另一个从终点开始。两个树在探索过程中逐渐连接，直到找到一条连接两个树的路径。

RRT-Connect算法的基本步骤如下：
1. 初始化两个树，一个包含起点，另一个包含终点。
2. 在自由空间中随机采样一个点，并在两个树中找到最近的节点。
3. 从最近节点向采样点生成一条新的路径段。
4. 检查路径段是否与障碍物相交，如果相交则丢弃，否则将路径段添加到相应的树中。
5. 交替更新两个树，重复步骤2到4，直到两个树相连接。
6. 如果两个树相连接，则找到一条连接起点和终点的路径。

RRT-Connect算法的优点是快速且容易实现，适用于高维空间和复杂环境。但它可能无法找到最优路径，并且在存在狭窄通道或高度局限的空间中表现不佳。