【Robot定位 学习笔记 1】GPS和IMU（惯导）在无人驾驶中的应用

无人驾驶定位技术

行车定位是无人驾驶最核心的技术之一，全球定位系统（GPS）在无人驾驶定位中也担负起相当重要的职责。然而无人车是在复杂的动态环境中行驶，尤其在大城市，GPS多路径反射的问题会很明显。这样得到的GPS定位信息很容易就有几米的误差。对于在有限宽度高速行驶的汽车来说，这样的误差很有可能导致交通事故。因此我们必须借助其他传感器来辅助定位，增强定位的精度。另外，由于GPS的更新频率低（10Hz），在车辆快速行驶时很难给出精准的实时定位。

惯性传感器（IMU）是检测加速度与旋转运动的高频（1KHz）传感器，对惯性传感器数据进行处理后我们可以实时得出车辆的位移与转动信息，但惯性传感器自身也有偏差与噪音等问题影响结果。而通过使用基于卡尔曼滤波的传感器融合技术，我们可以融合GPS与惯性传感器数据，各取所长，以达到较好的定位效果。注意由于无人驾驶对可靠性和安全性要求非常高，所以基于GPS和惯性传感器的定位并非无人驾驶里唯一的定位方式，我们还会使用LiDAR点云与高精地图匹配，以及视觉里程计算法等定位方法，让各种定位法互相纠正以达到更精准的效果。

GPS简介

全球定位系统（GPS）是当前行车定位不可或缺的技术，在无人驾驶定位中也担负起相当重要的职责。GPS系统包括太空中的32颗GPS卫星，地面上1个主控站、3个数据注入站和5个监测站及作为用户端的GPS接收机。最少只需其中3颗卫星，就能迅速确定用户端在地球上所处的位置及海拔高度。现在民用GPS可以达到十米左右的定位精度。GPS系统使用低频讯号，纵使天候不佳仍能保持相当的讯号穿透性。下面解析GPS的运作原理和技术缺陷。

图1 GPS三边测量法定位

三边测量法定位

如图1所示，GPS定位系统是利用卫星基本三角定位原理、GPS接收装置以量测无线电信号的传输时间来量测距离。由每颗卫星的所在位置，测量每颗卫星至接受器间距离，便可以算出接受器所在位置之三维空间坐标值。使用者只要利用接受装置接收到3个卫星信号，就可以定出使用者所在之位置。在实际应用中，GPS接受装置都是利用4个以上卫星信号，来定位出使用者所在之位置及高度。三角定位的工作原理如下：

• 假设我们测量到第一颗卫星的距离为18000公里，我们就可以把当前可能位置范围限定在离第一颗卫星18000公里的地球表面。
• 接下来，假设我们测量到第二颗卫星的距离为20000公里，那么我们可以进一步把当前位置范围限定在距离第一颗卫星18000公里和距离第二颗卫星20000公里的交叉区域。
• 然后我们再对第三颗卫星进行测量，通过三颗卫星的距离交汇点定位出当前的位置。通常，GPS接收器会使用第四颗卫星的位置对前三个卫星的位置测量进行确认以达到更好的效果。

距离测量与精准时间戳

在理论上，距离测量是个简单的过程，我们只需要用光速乘以信号传播时间就能得到距离信息。但是问题是测量的传播时间但凡有一点误差，都会造成距离上巨大的误差。我们日常使用的时钟是存在着一定的误差的，如果使用石英钟对传播时间进行测量，那么基于GPS的定位会有很大误差。为了解决这个问题，每颗卫星上都安装了原子钟以达到纳秒级的精度。而为了使卫星定位系统使用同步时钟，我们需要在所有接收机上也都安装原子钟。但原子钟的价格在几万美元的区间，让每一个GPS接收器安装如此昂贵的东西是不现实的。为了解决这一难题，可以在每一颗卫星上仍然使用原子钟，但在接收机上使用经常需要调校的普通石英钟。接收机接收来自四颗或更多卫星的信号并计算自身的误差，就可以将自身的时钟调整到统一时间值。

差分GPS

如上所述，卫星距离测量存在着卫星钟与播延迟导致的误差等问题。利用差分技术，我们可以消除或者降低这些误差，让GPS达到更高的精度。差分GPS的运作原理十分简单：如果两个GPS接收机都相当接近对方，那么两者的信号将具有几乎相同的误差，如果能精确地计算出第一个接收机的误差，我们就可以对第二个接收机的结果进行修正。

