張禮廉，屈 豪，毛 軍，胡小平

(國(guó)防科技大學(xué)智能科學(xué)學(xué)院，長(zhǎng)沙 410073)

0 引言

隨著無(wú)人機(jī)、無(wú)人車以及移動(dòng)機(jī)器人的井噴式發(fā)展，導(dǎo)航技術(shù)成為了制約無(wú)人平臺(tái)廣泛應(yīng)用的瓶頸技術(shù)之一。在應(yīng)用需求的牽引下，視覺/慣性組合導(dǎo)航技術(shù)，特別是視覺與微慣性傳感器的組合，逐漸發(fā)展成為當(dāng)前自主導(dǎo)航及機(jī)器人領(lǐng)域的研究熱點(diǎn)。本文介紹的視覺/慣性組合導(dǎo)航技術(shù)側(cè)重于利用視覺和慣性信息估計(jì)載體的位置、速度、姿態(tài)等運(yùn)動(dòng)參數(shù)以及環(huán)境的幾何結(jié)構(gòu)參數(shù)，而不包含場(chǎng)景障礙物檢測(cè)以及載體運(yùn)動(dòng)軌跡規(guī)劃等。

視覺/慣性組合導(dǎo)航具有顯著的優(yōu)點(diǎn)：1)微慣性器件和視覺傳感器具有體積小、成本低的優(yōu)點(diǎn)，隨著制造技術(shù)的不斷進(jìn)步，器件越來(lái)越小，且成本越來(lái)越低。2)不同于衛(wèi)星和無(wú)線電導(dǎo)航，視覺和慣性導(dǎo)航均不依賴外部設(shè)施支撐，可以實(shí)現(xiàn)自主導(dǎo)航。3)慣性器件和視覺器件具有很好的互補(bǔ)性，慣性導(dǎo)航誤差隨時(shí)間累積，但是在短時(shí)間內(nèi)可以很好地跟蹤載體快速運(yùn)動(dòng)，保證短時(shí)間的導(dǎo)航精度；而視覺導(dǎo)航在低動(dòng)態(tài)運(yùn)動(dòng)中具有很高的估計(jì)精度，且引入了視覺閉環(huán)矯正可以極大地抑制組合導(dǎo)航誤差，兩者的組合可以更好地估計(jì)導(dǎo)航參數(shù)。

視覺和慣性組合導(dǎo)航技術(shù)近年來(lái)取得了長(zhǎng)足的發(fā)展。孫永全和田紅麗[1]從同步定位與構(gòu)圖(Simultaneous Localization and Mapping, SLAM)的角度對(duì)視覺/慣性組合導(dǎo)航技術(shù)的基本原理和標(biāo)志性成果進(jìn)行了詳細(xì)分析。Huang[2]對(duì)基于濾波技術(shù)的視覺/慣性組合導(dǎo)航技術(shù)進(jìn)行了全面的描述，特別是對(duì)濾波器的可觀性和濾波狀態(tài)的一致性問題進(jìn)行了深入的探討。Huang和Zhao等[3]對(duì)基于激光和視覺傳感器的SLAM技術(shù)進(jìn)行了全面的介紹，該文引用的文獻(xiàn)十分全面，但缺乏基本原理的闡述。當(dāng)前隨著基于機(jī)器學(xué)習(xí)的視覺/慣性組合導(dǎo)航算法性能不斷提高，部分算法已達(dá)到甚至超過傳統(tǒng)的基于模型的組合導(dǎo)航算法性能。因此，非常有必要按照基于模型的算法和基于機(jī)器學(xué)習(xí)的算法對(duì)視覺/慣性組合導(dǎo)航技術(shù)進(jìn)行詳細(xì)的分析。

1 視覺/慣性組合導(dǎo)航技術(shù)發(fā)展簡(jiǎn)述

傳統(tǒng)的基于視覺幾何與運(yùn)動(dòng)學(xué)模型的視覺和慣性導(dǎo)航技術(shù)研究成果非常豐富。本文主要從純視覺導(dǎo)航以及組合導(dǎo)航2個(gè)層次梳理相關(guān)工作。

純視覺導(dǎo)航技術(shù)主要有2個(gè)分支：一個(gè)分支是視覺里程計(jì)(Visual Odometry，VO)技術(shù)；而另一個(gè)分支是視覺同步定位與構(gòu)圖(Visual Simultaneous Localization and Mapping，VSLAM)技術(shù)。Scaramuzza教授[4-5]對(duì)早期的VO技術(shù)進(jìn)行了詳細(xì)的介紹，并闡述了VO技術(shù)與VSLAM技術(shù)的區(qū)別與聯(lián)系：VO側(cè)重于利用連續(xù)圖像幀之間的位姿增量進(jìn)行路徑積分，至多包含滑動(dòng)窗口內(nèi)的局部地圖；VSLAM側(cè)重于全局路徑和地圖的優(yōu)化估計(jì)，支持重定位和閉環(huán)優(yōu)化；通常VO可以作為VSLAM算法框架的前端。

目前，視覺里程計(jì)可以根據(jù)使用相機(jī)個(gè)數(shù)的不同分為單目、雙目和多目視覺里程計(jì)。其中最具有代表性和影響力的主要有三種算法，分別是視覺里程計(jì)庫(kù)(Library for Visual Odometry，LIBVISO)[6]、半直接單目視覺里程計(jì)(Semi-Direct Monocular Visual Odometry，SVO)[7]和直接稀疏里程計(jì)(Direct Sparse Odometry，DSO)[8]。這三種算法由于代碼公開，易于使用，運(yùn)動(dòng)估計(jì)效果好，成為了研究者們廣泛使用和對(duì)比的算法。

對(duì)于VSLAM算法，目前主流的方法可以分為兩類：一類是基于濾波的方法；另一類是基于Bundle Adjustment的優(yōu)化算法。這兩類方法的開創(chuàng)性成果分別是Davison教授提出的Mono SLAM算法[9]和Klein博士提出的并行跟蹤與構(gòu)圖(Parallel Tracking And Mapping，PTAM)算法[10]。在2010年國(guó)際機(jī)器人和自動(dòng)化大會(huì)(IEEE International Conference on Robotics and Automation，ICRA)上，Strasdat的文章[11]指出優(yōu)化算法比濾波算法的性價(jià)比更高，從此以后基于非線性優(yōu)化的VSLAM算法就漸漸多起來(lái)。其中代表性的工作是ORB-SLAM[12]和LSD-SLAM[13]，二者的主要區(qū)別是ORB-SLAM的前端采用稀疏特征，而LSD-SLAM的前端采用稠密特征。

當(dāng)然，任何純視覺導(dǎo)航算法都存在無(wú)法避免的固有缺點(diǎn)：依賴于場(chǎng)景的紋理特征、易受光照條件影響以及難以處理快速旋轉(zhuǎn)運(yùn)動(dòng)等。因此，為了提高視覺導(dǎo)航系統(tǒng)的穩(wěn)定性，引入慣性信息是很好的策略。

視覺/慣性組合導(dǎo)航技術(shù)與VSLAM算法類似，主要采用兩種方案：一種是采用濾波技術(shù)融合慣性和視覺信息；另一種是采用非線性迭代優(yōu)化技術(shù)融合慣性和視覺信息。

