自適應(yīng)視覺慣性地磁緊耦合定位系統(tǒng)

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

（1.清華大學(xué) 精密儀器系，北京 100084；2.北京信息科技大學(xué) 儀器科學(xué)與光電工程學(xué)院，北京 100192；3.廣西大學(xué) 機械工程學(xué)院，廣西 南寧 530004）

1 引言

導(dǎo)航技術(shù)已成為人們生活行動的重要支撐，衛(wèi)星導(dǎo)航具有全天域、全天候、全天時、高精度的特點，是定位導(dǎo)航與授時（Positing Navigating and Timing，PNT）體系的重要組成部分。無線電信號弱、易被遮擋和干擾，不能覆蓋室內(nèi)/水下/深空等工作場景，因此，統(tǒng)籌發(fā)展各類PNT技術(shù)成為各方共識［1］。

近年來，隨著低成本相機的推廣以及計算機視覺技術(shù)的成熟，視覺導(dǎo)航因其結(jié)構(gòu)簡單、價格低廉、信息量大和抗干擾能力強等優(yōu)點，成為強拒止環(huán)境中代替衛(wèi)星導(dǎo)航的較好選擇。該技術(shù)廣泛借鑒計算機視覺領(lǐng)域中解決運動恢復(fù)結(jié)構(gòu)（Structure-from-Motion，SFM）問題的成熟方案，并在此基礎(chǔ)上結(jié)合機器人狀態(tài)估計理論中的濾波器理論、非線性優(yōu)化理論，最終形成基本的算法框架［2］。傳統(tǒng)的視覺里程計（Visual Odometry，VO）主要利用環(huán)境的局部圖像幾何紋理信息，容易受到光照、季節(jié)和天氣等因素的影響，多維度信息融合是目前的研究熱點之一［3］。慣性測量單元（Inertial Measurement Units，IMU）具有輸出頻率高、不依賴外部信息源等優(yōu)點，將相機與自身攜帶IMU 集成進(jìn)行運動狀態(tài)估計的視覺慣性里程計（Visual Inertial Odometry，VIO）與視覺-慣性導(dǎo)航系統(tǒng)（Visual-Inertial Navigation System，VINS）可以應(yīng)對更加復(fù)雜多變的任務(wù)需求。2007 年，Mouriki 等提出了多狀態(tài)約束卡爾曼濾波 器（Multi-State Constraint Kalman Filter，MSCKF）［4］，將特征點構(gòu)建為相機狀態(tài)間的幾何約束，實現(xiàn)了視覺-慣性傳感器信息的緊耦合。2014 年，Leutenegger 等提出了OKVIS（Open Keyframe-based Visual-Inertial SLAM）［5］，基于關(guān)鍵幀利用非線性優(yōu)化方法進(jìn)行狀態(tài)估計。2019年，沈劭劼團(tuán)隊提出的VINS-Fusion［6］，通過高效的初始化、重定位、地圖合并等機制，獲得了穩(wěn)定性強的視覺慣性系統(tǒng)。2020 年，Campos 等提出了ORB_SLAM3［7］，這是一種基于特征點法的視覺慣性緊耦合系統(tǒng)，可在大/小/室內(nèi)/室外環(huán)境下穩(wěn)定實時運行，并且能夠?qū)Χ嗟貓D進(jìn)行復(fù)用，支持單目/雙目/RGB-D 作為輸入以及魚眼相機模型。

視覺/慣性這種增量定位方式的絕對航向不可觀，累積的姿態(tài)誤差在經(jīng)過一定時間后也會與真實值有較大偏差，進(jìn)而影響系統(tǒng)最終的定位結(jié)果。地磁導(dǎo)航具有可用區(qū)域廣泛、無累積誤差、無源和隱蔽性強等優(yōu)勢，將地磁信息與視覺慣性導(dǎo)航手段相結(jié)合，對衛(wèi)星信號拒止環(huán)境下的機器人導(dǎo)航與定位有著很好的補充作用。2015 年，Kien 等開發(fā)了一種高精度、低成本的慣性測量單元［8］，利用解算出的地磁航向進(jìn)行卡爾曼濾波，消除了慣性傳感器積分運算隨時間增加的速度和位置漂移。2018 年，王澤華等提出了一種IMU/磁力傳感器與單目視覺融合的SLAM 方法［9］，利用磁力計測量IMU 航向角，采用松耦合方式對視覺算法與IMU 估計的位姿進(jìn)行卡爾曼濾波，在公開數(shù)據(jù)集EuRoC 上測試總的位置均方根誤差為5.73 cm。2022 年，Coulin 等提出了一種適用于室內(nèi)環(huán)境下磁場-視覺-慣性緊耦合定位方法［10］，根據(jù)室內(nèi)磁場數(shù)據(jù)及位置真值建立離線磁圖，實時運行時將磁圖在MSCKF 中長期重復(fù)使用，用于無漂移定位。2022 年，李磊磊等提出了一種基于磁航向約束的視覺慣性定位算法［11］，通過構(gòu)建地磁幀間信息約束，利用非線性優(yōu)化方法實現(xiàn)視覺、慣性和地磁數(shù)據(jù)融合，在車載實驗中平均定位均方根誤差相比于視覺慣性定位減小了約25%。

