路径规划算法学习笔记1——基于搜索
基于搜索
BFS
Dijkstra
A*
Hybrid A*
参考文献
在这里分享了路径规划方面的一些基本的算法原理和伪代码实现，主要包括基于搜索、基于采样、基于曲线插值和基于人工势场等四方面，计划每篇博客单列一类，其中内容可能存在不完善和错误之处，如有读者发现，欢迎批评指正。

基于搜索
第一部分是基于搜索的路径规划算法，这一部分依次介绍BFS、Dijkstra、A* 和Hybrid A*，从顺序上说，这四种算法也是一个不断迭代和优化的过程。另外，有关A* 的其他衍生的算法，日后更新…

BFS
BFS（Breadth-First-Searc），基本思想是以放射状扩散的方式遍历所有点。
图解BFS点这里。
BFS的伪代码解释如下：
1、基本过程

2、为生成路径，引入父节点的定义

3、为避免枚举的问题，引入启发式搜索，以当前点到终点的距离为代价（优先级）

4、路径回放，生成最终路径

5、BFS的一个问题是容易先入为主地认为最先遍历的就是最短路径上的点。

Dijkstra

Dijkstra也采用了BFS类似的方法，其代价的计算方式为以当前点到起点的距离为代价（优先级）。

A*

A*算法结合了BFS和Dijkstra的思想，综合当前点到起点和终点的距离和（代价和/优先级）。

Hybrid A*
A*算法规划的轨迹经过方格中心，现实车辆路径无法满足这一情况，因此提出了Hybrid A *。

1、这里整理了几种车辆的运动学模型：

① 自行车模型的基本假设
·不考虑车辆垂直方向的运动，认为车辆在二维平面上运动
·左右车轮在转速和转角上一致，两个车轮可以合为一个
·车速缓慢，忽略前后轴载荷的转移
·车身和悬架均为刚性
·车辆转向和驱动由前轮完成
② 以后轴中心为原点的车辆运动学模型——输入（a，φ） 输出车辆位姿（x，y，θ）

③ 以质心为中心的车辆运动学模型——输入（a，δ） 输出车辆位姿（x，y，ψ）

④ 前轮驱动的车辆运动学模型——输入（a，δ） 输出车辆位姿（x，y，ψ）

这里假设汽车是前轮驱动的，所以认为方向盘的转角就等于前轮的转角，后轮偏角为0。

2、一种运动学模型下的Hybrid A* 算法伪代码实现
①运动学模型：4-D 离散网格中的车辆状态 <x, y, θ, is_foward>，以后轴为中心的车辆运动学模型（离散）。

②伪代码

③与A*的不同
0x00：二维变三维，新增偏航角这一维
0x01：寻找当前位置周围四点，变成寻找当前位置在不同前轮转角下的多个点（range（-35,40,5））

下面是用QT写的一个简单实现：

参考文献
1、https://www.redblobgames.com/pathfinding/a-star/introduction.html
2、https://blog.csdn.net/AdamShan/article/details/79945175 无人驾驶汽车系统入门（十六）——最短路径搜索之A* 算法
3、https://zhuanlan.zhihu.com/p/103834150 自动驾驶中的车辆运动学模型
4、https://zhuanlan.zhihu.com/p/139489196 自动驾驶运动规划-Hybird A* 算法
5、https://blog.csdn.net/Rickey_lee09/article/details/104795530 无人驾驶学习笔记–路径规划（一）【Trajectory Generation – Hybrid A*】