雙目視覺與慣導融合的移動機器人定位方法

許智賓，李宏偉，張 斌，肖志遠，鄧 晨

1. 鄭州大學信息工程學院，河南 鄭州 450052； 2. 鄭州大學地球科學與技術學院，河南 鄭州 450052； 3. 鄭州大學水利科學與工程學院，河南 鄭州 450001

隨著社會的進步和人工智能技術的快速發展，自主智能機器人受到了廣泛關注。移動機器人就是自主智能機器人的一種，其應用領域眾多，如倉儲物流、自動駕駛、服務機器人等。研究移動機器人的同時定位與建圖技術對于提高移動機器人的自主化、智能化等具有重要意義[1]。

視覺SLAM使用的視覺傳感器主要有單目相機，雙目相機和RGB-D相機[2]。單目相機無法獨立地從一幀圖像中恢復出周圍環境的尺度信息[3]，RGB-D相機在室外環境中容易受到光照環境的影響[4]，而雙目相機可以做到通過數學方法得到準確的尺度信息，使其可以克服RGB-D相機在獲得尺度信息時容易受到光線的影響，以及單目相機產生的尺度不一致的問題。

視覺里程計是視覺SLAM系統中不可或缺的組成部分[5]。在視覺里程計中，根據其實現方法的不同可以分為基于特征法的視覺里程計和基于直接法的視覺里程計。基于特征法的視覺里程計是在圖像中提取特征點并計算描述子，通過描述子實現特征匹配，通過計算最小化重投影誤差來估計當前幀的位姿。基于特征法的視覺里程計具有定位精度較高的特點，但是由于需要計算圖像幀之間的特征點和描述子，會占用部分計算資源，影響視覺里程計的實時性。如ORB-SLAM2[6]、ProSLAM[7]、OpenVSLAM[8]等都是比較優秀的基于特征法的視覺SLAM系統。基于直接法的視覺里程計僅提取特征點而不計算描述子，通過計算光流得到相鄰兩個圖像幀之間特征點的匹配關系，之后通過計算最小化光度誤差估計移動機器人的位姿信息[9]。如LSD-SLAM[10]、DSO[11]、DVO-SLAM[12]、RGBD-SLAM-V2[13]等都是比較優秀的基于直接法的視覺SLAM系統。基于直接法的視覺里程由于不需要計算特征點的描述子，使其運行實時性較好，但是光流法的運動跟蹤策略使其定位精度往往不如基于特征法的視覺里程計。

近年來，慣導設備與視覺傳感器融合的SLAM系統已經成為一個熱門的SLAM技術研究課題[14]。視覺慣導融合[15]的SLAM系統現在有很多優秀的開源方案，如MSCKF[16]、ROVIO[17]、OKVIS[18]、StructVIO[19]、VINS-Mono[20]等。MSCKF是一種基于濾波的視覺慣導融合的視覺SLAM系統，它設計了一種滑動窗口模型，將滑動窗口中被多個圖像幀觀測到的特征點及其對應的相機位姿作為擴展卡爾曼濾波的更新。但是MSCKF需要一個良好的初始深度值以及初始協方差，否則將會導致位姿估計誤差較大。ROVIO也是一種基于擴展卡爾曼濾波的視覺慣導融合的SLAM系統，在運動跟蹤階段使用基于圖像塊的光流法進行特征跟蹤，將IMU數據用于狀態傳遞，將視覺數據用于狀態更新。OKVIS是一種基于非線性優化的視覺慣導融合的SLAM系統，它在滑動窗口模型種構建非線性優化函數進行位姿優化，包括了視覺重投影誤差和IMU測量誤差。OKVIS在前端視覺里程計部分采用基于特征法的視覺里程計，在圖像中提取Harris角點[21]并提取BRISK描述子[22]。VINS-Mono是一種視覺慣導融合的視覺SLAM系統，使用光流法進行運動跟蹤，使用IMU預積分技術對IMU數據進行預處理，同時使用一種滑動窗口模型將視覺數據與IMU數據進行緊耦合方式的非線性優化[20]。