目前，以地磁輔助視覺慣性導(dǎo)航的研究多是通過松耦合方式，預(yù)先建立磁圖將地磁匹配得到的位置信息或磁強計解算出的航向信息與視覺慣性定位結(jié)果進(jìn)行融合，少數(shù)通過緊耦合實現(xiàn)地磁信息的直接融合。其中，磁基準(zhǔn)圖的建立較為困難，對于未知環(huán)境下的自主導(dǎo)航需求具有一定的局限性，現(xiàn)有未知磁場環(huán)境下的視覺慣性地磁融合定位研究只能用于平穩(wěn)的地磁場環(huán)境，磁航向角的解算精度易受周圍磁場的影響，在具有部分磁場干擾的環(huán)境下不可用，其應(yīng)用場景受到制約。

本文在傳統(tǒng)VIO 的基礎(chǔ)上引入磁力計，提出了一種地磁信息全局及幀間約束殘差構(gòu)建方法，基于非線性優(yōu)化實現(xiàn)緊耦合，通過磁強度差異動態(tài)調(diào)整融合權(quán)重，設(shè)計實現(xiàn)了針對未知磁場環(huán)境下的自適應(yīng)視覺慣性地磁緊耦合定位系統(tǒng)，并在校園環(huán)境下開展多組戶外對比實驗。與傳統(tǒng)VIO相比，磁力計的融合提高了系統(tǒng)的姿態(tài)與位置精度，自適應(yīng)調(diào)整提升了系統(tǒng)在不同環(huán)境工況下的適應(yīng)能力，具有自主性好、抗干擾能力強的優(yōu)勢。

2 三軸磁力計標(biāo)定

2.1 內(nèi)參標(biāo)定

假設(shè)載體坐標(biāo)系中地磁場矢量表示為H=[Hx Hy Hz]T，磁力計測量值為M=[Mx My Mz]T，兩者之間的關(guān)系可以表述如下：

式中：矩陣K=(kij)3×3，表示載體本身被地磁場磁化產(chǎn)生的磁場（軟磁干擾）；矢量b=[bx by bz]T，表示載體本身的固有磁場（硬磁干擾）。

同一位置的地磁場矢量是穩(wěn)定不變的，在載體坐標(biāo)系中的變化軌跡是一個球面。通常軟磁干擾中的軸間干擾（軟磁干擾矩陣K中的非對角元）均較小，能夠保證KTK嚴(yán)格對角占優(yōu)，即KTK為正定矩陣。則由式（1）可得M的二次型為一橢球面，表達(dá)如下：

利用傳感器各方向測量數(shù)據(jù)進(jìn)行橢球擬合［12］，求得K和b后進(jìn)行補償，即：

如圖1 所示，校準(zhǔn)前的磁力計由于受到硬磁干擾導(dǎo)致原點偏移，軟磁干擾導(dǎo)致測量值形狀近似為一個橢球面；校準(zhǔn)后的磁力計輸出近似為一個以原點為球心，各軸矢量合強度相等的球面。

圖1 磁力計橢球校準(zhǔn)Fig.1 Magnetometer ellipsoid calibration

2.2 外參標(biāo)定

已知地球上任一位置的地磁矢量與重力矢量固定，且它們之間的夾角也固定。磁傾角定義為地磁矢量與地磁場水平分量之間的夾角，如圖2 虛線所示，可表示為：

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

