一種基于語義信息輔助的無人機視覺/慣性融合定位方法

呂 品，何 容，賴際舟，楊子寒，袁 誠

(南京航空航天大學自動化學院,南京 211106)

0 引言

無人機憑借其成本低、體積小及機動靈活等特點，在軍事和民事領域都有廣泛的應用。目前，隨著無人機智能化、微型化的發展，其應用領域也從室外逐漸向室內拓展。無人機常用的導航定位方案是全球衛星導航系統(Global Navigation Satellite System，GNSS)和慣性導航系統(Inertial Navigation System，INS)的組合導航方案，其中GNSS是決定其定位精度的關鍵性因素。與室外環境不同，無人機在室內場景中運行時，由于建筑物的遮擋，GNSS信號會嚴重衰減甚至完全消失，傳統的GNSS/INS導航方法將無法提供持續可靠的導航定位信息。

同步定位與地圖構建(Simultaneous Localization and Mapping，SLAM)是常用的室內導航方法，根據傳感器的不同，可以分為視覺SLAM和激光雷達SLAM。視覺傳感器能夠獲取環境的紋理、顏色與特征信息，利用視覺特征能夠構建出稀疏或稠密的特征地圖，無需先驗信息就能完成自身位置的解算。而且，視覺傳感器具有體積小、質量小及感知信息豐富等優點。因此，為了解決室內場景下無人機的自主飛行問題，越來越多的研究人員將視覺傳感器應用于無人機室內自主導航定位。根據利用圖像信息的不同，視覺SLAM中的前端里程計算法又可以被分為基于直接法的里程計(SVO、DSO)和基于間接法的里程計(ORB-SLAM2、ORB-SLAM3)。直接法直接利用圖像的灰度信息進行幀間匹配以及運動估計，而間接法通過先提取出圖像的特征，利用特征的匹配和跟蹤進行相機的位姿解算。間接法使用的特征描述受光照影響較小，與直接法相比具有更好的魯棒性。

傳統的間接法視覺里程計(Visual Odometry，VO)算法多提取環境中的低級視覺幾何特征，然后通過集束調整(Bundle Adjustment，BA)最小化重投影或光度誤差。基于點、線特征的VO通過底層亮度關系進行描述匹配，抗干擾能力不足，會出現匹配錯誤甚至失敗的情況。除了點、線這些環境中的低級特征，含有語義信息的物體也是環境重要的組成部分。提取環境中的物體并進行相應的語義信息映射，可以極大地提高無人機對環境的理解能力以及人機交互的智能性。此外，環境中的物體特征還可以提供額外的語義和幾何約束，以提高相機位姿估計的精度。文獻[10]提出了一種將語義信息整合到視覺SLAM系統中的方法，該方法利用語義信息在特征匹配環節進行外點的剔除，同時利用因子圖進行位姿的優化求解，在動態環境中該方法比傳統算法具有更好的魯棒性。文獻[11]提出了一種物體級別的SLAM算法，該方法基于Kinect fusion，使用MASK RCNN對圖像進行實例分割，建立每個物體的截斷符號距離函數，實現物體級別的跟蹤、重建以及回環。文獻[14]提出了一種面向特定類別對象的SLAM方法，通過目標識別建立相機與對象之間的觀測約束，并通過圖優化方法進行優化求解。目前，在語義輔助的視覺SLAM定位方法中，語義信息的使用具有多種形式，尚未形成統一的方法和架構。

雖然利用語義信息可以對傳統的VO算法進行輔助的位姿優化，但語義的獲取仍然依賴視覺傳感器，而單一的傳感器由于感知維度的匱乏，難以滿足無人機在室內復雜環境下的魯棒定位要求。慣性傳感器具有完全自主的優點，對短時間內的相對運動估計精度較高，但長時間的遞推估計會產生較大的累積誤差。將視覺與慣性測量單元(Inertial Measure-ment Unit，IMU)信息進行融合，綜合利用多維量測信息，充分利用數據的冗余及互補，彌補單個傳感器的固有缺陷，可以提高系統導航定位的精度和魯棒性。

由此可知，單獨基于特征點的VO由于單一傳感器以及低維特征的限制，在特征點稀疏、機動特性較大等挑戰性室內場景運行時，難以滿足無人機的智能自主導航定位需求。針對該問題，本文提出了一種基于語義信息輔助的無人機視覺/慣性融合定位方法。創新點主要包括：1)針對傳統VO算法提取低級幾何特征抗干擾能力不足的問題，根據室內環境的特點，通過提取環境中的平面語義信息，并輔助VO系統進行位姿優化，提高位姿估計的精度及魯棒性；2)針對單一視覺傳感器感知維度匱乏、易受外界干擾的問題，將視覺傳感器和慣性傳感器信息進行融合，以提高整個定位系統的精度、魯棒性和可靠性。

