基于Dog-Leg算法的服務(wù)機器人視覺SLAM系統(tǒng)

王素芳，陳 鵬，張維存，楊 林

(1.中國航天科工集團第二研究院 七〇六所，北京 100854；2.華北理工大學 機械工程學院，河北 唐山 063210；3.北京科技大學 自動化學院，北京 100083)

0 引 言

為解決服務(wù)型機器人在室內(nèi)環(huán)境準確定位[1]，快速運動時圖像易模糊的缺點[2,3]，本文采用了需要依靠IMU(inertial measurement unit)、慣性測量單元與單目視覺相融合的定位方法，即VIO視覺慣性里程計[4](visual-inertial odometry)。本文融合框架采用了緊耦合這種相對復(fù)雜但主流的研究方法。

英國Davision率先提出了使用通用運動模型進行平滑的相機運動以及單目視覺特征點預(yù)估計的MonoSLAM系統(tǒng)，極大提高了機器人在實時3D定位與地圖繪制中的應(yīng)用價值[5]。Mur-Artal等[6]提出了ORB-SLAM2系統(tǒng)，相比于其提出的ORB-SLAM系統(tǒng)，新系統(tǒng)優(yōu)化了后端的IMU與單目視覺系統(tǒng)，使其可以進行更精確的軌跡規(guī)劃[7]。

在視覺SLAM中，以位姿和路標來描述SLAM過程，圖像中所謂的特征點就是路標，圖像特征包括SIFT[8]、SURF[9]、ORB等。ORB改進了FAST沒有方向性的缺陷，并通過二進制的BRIEF描述子，大大加快了圖像特征提取的速度，達到了SLAM實時性的要求。因此本文采用了ORB特征點和特征提取與匹配方法。本文主要對當前視覺SLAM的后端優(yōu)化部分進行了算法創(chuàng)新，在原有非線性優(yōu)化算法基礎(chǔ)上應(yīng)用了改進的Dog-Leg算法，又設(shè)計了一種基于阻尼因子更新來計算信賴區(qū)域半徑的方法，最后應(yīng)用于YOBY機器人并在實際場景中進行了算法測試。

1 視覺SLAM系統(tǒng)

本文的視覺SLAM系統(tǒng)主要由傳感器數(shù)據(jù)處理采集、前端視覺里程計優(yōu)化、后端優(yōu)化、回環(huán)檢測、建圖五大功能模塊組成，如圖1所示。

圖1 SLAM整體流程

本文具體的系統(tǒng)流程如圖2所示，第一模塊中常用的傳感器有相機(單目相機、雙目相機、TOF深度相機)IMU(慣性測量單元)，激光雷達等。第二模塊中的視覺里程計是根據(jù)幾個相鄰圖像信息的相似性，通過相應(yīng)的算法處理，粗略地估計出相機的運動，為后端提供初始值。在第三模塊后端優(yōu)化中，由于前端視覺里程計只能通過相鄰圖像估計出局部相機位姿，由此構(gòu)建的增量式地圖存在累計誤差，這就需要后端優(yōu)化來構(gòu)建一個全局一致性的地圖來提高地圖和位姿估計的準確性。在優(yōu)化方法中，擴展卡爾曼濾波算法是通過上一時刻的狀態(tài)來估計當前時刻的狀態(tài)，而非線性優(yōu)化算法是把所有時刻的數(shù)據(jù)放在一起進行優(yōu)化，因此非線性優(yōu)化算法極大地提高了估計的精度。第四模塊回環(huán)檢測的作用就是判斷機器人是否到過相同的位置，從而形成閉環(huán)，增加約束條件，減少后端優(yōu)化的計算量和累計誤差，糾正機器人位姿。最后是地圖構(gòu)建。SLAM系統(tǒng)可以根據(jù)應(yīng)用場景與實際需求的不同，構(gòu)建出2D柵格地圖、2D拓撲地圖、3D點云地圖和3D網(wǎng)格地圖等地圖。

本文具體的系統(tǒng)流程如圖2所示：

圖2 本文系統(tǒng)流程

(1)數(shù)據(jù)預(yù)處理：相機為30 Hz的單目相機，采用ORB方法進行特征提取和匹配，利用二進制描述子進行特征標記；將IMU數(shù)據(jù)進行積分，得到當前時刻的位置、速度和旋轉(zhuǎn)(PVQ)，同時計算預(yù)積分增量、雅克比矩陣和協(xié)方差項；