基于濾波技術(shù)的視覺/慣性組合導(dǎo)航算法，可以進(jìn)一步分為松組合和緊組合兩種框架。文獻(xiàn)[14-15]使用了卡爾曼濾波器來(lái)融合雙目相機(jī)和慣性器件輸出。作為一種松組合方式，組合中沒有充分使用慣性器件的輸出來(lái)輔助圖像特征點(diǎn)的匹配、跟蹤與野值剔除。2007年，Veth提出了一種視覺輔助低精度慣性導(dǎo)航的方法[16]。該算法使用了多維隨機(jī)特征跟蹤方法，其最大的缺點(diǎn)是跟蹤的特征點(diǎn)個(gè)數(shù)必須保持不變。同年，Mourikis提出了基于多狀態(tài)約束的卡爾曼濾波器 (Multi-State Constraint Kalman Filter，MSCKF)算法[17]，其優(yōu)點(diǎn)是在觀測(cè)模型中不需要包含特征點(diǎn)的空間位置；但是MSCKF算法中存在濾波估計(jì)不一致問題：不可觀的狀態(tài)產(chǎn)生錯(cuò)誤的可觀性，如航向角是不可觀的，但MSCKF通過擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF)線性化后會(huì)使航向角產(chǎn)生錯(cuò)誤的可觀性。為了解決濾波估計(jì)不一致問題，李明陽(yáng)等[18]提出了首次估計(jì)雅可比EKF(the First Estimate Jacobian EKF，F(xiàn)EJ-EKF)算法；Huang等[19]提出了基于可觀性約束的無(wú)跡卡爾曼濾波(Unscented Kalman Filte，UKF)算法；Castellanos等[20]提出了Robocentric Mapping 濾波算法。這些算法均在一定程度上解決了濾波估計(jì)不一致問題。

2015年，Bloesch等提出了魯棒視覺慣性里程計(jì)(Robust Visual Inertial Odometry，ROVIO)[21]，該算法利用EKF將視覺信息和慣性測(cè)量單元(Inertial Measurement Unit，IMU)信息進(jìn)行緊耦合，在保持精度的同時(shí)降低了計(jì)算量。Indelman等基于EKF，綜合利用了2幅圖像間的對(duì)極約束和3幅圖像之間的三視圖約束融合單目相機(jī)和慣性器件[22]。基于相同的觀測(cè)模型，Hu等給出了基于UKF的實(shí)現(xiàn)方法[23]。

近年來(lái)，基于優(yōu)化的算法得到了快速發(fā)展。Lupton和Sukkarieh于2012年首次提出了利用無(wú)初值條件下的慣性積分增量方法來(lái)解決高動(dòng)態(tài)條件下的慣性視覺組合導(dǎo)航問題[24]。文中采用了Sliding Window Forced Independence Smoothing技術(shù)優(yōu)化求解狀態(tài)變量。預(yù)積分理論的建立，使得基于優(yōu)化的視覺/慣性組合導(dǎo)航算法得以實(shí)現(xiàn)。受此思想啟發(fā)，Stefan等采用Partial Marginalization技術(shù)，通過優(yōu)化非線性目標(biāo)函數(shù)來(lái)估計(jì)滑動(dòng)窗口內(nèi)關(guān)鍵幀的位姿參數(shù)[25]。其中，目標(biāo)函數(shù)分為視覺約束和慣性約束2個(gè)部分：視覺約束由空間特征點(diǎn)的重投影誤差表示，而慣性約束由IMU運(yùn)動(dòng)學(xué)中的誤差傳播特性表示。該方法不適用于長(zhǎng)航時(shí)高精度導(dǎo)航，因?yàn)闆]有閉環(huán)檢測(cè)功能，無(wú)法修正組合導(dǎo)航系統(tǒng)的累積誤差。2017年，F(xiàn)orster等完善了計(jì)算關(guān)鍵幀之間慣性積分增量的理論，將該理論擴(kuò)展到Rotation Group，并分析了其誤差傳播規(guī)律[26]。該算法也未考慮閉環(huán)檢測(cè)問題。同樣基于預(yù)積分理論，沈劭劼課題組提出了視覺慣性導(dǎo)航(Visual-Inertial Navigation System, VINS)算法[27]。該算法具備自動(dòng)初始化、在線外參標(biāo)定、重定位、閉環(huán)檢測(cè)等功能。ORB-SLAM的設(shè)計(jì)者M(jìn)ur-Artal等利用預(yù)積分理論，將慣性信息引入ORB-SLAM框架，設(shè)計(jì)了具有重定位和閉環(huán)檢測(cè)等功能的視覺/慣性組合導(dǎo)航算法[28]。關(guān)于預(yù)積分理論，目前還缺乏積分增量合并以及相應(yīng)的協(xié)方差矩陣合并方法。因此，文獻(xiàn)[28]去掉了ORB-SLAM中的關(guān)鍵幀刪除功能。表1匯總了基于視覺幾何與運(yùn)動(dòng)學(xué)模型的視覺和慣性導(dǎo)航技術(shù)的主要研究成果。

基于模型的視覺/慣性組合導(dǎo)航技術(shù)需要信噪比較高的輸入數(shù)據(jù)，算法的整體性能不僅受制于算法的基本原理，還取決于參數(shù)的合理性與精確度。相對(duì)而言，深度學(xué)習(xí)神經(jīng)網(wǎng)絡(luò)能夠通過大數(shù)據(jù)訓(xùn)練的方式自適應(yīng)地調(diào)節(jié)參數(shù)，對(duì)輸入數(shù)據(jù)具有一定的容錯(cuò)性，因此已有研究人員開發(fā)了一系列基于深度學(xué)習(xí)的視覺/慣性組合導(dǎo)航技術(shù)，并已取得一定成果。

使用深度學(xué)習(xí)神經(jīng)網(wǎng)絡(luò)替換傳統(tǒng)算法中的個(gè)別模塊是較為直接的算法設(shè)計(jì)思路，如利用深度學(xué)習(xí)神經(jīng)網(wǎng)絡(luò)實(shí)現(xiàn)里程計(jì)前端中的特征點(diǎn)識(shí)別與匹配。Detone等[29]提出了SuperPoint算法，該算法首先使用虛擬三維物體的角點(diǎn)作為初始訓(xùn)練集，并將特征點(diǎn)提取網(wǎng)絡(luò)在此數(shù)據(jù)集上進(jìn)行訓(xùn)練；對(duì)經(jīng)過訓(xùn)練的網(wǎng)絡(luò)在真實(shí)場(chǎng)景訓(xùn)練集中進(jìn)行檢測(cè)得到自標(biāo)注點(diǎn)，并將標(biāo)注有自標(biāo)注點(diǎn)的真實(shí)場(chǎng)景圖像進(jìn)行仿射變化得到匹配的自標(biāo)注點(diǎn)對(duì)，從而得到了最終的訓(xùn)練集；隨后使用對(duì)稱設(shè)計(jì)的特征點(diǎn)識(shí)別網(wǎng)絡(luò)，將特征提取器讀入的原始圖像經(jīng)過多層反卷積層轉(zhuǎn)換為特征點(diǎn)響應(yīng)圖像，響應(yīng)區(qū)域?yàn)橄噜弾瑘D像匹配特征點(diǎn)的位置。幾何對(duì)應(yīng)網(wǎng)絡(luò)(Geometric Correspondence Network, GCN)[30]則是利用相對(duì)位姿標(biāo)簽值構(gòu)建的幾何誤差作為匹配特征點(diǎn)空間位置估計(jì)值的約束；隨后使用多視覺幾何模型結(jié)合低層特征提取前端網(wǎng)絡(luò)得到的匹配特征點(diǎn)，求解載體的運(yùn)動(dòng)信息。此類低層特征提取前端易于與傳統(tǒng)實(shí)時(shí)定位與建圖系統(tǒng)相結(jié)合，并且較為輕量，可植入嵌入式平臺(tái)進(jìn)行實(shí)時(shí)解算。

