Apollo课程学习小记录2——定位

学习前言

今天的学习内容是定位原理与方法。

什么是定位

在日常生活中，我们使用GPS来定位。但大多数时候，GPS的精度在1到3米之间，这对无人驾驶来说不够精确，尤其是在一些情况下，比如被高楼、山脉围绕时或位于峡谷内，GPS的精度可能会更差，可能只有十米或甚至只有50米，所以我们不能完全信任GPS。

车辆坐标系被用来测量车辆与静态障碍物（如树木，电线杆，路标和墙壁等）之间的距离，以及这些静态障碍物的方向。在车辆坐标系中，汽车的前进方向始终向前， 当汽车左转或右转时，坐标系与汽车一起旋转。

为了更精准地确定车辆在地图上的位置，我们常将车辆传感器识别的地标与高精地图上存在的地标进行比对。为了进行该比对，无人驾驶软件必须将传感器的观测值从车辆坐标系转换为地图坐标系，这也是解决定位问题的关键。

GNSS(GPS)和RTK

一、GPS的组成部分

GPS即全球定位系统，是使用最广泛的GNSS系统，主要由三部分组成。

1. 卫星。在任何特定时间，大约有30颗GPS卫星在外层空间运行，它们各自距离地球表面约2万公里。
2. 世界各地的控制站。用于监视和控制卫星，使系统保持运行，并验证GPS广播信号的精确度。
3. GPS接收器。它存在于手机、电脑、汽车等设备中，如果周围没有高楼等障碍物，并且天气良好，那么GPS接收器每次应至少检测到四颗GPS卫星。GPS接收器首先测量信号的飞行时间，通过光速乘以飞行时间来计算离卫星的距离，由于光速的值很大，即使是少量的时间误差也会在距离计算中造成巨大的误差，所以每颗卫星都配备了高精确度的原子钟。

二、RTK

为了进一步减小误差，采用实时运动定位（RTK）。我们在地面上建立基站，已知的基站“地面实况”位置与通过GPS测量的基站位置之间的偏差就是GPS测量结果中的误差，其他GPS接收器就会根据这个误差来调整自身的位置计算，如下图所示。在RTK的帮助下，GPS可以把定位误差限制在10厘米以内。

三、GPS的缺点

1. 高楼和其他障碍物可能阻挡GPS信号，使定位变得困难或根本无法定位。
2. GPS的更新频率很低，大约为10赫兹或每秒更新10次。因为无人驾驶车在快速移动，所以我们需要更频繁地更新位置。

惯性导航

一、惯性测量单元(IMU)

1. 三轴加速度计精确测量加速度（车辆坐标系下）。
2. 三轴陀螺仪将车辆坐标系下的测量值转换为全局坐标系。如下图所示，三轴陀螺仪的三个外部平衡环一直在旋转，但其旋转轴始终固定在世界坐标系中。所以我们可以通过测量旋转轴和三个外部平衡环的相对位置来计算车辆在坐标系中的位置。

二、IMU的优缺点

1. 优点：IMU以高频率更新，其频率可达1000赫兹，可以提供接近实时的位置信息。
2. 缺点：IMU的运动误差随时间增加而增加，所以我们只能依靠惯性测量单元在很短的时间范围内进行定位。

三、GPS和IMU结合定位

1. 优势：IMU弥补了GPS更新频率较低的缺陷，同时GPS纠正了IMU的运动误差。
2. 不足：如果我们在山间、城市峡谷中或者在地下隧道中行驶时，即使将GPS和IMU系统相结合，也不能完全解决定位问题。因为我们可能会遭遇长时间没有GPS更新的麻烦，这会让整个定位面临较大的失败风险。

激光雷达LiDAR定位

利用激光雷达，我们可以通过点云匹配来获知汽车在高精度地图上的全球位置和行驶方向。

一、迭代最近点（ICP）

对于一次扫描中的每个点，我们需要找到另一次扫描中最接近的匹配点，形成匹配点对。然后把每对点之间的距离误差相加，计算得平均距离误差，最后通过点云旋转和平移来最大限度地降低该平均距离误差。然后我们就可以在扫描数据和地图之间找到匹配，实现了将传感器扫描到的车辆位置转换为全球地图上的位置的操作，然后就可以计算在地图上的精确位置了。

二、滤波算法

滤波算法可消除冗余信息，并在地图上找到最可能的车辆位置。

1、直方图滤波算法

直方图滤波算法又称误差平均和算法（SSD）。首先将传感器扫描的点云滑过地图上的每个位置，同时计算每个扫描的点与高精度地图上对应点之间的误差，然后对误差的平均求和。误差平均和越小，表示扫描结果与地图之间的匹配度越高。 下图中，红色表示对齐较好的点，蓝色表示对齐较差的点，绿色表示中等对齐的点。

2、卡尔曼滤波

卡尔曼滤波使用了预测更新周期，我们可根据之前的状态以及对移动距离和方向的估计来预测新位置。这种运动估计并不完美，所以需要使用传感器测量位置来加以纠正。一旦用传感器测量了新位置，便可使用概率规则，将不完美的传感器测量结果与现有的位置预测结合起来。然后我们将永远遵循这个预测更新周期。

三、LiDAR定位的优缺点

1. 优点：稳健性。只要拥有高精度地图和有效的传感器，我们就能够一直定位。
2. 缺点：难以构建高精度地图并使其保持最新。事实上几乎不可能让地图完全保持最新，因为每个地图均包含瞬态元素（如汽车、行人、街道上的垃圾等），世界上的许多元素都在不断发生变化。

视觉定位

一、粒子滤波

虽然图像最简单的数据类型且摄像头种类繁多便宜，但只通过图像实现精确定位非常困难。实际上，摄像头图像通常与来自其他传感器（如地图和GPS）的数据相结合以准确定位车辆，我们使用粒子或点来估计最可能的位置。

如道路车道线检测时，可以用粒子滤波原理对车道线进行拍照，然后将拍摄的图像与地图进行比较，从而确定车辆在道路中的位置。如下图所示，蓝色代表地图上两个不同位置的车道线，红色代表车辆摄像头观察到的车道线，红线与右侧蓝线的匹配度较高，所以我们可能位于与右侧图像对应的地图位置上。

二、视觉定位的优缺点

1. 优点：图像数据比较容易获得。
2. 缺点：缺乏三维信息和对三维地图的依赖。

Apollo定位

Apollo定位模块依赖于IMU、GPS、激光雷达、雷达和高精度地图，这些传感器同时支持GNSS定位和LiDAR定位，GNSS定位输出位置和速度信息，LiDAR定位输出位置和行进方向信息，融合框架通过卡尔曼滤波将这些输出结合在一起，其中惯性导航用于卡尔曼滤波的预测步骤，GNSS和LiDAR定位用于卡尔曼滤波的测量结果更新步骤。