Decision and Planning [4]
Jian YE决策规划（四）行为决策常用算法
满足两个要求: 安全性和舒适性
运动规划生成的轨迹是一种由二维空间和一维时间组成的三维空间中的曲线，是一种偏实时的路径规划。
PRM
概率路标法 (Probabilistic Road Maps, PRM），是一种经典的采样方法，由Lydia E.等人在1996年提出。PRM主要包含三个阶段，一是采样阶段，二是碰撞检测阶段，三是搜索阶段。
采样阶段: 在采样阶段中，PRM首先在地图空间进行均匀的随机采样，也就是对地图进行稀疏采样，目的是将大地图简化为较少的采样点。
碰撞检测阶段: 剔除落在障碍物上的采样点，并将剩下的点与其一定距离范围内的点相连，同时删除穿越障碍物的连线，从而构成一张无向图。
搜索阶段: 利用全局路径规划算法章节介绍的搜索算法（Dijkstra、A*等）在无向图中进行搜索，从而找出一条起点A到终点B之间的可行路径。
算法步骤可以总结为： （1）构造无向图G =（V，E），其中V代表随机采样的点集，E代表两采样点之间所有可能的无碰撞路径，G初始状态为空。 （2）随机撒点，并选取一个无碰撞的点c(i)加入到V中。 （3）定义距离r，如果c(i)与V中某些点的距离小于r，则将V中这些点定义为c(i)的邻域点。 （4）将c(i)与其邻域点相连，生成连线t，并检测连线t是否与障碍物发生碰撞，如果无碰撞，则将t加入E中。 （5）重复步骤2-4，直到所有采样点（满足采样数量要求）均已完成上述步骤。 （5）采用图搜索算法对无向图G进行搜索，如果能找到起始点A到终点B的路线，说明存在可行的行驶轨迹。
PRM算法相比基于搜索的算法，简化了环境、提高了效率。但是在有狭窄通道场景中，很难采样出可行路径，效率会大幅降低。
RRT
快速探索随机树（Rapidly Exploring Random Trees，RRT），是Steven M. LaValle和James J. Kuffner Jr.在1998年提出的一种基于随机生长树思想实现对非凸高维空间快速搜索的算法。
与PRM相同的是两者都是基于随机采样的算法，不同的是PRM最终生成的是一个无向图，而RRT生成的是一个随机树。RRT的最显著特征就是具备空间探索的能力，即从一点向外探索拓展的特征。
RRT分单树和双树两种类型，单树RRT将起点作为随机树的根节点，通过随机采样、碰撞检测的方式为随机树增加叶子节点，最终生成一颗随机树。而双树RRT则拥有两颗随机树，分别以起点和终点为根节点，以同样的方式进行向外的探索，直到两颗随机树相遇，从而达到提高规划效率的目的。
对于单树RRT算法，我们将起点A设置为随机树的根，并生成一个随机采样点，如图27所示，随机采样点有下面这几种情况。 （1）随机采样点1落在自由区域中，但是根节点A和随机采样点1之间的连线存在障碍物，无法通过碰撞检测，采样点1会被舍弃，重新再生成随机采样点。 （2）随机采样点2落在障碍物的位置，采样点2也会被舍弃，重新再生成随机采样点。 （3）随机采样点3落在自由区域，且与根节点A之间的连线不存在障碍物，但是超过根节点的步长限制。但此时这个节点不会被简单的舍弃掉，而是会沿着根节点和随机采样点3的连线，找出符合步长限制的中间点，将这个中间点作为新的采样点，也就是图29中的4。
接着我们继续生成新的随机采样点，如果新的随机采样点位于自由区域，那么我们就可以遍历随机树中已有的全部节点，找出距离新的随机采样点最近的节点，同时求出两者之间的距离，如果满足步长限制的话，我们将接着对这两个节点进行碰撞检测，如果不满足步长限制的话，我们需要沿着新的随机采样点和最近的节点的连线方向，找出一个符合步长限制的中间点，用来替代新的随机采样点。最后如果新的随机采样点和最近的节点通过了碰撞检测，就意味着二者之间存在边，我们便可以将新的随机采样点添加进随机树中，并将最近的点设置为新的随机采样点的父节点。
重复上述过程，直到新的随机采样点在终点的步长限制范围内，且满足碰撞检测。则将新的随机采样点设为终点B的父节点，并将终点加入随机树，从而完成迭代，生成如图30所示的完整随机树。
相比PRM，RRT无需搜索步骤、效率更高。通过增量式扩展的方式，找到路径后就立即结束，搜索终点的目的性更强。但是RRT作为一种纯粹的随机搜索算法，对环境类型不敏感，当地图空间中存在狭窄通道时，因被采样的概率低，导致算法的收敛速度慢，效率会大幅下降，有时候甚至难以在有狭窄通道的环境找到路径。
图31展示了 RRT应对存在狭窄通道地图空间时的两种表现，一种是RRT很快就找到了出路，一种是一直被困在障碍物里面。
围绕如何更好的“进行随机采样”、“定义最近的点”以及“进行树的扩展”等方面，诞生了多种改进型的算法，包括双树RRT-Connect（双树）、lazy-RRT, RRT-Extend等。 PRM和RRT都是一个概率完备但非最优的路径规划算法，也就是只要起点和终点之间存在有效的路径，那么只要规划的时间足够长，采样点足够多，必然可以找到有效的路径。但是这个解无法保证是最优的。 采用PRM和RRT等随机采样算法生成的行驶轨迹，大多是一条条线段，线段之间的曲率也不不连续，这样的行驶轨迹是不能保证舒适性的，所以还需要进一步进行曲线平滑、角度平滑处理。代表算法是基于曲线插值的方法：RS曲线、Dubins曲线、多项式曲线、贝塞尔曲线和样条曲线等。
所有基于曲线插值方法要解决的问题就是：在图32上的若干点中，求出一条光滑曲线尽可能逼近所有点。下文以多项式曲线和贝塞尔曲线为例，介绍曲线插值算法的示例。
多项式曲线
支付宝
微信