自动驾驶科普(百度Apollo)学习笔记

1. 写在前面

在过去的几年里，自动驾驶技术取得飞速发展，人类社会正逐渐走向一个新时代，这个时代中，汽车不仅仅是一个交通工具，更是一个智能的、能够感知环境、做出决策并自主导航的机器伙伴。现在正好也从事这块的工作，所以想把这方面的学习笔记也沉淀一下。

这是这个系列的第一篇文章，打算先写一篇科普笔记， 整理一篇百度apollo的入门课程的学习笔记，源起是现在的自动驾驶中，渐渐的放弃掉原来的分模块化独立学习的范式， 改成了端到端(传感器输入->模型->规控信号)，各个模块统一用一个大模型来学习，省去了之前各个模块的人为干预和信息损失，它标志着从感知到行动的整个过程的技术整合，为实现完全自动化的驾驶体验提供了可能。所以拥抱端到端估计是这两年自动驾驶的一个趋势， 但学习趋势之前，我脑子里突然产生了几个好奇： 之前的自动驾驶是怎么回事？ 有哪些模块？ 每个模块是干啥用的以及原理大概是怎样的？ 又是怎么从好几个独立的模块慢慢的演化到现在的大一统的？ 怀着对这几个问题的好奇，就看了百度的apollo入门课程， 梳理了这篇笔记，并结合着工作中了解的知识进行补充。 这个课程比较偏基础，非常我这样的小白，适合入门自动驾驶的同学。

2. 自动驾驶综述

2.1 系统组成

2.2 5大核心部件

自动驾驶主要是下面的一个过程：

5个核心部件：

• 感知： 是为了看清世界
  • 计算机视觉：摄像头看清世界， 周围有哪些物体，是什么类型的，离车有多远， 但识别到的物体种类有限（最开始的时候是先定义好类别，后面有了clip模型， 无须定义类别)
  • 传感器融合：雷达，激光雷达等其他传感器的数据，更加深入了解周围环境， 不同物体的外观，物体的移动速度， 离各个物体之间的距离，以及一些无语义的静态物体等
• 定位： 确定在世界中的位置，普通地图会有1-2米的误差， 所以这里要精确定位车辆，需要高精地图， 厘米级的误差， 需要通过一些静态地标去定位， 后面端到端出来之后，也渐渐的脱离了高精地图，模型直接学习地图内容，高精地图虽然准确，但是维护成本很高， 更新频率需要很及时才行。
• 规划： 知道自己在世界中的位置， 知道世界周围的环境，就可以使用路径规划来绘制这个世界的路线， 规划是非常难的一个模块
• 控制： 沿着规划的轨道，在方向盘，油门，刹车等方面的控制

2.3 硬件平台

• 开发无人驾驶车，先有一个通过电子控制的基础车辆，这种类型的车辆： 线控驾驶车辆， 包括以下几部分：
  • 传感器(sensors)：传感器参数由参考硬件规格定义
    • 激光雷达(lidar): 由一组脉冲激光器组成，激光束的反射形成了了解环境的点云
    • 摄像头(camera): 捕获图像数据，可以通过视觉技术来提取并了解周围环境
    • 雷达(radar): 也用来检测障碍物，分辨率较低，但经济实惠，适用于各种天气和照明条件，特别擅长测量其他车辆的速度
  • 控制器区域网络（CAN）：车辆的内部通信网络，计算机系统通过CAN卡连接汽车内部网络，发送加速、制动和转向信号
  • 全球定位系统（GPS）：通过绕地卫星接收信号，帮助确定位置
  • 惯性测量装置（IMU）：测量车辆的运动和位置，通过跟踪位置，速度、加速度和其他因素

2.4 软件架构

开放式软件层分为3个子层：

• 实时操作系统(RTOS)，传感器接收数据之后，需要实时的计算，分析规划做出动作。
• 运行时框架(ROS): 在RTOS上的软件框架，根据功能将自制系统划分为多个模块，每个模块接收，处理和发布自己的消息。模块需要通过运行时框架通信。
  • apollo在ros的基础上改进共享内存的功能和性能
    • 共享内存降低需要访问不同模块的数据复制需求，一次写入，多次读取，比如接收一次点云数据，可以同时障碍物检测，定位等
    • 去中心化解决单点（node)故障，不是ros里面的master node控制其他node，而是node之间互相关联
    • 使用protobuf的接口语言替代原生的ros消息， 使得各个node的消息通信数据兼容
• 应用程序模块层: MAP引擎、定位、感知、规划、控制、端到端驾驶、人机(HMI)，各个模块有自己的算法，也互相联系

2.5 云服务