图2 差分GPS

如何能精确地计算出第一个接收机的误差呢？我们可以在已知精准的地点安置参考接收机基准站，如图2所示，安装在基准站上的GPS接收机观测4颗卫星后便可进行三维定位，解算出基准站的测量坐标。然后我们通过测量坐标与已知坐标对比可以计算出误差。基准站再把误差值发送给方圆100公里内的差分GPS接收器，去纠正它们的测量数据。

图3 Multipath问题

多路径问题

如图3所示，多路径问题是指由于GPS信号的反射与折射造成信号传播时间的误差，这会导致定位的错误。特别在城市的环境中，空气中有许多悬浮介质会反射与折射GPS信号，另外信号也会在高楼大厦的外墙发生反射与折射，这些都造成距离测量的混乱。目前高精度的军用差分GPS，在静态和“理想”的环境下确实可以达到厘米级的精度。这里的“理想”环境是指大气中没有过多的悬浮介质，而且测量时GPS有较强的接收信号。然而无人车是在复杂的动态环境中行驶的，尤其是在大城市中，GPS多路径反射的问题会更加明显。这样得到的GPS定位信息很容易就有几米的误差，很有可能会导致交通事故发生。

即使有各种问题，GPS还是一个相对精准的传感器，而且GPS的误差不会随着时间的推进而增加。但是GPS有一个问题是更新频率低，大概在10Hz左右。由于无人车行驶速度快，我们需要实时精准定位以确保无人车的安全。因此我们必须借助其他的传感器来辅助定位，增强定位的精度。

惯性传感器简介

惯性传感器（IMU）是可以检测加速度与旋转运动的传感器。基础的惯性传感器包括加速度计与角速度计。本文主要讨论基于MEMS的六轴惯性传感器，主要由三个轴加速度传感器以及三个轴的陀螺仪组成。

MEMS惯性传感器分为三个级别：低精度惯性传感器作为消费电子类产品主要用在智能手机，此类传感器售价在50美分到几美元不等，但是测量的误差会比较大。中级惯性传感器主要用于汽车电子稳定系统以及GPS辅助导航系统，此类传感器售价在几百到上千美元，相对于低端惯性传感器，中级惯性传感器在控制芯片中对测量误差有一定修正，所以测量结果更加准确。但是长时间运行后，累计的误差也会越来越大。高精度的惯性传感器作为军用级和宇航级产品，要求高精度、全温区、抗冲击等指数。主要用于通讯卫星无线、导弹导引头、光学瞄准系统等稳定应用。此类传感器售价在几十万美元区间，即便经过长时间运行后，比如跨太平洋洲际导弹，仍然可以达到米级精度。

无人车使用的一般是中低级的惯性传感器。其特点是更新频率高（1KHz），可以提供实时位置信息。但是惯性传感器的致命缺点是它的误差会随着时间的推进而增加，所以我们只能在很短的时间内依赖惯性传感器进行定位。

图4 加速度计

加速度计

图4展示了MEMS加速度计，它的工作原理就是靠MEMS中可移动部分的惯性。由于中间电容板的质量很大，而且它是一种悬臂构造，当速度变化或者加速度足够大时，它所受到的惯性力超过固定或者支撑它的力，这时候它会移动，它跟上下电容板之间的距离就会变化，上下电容就会因此变化。电容的变化跟加速度成正比。根据不同测量范围，中间电容板悬臂构造的强度或者弹性系数可以设计得不同。还有如果要测量不同方向的加速度，这个MEMS的结构会有很大的不同。电容的变化会被另外一块专用芯片转化成电压信号，有时这个电压信号还会被放大。电压信号在数字化后经过一个数字信号处理过程，在零点和灵敏度校正后输出。

图5 角速度计

角速度计

图5展示了MEMS陀螺仪角速度计（MEMS gyroscope），其工作原理主要是利用角动量守恒，它是一个不停转动的物体，转轴指向不随承载它的支架的旋转而变化。与加速度计工作原理相似，陀螺仪的上层活动金属与下层金属形成电容。当陀螺仪转动时，它跟下面电容板之间的距离就会变化，上下电容也会因此而变化。电容的变化跟角速度成正比，由此我们可以测量当前的角速度。