另一種思路是使用深度學(xué)習(xí)神經(jīng)網(wǎng)絡(luò)實(shí)現(xiàn)從原始數(shù)據(jù)到導(dǎo)航參數(shù)的整個(gè)轉(zhuǎn)化過程。Kendall團(tuán)隊(duì)基于圖像識(shí)別網(wǎng)絡(luò)GoogleNet[31]開發(fā)了一種基于單張圖像信息的絕對(duì)位姿估計(jì)網(wǎng)絡(luò)PoseNet[32]。首先，搭建絕對(duì)位姿回歸數(shù)據(jù)集，配合高精度姿態(tài)捕捉設(shè)備，為單目相機(jī)拍攝的每一幀圖像標(biāo)注絕對(duì)位姿標(biāo)簽值；然后使用多層全連接層替換GoogleNet的多個(gè)softmax層，并構(gòu)成位姿回歸器，回歸器的輸出維度與使用歐拉角表示的位姿維數(shù)相同；通過長(zhǎng)時(shí)間的訓(xùn)練，PoseNet能較為準(zhǔn)確地將訓(xùn)練數(shù)據(jù)集圖像投影為對(duì)應(yīng)位姿標(biāo)簽，然而沒有額外的幾何約束，網(wǎng)絡(luò)收斂較為困難。Wang等在位姿估計(jì)網(wǎng)絡(luò)中引入相鄰幀圖像信息，構(gòu)建基于深度學(xué)習(xí)的單目視覺里程計(jì)DeepVO[33]，為了能夠同時(shí)處理相鄰兩幀圖像的信息，將FlowNet[34]網(wǎng)絡(luò)的主體作為視覺特征提取器，并使用輸入窗口大于1的長(zhǎng)短時(shí)記憶(Long Short Term Memory, LSTM)網(wǎng)絡(luò)聯(lián)合時(shí)間軸上相鄰多幀圖像的高層信息，以此來(lái)優(yōu)化里程計(jì)短時(shí)間內(nèi)的估計(jì)精度；最后使用全連接層綜合圖像高層信息，并轉(zhuǎn)化為相鄰幀圖像的相對(duì)位姿估計(jì)值。實(shí)驗(yàn)結(jié)果表明，DeepVO相對(duì)于早期基于模型的視覺里程計(jì)LIBVISO[6]性能具有一定提升，同時(shí)與同類型算法[35]相比，也有明顯的性能提升。

與基于模型的視覺/慣性組合導(dǎo)航技術(shù)類似，為了提高導(dǎo)航算法的自主性與抗干擾能力，研究人員在基于深度學(xué)習(xí)的視覺導(dǎo)航技術(shù)中引入慣導(dǎo)數(shù)據(jù)，并為其設(shè)計(jì)單獨(dú)的網(wǎng)絡(luò)來(lái)提取有用的數(shù)據(jù)特征。牛津大學(xué)的Clark團(tuán)隊(duì)設(shè)計(jì)了一種端對(duì)端的視覺/慣性組合里程計(jì)網(wǎng)絡(luò)VINet[36]，使用雙向光流提取網(wǎng)絡(luò)FlowNet-Corr[34]提取相鄰幀圖像的高層特征，使用單層全連接層對(duì)圖像高層特征進(jìn)行壓縮，并使用多節(jié)點(diǎn)LSTM網(wǎng)絡(luò)處理兩幀圖像間的慣性信息；隨后將兩種數(shù)據(jù)的高層特征在單維度上進(jìn)行結(jié)合，構(gòu)成視覺/慣性信息融合特征；最后使用全連接層將融合特征投影至SE(3)空間中，得到相對(duì)位姿估計(jì)值。VINet在道路與無(wú)人機(jī)數(shù)據(jù)中都顯示出較為優(yōu)秀的性能，同時(shí)為基于深度學(xué)習(xí)的組合導(dǎo)航技術(shù)提供了基礎(chǔ)模板。

陳昶昊于2019年提出了基于注意力模型的視覺/慣性組合里程計(jì)網(wǎng)絡(luò)Attention-based VIO[37]，網(wǎng)絡(luò)的基本框架與VINet類似，但視覺特征提取器使用更為輕量的FlowNetsimple[34]卷積層，以此來(lái)提高網(wǎng)絡(luò)運(yùn)行效率。借鑒自然語(yǔ)言處理領(lǐng)域的注意力機(jī)制，使用soft attention和hard attention兩種注意力網(wǎng)絡(luò)剔除融合特征中的噪聲高層特征，從而加快訓(xùn)練收斂,提高網(wǎng)絡(luò)性能。表2匯總了基于機(jī)器學(xué)習(xí)的視覺/慣性組合導(dǎo)航技術(shù)的主要研究成果。

表2 基于機(jī)器學(xué)習(xí)的視覺/慣性組合導(dǎo)航技術(shù)Tab.2 Learning based visual-inertial integrated navigation

在國(guó)內(nèi)，清華大學(xué)、上海交通大學(xué)、浙江大學(xué)、哈爾濱工程大學(xué)、國(guó)防科技大學(xué)、北京航空航天大學(xué)、北京理工大學(xué)、南京航空航天大學(xué)、西北工業(yè)大學(xué)、電子科技大學(xué)、中國(guó)科學(xué)院自動(dòng)化研究所等高校和科研機(jī)構(gòu)的多個(gè)研究團(tuán)隊(duì)近年來(lái)在慣性/視覺組合導(dǎo)航領(lǐng)域開展了系統(tǒng)性的研究工作，取得了諸多研究成果[38-44]。

2 基于模型的視覺/慣性組合導(dǎo)航技術(shù)

基于模型的視覺/慣性組合導(dǎo)航技術(shù)的通用結(jié)構(gòu)示意圖如圖1所示。

圖1 基于模型的視覺/慣性組合導(dǎo)航技術(shù)通用結(jié)構(gòu)示意圖Fig.1 Scheme of model based visual-inertial navigation technology

2.1 基于模型的純視覺導(dǎo)航算法

基于模型的視覺導(dǎo)航算法是指以多視圖幾何等數(shù)學(xué)模型構(gòu)建的VO和VSLAM算法。

(1)視覺里程計(jì)原理

載體在運(yùn)動(dòng)過程中，可以通過與其固聯(lián)的攝像機(jī)獲取圖像流。由于載體運(yùn)動(dòng)，同一個(gè)靜止的物體在不同幀圖像中的成像位置將發(fā)生變化。根據(jù)攝像機(jī)的成像幾何模型，可以利用同一物體在不同幀圖像中成像位置的關(guān)系，恢復(fù)出相機(jī)在拍攝圖像時(shí)的位置和姿態(tài)變化量。然后，將相鄰幀圖像的位置和姿態(tài)變化量進(jìn)行積分，可以推算出攝像機(jī)運(yùn)行的軌跡，如圖2所示。

圖2 多視圖幾何示意圖Fig.2 Scheme of multi-view geometry

攝像機(jī)的成像模型是從多視圖中恢復(fù)出載體運(yùn)動(dòng)參數(shù)的基礎(chǔ)。常用的相機(jī)模型包括透視模型(perspective model)、全景模型(omnidirectional model)和球形模型(spherical model)等。攝像機(jī)模型可以通過觀察棋盤格或二維碼等特征固定且尺度大小已知的物體進(jìn)行離線標(biāo)定。

