Coursera自动驾驶课程第17讲：An Autonomous Vehicle State Estimator

在第16讲《Coursera自动驾驶课程第16讲：LIDAR Sensing》我们学习了自动驾驶目前常用的3D 传感器，激光雷达，了解了激光雷达的工作原理，掌握了对点云数据的操作以及如何使用点云配准方法来进行汽车定位。

回顾一下，在本模块中我们学习了状态估计常用的方法、3D 几何知识和自动驾驶中常用的传感器。我们现在将所有这些所学工具结合起来构建一个可以在真正的自动驾驶汽车上使用的状态估计器。一辆真正的自动驾驶汽车通常会配备许多不同类型的传感器。 如下图所示，配备了摄像头、LiDAR、IMU、Radar、GPS 或 GNSS 接收器和一个车轮编码器。 所有这些传感器以不同的频率提供不同类型的数据。例如，IMU 可以以 200 Hz 的频率来发出加速度和角速度数据，而 LiDAR 每秒仅能完成 20 次完整扫描。

因此在实践中，我们通常需要进行多传感器融合以得到最佳估计。多传感器融合是自动驾驶汽车最重要的技术之一。 在本讲中：

• 我们将看到 IMU 和 GNSS 或 LiDAR 融合定位的例子，我们将学习如何使用 EKF 进行多传感器融合；
• 我们将讨论如何标定我们的传感器以确保传感器模型是准确的，从而知道所有传感器的参考坐标是如何相互关联的；
• 我们还会讨论一些应用常遇到的问题，如当一个或多个传感器发生故障、在实际应用中如何考虑速度和精度要求、定位失败时如何处理、以及如何应对我们周围正在移动和变化的环境。

1. State Estimation in Practice

1.1 Overview

如果我们有一辆配备了许多不同传感器的自动驾驶汽车，我们想做的是弄清楚如何结合所有这些不同的传感器信息来获得对车辆状态的最佳估计。融合所有这些数据似乎是一项艰巨的任务，但实际上，我们已经拥有执行此操作的工具。在下一节，我们将讨论如何使用熟悉的工具（如扩展卡尔曼滤波器）将所有传感器数据组合成对车辆状态的一致估计。

但是为了进行传感器融合，我们首先需要了解我们的传感器以及它们在车辆上的配置方式。一方面，我们的传感器模型可能取决于车辆特顶要求或传感器本身参数。一个很好的例子是使用车轮编码器来测量汽车的前进速度。车轮编码器测量车轮的角速度。但是如果我们想用它来获得车辆的前进速度，我们还需要知道轮胎的半径。关于车辆，我们需要了解的另一件事是每个传感器相对于车辆参考系的姿态（位置和方向）。这是因为我们正在组合来自位于不同位置的传感器的信息，所以我们需要知道如何转换所有测量值，以便它们在一个共同的参考系中表示。此外，我们需要考虑传感器测量的时间同步程度，以便我们可以准确地融合它们。 这很好理解，传感器时间同步越准确，状态估计就会越好。其中一部分涉及确定传感器记录测量值与估计器接收测量值进行处理之间的时间偏移。所有这些因素都是标定时需要考虑的，我们将在第三节中更详细地讨论。

---

1.2 Accuracy & Speed Requirements

自动驾驶汽车在道路上安全行驶的状态估计需要多准确呢？这取决于汽车的大小、车道的宽度和交通密度。一辆汽车的宽度大约为 1.8 米，一条普通的高速公路车道宽度大约有 3 米。因此，我们的装态估计器需要足够好，才能将汽车定位在车道两侧 60 厘米左右的范围内（这是假设我们确切地知道车道在哪里并且没有其它交通参与者）。作为比较，GPS 测量准确度的通道是 1-5 米，具体取决于硬件型号、可见的卫星数量和其他因素。因此，很明显，即使是车道保持功能，仅使用 GPS 也是不够的。我们需要结合来自许多不同传感器的信息。

