劉大鵬

（浙江農(nóng)林大學(xué)光機電工程學(xué)院，浙江 杭州 311300）

0 引言

隨著人工智能技術(shù)飛躍式的發(fā)展，室內(nèi)移動機器人開始廣泛進(jìn)入人們的日常生活中，智能家居、工業(yè)輔助生產(chǎn)、物流便捷式倉儲與服務(wù)型機器人等熱點內(nèi)容引起人們的關(guān)注與研究。而VI-SLAM（Visual And Inertial Simultaneous Localization And Mapping，視覺慣性即時定位與地圖構(gòu)建）[1-4]中定位技術(shù)在智能移動機器人執(zhí)行任務(wù)過程中發(fā)揮著舉重若輕的作用，使得移動機器人在未知場景中，通過識別障礙物的相對位置和方向，防止本體與物體接觸碰撞；通過感知且估計本體所在空間位姿信息，保證移動機器人構(gòu)建地圖與場景環(huán)境信息全局的一致性。因此，研究VI-SLAM定位技術(shù)有助于增強移動機器人在室內(nèi)環(huán)境中的空間信息感知與定位的功能，并為移動機器人自主導(dǎo)航提供先驗信息。

目前，VI-SLAM根據(jù)視覺傳感器與IMU數(shù)據(jù)融合方式分為松耦合和緊耦合。其中，松耦合[5]是指相機和IMU分別進(jìn)行各自的位姿估計，然后對它們各自的結(jié)果共同構(gòu)建運動方程和觀測方程進(jìn)行位姿估計。Lynen等[6]提出MSF-EKF框架，建立按時間順序的視覺測量位姿狀態(tài)緩存區(qū)，當(dāng)IMU狀態(tài)數(shù)據(jù)獲取時插入到相應(yīng)的時間戳的狀態(tài)緩存區(qū)，更新視覺慣性融合后的狀態(tài)，以提高系統(tǒng)定位的精確性與一致性。

與松耦合相比，視覺慣性系統(tǒng)采用IMU的狀態(tài)數(shù)據(jù)與相機的觀測數(shù)據(jù)聯(lián)合后進(jìn)行位姿估計優(yōu)化的緊耦合[7]關(guān)聯(lián)方式，系統(tǒng)更加健壯，且定位更精準(zhǔn)。Leutenegger等[8]提出采用緊耦合的方式將視覺與IMU測量數(shù)據(jù)關(guān)聯(lián)，構(gòu)建基于關(guān)鍵幀的全概率的視覺與慣性誤差項，并以關(guān)鍵幀的相對不確定性來替代全局不確定性，最后通過滑動窗口的形式對其誤差項進(jìn)行位姿優(yōu)化。

1 雙目視覺慣性系統(tǒng)框架

本文構(gòu)建的VI-SLAM框架系統(tǒng)如圖1所示：在系統(tǒng)前端的環(huán)境探索中，采用雙目相機以特征法提取場景特征，并以改進(jìn)光流法匹配跟蹤，IMU使用流形預(yù)積分感知移動機器人運動位姿；在初始化階段，移動機器人提供視覺慣性初始運動參數(shù)；采用基于關(guān)鍵幀的緊耦合方式對視覺慣性數(shù)據(jù)進(jìn)行定位融合，并在后端采取滑動窗口對運動位姿進(jìn)行非線性優(yōu)化得到移動機器人的軌跡姿態(tài)。

圖1 視覺慣性系統(tǒng)框架圖

2 雙目視覺里程計

在前端特征檢測器選擇的過程中，為了平衡系統(tǒng)的實時性與定位精度。對此，提出雙目特征點法和改進(jìn)光流結(jié)合的視覺前端里程計用于對場景特征的檢測與跟蹤。Fast角點是根據(jù)圖片像素點亮度差異提取環(huán)境局部特征，且在圖像特征點提取速度和數(shù)量上表現(xiàn)優(yōu)異，故選擇其作為特征提取器[9]。為提升系統(tǒng)實時性和提高系統(tǒng)適應(yīng)動態(tài)場景的魯棒性，在特征點匹配跟蹤時采取改進(jìn)光流算法。

在圖像特征點提取與正向跟蹤匹配后，特征點跟蹤由于噪聲存在誤匹配，錯誤匹配將影響下一幀中目標(biāo)位置的判斷。針對特征存在誤匹配，對于圖像幀采用逆向光流[10]跟蹤，如圖2所示，通過當(dāng)前幀追蹤上一幀的角點，篩除圖像像素距離過大的外點，提高特征點匹配質(zhì)量。