視覺里程計(jì)根據(jù)特征利用的方式可以分為間接法和直接法兩類。間接法通過最小化同一特征在不同圖像中的位置投影誤差來(lái)解算攝像機(jī)的運(yùn)動(dòng)參數(shù)；而直接法則基于光度(灰度)不變假設(shè)，通過最小化同一特征在不同圖像中的光度誤差來(lái)估計(jì)攝像機(jī)的運(yùn)動(dòng)參數(shù)。

間接法視覺里程計(jì)首先需要建立特征匹配關(guān)系，然后根據(jù)特征匹配對(duì)之間的坐標(biāo)關(guān)系，解算出相機(jī)的運(yùn)動(dòng)參數(shù)。設(shè)載體在運(yùn)動(dòng)過程中，攝像機(jī)拍攝了n幅圖像，表示為I1∶n={I1,…,In}；同時(shí)，在導(dǎo)航環(huán)境中有m個(gè)特征，特征的空間坐標(biāo)為p1∶m={p1,…,pm} ；第j個(gè)特征在k時(shí)刻拍攝圖像中的坐標(biāo)為zk,j=πk(pj)，其中πk表示相機(jī)在k時(shí)刻的投影模型，其與相機(jī)的成像模型和相機(jī)的位姿相關(guān)。

首先,通過特征匹配算法建立特征之間的對(duì)應(yīng)關(guān)系{zk,j?zk+1,j}，間接法視覺里程的運(yùn)動(dòng)估計(jì)可以表示為最小化如下誤差函數(shù)的過程

(1)

圖3 重投影誤差示意圖Fig.3 Scheme of reprojection error

與間接法不同，直接法視覺里程計(jì)則通過最小化光度誤差估計(jì)攝像機(jī)的運(yùn)動(dòng)參數(shù)。通常，同一特征在短時(shí)間內(nèi)拍攝的多幅圖像中，其光度基本不變，并且攝像機(jī)在短時(shí)間內(nèi)的位姿變化較小，同一特征在相鄰幀圖像中的成像位置變化不大。據(jù)此，直接法視覺里程計(jì)通過迭代優(yōu)化算法在狀態(tài)空間中進(jìn)行搜索，使得同一特征在不同圖像中的像點(diǎn)光度誤差最小，從而解算得到攝像機(jī)運(yùn)動(dòng)參數(shù)，具體優(yōu)化目標(biāo)函數(shù)為

(2)

其中，Ik(pj)和Ik+1(pj)分別表示同一特征在相鄰幀圖像中的光度。若直接法里程計(jì)在運(yùn)動(dòng)估計(jì)過程中使用了整幅圖像的像點(diǎn)光度，則為稠密視覺里程計(jì)算法；若僅使用部分像點(diǎn)光度，則為稀疏視覺里程計(jì)算法。由慕尼黑工業(yè)大學(xué)開發(fā)的DSO算法[8]就是一種稀疏直接法視覺里程計(jì)。

除直接法與間接法里程計(jì)外，F(xiàn)orster等還提出了一種半直接法視覺里程計(jì)[7]。在SVO中使用了直接法進(jìn)行運(yùn)動(dòng)解算，同時(shí)采用了間接法來(lái)估計(jì)特征的三維坐標(biāo)，建立局部地圖。

(2)閉環(huán)優(yōu)化與構(gòu)圖

視覺里程計(jì)是一種路徑積分方法，因此具有累積誤差。閉環(huán)優(yōu)化是廣泛使用的一種用于修正視覺里程計(jì)累積誤差的方法。閉環(huán)修正依賴于構(gòu)建的環(huán)境地圖，其基本原理是：載體在移動(dòng)過程中，將觀測(cè)的視覺特征與地圖中的視覺特征進(jìn)行匹配，并通過匹配關(guān)系解算出載體在地圖中所處的位置和姿態(tài)。由于建圖誤差和視覺里程計(jì)累積誤差的影響，通過里程計(jì)估計(jì)的攝像機(jī)位姿與通過閉環(huán)檢測(cè)估計(jì)的攝像機(jī)位姿之間具有差異，通過建立數(shù)學(xué)模型可以同時(shí)對(duì)里程計(jì)累積誤差和建圖誤差進(jìn)行修正。閉環(huán)優(yōu)化與構(gòu)圖可以描述為一個(gè)最大后驗(yàn)概率(Maximum A Posteriori, MAP)問題，具體表達(dá)式為

(3)

其中，X表示攝像機(jī)在整個(gè)運(yùn)動(dòng)過程中的位置和姿態(tài)構(gòu)成的狀態(tài)向量；L表示所有特征在參考系下的位置向量的集合；Z表示特征在攝像機(jī)圖像中的成像點(diǎn)位置的集合；U表示里程計(jì)測(cè)量的運(yùn)動(dòng)參數(shù)。在大范圍的導(dǎo)航應(yīng)用中，式(3)中包含的狀態(tài)量較多，因此需要對(duì)優(yōu)化算法進(jìn)行合理設(shè)計(jì)才能滿足算法的實(shí)時(shí)性需求。目前，廣泛使用的建圖與閉環(huán)優(yōu)化工具有G2O[45]、GTSAM[46]和Ceres[47]等。

2.2 基于模型的視覺/慣性組合導(dǎo)航算法

基于濾波技術(shù)和基于非線性迭代優(yōu)化技術(shù)是視覺/慣性信息融合的兩種典型方式。

(1)基于濾波技術(shù)的信息融合算法

基于濾波技術(shù)的信息融合算法主要考慮以下3個(gè)方面的問題：濾波器狀態(tài)變量的選取、狀態(tài)方程和觀測(cè)方程的建立以及濾波算法的選取。

首先是濾波器狀態(tài)變量的選取，常見的方式是將當(dāng)前時(shí)刻的慣性導(dǎo)航參數(shù)、鄰近n幀圖像對(duì)應(yīng)時(shí)刻的載體位姿參數(shù)以及這些圖像所觀測(cè)到的特征的空間位置參數(shù)加入到狀態(tài)變量中。當(dāng)前時(shí)刻慣性導(dǎo)航參數(shù)通常包含IMU的位置、姿態(tài)、速度和陀螺、加速度計(jì)的零偏等，其定義如下

(4)

(5)

其次是狀態(tài)方程和觀測(cè)方程的建立。由于通常假設(shè)場(chǎng)景是固定的，即特征點(diǎn)的空間位置變化率為0，因此系統(tǒng)的狀態(tài)方程只與載體的運(yùn)動(dòng)參數(shù)有關(guān)。

典型的系統(tǒng)狀態(tài)微分方程如式(6)所示

(6)

關(guān)于濾波器的選取，最常見的有EKF[17-18,20-22]和UKF[19,23,40-41,43]，二者都是在卡爾曼濾波器(Kalman Filter， KF)的基礎(chǔ)上發(fā)展起來(lái)的。EKF通過偏導(dǎo)數(shù)得到雅可比矩陣，將狀態(tài)方程和觀測(cè)方程線性化，從而解決視覺/慣性融合中的非線性問題。為了克服EKF中高階導(dǎo)數(shù)省略問題和雅克比矩陣計(jì)算難的問題，UKF按一定間隔和概率在狀態(tài)空間中選取采樣點(diǎn)(sigma points) 的方式，代入狀態(tài)方程和觀測(cè)方程，預(yù)測(cè)和更新狀態(tài)值及其對(duì)應(yīng)的協(xié)方差矩陣。

由于計(jì)算量的限制，一般不會(huì)將全局地圖中的特征空間位置參數(shù)加入濾波器狀態(tài)變量中，因此基于濾波技術(shù)的視覺/慣性組合導(dǎo)航算法通常無(wú)法構(gòu)建全局地圖，不支持閉環(huán)檢測(cè)與優(yōu)化。

