Hybrid A* 算法文献解读

Hybrid A* 算法文献解读

文献标题 : Practical Search Techniques in Path Planning for Autonomous Driving
作者 : Dmitri Dolgov, Sebastian Thrun, Michael Montemerlo, James Diebel
机构 : Stanford University, Toyota Research Institute
发表会议 : AAAI 2008
应用场景: 2007 DARPA Urban Challenge（斯坦福赛车队机器人 Junior）

---

目录

1. 研究背景与问题定义
2. 关键科学问题与技术挑战
3. 研究方法与技术路线
4. 算法原理详解
5. 实验设计与验证
6. 主要创新点与学术贡献
7. 技术局限性与后续发展
8. 与项目经历的关联

---

1. 研究背景与问题定义

1.1 研究动机

本文研究动机源于2007年DARPA Urban Challenge自动驾驶挑战赛。在该赛事中，参赛车辆需要在真实城市环境中自主导航，包括：

• 停车场自由导航
• U型掉头
• 拥堵道路处理
• 交叉口通行

斯坦福赛车队的机器人 Junior（图1）需要一种能够在未知环境中实时规划可行路径的算法，且规划路径必须满足车辆的运动学约束。

1.2 核心问题

如何在保证运动学可行性的前提下，实现复杂场景下的实时路径规划？

具体而言，需要同时满足以下要求：

• 运动学可行性：路径必须满足车辆非完整性约束（最大曲率、前后行驶）
• 实时性：全周期重规划时间需在 50-300ms 内
• 避障能力：能够在线感知并避开动态障碍物
• 路径质量：路径应接近最优，且平滑可执行

---

2. 关键科学问题与技术挑战

2.1 连续空间搜索的复杂性

传统路径规划算法面临的核心矛盾：

算法类型	优点	缺点
*离散搜索（A）**	计算高效，理论最优性保证	路径为分段线性，不满足车辆运动学约束
连续采样（RRT）	概率完备，满足运动学约束	收敛慢，路径质量差，实时性不足
数值优化	可处理多约束	易陷入局部最优，收敛速度难以保证

关键挑战：离散搜索的高效性与连续运动的可行性之间存在本质冲突。

2.2 非完整性约束的处理

车辆作为非完整系统，其运动受到以下约束：

• 最大曲率约束：受最小转弯半径限制
• 方向约束：不能原地旋转（差速驱动除外）
• 运动方向约束：支持前进和后退，但切换有代价

2.3 势场的局部极小值问题

传统势场法在狭窄通道中会产生高势区域，导致通道"有效不可通行"。如何在保持避障能力的同时，确保狭窄通道的可通过性，是另一个关键挑战。

---

3. 研究方法与技术路线

3.1 整体框架

本文提出了一种两阶段路径规划框架：

┌─────────────────────────────────────────────────────────────┐
│                     Hybrid A* 路径规划框架                    │
├─────────────────────────────────────────────────────────────┤
│  Phase 1: Hybrid-State A* 搜索                               │
│  ├── 3D运动学状态空间 (x, y, θ)                              │
│  ├── 改进的状态更新规则（连续状态→离散节点）                   │
│  ├── 双启发式函数引导                                        │
│  └── Reed-Shepp 解析扩展                                     │
├─────────────────────────────────────────────────────────────┤
│  Phase 2: 局部优化与平滑                                     │
│  ├── 共轭梯度（CG）非线性优化                                │
│  ├── Voronoi 势场代价函数                                    │
│  └── 非参数插值细化                                          │
└─────────────────────────────────────────────────────────────┘

3.2 技术路线特点

1. 离散与连续的结合：在离散网格中维护连续状态，兼顾搜索效率与运动学可行性
2. 启发式引导：设计两种互补的启发式函数，大幅剪枝搜索空间
3. 解析扩展：利用 Reed-Shepp 模型进行解析路径扩展，提升精度与速度
4. 后处理优化：通过数值优化进一步提升路径质量

---

4. 算法原理详解

4.1 Hybrid-State A* 搜索

4.1.1 核心思想

与传统 A* 的关键区别：

特性	传统 A*	Hybrid A*
状态表示	网格中心点 (x, y)	连续 3D 位姿 (x, y, θ)
节点扩展	移动到相邻网格中心	基于运动学模型模拟
路径特性	分段线性，不可驱动	连续可驱动
最优性	理论最优	不保证全局最优，但接近最优

关键创新：每个网格单元关联一个连续的车辆状态，而非仅记录网格中心点。

4.1.2 状态更新规则

