Hybrid A*算法详解

ops/2025/1/12 10:19:53/

在这里插入图片描述

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 状态空间设计

python">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_indself.y_index = y_indself.yaw_index = yaw_indself.direction = directionself.x_list = x_listself.y_list = y_listself.yaw_list = yaw_listself.directions = directionsself.steer = steerself.parent_index = parent_indexself.cost = cost

状态空间的离散化参数：

python">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. 基础移动代价：
python">arc_l = XY_GRID_RESOLUTION * 1.5  # 基础路径长度
cost = current.cost + added_cost + arc_l  # 总代价计算
  1. 特殊动作惩罚：
python">SB_COST = 100.0  # 换向惩罚成本
BACK_COST = 5.0  # 后退惩罚成本
STEER_CHANGE_COST = 5.0  # 转向角变化惩罚成本
STEER_COST = 1.0  # 转向角惩罚成本
  1. 启发式代价：
python">H_COST = 5.0  # 启发式成本权重

3. 车辆模型

3.1 自行车模型

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

python">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 运动学方程

车辆状态转移方程：

python">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. 转向角采样：
python">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]
  1. 下一状态计算：
python">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.5x_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)

4.2 碰撞检测

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

  1. 快速检测：使用圆形包络进行初步筛选
python">def check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree):for i_x, i_y, i_yaw in zip(x_list, y_list, yaw_list):cx = i_x + BUBBLE_DIST * cos(i_yaw)cy = i_y + BUBBLE_DIST * sin(i_yaw)# 使用kd树加速近邻搜索ids = kd_tree.query_ball_point([cx, cy], BUBBLE_R)
  1. 精确检测：使用矩形模型进行精确碰撞检测
python">def rectangle_check(x, y, yaw, ox, oy):# 将障碍物转换到车体坐标系rot = rot_mat_2d(yaw)for iox, ioy in zip(ox, oy):tx = iox - xty = ioy - yconverted_xy = np.stack([tx, ty]).T @ rot

5. 启发式函数设计

5.1 动态规划启发式

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

python">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))]

5.2 Reed-Shepp曲线

在局部路径规划中使用Reed-Shepp曲线进行解析扩展：

python">def analytic_expansion(current, goal, ox, oy, kd_tree):start_x = current.x_list[-1]start_y = current.y_list[-1]start_yaw = current.yaw_list[-1]# 计算Reed-Shepp路径paths = rs.calc_paths(start_x, start_y, start_yaw,goal_x, goal_y, goal_yaw,max_curvature, step_size=MOTION_RESOLUTION)

6. 完整搜索流程

6.1 主函数实现

python">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. 初始化开启列表：
python">    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)))
  1. 主循环搜索：
python">    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] = neighborheapq.heappush(open_queue,(calc_cost(neighbor, heuristic_table),calc_index(neighbor, config)))

7. 路径优化

7.1 Reed-Shepp路径优化

使用Reed-Shepp曲线进行局部路径优化：

python">def analytic_expansion(current, goal, ox, oy, kd_tree):# 计算所有可能的Reed-Shepp路径paths = rs.calc_paths(start_x, start_y, start_yaw,goal_x, goal_y, goal_yaw,max_curvature)# 选择最优路径best_path = Nonemin_cost = float('inf')for path in paths:if check_car_collision(path.x, path.y, path.yaw, ox, oy, kd_tree):cost = calc_rs_path_cost(path)if cost < min_cost:min_cost = costbest_path = path

7.2 路径平滑

实现了基于样条曲线的路径平滑方法：

python">def path_smoothing(path_x, path_y, max_iter):for _ in range(max_iter):for i in range(1, len(path_x) - 1):# 使用三次样条曲线进行平滑path_x[i] += random.uniform(-SMOOTH_WEIGHT, SMOOTH_WEIGHT)path_y[i] += random.uniform(-SMOOTH_WEIGHT, SMOOTH_WEIGHT)

8. 性能优化

8.1 计算效率优化

  1. KD树加速：
python"># 使用KD树加速最近邻搜索
obstacle_kd_tree = cKDTree(np.vstack((ox, oy)).T)
ids = obstacle_kd_tree.query_ball_point([x, y], r)
  1. 哈希表优化：
python"># 使用哈希表存储已访问节点
closed_set = {}
node_id = calc_index(node, config)
if node_id in closed_set:continue

8.2 内存优化

  1. 节点压缩：
python">def compress_node(node):# 只保存关键点，减少内存占用if len(node.x_list) > 2:x_list = [node.x_list[0], node.x_list[-1]]y_list = [node.y_list[0], node.y_list[-1]]yaw_list = [node.yaw_list[0], node.yaw_list[-1]]return x_list, y_list, yaw_list

9. 实际应用案例

9.1 自动泊车系统

python">def parking_planning():# 定义停车场景start = [x, y, yaw]goal = [parking_x, parking_y, parking_yaw]# 定义障碍物（其他车辆和停车场边界）ox = [...]  # 障碍物x坐标列表oy = [...]  # 障碍物y坐标列表# 路径规划path = hybrid_a_star_planning(start, goal, ox, oy,XY_GRID_RESOLUTION,YAW_GRID_RESOLUTION)

