復(fù)雜環(huán)境下基于視覺/慣性的單兵自主定位性能評(píng)估

摘" 要： 針對(duì)在衛(wèi)星信號(hào)拒止等復(fù)雜環(huán)境下單兵自主定位精度低的問題，提出基于視覺/慣性測(cè)量單元（IMU）的單兵自主定位技術(shù)。將高效的IMU初始化方法融合到ORB?SLAM3算法并應(yīng)用于單兵自主定位技術(shù)中，評(píng)估了單兵實(shí)時(shí)運(yùn)動(dòng)整體定位精度，重點(diǎn)分析了改進(jìn)ORB?SLAM3算法在單目/IMU、雙目/IMU下的定位精度準(zhǔn)確性和魯棒性。實(shí)驗(yàn)表明：在復(fù)雜樹林環(huán)境下，單兵在基于視覺/IMU自主定位時(shí)，改進(jìn)算法的IMU初始化時(shí)間最少提高了10 s，雙目/IMU的估計(jì)軌跡曲線更加吻合實(shí)際運(yùn)動(dòng)軌跡曲線。在絕對(duì)軌跡誤差的各項(xiàng)評(píng)估指標(biāo)中，雙目/IMU比單目/IMU的均方根誤差提高了24.67%，而且定位精度和魯棒性都要優(yōu)于單目/IMU，為城市巷戰(zhàn)中單兵自主高精度定位的實(shí)現(xiàn)提供了一種切實(shí)可行的思路和舉措。

關(guān)鍵詞： 單兵自主定位； 慣性； 視覺； 復(fù)雜環(huán)境； 單目； 雙目

中圖分類號(hào)： TN99?34" " " " " " " " " " " " " " "文獻(xiàn)標(biāo)識(shí)碼： A" " " " " " " " " " " " 文章編號(hào)： 1004?373X（2024）13?0117?06

Performance evaluation of individual soldier autonomous positioning

based on vision/inertia in complex environment

CHANG Wei， HUANG Tushun， HAN Feng， CHEN Gang

（Unit 63870 of PLA， Huayin 714200， China）

Abstract： In view of the low accuracy of individual soldier autonomous positioning in complex environments such as satellite signal denial， an individual soldier autonomous positioning technology based on vision/inertia measurement unit （IMU） is proposed. An efficient IMU initialization method is integrated into the ORB?SLAM3 algorithm and applied to individual soldier autonomous positioning technology. The overall positioning accuracy of individual soldier real?time motion is evaluated. The accuracy and robustness of the improved ORB?SLAM3 algorithm under monocular/IMU and binocular/IMU are analyzed. Experiments show that in complex forest environments， when individual soldier autonomous positioning is carried out based on vision/IMU， the IMU initialization time of the improved algorithm is decreased by at least 10 seconds， and the estimated trajectory curve of binocular/IMU is more consistent with the actual motion trajectory curve. Among the various evaluation indicators of absolute trajectory error （ATE）， the root mean square error （RMSE） of binocular/IMU is 24.67% higher than that of monocular/IMU， and its positioning accuracy and robustness are both superior to those of monocular/IMU， which provides a practical approach and measure for the high?precision individual soldier autonomous positioning in urban street fighting.

Keywords： individual soldier autonomous positioning; inertia; vision; complex environment; monocular; binocular

0" 引" 言