由于視覺慣導融合的視覺SLAM系統中實現運動跟蹤方式的不同，仍然受到特征法視覺里程計與直接法視覺里程計缺點的影響，如特征法計算描述子會影響其實時性，直接法帶來的累計誤差會影響其定位精度。針對上述問題，本文采用一種融合特征法與直接法的半直接法視覺里程計進行運動跟蹤，提高其運行效率，同時采用視覺慣導融合的方式，構建基于滑動窗口的非線性優化的方式，進一步提高位姿估計的精度。

1 方法與原理

本文在基于特征法[23]的視覺SLAM系統和基于直接法的視覺SLAM系統的基礎上提出一個融合了特征法與直接法的視覺里程計，并在此基礎上融合視覺傳感器與慣性測量單元，提出一種視覺慣導融合的視覺SLAM框架。在融合算法中，利用光流法[24]進行運動跟蹤得到移動機器人的初始姿態，并將地圖點投影到當前幀中，通過構建重投影誤差函數估計位姿。同時利用IMU的瞬時高精度測量數據來彌補穩健性較低的視覺數據，以及利用長時間視覺數據的相對穩健性來抑制IMU數據的累計漂移誤差。雙目視覺與IMU融合的SLAM系統框架如圖1所示。

1.1 融合特征法與直接法的半直接法位姿估計方法

為了克服直接法和特征法自身的缺點，并且保持直接法計算速度快，特征法精度高的特點，提出一種融合直接法和特征法的半直接法雙目視覺里程計。融合算法的流程如圖2所示。

融合算法主要包括雙目視覺初始化、直接法估計初始位姿、特征法優化位姿3個模塊。

1.1.1 雙目視覺初始化

由于單目相機無法從一幀圖像中恢復出觀測到的目標點的尺度，因此單目SLAM系統通常采用三角化的方式，通過連續幾幀圖像恢復尺度信息，導致視覺里程計在開始工作之前需要有一個初始化的階段，為之后的位姿估計提供尺度信息。而對于雙目相機，可以根據雙目相機模型使用單幀圖像計算得到圖像中觀測到的特征點的尺度。使用雙目相機進行視覺里程計的實現可以簡化初始化的過程，提高運行效率。

圖1 視覺慣導融合的SLAM系統框架Fig.1 SLAM system framework of visual inertial navigation fusion

圖2 融合算法流程Fig.2 Flow chart of the fusion algorithm

在初始化階段，首先在左目圖像中提取特征點，然后使用光流法在右目圖像中跟蹤左目圖像中的特征點，獲取左右兩幅圖像中的匹配點對。

雙目相機在標定之后其基線b和焦距f已知。假設對應一組匹配正確的點對，在左目圖像中的坐標為UL，在右目圖像中的坐標為UR。則根據雙目相機模型有

(1)

式中，z為觀測到的特征點在相機坐標系下的尺度信息；b為雙目相機基線；f為焦距。

在有了尺度信息之后，可以得到特征點在相機坐標系下的世界坐標。假設空間點在相機坐標系下的坐標為(X,Y,Z)，特征點的像素坐標為(u,v)，則有

(2)

即可求解得到相機坐標系下的三維坐標(X,Y,Z)，得到特征點的尺度信息，為雙目視覺里程計提供一個良好的初始值。

1.1.2 直接法估計初始位姿

在視覺里程計通過雙目相機獲取到最新一幀的圖像之后，首先使用光流法跟蹤前一個圖像中的特征點，得到它們在當前圖像中的位置信息，并且使用逆向光流的方法去除誤匹配的特征點對，然后根據最小化光度誤差來計算初始位姿。

(3)

光度誤差δI可以表示為

δI(Tt,t+1,u)=It+1(πTt,t+1·π-1(u,dp))-It(u)

(4)

式中，dp是特征點在前一幀圖像中的深度信息；It表示滑動窗口中第t幀圖像。