1 基于平面語義信息輔助的視覺/慣性融合定位方案設計

基于平面語義信息輔助的視覺/慣性融合定位方案以傳統的VO為基礎，輔助增加語義信息以及慣性信息的融合利用，從而提升傳統基于特征點的視覺VO導航定位精度。如圖1所示，其可以被分為前端處理與后端融合優化兩個部分，每一個部分都可以被分為傳統視覺VO、語義融合和慣性融合3個子處理模塊。在圖1中，傳統視覺VO部分用藍色進行標注，語義部分用橙色進行標注，慣性部分用綠色進行標注。本文在傳統視覺VO的基礎上，首先實現語義信息的提取和融合；然后在語義輔助的VO的基礎上，實現慣性信息的預處理與融合；最終實現語義信息輔助的視覺/慣性融合定位方案。

圖1 基于平面語義信息輔助的視覺/慣性融合定位方案框架Fig.1 Framework of plane semantic information aided visual/inertial fusion positioning

在語義信息的融合方面，通過提取的語義平面對環境中的語義信息進行特征表示，并建立對應的數據關聯方法。在慣性融合方面，利用慣性預積分技術，獲取兩視覺關鍵幀之間的慣性信息約束。通過對語義特征以及慣性預積分的因子圖建模，構建融合語義信息和慣性信息的VO因子圖架構，實現語義和慣性對VO的輔助優化。

2 基于平面語義信息輔助的VO算法

語義信息通常需要與一些具體的特征相結合，為其提供相應的概念與約束，才能與傳統的VO融合，輔助提高位姿優化精度。本文將語義信息與平面特征相結合，構建出語義平面，作為視覺VO額外的高維特征，為傳統的VO增添除特征點之外的環境觀測與約束。而后，對語義平面特征進行因子建模，構建相應的因子圖優化方案，通過對傳統視覺VO的因子圖結構進行擴充與改善，以提高無人機的導航定位精度。

語義信息輔助的改進VO與傳統VO因子圖融合框架對比如圖2所示。在圖2中，圓圈表示因子圖的優化節點，節點間的連線表示兩節點之間存在相應的約束關系，這些約束關系通過連線上的方塊進行表示；表示第關鍵幀時刻相機的位姿，表示構建的第個傳統特征點，表示構建的第個語義平面特征；橙色方框表示語義平面觀測約束，黑色方框表示VO得到的幀間相對位姿約束，灰色方框表示傳統特征點的觀測約束。

(a)傳統VO因子圖結構

(b)語義信息輔助的改進VO因子圖結構

傳統VO由于只提取低維的點特征，因此其構建的因子圖模型中只包含特征點與相機位姿節點，且構建的約束只包括二者之間的觀測約束，其結構如圖2(a)所示。本文在傳統VO的基礎上實現了語義信息輔助的VO因子圖結構，如圖2(b)所示。在傳統算法的基礎上，本文首先根據VO量測，增添幀間相對位姿約束因子，為相鄰兩關鍵幀之間提供直接的相對位姿約束，而后通過語義平面的特征提取及數據關聯，獲取關鍵幀與語義平面特征之間的觀測約束，并進行相應的因子構建。

2.1 語義平面特征構建及數據關聯

在語義信息的提取上，為了實現對感興趣語義對象的快速精確檢測，本文采用基于卷積神經網絡的視覺檢測方法，實現對RGB圖像語義對象的實時檢測，獲取室內感興趣語義對象的二維BBOX。在點云分割及平面擬合方面，本文基于深度圖像點云的結構化特性，在獲取的語義對象二維BBOX內，使用基于連通域的點云分割與平面擬合方法，進行點云分割及平面擬合，獲取感興趣語義對象的三維平面。結合語義信息的提取及平面分割結果，可以構建具有語義屬性的三維平面。通過上述過程，可以獲取語義物體表面所在平面參數、平面類型、所屬物體類別，利用這些屬性以及三維幾何空間信息，可以構建豐富的語義平面特征。

在室內場景中，本文關注的語義對象中的表平面通常為水平或垂直向。因此，在語義平面提取過程中，本文也對平面分割結果進行篩選，并構建語義平面特征。

2.1.1 平面篩選

首先，根據提取的所有平面的質心和法線，判斷平面，如式(1)

(1)

