視覺慣性導(dǎo)航融合算法研究進(jìn)展

張宇 謝波 江一夫 王猛 王萌

摘 要：視覺慣性導(dǎo)航一直是無人駕駛與機(jī)器人領(lǐng)域重點(diǎn)、難點(diǎn)環(huán)節(jié)。首先介紹經(jīng)典算法框架，從基于最優(yōu)平滑算法和預(yù)積分理論兩個(gè)層面介紹經(jīng)典視覺慣性融合算法。然后介紹新型算法框架，主要在深度學(xué)習(xí)基礎(chǔ)上，從對整體框架學(xué)習(xí)的程度，將新型視覺慣性融合算法分為兩類，部分學(xué)習(xí)和整體學(xué)習(xí)，最后介紹了主流算法中關(guān)鍵技術(shù)與未來展望。

關(guān)鍵詞：視覺慣性導(dǎo)航；優(yōu)化；耦合算法；深度學(xué)習(xí)

0 引言

近些年來，伴隨著圖形處理技術(shù)、計(jì)算機(jī)視覺和元宇宙等相關(guān)技術(shù)的進(jìn)步，實(shí)現(xiàn)無人設(shè)備高精度、強(qiáng)魯棒性的定位導(dǎo)航始終為無人系統(tǒng)中的關(guān)鍵環(huán)節(jié)和待突破環(huán)節(jié)。美國國防研究與工程署連續(xù)數(shù)年組織機(jī)器人生存挑戰(zhàn)賽，以求在復(fù)雜環(huán)境下探索提升導(dǎo)航性能，確保美軍在無人領(lǐng)域的領(lǐng)先地位。中國科協(xié)也于2020年將無人車的高精度智能導(dǎo)航問題列為十大工程技術(shù)難題之一[1]。

無人系統(tǒng)導(dǎo)航領(lǐng)域一直以即時(shí)定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM）技術(shù)為核心。純視覺導(dǎo)航由于缺乏深度信息導(dǎo)致對運(yùn)動敏感薄弱，需要依賴回環(huán)檢測來校正誤差，這導(dǎo)致其魯棒性不足。視覺導(dǎo)航與衛(wèi)星導(dǎo)航系統(tǒng)的組合導(dǎo)航在復(fù)雜的城市場景下，光影變幻導(dǎo)致有效特征點(diǎn)選取難度變大，諸如地下車庫等場景對衛(wèi)星信號干擾強(qiáng)烈，而本文介紹的視覺慣性導(dǎo)航有著顯著的優(yōu)勢。

一是誤差的互補(bǔ)：視覺導(dǎo)航的穩(wěn)定位姿估計(jì)可以彌補(bǔ)慣性導(dǎo)航自身固存的累積誤差，慣性導(dǎo)航測量單元（Inertial Measurement Unit，IMU）高頻率的動態(tài)信號可以補(bǔ)足相機(jī)的深度信息；二是適用速度領(lǐng)域的 互補(bǔ)：視覺導(dǎo)航在低速靜態(tài)領(lǐng)域的優(yōu)秀表現(xiàn)，可以抑制IMU的零漂。而在高速運(yùn)動場景領(lǐng)域，慣性導(dǎo)航可以解決視覺導(dǎo)航由于圖像幀之間缺乏重疊區(qū)域，導(dǎo)致特征提取算法失效的問題。

1 經(jīng)典融合框架

經(jīng)典融合框架中，松耦合算法是視覺慣性領(lǐng)域早期重點(diǎn)研究內(nèi)容。松耦合算法主要依靠卡爾曼濾波器及其后續(xù)改進(jìn)版本，諸如應(yīng)用廣泛的擴(kuò)展卡爾曼濾波器（Extended Kalman Filter，EKF）、基于蒙特卡洛原理的無跡卡爾曼濾波器（Unscented Kalman Filter，UKF）、非線性非高斯領(lǐng)域表現(xiàn)不俗的粒子濾波器（Particle Filter，PF）等的松耦合方 案陸續(xù)提出。Kelly提出的無人機(jī)視覺慣性導(dǎo)航[2]缺少對傳感器信息缺乏交叉項(xiàng)誤差，無法全面利用傳感器信息，故精度有限。