為了簡化計算，使在計算增量過程中優化方程的雅克比矩陣保持不變，本文采用逆計算的方法計算光度誤差

δI(Tt,t+1,u)=It+1(πTt,t+1·π-1(u,dp))-

It(π(T(ε)·π-1(u,dp)))

(5)

由于構建的計算最小化光度誤差的優化問題是一個非線性方程，因此這里使用高斯-牛頓的方法求解，通過不斷迭代的方式，從相機的一個初始位姿開始，不斷地計算更新增量來更新位姿的估計值

Tt,t+1←Tt,t+1·T(ε)-1

(6)

1.1.3 特征法優化位姿

使用直接法估計初始位姿，在長時間跟蹤后容易產生累計誤差，此時可以通過跟蹤地圖點，將當前圖像中觀察到的地圖點投影到當前幀中，通過計算視覺重投影誤差的方式來消除累計誤差。

將滑動窗口中所有幀觀測到的特征點記為地圖點，然后將所有地圖點根據當前幀的初始位姿投影到當前幀，得到當前幀中特征點與地圖點的匹配關系。為了使投影位置更加精確，使重投影誤差計算有一個良好的初值，本文通過構建特征點位置與地圖點的投影坐標的光度誤差函數來優化當前幀中所有投影點的位置

(7)

對于最小化光度誤差這個優化問題，可以使用迭代高斯-牛頓的方法來求解，最終可以得到更為精確的地圖點在當前幀中的投影位置。

在得到更為精確的地圖點在當前幀中的投影位置之后，通過構建視覺重投影誤差，使用迭代高斯-牛頓法求解，來進一步優化相機位姿

(8)

1.2 IMU數據預處理

IMU傳感器通常是由三軸陀螺儀和三軸加速度計組成的，可以測量得到移動機器人運動的角速度和加速度，但是IMU在獲取數據時會產生測量偏差和噪聲，因此，IMU的數據測量模型可以表示為

(9)

得到IMU的運動模型為

(10)

式中，q表示旋轉；v表示速度；p表示位移。

由于實際使用的情況下，IMU的數據采集頻率要遠大于相機采集數據的頻率，因此需要在離散的時間下對IMU的數據進行積分處理。這里采用一種中值積分的方法對IMU數據進行積分處理，即對于k幀和k+1幀，二者之間的位姿可以使用兩幀之間IMU采集到的所有數據的平均值表示，也就是i時刻與i+1時刻之間的所有IMU數據的平均值

(11)

式中

由于每次計算新的圖像幀的位姿，都需要對所有IMU數據進行積分處理，因此增加了系統的計算量，會導致SLAM系統的運行效率降低。采用IMU預積分可以通過計算兩幀之間p、v、q的增量更新積分值，以降低SLAM系統的計算量。

在連續時間下，IMU預積分的值只與不同時刻的IMU的測量值和IMU偏差有關。根據中值積分的方法，IMU的預積分值在離散時間下的形式為

(12)

式中

1.3 視覺慣導聯合初始化

由于SLAM系統本質上是一個位姿狀態估計器，因此一個良好的初始值能夠很好地提高系統的定位精度，提高系統的穩健性。本文采用松耦合的方式來設計實現雙目視覺與慣導融合的SLAM系統的初始化：首先，僅適用基于純視覺的SfM和雙目相機模型來恢復相機運動并計算圖像中觀測到的特征點的深度信息；然后，將視覺SfM計算得到圖像的旋轉、平移以及特征點的尺度信息通過松耦合的方法與IMU數據進行對齊，完成視覺慣導聯合初始化，為系統提供良好的視覺位姿初始值和IMU初始值。視覺慣導聯合初始化的流程如圖3所示。

圖3 視覺慣導聯合初始化流程Fig.3 Visual inertial navigation joint initialization process

使用半直接法估計相機位姿的流程與第2節中介紹的方法相同，得到當前相機位姿之后根據雙目相機模型恢復特征點的尺度信息，然后通過松耦合的方式將IMU預積分數據與半直接法計算得到旋轉、平移和尺度信息對齊，估計重力、速度及IMU的偏差。

