规划控制 | 详解自动驾驶轨迹规划Hybrid A*算法

作者 | 涅索斯衬衫  编辑 | 汽车人

原文链接：https://zhuanlan.zhihu.com/p/610206756?

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心技术交流群

后台回复【规划控制综述】获取自动驾驶、智能机器人规划控制最新综述论文！

传统的A*算法是一种基于图搜索(Graph Search)的启发式搜索算法，启发式搜索在一定程度上避免了无效的搜索路径，提高了搜索效率。它作为一种最优优先策略的搜索算法，在规划中将车辆看作一个质点，但仅考虑了(x, y)两个维度。

Fig.1 A*与Hybrid A*算法对比

传统的A* 算法能够在二维网络中生成一条粗略的路径，它保证了空间的联通性，却并未充分考虑车辆的运动学约束。不同于差速、全向机器人等不存在运动约束的场景，其生成的轨迹几乎无法被车辆直接跟踪执行。而作为改进后的A* 算法(Hybrid A*)增加了theta维度，优化了节点拓展的方式，它不仅考虑位置方向(x,y)的拓展，还考虑了运动方向theta的拓展，是三维的(x, y, theta)，状态是连续的。相较于A* 算法的四叉树或八叉树的方法来扩展节点(如图所示)，其规划的路径更符合车辆的运动特性，保证了轨迹的可行驶性。

Fig.2 A* 与Hybrid-A* 节点拓展示意图

Hybrid-A*算法在规划路径时主要分为如下几个步骤：

1. 搜索空间的离散化：将空间划分成小网格(grid)；

2. 搜索树的扩展(Analytic Expansions)：利用RS/Dubins等曲线连接当前节点与目标节点，并对该轨迹进行碰撞检测，若无碰撞，则结束搜索，其规划轨迹为当前路径到目标节点的path1+当前节点回溯到起始节点的路径path2；若碰撞，则放弃该曲线，并从Open List中重新寻找节点，继续搜索；

3. 路径优化：经过Hybrid-A*得出的路径往往不是最优的，通常存在不必要的转向，为了获得更好的预期驾驶行为，还需对轨迹进行进一步优化（如轨迹平滑/插值等）；

Fig.3 梯度下降法平滑轨迹

• 启发式的选择

对于A* 算法而言，在某一结点n处的代价函数可以表示为：

f(n) = g(n) + h(n)

式中，g(n)为状态空间中从初始点到当前节点n的实际代价；

------h(n)为当前节点n到目标节点最佳路径的估计代价；

在代价函数的选择上，Hybrid-A*结合实际运行情况以及车辆当前的位姿，对车辆的航向、档位、曲率以及方向等施加惩罚，使其更符合实际运动场景。

在A* 算法中，启发式一般采用两个节点间的距离之和，而Hybrid-A*算法完善了启发式函数h的定义，主要采用了如下两种启发式以提高搜索效率。

1. non-holonomic-without-obstacles-heuristic：考虑车辆的运动约束但不考虑障碍物，一般使用RS曲线、Dubins曲线等；

2. holonomic-with-obstacles-heuristic：考虑障碍物，不考虑车辆的运动约束；

Fig.4 启发式对比

• 项目参考

[1] karlkurzer/path_planner

该项目为karl kurzer的硕士课题，其论文及链接：Path Planning in Unstructured Environments: A Real-time Hybrid A* Implementation for Fast and Deterministic Path Generation for the KTH Research Concept Vehicle

Fig.5 规划轨迹

[2] HAStar_ParkingPlanner

该项目主要是基于Apollo开源项目中Hybrid A* 算法实现泊车的路径规划方法，分别进行了垂直和水平车位的测试，算是一个阉割版，效果有待提升。

Fig.6 垂直车位

[3] Matlab Hybrid_A*

该项目为Hybrid- A*的Matlab版本实现，对于理解其实现的原理比较有帮组。

Fig.7 泊车示意图

• 参考文献

[1] Dolgov D , Thrun S , Montemerlo M , et al. Practical Search Techniques in Path Planning for Autonomous Driving[J]. ann arbor, 2008.

[2] Dolgov D , Thrun S , Montemerlo M , et al. Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments[J]. The International Journal of Robotics Research, 2010, 29(5):485-501.

[3]古月居_混合A*算法研究

[4] 自动驾驶运动规划-Hybird A*算法

[5] Gradient-Informed Path Smoothing for Wheeled Mobile Robots (GRIPS) (github.com)

国内首个自动驾驶学习社区

近1000人的交流社区，和20+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！

【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向；

添加汽车人助理微信邀请入群

备注：学校/公司+方向+昵称