1 前言

在计算机领域, 运动状态估计有着广泛的应用场景, 涉及机器人导航、自动驾驶、虚拟现实以及增强现实等诸多领域. 在众多传感器融合方案中, 视觉与惯性传感器融合更好的鲁棒性. 一方面, 相机传感器能够提供丰富的环境视觉信息, 以便进行环境定位、回环检测以及三维重建等; 另一方面, 惯性传感器(inertial measurement unit, IMU)可直接提供自身运动信息, 可恢复真实尺度信息, 重力方向以及自身姿态信息.

由于光照信息变化、弱纹理以及运动模糊等外部环境因素, 从而导致视觉跟踪失败. 而IMU预积分估计可有效弥补纯视觉跟踪效果, 从而有效改善运动跟踪性能.

然而, 对于单目惯性视觉运动估计仍存在些许问题会影响实际应用. 一是系统初始化问题, 由于缺少直接距离(深度)信息, 很难将单目视觉跟踪系统与惯性估计相结合. 二是单目惯性视觉问题是高度非线性的, 数值迭代求解方面仍面临着挑战以及位姿估计在长时间运动下的轨迹漂移问题. 为此, 希望运动状态估计系统在已知空间位置进行初始化, 并缓慢移动. 基于以上两点, 限制了运动状态估计在实际中的应用.

目前, 以单目视觉为基础的里程计以及同步定位与地图构建(simultaneous localization and mapping, SLAM)等领域有着很多优秀的研究工作. 如首次采用多线程方法的PTAM^[1], 以及后续改进的SVO^[2]、LSD-SLAM^[3]以及ORB-SLAM^[4]等.

一般的单目惯性测量, 如Weiss^[5]和Lynen^[6], 采用松耦合的方式, 将视觉信息与IMU信息作为独立模块计算. 松耦合采用扩展卡尔曼滤波, IMU作为状态变量, 视觉信息用于更新变量. 更进一步, 则是采用扩展卡尔曼滤波^{[5, 7, 8]}或图优化算法^{[9, 10]}, 将图像信息与IMU信息联合优化. MSCKF^[11]是著名的基于扩展卡尔曼滤波EKF的惯性视觉里程计, 通过相机姿态作为状态变量, 并使用多角度相机的多相机视图来形成约束更新. 为实现一致处理时间, 基于图优化的视觉惯性里程计(visual inertial odometry, VIO)方法通常采用边缘化窗口外的状态与测量值, 在最近状态的有限窗口进行数据优化. 由于迭代求解非线性系统的高计算量, 很难实现实时计算.

根据视觉信息处理方式, 位姿优化可分为最小化光流误差的直接法^[2,3,11]和基于最小化重投影误差的非直接方法^[7,9,12]. 直接法需要较好的初始值估计, 而非直接法则需要更多的计算资源进行特征提取与匹配. 目前, 非直接法因算法成熟及鲁棒性受到更多的应用, 而直接法的优势则是可像素操作并可使密集匹配变得更加容易.

通常IMU数据的获取频率远远超过相机图像数据. 为此提出了很多方法计算高频IMU数据测量. 最直接的方法采用EKF方法将IMU数据作为状态变量进行计算^{[5, 11]}. 在图优化算法中, 采用IMU预积分方式避免IMU积分重计算. 这种方法是由Lupton等^[13]第一次提出, 采用欧拉角参数化旋转变量. Forster等^{[10, 14]}通过添加IMU残差校正进一步提升了预积分方法.

香港科技大学提出以稀疏光流法跟踪特征点为前端, 采用IMU预积分与视觉构建紧耦合框架-VINS-Mono单目视觉SLAM系统^[15], 解决了视觉与IMU尺度对齐, 重力方向、IMU数据残差等问题. Campos等^[16]在2020年提出第一个支持纯视觉、视觉-惯性以及多地图的实时SLAM系统, 可在单目、双目和RGB-D相机上利用针孔或鱼眼成像模型运行. Campos等在单目-惯性初始化^[17]对IMU进行了残差计算, 恢复了单目相机尺度, 并对齐到惯性系.

本文方法采用惯性传感器数据与相机图像数据以紧耦合策略构建联合状态变量, 通过最小化加权重投影误差与惯性运动误差在移动端实现实时位姿优化计算. 紧耦合策略可有效保证全局轨迹位姿一致性, 减小漂移误差. 在移动端可实时并行采集IMU数据与相机图像数据, 在保证实时性的同时, 可获得更小误差参数的重建轨迹位姿.

2 位姿优化方法