在作戰(zhàn)領(lǐng)域中，城市巷戰(zhàn)的戰(zhàn)斗環(huán)境復(fù)雜且相關(guān)戰(zhàn)術(shù)操作充滿挑戰(zhàn)，單兵在城市環(huán)境作戰(zhàn)能力得到廣泛關(guān)注。單兵自主定位在城市作戰(zhàn)中起著舉足輕重的作用，即單兵只依靠自身攜帶的傳感器而不借助其他額外設(shè)備進(jìn)行自主定位，實(shí)現(xiàn)了在未知環(huán)境下無需提前布置相關(guān)定位設(shè)備，僅依靠自身傳感器就可為單兵提供精確的實(shí)時(shí)定位信息。單兵自主定位技術(shù)不僅可以實(shí)時(shí)獲取自身定位信息，而且還對(duì)掌握周圍陌生環(huán)境有著一定幫助，如果處于協(xié)同作戰(zhàn)任務(wù)中，單兵之間相互分享各自所在的位置以及戰(zhàn)場(chǎng)周圍的環(huán)境情況，對(duì)戰(zhàn)術(shù)的下一步制定有著很大的幫助。當(dāng)前常用的單兵自主定位技術(shù)大多都以衛(wèi)星導(dǎo)航為核心，在室外開闊環(huán)境且電磁環(huán)境簡(jiǎn)單的情況下，很容易實(shí)現(xiàn)單兵的精準(zhǔn)定位。但是在城市巷戰(zhàn)環(huán)境中，高樓大廈建筑物、隧道遮蔽空間以及復(fù)雜的電磁環(huán)境，都會(huì)導(dǎo)致出現(xiàn)全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System， GNSS）信號(hào)拒止的情況[1]。單兵在這種復(fù)雜環(huán)境下執(zhí)行城市作戰(zhàn)任務(wù)時(shí)，自身定位信息會(huì)丟失，無法持續(xù)提供精確的定位信息，增大了單兵作戰(zhàn)時(shí)的安全隱患。因此，設(shè)計(jì)一種體積小、重量輕、可靠性強(qiáng)且能適用于GNSS信號(hào)拒止等復(fù)雜環(huán)境下的單兵自主定位系統(tǒng)已成為了城市巷戰(zhàn)中單兵作戰(zhàn)的必然需求。

在單兵自主定位方面，國內(nèi)外學(xué)者做了許多研究工作，20世紀(jì)90年代，美軍單兵導(dǎo)航系統(tǒng)中將步行航位推算模塊和GPS相組合，在衛(wèi)星信號(hào)不好的情況下，單兵導(dǎo)航系統(tǒng)可切換為航位推算模式[2]，隨后不斷優(yōu)化單兵裝備中步行航位推算（Pedestrian Dead Reckoning， PDR）算法[3]，PDR算法需要對(duì)人體運(yùn)動(dòng)過程建模，誤差僅與單兵累計(jì)的步行路程相關(guān)，但不同單兵個(gè)體模型差異較大，具體應(yīng)用有局限性，而且定位精度提升能力有限。許多學(xué)者也提出了運(yùn)動(dòng)狀態(tài)的零速修正（Zero Velocity UPdate， ZUPT）算法[4]，利用單兵腳落地時(shí)的零速特點(diǎn)作為導(dǎo)航濾波算法中的虛擬觀測(cè)，通過周期性的速度約束來提高適應(yīng)性，相比PDR算法，該算法的精度與單兵的動(dòng)作密切相關(guān)，長時(shí)間復(fù)雜運(yùn)動(dòng)不能確保單兵良好的定位精度[5]。文獻(xiàn)[6]基于地圖匹配導(dǎo)航的思路，提出了一種基于擴(kuò)展卡爾曼濾波（Extended Kalman Filter， EKF）框架的建筑物參考修正方法，但單兵在一個(gè)新的未知環(huán)境下很難有足夠詳細(xì)的數(shù)字地圖去實(shí)現(xiàn)地圖匹配導(dǎo)航定位。國內(nèi)也有學(xué)者通過慣性測(cè)量單元（Inertial Measuring Unit， IMU）和超寬帶協(xié)同導(dǎo)航定位技術(shù)去解決GNSS信號(hào)拒止環(huán)境下單兵的自主定位問題[7]，但這種方法需要提前在已知的環(huán)境中布置基站，在實(shí)際的陌生作戰(zhàn)環(huán)境中提前布設(shè)大量基站有一定的困難性。隨著相機(jī)的微小型化，單兵隨身裝備配置攝像頭也容易實(shí)現(xiàn)，視覺信息可作為單兵定位輔助信息。相機(jī)可實(shí)時(shí)獲取周圍環(huán)境影像信息并進(jìn)行特征提取，基于相關(guān)即時(shí)定位與建圖（Simultaneous Localization and Mapping， SLAM）算法，可對(duì)慣性導(dǎo)航進(jìn)行位置和姿態(tài)的輔助觀測(cè)，達(dá)到修正定位誤差的目的。文獻(xiàn)[8]開始利用手機(jī)自帶的攝像頭進(jìn)行行人自主導(dǎo)航，通過提升手機(jī)性能和優(yōu)化視覺算法來提高行人定位精度。文獻(xiàn)[9]在基于PDR算法的慣性導(dǎo)航中融入了視覺信息輔助實(shí)現(xiàn)行人的實(shí)時(shí)定位，但誤差較大。同一年，文獻(xiàn)[10]提出了一種單目視覺融合慣性導(dǎo)航的定位系統(tǒng)VINS?Mono，可應(yīng)用在行人導(dǎo)航，文獻(xiàn)[11]研究了在GNSS信號(hào)拒止環(huán)境下，利用單目相機(jī)連續(xù)地校正慣性導(dǎo)航的誤差，但上述單目視覺缺少尺度信息，定位容易出現(xiàn)誤差，且相關(guān)算法優(yōu)化一般，對(duì)定位精度提升有限。