自2010年起，慣性輔助視覺的緊耦合算法大量出現(xiàn)，由Davison等人提出的單目SLAM[3]開辟了緊耦合領(lǐng)域的先河。SLAM算法框架如圖1所示。

同一時(shí)間，Klein[4]等提出了平行跟蹤與建圖（Parallel Tracking And Mapping，PTAM）該文章也是視覺SLAM中里程碑式的文章，為視覺建圖開辟了新的路徑。由于早期文章均針對簡單場景建模，且傳感器采樣率低，特征點(diǎn)提取成功率有限[5]。之后相關(guān)研究跟進(jìn)，經(jīng)典融合框架發(fā)展迅速。自此以視覺傳感器為核心，慣導(dǎo)信息輔助優(yōu)化后端環(huán)節(jié)成為了主流研究方案。

H.Ranch、F.Tung與C.Striebel合作提出了固定區(qū)間最優(yōu)平滑算法[6]。該算法在數(shù)據(jù)后端處理中具有重要的應(yīng)用價(jià)值，在前向估計(jì)的基礎(chǔ)上加上反向?yàn)V波，充分利用了量測區(qū)間內(nèi)的所有量測值，具有更高的估計(jì)精度，所以主流的算法大都使用該理論作為航跡推算框架。

其中以擴(kuò)展卡爾曼濾波器為基礎(chǔ)的多態(tài)融合卡爾曼濾波器（Multi-State Constraint Kalman Filter，MSCKF）最具有代表性[7]，此算法最先由Mouriki和Roumeliotis于2007年提出，MSCKF最重要的突破點(diǎn)在于，解決了傳統(tǒng)松耦合算法在圖像信息豐富情景下，視覺算法選取特征點(diǎn)超出系統(tǒng)處理能力的問題。

2012年，預(yù)積分理論[8]的提出，為本領(lǐng)域研究帶來了深遠(yuǎn)影響。系統(tǒng)中為了減小優(yōu)化求解器的負(fù)擔(dān)，采用了關(guān)鍵幀策略，由于IMU的速率顯然要快于關(guān)鍵幀的插入，預(yù)積分干通過重新參數(shù)化，將關(guān)鍵幀間的IMU測量值積分成相對運(yùn)動的形式，避免了因?yàn)槌跏紬l件變化造成的無用信息。隨后Foster也將其拓展到李代數(shù)上，當(dāng)然也增大了預(yù)積分的計(jì)算量[9]。

在預(yù)積分理論基礎(chǔ)上，Raul Mur-Artal等人提出了ORB-SLAM算法。在此基礎(chǔ)上結(jié)合慣性技術(shù)，構(gòu)想出了一種視覺慣性快速旋轉(zhuǎn)二進(jìn)制描述子算法[10]（Visual Inertial-Oriented fast and Rotated Brief SLAM，VI-ORB SLAM）。VI-ORB算法對特征提取也做出了改進(jìn)，在BRIEF采點(diǎn)的基礎(chǔ)上提出了Fast采點(diǎn)方案，提出定義特征點(diǎn)方向，來實(shí)現(xiàn)特征點(diǎn)的旋轉(zhuǎn)不變形。

如果說ORB-SLAM是導(dǎo)航領(lǐng)域的一顆明珠，那么香港科技大學(xué)的沈劭劼的單目視覺慣性系統(tǒng)算法[12]（Monocular Visual-Inertial System，VINS-Mono）無疑是另一個(gè)代表性成果。VINS-Mono提出了一種健壯的基于緊耦合滑動窗口非線性優(yōu)化的單目視覺慣性里程計(jì)，初始化策略，包括對相機(jī)與IMU間的外參、數(shù)據(jù)間時(shí)間戳進(jìn)行在線校準(zhǔn)。該算法在前端的處理上使用光流追蹤特征點(diǎn)，當(dāng)讀取到新圖像后提取Harris角點(diǎn)[13]，然后使用光流法在更新環(huán)節(jié)迭代計(jì)算，最后利用RANSAC[14]算法對跟蹤錯(cuò)誤的特征點(diǎn)進(jìn)行剔除，其框架如如圖2所示。