圖2 正向和逆向光流跟蹤圖

稀疏LK光流法在實際場景下，相機獲取的圖像特征點相對速度過快，導(dǎo)致像素運動偏移量過大，造成跟蹤失敗，因此，選取圖像金字塔來追蹤特征點偏移量d。

如圖3所示，圖像金字塔指對同一圖像進(jìn)行尺寸縮放得到不同分辨率下的圖像，光流跟蹤時自頂層到原始圖像由粗到細(xì)依次優(yōu)化得到最佳光流，在最優(yōu)解處，此時前后相鄰幀圖像對應(yīng)特征點相似度最高。為進(jìn)一步提升結(jié)果的精確性，金字塔光流跟蹤時在金字塔的每一層采取多次迭代來求得精確的最優(yōu)解，最終獲得領(lǐng)域內(nèi)匹配誤差最小的光流。

圖3 金字塔光流法示意圖

相機位姿的運動估計，以左目圖像為基準(zhǔn)，在經(jīng)過左右立體特征提取、光流跟蹤和前后幀間特征正逆向光流追蹤后，采用三角化獲得特征點深度信息，進(jìn)而解算出上一幀與當(dāng)前幀的特征點的三維坐標(biāo)。并且已知相應(yīng)的圖像特征點二維坐標(biāo)，采用PNP（Perspective-n-Point，透視N點投影）構(gòu)建BA最小二乘法優(yōu)化相機幀間位姿（R，T）。

3 視覺慣性數(shù)據(jù)初始化

在移動機器人視覺慣性數(shù)據(jù)融合位姿估計之前，需要對視覺傳感器與IMU數(shù)據(jù)進(jìn)行對齊校準(zhǔn)，為狀態(tài)變量的精準(zhǔn)求解與系統(tǒng)快速收斂提供必要的初始化參數(shù)。且良好的初始化在系統(tǒng)丟失時能夠快速重啟進(jìn)入工作狀態(tài)。

1）建立IMU預(yù)計分離散模型，根據(jù)視覺與慣性旋轉(zhuǎn)量一致性建立最小二乘法方程迭代優(yōu)化陀螺儀偏置bg；

2）由位姿的平移轉(zhuǎn)換關(guān)系線性推算系統(tǒng)尺度S和重力g的近似值；

3）IMU坐標(biāo)系與重力旋轉(zhuǎn)構(gòu)建線性方程，獲取加速度計偏置ba，并對尺度和重力進(jìn)一步計算精確數(shù)值；

4）利用IMU速度預(yù)計分模型求得速度v參數(shù)。

4 滑動窗口緊耦合的非線性優(yōu)化

隨著移動機器人在各種場景的不斷探索，視覺慣性系統(tǒng)獲取的環(huán)境路標(biāo)、特征點的觀測量和狀態(tài)估計量的規(guī)模劇增，系統(tǒng)的實時性與魯棒性受到約束，系統(tǒng)在衡量精度與計算效率后，采取基于關(guān)鍵幀的滑動窗口來控制系統(tǒng)狀態(tài)量的參數(shù)優(yōu)化。

滑動窗口：在窗口內(nèi)固定關(guān)鍵幀數(shù)量N=10，在加入一個新的關(guān)鍵幀以及觀測到的路標(biāo)點時，對一個舊的關(guān)鍵幀進(jìn)行“剔除”。基于滑動窗口狀態(tài)量緊耦合的非線性優(yōu)化中，通過邊緣化把舊關(guān)鍵幀的狀態(tài)量從當(dāng)前固定窗口的狀態(tài)估計中移除，但移除的狀態(tài)量作為先驗信息保留在當(dāng)前滑動窗口中，避免窗口中其他幀和路標(biāo)點信息的丟失。

為了達(dá)到視覺慣性系統(tǒng)的定位準(zhǔn)確性和全局一致性，在滑動窗口中構(gòu)造邊緣化殘差、IMU殘差和視覺誤差項來非線性優(yōu)化移動機器人的狀態(tài)變量，如下所示：

邊緣化殘差為滑動窗口移除舊關(guān)鍵幀的位姿與特征時，將與其關(guān)聯(lián)的約束關(guān)系轉(zhuǎn)化為先驗（prior）參與系統(tǒng)優(yōu)化；IMU殘差為相鄰幀間預(yù)積分觀測值與估計值的殘差；視覺誤差是場景特征點在相機下的重投影誤差，即特征點在當(dāng)前幀中光流跟蹤的歸一化坐標(biāo)與特征點在當(dāng)前幀的重投影歸一化坐標(biāo)的殘差[11-14]。