綜上所述，視覺傳感器和IMU都是無源器件，且二者之間具有較好的互補(bǔ)性，將視覺和慣性相融合，可在復(fù)雜、未知環(huán)境下真正實(shí)現(xiàn)自主定位。本文改進(jìn)了當(dāng)前最新的視覺SLAM和IMU多源信息融合定位效果較好的ORB?SLAM3（Oriented Fast and Rotated Brief SLAM3）算法[12]，將視覺/IMU組合定位技術(shù)應(yīng)用于單兵自主定位中，在一定實(shí)驗(yàn)環(huán)境下對(duì)視覺與IMU融合的單兵自主定位性能進(jìn)行評(píng)估，將單目/IMU和雙目/IMU進(jìn)行對(duì)比，評(píng)估整體定位精度、實(shí)時(shí)性及魯棒性，分析復(fù)雜外界環(huán)境和單兵自身運(yùn)動(dòng)對(duì)定位精度的影響。

1" 系統(tǒng)組成

基于視覺/慣性的單兵自主定位系統(tǒng)主要包含視覺傳感器和慣性測(cè)量單元（IMU）、數(shù)據(jù)預(yù)處理模塊以及數(shù)據(jù)融合的相關(guān)算法，系統(tǒng)組成如圖1所示。

1.1" 視覺傳感器組成

基于視覺/慣性的單兵自主定位系統(tǒng)的傳感器由視覺相機(jī)和慣性測(cè)量單元兩部分組成。常用的視覺相機(jī)主要有三種，分別為單目、雙目、RGB?D相機(jī)，其主要功能是采集單元圖像，為后續(xù)位姿估計(jì)提供原始數(shù)據(jù)，視覺相機(jī)輸出頻率一般為20 Hz。慣性測(cè)量單元IMU主要由加速度計(jì)和陀螺儀組成，可以直接輸出慣性坐標(biāo)系下的三軸加速度和三軸角速度，有的IMU還含有磁力計(jì)，也可以輸出三軸磁力計(jì)數(shù)據(jù)。IMU作為單兵自主定位系統(tǒng)的核心傳感器單元，其輸出頻率最高，一般為50 Hz、100 Hz、200 Hz，甚至更高。因?yàn)橐曈X相機(jī)和IMU的輸出頻率較高，因此在局部或短期內(nèi)利用視覺/慣性組合可以提供精度更高的定位結(jié)果。

1.2" 數(shù)據(jù)預(yù)處理

移動(dòng)前端采集到的圖像和慣性數(shù)據(jù)都是在各自定義下的坐標(biāo)系，而且不同傳感器輸出頻率也不相同，所以在數(shù)據(jù)預(yù)處理模塊，對(duì)不同傳感器采集的數(shù)據(jù)要實(shí)現(xiàn)時(shí)間同步和空間同步。時(shí)間同步是為了避免后期數(shù)據(jù)融合時(shí)產(chǎn)生較大的時(shí)間偏移，導(dǎo)致整個(gè)系統(tǒng)發(fā)散，因此需要將傳感器輸出數(shù)據(jù)的時(shí)間戳統(tǒng)一到同一個(gè)時(shí)間參考系下；空間同步就是整個(gè)系統(tǒng)的初始化，用以確定相機(jī)和IMU坐標(biāo)系之間的旋轉(zhuǎn)平移關(guān)系，即相機(jī)和IMU坐標(biāo)系之間的相機(jī)?慣性外參數(shù)[13]。

1.3" 數(shù)據(jù)融合

