【笔记：3D航路规划算法】二、RRT*

3D路径规划是在三维空间中寻找从起点到终点的最短或最优路径的一种技术。它广泛应用于无人机导航、机器人运动规划、虚拟现实等领域

一、RRT*于RRT的不同之处

1、路径优化：

• RRT：仅寻找一条从起点到终点的路径，不保证是最优路径。
• RRT*：在寻找路径的过程中，不断优化路径，通过重连线步骤逐步逼近最优路径。

2、成本计算：

• RRT：不进行成本计算，路径成本可能较高。
• RRT*：每次添加新节点后，计算路径成本，通过重连线步骤降低路径成本。

3、重连线步骤：

• RRT：无重连线步骤。
• RRT*：每次添加新节点后，检查附近节点并尝试通过新节点来重新连接，以优化路径。

二、图解

1、初始化

在初始化上，比RRT多了一个参数rewire_count
该参数使得在每次添加新节点到树中后，需要检查并可能重新连接的附近节点的数量

2、路径搜索

可以看到这是在某一次循环时，生成的随机点x_new

假设rewire_count为3，这样就找到了3个点，分别为near_1、near_2、near_3

接下来获取x_new到near_1、near_2、near_3的成本

成本计算时是从起点位置到near_*位置的边长，加上蓝色虚线的距离，这就得到了3条路径的成本值，以其中near_2举例说明

得到3条路径后，连接最小成本得路径，这里很明显是near_1到x_new路径最短

开始将虚线⚪中的路径进行重连线

for _, near in round:
    near_cost = path_cost(self.trees[tree].E, self.z_start, near)
    new_cost = path_cost(self.trees[tree].E, self.z_start, x_new) + segment_cost(x_new, near)
    if new_cost < near_cost and self.X.collision_free(near, x_new, self.r):
        self.trees[tree].E[near] = x_new

还是循环这3个点，

• near_cost = 计算从起点start到这near的路径长度
• new_cost = 计算起点start到x_new路径长度+ x_new到near 路径长度
• 当new_cost < near_cost时，同时x_new到near 的路径没有和障碍物相交
  • 将near的父节点设置为x_new

x_new到near_1、near_2、near_3，三条路线中，near_2明显与障碍物相交，所以不考虑，就只剩这两条路线了

其中橘黄色路线明显不符合new_cost < near_cost，因为near_cost是在new_cost的基础上增加了x_new到near_1
粉红色的路线符合new_cost < near_cost，很明显粉色长度大于最下面那一根橙色线长度，并且没有经过障碍物
所以将near_3的父节点变为x_new

最终就得到了

相比于之前，路径成本更低了，RRT*就是这么优化的，最后一步从终点反推起点生成路径

3、效果展示

可以看到比RRT更加丝滑
3D效果展示