(2)基于優(yōu)化技術(shù)的信息融合算法

為了實(shí)現(xiàn)迭代優(yōu)化算法框架下的視覺/慣性導(dǎo)航信息融合，必須解決慣性約束和視覺約束的統(tǒng)一表示問題。對(duì)于視覺信息，關(guān)鍵幀之間的位置和姿態(tài)約束可以通過它們共同觀測(cè)的圖像特征之間的匹配關(guān)系來(lái)確立。而對(duì)于慣性信息，2個(gè)時(shí)刻間的位置和姿態(tài)約束可以通過2個(gè)時(shí)刻間的陀螺和加速計(jì)測(cè)量信息來(lái)建立。在視覺/慣性組合導(dǎo)航系統(tǒng)中，當(dāng)前時(shí)刻的關(guān)鍵幀位姿參數(shù)是在前一時(shí)刻關(guān)鍵幀的位姿參數(shù)基礎(chǔ)上，利用陀螺和加速度計(jì)測(cè)量值遞推得到。由于關(guān)鍵幀的位姿參數(shù)屬于迭代優(yōu)化的狀態(tài)變量，在優(yōu)化過程中，每一次迭代都會(huì)改變，所以由前一時(shí)刻關(guān)鍵幀的位姿參數(shù)遞推得到的當(dāng)前幀的位姿參數(shù)，需要重新利用兩幀之間的陀螺和加速度計(jì)測(cè)量值推算，處理效率非常低。為了避免該問題，需要設(shè)計(jì)一種不依賴于積分初值的慣性積分增量計(jì)算方法，使得在迭代優(yōu)化過程中，前一時(shí)刻關(guān)鍵幀位姿參數(shù)變化之后，可以根據(jù)積分增量快速更新當(dāng)前時(shí)刻的關(guān)鍵幀位姿參數(shù)。

圖4 基于迭代優(yōu)化技術(shù)的視覺/慣性組合導(dǎo)航示意圖Fig.4 Scheme of visual-inertial integrated navigation based on iterative optimization

慣性預(yù)積分技術(shù)應(yīng)運(yùn)而生[24]，其核心思想是定義位置、姿態(tài)和速度積分增量，使得積分增量與積分初值無(wú)關(guān)。從系統(tǒng)的運(yùn)動(dòng)學(xué)模型式(6)出發(fā)，可以得到關(guān)鍵幀[ti,tj]時(shí)刻間的位姿參數(shù)與慣性測(cè)量值之間的關(guān)系為

(7)

其中，g是重力矢量，η是加速計(jì)測(cè)量噪聲，n是積分時(shí)段內(nèi)慣性傳感器的采樣個(gè)數(shù)。從式(7)可以看出，tj時(shí)刻關(guān)鍵幀的位姿參數(shù)與ti時(shí)刻關(guān)鍵幀的位姿參數(shù)以及[ti,tj]時(shí)刻間的慣性測(cè)量值有關(guān)。為了消除ti時(shí)刻關(guān)鍵幀的位姿參數(shù)的影響，定義ti和tj時(shí)刻關(guān)鍵幀之間的狀態(tài)變量增量計(jì)算公式如下

(8)

式中，Δtij=tj-ti。從式(8)可以看出，慣性積分增量ΔRij、Δvij、Δpij僅與[ti,tj]時(shí)刻間的陀螺測(cè)量值ω和加速度計(jì)測(cè)量值a有關(guān)，與積分的初值Ri、vi和pi無(wú)關(guān)。

通過預(yù)積分對(duì)慣性信息進(jìn)行預(yù)處理之后，就可以建立統(tǒng)一視覺約束和慣性約束的優(yōu)化目標(biāo)函數(shù)。以VINS為例，其目標(biāo)函數(shù)具有如下形式[27]

(9)

其中，3個(gè)殘差項(xiàng)依次是邊緣化的先驗(yàn)信息、IMU測(cè)量殘差以及視覺的觀測(cè)殘差，X是待優(yōu)化的狀態(tài)向量，包含關(guān)鍵幀的相機(jī)位姿、特征的空間位置、慣性器件的零偏等。

當(dāng)然，一個(gè)完整的視覺/慣性組合導(dǎo)航系統(tǒng)還包含系統(tǒng)初始化、閉環(huán)修正與優(yōu)化等。此處不再贅述，感興趣的讀者可以查閱文獻(xiàn)[25-28]。

3 基于機(jī)器學(xué)習(xí)的視覺/慣性組合導(dǎo)航技術(shù)

深度學(xué)習(xí)神經(jīng)網(wǎng)絡(luò)是機(jī)器學(xué)習(xí)概念的重要分支，具有參數(shù)學(xué)習(xí)與非線性模型擬合的能力，利用深度學(xué)習(xí)解決組合導(dǎo)航問題，實(shí)質(zhì)上是使用神經(jīng)網(wǎng)絡(luò)對(duì)原始數(shù)據(jù)與導(dǎo)航參數(shù)之間的關(guān)系進(jìn)行建模，并通過長(zhǎng)時(shí)間訓(xùn)練來(lái)優(yōu)化模型的參數(shù)。為了增強(qiáng)深度學(xué)習(xí)網(wǎng)絡(luò)的可解釋性，需對(duì)網(wǎng)絡(luò)不同功能模塊使用不同種類的網(wǎng)絡(luò)進(jìn)行建模。圖5所示為基于深度學(xué)習(xí)的視覺/慣性組合導(dǎo)航技術(shù)的通用結(jié)構(gòu)示意圖。

圖5 基于機(jī)器學(xué)習(xí)的視覺/慣性組合導(dǎo)航技術(shù)通用結(jié)構(gòu)示意圖Fig.5 Scheme of learning based visual-inertial navigation technology

3.1 前端網(wǎng)絡(luò)

(1)視覺特征提取器

與基于模型的組合導(dǎo)航技術(shù)類似，基于深度學(xué)習(xí)的導(dǎo)航技術(shù)也存在前端，即處理原始數(shù)據(jù)的模塊。針對(duì)圖像這種高維度的信息，需從中捕獲高層特征來(lái)解析相機(jī)運(yùn)動(dòng)信息。

(10)

文獻(xiàn)[33,37]使用單輸入的光流估計(jì)網(wǎng)絡(luò) (FlowNetSimple)[34]的卷積層部分搭建視覺特征提取器，并將網(wǎng)絡(luò)的輸入層通道數(shù)設(shè)置為6，接收時(shí)間軸上相鄰兩幀的RGB圖像。為了能對(duì)相鄰圖像的高層信息進(jìn)行更充分的解析，文獻(xiàn)[36]使用雙輸入的光流估計(jì)網(wǎng)絡(luò)(FlowNetCorr)[34]的卷積層部分搭建視覺特征提取器，為前后兩幀圖像分別構(gòu)建卷積神經(jīng)網(wǎng)絡(luò)，解析2張圖像中的高層特征，并使用correlate操作融合兩幀圖像的高層特征。FlowNetCorr的層數(shù)較多，訓(xùn)練成本較大，因此在基于深度學(xué)習(xí)的視覺里程計(jì)中一般選用FlowNetSimple的卷積層部分搭建視覺特征提取器。上述兩種視覺特征提取器依據(jù)成熟的卷積神經(jīng)網(wǎng)絡(luò)進(jìn)行設(shè)計(jì)，同時(shí)Dosovitskiy 等[34]已公開這兩種卷積神經(jīng)網(wǎng)絡(luò)的預(yù)訓(xùn)練參數(shù)，有利于開發(fā)基于視覺信息的深度學(xué)習(xí)導(dǎo)航技術(shù)。然而FlowNetCorr與FlowNetSimple都屬于層數(shù)較多的卷積神經(jīng)網(wǎng)絡(luò)，參數(shù)量較大，其中FlowNetCorr參數(shù)占磁盤空間149M，F(xiàn)lowNetSimple占148M，因此這兩種卷積神經(jīng)網(wǎng)絡(luò)不適用于包含深度信息的全導(dǎo)航參數(shù)估計(jì)算法。針對(duì)此問題，文獻(xiàn)[49-50]設(shè)計(jì)了僅由6層卷積核構(gòu)成的視覺特征提取器，并且使用均值池化操作將視覺高層特征直接壓縮為6維度的相鄰圖像幀位姿；但較少的層數(shù)也導(dǎo)致提取器的解析能力較弱，在深度以及位姿估計(jì)任務(wù)中的性能也有一定局限性，訓(xùn)練收斂速度較慢。

