在上一讲《Coursera自动驾驶课程第14讲：Linear and Nonlinear Kalman Filters》 我们学习了卡尔曼滤波相关知识，包括：线性卡尔曼滤波（KF）、扩展卡尔曼滤波（EKF）、误差卡尔曼滤波（ES-EKF）、无损卡尔曼滤波（UKF）。

本讲我们学习自动驾驶定位中常用的两种传感器：惯性测量单元（IMU） 和全球导航卫星系统（GNSS）。

B站视频链接：

• 3D Geometry and Reference Frames (Video)
• The Inertial Measurement Unit (Video)
• Global Navigation Satellite System (Video)

---

1. 3D Geometry and Reference Frames

1.1 Introduction

回顾一下中学所学知识，向量是具有大小和方向的矢量。在不同的坐标系下，同一个向量可以使用不同的坐标来表示。如下图所示的向量 $r$，在两个坐标系 ${\stackrel{\mathcal{F}}{\to }}_a$ ，${\stackrel{\mathcal{F}}{\to }}_b$ 下将会有两个不同的表示 ${\mathbf{r}}_a$ 和 ${\mathbf{r}}_b$。

很显然，向量${\mathbf{r}}_b$ 可由向量 ${\mathbf{r}}_a$ 经过旋转变换而来，即可以找到一个旋转矩阵 ${\mathbf{C}}_{ba}$，满足 ：
$${\mathbf{r}}_b={\mathbf{C}}_{ba}{\mathbf{r}}_a$$

旋转矩阵 ${\mathbf{C}}_{ba}$ 表示向量从 $a$ 坐标系向 $b$ 坐标系进行变换。

然而在现实世界中，两个坐标系的原点经常不是同一个原点，这时候还需要进行平移变换处理，如下图所示。惯性坐标系为 ${\stackrel{\mathcal{F}}{\to }}_i$，车辆坐标系为 ${\stackrel{\mathcal{F}}{\to }}_v$。现在我们用 ${\mathbf{r}}_i^{pi}$ 表示点 $P$ 在惯性坐标系的位置，${\mathbf{r}}_i^{vi}$ 表示车辆坐标系在惯性坐标系下的位置，${\mathbf{C}}_{iv}$ 表示车辆坐标系到惯性坐标系的旋转变换矩阵，${\mathbf{r}}_v^{pv}$ 表示点 $P$ 在车辆坐标系下的位置。则 ${\mathbf{r}}_i^{pi}$ 可以写成：
$${\mathbf{r}}_i^{pi}={\mathbf{r}}_i^{vi}+{\mathbf{C}}_{iv}{\mathbf{r}}_v^{pv}$$

---

1.2 Rotation Representation

旋转变换可以有很多方式来表示。 最常见的是使用旋转矩阵。 它通常也被称为方向余弦矩阵。 旋转矩阵的一个重要的性质就是 $R^{-1}=R^T$。

第二种表示方法就是使用单位四元数。 四元数本身就是一个有趣的数学主题，但在本课程中，知道单位四元数可以表示为单位长度的四维向量就足够了，这里有两个需要注意的东西，一是旋转轴 $\mathbf{u}$，二是旋转角度 $\varphi$。 为什么要使用四元数？ 因为它不受奇点的影响，与旋转矩阵相比它只需要四个参数而不是九个。

第三种表示方法就是使用欧拉角。欧拉角之所以有吸引力，原因在于它是一种直观简约的表示，只需要知道绕每个轴转动的角度就可以得到最终的旋转变换。

在自动驾驶中会选择哪一种旋转表示方式呢？ 这要看具体情况。 每种表达方式都有优缺点。旋转矩阵可以表示任何旋转，但需要九个参数并有六个约束。单位四元数也可用于表示任何旋转，但它也有一个约束。 最后，欧拉角不受约束、直观地可视化并且仅需要三个参数，但受奇点影响。

---

1.3 Reference Frames

现在让我们看一下车辆定位时将使用到的重要参考坐标系。 第一个坐标系是地心惯性坐标系或 ECIF。该坐标系的原点位于地球中心，$\mathbf{z}$ 轴指向正北，$\mathbf{x}$ 轴和 $\mathbf{y}$ 轴相对于非常遥远的恒星是固定的。 这意味着虽然地球绕 $\mathbf{z}$ 轴旋转，但 $\mathbf{x}$ 和 $\mathbf{y}$ 轴不会移动。

接下来，以地球为中心的（ECEF）与 ECIF 类似，只是它的 $\mathbf{x}$ 轴与本初子午线对齐并与地球一起旋转。$\mathbf{y}$ 轴由右手定则确定。ECEF坐标系是定位中经常用到的一个坐标系，主要是与GNSS传感器有关。

