混合A*算法(Hybrid A*)
1、混合A*算法(Hybrid A*)简介
本课题为了完成在已知环境下的侧方位停车和倒车入库的任务规划，采用斯坦福大学所提出来的Hybrid A*算法，并基于开源机器人操作系统其进行实现。其中，在2010年斯坦福的Dmitri Dolgov, Sebastian Thrun, Michael Montemerlo等人在国际顶级期刊International Journal of Robotics Reaserch上首次提出混合A*算法(Hybrid A*算法)用于解决在侧方位停车和倒车入库过程中的路径规划问题。同时，混合A*算法是A*算法和车辆运动学相结合的一种用于A*算法不满足车辆运动学的问题。
2、启发函数
启发函数对于A*算法的搜索效率是至关重要的，而研究合理的、有效的启发来估计扩展节点到目标点的代价至关重要。在Hybrid A*算法中将启发函数分成两种：无障碍物的非完整性约束启发代价和有障碍物的完整性启发式代价。
2.1无障碍物的非完整约束启发代价
这第一个启发式代价主要是忽略了在路径规划中障碍物对规划器的影响。这项启发函数必须获得车辆的最小转弯半径作为输入。并且，改启发函数主要是通过离散周围的栅格地图，获得不同方向坐标点 并使用最优曲线计算路径长度。比如，在需要计算起始点到目标点
的最优曲线的路径长度时。首先，是在起始点 
使用Dubins曲线或者Reeds Shepp曲线连接目标点 
，得到最优曲线的路径长度。然后，计算最优曲线的距离，并作为启发函数的代价值
。因为这项代价函数忽略了环境中障碍物的影响，所以可以通过离线的形式计算出来。同时，这项代价函数的主要目的是为修剪A*搜索树的分支，避免了到达目标点时，所规划路径方向与目标方向不一致的问题。
2.2有障碍物的完整性启发式代价
第二项代价函数主要是考虑环境中障碍物信息，但忽略了车辆的最小转弯半径对其的影响。通过在每个节点上使用Djikstra算法获得该节点到达目标点的最近距离作为其代价函数的代价值
。由于Djikstra算法能够在类似迷宫环境中获得扩展节点到目标点的最近路径，因此可以避免在扩展过程中Hybrid A*算法扩展朝向不正确的问题。
通过比较无障碍物的非完整约束启发代价的代价值 和有障碍物的完整性启发式代价的代价值 的大小，选中其中较大的代价作为Hybrid A*的代价值
。
3、维诺地图
在路径规划中，地图是最基本的要素，也是反映现实世界的一种重要的表达。在常用的地图中，距离地图、栅格地图和维诺地图等。其中，栅格地图是地图分成一个个边长相等的栅格来表示环境信息。距离地图是在规划算法进行搜索过程中提供扩展节点到障碍物的最近距离。其中，最近距离是通过欧式距离所计算得到。维诺地图是一种对距离地图的扩展。通过距离地图和广义泰森多边形(generalized Voronoi diagram，GVD)相结合所形成。其中，广义泰森多边形是由气象学家Alfred H.Thiessen将邻近气象站连接为三角形，同时在这些三角形的边上做垂直平分线，并连接形成的形状如图1。这样的图形主要包括三个特点：
(1)每个多边形只包含一个气象站。
(2)相邻多边形里面的气象站到公共边的距离相等。
(3)在多边形区域内的点到达气象站(为图1多边形的交点)的距离相比到达另外气象站的距离都要远。
图1
在Hybrid A*算法中，通过使用维诺地图获得扩展节点的势能值，并将其作为代价值添加到代价函数中，使用这样的方式让扩展节点与障碍物形成一定的距离。
势能值得具体计算主要是通过使用公式(1)。其中，
是节点在
处的势能，
是节点到达最近障碍物的距离、
是节点到达维诺地图边的最近距离、
和
为常数分别表示势能下降速率和势能的控制范围。另外![\alpha \in \left [ 0,\infty \right ]](http://img.e-com-net.com/image/info8/d62ac9ed6cee4632a1368317826f0e08.gif){kind=small}
,当值越大，
处的势能下降的越慢。当
时，
等于零。势能取值是从0到1，当节点 越靠近广义泰森多边形的边的时候，势能越来越小。
(1)
(a)距离地图 (b)维诺地图
图2 距离地图和维诺地图
通过将距离地图与广义泰森多边形想结合可以得到图2(a)。其中，蓝线为广义泰森多边形的边和绿色的长方形为障碍物。而图2 (b)是在图2 (a)的基础上面使用公式(1)所得维诺地图。在图2(b)和公式1中，可以得到如下四个结论:
(1)当![\[{{d}_{O}}>d_{O}^{\max }\]](http://img.e-com-net.com/image/info8/e70666377f43491ab07bf3cd251f79cf.gif){kind=small}
时， ![\[(x,y)\]](http://img.e-com-net.com/image/info8/abd241dca1084155a151e1a138246336.gif){kind=small}
的势能为0。
(2)势能 ![\[\rho \nu (x,y)\]](http://img.e-com-net.com/image/info8/36f32aa8299943a1a263d3c9bea2eb93.gif){kind=small}
的取值范围是在0到1之间。
(3)势能值到达最大的时候是 在障碍物上或者里面。
(4)势能值到达最小的时候是 在广义泰森多边形的边上。
4、 Reeds-Shepp曲线
Reeds-Shepp曲线是1990年由Reeds和Shepp二人在基于Dubins曲线所提出来满足车辆运动学的最优曲线。此外，Reeds-Shepp曲线解决了Dubins曲线不能生成倒退路径的问题。在Reeds-Shepp曲线中，将车辆行驶的最短路径用48个运动本原或者9个基字进行表示(如图4-36所示)。同时，在Reeds-Shepp曲线组成中主要由直线段、向前的曲线和向后的曲线构成。其中，在曲线部分的曲率主要由最小转弯半径所决定。如下图所示，S代表直线部分、C表示向左转还是向右转的曲线、
表示有两个曲线长度一样的曲线部分、
表示曲线部分的角度有90°、|表示齿轮转换(从前行驶转到向后行驶或者从后行驶转到向前行驶)。在使用Reeds-Shepp曲线的过程，需要输入起始点的姿态
和目标点的姿态
去求解68个方程就能得到一个最短距离的运动本原。最后，通过图3从最优的运动本原中解算出路径点。
图3 Reeds-Shepp曲线9个基字
图4 Reeds-Shepp曲线结算流程图
在执行规划算法之前或在目标点附近，使用Reeds-Shepp曲线进行预处理得到路径，并判断路径上是否与障碍物发生碰撞。若没有，则保留Reeds-Shepp曲线所生成的路径，完成规划(如图4-39所示)。
(a) 起始点朝向与目标点朝向相同且在转折的情况
(b) 起始点朝向与目标点朝向垂直的情况
图5 预处理过程
5、扩展方式
Hybrid A*算法是标准A*算法的一种改进，通过这种方式能够使得A*算法满足车辆运动学。首先，A*算法的扩展节点是栅格的交点(如图6 (a)所示)，因此不能解决在连续空间内车辆朝向和位置的问题。而Hybrid A*算法通过使用车辆运动学生成的轨迹(如图6 (b)所示)替代A*算法中的节点，从而解决路径不满足车辆运动学的问题。同时，在扩展节点使用Dubins曲线和Djikstra算法计算该节点到目标点的距离，并选在其中最大的一个代价值作为
。此外，判断父节点和子节点的运动方向是否一致、控制方向是否一致并将这两项改变加入到公式(2和3)中。最后，在判断节点与目标点的距离是否小于一个所设置的阈值(本课题设置为10米)。如果小于该阈值，则用Reed-Shepp曲线与目标向连接，并加入车身轮廓判断是否与障碍物碰撞。如果和障碍物无碰撞，则保留该Reed-Shepp曲线并生成路径。如果与障碍物发生碰撞则放弃该Reed-Shepp曲线，并从开启列表中重新寻找节点，重新进行扩展。如果该节点与目标点的距离大于阈值，则从开启列表中重新寻找节点，重新进行扩展。
(a)A*算法的扩展节点
(b) Hybrid A*算法的扩展节点
图6 扩展节点示意图
(a) (b) (c)
图7 用于扩展的运动基元和扩展节点示意图
实现过程中所使用的运动基元含有六个运动模式(如图7a)所示)，向前扩展三个运动模式，向后扩展三个运动模式。而运动基元的末端位置是在车身轮廓的交点处(如图7 (b)所示)。蓝色的正方形为六个运动模式的末端位置，红色方框为车身轮廓，绿线为扩展一次所选择的运动模式。图7 (c)表示Hybrid A*算法向后扩展的过程(未使用Reeds-Shepp曲线)。其中，蓝色正方形所组成的路径为运动基元连接的结果，绿色箭头为目标点。
（2）
（3）