双目视觉与惯性测量单元相对姿态在线标定

施佳良，宋来亮，江泽

摘 要：为了对两种传感器进行更好的融合，以实现更高的组合导航定位精度，文章基于多状态约束卡尔曼滤波器，提出了一种能够在线标定双目相机与IMU相对姿态误差的方法。

关键词：视觉导航;在线标定;紧耦合;惯性测量单元;多状态约束卡尔曼滤波

传统GPS/INS组合导航方式在一些区域无法实现组合导航定位，比如在高楼林立的楼宇间以及山区等地方，而视觉图像技术则可以在上述区域实现导航定位。虽然通过视觉图像技术可以获得较丰富的场景信息，但容易受到外界因素干扰;惯性导航技术虽然有累积误差，但是频率高、数据稳定、短期精度较高。如果将两者有效结合，则可以实现对载体精度较高的运动状态估计[1]。

視觉图像技术与惯性导航技术分别依赖于相机和惯性测量单元（Inertial Measurement Unit，IMU），如果能够准确估计相机和IMU之间的相对位姿，对于提高相机和IMU两种传感器之间的数据融合精度非常关键[2]。

通常，对于两种传感器相对姿态和位置的确定是依赖于标定板等其他外部辅助设备，但对于两种传感器相对位姿的标定结果会由于时间推移等因素影响变得不够准确。如果直接将两者之前确定的坐标关系代入视觉惯导融合算法中，会导致融合定位的效果非常不好，甚至会使估计的结果发散[3]。因此，准确地估计相机和IMU之间的相对位姿有重要的意义。

传统的相机-IMU标定多是离线标定，并依赖于标定板等其他辅助设备。通常，离线标定可以分为基于优化与基于滤波两种方法，基于滤波器的方法相较于基于优化的方法[3-4]，计算量通常比较小;基于优化的方法为了防止计算量过大，采用了滑动窗口优化的方法。

在线标定相比于离线标定，不需要标定板等其他辅助设备，成本更低、操作更容易，不过算法难度较大。

本文基于多状态卡尔曼滤波，实现了双目视觉与惯性测量单元紧耦合在线标定，与传统的离线标定相机-IMU的方法相比，本算法不依赖于任何标定设备就能够在环境未知的情况下，通过运动激励在线估计相机与惯性测量单元相对位姿。

1 系统描述

全局坐标系：OGXGYGZG;IMU坐标系：OIXIYIZI;相机坐标系：OCXCYCZC。

特征点在全局，IMU与相机3个坐标系下分别表示为，，，这3个参数都是待估计量。表示IMU在全局坐标系下的位置与姿态。表示特征点在全局坐标系下的位置与姿态，待标定的相机与IMU的相对位姿用表示，表示归一化四元数。

2 MSCKF算法

多状态约束下的卡尔曼滤波器（Multi-State Constraint Kalman Filter，MSCKF）算法的流程如下：

（1）IMU积分，先利用IMU的加速度计和陀螺仪测得的加速度和角速度的信息对状态向量中的IMU状态进行预测，一般会处理多帧的IMU观测数据。

（2）相机状态扩增，每收到一张图片后，计算当前相机状态并加入到状态向量中，同时扩充状态协方差矩阵。

（3）特征点三角化，根据历史相机状态三角化估计3D特征点在全局坐标系下的坐标。

（4）特征更新，再利用特征点对多个历史相机状态的约束，来更新状态向量。

（5）历史相机状态移除，如果相机的状态个数超过N，则剔除最老或最近的相机状态以及对应的协方差。

关于初始化，保持载体（IMU）静止将前200帧加速度和角速度求平均，平均加速度的模值g作为重力加速度，平均角速度作为陀螺仪的零偏，计算重力向量（0，0，﹣g）和平均加速度之间的夹角（通过旋转四元数），标定初始时刻IMU系与全局坐标系之间的夹角。

3 视觉/惯性紧耦合模型

IMU的状态向量如下：

（1）

其中，bg和ba分别是惯性测量单元在全局坐标系下的陀螺仪和加速度计的零偏。IMU的误差状态向量表示如下：

（2）

由于最后对应的N个相机状态也被增广到状态向量中，得到整个误差状态向量，包含一个IMU状态向量以及N个相机状态：

（3）

， （4）

3.1 过程模型

式（5）和式（6）分别表示IMU的加速度计和陀螺仪的量测模型，没有考虑地球自转角速度：

（5）

（6）

wm和am分别为陀螺和加表在IMU坐标系下的量测值，ng和na是陀螺仪和加速度计的零均值高斯白噪声。

由于IMU的更新频率是相机的十几倍，所以两帧图像之间的IMU更新离散化如式。其中，带上三角表示估计值。表示四元数转旋转矩阵。

（7）

（8）

（9）

其中：

（10）

（11）

（12）

（13）

线性连续IMU误差状态模型：

（14）

状态转移矩阵以及噪声方差矩阵的离散化处理：

（15）

（16）

其中，是系统的连续时间噪声方差矩阵。表示离散时间系统IMU状态转移矩阵。表示离散时间系统中IMU的噪声方差阵。误差状态模型离散化得：

（17）

是IMU中传感器的噪声。

计算IMU状态的一步预测方差阵公式表示如式（18）：

（18）

整体状态的一步预测方差阵表示如式（19）：

（19）

3.2 观测模型