尽管ECEF和ECIF在我们讨论卫星和飞机上的惯性传感时很有用，但对于汽车应用，我们通常希望使用相对于地面固定的框架。为此，我们将使用我们所说的导航坐标系。 一个常见的导航坐标系是选择某个位置作为起点并且坐标系与北、东和向下反向对齐的导航坐标系（NED坐标）。 最后，我们还经常需要考虑刚性连接到传感器的传感器坐标系，例如 LIDAR、GPS接收器或惯性测量单元。传感器坐标系通常与车辆坐标系不同，后者可以放置在车辆上任何方便的地方，例如在质量中心。 对于定位，我们通常会忽略车辆和传感器坐标系之间的区别，并假设如果我们可以跟踪传感器，我们应该能够在适当校准的情况下跟踪车辆上的任何点，这是我们稍后研究的主题。

---

2. The Inertial Measurement Unit

2.1 Introduction

惯性测量单元或IMU测量物体在惯性空间中的运动。如今，几乎所有智能手机中都配备了IMU。它们经常用于健康跟踪的计步，以及最近用作增强现实设备。

尽管它们在今天无处不在，但能够准确跟踪运动物体运动的传感器的开发是20世纪的一项重大成就。 IMU早在GPS之前就为跨洋飞行提供了帮助，并且作为机载制导、导航和控制系统的一部分对阿波罗任务至关重要。阿波罗航天器依靠IMU来准确跟踪飞行器在漫长的月球航行中的位置和方向。

在现代自动驾驶汽车中，IMU扮演着非常相似的角色。在来自其他传感器的导航信息不可用或不可靠时来进行导航计算。那么什么是IMU呢？

一般来说，惯性测量单元是一个复合传感器套件，它结合了三轴陀螺仪和三轴加速度计来跟踪刚体的外部自由运动。一些IMU还包含磁力计或指南针，以帮助跟踪方向。IMU有多种形状和形式。现代智能手机中的传感器相对便宜，批量购买时的成本通常不到几美元。它们重量轻，需要的功率相对较小，但会产生相当嘈杂的测量结果。而更昂贵的IMU使用更复杂的组件并具有更准确的校准模型，可以消除温度波动等的影响。

---

下面简单介绍下陀螺仪。陀螺仪这个词本身来自希腊词。历史上，陀螺仪是一个旋转的圆盘，它的角动量会阻止方向的变化。在 19 世纪末和 20 世纪初，工程师们意识到这种设备可以用作海洋和航空导航的方向参考。

在现代陀螺仪中，旋转轮通常被微机电系统取代，该系统由一个小的硅音叉组成，该系统根据施加的旋转或方向变化来改变其共振特性。这种传感器要便宜得多，而且体积也很小。然而，它们会产生嘈杂的测量结果并且对温度的波动很敏感。此外，它们测量旋转速率，而不是直接测量方向，因此必须对输出信号进行数值积分以确定方向变化。这个过程会在最终的方向估计中引入额外的误差。

---

加速度计测量沿单个轴的加速度。更便宜的基于MEMS的加速度计使用微型悬臂梁，并附有检测质量。当传感器加速时，光束发生偏转。可以通过电容电路测量这种偏转，并将其转换为加速度值。

然而，出于导航目的，我们更关心的是相对于某个固定参考系的加速度。为了计算这个加速度，我们需要使用重力场中加速度计的基本方程。
$$\mathbf{f}+\mathbf{g}={\ddot{\mathbf{r}}}_i$$

---

2.2 IMU Measurement Models

首先介绍陀螺仪测量模型，模型方程如下：
$$\omega (t)={\omega }_s(t)+{\mathbf{b}}_{\mathrm{g}\mathrm{y}\mathrm{r}\mathrm{o}}(t)+{\mathbf{n}}_{\mathrm{g}\mathrm{y}\mathrm{r}\mathrm{o}}(t)$$

其中 ${\omega }_s(t)$ 是陀螺仪测量值， 测量模型中包含一个缓慢变化的偏置项 ${\mathbf{b}}_{\mathrm{g}\mathrm{y}\mathrm{r}\mathrm{o}}(t)$ 和一个白高斯加性噪声项 ${\mathbf{n}}_{\mathrm{g}\mathrm{y}\mathrm{r}\mathrm{o}}(t)$ 来模拟传感器误差。尽管陀螺仪确实测量地球的自转，但对于我们只关心短时间运动的应用，忽略这一点通常是安全的。

加速度计测量模型将具有类似的噪声和偏置项。但是现在，我们不再像使用旋转角速度那样直接测量身体加速度，而是需要使用重力场中加速度计的基本方程来消除重力的影响。
$$\mathbf{a}(t)={\mathbf{C}}_{sn}(t)\left({\ddot{\mathbf{r}}}_n^{sn}(t)-{\mathbf{g}}_n\right)+{\mathbf{b}}_{\mathrm{a}\mathrm{c}\mathrm{c}\mathrm{e}\mathrm{l}}(t)+{\mathbf{n}}_{\mathrm{a}\mathrm{c}\mathrm{c}\mathrm{e}\mathrm{l}}(t)$$

可以看到，为了得到准确的加速度，我们首先要得到准确的方向矩阵 ${\mathbf{C}}_{sn}(t)$，而这又需要我们得到准确的角速度。