传统纯视觉位姿优化算法一般采用相机图像信息, 首先对相机进行参数标定, 获取包含相机焦距等参数在内的内参矩阵信息; 而后对所获取图像进行特征提取、匹配, 建立图像关键帧; 根据同一场景图像间的对极几何约束关系, 求解出对应基础矩阵或本质矩阵; 对基础矩阵或本质矩阵进一步分解, 可求解出的相机在两幅图像间的旋转、位移; 而后通过计算特征点重投影误差进行位姿优化, 获取相机轨迹位姿信息.

本文方法在传统纯视觉位姿优化算法上, 增加了惯性传感器数据对轨迹位姿测量约束, 并采用加权视觉重投影误差与惯性运动误差更加优化求解出相机位姿轨迹信息. 优化方法主要内容如下.

惯性运动测量单元在惯性坐标系B下, 在 $\Delta t$ 间隔时间内测量所得加速度为 ${a_B}$ 、角加速度 ${w_B}$ . 由于传感器自身构造原因, 尤其是存在传感器噪声, 从而导致测量值存在误差. 为消除传感器噪声, 采用加速度计误差 ${b_a}$ 和陀螺仪误差 ${b_g}$ 表示.

在世界坐标系 $W$ 下, 离散化IMU旋转变量 ${R_{WB}} \in $ $ SO(3)$ , 位置变量 ${P_{WB}}$ 以及速度变量 ${v_{WB}}$ , 可采用速度学公式进行计算如下:

