从 Apollo 起步-Lesson 3：定位
定位
定位是让无人驾驶车知道自身确切位置的方法,这是一个美妙而富有挑战性的任务.
定位简介
现在我们假设你有一张全球的高精度地图，定位的任务是确定你的车辆在这张高精度地图上的位置。
在我的日常生活中，我一直使用手机 GPS 来确定自己的位置，但 GPS 对于无人驾驶车来说不够精确。大多数时候，GPS 的精度在1-3米间，这对于无人驾驶车来说太不精确了。
在一些情况下比如我们被高楼、山脉围绕或位于峡谷内，GPS 的精度可能只有10 米、甚至只有 50 米。
由于我们无法完全信任 GPS，因此我们必须找到另一种方法来更准确地确定车辆在地图上的位置。最常用的方法是将汽车传感器所看到的内容与地图上所显示的内容进行比较。
车辆传感器可以测量车辆与静态障碍物之间的距离，如树木、电线杆、路标和墙壁等。我们在车辆自身的坐标系中测量这些距离、这些静态障碍物的方向。随着车辆转弯，车辆自身的坐标系必然与地图坐标系不一致，车辆的坐标和地图坐标系可能均取决于手机导航系统中的设置。
为估计车辆在地图上的位置，我们将传感器的地标观测值与这些地标在地图上的位置进行匹配。地图自带坐标系，无人驾驶软件必须将传感器的测量值从车辆坐标系转换为地图坐标系，反之亦然。执行这类转换是解决定位问题的关键步骤。
我来做一个总结：对于定位，车辆将其传感器识别的地标与其高精度地图上存在的地标进行比对。为了进行该比对，必须能够在它自身坐标系和地图坐标系之间转换数据。然后，系统必须在地图上以十厘米的精度确定车辆的精确位置。
定位提供了许多可供选择的方法，每种方法都有各自的优缺点。我们将探讨几种常见的无人驾驶车定位方法 如GNSS RTK、惯性导航、LiDAR 定位和视觉定位
最后 我们将了解 Apollo 框架是如何解决定位问题的。
Sebastian介绍定位
什么是定位比如机器人需要知道它在哪里，这包括 X、Y 坐标位置、航向等信息。精确定位，我们要精确到 以准确地知道我们在哪个地方以及我们在车道内的哪个位置。
为什么要追求精确定位呢？因为如果我们确切地知道我们在哪里，如果没有其他汽车和行人等，我们几乎可以闭着眼开车。
所以要做一个关于定位的全景图，它将涉及不同类型的传感器也就是我们所谓的惯性感知，此外，还有不同类型的外部传感器包括摄像头、激光雷达等。
GNSS RTK
如果在野外迷路 你会怎么做？
假如你看到自己离一棵树 75 米远,你可能在哪里?你只知道自己位于离树 75 米远的地方,更确切地说 你位于一个以树为中心 半径为 75 米的圆上。
现在你看到了一个离自己 64 米远的房子，你知道自己现在在哪里吗？ 信不信由你 你可能不知道。你知道自己位于两个圆的交点处，但可能有两个交点，你不知道自己位于哪个交点上。
现在假设你看到了第三个路标，即路灯 经过测量 你发现自己离路灯 55 米远，你总算知道了相对于这些地标的确切位置。
如果你有一张地图，里面注明了这些地标在世界上的位置，你就知道了自己在世界上的确切位置,该过程被称为 三角测量。
我们刚刚讲解的示例有两个维度，想象一下 我们试图在地球表面上进行三维定位。我们将使用传送它们与我们之间距离的卫星，而不是我们可以看到的地标，这就是 GPS 的工作原理。
如果我们在地球上某一处，我们至少需要有多少卫星才能知道我们在哪里？
答案与二维平面中的问题相同。 然而，实际生活中，GPS 使用另外一颗卫星来确定海拔高度。所以，每时每刻总共有 4 颗卫星确定你的位置。
GPS 即全球定位系统，这是一种由美国政府开发并在全球范围内运营的卫星导航系统。这类系统的通用名称为全球导航卫星系统或 GNSS。GPS 是使用最广泛的 GNSS 系统。
- GPS 分为三部分。第一部分是卫星。在任何特定时间 大约有 30 颗 GPS 卫星在外层空间运行。它们各自距离地球表面约 2 万公里。
- 该系统的第二部分由世界各地的控制站组成。控制站用于监视和控制卫星，其主要目的是让系统保持运行，并验证 GPS 广播信号的精确度
- 系统的最后一部分是 GPS 接收器。GPS 接收器存在于手机、电脑、汽车、船只以及许多其他设备中。如果周围没有高楼等障碍物，并且天气良好，那么无论你身在何处，GPS 接收器每次应至少检测到四颗 GPS 卫星。
GPS 接收器实际上并不直接探测你与卫星之间的距离，它首先测量信号的飞行时间，也就是说 信号从卫星传播到你的 GPS 接收器需要多长时间？通过将光速乘以这个飞行时间，来计算离卫星的距离。由于光速的值很大，即使是少量的时间误差也会在距离计算中造成巨大的误差。因此 为进一步减小误差 每颗卫星都配备了高精确度的原子钟。
我们可以使用 实时运动定位（或 RTK）。RTK 涉及在地面上建立几个基站，每个基站都知道自己精确的“地面实况”位置。但是 每个基站也通过 GPS 测量自己的位置，已知的“地面实况”位置，与通过 GPS 测量的位置之间的偏差为 GPS 测量结果中的误差.
然后 将这个误差传递给其他 GPS 接收器,以供其调整自身的位置计算。在 RTK 的帮助下，GPS 可以将定位误差限制在 10 厘米以内。但是 仍存在高楼和其他障碍物可能阻挡 GPS 信号的问题，这使定位变得困难 或根本无法定位。
GPS 的另一个缺点在于它的更新频率很低，大约为 10 赫兹 或每秒更新 10 次，由于无人驾驶车在快速移动，我们可能需要更频繁地更新位置。
惯性导航
我们来学习一种被称为惯性导航的定位方法。
假设一辆汽车正以恒定速度直线行驶，如果我为你提供了汽车的初始位置、速度、行驶时长。你可以告诉我汽车现在处于什么位置吗？即从初始位置开始，然后速度乘以时间。
接下来 让我们尝试一个更难的问题。还是同样的问题 但不是初始位置和速度，而是 我向你提供初始速度和加速度，稍后你能告诉我车辆的速度吗？
我们可以使用加速度、初始速度和初始位置来计算汽车在任何时间点的车速和位置。
但是 这又引出了另一个问题：我们该如何测量加速度？
我们需要一个名为三轴加速计的传感器。有三种不同类型的三轴加速度计，它们采用不同的方法，但共同的目标是精确测量加速度。
然而 加速度计本身，不足以计算我们的位置和速度。加速度计根据车辆的坐标系记录测量结果，我们需要知道如何将这些测量值转换为全局坐标系，这种转换需要另一个名为陀螺仪的传感器。
三轴陀螺仪的三个外部平衡环一直在旋转，但三轴陀螺仪中的旋转轴始终固定在世界坐标系中，我们计算车辆在坐标系中的位置是通过测量旋转轴和三个外部平衡环的相对位置来计算的。
加速度计和陀螺仪是惯性测量单元 (或 IMU) 的主要组件。IMU 的一个重要特征在于它以高频率更新，其频率可达到 1000 赫兹，所以 IMU 可以提供接近实时的位置信息。
遗憾的是 惯性测量单元的缺点在于其运动误差随时间增加而增加，我们只能依靠惯性测量单元，在很短的时间范围内进行定位
但是 我们可以结合 GPS 和 IMU 来定位汽车。一方面 IMU 弥补了 GPS 更新频率较低的缺陷，另一方面 GPS 纠正了 IMU 的运动误差。
但是 即使将 GPS 和 IMU 系统相结合，也不能完全解决我们的定位问题。如果我们在山间行驶，或城市峡谷中，或者最糟糕的是在地下隧道中行驶。那么我们可能会长时间没有 GPS 更新，这会让整个定位面临失败风险。
LiDAR定位
利用激光雷达 我们可以通过点云匹配来对汽车进行定位。该方法将来自激光雷达传感器的检测数据，与预先存在的高精度地图连续匹配。通过这种比较，可获知汽车在高精度地图上的全球位置和行驶方向。
有许多算法可用于匹配点云。
- 迭代最近点（或 ICP）是一种方法。假设我们想对两次点云扫描进行匹配，对于第一次扫描中的每个点，我们需要找到另一次扫描中最接近的匹配点。最终我们会收集到许多匹配点对，我们把每对点之间的距离误差相加，然后计算平均距离误差。我们的目标是通过点云旋转和平移来最大限度地降低这一平均距离误差，一旦我们最大限度地降低了点云之间的误差，我们就可以在传感器扫描和地图之间找到匹配，我们将通过传感器扫描到的车辆位置转换为全球地图上的位置，并计算出在地图上的精确位置。
- 滤波算法是另一种LiDAR 定位方法。滤波算法可消除冗余信息，并在地图上找到最可能的车辆位置。Apollo 使用了 直方图滤波算法，该方法有时也被称为 误差平方和算法（或 SSD）。为了应用直方图滤波，我们将通过传感器扫描的点云滑过地图上的每个位置。在每个位置我们计算扫描的点与高精度地图上的对应点之间的误差或距离，然后对误差的平方求和。求得的和越小，扫描结果与地图之间的匹配越好。该示例显示一些对齐较好的点 以红色表示，以及一些对齐较差的点 以蓝色表示。在该示例中 绿色表示中等对齐。
- 卡尔曼滤波是另一种LiDAR 定位方法。卡尔曼滤波是一种算法,用于根据我们在过去的状态和新的传感器测量结果预测我们当前的状态。具体来说 卡尔曼滤波使用了预测更新周期。首先 我们根据之前的状态以及对移动距离和方向的估计，来估计或“预测”我们的新位置。当然 我们的运动估计并不完美，所以需要通过使用传感器测量我们的位置并加以纠正。
一旦用传感器测量了我们的新位置，我们便可以使用概率规则 ，将也不完美的传感器测量结果与我们现有的位置预测结合起来，我们会永远遵循这个预测更新周期，只要我们需要对车辆进行定位，先预测我们的新位置 然后用传感器测量我们的位置。
LiDAR 定位的主要优势在于稳健性。只要从高精度地图开始 并且存在有效的传感器，我们就始终能够进行定位。
主要缺点在于难以构建高精度地图,并使其保持最新。事实上 几乎不可能让地图完全保持最新，因为几乎每个地图均包含瞬态元素，汽车和行人 甚至停放的汽车，在我们下次驾车驶过时都会消失，街道上的垃圾会被吹走，世界上的许多元素都在不断发生变化。
视觉定位
图像是要收集的最简单的数据类型。摄像头便宜且种类繁多，还易于使用，我们可以使用图像来定位汽车吗？
通过图像实现精确定位非常困难，实际上 摄像头图像通常与来自其他传感器的数据相结合，以准确定位车辆。但 将摄像头数据与地图和 GPS 数据相结合比单独使用摄像头图像进行定位的效果更好。
假设一辆车正在路上行驶 它感知到右边有一棵树，但是 地图显示道路右侧有几棵树，全部位于不同的位置，我们如何知道车辆现在看到哪棵树？
我们可以用概率来解决这个问题。想象一下 我们位于道路上许多不同点中的任意一点处，使用概率来确定哪个点，最可能代表我们的实际位置。
我们知道在右边看到一棵树，我们假设从一些点可以看到右边有一棵树 而从另一些点则看不到，当然 我们很可能位于可以看到右边有一棵树的地方。我们可以排除在地图上无法看到右边那棵树的点。
我们可以在开车的同时继续观察周边世界。想象一下 我们开车前行 然后观察到右边的另一棵树。在观察地图上的其余点之后。我们发现仅在少数几个位置，会发现车辆右侧有成排的两棵树，我们当然最有可能位于这些位置之一，所以我们可以排除所有其他位置。
我们继续该过程 通过观察结果、概率和地图来确定我们最可能的位置。该过程被称为粒子滤波。
因为我们使用粒子或点来估计最可能的位置,当然 树木在许多道路上比较稀少，但是 车道线在许多道路上却很常见，可以使用相同的粒子滤波原理对车道线进行拍照，然后使用拍摄的图像来确定车辆在道路中的位置。可以将道路摄像头图像与地图进行比较，我们的摄像头图像与地图的某些部分匹配得很好，但与地图的其他部分匹配得没那么好。
这是视觉车道线匹配的一个示例。蓝色代表地图上两个不同位置的车道线。我们想确定哪个位置更有可能是我们所处的位置。红色代表车辆摄像头观察到的车道线，红线与右侧蓝线的匹配度要比与左侧蓝线的匹配度高得多，因此 我们更有可能位于与右侧图像对应的地图位置上。
视觉定位的优点在于图像数据很容易获得，缺点在于缺乏三维信息和对三维地图的依赖。
Apollo定位
Apollo 使用基于 GPS、IMU 和激光雷达的多传感器融合定位系统。这种融合方法利用了不同传感器的互补优势，它也提高了稳定性和准确性，
Apollo 定位模块依赖于 IMU、GPS、激光雷达、雷达和高精度地图。这些传感器同时支持 GNSS 定位和LiDAR 定位。GNSS 定位输出位置和速度信息，LiDAR 定位输出位置和行进方向信息。
融合框架通过卡尔曼滤波将这些输出结合在一起，卡尔曼滤波建立在两步预测测量周期之上。在 Apollo 中 惯性导航解决方案用于卡尔曼滤波的预测步骤，GNSS 和 LiDAR 定位用于卡尔曼滤波的测量结果更新步骤。
点击 这里 了解卡尔曼滤波的工作原理。
关于 Apollo 定位的论文，可以查阅 “Robust and Precise Vehicle Localization based on Multi-sensor Fusion in Diverse CityScenes, ICRA, 2018”.