视觉惯导SLAM 初始化算法研究

2021-08-07葛洪伟

计算机与生活 2021年8期

刘刚，葛洪伟+

1.江苏省模式识别与计算智能工程实验室（江南大学），江苏无锡 214122

2.江南大学人工智能与计算机学院，江苏无锡 214122

同时定位与地图构建（simultaneous localization and mapping，SLAM）研究工作最早可以追溯到1986年，至今已有30 多年历史。SLAM 是机器人与计算机视觉领域中的基本问题，它主要研究装置通过各种传感器在未知环境中感知和定位自身方位并同时构建环境三维地图。使用单目相机来进行运动状态的评估的方法由于其体积小、成本低和硬件设置简单而获得了社会的极大兴趣[1-5]。然而，仅仅使用单目视觉系统无法恢复度量尺度，这样也限制了它们在实际工程中的应用。随着智能手机等消费电子的迅猛发展，惯性测量单元（inertial measurement unit，IMU）和相机模组的迅速普及，用低成本惯性测量单元辅助视觉系统进行状态评估已成为相关产业的发展趋势。

近年来很多优秀单目视觉惯导SLAM 系统被提出，比如MSCKF[2]、视觉惯导ORB-SLAM[3]、Vins-Mono[4]以及苏黎世大学Leutenegger 等人提出的OKVIS[5]（open keyframe based visual inertial SLAM）等。根据上述系统的求解方法，可以划分为基于滤波[6]的紧耦合和基于优化[7]的紧耦合。与滤波法[7-10]相比，基于优化的紧耦合方法对一段时间内所有时刻的所有状态同时进行优化，有其较高的精度和计算效率[11]，从而引起了学术界的极大的兴趣。但是这些基于优化的单目视觉惯导融合的SLAM 算法有高度的非线性性，其性能严重依赖于初始状态（视觉尺度、惯性传感器偏置等）估计的准确度[12-15]。若初始状态不准确，其误差将随着系统运行时间的增加而不断积累，大大降低系统的收敛速度甚至导致不正确的估计结果。另一方面，惯性测量单元需要加速度激励，以便于进行相关测量。这意味着IMU 不能从静止状态启动，必须从未知的移动状态启动。因此如何选择这一未知的移动状态在初始化阶段也显得尤为重要。

不少学者对视觉惯导的SLAM 的初始化方法进行了深入研究；在文献[16]中提出一种确定性的解析解来估计重力加速度和视觉尺度的初始化算法，但是该算法由于忽略了IMU 偏置导致评估精度并不高。文献[17]中作者假定了微小型飞行器的初始状态处于近似水平的环境下，进行松耦合的系统初始化。类似的初始化方法在文献[18]中也被提出，作者假设重力加速度向量与飞行器坐标系进行了对齐，在这条件下进行初始化。然而以上这两种方法，在没有先验条件下，飞行器并不能真正实现在线初始化。此外，在文献[19-20]中提出的视觉惯导初始化方法，由于陀螺仪偏置在系统初始化时被忽略，导致初始状态不能被准确估计。港科大Qin 等人提出了一种较有开创性的视觉惯导初始化算法[4,21]，并在其贡献的开源算法Vins-Mono 中实现。该算法可以在运行中对惯性测量单元的各初始状态进行标定，但是该初始化方法忽略了惯性测量单元加速度偏置误差和测量噪声，从而影响了系统初始化的准确度和运行效率。Mur-Artal 等人提出了一个较为高效的视觉惯导初始化方法[22]，将初始化过程细化为多个步骤进行，但是由于分步进行求解各初始状态，并未充分考虑误差状态的传播对估计结果的影响，且没有设定鲁棒的终止条件来判断初始化是否成功。Forster等人提出了惯性测量单元在流形面的预积分方法[23]，该方法是对IMU 观测值进行数值积分的理论基础，但是其并未对误差状态的传播机理进行详细分析。

本文在预积分理论基础上利用最大后验估计（maximum a posteriori estimation，MAP）推导出了一个线性时变初始化系统来对尺度、速度、位移、姿态、加速度、惯性测量单元偏置等初始运动状态进行联合估计。这种统一求解的方法，综合考虑了系统误差状态的传播对初始化系统的影响，以及重力加速度的约束这两个因素，因此可以对重力加速度方向和加速度偏置进行精确求解。此外，本文提出了一种判别视觉惯导初始化算法终止条件的方法，通过提取观测数据在估计方程上的费歇尔信息矩阵（Fisher information matrix）[24]来评估估计的状态准确度，从而加快了算法初始化的收敛时间，提高了初始化的鲁棒性。