1.4 基于滑動窗口的后端優化方法

設計一種基于滑動窗口的后端優化方法的目的是為了減少SLAM系統位姿計算的累計誤差的同時通過限制處理數據的規模減少計算量，提高SLAM系統的運行效率。

在滑動窗口中僅維持少量的圖像幀，同時記錄滑動窗口中所有的圖像幀的視覺觀測數據與圖像幀之間IMU的預積分數據。

滑動窗口中主要保存了圖像幀之間的IMU數據即滑動窗口中第k幀圖像對應的IMU狀態包括了速度、旋轉、加速度計的偏置及陀螺儀的偏置，相機外參即相機與慣性測量單元之間旋轉和平移，以及特征點的逆深度信息。這些都是要在后端優化階段進行優化的信息。將優化信息表示為

(13)

式中，xk表示了滑動窗口中第k幀圖像對應的IMU狀態；n表示滑動窗口中關鍵幀的總數；m是滑動窗口中所有圖像幀觀測到的特征總數；λi是第i個特征點在第1次觀測到它的圖像幀中計算得到的逆深度。

對于優化變量的計算可以使用非線性優化的方法，通過構建滑動窗口中的視覺觀測值誤差，IMU測量誤差以及滑動窗口中的邊緣化誤差進行優化變量的計算。滑動窗口內誤差函數為

(14)

1.4.1IMU測量誤差

由于慣性測量單元與視覺傳感器采集數據的頻率不同，可以使用流形預積分的方式在離散時間對IMU數據進行預積分，將IMU數據與圖像幀中視覺觀測數據對齊。所以IMU測量誤差可以表示為IMU的實際測量值與IMU預積分的差值

(15)

1.4.2 視覺測量誤差

對于視覺觀測誤差，可以使用計算重投影誤差的方式表示。為了使計算方法適用于多種類型的視覺傳感器，這里采用一種在球面坐標系下進行誤差計算方法

(16)

2 試驗與分析

為了驗證本文提出的雙目視覺與慣導融合的視覺SLAM系統方案的可行性和有效性，本文使用EuRoc這一個公共數據集進行相關試驗分析。EuRoc數據集使用一種搭載雙目視覺傳感器和慣性測量單元的六翼飛行器采集數據，同時使用一種Vicon動作捕捉系統實時捕捉飛行器飛行時的位姿，并將其保存為六自由度的位姿數據作為數據集中飛行器飛行的真實軌跡。

EuRoc一共包含了11個圖像序列，5個機械廠房圖像數據(MH01-MH05),2個普通房間圖像序列(VR1和VR2)，每一個數據集官方都給了一個跟蹤難度，見表1。

表1 EuRoc圖像序列

2.1 基于半直接法的位姿估計方法試驗分析

基于半直接法的視覺里程計與基于特征法的ORB-SLAM2在EuRoc數據集[25]上進行測試，計算相對軌跡誤差與絕對軌跡誤差的均方根誤差，驗證方法的有效性。其中試驗數據集選擇MH01、MH03、MH05這3個代表不同跟蹤難度的數據集。

以MH01為例，本文提出的基于半直接法的視覺里程計與ORBSLAM2的MH01數據集上的軌跡對比如圖4所示。

根據SLAM系統估計的位姿信息與真實軌跡對比，計算相對軌跡誤差與絕對軌跡誤差如圖5、圖6所示。

圖4 軌跡對比Fig.4 Trajectory comparison

兩種SLAM系統在MH01、MH02、MH03這3個數據集中相對軌跡誤差與絕對軌跡誤差的對比結果見表2、表3。

表2 相對軌跡誤差

圖5 兩種算法在MH01上的APEFig.5 APE of two algorithms on MH01

圖6 兩種算法在MH01上的RPEFig.6 RPE of two algorithms on MH01

表3 絕對軌跡誤差

