Hybrid A* 算法的详细解释

Hybrid A*（H A*）算法是一种结合了传统 A* 算法和车辆运动学约束的路径规划方法，专门用于处理具有非完整约束（如车辆的转弯半径）的路径规划问题。它在自动驾驶车辆和移动机器人路径规划中表现出色，能够生成符合车辆动力学的平滑路径。

1. 算法原理

Hybrid A* 算法的核心思想是将连续状态空间离散化，同时保持车辆运动学约束。与传统 A* 算法的主要区别在于：

1. 状态表示：

   • 传统 A*：离散网格坐标（x, y）。
   • Hybrid A*：连续状态（x, y, θ），其中 θ 表示航向角。

2. 状态转移：

   • 传统 A*：八个方向的简单移动。
   • Hybrid A*：考虑车辆运动学约束的实际可行路径。

3. 节点信息：

   • 每个节点存储的信息包括位置坐标（x, y）、航向角（yaw）、转向角（steer）、运动方向（前进/后退）、路径代价和父节点索引。

2. 状态空间设计

Hybrid A* 使用以下参数离散化状态空间：

XY_GRID_RESOLUTION = 2.0  # 空间分辨率 [m]
YAW_GRID_RESOLUTION = np.deg2rad(15.0)  # 航向角分辨率 [rad]
MOTION_RESOLUTION = 0.1  # 路径插值分辨率 [m]
N_STEER = 20  # 转向角采样数

3. 代价函数设计

代价函数是算法性能的关键，包含多个组成部分：

1. 基础移动代价：

   arc_l = XY_GRID_RESOLUTION * 1.5  # 基础路径长度
   cost = current.cost + added_cost + arc_l  # 总代价计算
   

2. 特殊动作惩罚：

   SB_COST = 100.0  # 换向惩罚成本
   BACK_COST = 5.0  # 后退惩罚成本
   STEER_CHANGE_COST = 5.0  # 转向角变化惩罚成本
   STEER_COST = 1.0  # 转向角惩罚成本
   

3. 启发式代价：

   H_COST = 5.0  # 启发式成本权重
   

4. 车辆模型

Hybrid A* 采用简化的自行车模型来表示车辆运动学特性：

class Car:
    WB = 3.0  # 轴距 [m]
    W = 2.0   # 车宽 [m]
    LF = 3.3  # 前悬长度 [m]
    LB = 1.0  # 后悬长度 [m]
    MAX_STEER = 0.6  # 最大转向角 [rad]
    BUBBLE_R = np.hypot((LF + LB) / 2.0, W / 2.0)  # 碰撞检测半径

5. 核心算法实现

5.1 节点扩展

节点扩展是算法的核心部分，主要包括：

1. 转向角采样：

   def calc_motion_inputs():
       for steer in np.concatenate((np.linspace(-MAX_STEER, MAX_STEER, N_STEER), [0.0])):
           for d in [1, -1]:  # 前进和后退
               yield [steer, d]
   

2. 下一状态计算：

   def calc_next_node(current, steer, direction, config, ox, oy, kd_tree):
       x, y, yaw = current.x_list[-1], current.y_list[-1], current.yaw_list[-1]
       arc_l = XY_GRID_RESOLUTION * 1.5
       x_list, y_list, yaw_list = [], [], []
   
       # 使用运动学方程计算路径点
       for _ in np.arange(0, arc_l, MOTION_RESOLUTION):
           x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer)
           x_list.append(x)
           y_list.append(y)
           yaw_list.append(yaw)
   

5.2 碰撞检测

Hybrid A* 实现了两层碰撞检测机制：

1. 快速检测：使用圆形包络进行初步筛选。
2. 精确检测：使用矩形模型进行精确碰撞检测。

5.3 启发式函数设计

Hybrid A* 使用动态规划预计算启发式值，提高搜索效率：

def calc_distance_heuristic(gx, gy, ox, oy, resolution, rr):
    goal_node = Node(round(gx / resolution), round(gy / resolution), 0.0, -1)
    motion = get_motion_model()

    # 使用优先队列进行动态规划搜索
    open_set, closed_set = dict(), dict()
    priority_queue = [(0, calc_index(goal_node, x_w, min_x, min_y))]

6. 完整搜索流程

6.1 主函数实现

def hybrid_a_star_planning(start, goal, ox, oy, xy_resolution, yaw_resolution):
    # 初始化配置
    config = Config(ox, oy, xy_resolution, yaw_resolution)

    # 构建 KD 树用于快速碰撞检测
    obstacle_kd_tree = cKDTree(np.vstack((ox, oy)).T)

    # 计算启发式表格
    heuristic_table = calc_distance_heuristic(
        goal[0], goal[1], ox, oy, xy_resolution, BUBBLE_R)

6.2 搜索过程

主要包含以下步骤：

1. 初始化开启列表：

   open_set = {}
   closed_set = {}
   
   # 起始节点
   start_node = Node(round(start[0] / xy_resolution),
                     round(start[1] / xy_resolution),
                     round(start[2] / yaw_resolution), True,
                     [start[0]], [start[1]], [start[2]], [True],
                     0.0, -1, 0.0)
   
   heapq.heappush(open_queue,
                  (calc_cost(start_node, heuristic_table),
                   calc_index(start_node, config)))
   

2. 主循环搜索：

   while True:
       if not open_queue:
           return None  # 搜索失败
   
       # 获取代价最小的节点
       _, current_id = heapq.heappop(open_queue)
       current = open_set[current_id]
   
       # 判断是否到达目标
       if is_goal(current, goal):
           return get_final_path(closed_set, current)
   
       # 节点扩展
       for neighbor in get_neighbors(current, config, ox, oy, obstacle_kd_tree):
           # 计算代价并更新开启列表
           if is_valid_node(neighbor, closed_set):
               open_set[neighbor_id] = neighbor
               heapq.heappush(open_queue,
                             (calc_cost(neighbor, heuristic_table),
                              calc_index(neighbor, config)))
   

7. 优点与局限性

优点

1. 考虑运动学约束：Hybrid A* 考虑了车辆的实际运动特性，生成的路径更符合车辆动力学。
2. 高效搜索：通过动态规划预计算启发式值，提高了搜索效率。
3. 平滑路径：使用 Reeds-Shepp 曲线生成平滑路径。

局限性

1. 控制离散化：车辆的控制输入是离散化的，与实际车辆的连续控制特性存在差异。
2. 节点剪枝：基于“代价到目前为止”的节点剪枝可能导致次优解。

8. 总结

Hybrid A* 算法通过结合 A* 的高效搜索能力和车辆运动学约束，适用于具有运动学约束的路径规划问题。通过合理设置分辨率参数、启发式函数权重和运动学参数，可以优化路径规划的效率和精度。