通过互联网访问服务器， 云服务器包括： 高精地图，仿真平台，数据平台，安全， OTA服务，DuerOS(智能语音系统）

• 仿真平台：apollo重要工具，仿真不同驾驶场景（障碍物，路线，信号等），通过指标对场景评估，碰撞检测，交通灯识别，速度检测，障碍物检测和路线逻辑，三维可视化
• 数据平台： 数据很重要， 来自模拟场景或者道路测试， 公开数据集： ApolloScape数据集

3. 模块细节

这里开始介绍自动驾驶的几大模块以及每个模块是怎么工作的，只是一些基础科普内容，不会涉及算法的细节。

3.1 高精地图(HD Map)

高精地图(Highly Driving Map)的作用： 定位 ，预测规划等模块都依赖高精地图， 是一个上帝视角。

what?

• 一种专题地图，目前主要用于自动驾驶, 为满足自动驾驶需求制作多种高精度的路网属性和要素信息
• 一种电子地图，相对于传统导航地图(Standard Definition MAP)定义的，它更高精度，高多种类，更高鲜度
• 属于测绘地理信息数据，此类数据采集和制作过程涉及空间地理信息和敏感信息，需要有机制保证数据安全

高精地图(HD) vs 普通地图(SD)：

• 高精地图包含大量的驾驶辅助信息， 最重要的信息（道路网的精确三维表征），语义信息（限速信息，红绿灯信息，左转车道的开始位置等）
• 高精地图的最重要特征： 精度，普通地图米级， 高精地图厘米级精度

与其他模块的关系：

• 有高精地图，可以定位，如果提供地图上的一小块和整个地图，去实现匹配就能知道自车在哪？
  • 车辆先通过传感器找地标，图像或者点云， 找到之后，与地图上的地标比较，这个比较需要预处理（去掉质量差的数据），坐标转换（不同视角的数据统一坐标）和数据融合（传感器数据融合）过程
• 高精地图帮助感知： 相机、雷达等探测物体的能力有距离限制，天气情况，光线条件等限制，比如， 摄像机在弱光以及高度比度场景很难捕捉足够的视觉信息，激光雷达在雾气，雨滴，雪花，汽车尾气，反射场景容易形成虚假点，radar在通过隧道大桥等可信度低，高精地图可以作为辅助看到更远方，还可以帮助缩小传感器检测范围，比如在特定位置找停车标志（感兴趣区域ROI），ROI可以帮助提高检测精度和速度，节约计算资源
• 高精地图帮助规划： 帮助车辆找合适的行车空间，帮助规划器确定不同的路线选择，并帮助预测器预测道路上其他车辆将来的位置。
  • 识别车道线、限速标志、人行横道，减速带，障碍物？
  • 自动变道，自动超越，设定巡航车速，匝道的自动驶入和出

apollo高精地图：

• 道路定义，交叉路口，交通信号，车辆规则，汽车导航的其他元素
• 交通信号等的精确位置和高度，降低感知难度

保持地图及时更新很重要。制图精度高要求。 OpenDrive格式实现数据共享。

地图构建， 5个步骤：

1. 数据采集： 大工程，数采车去采集源数据，地图的更新和维护

2. 数据处理： 对数据整理，分类，清洗获得没有任何语义的初时地图模板

   

3. 对象检测： 人工智能技术检测静态对象并分类，车道线，电线杆，交通标志等

4. 手动验证： 高效标记和编辑地图，保证自动构图正确进行并及时发现问题

5. 地图发布： 不仅发布高精地图，还包括自上而下的定位地图和三维点云地图

构建和更新地图使用众包，意思是向公众发布数据采集工具，任何人都可以参与制作高精地图的任务。加快了高精地图的制作和维护过程。

高精地图要搭配高精定位。

关于地图，经常听到的有3个名字：HD, HQ, SD

hd高精地图，精度非常高，一般是厘米级， HQ， 高质量实时， 精度稍微比hd低一些， 精简了hd里面的一些信息， 只保留了必要的。 sd 标准的地图， 精度就更差了

地图的发展史是从HD->HQ， 慢慢的后面开始趋向无图化， 成本越来越低。

3.2 定位(Localization)

回答的是"我在哪"的问题。

3.2.1 简述

定位让车知道自身确切位置。

需要精确的确定车辆在地图上的位置， GPS是不靠谱的。

一种常用方法： 汽车传感器看到的内容与地图上所显示的内容比较。

• 车辆传感器可以测量车与静态障碍物（树木，电线杆，路标，墙壁）之间的距离，在自车坐标系中测量这些距离以及静态障碍物的方向
• 地图上也会找到传感器所锁定的地标，在地图坐标系中这些地标也有一个位置
• 将传感器的地标观测值与地标在地图上的位置匹配，关键是无人驾驶软件必须将地标的测量值从车辆坐标系转换到地图坐标系
• 这样， 车辆就能知道自己在地图的什么位置了（地图坐标系中的位置）

但这里就有几个问题了：

1. 车辆传感器可以识别很多障碍物， 地图上也有很多障碍物， 那么怎么比对？
2. 自车坐标系与地图坐标系是怎么转换的？

定位会有不同的方法： GNSSRTK， 惯性导航，激光雷达定位，视觉定位。

3.2.2 GNSS+RTK

三角测量法则， 看看自己在哪？

三个地标，如果知道自己相对于三个地标的相对距离， 就能唯一确定一个自己的位置，如果再有一张地图，知道这三个地标在地图上的位置，那么也就知道自己在地图上的位置了。

GPS定位的原理和这个类似，只不过不是通过地标了，而是通过卫星。 GPS是使用最广泛的全球定位卫星导航系统(GNSS)。GPS分为3个部分：

1. 卫星： 任何特定时间，30颗GPS卫星外层空间运行， 距离地球表明2万公里
2. 世界各地的控制站： 用于监视和控制卫星，保证系统正常运行并验证GPS精确度
3. GPS接收器： 存在于定位设备中，没有障碍物遮挡且天气良好的状况下，每次接收器能至少检测到4颗GPS卫星，接收器不会直接探测到卫星之间的距离。
   1. 它首先测量信号的飞行时间（信号从卫星传播到接收器的时长） distince=光速*time， 由于光速太大，time不大起作用，少量误差最后也会导致误差很大，所以每颗卫星配备了高精确度的原子钟减少误差。
   2. 使用实时运动定位(RTK)作为辅助
      1. RTK是在地面建立几个基站，每个基站知道自己精确的“地面实况”的位置，每个基站也通过GPS测量自己的位置
      2. 已知的地面实况位置，与GPS里面自己的位置进行位置偏差计算
      3. 把这个误差传递给其他GPS接收器，调整自身位置计算

有RTK的辅助， GPS的定位误差会在10厘米之内，但会有高楼或者障碍物阻挡GPS信号的问题。此时会定位困难，另外一个问题就是GPS的更新频率低10hz, 自动驾驶会快速移动，这个频率可能不太行， 很多时候位置更新不及时。

3.2.3 惯性导航定位

假设一辆车知道初始位置， 知道恒定的速度和行使的一段距离t， 这段时间内汽车行使的距离很容易就能计算得到。 那如果知道汽车的速度， 加速度和初始位置呢？

其实有自车的初始位置，有汽车的速度和加速度， 我们可以计算汽车在任何时候的车速和位置。 那么加速度怎么测量呢？

• 需要一个三轴(three-axis)加速度计的传感器，这东西可以精确测量汽车的加速度
• 加速度计本身不足以计算汽车位置偏移，加速度和速度能得到在自车坐标系的位置偏移测量结果
• 但需要把这些记录测量结果转换到全局坐标系里面，这需要一个三轴陀螺仪的传感器

三个外部平衡环一直旋转，但旋转轴始终固定在世界坐标系。计算车辆在坐标系中的位置是通过测量旋转轴和三个外部平衡环的相对位置计算的。

加速度计+三轴陀螺仪是惯性测量单元(IMU)的主要组件。

IMU的一个重要特征是可以高频更新(1000hz), 可以提供接近实时的位置信息，但缺点是运动误差会随时间累加，所以这个东西只能在短时间内用于定位。

但是可以利用GNSS+IMU来定位汽车，这样IMU弥补了GPS更新频率低的缺陷，GPS弥补了IMU运动误差累计的缺陷， GPS纠正IMU的运动误差。

但还有些情景，比如山中，隧道，峡谷等场景， GNSS可能长时间没法更新， 所以还需要有别的定位方法来处理这种情况。

3.2.4 激光雷达定位

有激光雷达，通过点云进行汽车定位。

需要用到激光雷达传感器中的数据与预先存在的高精地图连续匹配， 可获知汽车在高精地图上全球位置和行驶方向。

匹配点云算法： 迭代最近点(ICP)是一种

• 假设想对两次点云扫描进行匹配
  • 第一次扫描中的每个点，需要找到另一次扫描中最接近的匹配点，最终会有很多匹配点对。
  • 每个点对之间的误差相加，计算平均距离误差
  • 目标是通过点云旋转和平移，来最大限度降低平均距离误差，这样就能在传感器扫描和地图之间找到匹配
  • 传感器扫描到的自车位置转为地图上的位置，就能知道定位到自车

滤波算法是另外一种lidar定位算法。可以消除冗余信息。并在地图上找到最可能的车辆位置。

• Apollo使用了直方图滤波算法（误差平方和算法SSD）
• 通过传感器扫描的点云滑过地图上的每个位置
  • 每个位置，计算扫描的点和高精地图对应的点之间的误差或距离，然后误差平方求和， 这个越小越好
• 卡尔曼滤波是另外一种Lidar定位方法（这个没弄明白具体是咋做的？）
  • 卡尔曼滤波算法用于根据我们过去的状态和新的传感器测量结果预测当前的状态
  • 卡尔曼滤波使用了预测更新周期
    • 首先，根据之前的状态以及对移动距离和方向的估计，预测新位置
    • 运动估计不完美，所以需要使用传感器测量对位置进行修正， 一旦传感器测量了新位置，可以使用概率规则，将传感器策略结果与现有的位置预测结合
    • 遵循预测更新周期，需要自车定位时， 需要先预测位置，然后用传感器测量位置，

Lidar优势在于稳健，从高精地图开始，有效传感器，始终能定位。缺点在于难以构建高精地图并保持更新。

3.2.5 视觉定位

图像和其他传感器数据结合可以准确定位车辆，单独图像不行。

摄像头数据+地图匹配定位效果较好。

假设汽车在路上行走，图像数据显示右边有棵树， 然后看地图之后， 有好几个地方都符合右边有树的场景， 怎么确定现在汽车在哪呢？

用概率解决这个问题， 候选点如上。

• 首先，根据看到树的形状， 可以排除汽车横着走的两个位置或者减少概率
• 其次， 汽车走的时候再观察， 看看前面会有啥，如果再看到一棵树， 就增大纵向走的左边这个的概率，因为这是两个并排的树
• 继续上面的过程， 观察结果，概率和地图确定最可能的位置

这个过程叫粒子滤波。因为使用粒子或者点估计最有可能的位置。

实际定位不是用树， 而是车道线。相同的粒子滤波原理对车道线拍照，使用拍摄的图像确定车辆在道路中的位置。将道路摄像头图像与地图图像比较。

图像定位：

• 优点： 图像数据容易获取
• 缺点：却泛三维信息和对三维地图的依赖

3.2.6 百度的apollo定位

apollo用基于gps，IMU和激光雷达多传感器融合定位系统，利用了不同传感器优势，稳定性和准确性都能保证。

IMU， GPS， 激光雷达，雷达和高精地图

通过卡尔曼滤波将输出结合在一起。

卡尔曼滤波建立在两步预测测量周期上。IMU用于卡尔曼滤波的预测步骤，GNSS和lidar用于卡尔曼滤波测量结果更新步骤。

what is 卡尔曼滤波？

3.3 感知(Perception)

3.3.1 计算机视觉

自动驾驶有4个感知世界的核心任务：

• 检测： 找出物体在环境中的位置
• 分类： 物体是什么
• 跟踪： 随时间推移观察移动物体
• 分割： 图像中每个像素与语义类别匹配

目标分类的pipeline:

图像数据主要是两类：

• 摄像头图像：
  • 图像在计算机的视角是矩阵，由像素构成， 值表示颜色或强度单位
  • 彩色图像有RGB三个通道， 灰色图像只有一个通道
• LiDAR图像
  • 雷达传感器创建环境的点云表征，提供难以通过摄像头图像获得的信息（距离或者高度）
  • 雷达传感器使用激光，测量与环境中反射该光线的物体之间的距离（发射激光脉冲，记录从发射到返回传感器的时间，然后再有光线的速度，就能测量距离)
  • 黑色代表自车， 点云中每个点代表反射回传感器的激光束
  • 点云可以描述物体的行状，表面纹理特征， 可以对物体聚类分析，对象检测，跟踪和分类
  • 计算机视觉技术