在數(shù)據(jù)融合過程中，采用改進(jìn)后的ORB?SLAM3算法對(duì)預(yù)處理后的數(shù)據(jù)進(jìn)行融合，算法可實(shí)現(xiàn)閉環(huán)矯正和地圖融合，使得定位精度進(jìn)一步提高。另外，ORB?SLAM3是基于圖優(yōu)化的算法框架，這種優(yōu)化方法會(huì)將SLAM問題構(gòu)建成最小二乘問題，利用迭代重新線性化的方式進(jìn)行求解，精度高、魯棒性好。

2" 系統(tǒng)融合算法與SLAM算法評(píng)價(jià)指標(biāo)

2.1" 系統(tǒng)融合算法

基于視覺/慣性的單兵自主定位算法，是將視覺相機(jī)與IMU相組合，IMU較高的數(shù)據(jù)采樣頻率可以提升整個(gè)系統(tǒng)的輸出頻率，增強(qiáng)了視覺定位的魯棒性；慣性定位也可依靠視覺定位有效消除IMU的積分漂移，校正IMU隨時(shí)間累計(jì)的誤差。整個(gè)系統(tǒng)算法框架依賴于改進(jìn)后的ORB?SLAM3算法，主要包含3個(gè)并行處理的線程，分別是特征跟蹤、局部建圖和回環(huán)檢測(cè)與地圖融合，系統(tǒng)算法的整體架構(gòu)如圖2所示。

2.1.1" 特征跟蹤

特征跟蹤線程負(fù)責(zé)對(duì)采集的每幀圖像進(jìn)行位姿估計(jì)。對(duì)輸入的圖像數(shù)據(jù)提取ORB特征點(diǎn)，實(shí)現(xiàn)兩幀圖像前后的特征點(diǎn)匹配，運(yùn)用光束平差法（Bundle Adjustment， BA）最小化重投影誤差獲取當(dāng)前幀相機(jī)的位姿初始值，然后以此初始值為基礎(chǔ)，匹配更多能被當(dāng)前幀圖像觀測(cè)到的地圖點(diǎn)坐標(biāo)，構(gòu)建出重投影誤差函數(shù)。對(duì)兩幀圖像間的IMU進(jìn)行預(yù)積分，通過BA最小化重投影誤差和預(yù)積分誤差優(yōu)化當(dāng)前相機(jī)的位姿。符合一定條件后，當(dāng)前幀便選定為關(guān)鍵幀，特征跟蹤線程中對(duì)當(dāng)前幀位姿的初次優(yōu)化稱為純視覺位姿BA。

[（Rk，tk）*=argminRk，tki=1Nρpi-K（RkPi+tk）2Σ]" （1）式中：[Pi=Xi，Yi，ZiT]，[pi=ui，viT]，[i]=1，2，…，[n]為地圖點(diǎn)坐標(biāo)和對(duì)應(yīng)的特征點(diǎn)像素坐標(biāo)；[Rk]、[tk]為[k]時(shí)刻相機(jī)相對(duì)于世界坐標(biāo)系的旋轉(zhuǎn)矩陣和平移向量；[K]是相機(jī)內(nèi)參；[ρ]是Huber函數(shù)；[Σ]是協(xié)方差矩陣。

2.1.2" 局部建圖

局部建圖線程負(fù)責(zé)對(duì)局部地圖的關(guān)鍵幀位姿和其對(duì)應(yīng)的地圖點(diǎn)進(jìn)行估計(jì)優(yōu)化。對(duì)新添加的關(guān)鍵幀進(jìn)行預(yù)處理，維護(hù)和拓展新的局部地圖點(diǎn)，若當(dāng)前IMU未完成初始化，根據(jù)條件對(duì)IMU進(jìn)行初始化。當(dāng)IMU完成初始化，同時(shí)成功跟蹤當(dāng)前關(guān)鍵幀，則通過BA最小化重投影和預(yù)積分誤差，共同優(yōu)化當(dāng)前幀的狀態(tài)量，最后對(duì)關(guān)鍵幀進(jìn)行篩選，剔除冗余關(guān)鍵幀[14]。整個(gè)過程可稱為視覺與IMU融合的圖像幀狀態(tài)量BA，具體可用如下公式描述。

[x*k=argminxki=1N（ρrcam2Σcam）+rimu2Σcam]" "（2）

[xk=pwb，vwk，qwbk，bak，bgk]" " " " " （3）

