【04】无人驾驶的定位（Localization）

今天我们来了解无人驾驶车辆如何以厘米级别的精度进行定位。

1.无人驾驶的定位

定位是让无人驾驶车知道自身确切位置的方法。在我们日常生活中，我们一直是使用手机GPS来确定自己的位置的，但是对于无人车而言，GPS的定位是不够精确的，不精确性体现在两个方面，一个是精度，民用的GPS精度通常会在几米，我们在地图上经常看到我们的定位在河里。另一个就是效率，通常GPS定位的频率在10Hz，10Hz是什么概念，100ms定位一次，假定一辆车以80KM/h的速度行驶，那它就是22m/s的速度，100ms的时间车辆已经开过了2.2m的距离了，这种距离对于车辆来说，是不能接受的。因此无人驾驶车辆需要有更高精度和更高效率的定位方式来满足定位功能的要求。

无人驾驶中最常用的定位方式是将汽车传感器所看的内容与地图上所显示的内容进行比较，车辆传感器可以测量车辆与静态障碍物之间的距离，例如树木、电线杆、路标和墙壁等。

除了对静态障碍物的测量，我们还需要考虑自车的坐标系，在车辆的坐标系中，坐标的X轴始终指向汽车前进方向，汽车转向的时候，坐标轴跟随着车一起动，这必然会形成车辆坐标系和地图坐标系的偏差角。

所以这就涉及到坐标系的转换，我们把传感器的地标观测值，与地图上的地标上的位置进行匹配，地图上的地标位置是已知的，无人驾驶系统将传感器的测量值从车辆的坐标系转换为地图坐标系，执行这类转换后，就可以推出自车的定位。

其实除了上述的描述，无人驾驶中有各种各样的定位方式，每种方式又自己的优缺点，接下来我们将探讨几种常见的定位方法，如GNSS RTK，惯性导航、LiDAR定位和视觉定位。

2.GNSSRTK

2.1 GNSS

全球卫星定位系统，简称GNSS，是指通过导航卫星，向定位物体发送信号，用光速 * 接收到信号的时间，就能得到定位物体到导航卫星的距离。

GNSS系统泛指所有的卫星导航系统，包括全球的、区域的和增强的，如美国的GPS、俄罗斯的Glonass、欧洲的Galileo、中国的北斗卫星导航系统，以及相关的增强系统，如美国的WAAS（广域增强系统）、欧洲的EGNOS（欧洲静地导航重叠系统）和日本的MSAS（多功能运输卫星增强系统）等，还涵盖在建和以后要建设的其他卫星导航系统。其中，GPS是使用最广泛的GNSS系统。

2.2 GNSS定位的原理

GNSS系统定位使用的原理很简单，定位物体在同时接受4颗导航卫星的信号，根据光速 * 时间得到距离卫星的距离，然后以每颗卫星为圆心，以距离为半径画球，四个球体与地球的相交之处，就是定位物体的位置。

这个原理中其实存在一个小疑问，为什么要以卫星为圆心画球，那卫星的位置到底是怎么确定的。卫星的三维坐标，这个坐标可以通过卫星星历计算获得。星历是描述卫星运行轨道的一组参数，卫星轨道是一个椭圆，通过几个参数和时间，可以唯一确定卫星的准确位置。这里我们不对其他的展开太多，有兴趣的小伙伴可以研究更详细的卫星定位的原理讲解。

我们知道工程问题一定是远比数学问题复杂的，所以GNSS的理论虽然很棒，但是会因为各种原因产生定位误差，例如卫星信号在穿过大气层的时候发生折射，在高楼林立的城市中会因为障碍物阻挡发生反射等，这些都会导致传播时间计算误差，继而带来几十米甚至上百米的定位误差。

解决这个问题，差分定位技术RTK就很好的解决了这个问题。

2.3 差分定位技术（RTK）

我们建立一个已知精确坐标的基准站，通过导航卫星对基准站进行定位，得到基准实时定位坐标，这样，我们就知道实时坐标和实际坐标之间的差值，从而得到一个综合定位误差。这个误差是怎么使用的呢？

我们认为基准站为中心的20-40千米范围内，大气层、障碍物遮挡等因素处在一个基本一致的范围内，所以在该范围内，所有定位物体都应该有相同的综合定位误差，只要基站把综合定位误差实时发送给该范围内的定位终端，他们就可以在卫星定位时将定位误差计算进去，从而实现分米级甚至厘米级的定位精度。这就是差分定位技术，也叫RTK技术。

在RTK技术的帮助下，我们已经实现了很高精度的定位，但是仍然存在一些其他的问题：局部障碍物的阻挡无法通过RTK解决，同时GNSS本身定位频率太低也不足以支持无人车的使用（关于频率问题以GPS为例已经在第一章讲述过）。这种控制频率下是毫无安全性和体验感的，所以在这种情况下，我们还引入了IMU、计算机视觉等其他定位方式。

3.惯性导航定位（IMU）

MU 全称 inertial measurement unit，即惯性测量单元，它由三个单轴的加速度计和三个单轴的陀螺仪组成，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺仪检测载体相对于导航坐标系的角速度信号，对这些信号进行处理之后，便可解算出物体的姿态。

值得注意的是，IMU 提供的是一个相对的定位信息，它的作用是测量相对于起点物体所运动的路线，所以它并不能提供你所在的具体位置的信息，因此，它常常和 GPS 一起使用，当在某些 GPS 信号微弱的地方时，IMU 就可以发挥它的作用，可以让汽车继续获得绝对位置的信息，不至于“迷路”。

