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)。 - 检查是否能改进路径:
- 快速筛选:
g_t[v] + 欧氏距离(v,w) + h_hat(w) < c_max。 - 严格验证:
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函数回溯的最优路径(从目标节点沿父节点追溯到起点)。 - 达到最大迭代次数或采样点数,未找到路径则返回失败。