【规划】常用算法大汇总

一、图搜索

参考[链接]。

图包含节点和边，分为无向图、有向图、权重图等。在自动驾驶中，通常可以将地图存储为栅格地图。

1. DFS&BFS、GBFS

提到图搜索，不得不介绍两种最基本的路径搜索算法：深度优先搜索（DFS）和广度优先搜索（BFS）。关于这两种算法，可以看我之前的blog，有详细介绍。DFS是一条路走到底，最后访问的节点最先拿来处理，整个过程可以用栈（stack）来表示，符合“后进先出”的原则；而BFS是一层中先访问的节点拿来处理，可以用队列（queue）来表示，符合“先进先出”的原则。

对比而言，BFS可以找到最短路径，DFS速度快，但随机性大，生成路径不是最优。因此，目前主流的基于图搜索的规划算法，原理都是基于BFS延伸出来的。

但BFS遍历的无效节点过多，从而导致搜索效率太慢。针对此缺陷，引入了启发式搜索（heuristic search），即加入终点信息，从而使得搜索的目标更明确，避免过多的无效搜索，即GBFS（Gready Breath-First Search）。GBFS将与终点的距离考虑进来，构造一个规则来挑选依次要访问的节点。与终点的距离有多种形式，最常用的三种为Euclidean Distance、Manhattan Distance以及Diagonal Distance。

2. Dijkstra算法和A*算法

Dijkstra算法（1959年）： 考虑起点到当前节点的代价，从小到大依次遍历，然后根据父节点找到最优路径。原始论文：A Note on Two Problems in Connexion with Graphs

A*算法（1968年）：在Dijkstra的基础上加入启发式函数h(n)，即从n节点到目标点的最小代价评估值（标准的启发式函数是曼哈顿距离）。原始论文：《A Formal Basis for the Heuristic Determination of Minimum Cost Paths》。参考资料：https://blog.csdn.net/autonavi2012/article/details/80923431

Dijkstra算法其实是BFS的进阶版，可以用于处理带权重边的地图。
A算法和Dijkstra算法的唯一区别在于A在存储节点时，还会考虑和终点的距离，f(n) = g(n) + h(n)，其中h(n)为启发项。A*算法可以更快地找到终点，避免了许多的无效搜索。

此外，Dijkstra由于无差别的搜索可以保证最短路径。A* 算法为了保证最优性，需要保证启发函数的值小于等于实际上该点到终点的距离。因此如果我们需要A*给出最短路径的话，我们可以将启发函数设定为欧式距离或者对角距离，而不是曼哈顿距离。

3. A*变种

3.1 混合A*算法

（2008年）在A *的基础上，将离散空间变为连续空间。采用两个启发式函数，分别为Non Holonomic Without Obstacles 和 Obstacles Without Holonomic，取最大值作为代价值。前者在快到终点时可以快速到达终点，后者对于U型弯道或者死角效果很好。
参考资料：https://www.cnblogs.com/youngsea/p/14833592.html
https://blog.csdn.net/gophae/article/details/107344216
https://zhuanlan.zhihu.com/p/144815425（Voronoi Field讲得很好）
https://zhuanlan.zhihu.com/p/139489196（视频很直观）
原始论文：《Practical Search Techniques in Path Planning for Autonomous Driving》
《Autonomous Driving in Semi-Structured Environments: Mapping and
Planning》(2009年)

3.2 LPA*算法

增量式搜索，对以前的搜索结果信息进行再利用来实现高效搜索

3.3 其它

• D*算法（1994年）：原始论文：《Optimal and Efficient Path Planning for Partially-Known Environments》
• AD*算法
• Theta*算法
• lazy Theta*算法
• 双向探索A*算法

总结：基于图搜索的规划算法总是能够给出一个全局范围内的最优解，而以此为代价的就是当地图过大，规划的维度过高时，它的搜索效率低。

附一张很完整的时间图，链接为[点击]。

二、采样

参考[链接]。有时我们需要为了效率牺牲路径的最优性，一般会采用基于采样的算法。

采样派讲究采样的有效性、离散度，是否能增加算法的相对完备性。

1. 随机性采样

随机采样就是用随机函数生成的位置坐标，并用碰撞检测算法去除有碰撞风险的采样点。如今在特定的空间里面进行确定性采样并用代价函数来评价的规划算法已经盖过了随机采样算法的风头。

1.1 PRM算法

经典算法之PRM（Probabilistic Road Map）主要包含了两个步骤，一是学习阶段，二是查询阶段。在学习阶段中，其会在地图空间中进行均匀的随机采样，并删除采样落在障碍物上的点，接着对相邻的点进行连接并做碰撞检测，剔除不是collision-free的连线。而在查询阶段中，就是利用上一步构建好的采样节点及连续，运用图搜索的方法（Dijkstra或者A*）来找出一条可行路径。