總的來說，視覺慣性導(dǎo)航算法已經(jīng)經(jīng)過了長足的發(fā)展，在前端和后端上的大量改進(jìn)使得導(dǎo)航精度不斷提升，伴隨著性能的提升，其計(jì)算量也在大幅提升，對于實(shí)際應(yīng)用仍是一個(gè)考驗(yàn)。

2 新型融合框架

為提高圖像 識別準(zhǔn)確度，斯坦福大學(xué)李飛飛團(tuán)隊(duì)舉辦了圖像識別挑戰(zhàn)賽，Alex等人設(shè)計(jì)的Alex Net模型[15]以領(lǐng)先第2名近10%的準(zhǔn)確度奪冠，迅速席卷了深度學(xué)習(xí)領(lǐng)域，使得卷積神經(jīng)網(wǎng)絡(luò)聲名大噪，隨后的卷積神經(jīng)網(wǎng)路的快速算法也廣泛應(yīng)用于本領(lǐng)域[16]。

Huang提出了使用循環(huán)神經(jīng)網(wǎng)絡(luò)（Recurrent Neural Network，RNN）網(wǎng)絡(luò)學(xué)習(xí)光流法的視覺里程計(jì)[17]，該算法就是一種典型的用深度學(xué)習(xí)替代前端視覺里程計(jì)的計(jì)算過程的方法。作者立足于單目視覺里 程計(jì)，提出了用于估計(jì)相機(jī)的自我運(yùn)動的序列到序列算法。該網(wǎng)絡(luò)學(xué)習(xí)光流的潛在空間并建立圖像序列動力學(xué)模型，并且計(jì)算連續(xù)圖像的光流場，最終以自編碼方式提取潛在的光流表示。

Li提出了端到端EKF的深度視覺-慣導(dǎo)里程計(jì)[18]（Towards End-to-end Learning of Visual Inertial Odometry with an EKF）。在耦合思路上仿照專家系統(tǒng)，將傳統(tǒng)算法中表現(xiàn)良好的環(huán)節(jié)予以保留。采用深度學(xué)習(xí)得到的姿態(tài)構(gòu)建量測方程，整體上采用RTS算法模型，在正反雙方向的計(jì)算上優(yōu)化結(jié)果。在數(shù)據(jù)集上測試并證明了系統(tǒng)的有效性。

牛津大學(xué)計(jì)算機(jī)科學(xué)系Clark團(tuán)隊(duì)提出了一種基于流形的序列訓(xùn)練方法VINet[19]。其改進(jìn)之處是在中間特征表示水平上進(jìn)行數(shù)據(jù)融合。作者提出使用長短期記憶（Long Short-Term Memory，LSTM）來彌補(bǔ)RNN網(wǎng)絡(luò)對全局信息的把握不足。VINet整體框圖如圖3所示。

除此以外，Chen提出了一種新穎的端對端選擇性傳感器融合框架[20]，主要應(yīng)用于復(fù)雜環(huán)境下數(shù)據(jù)丟失問題，以及處理野值的框架。由于單目相機(jī)圖像信息較少，且缺乏深度信息，在預(yù)測中常出現(xiàn)誤差較大的問題。故構(gòu)架了不同掩蔽策略的方案。將丟失信息的程度劃分為兩類工作環(huán)境，對應(yīng)不同環(huán)境采用不同的策略。結(jié)合故障診斷思想，分為軟、硬兩種融合方案。后驗(yàn)型視覺慣性導(dǎo)航算法對比表如表1所示。

綜合來看，深度學(xué)習(xí)在純視覺SLAM方向廣泛使用，在視覺慣性領(lǐng)域仍有較大可發(fā)展空間。對于深度學(xué)習(xí)在整體框架中使用的比例不盡相同，整體使用框架雖然性能更好但是計(jì)算量過大，局部使用計(jì)算量更小但是性能表現(xiàn)稍弱。

