### 基于Informed采样的RRT*算法解释 #### Informed RRT*的核心概念 传统的快速扩展随机树（RRT*）通过不断改进路径来逼近全局最优解。然而，在早期阶段，它可能会浪费大量时间探索与当前最佳路径无关的空间区域。为了提高效率，引入了 **Informed Sampling** 的策略[^1]。 在Informed RRT*中，仅当样本位于由当前已知的最佳路径定义的椭圆区域内时才被考虑。这个椭圆形区域是由起点 \(q_{start}\) 和终点 \(q_{goal}\) 定义的一个约束空间，其边界表示可能存在的更优路径的最大范围[^2]。 #### 数学描述 假设目前找到的最短路径长度为 \(c_{best}\)，则可以构建一个以 \(q_{start}\) 和 \(q_{goal}\) 为中心、半径等于 \(c_{best}/2\) 的椭圆。任何新的节点如果能够缩短该距离，则必然落在此椭圆内部。因此，采样过程会优先关注这一特定子集内的候选点，从而显著减少不必要的计算开销[^3]。 ```python import numpy as np def informed_sample(start, goal, c_best): """ Generate a sample within the informed set defined by start and goal. :param start: Start node position (numpy array) :param goal: Goal node position (numpy array) :param c_best: Current best path cost :return: A new sampled point inside the ellipse region (numpy array) """ theta = np.random.uniform(0, 2 * np.pi) r = c_best / 2 # Ellipse parametric equation delta = goal - start center = (start + goal) / 2 axis_a = r axis_b = np.sqrt(r**2 - np.linalg.norm(delta)**2/4) x_new = center + axis_a * np.cos(theta) * delta / np.linalg.norm(delta) \ + axis_b * np.sin(theta) * np.array([-delta[1], delta[0]]) / np.linalg.norm(delta) return x_new ``` #### 实现细节 除了上述核心逻辑外，完整的Informed RRT*还需要继承标准RRT*的功能框架，包括但不限于： - 随机选取状态并尝试连接到最近邻居； - 计算局部代价更新整体估计值； - 对新加入顶点重新规划父级关系以便进一步优化现有结构[^4]。 以上方法不仅保留了渐近收敛至全局最小的特点，而且大幅提升了实际运行性能表现。 ---