速度呢？我们需要以多快的速度更新车辆状态或者车辆可以对快速变化的环境或意外事件做出反应？这一切都取决于汽车在什么样的环境中运行。想象一下，我们正在闭着眼睛驾驶汽车，并且恰好每秒睁一次眼，看看周围的环境并进行一些调整。对于在没有其它交通参与者的街道上行驶，也许我们会觉得这样做相对安全。但是，如果我们开车穿过一个繁忙的城市十字路口，周围有几十辆其它汽车、公共汽车、骑自行车的人和行人呢？每秒钟睁一次眼睛就变得不够安全了。

根据经验，自动驾驶汽车需要能够达到15-30Hz的更新频率。但是，实际应用时需要进行权衡。自动驾驶汽车只有这么多可用的车载计算能力，除了状态估计之外，计算机还需要处理许多不同的操作，如控制、路径规划和感知。更重要的是，车载可用的计算能力总量可能会受到对计算机实际允许消耗多少功率的限制。

使用有限的计算资源进行状态估计，对我们的算法复杂度和计算解决方案所允许的时间之间需要进行权衡。作为自动驾驶汽车工程师，我们需要决定自动驾驶汽车在这条折衷曲线上的位置。

---

1.3 Localization Failures & Localization Failures

即使我们有一个快速准确的估计算法，也会出现定位失败的情况。这是因为：

• 我们可能有一个或多个传感器报告错误数据，甚至数据可能完全无效。一个很好的例子是 GPS，它在隧道和车路中根本不起作用，并且在有很多高层建筑的城市中很难应对反射信号；
• 我们也可能在状态估计算法本身中遇到错误。如果我们使用具有高度非线性传感器模型的扩展卡尔曼滤波器，我们可能会发现估计器中固有的线性化误差意味着我们可能会失去状态估计的准确性，即使估计器对其输出非常有信心；
• 或者我们的估计器本身就有很大的不确定性。回想一下卡尔曼滤波器方程，当我们通过运动模型向前传播时（使用 IMU 进行状态估计），我们状态的不确定性会增加，并且只有在我们结合来自 LiDAR 或 GPS 的外部观察时才会缩小；
• 如果我们的 LiDAR 坏了，并且我们在没有 GPS 的情况下在隧道中行驶，我们可以依赖 IMU 和运动模型行驶多长时间呢？我们将在第四节中讨论检测和处理此类定位失败问题的方法；

最后，我们需要考虑汽车所处的世界。在大多数情况下，我们为 LiDAR 等传感器开发了我们的模型，假设世界是静止且不变的。但在现实中，世界总是在不断变化。例如，其它汽车、行人和骑自行车的人可能正在移动。一天中的灯光会发生变化，甚至世界的几何形状也会随着季节而变化。自动驾驶汽车面临的一大挑战是找到解释这些变化的方法，无论是通过建模还是通过找到识别和忽略违反我们假设的对象的方法。事实上，这仍然是一个非常活跃的研究领域。

因此，总结这一小节：

• 自动驾驶汽车状态估计时通常会通过多传感器融合来组合来自许多不同类型传感器的信息，如 IMU、LiDAR、相机和 GPS 或 GNSS 接收器；
• 为了使传感器融合按预期工作，我们需要通过确定传感器模型的参数来标定传感器。所有传感器的相对位置和方向以及轮询时间的任何差异。我们还需要在算法中考虑速度和准确性之间的权衡；
• 最后，我们需要考虑如何安全地处理定位失败和不符合我们假设的情况，例如移动物体；

---

2. Multisensor Fusion for State Estimation

2.1 Overview

现在我们已经学习了定位所使用的硬件和方法，让我们将所有内容放在一起。 在本小节中，我们将构建一个误差状态扩展卡尔曼（ES-EKF）滤波器，该滤波器使用 IMU、GNSS 接收器和 LiDAR 估计自动驾驶汽车的位置、速度和方向（如下图所示）。