式中：G表示當(dāng)?shù)刂亓铀俣?，H表示當(dāng)?shù)氐卮攀噶俊?/p>

由于磁力計與加速度計之間存在未對準(zhǔn)誤差，即磁力計與加速度計之間的坐標(biāo)系變換關(guān)系為，將磁力計測量值Hm轉(zhuǎn)換到IMU 系下有：

則地球上任意位置的重力矢量與地磁矢量間的點積值可表示為：

已知L不變，觀測量為靜止?fàn)顟B(tài)下加速度計測量值Gb與磁力計測量值Hm，需求解磁力計與IMU 之間的外參。這個線性回歸問題可以通過最小二乘法［13］求解，進(jìn)而完成標(biāo)定。

3 多傳感器融合定位

地磁場可以為系統(tǒng)提供絕對航向參考，將地磁信息與視覺/慣性系統(tǒng)結(jié)合，在視覺特征弱或長時間運動時能夠有效修正姿態(tài)誤差，提升系統(tǒng)的定位精度。系統(tǒng)結(jié)構(gòu)設(shè)計如圖3 所示，包括雙目相機、IMU 以及三軸磁力計。雙目相機用于特征點提取、匹配，從而進(jìn)行深度估計與初步位姿求解；IMU 可以測得載體的加速度和角速度，具有高頻率輸出，用于提升系統(tǒng)的運動跟蹤性能；磁力計通過測量載體位置的磁場矢量，初始化系統(tǒng)航向，并為系統(tǒng)提供全局及幀間姿態(tài)約束。后端采用非線性優(yōu)化方法建立視覺約束、IMU 約束及地磁約束，構(gòu)建待優(yōu)化目標(biāo)函數(shù)，實現(xiàn)3 種傳感器信息的緊耦合。

圖3 視覺/慣性/地磁信息緊耦合定位系統(tǒng)框架Fig.3 Framework of visual/inertial/geomagnetic information tightly coupled positioning system

3.1 系統(tǒng)初始化

由相機、IMU 及磁力計共同完成初始化，保持IMU 處于靜止?fàn)顟B(tài)，將一段時間內(nèi)收集到的角速度數(shù)據(jù)求平均值，得到陀螺儀的零偏；使用初始靜止時刻加速度計的平均測量值估計重力方向，計算載體水平姿態(tài)角：

其 中：ax，ay，az分別為加速度計在3 個軸上的平均測量值，γ和θ分別為載體的滾轉(zhuǎn)角與俯仰角。

根據(jù)磁場水平分量矢量指北的特點，可得載體的初始磁航向角：

其中：mx，my，mz分別為磁力計在3 個軸上的平均測量值。測得磁航向角后，需要利用磁偏角α進(jìn)行補償才能得到載體相對于地理正北方向的航向角，磁偏角因地理位置的差異而偏向不同，可查閱世界地磁場模型得到。實際載體航向角為：

用計算得到的3 個姿態(tài)角度共同初始化一個新的旋轉(zhuǎn)矩陣，使之對齊到導(dǎo)航坐標(biāo)系下。

3.2 視覺前端

出于實時性考慮，采用稀疏光流法進(jìn)行特征跟蹤，即LK 光流跟蹤法。LK 光流法基于3 個假設(shè)：（1）灰度不變假設(shè)，即兩張圖像中同一空間點的灰度值不變；（2）像素運動量相對較小；（3）某點鄰域空間內(nèi)小范圍的點運動具有一致性。

將相機圖像看作是隨時間變化的函數(shù)，假設(shè)t時刻，位于(x，y)處的像素灰度值為I(x，y，t)，則t+dt時刻灰度不變性的轉(zhuǎn)換方程為：

對右邊進(jìn)行泰勒展開，保留一階項，兩邊除以dt得到：

根據(jù)假設(shè)條件（3），引入一個尺寸為ω×ω的窗口，含有ω2數(shù)量的像素，整個窗口內(nèi)像素的運動約束方程如下：

由于運動約束方程是一個關(guān)于u，v的超定線性方程，求取最小二乘解：

得到像素在圖像間的運動u，v，再根據(jù)匹配結(jié)果進(jìn)行三角化與初步位姿求解。

3.3 非線性優(yōu)化后端