3.3.2 机器学习

监督学习：数据 + label(人类创造的真值) → 分类， 分割，用真值去监督

无监督学习：数据 + 无label → 找相似，聚类

半监督学习：少部分标记数据 + 大量无标记数据

强化学习：尝试不同方法解决问题， 不同方法会有奖励， 最终方法与环节适应

卷积神经网络：

3.3.3 检测分类

如何对障碍物进行检测和分类？

• 检测CNN查找图像中的对象位置
• 在网络体系末端加几个head，来执行分类或者检测

经典架构：

• 两阶段：RCNN FastRCNN FasterRCNN
• 一阶段： YOLO， SSD

3.3.4 跟踪

检测完对象后，需要跟踪

跟踪：每一帧的每个对象检测并用边界框对每个对象标识

why:

• 追踪在检测失败时非常重要，如果某个对象被另一个对象遮挡，可能会检测失败，追踪可以解决遮挡问题。
• 追踪可以保留身份，障碍物检测的输出为包含对象的边界框，但障碍物没有与任何身份关联，单独使用障碍物检测时， 计算机不知道一个帧中的哪些对象与下一帧中的哪些对象对应

How:

• 确认身份
  • 查找特征相似度最高的对象
  • 通过之前帧的所有对象与当前帧的所有对象进行匹配
  • 也可以利用连续视频中两个障碍物的位置和速度信息去帮助匹配
