緊耦合的移動端實時位姿優化方法①

孫曉明,宋 瀅

(浙江理工大學 信息學院,杭州 310018)

1 前言

在計算機領域,運動狀態估計有著廣泛的應用場景,涉及機器人導航、自動駕駛、虛擬現實以及增強現實等諸多領域.在眾多傳感器融合方案中,視覺與慣性傳感器融合更好的魯棒性.一方面,相機傳感器能夠提供豐富的環境視覺信息,以便進行環境定位、回環檢測以及三維重建等;另一方面,慣性傳感器(inertial measurement unit,IMU)可直接提供自身運動信息,可恢復真實尺度信息,重力方向以及自身姿態信息.

由于光照信息變化、弱紋理以及運動模糊等外部環境因素,從而導致視覺跟蹤失敗.而IMU 預積分估計可有效彌補純視覺跟蹤效果,從而有效改善運動跟蹤性能.

然而,對于單目慣性視覺運動估計仍存在些許問題會影響實際應用.一是系統初始化問題,由于缺少直接距離(深度)信息,很難將單目視覺跟蹤系統與慣性估計相結合.二是單目慣性視覺問題是高度非線性的,數值迭代求解方面仍面臨著挑戰以及位姿估計在長時間運動下的軌跡漂移問題.為此,希望運動狀態估計系統在已知空間位置進行初始化,并緩慢移動.基于以上兩點,限制了運動狀態估計在實際中的應用.

目前,以單目視覺為基礎的里程計以及同步定位與地圖構建(simultaneous localization and mapping,SLAM)等領域有著很多優秀的研究工作.如首次采用多線程方法的PTAM[1],以及后續改進的SVO[2]、LSDSLAM[3]以及ORB-SLAM[4]等.

一般的單目慣性測量,如Weiss[5]和Lynen[6],采用松耦合的方式,將視覺信息與IMU 信息作為獨立模塊計算.松耦合采用擴展卡爾曼濾波,IMU 作為狀態變量,視覺信息用于更新變量.更進一步,則是采用擴展卡爾曼濾波[5,7,8]或圖優化算法[9,10],將圖像信息與IMU 信息聯合優化.MSCKF[11]是著名的基于擴展卡爾曼濾波EKF的慣性視覺里程計,通過相機姿態作為狀態變量,并使用多角度相機的多相機視圖來形成約束更新.為實現一致處理時間,基于圖優化的視覺慣性里程計(visual inertial odometry,VIO)方法通常采用邊緣化窗口外的狀態與測量值,在最近狀態的有限窗口進行數據優化.由于迭代求解非線性系統的高計算量,很難實現實時計算.

根據視覺信息處理方式,位姿優化可分為最小化光流誤差的直接法[2,3,11]和基于最小化重投影誤差的非直接方法[7,9,12].直接法需要較好的初始值估計,而非直接法則需要更多的計算資源進行特征提取與匹配.目前,非直接法因算法成熟及魯棒性受到更多的應用,而直接法的優勢則是可像素操作并可使密集匹配變得更加容易.

通常IMU 數據的獲取頻率遠遠超過相機圖像數據.為此提出了很多方法計算高頻IMU 數據測量.最直接的方法采用EKF 方法將IMU 數據作為狀態變量進行計算[5,11].在圖優化算法中,采用IMU 預積分方式避免IMU 積分重計算.這種方法是由Lupton 等[13]第一次提出,采用歐拉角參數化旋轉變量.Forster 等[10,14]通過添加IMU 殘差校正進一步提升了預積分方法.

香港科技大學提出以稀疏光流法跟蹤特征點為前端,采用IMU 預積分與視覺構建緊耦合框架-VINSMono 單目視覺SLAM 系統[15],解決了視覺與IMU 尺度對齊,重力方向、IMU 數據殘差等問題.Campos等[16]在2020年提出第一個支持純視覺、視覺-慣性以及多地圖的實時SLAM 系統,可在單目、雙目和RGB-D相機上利用針孔或魚眼成像模型運行.Campos 等在單目-慣性初始化[17]對IMU 進行了殘差計算,恢復了單目相機尺度,并對齊到慣性系.

本文方法采用慣性傳感器數據與相機圖像數據以緊耦合策略構建聯合狀態變量,通過最小化加權重投影誤差與慣性運動誤差在移動端實現實時位姿優化計算.緊耦合策略可有效保證全局軌跡位姿一致性,減小漂移誤差.在移動端可實時并行采集IMU 數據與相機圖像數據,在保證實時性的同時,可獲得更小誤差參數的重建軌跡位姿.

2 位姿優化方法