3 展望未來

3.1 多傳感器融合

視覺與慣性耦合算法在機(jī)器人領(lǐng)域取得了顯著的成果，在自動駕駛領(lǐng)域，多傳感器融合算法已經(jīng)得到了廣泛應(yīng)用，諸如全球定位系統(tǒng)（Global Positioning System，GPS）等等。

Lee提出一種有效而且魯棒的GPS輔助的視覺慣導(dǎo)里程計(jì)系統(tǒng)（GPS-VIO）[21]，該系統(tǒng)使用低頻的GPS測量數(shù)據(jù)融合相機(jī)IMU數(shù)據(jù)，作者提出一種在線標(biāo)定的方法對于外參、時(shí)間偏移以及對傳感器噪聲具有魯棒性的參考幀初始化步驟。香港大學(xué)張富團(tuán)隊(duì)提出了一種快速、穩(wěn)健且通用的激光慣性里程計(jì)框架[22]。創(chuàng)新之處是不提取特征直接將原始點(diǎn)配準(zhǔn)到地圖，而這使得環(huán)境中的細(xì)微特征能夠被使用.

3.2 仿生導(dǎo)航

仿生導(dǎo)航作為一種多學(xué)科交叉的新興學(xué)科。動物行為學(xué)和生理學(xué)的研究成果對新型科技領(lǐng)域如計(jì)算機(jī)、通信領(lǐng)域一直有促進(jìn)推動作用。Hao結(jié)合了昆蟲復(fù)眼結(jié)構(gòu)和人類視覺器官結(jié)構(gòu)，研制了一種混合型的仿生復(fù)眼系統(tǒng)[23]。國防科技大學(xué)胡小平教授團(tuán)隊(duì)研制了六通道點(diǎn)測量式仿生光羅盤[24]。Milford等基于吸引子網(wǎng)絡(luò)模型，構(gòu)建了包含位置細(xì)胞和方向細(xì)胞模型的RatSLAM算法[25]。

4 結(jié)束語

將視覺導(dǎo)航系統(tǒng)與慣性導(dǎo)航系統(tǒng)融合，互相取長補(bǔ)短，加以耦合的算法使低成本傳感器獲得了高精度和高穩(wěn)定性，讓整個(gè)系統(tǒng)邁向小型化、輕量化。視覺慣性導(dǎo)航是一個(gè)多學(xué)科交叉，對算法和硬件要求都很高的綜合性平臺。其進(jìn)步必然得益于算法效率的提升與計(jì)算機(jī)算力的進(jìn)步。所以仍需要后繼者開闊思路，進(jìn)一步探索與挖掘。

參考文獻(xiàn)：

[1] 中國科學(xué)技術(shù)協(xié)會.面向未來的科技—2020重大科學(xué)問題和工程技術(shù)難題解讀[M]. 北京：中國科學(xué)技術(shù)出版社，2020.

[2] KELLY J， SUKHATME G S. Visual-inertial sensor fusion： localization， mapping and sensor-to-sensor selfcalibration[J].International Journal of Robotics Research， 2011， 30（1）： 56-79.

[3] A. J. DAVISON， I. D. Reid， N. D. MOLTON. Mono SLAM： Real-time single camera SLAM[J].IEEE Transactions on Pattern Analysis and Machine Intelligence， 2007，29（6）：1052-1067.

[4] KLEIN G， MURRAY D. Parallel tracking and mapping for small AR work-spaces [C].// Proc of the 6th IEEE/ACM International Symposium on Mixed and Augmented Reality. Washington DC： IEEE Computer Society， 2007.

[5] CARRILLO L R. LOPEZ A E. Lozano R. et al. Combining stereo vision and inertial navigation system for a quad-rotor UAV [J].Journal of Intelligent and Robotic Systems， 2012， 65 （1） ： 373-387.

[6] 嚴(yán)恭敏，鄧瑀.傳統(tǒng)組合導(dǎo)航中的實(shí)用Kalman濾波技術(shù)評述[J].導(dǎo)航定位與授時(shí)，2020，7（2）： 19-24.

