高鲁棒性Camera-IMU外参在线标定算法

针对影像实时动态测量设备中相机和惯性测量单元（IMU）相对位置发生变化时，需要重新进行外参标定的问题，提出了一种Camera-IMU外参在线标定方法，能够在机械构型未知的情况下自动估计初始值和外参数。首先，使用全球卫星导航时间对齐IMU和相机的时间戳，通过奇异值分解求解旋转量的超定线性方程，改变阈值判定条件和加权方式，减少方程中的退化运动，并剔除外点，提高系统鲁棒性与外参精度，从而获得恒定的Camera-IMU旋转外参。然后基于获得的Camera-IMU旋转外参，固定滑动窗口，利用高斯牛顿法估计Camera-IMU的外参平移量。与原有在线标定方法相比，旋转外参标定方法的精度提高15%，平移外参的精度提高35%，实验结果表明所提方法是有效的。

Abstract

In this study, we aim to solve the recalibration of the external parameters owing to the changing relative positions of the camera and inertial measurement unit (IMU) in real-time dynamic imaging equipment. Thus, we propose a highly robust Camera-IMU external parameter online calibration method, which automatically estimates the initial value and external parameters when the mechanical configuration is unknown. The global satellite navigation time is used to align the timestamps of the IMU and camera. Then, the singular value decomposition method is used to solve the overdetermined linear equation of rotation. The threshold determination condition and weighting method are modified to reduce degenerate motion in the equation and eliminate the external points, improve system robustness and external parameter accuracy, and obtain constant Camera-IMU rotation external parameters. Then, based on the obtained Camera-IMU rotation external parameters, the sliding window is fixed and the Gaussian-Newton method is used to estimate the Camera-IMU external parameter translation. Compared with the original online calibration method, the calibration accuracy of the rotating external parameter is increased by 15% and the accuracy of the translation external parameters is improved by 35%. Experimental results show the effectiveness of the proposed method.

1　引言

在实时定位与建图（Simultaneous localization and mapping，SLAM）技术中^［1］，相机和惯性测量单元（Inertial measurement unit，IMU）组合是常见的传感器组合，也是当下机器人导航^［2］、视觉自动驾驶^［3］和测量测绘等领域的主流传感器组合。其中，相机在低速运动时能够稳定成像，且短时间内成像变化不大，因此特征匹配时表现较好；而在高速运动时，相机容易成像模糊，且短时间内图像差异较大，这会导致重叠区域较少进而出现特征误匹配的问题。而IMU输出的是线加速度和角速度，在高速运动时能够输出可靠的测量结果，但在低速运动时测量结果反而不可靠。因此，在低速和高速运动相结合的作业场景下，基于相机和IMU的传感器组合，其在数据的可靠性方面具有互补性。基于相机和IMU融合的定位算法^［4-6］已成为近年来研究的热点，特别是将Camera-IMU与实时动态测量（Real-time kinematic，RTK）相融合逐渐成为测绘领域的重点研究方向。然而，由于缺乏视觉尺度的直接观测，单目相机处理信息的算法挑战明显多于双目相机^［7］，并且现在单目视觉系统^［8］的性能严重依赖于初值（速度、姿态、视觉尺度）的精度以及相机和IMU的标定。提高输出定位结果的精度是所有融合定位算法的目标，在此之前，需尽可能地准确标定各个传感器之间的转换关系，即估计相机和IMU之间的旋转和平移变换。在设备长期的使用与晃动过程中，机械构型发生变化，每隔一段时间进行人工测量IMU和相机之间的外参是很难实现的，因此将重点研究如何在算法中实现Camera-IMU传感器组合的在线标定和初始化，并改进标定算法，提高标定精度与鲁棒性。

Camera-IMU的外参标定方法主要有离线标定和在线标定^［9］，其中离线标定方法需要手持标定板进行一系列操作，费时费力，而在线标定在系统运行过程中即可完成标定，操作简单、实时性高。Dong-Si^［10］运用几何方法直接从传感器测量中计算系统的可观测量（姿态、速度、Camera-IMU外参），但没有考虑传感器的噪声和IMU零偏对系统产生的影响。为了以最小的运算量实现快速外参标定，Li^［11］提出一种快速的基于扩展卡尔曼滤波（Extended kalman filter，EKF）的视觉惯性里程计（Visual-inertial odometry，VIO）融合定位SLAM技术，在整个SLAM系统中使用扩展卡尔曼滤波实现对Camera-IMU外参的快速估计，因为算法的收敛性依赖于初始化的精度，所以外参标定效果较差。随后，外参离线标定工具Kalibr被提出并应用于Camera-IMU的外参标定，其可以解决多摄像机内外参标定^［12］、视觉惯性标定等问题、卷帘相机内参标定、IMU内参标定等问题，该工具可以得到相对可靠的标定结果，但标定过程较为复杂。Yang^［13］在Dong-Si的基础上利用线性估计方程优化方法求解外参和初始化参数，但该算法没有考虑IMU零偏，导致算法的精度较低。香港科技大学沈劭劼团队^［14］将经典的机器人手眼标定方法应用到外参在线标定中，先对Camera-IMU之间相对旋转量进行标定，在初始化过程中假设IMU的零偏不变，构造线性方程组进行外参线性初始化，标定和初始化完成后进行视觉惯性联合的状态估计，并且把标定量也作为估计的状态放在联合优化中^［15-16］。Huang等^［17］借鉴了沈劭劼团队的方法，在视觉惯性SLAM的基础上加上相机和IMU的相对位姿进行在线标定，此方法在系统运行之初就能快速得到足够高精度的标定结果，因此没有将外参纳入状态估计的优化变量中，大大节省了计算量。