def hybrid_a_star_search(start, goal, grid_map):
    """
    Hybrid A* 搜索算法核心逻辑
    """
    open_list = PriorityQueue()
    closed_list = {}  # 每个网格只保留最优连续状态
    
    # 初始化
    start_node = Node(state=start, g=0, h=heuristic(start, goal))
    open_list.push(start_node)
    
    while not open_list.empty():
        current = open_list.pop()  # 取 f = g + h 最小的节点
        
        if is_goal_reached(current.state, goal):
            return reconstruct_path(current)
        
        # 网格剪枝：同一网格只保留代价最小的节点
        cell = discretize(current.state)
        if cell in closed_list and closed_list[cell].g <= current.g:
            continue
        closed_list[cell] = current
        
        # 节点扩展：基于运动学模型
        for steer in [-max_steer, 0, max_steer]:
            for direction in [1, -1]:  # 前进/后退
                new_state = kinematic_model(
                    current.state, 
                    steer=steer, 
                    direction=direction,
                    dt=dt
                )
                
                if collision_check(new_state, grid_map):
                    continue
                
                # 代价计算
                g = current.g + step_cost(direction)
                h = max(
                    non_holonomic_heuristic(new_state, goal),
                    holonomic_heuristic(new_state, goal)
                )
                
                new_node = Node(
                    state=new_state,
                    g=g,
                    h=h,
                    parent=current,
                    direction=direction
                )
                open_list.push(new_node)
    
    return None  # 未找到路径

4.1.3 运动学模型

采用简化的自行车模型进行状态传播：

x' = x + v * cos(θ) * dt
y' = y + v * sin(θ) * dt
θ' = θ + (v / L) * tan(φ) * dt

其中：

• (x, y, θ)：车辆位姿
• v：线速度（前进/后退）
• φ：前轮转向角
• L：轴距

4.1.4 双启发式函数设计

启发式 1：Non-Holonomic-Without-Obstacles

• 思想：忽略障碍物，但考虑车辆非完整性约束
• 实现：预计算从目标点到周围各状态的最短路径（使用 Reeds-Shepp 曲线）
• 作用：剪除以错误朝向接近目标的搜索分支
• 效果：相比欧氏距离，节点扩展数降低约一个数量级

启发式 2：Holonomic-With-Obstacles

• 思想：忽略车辆非完整性约束，但考虑障碍物
• 实现：在 2D 网格上运行动态规划，计算到目标的最短距离
• 作用：发现 U 型障碍物和死胡同，引导 3D 搜索避开这些区域

最终启发式：取两者最大值

h(n) = max(h_non_holonomic(n), h_holonomic(n))

两种启发式均为可采纳的（admissible），因此最大值也是可采纳的。

4.1.5 Reed-Shepp 解析扩展

问题：离散控制空间无法精确到达目标状态。

解决方案：定期进行解析扩展

def analytic_expansion(current_state, goal, obstacle_map, expansion_prob):
    """
    Reed-Shepp 解析扩展
    """
    # 以一定概率进行解析扩展（距离目标越近概率越高）
    if random() > expansion_prob:
        return None
    
    # 计算 Reed-Shepp 最优路径（假设无障碍物）
    rs_path = reeds_shepp_path(current_state, goal)
    
    # 碰撞检测
    if is_collision_free(rs_path, obstacle_map):
        return rs_path
    
    return None

Reed-Shepp 路径类型：

• LSL, RSR, LSR, RSL（含直线段）
• LRL, RLR（纯转弯，适用于短距离）

效果：显著提升搜索速度和路径精度。

4.2 Voronoi 势场代价函数

4.2.1 传统势场的问题

传统势场定义：

ρ(x,y) = α / (α + d_O(x,y))

问题 ：在狭窄通道中，两侧障碍物距离 d_O 都很小，导致势场值很高，通道"有效关闭"。

4.2.2 Voronoi 势场设计

本文提出的 Voronoi 势场：

ρ_V(x,y) = (α / (α + d_O(x,y))) * ((d_O(x,y) - d_max)^2 / d_max^2) * (d_V(x,y) / (d_O(x,y) + d_V(x,y)))

其中：

• d_O：到最近障碍物的距离
• d_V：到最近 Voronoi 图边的距离
• d_max：势场最大有效范围
• α：控制衰减率

关键特性：

1. 自适应缩放：势场值与通道宽度成比例，狭窄通道仍保持可通过
2. 零势通道：Voronoi 图边（通道中心线）上势场值为零
3. 有界性 ：ρ_V ∈ [0, 1]，且在障碍物内部达到最大值