• 确定身份后，使用对象的位置，结合预测算法，估计在下个时间步的位置和速度，该预测可以帮助下一帧中识别对象

3.3.5 分割

语义分割： 对图像中的每个像素进行分类，目标是了解环境的细节并确定车辆的可行区域

依赖于一种特殊类型的CNN，全卷积(FCN)

• 卷积层代替最后的全连接层
• 每一层都是卷积
• 可在原始图像之上叠加逐像素的输出

必须考虑的一个因素是大小，传统的CNN， 随着层数越来越深，输出会比原始图像越来越小，但为了分割像素，输入和输出必须要相等。为达到这个目的，对中间输出进行上采样处理。

• Encoder: 输入图像的特征编码提取
• Decoder: 解码图像特征并输出

3.3.6 Apollo感知

apollo软件栈可感知障碍物，交通信号和车道

• 对于三维图像检测，apollo在高精地图上使用ROI重点关注相关对象
• apollo将ROI过滤器用于点云和图像数据，缩小搜索范围，加快感知
• 然后通过检测网络输出检测物体的三维边界框
• 最后，使用检测跟踪关联的算法跨时间步识别单个对象，该算法先保留每个时间步要跟踪的对象列表，然后在下一个时间步找到每个图像的最佳匹配

对于交通灯的分类， apollo先用高精地图确定前方是否有红绿灯。

• 如果有，高精地图会返回灯的位置。
• 摄像头搜索到前方的红绿灯，使用检测网络对红绿灯在图像中定位
• 然后从图像中提取红绿灯，将裁剪的交通等仍给分类网络确定灯的颜色
• 如果有多种灯，系统需要选择哪些灯与车道有关

apollo使用YOLO检测车道线和动态物体（车辆，行人），然后给到在线检测模块， 这个东西会并入其他传感器的数据，对车道线预测进行调整。车道线最终并入“虚拟车道”的单一数据结构中。

YOLO检测到的动态对象也通过其他传感器调整，以获得每个对象的类型，位置，速度和前进方向。

虚拟通道和动态对象被传递到规划和控制模块。

3.3.7 传感器比较

摄像头，lidar, Radar

• camera: 适用于分类，比如交通信号灯，车道检测
• lidar: 适用于障碍物检测，夜间或者没有自然光也好使
• radar: 探测范围和应对恶劣天气方面占优势

3.3.8 感知融合策略

apollo利用lidar和radar来检测障碍物，融合的主要算法是卡尔曼滤波，两个步骤：

• 预测状态
  • 假设跟踪一个行人，状态就是位置和速度
  • 第一步： 掌握行人的初始位置和速度
  • 然后使用卡尔曼滤波，预测行人将来的状态
  • 然后是误差分析，用新的传感器更新所认为的行人状态
