学习笔记5--车用地图与导航技术(下)_静态路径规划和动态路径规划

本系列博客包括6个专栏，分别为：《自动驾驶技术概览》、《自动驾驶汽车平台技术基础》、《自动驾驶汽车定位技术》、《自动驾驶汽车环境感知》、《自动驾驶汽车决策与控制》、《自动驾驶系统设计及应用》。
此专栏是关于《自动驾驶汽车决策与控制》书籍的笔记.

1.车用地图与导航技术

1.2 车用地图与导航技术

1.2.3 路径规划算法分类

• 路径规划是帮助驾驶员在旅行前或旅行中规划行驶路线的过程，路径规划最终目的是实现无人驾驶智能车在有障碍物的环境中快速、准确地找到一条无碰撞路径，最终达到目标点；
• 根据不同的规划目的，路径规划的方式分为：一种用于大型车队的调度和进行交通管制的多汽车路径规划；另一种是广泛应用于各种汽车导航系统的单汽车路径规划；单汽车路径规划包括汽车导航系统中的路径规划，主要解决的问题：在一个道路网络中，寻找从起始点到目标点之间的最佳路径；根据在实际应用的不同需求，很多优化标准都可以应用于汽车的路径规划，如：最少行车费用、最短行车时间、最短行车距离等；
• 路径规划流程：
  
  路径规划前先确定路径规划模型，然后收集各种信息数据，从收集的信息数据中提取对解决问题有用的信息，抛弃无用的、不相关的信息；在数学模型约束条件下对提取后的信息数据进行计算，得到所需要的路径；
• 路径规划分为：静态路径规划和动态路径规划
  • 静态路径规划：主要以静态道路交通信息为基础的路径规划；
  • 动态路径规划：主要以动态交通信息来确定路权大小，以起始点和终止点间的交通阻抗最小为原则确定路径规划的最小代价路线；交通阻抗定义根据实际应用不同，采取不同的阻抗标准，如：最短行车距离、最少旅行时间、最低通行收费等；距离、时间、收费等信息可存储在数字道路地图图层的道路属性中，最终计算道路网络中两点之间的最优路径问题归结为图论中求解带权有向图的最短路径问题；
• 最短路径问题分类：
  
• 自动驾驶汽车全局路径规划问题本质：在已知地图或未知地图前提下的最优路径规划问题；将路径规划算法大致分为基于图的方法及基于采样的方法两类；
• 路径规划算法发展过程：
  

1.2.4 $Dijkstra$算法

• $Dijkstra$算法可以给出从指定结点到图中其他结点的最短路径，及任意两点的最短路径；
• $Dijkstra$算法是一种基于贪心策略的最短路径算法，该种算法原理是按照路径长度逐点增长的方法构造一棵路径树，从而得出从该树的根节点到其他所有结点的最短路径；
• 算法核心思想：设置两个点的集合$S_n$和$T_n$，集合$S_n$中存放已找到最短路径的结点，$T_n$集合中存放当前还未找到最短路径的结点；初始状态时，集合$S_n$中只包含起始点，然后不断从集合中选择到起始结点路径长度最短的结点加入集合$S_n$中；集合$S_n$中每加入一个新的结点，都要修改从起始点到集合$T_n$中剩余结点的当前最短路径长度值，集合$S_n$中各结点新的当前最短路径长度值为原来最短路径长度值与从起始点过新加入结点到达该结点的路径长度中的较小者；不断重复此过程，直到集合中所有结点全部加入集合中为止；
• $Dijkstra$算法过程包括三个循环：第一个循环的时间复杂度为$O(n)$，第二、三个循环为循环嵌套，总的时间复杂度为$O(n^2)$；

1.2.5 $Floyd$算法

• 1962年，Floyd研究并提出一种用于求解带权图中所有结点对之间的最短路径算法，被命名为Floyd算法，亦称插点法；
• 该算法在求解过程中，将以每个结点轮流作为原点，重复执行$N$次Dijkstra算法；基本思想：通过一个图的权值矩阵求出它的每两点间的最短路径矩阵；先从任意一条单边路径开始，所有两点间的距离是边的权，如果两点间没有边相连，则权为无穷大，然后对于每一对顶点$u$和$v$，查看是否存在一个顶点$\omega$使得从$u$到$\omega$再到$v$比已知的路径短，如果是，则进行更新；
• 带权有向图及邻接矩阵：
  