为了实现影像RTK中Camera-IMU的初始化和在线外参标定，本文提出了一种高精度Camera-IMU在线外参标定算法。首先将Camera-IMU旋转外参线性初始化，假设已经追踪到足够多的特征，使用随机采样一致性算法对异常值进行判别与剔除，提高特征匹配的准确性，通过两帧图像的对极约束和经典的八点算法^［18］求得本质矩阵E，对求得的E进行奇异值分解（Single value decomposition，SVD）来恢复两帧图像之间的旋转增量。通过IMU预积分得到IMU的旋转增量，已知图像和IMU的旋转增量便可计算得到Camera-IMU的外参旋转量。其次，对Camera-IMU的速度、姿态、特征深度以及平移量进行线性初始化，当旋转量固定以后，在紧耦合滑动窗口中进行线性初始化，合并大量IMU和相机测量值，通过IMU的预积分来完成初始化。上述过程连续运行，自由切换，需要在自然环境下充分运动Camera-IMU套件，使其得到充分激励。所提在线标定算法对退化传感器数据也能展现出良好的效果。

2　基本原理

2.1　符号说明

坐标系转换如图1所示，w为世界坐标系，b为IMU坐标系，c为相机坐标系， $P_{y}^{x}$ 、 $V_{y}^{x}$ 、 $R_{y}^{x}$ 分别为y帧相对于x帧的三维位置、速度、旋转， $P_{t}^{x}$ 为t时刻IMU的坐标系位置相对于x帧的位置， $P_{c}^{b}$ 、 $R_{c}^{b}$ 分别为IMU坐标系到相机坐标系的平移量和旋转矩阵^［19］，其中 $q = {[q_{x}, q_{y}, q_{z}, q_{w}]}^{T}$ 为用四元数表示旋转， $g^{w} = {[0,0, g]}^{T}$ 为世界坐标系下的重力矢量， $g_{x}^{b}$ 为第x帧IMU坐标系下的地球重力矢量。

图 1. 坐标系转换示意图

Fig. 1. Schematic diagram of coordinate system transformation

下载图片查看所有图片

2.2　IMU模型和预积分

IMU由加速度计和陀螺仪组成，其以固定的频率在IMU坐标系下采集物体在三维空间的三轴加速度和三轴角速度，这就是最常见的六轴IMU。加速度计和陀螺仪的输出不仅受高斯噪声 $η_{a}$ 和 $η_{ω}$ 影响，还受随时间变化而变化的零偏 $b_{a}$ 和 $b_{ω}$ 影响，长时间的误差累积会影响IMU的测量精度。IMU的加速度计可以感知重力，由于重力的大小会影响加速度测量，所以通常会减去世界坐标系下 $g^{w}$ 的影响。

预积分最早在文献［20］中被提出，主要是为了解决重复积分问题，而后Forster等^［21］将其引进SLAM系统，通过对相机两个关键帧进行IMU预积分，得到两帧的状态增量，这样就避免了在优化过程中姿态发生改变时需要大量的重复积分，从而减小运算量。给定k时刻和k+1时刻两幅图像的IMU测量值，IMU在地球惯性坐标系下的位置和速度传播模型可表示为

