论文阅读 Calib: Inertial Aided Multi-Sensor Calibration

摘要

• 大多数自主机器人上可用的异构传感器阵列广泛用于状态估计和环境感知，需要对内参、外参、时间进行进行精确校准。
• 在本文中，我们提出了一种基于整体非线性最小二乘法 (NLS) 的多传感器校准系统，该系统利用高速惯导并可以处理机器人上常见的多种异步传感器（例如 IMU、相机、激光雷达） ，和车轮里程计），而只需要初始校准猜测。
• 我们进行了广泛的模拟，以验证系统在理想运动情况下的准确性、一致性和性能。特别是，我们对平面运动如何导致标定退化以及它如何直接影响我们标定参数的能力进行了深入研究，这对无人地面车辆（UGV）具有实际意义。
• 为了证明在这些退化场景中执行校准的实际挑战，我们在一系列真实世界的数据集上进一步评估了所提出的系统，并与现有的最先进的校准工具箱进行了比较。

前言

为何需要校准：

• 在自主导航系统中，惯性测量单元 (IMU)、多个摄像头、距离传感器（例如 LiDAR）和车轮编码器被广泛用于估计机器人的 6 自由度 (DoF) 姿态（位置和方向） 通过感知环境并感知其自身的运动。 例如，IMU 和车轮编码器可以直接测量机器人的运动（例如，角速度和线速度以及线加速度），如果与相机 [1]、[2] 或 LiDAR [3]、[ 4]。 此外，来自相机的纹理丰富的图像和来自 LiDAR 的精确 3D 点可以联合用于障碍物检测和环境重建 [5]。 为了正确融合来自这些异构多模态传感器的测量值以完成上述任务，必须准确校准这些传感器的空间和时间关系。

外参+时间校准

• 外参校准问题指：传感器对（例如，IMU-camera、camera-LiDAR 和 camerawheel）之间的 6 DoF 刚性变换，而时间校准是找到传感器测量时钟帧之间的时间偏移。 由于异步硬件触发或原始数据传输时间而可能发生的引用。 两者都需要准确融合多传感器数据为下游处理流程（例如导航、感知）。

现状 - 都没有考虑传感器之间的时间偏移。

• Kalibr [6]一种基于连续时间批量估计的多相机、多 IMU 和 IMU 相机校准系统。- [9] 采用基于B-Spline 的连续时间轨迹公式，将 IMU 和 LiDAR 读数与外部校准融合，点到平面的对应关系被用来提高校准精度，因此不需要特定的目标。- 相机-LiDAR 校准也引起了人们的极大兴趣。例如，欧文斯等人。 [8] 提出了 MSG-CAL，这是一种用于估计多个相机和距离传感器之间刚性转换的通用框架。从传感器数据中提取的几何特征（点、线或平面）用于联合优化成本函数，从而在传感器之间产生外部校准。盖格等人。 [11] 提出了一种单发相机-LiDAR 外在校准。然而，在实践中需要许多棋盘，并且应该小心放置以确保优化问题对于所有自由度都是适定的。同样，周等人。 [12] 通过使用带有一个校准板的线到平面对应关系，提出了一种相机-LiDAR 外部校准。

总结：

• 虽然上述都是 基于非线性最小二乘的优化用于离线标定，在基于贝叶斯滤波的在线校准方面存在着重要的工作和识别可能导致传感参数变得不可观测的退化运动。注意，所有这些方法都需要高保真初始猜测进行校准，而这一工作假设只有一些初步猜测可用，并寻求通过批量NLS找到全局最优解决方案。
• 具体来说，在这项工作中，我们在 NLS 估计框架内开发了一种通用的统一多传感器校准算法，以找到相机内部参数、相机 IMU、LiDARIMU 和车轮 IMU 异步传感器的时空参数的最佳校准：
  • 我们为异步 IMU、相机、LiDAR 和车轮里程计传感器开发了一个利用高速惯性运动（称为 iCalib）的整体多传感器校准系统，在一般运动情况下，该系统能够准确地标定所有传感器之间的时空参数。
  • 我们基于 IMU 运动学或边界姿态开发了两种不同的插值方案来融合异构测量，从而实现通用的时空校准。
  • 我们执行广泛的蒙特卡洛模拟来验证所提出的系统，并在模拟和现实世界的实验中演示校准退化问题。 通过与当前最先进的工具箱 Kalibr [6] 和 MSG-CAL [8] 进行比较，显示了所提出系统的竞争性能。

问题表述

A 状态向量

• 我们提出的惯性辅助校准 (iCalib) 系统的状态向量包括：惯性导航状态 $X_I$、一组环境特征 $X_F$、一组传感器的时空和内在校准 参数$X_{calib}$ 。
  $$X=[x_{I_0}^T...x_{I_k}^T{x}_F^T{x}_{calib}^T{]}^T(1)$$
  $x_{I_k}={[}_{I_k}^G{\bar{q}}^T{}^G{p}_{I_k}^T{}^G{v}_{I_k}^T{b}_{w_k}^T{b}_{a_k}^T{]}^T(2)$
  $x_F={[}^G{p}_{f_1}^T..{.}^G{p}_{f_m}^T{}^G{p}_{{\pi }_1}^T..{.}^G{p}_{{\pi }_m}^T{]}^T(3)$
  $x_{calib}=[x_{C_{I_1}}^T...x_{C_{I_s}}^T{x}_{L_I}^T{x}_{W_I}^T{]}^T(4)$
  $x_{C_I}={[}_C^I{\bar{q}}^T{}^I{p}_C^T{t}_{C_I}{\varsigma }^T{]}^T(5)$
  $x_{L_I}={[}_L^I{\bar{q}}^T{}^I{p}_L^T{t}_{L_I}{]}^T(6)$
  $x_{W_I}={[}_W^I{\bar{q}}^T{}^I{p}_W^T{t}_{W_I}{]}^T(7)$
