基于磁航向约束的视觉惯性定位算法

李磊磊，梁 琳，陈家斌，韩勇强

（北京理工大学 自动化学院，北京 100081）

自主导航作为无人平台核心技术，多年来一直备受关注。伴随着软硬件平台性能的提升，同步定位与构图（Simultaneous Localization and Mapping, SLAM）技术从理论走向应用成为可能[1-3]。常用的SLAM有：激光SLAM、视觉SLAM和视觉惯性SLAM。激光SLAM凭借其高精度和避障能力在SLAM发展初期处于优势地位[4]，但其尺寸、重量与成本限制了其推广应用。视觉SLAM结构简单、成本低、实现方便。但存在很多不足。比如普遍存在高动态时丢失目标特征点情况，性能不稳定；无法区分是自身的移动，还是背景的移动；在弱纹理场景性能表现不佳等。视觉和惯性具有天然的互补性，相机能够捕捉场景下丰富的信息，而IMU能够减轻高动态对相机的影响，静态时相机数据也能有效地修正IMU漂移，抑制误差发散。于是，视觉惯性SLAM成为研究热点。惯性和视觉信息的融合使得俯仰角和横滚角可观测，但偏航角仍无法观测，导航系统缺失绝对航向信息，并且在长时间的导航过程中，相对航向也会严重发散，致使系统的导航精度下降，甚至导航失败。因此，高精度的偏航角估计在提高视觉惯性SLAM系统导航整体精度方面起着重要作用。

QIN等人[5]于2017年提出了单目视觉惯性状态估计器（Monocular Visual-Inertial State Estimator,VINS-Mono)，通过单目视觉与IMU惯性测量单元耦合在初始化中估计重力方向，为系统提供姿态约束，采用紧耦合的方式构建局部和全局优化函数，实现了较为精确的定位导航。QIN等人于2019年提出了VINS-Fusion[6]，为双目视觉、IMU、GPS以及其他传感器融合定位导航提供了理论依据和开源方案。程传奇等[7]构建了基于非线性优化的单目视觉惯性紧耦合模型，保证了导航的局部精度及全局一致性。Wang等人[8]提出了一种基于先验地磁图的定位方法，利用地磁场在不同室内区域具有的不同特征，通过实时获取地磁序列来修正姿态漂移，所需传感器成本低，但只适用于室内环境，较为局限。

为使视觉惯性SLAM系统获得准确的航向信息，通常采用多传感器融合的方式来辅助定位导航。地磁传感器一直被用在导航领域，通过测量地球磁场信息来获取磁航向，借助地球磁场分布规律完成磁偏角修正，能够得到真实航向信息[9]。Dissanayake等人[10,11]提出了融合地磁信息的卡尔曼滤波算法，该算法解决了惯性传感器信号积分导致速度和位置信息漂移的问题。郑威等人[12]提出了用磁力传感器校正航向误差的方法，有效抑制惯性导航过程中航向角的发散。2018年有学者提出在低成本IMU上结合三轴磁力计，可实现对航向角偏差的补偿[13]。

本文将地磁传感器引入视觉惯性SLAM系统中，从理论上建立了磁航约束的数学模型，利用地磁场来给定初始航向并抑制偏航角的漂移，详细解释了与初始化有关的最小二乘问题，融合地磁传感器的视觉惯性初始化方案以及闭环优化方法提高了系统的航向精度和定位能力。实验证明，该方法在导航过程中能有效抑制航向误差发散，增强系统稳定性，高效提高定位导航精度。

1 系统框架

本文采用非线性优化方法实现视觉、惯性和地磁信息的融合，系统整体结构如图1。通过系统初始化获取陀螺仪漂移、重力向量、相机到IMU的外参、尺度因子、初始速度以及真实航向信息，完成视觉/惯性/地磁传感器对齐。利用视觉重投影误差、IMU预积分残差、磁力计测量残差，构建非线性优化代价函数，计算各项因子的雅克比矩阵，进行基于光束平差法（Bundle Adjustment, BA）的位姿估计与非线性优化，当检测到回环信息时，则融合先前的关键帧信息进行全局优化。

图1 系统整体结构Fig.1 Overall system structure

2 视觉/惯性/地磁导航系统模型

在介绍模型之前，首先对坐标系的表示方式进行统一，本文用（.）w表示世界坐标系，（.）n表示导航坐标系，也即东北天地理坐标系，（.）b表示载体坐标系，（.）c表示相机坐标系，（.）m表示地磁传感器坐标系。

2.1 视觉模型

视觉信息是通过工业相机获取，将三维空间的坐标点投影到二维像素平面。描述投影过程可以采用不同的模型，其中针孔模型简单有效。在针孔相机投影模型下，将3D点投影到Z归一化的平面上，然后使用相机固有的投影参数投影到图像平面上：