(2)慣性信息特征提取器

(11)

文獻(xiàn)[36-37]使用慣性信息與圖像的融合特征進(jìn)行姿態(tài)解算，實(shí)驗(yàn)結(jié)果表明，添加慣性信息的里程計(jì)網(wǎng)絡(luò)收斂較快并且測(cè)試精度較高。為了進(jìn)一步提高里程計(jì)的解算精度，文獻(xiàn)[37]設(shè)計(jì)了兩種注意力網(wǎng)絡(luò)，注意力網(wǎng)絡(luò)輸出與原始數(shù)據(jù)高層特征同尺寸的權(quán)重掩膜，并通過改變特征元素的相對(duì)大小，從而調(diào)整網(wǎng)絡(luò)的訓(xùn)練方向，規(guī)避噪聲特征對(duì)網(wǎng)絡(luò)性能的影響。文獻(xiàn)[37]的實(shí)驗(yàn)結(jié)果表明，基于深度學(xué)習(xí)的慣導(dǎo)信息特征提取器在多種慣導(dǎo)信息噪聲的環(huán)境下也具有較為穩(wěn)定的性能。然而由于深度學(xué)習(xí)神經(jīng)網(wǎng)絡(luò)參數(shù)對(duì)于訓(xùn)練數(shù)據(jù)具有一定的依賴性，對(duì)于不同場(chǎng)景數(shù)據(jù)的泛化能力較差，這限制了基于深度學(xué)習(xí)的特征提取器的應(yīng)用范圍。文獻(xiàn)[52]使用遷移學(xué)習(xí)的方法，找到不同場(chǎng)景中慣導(dǎo)數(shù)據(jù)的共有特征并結(jié)合其物理模型，在沒有標(biāo)簽數(shù)據(jù)的情況下，利用低精度的手持設(shè)備數(shù)據(jù)也能得到精度較高的位姿解算結(jié)果。

原始數(shù)據(jù)的高層特征，需使用位姿回歸器將高層特征投影至標(biāo)簽空間中。常見的位姿回歸器由多層全連接層組成，全連接層的輸出通道數(shù)與位姿估計(jì)值的形式有關(guān)?，F(xiàn)階段基于深度學(xué)習(xí)的視覺里程計(jì)都采用歐拉角來(lái)表示姿態(tài)，因此一般將位姿回歸器中最后一層的全連接層設(shè)置為6[33,37]，也可以將位姿回歸的過程解耦，分別設(shè)置3維度的位置回歸器與姿態(tài)回歸器。

(3)深度估計(jì)網(wǎng)絡(luò)

除了提取原始數(shù)據(jù)的特征以外，前端還需給出當(dāng)前視角內(nèi)特征點(diǎn)的深度信息。基于模型的視覺/慣性組合導(dǎo)航技術(shù)使用多視覺幾何模型聯(lián)合相鄰幀圖像的匹配特征點(diǎn)，求得相對(duì)位姿以及無(wú)尺度的特征點(diǎn)深度值。然而，在紋理缺失以及光線較暗的部分，特征點(diǎn)識(shí)別算法失效導(dǎo)致無(wú)法得到較為準(zhǔn)確的深度值。深度學(xué)習(xí)神經(jīng)網(wǎng)絡(luò)通過前向傳播直接得到原始圖像像素點(diǎn)對(duì)應(yīng)的深度值，同時(shí)設(shè)計(jì)具有幾何約束的誤差項(xiàng)來(lái)校正神經(jīng)網(wǎng)絡(luò)參數(shù)，從而提高深度估計(jì)的精度。文獻(xiàn)[50,55-56]構(gòu)建了類U-NET的深度估計(jì)網(wǎng)絡(luò)，使用多層卷積神經(jīng)網(wǎng)絡(luò)構(gòu)建特征提取器，其中文獻(xiàn)[54-55]使用主流的PackNet和ResNet網(wǎng)絡(luò)作為特征提取器，在訓(xùn)練前使用預(yù)訓(xùn)練參數(shù)進(jìn)行初始化，便于訓(xùn)練的收斂；隨后使用深度解碼器將特征提取器解析的圖像高層特征變?yōu)榕c原圖尺寸一致的深度估計(jì)值，深度解碼器由多層反卷積層構(gòu)成，同時(shí)將特征提取器輸出圖像的不同層次的特征輸入到深度解碼器對(duì)應(yīng)的反卷積層中，強(qiáng)化深度估計(jì)圖像的多尺度細(xì)節(jié)。為了提高深度估計(jì)網(wǎng)絡(luò)的性能，現(xiàn)有兩種思路：1)改進(jìn)網(wǎng)絡(luò)的結(jié)構(gòu)，例如將網(wǎng)絡(luò)的高低特征聯(lián)結(jié)[50,55-56]，增強(qiáng)輸出的深度圖像質(zhì)量；2)在設(shè)計(jì)誤差函數(shù)時(shí)添加約束條件，例如文獻(xiàn)[55]引入時(shí)空最小誤差，剔除在連續(xù)兩幀圖像中因相機(jī)旋轉(zhuǎn)而移出視場(chǎng)范圍的像素點(diǎn)，避免了在計(jì)算重投影誤差時(shí)出現(xiàn)局部異常極大值的現(xiàn)象；文獻(xiàn)[56]則在總誤差中引入極線誤差，使得網(wǎng)絡(luò)能夠充分利用相鄰幀的點(diǎn)線特征，從而增強(qiáng)網(wǎng)絡(luò)性能。

3.2 特征優(yōu)化

特征優(yōu)化環(huán)節(jié)對(duì)應(yīng)基于模型的視覺/慣性組合導(dǎo)航技術(shù)中的非線性優(yōu)化模塊，該模塊利用前端提取的低層特征以及里程計(jì)估計(jì)的位姿參數(shù)構(gòu)建幾何誤差函數(shù)，使用特定的非線性優(yōu)化算法降低誤差函數(shù)值，以此得到優(yōu)化的導(dǎo)航參數(shù)。同樣地，特征優(yōu)化環(huán)節(jié)也設(shè)計(jì)了特定的網(wǎng)絡(luò)來(lái)優(yōu)化前端網(wǎng)絡(luò)得到的數(shù)據(jù)特征或者導(dǎo)航參數(shù)估計(jì)值。

借鑒傳統(tǒng)SLAM窗口優(yōu)化的思想，文獻(xiàn)[33,36-37]在視覺特征提取器的最后一層卷積層中添加LSTM網(wǎng)絡(luò)，以綜合前后多幀原始數(shù)據(jù)的高層特征，優(yōu)化當(dāng)前時(shí)刻的高層特征。上述過程如式(12)所示，其中flstm的每一時(shí)刻都引出隱藏變量，使得經(jīng)優(yōu)化的特征與未優(yōu)化特征的尺寸保持一致。

