本文参考论文:https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf
这篇文章是基于经典A*算法与reeds-shepp曲线的,不清楚的读者请参考
【自动驾驶轨迹规划5之A*算法】_无意2121的博客-CSDN博客
【自动驾驶轨迹规划之dubins曲线与reeds-shepp曲线】_无意2121的博客-CSDN博客
目录
1 hybrid A* 算法创新点
1.1 搜索方式
A算法:在二维网格中进行搜索,本质上就是把车辆简化为 质点,并且移动方向是 固定的八个方向(或四个方向),移动距离也是确定的。但这 不符合实际的车辆运动学*模型。
hybrid A算法:引入 航向角*,将搜索变成在
三个维度的空间中进行。符合车辆运动学模型。第一幅图是 A的搜索方式,第二幅图是hybrid A的搜索方式。
1.2 车辆运动学模型
为了便于计算,hybrid A采用车辆二自由度运动学模型(见上图),但是 忽略了车辆加速度与前轮转角速度*,于是经过简化的运动学模型如下
所以这里的 控制变量是速度 和前轮转向角 ,根据状态变量
横坐标, 纵坐标, 航向角的目前所处状态, 就是上图中的 b ,代表前后轴距离,通过控制变量的输入,在单位时间内,就能得到下一个时刻的状态。1.3 reeds-shepp曲线的引入
reeds-shepp曲线证明,任何始末位姿的最短路径假如包含圆弧,一定在最小转弯半径时取到,也就是
, 前轮转向角在这样的取值时路径最短,同时 时,车辆的耗时也更短,因此相当于每个状态能够通向另外六个状态,类似于经典A,从一个状态从八个方向移动到另外八个状态,但是经典A是 状态采样, 从一个状态跳到另一个状态,忽略了其中的运动学过程。但是hybrid A是 控制采样,更加符合运动学实际。当然真正运动时不一定一直按照最小转弯半径,这里为了简单,就先这样处理。下图是hybrid A的六个子节点。但是reeds-shepp曲线 完全未考虑避障因素,但是由于reeds-shepp曲线比较简单易计算,所以构造速度非常快,使得 先构造再检验是否碰撞成为可能,这里的碰撞指的是 整条轨迹是否会与障碍物有 交集,因此不像传统A不考虑中间过程,所以需要 均匀采样整段时间,进行碰撞检验。假如该段轨迹 无碰撞则加入搜索树中,作为候选轨迹*,如下图
图a:假如 每次搜索都用reeds-shepp,由下图a可见这个 搜索轨迹构成的树规模很庞大,节点很多将会造成极大的计算量。
1.4 G的定义更丰富
在传统A算法中,G是从起点到当前节点的路径消耗,由于一段直线前进的轨迹肯定优于反复前进倒退或扭曲的轨迹,因此我们对 频繁切换速度 和前轮转向角 两个控制量的值这种行为进行 惩罚,这样就能使得最后的轨迹更加合理。还有 很多为了轨迹合理可以惩罚的地方,这其实就是一个 评价函数的设计,具体可以参考【自动驾驶轨迹规划之最优控制】_无意2121的博客-CSDN博客 但需要说明的是,如果是 极端狭窄的泊车场景*中,我们不得不采用复杂扭曲的轨迹。如下图
1.5 H的定义更丰富
按理来说,H函数应该是从当前节点位姿到终点节点位姿,同时满足避障以及车辆运动学约束的最短路径长度,这是 真实路径,但这很难在还没采样搜索剩下的位置环境时就知道真实路径,所以hybrid A 设计两个H的子函数, H1代表符合车辆运动学约束但忽略碰撞因素的最短路径,H2代表满足避障约束但是忽略车辆运动学约束的最短路径,H定义为H1与H2的最大值*。
H1:当前节点离终点较近时,更应该关注车辆运动学约束,忽略障碍物的情况下, 路径不依赖于任何在线场景信息,可以通过 离线的方式采样枚举所有reeds-shepp曲线可能,提前将路径长度记录出来,在线调用该函数时只需索引、插值即可返回函数值。同时,这项启发函数的主要目的是为 修剪传统A*搜索树的分支,保证最后能 精准衔接终止位姿。。如下图
H2: 当前节点离终点较远时,更应该关注避障行驶,防止陷入死胡同,利用 传统A*的H进行计算。如下图
可见这样对 启发函数的设计有利于提高搜索速度。
1.6 Voronoi势场函数
生成的路径必须与障碍物 保持一定的距离,这也是最优轨迹的要求,由于 传统的人工势场法的缺点是在狭窄路段构造了高势场,使得机器人或车辆无法通过,因此,构造Voronoi势场函数。首先,介绍一下Voronoi图。
每个Cell中包含的都是距离当前Cell距离最近的所有点,因此Cell的 边界就是距离种子点 最远的点的集合。利用这个特性,将 采样障碍物的边界当做种子点,那么Cell的边界就是远离所有障碍物的可行驶路径。效果见下图
当然不可能就照着cell的边界去运动,因为设计的 出发点是为了通过较窄的地方。在路较宽时, 没有必要,所以我们需要构建一个势场,能够让车辆或机器人趋于cell的边界去运动。势场函数如下
公式中
、 分别代表路径节点到最近的障碍物和最近的GVD(广义voronoi图)的长度, 控制势场的衰减率, 控制势场的影响范围。同时voronoi势场有如下的一些特点:(1)
时,场的值为0(2)
的区间在[0, 1] 时,且连续, 不能同时为0(3)在障碍物附近Voronoi势场的值达到最大值
(4)Voronoi势场的值在GVD的边上达到最小值
构建的Voronoi势场效果图如下( 越黑,势场越大)
1.7 损失函数的设计
由于搜索树有许多候选轨迹,我们需要设计一个损失函数, 来评价这些候选轨迹
按着原始论文损失函数各成分的顺序
基于之前定义的voronoi场函数,使 路径远离障碍物 惩罚与障碍物的碰撞, 是当前节点的坐标, 是最近障碍物的坐标, 是离障碍物的最大距离,选择二次函数 作为惩罚项的目的是放大障碍物与节点越来越靠近的效果 相当于对路径的 每个节点的瞬时曲率设置一个上限 ,二次函数 作为惩罚项, 作为光滑度项,计算每个节点之间 位移向量的差值的平方。这一项将损失值赋给非均匀分布和方向变化的节点,以保证路径的 平滑性优化分为两个阶段: a. 对路径的顶点坐标进行非线性优化规划问题的建模(也就是上文的损失函数定义) b. 采用共轭梯度方法进行非参数化插值(具体请参考原始论文)
2 hybrid A* 算法实现
2.1 伪代码
2.2 hybrid A* 算法效果图
Original: https://blog.csdn.net/weixin_65089713/article/details/124060155
Author: 无意2121
Title: 【自动驾驶轨迹规划之hybrid A*算法】
原创文章受到原创版权保护。转载请注明出处:https://www.johngo689.com/558967/
转载文章受原作者版权保护。转载请注明原作者出处!