[rcam=pi-K（RkPi+tk）] （4）

[rimu=qbiw（pwbj-pwbi-vwiΔt+12gwΔt2）-αbibj" " " " " " " "2[qbjbi?（qbiw?qwbj）]xyz" " " " " " " qbiw（vwj-vwi+gwΔt）-βbibj" " " " " " " " " " " " " " " baj-bai" " " " " " " " " " " " " " " bgj-bgi] （5）

式中：[xk]為[k]時(shí)刻的狀態(tài)變量，具體包括位置[pwb]、速度[vwk]、姿態(tài)[qwbk]、加速度計(jì)偏差[bak]以及陀螺儀偏差[bgk]；[rimu]與[Σimu]分別為預(yù)積分誤差及其協(xié)方差陣；[rcam]與[Σcam]分別為重投影誤差及其協(xié)方差陣；[αbibj]、[qbjbi]、[βbibj]分別為[i]、[j]時(shí)刻IMU預(yù)積分得到的相對(duì)位姿、姿態(tài)及速度。

2.1.3" 回環(huán)檢測(cè)與地圖融合

篩選后的關(guān)鍵幀進(jìn)入回環(huán)檢測(cè)與地圖融合線程，檢測(cè)其與之前關(guān)鍵幀的共視區(qū)域，判斷是否存在融合與回環(huán)，如果存在，則利用相似變換進(jìn)行地圖合并或回環(huán)矯正，然后執(zhí)行本質(zhì)圖優(yōu)化抑制漂移累計(jì)的誤差。在本質(zhì)圖優(yōu)化完成之后，會(huì)臨時(shí)發(fā)起一個(gè)獨(dú)立線程執(zhí)行全局BA，即視覺與IMU融合的狀態(tài)量和地圖點(diǎn)BA，具體優(yōu)化過程與局部建圖線程中視覺與IMU融合的狀態(tài)量BA類似，誤差函數(shù)由多幀圖像間的信息共同構(gòu)成，待優(yōu)化的變量為多幀圖像對(duì)應(yīng)的狀態(tài)量及地圖點(diǎn)，最后得到整個(gè)系統(tǒng)的最優(yōu)結(jié)果。

[（x，P）*=argmin（x，P）i=1N（ρrcam2Σcam）+rimu2Σcam] （6）

ORB?SLAM3不管是短時(shí)間運(yùn)行還是長時(shí)間運(yùn)行，都能對(duì)數(shù)據(jù)進(jìn)行很好的關(guān)聯(lián)，算法可以有效保證進(jìn)行地圖匹配和光束法平差優(yōu)化，從而構(gòu)建一幅地圖實(shí)現(xiàn)精確定位[15]。尤其在長時(shí)間運(yùn)行時(shí)，它可以利用場(chǎng)景重識(shí)別來匹配當(dāng)前觀測(cè)和之前觀測(cè)，即使特征跟蹤失效也可以實(shí)現(xiàn)匹配，同時(shí)可以使用位姿圖優(yōu)化重新設(shè)置漂移的誤差，這在復(fù)雜環(huán)境下長時(shí)間使用時(shí)，精度有一定的保證。

2.2" SLAM算法評(píng)價(jià)指標(biāo)

本文從準(zhǔn)確度和魯棒性兩個(gè)方面評(píng)估視覺/慣性單兵自主定位算法性能，其中準(zhǔn)確度評(píng)估通常以SLAM算法評(píng)價(jià)指標(biāo)中的絕對(duì)軌跡誤差（Absolute Trajectory Error， ATE）和相對(duì)位姿誤差（Relative Pose Error， RPE）作為衡量標(biāo)準(zhǔn)。

2.2.1" 絕對(duì)軌跡誤差

絕對(duì)軌跡誤差（ATE）是指實(shí)時(shí)估計(jì)位姿與真實(shí)位姿的差值，該項(xiàng)指標(biāo)可以直觀地反映算法定位精度和實(shí)時(shí)估計(jì)軌跡的全局一致性，適合測(cè)量SLAM系統(tǒng)的性能。實(shí)時(shí)估計(jì)位姿和真實(shí)的軌跡有時(shí)候并不在同一個(gè)坐標(biāo)系中，所以通常需要利用最小二乘法計(jì)算估計(jì)位姿到真實(shí)位姿的轉(zhuǎn)換矩陣[S]，再將它們作差值運(yùn)算。假設(shè)第[i]幀相機(jī)估計(jì)位姿是[Pi]，真實(shí)位姿是[Qi]，則第[i]幀的ATE數(shù)值[Fi]為：

[Fi=Q-1iSPi]" " （7）

假設(shè)軌跡總個(gè)數(shù)為[n]，時(shí)間間隔為[t]，那么可以得到[m]=[nt]個(gè)絕對(duì)軌跡誤差值。通過均方根誤差（Root Mean Square Error， RMSE）統(tǒng)計(jì)其誤差，這樣可以得到一個(gè)統(tǒng)計(jì)值[FATE]，如式（8）所示：

[FATE=1mi=1mT（Fi）212]" "（8）

式中[T（Fi）]為絕對(duì)軌跡誤差中的平移部分。

2.2.2" 相對(duì)位姿誤差

相對(duì)位姿誤差（RPE）指相隔固定時(shí)間差[Δt]兩幀誤差的精度，即用時(shí)間戳對(duì)齊之后，實(shí)時(shí)估計(jì)位姿和真實(shí)位姿每隔一段固定時(shí)間差[Δt]計(jì)算位姿的變化量，然后對(duì)該變化量再作差。以獲得相對(duì)位姿誤差。該項(xiàng)指標(biāo)評(píng)估系統(tǒng)的漂移，第[i]幀的相對(duì)位姿誤差為：

[Ei=（Q-1iQ-1i+t）（P-1iPi+t）]" "（9）

與ATE計(jì)算相似，相對(duì)位姿誤差通常也使用均方根誤差來衡量，得到一個(gè)總體值[ERPE]為：

[ERPE=1mi=1mT（Ei）212] （10）

3" 實(shí)驗(yàn)及結(jié)果分析

3.1" 實(shí)驗(yàn)條件

實(shí)驗(yàn)采用Intel公司生產(chǎn)的RealSense D435深度相機(jī)，它是一款USB供電的深度攝像頭，由一對(duì)深度傳感器、RGB傳感器和紅外投影儀組成，它體積小，非常適合手持應(yīng)用和機(jī)器人等場(chǎng)景，而且RealSense D435具有自標(biāo)定功能，大大提高了相機(jī)內(nèi)參標(biāo)定的自動(dòng)化程度。在SLAM系統(tǒng)中，D435可以迅速捕捉場(chǎng)景中的3D信息，適用于視覺與慣性融合算法研究。如果用IMU數(shù)據(jù)輔助其位姿估計(jì)，能夠很好地實(shí)現(xiàn)高精度的建圖和定位功能。相機(jī)的圖片采樣頻率設(shè)置為20 Hz，圖像大小設(shè)置為640×480像素，采用的慣性測(cè)量單元型號(hào)為HWT9073，由深圳維特智能科技有限公司生產(chǎn)，IMU采樣頻率設(shè)置為200 Hz。將視覺相機(jī)和IMU組合，借助Kalibr工具箱中的視覺相機(jī)/IMU聯(lián)合標(biāo)定模塊，對(duì)視覺相機(jī)和IMU之間的外參進(jìn)行標(biāo)定，誤差精度維持在0.5個(gè)像素精度以內(nèi)。

視覺相機(jī)和IMU組合進(jìn)行實(shí)驗(yàn)測(cè)試，如圖3所示。

本文實(shí)驗(yàn)數(shù)據(jù)在線采集和處理以及單兵行動(dòng)軌跡的實(shí)時(shí)展示，均在烏班圖（Ubuntu）18.04系統(tǒng)和開源計(jì)算機(jī)視覺庫（Open Source Computer Vision Library， OpenCV）3.2.0等環(huán)境中實(shí)現(xiàn)，實(shí)驗(yàn)平臺(tái)為榮耀電腦（中央處理器（CPU） AMD Ryzen 7 5800H，3.20 GHz 8核，內(nèi)存16 GB）。因?yàn)镺RB?SLAM3在IMU初始化階段需要的時(shí)間較長，需要16 s多的時(shí)間，而且容易初始化失敗，使得整個(gè)算法系統(tǒng)短暫的無法正常工作，針對(duì)這個(gè)問題，對(duì)算法源碼進(jìn)行了相關(guān)的改進(jìn)，將高效的IMU初始化方法融合到ORB?SLAM3中。在公開的Euroc數(shù)據(jù)集上對(duì)改進(jìn)的算法進(jìn)行了測(cè)試，發(fā)現(xiàn)雙目/IMU初始化最少需要4 s，單目/IMU最少需要6 s，而且和原有的IMU初始化精度幾乎一樣，這使得初始化時(shí)間大幅度降低，提高了整個(gè)算法的工作效率。