1 Vins-Mono 初始化方法及其缺陷

Vins-Mono 初始化算法是一种松耦合传感器融合的初始化方法。它首先使用视觉运动中恢复结构（structure from motion，SFM）算法获得比较有价值的初始值，与IMU 的预积分结果构建线性方程进行分步求解，得出尺度、重力加速度、速度以及IMU 偏置的初始值，其初始化流程如图1 所示。

Vins-Mono 初始化算法存在以下缺陷：首先，它忽略了IMU 的加速度偏置对系统的影响，此举必然会影响算法求解的准确度；其次，它采用逐步求解的方法，因而无法综合考虑IMU 的观测噪声和偏置噪声导致的误差状态对系统的影响；再次，由于其没有考虑算法的终止条件，仅仅简单地利用初始阶段中某一段时间的数据进行初始化，严重影响了算法的收敛速度和鲁棒性。

2 惯性测量单元模型

IMU 测量的是离散的比力和角速度，其测量值含有随机游走且缓慢变化的偏置，以及高斯分布的测量噪声，因此想要得到每个时刻传感器的位置、速度、姿态等状态值，首先需要知道传感器的初始时的位置、速度、姿态等信息，然后对传感器的测量值进行数值积分。一般情况下惯性测量单元的评估状态可以表示为：

Fig.1 Vins-Mono initialization flow图1 Vins-Mono 初始化流程

式中，p∈R3表示IMU的位移，四元数q∈ℍ 表示IMU相对于世界坐标系的旋转，表示IMU 的运动速度，bg∈R3表示IMU 的陀螺仪偏置，ba∈R3表示IMU 的加速度计偏置。

2.1 惯性传感器测量模型

IMU 的测量值是传感器视加速度和角速度的离散值，通常这些测量值会受到传感器随时间缓慢变化的偏置和测量白噪声的影响，因此其测量值由实际状态、随机高斯噪声和偏置三部分叠加而成，可以用式（2）建模。

式中，(·)m表示在t时刻点IMU 的原始测量值，(·)t表示在t时刻传感器的实际状态。R表示IMU 坐标系相对于世界坐标系的位姿（旋转矩阵）假设测量的加速度和角速度的噪声服从高斯分布有，。由于加速度偏置ba和角速度偏置bg扰动是随机游走的，因而相应的导数也服从高斯分布,可以表示为。

2.2 惯性传感器运动学模型

IMU 的测量数据视加速度am与角速度ωm均相对于IMU 坐标系，需要将其转换到世界坐标系中，定义表示四元数q与旋转矩阵R的对应关系。根据IMU 的测量模型（2）和偏置随时间游走的噪声模型，利用IMU 测量数据和积分动力学模型[25]来描述IMU 的位姿与速度。在式（3）中定义u≔(am,ωm) 是IMU 在某一时刻点测量值的集合，运算符⊗表示四元数乘法。n≔[na,ng,nbg,nba]T用来表示IMU 测量噪声和偏置噪声的集合，其协方差一般会在IMU 的数据手册中给出或者通过相关实验进行标定。

2.3 预积分

预积分的思想在文献[26]中首次被提出，Forster等人将其进一步拓展到李群流形上，形成了一套优雅的理论体系[23]。本文使用了基于流形的IMU 预积分思想对IMU 数据进行处理。通常情况下，如图2 所示IMU 测量数据的输出频率远高于相机等视觉传感器。当视觉与惯导信息进行融合时，首先需要考虑两者测量频率不同步的问题。IMU 的预积分是指预先对两个图像间的IMU 观测数据进行积分，此方法能极大地降低IMU 的输出频率，且有效地解决视觉和IMU 惯导两种传感器频率不一致的问题，同时也能获得相邻视觉帧之间的相对位置。

Fig.2 IMU and visual observation model图2 IMU 与视觉观测模型

2.3.1 标准状态传播

假设在上述运动解析方程（3）中忽略噪声对系统的影响得到标准状态的IMU 运动方程。

将以上运动解析方程（4）进行离散化，i时刻点到j时刻点的状态转移方程F(∙,ui:j)一般由状态变量运算和积分项构成[25]。

式（6）中Δt表示第i时刻点到第j时刻点之间的时间差，αij、βij、γij表示i时刻点到j时刻点的积分项（7）。可以利用解析方程推导出不同精度的数值计算方法，例如欧拉法、中值法和龙格-库塔法等方法进行积分。

