路径规划——RRT、RRT*、RRT-Connect算法说明

1. RRT 伪代码

RRT 的目标是在状态空间中随机采样并扩展树结构，直到找到连接起点和终点的路径。

RRT（Rapidly-exploring Random Tree，快速随机扩展树）是一种用于解决运动规划问题的采样算法。它通过随机采样的方式，逐步构建一棵树，直到找到一条从起始状态到目标状态的路径。RRT算法的主要步骤包括：

1. 初始化：选择一个起始点作为树的根节点。
2. 采样：在状态空间中随机采样一个点。
3. 寻找最近节点：在已有的树中找到离采样点最近的节点。
4. 扩展：从最近节点向采样点方向扩展一定距离，形成一个新的节点，如果这个新节点不与障碍物碰撞，就将其加入到树中。
5. 连接：检查新节点是否与目标点足够接近，如果是，则认为找到了一条路径。
6. 迭代：重复上述过程，直到找到路径或达到迭代次数限制。

初始化树T，起始节点xstart
while (未达到迭代次数且未找到目标) {
    xrand = 随机采样状态空间中的点；
    xnear = T中离xrand最近的节点；
    xnew = 从xnear向xrand方向扩展得到的新节点；
    if (xnew与障碍物没有碰撞) {
        将xnew添加到树T中；
        if (xnew接近目标) {
            返回从xstart到xnew的路径；
        }
    }
}

2. RRT* 伪代码

RRT* 是对 RRT 的优化，它在构建树的过程中尝试寻找最优路径。 每次添加新节点时，它会重新检查并优化节点间的连接，以保证路径是最短的。可以理解为将原本可能比较迂回的路径改为更加直接和顺畅的路径。

1. 初始化：选择一个起始点作为树的根节点，并设置一些算法参数，如最大迭代次数、路径优化的阈值等。

2. 迭代过程：重复以下步骤，直到达到最大迭代次数或找到目标点： a. 在状态空间中随机采样一个点。 b. 找到树中离采样点最近的节点。 c. 尝试从最近节点向采样点扩展，生成一个新的节点。 d. 如果新节点与障碍物发生碰撞，则丢弃该节点并返回步骤a。 e. 如果新节点有效，将其添加到树中。

3. 路径优化：对于新添加的节点，检查树中是否存在其他节点可以通过连接到新节点来获得更优的路径。如果存在，更新这些节点的父节点为新节点，从而优化路径。

4. 目标检查：每次添加新节点后，检查该节点是否接近目标点。如果足够接近，则认为找到了一条可行的路径。

5. 终止条件：当达到最大迭代次数或找到一条接近目标的路径时，算法终止。

初始化树T，起始节点xstart
while (未达到迭代次数且未找到目标) {
    xrand = 随机采样状态空间中的点；
    xnear = T中离xrand最近的节点；
    xnew = 从xnear向xrand方向扩展得到的新节点；
    if (xnew与障碍物没有碰撞) {
        将xnew添加到树T中；
        执行重连操作以优化树T；
        if (xnew接近目标) {
            返回从xstart到xnew的路径；
        }
    }
}

3. RRT-Connect 伪代码

RRT-Connect 是对 RRT 的一种改进，通过同时扩展两棵树（从起点和终点分别扩展），并尝试连接这两棵树，从而加速路径的找到过程。

1. 初始化：创建两个树，一个以起始点为根，另一个以目标点为根。
2. 采样：为两棵树分别随机采样点。
3. 扩展：从每棵树中最近的节点向采样点方向扩展新节点。
4. 碰撞检测：检查新节点是否与障碍物碰撞。
5. 添加节点：如果新节点有效，则添加到相应的树中。
6. 连接检查：如果两棵树的节点彼此接近到一定距离，认为两棵树已经连接，算法终止。
7. 迭代：重复步骤2-6，直到两棵树连接或达到迭代次数。

初始化起始点树T1，起始节点xstart，目标点树T2，目标节点xgoal
while (未达到迭代次数且两棵树未连接) {
    xrand1 = 随机采样状态空间中的点；
    xnear1 = T1中离xrand1最近的节点；
    xnew1 = 从xnear1向xrand1方向扩展得到的新节点；
    if (xnew1与障碍物没有碰撞) {
        将xnew1添加到树T1中；
    }
    xrand2 = 随机采样状态空间中的点；
    xnear2 = T2中离xrand2最近的节点；
    xnew2 = 从xnear2向xrand2方向扩展得到的新节点；
    if (xnew2与障碍物没有碰撞) {
        将xnew2添加到树T2中；
    }
    if (xnew1和xnew2足够接近) {
        返回连接两棵树的路径；
    }
}

总结

• RRT 是一种快速的路径规划方法，通过随机采样不断扩展树，适用于复杂环境中的初步路径搜索。
• RRT* 是 RRT 的优化版本，强调寻找最优路径，在每次扩展时对路径进行修正和优化。
• RRT-Connect 通过双树结构加速路径找到过程，是效率更高的版本，尤其适用于目标点比较远的情况。