(2)初始化：首先利用運動恢復(fù)結(jié)構(gòu)(SFM)估計滑窗內(nèi)所有幀的位姿和路標點的位置，然后與IMU數(shù)據(jù)融合恢復(fù)真實的尺度信息，最后求解初始化參數(shù)(如重力、速度、陀螺儀偏置)；

(3)后端非線性優(yōu)化：將視覺約束、IMU約束和邊緣化的先驗信息約束放在一個大的目標函數(shù)中進行非線性優(yōu)化，求解滑窗內(nèi)所有幀的PVQ和bias；

(4)回環(huán)檢測：利用DBoW2每3個關(guān)鍵幀檢測一幀，相當于是跳兩幀做一次回環(huán)檢測，當檢測成功后進行重定位；

(5)全局位姿圖優(yōu)化：滑窗內(nèi)的優(yōu)化僅得到了滑窗內(nèi)相機的位姿，且回環(huán)檢測利用回環(huán)幀也只能糾正滑窗內(nèi)的相機位姿，也就是只進行了局部位姿的優(yōu)化，并沒有對整個相機的位姿進行修正。鑒于地圖軌跡全局的一致性考慮，相機位姿圖優(yōu)化除了局部優(yōu)化以外再做一次全局的優(yōu)化。

1.1 SLAM前端

SLAM前端也被叫作視覺里程計(VO)，VO的實現(xiàn)方法，按是否需要提取特征，分為特征點法和直接法兩種。對于光流法來講，前提假設(shè)時間是連續(xù)的，并且運動是微小的以及灰度不變等條件苛刻，當相機發(fā)生大尺度運動或者旋轉(zhuǎn)時無法較好的追蹤；本文采用的特征點法被認為是視覺里程計的主流方法，在運動過大時，只要匹配點還在圖像內(nèi)，就不會引起幀丟失的情況，相對于光流法有更好的魯棒性。

FAST[11]關(guān)鍵點以檢測圖像局部像素的灰度值為主，假如圖像中的某一像素灰度值和它鄰近像素之間灰度值差異明顯，說明這個像素點很可能就是角點。FAST關(guān)鍵點如圖3所示，突出顯示的正方形是在角點檢測中使用的像素，p處的像素是候選角的中心，圓弧由虛線表示，穿過12個比像素p更亮或更暗且連續(xù)的像素，稱之為FAST-12。首先檢測圖像中的FAST點，選取像素p，且p處的亮度為Ip。快速取一個參數(shù)，即中心像素p和圍繞中心的圓環(huán)中的像素之間的閾值T。如果圓中存在一組N個連續(xù)的像素，它們的亮度都大于Ip+T，或者都小于Ip-T，選擇N為12是因為它經(jīng)過了大量實驗驗證，可以用來排除大量非角點。為了更加高效，可以添加一項預(yù)測試，測試僅檢查1、5、9和13處的4個像素的亮度。如果p是一個角點，那么其中至少3個亮度大于Ip+T或小于Ip-T。如果都不成立，則p不能成為一個角點。然后，可以通過檢查圓中的所有像素，將完整的分段測試標準應(yīng)用于剩余的候選項。預(yù)測試快速排除了絕大多數(shù)非角點，角點檢測的速度得以提高。

圖3 FAST關(guān)鍵點

FAST及其變體是在與視覺功能(例如，并行跟蹤和映射)相匹配的實時系統(tǒng)中查找關(guān)鍵點的方法。它必須有效并且可以檢測到合理的拐角關(guān)鍵點，因此必須通過金字塔處理方法來匹配不同縮放倍率下的圖像，從而實現(xiàn)尺度不變性。對于旋轉(zhuǎn)，本文采用了一個簡單但有效的角點定向度量方法，即灰度質(zhì)心?；叶荣|(zhì)心假定角點的灰度偏離中心，這個向量便可以被用來表示特征點的方向。具體步驟如下：

定義圖像塊的距為

(1)

式中：I(x,y)為點(x,y)處的灰度值，通過這些距便能求出圖像塊的質(zhì)心

(2)

計算夾角求出具體的方向信息便可以得到FAST角點的旋轉(zhuǎn)描述

θ=arctan(m01/m10)

(3)

通過對FAST角點添加尺度和旋轉(zhuǎn)的描述，大大提升了ORB方法在不同圖像之間的魯棒性，因而又稱改進的FAST為OrientedFAST。