2.3.2 误差状态的运动模型

虽然误差状态相对于标准状态是比较细微的，但是随着标准状态模型的运行，误差会随时间积累，变得越来越大。虽然惯性测量单元的观测噪声和扰动是随机发生的，但是它们的统计学特征是可估计的。因此为了计算出精确的实际状态，本文定义标准状态与实际状态之差为误差状态。一方面，在不考虑系统噪声和扰动的情况下利用标准状态传播模型计算系统的标准状态；另一方面，通过对误差状态随时间的传播进行分析，推导出误差状态的统计学特征，为进行最大后验估算做准备，将误差状态δx定义为：

将系统的误差状态视为高斯分布，根据运动方程和误差状态方程可以得到误差状态的传播方程。不难发现惯性传感器的状态误差传播可以定义为时变线性系统。通过测量值、标准状态和系统噪声来估计误差状态传播。由运动方程（3）和误差方程（8）得到如下误差状态运动模型：

其中：

在不考虑噪声影响下，根据误差状态传播方程（9）得到误差状态转移方程。

由误差状态的转移方程（10）得到误差状态δx的递归形式的雅可比矩阵，表示为Ji+1=Φi,i+1Ji，其相应的递归的初始状态J0=I。另外，也可推导出在系统噪声影响下，误差状态的协方差矩阵的传播方程，表示为如下形式：

这里Qi表示噪声的协方差矩阵，其为对角阵，定义为。

系统的实际状态是由标准状态叠加上相应的误差状态，根据系统的标准状态转移方程（5）和误差的定义（8）可推导出如下表达式：

式（12）中广义加法⊕是⊖的逆运算，定义如下：

由误差状态运动模型（9）得到Φ=eAΔt。由于IMU 的采样时间间隔较短，可对其进行一阶近似，得出Φ≈I+AΔt。在一些运算资源比较充沛的系统中，可以根据解析解进行更高阶的泰勒近似，或者通过龙格-库塔数值积分等方法得到更为精确的近似。

3 视觉惯导联合初始化

进行视觉惯导联合初始化，首先需要执行视觉部分的初始化，以获得良好的初始值，然后进行惯性测量单元的初始化。这种延迟初始化的方法有两个好处：（1）IMU 能够进行充分运动让所有的变量可观测；（2）在纯视觉SLAM 的姿态评估后，系统进行局部图优化，能够优化关键帧的位姿状态，这样就可以利用优化后的关键帧位姿来初始化IMU。一般情况下，纯视觉的初始化在进行局部图优化后，可以获得性能良好的相机初始姿态，这也为下一步进行惯性测量单元的初始化打下了坚实的基础。在完成纯视觉的初始化后，得到相机各关键帧的姿态，通过相机与惯性测量单元之间的标定可以进行相机坐标与惯性测量单元坐标之间的相互转换。联合惯性测量单元的预积分，执行视觉惯导的联合初始化，以获得精准的系统初始参数。

3.1 惯性测量残差模型

已知惯性测量单元的运动状态Xj及在时间点i与j之间的测量集合为ui:j。可以通过标准状态转移方程（5）、误差的定义（8）和误差状态的传播模型（9），在忽略IMU 轴偏差和尺度误差的情形下，将标准状态和实际状态之间的误差近似为高斯白噪声，表示如下：

协方差矩阵Pj可以由传播方程（11）求得。因此将预测状态与标准状态之间的残差表示如下：

根据标准状态转移方程（6）求得j时刻点的状态，另外利用误差状态转移方程（10）求得其相应的误差状态转移的雅可比矩阵。由于惯性测量单元的加速度偏置和旋转偏置是随时间缓慢变化的，且第i到j时间点之间的采样时间Δt比较短，因此可以对其进行一阶近似，以估算出因偏置所产生的误差。得到如下惯性测量单元运动的残差方程：

3.2 视觉状态评估

目前视觉部分初始化的方法较为成熟，一般流程是通过筛选关键帧后，使用对极几何、PnP（perspective-n-point）、ICP（iterative closest point）等SFM 算法进行视觉帧的位姿评估，然后利用局部图优化对关键帧的位姿进行进一步修正，得到更精确的位姿数据。该部分延用了Vins-Mono 的相关视觉初始化算法，在得到较为精确的关键帧位姿数据后，将其用于视觉惯导联合初始化算法。假定视觉初始化后的输出第i关键帧的位姿值记为，由于单目视觉尺度信息的不可观测性，位姿值没有包含相应的尺度信息，因此需要将其转化为IMU 估计模型下的位姿值(qi,pi)，则有式（17）：

