无人驾驶之路径规划：Dijkstra , A*, D*, D* Lite算法

无人驾驶系统概述

上图为一无人驾驶车辆的简易系统框图。无人车辆首先通过摄像头，雷达之类的传感器来感知外部的信息，借助GPS/IMU等设备来确定自车在地理上的绝对位置以及姿态。紧接着，软件算法层面会将这些收集到的原始数据进行分步地处理。比如通过传统的机器视觉或者深层神经网络等算法来对车道线、障碍物、交通标识等信息进行分类识别以及追踪；当然也有相应的定位算法如卡尔曼滤波来提升从定位设备获取的车辆位姿的可靠性与准确性。目前感知融合比较火热的原因也是在于各个单一信息源给予的数据存在各自的不足，而将这些数据做融合处理，不仅符合我们对系统的冗余要求，更是可以提升其鲁棒性。感知以及定位算法将上述这些来自传感器的数据进行一定处理的过程，本质上就是针对现实时空做了到数学时空的提取及映射。这些经过提炼后的更精细的数据，更方便我们进一步地进行功能开发。虽然目前感知和定位算法方面还存在许多比如精度不高、无法在全场景下适用等的技术瓶颈，但是我觉得无人驾驶的算法核心还是决策规划层面。这部分算法的功能就如字面所述，相当于无人车的大脑，起到居中调度的作用。
路径规划模块会根据高精度地图以及车辆当前位姿规划出一条能抵达目标终点且时空最优的全局道路序列。而在车辆实际行驶过程中，主要发挥作用的就是行为决策以及轨迹规划了。
其中，行为决策需要综合考量来自感知、定位、高精度地图以及全局规划的信息，结合当地的交通法规和当时的实际状况，有针对性地下达无人车在下一周期内应该做出的动作指令。比如当我们的无人车来到十字路口时，检测到信号灯为红灯，那么它应该下达一个停车指令；若是本车察觉到本车道的前车速度较缓而隔壁车道空置时，可以下发一个换道超车的指令；当它发现由于发生突发事故，导致按原先规划出的全局路径无法通行时，可以向路径规划模块请求进行重新规划。而上面提到的具体的变道、超车、停车等动作的实现就要依靠轨迹规划了。这个模块和路径规划最大的区别在于，轨迹实际上是包含了时间信息且满足车辆运动特性的路径。即车辆不仅需要规划出完成对应决策指令所需要的空间上的一系列离散点，更是隐含着自车应在何时刻，抵达哪一个位置的要求。也只有这样才能在预测模块的帮助下，满足无人车辆躲避动态障碍物（行人、其余车辆等）的需求。而在有了实时规划出来的轨迹之后，下游的控制模块便可以将其转化为车辆的控制信号（转向、加减速）下发给底层的执行模块。

路径规划

路径规划的任务是找到一系列最优的动作（actions）使得智能体（agent）能从它的起始状态（state）一步步过渡到它被期望的目标状态。在无人驾驶中，上面提到的智能体就是我们的无人车辆，而它的状态主要指车辆所处的地理位置，航向等信息。车辆在两个相邻状态之间过渡（transitions）采取的动作会伴随着一个由人为设计好的损耗值（cost）。那么我们就可以将规划出来的路径的好坏用数学语言来定量地描述评估，一条好的路径的总损耗值一定是比较低的。如果一个规划算法总能找到一条可行路径（如果存在的话），那么我们称此算法是概率完备的；如果找出来的路径总是最优的，那么我们可以认为这个算法是最优的。

一般来说，规划算法可概括为两大类：确定性(deterministic)的基于启发式(heuristic)算法如A*和随机化(randomized)的基于概率的算法如RRT。当我们需要处理的规划问题维度不高时，确定性的规划算法由于能在有限范围内保证解的质量，因此深受广大人民群众的欢迎。

规划全局导航路径可转换为在一给定地图上进行搜索的问题。多数的图搜索算法具备基于有向权重图，计算最小代价的能力。

$A^{\ast }$ 以及 Dijkstra 算法

这两种算法的基本思想是一致的，需要创建OPEN（保存已生成但未访问过的节点）与CLOSE（保存已访问过的节点）两张表。
具体流程可概括如下：

1. 将与起始点相邻的可行节点放入open表中
2. 选出代价值F最低的节点放入close表，找出该节点所有的相邻可行节点。
3. 计算2中找出的所有可行节点的代价F，并把这些节点放入open表中。
4. 重复步骤2、3直到找到目标节点（规划出最优可行路径），或者open表为空（找不到可行路径）
   伪代码：
   
   两种算法的区别在于代价函数F的计算方式。
   Dijkstra是在广度优先搜索算法的基础上引入了从起始点到当前节点的代价。
   即:$F=g$
   $g$一般设计成起始点到当前点的距离花费。可以看到本算法实际上还是一种无方向偏向性的遍历，虽然可以保证在有可行解的前提下找到那条最优路径，但是其运行效率不高。
   $A^{\ast }$算法的代价函数设计充分利用了目标节点相对于当前位置这一关键信息。在Dijkstra的基础上引入了启发式代价的计算。
   即:$F=g+h$
   其中$h$为当前节点到目标节点的欧式距离。因此，本算法会优先朝着目标节点的方向进行搜索，在多数情况下，会极大地节省计算资源。

$D^{\ast }$算法 以及 $D^{\ast }$ Lite 算法

上文中提及的$A^{\ast }$ 算法实际上只适用于已经给定了从起始点到终点范围的明确地图或规划空间的情况。然而在很多实际应用场景中，智能体并不完全清楚自己周边的环境信息，或者只拥有一张不完整的地图。那么如果我们基于起始状态的地图进行规划得到的路径大概率是不正确的或是次优的。因此要求智能体具备根据最新状态来实时更新地图以及重规划的能力。一个简单的方法是每次收集到新的信息后就根据更新的地图进行一次上文中的$A^{\ast }$。但是这样对计算资源的损耗与浪费无疑是极大的，尤其是当更新信息不会实际影响到当前求解出的最优路径时。所以更优的方法应该是基于上一个周期的规划路径结合新的更新信息做一定的修复改变。这类算法就叫做增量式的重规划算法，$D^{\ast }$(Dynamic $A^{\ast }$) 则是其中的代表之作。火星探测器漫游者号上就搭载了基于本算法开发的导航系统，来应对火星上随着机器人移动过程中遇到的动态变化的环境带来的挑战。本节中的重点$D^{\ast }$ Lite可以理解为是继承了$D^{\ast }$思想的简化版算法，但是具有相较于原版而言更高的运行效率。
伪码：