RRT 算法原理以及过程演示

RRT 适用于涉及非完整约束场合下的路径规划问题。
RRT 算法为一种递增式的构造方法，在构造过程中，算法不断在搜索空间中随机生成状态点，如果该点位于无碰撞位置，则寻找搜索树中离该节点最近的结点为基准结点，由基准结点出发以一定步长朝着该随机结点进行延伸，延伸线的终点所在的位置被当做有效结点加入搜索树中。这个搜索树的生长过程一直持续，直到目标结点与搜索树的距离在一定范围以内时终止。随后搜索算法在搜索树中寻找一条连接起点到终点的最短路径。

下面用一个示例来说明RRT算法的过程。

1. 初始化一个环境，包括地图，起点，终点。如下图所示,黑色物体为障碍物，蓝色飞机位于起点位置，红色五角星为目标终点位置。
   

2. 从环境中随机采样状态点，如下图所示，采样点为 Xrand。
   

3. 从所构建的树中寻找距离采样点 Xrand最近的结点 Xnear。现在树中只有起点一个结点，所有最近的结点就是起点。

   

4. 开始树的生长过程。首先连接 Xnear 和 Xrand连接起来，这个连接线的方向就是树生长的方向。设置一个步长 Stepsize作为树一次生长的步长，在树生长的这个方向上生长一个步长，然后就会在生长的末端会产生一个新的结点 Xnew。
   

5. 判断从 Xnear 到 Xrand是否穿过障碍物，如果穿过，则放弃该新的结点，如果没有，则将 Xnew 结点加入到树中。
   

6. 从步骤 2 开始再循环执行，从环境中随机采样状态点。
   

7. 从树中寻找距离 Xrand 结点最近的结点为 Xnew。

8. 开始书的生长过程。首在 Xnear 和 Xrand的连接方向上生长一个步长到新的一个结点 Xnew。
   

9. 判断从 Xnear 到 Xrand是否穿过障碍物，如果穿过，则放弃该新的结点，如果没有，则将 Xnew 结点加入到树中。如上图所示，穿过了障碍物，所以放弃该新的结点。
   

10. 重复上述树的生长过程，直到树新生成的结点到目标点的距离小于一个步长，则终止树的生长。直接将该新结点与目标点相连。

    

整个过程图如下（中间过程有些跳动，不必再在意）：

算法流程图总结如下：

如果对您有帮助，记得在下方点赞哟！也欢迎在评论区留言讨论！