其中，i=0,1,…,N,s∈R 表示视觉尺度。

3.3 视觉惯性对齐

假设相邻两个关键帧的纯视觉评估状态分别为Xi、Xi+1，将视觉评估结果式（17）代入到残差方程（16），发现惯性测量单元的待初始化状态变量s、g、v、bg、ba在方程（18）中是线性的，这样就变为已知载体的姿态状态[qi,pi]来求解r(Z)的最小值的MAP问题，优化方程表示如下：

其中，Z=[s,g,v,bg,ba]T，由于r(Z)是线性方程可以直接求解相关状态变量，然而式（18）没有考虑到||g||=9.8 的约束，很容易得到是病态的收敛结果。因此必须加上重力加速度的约束，可以得到如下优化方程。

不难看出新的优化方程是典型的二次约束二次规划问题，这样就变成一个凸函数最优化问题。因为凸函数最优化问题的局部解即是其全局解，理论上可以通过优化方程（19）求解出视觉惯导在当前观测数据下初始状态的最优的解。对于方程（19）可以将其转化为在黎曼流形上非约束的优化问题进行求解。

其中，回缩因子如下：

其中，ζ=[ζs,ζθ,ζv,ζbg,ζba]T，以上回缩因子可以确保其流形面上重力加速度g的强度保持不变。

IMU 是典型的内感受型传感器，需要运动进行激励以便进行观测。这意味着不能从静止状态来进行初始化，需要惯性测量单元有充分的运动，以便充分获取惯性测量单元的测量数据的统计信息。因此确保观测数据是由载体充分运动所产生的，是算法鲁棒性的重要前提条件。在统计学里，克拉美罗下界（Cramer-Rao lower bound，CRLB）设定了一个基本界限，它明确定义出利用已观测值的估计效果来衡量估计方式好坏的标准，对于无偏估计其定义的边界可以由费歇尔信息矩阵来确定[24]。因此可以用费歇尔信息矩阵来度量观测数据估计的未知状态的置信度，一般在正态分布的负对数似然估计中，费歇尔信息矩阵即是其观测值处的海森矩阵。这种关于样本选择的最差情形误差评估的方法在文献[27]和文献[28]中也有相关论述和实验。在视觉惯导初始化系统中，通过计算r(Z)的海森矩阵来进行最差情形的误差评估，从而加快初始化的收敛速度，确保初始化的状态变量的鲁棒性。将观测样本最差情形评估函数定义如下：

4 效果评估

使用Euroc 数据集[29]来测试本文算法，其包含两个场景，分别是工厂车间和卧室环境，利用无人机采集了11 个数据集，并按照光照、纹理、快速/慢速运动、运动模糊等分为容易、中等、困难3 个难度等级。Euroc 数据集包含同步的752×480 双目图像和IMU数据，频率分别为20 Hz、200 Hz，并提供相机运动ground-truth 轨迹。实验硬件平台Core i7-6700HQ 2.6 GHz、8 GB RAM。

为了评估本文提出的视觉惯导初始化算法，在Vins-Mono 基础上添加了该初始化算法，并在其视觉图优化窗口对视觉关键帧评估然后执行优化算法。使用Ceres[30]来实现牛顿高斯法在黎曼流形面上的迭代搜索，使算法快速收敛。经多次实验初始化系统对估计的状态变量实现收敛的时间为8～11 s。

图3～图6 为本文所提视觉惯导初始化算法与Vins-Mono 初始化算法在Euroc Machine Hall 03 数据集上测试对比结果。可以看出，陀螺仪偏置在约2～4 s收敛，其x、y、z3 个方向上最终收敛于[-0.003,0.125,0.058]，单位rad/s，陀螺仪偏置比加速度偏置收敛快。这是由于陀螺仪偏置相较于加速度偏置不需要区分重力加速度，而加速度偏置因为有重力加速度的影响，所以相较于陀螺仪偏置收敛较慢。这也是引入最差情形评估函数时，将重力加速度和尺度相关费歇尔信息作为最差情形评估参数的原因。实验中根据惯性传感器特性，将加速度偏置初始值设为0，将其值域设置在[-0.5,0.5]，单位m/s2，这样大大缩小了加速度偏置的迭代范围，缩短了收敛时间。