做餐厅logo用什么软件网站,网站手机端自适应,wordpress文件系统插件,优化步骤在第16讲《Coursera自动驾驶课程第16讲#xff1a;LIDAR Sensing》我们学习了自动驾驶目前常用的3D 传感器#xff0c;激光雷达#xff0c;了解了激光雷达的工作原理#xff0c;掌握了对点云数据的操作以及如何使用点云配准方法来进行汽车定位。
回顾一下#xff0c;在本…在第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 Practice1.1 Overview1.2 Accuracy Speed Requirements1.3 Localization Failures Localization Failures2. Multisensor Fusion for State Estimation2.1 Overview2.2 Motion Model Measurement Model2.3 EKFIMU GNSS LIDAR3. Sensor Calibration - A Necessary Evil3.1 Intrinsic Calibration3.2 Extrinsic Calibration3.3 Temporal Calibration4. Loss of One or More Sensors4.1 Overview4.2 Redundancy is Crucial推荐阅读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 测量进行融合更新同时来纠正我们的预测状态卡尔曼系统中的更新模块。 那么我们的状态是什么呢为了实现状态估计我们将使用一个十维的状态向量 xk\mathbf{x}_{k}xk 包括3D 位置 pk\mathbf{p}_{k}pk、3D 速度 vk\mathbf{v}_{k}vk、 单位四元数 qk\mathbf{q}_{k}qk这些四元数将表示我们的车辆相对于导航坐标系的方向数学表示为 xk[pkvkqk]∈R10(17.1)\mathbf{x}_{k}\left[\begin{array}{l} \mathbf{p}_{k} \\ \mathbf{v}_{k} \\ \mathbf{q}_{k} \end{array}\right] \in R^{10} \tag{17.1} xk⎣⎡pkvkqk⎦⎤∈R10(17.1)
我们假设 IMU 在传感器坐标系中输出加速度和旋转角速度并将它们组合成单个输入向量 uk\mathbf{u}_{k}uk。同样重要的是要指出我们这里没有讨论加速度计或陀螺仪的偏差。这些通常被放入状态向量中进行估计然后从我们的 IMU 测量中减去。为方便讨论这里假设 IMU 测量是无偏的输入量数学表示为 uk[fkωk]∈R6(17.2)\mathbf{u}_{k}\left[\begin{array}{c} \mathbf{f}_{k} \\ \omega_{k} \end{array}\right] \in R^{6} \tag{17.2} uk[fkωk]∈R6(17.2)
下面来介绍运动模型运动模型输入为 IMU 的加速度和旋转速率。运动模型数学公式如下 pkpk−1Δtvk−1Δt22(Cnsfk−1g)vkvk−1Δt(Cnsfk−1−g)qkΩ(q(ωk−1Δt))qk−1(17.3)\begin{aligned} \mathbf{p}_{k}\mathbf{p}_{k-1}\Delta t \mathbf{v}_{k-1}\frac{\Delta t^{2}}{2}\left(\mathbf{C}_{n s} \mathbf{f}_{k-1}\mathbf{g}\right) \\ \mathbf{v}_{k}\mathbf{v}_{k-1}\Delta t\left(\mathbf{C}_{n s} \mathbf{f}_{k-1}-\mathbf{g}\right) \\ \mathbf{q}_{k}\boldsymbol{\Omega}\left(\mathbf{q}\left(\boldsymbol{\omega}_{k-1} \Delta t\right)\right) \mathbf{q}_{k-1} \end{aligned} \tag{17.3} pkpk−1Δtvk−12Δt2(Cnsfk−1g)vkvk−1Δt(Cnsfk−1−g)qkΩ(q(ωk−1Δt))qk−1(17.3)
其中Cns\mathbf{C}_{n s}Cns 为从传感器坐标 sss 到导航坐标 nnn 的旋转矩阵。 CnsCns(qk−1)(17.4)\mathbf{C}_{n s}\mathbf{C}_{n s}\left(\mathbf{q}_{k-1}\right) \tag{17.4} CnsCns(qk−1)(17.4) Ω([qwqv])\mathbf{\Omega}\left(\left[\begin{array}{l} q_{w} \\ \mathbf{q}_{v} \end{array}\right]\right)Ω([qwqv]) 为四元数相乘时四元数矩阵表示形式 Ω([qwqv])qw1[0−qvTqv−{qv}×](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} Ω([qwqv])qw1[0qv−qvT−{qv}×](17.5) q(θ)\mathbf{q}(\boldsymbol{\theta})q(θ) 为旋转向量与四元数的变换形式 q(θ)[cos∣θ∣2θ∣θ∣sin∣θ∣2](17.6)\mathbf{q}(\boldsymbol{\theta})\left[\begin{array}{c} \cos \frac{|\boldsymbol{\theta}|}{2} \\ \frac{\boldsymbol{\theta}}{|\boldsymbol{\theta}|} \sin \frac{|\boldsymbol{\theta}|}{2} \end{array}\right] \tag{17.6} q(θ)[cos2∣θ∣∣θ∣θsin2∣θ∣](17.6)
由于我们的运动模型不是线性的。要在我们的 EKF 中使用它我们需要针对预测状态的误差项进行线性化。为此我们将定义一个误差状态向量包括位置、速度、方向的误差项注意这是一个九维的状态向量数学表示为 δxk[δpkδvkδϕk]∈R9(17.7)\delta \mathbf{x}_{k}\left[\begin{array}{c} \delta \mathbf{p}_{k} \\ \delta \mathbf{v}_{k} \\ \delta \boldsymbol{\phi}_{k} \end{array}\right] \in R^{9} \tag{17.7} δxk⎣⎡δpkδvkδϕk⎦⎤∈R9(17.7)
其中 δϕk\delta \boldsymbol{\phi}_{k}δϕk 是一个三乘一的方向误差状态量。此外我们可以得到误差状态量的状态转移方程 δxkFk−1δxk−1Lk−1nk−1(17.8)\delta \mathbf{x}_{k}\mathbf{F}_{k-1} \delta \mathbf{x}_{k-1}\mathbf{L}_{k-1} \mathbf{n}_{k-1} \tag{17.8} δxkFk−1δxk−1Lk−1nk−1(17.8) 其中 Fk−1\mathbf{F}_{k-1}Fk−1 为状态转移矩阵数学表示为 Fk−1[11Δt001−[Cnsfk−1]×Δt001](17.9)\mathbf{F}_{k-1}\left[\begin{array}{ccc} \mathbf{1} \mathbf{1} \Delta t 0 \\ 0 \mathbf{1} -\left[\mathbf{C}_{n s} \mathbf{f}_{k-1}\right]_{\times} \Delta t \\ 0 0 \mathbf{1} \end{array}\right] \tag{17.9} Fk−1⎣⎡1001Δt100−[Cnsfk−1]×Δt1⎦⎤(17.9) 其中 1\mathbf{1}1 是一个 3×33\times33×3 的单位阵。IMU 测量噪声雅可比矩阵为Lk−1\mathbf{L}_{k-1}Lk−1测量噪声为 nk\mathbf{n}_{k}nk数学表示为
Lk−1[001001]nk∼N(0,Qk)∼N(0,Δt2[σacc2σgyro2])(17.10)\begin{aligned} \mathbf{L}_{k-1}\left[\begin{array}{ll} 0 0 \\ \mathbf{1} 0 \\ 0 \mathbf{1} \end{array}\right] \quad \quad \mathbf{n}_{k} \sim \mathcal{N}\left(\mathbf{0}, \mathbf{Q}_{k}\right) \sim \mathcal{N}\left(\mathbf{0}, \Delta t^{2}\left[\begin{array}{ll} \sigma_{\mathrm{acc}}^{2} \\ \sigma_{\mathrm{gyro}}^{2} \end{array}\right]\right) \end{aligned} \tag{17.10} Lk−1⎣⎡010001⎦⎤nk∼N(0,Qk)∼N(0,Δt2[σacc2σgyro2])(17.10)
对于测量模型我们将使用非常简单的位置观察加上一些高斯噪声。测量模型数学表示为 ykh(xk)νkHkxkνk[100]xkνkpkνkνk∼N(0,R)(17.11)\begin{aligned} \mathbf{y}_{k} \mathbf{h}\left(\mathbf{x}_{k}\right)\boldsymbol{\nu}_{k} \\ \mathbf{H}_{k} \mathbf{x}_{k}\boldsymbol{\nu}_{k}\left[\begin{array}{lll} \mathbf{1} \mathbf{0} \mathbf{0} \end{array}\right] \mathbf{x}_{k}\boldsymbol{\nu}_{k} \\ \mathbf{p}_{k}\boldsymbol{\nu}_{k} \\ \boldsymbol{\nu}_{k} \sim \mathcal{N}\left(\mathbf{0}, \mathbf{R}\right) \end{aligned} \tag{17.11} ykνkh(xk)νkHkxkνk[100]xkνkpkνk∼N(0,R)(17.11)
其中位置噪声可以为 RGNSS\ \mathbf{R}_{\mathrm{GNSS}} RGNSS 或 RLiDAR\ \mathbf{R}_{\mathrm{LiDAR}} RLiDAR。需要注意的是我们这里假设我们的 LiDAR 和 GNSS 将在同一坐标系中提供位置测量。 2.3 EKFIMU GNSS LIDAR
根据上一小节介绍的运动模型和测量模型我们现在来实现一个完整的的扩展卡尔曼滤波器。
Update state with IMU inputs。当 IMU 测量值到来时滤波器根据运动模型进行状态预测预测状态为xˇk[pˇkvˇkqˇk]\check{\mathbf{x}}_{k}\left[\begin{array}{c}\check{\mathbf{p}}_{k} \\ \check{\mathbf{v}}_{k} \\ \check{\mathbf{q}}_{k}\end{array}\right]xˇk⎣⎡pˇkvˇkqˇk⎦⎤。这里的状态量可以为更新后或未更新的状态量。 pˇkpk−1Δtvk−1Δt22(Cnsfk−1gn)vˇkvk−1Δt(Cnsfk−1gn)qˇkΩ(q(ωk−1Δt))qk−1(17.12)\begin{aligned} \check{\mathbf{p}}_{k}\mathbf{p}_{k-1}\Delta t \mathbf{v}_{k-1}\frac{\Delta t^{2}}{2}\left(\mathbf{C}_{n s} \mathbf{f}_{k-1}\mathbf{g}_{n}\right) \\ \check{\mathbf{v}}_{k}\mathbf{v}_{k-1}\Delta t\left(\mathbf{C}_{n s} \mathbf{f}_{k-1}\mathbf{g}_{n}\right) \\ \check{\mathbf{q}}_{k}\boldsymbol{\Omega}\left(\mathbf{q}\left(\boldsymbol{\omega}_{k-1} \Delta t\right)\right) \mathbf{q}_{k-1} \end{aligned} \tag{17.12} pˇkpk−1Δtvk−12Δt2(Cnsfk−1gn)vˇkvk−1Δt(Cnsfk−1gn)qˇkΩ(q(ωk−1Δt))qk−1(17.12)Propagate uncertainty。然后是状态不确定性状态协方差矩阵更新。同样地状态协方差矩阵也可以是更新或未更新的协方差矩阵。此时如果我们没有任何可用的 GNSS 或 LiDAR 测量我们就一直重复步骤一和二。 PˇkFk−1Pk−1Fk−1TLk−1Qk−1Lk−1T(17.13)\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} \tag{17.13} PˇkFk−1Pk−1Fk−1TLk−1Qk−1Lk−1T(17.13)GNSS or LIDAR position available。如果有任何可用的 GNSS 或 LiDAR 测量我们将首先计算卡尔曼增益。 KkPˇkHkT(HkPˇkHkTR)−1(17.14)\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} \tag{17.14} KkPˇkHkT(HkPˇkHkTR)−1(17.14) 然后我们计算一个误差状态我们将使用它来纠正我们的预测状态。该误差状态为卡尔曼增益与预测位置和观察位置之间的差的乘积。 δxkKk(yk−pˇk)(17.15)\delta \mathbf{x}_{k}\mathbf{K}_{k}\left(\mathbf{y}_{k}-\check{\mathbf{p}}_{k}\right) \tag{17.15} δxkKk(yk−pˇk)(17.15) 然后我们将使用我们的误差状态来纠正我们的预测状态。这种纠正对于位置和速度很简单但是需要一些更复杂的代数来纠正四元数这里可参考推荐阅读中的四元数运动学一文。 p^kpˇkδpkv^kvˇkδvkq^kΩ(q(δϕ))qˇk(17.16)\begin{aligned} \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} \boldsymbol{\Omega}(\mathbf{q}(\delta \boldsymbol{\phi})) \check{\mathbf{q}}_{k} \end{aligned} \tag{17.16} p^kv^kq^kpˇkδpkvˇkδvkΩ(q(δϕ))qˇk(17.16) 最后是更新状态协方差矩阵。 P^k(1−KkHk)Pˇk(17.17)\hat{\mathbf{P}}_{k}\left(\mathbf{1}-\mathbf{K}_{k} \mathbf{H}_{k}\right) \check{\mathbf{P}}_{k} \tag{17.17} P^k(1−KkHk)Pˇk(17.17)
至此就是一个完整的卡尔曼滤波过程。简单总结以下这一小节
我们使用松耦合的扩展卡尔曼滤波器框架将来自 IMU 的惯性测量与来自 GNSS 接收器和 LiDAR 的位置测量融合在一起进行状态估计。我们假设 GNSS 和 LiDAR 在同一坐标系中为我们提供了位置估计在实际中通常需要一些预处理。其次我们没有考虑加速度计和陀螺仪偏差。这简化了我们的代数但这不是一个现实的假设。在实际应用中如何不考虑偏差我们的定位误差会越来越大。接下来我们没有讨论状态初始化。这通常被认为是定位过程开始时的某个已知状态。最后我们还假设我们的传感器在空间和时间上都是对齐的。我们假设我们的传感器在某种意义上是经过标定的即我们不用担心不同的时间步长也不用担心我们如何将多组测量值全部对齐到一个坐标系中。我们将在下一小节介绍这部分内容。 3. Sensor Calibration - A Necessary Evil
3.1 Intrinsic Calibration
在上一小节我们已经了解了如何结合多个传感器数据来估计车辆状态现在是时候讨论一个我们迄今为止一直未讨论的话题了。那就是传感器标定这是工程师们不太喜欢谈论的事情之一但它对于正确进行状态估计又是十分必要的。就我个人而言这里指课程讲师传感器标定对我来说很重要因为我的博士研究课题就是 相机和 IMU 的标定这也是我的学生今天继续研究的主题。在本小中我们将讨论传感器标定的三种主要类型以及为什么我们在为自动驾驶汽车设计状态估计器时需要考虑它们。要讨论的三种主要标定是
内参标定求解传感器自身参数例如相机内参外参标定求解传感器相对于汽车坐标的安装位置和定向时间标定也成为时间同步处理不同传感器之间的测量时间差。 让我们先看看内参标定。在内参标定中我们希望确定传感器模型的固定参数以便我们可以在扩展卡尔曼滤波器等估计器中使用它们。每个传感器都有与之相关的参数这些参数对于该特定传感器是唯一的并且通常是恒定的。
例如我们可能在汽车的车轮上安装了车轮编码器用于测量车轮转速 ω\omegaω。如果我们想使用 ω\omegaω 来估计车速 vωRv\omega RvωR我们需要知道车轮的半径 RRR。在这个例子中RRR 就是传感器模型参数它对应编码器所连接的车轮对不同的车轮有不同的 RRR。另一个例子是激光雷达传感器中扫描线的俯仰角度着是一个固定的量但我们需要提前知道它以便我们能够正确解释每次扫描。 那么我们如何确定这些内参呢 实际中可以通过以下三方面来确定内参
最简单的就是从供应商那里得到。通常传感器会在工厂进行标定并附带一份规格表会介绍与传感器相关的所有参数例如激光雷达线数、不同扫描线间隔角度等但规格表并不总是足以进行真正准确的状态估计因为没有两个传感器完全相同并且参数的真实值会有一些变化。 另一个方法是手动测量这些参数。这对于轮胎半径测量来说是非常简单的但对于像激光雷达这样的东西来说就不是那么简单了因为在传感器内部用量角器测量是不切实际的另一种方法是将传感器内在参数作为车辆状态的一部分进行估计如 IMU 加速度计和陀螺仪偏差这种方法对特定传感器可以精确标定并且也可以用来处理随时间缓慢变化的参数。例如如果不断估计轮胎的半径可以帮助检测何时会发生爆胎
现在因为我们在本课程中讨论的估计器是通用的所以我们已经有了进行这种自动标定的工具。要了解它是如何工作的让我们回到我们的汽车在一维运动的示例。 我们在后轮上安装了一个编码器来测量车轮的转速。如果我们想估计车轮半径以及位置和速度我们需要做的就是将它添加到状态向量中并计算出新的运动和测量模型。此时状态向量如下只是多了一个车轮半径 RRR。 x[pp˙R](17.18)\mathbf{x}\left[\begin{array}{l} p \\ \dot{p} \\ R \end{array}\right] \tag{17.18} x⎣⎡pp˙R⎦⎤(17.18)
此外我们可以得到如下关系 up¨p˙vωR(17.19)\mathbf{u} \ddot{p} \\ \dot{p}v\omega R \tag{17.19} up¨p˙vωR(17.19) 此时运动模型为 xk[1Δt0010001]xk−1[0Δt]uk−1wk−1,wk∼N(0,Qk)(17.20)\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}{ccc} 0 \\ \Delta t \end{array}\right]\mathbf{u}_{k-1}\mathbf{w}_{k-1}, \mathbf{w}_{k} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_{k}) \tag{17.20} xk⎣⎡100Δt10001⎦⎤xk−1[0Δt]uk−1wk−1,wk∼N(0,Qk)(17.20)
对于测量模型我们仍然是直接通过 GPS 观察位置但现在我们也通过编码器观察车轮转速。 因此我们在模型中包含了额外的非线性观察。我们可以使用扩展或无迹卡尔曼滤波器来估计车轮半径以及车辆的位置和速度。 因此内参标定对于使用单个传感器进行状态估计是必不可少的。 yk[pkp˙k/Rk]vk,vk∼N(0,Rk)(17.21)\mathbf{y}_{k}\left[\begin{array}{ccc} p_{k} \\ \dot{p}_k/R_{k} \end{array}\right]\mathbf{v}_{k}, \mathbf{v}_{k} \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_{k}) \tag{17.21} yk[pkp˙k/Rk]vk,vk∼N(0,Rk)(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》
