hybrid A*开发心得

实现细节：

（１）open_list分成两部分，取Ｆ最小值部分使用std::priority_queue(优先队列)，查找部分使用unordered_map，close_list使用unordered_map，进一步来说优先队列在修改值并不刷新堆的结构，最好自己实现优先队列；

（２）根据节点到最近障碍物的距离和到最近维诺边的距离，灵活变动下一步拓展子节点的新距离;

（３）Ｇ值不仅仅是走过的距离，还包括前后节点方向的改变，转向角的改变，转角变化率，是否倒车，到最近障碍物的距离;

（４）H值部分分成三部分：

　　　1.带运动学模型的dubins或者RS曲线长度，可以根据搜索网格大小提前做成距离表，通过空间换时间的方式加速计算;

　　　2.带碰撞检测的搜索路径长度，比如使用Ａ*以终点作为起点，计算出到网格所有位置的长度，或者使用DP计算距离，或者采用泡泡算法计算距离，核心要加速运算速度;

　　　3.欧式距离或者曼哈顿距离;

（５）在搜索过程中使用dubins或者RS连接终点，一旦连接成功并且通过碰撞检测就结束搜索，可以将该尝试过程变成变时间的，距离终点越远频次越低，终点附近加大频次，dubins或者RS在连接点处曲率不连续，需要引入螺旋线进行改善；

（６）使用共轭梯度算法生成的曲线进行优化，主要优化条件有：惩罚到障碍物距离，惩罚曲率，惩罚相邻断点，惩罚维诺图信息；

（７）曲线平滑之后如果无法通过碰撞检测，可以对碰撞点进行锁定，即在碰撞位置使用原始点，最坏的结果是没有进行任何平滑；

工程化结果：

（１）使用Ａ*计算Ｈ值，作为cost_map能够有效降低hybrid A*的搜索时间，但是Ａ*较耗时：

　　　－使用欧式距离做Ｈ值：hybrid A*搜索时间50~70ms，算法总时间50~70ms

　　　－使用Ａ*计算Ｈ值：hybrid A*搜索时间30~40ms，算法总时间550~590ms(Ａ*计算Ｈ值耗时)

（２）open_list在更新open_list的时候，std::priority_queue(优先队列)并不会实时更新，只能在下一次pop或者insert的时候才改变，可能造成拿到的是次优值，可以在更新的时候pop一次top，然后在进行insert，保证下一次拿到的一定是最小值；