傳統純視覺位姿優化算法一般采用相機圖像信息,首先對相機進行參數標定,獲取包含相機焦距等參數在內的內參矩陣信息;而后對所獲取圖像進行特征提取、匹配,建立圖像關鍵幀;根據同一場景圖像間的對極幾何約束關系,求解出對應基礎矩陣或本質矩陣;對基礎矩陣或本質矩陣進一步分解,可求解出的相機在兩幅圖像間的旋轉、位移;而后通過計算特征點重投影誤差進行位姿優化,獲取相機軌跡位姿信息.

本文方法在傳統純視覺位姿優化算法上,增加了慣性傳感器數據對軌跡位姿測量約束,并采用加權視覺重投影誤差與慣性運動誤差更加優化求解出相機位姿軌跡信息.優化方法主要內容如下.

慣性運動測量單元在慣性坐標系B下,在 ?t間隔時間內測量所得加速度為aB、角加速度wB.由于傳感器自身構造原因,尤其是存在傳感器噪聲,從而導致測量值存在誤差.為消除傳感器噪聲,采用加速度計誤差ba和陀螺儀誤差bg表示.

在世界坐標系W下,離散化IMU 旋轉變量RWB∈S O(3),位置變量PWB以及速度變量vWB,可采用速度學公式進行計算如下:

本文提出采用緊耦合的方式,將IMU 數據與圖像信息數據緊密結合在一起,通過聯合優化方式求解.基于Mur-Artal 等[18]提出的采用ORB 特征點的實時ORB-SLAM 系統,采用初始定義兩個關鍵幀之間的運動變化,旋轉變量差值 ?R,速度變量差值 ?v,位置變量差值 ?P,根據兩關鍵幀之間的IMU 數據預積分計算,以第i關鍵幀的運動差值變量定義如下:

基于已實現的實時ORB-SLAM 系統,可運行在大尺度場景中,重建局部地圖以便跟蹤與映射,并可構建真實環境的點云地圖.本文方法可實現在預設幀率下的傳感器姿態、速度以及IMU 誤差的位姿跟蹤計算代替原始單目跟蹤系統.根據初始的ORB-SLAM 系統,預計算得到相機位姿之后,局部地圖中的地圖點可投影與對應幀圖像中的關鍵點相匹配.相較于之前關鍵幀i,通過最小化特征點誤差投影和IMU 誤差來優化當前關鍵幀j,故提出優化目標定義如下:

其中,λ,μ為誤差權重系數且λ+μ=1,更加置信IMU數據,取權重 μ=0.6.

而基于ORB 特征點的重投影誤差函數Eproj定 義如下:

其中,xk為特征點圖像坐標,XWk為世界坐標系下的地圖點坐標,Σk為關鍵點的尺度信息矩陣.而IMU 運動軌跡誤差函數定義如下:

其中,ΣI是預積分信息矩陣,ΣR是IMU 誤差隨機參數,ρ是Huber 損失函數,用于表示穩健回歸損失[20].本文為解決以上優化問題,采用高斯-牛頓法進行優化求解計算[21].

當第i幀關鍵幀與當前關鍵幀j優化計算完成后,采用連續兩關鍵幀IMU 數據與前一幀關鍵幀進行聯合優化.當SLAM 系統通過圖像數據計算產生第j+1幀關鍵幀時,保存當前優化結果,舍棄第i幀,通過累積優化計算當前兩關鍵幀,重復上述算法流程,直至整個系統流程結束.

緊耦合的實時位姿優化方法整體流程如下:1) 預處理采用張正友標定法進行相機標定;2) 采用相機圖像數據與慣性傳感器數據作為實時輸入,對圖像信息進行特征提取、匹配;3) 根據對極幾何約束求解出初始相機位姿,并預積分計算兩幀間的慣性運動數據;4) 構建聯合狀態變量,最小化加權重投影誤差與慣性運動誤差求解出相機位姿信息并保存;5) 實時輸入新的圖像數據與慣性運動數據進行迭代算法求解.

3 實驗分析

為了更好的衡量評價本文提出的移動端位姿優化方法,采用EuROC 數據集[22]進行系統評估與誤差分析.EuRoC 數據集使用MAV 采集了兩個不同房間及一個工業環境,總共11 組數據,并根據照明、紋理、運動快慢以及模糊程度分為簡單、中等、困難3 等級數據組.此數據集的良好特性使得可以很好的檢驗評估類似慣性視覺的位姿軌跡系統,數據測試時僅采用單目圖像(左目)作為視覺圖像流輸入,IMU 數據流輸入以及真實軌跡參照.