其中，(u,v)表示投影到平面上点的坐标，K表示相机内参，P表示点在三维空间内的坐标。在大多数情况下，坐标在经过缩放和位移以形成最终像素坐标之前，会在归一化平面上经历径向和切向畸变。径向畸变主要是由透镜形状引起；切向畸变主要是由相机组装时透镜和成像面无法严格平行导致。畸变系数的值和数量根据相机镜头的不同而各不相同，本文采用的镜头需用4个畸变系数进行补偿。

其中(xdis,ydis)为畸变后的坐标，(x,y)为畸变前的坐标点，k1、k2、p1、p2为畸变参数，r为该点距离成像面中心的距离。

在非线性优化中需要的目标函数相对各个待优化变量的雅各比矩阵，在下面列出。式(3)为像素坐标相对于畸变后的像素坐标点的雅各比矩阵；式(4)为畸变后的像素坐标点相对于畸变前归一化平面上的点的雅各比矩阵。式(5)为归一化平面坐标点相对于3D坐标的雅各比矩阵。

其中，然后通过式(6)来获得整个雅可比矩阵：

2.2 惯性模型

惯性数据从IMU获取，具体为：

其中，上标g表示陀螺仪，a表示加速度计，w表示世界坐标系world，b表示IMU机体坐标系，ω和a分别为陀螺仪和加速度计的真实值，ω～和a～为测量值，n表示高斯白噪声，b表示传感器的漂移，g表示重力向量，qbw为世界坐标系相对于IMU本体坐标系的旋转。

位置P、速度V、旋转Q对时间t的导数为：

对于基于非线性优化的导航系统来说，初始姿态是未知的，并且每次更新系统状态量后都需要对相应的雅克比矩阵重新线性化，为了减少计算量，算法引入IMU预积分，其主要优点在于它所代表的是某一段很短时间内惯性数据的积分值，当某一段预积分的起始姿态发生改变后，只需要将整段积分值进行相应的操作，而不必进行重新积分。根据式(8)，可由i时刻的PVQ通过积分得到j时刻的PVQ，如式(9)。

本文采用中值积分法，离散的积分公式为：

其中，

将一段时间内IMU构建的预积分量作为测量值，对两时刻之间的状态量进行约束。IMU积分误差表示为：

其中，

式(12)(13)中，Δt是间隔时间，下标i、j是IMU的帧数，[∙]xyz表示只取四元数的虚部(x,y,z)组成的三维向量，αbibj、βbibj、qbibj表示一段时间的IMU预积分量。IMU误差更新只需求出误差对于优化变量的雅克比矩阵即可，篇幅所限，本文在此不作详细推导。

2.3 磁力计模型

地球本身是一个大磁体，磁感线方向从南极到北极。磁场南北两极处，磁场垂直于水平面，靠近地球赤道，磁场方向平行于水平面。地球上某一点的磁场可以用一个矢量表示，该矢量的大小和方向分别为该点磁场强度和磁场方向。磁场信息可以通过磁力计获取，磁力计的校准模型为：

其中，[M xMyMz]T是磁力计的原始读数，它是系数矩阵和偏离传感器主体框架[MxtMytMzt]T上真实磁投影的偏差矢量。kxx、kyy、kzz是比例因子系数；kxy、kyz、kzx是磁力计的轴非正交引起的横向系数；bmx、bmy、bmz是由局部磁场产生的偏差的校正系数。

图2表示总密度电磁场的矢量及其在地磁和地理坐标轴上的投影。

图2 地磁与地理坐标轴Fig.2 The geomagnetic and geographic coordinate frames

其中，T表示总密度电磁场矢量，X-Y-Z表示地球地理坐标系，I表示磁倾角，D表示磁偏角。T、I、D可以通过查阅全球地磁图来确定。磁力计的观测模型可表示为：

其中，MM是磁场强度，RMB是从载体坐标系到磁力计坐标系的相对旋转矩阵，该旋转矩阵通过校准确定，HW是地磁水平分量，指向磁北。

3 磁约束的位姿估计方法

3.1 初始化

视觉惯性SLAM的初始化至关重要，直接影响导航状态系统稳定运行。本系统的创新之处在于在视觉恢复过程中使用磁信息作为旋转约束，加快了系统初始化过程，提高了视觉恢复的精度。初始化工作主要是实现视觉/惯性/地磁数据对齐，其流程如图3所示。

图3 初始化流程图Fig.3 The flowchart of initialization

首先进行纯视觉初始化，即运动结构恢复（Structure From Motion, SFM），从视觉信息中恢复出相机帧间的相对运动。然后利用由SFM恢复出来的帧间相对运动与惯性信息对齐，可以估算陀螺仪的偏置bg，相应地更新IMU的预积分量。最后利用三角化、多点透视成像（Perspective-n-Point, PnP）等方法得到重力方向、速度、尺度因子。