• 更新测量结果
  • 同步更新： 同时更新来自不同传感器的融合结果
  • 异步更新：逐个更新所收到的传感器的测量结果
• 重复上面两步

传感器融合可提高感知性能，也可以减少跟踪误差， 这样会更加确信对道路上其他物体的位置预测

3.4 预测(Prediction)

在一段时间之内，需要对环境中的各个动态物体的轨迹做出预测，每个时间段，都会重新给每一辆车重新预测并生成轨迹，这些路径给规控阶段做决策提供重要信息。

预测路径有实时性和准确性需求。

另外，预测模块也应该能学习新的行为， 路上有很多车辆的时候，情况很复杂，不可能每个场景都有静态模型或者固定的行为，所以需要预测模块也能学习新行为。需要多源的数据反复训练

3.4.1 不同的预测方式

基于模型的预测

自车往前开， 此时发现左边一辆白色的车。 基于模型的预测，此时会有两个候选模型：

• M1： 继续往前，然后右转
• M2： 继续前行

P(M1)=P(M2) 初始化状态

继续观察车的轨迹，看与哪条更加匹配。

• 如果车辆开始向左变道， 说明直行的概率变大
• 如果车辆继续前行，说明右转的变大

优点：直观，结合物理知识，交通法规和人类行为

基于数据的预测

• 使用ML算法，通过观察结果训练模型，然后根据观测的数据去预测
• 数据越多，效果越好

3.4.2 障碍物状态

预测物体的运动，物体的状态也很重要。

观察物体的朝向，位置，速度，加速度预测它将会做什么

还需考虑车端内物体的位置，比如预测模块会预测一个车的横向和纵向距离， 还包含之前时间间隔的物体状态信息。

3.4.3 预测目标车道

apollo车道序列预测框架：

• 先将道路分成多个部分，每个部分覆盖了容易描述车辆运动的区域
• 预测更关心车辆如何在区域之间变换，而不关心在一个区域内的具体行文
• 这样车辆的行为就划分为了一组有限的模式组合，这些模式组合称为车道序列
• 比如，直行运动的汽车的车道序论： 0 → 1 → 3 → 7

使用车道序列预测框架的目标是：为道路上的物体生成轨迹，这个问题比较复杂

从简单的问题开始：预测车道线段之间的过渡，假设0阶段检测到一辆车，预测接下来怎么走。两个选择：

1. 右转
2. 左变道然后直行

预测问题转为选择问题。接下来选择车辆最有可能采取的车道顺序。

**将复杂的车辆运动转成车道转换序论的选择问题，**大幅度减少了场景的数量。

计算每个车道序列的概率，需要一个模型：

• 输入：车辆状态 + 车道段
• 输出：采用每个车道序列的概率

希望模型学习新的行为，应该使用观测数据对模型经验性训练。训练中真实的车辆提供给模型，车道段+车辆状态+对象最终选择哪条车道。 构成这样的训练样本训练模型，使得模型具有经验

RNN网络：

apollo使用RNN建立一个模型预测对象的目标车道。

为车道序列提供一个RNN模型， 为相关的对象状态提供另一个RNN模型， 这两块的结果concat然后通过一个MLP，估计每个车道序列的概率。

选择概率最高的车道序列作为预测目标车辆将遵循的序列。

训练这个网络，现有的记录，每条记录包括一个车道序列 + 相关的对象状态 + 一个标签

3.4.4 轨迹生成

预测到物体的车道序列，加上车辆物理的知识，就可以预测物体的轨迹

A点→B点，自车有很多轨迹可以走，如何预测最有可能的轨迹呢？

先通过设置约束条件去除大部分候选轨迹。

• 首先，假定汽车将与目标车道的中心对齐，就能去除上面不对齐的三条
• 然后，去除车辆无法实际直行的轨迹，这样下面两条去除
• 最后，只剩下两条可能的轨迹， 这个通过车的速度和加速度去除

注意车辆在两点的位置和方位，这两个姿势表示运动模型的初态和终态，使用这两个条件拟合一个多项式模型去进行轨迹预测，而不用穷举出所有的轨迹。

运动预测对于规划中构建无碰撞的安全路径非常重要。

3.5 规划(Planning)

规划中，通过结合高精地图，定位和预测来构建车辆轨迹， apollo中，路线规划模块完成任务。

1. 路线导航： 如何在地图上从A到B， 输入是地图数据， 输出是可行驶路径

规划的目标： 生成免碰撞和舒适的可执行轨迹， 轨迹由一系列点定义，每个点都有一个关联速度和一个指示何时应抵达点的时间戳

3.5.1 路由(Routing)

路径规划的目标： 找到从a到b的最佳路径 on the map

• 输入： 地图（公路网和实时交通数据）， 当前在地图上的位置，目的地
• 输出： 前往目的地的最佳路径

人尝试从A到B找路径的时候，往往是沿着道路去追踪路径，这就是搜索， 自动驾驶也是通过搜索去查找路线，但它用了更智能的算法。

而用智能搜索算法的前提是， 需要将地图数据重新格式化为“图形”的数据结果， 图形由点和边构成，节点表示路段， 边表示路段间的连接，在图结构上使用搜索算法。

比如下面的4个node

可以对一个节点到另外一个节点所需的成本建模，

从node1到node4的成本（左转）要高于node1 → node3(直行)

地图转为图的好处：可以使用在图上查找路径的算法，一旦在图上找到了路径，就可以很很轻松的还原到地图上去。

3.5.2 网格世界

A*算法是经典的在图上进行路径查找的算法。通过网格工作。

