自适应视觉惯性地磁紧耦合定位系统

付 鹏，万振华，王琨锋，赵开春*

（1.清华大学 精密仪器系，北京 100084；2.北京信息科技大学 仪器科学与光电工程学院，北京 100192；3.广西大学 机械工程学院，广西 南宁 530004）

1 引言

导航技术已成为人们生活行动的重要支撑，卫星导航具有全天域、全天候、全天时、高精度的特点，是定位导航与授时（Positing Navigating and Timing，PNT）体系的重要组成部分。无线电信号弱、易被遮挡和干扰，不能覆盖室内/水下/深空等工作场景，因此，统筹发展各类PNT技术成为各方共识［1］。

近年来，随着低成本相机的推广以及计算机视觉技术的成熟，视觉导航因其结构简单、价格低廉、信息量大和抗干扰能力强等优点，成为强拒止环境中代替卫星导航的较好选择。该技术广泛借鉴计算机视觉领域中解决运动恢复结构（Structure-from-Motion，SFM）问题的成熟方案，并在此基础上结合机器人状态估计理论中的滤波器理论、非线性优化理论，最终形成基本的算法框架［2］。传统的视觉里程计（Visual Odometry，VO）主要利用环境的局部图像几何纹理信息，容易受到光照、季节和天气等因素的影响，多维度信息融合是目前的研究热点之一［3］。惯性测量单元（Inertial Measurement Units，IMU）具有输出频率高、不依赖外部信息源等优点，将相机与自身携带IMU 集成进行运动状态估计的视觉惯性里程计（Visual Inertial Odometry，VIO）与视觉-惯性导航系统（Visual-Inertial Navigation System，VINS）可以应对更加复杂多变的任务需求。2007 年，Mouriki 等提出了多状态约束卡尔曼滤波 器（Multi-State Constraint Kalman Filter，MSCKF）［4］，将特征点构建为相机状态间的几何约束，实现了视觉-惯性传感器信息的紧耦合。2014 年，Leutenegger 等提出了OKVIS（Open Keyframe-based Visual-Inertial SLAM）［5］，基于关键帧利用非线性优化方法进行状态估计。2019年，沈劭劼团队提出的VINS-Fusion［6］，通过高效的初始化、重定位、地图合并等机制，获得了稳定性强的视觉惯性系统。2020 年，Campos 等提出了ORB_SLAM3［7］，这是一种基于特征点法的视觉惯性紧耦合系统，可在大/小/室内/室外环境下稳定实时运行，并且能够对多地图进行复用，支持单目/双目/RGB-D 作为输入以及鱼眼相机模型。

视觉/惯性这种增量定位方式的绝对航向不可观，累积的姿态误差在经过一定时间后也会与真实值有较大偏差，进而影响系统最终的定位结果。地磁导航具有可用区域广泛、无累积误差、无源和隐蔽性强等优势，将地磁信息与视觉惯性导航手段相结合，对卫星信号拒止环境下的机器人导航与定位有着很好的补充作用。2015 年，Kien 等开发了一种高精度、低成本的惯性测量单元［8］，利用解算出的地磁航向进行卡尔曼滤波，消除了惯性传感器积分运算随时间增加的速度和位置漂移。2018 年，王泽华等提出了一种IMU/磁力传感器与单目视觉融合的SLAM 方法［9］，利用磁力计测量IMU 航向角，采用松耦合方式对视觉算法与IMU 估计的位姿进行卡尔曼滤波，在公开数据集EuRoC 上测试总的位置均方根误差为5.73 cm。2022 年，Coulin 等提出了一种适用于室内环境下磁场-视觉-惯性紧耦合定位方法［10］，根据室内磁场数据及位置真值建立离线磁图，实时运行时将磁图在MSCKF 中长期重复使用，用于无漂移定位。2022 年，李磊磊等提出了一种基于磁航向约束的视觉惯性定位算法［11］，通过构建地磁帧间信息约束，利用非线性优化方法实现视觉、惯性和地磁数据融合，在车载实验中平均定位均方根误差相比于视觉惯性定位减小了约25%。