其中，為提取的平面法線；為已知的地平面法線。如果與的差值小于設定的水平面法線差閾值，則以為法線的平面被標記水平面；而如果大于設定閾值，則檢查平面是否為垂直平面。同樣，設置垂直面法線差閾值。若平面為一垂直平面，則其法向量將與本文設置的標準水平面法向垂直，即二者點積為0。由此，可以通過點積的方式計算平面法線與垂直平面法線的差值，如式(2)

=·

(2)

如果小于設定的閾值，以為法線的平面被標記垂直平面。

2.1.2 語義平面特征構建

由上述流程步驟，可以獲取平面的多種信息：語義類別、形心、法線方向、平面類別(垂直/水平平面)。由此構建語義平面特征，如式(3)

(3)

其中，為平面形心；為平面法線參數；為平面類別標簽(水平垂直)；為平面語義類別標簽，其取決于平面所對應的語義對象類別。

2.1.3 語義特征關聯

通過平面分割及語義特征構建，可以得到如式(4)所示的語義平面

={,,,}

(4)

其中，對于，其所有位置相關特征項均以相機系為參考系。

算法中第一個接收到的語義對象不用經過數據關聯，直接映射為第一個語義路標，將其表示為

={,,,,}

(5)

其中，對于，及表示在世界坐標系下的語義路標質心及法向。其可利用和通過坐標變換求得，具體公式如式(6)

(6)

完成初始語義路標設置后，通過以下3個步驟實現每幀檢測語義平面與語義路標圖之間的數據關聯：

1)首先，將從幀中提取的語義平面與語義路標進行類別及平面類型的匹配。匹配成功后，計算語義平面的點云個數及區域面積。本文為點云數據及語義平面面積分別設置了最小閾值：和，用于刪除由于檢測邊界框不完全適合語義對象導致的質心坐標存在偏差的語義平面。經過上述檢測，匹配成功的語義平面才能進行后續匹配，否則匹配失敗。

2)而后，利用坐標變換，將語義平面的法線方向由相機坐標系轉換到世界坐標系下，得到，將其與語義路標的法向進行匹配。若與間的偏差小于設定的閾值，則認為語義平面與路標法向一致，進入后續的坐標關聯，否則匹配失敗。

3)通過上述兩步驟，將語義平面的形心由相機坐標系轉換到世界坐標系下，計算語義平面形心與其相匹配的路標形心的馬氏距離，若大于設定的閾值，則將檢測到的語義對象映射為新的語義路標，否則語義對象與當前的語義路標相匹配。

利用語義平面特征構建及數據關聯，可以得到補充的相機關鍵幀位姿幀間約束。

2.2 基于語義信息輔助的位姿非線性優化方法

本文所提方法的優化函數可以表示為

=

(7)

其中，Semantic部分代表語義路標觀測的殘差；,表示相機某幀觀測到的語義路標位置誤差;VO部分代表VO中相對位姿的觀測殘差；(·)代表魯棒核函數；表示各誤差量對應的信息矩陣，其為協方差矩陣的逆；VO的協方差矩陣與視覺相機標定后的重投影誤差相關，語義殘差的協方差矩陣與語義路標的不確定性相關。式中，、分別指第、圖像幀，表示第個語義路標。對于構建的殘差模型，本文中設定的優化狀態變量為相機的位姿以及語義特征的位置，分別用與表示。

對于語義路標觀測誤差，如式(8)

(8)

對于VO位姿觀測殘差，如式(9)

(9)

采用L-M優化算法對優化代價函數進行優化求解，即可實現狀態量的優化，即實現對相機位姿以及語義路標位置的同時優化。

3 基于慣性預積分的語義視覺/慣性融合算法

通過語義信息的提取與融合，可以提升傳統VO算法的定位精度。然而，語義信息和點特征都利用視覺傳感器進行提取，都屬于視覺特征。在實際應用場景中，由于無人機的快速運動及角度的快速轉變，視覺信息通常會受到一定的限制。在大機動情況下，基于視覺的位置解算很可能會存在較大的偏差。與視覺傳感器不同，IMU是一種完全自主的導航傳感器，其不依賴對外界環境的感知，與視覺傳感器進行融合，可以得到更魯棒的導航系統。

本文基于IMU預積分，建立相鄰兩圖像幀之間的運動約束，利用該約束對相機位姿估計狀態量進行輔助優化。本文構建的語義視覺/慣性融合系統框圖如圖3所示。IMU預積分在視覺關鍵幀之間進行，得到的是兩關鍵幀之間的慣性預積分約束，在構建好約束之后，與語義VO聯合進行優化量的優化求解。

圖3 語義視覺/慣性里程計系統框圖Fig.3 Frame diagram of semantic visual/inertial odometry system

