基于RGB-D圖像的機(jī)器視覺(jué)定位算法

林秋葉，胡志恒

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

0 引言

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

VSLAM主要分為前端視覺(jué)里程計(jì)(VO)、后端優(yōu)化、閉環(huán)檢測(cè)和地圖構(gòu)建四個(gè)主要部分[2]。其中視覺(jué)里程計(jì)根據(jù)相鄰圖像幀的運(yùn)動(dòng)的估計(jì)出相機(jī)的運(yùn)動(dòng)，并選取合適關(guān)鍵幀給后端優(yōu)化和建圖提供較好的初始值[3-4]。VO可以根據(jù)利用圖像信息的不同分為特征點(diǎn)法的前端和不提特征的直接法前端[4]。

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

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

1 算法結(jié)構(gòu)

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

圖1 本算法整體結(jié)構(gòu)

1.1 圖像特征提取與匹配

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

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

表1 特征點(diǎn)提取算法對(duì)比

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

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

通過(guò)圖像塊B的矩mpq可以求得B的質(zhì)心：

針對(duì)傳統(tǒng)方法提取ORB特征點(diǎn)過(guò)于集中，出現(xiàn)扎堆的現(xiàn)象[9]，基于四叉樹(shù)策略均勻化提取特征，使得關(guān)鍵點(diǎn)分布均勻，針對(duì)圖像紋理弱區(qū)域無(wú)法取到有效特征點(diǎn)的現(xiàn)象，提出一種自適應(yīng)閾值算法，根據(jù)圖像不同區(qū)域亮度分布，計(jì)算其局部閾值。

對(duì)前面構(gòu)建的金字塔每一層劃分大小為30×30像素的局部區(qū)域，基于初始閾值對(duì)每個(gè)局部區(qū)域提取FAST關(guān)鍵點(diǎn)，若在初始區(qū)域提取不到關(guān)鍵點(diǎn)，則使用自適應(yīng)閾值算法取值，若達(dá)到最小閾值還為取到FAST時(shí)標(biāo)記該區(qū)域特征點(diǎn)為0。利用四叉樹(shù)策略對(duì)所有提取到的關(guān)鍵點(diǎn)進(jìn)行劃分，均勻的選取N個(gè)FAST關(guān)鍵點(diǎn)。

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

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

特征匹配是VSLAM數(shù)據(jù)關(guān)聯(lián)部分，可以將當(dāng)前觀察到的路標(biāo)與前一時(shí)刻的路標(biāo)進(jìn)行匹配比較，估計(jì)相機(jī)的運(yùn)動(dòng)軌跡。然后由于場(chǎng)景中常常存在大量重復(fù)紋理，使得特征描述非常相似，誤匹配率情況廣泛存在，這已經(jīng)成為VSLAM中制約性能提升的一個(gè)大瓶頸[]。

1.2 圖像剔除誤匹配

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

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

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

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

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

1.3 位姿估計(jì)

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

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

圖2 3D-2D匹配點(diǎn)投影關(guān)系

2 實(shí)驗(yàn)及結(jié)果分析

在為了驗(yàn)證本系統(tǒng)的有效性，在如表2的實(shí)驗(yàn)環(huán)境下，采用德國(guó)慕尼黑工業(yè)大學(xué)的TUM數(shù)據(jù)集[20]，做不同特征點(diǎn)提取算法對(duì)比實(shí)驗(yàn)，不同誤匹配剔除算法對(duì)比實(shí)驗(yàn)和定位的絕對(duì)軌跡誤差分析實(shí)驗(yàn)。

表2 實(shí)驗(yàn)環(huán)境

2.1 特征點(diǎn)提取實(shí)驗(yàn)

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

圖3 ORB特征點(diǎn)提取

圖4 本算法特征點(diǎn)提取

2.2 特征點(diǎn)提取實(shí)驗(yàn)

將兩組相鄰連續(xù)2幅圖像進(jìn)行特征提取與匹配，其中第一組圖像特征點(diǎn)較為明顯，圖像紋理清晰，第二組圖像紋理較弱。將提取到的關(guān)鍵點(diǎn)計(jì)算其描述子漢明距離，進(jìn)行特征點(diǎn)的匹配，對(duì)誤匹配的特征點(diǎn)進(jìn)行剔除，本系統(tǒng)誤匹配剔除算法與傳統(tǒng)RANSAC算法的實(shí)驗(yàn)計(jì)算出的性能指標(biāo)如表3所示。結(jié)果顯示，本算法在圖像紋理較弱的地方也能取到有效特征點(diǎn)進(jìn)行匹配，經(jīng)過(guò)在誤匹配剔除階段的額耗時(shí)比傳統(tǒng)算法耗時(shí)少了近10倍，總匹配時(shí)間耗時(shí)少了1倍，整體提高了系統(tǒng)的實(shí)時(shí)性。

表3 誤匹配剔除實(shí)驗(yàn)結(jié)果

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

2.3 定位分析實(shí)驗(yàn)

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

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

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

表4 系統(tǒng)APE精度對(duì)比結(jié)果

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

圖7 絕對(duì)軌跡誤差趨圖

圖8 估計(jì)相機(jī)位姿與真實(shí)軌跡對(duì)比

3 結(jié)語(yǔ)

本文構(gòu)建的SLAM系統(tǒng)在運(yùn)行效率上較傳統(tǒng)算法提高了一倍，在特征紋理較弱場(chǎng)景下的魯棒性強(qiáng)于傳統(tǒng)視覺(jué)定位方法，且定位精度與傳統(tǒng)算法的定位精度幾乎相同。本文構(gòu)建的視覺(jué)定位方法構(gòu)建的地圖是稀疏地圖，無(wú)法利用于實(shí)際場(chǎng)景下機(jī)器人的導(dǎo)航，接下來(lái)我們會(huì)基于八叉樹(shù)構(gòu)建實(shí)時(shí)地圖，探索機(jī)器人的路徑規(guī)劃與避障，實(shí)現(xiàn)機(jī)器人的自主導(dǎo)航。