目前，以地磁辅助视觉惯性导航的研究多是通过松耦合方式，预先建立磁图将地磁匹配得到的位置信息或磁强计解算出的航向信息与视觉惯性定位结果进行融合，少数通过紧耦合实现地磁信息的直接融合。其中，磁基准图的建立较为困难，对于未知环境下的自主导航需求具有一定的局限性，现有未知磁场环境下的视觉惯性地磁融合定位研究只能用于平稳的地磁场环境，磁航向角的解算精度易受周围磁场的影响，在具有部分磁场干扰的环境下不可用，其应用场景受到制约。

本文在传统VIO 的基础上引入磁力计，提出了一种地磁信息全局及帧间约束残差构建方法，基于非线性优化实现紧耦合，通过磁强度差异动态调整融合权重，设计实现了针对未知磁场环境下的自适应视觉惯性地磁紧耦合定位系统，并在校园环境下开展多组户外对比实验。与传统VIO相比，磁力计的融合提高了系统的姿态与位置精度，自适应调整提升了系统在不同环境工况下的适应能力，具有自主性好、抗干扰能力强的优势。

2 三轴磁力计标定

2.1 内参标定

假设载体坐标系中地磁场矢量表示为H=[Hx Hy Hz]T，磁力计测量值为M=[Mx My Mz]T，两者之间的关系可以表述如下：

式中：矩阵K=(kij)3×3，表示载体本身被地磁场磁化产生的磁场（软磁干扰）；矢量b=[bx by bz]T，表示载体本身的固有磁场（硬磁干扰）。

同一位置的地磁场矢量是稳定不变的，在载体坐标系中的变化轨迹是一个球面。通常软磁干扰中的轴间干扰（软磁干扰矩阵K中的非对角元）均较小，能够保证KTK严格对角占优，即KTK为正定矩阵。则由式（1）可得M的二次型为一椭球面，表达如下：

利用传感器各方向测量数据进行椭球拟合［12］，求得K和b后进行补偿，即：

如图1 所示，校准前的磁力计由于受到硬磁干扰导致原点偏移，软磁干扰导致测量值形状近似为一个椭球面；校准后的磁力计输出近似为一个以原点为球心，各轴矢量合强度相等的球面。

图1 磁力计椭球校准Fig.1 Magnetometer ellipsoid calibration

2.2 外参标定

已知地球上任一位置的地磁矢量与重力矢量固定，且它们之间的夹角也固定。磁倾角定义为地磁矢量与地磁场水平分量之间的夹角，如图2 虚线所示，可表示为：

图2 地球上任一位置的矢量示意图Fig.2 Vector diagram of any location on earth

式中：G表示当地重力加速度，H表示当地地磁矢量。

由于磁力计与加速度计之间存在未对准误差，即磁力计与加速度计之间的坐标系变换关系为，将磁力计测量值Hm转换到IMU 系下有：

则地球上任意位置的重力矢量与地磁矢量间的点积值可表示为：

已知L不变，观测量为静止状态下加速度计测量值Gb与磁力计测量值Hm，需求解磁力计与IMU 之间的外参。这个线性回归问题可以通过最小二乘法［13］求解，进而完成标定。

3 多传感器融合定位

地磁场可以为系统提供绝对航向参考，将地磁信息与视觉/惯性系统结合，在视觉特征弱或长时间运动时能够有效修正姿态误差，提升系统的定位精度。系统结构设计如图3 所示，包括双目相机、IMU 以及三轴磁力计。双目相机用于特征点提取、匹配，从而进行深度估计与初步位姿求解；IMU 可以测得载体的加速度和角速度，具有高频率输出，用于提升系统的运动跟踪性能；磁力计通过测量载体位置的磁场矢量，初始化系统航向，并为系统提供全局及帧间姿态约束。后端采用非线性优化方法建立视觉约束、IMU 约束及地磁约束，构建待优化目标函数，实现3 种传感器信息的紧耦合。