def voronoi_field(x, y, obstacle_map, voronoi_diagram):
    """
    计算 Voronoi 势场值
    """
    d_O = distance_to_nearest_obstacle(x, y, obstacle_map)
    
    if d_O >= d_max:
        return 0.0
    
    d_V = distance_to_voronoi_edge(x, y, voronoi_diagram)
    
    # 避免除零
    if d_O + d_V == 0:
        return 1.0
    
    rho = (alpha / (alpha + d_O)) * \
          ((d_O - d_max)**2 / d_max**2) * \
          (d_V / (d_O + d_V))
    
    return rho

4.3 局部优化与平滑

4.3.1 共轭梯度优化

目标函数：

J = w_ρ * Σρ_V(x_i, y_i) +           # Voronoi 势场（避障）
    w_o * Σσ_o(|x_i - o_i| - d_max) +  # 碰撞惩罚
    w_κ * Σσ_κ(|κ_i| - κ_max) +        # 曲率约束
    w_s * Σ(Δφ_i)^2                     # 平滑性

其中：

• κ_i = Δφ_i / |Δx_i|：顶点 i 处的曲率
• Δφ_i = cos⁻¹(Δx_i^T Δx_{i+1} / (|Δx_i| |Δx_{i+1}|))：切向角变化
• σ_o, σ_κ：惩罚函数（通常取二次函数）

梯度计算：

对 Voronoi 势场项：

∂ρ_V/∂x_i = ∂ρ_V/∂d_O * ∂d_O/∂x_i + ∂ρ_V/∂d_V * ∂d_V/∂x_i

对曲率项（涉及三个连续顶点）：

∂κ_i/∂x_i     = -(1/|Δx_i|) * (∂Δφ_i/∂cos(Δφ_i)) * (∂cos(Δφ_i)/∂x_i)     - (Δφ_i/|Δx_i|²) * ∂|Δx_i|/∂x_i
∂κ_i/∂x_{i-1} = -(1/|Δx_i|) * (∂Δφ_i/∂cos(Δφ_i)) * (∂cos(Δφ_i)/∂x_{i-1}) - (Δφ_i/|Δx_i|²) * ∂|Δx_i|/∂x_{i-1}
∂κ_i/∂x_{i+1} = -(1/|Δx_i|) * (∂Δφ_i/∂cos(Δφ_i)) * (∂cos(Δφ_i)/∂x_{i+1})

4.3.2 非参数插值

问题：CG 优化后的路径顶点间距较大（0.5-1m），直接跟踪会导致方向盘剧烈抖动。

解决方案：非参数插值

def non_parametric_interpolation(path, num_subsamples):
    """
    非参数插值：超采样 + 固定原顶点优化
    """
    # Step 1: 在路径顶点间插入新顶点
    interpolated_path = []
    for i in range(len(path) - 1):
        interpolated_path.append(path[i])
        for j in range(1, num_subsamples):
            t = j / num_subsamples
            new_vertex = interpolate(path[i], path[i+1], t)
            interpolated_path.append(new_vertex)
    interpolated_path.append(path[-1])
    
    # Step 2: 共轭梯度优化（仅优化新顶点，固定原顶点）
    optimized_path = conjugate_gradient(
        interpolated_path,
        fixed_indices=range(0, len(interpolated_path), num_subsamples)
    )
    
    return optimized_path

优势：

• 避免参数化插值（如三次样条）的振荡问题
• 保持原路径拓扑结构
• 通过优化保证曲率连续性

---

5. 实验设计与验证

5.1 实验平台

• 机器人：Junior（斯坦福赛车队）
• 传感器：多激光雷达、雷达、高精度惯性测量系统
• 计算平台：现代 PC
• 环境：真实停车场、城市道路、仿真迷宫

5.2 参数设置

参数	取值
地图尺寸	160m × 160m
栅格分辨率	0.15m（障碍物地图）/ 0.5m（A* 搜索）
角度分辨率	5°
启发式预计算网格	160 × 160 × 72（x, y, θ）

5.3 实验结果

性能指标：

• 全周期重规划时间：50-300ms
• 场景覆盖：停车场导航、U型掉头、拥堵道路处理
• 实际表现：DARPA Urban Challenge 中零失误完成复杂路径规划任务

启发式函数效果对比：

启发式类型	节点扩展数	效果
2D 欧氏距离	21,515	基准
Non-Holonomic（无障碍物）	1,465	提升约 15 倍
Non-Holonomic（有障碍物）	68,730	复杂场景退化
双启发式结合	10,588	综合最优

---

6. 主要创新点与学术贡献

6.1 核心创新

创新点 1：Hybrid-State A* 搜索机制

