基于RGB-D圖像的機器視覺定位算法

林秋葉，胡志恒

(成都信息工程大學通信工程學院，成都 610225)

0 引言

近幾年，通信和電子等科技領域取得的創新和進步為機器人技術的進步做了極大的鋪墊，然而距離機器人完全普及化和實用化還有許多技術壁壘需要突破，其中一個難點就是機器人的定位問題[1]。視覺定位精度高、系統柔性強、能夠提供豐富的場景信息、攜帶方便且成本較低，符合移動機器人智能化的發展趨勢[2-6]。因此基于視覺的同時定位與構建地圖(Visual Simultaneous Localization and Mapping，VSLAM)[3]逐漸成為當前移動機器人領域研究的熱點。

VSLAM主要分為前端視覺里程計(VO)、后端優化、閉環檢測和地圖構建四個主要部分[2]。其中視覺里程計根據相鄰圖像幀的運動的估計出相機的運動，并選取合適關鍵幀給后端優化和建圖提供較好的初始值[3-4]。VO可以根據利用圖像信息的不同分為特征點法的前端和不提特征的直接法前端[4]。

其中，前端使用直接法實現的SLAM系統主要代表有德國慕尼黑工業大學J.Engel等人提出的LSD-SLAM[3]，其使用單目相機選擇針對像素運算的直接法構建半稠密地圖，能夠不使用GPU或者深度相機就能構建半稠密地圖，但是系統易受相機曝光和內參影響，但在相機運動變化快時常出現跟丟情況[3-7]。而基于特征點法的前端對光照和動態物體不敏感，運行較為穩定，因此也常被選為視覺里程計的主要解決方案。特征點法中，Hentry等人搭建了一套基于特征點的 RGB-D SLAM[4]，可以完成稠密點云的重建，但系統運算時間長，實時性不高。Endres在迭代最近點(Iterative Closest Point，ICP)的基礎上提出了 RGB-D SLAMv2系統，建立了一套完整的RGB-D SLAM系統[4]，但系統在運行時有卡頓現象，且生成的點云文件占用空間存儲量過大，機器人在導航時難以應用。西班牙Zaragoz大學的Raul Mur-Artal基于PTAM架構[12]下提出ORB-SLAM，相對于原始的PTAM方案提升了追蹤效果和地圖精度，增加了地圖初始化、回環檢測功能，且結合場景特點優化了地圖構建及關鍵幀選取方法，該方案圖像處理中使用具有良好不變性的ORB特征點和描述子進行特征追蹤和回環檢測，使用基于關鍵幀的后端非線性優化，所以命名為ORB-SLAM[15]，在2017年提出ORB-SLAM2[4]，其創新地使用了三線程，跟蹤和建圖效果較好，回環檢測也有效地防止和消除累積誤差，但特征紋理弱的地方容易跟蹤丟失，且重定位誤差較大[4-14]。

針對以上研究現狀，本文基于RGB-D相機搭建一套VSLAM系統，前端視覺傳感器使用RGB-D相機采集圖像信息，并采用改進的RGB算法進行特征提取與匹配，解決特征紋理弱特征點跟蹤丟失的問題，提高系統魯棒性；基于RANSAC改進的PROSAC算法統計置信度更高的點，剔除誤匹配，得到優化配對點，優化計算單應矩陣的迭代過程，提高運算效率，保證系統實時性；最后再利用PNP(Perspective N Point)與ICP算法結合求解相機位姿，后端優化使用光束平差法對前面求解的相機位姿即機器人的位姿進行圖優化，得到機器人全局位姿估計。

1 算法結構

本文的算法整體結構如圖1所示，主要分為圖像特征提取與匹配，相機位姿估計，全局優化幾個部分。其中數據輸入為深度相機在同一時間戳采集到的RGB彩色圖像幀和Deep深度圖像幀；將彩色圖像序列和深度圖像序列按時間戳對齊后，基于改進的四叉樹策略均勻化提取特征點的FAST關鍵點，計算這些關鍵點的BRIEF描述子，然后根據描述子的漢明距離進行粗匹配，再用PROSAC剔除誤匹配的特征點對，篩選得到質量較高的匹配點對；最后按照RANSAC迭代方式，結合PNP和ICP算法估計出機器人的位姿，基于BA算法求解得到機器人位姿軌跡的最優解。