由表2和表3可以看出，半直接法視覺里程計的定位精度在絕對軌跡誤差和相對軌跡誤差方面都遠不如基于特征法的ORB-SLAM2，為了進一步提高定位精度，本文在半直接法視覺里程計的基礎上構建了視覺慣導融合的視覺SLAM系統，其定位精度將在2.2節進行詳細分析。

表4是計算不同的SLAM系統對數據集中每幅圖像幀的跟蹤時間，反映SLAM系統的運行效率。由表4中可以看出，基于直接法的DSO在3種跟蹤難度的數據集中處理圖像幀的時間都是最少的，因此其運行效率是最高的，而視覺慣導融合的視覺SLAM系統的運行效率比半直接法視覺里程計的運行效率略低，是因為視覺慣導融合的視覺SLAM系統中增加了對IMU數據的處理以及回環檢測模塊，占用了一些計算資源。雖然視覺慣導融合的視覺SLAM系統的運行效率不如基于直接法的DSO，但是相比于基于特征法的ORB-SLAM2系統，其運行效率提升了約50%，具有較好的實時性。

表4 圖像幀的平均跟蹤時間

2.2 視覺慣導融合的視覺移動機器人定位系統試驗分析

為了驗證本文構建的視覺慣導融合的視覺SLAM系統的可行性，使用EuRoc數據集中不同的試驗場景對其進行驗證，選取數據集中MH01、VR12、MH05這幾個代表不同場景不同跟蹤難度的圖像序列。本文構建的視覺慣導融合的視覺SLAM系統在數據集中運行結果如圖7所示。

圖7表示在該數據集中SLAM系統估計的相機位姿與數據集中真實軌跡的對比結果，其中彩色線條表示位姿的估計結果，不同顏色代表與真實軌跡不同的差值，灰色線條代表數據集中由動作捕捉系統捕捉到的真實軌跡數據。可以看出，視覺慣導融合的SLAM系統在機械廠房和普通房間這兩個場景中都可以有效地進行運動跟蹤并且估計相機位姿，而且可以看出，在跟蹤難度較低的數據集中，位姿的估計結果與真實軌跡的誤差值要小于跟蹤難度較高的數據集，這是由于難度較高的數據集中相機運動較快且場景光照較差導致的。

圖7 數據集運行結果Fig.7 Data set running results

此外，還計算了目前主流的幾個視覺慣導融合的視覺SLAM系統在EuRoc數據集中其他幾個圖像序列上的絕對軌跡誤差，見表5。

由表5可以看出，在上述圖像序列中，本文視覺慣導融合的視覺SLAM系統的絕對軌跡誤差均要優于OKVIS、ROVIO、VINS-Mono這3個主流的視覺SLAM系統方案。同時相比于本文采用的半直接法視覺里程計，在融合了IMU數據之后，定位精度得了約50%的提升。證明本文系統可以在提高SLAM系統運行效率的同時，仍然具有良好的定位精度。同時本文系統在EuRoc數據集中MachineHall與Vicon Room1、Vicon Room2這兩個不同場景的圖像序列中都可以良好運行，并保持良好的穩健性，證明本文系統可以適用在不同的應用場景中。

表5 絕對軌跡誤差

3 總 結

本文提出的雙目視覺與IMU融合的視覺SLAM系統使用基于半直接法的視覺里程計，提高了運動跟蹤效率。首先，利用光流法進行相機運動追蹤，再利用幀間特征點的匹配關系計算移動機器人初始位姿。其次，將滑動窗口中的三維路標點投影到當前圖像幀中，并構建重投影誤差函數估計相機位姿。然后，將視覺數據與IMU數據相互融合，利用IMU的瞬時高精度測量數據來彌補穩健性較低的視覺數據，同時也利用長時間視覺數據的相對穩健性來抑制IMU數據的累計漂移誤差。最后，在EuRoc數據集中對本文算法進行試驗驗證。試驗表明本文提出的雙目視覺與IMU融合的視覺SLAM系統在不同的場景下都可以保持較高的定位精度。