緊耦合的移動端實時位姿優(yōu)化方法①

孫曉明,宋 瀅

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

1 前言

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

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

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

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

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

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

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

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

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

2 位姿優(yōu)化方法

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

本文方法在傳統(tǒng)純視覺位姿優(yōu)化算法上,增加了慣性傳感器數(shù)據(jù)對軌跡位姿測量約束,并采用加權(quán)視覺重投影誤差與慣性運動誤差更加優(yōu)化求解出相機位姿軌跡信息.優(yōu)化方法主要內(nèi)容如下.

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

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

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

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

其中,λ,μ為誤差權(quán)重系數(shù)且λ+μ=1,更加置信IMU數(shù)據(jù),取權(quán)重 μ=0.6.

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

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

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

當(dāng)?shù)趇幀關(guān)鍵幀與當(dāng)前關(guān)鍵幀j優(yōu)化計算完成后,采用連續(xù)兩關(guān)鍵幀IMU 數(shù)據(jù)與前一幀關(guān)鍵幀進行聯(lián)合優(yōu)化.當(dāng)SLAM 系統(tǒng)通過圖像數(shù)據(jù)計算產(chǎn)生第j+1幀關(guān)鍵幀時,保存當(dāng)前優(yōu)化結(jié)果,舍棄第i幀,通過累積優(yōu)化計算當(dāng)前兩關(guān)鍵幀,重復(fù)上述算法流程,直至整個系統(tǒng)流程結(jié)束.

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

3 實驗分析

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

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

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

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

圖1 RMSE 評測結(jié)果

由圖1可知,當(dāng) λ 取值為0.4,0.3,0.2時,重建軌跡位姿與真實軌跡位姿的均方根誤差最小.結(jié)合圖2誤差平方和SSE 可得,當(dāng) λ=0.4,μ=0.6時,SSE 值最優(yōu),故選擇權(quán)重系數(shù)為λ=0.4,μ=0.6.

圖2 SSE 評測結(jié)果

從EuRoC 數(shù)據(jù)集中選取了6 組對比實驗數(shù)據(jù),評估位姿優(yōu)化系統(tǒng)的精確性與穩(wěn)定性,通過計算RMSE(軌跡誤差),不同方法計算結(jié)果如表1所示.

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

表1 不同算法的數(shù)據(jù)集RMSE 評測結(jié)果

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

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

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

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

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

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

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

4 結(jié)論與展望

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

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

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

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