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

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

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

初始化：选择一个起始点作为树的根节点。
采样：在状态空间中随机采样一个点。
寻找最近节点：在已有的树中找到离采样点最近的节点。
扩展：从最近节点向采样点方向扩展一定距离，形成一个新的节点，如果这个新节点不与障碍物碰撞，就将其加入到树中。
连接：检查新节点是否与目标点足够接近，如果是，则认为找到了一条路径。
迭代：重复上述过程，直到找到路径或达到迭代次数限制。
初始化树T，起始节点xstart
while (未达到迭代次数且未找到目标) {
    xrand = 随机采样状态空间中的点；
    xnear = T中离xrand最近的节点；
    xnew = 从xnear向xrand方向扩展得到的新节点；
    if (xnew与障碍物没有碰撞) {
        将xnew添加到树T中；
        if (xnew接近目标) {
            返回从xstart到xnew的路径；
        }
    }
}

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

初始化：选择一个起始点作为