在传感器融合进行状态估计时，首先要考虑的一个重要因素是来自不同传感器的误差是否相关。换句话说，如果一个传感器发生失效，另一个也可能会失效。在本节中，我们讨论的三个传感器分别使用不同的测量方法，因此三个传感器不太可能因为相同的原因而同时失效。其次，我们应该尽可能选择具有互补性的传感器。 在我们的这个例子中，IMU 充当 GPS 或 GNSS 位置估计的高频平滑器。 GNSS 可以减轻由 IMU 漂移引起的误差，也可以使用车轮里程计消除误差。在这里，我们选择使用 IMU，因为它们可以提供完整的三维位置和方向信息，而车轮里程计仅能提供二维信息。最后，LiDAR 可以根据已知地图信息提供非常准确的位置估计来补充 GNSS 信息。 相反，GNSS 可以告诉 LiDAR 大致位置从而在定位时选择要使用的地图。 出于 EKF 状态估计的目的，我们可以实现所谓的松耦合估计器或紧耦合估计器。在紧耦合的 EKF 中，我们使用来自 GNSS 和 LiDAR 的原始伪距和点云测量值作为观测值。 在一个松耦合的系统中，我们假设这些数据已经被预处理以产生一个位置估计。 尽管紧耦合的方法可以实现更准确的定位，但实施起来通常很繁琐，并且需要大量调整。出于这个原因，我们将在这里实现一个松耦合的 EKF。

---

2.2 Motion Model & Measurement Model

下图是我们整个状态估计的框架，这是一个卡尔曼滤波系统框架。 我们将使用高频的 IMU 测量作为运动模型的输入，这将为我们提供预测状态，每次进行 IMU 测量时都会更新（卡尔曼系统中的预测模块）。 使用低频的 GNSS 和 LiDAR 测量进行融合更新，同时来纠正我们的预测状态（卡尔曼系统中的更新模块）。

那么，我们的状态是什么呢？为了实现状态估计，我们将使用一个十维的状态向量 ${\mathbf{x}}_k$ ，包括：3D 位置 ${\mathbf{p}}_k$、3D 速度 ${\mathbf{v}}_k$、 单位四元数 ${\mathbf{q}}_k$，这些四元数将表示我们的车辆相对于导航坐标系的方向，数学表示为：
$${\mathbf{x}}_k=\left[\begin{array}{l}{\mathbf{p}}_k\\ {\mathbf{v}}_k\\ {\mathbf{q}}_k\end{array}\right]\in R^{10}\text{\hspace{1em}(17.1)}$$

我们假设 IMU 在传感器坐标系中输出加速度和旋转角速度，并将它们组合成单个输入向量 ${\mathbf{u}}_k$。同样重要的是要指出我们这里没有讨论加速度计或陀螺仪的偏差。这些通常被放入状态向量中，进行估计，然后从我们的 IMU 测量中减去。为方便讨论，这里假设 IMU 测量是无偏的，输入量数学表示为：
$${\mathbf{u}}_k=\left[\begin{array}{c}{\mathbf{f}}_k\\ {\omega }_k\end{array}\right]\in R^6\text{\hspace{1em}(17.2)}$$

下面来介绍运动模型，运动模型输入为 IMU 的加速度和旋转速率。运动模型数学公式如下：
$$\begin{array}{rl}& {\mathbf{p}}_k={\mathbf{p}}_{k-1}+\Delta t{\mathbf{v}}_{k-1}+\frac{\Delta t^2}{2}\left({\mathbf{C}}_{ns}{\mathbf{f}}_{k-1}+\mathbf{g}\right)\\ & {\mathbf{v}}_k={\mathbf{v}}_{k-1}+\Delta t\left({\mathbf{C}}_{ns}{\mathbf{f}}_{k-1}-\mathbf{g}\right)\\ & {\mathbf{q}}_k=\mathbf{\Omega }\left(\mathbf{q}\left({\mathbf{\omega }}_{k-1}\Delta t\right)\right){\mathbf{q}}_{k-1}\end{array}\text{\hspace{1em}(17.3)}$$

