自动驾驶（六十二）---------导航路径规划

    实际上关于路径规划，我在轨迹规划中有介绍，主要是A*算法，但是目前有强需求，所以只能在系统总结一下了，可以算是水一篇文章，抓紧时间吧，有效的时间不多了！

      导航路径规划需要掌握的方法我总结有四个：RRT、PRM、A*、hybrid A*。A*我之前有介绍：再论轨迹规划，这里就不做赘述。

1. RRT

       RRT（快速探索随机树），首先在环境中，我们有一个起始点，定义为Xinit, 然后我们在环境中随机撒一个点，得到点x_rand,如果x_rand不在障碍物区域，则连接起x_init和x_rand, 我们得到一条连线L,如果L整个不在障碍物里面，则沿着L,从x_init向x_rand的方向移动一定的距离，得到一个新的点，x_new,则x_init,x_new和他们之间的线段构成了一颗最简单的树。

       树的扩展： 在开始的基础上，继续重复，在环境中撒点，得到无障碍物区域的点x_rand,然后在已经存在的树上找一个离x_rand最近的点x_near,连接两个点，如果这条线没有障碍物，则沿着这条线，从x_near到x_rand移动一定的距离，得到新的点，x_new, 该点被添加到已经存在的树上

       规划： 重复上述过程，直到目标点（或其附近的点）被添加到树上，这时我们就可以在树上找到一条从起点到目标点的路径

2. PRM

      RRT当环境中的障碍物较为复杂时计算量较大，有没有什么办法优化呢？PRM(Probabilistic Roadmaps)是一种基于图搜索的方法，一共分为两个步骤：学习阶段、查询阶段。它将连续空间转换成离散空间，再利用A*等搜索算法在路线图上寻找路径，以提高搜索效率。

      学习阶段：在给定图的自由空间里随机撒点，构建一个路径网络图。  必须是自由空间里的随机点，每个点都要确保机器人与障碍物无碰撞。

      查询阶段：根据设定的起点s和终点g，选择合适的路径，首先将s和g点与路径网络中的两个点x,y分别连接，寻找无向路径网络图中x与y连接的路径，这样就可以将起点和终点连接起来，构成全局路径。得到全局路径后，可以使用平滑的方法寻找捷径，优化路径。

3. Hybrid A*

       A*算法主要是用在导航中，在车辆轨迹规划中并不好用，这是因为生成的轨迹是折线，这不满足车辆运动学特性，但是A*算法也同样是从一个点到另一个点的一条最优路径，有没有办法转化成车辆可以行使的轨迹线呢？

      2010年，斯坦福首次提出一种满足车辆运动学的算法(Hybrid A*),并在(DARPA)的城市挑战赛中得以运用。首先我们采用A*生成一天折线轨迹，节点之间不采用直线相连，而是Reeds-Shepp曲线相连，当然每个节点与节点相连的曲线，要考虑与之相连的节点状态。这样生成的轨迹就是Hybrid A*。那么什么是Reeds-Shepp呢？请看下节：

4. Reeds-Shepp

      APA自动泊车可以有各种轨迹实现，如何用最短路径来实现自动泊车呢？首先假设车辆能以固定的半径转向，且车辆能够前进和后退，那么Reeds-Shepp曲线就是车辆在上述条件下从起点到终点的最短路径。在轨迹规划中不仅要求车辆能够到达终点，而且需要车辆的角度也有要求，比如在垂直泊车的过程中，开始车辆平行于道路，终点要求车辆垂直于道路等等，总之车辆的终点位置和终点角度都提出了要求。

      首先我们回到车辆运动学：

                                其中：

       这种固定的运动方式在低速情况下轨迹可以近似一个圆，方向盘转角转到最大，其转向半径最小，假设最小转向半径为rmin。为了方便起见Reeds-Shepp中最小转向半径强制设置为1，如果车辆的实际最小转向半径不是1，可也通过适当放缩终点坐标来计算该曲线。

      下面曲线就是一条Reeds-Shepp曲线，R表示右打方向，+表示前进，图中从到 ，首先右打方向前进，再左打方向后退，最后右打方向前进到达终点。

      下表是Reeds-Shepp曲线的基本操作方式，一共有48种操作模式，归为9种Base word，但是在编程求解我们不需要每一种都分开求解，其中他们之间的对称性可以帮助我们减少工作量。例如求解到终点  的  后，我们可以用同样的方法来求解 但是需要将终点换为  ，这种处理方法在文中叫timeflip，还有其他的f对称，这里不做介绍可以参看原文。

      通过这几种组合可以得到最短路径。这里举个例子，从不同位置到达同一目标的Reeds-sheep: