孫曉明,宋 瀅

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

1 前言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3 實(shí)驗(yàn)分析

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

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

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

為確定重投影誤差和IMU 誤差的最佳權(quán)重系數(shù),針對 λ,μ估值,采用同一數(shù)據(jù)集,采用不同的權(quán)重系數(shù)進(jìn)行重建實(shí)驗(yàn),評測權(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時(shí),重建軌跡位姿與真實(shí)軌跡位姿的均方根誤差最小.結(jié)合圖2誤差平方和SSE 可得,當(dāng) λ=0.4,μ=0.6時(shí),SSE 值最優(yōu),故選擇權(quán)重系數(shù)為λ=0.4,μ=0.6.

圖2 SSE 評測結(jié)果

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

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

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

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

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

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

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

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

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

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

4 結(jié)論與展望

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

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

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

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