在計算完成位姿軌跡后,采用位姿軌跡文件TUM導出,采用evo 軌跡評測對重建位姿進行誤差評價.假設真實的運動軌跡為T,預估軌跡為Te.采用TUM 軌跡文件格式,由時間戳、三維空間位置以及四元數表示的旋轉變量.假定真實軌跡與重建軌跡一一對應,那么針對任意第i個位姿,那么將軌跡誤差定義為:

則根據兩條軌跡的均方根(root mean square error,RMSE)誤差為:

為確定重投影誤差和IMU 誤差的最佳權重系數,針對 λ,μ估值,采用同一數據集,采用不同的權重系數進行重建實驗,評測權重系數的有效性與穩定性.采用順序搜索遍歷,λ分別取0.1,0.2,0.3,···,0.9,則 μ取0.9,0.8,0.7,···,0.1,得到RMSE和SSE 結果如圖1、圖2所示.

圖1 RMSE 評測結果

由圖1可知,當 λ 取值為0.4,0.3,0.2時,重建軌跡位姿與真實軌跡位姿的均方根誤差最小.結合圖2誤差平方和SSE 可得,當 λ=0.4,μ=0.6時,SSE 值最優,故選擇權重系數為λ=0.4,μ=0.6.

圖2 SSE 評測結果

從EuRoC 數據集中選取了6 組對比實驗數據,評估位姿優化系統的精確性與穩定性,通過計算RMSE(軌跡誤差),不同方法計算結果如表1所示.

從表1可見,在不同數據集上,采用視覺信息與IMU數據同時約束方法的軌跡重建方法較僅采用純視覺方法的ORB-SLAM2 獲取的軌跡效果均好.香港科技大學提出的VINS-Mono 視覺慣性里程計,對相機軌跡的重建的效果較ORB-SLAM 方法較好.

表1 不同算法的數據集RMSE 評測結果

而本文提出的位姿優化算法在EuRoC的6 個數據集上的表現整體偏好,除MH_01_easy 數據庫集誤差相差0.115 之外,其他數據集表現均優于其他方法,軌跡誤差更小,更加接近真實的軌跡位姿,取得較好的相機軌跡效果.

通過上述分析可見,本文提出的算法在相機軌跡重建效果較好,提供了一種新的位姿優化方式.

由于ORB-SLAM2 并沒有采用IMU 數據處理,僅采用純視覺方式進行相機位姿重建,缺少一個尺度因子,故評測絕對誤差時不與考慮.

對3 種不同的重建方法采用EVO 評測軌跡的真實誤差,效果如表2.

將不同方法的重建相機位軌跡與數據集的真實軌跡作比較,驗證重建相機軌跡的重建效果.

結合表1,根據表2可知,3 種方法在重建軌跡誤差相差不大的情況下,展現出不同的軌跡誤差分布.從表2中的軌跡圖可知,ORB-SLAM3 雖然均方根誤差較大,軌跡誤差分布均勻,說明對軌跡處理的綜合性能較好,可處理更多種的軌跡情況,魯棒性更好.而VINS-Mono 與本文方法呈現兩種不同的誤差浮動表現.本文方法在相機移動位移幅度較大時,誤差顯著浮動提升,歸其原因在位移較大時,IMU 數據積分誤差較大,而優化方法給予更高的權重比,不能很好的優化計算大位移時的位姿軌跡.基于VINS-Mono初始化算法,采用松耦合的傳感器融合方法,在軌跡重建的初始階段對相機位姿軌跡的重建效果較差,相機軌跡誤差較大.

表2 不同算法絕對軌跡誤差

4 結論與展望

本文提出了一種可實時運行在移動端的緊耦合慣性視覺位姿優化方法.可根據實時相機圖像數據與慣性運動數據計算載體在自身所構建地圖中的位置,并可獲得優化后的相機位姿信息,為后續三維重建工作提供可靠相機姿態信息,更好地為三維重建服務.

實驗證明,本文提出位姿優化方法,相較于ORBSLAM3 基于IMU 最大后驗估計的SLAM 系統和VINSMono 視覺慣性里程計,在相同數據集下,具有更高的尺度還原性以及軌跡準確性.

該位姿優化方法也存在不足,在位移較大時,IMU數據不能很好的積分優化,導致局部軌跡誤差較大.這需要探索更好的IMU 數據模型,以期修正較長時間下大位移的IMU 數據誤差問題,達到更好的相機軌跡還原.本方法并過于依賴ORB-SLAM 系統進行初始化,初始化時間較ORB-SLAM2 系統略長.

未來工作預期利用移動端的多相機鏡頭信息(類似雙目)以及景深鏡頭信息,加以融合求解,優化IMU 數據模型.以期通過更加精確的傳感器與視覺信息數據模型完成初始化,緊耦合優化計算,提高整個系統精確性與穩定性.