其中，${\mathbf{C}}_{ns}$ 为从传感器坐标 $s$ 到导航坐标 $n$ 的旋转矩阵。
$${\mathbf{C}}_{ns}={\mathbf{C}}_{ns}\left({\mathbf{q}}_{k-1}\right)\text{\hspace{1em}(17.4)}$$
$\mathbf{\Omega }\left(\left[\begin{array}{l}{q}_w\\ {\mathbf{q}}_v\end{array}\right]\right)$ 为四元数相乘时四元数矩阵表示形式：
Ω ( [ q w q v ] ) = q w 1 + [ 0 − q v T q v − { q v } × ] (17.5) \mathbf{\Omega}\left(\left[\begin{array}{l} q_{w} \\ \mathbf{q}_{v} \end{array}\right]\right)=q_{w} \mathbf{1}+\left[\begin{array}{cc} 0 & -\mathbf{q}_{v}^{T} \\ \mathbf{q}_{v} & -\left\{\mathbf{q}_{v}\right\}_{\times} \end{array}\right] \tag{17.5} Ω([qw​qv​​])=qw​1+[0qv​​−qvT​−{qv​}×​​](17.5)
$\mathbf{q}(\mathbf{\theta })$ 为旋转向量与四元数的变换形式：
$$\mathbf{q}(\mathbf{\theta })=\left[\begin{array}{c}\cos \frac{|\mathbf{\theta }|}{2}\\ \frac{\mathbf{\theta }}{|\mathbf{\theta }|}\sin \frac{|\mathbf{\theta }|}{2}\end{array}\right]\text{\hspace{1em}(17.6)}$$

由于我们的运动模型不是线性的。要在我们的 EKF 中使用它，我们需要针对预测状态的误差项进行线性化。为此，我们将定义一个误差状态向量，包括位置、速度、方向的误差项（注意这是一个九维的状态向量），数学表示为：
$$\delta {\mathbf{x}}_k=\left[\begin{array}{c}\delta {\mathbf{p}}_k\\ \delta {\mathbf{v}}_k\\ \delta {\mathbf{\varphi }}_k\end{array}\right]\in R^9\text{\hspace{1em}(17.7)}$$

其中 $\delta {\mathbf{\varphi }}_k$ 是一个三乘一的方向误差状态量。此外，我们可以得到误差状态量的状态转移方程：
$$\delta {\mathbf{x}}_k={\mathbf{F}}_{k-1}\delta {\mathbf{x}}_{k-1}+{\mathbf{L}}_{k-1}{\mathbf{n}}_{k-1}\text{\hspace{1em}(17.8)}$$
其中， ${\mathbf{F}}_{k-1}$ 为状态转移矩阵，数学表示为：
$${\mathbf{F}}_{k-1}=\left[\begin{array}{ccc}1& 1\Delta t& 0\\ 0& 1& -{\left[{\mathbf{C}}_{ns}{\mathbf{f}}_{k-1}\right]}_{\times }\Delta t\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(17.9)}$$
其中， $1$ 是一个 $3\times 3$ 的单位阵。IMU 测量噪声雅可比矩阵为${\mathbf{L}}_{k-1}$，测量噪声为 ${\mathbf{n}}_k$，数学表示为：

$$\begin{array}{rl}{\mathbf{L}}_{k-1}=\left[\begin{array}{ll}0& 0\\ 1& 0\\ 0& 1\end{array}\right]& {\mathbf{n}}_k\sim \mathcal{N}\left(0,{\mathbf{Q}}_k\right)\sim \mathcal{N}\left(0,\Delta t^2\left[\begin{array}{ll}{\sigma }_{\mathrm{a}\mathrm{c}\mathrm{c}}^2& \\ & {\sigma }_{\mathrm{g}\mathrm{y}\mathrm{r}\mathrm{o}}^2\end{array}\right]\right)\end{array}\text{\hspace{1em}(17.10)}$$

对于测量模型，我们将使用非常简单的位置观察加上一些高斯噪声。测量模型数学表示为：
$$\begin{array}{rl}{\mathbf{y}}_k& =\mathbf{h}\left({\mathbf{x}}_k\right)+{\mathbf{\nu }}_k\\ & ={\mathbf{H}}_k{\mathbf{x}}_k+{\mathbf{\nu }}_k=\left[\begin{array}{lll}1& 0& 0\end{array}\right]{\mathbf{x}}_k+{\mathbf{\nu }}_k\\ & ={\mathbf{p}}_k+{\mathbf{\nu }}_k\\ {\mathbf{\nu }}_k& \sim \mathcal{N}\left(0,\mathbf{R}\right)\end{array}\text{\hspace{1em}(17.11)}$$