后端優(yōu)化采用緊耦合方式，將視覺重投影構(gòu)造的殘差項、IMU 預(yù)積分殘差項與地磁姿態(tài)殘差放在一起進(jìn)行聯(lián)合優(yōu)化，整個優(yōu)化問題的最優(yōu)解即認(rèn)為是比較準(zhǔn)確的狀態(tài)估計，優(yōu)化因子圖結(jié)構(gòu)如圖4 所示。在后端優(yōu)化過程中，要保障系統(tǒng)的實時性，不適合對每一幀圖像都進(jìn)行處理，對非關(guān)鍵幀只執(zhí)行系統(tǒng)前端的算法，不參與后端優(yōu)化進(jìn)程，同時為了限制變量的數(shù)目，采用滑動窗口進(jìn)行優(yōu)化過程。

圖4 視覺/慣性/地磁優(yōu)化因子圖結(jié)構(gòu)Fig.4 Structure of visual/inertial/geomagnetic optimization factor graph

3.3.1 狀態(tài)向量

需要估計的主要狀態(tài)包括滑動窗口內(nèi)n+1個載體的三維位置與姿態(tài)，m+1 個3D 點的逆深度，以及IMU 引入的中間變量速度、加速度計和陀螺儀的測量偏置。整個狀態(tài)向量χ定義如下：

3.3.2 目標(biāo)函數(shù)

待優(yōu)化的目標(biāo)函數(shù)由4 部分誤差項組成，分別是來自邊緣化的先驗信息、視覺的重投影殘差、IMU 測量殘差以及地磁姿態(tài)殘差。其中，地磁姿態(tài)殘差包含全局姿態(tài)殘差與幀間姿態(tài)殘差，分別用于系統(tǒng)全局及關(guān)鍵幀間的姿態(tài)約束，使用馬氏距離表示。代價函數(shù)定義為：

3.3.3 傳感器約束

3.3.3.1 視覺殘差

視覺殘差定義為重投影誤差。對于某個路標(biāo)點Pl，將其從上一次觀測到它的第i個相機坐標(biāo)，轉(zhuǎn)換到當(dāng)前第j個相機坐標(biāo)系下的像素坐標(biāo)，定義視覺殘差項為：

其中：表示該路標(biāo)點在第j個相機歸一化坐標(biāo)系中觀測到的坐標(biāo)：

其中πc(·)表示相機投影模型，將相機系下的3D特征點投影到相機成像平面上；表示該路標(biāo)點從第i個相機系轉(zhuǎn)換到第j個相機歸一化坐標(biāo)系的預(yù)測坐標(biāo)：

因為視覺殘差的自由度為2，將視覺殘差投影到正切平面上，b1，b2為正切平面上的任意兩個正交基。

3.3.3.2 IMU 殘差

利用預(yù)積分算法構(gòu)造的IMU 殘差項為兩關(guān)鍵幀之間的位置、速度、姿態(tài)和IMU 偏置的變化量與對應(yīng)優(yōu)化參數(shù)變化量之差［14］，即：

3.3.3.3 地磁全局約束殘差

假設(shè)目標(biāo)區(qū)域范圍內(nèi)的地磁場強度與方向穩(wěn)定，即測量環(huán)境磁場恒定不變，無論如何移動磁力計，三軸測量值矢量合都是一個與位置無關(guān)的定值。設(shè)系統(tǒng)初始化時載體姿態(tài)為（IMU系），校準(zhǔn)后的磁力計測量值為H0，k時刻載體姿態(tài)與磁力計測量值分別為與Hk，則有：

其中為IMU 與磁力計之間的外參，由第2 節(jié)標(biāo)定得到。定義地磁全局約束殘差項為：

構(gòu)建地磁全局姿態(tài)約束，關(guān)于優(yōu)化變量qwbk的雅可比矩陣經(jīng)推導(dǎo)可得：

磁力計在使用時易受周圍鐵磁材料建筑以及強電磁信號干擾，測量值等于當(dāng)?shù)氐卮艌雠c環(huán)境干擾磁場的疊加。由于地磁場矢量是穩(wěn)定的，疊加干擾磁場后必定在強度上有所變化，因此，地磁場受干擾程度可以通過總強度值來判斷。設(shè)置地磁全局約束的噪聲協(xié)方差為：

其中：H為地磁場強度，Hth為磁異常判斷閾值，Ωm大小與地磁傳感器的精度及噪聲有關(guān)。當(dāng)磁力計測量強度與地磁場強度之差大于該閾值時，噪聲協(xié)方差設(shè)置為無窮大，即優(yōu)化權(quán)重占比為0，不參與融合過程。

