BIT*算法 - 技术栈

BIT*（Batch Informed Trees）算法是一种基于采样的最优路径规划算法，结合了 批量采样 、椭圆约束 和 启发式剪枝 策略，能高效找到起点到目标的最优路径。其核心流程可分为 初始化 、批量采样与扩展 、路径验证与更新 、剪枝优化 四个阶段，具体步骤如下：

一、初始化阶段

参数与数据结构初始化
- 定义起点（start）和目标（goal）节点，计算起点到目标的直线距离 c_min（理论最短路径下界）。
- 初始化路径树（Tree），包含：
  - vertices：已验证的有效顶点集合（初始仅含起点 start）。
  - edges：已验证的有效边集合（初始为空）。
  - queue_vertices：待验证顶点队列（初始含起点 start）。
  - queue_edges：待验证边队列（初始为空）。
- 初始化代价字典 g_t：记录每个节点的实际路径代价（起点代价为 0，目标及其他节点初始为 np.inf）。
- 初始化采样集 x_sample：存储所有候选采样点（初始可包含起点）。
初始路径判断
- 检查起点到目标的直线路径是否无碰撞（path_through_obstacle）。若直接可达，则目标代价 g_t[goal] 设为该直线距离，算法可提前终止。

二、批量采样与扩展阶段

循环执行 "批量采样→生成待验证边→筛选有效边"，直到找到目标路径或满足终止条件：

批量采样（椭圆约束内）
- 根据当前已知的最优路径代价 c_max（初始为 np.inf，找到路径后更新为实际代价），在 椭圆区域 内采样 num_samples 个点（默认 100 个）。
  - 椭圆定义：以起点和目标为焦点，长轴 a = c_max/2，短轴 b = √(a² - (c_min/2)²)，确保采样点集中在可能改进路径的区域。
- 采样过程：通过 "单位圆采样→缩放→旋转→平移" 生成椭圆内的点（rand），封装为 Node 对象，加入采样集 x_sample。
从待验证顶点生成边
- 从 queue_vertices 中选择 代价最小的顶点 v （通过 best_in_queue_vertex，代价为 g_hat(v) + h_hat(v)）。
- 对采样集 x_sample 中的每个点 w，生成边 (v, w)，并通过以下条件筛选后加入 queue_edges：
  - 边 (v, w) 未在 edges 中（去重）。
  - 理论总代价（g_hat(v) + 欧氏距离(v,w) + h_hat(w)）< c_max（有全局改进潜力）。
  - 局部代价（g_t[v] + 欧氏距离(v,w)）< w 的当前代价（有局部优势）。

三、路径验证与更新阶段

验证待验证边
- 从 queue_edges 中选择 代价最小的边 (v, w) （通过 best_queue_edge_value，代价为 g_t[v] + 欧氏距离(v,w) + h_hat(w)）。
- 验证边的有效性：
  - 检查 v 到 w 的路径是否无碰撞（path_through_obstacle）。
  - 检查是否能改进路径：
    1. 快速筛选：g_t[v] + 欧氏距离(v,w) + h_hat(w) < c_max。
    2. 严格验证：g_hat(v) + 实际代价(v,w) + h_hat(w) < c_max（实际代价含碰撞检测）。
更新路径树与代价
- 若边 (v, w) 有效，且通过 v 到达 w 的代价（g_t[v] + 实际代价(v,w)）小于 w 的当前代价 g_t[w]：
  - 更新 g_t[w] = g_t[v] + 实际代价(v,w)，设置 w 的父节点为 v。
  - 将 w 加入 vertices，边 (v, w) 加入 edges。
  - 若 w 是目标节点，更新 c_max = g_t[w]（当前最优路径代价）。
- 将 w 加入 queue_vertices（作为新的待扩展顶点）。

四、剪枝优化阶段

每次找到更优路径（c_max 减小）后，执行剪枝（prune 函数），移除冗余元素：

从 x_sample、vertices、edges 中删除所有 总启发式代价 ≥ c_max 的节点和边（它们不可能改进当前最优路径）。
将代价为 np.inf（不可达）的顶点从 vertices 移回 x_sample（留待后续可能利用）。

五、终止条件

找到目标节点且验证路径有效，返回通过 backtrack 函数回溯的最优路径（从目标节点沿父节点追溯到起点）。
达到最大迭代次数或采样点数，未找到路径则返回失败。