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

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*就是这么优化的，最后一步从终点反推起点生成路径