在如圖4所示的實(shí)驗(yàn)環(huán)境進(jìn)行測(cè)試，場(chǎng)景為40 m×20 m復(fù)雜樹林環(huán)境的長方形區(qū)域。單兵在此環(huán)境分別以單目/IMU模式和雙目/IMU模式正常行走2個(gè)來回，實(shí)驗(yàn)的起點(diǎn)和終點(diǎn)為同一標(biāo)記點(diǎn)，行走的距離約200 m。

單兵具體實(shí)時(shí)運(yùn)動(dòng)軌跡如圖5所示。發(fā)現(xiàn)在較為復(fù)雜的環(huán)境下，改進(jìn)的視覺/IMU融合算法仍然可以成功地跟蹤地圖提取特征點(diǎn)，而且實(shí)現(xiàn)了閉環(huán)檢測(cè)，矯正了運(yùn)動(dòng)過程中的定位精度，說明改進(jìn)的視覺/IMU融合算法具有較好的魯棒性。

3.2" 結(jié)果及分析

如圖6和圖7所示，本文繪制了系統(tǒng)中單目/IMU、雙目/IMU以及實(shí)際運(yùn)動(dòng)的參考軌跡圖，解算了兩種不同組合模式實(shí)時(shí)估計(jì)軌跡與實(shí)際軌跡參考值的偏差大小，解算的偏差值結(jié)果由不同的曲線表示，可以發(fā)現(xiàn)雙目/IMU模式的軌跡和實(shí)際運(yùn)動(dòng)參考軌跡更為接近，定位精度要好于單目/IMU模式。