圖1 本算法整體結構

1.1 圖像特征提取與匹配

常用的圖像特征提取方法有ORB(Oriented FAST and Rotated BRIEF)[9]、SIFT(Scale Invariant Feature Transform)[11]、SURF(Speeded Up Robust Features)[12]等。其中SIFT具有尺度、旋轉和光照等的不變性，但計算時間長；基于SIFT特征改進的SURF特征提高了特征提取的速度；同樣以速度快著稱的典型代表有FAST，但是FAST特征不具有方向信息[13]；ORB特征改進了FAST角點不具有方向性的問題[2]，采用描述特征點周圍圖像信息的二進制描述子BRIEF[13]，能夠有效替代SIFT和SURF的特征。

ORB保留了特征子具有旋轉不變性，尺度不變性的特點，雖然降低了提升了計算速度，是目前SLAM方案中性能與質量的較好折中[2]。SIFT、SURF、ORB在同一幅圖像中同時提取1000個特征點的情況下，ORB算法在運算速度上比SIFT算法快幾百倍，比SURF算法快幾十倍[7]，且魯棒性也能滿足定位需求，在實時性要求較高的情況下，本文選用ORB特征點法。

表1 特征點提取算法對比

本方法在前端VO通過關鍵點FAST和描述子BRIEF進行計算，進行圖像特征點提取與匹配。首先構建圖像金字塔，在金字塔的每一層檢測FAST，實現特征點的尺度不變性。基于金字塔的錐形比例可知金字塔層數i越高，圖像的面積si越小，所能提取到的特征點數量ni就越小，即第i層金字塔面積si和特征點數量ni成正比。我們假設第0層圖像的面積為s0，縮放因子為α(0<α<1)，特征總數為N，金字塔總面積為每層面積之和S，那么每個單位縮放系數所希望的特征點個數Ns為：

FAST不具備方向信息，因此選用灰度質心法和矩法確定FAST的方向，實現特征點的旋轉不變性。定義圖像塊B的灰度表達式I(x，y)，定義p，q={0，1}，那么圖像塊B的矩為：

通過圖像塊B的矩mpq可以求得B的質心：

針對傳統方法提取ORB特征點過于集中，出現扎堆的現象[9]，基于四叉樹策略均勻化提取特征，使得關鍵點分布均勻，針對圖像紋理弱區域無法取到有效特征點的現象，提出一種自適應閾值算法，根據圖像不同區域亮度分布，計算其局部閾值。

對前面構建的金字塔每一層劃分大小為30×30像素的局部區域，基于初始閾值對每個局部區域提取FAST關鍵點，若在初始區域提取不到關鍵點，則使用自適應閾值算法取值，若達到最小閾值還為取到FAST時標記該區域特征點為0。利用四叉樹策略對所有提取到的關鍵點進行劃分，均勻的選取N個FAST關鍵點。

過程1均勻提取特征點流程輸入:金字塔每一層圖像mi,特征點數目N,初始閾值h0輸出:特征點圖像坐標Step1.初始節點數目ni=1,初始化金字塔圖像m0,初始化閾值h0;Step2.計算當前節點的特征點數量;Step3.若該節點特征點數量小于1,停止該節點的劃分,否則在當前節點繼續按照四叉樹進行劃分;Step4.比較當前節點數目ni與特征點數目N,若ni>N,則停止所有劃分;Step5.遍歷所有節點,若該節點特征數大于1,則選取響應值最高的特征點作為該節點的代表;Step6.若該節點特征點數小于等于1,則調用自適應閾值算法降低閾值,繼續檢測,直到檢測到特征點,選出響應值最高的特征點,若直到達到最小閾值還未檢測到特征點則淘汰此節點;Step7.最后得到每一層金字塔選出N個局部區域,每個局部區域有一個FAST關鍵點,實現N個均勻分配的特征點。