[7] MOURIKIS A I， ROUMELIOTIS S I. A multistate constraint Kalman filter for vision-aided inertial navigation[C].// Proceedings of IEEE International Conference on Robotics and Automation. Piscataway， USA：IEEE， 2007.

[8] LUPTON T， SUKKARIEH S. Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions[J]. IEEE Transactions on Robotics， 2012， 28（1） ：61-76.

[9] FORSTER C， CARLONE L， DELLAERT F， et al. IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation[C]. Proceedings of Robotics Science and Systems，2015.

[10] MUR-ARTAL R， TARDOS J D. Visual-inertial monocular SLAM with map reuse[J]. IEEE Robotics and Automation Letters， 2017，2（2）：796-803.

[11] CARLOS CAMPOS， RICHARD ELVIRA. An accurate open-source library for visual， visual-inertial and multi-map SLAM[C].IEEE Transactions on Robotics 37（6）：1874-1890， Dec.2021.

[12] SHEN S J， MICHAEL N， KUMAR V. Tightly-coupled monocular visual-inertial fusion for autonomous flight of rotorcraft MAVs[C].IEEE International Conference on Robotics and Automation. Piscataway， USA：IEEE 2015：5303-5310. DOI： 10.1145/3065386.

[13] SEMMA ABDELILLAH， HANNAD YAACOUB et al. Writer identification using deep learning with FAST key points and Harris corner detector[J].Expert Systems With Applications，2021，184.

[14] PAN Z T， WEN Z W. Analysis of image registration algorithm RANSAC and its improved algorithm[J]. IOP Conference Series： Earth and Environmental Science，2021（1）：783.

[15] ALEX K， ILYA S， GEOFFREY E H. Image net classification with deep convolutional neural networks[J]. Communications of The ACM. 2017.

[16] A.LAVIN， S.GRAY. Fast algorithms for convolutional neural networks[C].// Proceedings of Computer Vision and Pattern Recognition （CVPR）， 2016.

[17] ZHAN H， C.S.WEERASEKERA， J.W.BIAN. Visual odometry revisited： what should be learnt？[C].International Conference on Robotics and Automation （ICRA）， 2020.

[18] LI C， S.L.WASLANDER. Towards end-to-end learning of visual inertial odometry with an EKF[C].Conference on Computer and Robot Vision （CRV）， 2020：190-197.

[19] CLARK R， WANG S， WEN H， et al. VINet： visual inertial odometry as a sequence to sequence learning problem[C]. Proceedings of International Conference on Artificial Intelligence， 2017.

[20] CHEN C， ROSA S， MIAO Y， et al. Selective sensor fusion for neural visual inertial odometry[C].Proceedings of Computer Vision and Pattern Recognition （CVPR）， 2019.

[21] LEE W， K. ECKENHOFF. Intermittent GPS-aided VIO： online initialization and calibration[C].International Conference on Robotics and Automation （ICRA）， 2020.

[22] XU W， CAI Y. FAST-LIO2： fast direct Li DAR-inertial odometry[C].Transactions on Robotics. DOI： 10.1109.

[23] HAO Q， WANG Z， CAO J， et al. A hybrid bionic image sensor achieving FOV extension and foveated imaging[J]. Sensors， 2018， 18（4）：1042.

[24] XIAN Z， HU X， LIAN J， et al. A novel angle computation and calibration algorithm of bio-inspired sky-light polarization navigation sensor[J].Sensors，2014，14（9）：17068-17088.

[25] MILFORD M J， WYETH G F. Mapping a suburb with a single camera using a biologically inspired SLAM system[J]. IEEE Transactions on Robotics， 2008， 24（5）：1038-1053.

[26] S.W.CHEN et al. SLOAM： semantic Lidar odometry and Mapping for Forest Inventory[J]. Robotics and Automation Letters， 2020.

[27] WANG H， XUE C， ZHOU Y et al. Visual semantic localization based on HD map for autonomous vehicles in urban scenarios. International Conference on Robotics and Automation （ICRA）， 2021.