目录
高精地图定位GNSS RTK定位惯性导航定位激光雷达定位视觉定位 感知检测与分类跟踪分割 预测规划路径规划轨迹生成Frenet坐标路径速度解耦规划ST 图生成路径的优化，Lattice规划 控制PID控制线性二次调节器（LQR）模型预测控制（MPC） 结语本文主要介绍了自动驾驶的几大模块，通过阅读本篇文章，可以对自动驾驶有一个简单的认识。主要参考了:Apollo自动驾驶入门课程。
高精地图
无人驾驶使用的地图不同于汽车导航所使用的地图。由于无人驾驶对于自身位置需要自我感知，因此其使用的地图精度更高。
无人驾驶车不能够像人类一样，简单而又快速识别路上的各种交通标志及可能遇到的交通管制等，因此需要高精度的地图来帮助它。高精度地图包含着大量的驾驶辅助信息，包括道路网的精确三维表征、语义信息等。手机导航的地图精度只有米级的精度，这个精度有时候对无人驾驶来说完全不够用，甚至可能偏离道路中心，与其他车辆相撞。而一般的高精度地图都是厘米级别的精度。
高精度地图帮助定位车辆。车辆将自己感知到的信息与高精度地图相比较，从而判断自身的位置。
高精度地图也可以帮助来感知。任何传感器的探测距离都是有限的，且有可能受到障碍物阻挡。但高精度地图可以提前获知道路的状态，或者根据高精度地图集中资源寻找某一地标，即为感兴趣区域（ROI）。ROI可以帮助我们提高检测精确度和速度。
高精度地图可以提前规划大致的行车路线，从而帮助软件规划路径。同时高精度地图上的各种交通标志、速度限制等也可以辅助规划模块找到最优路径的选择。
定位
定位就是让无人驾驶车明确自身的位置。手机的GPS精度不够，并且在隧道、山林等环境下表现更差，可能定位误差在3~10米。常用方法是利用车辆传感器探测的信息在融合处理之后，与高精度地图进行对比，从而得到自身的位置。为了进行对比，需要在汽车坐标系和地图坐标系之间转换数据。定位提供了许多中方法，如GNSS、RTK、惯性导航、激光雷达定位，视觉定位等等。
GNSS RTK定位
GNSS，全球导航卫星系统。该系统由太空中的卫星、地面的控制站以及接收器组成。如下图所示。接收器并不直接测量你与卫星之间的距离，它首先测量信号的飞行时间，也即卫星传播到接收器需要多长时间。光速c*时间t=距离d。但由于光速c很大，即使时间t只有少量的误差也会在距离计算中造成巨大的误差。因此，每颗卫星都配备了高精度的原子钟。同时为了进一步地减小误差，可以使用实时运动定位（RTK）。
RTK在地面有许多基站，每个基站都知道自己精确的“地面实况”位置，每个基站也通过卫星来测量自身的位置。二者之间的差别即为卫星测量的误差，然后将这个误差传递给其他的接收器，从而调整自身的位置计算。在RTK的帮助下，可以将卫星导航系统的定位误差限制在10cm之内。但仍存在建筑物阻碍辛信号传播的问题。同时卫星导航系统的更新频率不高，大约为10Hz或每秒更新10次。因此也需要其他的定位方法来补充。
惯性导航定位
利用初始加速度和速度以及汽车的初始位置来计算车辆此刻的状态。加速度可以由加速度计来计算。三轴加速度计根据车辆的坐标系记录测量结果，然后通过三轴陀螺仪将这些测量值转换为全局坐标系。三轴陀螺仪的三个外部平衡环一直在旋转，但三轴陀螺仪的旋转轴始终固定在世界坐标系中。通过测量旋转轴和三个外部平衡环的相对位置来计算车辆在坐标系中的位置。加速度计和陀螺仪共同组成了惯性导航（IMU）的主要组成部件。
IMU的一个重要特征是它更新频率高，可达到1000Hz。但IMU同样有缺点，其测量的运动误差会随着时间的增加而增加。因此我们只能利用IMU在很短的时间范围内进行定位。将IMU与GPS结合起来，IMU弥补GPS更新频率低的缺点，GPS纠正了IMU累计的 运动误差。
然而IMU+GPS仍然不能适应所有场景，例如隧道。
激光雷达定位
激光雷达可以通过测量数据匹配高精度地图，从而获知车辆的精确位置。有很多算法可用于匹配点云，例如ICP(迭代最近点法)、滤波算法（包括直方图滤波，卡尔曼滤波等）。
关于激光雷达及卡尔曼滤波，可以参看我之前写的其他文章：
卡尔曼滤波原理详解
激光雷达系统原理及相关介绍
激光雷达方程推导与激光器参数指标
激光器对人眼安全的评估(MPE与NOHD计算)
激光雷达的优点在于其稳健性。在大多数情况下，激光雷达在拥有高精度地图的前提下，都能够准确定位。同时其主要缺点在于难以构建高精度地图并使其保持最新。
视觉定位
通过摄像头的图像实现定位显然是困难的，但将摄像头图像与来自其他传感器的数据相结合可以实现准确定位车辆。尤其是通过车道线的图像来判断车辆在道路上的位置。
视觉定位的优点在于图像数据很容易获得，缺点在于缺乏三维信息和对三维地图的依赖。
感知
感知包括：
Detection 检测 找出物体在环境中的位置；Classification 分类 明确对象是什么；Tracking 跟踪 跟踪特定的物体；segmentation 语义分割 将每个图像中的像素与语义类别进行匹配。
检测与分类
汽车在路上行驶过程中可能会遇到很多静态障碍物和动态障碍物，例如树木、行人、机动车等。 汽车在检测物体后，对物体进行分类，以便确定自身下一步的运动状态。无人驾驶一般使用CNN来检测遇到的物体，再送入另一个CNN中对对象进行检测和分类。常用的还有R-CNN及其变体Fast R-CNN和Faster R-CNN。YOLO和SSD是具有类似形式的不同体系结构。
跟踪
跟踪在检测失败时是至关重要的。当在检测物体时，某对象被其他对象遮挡了一部分，检测算法可能会失败。追踪便可以解决这个问题，追踪可以保留身份，将之前帧中检测到的所有对象与当前帧检测到的对象进行匹配，从而确定前后两帧相似度最高的对象，从而确定身份。这样在某对象被障碍物遮挡时，也可以通过跟踪确定该对象，从而为自身的下一步运动提供信息。
分割
语义分割用于确定尽可能详细地了解环境，并确定车辆可驾驶的区域。语义分割依赖于全卷积网络FCN。网络中的每一层都是卷积层。卷积之后的信息比原始输入信息的大小小很多，为了使得输入输出的大小相同，对中间输出层进行上采样处理，直到最终输出的大小与原始输出图像的大小相匹配。
预测
无人车需要预测路上很多动态障碍物的行为，以便我们的无人车能够做出最佳的决策。
预测要求实时性和准确性。预测方式分为基于模型的预测（Model-Based）和数据驱动预测(Data-Driven)。基于模型的预测比较直观，数据驱动预测与深度学习有关，一般数据量越大越准确。
通过生成一条路径来预测一个物体的行为，对其他物体也这样预测生成一段路径，这些共同形成了在一段时间内的预测路径。在每一个时间段内，我们会为每一辆车辆重新计算他们所新生成的路径。从而为无人车在规划阶段做出决策提供必要的信息。
为了更好的预测，我们也需要知道障碍物的状态，包括速度、朝向、加速度以及之前时间间隔的状态信息。以便更好地做出预测。
车道预测，预测车辆会选择哪个车道，可以通过计算每个车道序列的概率进行选择。
规划
规划的主要目的时生成免碰撞和舒适的可行轨迹。
生成的轨迹由一系列的点组成。每个点都有一个关联速度以及一个指示何时应到达那个点的时间戳。
路径规划
路径规划的目标是找到地图上从A到B的最佳路径，其输入包括三个：
地图，地图数据包括公路网和实时交通信息；车辆在地图上的位置；目的地
有这三个目标，路径规划模块便会提供最佳的路径。
轨迹生成
生成的轨迹包括2D平面的位置+时间。
生成的轨迹应当满足：
无障碍物（不碰撞）；考虑乘客的舒适性；可执行性，例如车子无法实现立刻掉头；合法。
显然，一般车子在行驶过程中，满足以上4个条件的轨迹仍有很多。可以采用成本函数来进一步地选择轨迹，给轨迹分配成本。例如认为偏离道路中心，加速度增大，超速等行为会增大成本，从而生成成本最低的轨迹。
Frenet坐标
我们所熟知的笛卡尔坐标系只能描述车辆位置而无法描述道路，判断不了道路与车辆的相对位置。
纵坐标表示在道路中的行驶距离；横坐标表示汽车偏离中心线的距离。
路径速度解耦规划
路径速度解耦规划包括路径规划和速度规划。路径规划负责生成候选曲线，包括成本函数，成本函数包含平滑度、安全性与车道中心的偏离。速度规划包括生成速度曲线，对应路径点。生成路径：
将道路分成单元格；对于单元格中的点随机采样；在单元格中选取点连接创建候选路径；构建多个路径并选择成本最低的路径。
ST 图
“ST图”这一工具可以帮助我们设计和选择速度曲线。
S表示车辆纵向位移，T代表时间，斜率代表速度。ST图说明了在不同时间的位置。
为了简化速度曲线的构建，可以将ST图划分成为一个个小网格。同时认为在小网格内的车辆速度是不变的，也即斜率是一定的。这样的方法在一定程度上可以简化速度曲线的构建，维持速度曲线的近似度。
假设在 t 0 t_0 t0和 t 1 t_1 t1时间之间，某车阻挡了 S 0 S_0 S0到 S 1 S_1 S1之间的路段，那么体现在ST图上，速度曲线不能与 t 0 t_0 t0和 t 1 t_1 t1时间之间， S 0 S_0 S0到 S 1 S_1 S1之间的路段所形成的矩形框相交。如下图所示。
上述的路径规划和速度规划都基于离散化。但轨迹并不平滑，使用二次规划（Quadratic Programmming）使得拟合曲线平滑，生成平滑的轨迹。
生成路径的优化，Lattice规划
Lattice规划将横向维度和纵向维度分隔开，将三维问题转换为两个二维问题：具有时间戳的纵向轨迹（ST轨迹）；相对于纵向轨迹的横向偏移（SL轨迹）。
Lattice规划分别建立ST和SL轨迹，然后将他们合并。为生成纵向和横向的二维轨迹，将初始车辆状态投射到ST坐标系和SL坐标系中去。在生成ST、SL轨迹之后，投射到笛卡尔坐标系中，完成轨迹的生成。
控制
控制需要保证可行性、平稳性以及确保到达每一个生成路径点。在百度的Apollo框架中，控制主要采用的是PID、LQR以及MPC。
控制输入包括：
目标轨迹（规划模块）车辆状态（车辆位置等）
PID控制
简单但有效。PID依赖实时测量误差。
线性二次调节器（LQR）
基于模型的控制器。使用车辆状态来使误差最小化。
apollo使用LQR进行横向控制，选取的状态变量有
横向误差（cte）横向误差的变化率( c t e ˙ \dot {cte} cte˙)朝向误差( θ \theta θ)朝向误差的变化率( θ ˙ \dot \theta θ˙)
也即令：
三个控制输入包括：转向（steering）、加速（throttle）、制动（brake）
也即：
将上述写成状态空间的表达形式为：
也即
在误差最小化的同时希望控制输入尽可能小，因此控制误差总和与控制输入作为系统的成本综合。
可以定义以下二次项：
写成紧凑的矩阵形式：
其中Q和R分别代表x和u的权重集合。
u一般可以通过x来表示。
模型预测控制（MPC）
模型预测控制可以参见我的另一篇博客文章：mpc模型预测控制原理详解。
优点：更精确（因为考虑了车辆的物理模型）。
缺点：更加复杂，更加难以实现。
结语
本文主要参考了Apollo自动驾驶入门课，文章中的图片部分也来自课程，如有侵权，请联系我删除。
如果觉得我写的不错，请给我一个免费的赞，如有错误，也欢迎向我反馈。