$ \left\{ {\begin{split} & R_{WB}^{k + 1} = R_{WB}^k{\rm{exp}}\left(\left(w_B^k - b_g^k\right)\Delta t\right) \hfill \\ & v_{WB}^{k + 1}{\text{ = }}v_{WB}^k + R_{WB}^k\left(a_B^k - b_g^k\right)\Delta t \hfill \\ & P_{WB}^{k + 1} = P_B^k + v_{WB}^k\Delta t + R_{WB}^k\left(a_B^k - b_g^k\right)\Delta {t^2} \hfill \end{split} } \right.$

(1)

本文提出采用紧耦合的方式, 将IMU数据与图像信息数据紧密结合在一起, 通过联合优化方式求解. 基于Mur-Artal等^[18]提出的采用ORB特征点的实时ORB-SLAM系统, 采用初始定义两个关键帧之间的运动变化, 旋转变量差值 $\Delta R$ , 速度变量差值 $\Delta v$ , 位置变量差值 $\Delta P$ , 根据两关键帧之间的IMU数据预积分计算, 以第 $i$ 关键帧的运动差值变量定义如下:

$\left\{ { \begin{split} & R_{WB}^{i + 1} = R_{WB}^i\Delta {R_{i,i + 1}}{\rm{exp}}\left(J_{\Delta R}^gb_g^i\right) \hfill \\ & v_{WB}^{i + 1}{\text{ = }}v_{WB}^i + R_{WB}^i\left(\Delta {v_{i,i + 1}}{\text{ + }}J_{\Delta {{v}}}^gb_g^i + J_{\Delta v}^ab_a^i\right) \hfill \\ & P_{WB}^{i + 1}{\text{ = }}P_B^i + v_{WB}^i\Delta {t_{i,i + 1}} + R_{WB}^i\left(\Delta {P_{i,i + 1}}{\text{ + }}J_{\Delta p}^gb_g^i + J_{\Delta p}^ab_a^i\right) \hfill \end{split}} \right. $

(2)

其中, $J_{( \cdot )}^a,J_{( \cdot )}^g$ 分别是加速度计和陀螺仪误差的一阶雅可比矩阵^[19]. 上述预积分计算以及雅可比矩阵的计算都可在IMU数据输入时计算完成. 通过相机图像信息和运动传感器数据求解出下相机在世界坐标系下两关联对应关键帧的位姿矩阵 ${{T}} = [{{R}}|{{p}}]$ .

基于已实现的实时ORB-SLAM系统, 可运行在大尺度场景中, 重建局部地图以便跟踪与映射, 并可构建真实环境的点云地图. 本文方法可实现在预设帧率下的传感器姿态、速度以及IMU误差的位姿跟踪计算代替原始单目跟踪系统. 根据初始的ORB-SLAM系统, 预计算得到相机位姿之后, 局部地图中的地图点可投影与对应帧图像中的关键点相匹配. 相较于之前关键帧 $i$ , 通过最小化特征点误差投影和IMU误差来优化当前关键帧 $j$ , 故提出优化目标定义如下:

$ \left\{ {\begin{split} & \varepsilon = \left\{ {R_{WB}^j,P_{WB}^j,v_{WB}^j,b_g^j,b_a^j} \right\} \hfill \\ & {\varepsilon ^*} = \mathop {\arg \min }\limits_\varepsilon \left(\lambda \sum\limits_k {{E_{\rm{proj}}}(k,j) + \mu {E_{\rm{IMU}}}(i,j)} \right) \hfill \end{split} } \right.$

(3)

其中, $\lambda ,\mu $ 为误差权重系数且 $\lambda + \mu = 1$ , 更加置信IMU数据, 取权重 $\;\mu = 0.6$ .

而基于ORB特征点的重投影误差函数 ${E_{\rm{proj}}}$ 定义如下:

$ \left\{ {\begin{split} & {E_{\rm{proj}}}(k,j) = \rho \left({({x^k} - \pi (X_C^k))^{\rm{T}}}{\Sigma _k}\left({x^k} - \pi \left(X_C^k\right)\right)\right) \hfill \\ & X_C^k = {R_{CB}}R_{BW}^j\left(X_W^k - P_{WB}^j\right) + {P_{CB}} \hfill \end{split}} \right. $

(4)

其中, ${x^k}$ 为特征点图像坐标, $X_W^k$ 为世界坐标系下的地图点坐标, ${\Sigma _k}$ 为关键点的尺度信息矩阵. 而IMU运动轨迹误差函数定义如下:

$\left\{ { \begin{split} & {E_{\rm{IMU}}}(i,j) = \rho \left(\left[e_R^{\rm{T}}e_v^{\rm{T}}e_p^{\rm{T}}\right]{\Sigma _I}{\left[e_R^{\rm{T}}e_v^{\rm{T}}e_p^{\rm{T}}\right]^{\rm{T}}}\right) + \rho \left(e_b^{\rm{T}}{\Sigma _R}{e_b}\right) \hfill \\ & {e_R} = \ln\left({\left(\Delta {R_{ij}}{\rm{exp}}\left(J_{\Delta R}^gb_g^j\right)\right)^{\rm{T}}}R_{BW}^iR_{WB}^j\right) \hfill \\ & {e_v} = R_{BW}^i\left(v_{WB}^j - v_{WB}^i\right) - \left(\Delta {v_{ij}} + J_{\Delta v}^gb_g^j + J_{\Delta v}^ab_a^j\right) \hfill \\ & {e_p} = R_{BW}^i\left(P_{WB}^j - P_{WB}^i - v_{WB}^i\Delta {t_{ij}}\right) - \left(\Delta {p_{ij}} + J_{\Delta p}^gb_g^j + J_{\Delta p}^ab_a^j\right) \hfill \\ & {e_b} = {b^j} - {b^i} \hfill \end{split} } \right.$

(5)

其中, ${\Sigma _I}$ 是预积分信息矩阵, ${\Sigma _R}$ 是IMU误差随机参数, $\rho $ 是Huber损失函数, 用于表示稳健回归损失^[20]. 本文为解决以上优化问题, 采用高斯-牛顿法进行优化求解计算^[21].

当第 $i$ 帧关键帧与当前关键帧 $j$ 优化计算完成后, 采用连续两关键帧IMU数据与前一帧关键帧进行联合优化. 当SLAM系统通过图像数据计算产生第 $j + 1$ 帧关键帧时, 保存当前优化结果, 舍弃第 $i$ 帧, 通过累积优化计算当前两关键帧, 重复上述算法流程, 直至整个系统流程结束.

紧耦合的实时位姿优化方法整体流程如下: 1) 预处理采用张正友标定法进行相机标定; 2) 采用相机图像数据与惯性传感器数据作为实时输入, 对图像信息进行特征提取、匹配; 3) 根据对极几何约束求解出初始相机位姿, 并预积分计算两帧间的惯性运动数据; 4) 构建联合状态变量, 最小化加权重投影误差与惯性运动误差求解出相机位姿信息并保存; 5) 实时输入新的图像数据与惯性运动数据进行迭代算法求解.

3 实验分析

为了更好的衡量评价本文提出的移动端位姿优化方法, 采用EuROC数据集^[22]进行系统评估与误差分析. EuRoC数据集使用MAV采集了两个不同房间及一个工业环境, 总共11组数据, 并根据照明、纹理、运动快慢以及模糊程度分为简单、中等、困难3等级数据组. 此数据集的良好特性使得可以很好的检验评估类似惯性视觉的位姿轨迹系统, 数据测试时仅采用单目图像(左目)作为视觉图像流输入, IMU数据流输入以及真实轨迹参照.

在计算完成位姿轨迹后, 采用位姿轨迹文件TUM导出, 采用evo轨迹评测对重建位姿进行误差评价. 假设真实的运动轨迹为T, 预估轨迹为 ${T_e}$ . 采用TUM轨迹文件格式, 由时间戳、三维空间位置以及四元数表示的旋转变量. 假定真实轨迹与重建轨迹一一对应, 那么针对任意第 $i$ 个位姿, 那么将轨迹误差定义为:

$ {e_i} = {\left\| {\ln {{(T_i^{ - 1}{T_{ei}})}^V}} \right\|_2} $