其中，位置噪声可以为 ${\mathbf{R}}_{\mathrm{G}\mathrm{N}\mathrm{S}\mathrm{S}}$ 或 ${\mathbf{R}}_{\mathrm{L}\mathrm{i}\mathrm{D}\mathrm{A}\mathrm{R}}$。需要注意的是，我们这里假设我们的 LiDAR 和 GNSS 将在同一坐标系中提供位置测量。

---

2.3 EKF：IMU + GNSS + LIDAR

根据上一小节介绍的运动模型和测量模型，我们现在来实现一个完整的的扩展卡尔曼滤波器。

• Update state with IMU inputs。当 IMU 测量值到来时，滤波器根据运动模型进行状态预测，预测状态为：${\check{\mathbf{x}}}_k=\left[\begin{array}{c}{\check{\mathbf{p}}}_k\\ {\check{\mathbf{v}}}_k\\ {\check{\mathbf{q}}}_k\end{array}\right]$。这里的状态量可以为更新后或未更新的状态量。
  $$\begin{array}{rl}& {\check{\mathbf{p}}}_k={\mathbf{p}}_{k-1}+\Delta t{\mathbf{v}}_{k-1}+\frac{\Delta t^2}{2}\left({\mathbf{C}}_{ns}{\mathbf{f}}_{k-1}+{\mathbf{g}}_n\right)\\ & {\check{\mathbf{v}}}_k={\mathbf{v}}_{k-1}+\Delta t\left({\mathbf{C}}_{ns}{\mathbf{f}}_{k-1}+{\mathbf{g}}_n\right)\\ & {\check{\mathbf{q}}}_k=\mathbf{\Omega }\left(\mathbf{q}\left({\mathbf{\omega }}_{k-1}\Delta t\right)\right){\mathbf{q}}_{k-1}\end{array}\text{\hspace{1em}(17.12)}$$
• Propagate uncertainty。然后是状态不确定性（状态协方差矩阵）更新。同样地，状态协方差矩阵也可以是更新或未更新的协方差矩阵。此时，如果我们没有任何可用的 GNSS 或 LiDAR 测量，我们就一直重复步骤一和二。
  $${\check{\mathbf{P}}}_k={\mathbf{F}}_{k-1}{\mathbf{P}}_{k-1}{\mathbf{F}}_{k-1}^T+{\mathbf{L}}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{L}}_{k-1}^T\text{\hspace{1em}(17.13)}$$
• GNSS or LIDAR position available。如果有任何可用的 GNSS 或 LiDAR 测量，我们将首先计算卡尔曼增益。
  $${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\check{\mathbf{P}}}_k{\mathbf{H}}_k^T+\mathbf{R}\right)}^{-1}\text{\hspace{1em}(17.14)}$$
  然后我们计算一个误差状态，我们将使用它来纠正我们的预测状态。该误差状态为卡尔曼增益与预测位置和观察位置之间的差的乘积。
  $$\delta {\mathbf{x}}_k={\mathbf{K}}_k\left({\mathbf{y}}_k-{\check{\mathbf{p}}}_k\right)\text{\hspace{1em}(17.15)}$$
  然后，我们将使用我们的误差状态来纠正我们的预测状态。这种纠正对于位置和速度很简单，但是需要一些更复杂的代数来纠正四元数（这里可参考推荐阅读中的四元数运动学一文）。
  $$\begin{array}{rl}{\hat{\mathbf{p}}}_k& ={\check{\mathbf{p}}}_k+\delta {\mathbf{p}}_k\\ {\hat{\mathbf{v}}}_k& ={\check{\mathbf{v}}}_k+\delta {\mathbf{v}}_k\\ {\hat{\mathbf{q}}}_k& =\mathbf{\Omega }(\mathbf{q}(\delta \mathbf{\varphi })){\check{\mathbf{q}}}_k\end{array}\text{\hspace{1em}(17.16)}$$
  最后，是更新状态协方差矩阵。
  $${\hat{\mathbf{P}}}_k=\left(1-{\mathbf{K}}_k{\mathbf{H}}_k\right){\check{\mathbf{P}}}_k\text{\hspace{1em}(17.17)}$$