結合IMU預積分，同時考慮IMU及VO、語義殘差的后端優化函數如式(10)所示

(10)

融合慣性信息后，第時刻的系統狀態變量為

(11)

式(10)中，IMU預積分誤差的具體表示如式(12)所示

(12)

采用L-M優化算法對式(10)進行優化求解，得到系統狀態變量的最優估計。

4 試驗測試與分析

4.1 試驗設置

為了驗證本文所提方法的性能，開展了TUM-RGBD數據集試驗和實際的無人機飛行驗證試驗，通過與傳統VO方法定位精度的對比分析，驗證了本文所提方法的可靠性和精度。

4.2 TUM-RGBD數據集試驗與分析

TUM數據集包括不同場景的室內序列，用于評估視覺里程計的性能，其提供了RGB-D傳感器在環境中的真實運動軌跡及采集到的RGB圖像與深度圖像。RGB-D傳感器使用的是微軟的kinect，采用結構光進行深度信息的獲取，能夠提供分辨率較高較為精細的深度圖。RGB圖像分辨率為640dpi×480dpi，頻率為30Hz；深度圖像分辨率為320dpi×240dpi，頻率為30Hz。

本文選取TUM數據集中含有語義路標的室內場景Freiburg2視頻序列進行驗證，將所提的平面語義信息輔助VO算法與傳統VO方法進行對比分析。在試驗數據中，出現的典型語義對象為椅子、屏幕、書以及鍵盤等，本文算法能檢測出這些典型的語義對象，并按需求將其設置為語義路標。Freiburg2視頻序列場景如圖4所示。

圖4 Freiburg2 xyz視頻序列場景圖Fig.4 Scene image of Freiburg2 xyz video sequence

在該數據集視頻序列中，相機動態沿著、和軸做平移和旋轉運動。在相機的視野范圍內，始終存在兩個語義對象，一個電視監視器和一個鍵盤。在試驗過程中，這兩個語義對象在語義信息輔助的視覺VO中被檢測出，并作為語義路標添加進因子圖優化框架中，協助傳統的基于特征點的VO進行輔助的位姿優化。圖5顯示了語義信息輔助的視覺里程計解算出的相機運動軌跡以及所構建的語義平面對象。其中，紅色垂直向平面為對數據集中“電腦屏幕(TVmonitor)”的語義平面，紫色水平向平面為對數據集中“鍵盤(Keyboard)”的語義平面。

圖5 語義信息輔助的VO定位及構圖結果Fig.5 Semantic information aide VO positioning and mapping results

圖6所示為TUM數據集試驗軌跡對比圖，綠線代表無語義信息輔助的RGB-D VO解算結果，紅線代表語義信息輔助下的RGB-D VO結果，藍線代表真值，藍色圓圈所在處為相機的初始位置。由圖6可以看出，使用語義信息輔助的視覺里程計解算得到的相機軌跡與真值間的偏移量更小，與傳統的RGB-D視覺里程計相比具有更好的定位精度。圖7所示為TUM數據集試驗三軸位置誤差對比圖。從圖7可以看出，經語義信息融合后，相機的三軸位置誤差在一定程度上都有所修正。

圖6 TUM數據集試驗軌跡對比圖Fig.6 Trajectories comparison diagram of TUM dataset experiment

圖7 TUM數據集試驗三軸位置誤差對比圖Fig.7 Comparison diagram of three-axis position error in TUM dataset experiment

圖8所示為傳統VO與本文所提語義信息輔助VO的誤差統計箱線圖。從圖8可以看出，在統計意義上，改進算法的誤差不管是從數值還是從誤差范圍的分布上，與傳統算法相比都有更好的表現。表1對比了傳統VO與本文所提語義信息輔助VO的定位誤差，可以看出，本文所提的語義信息輔助的VO方法具有更好的精度。

圖8 TUM數據集試驗位置誤差箱線對比圖Fig.8 Comparison diagram of position error box-plot of TUM dataset experiment

表1 TUM數據集試驗定位誤差統計

4.3 無人機試驗與分析

為了進一步驗證本文所提方法的有效性，利用無人機平臺進行實際試驗驗證，通過運動捕捉系統獲取無人機的基準位置信息，其定位精度可達毫米級。飛行試驗硬件平臺如圖9所示，選用的無人機為大疆創新(DJI)的Matrix 100四軸飛行器；視覺傳感器型號為Intel RealSense D435i，可以同時獲取RGB及深度圖像信息；慣性傳感器為Intel RealSense D435i自帶的IMU，其型號為博世BMI055；機載計算機為DJI Manifold。

圖9 飛行試驗硬件平臺Fig.9 UAV flight experiment hardware platform