(6)

则根据两条轨迹的均方根(root mean square error, RMSE)误差为:

$ {\textit{RMSE}}(e) = \sqrt {\frac{1}{n}\sum\limits_{i = 1}^n {e_i^2} } $

(7)

为确定重投影误差和IMU误差的最佳权重系数, 针对 $\lambda ,\mu $ 估值, 采用同一数据集, 采用不同的权重系数进行重建实验, 评测权重系数的有效性与稳定性. 采用顺序搜索遍历, $\lambda $ 分别取 ${\text{0}}{\text{.1, 0}}{\text{.2, 0}}{\text{.3, }} \cdots {\text{, 0}}{\text{.9,}}$ 则 $\;\mu $ 取 $0.9,\;0.8,\;0.7,\; \cdots ,\;0.1$ , 得到RMSE和SSE结果如图1、图2所示.

由图1可知, 当 $\lambda $ 取值为 $0.4,\;0.3,\;0.2$ 时, 重建轨迹位姿与真实轨迹位姿的均方根误差最小. 结合图2误差平方和SSE可得, 当 $\lambda = 0.4,\;\mu = 0.6$ 时, SSE值最优, 故选择权重系数为 $\lambda = 0.4,\;\mu = 0.6$ .

从EuRoC数据集中选取了6组对比实验数据, 评估位姿优化系统的精确性与稳定性, 通过计算RMSE(轨迹误差), 不同方法计算结果如表1所示.

从表1可见, 在不同数据集上, 采用视觉信息与IMU数据同时约束方法的轨迹重建方法较仅采用纯视觉方法的ORB-SLAM2获取的轨迹效果均好. 香港科技大学提出的VINS-Mono视觉惯性里程计, 对相机轨迹的重建的效果较ORB-SLAM方法较好.

而本文提出的位姿优化算法在EuRoC的6个数据集上的表现整体偏好, 除MH_01_easy数据库集误差相差0.115之外, 其他数据集表现均优于其他方法, 轨迹误差更小, 更加接近真实的轨迹位姿, 取得较好的相机轨迹效果.

通过上述分析可见, 本文提出的算法在相机轨迹重建效果较好, 提供了一种新的位姿优化方式.

由于ORB-SLAM2并没有采用IMU数据处理, 仅采用纯视觉方式进行相机位姿重建, 缺少一个尺度因子, 故评测绝对误差时不与考虑.

对3种不同的重建方法采用EVO评测轨迹的真实误差, 效果如表2.

将不同方法的重建相机位轨迹与数据集的真实轨迹作比较, 验证重建相机轨迹的重建效果.

结合表1, 根据表2可知, 3种方法在重建轨迹误差相差不大的情况下, 展现出不同的轨迹误差分布. 从表2中的轨迹图可知, ORB-SLAM3虽然均方根误差较大, 轨迹误差分布均匀, 说明对轨迹处理的综合性能较好, 可处理更多种的轨迹情况, 鲁棒性更好. 而VINS-Mono与本文方法呈现两种不同的误差浮动表现. 本文方法在相机移动位移幅度较大时, 误差显著浮动提升, 归其原因在位移较大时, IMU数据积分误差较大, 而优化方法给予更高的权重比, 不能很好的优化计算大位移时的位姿轨迹. 基于VINS-Mono初始化算法, 采用松耦合的传感器融合方法, 在轨迹重建的初始阶段对相机位姿轨迹的重建效果较差, 相机轨迹误差较大.

4 结论与展望

本文提出了一种可实时运行在移动端的紧耦合惯性视觉位姿优化方法. 可根据实时相机图像数据与惯性运动数据计算载体在自身所构建地图中的位置, 并可获得优化后的相机位姿信息, 为后续三维重建工作提供可靠相机姿态信息, 更好地为三维重建服务.

实验证明, 本文提出位姿优化方法, 相较于ORB-SLAM3基于IMU最大后验估计的SLAM系统和VINS-Mono视觉惯性里程计, 在相同数据集下, 具有更高的尺度还原性以及轨迹准确性.

该位姿优化方法也存在不足, 在位移较大时, IMU数据不能很好的积分优化, 导致局部轨迹误差较大. 这需要探索更好的IMU数据模型, 以期修正较长时间下大位移的IMU数据误差问题, 达到更好的相机轨迹还原. 本方法并过于依赖ORB-SLAM系统进行初始化, 初始化时间较ORB-SLAM2系统略长.

未来工作预期利用移动端的多相机镜头信息(类似双目)以及景深镜头信息, 加以融合求解, 优化IMU数据模型. 以期通过更加精确的传感器与视觉信息数据模型完成初始化, 紧耦合优化计算, 提高整个系统精确性与稳定性.