• 该算法核心思路：通过一个图的权值矩阵求出该图中任意两个结点之间的最短路径；
• 若图的带权邻接矩阵为$A=[a(i,j){]}_{n\times n}$，由此开始，进行$n$次递归并更新，即由矩阵$D(0)=A$，按照一个公式，建立矩阵$D(1)$；相同地，由$D(1)$构造出$D(2)，\dots ；$最后由$D(n-1)$构造出矩阵$D(n)$；矩阵$D(n)$的第$i$行第$j$列元素，是从$i$号到$j$号结点的最短路径长度，$D(n)$称为图的距离矩阵，同时可以引入一个包含后续结点的矩阵，用来记录任意两点间的最短路径；
• 算法的基本过程：用邻接矩阵$A$来表示一个图，若从$v_i$到$v_j$有路可达，则$A[i,j]=d,d$表示该路段的长度；否则$A[i,j]$为空；
• 定义矩阵$D$，记录的是所插入的点的信息，$D[i,j]$表示从$v_i$到$v_j$需要经过的点，初始化$D[i,j]=j$；把各个顶点插入图中，比较插点后的距离与原来的距离，$A[i,j]=\min (A[i,j],A[i,k]+A[k,j])$，若$A[i,j]$的值变小，则$D[i,j]=k$；
• 在矩阵$A$中包含两点间的最短道路的信息，在矩阵$D$中包含最短路径的信息；

1.2.6 $A^{\ast }$算法

• 状态空间搜索是在一定的状态空间中，寻找从初始状态到目标状态的路径的过程；在求解过程中存在很多分支，求解条件的不确定性和不完备性使得最终计算得到的路径有很多条，这些路径组成一个图，这个图就是状态空间；问题的求解实际就是在这个图中寻找一条路径，可以从初始点顺利到达目标点，这个寻找路径的过程就是状态空间搜索；

• 常用的状态空间搜索包括：深度优先搜索和广度优先搜索；

  • 广度优先搜索算法亦称宽度优先搜索算法或横向优先搜索算法，是一种图形搜索算法；该算法是一种盲目搜索法，从初始结点逐层搜索，将遍历图中所有结点来寻找目标结点；
  • 深度优先搜索算法是按照一定的顺序先查找一个分支，尽可能深度地搜索该分支，直到遍历该分支的结点，若此时图中还有未被搜索过的分支，则继续遍历其他分支，直到找到目标点；这个遍历图的过程实际上是查找每个顶点或弧的邻接点的过程；

• 启发式搜索：在状态空间中搜索，同时在搜索过程中加入与问题有关的启发式信息，引导搜索朝着最优的方向前进；该方法会评估每一个结点，通过比较搜索到的结点的评估值选择出最好的结点，然后将这个最好的结点作为下一次搜索的起始点，沿着搜索的方向继续搜索，直到搜索到目标点；

• $A^{\ast }$算法是建立在$Dijkstra$算法基础上的启发式搜索算法，多应用于实现道路网的最佳优先搜索；该算法特点：在选择下一个搜索结点时，通过引入多种有用的路网的信息，计算所有的候选结点与目标点间的某种目标函数，如：最短行车距离、最短行车时间、最少行车费用等，以此目标函数值为标准来评价该候选结点是否为最优路径应该选择的结点，符合所选择的最优目标函数的候选结点将优先选择为进行下一个搜索的起点；

• $A^{\ast }$算法的关键：确立如下形式的启发式估价函数：
  $$f^{\prime }(n)=g(n)+h^{\prime }(n)$$
  其中：$g(n)$是从起点$s$到候选结点$n$的实际代价；$h^{\prime }(n)$是从候选结点$n$到目标点$D$的估计代价；

  必须保证$h^{\prime }(n)\le h^{\ast }(n)$，其中$h^{\ast }(n)$表示结点$n$到目的地结点的实际最小代价；

  该算法在搜索过程中，优先搜索$f^{\prime }(n)$值最小的结点；