5 實驗測試與結(jié)果

對構(gòu)建的視覺慣性SLAM算法（簡稱為Ours）采用公共EuRoc數(shù)據(jù)集進(jìn)行測試評價，并與當(dāng)前開源的OKVIS、VINS-Fusion對比，獲取實驗測試現(xiàn)象與結(jié)果，如表1所示。EuRoc數(shù)據(jù)集共五個序列（采用簡寫表示），MH_01至MH_05數(shù)據(jù)集背景為大型機器廠房。數(shù)據(jù)集含有豐富的信息，并設(shè)置紋理、光照和速度組合的對比場景。

1）定位精度的檢驗：由表1可知，在公開的EuRoc的五個數(shù)據(jù)集中，Ours與VINS-Fusion、OKVIS實驗測試中，性能最好的算法，并以加粗字體突出顯示。在MH_01良好的紋理、場景明亮和低速運動的狀況下，系統(tǒng)保持著優(yōu)異的位姿估計精準(zhǔn)性。與MH_01相比MH_03在快速運動時，移動機器人系統(tǒng)穩(wěn)健性良好。在MH_04與MH_05中，移動機器人快速運動中，降低環(huán)境光線強度，數(shù)據(jù)表明VINS-Fusion定位性能最優(yōu)，Ours其次，OKVIS表現(xiàn)最差。然而，Ours在五個場景數(shù)據(jù)的均方根誤差的平均值最小，可知Ours在各種變化場景中保持良好的定位準(zhǔn)確性。

表1 雙目視覺慣性SLAM算法的均方根誤差

2）累計誤差的檢驗：移動機器人對場景的空間信息進(jìn)行感知，在噪聲的干擾和位姿的錯誤恢復(fù)下，系統(tǒng)的軌跡定位在長時間的運行中產(chǎn)生累計誤差，對三種算法進(jìn)行軌跡定量剖析。

隨著運動距離的增加，移動機器人視覺慣性的誤差逐漸累積，采用折線圖來比較絕對位姿中的平移偏量。如圖4所示，通過MH_01、MH_03和MH_05三種不同條件場景下，觀察RMSE的中位數(shù)，對比三種算法在長時間運行下系統(tǒng)累積平移誤差的大小，并觀測三種算法箱體波動程度的走勢圖。結(jié)果表明，Ours算法隨著距離的增加系統(tǒng)累計誤差在箱體上下波動幅值最小，VINS-Fusion中等，OKVIS波動最大，體現(xiàn)出Ours算法系統(tǒng)的穩(wěn)定性。

圖4 OKVIS、VINS-Fusion和Ours絕對位姿的平移誤差對比圖

3）系統(tǒng)的穩(wěn)健性測試：在教學(xué)樓實際環(huán)境下，手持與裝載Ubuntu18.04電腦連接的Realsense T265，行走路線從一樓至四樓，中途經(jīng)走廊和帶有落地窗的樓梯平臺，測試期間在光照不足和弱紋理的走廊快速移動，上樓時設(shè)置相機抖動，轉(zhuǎn)彎時快速旋轉(zhuǎn)變換前進(jìn)方向。

構(gòu)建的視覺慣性SLAM系統(tǒng)數(shù)據(jù)解耦后生成的軌跡路線如圖5所示，結(jié)果表明，算法能夠在弱紋理、光線不足、快速旋轉(zhuǎn)和移動的挑戰(zhàn)場景中實現(xiàn)連貫的位姿追蹤，驗證了系統(tǒng)良好的穩(wěn)健性。

圖5 教學(xué)樓軌跡路線圖

6 結(jié)束語

通過IMU數(shù)據(jù)與視覺圖像幀信息融合，在兼顧系統(tǒng)精度的前提下，通過構(gòu)建改進(jìn)LK光流法跟蹤Fast角點的輕量型雙目視覺里程計，并與IMU流形預(yù)計分?jǐn)?shù)據(jù)緊耦合實現(xiàn)數(shù)據(jù)關(guān)聯(lián)，經(jīng)基于關(guān)鍵幀的滑動窗口內(nèi)非線性優(yōu)化移動機器人位姿，實現(xiàn)移動機器人在室內(nèi)定位的穩(wěn)健性與魯棒性。