3.3.3.4 地磁幀間約束殘差

當(dāng)?shù)卮艌鍪艿街車h(huán)境鐵磁材料干擾時，其測量磁場矢量與地磁矢量出現(xiàn)偏差，不滿足地磁全局約束假設(shè)，但是這種偏差在一定區(qū)域內(nèi)是穩(wěn)定的，在載體做純旋轉(zhuǎn)或小位移運動時依舊可以利用測量磁場矢量進(jìn)行姿態(tài)約束。設(shè)第k+1 關(guān)鍵幀的載體姿態(tài)與磁力計測量值分別為與Hk+1，上一關(guān)鍵幀k時的載體姿態(tài)與磁力計測量值分別為與Hk，則有：

由此定義地磁幀間約束殘差為：

噪聲協(xié)方差由兩幀之間的磁場測量強度決定：

當(dāng)相鄰兩關(guān)鍵幀磁場的測量總強度差別過大時，其磁場矢量方向也可能發(fā)生較大變化，不能用于幀間姿態(tài)約束，調(diào)整融合權(quán)重實現(xiàn)地磁信息的自適應(yīng)融合。

在計算地磁約束殘差時，優(yōu)先考慮地磁全局約束假設(shè)，若滿足，則無需計算地磁幀間的約束殘差；若不滿足，則判斷地磁幀間約束假設(shè)是否成立，兩種約束不同時參與優(yōu)化過程。

4 實驗與結(jié)果

自主搭建的導(dǎo)航實驗平臺如圖5 所示，包含雙目相機、九軸IMU，以及高精度衛(wèi)星定位接收機，其中高精度衛(wèi)導(dǎo)用于評估定位誤差，提供真值軌跡。計算處理設(shè)備為安裝Linux 操作系統(tǒng)的個人筆記本電腦，在ROS 環(huán)境下完成各傳感器的數(shù)據(jù)采集與解算，多傳感器時間同步利用比較法實現(xiàn)，比較傳感器時間戳，取最近時間間隔的數(shù)據(jù)進(jìn)行融合。傳感器及設(shè)備的性能參數(shù)如表1所示。

表1 傳感器及設(shè)備參數(shù)Tab.1 Sensor and equipment parameters

圖5 視覺/慣性/地磁傳感器融合定位實驗平臺Fig.5 Vision/inertial/geomagnetic sensor fusion positioning experimental platform

實驗地點位于清華大學(xué)校園內(nèi)，手持設(shè)備運動共采集3 組實驗數(shù)據(jù)，軌跡分別為圓形、方形以及“L”形的封閉曲線。視覺場景包括人行小路、草地、兩側(cè)樹木及鋼筋混凝土建筑等，整體軌跡較為平滑，在轉(zhuǎn)彎處存在較大的視場變化。數(shù)據(jù)采集過程中有少量的行人與車輛移動，實驗磁場測量結(jié)果如圖6 所示。

圖6 實驗環(huán)境測量磁場變化曲線Fig.6 Measurement of magnetic field change curve in experimental environment

當(dāng)?shù)氐钠骄卮艌鰪姸燃s為51 μT，由于存在鐵磁材料建筑以及移動車輛磁場干擾，其磁場測量結(jié)果在部分區(qū)域出現(xiàn)波動甚至是較大的異常值。

對雙目相機、IMU 以及磁力計的內(nèi)外參數(shù)進(jìn)行標(biāo)定，使用初始靜止時刻加速度計與磁力計的平均測量值初始化系統(tǒng)姿態(tài)。分別使用開源VIO 算法VINS-Fusion（無回環(huán)）與本文地磁融合定位算法運行實驗數(shù)據(jù)，并對定位結(jié)果進(jìn)行精度評估，計算絕對位置誤差（Average Positioning Error，APE），3 組實驗結(jié)果如圖7～9 所示。圖中的上半部分為數(shù)據(jù)運行二維軌跡與三自由度位置，下半部分為實驗的絕對位置誤差曲線與誤差統(tǒng)計結(jié)果。

圖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）