1.2 后端優(yōu)化

SLAM前端視覺慣性里程計構(gòu)建的地圖為增量式地圖數(shù)據(jù)會受到各種未知噪聲干擾，此時的運動方程和觀測方程不是精確成立的，而同時定位與地圖構(gòu)建旨在構(gòu)建一個精確、可靠、生命周期長的地圖，因此，只有視覺前端是遠遠不夠的。這就需要后端，將帶有噪聲的數(shù)據(jù)通過優(yōu)化方法進行準確的狀態(tài)估計，從而降低誤差帶來的影響。后端優(yōu)化主要有基于擴展卡爾曼濾波(EKF)的濾波方法和基于圖優(yōu)化的非線性優(yōu)化方法[12]。

根據(jù)文獻[13]，考慮使用立體相機和單目相機的SLAM以及各種不同的場景和運動模式，非線性優(yōu)化優(yōu)于濾波，因為它提供了每單位時間內(nèi)計算的最高精度。非線性優(yōu)化算法在精度和魯棒性方面表現(xiàn)出比KF算法極高的優(yōu)越性[14]。卡爾曼濾波方法在此刻的狀態(tài)和觀測只與上一時刻有關(guān)，而忽略了之前可能有關(guān)的數(shù)據(jù)的影響，相當于優(yōu)化只迭代了一次，也就是只考慮上一幀的影響。而非線性優(yōu)化算法[15]使用的是之前全部的數(shù)據(jù)，考慮的是所有幀對系統(tǒng)的影響來求解狀態(tài)估計問題[16]，因此本文后端采用非線性優(yōu)化算法。

構(gòu)建函數(shù)h，假設(shè)三維點云圖是由多個觀測點組成，設(shè)觀測點一共有N個，用y1,…,yN來表示。其中傳感器在xk位置上看到某個觀測點yj時，產(chǎn)生了一個觀測數(shù)據(jù)zk,j，對于觀測方程來說，第k次觀測

zk,j=h(yj,xk)+vk,j

(4)

觀測數(shù)據(jù)的條件概率為

P(zk,i|xk,yi)=N(h(yj,xk),Qk,j)

(5)

因為噪聲影響服從高斯分布v(k)～N(h(yj,xk),Qk,j)，因此條件概率仍然為高斯分布

(6)

對上式取負對數(shù)可得

(7)

將式(7)中與x無關(guān)的項去除并帶入SLAM的觀測方程中，求得狀態(tài)的最大似然估計

(8)

(9)

得到了一個總體意義下的最小二乘問題

(10)

這個可以最小二乘問題表示為

(11)

假設(shè)對f(x)的第i個分量fi(x)在點xk處Taylor展開：fi(xk+h)≈fi(xk)+▽fi(xk)Th，i=1,2,…,n；可以得到f(xk+h)≈f(xk)+J(xk)h，其中Jacobin矩陣的計算公式為

(12)

1.3 回環(huán)檢測

由于隨著關(guān)鍵幀數(shù)量的不斷增加，地圖的規(guī)模也在不斷地擴大，這時，最小二乘問題中的殘差數(shù)據(jù)就會越來越多，信息矩陣也會越來越大，計算量將會隨之加大。因此，為了保持計算的實時性，本文采用滑動窗口方法[17,18]來限制后端優(yōu)化的計算時間，即保持優(yōu)化變量的個數(shù)在一定的范圍內(nèi)，將其放在一個滑動的時間窗口中，動態(tài)地增加或丟棄優(yōu)化變量。也就是說隨著新的關(guān)鍵幀的加入，舊的關(guān)鍵幀將被丟棄，新的變量加入到最小二乘系統(tǒng)中進行優(yōu)化。為了既丟棄掉舊的變量，又能夠保留下被丟棄變量攜帶的信息，并將信息傳遞給剩余的變量，這里便采用了邊際概率的做法來移除舊的變量。

在滑動窗口中，有相機位姿、IMU測量值和視覺測量值，它是滑動窗口內(nèi)狀態(tài)向量、IMU和視覺的緊耦合。將視覺約束、IMU約束和先驗約束放在一個大的目標函數(shù)中進行非線性優(yōu)化，求解滑窗內(nèi)所有幀的PVQ和bias。目標函數(shù)為

(13)

式(13)中，第一部分為基于舒爾補得到的邊緣化的先驗信息，第二部分為IMU測量殘差，第三部分為視覺重投影誤差。