\{\begin{array}{l} p_{b k + 1}^{w} = p_{b k}^{w} + v_{b k}^{w} Δ t + \iint_{t \in [k, k + 1]}^{} (R_{t}^{w} a_{t}^{b} - g^{w}) d t^{2} \\ v_{b k + 1}^{w} = v_{b k}^{w} + \int_{t \in [k, k + 1]} (R_{t}^{w} a_{t}^{b} - g^{w}) d t \end{array}

，（1）

式中： $p_{b k + 1}^{w}$ 为k+1时刻IMU坐标系相对于世界坐标系的位置； $v_{b k + 1}^{w}$ 为k+1时刻在IMU坐标系相对于世界坐标系的速度； $a_{t}^{b}$ 为IMU在t时刻的瞬时加速度； $Δ t$ 为两次图像采集时间 $[k, k + 1]$ 的间隔时间。由式（1）可知，IMU的状态传播模型需要t时刻IMU和世界坐标系的旋转矩阵。IMU从k时刻到k+1时刻位置、速度、旋转的预积分量可表示为

\{\begin{array}{l} a_{b k + 1}^{b k} = \iint_{t \in [k, k + 1]}^{} R_{t}^{b k} (\bar{a_{t}} - b_{a_{t}} - η_{a}) d t^{2} \\ β_{b k + 1}^{b k} = \int_{t \in [k, k + 1]} R_{t}^{b k} (\bar{a_{t}} - b_{a_{t}} - η_{a}) d t \\ R_{b k + 1}^{b k} = \int_{t \in [k, k + 1]} \frac{1}{2} R_{t}^{b k} [\begin{matrix} 1 \\ (\bar{ω_{t}} - b_{ω_{t}} - η_{ω}) \end{matrix}] d t \end{array}

，（2）

式中： $\bar{a_{t}}$ 、 $\bar{ω_{t}}$ 分别为加速度计和陀螺仪在t时刻的测量值，通过预积分可以在 $[k, k + 1]$ 内单独利用IMU的测量值获取，消除模型对全局的依赖。 $R_{t}^{b k}$ 为IMU的k时刻到当前t时刻的旋转增量，其值可以通过陀螺仪测量值短期积分得到，因此式（1）、式（2）也可表示为

\{\begin{array}{l} p_{b k + 1}^{b 0} = p_{b k}^{b 0} + R_{b k}^{b 0} v_{b k}^{b k} Δ t - g^{b 0} Δ t^{2} / 2 + R_{b k}^{b 0} α_{b k + 1}^{b k} \\ v_{b k + 1}^{b k + 1} = R_{b k}^{b k + 1} v_{b k}^{b k} Δ t - g^{b 0} Δ t + R_{b k}^{b k + 1} β_{b k + 1}^{b k} \\ R_{b k + 1}^{b 0} = R_{b k}^{b 0} R_{b k + 1}^{b k} \end{array}

。（3）

2.3　旋转外参标定

相机和IMU之间的旋转变换为 $R_{c}^{b}$ ，根据手眼标定公式，其旋转关系可表示为

R_{b k + 1}^{b k} \cdot R_{b}^{c} = R_{b}^{c} \cdot R_{c k + 1}^{c k}

，（4）

式中：相机从k到k+1时刻的旋转矩阵 $R_{c k + 1}^{c k}$ 可以通过两帧图像的对极约束和经典的八点算法来恢复， $R_{b k + 1}^{b k}$ 可以通过预积分得到，式（4）用四元数可表示为

q_{b k + 1}^{b k} \otimes q_{c}^{b} = q_{c}^{b} \otimes q_{c k + 1}^{c k}

，（5）

式中： $\otimes$ 为两个四元数之间的乘法运算。由式（5）可得到

[Q_{1} (q_{b k + 1}^{b k}) - Q_{2} (q_{c k + 1}^{c k})] q_{c}^{b} = Q_{k + 1}^{k} q_{c}^{b} = 0

，（6）

式中： $Q_{k + 1}^{k}$ 为 $Q_{1} (q_{b k + 1}^{b k}) - Q_{2} (q_{c k + 1}^{c k})$ ； $Q_{1} (q_{b k + 1}^{b k})$ 和 $Q_{2} (q_{c k + 1}^{c k})$ 分别为 $q_{b k + 1}^{b k}$ 和 $q_{c k + 1}^{c k}$ 的左乘和右乘的矩阵形式，可表示为

\{\begin{array}{l} Q_{1} (q) = [\begin{matrix} q_{w} I + {[q_{v}]}_{\times} & q_{v} \\ - q_{v}^{T} & q_{w} \end{matrix}] \\ Q_{2} (q) = [\begin{matrix} q_{w} I - {[q_{v}]}_{\times} & q_{v} \\ - q_{v}^{T} & q_{w} \end{matrix}] \end{array}

，（7）

式中： $q_{w}$ 为四元数的实部； $q_{v}$ 为四元数虚部； $I$ 为单位矩阵； ${[q_{v}]}_{\times}$ 为虚部向量的反对称矩阵，可表示为

q_{v} = {[q_{x}, q_{y}, q_{z}]}^{T}

，（8）

{[q_{v}]}_{\times} = [\begin{array}{r} 0 & - q_{z} & q_{y} \\ q_{z} & 0 & - q_{x} \\ - q_{y} & q_{x} & 0 \end{array}]

。（9）

上述公式表示两帧图像之间的旋转，大量图像之间的旋转可以用超定方程 $Q_{N} q_{c}^{b} = 0$ 表示，通过求解超定方程可求得旋转外参 $q_{c}^{b}$ 。

2.4　滑动窗口迭代优化

当Camera-IMU之间的旋转外参固定以后，可以估计速度、姿态、视觉尺度、外参平移量。在同时定位与地图构建中，随着时间的增加，关键帧和路标点会不断增多，优化变量也会快速增多，后端优化的效率会逐渐下降。为解决此问题，使用一个紧耦合的滑动窗口，保留距离当前时刻最近的几个关键帧，对之前的部分关键帧进行边缘化处理，使其维持在一定数量内，并且只对窗口内相关变量进行优化，减小了计算量，在计算复杂度不变的情况下融合单目相机和IMU测量值，滑动窗口模型如图2所示。其中 $X$ 表示关键帧的位姿，T₀和T₁表示两个路标点的位置。

图 2. 滑动窗口模型示意图

Fig. 2. Schematic diagram of sliding window model

下载图片查看所有图片

当出现新的观测约束时，滑动窗口将会丢弃窗口内最旧的状态，将新的状态纳入窗口。窗口内所有状态量定义为 $χ$ ， $χ$ 可表示为

\{\begin{array}{l} χ = [x_{0}, x_{1}, \dots, x_{n}, x_{c}^{b}, λ_{0}, λ_{1}, \dots, λ_{m}] \\ x_{k} = [p_{b k}^{w}, v_{b k}^{w}, q_{b k}^{w}, b_{a}, b_{ω}] \\ x_{c}^{b} = [p_{c}^{b}, q_{c}^{b}] \end{array}

，（10）

式中： $x_{k}$ 为k时刻相机帧的IMU状态，其包含了IMU在世界坐标系下的位置 $p_{b k}^{w}$ 、速度 $v_{b k}^{w}$ ；m为滑动窗口的大小； $q_{b k}^{w}$ 为姿态； $x_{c}^{b}$ 为相机和IMU包含旋转量和平移量的外参。 $χ$ 求解过程可被表述为一个非线性最小二乘问题^［22］并采用高斯牛顿法求解，

\begin{array}{l} \underset{χ}{m i n} {\{‖\begin{matrix}  \end{matrix} r_{p} (χ) - H_{p} χ‖}^{2} + {‖r_{β} ({\bar{z}}_{b k + 1}^{b k}, χ)‖}_{p_{b k + 1}^{b k}}^{2} + \\ \sum_{(l, j) \in c} ρ ({‖r_{c} ({\bar{z_{l}}}^{c_{j}}, χ)‖}_{P_{l}^{c_{j}}}^{2})\} \end{array}

，（11）

式中： $r_{p} (χ)$ 、 $H_{p}$ 为过去的状态量被边缘化并转化为先验残差； $r_{β} ({\bar{z}}_{b k + 1}^{b k}, χ)$ 为IMU因子残差，是IMU预积分的平移和速度建立的约束项； $r_{c} ({\bar{z_{l}}}^{c_{j}}, χ)$ 为视觉因子残差，是重投影建立的测量值 ${\bar{z_{l}}}^{c_{j}}$ 和状态量之间建立的约束项； $‖\cdot‖$ 为协方差P加权的马氏距离； $ρ (\cdot)$ 为Huber范数。通过式（11）可得 $χ$ 的最大后验估计。在Hamilton表达中四元数转化为旋转矩阵的公式可表示为

R = [\begin{matrix} 1 - 2 q_{z}^{2} - 2 q_{w}^{2} & 2 q_{y} q_{z} - 2 q_{x} q_{w} & 2 q_{y} q_{w} + 2 q_{x} q_{z} \\ 2 q_{y} q_{z} + 2 q_{x} q_{w} & 1 - 2 q_{y}^{2} - 2 q_{w}^{2} & 2 q_{z} q_{w} - 2 q_{x} q_{y} \\ 2 q_{y} q_{w} - 2 q_{x} q_{z} & 2 q_{z} q_{w} + 2 q_{x} q_{y} & 1 - 2 q_{y}^{2} - 2 q_{z}^{2} \end{matrix}]

。（12）

利用式（12）将四元数 $q_{c}^{b}$ 转换为旋转矩阵 $R_{c}^{b}$ ，将平移和旋转放在外参矩阵 $x_{c}^{b}$ 中，则可表示为 $x_{c}^{b} = [\begin{matrix} R_{c}^{b} & p_{c}^{b} \\ 0^{T} & 1 \end{matrix}]$ 。通过高斯牛顿求解非线性最小二乘问题，完成初始化和标定过程。

3　外参在线标定实现

3.1　算法框架

Camera-IMU外参标定和初始化的算法框架如图3所示。主要是两部分的求解，第一部分为旋转外参的初值求解和利用非线性滑动窗口迭代求解平移外参和系统初始化。在旋转外参的初值求解之前，首先，利用高精度RTK的时间信息将单目相机和IMU的时间戳对齐，实现时间同步。然后，考虑到有些图像在初始化过程中，聚焦较差、运动速度过快，以及噪声影响导致图片质量下降，图像出现退化和畸变现象，需要将像素坐标系转换到归一化坐标系下去除畸变，通过建立Camera-IMU的约束方程，剔除较小旋转增量的运动，使用两个里程计的相对旋转角度 $r_{k + 1}^{k} < t_{0}$ 作为约束条件来剔除位于图像边缘或图像视野中不稳定的外点，从而去除退化的传感器数据。最后，对筛选出来的数据通过手眼标定公式 $R_{b k + 1}^{b k} \cdot R_{b}^{c} = R_{b}^{c} \cdot R_{c k + 1}^{c k}$ 来求Camera-IMU之间的旋转量，将滑动窗口引入旋转外参标定中，由于单目相机的采样频率为30 Hz，IMU的采样频率为300 Hz，基于每两幅图像之间的多个IMU数据，滑动窗口对整个数据集满5帧就求解一次旋转外参，在SVD中调整第二小奇异值的阈值为0.25，并且将最后两个奇异值的比值与阈值比较，作为筛选旋转外参的条件。初始化和平移标定是将固定旋转外参作为非线性优化窗口的初值，在紧耦合滑动窗口中进行线性初始化，合并大量IMU和相机测量值，通过IMU的预积分来完成初始化。

图 3. Camera-IMU外参标定框架流程图

Fig. 3. Flow chart of Camera-IMU external parameter calibration framework

下载图片查看所有图片

3.2　剔除外点的标定模型

所提的标定问题可以用相对几何位姿变换来解决，相对变换法是使IMU和相机充分激励得到足够数据，利用不同时刻在同一参照系下的几何位姿差异来求解的一种方法。相机和IMU传感器由机械套件刚性连接，其相对位置不随时间变化而变化，如图4所示。

图 4. 旋转增量标定模型示意图

Fig. 4. Schematic diagram of rotation increment calibration model

下载图片查看所有图片

相机由k时刻到k+1时刻的旋转变换为 $R_{c k + 1}^{c k}$ ，IMU由k时刻到k+1时刻的旋转变换为 $R_{b k + 1}^{b k}$ ，在求解大量图像之间的旋转时，可用超定线性方程表示：

Q_{N} q_{c}^{b} = [\begin{matrix} ω_{1}^{0} Q_{1}^{0} \\ ω_{2}^{1} Q_{2}^{1} \\ ⋮ \\ ω_{N}^{N - 1} Q_{N}^{N - 1} \end{matrix}] q_{c}^{b} = 0

，（13）

式中：N为图片帧数，由滑动窗口的大小决定；由于相机八点算法得到的旋转增量包含退化运动（如直线运动）下的异常值，为了更好地处理异常值，加入权重 $ω_{N}^{N - 1}$ 进行外点剔除，增加鲁棒性，且其与相对旋转角度有关， $ω_{k + 1}^{k}$ 可表示为

ω_{k + 1}^{k} = \{\begin{array}{l} 1, r_{k + 1}^{k} < t_{0} \\ \frac{t_{0}}{r_{k + 1}^{k}}, r_{k + 1}^{k} \geq t_{0} \end{array}

，（14）

式中： $t_{0}$ 为设定的阈值； $r_{k + 1}^{k}$ 为相对旋转角度， $r_{k + 1}^{k}$ 可表示为

r_{k + 1}^{k} = a r c c o s \{\frac{t r [{(R_{c}^{b})}^{- 1} {(R_{b k + 1}^{b k})}^{- 1} R_{c}^{b} R_{c k + 1}^{c k}] - 1}{2}\}

，（15）

式中： $t r (\cdot)$ 为矩阵的迹。在手眼标定模型 $R_{b k + 1}^{b k} \cdot R_{b}^{c} = R_{b}^{c} \cdot R_{c k + 1}^{c k}$ 中，IMU的旋转增量 $R_{b k + 1}^{b k}$ 和相机的旋转增量 $R_{c k + 1}^{c k}$ 的相对旋转角度为 $r_{k + 1}^{k}$ ，如图5所示。

图 5. 里程计增量夹角

Fig. 5. Odometer incremental angle

下载图片查看所有图片

先根据式（15）求得相机里程计和IMU里程计增量之间的夹角，算法中使夹角 $r_{k + 1}^{k} < t_{0}$ 以剔除外点，使用剔除外点后的 $R_{b k + 1}^{b k}$ 和 $R_{c k + 1}^{c k}$ 来构建方程组。

此外，当传感器设备在一定时间内没有旋转运动，只有直线运动时，两个里程计旋转矩阵增量都接近于单位矩阵 $I$ ，就有 $I \cdot R_{b}^{c} = R_{b}^{c} \cdot I$ ，该等式对任意的矩阵 $R_{b}^{c}$ 都成立，这显然不符合需要求解的旋转标定。在建立Camera-IMU的约束方程式之前，需将直线运动时的数据作为外点将其剔除，从而筛选出具有一定旋转增量的数据，IMU的旋转增量 $R_{b k + 1}^{b k}$ 和相机的旋转增量 $R_{c k + 1}^{c k}$ 的相对旋转角度差 $r_{k + 1}^{k}$ 小于阈值，这就解决了约束方程的退化问题，能够剔除模型中的外点。

3.3　标定模型求解判定条件改进

在旋转运动足够多时，运动得到充分激励，这时对应着唯一解，此时零空间的秩为1， $Q_{N}$ 对应着唯一解，但在标定过程中，某些轴上的运动可能是退化运动，这时 $Q_{N}$ 的零空间的秩就有可能大于1。在用SVD方法求解超定方程 $Q_{N} q_{c}^{b} = 0$ 的最小二乘解时，将 $Q_{N}$ 奇异值分解为

Q_{N} = U Σ V^{T}

，（16）

式中：U和V为半酉阵； $Σ$ 为对角矩阵。U和V满足如下关系

\{\begin{array}{l} U^{T} U = I \\ V V^{T} = I \end{array}

，（17）

则

‖Q_{N} q_{c}^{b}‖ = ‖U \sum V^{T} q_{c}^{b}‖ = ‖\sum V^{T} q_{c}^{b}‖

。（18）

在奇异值分解中 $σ_{1}, σ_{2}, σ_{3}, σ_{4}$ 逐个递减，取 $V^{T} q_{c}^{b} = (\begin{matrix} 0 & 0 & 0 & 1 \end{matrix})$ ，则 $σ_{4}$ 为 $Q_{N}$ 4个奇异值中的最小值，对角矩阵 $Σ$ 中的最小奇异值 $σ_{4}$ 所对应的特征向量 $ν_{4}$ 为旋转外参 $q_{c}^{b}$ 的解。

传统方法^［23］判定收敛的终止条件是使用 $Q_{N}$ 来比较第二小奇异值 $σ_{3}$ 与阈值0.25，所求的奇异值的物理含义表示在每个维度上的运动激励，奇异值 $σ_{3} > 0.25$ ，表示在 $σ_{3}$ 这个维度上的运动激励尺度大于0.25，且迭代达到一定次数时，说明旋转外参标定成功。

然而在实验中发现，当设备在某一轴上出现匀速运动时，会产生退化现象，当使用判断条件 $σ_{3} > 0.25$ ，总会存在着 $σ_{2} \geq σ_{3} \geq σ_{4} > 0.25$ 的情况，此时4个奇异值的大小相等，最小奇异值 $σ_{4}$ 对应的特征向量并不是最佳解，这表明标定失败。为了使特征向量 $v_{4}$ 是 $σ_{4}$ 的最小二乘解且不陷入局部最小的情况，利用 $σ_{4}$ 和 $σ_{3}$ 的差异性来减小退化运动的影响，在算法上增加奇异值 $σ_{3} / σ_{4} \geq σ_{t}$ 作为判定条件，这使最小奇异值足够小，且和第二小奇异值有足够的差距。若条件成立，则说明最小二乘解为最优解，此时标定成功；若条件不成立，则需额外剔除外点的旋转序列来提供约束。对包含退化的数据而言，通过增加判断条件可以减少退化数据，增加旋转序列，提高标定准确度。实验结果表明：改进后的算法比传统算法外参标定精度更高，更具有鲁棒性。

4　实验分析

4.1　实验平台与传感器数据预处理

实验平台由司南导航T300PLUS RTK接收机、CMOS图像传感器PS5268和博世SMI130 IMU构成，如图6所示。实验平台固定在对中杆上方便于手持采集数据，如图6（a）所示。在世界坐标系下，（·）^c为相机坐标系，相机坐标系原点在相机光心，（·）^b为IMU坐标系，IMU坐标系原点在IMU的中心，相机和IMU的x轴、y轴、z轴，如图6（b）所示。为了方便后期实验结果的比较和观察，使用偏航角、俯仰角、滚转角来分析旋转量，用x、y、z的移动来分析平移量。

图 6. 影像RTK实验平台。（a）实验平台结构；（b）坐标系示意图

Fig. 6. Image RTK experimental platform. (a) Experimental platform structure; (b) schematic diagram of the coordinate system

下载图片查看所有图片

传感器预处理步骤如下：

a）充分晃动设备，激励试验平台的运动状态；

b）分批多次采集时长大于1 min的数据，以确保在旋转外参求解过程中有充足的满足条件的IMU数据和相机数据；

c）在获取相机和IMU两个传感器数据以后，将图像的开始时间和IMU时间对齐，丢弃没有IMU数据的图像，然后通过全球导航卫星系统（GNSS）时间使两个传感器数据对齐，制作具有同一时间序列的相机和IMU位姿数据包。

4.2　改进阈值与加权方式对比实验

在求解 $Q_{N} q_{c}^{b} = 0$ 过程中，改进判断最小二乘解的最优解条件，判定条件为判定奇异值 $σ_{3} / σ_{4} \geq σ_{t}$ 是否成立，当条件成立时，表明已经有足够的旋转序列可以求得旋转外参，若该最小二乘解不是最优解，则需获得已经剔除外点的旋转序列以提供约束。为了尽可能减小误差，通过不同的加权方式获得最优旋转外参标定初值，通过大量实验将 $σ_{3} / σ_{4}$ 的阈值控制在2时会取得较好的效果，选择阈值分别为1、2、3进行对比实验，加权方式选择平均值加权和 $σ_{3} / σ_{4}$ 两种加权方式进行对比实验，平均值即满足阈值条件所有解的平均值作为加权条件， $σ_{3} / σ_{4}$ 加权以其比值作为各个解的权重，探索不同阈值和加权方式对外参旋转初值精度的影响，求得最优阈值和加权方式组合，使旋转外参标定结果更加准确。实验结果如表1所示。

表 1. 不同阈值加权方式旋转外参标定误差

Table 1. Rotation external parameter calibration error in different threshold weighting methods

Threshold	Weighting method	Error			Mean error
Threshold	Weighting method	Yaw	Pitch	Roll	Mean error
1	$σ_{3} / σ_{4}$	2.933	3.232	1.224	2.463
1	Average	3.376	2.234	1.893	2.501
2	$σ_{3} / σ_{4}$	1.378	0.987	1.603	1.323
2	Average	3.738	3.479	1.673	2.963
3	$σ_{3} / σ_{4}$	2.390	0.674	2.786	1.950
3	Average	4.379	1.023	3.356	2.919

查看所有表

通过不同阈值和加权方式的组合，获得旋转外参的偏航角、俯仰角、滚转角的结果与真实目标结果的误差，误差均值为旋转外参3个误差量的平均值。由表1可知，当阈值为1、以 $σ_{3} / σ_{4}$ 的比值作为加权时，翻滚角的误差最小；当阈值为2、以 $σ_{3} / σ_{4}$ 的比值作为加权时，偏航角的误差最小；当阈值为3、以 $σ_{3} / σ_{4}$ 的比值作为加权时，俯仰角的误差最小。综合3个误差量，当阈值为2、以 $σ_{3} / σ_{4}$ 的比值作为加权时，误差均值最小为1.323°，此时为最佳阈值和加权方式组合。

4.3　外参收敛过程分析

通过带有RTK的相机与IMU实验平台采集数据，通过4.2节的实验结论将阈值确定为2，并以 $σ_{3} / σ_{4}$ 的比值作为旋转外参和平移外参在线估计分析的加权方式，通过非线性迭代优化旋转外参（偏航角、俯仰角、滚转角），实验结果表明：3个外参都在约20 s处收敛近似外参真值，旋转外参收敛过程如图7所示。

图 7. 旋转外参收敛过程。（a）偏航角；（b）俯仰角；（c）翻滚角

Fig. 7. Rotation external parameter convergence process. (a) Yaw; (b) pitch; (c) roll

下载图片查看所有图片

得到旋转外参标定结果以后，将该标定结果作为非线性优化窗口的初值，在紧耦合滑动窗口中进行线性初始化，合并大量IMU和相机测量值，并通过IMU的预积分来完成平移量的标定，平移外参收敛过程如图8所示。实验结果表明各状态量在约29 s处开始收敛。

图 8. 平移外参收敛过程。（a）x轴；（b）y轴；（c）z轴

Fig. 8. Translation external parameter convergence process. (a) x-axis; (b) y-axis; (c) z-axis

下载图片查看所有图片

为了计算改进算法的精度，通过手动测量和Kalibr多次测量得到相机与IMU的外参真值，由于该值的误差非常小，将该值近似为真值，具体结果如表2所示。

表 2. 外参测量值与收敛时间

Table 2. External parameter measurement and convergence time

Parameter	Rotate the outer parameters			Pan the outer parameter
Parameter	Yaw /（°）	Pitch /（°）	Roll /（°）	x /cm	y /cm	z /cm
Convergence time /s	22	23	20	20	35	31
True value	0	0	80	6.8	2.5	7.2
Calibration value	－1.8	1.7	78.3	7.6	3.3	6.3
Error	1.8	1.7	1.7	0.8	0.8	0.9

查看所有表

由表2可知，所提在线标定的旋转外参误差约为1.7°，平移外参的误差约为0.8 cm。虽然修改阈值和权重使平移外参的收敛时间由15 s增加到约29 s，但平移外参误差由1.6 cm减小到1 cm，旋转外参误差由2°减小到1.7°，这说明改进的Camera-IMU外参在线标定算法能提高标定精度，且有利于提升整个系统的精度和鲁棒性。

4.4　轨迹可视化对比验证

在3.2节已通过实验对改进的在线标定算法的精度和收敛时间进行评估，所提算法除了估计IMU和相机之间的外参，还融合定位构建运动轨迹。为了比较所提算法的准确率，将所提算法与传统算法在同一数据集下进行轨迹可视化对比，通过对比轨迹的定位轨迹精度、绝对位姿误差（APE）等方面来验证改进算法的优越性。所提算法与传统算法分别在数据集中与真实轨迹的重合度如图9所示。

图 9. 轨迹可视化。（a）所提算法轨迹；（b）传统算法轨迹

Fig. 9. Track visualization. (a) Trajectory of proposed algorithm; (b) trajectory of traditional algorithm

下载图片查看所有图片

由图9可知，传统算法与所提算法运行得到的轨迹和真实轨迹的重合度都很高，表明所提算法对系统整体运行具有良好的效果。具体量化分析两种算法的误差，首先用APE来计算位姿的真实值与系统估计值之间的差值^［24］，获得所提算法的精度和全局一致性，APE的各项评价参数如图10所示。由图10可知，所提算法的平均误差、误差中位数、均方根误差均小于传统算法，其中传统算法的平均误差为0.227 m，误差中位数为0.196 m，均方根误差为0.268 m，所提算法的平均误差为0.211 m，误差中位数为0.176 m，均方根误差为0.253 m。该结果表明：所提算法能有效提高外参标定的精度，也能提高RTK/Camera/INS（Inertial navigation system）多源系统融合定位的精度。

图 10. APE的评价参数

Fig. 10. Evaluation parameters of APE

下载图片查看所有图片

由图9、图10可知，与传统算法相比，所提算法在某些特定环境下能有效提升系统的精度与鲁棒性。

5　结论

提出一种改进的Camera-IMU外参的标定方法，通过改变判断最小二乘解的最优解条件，利用新的加权方式，实现对旋转外参和平移外参的在线标定，在传统算法基础上改进求解旋转外参过程，提高标定结果的精度。其中，旋转外参的误差为1.7°，平移外参的误差为0.8 cm，所提算法旋转外参和平移外参的标定精度较传统算法分别提高15%和35%。在同一数据集下运行传统算法和所提算法，验证了所提算法在外参在线标定与系统融合定位精度方面的性能。对其轨迹和相对位姿误差进行比较，证明运行轨迹基本与真实轨迹重合，而且所提算法的APE表现较好，具有较高的精度和一致性，这表明所提算法在Camera-IMU外参在线标定过程中具有较好的可靠性。

参考文献

[1] 徐陈, 周怡君, 罗晨. 动态场景下基于光流和实例分割的视觉SLAM方法[J]. 光学学报, 2022, 42(14): 1415002.

Xu C, Zhou Y J, Luo C. Visual SLAM method based on optical flow and instance segmentation for dynamic scenes[J]. Acta Optica Sinica, 2022, 42(14): 1415002.

[2] Zhang C, Yang Z, Xu H, et al. RRVPE: a robust and real-time visual-inertial-GNSS pose estimator for aerial robot navigation[J]. Wuhan University Journal of Natural Sciences, 2023, 28(1): 20-28.

[3] BaheriA, KolmanovskyI, GirardA, et al. Vision-based autonomous driving: a model learning approach[C]∥2020 American Control Conference (ACC), July 1-3, 2020, Denver, CO, USA. New York: IEEE Press, 2020: 2520-2525.

[4] 王鑫, 李伟, 梁炜, 等. 单目相机-IMU-机械臂的空间联合标定方法[J]. 仪器仪表学报, 2021, 42(6): 221-231.

Wang X, Li W, Liang W, et al. A space joint calibration method of monocular camera-IMU-manipulator[J]. Chinese Journal of Scientific Instrument, 2021, 42(6): 221-231.

[5] 余哲琳. 基于视觉惯导融合的智能车辆定位技术研究[D]. 成都: 电子科技大学, 2022.

YuZ L. Intelligent vehicle localization technology research based on vision and inertial fusion[D]. Chengdu: University of Electronic Science and Technology of China, 2022.

[6] MishraS, PandeyG, SaripalliS. Target-free extrinsic calibration of a 3D-lidar and an IMU[C]∥2021 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI), September 23-25, 2021, Karlsruhe, Germany. New York: IEEE Press, 2021.

[7] 吕钧澔, 娄群, 校金友, 等. 大倾角靶标的双目相机高精度标定方法[J]. 光学学报, 2022, 42(23): 2312002.

Lü J H, Lou Q, Xiao J Y, et al. High-precision calibration method for binocular camera with large inclination target[J]. Acta Optica Sinica, 2022, 42(23): 2312002.

[8] OgataK, TanakaH, MatsumotoY. A robust position and posture measurement system using visual markers and an inertia measurement unit[C]∥2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), November 3-8, 2019, Macau, China. New York: IEEE Press, 2020: 7497-7502.

[9] 黄文文, 彭小红, 李丽圆, 等. 相机标定方法及进展研究综述[J]. 激光与光电子学进展, 2023, 60(16): 1-17.

Wen W W, Peng X H, Li L Y, et al. A review of camera calibration methods and research progress[J]. Laser & Optoelectronics Progress, 2023, 60(16): 1-17.

[10] Dong-SiT C, MourikisA I. Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration[C]∥2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, October 7-12, 2012, Vilamoura-Algarve, Portugal. New York: IEEE Press, 2012: 1064-1071.

[11] Li M Y, Mourikis A I. High-precision, consistent EKF-based visual-inertial odometry[J]. The International Journal of Robotics Research, 2013, 32(6): 690-711.

[12] 杨昭辉, 朱华炳, 殷玉龙, 等. 基于编码立体靶标的高精度双目相机标定[J]. 中国激光, 2023, 50(6): 0604002.

Yang Z H, Zhu H B, Yin Y L, et al. High-precision binocular camera calibration based on coding stereoscopic target[J]. Chinese Journal of Lasers, 2023, 50(6): 0604002.

[13] Yang Z F, Shen S J. Monocular visual-inertial state estimation with online initialization and camera-IMU extrinsic calibration[J]. IEEE Transactions on Automation Science and Engineering, 2017, 14(1): 39-51.

[14] Qin T, Li P L, Shen S J. VINS-mono: a robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020.

[15] 曹力科, 肖晓晖. 基于卷帘快门RGB-D相机的视觉惯性SLAM方法[J]. 机器人, 2021, 43(2): 193-202.

Cao L K, Xiao X H. A visual-inertial SLAM method based on rolling shutter RGB-D cameras[J]. Robot, 2021, 43(2): 193-202.

[16] 潘林豪, 田福庆, 应文健, 等. 单目相机-IMU外参自动标定与在线估计的视觉-惯导SLAM[J]. 仪器仪表学报, 2019, 40(6): 56-67.

Pan L H, Tian F Q, Ying W J, et al. VI-SLAM algorithm with camera-IMU extrinsic automatic calibration and online estimation[J]. Chinese Journal of Scientific Instrument, 2019, 40(6): 56-67.

[17] HuangW B, LiuH. Online initialization and automatic camera-IMU extrinsic calibration for monocular visual-inertial SLAM[C]∥2018 IEEE International Conference on Robotics and Automation (ICRA), May 21-25, 2018, Brisbane, QLD, Australia. New York: IEEE Press, 2018: 5182-5189.

[18] Hartley R I. In defense of the eight-point algorithm[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1997, 19(6): 580-593.

[19] 黄秀珍, 伍一帆, 李凯涛. 基于李群的无迹卡尔曼滤波视觉SLAM算法[J]. 无线电通信技术, 2022, 48(2): 342-346.

Huang X Z, Wu Y F, Li K T. An unscented Kalman filter visual SLAM algorithm based on lie group[J]. Radio Communications Technology, 2022, 48(2): 342-346.

[20] Lupton T, Sukkarieh S. Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions[J]. IEEE Transactions on Robotics, 2012, 28(1): 61-76.

[21] Forster C, Carlone L, Dellaert F, et al. On-manifold preintegration for real-time visual: inertial odometry[J]. IEEE Transactions on Robotics, 2017, 33(1): 1-21.

[22] Lin Y, Gao F, Qin T, et al. Autonomous aerial navigation using monocular visual-inertial fusion[J]. Journal of Field Robotics, 2018, 35(1): 23-51.

[23] Wang K, Cao C, Ma S, et al. An optimization-based multi-sensor fusion approach towards global drift-free motion estimation[J]. IEEE Sensors Journal, 2021, 21(10): 12228-12235.

[24] 林伟文, 甘海云, 朱冰冰, 等. 视觉同步定位与建图中特征点匹配算法优化[J]. 汽车工程师, 2021(12): 13-17.

Lin W W, Gan H Y, Zhu B B, et al. Visual simultaneous localization and mapping feature point matching algorithm optimization[J]. Automotive Engineer, 2021(12): 13-17.

茆子念, 周志峰, 沈亦纯, 王立端. 高鲁棒性Camera-IMU外参在线标定算法[J]. 激光与光电子学进展, 2024, 61(4): 0411005. Zinian Mao, Zhifeng Zhou, Yichun Shen, Liduan Wang. Camera-IMU State Estimation and External Parameter Online Calibration Algorithm[J]. Laser & Optoelectronics Progress, 2024, 61(4): 0411005.