IMU与GPS组合过程中，GPS可以直接给出载体位置GpI的观测量，而视觉提供的不是位置信息。在视觉当中，约束通常是特征点到相机的重投影误差（空间中一个3D特征点根据相机的姿态和位置投影到相机平面，与实际观测的特征点之间像素的误差），通过重投影误差约束等来构建观测模型。

整个系统的量测用zm，k表示，由k时刻对应的m个特征点的像素坐标构成，系统的量测模型如式（20）所示：

（20）

式（21）构建的是简化后的双目相机视觉测量模型：

（21）

其中，和分别表示特征点fj在左相机和右相机下的像素坐标，为了方便表达，这里的像素坐标是没有考虑内参矩阵以及畸变系数的坐标值。表示特征点fj对应的相机第i次观测，j对应一帧图像里特征点的个数。测量残差可以表示如式（22）：

（22）

式（23）表示利用IMU的状态估计以及相机和IMU之间的外参估计，最终得到点在相机坐标系下的坐标， 是上述MSCKF算法中通过第3个步骤估计的。

（23）

上述变量线性化后可得残差表示如式（24）：

（24）

上述公式（24）是單个相机对单个特征点的观测模型，，分别是残差对状态向量和特征点的雅可比矩阵，其中：

（25）

式（25）中是相机状态对应的雅可比矩阵。

对单个特征点对应的所有相机观测模型进行合并，可以得到单个特征点的观测模型：

（26）

根据MSCKF算法并未包含在状态向量中，所以希望将特征点从观测模型中移除，移除方式是观测模型两边乘以的左零空间，左零空间定义为：

（27）

对所有的特征点残差模型进行合并，得到整体的残差模型：

（28）

最后得到MSCKF的最优增益为：

（29）

状态方程的修正量：

ΔX=Krn （30）

状态协方差矩阵的更新量：

（31）

式（31）中，ξ=6N+21是协方差矩阵的维数，6N对应的是相机在全局坐标系下的姿态和位置状态[4]。

4 实验与分析

数据集中，左右相机与IMU之间的相对位置和姿态的真实值是已知的（提前离线标定过），如表1所示。

采集设备的坐标关系如图1所示。左相机绕先沿X轴正向移动5 cm，使得相机坐标系中心与IMU坐标系中心重合，最后绕Z轴逆时针旋转90°（真实值），使两者坐标系完全重合，左相机并不绕X轴或Y轴旋转。

同理，右相机先沿X轴负向移动5 cm，使得相机坐标系中心与IMU坐标系中心重合。最后绕Z轴逆时针旋转90°（真实值），使两者坐标系完全重合，同理其他两个自由度姿态没有变化。这里，绕Z轴变换表示横滚角发生变化[5]。

首先，设置左相机和IMU之间的初始相对姿态相对于相机与IMU真实的相对姿态之间的欧拉角偏差，其中，只在绕Z轴的姿态变化上叠加了1°误差，横滚、俯仰以及航向角误差表示如下：[-1°，0°，0°]。

同理，设置的右相机和IMU之间的初始相对姿态相对于相机与IMU真实的相对姿态之间的横滚角，俯仰角以及航向角偏差为[-2.25°，0°，0°]。同时，图像噪声设为噪声水平为1个像素的高斯白噪声。

如图2—3所示，左相机与IMU的相对姿态和真实两者的相对姿态之间的误差最终收敛于0，右相机与IMU的相对姿态和真实两者的相对姿态误差也最终收敛于0，且误差收敛速度快。

5 结语

本文提出了在初始误差较大的情况下，双目相机与IMU相对姿态的在线标定方法。通过实验表明，该方法有效且准确。不同传感器之间的数据同步对于视觉惯性组合系统数据融合的影响也很大，除了使用硬件同步的方式对数据进行同步处理外，也可以考虑把时间同步误差也加入到状态量里面，进行实时估计。

作者简介：施佳良（1994— ），男，陕西西安人，硕士研究生;研究方向：惯性导航，组合导航。

[参考文献]

[1]SUN K，MOHTA K，PFROMMER B，et al.Robust stereo visual inertial odometry for fast autonomous flight[J].IEEE Robotics and Automation Letters，2018（2）：965-972.

[2]杨浩，张峰，叶军涛.摄像机和惯性测量单元的相对位姿标定方法[J].机器人，2011（4）：419-426.

[3]YANG H，ZHANG F，YE J.A Camera-IMU relative pose calibration method[J].Jiqiren/Robot，2011（4）：419-426.

[4]BRINK K，SOLOVIEV A.Filter-based calibration for an IMU and multi-camera system[C].Washington：Proceedings of the 2012 IEEE/ION Position，Location and Navigation Symposium，2012.

[5]MIRZAEI F M，ROUMELIOTIS S I.A kalman filter-based algorithm for IMU-Camera calibration：observability analysis and performance evaluation[J].IEEE Transactions on Robotics，2008（5）：1143-1156.

Online calibration method for stereo cameras-IMU relative pose

Shi Jialiang1， Song Lailiang2， Jiang Ze3

（1.Xian Institute of Aerospace Precision Mechatronics， Xian 710100， China; 2.Beihang University， School of Instrumentation and Optoelectronic Engineering， Beijing 100083， China）

Abstract：In order to combine these two kinds of sensors and achieve better fusion results， this paper puts forward a method which can precisely calibrate the relative pose between the stereo cameras and IMU online.

Key words：visual navigation; online calibration; tightly-coupling; inertial measurement unit; multi states constraint kalman filter