滑動窗口和邊緣化方案雖然減少了計算量和計算復(fù)雜度，但同時也使系統(tǒng)漂移累積起來[19]，為了消除漂移，將重定位加入到緊耦合中。重定位從回環(huán)檢測[20,21]模塊開始，用BRIEF二進制描述子通過DBoW2[22,23]詞袋模型進行循環(huán)檢測，當識別到已經(jīng)訪問過的位置時，將回環(huán)幀添加到滑窗中進行優(yōu)化，優(yōu)化出的回環(huán)幀位置如果與之前窗口中的對應(yīng)幀優(yōu)化出的位姿值大于20的時候，則認為回環(huán)是錯誤的，剔除此回環(huán)幀。如果滿足判斷條件，則加入到滑窗中一起進行位姿優(yōu)化

(14)

式(14)中的第四部分則是在回環(huán)檢測中的預(yù)測值和測量值之間的誤差。將回環(huán)幀的位姿視為常量，加入到滑窗進行緊耦合優(yōu)化時，狀態(tài)量的維度保持不變。由于IMU的加入，roll(滾轉(zhuǎn)角)和pitch(俯仰角)變得可觀，尺度信息在初始化的時候也已得到，因此最后使用回環(huán)幀和對應(yīng)幀的關(guān)系進行四自由度全局位姿圖優(yōu)化，全局優(yōu)化后得到的位姿準確度更高以及平滑性更好。

2 增強實時性的Dog-Leg算法

非線性優(yōu)化問題的框架大致分為線搜索和信賴區(qū)域兩大類。高斯牛頓法屬于線搜索，即先確定搜索方向，再確定步長。而列文伯格-馬夸爾特法屬于信賴區(qū)域方法，即先確定區(qū)域，然后在區(qū)域內(nèi)尋找最優(yōu)，它認為近似只在區(qū)域內(nèi)可靠。列文伯格-馬夸爾特法是最速下降法和高斯牛頓法的混合，根據(jù)阻尼因子的不同在兩者之間切換，Dog-Leg[24]方法同樣是最速下降法和高斯牛頓法的混合，不同之處在于Dog-Leg方法把調(diào)整阻尼因子變?yōu)榱烁淖冃刨噮^(qū)域。

Dog-Leg方法[25]流程如圖4所示。

圖4 Dog-Leg方法流程

本文對Dog-Leg方法已有的更新方法進行改進，得到新的更新策略如圖5所示。

圖5 本文阻尼因子更新策略

通過MATLAB軟件畫出阻尼因子隨著迭代變化的曲線圖，從圖6可以看出，原策略迭代次數(shù)多于本文所設(shè)計更新策略的迭代次數(shù)，當?shù)恋?次時本文所設(shè)計的更新策略的阻尼因子值達到最大值，但遠遠小于原策略的最大值，在迭代至第9次時原策略阻尼因子值達到最大值，此時本文所用更新策略的阻尼因子已下降到很小的值。因此，本文所用更新策略要優(yōu)于原更新策略。

圖6 更新策略迭代次數(shù)對比

3 實驗驗證

3.1 基于標準數(shù)據(jù)集的測試與分析

在EuRoc MAV-MH_05公共數(shù)據(jù)集上運行和評測本文所設(shè)計的方法。EuRoc數(shù)據(jù)集是在微型飛行器上收集的視覺慣性數(shù)據(jù)集，該數(shù)據(jù)集包含雙目圖像、同步的IMU測量值以及三維位置的真值，著重于視覺慣性SLAM的評估測試。ORB-SLAM2與VINS-Mono都是開源的實時SLAM框架，本文SLAM系統(tǒng)與這兩種SLAM框架的對比結(jié)果如圖7與表1所示。

圖7 本文算法誤差

表1 相對位姿誤差

表1中RPE表示相對位姿誤差，mean表示平均值，median表示中值，rmse表示均方根誤差，sse表示誤差平方和，std表示標準差。圖7(a)表示本文算法的軌跡誤差，圖7(b)為本文算法的相對位姿誤差圖。由表1可以看出，本文方法的RPE在0.0527～0.000 02之間波動。rmse對數(shù)據(jù)中的特大或者特小值敏感，可以很好地反映出軌跡的精度，sse是擬合數(shù)據(jù)和真值對應(yīng)點的誤差的平方和，rmse和sse越小,說明測量的數(shù)據(jù)精度越高，穩(wěn)定性越好。在表1中，rmse大小關(guān)系為：rmse(ORB-SLAM2)>rmse(VINS-Mono)>rmse(本文的方法)，sse的大小關(guān)系為：sse(ORB-SLAM2)>sse(VINS-Mono)>sse(本文的方法)。驗證本文算法具有較高的精度和較好的穩(wěn)定性。