网格中的每个单元当作节点，能够从任何一个节点移动到任意相邻节点， 但里面会有些阻挡的墙壁

现在的目标是从红点到绿点。 计算机会穷尽所有可能的路径，然后对比找到最短路径，这在地图很大的时候， 非常耗时， 因为从任何给定的点，最多有8个用于下一步的选项，因为存在8个相邻的单元格，对于每个相邻的单元格，又会有8个相邻单元格用于下一步， 这个复杂度指数增长非常致命。

从初始节点开始，需要确定8个相邻节点中，哪个是最优希望的候选节点，要考虑两个事情：

1. 从开始节点到候选节点的成本
2. 估计从候选节点到目的地的成本

每一步， 要尽量使得这两个成本小， 现在比较难的是，如何估计2？

先设定2个变量g和h来分别表示上面的两个成本。

• g: 从开始节点到候选节点的成本
• h: 候选节点到目的地的估计成本(启发式成本)
  • 根据实际情况，自定义成本估算公式，比如交通堵塞会增加前往目的地的成本

每个节点，都通过添加g和h计算总和得到f, 每一步最好的候选节点即f最小的那个节点

当走到下一个节点时，重复上面的候选节点步骤，总是选择未访问过，且f值最小的节点。

这就是A*算法的思路，建立了一条前往目的地的稳定路径。

但这里比较难的是， 怎么去估算h呢？

按照上面的思路， 回到真实世界的地图。假设到了一个交叉路口，前面3个候选点： 高速直行，或左转，或右转，接下来对选项评估：

1. 实践中，拐过交叉路口比直行成本高， 所以根据实践经验对三个点分配了g值
2. 在查看直行道路之后，意识到必须走很长的路，才能离开公路，回到目的地，所以直行道路的h值会更高

1. 所以，根据最终的f值，选定了右转的这个候选节点。

3.5.3 路由到轨迹

高级地图路线是规划过程的一部分，仍需要构建沿这条线前进的低等级轨迹。

意味着要处理不属于地图的物体，如其他车辆，行人，自行车等

这些场景需要更低级别，更高精确度的规划，将这一级别的规划称为轨迹生成。

轨迹生成的目标：生成由一系列路径点所定义的轨迹，每个轨迹点分配速度和到达该点的时间戳。

要生成一条能拟合这些路径点的曲线，生成轨迹的几何表征。

移动障碍物可能会暂时阻挡部分路段，所以每个点都有一个预计到达的时间戳。将时间戳与预测模块的输出相结合，确保我们在计划通过时轨迹上的每个点均未被占用。

时间戳创建了一个三维的轨迹：2D坐标 + 时间， 每个路径点还指定了一个速度，用于确保车辆按时到达每个路径点。

3.5.4 评估一条轨迹

现实中的规划面临多种约束

1. 轨迹应免予碰撞，必须无障碍物
2. 要让乘客感到舒适，所以路径点的过渡以及速度的变化要平滑
3. 路径点对车辆实际可行，比如高速行使车辆不能立即180度大转弯
4. 轨迹路径要合法，每个路径点要考虑法规

道路上任何两点之间，可能有多个不会碰撞，舒适，可行且合法的轨迹，怎么选择最好的轨迹呢？

使用成本函数。给每个轨迹分配了一个成本

• 轨迹成本由各种犯规，处罚组成， 下面几个会使得成本增加
  • 偏离道路中心
  • 可能发生碰撞
  • 超出速度限制
  • 轨迹的曲率和加速度让乘客不舒服
• 轨迹成本由上面的成本汇总加和，得到每条轨迹的成本

和driving score有点像。

车辆也可能在不同环境使用不同的成本函数。比如高速和停车场

3.5.5 Frenet坐标

通常使用笛卡尔坐标描述物体的位置，但笛卡尔坐标对车辆不是最好的选择

如果给定车的(x,y)坐标，但不知道路在哪，也很难知道车行驶了多远以及难以确定是否偏离道路中心。

所以Frenet坐标代替笛卡尔坐标。

Frenet坐标描述汽车相对道路的位置。这个坐标中，s代表沿道路的距离(纵坐标), d表示与纵向线的位移(横坐标)

道路中的每个点，纵坐标表示在道路的行驶距离，横坐标表示偏离中心线的距离。

3.5.6 路径-速度解耦规划

将轨迹规划分为两步：

1. 路径规划
   1. 生成候选曲线， 这是车辆可行驶的路径
   2. 成本函数，对每条路径评估，该函数考虑平滑度，安全性，与车道中心的偏离以及其他因素
   3. 选择成本最低的路径
2. 速度规划
   1. 有了最佳路径，然后确定沿这条路线行进的速度
   2. 需要选择的是与路径点相关的一系列速度，而不是单个点上的速度，这个序列称为速度曲线
   3. 使用优化测量为路径选择受到各种限制后的好的速度曲线
3. 路径和速度曲线结合，可构建车辆行驶轨迹

候选路径的生成：

1. 首先将路段分成单元格，然后对单元格的点随机采样，每个单元格取一个点并将点连接，就创建一条候选路径
2. 重复上面过程，就得到多条路径

1. 使用成本函数选择成本最低的路径
   1. 车道中心的偏离
   2. 障碍物的距离
   3. 速度和曲率变化
   4. 对车辆的压力
   5. 其他因素，比如道路拥挤等