在提取FAST關鍵點后，我們對每個FAST計算其描述子，本文采用改進的BRIEF。采用高斯平滑法進行灰度值大小的比較，令BREIEF更加穩定。描述子距離表示兩個特征間的相似程度[2]，因此采用漢明距離來表示特征點的相似程度進行特征點的匹配。

特征匹配是VSLAM數據關聯部分，可以將當前觀察到的路標與前一時刻的路標進行匹配比較，估計相機的運動軌跡。然后由于場景中常常存在大量重復紋理，使得特征描述非常相似，誤匹配率情況廣泛存在，這已經成為VSLAM中制約性能提升的一個大瓶頸[]。

1.2 圖像剔除誤匹配

特征描述子完成描述子匹配之后，會得到一些關鍵點對。最小二乘方法在描述子匹配輸出的點對質量很好，理想情況下是圖像沒有噪聲污染與像素遷移與光線恒定，但是實際情況下圖像特別容易受到光線、噪聲導致像素遷移，從而產生額外的多余描述子匹配，這些點對可以分為outlier跟inlier兩類。RANSAC(Random Sample Consensus)[14]可以很好的過濾掉outlier點對，它會從給定的數據中隨機選取一部分進行模型參數計算，然后使用全部點對進行計算結果評價，不斷迭代，直到選取的數據計算出來的錯誤是最小。RANSAC算法可以剔除誤匹配點對，但是算法是一種全隨機的數據選取方式，導致迭代次數不穩定，效率較低，魯棒性較差，算法效率相對較低。

因此本文采用基于RANSAC改進的算法PROSAC(Progressive Sampling Consensus)[16]，該方法采用半隨機方法，對所有特征點的匹配質量好壞進行排序，定義一個質量因子q來衡量匹配點對質量的好壞，然后根據q值降序排列，取質量較高的點對求取單應矩陣。

在圖像特征點描述子匹配過程中用漢明距離表示特征點的相似程度。用最小距離dmin1和次小距離dmin2的比值β表示特征點匹配質量。

PROSAC 算法的具體流程如下所示：

過程2誤匹配剔除算法流程輸入:最大迭代次數Mm,內點的誤差門限δ,內點數目閾值Y輸出:單應性矩陣HStep1.計算特征點最小歐氏距離dmin1和dmin2,歐氏距離比值β,質量因子q;Step3.取質量排名最好的m個點組合,計算每組的質量之和,并按組合的質量之和排列;Step4.選排序中最高的4組匹配點,作為初始的內點集合,通過該內點集合計算出變換矩陣T和單應性矩陣H;Step5.依次判斷剩余的匹配點對按照H計算對應的投影點;Step6.計算投影點與匹配點的誤差e與誤差門限δ比較,若e<δ則將當前點加入到內點集合中;Step7.統計內點數目y并與內點數目閾值Y比較,若y>Y,則內點數目更新為y,反之迭代次數加1,回到Sep4;Step8.利用更新后的y個內點重新計算單應性矩陣H以及新的內點;Step9.若M

1.3 位姿估計

本文根據小孔相機模型描述成像原理[17]，采用PNP(Perspective N Point)[18]和BA[19]算法結合求解相機位姿。小孔相機模型是相機將三維空間數據信息反映到二維圖像平面信息的最簡單、最常用的模型，小孔相機模型的定義如下：

P3P[18]算法是PnP三對3D-2D匹配點估計位姿中的一種，對數據要求較少。如圖2所示，相機光心為O，3D點A，B，C為世界坐標系中的坐標，2D點為a，b，c為相機坐標系上的坐標，即A，B，C在相機成像平面上的投影。根據相似三角形定理可以求得關于x，y的二元二次方程，其中已知的變量有，由3D點的世界坐標A，B，C求出的u，v，2D點的圖像位置a，b，c和三個余弦角cos，cos，cos。x，y是未知變量，隨著相機移動發生變化。

圖2 3D-2D匹配點投影關系

