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 连续空间搜索的复杂性

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

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

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* 的关键区别：

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

4.1.2 状态更新规则

defhybrid_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)whilenotopen_list.empty():current=open_list.pop()# 取 f = g + h 最小的节点ifis_goal_reached(current.state,goal):returnreconstruct_path(current)# 网格剪枝：同一网格只保留代价最小的节点cell=discretize(current.state)ifcellinclosed_listandclosed_list[cell].g<=current.g:continueclosed_list[cell]=current# 节点扩展：基于运动学模型forsteerin[-max_steer,0,max_steer]:fordirectionin[1,-1]:# 前进/后退new_state=kinematic_model(current.state,steer=steer,direction=direction,dt=dt)ifcollision_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)returnNone# 未找到路径

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 解析扩展

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

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

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

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]，且在障碍物内部达到最大值

defvoronoi_field(x,y,obstacle_map,voronoi_diagram):""" 计算 Voronoi 势场值 """d_O=distance_to_nearest_obstacle(x,y,obstacle_map)ifd_O>=d_max:return0.0d_V=distance_to_voronoi_edge(x,y,voronoi_diagram)# 避免除零ifd_O+d_V==0:return1.0rho=(alpha/(alpha+d_O))*\((d_O-d_max)**2/d_max**2)*\(d_V/(d_O+d_V))returnrho

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），直接跟踪会导致方向盘剧烈抖动。

解决方案：非参数插值

defnon_parametric_interpolation(path,num_subsamples):""" 非参数插值：超采样 + 固定原顶点优化 """# Step 1: 在路径顶点间插入新顶点interpolated_path=[]foriinrange(len(path)-1):interpolated_path.append(path[i])forjinrange(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))returnoptimized_path

优势：

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

5. 实验设计与验证

5.1 实验平台

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

5.2 参数设置

5.3 实验结果

性能指标：

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

启发式函数效果对比：

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 后续发展

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* 的核心思想：

主要区别：

• 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*搜索, 自动驾驶, 非完整系统