论文解读——自动驾驶决策规划算法之Hybrid A*_hybrid a*+五次曲线

论文解读——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*介绍

视频地址https://www.youtube.com/watch?v=qXZt-B7iUyw，YouTube机翻的字幕不一定正确，辩证学习

上面是关于hybrid A*的介绍视频。HA *算法由2010年斯坦福大学首次提出，并在(DARPA)的城市挑战赛中得以运用。大家在学习HA * 算法的过程中请关注以下几个问题：

• 启发式函数如何设计
• 什么是Analytic Expansions
• 如何平滑路径

1 基础知识

1.1 A*算法admissible结论

当A*算法中启发式函数$\hat{h}$满足$\hat{h}<h$时，A *算法找到的路径是最优的，所以一般希望启发式函数设计的大一些，越靠近$h$意味着所拓展的节点数量较少。

注：
$\hat{h}$ : 启发式函数;
$h$ : 当前节点到目标节点的最短距离；
（上述是A*算法原文结论，一般来说都用$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_v$ : 广义Voronoi图(GVD)的边的距离
• $\alpha$ : 控制势场的衰减率
• $d_o^{max}$ : 控制势场的影响范围

性质说明：

• 当$d_o<=d_o^{max}$时上式成立，否则为0
• ${\rho }_v(x,y)$位于[0,1]之间，且连续
• 仅仅在障碍物中${\rho }_v(x,y)$达到最大值
• GVD边上的势场值最小

构造出来的效果图如下（越黑数值越高）

2 初始解生成

2.1 启发式函数设计

在HA*中，作者的启发式函数设计为$max(h_1,h_2)$,下面具体介绍$h_1$和$h_2$启发式函数的设计过程。

• $h_1$(non-holonomic-without-obstacles):考虑车的特性，忽略障碍物。具体来说，$h_1$值为当前节点到目标节点的Reeds-Sheep曲线长度，
• $h_2$(the holonomic-with-obstacles):不考虑车的特性，考虑障碍物。具体来说，$h_1$值为当前节点到目标节点的欧式距离，设置该启发式函数可以引导车辆远离死胡同和绕过u形障碍物

Figure3 为启发式函数设计对比图，Figure3（a）仅仅使用欧式距离，Figure3（b）使用$h_1$，Figure3（c）为$h_1$遇到死胡同的情况，Figure3（d）为$max(h_1,h_2)$。大家请注意拓展节点数量的对比，$h_1$可以大幅度减小拓展节点的数量，但是可能遇到死胡同的情形（c）,而$h_2$可以引导车辆远离死胡同和绕过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$最近障碍物的距离，$d_{obs}^{\vee }$为一阈值

• 曲率项：与障碍物项类似，${\kappa }_{max}$为最大曲率,$\Delta {\Phi }_i={\cos }^{-1}\frac{x_i{x}_{i+1}}{|x_i||x_{i+1}|}$
  

• 平滑项 ：相邻点之间的距离平方和
  

• Voronoi场的信息：所有点的Voronoi场值和
  

求解使用的是gradient descent求解，这个暂时先mark住，改天学会了回来补充。下图为优化前后路径对比图。