至此，就是一个完整的卡尔曼滤波过程。简单总结以下这一小节：

• 我们使用松耦合的扩展卡尔曼滤波器框架将来自 IMU 的惯性测量与来自 GNSS 接收器和 LiDAR 的位置测量融合在一起进行状态估计。
• 我们假设 GNSS 和 LiDAR 在同一坐标系中为我们提供了位置估计，在实际中通常需要一些预处理。
• 其次，我们没有考虑加速度计和陀螺仪偏差。这简化了我们的代数，但这不是一个现实的假设。在实际应用中，如何不考虑偏差，我们的定位误差会越来越大。
• 接下来，我们没有讨论状态初始化。这通常被认为是定位过程开始时的某个已知状态。
• 最后，我们还假设我们的传感器在空间和时间上都是对齐的。我们假设我们的传感器在某种意义上是经过标定的，即我们不用担心不同的时间步长，也不用担心我们如何将多组测量值全部对齐到一个坐标系中。我们将在下一小节介绍这部分内容。

---

3. Sensor Calibration - A Necessary Evil

3.1 Intrinsic Calibration

在上一小节我们已经了解了如何结合多个传感器数据来估计车辆状态，现在是时候讨论一个我们迄今为止一直未讨论的话题了。那就是传感器标定，这是工程师们不太喜欢谈论的事情之一，但它对于正确进行状态估计又是十分必要的。就我个人而言（这里指课程讲师），传感器标定对我来说很重要，因为我的博士研究课题就是 相机和 IMU 的标定，这也是我的学生今天继续研究的主题。在本小中，我们将讨论传感器标定的三种主要类型，以及为什么我们在为自动驾驶汽车设计状态估计器时需要考虑它们。要讨论的三种主要标定是：

• 内参标定，求解传感器自身参数（例如相机内参）；
• 外参标定，求解传感器相对于汽车坐标的安装位置和定向；
• 时间标定（也成为时间同步），处理不同传感器之间的测量时间差。

---

让我们先看看内参标定。在内参标定中，我们希望确定传感器模型的固定参数，以便我们可以在扩展卡尔曼滤波器等估计器中使用它们。每个传感器都有与之相关的参数，这些参数对于该特定传感器是唯一的，并且通常是恒定的。

例如，我们可能在汽车的车轮上安装了车轮编码器，用于测量车轮转速 $\omega$。如果我们想使用 $\omega$ 来估计车速 $v=\omega R$，我们需要知道车轮的半径 $R$。在这个例子中，$R$ 就是传感器模型参数，它对应编码器所连接的车轮，对不同的车轮有不同的 $R$。另一个例子是激光雷达传感器中扫描线的俯仰角度，着是一个固定的量，但我们需要提前知道它，以便我们能够正确解释每次扫描。

那么，我们如何确定这些内参呢？ 实际中可以通过以下三方面来确定内参：

• 最简单的就是从供应商那里得到。通常，传感器会在工厂进行标定，并附带一份规格表，会介绍与传感器相关的所有参数，例如激光雷达线数、不同扫描线间隔角度等；
• 但规格表并不总是足以进行真正准确的状态估计，因为没有两个传感器完全相同，并且参数的真实值会有一些变化。 另一个方法是手动测量这些参数。这对于轮胎半径测量来说是非常简单的，但对于像激光雷达这样的东西来说就不是那么简单了，因为在传感器内部用量角器测量是不切实际的；
• 另一种方法是将传感器内在参数作为车辆状态的一部分进行估计（如 IMU 加速度计和陀螺仪偏差），这种方法对特定传感器可以精确标定，并且也可以用来处理随时间缓慢变化的参数。例如，如果不断估计轮胎的半径，可以帮助检测何时会发生爆胎；

