自適應視覺慣性地磁緊耦合定位系統

付 鵬，萬振華，王琨鋒，趙開春*

（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%，能夠在本文實驗計算平臺上實時運行，驗證了所提出方法的有效性。磁力計及自適應融合方法的引入可以有效改善現有視覺慣性導航系統的定位性能，對于未知環境下無人系統的高精度實時定位研究具有重要意義。