一、HybridA*整体思路流程

HybridA*算法的搜路逻辑与A*算法一致，遵循以下流程：

1、从起点开始，扩展栅格（A*是直线扩展到周围的8个栅格的中心点，HybridA*是符合车辆动力学的弧线扩展，扩展一定的距离，且到达的点不一定是栅格中心）

2、计算扩展时的cost，记为H（已行走的），计算扩展点到终点的预估cost，记为G，利用F = H + G，选取F最小的扩展点作为最终的扩展方向（A*的H、G比较简单，HybridA*的H、G较为复杂，也是主要的设计项）

3、重复1、2步，直至搜到终点，然后从终点回溯父节点，找到起点，从而生成路径（A*较为理想化，每次都是扩展至栅格中心，能够严格执行这一步的思路，HybridA*有一定的简化，但思路还是如此）

二、HybridA*与A*区别的本质

清楚了整体思路，结合前边的介绍，现在问题变为如何将车辆动力学、RS曲线化用到HybridA*中。

简单来看，A*的思想其实是比较固定的，可操作空间仅为两部分，一个是扩展方法，一个是cost的计算。

常说A*的直线扩展不符合车辆的运动特性，HybridA*对其进行了改善，很明显，这个改善是改善的扩展方法，即将车辆二自由度模型应用到扩展中；

RS曲线计算的是一点到另一点，符合车辆运动特性的路径，是一个长度，长度就可以代表cost，A*利用的是欧式距离，因此，RS曲线可以用到cost计算中，作为G。

那么，问题是，HybridA*中G的cost用欧式距离可以吗？

答案是可以，这里要明确一下，HybridA*和A*的本质区别其实就是扩展方式。RS的应用只是锦上添花，毕竟不用RS，直接用欧式距离，也是可以搜到终点的，每一步扩展都符合车辆运动特性，这样回溯到起点后，整条路径就是纯粹的扩展而来，也是可以的。

因此，只要A*中的直线扩展，替换为符合车辆运动特性的扩展，A*就变为了HybridA*，但这是最简单的变种，HybridA*还可以有其他方式的改动，这些其他的改动，大部分集中在流程中的第二步，也就是cost的配置上。

三、HybridA*的花样以及泊车代码应用HybridA*的逻辑

在这个泊车实例的代码中，对第一部分的逻辑有了变动，RS不是单纯的作为G参与到计算中，而是利用了他  “点到点的符合车辆运动特性的一段路”  这一主要特性，直接将RS路线当做结果，添加到路径当中，使泊车的路线 = 扩展路径+RS路径，总的来说逻辑如下：

每选定一个扩展点，就把扩展点当做A点，终点为B点，利用RS曲线，搜索A到B的RS路径，如果RS路径存在，则立即停止搜索，如果不存在，在继续按照第一部分的逻辑计算cost，扩展。

既然把RS当做了简化搜索，提高效率的方法，那么谁来当G的cost？

这时，就用到了之前A*计算的cost地图，用这个cost来预估剩余距离的代价G，那还可以用RS当G吗？可以，只要愿意，RS既当cost，又当提速的方法，完全可以。

此时，扩展方式，已花费的代价H，预估花费的代价G，缩短搜索时间的RS都已经具备，HybridA*的主体已经搭建完成，但这还不够，要再给他添加新花样。

泊车时，我们希望乘坐舒适，经济。HybridA*得到的路不是全局最短路径，但胜在速度快，经济上做出了牺牲，就尽量在舒适上找回来。

（1）泊车舒适，可以体现在路线平滑，于是可以提出少换挡的要求；

（2）当然，也不希望车辆来回转向，走出一条蛇形路径，于是可以提出尽量走直线的要求，或者方向盘不要连续转弯的要求；

（3）还比如，不能转急弯，于是可以提出方向盘不要打的太大；

（4）如果偏激一点，就想让车D挡开进车库，不想倒车，于是可以提出少倒车的需求；

结合上述需求，可以归纳出以下对应的方案：

（1）增加换挡cost，如果扩展时车辆已经是D挡，但cost地图显示，倒车达到的点才是距离最短的点，这时引进一个换挡的cost，就能把他的cost加大，从而排除这条路；

（2）增加方向盘转动的cost，检测到方向盘转弯，就加上这个cost，而直行，则不需要增加，这样就有利于让直行的路径cost相对较小；

（3）增加方向盘角度大小的cost，比如方向盘转角越大，这个cost越大，显然可以在方向盘转角那乘以一个系数实现；

（4）因为泊车需要倒车，这里就不增加倒车的cost，如果愿意，还可以加上R挡的cost。

通过上述例子，可以看出，HybridA*的花样体现在cost设置上，而这些cost的设计，也就成了HybridA*算法中的调参，不同的设计会导致搜出不同的路。

此次分享的泊车实例代码中，就采用了这些cost设计，保证搜出的路径符合人的主观意愿。