图3 视觉/惯性/地磁信息紧耦合定位系统框架Fig.3 Framework of visual/inertial/geomagnetic information tightly coupled positioning system

3.1 系统初始化

由相机、IMU 及磁力计共同完成初始化，保持IMU 处于静止状态，将一段时间内收集到的角速度数据求平均值，得到陀螺仪的零偏；使用初始静止时刻加速度计的平均测量值估计重力方向，计算载体水平姿态角：

其 中：ax，ay，az分别为加速度计在3 个轴上的平均测量值，γ和θ分别为载体的滚转角与俯仰角。

根据磁场水平分量矢量指北的特点，可得载体的初始磁航向角：

其中：mx，my，mz分别为磁力计在3 个轴上的平均测量值。测得磁航向角后，需要利用磁偏角α进行补偿才能得到载体相对于地理正北方向的航向角，磁偏角因地理位置的差异而偏向不同，可查阅世界地磁场模型得到。实际载体航向角为：

用计算得到的3 个姿态角度共同初始化一个新的旋转矩阵，使之对齐到导航坐标系下。

3.2 视觉前端

出于实时性考虑，采用稀疏光流法进行特征跟踪，即LK 光流跟踪法。LK 光流法基于3 个假设：（1）灰度不变假设，即两张图像中同一空间点的灰度值不变；（2）像素运动量相对较小；（3）某点邻域空间内小范围的点运动具有一致性。

将相机图像看作是随时间变化的函数，假设t时刻，位于(x，y)处的像素灰度值为I(x，y，t)，则t+dt时刻灰度不变性的转换方程为：

对右边进行泰勒展开，保留一阶项，两边除以dt得到：

根据假设条件（3），引入一个尺寸为ω×ω的窗口，含有ω2数量的像素，整个窗口内像素的运动约束方程如下：

由于运动约束方程是一个关于u，v的超定线性方程，求取最小二乘解：

得到像素在图像间的运动u，v，再根据匹配结果进行三角化与初步位姿求解。

3.3 非线性优化后端

后端优化采用紧耦合方式，将视觉重投影构造的残差项、IMU 预积分残差项与地磁姿态残差放在一起进行联合优化，整个优化问题的最优解即认为是比较准确的状态估计，优化因子图结构如图4 所示。在后端优化过程中，要保障系统的实时性，不适合对每一帧图像都进行处理，对非关键帧只执行系统前端的算法，不参与后端优化进程，同时为了限制变量的数目，采用滑动窗口进行优化过程。

图4 视觉/惯性/地磁优化因子图结构Fig.4 Structure of visual/inertial/geomagnetic optimization factor graph

3.3.1 状态向量

需要估计的主要状态包括滑动窗口内n+1个载体的三维位置与姿态，m+1 个3D 点的逆深度，以及IMU 引入的中间变量速度、加速度计和陀螺仪的测量偏置。整个状态向量χ定义如下：

3.3.2 目标函数

待优化的目标函数由4 部分误差项组成，分别是来自边缘化的先验信息、视觉的重投影残差、IMU 测量残差以及地磁姿态残差。其中，地磁姿态残差包含全局姿态残差与帧间姿态残差，分别用于系统全局及关键帧间的姿态约束，使用马氏距离表示。代价函数定义为：

3.3.3 传感器约束

3.3.3.1 视觉残差

视觉残差定义为重投影误差。对于某个路标点Pl，将其从上一次观测到它的第i个相机坐标，转换到当前第j个相机坐标系下的像素坐标，定义视觉残差项为：

其中：表示该路标点在第j个相机归一化坐标系中观测到的坐标：

其中πc(·)表示相机投影模型，将相机系下的3D特征点投影到相机成像平面上；表示该路标点从第i个相机系转换到第j个相机归一化坐标系的预测坐标：

因为视觉残差的自由度为2，将视觉残差投影到正切平面上，b1，b2为正切平面上的任意两个正交基。

3.3.3.2 IMU 残差

