Hybrid A*算法详解

1. 引言

Hybrid A算法是一种用于自动驾驶车辆路径规划的高效算法，它巧妙地结合了传统A算法的离散搜索特性和连续空间中的运动学约束。本文将从理论到实践，深入剖析Hybrid A*算法的工作原理和实现细节。

2. 算法原理

2.1 基本概念

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

1. 状态表示：

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

2. 状态转移：

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

3. 节点信息：每个节点存储的信息包括：

   • 位置坐标(x, y)
   • 航向角(yaw)
   • 转向角(steer)
   • 运动方向(前进/后退)
   • 路径代价
   • 父节点索引

2.2 状态空间设计

class Node:
    def __init__(self, x_ind, y_ind, yaw_ind, direction,
                 x_list, y_list, yaw_list, directions,
                 steer=0.0, parent_index=None, cost=None):
        self.x_index = x_ind
        self.y_index = y_ind
        self.yaw_index = yaw_ind
        self.direction = direction
        self.x_list = x_list
        self.y_list = y_list
        self.yaw_list = yaw_list
        self.directions = directions
        self.steer = steer
        self.parent_index = parent_index
        self.cost = cost

状态空间的离散化参数：

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

2.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  # 启发式成本权重

3. 车辆模型

3.1 自行车模型

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

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)  # 碰撞检测半径

3.2 运动学方程

车辆状态转移方程：

def move(x, y, yaw, distance, steer, L=WB):
    x += distance * cos(yaw)
    y += distance * sin(yaw)
    yaw = pi_2_pi(yaw + distance * tan(steer) / L)
    return x, y, yaw

4. 核心算法实现

4.1 节点扩展

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

1. 转向角采样：