贡献：首次提出在离散网格中维护连续状态的路径搜索方法。

意义：

• 解决了传统 A* 路径不可驱动的问题
• 保持了离散搜索的计算效率
• 为后续运动学约束路径规划提供了新范式

创新点 2：双启发式函数设计

贡献：设计了两种互补的启发式函数，并证明其可采纳性。

意义：

• Non-Holonomic 启发式剪除错误朝向分支
• Holonomic 启发式避开障碍物死胡同
• 两者结合实现近一个数量级的效率提升

创新点 3：Voronoi 势场

贡献：提出基于通道几何自适应缩放的势场函数。

意义：

• 解决了传统势场在狭窄通道中的"关闭"问题
• 保证狭窄通道的可通过性
• 为势场法在复杂环境中的应用提供了新思路

创新点 4：两阶段优化框架

贡献：搜索 + 优化的两阶段框架，兼顾实时性与路径质量。

意义：

• 第一阶段快速获得可行解
• 第二阶段通过数值优化逼近最优
• 非参数插值保证路径平滑可执行

6.2 学术影响

Hybrid A* 算法已成为自动驾驶领域开放空间路径规划的经典方法，被广泛应用于：

• 自动泊车系统
• 低速避障场景
• 复杂机动任务（U型掉头、倒车入库）

后续改进方向包括：

• DL-IAPS：分段路径平滑算法
• PJSO：速度规划优化
• OBACA：基于优化的轨迹规划

---

7. 技术局限性与后续发展

7.1 局限性

1. 计算复杂度：在高维状态空间或密集障碍物场景中，搜索时间可能超过实时要求
2. 不保证全局最优：由于网格剪枝，可能丢失全局最优解
3. 参数敏感：栅格分辨率、转向角离散化等参数需要精细调参
4. 高维扩展性：扩展到更高维状态空间（如包含速度、加速度）较为困难

7.2 后续发展

发展方向	代表工作	改进点
平滑优化	DL-IAPS	分段平滑，保证曲率约束和无碰撞
速度规划	PJSO	在路径基础上优化速度曲线
采样优化	BIT*	结合批量采样与启发式搜索
学习型	Neural A*	使用神经网络学习启发式函数
时空规划	ST-A*	在时空联合空间中规划，处理动态障碍物

---

8. 与项目经历的关联

8.1 与 Kinodynamic Lazy Theta* 项目的关联

在 [项目经历：Kinodynamic Lazy Theta*](file:///media/zjs/软件盘/lenovo/lenovo/Desktop/work_md/1_blog_md/项目经历/项目经历：Kinodynamic%20Lazy%20Theta.md) 中，借鉴了 Hybrid A* 的核心思想：

技术点	Hybrid A*	Kinodynamic Lazy Theta*
状态空间	3D (x, y, θ)	3D (x, y, θ)
搜索机制	A* + 运动学模型	Lazy Theta* + 运动学模型
路径生成	Reed-Shepp / 运动学模拟	Dubins / Bezier
优化后处理	共轭梯度 + 插值	能量函数平滑
应用场景	停车场、城市道路	甲板转运、工业环境

主要区别：

• Kinodynamic Lazy Theta* 采用 Lazy Theta* 而非 A*，减少不必要的节点扩展
• 引入 Dubins 曲线 和 Bezier 曲线 作为路径生成基元
• 设计 能量函数平滑 替代共轭梯度优化

8.2 技术迁移价值

Hybrid A* 的核心思想对路径规划项目具有重要指导意义：

1. 离散与连续结合：在保持搜索效率的同时满足运动学约束
2. 启发式设计：针对具体场景设计专用启发式函数，可大幅提升效率
3. 后处理优化：搜索获得初始解后，通过优化进一步提升质量
4. 势场设计：Voronoi 势场的自适应缩放思想可应用于避障代价函数设计

---

参考文献

1. Dolgov D, Thrun S, Montemerlo M, et al. Practical search techniques in path planning for autonomous driving[J]. Ann Arbor, 2008, 1001(48105): 18-80.
2. Reeds J, Shepp L. Optimal paths for a car that goes both forwards and backwards[J]. Pacific journal of mathematics, 1990, 145(2): 367-393.
3. Ferguson D, Stentz A. Field D*: An interpolation-based path planner and replanner[C]. ISRR. 2005: 1927.
4. Koenig S, Likhachev M. D* lite[C]. AAAI. 2002: 476-483.

---

文档生成日期 : 2026-04-24
关联项目 : Kinodynamic Lazy Theta* 路径规划
技术标签: 路径规划, 运动学约束, A*搜索, 自动驾驶, 非完整系统