利用预积分算法构造的IMU 残差项为两关键帧之间的位置、速度、姿态和IMU 偏置的变化量与对应优化参数变化量之差［14］，即：

3.3.3.3 地磁全局约束残差

假设目标区域范围内的地磁场强度与方向稳定，即测量环境磁场恒定不变，无论如何移动磁力计，三轴测量值矢量合都是一个与位置无关的定值。设系统初始化时载体姿态为（IMU系），校准后的磁力计测量值为H0，k时刻载体姿态与磁力计测量值分别为与Hk，则有：

其中为IMU 与磁力计之间的外参，由第2 节标定得到。定义地磁全局约束残差项为：

构建地磁全局姿态约束，关于优化变量qwbk的雅可比矩阵经推导可得：

磁力计在使用时易受周围铁磁材料建筑以及强电磁信号干扰，测量值等于当地地磁场与环境干扰磁场的叠加。由于地磁场矢量是稳定的，叠加干扰磁场后必定在强度上有所变化，因此，地磁场受干扰程度可以通过总强度值来判断。设置地磁全局约束的噪声协方差为：

其中：H为地磁场强度，Hth为磁异常判断阈值，Ωm大小与地磁传感器的精度及噪声有关。当磁力计测量强度与地磁场强度之差大于该阈值时，噪声协方差设置为无穷大，即优化权重占比为0，不参与融合过程。

3.3.3.4 地磁帧间约束残差

当地磁场受到周围环境铁磁材料干扰时，其测量磁场矢量与地磁矢量出现偏差，不满足地磁全局约束假设，但是这种偏差在一定区域内是稳定的，在载体做纯旋转或小位移运动时依旧可以利用测量磁场矢量进行姿态约束。设第k+1 关键帧的载体姿态与磁力计测量值分别为与Hk+1，上一关键帧k时的载体姿态与磁力计测量值分别为与Hk，则有：

由此定义地磁帧间约束残差为：

噪声协方差由两帧之间的磁场测量强度决定：

当相邻两关键帧磁场的测量总强度差别过大时，其磁场矢量方向也可能发生较大变化，不能用于帧间姿态约束，调整融合权重实现地磁信息的自适应融合。

在计算地磁约束残差时，优先考虑地磁全局约束假设，若满足，则无需计算地磁帧间的约束残差；若不满足，则判断地磁帧间约束假设是否成立，两种约束不同时参与优化过程。

4 实验与结果

自主搭建的导航实验平台如图5 所示，包含双目相机、九轴IMU，以及高精度卫星定位接收机，其中高精度卫导用于评估定位误差，提供真值轨迹。计算处理设备为安装Linux 操作系统的个人笔记本电脑，在ROS 环境下完成各传感器的数据采集与解算，多传感器时间同步利用比较法实现，比较传感器时间戳，取最近时间间隔的数据进行融合。传感器及设备的性能参数如表1所示。

表1 传感器及设备参数Tab.1 Sensor and equipment parameters

图5 视觉/惯性/地磁传感器融合定位实验平台Fig.5 Vision/inertial/geomagnetic sensor fusion positioning experimental platform

实验地点位于清华大学校园内，手持设备运动共采集3 组实验数据，轨迹分别为圆形、方形以及“L”形的封闭曲线。视觉场景包括人行小路、草地、两侧树木及钢筋混凝土建筑等，整体轨迹较为平滑，在转弯处存在较大的视场变化。数据采集过程中有少量的行人与车辆移动，实验磁场测量结果如图6 所示。

图6 实验环境测量磁场变化曲线Fig.6 Measurement of magnetic field change curve in experimental environment

当地的平均地磁场强度约为51 μT，由于存在铁磁材料建筑以及移动车辆磁场干扰，其磁场测量结果在部分区域出现波动甚至是较大的异常值。