圖8和圖9分別展現(xiàn)了ORB-SLAM2、VINS-Mono和本文優(yōu)化后SLAM算法的平移和旋轉(zhuǎn)的細節(jié)描述。本文優(yōu)化后的SLAM算法與ORB-SLAM2和VINS-Mono相比較，在平移和旋轉(zhuǎn)上與真值之間的誤差較小，擬合效果較好。

圖8 全局軌跡對比

圖9 六自由度軌跡對比

3.2 基于YOBY機器人實際場景的測試與分析

在本文的實驗環(huán)境中，軟件平臺使用的系統(tǒng)為ROS機器人操作系統(tǒng)。硬件平臺采用游爾機器人公司的底盤、國產(chǎn)Firefly-RK3399開發(fā)板和小覓相機等硬件搭建測試環(huán)境。

如圖10(a)為機器人的三維結(jié)構(gòu)圖，圖10(d)機器人的底盤由一個萬向輪和兩個驅(qū)動輪組成。底盤中央處理器為八核Cortex-A53(64 bit)架構(gòu)CPU1.3GHz。頭部配備圖10(b)國產(chǎn)Firefly-RK3399控制板，作為Firefly新一代頂級平臺，采用六核64位處理器，具備高性能數(shù)據(jù)傳輸和顯示接口，支持2.4 Hz/5 Hz雙頻WiFi,Bluetooth和千兆以太網(wǎng)。安裝了圖10(e)小覓相機與LM2596S降壓穩(wěn)壓電源模塊，軟件系統(tǒng)為Ubuntu16.04系統(tǒng)。

圖10 YOBY機器人搭載的硬件

針對實驗室這類特征點相對較多，空間狹小，光照較為均勻的環(huán)境，本文所設(shè)計的方法在運行過程中特征點提取和匹配效果良好，回環(huán)檢測后，運行的軌跡與優(yōu)化得到的軌跡誤差相對較小，如圖11所示。驗證本文方法魯棒性優(yōu)秀。

圖11 實驗室小場景效果

圖12為在中國科學院軟件研究所博物館應(yīng)用本文所設(shè)計的方法YOBY機器人得到的實際場景軌跡。博物館這類環(huán)境特點為空間大，特征點較少；四周為玻璃墻壁，光照強度受外界影響很大?？梢钥闯?，只要初始化成功以后，無論如何原地打轉(zhuǎn)還是運動幾圈，軌跡都不會飄，驗證本文方法在穩(wěn)定性方面有了一定的提高。

圖12 博物館大場景

4 總 結(jié)

(1)采用最速下降法和高斯牛頓法的混合法Dog-Leg方法來求解非線性最小二乘問題，并通過阻尼因子更新策略來計算信賴區(qū)域半徑。減少了迭代次數(shù)，縮短了計算時間，提高了視覺SLAM系統(tǒng)的實時性。

(2)依托于低成本硬件嵌入式機器人,以Dog-Leg算法為核心，在YOBO系統(tǒng)平臺上融合單目視覺相機與慣性測量傳感器并進行標定從而提高地圖精度。最后通過實際場景實驗和在公共數(shù)據(jù)集上通過EVO測評工具得出測評結(jié)果，驗證算法的有效性和魯棒性。

5 結(jié)束語

(1)光照變化和地面鏡面反射光源仍然是困擾視覺SLAM系統(tǒng)研究者的重大問題，即使本文采用對光照變化不敏感的特征點法，以及加入回環(huán)檢測和閉環(huán)校正，也只使精度和魯棒性得以提升而非完善。因此在單個地圖中融合點、線和面的多特征融合方案也變?yōu)檠芯繜狳c。

(2)目前服務(wù)型機器人上應(yīng)用的皆為二維環(huán)境下的SLAM算法，實際生活中三維環(huán)境下的SLAM算法應(yīng)用場景更加有價值。

(3)在動態(tài)場景中，相機有可能受到遮擋、晃動或者不規(guī)則的運動，此時會遇到丟幀，特征點丟失，以及跟蹤失敗的情況，需要對系統(tǒng)進一步的研究和完善。