informed-rrt*算法原理

Informed RRT*是一种基于快速随机树（RRT）算法的路径规划算法，它利用启发式信息来指导树的生长，从而使得搜索效率更高，找到最优解的概率更大。

具体来说，Informed RRT*算法的步骤如下：

1. 初始化一棵RRT树，包含起点并将其设置为根节点。
2. 重复以下步骤直到找到一条从起点到终点的路径或达到最大迭代次数：
a. 从搜索树中随机采样一个点。
b. 使用启发式函数计算该点与终点之间的距离，并将该距离作为阈值。
c. 在搜索树中寻找最近邻点，该点与随机采样点之间的距离小于阈值，且从该点向随机采样点的直线路径上没有障碍物。
d. 计算从最近邻点到随机采样点的路径代价，并将其加入到搜索树中。
e. 优化搜索树，删除代价较高的路径，将代价较低的路径保留并合并。
3. 如果达到最大迭代次数，返回最优路径。

Informed RRT*算法的启发式函数可以根据实际问题进行设计，例如使用欧几里得距离或曼哈顿距离等。通过使用启发式函数，Informed RRT*算法可以更快地找到最优解，并且在搜索空间较大时表现更优。