(12)

同時(shí)LSTM網(wǎng)絡(luò)采用多層次級(jí)聯(lián)設(shè)計(jì)，并添加多個(gè)節(jié)點(diǎn)以增加網(wǎng)絡(luò)的解析能力。然而，此類算法屬于端對(duì)端優(yōu)化算法，不具有可解釋性。為了能在優(yōu)化原始數(shù)據(jù)高層特征的過程中考慮到幾何模型的因素，文獻(xiàn)[49-50，53-55]在總誤差中設(shè)計(jì)了重投影誤差，耦合了深度估計(jì)網(wǎng)絡(luò)與位姿估計(jì)網(wǎng)絡(luò)參數(shù)的優(yōu)化過程。然而，以上工作都僅將重投影模型體現(xiàn)在總誤差函數(shù)中，沒有構(gòu)建顯示的網(wǎng)絡(luò)結(jié)構(gòu)對(duì)重投影模型進(jìn)行求解，網(wǎng)絡(luò)設(shè)計(jì)依舊欠缺一定的可解釋性，因此很難確定網(wǎng)絡(luò)是否擬合出了圖像像素值、深度與相對(duì)位姿之間存在的重投影模型；同時(shí)從以上文獻(xiàn)的算法性能驗(yàn)證實(shí)驗(yàn)可以看出，以上算法相對(duì)于端對(duì)端的里程計(jì)或者深度估計(jì)網(wǎng)絡(luò)的性能并沒有顯著的提升，這從另一個(gè)側(cè)面說(shuō)明了以上算法在構(gòu)建網(wǎng)絡(luò)時(shí)并沒有充分利用重投影模型原理。鑒于此，Tang 等[57]構(gòu)建了可微重投影約束層(BA-Layer)，對(duì)重投影模型的每個(gè)參數(shù)進(jìn)行顯示建模，從而對(duì)輸出的導(dǎo)航參數(shù)進(jìn)行優(yōu)化。分別設(shè)計(jì)了基礎(chǔ)深度生成網(wǎng)絡(luò)以及多尺度特征提取網(wǎng)絡(luò)，將時(shí)間上相鄰的一組圖像代入基礎(chǔ)深度生成網(wǎng)絡(luò)得到每一幀圖像的深度圖像族，并使用與深度圖像族對(duì)應(yīng)的可微系數(shù)，將深度圖像族加權(quán)組合為深度估計(jì)值圖像；同時(shí)使用多尺度特征提取網(wǎng)絡(luò)得到圖像幀的高層特征，隨后構(gòu)建特征級(jí)的重投影誤差，并代入BA-Layer層中進(jìn)行優(yōu)化。BA-Layer層根據(jù)前一時(shí)刻的狀態(tài)優(yōu)化量計(jì)算雅克比矩陣、正規(guī)方程、阻尼系數(shù)以及海森矩陣，進(jìn)而得到狀態(tài)量的變化量，從而得到當(dāng)前時(shí)刻的狀態(tài)優(yōu)化量。為了確保BA-Layer層的可微性，固定了特征級(jí)重投影誤差的優(yōu)化步數(shù)，同時(shí)使用多層全連接層將特征級(jí)重投影誤差轉(zhuǎn)化為阻尼系數(shù)。從實(shí)驗(yàn)結(jié)果來(lái)看，相比于使用光度重投影誤差與幾何重投影誤差的位姿估計(jì)方法，該文設(shè)計(jì)的相對(duì)位姿估計(jì)網(wǎng)絡(luò)的旋轉(zhuǎn)角與平移矢量測(cè)試精度更高。首先，這說(shuō)明BA-Layer能對(duì)重投影誤差進(jìn)行有效建模。其次，文獻(xiàn)提到使用幾何重投影誤差的位姿估計(jì)方法在室內(nèi)環(huán)境中可能無(wú)法進(jìn)行有效的特征匹配，光度重投影誤差則會(huì)增加優(yōu)化函數(shù)的非凸性，導(dǎo)致優(yōu)化算法對(duì)初值設(shè)置較為敏感。相比較而言，BA-Layer使用經(jīng)卷積神經(jīng)網(wǎng)絡(luò)解析的高層特征進(jìn)行導(dǎo)航參數(shù)的求解，相比于特征點(diǎn)、光流等底層特征，高層特征具有較高的穩(wěn)定性，因此算法的魯棒性較好。此外，卷積神經(jīng)網(wǎng)絡(luò)具有較強(qiáng)的非線性擬合能力，可以在訓(xùn)練過程中對(duì)狀態(tài)初值進(jìn)行隱式估計(jì)，不需要人為指定。

Chen等[58]則提出了一種基于深度學(xué)習(xí)的卡爾曼濾波算法DynaNet。該算法首先假設(shè)視覺/慣性組合里程計(jì)是一個(gè)馬爾科夫過程，即當(dāng)前時(shí)刻的狀態(tài)量與前一時(shí)刻的狀態(tài)量有關(guān)，并且能用線性模型來(lái)描述狀態(tài)傳遞過程。DynaNet算法使用LSTM網(wǎng)絡(luò)估計(jì)狀態(tài)傳遞矩陣以及協(xié)方差傳遞誤差，并使用卷積神經(jīng)網(wǎng)絡(luò)得到視覺/慣性原始數(shù)據(jù)的高層特征以及測(cè)量誤差；隨后構(gòu)建卡爾曼濾波方程，經(jīng)過迭代得到當(dāng)前時(shí)刻的狀態(tài)量估計(jì)值；最后結(jié)合狀態(tài)量的標(biāo)簽值構(gòu)建訓(xùn)練誤差，經(jīng)過多輪訓(xùn)練得到精度更高的狀態(tài)量估計(jì)值。相比于Tang 等的工作，DynaNet使用深度學(xué)習(xí)神經(jīng)網(wǎng)絡(luò)重構(gòu)線性卡爾曼濾波方程，但鑒于深度學(xué)習(xí)具有強(qiáng)大的非線性擬合能力，DynaNet的狀態(tài)傳遞矩陣估計(jì)網(wǎng)絡(luò)也能對(duì)位姿求解過程進(jìn)行建模。從實(shí)驗(yàn)結(jié)果來(lái)看，DynaNet的位姿解算精度高于基于模型的ORB-SLAM[12]以及基于深度學(xué)習(xí)的VO-Feat[50],這證明了經(jīng)過精心設(shè)計(jì)的深度學(xué)習(xí)網(wǎng)絡(luò)具有超越基于模型的導(dǎo)航算法的能力；同時(shí)也說(shuō)明了使用深度學(xué)習(xí)重構(gòu)傳統(tǒng)卡爾曼濾波模型能有效提升深度學(xué)習(xí)框架求解位姿問題的能力。

3.3 閉環(huán)優(yōu)化與建圖

(13)