选择与该路经关联的速度曲线。ST图可以帮助设计和选择速度曲线（S表示纵向位移，t表示时间）ST图上的曲线是对车辆运动的描述。说明车辆不同时间的位置，通过查看曲线的斜率从ST图上推断速度。

那速度怎么规划呢？

1. 先将ST图离散为多个单元格，单元格之间的速度有所变化，但单元格内速度保持不变
2. 障碍物绘制在特定时间段内，阻挡道路的部分矩形
3. 比如，预测模块预测t0-t1时间段， 一辆车驶入自车车道， 阻挡s0-s1， 所以这段时间画上黄色， 为了避免碰撞，这段时间速度曲线不能相交黄色，

1. 有了各种单元格被阻挡的ST图，可以使用优化引擎为该图选择最佳速度曲线
2. 优化算法通过复杂的数学运算搜索受到各种限制的低成本解决方案，限制包括法律限制，比如限速，距离限制（障碍物的距离），汽车的物理限制（加速度）

上面的规划很大程度上取决于离散化。路径选择降道路划分单元格，而速度曲线降ST图划分为单元格。离散化容易解决问题，但生成的轨迹并不平滑。

离散解决方案 → 平滑轨迹，使用”二次规划”技术

二次规划将平滑的非线性曲线和分段式的线性段拟合。

回顾以下上面的路径规划流程：

1. 自车周围生成多条候选路线，使用成本函数对候选路径评估，并选择成本最低的路径

1. 使用ST图来进行速度规划，根据其他车辆随时间变化的位置阻挡了ST图的部分区域， 优化引擎根据约束和成本函数， 选择一条最佳的速度曲线

1. 使用二次规划技术，让路径和速度曲线变平滑
2. 最后， 将路径和速度曲线合并为构建轨迹

3.5.7 Lattice 规划

通过使用Frenet坐标，可以将环境投射到路的横轴或纵轴上

目标： 生成三维轨迹（纵向维度，横向维度，时间维度）

方法：三维问题分解成两个单独的二维问题， 分离轨迹的纵向和横向分量解决的

ST轨迹：具有时间戳的纵向轨迹

SL轨迹： 相对于纵向的横向偏移轨迹

两个步骤：

1. 先创建ST和SL轨迹，然后合并
   1. 先将初始车辆状态投射ST和SL坐标系
   2. 对预选模式中的多个候选最终状态采样，来选择车辆最终状态
   3. 对每个候选最终状态，构建一组轨迹，将车辆从开始转换到最终状态，使用成本函数对这些轨迹评估选择成本最低的轨迹

那么，ST轨迹的终止状态是什么样的？ 根据情况，将状态分为3组： 巡航、跟随和停止

• 巡航： 车辆完成规划后定速行驶，最终态的加速度为0, 速度和前一时刻的速度一样

• 跟随：对位置和时间状态采样并尝试t出现在某辆车的后面，跟车需要和前方保持一定的安全距离，这时的速度，加速度取决于前方的车辆

• 停止： 只需对汽车何时何地停止抽样，最终速度和加速度被修正为0

SL轨迹的终止状态：

• 基于一个假设规划SL状态，即无论车辆进入怎么样的终止状态，车辆都应该平稳的与车道中心线对齐
• 这样我们只需要在一个小区域内，对横向终止位置进行采样，比如采样道路上相邻车道中心线周围的位置

• 根据上面的分析，为了稳定，最后的车头朝向应该是与车道线中心对齐的， 为达到这种状态，车的朝向和位置的一阶（车辆不能横向移动）和二阶（车辆不能横向加速）导数应该为0，

轨迹生成：同时有了SL和ST轨迹之后，需要将他们重新转换为笛卡尔坐标系，然后将他们结合，构建由二维路径点和一维时间戳组成的3维轨迹。

ST轨迹是随时间变化的纵向偏移， SL轨迹是纵向轨迹每个点上横向偏移， Frenet坐标下。

两个轨迹下都有S， 可以将S值进行匹配合并轨迹。

3.6 控制(Control)

对于汽车而言，最基本的控制输入：转向、加速和制动

控制器使用一系列路径点接收轨迹，控制器的任务是使用控制输入，让车辆通过这些路径点

控制器必须准确，应当避免偏离目标轨迹

其次是具备可行性

最后要考虑平稳度

总之，目标是使用可行的控制输入，最大限度降低与目标轨迹的偏差，最大限度提高舒适度，控制就是让自己的车在规划的轨迹上平稳运行。

实现这个目标的三种控制策略：

1. 比例积分微分控制（PID）
2. 线性二次调节器（LQR）
3. 模型预测控制（MPC）

3.6.1 控制流程

控制器有两种输入：目标轨迹与车辆状态

• 目标轨迹来自规划模块， 每个轨迹点，都提供一个位置和一个参考速度，每个时间步都会对轨迹进行更新

• 还需要了解车辆状态，包括车辆位置（定位模块计算）， 包括车辆内部传感器获取的数据（速度，加速度，转向）

使用上面两个输入计算目标轨迹与实际行进轨迹之间的偏差

控制器的输出：控制输入（转向，加速和制动)的值

方向盘，油门，刹车

下面了解3中控制算法，计算转向，加速和制动的值的。

3.6.2 PID控制