2 實驗及結果分析

在為了驗證本系統的有效性，在如表2的實驗環境下，采用德國慕尼黑工業大學的TUM數據集[20]，做不同特征點提取算法對比實驗，不同誤匹配剔除算法對比實驗和定位的絕對軌跡誤差分析實驗。

表2 實驗環境

2.1 特征點提取實驗

本文分別在傳統ORB特征點提取算法上和本系統特征提取算法上，選用圖像特征較為明顯的圖像Img1和圖像特征較為模糊的圖像Img2進行特征點的提取。實驗中的設置金字塔層數為8，縮放因子為1.2，得到如圖2所示實驗結果，傳統ORB算法在特征較為明顯的Img1取的特征點數量較多，但是提取的特征點較為集中，不能有效覆蓋圖像的各個部分信息；在特征較為模糊的Img2不能提取到的特征點非常少。而本文算法能在圖像1和圖像2中都能提取到有效特征點，且特征點數量滿足要求，分布均勻。可見，在耗時幾乎相等的情況下，本算法能夠在圖像特征點文理較弱的地方也能提取到有效特征點，減少傳統ORB算法容易在圖像較弱區域特征點容易跟蹤丟失的風險，提高了整個系統的魯棒性。

圖3 ORB特征點提取

圖4 本算法特征點提取

2.2 特征點提取實驗

將兩組相鄰連續2幅圖像進行特征提取與匹配，其中第一組圖像特征點較為明顯，圖像紋理清晰，第二組圖像紋理較弱。將提取到的關鍵點計算其描述子漢明距離，進行特征點的匹配，對誤匹配的特征點進行剔除，本系統誤匹配剔除算法與傳統RANSAC算法的實驗計算出的性能指標如表3所示。結果顯示，本算法在圖像紋理較弱的地方也能取到有效特征點進行匹配，經過在誤匹配剔除階段的額耗時比傳統算法耗時少了近10倍，總匹配時間耗時少了1倍，整體提高了系統的實時性。

表3 誤匹配剔除實驗結果

圖5 RANSAC算法誤匹配剔除效果

2.3 定位分析實驗

相機的真實姿態和估計姿態之間的直接差值可以用絕對軌跡誤差(Absolute Trajectory Error，ATE)表示，可以非常直觀地反映算法精度和軌跡全局一致性。當算法估計位姿表示為p1，…，pn∈SE(3)，真實位姿表示為Q1，…，Qn∈SE(3)，通過最小二乘法計算從估計位姿到真實位姿的轉換矩陣S∈SE(3)，則第i幀的ATE為：

使用均方根誤差(Root Mean Square Error，RMSE)統計總體值：

本算法在兩組序列的估計軌跡與真實軌跡的絕對位姿誤差歲時間的變化趨勢曲線如圖6所示，本系統算法與傳統算法針對數據集的兩個不同序列在本系統上運行的得到的估計軌跡(藍色實線表示)與真實軌跡(灰色虛線表示)的對比圖，紅色到藍色色帶表示誤差大小程度，如圖7所示。圖7為兩組序列的估計軌跡與真實軌跡絕對位姿APE隨時間的變化趨勢曲線，表4為誤差量化指標數據，包括最大值max、最小值min、均值mean、中位數median和標準差std。結果顯示，本文算法精度，在光照特征不明顯的圖像序列中，也能跟蹤到特征點，且定位的絕對誤差較低。

表4 系統APE精度對比結果

圖6 本算法誤匹配剔除效果

圖7 絕對軌跡誤差趨圖

圖8 估計相機位姿與真實軌跡對比

3 結語

本文構建的SLAM系統在運行效率上較傳統算法提高了一倍，在特征紋理較弱場景下的魯棒性強于傳統視覺定位方法，且定位精度與傳統算法的定位精度幾乎相同。本文構建的視覺定位方法構建的地圖是稀疏地圖，無法利用于實際場景下機器人的導航，接下來我們會基于八叉樹構建實時地圖，探索機器人的路徑規劃與避障，實現機器人的自主導航。