混合A星的启发式函数和惩罚项分析与实验

启发式函数分析

启发函数有两种，一个是全向考虑障碍物，另一个是非全向不考虑障碍物。
全向考虑障碍物的启发式距离可用A星搜索求得邻居节点到终点的距离，但是由于混合A星每次扩展一个节点都需要调用A星搜索一次，这会累加扩展节点个A星搜索的时间，导致混合A星实时性变差。所以可以一次搜索得到整个地图的节点到终点的cost并存储在节点中，这样的话混合A星每次扩展节点只需要查询扩展节点到终点的距离即可，大大节省了搜索时间，属于空间换时间的策略。
非全向不考虑障碍物的启发距离可由RS曲线获得，可以在给定起始点位姿和终止点位姿之后直接计算连接两点位姿而且不考虑障碍物的RS曲线的长度并作为启发距离。
原始混合A星论文在不同的场景下试验了两种启发函数，如下图所示，

从图(a), (b)可以看出，在障碍物不太密集的场景下使用非全向不考虑障碍物的启发函数比用欧式距离做启发函数少扩展了很多节点，因为欧式距离没有考虑车的非全向运动学，所以这个启发不太准。
从图©,(d)可以看出，在障碍物有死胡同的场景下，左图使用非全向不考虑障碍物的启发函数比右图全向考虑障碍物多扩展很多节点，因为非全向不考虑障碍物直接就把车往胡同里引导，到里边才发现是个死胡同，而全向考虑障碍物因为考虑了障碍物，死胡同里边节点里终点的距离很大，所以提前知道了那是个死胡同就不往里进了。
综上，论文采用结合两种启发函数的方法，取二者最大值，这时在障碍物稀疏情况下就会用非全向不考虑障碍物，有死胡同就会用全向考虑障碍物，也算一个小自适应。

下面用自己写的程序对上面两种启发函数进行复现，
1 带死角的场景用非全向不考虑障碍物启发函数

2 带死角的场景用全向考虑障碍物启发函数

就这种带死角的差别最明显，其他的看不出什么太大差别。
最后李柏老师的书上有个工程经验，进这种狭窄的空间可以把起点和终点互换，搜索出去比搜索进去简单一些，效果图如下，

惩罚项实验

1 前向后向惩罚
对前向惩罚应该用不太到，毕竟车多数往前开，如果用到倒车的话后向也不太用吧。
没加惩罚前，目标在前边，还向后搜索了

现在增加一个后向的权重，向后搜索少了

2 前轮转角变化惩罚
这个能让当前节点的前轮转角和下一个节点的前轮转角变化减小
如下图所示，没加惩罚前急了拐弯的，

加前轮打角变化惩罚后，

看下面这两幅图，第一个前轮打角变化没加惩罚，第二个加上了，可以分析出加不加这个惩罚是对路径长度和平滑性的权衡。


还有前轮打角的惩罚，这个是不希望前轮转角老打死啥的。
还有频繁前进后退的惩罚，加上这个就会减少前进后退，这个我实验中出现较少，展示不了了。