因?yàn)橹饕u(píng)估整個(gè)SLAM系統(tǒng)的性能和準(zhǔn)確度，所以主要分析絕對(duì)軌跡誤差指標(biāo)。圖8和圖9分別表示單目/IMU和雙目/IMU所對(duì)應(yīng)的絕對(duì)軌跡誤差，具體數(shù)值統(tǒng)計(jì)在表1中。

可以看出，單目/IMU模式雖然相對(duì)最大誤差指標(biāo)要小于雙目/IMU模式，但其他性能指標(biāo)均要高于雙目/IMU模式的性能指標(biāo)，雙目/IMU比單目/IMU的整體均方根誤差（RMSE）提高了24.67%，說明雙目/IMU模式估計(jì)的位姿準(zhǔn)確性更高，所獲取的數(shù)據(jù)整體更加穩(wěn)定，兩種模式下的運(yùn)動(dòng)軌跡對(duì)比圖也說明了這一點(diǎn)，雙目/IMU模式的估計(jì)軌跡要更加貼近實(shí)際的參考運(yùn)動(dòng)軌跡。

整個(gè)實(shí)驗(yàn)過程也發(fā)現(xiàn)了單目/IMU模式在運(yùn)動(dòng)速度過快和相機(jī)曝光嚴(yán)重的情況下，特征點(diǎn)提取較困難，尤其在沒有回環(huán)矯正的情況下，定位結(jié)果就不太好，而且在視覺缺失的情況下，尺度估計(jì)不夠準(zhǔn)確。當(dāng)然，復(fù)雜動(dòng)作或光強(qiáng)突變等環(huán)境對(duì)整個(gè)視覺/IMU定位效果有一定的影響，但雙目/IMU模式總體魯棒性和定位精度要好于單目/IMU模式，尤其在有回環(huán)矯正的情況下。

4" 結(jié)" 語

