1、算法概述
存在问题: 机器人的控制空间、轨迹空间都是连续的,而现存算法基本都是适用于离散情况的,所以生成的路径是不光滑的。不满足机器人的非完整性约束
解决问题:
假设无人车有充分的感知和定位能力,则其能够在线重新规划,生成障碍物地图,并且能够在未知环境中行驶。解决了在未知环境中自动驾驶车辆的路径规划问题。
流程:
第一步在连续坐标系中使用启发式搜索,以保证计算轨迹的运动学可行性。并且能够保证生成的轨迹满足车辆非完整性约束,通常会生成一条位于全局最优解附近的轨迹。
第二步使用数值非线性优化(共轭梯度(CG)下降)来提高局部解的质量,产生一条至少是局部最优的路径,但通常也会达到全局最优。
2、搜索空间离散化
A* 是赋予每个网格的中心点相应的损失并且算法只访问这些中心点,而hybrid A* 是先在这些网格中挑选满足车辆3D连续状态的点,并将损失赋值给这些点。
Hybird A*算法同时考虑空间连通性和车辆朝向,将二维平面空间和角度同时进行二维离散化。
在(X,Y,theta)三个维度上进行搜索树(Search Tree)扩展时Hybird A*将车辆的运动学约束引入其中,路径节点可以是二维小网格内的任意一点,保证了搜索出的路径一定是车辆实际可以行驶的。
3、启发函数
传统的A* 算法的启发函数一般是2D欧几里得距离,而hybrid A* 算法构造了两个启发函数。
第一个启发函数是Constrained heuristics ,只考虑车辆的非完整性约束而不考虑障碍物(相比直接用欧几里得距离损失要好一个数量级,扩展更少的结点)(图a,b)。
该启发函数忽略了环境中的障碍物等信息,只考虑车辆的运动学特性,从终止点开始,计算从该点到其他点的最短路径。具体的返回值就是:
第二个启发函数是第一个的对偶,称为Unconstrained heuristics,只考虑障碍物信息而不考虑车辆的非完整性约束条件
该启发函数使用传统的2D 路径规划算法,如A* 算法,计算每个节点到终点的最短路径。能够发现2D空间中所有的U形障碍物和死胡同dead end,避免在更复杂的环境中,导致浪费性探索(图c)。最终算法使用的损失函数就是两种启发函数的最大值。(图d)
4、搜索树扩展
车辆的控制输入主要有两个:方向盘转角(Steering Angle)和运动方向(direction)。
将方向盘转角从最小转角(Min Steering Angle)到最大转角(Max Steering Angle)按照一定间隔进行采样;车辆的运动方向:向前运动和向后运动。
正向搜索使用离散化的控制动作空间(转向)。搜索永远不会达到精确的连续坐标目标状态(精度取决于A*中网格的分辨率)。
为了提高精度和搜索速度,在Reed-Shepp模型(Reeds and Shepp 1990)的基础上,通过分析扩展来增强搜索。在搜索中,树中的一个节点通过在一小段时间内(对应于网格的分辨率)使用特定的控制动作模拟汽车的运动学模型来扩展。
除此之外,通过计算从当前状态到目标(假设无障碍环境)的最佳Reed-Shepp路径来生成额外的子节点。然后检查Reed and Shepp路径是否与当前障碍物贴图发生碰撞,并且仅将子节点添加到 如果路径是无冲突的,则子节点添加到树中。
5、其他注意
应用场景:
在自动驾驶领域:
APA(辅助泊车)
AVP(代客泊车)
U-Turn(行车场景下的车辆掉头)
参考链接:
算法解析(伪代码流程):
https://zhuanlan.zhihu.com/p/139489196
原理代码解析:
https://zhuanlan.zhihu.com/p/139489196
ros平台c++/matlab代码带注释实现:
https://github.com/teddyluo/hybrid-a-star-annotation
参考论文:《Practical Search Techniques in Path Planning for Autonomous Driving》
内容皆来源于网络和参考链接,侵权联系删。