对双目相机、IMU 以及磁力计的内外参数进行标定，使用初始静止时刻加速度计与磁力计的平均测量值初始化系统姿态。分别使用开源VIO 算法VINS-Fusion（无回环）与本文地磁融合定位算法运行实验数据，并对定位结果进行精度评估，计算绝对位置误差（Average Positioning Error，APE），3 组实验结果如图7～9 所示。图中的上半部分为数据运行二维轨迹与三自由度位置，下半部分为实验的绝对位置误差曲线与误差统计结果。

图7 运行轨迹与绝对位置误差（实验一）Fig.7 Operation trajectory and absolute position error（Experiment 1）

图8 运行轨迹与绝对位置误差（实验二）Fig.8 Operation trajectory and absolute position error（Experiment 2）

图9 运行轨迹与绝对位置误差（实验三）Fig.9 Operation trajectory and absolute position error（Experiment 3）

系统最终的定位精度与实际场景有关，多数室外开阔场景由于近处特征点数目较少导致视觉定位效果不佳。从图中可以看出，纯视觉定位结果高度发散较快，融合IMU 后由于陀螺姿态约束以及加速度计对重力的观测作用，高度误差明显降低；在部分区间段，融合IMU 后误差变大，因为视觉是更高精度的里程计，在适用的环境及运动状态下相比于低成本IMU 能够达到更高的精度，但是从运行轨迹趋势来看，融合IMU对于系统的姿态与高度有较强的约束作用，有助于提高系统的稳定性。在弯道处系统由于视角变化相对较快更容易产生误差，对应图中绝对位置误差曲线斜率变化快的地方，融合磁力计后由于对姿态的约束作用表现出更加平稳的跟踪性能，最大误差与起终点误差均有所改善，同时全局高度误差进一步降低。

实验场景中存在部分建筑及车辆磁场干扰，为进一步验证自适应磁场约束方案，与文献［11］中非自适应磁场融合方法进行对比，运行结果如图10 所示，图中Reference 表示直接进行磁场约束的视觉/惯性/地磁融合方案。在地磁场平稳区域，磁力计的直接融合有助于提高系统的定位精度，但是在地磁场受干扰区域，系统因融合错误的测量数据导致误差突然增大，而本系统通过磁强度差异动态调整融合权重，使磁力计在地磁场无明显干扰时提供全局姿态约束，在受干扰区域提供帧间姿态约束，视觉及视觉惯性系统融合地磁信息之后，定位结果更接近真实轨迹，并不会因为融合错误的磁场观测结果而发散。

图10 非自适应融合方案对比Fig.10 Comparison of non adaptive fusion schemes

最终的定位统计结果如表2 所示。本文提出的自适应视觉惯性地磁紧耦合定位系统，在部分地磁异常干扰场景下依旧能够稳定运行，3 组实验的定位精度均优于0.8%（均方根误差除以总运动距离），融合地磁信息后相比于VINS 位置误差平均降低了约24%。

表2 定位均方根误差统计Tab.2 RMSE statistics for positioning（m）

在滑动窗口内，磁力计测量的数据量较少，残差计算及优化简便，3 组实验数据平均单次滑窗优化所用的时间如表3 所示。融合磁力计后系统运行耗时无明显增加，均小于15 ms，可以在本文实验计算平台上实时运行。

表3 平均优化时间对比Tab.3 Average optimization time comparison（ms）

5 结论

本文设计了针对未知磁场环境下的自适应视觉惯性地磁紧耦合定位系统，提出了一种地磁信息全局及帧间约束残差构建方法，通过磁强度差异动态调整融合权重，基于非线性优化方法实现3种传感器的紧耦合，能够提供具有绝对航向的六自由度位姿估计结果。搭建传感器融合定位实验平台，并在校园环境下开展多组户外实验。实验结果表明，所提出的定位系统在部分建筑及车辆磁场干扰环境下依旧能够稳定运行，定位精度均优于0.8%，相比于VINS 位置误差平均降低了约24%，能够在本文实验计算平台上实时运行，验证了所提出方法的有效性。磁力计及自适应融合方法的引入可以有效改善现有视觉惯性导航系统的定位性能，对于未知环境下无人系统的高精度实时定位研究具有重要意义。