本文在初始化过程中使用地磁信息对旋转量进行校正，利用RTK差分GPS可以测得经度、纬度、海拔高度。进而得到当地的磁场强度Hn在导航坐标系下的三轴分量，磁力计坐标系上的三轴磁分量Hm，并可由标定得到IMU与磁力计的旋转矩阵。

3.2 非线性优化

本系统采用基于滑动窗口的优化方法，系统状态向量定义为：

其中，

定义目标函数中包含三项因子，为所有测量残差的马氏范数Mahalanobis Norm之和，得到最大后验概率估计如式(20)所示。

鲁棒核函数的定义为：

目标函数的整体增量方程为：

对于磁力计测量因子，取滑动窗口相邻两帧的地磁数据则有：

其中，Mmk表示第k帧的磁力计测量数据，表示第k帧在导航坐标系下的姿态矩阵，表示载体坐标系与磁坐标系的姿态变换矩阵，Hn表示当地磁场强度。在原有视觉惯性最小二乘问题中加入关于地磁信息的约束条件：

其中，（·）^表示反对称矩阵，根据视觉因子和IMU因子的雅克比矩阵以及式(25)可求解式(20)的目标函数，实现对式(19)中状态量的估计。

4 实验验证

4.1 实验平台

为了验证基于磁航向约束的视觉惯性导航定位算法，搭建了车载实验平台。工业相机选用FLIR公司的Grasshopper3，图像输出帧率为20 Hz，分辨率为960×600；IMU选用SENSONOR公司的STIM300，数据输出频率为200 Hz；磁力计选用纳特斯拉公司的DMC-NT 10C10，数据输出频率为10 Hz；数据处理平台选用Intel NUC7，处理器为Intel R CoreTM i7-7567U，内存16G。实验采集装置如图4，为了减少车体对磁力计的干扰，将实验设备架高60 cm，如图5。

图5 车载实验平台Fig.5 Vehicle experiment platform

4.2 车载实验

实验在北京理工大学中关村校区校园内进行，以高精度差分GPS作为基准，通过两组实验分析对比VINS-mono与本文所提出的基于磁约束VI-SLAM的导航精度。

图7 VINS-mono绝对位置误差(实验一)Fig.7 Absolute position error of VINS-mono (first experiment)

图8 磁约束VI-SLAM绝对位置误差(实验一)Fig.8 Absolute position error of VI-SLAM based on magnetic constraint (first experiment)

图9 实验轨迹图(实验二)Fig.9 The trajectory of experiments (second experiment)

图10 VINS-mono绝对位置误差(实验二)Fig.10 Absolute position error of VINS-mono (second experiment)

图6、图11中红色△标记处为起始位置，分析可知，当系统在初始化后保持前进方向不变时，VINS-mono的航向角极易产生漂移，而融合地磁传感器的VI-SLAM可在初始化过程中获取真实航向信息，在前进方向不变时其航向基本保持稳定，基于该优势，当前进方向改变时，磁约束的VI-SLAM也能较好地保证航向精度，使得系统位姿估计结果更加准确，表1对二者的误差进行了比较。

图6 实验轨迹图(实验一)Fig.6 The trajectory of experiments (first experiment)

图11 磁约束VI-SLAM绝对位置误差(实验二)Fig.11 Absolute position error of VI-SLAM based on magnetic constraint (second experiment)

表1 VINS-mono与磁约束VI-SLAM的误差比较Tab.1 Error comparison between VINS-mono and VI-SLAM based on magnetic constraint

实验结果表明，磁约束VI-SLAM定位结果的各项误差均小于VINS-mono定位结果对应的误差，其中，多次实验的平均均方根误差（Root Mean Square Error, RMSE）减小了约25%。进一步分析实验结果可知，实验环境的图像特征是否明显，周围环境是否对地磁传感器造成干扰等因素均会影响实验效果。结合图6-11以及表1可得，本文提出的基于磁约束的VI-SLAM可有效提高系统定位精度。

5 结 论

本文提出了一种基于磁航向约束的视觉惯性定位算法，建立了关于相机、IMU以及地磁传感器的模型，在初始化过程中获取真实航向信息，完成视觉/惯性/地磁信息对齐，利用视觉因子、IMU残差因子、磁力计残差因子构建了目标优化函数，利用BA对目标函数进行非线性优化，实现了视觉、惯性和地磁数据融合。通过车载实验对本文所提出的算法进行了验证，实验结果表明：通过引入磁约束能有效抑制VI-SLAM航向误差的发散，使系统导航定位精度明显提升，在室外环境下，相比于VINS-mono，定位误差大大减小。

基于多传感器融合的导航方法是一种非常有发展前景的自主导航方法，如何能在长航时、长距离导航中保持较高的定位精度，是我们下一步要开展的工作。