论文解读——Hybrid A*
参考资料来源:《Practical Search Techniques in Path Planning for Autonomous Driving》、《Path Planning in Unstructured Environments:A Real-time Hybrid A* Implementation for Fast and Deterministic Path Generation for the KTH Research Concept Vehicle》、《自动驾驶汽车决策和控制》
注:第二篇论文ROS实现的github链接:https://github.com/karlkurzer/path_planner
0 Hybrid A*介绍
hybrid A*带字幕
视频地址https://www.youtube.com/watch?v=qXZt-B7iUyw,YouTube机翻的字幕不一定正确,辩证学习
上面是关于hybrid A*的介绍视频。HA *算法由2010年斯坦福大学首次提出,并在(DARPA)的城市挑战赛中得以运用。大家在学习HA * 算法的过程中请关注以下几个问题:
- 启发式函数如何设计
- 什么是Analytic Expansions
- 如何平滑路径
1 基础知识
1.1 A*算法admissible结论
当A*算法中启发式函数 h ^ \hat{h} h^满足 h ^ < h \hat{h}<h h^<h时,A *算法找到的路径是最优的,所以一般希望启发式函数设计的大一些,越靠近 h h h意味着所拓展的节点数量较少。
注:
h
^
\hat{h}
h^ : 启发式函数;
h
h
h : 当前节点到目标节点的最短距离;
(上述是A*算法原文结论,一般来说都用
h
h
h表示启发式函数!!!)
1.2 hybrid A*的搜索方式
上图是传统A*的搜索方式,其搜索方向共8个(东西南北、东北、东南、西北、西南方向),节点只能位于cell中心中,更为关键的是无人车不能执行的
该图是hybrid A*的搜索方式,其搜索方向共6个,节点可以不位于cell中心中,同时也是可以无人车可执行的,下面详细说一下搜索方向的问题。
HA*采用控制采样的方式,在给定T和U时,可以得到基于车辆运动学的轨迹,如下图所示共6条。(关于车辆运动学,后续会在自动驾驶控制模块介绍)
随后从起点到终点按照6个方向不断搜索,这也是与传统A*不相同的地方
1.3 Dubins和Reeds-Sheep曲线
关于Dubins和Reeds-Sheep曲线大家可以参考《自动驾驶汽车决策和控制》一书介绍的内容了解(懒得敲字了)
注:上述说明的两条曲线是实际可运行的,HA*中使用的曲线是Reeds-Sheep曲线
1.4 Voronoi Field
因为要求无人车生成的路径必须与障碍物保持一定距离,由于传统的人工势场法在狭窄路段有较高的数值,使得车辆无法通过,因此,构造需要Voronoi函数如下:
Voronoi图:
简单的说,当看到空间中的一系列给定的点,例如x,…,我们希望为每个点,例如点x,划定一个包围这个点的区域,例如区域Cx,这一包含了点x的区域 Cx 我们称为Voronoi Cell,构成的图即为Voronoi图
符号定义:
- d o d_{o} do : 离最近障碍物的距离
- d v d_{v} dv : 广义Voronoi图(GVD)的边的距离
- α \alpha α : 控制势场的衰减率
- d o m a x d_{o}^{max} domax : 控制势场的影响范围
性质说明:
- 当 d o < = d o m a x d_{o}<=d_{o}^{max} do<=domax时上式成立,否则为0
- ρ v ( x , y ) \rho_{v}(x,y) ρv(x,y)位于[0,1]之间,且连续
- 仅仅在障碍物中 ρ v ( x , y ) \rho_{v}(x,y) ρv(x,y)达到最大值
- GVD边上的势场值最小
构造出来的效果图如下(越黑数值越高)
2 初始解生成
2.1 启发式函数设计
在HA*中,作者的启发式函数设计为 m a x ( h 1 , h 2 ) max(h_{1},h_{2}) max(h1,h2),下面具体介绍 h 1 h_{1} h1和 h 2 h_{2} h2启发式函数的设计过程。
- h 1 h_{1} h1(non-holonomic-without-obstacles):考虑车的特性,忽略障碍物。具体来说, h 1 h_{1} h1值为当前节点到目标节点的Reeds-Sheep曲线长度,
- h 2 h_{2} h2(the holonomic-with-obstacles):不考虑车的特性,考虑障碍物。具体来说, h 1 h_{1} h1值为当前节点到目标节点的欧式距离,设置该启发式函数可以引导车辆远离死胡同和绕过u形障碍物
Figure3 为启发式函数设计对比图,Figure3(a)仅仅使用欧式距离,Figure3(b)使用 h 1 h_{1} h1,Figure3(c)为 h 1 h_{1} h1遇到死胡同的情况,Figure3(d)为 m a x ( h 1 , h 2 ) max(h_{1},h_{2}) max(h1,h2)。大家请注意拓展节点数量的对比, h 1 h_{1} h1可以大幅度减小拓展节点的数量,但是可能遇到死胡同的情形(c),而 h 2 h_{2} h2可以引导车辆远离死胡同和绕过u形障碍物。这样设计的启发式函数所需要的拓展节点数量很少。
注:关于为什么取max,可以参考1.1部分
2.2 Analytic Expansions
这个可以算是一个工程上的技巧吧,当拓展N个节点时,会尝试用Reeds-Sheep曲线连接终点,如果没有障碍物,则HA*的初始解已经找到。如果有障碍物,继续拓展节点,然后继续尝试连接…
注:显然对于每个节点都进行尝试连接终点是不明智的,因为初始节点有很大概率与终点的连线上会有障碍物。所以可以到后期的时候连接尝试频率高一些,前期可以频率低一些
如下图所示,紫色就是Analytic Expansions的过程,使用Reeds-Sheep曲线连接终点
3 平滑算法
先思考一个问题:
Q:HA* 的初解路径是否可执行,为什么要平滑?
A:HA* 算法的初解路径是可以执行,但是有很多是由不必要的转向动作等组成的,所以需要进行平滑。
如下是平滑函数的设计过程,(6.1)为平滑函数总项,第一项障碍物距离项,第二项曲率项,第三项是平滑项,第四项是Voronoi场的信息
- 障碍物项: ∣ x i − o i ∣ |x_{i}-o_{i}| ∣xi−oi∣为距离 x i x_{i} xi最近障碍物的距离, d o b s ∨ d_{obs}^{\vee} dobs∨为一阈值
-
曲率项:与障碍物项类似, κ m a x \kappa_{max} κmax为最大曲率, Δ Φ i = cos − 1 x i x i + 1 ∣ x i ∣ ∣ x i + 1 ∣ \Delta\Phi _{i}=\cos^{-1}{\frac{x_{i}x_{i+1}}{\left | x_{i}\right|\left|x_{i+1}\right|}} ΔΦi=cos−1∣xi∣∣xi+1∣xixi+1
-
平滑项 :相邻点之间的距离平方和
-
Voronoi场的信息:所有点的Voronoi场值和
求解使用的是gradient descent求解,这个暂时先mark住,改天学会了回来补充。下图为优化前后路径对比图。