PID(proportional比例，） 控制：

• 优点： 简单

只需要知道与目标轨迹有多大的偏离。

一辆车正遵循目标轨迹，P控制器在车辆开始偏移时，立即将其拉回目标轨迹，偏离的越狠，就越难拉回目标轨迹。

实践中，P控制器的一个问题：很容易超出参考轨迹

当车辆离目标轨迹越来越近的时候，需要控制器更加稳定。

D(derivative, 微分)致力于使得运动趋于稳定状态。

PD控制器，相对于P控制器，增加了一个阻尼项，可最大限度的减少控制器输出的变化速度。

I(Integral, 积分): 该项负责纠正车辆任何系统性偏差，比如车辆可能失准，可能造成恒定的转向偏移，此时需要稍微向一侧转向，以保持直行。

为解决这个问题，控制器会对系统的累计误差进行惩罚

优点： 简单， 很多情况下效果很好，只需要知道车辆实际轨迹与目标轨迹的偏差

缺点：

• 只是一种线性算法 ，对于复杂系统不够，对与自动驾驶，需要用不同的PID控制器来控制转向和加速，意味很难将纵向和横向控制结合起来
• 依赖于实时误差测量，受到测量延迟控制时可能失

3.6.3 线性二次调节器

LQR是基于模型的控制器，使用车辆状态使得误差最小化

apollo使用LQR进行横向控制，包含四个组件：

1. 横向误差(cte)
2. 横向误差的变化率(cte.)
3. 朝向误差(theta)
4. 朝向误差的变化率(theta.)

同时，每个时刻的车辆状态也有3个控制输入。

LQR处理线性控制Linear，这种类型模型用下面公式表示：

即X方向上的变化率，等于后面两个的线性组合。X点是如何受到当前状态x和控制输入影响的。

下面两个等式也成立：

那么Q是啥意思？

• 目标是让误差最小化，但也希望尽可能减少控制输入，节约成本
• 为减少这些因素干扰，可以保持误差的运行总和以及控制输入的运行总和，当汽车往右偏的特别利害的时候，添加到误差总和中，当控制输入将汽车往左转时，从控制输入总和中减去一点
• 但这种方法导致的问题：右侧的正误差会将左侧的负误差抵消， 对于控制输入总和也是如此，相反，所以可以让x和u里面的各个项各自乘以他们自己，这样负值也会产生正平方，得到平方误差（quadratic)，然后分配权重加和

• 最优的目标函数u应该是最小化误差的平方总和，数学里面这叫作成本函数

这里是换成了矩阵运算。中间的Q，R是权重参数。

在LQR中，控制方法被描述为

k是一个复杂的概念，代表如何从x计算出u，所以找到一个最优的u，本质上是找到一个最优的K

3.6.4 模型预测控制

一种更复杂的控制器，依赖于数学优化

三个步骤：

1. 建立车辆模型
2. 使用优化引擎计算有限时间范围内的控制输入
3. 执行第一组控制输入
4. 重复

每次只往前计算一个点，因为要考虑车辆实际运行的状态

建立车辆模型

该模型近似于汽车的物理特性，估计了例如将一组控制输入给到车辆会发生什么事情

接下来，决定MPC预测未来的能力

预测越深入，控制器越准确，需要的时间越长，需要在准确度和快速获取结果之前取舍

获取结果越快，越能更快的应用到实际车辆。

下一步，将模型发送到优化引擎，搜索最佳控制输入，工作原理是通过搜索密集数学空间来寻求最佳解决方案，为缩小搜索空间，依赖车辆模型的约束条件。

优化引擎可间接评估控制输入，是通过使用这些方法对车辆轨迹进行建模，可根据成本函数对轨迹评估，成本函数基于对目标轨迹的偏差，其他因素如加速度和舒适度，碰撞等

为了舒适感，对控制输入的调整要小，根据具体情况，可能需要进一步考虑成本并设计成本函数，模型，约束和成本函数合并到一起，并作为优化问题得以解决。可以在不同优化引擎中，选择一种最佳解决方案

优点：

• 考虑车辆模型，所以比PID更准确
• 也适用于不同的成本函数，可以在不同情况优化不同成本

缺点：

• 相比PID，更复杂，更缓慢，实现困难

4. 小总

到这里终于可以做个总结了， 这篇文章算是自动驾驶方向的入门笔记， 主要是写了自动驾驶中几个核心模块：

• 感知： 回答了自车周围有什么的问题？ 一般包括OD， 也就是障碍物检测，Agent, VRU…， 还有LD， 即车道线检测等
• 定位： 回答了自车在哪的问题， 包括local定位（local坐标系： 车发动的时候是原点，是一个相对的坐标系），还有全局定位等
• 预测： 回答了agent 要怎么走的问题， 其他与自车交互的障碍物会影响自车未来的轨迹
• 规划： 回答了 自车要怎么走的问题， 结合了感知，定位， 预测的结果， 规划出未来一段时间内自车要怎么走的问题
• 控制： 接收规划的轨迹， 然后再结合周围的环境信息，选择出最合适的一条路线， 落实到行动

通过这篇文章， 可以从一个全局的视角去看自动驾驶这个问题。 在最开始的时候， 大约2010年左右的时候， 这些模块都是分开团队， 分模块去做， 然后用接口交互， 但这样存在的问题就是信息传递过程中会存在信息损失， 后面的模块也无法及时的反馈到前面去， 于是在这个架构现在慢慢的开始朝着端到端的技术路线演进， 不过现在一般提到端到端， 还是狭义方面的端到端，也就是可能是几个模块先拼接，比如感知，prediction和planning三个模块，中间换成一个模型去搞，接收传感器信号之后，直接输出planning的轨迹。 还没有完全实现一个从传感器输入到行为方面的端到端系统，不过未来我觉得这种系统一定会实现， 加油加油 😉