现在，因为我们在本课程中讨论的估计器是通用的，所以我们已经有了进行这种自动标定的工具。要了解它是如何工作的，让我们回到我们的汽车在一维运动的示例。 我们在后轮上安装了一个编码器来测量车轮的转速。如果我们想估计车轮半径以及位置和速度，我们需要做的就是将它添加到状态向量中，并计算出新的运动和测量模型。此时，状态向量如下，只是多了一个车轮半径 $R$。
$$\mathbf{x}=\left[\begin{array}{l}p\\ \dot{p}\\ R\end{array}\right]\text{\hspace{1em}(17.18)}$$

此外，我们可以得到如下关系：
$$\begin{gathered} \mathbf{u}=\ddot{p} \\ \dot{p}=v=\omega R\text{\hspace{1em}(17.19)} \end{gathered}$$
此时运动模型为：
$${\mathbf{x}}_k=\left[\begin{array}{ccc}1& \Delta t& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]{\mathbf{x}}_{k-1}+\left[\begin{array}{c}0\\ \Delta t\end{array}\right]{\mathbf{u}}_{k-1}+{\mathbf{w}}_{k-1},{\mathbf{w}}_k\sim \mathcal{N}(0,{\mathbf{Q}}_k)\text{\hspace{1em}(17.20)}$$

对于测量模型，我们仍然是直接通过 GPS 观察位置，但现在我们也通过编码器观察车轮转速。 因此，我们在模型中包含了额外的非线性观察。我们可以使用扩展或无迹卡尔曼滤波器来估计车轮半径以及车辆的位置和速度。 因此，内参标定对于使用单个传感器进行状态估计是必不可少的。
$${\mathbf{y}}_k=\left[\begin{array}{c}{p}_k\\ {\dot{p}}_k/R_k\end{array}\right]+{\mathbf{v}}_k,{\mathbf{v}}_k\sim \mathcal{N}(0,{\mathbf{R}}_k)\text{\hspace{1em}(17.21)}$$

---

3.2 Extrinsic Calibration

外参标定对于多传感器融合十分重要。 在外参标定中，我们感兴趣的是确定所有传感器通常相对于车辆的相对姿态。就像内参标定一样，外参标定也有不同的技术：

• 如果可以的话，可以访问像下图这样精确的车辆 CAD 模型，其中所有传感器坐标系都已经精心布置；
• 如果不行的话，可能需要尝试手东测量。 不幸的是，这通常很难或不可能准确地完成，因为许多传感器的坐标系原点位于传感器本身内部，我们可能不想拆卸汽车和所有传感器；
• 同样地，我们也可以将外参包含在我们的状态中来估计它们。对于任意传感器配置，这可能会变得有点复杂，目前有很多研究在使用不同的技术来可靠地做到这一点；

---

3.3 Temporal Calibration

最后，一种实践中经常被忽视但仍然很重要的标定是时间标定。在我们所有关于多传感器融合的讨论中，我们一直在隐含地假设我们的所有测量值都是在同一时刻捕捉到的，或者至少足够接近给定的精度水平。但是我们如何确定两个测量是否足够接近以被认为是同步的呢？

一种方法是在车载计算机接收到每个测量值时为其添加时间戳，并匹配彼此最接近的测量值。例如，如果我们获得 15 Hz 的 LiDAR 扫描和 200 Hz的 IMU 读数，我们可能希望将每个 LiDAR 扫描与时间戳最接近的 IMU 读数配对。但实际上，在 LiDAR 或 IMU 实际记录观察结果与到达计算机之间存在未知延迟。这些延迟可能是由传感器数据传输到主机所需的时间或由传感器电路执行的预处理步骤引起的，并且不同传感器的延迟可能不同。因此，如果我们想要获得真正准确的状态估计，我们需要考虑我们的传感器实际同步的程度，并且有不同的方法可以解决这个问题：

• 最简单和最常见的做法就是假设延迟为零。仍然可以通过这种方式获得有效的估计器，但结果可能不如通过更好的时间同步获得的结果准确；
• 另一种常见的做法是使用硬件同步传感器，但这通常仅适用于更昂贵的传感器设置；
• 正如我们刚刚介绍的那样，也可以尝试将这些时间延迟作为车辆状态的一部分进行估算，但这可能会变得复杂。事实上，我（这里指课程讲师）博士论文的一整章都致力于解决相机和 IMU 的时间同步问题。