其生成效率高，但不适用于有狭窄通道的场景。

1.2 RRT算法

经典算法之RRT(Rapidly-exploring Random Tree)是目前机器人领域运用最广泛的一种算法，其变种也有许多。RRT的算法流程：首先对地图进行随机采样，得到采样点x_rand。再从已有的树中找到距离x_rand最近的点x_near，从x_near往x_rand的方向行驶距离StepSize得到x_new，并将x_near与x_new的连线添加进一个临时变量中。接着对这段路进行碰撞检测，如果无碰撞，则将x_new添加进树中。如此不停循环，直到x_new到达期望的终点时结束。

RRT算法不再需要PRM算法的搜索步骤，效率更高；并且通过增量式扩展方式，找到路径后就立即结束，搜索重点的目的性更强。但只能给出一条可行解，无法保证最优性，对于narrow passage效率很低。（在全场景下进行采样没有必要，可以更加有针对性地采样）

1.3 RRT算法的变种

(1) Bidirectional RRT / RRT Connect

分别从起始点和目标点构建搜索树，直到两棵搜索树相交，找到一条可通行的路径（双回溯）。这种思路可以很好地解决由于狭窄通道采样概率较低的问题。

(2) RRT *

在RRT算法中，每次都会选择距离最近的点作为新加入点的父节点，但显然这种做法不一定是最合理的。而RRT*所进行的改变主要为两点：重选父节点和重连接。

它在采样点加入进树结构之后，以其为圆心再画一个圆，在这个圆之内，搜索是否有其余点与采样点连接后，能够使得起点到采样点的距离更短。如果有的话，就对采样点的父节点进行更新，并重连采样点与其新的父节点。RRT * 的优势就在于，不同于RRT算法只找到一条可行路径就结束，它会不停的循环迭代，不停地更新当前路径，因此只要时间足够长的话，RRT*是可以求得最短路径的。

(3) kinodynamic-RRT*

上面讲的RRT类都是简单的将两点进行连线，这样导致最终生成的路径会相当不平滑，甚至相邻的线段都是无法满足机器人的动力学要求的。也就是说虽然规划出了路径，但是给到控制模块却没有办法执行。因此，该变种算法不同于直线连接，而是通过曲线、多项式等来连接x_near和x_new，从而使得生成的路径能够符合车辆动力学的相关约束。

(4) Any-Time RRT*

会不停地更新路径，不停地以当前位置作为规划起始点来重新规划路径。这种方式可以更好地应对动态环境变化比较多的场景。

(5) Informed RRT*

RRT* 在找到一条路径之后，继续不停地在全局空间中进行重采样从而更新路径。但是在已经找到一条路径的情况下，再在全局范围内随机撒点显然不是一种高效的行为，而Informed RRT*的主要思想就是在生成一条初始路径后，构建出一个椭圆区域，然后在椭圆区域内进行随机采样并更新路径，这样就避免了在无效区域内的浪费时间，从而极大提升了路径优化的效率。

2. 确定性采样

参考[链接]。确定性采样就是人为地设置某种采样规则来进行采样，比如在行车道里面可以沿着道路中心线的法向量采样，采样点的航向角、曲率和道路中心线的切向量有关。

2.1 控制空间采样

基于车辆的运动学模型，已知车辆位姿、速度和横摆角速度，根据车辆的性能约束在速度和角速度空间里面均匀采样，前向模拟，然后程序给这些轨迹打分。所以一般可以提前构造离线的采样轨迹，等程序运行时调用即可。求顺解。

2.2 状态空间采样

控制空间采样难以对环境约束有较好地处理，很多采样是无效的，并且离散度问题和相对完备性问题不好处理，所以状态空间采样开始流行起来。

状态空间采样和控制空间采样大体相似，不过它不是在速度空间里面采样，而是均匀采样目标点，采样目标点后，也就成了两点边值问题，为了从起点到达目标点，我们需要求逆解。那么怎么求逆解呢，我们知道求逆解是一个有点复杂的问题，常见的有使用贝塞尔曲线，回旋曲线，以及近年来大火的MPC等等。求出了单条轨迹后，再均匀采样目标点，那么也就有了多条轨迹，接下来用代价函数选择一个最优的就行了。

state lattice采样根据状态空间采样，无限向前连接，也就相当于把连续的构型空间给离散化了成为lattice空间，然后再基于图搜索算法，对于这种带权有向无环图，使用动态规划。

再附一张很完整的算法时间图，出自[链接]：

三、优化

常用的有多项式曲线、贝塞尔曲线、B样条曲线去进行插值。
【之后有空再补全这一部分】