在得到經(jīng)過優(yōu)化的絕對(duì)位姿之后，需結(jié)合關(guān)鍵幀的深度信息構(gòu)建全局的三維立體模型，然而基于重投影誤差估計(jì)的關(guān)鍵幀深度值不具有全局一致的尺度，因此還需設(shè)計(jì)更多的幾何約束使得網(wǎng)絡(luò)在長(zhǎng)時(shí)間的訓(xùn)練過程中逐漸恢復(fù)關(guān)鍵幀的尺度。Guizilini 等[54]提出了在訓(xùn)練誤差函數(shù)中添加訓(xùn)練數(shù)據(jù)集中的速度標(biāo)簽，使得相對(duì)位姿估計(jì)網(wǎng)絡(luò)輸出的相對(duì)平移量具有與標(biāo)簽值一致的尺度。Bian 等[61]則使用深度估計(jì)網(wǎng)絡(luò)同時(shí)估計(jì)參考幀與目標(biāo)幀的深度，隨后使用匹配特征點(diǎn)對(duì)應(yīng)的空間點(diǎn)坐標(biāo)構(gòu)建投影誤差。

4 視覺/慣性組合導(dǎo)航技術(shù)的典型應(yīng)用及發(fā)展趨勢(shì)

視覺/慣性組合導(dǎo)航技術(shù)是機(jī)器人、計(jì)算機(jī)視覺、導(dǎo)航等領(lǐng)域的研究熱點(diǎn)，在國(guó)民經(jīng)濟(jì)和國(guó)防建設(shè)中取得了廣泛的應(yīng)用，但也面臨著諸多挑戰(zhàn)。

4.1 視覺/慣性組合導(dǎo)航技術(shù)的典型應(yīng)用

國(guó)民經(jīng)濟(jì)領(lǐng)域，在無(wú)人機(jī)、無(wú)人車、機(jī)器人、現(xiàn)實(shí)增強(qiáng)、高精度地圖等應(yīng)用的推動(dòng)下，視覺/慣性組合導(dǎo)航技術(shù)取得了快速發(fā)展。例如Google的Tango項(xiàng)目和無(wú)人車項(xiàng)目、微軟的Hololens項(xiàng)目、蘋果的ARKit項(xiàng)目、百度無(wú)人車項(xiàng)目、大疆無(wú)人機(jī)項(xiàng)目、高德高精度地圖項(xiàng)目等大型應(yīng)用項(xiàng)目都成立了視覺/慣性組合導(dǎo)航技術(shù)相關(guān)的研究小組，極大地促進(jìn)了視覺/慣性組合導(dǎo)航技術(shù)在國(guó)民經(jīng)濟(jì)中的應(yīng)用。以Google的Tango項(xiàng)目為例，其導(dǎo)航定位核心算法是基于濾波框架的MSCKF算法；微軟的Hololens項(xiàng)目則是以KinectFusion為基礎(chǔ)的SLAM算法。

國(guó)防建設(shè)領(lǐng)域，由于視覺/慣性組合導(dǎo)航技術(shù)不依賴外部人造實(shí)施，在衛(wèi)星拒止環(huán)境中有著重要的應(yīng)用價(jià)值。例如美國(guó)陸軍研發(fā)的一種新型聯(lián)合精確空投系統(tǒng)采用慣性/視覺組合導(dǎo)航技術(shù)解決高精度定位問題。嫦娥三號(hào)巡視器也采用視覺與慣性組合實(shí)現(xiàn)定姿定位。李豐陽(yáng)等[62]總結(jié)了視覺/慣性組合導(dǎo)航技術(shù)在地面、空中、水下和深空等多種場(chǎng)景中的應(yīng)用。

4.2 視覺/慣性組合導(dǎo)航技術(shù)的未來(lái)發(fā)展趨勢(shì)

視覺/慣性組合導(dǎo)航技術(shù)取得了廣泛的應(yīng)用，但在復(fù)雜條件下的可靠性還有待加強(qiáng)，其未來(lái)的發(fā)展主要體現(xiàn)在以下4個(gè)方向：

1)提升信息源的質(zhì)量。首先是提升慣性器件(特別是基于微機(jī)電系統(tǒng)(Micro-Electro-Mecha-nical System，MEMS)工藝的微慣性器件)的零偏穩(wěn)定性和環(huán)境適應(yīng)性等性能指標(biāo)；其次是提升視覺傳感器的光照動(dòng)態(tài)適應(yīng)性、快速運(yùn)動(dòng)適應(yīng)性等性能指標(biāo)；此外，還可以引入更多的傳感器，如磁傳感器、超聲波傳感器、激光雷達(dá)等，提升復(fù)雜條件下組合導(dǎo)航系統(tǒng)的綜合性能。

2)提升信息融合算法的水平。視覺和慣性信息各有特點(diǎn)，不同條件下信息的質(zhì)量也不盡相同，需要設(shè)計(jì)智能的信息融合機(jī)制。目前的算法大多是基于靜態(tài)場(chǎng)景假設(shè)，但在實(shí)際應(yīng)用中，場(chǎng)景都有一定的動(dòng)態(tài)性，動(dòng)態(tài)環(huán)境下的視覺/慣性組合導(dǎo)航是提升復(fù)雜條件下導(dǎo)航可靠性的重要研究方向。此外，目前基于濾波的信息融合算法仍然存在濾波狀態(tài)發(fā)散或者狀態(tài)收斂到錯(cuò)誤值的情況，需要對(duì)系統(tǒng)的可觀性進(jìn)行分析，提升狀態(tài)估計(jì)的一致性。對(duì)于優(yōu)化框架的信息融合算法，目前的預(yù)積分理論還有待完善，特別是在SLAM的地圖管理中刪除關(guān)鍵幀時(shí)，與關(guān)鍵幀相關(guān)的積分增量及對(duì)應(yīng)的協(xié)方差需要合并，目前還缺乏協(xié)方差合并方法；而且基于BA的優(yōu)化算法計(jì)算量較大，對(duì)于大尺度的閉環(huán)優(yōu)化，計(jì)算耗時(shí)太久，存在錯(cuò)失閉環(huán)優(yōu)化的情況，急需提升BA算法的效率。

3)發(fā)展新的導(dǎo)航理論。大自然中許多動(dòng)物具有驚人的導(dǎo)航本領(lǐng)，例如：北極燕鷗每年往返于相距數(shù)萬(wàn)km遠(yuǎn)的南北兩極地區(qū)；信鴿能夠在距離飼養(yǎng)巢穴數(shù)百km遠(yuǎn)的地方順利返回巢穴。模仿和借鑒動(dòng)物導(dǎo)航本領(lǐng)的仿生導(dǎo)航技術(shù)逐漸成為了導(dǎo)航領(lǐng)域研究的熱點(diǎn)。胡小平等[63]對(duì)仿生導(dǎo)航技術(shù)進(jìn)行了全面的總結(jié)。此外，隨著多平臺(tái)集群應(yīng)用的普及，利用組網(wǎng)編隊(duì)中平臺(tái)間導(dǎo)航信息交互來(lái)提升位置、速度、姿態(tài)等參數(shù)估計(jì)精度的協(xié)同導(dǎo)航技術(shù)方興未艾。謝啟龍等[64]從無(wú)人機(jī)、機(jī)器人、無(wú)人水下潛航器、導(dǎo)彈4個(gè)應(yīng)用層面梳理了協(xié)同導(dǎo)航技術(shù)的國(guó)內(nèi)外發(fā)展現(xiàn)狀。

4)擴(kuò)充導(dǎo)航系統(tǒng)的功能。目前的視覺/慣性組合導(dǎo)航側(cè)重于導(dǎo)航參數(shù)的估計(jì)，對(duì)于引導(dǎo)和控制等關(guān)注較少。隨著機(jī)器學(xué)習(xí)技術(shù)在視覺/慣性組合導(dǎo)航領(lǐng)域的應(yīng)用，可以將機(jī)器學(xué)習(xí)在環(huán)境理解、避障檢測(cè)、引導(dǎo)控制等方面的成果融入到導(dǎo)航系統(tǒng)中。