• $A^{\ast }$算法在搜索过程中会建立两个表：OPEN表和CLOSE表；OPEN表中存储已经生成但还没有被扩展的结点，CLOSE表中存储已经被扩展的结点；每扩展一个结点，都要计算其代价值，若新扩展的结点已经存在于OPEN表中，则比较这两个结点的代价值，用代价值小的点替换代价值大的点，每次扩展一个新结点，都会根据所采用的启发式信息进行排序；

• 设初始结点为$S$，目标结点为$T$，则搜索由$S$到$T$的最优路径的具体步骤如下：

  • 建立空的OPEN表和CLOSE表；把起点$S$放入OPEN表中，CLOSE表为空，此时其他结点与起始结点$S$的距离为$\infty$；
  • 如果OPEN表为空，则搜索失败，算法结束；否则扩展$S$结点，选取OPEN表中$f^{\prime }$值最小的结点，并将该结点从OPEN表移至CLOSE表中，同时判断该结点是否为目标结点；若是目标结点，则从该结点回溯，即从该结点的后向指针一直到初始结点遍历结点获得最优路径，算法结束；若该结点不是目标结点，则继续扩展下一结点；
  • 依次扩展$S$结点后，扩展$S$结点得所有后继结点组成集合$A$，遍历$A$中的结点；如果存在某一结点既不在OPEN表中，也不在CLOSE表中，将该结点放入OPEN表中，同时计算该结点的估价值，并对该结点的代价值与已经存在于OPEN表或CLOSE表中的结点代价值比较，若该结点的代价值小于其他两个估价值，则更新OPEN表中的代价值及其父节点；
  • 根据所选取的估价函数计算各点的估价值，并按照估价值递增的顺序，对CLOSE表中的所有结点进行排序，这些结点的扩展过程就是通过算法计算得到的最优路径，算法结束；

• 算法流程图：
  

1.2.7 $RRT$算法

• RRT算法既是一种算法，也是一种数据结构，被设计用来高效地在高维空间中进行搜索；特别适合用于在设计非完整约束场合下的路径规划问题；
• RRT算法为一种递增式构造方式，在构造过程中，算法不断在搜索空间中随机生成状态点，如果该点位于无碰撞位置，则寻找搜索树中离该结点最近的结点为基准结点，由基准结点出发以一定步长朝着该随机结点进行延伸，延长线的终点所在位置被当作有效结点加入搜索树中；这个搜索树的生长过程一直持续，直到目标结点与搜索树的距离在一定范围内终止，随后搜索算法在搜索树中寻找一条连接起点和终点的最短路径；
• 常用有向图表示路径$G=(V,E)$，一条可行的路径就是一个顶点的序列$(v_1,v_2,\dots ,v_n),v_1=q_{init}$,$v_n=q_{goal}$；同时各个顶点属于集合$E$；问题变成了使用采样点来扩展图$G$，使之能找到一条从初始结点到达目标结点的路径；
• RRT算法伪代码：
  

  • 步骤：

    • 初始化顶点为$q_{init}$，边集$E$；
    • 进入$while$循环，迭代$N$次停止；
    • $Sample(i)$采样一个新的点$q_{new}$；
    • 利用新的点扩展图$G$；

  • RRT算法Extend()函数步骤：

    • 把$V,E$暂存；
    • $Nearest(G,q)$函数表示求图$G$中离$q$欧式距离最近的点$q_{nearest}$，一般情况下采用$kd-tree$来存储图中的结点，这样节约搜索时间；
    • $Steer(q_{nearest},q_{new})$表示存在一个$q_{new}$点，将最小化$||q_{new}-q||$，但$||q_{new}-q||<\eta ，\eta$为人为设定的一个值，即向$q$方向步进了一段距离；
    • 将$ObstacleFree(q_{nearest},q_{new})$进行碰撞检测，然后判断这一段$(q_{nearest},q_{new})$路径是否与障碍物发生碰撞，即判断路径是否属于$C_{free}$中；
    • 把$q_{new}$加到顶点集中；
    • 返回扩展后的图$G^{\prime }$；

1.2.8 路径规划算法发展

• 与汽车动力学结合；
• 与状态参数估计结合；
• 与机器学习结合；