简单总结下这一小节：

• 如果没有标定，传感器融合是不可能的。在本小节中，我们了解了内参标定，它标定我们传感器模型的参数。
• 外参标定，它为我们提供了将传感器测量值转换为公共参考坐标系架所需的坐标转换。
• 时间标定，处理同步测量以确保它们都对应于相同的车辆状态。
• 虽然有一些标定技术可以解决所有这些问题，但标定仍然是一个非常活跃的研究领域。

---

4. Loss of One or More Sensors

4.1 Overview

在自动驾驶汽车等应用中，传感器可能会因多种不同原因而发生故障或性能下降，例如天气恶化、固件出现故障或电线出现松动。我们在本模块中已经看到，即使我们所有的传感器都正常工作，拥有多个互补传感器对提供稳健准确的定位估计仍然是有益的。 但是，如果其中一个传感器发生故障会发生什么？

到目前为止，我们已经讨论了 GNSS、IMU 和 LiDAR，但大多数现代自动驾驶汽车还包括车轮编码器、毫米波雷达、超声波雷达和多个摄像头等传感器（如下图所示）。

为了制造一辆安全的自动驾驶汽车，了解当一个或多个传感器发生故障时会发生什么，以及了解维持汽车安全运行所需的最小感知是很有必要的。在本小节中，我们将讨论传感器冗余对于稳定定位的重要性，并探讨定位中传感器失败的几个示例。 这类问题分析的一个重要考虑因素是传感器测量范围和操作限制。

GNSS 接收器无法在桥下工作，并且在高层建筑之间测量精度会下降。 IMU 可能对温度比较敏感，有时需要定期重新校准。更重要的是，对于激光雷达、超声波雷达、摄像头或毫米波雷达等观察外部环境的传感器来说，其测量范围对汽车安全运行起着非常重要的作用。大多数汽车都有长距离、中距离和短距离感知传感器。如果其中一个传感器出现故障，就要立即限制汽车的运动。例如，在定位中，我们可能会在进行停车时使用到短距离传感器（例如超声波雷达），这样能确保我们不会与附近的车辆发生碰撞。中距离传感器可以帮助检测行人和骑自行车的人以及跟随车道。远距离传感器可以帮助我们检测和预测远处障碍物的运动。如果其中一个出现故障，我们必须采取适当的行动，以确保车内乘员或我们周围人的安全不受影响。这意味着自动驾驶汽车工程师必须考虑执行每个步骤所需的最小允许传感设备。

---

4.2 Redundancy is Crucial

对于这种冗余系统设计，我们可以查看以严格的安全标准而闻名的商业航空行业的示例。 作为冗余系统的一个例子，波音777 以三重冗余原则运行。 所有主要系统，包括这台飞行计算机，都有两个备份，每个都有独立的电源。在这里，我们可以看到每台飞行计算机都有不同的处理器，以确保架构本身的错误不会影响所有三台计算机。 如果其中一台计算机出现故障，777 会使用共识算法无缝切换到另一台计算机。

尽管自动驾驶技术已经取得了长足的进步，但我们距离最初的一些自动驾驶挑战（例如 2007 年 DARPA 城市挑战赛）只有 10 年的时间。在这里，我们可以看到缺乏安全冗余是如何导致 MIT 和 Cornell 团队相互碰撞的。如果这次碰撞以更高的速度发生，结果可能会更糟。

还有一个例子，特斯拉 Model S 几乎直接驶入加利福尼亚高速公路上的的例子。这是一个发人深省的提醒，对于设计自动驾驶汽车的工程师来说，仔细考虑所使用的不同传感器的不同故障模式至关重要，以确保一个故障组件不会造成悲剧。

总而言之，考虑任何一组传感器的局限性并使用多个互补传感器进行鲁棒性定位是很重要的。

---

推荐阅读

• 《动手学无人驾驶（6）：基于IMU和GPS数据融合的自车定位》
• 重读经典《Quaternion kinematics for the error-state Kalman filter》