• 公式2中，这儿，${}_{I_k}^G{\bar{q}}^T$ 是 IMU 在时间$t_k$从 局部坐标系 { I k } {\{I_k\}} {Ik​} 到全局坐标系 { G } {\{G\}} {G}的单位四元素旋转 $R{(}_{I_k}^G\bar{q}){=}_{I_k}^{G}R$，$b_{w_k}、b_{a_k}$是陀螺仪和加速度偏差，${}^G{p}_{I_k}{、}^G{v}_{I_k}$ 是IMU的速度和位置。
• 公式3中，$p_f和p_{\pi }$ 分布是点和平面特征。$x_F$包含了$m$个3D点特征的全局位姿$p_f$和$l$个平面特征的全局最近点位姿$p_{\pi }$。
• 公式4中，标定向量$x_{calib}$包括$s$个相机-IMU的标定值$x_{C_I}$，激光-IMU的时空(时间空间)的标定值$x_{L_I}$ 和 里程计-IMU的时空标定值$x_{W_I}$。
• $x_{C_I}$由相机内在参数(焦距，相机中心和失真参数)和相机到IMU的时空参数组成，注：此系统很容易扩展到多个激光，时间偏移$t_{S_I}$为$t_I=t_s+t_{S_I}$。
• 状态变量$x_{I_K}$ 可以由任意频率担任，但实际上我们将频率最高的相机指定为系统的“base”，所有测量将作为每个状态估计的真实IMU时间(包括系统基准)的函数写入，因此需要处理每个传感器测量值时间与该状态之间的时间偏移。

B 基于运动学的插值

例如：传感器$S$在时间$t_{S,in}$的测量，$t_{SI}$是传感器差值，需要获得 { S i n G R 、 G p S i n } {\{^G_{S_{in}}R、^Gp_{S_in}\}} {Sin​G​R、GpSi​n​}，其中$\delta t=t_{S,in}+t_{SI}-t_{I_k}$。
$${{}_{S_{in}}^{G}R{=}_{I_k}^{G}RExp{(}^{I_k}w{\delta }_t)}_S^{I}R(8)$$
$${}^G{p}_{S_{i}n}{=}^G{p}_{I_k}{+}^G{v}_{I_k}\delta t{+}_{S_{in}}^G{R}^I{p}_S(9)$$

• 差值(时间和参数空间参数的导数)：基于IMU差值，但考虑了IMU的时间偏差。
• 这种差值只是在我们差值的姿态附近时间找了一个近似值，只适用于发生在惰性附近的姿态测量。

C 基于位姿的差值

另一种更为传统的姿态插值方案是在两个边界惯性状态之间找到一个姿态。我们将其用于激光雷达和非基础相机，它们更可能具有大量插值。要找到两个IMU边界状态$x_{I_k}$和$x_{I_{k+1}}$的传感器姿态:
$${{}_{S_{in}}^{G}R{=}_{I_k}^{G}RExp(\lambda Log{(}_{I_{k+1}}^{I_k}R))}_S^{I}R(10)$$
$${}^G{p}_{S_{i}n}{=}^G{p}_{I_k}{+}_{I_k}^{G}R({\lambda }^{I_k}{p}_{I_{k+1}}){+}_{S_{in}}^G{R}^I{p}_S(11)$$
$$\lambda =(t_{S,in}+t_{SI}-t_{I_k})/(t_{I_{k+1}-t_{I_k}})(12)$$
这儿，$t_{S,in}$和$t_{SI}$是测量时间和传感器差值。

D 非线性最小二乘优化

给测量值加高斯白噪声。
$$z_S=h_S(x)+n_S,n_S\sim N(0,R_S))(13)$$
我们可将其公式转换为：
$$\underset{x}{\min }\sum {\Vert z_S-h_S(x))\Vert }_{R_S^{-1}}^2(14)$$

• 优化开始前需提供初始值 ${\hat{x}}^{\ominus }$，求解状态增量$\delta x$，可以定义状态估计${\hat{x}}^{\oplus }={\hat{x}}^{\ominus }■\tilde{x}$，其中$■$代表状态更新
  $$\underset{x}{\min }\sum {\mathbb{C}}_I+\sum {\mathbb{C}}_C+\sum {\mathbb{C}}_L+\sum {\mathbb{C}}_W(15)$$

惯性辅助多传感器校准

A IMU 测量

跟IMU一样

B 视觉点测量

C 激光平面点测量

D 轮速里程计测量

我们还可以考虑配备差速驱动系统的地面车辆，其中我们可以将左车轮和右轮速度读数从TK到TK + 1集成，并获得2D相对位姿测量（YAW和X，Y在车轮框架中的平移）。

实验

为了验证提议的 iCalib，我们利用我们之前开发的模拟器 [1]、[2]、[25] 进行了广泛的蒙特卡罗模拟，以模拟异步 IMU、3D LiDAR、2D 车轮里程计和相机。 表 II 详细说明了用于在具有平面和点特征的结构房间环境中生成相对平滑的 3D 轨迹的精确模拟参数。