惯性传感器的问题

由于制作工艺的原因，惯性传感器测量的数据通常都会有一定误差。第一种误差是偏移误差，也就是陀螺仪和加速度计即使在没有旋转或加速的情况下也会有非零的数据输出。要想得到位移数据，我们需要对加速度计的输出进行两次积分。在两次积分后，即使很小的偏移误差会被放大，随着时间推进，位移误差会不断积累，最终导致我们没法再跟踪无人车的位置。第二种误差是比例误差，所测量的输出和被检测输入的变化之间的比率。与偏移误差相似，在两次积分后，随着时间推进，其造成的位移误差也会不断积累。第三种误差是背景白噪声，如果不给予纠正，也会导致我们没法再跟踪无人车的位置。

为了纠正这些误差，我们必须对惯性传感器进行校准，找出偏移误差、比例误差，然后使用校准参数对惯性传感器原数据进行修正。但复杂的是，惯性传感器的误差也会随着温度而变化。即使我们校准得再好，随着时间的推进，位移的误差还是会不断积累，所以我们很难单独使用惯性传感器对无人车进行定位。

GPS和惯性传感器融合

如上所述，纵使有多路径等问题，GPS是一种相对精准的定位传感器，但更新频率低，并不能满足实时计算的要求。而惯性传感器的定位误差会随着运行时间增长，但由于其是高频传感器，在短时间内可以提供稳定的实时位置更新。所以我们只要找到一个方法能融合这两种传感器的优点，各取所长，就可以得到比较实时与精准的定位。下面我们讨论如何使用卡尔曼滤波器融合这两种传感器数据。

卡尔曼滤波器简介

卡尔曼滤波器可以从一组有限的、包含噪声的物体位置的观察序列预测出物体的位置坐标及速度。它具有很强的鲁棒性，即使对物体位置的观测有误差，根据物体历史状态与当前对位置的观测，我们也可以较准确地推算出物体的位置。卡尔曼滤波器运行时主要分两个阶段：预测阶段基于上个时间点的位置信息去预测当前的位置信息；更新阶段通过当前对物体位置的观测去纠正位置预测，从而更新物体的位置。

举个具体例子，假设你家停电，没有任何灯光，你想从客厅走回卧室。你十分清楚客厅与卧室的相对位置，于是你在黑暗中行走，并试图通过计算步数来预测当前位置。走到一半时，你摸到了电视机。由于你事先知道电视机在客厅中的大致位置，你可以通过你印象中电视机的位置去更正你对当前位置的预测，然后在这个调整过的更加准确的位置估计的基础上继续依靠计算步数向卧室前行。依靠计算步数与触摸物体，你最终从客厅摸黑走回了卧室，这背后的道理就是卡尔曼滤波器的核心原理。

图6 GPS与IMU的传感器融合定位

多传感器融合

如图6所示，使用卡尔曼滤波器对惯性传感器与GPS数据进性融合与上面给出的例子很相似。这里惯性传感器相当于数步数，而GPS数据相当于电视等参照物的位置。首先我们在上一次位置估算的基础上使用惯性传感器对当前的位置进行实时预测。在得到新GPS数据前，我们只能通过积分惯性传感器的数据来预测当前位置。但惯性传感器的定位误差会随着运行时间增长，所以当接收到新的比较精准的GPS数据时，我们可以使用这个GPS数据对当前的位置预测进行更新。通过不断地执行这两个步骤，我们可以取两者所长，对无人车进行准确实时定位。假设惯性传感器的频率是1KHz，而GPS的频率是10Hz，那么每两次GPS更新之间，我们可以使用100个惯性传感器数据点进行位置预测。

结论

本文介绍了在无人驾驶场景如何使用GPS与惯性传感器对车辆进行精准定位的原理。这个系统包含了三个部分，一个相对精准但是低频更新的GPS，一个高频更新但是精度随着时间流逝而越发不稳定的惯性传感器，以及一个基于卡尔曼滤波器的数学模型去融合这两种传感器，各取所长，以达到又快又准的定位效果。但是由于无人驾驶对可靠性和安全性要求非常高，所以除了GPS与惯性传感器外，我们通常还会使用LiDAR点云与高精地图匹配，以及视觉里程计算法等定位方法，让各种定位法互相纠正以达到更精准的效果。