最后，让我们讨论一下IMU模型的一些限制。

• 首先，准确的方向估计对于准确的位置估计至关重要。 当我们将测得的比力转换为加速度时，我们必须确保重力方向是正确的。 否则，即使方向上的一个小错误也会让我们认为我们正在加速，而实际上我们并没有。
• 其次，我们推导出的两个模型都忽略了地球自转的影响。 对于更远距离的导航（飞机或导弹导航时不能忽略），这实际上很重要。
• 最后，我们推导出的模型适用于捷联IMU。 这些IMU是物理固定在车辆上，并且没有在万向节上安装旋转轮。 尽管后者可能更准确，但由于体积大且成本高，它们很少用于汽车应用。

---

3. Global Navigation Satellite System

3.1 Introduction

与上一节介绍的IMU一样，几乎每部现代智能手机都至少有一种类型的GNSS接收器。尽管我们现在认为它们是理所当然的，但第一个现代全球定位卫星系GPS 是在 1980 年代为军事用途而建造的。

到 1995 年第二版系统全面运行时，GPS 已免费向公众开放。部分原因是大韩航空 007 号航班在 1983 年坠毁。007 号航班是一架 747 客机，从纽约飞往首尔，在阿拉斯加安克雷奇加油。当它从安克雷奇飞往首尔时，它偏离了计划的飞行路线，在苏联领空停留了几个小时后被一架战斗机击落。

为了穿越北太平洋，007 航班依靠惯性导航系统进行引导。飞行员未能正确初始化系统，错误地将飞机保持在特定的磁航向。反过来，这架飞机偏离了计划航线 300 多公里。在 007 航班被击落后不久，罗纳德·里根总统发布了一项指令，允许美国全球定位系统在完全开发后免费向公众开放。

---

GPS系统由位于六个轨道平面的 24-32 颗卫星组成。卫星会定期退役和更换。高度约为 20,000 公里，轨道周期不到 12 小时。系统的设计使得至少四颗卫星在地球上的任何表面点始终可见。每颗卫星以两种频率进行广播，一种用于民用，一种用于军用。每个广播信号都包含一个伪随机码，用于识别卫星位置和信号传输时间。

GPS背后的基本原理是到达时间测距。接收器通过将自己的内部时钟与传输时间的时钟进行比较来计算每个可见卫星的距离。使用电磁信号以光速传播的知识将时间差转换为距离。为了计算 3D 位置，测距方程需要至少四颗可见卫星。如果已知高度并且只需要 2D 位置，则只需要三颗卫星。

---

3.2 Trilateration

对于每颗卫星，伪距测量方程为：
$${\rho }^{(i)}=c\left(t_r-t_s\right)=\sqrt{{\left({\mathbf{p}}^{(i)}-\mathbf{r}\right)}^T\left({\mathbf{p}}^{(i)}-\mathbf{r}\right)}+c\Delta t_r+c\Delta t_a^{(i)}+{\eta }^{(i)}$$

各物理量含义如下：

请注意，这与三角测量不同，三角测量基于角度测量计算位置。 该模型考虑了接收器时钟误差、大气传播延迟和测量噪声。术语伪距是指距离信息被上述误差源破坏的事实。 每个伪距测量在 2D 中定义一个圆或在 3D 中定义一个球体。 如果我们正好有四颗卫星，我们可以明确地求解接收器位置和接收器时钟误差。

如果我们有四个以上，我们可以使用最小二乘法找到假设高斯噪声的最大似然位置。

---

3.3 Error Sources and Improvements

GPS 存在多种误差源：

• 首先，电离层中的带电离子可以将信号延迟。
• 周围的地形和建筑物可能会引起反射，从而增加信号在到达接收器之前传播的距离，这些被称为多径误差。
• 时钟同步或卫星位置信息中的任何小错误都可能导致灾难性后果。即使是 1 微秒的计时误差也会导致 300 米的重大位置误差。星历数据和卫星时钟都会经常更新和重新校准，但校准可能会过时。
• 最后，可见卫星的几何配置也会导致定位精度的变化。这被称为精度的几何稀释。为了获得更高的精度，最好采用卫星遍布天空的配置。

幸运的是，对于某些应用程序，我们可以通过以各种方式增强系统来提高GNSS精度。

• 差分GPS 可以通过使用一个或多个固定基站更准确的已知位置来校正接收机定位估计。修正以不同的频率广播到移动车辆中的 GNSS 接收器。
• 在某些情况下，实时动态或RTK GPS 利用载波相位信息将定位精度提高到两厘米。

尽管这两种技术都可以显着提高 GPS 的精度，但它们的实施成本通常非常高。正如我们在之前的视频中提到的，惯性传感器对于导航非常有用。然而，随着时间的推移，它们会漂移或累积无限的误差。相比之下，GPS 系统提供有界误差定位更新。配备 GPS 的自动驾驶汽车将始终保持有保证的定位精度水平，除非 GPS 接收器出现故障或丢失至少四颗卫星的轨道。