9.2 动态环境导航

python">def dynamic_navigation():while not reach_goal:# 感知环境obstacles = detect_obstacles()# 更新障碍物信息update_obstacle_map(obstacles)# 重新规划路径path = hybrid_a_star_planning(current, goal,obstacles_x, obstacles_y,resolution, yaw_resolution)

参考资料

  1. Dolgov, D., et al. “Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments” 论文链接
  2. Python Robotics Implementation: AtsushiSakai/PythonRobotics
  3. LaValle, S. M. “Planning Algorithms” 在线书籍
  4. Latombe, J. C. “Robot Motion Planning” Springer链接
  5. Hybrid A*实现细节：Stanford论文
  6. 自动驾驶规划算法综述：IEEE论文
  7. Reed-Shepp曲线理论：原始论文

http://www.ppmy.cn/ops/149089.html

相关文章

SQLAlchemy 创建索引

SQLAlchemy 创建索引

以下是使用 SQLAlchemy 创建索引的步骤&#xff1a; 解决思路&#xff1a; 首先&#xff0c;需要导入必要的 SQLAlchemy 模块。定义一个表&#xff0c;在表的列上添加索引。可以使用 Index 类来创建索引&#xff0c;指定索引的名称和列。 示例代码&#xff1a; from sqlalc…
阅读更多...
【在安卓平台上，Unity与C/C++编写的.so动态库交互的实现】

【在安卓平台上，Unity与C/C++编写的.so动态库交互的实现】

在安卓平台上，Unity与C/C++编写的.so动态库交互的实现，通常通过JNI（Java Native Interface）和P/Invoke机制来完成。通过这种方式，C#脚本可以调用C/C++代码中的函数，并与本地库进行交互。 以下是一个简单的步骤演示，展示如何在Unity中与安卓平台上的.so动态库交互。 步…
阅读更多...
whowantstobeking靶机

whowantstobeking靶机

一.打开靶机 发现了个用户名 kali扫ip&#xff08;arp-scan -l&#xff09;&#xff0c;去浏览器访问 二.漏洞利用 curl http://192.168.95.148:80/skeylogger -o skeylogger file skeylogger 是个ELF文件 利用strings命令&#xff0c;通过此命令可以获取到一些有用的信息 …
阅读更多...
Vue.js组件开发-Vue CLI如何配置浏览器兼容性

Vue.js组件开发-Vue CLI如何配置浏览器兼容性

Vue.js组件开发中&#xff0c;使用Vue CLI配置浏览器兼容性主要涉及到对Babel、Polyfills以及CSS处理工具的配置。 1. 配置Babel Vue CLI默认使用Babel进行代码转换&#xff0c;以支持旧版浏览器。可以通过修改或创建Babel配置文件&#xff08;.babelrc或babel.config.js&…
阅读更多...
算法：两个升序单链表的合并

算法：两个升序单链表的合并

将两个按值排序的带头结点的单链表La和Lb排列成一个升序的 单链表&#xff0c;并返回一个新的单链表的表头指针 &#xff08;两个升序合并成升序&#xff0c;用尾插法&#xff09; LinkList Merge_LinkList(LNode* La, LNode* Lb) {//准备工作LNode* Lc;//新链表的头结点LNode…
阅读更多...
Spring Boot 集成 MyBatis 全面讲解

Spring Boot 集成 MyBatis 全面讲解

Spring Boot 集成 MyBatis 全面讲解 MyBatis 是一款优秀的持久层框架&#xff0c;与 Spring Boot 集成后可以大大简化开发流程。本文将全面讲解如何在 Spring Boot 中集成 MyBatis&#xff0c;包括环境配置、基础操作、高级功能和最佳实践。 一、MyBatis 简介 1. SqlSession …
阅读更多...
高可用虚拟IP-keepalived

高可用虚拟IP-keepalived

个人觉得华为云这个文档十分详细&#xff1a;使用虚拟IP和Keepalived搭建高可用Web集群_弹性云服务器 ECS_华为云 应用场景&#xff1a;虚拟IP技术。虚拟IP&#xff0c;就是一个未分配给真实主机的IP&#xff0c;也就是说对外提供数据库服务器的主机除了有一个真实IP外还有一个…
阅读更多...
Spring Boot + MyBatis Plus 存储 JSON 或 List 列表全攻略

Spring Boot + MyBatis Plus 存储 JSON 或 List 列表全攻略

在现代的后端开发中&#xff0c;我们常常需要处理复杂的数据结构&#xff0c;JSON 数据以及列表&#xff08;List&#xff09;数据屡见不鲜。如何高效地使用 Spring Boot 和 MyBatis Plus 来存储这些复杂数据类型&#xff0c;是这篇博客要探讨的重点。 一、为什么要存储 JSON …
阅读更多...
最新文章

Copyright @ 2022~2023