試驗場景如圖10所示，在本次飛行試驗室內場景中出現的典型語義對象主要為椅子、電腦屏幕等。為了驗證語義信息的輔助效果以及慣性融合對定位結果的優化程度，檢驗整體的改進算法的性能，本次試驗在場景中采集了多組測試數據，對無人機定位系統的整體性能精度及誤差特性進行評估。在測試過程中，無人機圍繞著語義對象進行無規律地平移以及旋轉運動，試驗中同時還設置了大范圍轉角等機動性較強的無人機飛行軌跡，以保證對算法的魯棒性進行驗證。在運動過程中，無人機始終保持視野范圍內有語義對象的存在，每組數據無人機的運動過程持續約3min。試驗過程中，無人機始終控制在動捕系統的追蹤范圍內。

圖10 無人機飛行試驗場景Fig.10 Scene image of UAV flight experiment

選取一組試驗數據進行無人機軌跡繪制及定位誤差分析。圖11顯示了在語義信息與慣性的輔助下，算法的相機運動軌跡解算結果以及構建的語義平面對象。其中，紅色平面為對數據集中“電腦屏幕(TVmonitor)”的語義平面，綠色平面為對數據集中“椅子(chair)”的語義平面。

圖11 語義信息輔助的視覺/慣性融合定位及構圖結果Fig.11 Semantic information aide visual/inertial fusion positioning and mapping results

為了體現本文所提方法的改進效果，利用傳統VO與本文算法的解算結果進行導航結果繪制，并將其與運動捕捉系統提供的參考基準軌跡進行對比，可以得到如圖12所示的無人機軌跡對比圖。其中藍色為運動捕捉系統獲取的無人機的真實飛行軌跡，紅色為本文所提方法得到的無人機軌跡，綠色為傳統VO算法得到的無人機軌跡，藍色圓圈為試驗中設置的無人機運動的起點。由圖12可以看出，本文改進算法解算的無人機位置軌跡與真值的偏移量更小，相比之下，傳統VO算法具有更大的定位偏差。在試驗過程中，觀測到同一語義對象的關鍵幀之間形成了十分緊密的共視關系，為位姿優化提供了一個全局約束。因此，在本次試驗中，改進算法軌跡相對于無人機的真實軌跡漂移較少。在一些設置的轉彎處，無人機機動性較大，傳統VO算法在該處出現了較大的漂移，而本文算法通過慣性信息的輔助，在該處精度有所提升。

圖12 無人機室內定位試驗軌跡對比圖Fig.12 Trajectories comparison diagram of UAV indoor positioning experiment

圖13 無人機室內定位試驗三軸位置誤差對比圖Fig.13 Comparison diagram of three-axis position error in UAV indoor positioning experiment

圖14 無人機室內定位試驗位置誤差箱線對比圖Fig.14 Comparison diagram of position error box-plot of UAV indoor positioning experiment

圖13和圖14所示分別為無人機飛行試驗中的三軸位置誤差對比圖以及位置誤差箱線對比圖。圖13更直觀地顯示了各算法的三維位置誤差隨時間的變化。從圖13可以看出，經過語義信息輔助以及IMU融合的視覺里程計導航誤差總體更小。從圖14可以看出，在飛行過程中改進算法相對傳統VO算法保持了更優的精度。

為了更加客觀地評價本文算法的有效性，本文對采集的共三組試驗數據的定位結果進行了定量統計，表2所示為定位誤差統計結果。表2中包含了傳統VO以及本文所提的改進慣性語義融合VO的三軸位置誤差以及總定位誤差，其中總定位誤差由對三軸位置誤差的平方和開根號求得，對應表中的位置誤差項。

表2 無人機飛行試驗位置誤差統計

從表2可以看出，在無人機室內定位試驗中，與傳統視覺VO相比，本文所提算法具有更小的三軸定位誤差以及總定位誤差，總體定位精度相較于傳統VO算法提高了約40.0%。因此，本文所提方法具有更高的定位精度和魯棒性。

5 結論

針對傳統VO算法抗干擾能力不足、定位精度及魯棒性差的問題，本文提出了一種基于語義信息輔助的無人機視覺/慣性融合定位方法。該方法將語義平面表述為語義路標，利用VO信息構建位姿-語義路標圖，并通過非線性優化進行位姿的優化求解。為了彌補單一傳感器的不足，本文通過IMU預積分技術進一步融合了慣性傳感器信息。通過實際的無人機飛行試驗對所提方法進行了驗證，以運動捕捉系統獲取的定位結果為基準參考，結果表明，所提方法能夠有效提高傳統視覺里程計的定位精度。