本文將改進(jìn)的ORB?SLAM3視覺/IMU融合定位算法應(yīng)用于單兵自主定位系統(tǒng)中，并對(duì)其精度和魯棒性進(jìn)行了評(píng)估。實(shí)驗(yàn)表明：雖然在復(fù)雜動(dòng)作或光強(qiáng)突變等環(huán)境對(duì)雙目/IMU模式的定位效果有一定影響，但總體性能要好于單目/IMU模式，雙目/IMU模式魯棒性強(qiáng)，定位精度高，能夠在復(fù)雜環(huán)境情況下正常運(yùn)行，能夠?qū)伪玫剡M(jìn)行定位定姿，還能夠更準(zhǔn)確地進(jìn)行場(chǎng)景識(shí)別，提高單兵運(yùn)動(dòng)時(shí)的定位精度，也增強(qiáng)了單兵在復(fù)雜環(huán)境下作戰(zhàn)的適應(yīng)性，對(duì)提升單兵實(shí)際作戰(zhàn)能力有著一定的幫助。

注：本文通訊作者為黃土順。

參考文獻(xiàn)

[1] 徐亞運(yùn)，王波，趙耀東.城市電磁環(huán)境可視化及系統(tǒng)設(shè)計(jì)研究[J].現(xiàn)代電子技術(shù)，2023，46（10）：154?158.

[2] 潘獻(xiàn)飛，穆華，胡小平.單兵自主導(dǎo)航技術(shù)發(fā)展綜述[J].導(dǎo)航定位與授時(shí)，2018，5（1）：1?11.

[3] DORNEICH M C， VERVERS P M， WHITLOW S D， et al. Evaluation of a tactile navigation cueing system and real?time assessment of cognitive state [J]. Human factors and ergonomics society annual meeting， 2006， 50（24）： 2600?2604.

[4] 谷洪浩，蔡成林，蔡勁，等.基于彎曲傳感器輔助的行人室內(nèi)定位零速修正方法[J].現(xiàn)代電子技術(shù)，2019，42（16）：164?168.

[5] 蘇中，王一靜，李磊，等.遮蔽空間單兵雙足不等式約束融合定位方法[J].中國慣性技術(shù)學(xué)報(bào)，2023，31（6）：540?546.

[6] ABDULRAHIM K， HIDE C， MOORE T， et al. Integrating low cost IMU with building heading in indoor pedestrian navigation [J]. Journal of global positioning systems， 2011， 10（1）： 30?38.

[7] 穆華，潘獻(xiàn)飛，吳美平，等.基于步進(jìn)式運(yùn)動(dòng)模型的單兵協(xié)同導(dǎo)航算法設(shè)計(jì)[J].導(dǎo)航定位與授時(shí)，2021，8（1）：14?20.

[8] ELLOUMI W， GUISSOUS K， CHETOUANI A， et al. Indoor navigation assistance with a Smartphone camera based on vanishing points [C]// International Conference on Indoor Positioning and Indoor Navigation. New York： IEEE， 2013： 1?9.

[9] ANTIGNY N， SERVIèRES M， RENAUDIN V. Pedestrian track estimation with handheld monocular camera and inertial?magnetic sensor for urban augmented reality [C]// 2017 International Conference on Indoor Positioning and Indoor Navigation （IPIN）. New York： IEEE， 2017： 1?8.

[10] QIN T， LI P L， SHEN S J. VINS?Mono： A robust and versatile monocular visual?inertial state estimator [J]. IEEE transactions on robotics， 2017， 34（4）： 1?17.

[11] 莫善會(huì).GNSS與視覺里程計(jì)輔助IMU的行人導(dǎo)航方法研究[D].成都：電子科技大學(xué)，2019.

[12] CAMPOS C， ELVIRA R， RODRIGUEZ J J G， et al. ORB?SLAM3： An accurate open?source library for visual， visual?inertial and multi?map SLAM [J]. IEEE transactions on robotics， 2021， 37（6）： 1874?1890.

[13] 李凱林，李建勝，王安成.GNSS/INS/視覺組合導(dǎo)航數(shù)據(jù)融合研究探討[J].導(dǎo)航定位學(xué)報(bào)，2023，11（1）：9?15.

[14] 熊超，烏萌，劉宗毅，等.ORB?SLAM3車載雙目視覺自主導(dǎo)航性能評(píng)估[J].導(dǎo)航定位學(xué)報(bào)，2022，10（6）：9?17.

[15] 伍曉東，張松柏，湯適榮，等.基于改進(jìn)關(guān)鍵幀選擇的ORB?SLAM3算法[J].計(jì)算機(jī)應(yīng)用研究，2023，40（5）：1428?1433.