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

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

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

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

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

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

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

1 方法與原理

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

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

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

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

1.1.1 雙目視覺初始化

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

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

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

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

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

(1)

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

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

(2)

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

1.1.2 直接法估計初始位姿

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

(3)

光度誤差δI可以表示為

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

(4)

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

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

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

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

(5)

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

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

(6)

1.1.3 特征法優(yōu)化位姿

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

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

(7)

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

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

(8)

1.2 IMU數(shù)據(jù)預處理

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

(9)

得到IMU的運動模型為

(10)

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

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

(11)

式中

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

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

(12)

式中

1.3 視覺慣導聯(lián)合初始化

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

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

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

1.4 基于滑動窗口的后端優(yōu)化方法

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

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

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

(13)

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

對于優(yōu)化變量的計算可以使用非線性優(yōu)化的方法，通過構建滑動窗口中的視覺觀測值誤差，IMU測量誤差以及滑動窗口中的邊緣化誤差進行優(yōu)化變量的計算?；瑒哟翱趦日`差函數(shù)為

(14)

1.4.1IMU測量誤差

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

(15)

1.4.2 視覺測量誤差

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

(16)

2 試驗與分析

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

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

表1 EuRoc圖像序列

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

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

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

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

圖4 軌跡對比Fig.4 Trajectory comparison

兩種SLAM系統(tǒng)在MH01、MH02、MH03這3個數(shù)據(jù)集中相對軌跡誤差與絕對軌跡誤差的對比結果見表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系統(tǒng)，其定位精度將在2.2節(jié)進行詳細分析。

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

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

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

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

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

圖7 數(shù)據(jù)集運行結果Fig.7 Data set running results

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

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

表5 絕對軌跡誤差

3 總 結

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