Hybrid A*算法与A*算法伪代码差异及车载机器人应用问询

标准A*算法伪代码 #

function A*(start, goal):
    open_set = priority_queue() // 按f值排序的优先队列
    open_set.add(start, f=h(start))
    came_from = dict() // 存储节点父节点，用于路径回溯
    g_score = dict() // 存储从起点到当前节点的实际代价
    g_score[start] = 0
    f_score = dict()
    f_score[start] = h(start)

    while open_set is not empty:
        current = open_set.pop_min() // 取出f值最小的节点
        if current == goal:
            return reconstruct_path(came_from, current) // 回溯生成路径
        for neighbor in get_8_neighbors(current): // 取当前节点的8个网格邻接节点
            tentative_g = g_score[current] + distance(current, neighbor)
            if neighbor not in g_score or tentative_g < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g
                f_score[neighbor] = tentative_g + h(neighbor)
                if neighbor not in open_set:
                    open_set.add(neighbor, f_score[neighbor])
    return failure // 无可行路径

Hybrid A* 算法伪代码（适配非完整约束运动体） #

function Hybrid_A*(start, goal):
    open_set = priority_queue() // 按f值排序的优先队列
    open_set.add(start, f=h(start))
    came_from = dict()
    g_score = dict()
    g_score[start] = 0
    f_score = dict()
    f_score[start] = h(start)
    motion_primitives = get_car_motion_primitives() // 小车预定义运动基元：前进、左转、右转等，符合最小转弯半径约束

    while open_set is not empty:
        current = open_set.pop_min()
        // 目标判断需同时满足位置+姿态误差在阈值内，符合小车停车要求
        if distance(current, goal) < pos_threshold and abs(current.theta - goal.theta) < theta_threshold:
            return reconstruct_path(came_from, current)
        // 不是取网格邻节点，而是基于运动学模型生成符合小车运动约束的后继节点
        for motion in motion_primitives:
            next_node = apply_motion(current, motion) // 基于当前位姿(x,y,theta)和运动基元计算下一位姿
            if next_node is out of map or collide_with_obstacle(next_node):
                continue
            // 网格降采样：同一网格内的节点只保留g值最小的，避免状态空间爆炸
            grid_idx = get_grid_index(next_node.x, next_node.y)
            tentative_g = g_score[current] + motion.cost
            if grid_idx not in g_score or tentative_g < g_score[grid_idx]:
                came_from[next_node] = current
                g_score[grid_idx] = tentative_g
                // 启发式函数一般用两个：不考虑约束的欧氏距离+考虑非完整约束的Reeds-Shepp距离，取最大值保证最优性
                f_score[next_node] = tentative_g + max(h_euclidean(next_node, goal), h_reeds_shepp(next_node, goal))
                if next_node not in open_set:
                    open_set.add(next_node, f_score[next_node])
    return failure

二者伪代码核心差异点 #

• 节点定义差异：
  A的节点仅包含二维网格坐标(x,y)，Hybrid A的节点包含三维位姿(x,y,theta)，theta为小车当前朝向，适配运动学约束
• 后继节点生成逻辑差异：
  A直接取当前节点的4邻接/8邻接网格节点，无任何运动约束，所以容易生成90度转角路径；Hybrid A基于小车运动基元/运动学模型生成后继节点，所有节点天然符合最小转弯半径要求，不会出现不可执行的转角
• 目标判断逻辑差异：
  A仅判断坐标是否匹配目标点；Hybrid A需要同时判断位置误差和朝向误差都在允许阈值内，满足车载机器人停车的姿态要求
• 状态空间优化逻辑差异：
  A每个网格仅对应一个节点，无需额外降采样；Hybrid A的三维状态空间过大，会新增网格降采样逻辑，同一个网格内只保留g值最小的节点，避免搜索爆炸
• 启发式函数差异：
  A一般只用欧氏距离/曼哈顿距离作为启发式；Hybrid A会同时采用无约束启发式和考虑非完整约束的Reeds-Shepp/Dubins距离作为启发式，保证搜索效率同时符合小车运动特性
• 路径输出差异：
  A输出的是离散网格点序列，需要后续做平滑处理才能给小车执行；Hybrid A输出的路径本身就是符合运动约束的连续轨迹，可直接给底层控制执行

内容的提问来源于stack exchange，提问作者Zheng Xian