系統(tǒng)最終的定位精度與實際場景有關(guān)，多數(shù)室外開闊場景由于近處特征點數(shù)目較少導(dǎo)致視覺定位效果不佳。從圖中可以看出，純視覺定位結(jié)果高度發(fā)散較快，融合IMU 后由于陀螺姿態(tài)約束以及加速度計對重力的觀測作用，高度誤差明顯降低；在部分區(qū)間段，融合IMU 后誤差變大，因為視覺是更高精度的里程計，在適用的環(huán)境及運動狀態(tài)下相比于低成本IMU 能夠達(dá)到更高的精度，但是從運行軌跡趨勢來看，融合IMU對于系統(tǒng)的姿態(tài)與高度有較強的約束作用，有助于提高系統(tǒng)的穩(wěn)定性。在彎道處系統(tǒng)由于視角變化相對較快更容易產(chǎn)生誤差，對應(yīng)圖中絕對位置誤差曲線斜率變化快的地方，融合磁力計后由于對姿態(tài)的約束作用表現(xiàn)出更加平穩(wěn)的跟蹤性能，最大誤差與起終點誤差均有所改善，同時全局高度誤差進(jìn)一步降低。

實驗場景中存在部分建筑及車輛磁場干擾，為進(jìn)一步驗證自適應(yīng)磁場約束方案，與文獻(xiàn)［11］中非自適應(yīng)磁場融合方法進(jìn)行對比，運行結(jié)果如圖10 所示，圖中Reference 表示直接進(jìn)行磁場約束的視覺/慣性/地磁融合方案。在地磁場平穩(wěn)區(qū)域，磁力計的直接融合有助于提高系統(tǒng)的定位精度，但是在地磁場受干擾區(qū)域，系統(tǒng)因融合錯誤的測量數(shù)據(jù)導(dǎo)致誤差突然增大，而本系統(tǒng)通過磁強度差異動態(tài)調(diào)整融合權(quán)重，使磁力計在地磁場無明顯干擾時提供全局姿態(tài)約束，在受干擾區(qū)域提供幀間姿態(tài)約束，視覺及視覺慣性系統(tǒng)融合地磁信息之后，定位結(jié)果更接近真實軌跡，并不會因為融合錯誤的磁場觀測結(jié)果而發(fā)散。

圖10 非自適應(yīng)融合方案對比Fig.10 Comparison of non adaptive fusion schemes

最終的定位統(tǒng)計結(jié)果如表2 所示。本文提出的自適應(yīng)視覺慣性地磁緊耦合定位系統(tǒng)，在部分地磁異常干擾場景下依舊能夠穩(wěn)定運行，3 組實驗的定位精度均優(yōu)于0.8%（均方根誤差除以總運動距離），融合地磁信息后相比于VINS 位置誤差平均降低了約24%。

表2 定位均方根誤差統(tǒng)計Tab.2 RMSE statistics for positioning（m）

在滑動窗口內(nèi)，磁力計測量的數(shù)據(jù)量較少，殘差計算及優(yōu)化簡便，3 組實驗數(shù)據(jù)平均單次滑窗優(yōu)化所用的時間如表3 所示。融合磁力計后系統(tǒng)運行耗時無明顯增加，均小于15 ms，可以在本文實驗計算平臺上實時運行。

表3 平均優(yōu)化時間對比Tab.3 Average optimization time comparison（ms）

5 結(jié)論

本文設(shè)計了針對未知磁場環(huán)境下的自適應(yīng)視覺慣性地磁緊耦合定位系統(tǒng)，提出了一種地磁信息全局及幀間約束殘差構(gòu)建方法，通過磁強度差異動態(tài)調(diào)整融合權(quán)重，基于非線性優(yōu)化方法實現(xiàn)3種傳感器的緊耦合，能夠提供具有絕對航向的六自由度位姿估計結(jié)果。搭建傳感器融合定位實驗平臺，并在校園環(huán)境下開展多組戶外實驗。實驗結(jié)果表明，所提出的定位系統(tǒng)在部分建筑及車輛磁場干擾環(huán)境下依舊能夠穩(wěn)定運行，定位精度均優(yōu)于0.8%，相比于VINS 位置誤差平均降低了約24%，能夠在本文實驗計算平臺上實時運行，驗證了所提出方法的有效性。磁力計及自適應(yīng)融合方法的引入可以有效改善現(xiàn)有視覺慣性導(dǎo)航系統(tǒng)的定位性能，對于未知環(huán)境下無人系統(tǒng)的高精度實時定位研究具有重要意義。