我们每天使用的手机，出行会用到的汽车、飞机，甚至导弹、宇宙飞船都会使用到IMU。根据不同的使用场景，对IMU的精度有不同的要求，精度高，也意味着成本高。精度最高的IMU会用于导弹或航天飞机。就以导弹为例，从导弹发射到击中目标，宇航级的IMU可以达到极高精度的推算，误差甚至可以小于一米。

除了精度和成本的特性外，IMU 还有两个十分关键的特性，第一是更新频率高，工作频率可以达到 100Hz 以上；第二是短时间内的推算精度高，不会有太大的误差。

看看这个工作频率，100Hz，刚好是GPS的10Hz的十倍，GPS每进行一次定位，IMU就进行十次定位辅助推算，所以我们也经常讲GPS + IMU称为黄金搭档，他们两者相辅相成，让无人车有了非常高精度的定位。

4.激光雷达定位

利用激光雷达，我们可以通过点云匹配来对无人车进行定位。该方法将来自激光雷达传感器的检测数据与预先存在高精度地图的数据进行匹配，通过这种比较，可以知道无人车在高精度地图上的全球位置和行驶方向，有很多的算法可以用于匹配点云。

4.1 迭代最近点（IPC）

迭代最近点（ICP）是匹配点云的一种方法，假设我们相对两次点云扫描进行匹配，对于一次扫描的点，我们需要找到另一次扫描中最接近的匹配点，经过匹配我们搜集到许多匹配的点对（两个最接近的点是一个点对），我们把每对点之间的距离误差相加然后计算平均距离误差，通过点云旋转和平移最大限度的降低这一平均距离误差，一旦我们最大限度降低了点云之间的误差，我们就可以在传感器扫描和地图之间找到匹配，然后将传感器扫描到的车辆位置转换为全球地图上的位置，最终计算出无人车在地图上的精确位置。

4.2 滤波算法

滤波算法是另一种雷达定位的方法，滤波算法可以消除冗余信息，并在地图上找到最可能的车辆位置，最常见的滤波算法是直方图滤波算法，该算法有时也被称为误差平方和算法（SSD）。我们将传感器扫描的点云滑过地图上的每一个位置，我们计算扫描的点与高精度地图上的对应点之间的误差或距离，然后对误差的平方求和，求得的和越小，扫描结果与地图的匹配越好。

4.3 卡尔曼滤波算法

卡尔曼滤波算法用于根据我们在过去的状态和新的传感器测量结果预测我们的当前状态，具体来讲，卡尔曼滤波使用了预测更新周期，首先，我们根据之前的状态以及对移动距离和方向进行估计，来预估出我们新的位置，当然这种预测并不是非常准确，所以需要通过使用传感器测量我们的位置并加以纠正，一旦用传感器测量了我们的新位置，我们就可以使用概率规则，将也不是非常准确的传感器测量结果与我们先有的位置预测结合起来，以此得到更加准确的定位。

雷达定位的主要优势在于稳定性，只要具备高精度地图，且存在有效的传感器，我们就能够进行定位，这种方法的主要缺点就在于难以构建高精度地图，并保持地图持续更新。

5.视觉定位

通过图像实现精确定位是非常困难的，所以摄像头图像通常与其他传感器的数据相结合，以确定车辆的准确位置。但将摄像头数据与地图和GPS数据相结合比单独使用摄像头图像进行定位效果更好。

假如一辆车正在路上行驶，它感知到右边有一颗树，但是地图显示道路右侧有好几棵树，全部位于不同的位置，那我们面临的问题就是如何知道车辆现在看到的是哪一棵树。

我们可以用概率的方法来解决这个问题，想象一下，我们位于道路上许多不同点的任意一处，使用概率来确定哪个点最可能代表我们的实时位置，我们知道我们在右边看到了一棵树，我们假设从一些点可以看到右边有一颗树，从一些点则看不到，自然而言，我们很可能位于可以看到右边有一棵树的点位，因此我们可以排除那些在地图上无法看到右边那棵树的点，我们可以在开车的同时继续观察周边世界，想象一下，我们开车前行，然后观察到右边的另一棵树，在观察地图上的其余点之后，我们发现仅在几个少数位置会发现右侧有成排的两棵树，这样一来我们当然是最有可能位于这些位置之一。我们继续这个过程，通过观察结果、概率和地图来确定们最可能的位置，这个过程被称为是粒子滤波（使用粒子或者点来估计最可能的位置）。

当然了树木在一些道路上可能比较稀少，但是车道线确实非常常见的，我们可以使用相同的粒子滤波原理对车道线进行拍照，然后使用拍摄的图像来确定车辆在道路中的位置。

我们来看一个视觉车道线匹配的例子，蓝色代表地图上两个不同位置的车道线，红色代表车辆摄像头观察到的车道线，我们用红线和蓝线进行匹配，很明显，当前我们更大概率是在右边的蓝线上。

视觉定位的优点在于图像数据很容易获得，毕竟摄像头本身也更便宜，缺点在于缺乏三位信息和对三维地图的依赖。

5.Apolo的定位

我们来说说百度Apollo的定位方法。

Apollo使用基于GPS、IMU和激光雷达的多传感器融合定位系统，这种融合方法利用了不同传感器的互补优势，它提高了稳定性和准确性，Apollo定位模块依赖于IMU、GPS、激光雷达、毫米波雷达和高精度地图，这些传感器同时支持GNSS定位和LiDAR定位，GNSS定位输出位置和速度信息，LiDAR定位输出位置和行进方向信息，融合框架通过卡尔曼滤波将这些输出结合在一起，卡尔曼滤波建立在两步预测测量周期之上，在Apolo中，惯性导航解决方案，用于卡尔曼滤波的预测步骤，GNSS和LiDAR定位用于卡尔曼滤波的测量结果更新步骤。