周夢妮， 張國偉,， 盧秋紅， 吉志鵬， 宋孟良， 紀(jì)棟梁

（1.上海電力大學(xué)自動化工程學(xué)院，上海200082；2.上海合時智能科技有限公司，上海201100）

0 引 言

本文研究了基于RGB-D傳感器的同步定位與建圖研究，使用ORB算法得到具有旋轉(zhuǎn)不變性的特征描述子且進(jìn)行前后幀特征點(diǎn)的匹配，為了提高算法速度和精度，在外點(diǎn)剔除上采用了基于K最近領(lǐng)域算法和使用交叉匹配過濾算法，用綁定深度信息的RANSAC算法得到可靠性高的內(nèi)點(diǎn)集后實(shí)現(xiàn)圖像配準(zhǔn)及運(yùn)動估計(jì)；消除前段噪聲誤差的最有效方法是進(jìn)行閉環(huán)檢測，當(dāng)前最有效的閉環(huán)檢測方法是基于詞袋模型的閉環(huán)檢測法；最后進(jìn)行圖優(yōu)化算法，使閉環(huán)上每相鄰節(jié)點(diǎn)與邊的剛體模型滿足最小誤差函數(shù)。

1 圖像感知模塊

基于RGB-D傳感器需要同時獲得周圍環(huán)境圖像及深度信息，由此估計(jì)機(jī)器人的運(yùn)動。RGB-D相機(jī)搭載光學(xué)RGB相機(jī)與紅外相機(jī)，可以在獲得RGB圖的同時獲得圖像的深度數(shù)據(jù)，它通常利用TOF技術(shù)（Time of flight，飛行時間）[1]。可采用微軟推出的Kinect 2代相機(jī)，它由一個紅外接收器與一個紅外發(fā)射器構(gòu)成，能夠較好地抑制外界光譜對傳感器的干擾，同時可以得到豐富的場景數(shù)據(jù)（三維點(diǎn)云信息），利用這些信息可對空間環(huán)境進(jìn)行三維重建[2]。

2 相機(jī)標(biāo)定

相機(jī)標(biāo)定主要是為了解決相機(jī)成像的幾何模型問題與矯正透鏡畸變問題。為了建立起物體坐標(biāo)與圖像坐標(biāo)之間的關(guān)系，確定空間物體表面某點(diǎn)的三維幾何位置與其在圖像對應(yīng)點(diǎn)之間的關(guān)系（具體對應(yīng)關(guān)系如圖1所示）需要在構(gòu)建成像模型后得到相機(jī)的自身屬性參數(shù)，得到內(nèi)外參數(shù)信息的步驟稱為相機(jī)標(biāo)定，最常見的相機(jī)標(biāo)定方法為傳統(tǒng)方法中的張正友相機(jī)標(biāo)定法。

由圖1中三個坐標(biāo)系的對應(yīng)關(guān)系得

相機(jī)內(nèi)參數(shù)矩陣K為

張正友相機(jī)標(biāo)定法的核心思想是利用棋盤圖樣推算圖像平面與棋盤圖樣平面之間的單應(yīng)性矩陣[3]。黑白棋盤的角點(diǎn)坐標(biāo)是已知的。根據(jù)不同角度的棋盤圖片角點(diǎn)坐標(biāo)可以求出相機(jī)內(nèi)參數(shù)。

為了取得更豐富的圖像信息，通常需要在RGB-D相機(jī)鏡頭前安裝透鏡，透鏡在光線影響下會產(chǎn)生圖像畸變，透鏡往往是鏡頭中心對稱的，越靠近鏡頭邊緣，畸變現(xiàn)象越明顯，這種對稱的畸變稱為徑向畸變。

可轉(zhuǎn)換為擁有相同相機(jī)主心的像素坐標(biāo)：

得到畸變參數(shù)k1、k2后，先將圖片進(jìn)行去畸變處理，然后用去畸變的圖像坐標(biāo)估計(jì)相機(jī)內(nèi)參數(shù)。

3 圖像特征提取匹配與運(yùn)動估計(jì)方法

圖像特征提取匹配與運(yùn)動估計(jì)是視覺SLAM中重要的一環(huán)。作為視覺SLAM的前端，利用獲得的視覺信息進(jìn)行圖像特征的提取匹配[4]，結(jié)合利用深度信息可求得相鄰兩幀點(diǎn)云集的對應(yīng)關(guān)系，有了已知的3D-3D對應(yīng)關(guān)系便可以進(jìn)行運(yùn)動估計(jì)，即相鄰兩幀3D點(diǎn)對的歐式變換。那么得到最優(yōu)的路徑值，也就是求解最小二乘問題。

3.1 基于視覺傳感器的特征提取方法

ORB算法是進(jìn)行特征算子的檢測與描述算法，為了減少描述符提取計(jì)算壓力，使算法結(jié)果更精確，本文對所有檢測出的特征點(diǎn)進(jìn)行深度過濾，過濾后的特征點(diǎn)具有高精度特性，使后續(xù)特征點(diǎn)描述速度加快，算法結(jié)果更準(zhǔn)確。下面將具體介紹改進(jìn)后具有高精度、尺度不變性且具有旋轉(zhuǎn)不變性的ORB算法[5]。

ORB算法用FAST算法對圖像進(jìn)行特征點(diǎn)檢測，F(xiàn)AST算法的本質(zhì)是尋找角點(diǎn)[6]，其原理是尋找圖像像素點(diǎn)的灰度值梯度突變點(diǎn)，基于候選特征點(diǎn)周圍一圈的像素值，選定一個半徑將半徑領(lǐng)域內(nèi)的像素點(diǎn)與候選特征點(diǎn)進(jìn)行灰度值的比較，如果灰度值差別足夠大，則判定該像素點(diǎn)為特征點(diǎn)，由于FAST特征檢測算子沒有朝向所以會受圖像旋轉(zhuǎn)的干擾，為了解決這一問題，加入灰度質(zhì)心法（IntensityCentriod）的oFAST特征檢測算子具有方向性，灰度質(zhì)心法假設(shè)某特征點(diǎn)與灰度值半徑領(lǐng)域內(nèi)的重心存在偏移，通過計(jì)算特征點(diǎn)到重心的向量，就可得到該特征點(diǎn)的主方向。

圖像中深度大的特征點(diǎn)往往精度低且降低了算法速度，對得到的所有具有方向的特征點(diǎn)在深度圖中進(jìn)行深度過濾，設(shè)定深度閾值，篩選出高精度特征點(diǎn)。ORB算法利用BRIEF描述子描述特征點(diǎn)具備的屬性，它計(jì)算出的是一個二進(jìn)制串的特征描述符。比較所有特征點(diǎn)的灰度值，如果P（i）>P（i′），則生成二進(jìn)制串中的1，否則為0。為了使當(dāng)前幀所有的特征點(diǎn)具有旋轉(zhuǎn)不變性，改進(jìn)后的BRIEF描述子在建立坐標(biāo)系時，使當(dāng)前特征點(diǎn)為圓心，以特征點(diǎn)與取點(diǎn)區(qū)域的形心連線為X軸建立坐標(biāo)系。

漢明距離越小說明兩特征點(diǎn)相似度越高，匹配精度越高[7]。

3.2 外點(diǎn)剔除與運(yùn)動估計(jì)

3.2.1 外點(diǎn)剔除

剔除某些相似物體之間的匹配可提高精度，使后續(xù)運(yùn)動估計(jì)算法復(fù)雜度降低。本文在漢明距離的基礎(chǔ)上提出基于K最近領(lǐng)域算法（KNN）和使用交叉匹配過濾方法對外點(diǎn)進(jìn)行剔除，最后剩下的高精度匹配對稱為內(nèi)點(diǎn)。

1）KNN算法。設(shè)第二幀圖上特征點(diǎn)P及其描述P（Vp）在相鄰前一幀尋找最近點(diǎn)Q（Vq）與次近點(diǎn)Q˙（Vq˙），特征點(diǎn)P到兩點(diǎn)的漢明距離分別為D（Vp,Vq）、D（Vp,Vq˙）。令R=D（Vp,Vq）/D（Vp,Vq˙)，當(dāng)D(Vp,Vq)遠(yuǎn)小于D(Vp,Vq˙)時，R越小，則精度越高。在此可設(shè)定閾值T，當(dāng)R>T時，表示點(diǎn)p、q為錯誤匹配，在多次實(shí)驗(yàn)中，可令T=0.62。

2）交叉匹配過濾方法。交叉匹配過濾方法是一種雙向篩選方法，在當(dāng)前幀得到的特征匹配點(diǎn)集為S1={Pi（Vq,Vq˙）|i=1,2…m}，下一幀得到的特征匹配點(diǎn)集為S2={Qi（Vp,Vp˙）|i=1,2…l}，采用交叉匹配思路，對S1、S2進(jìn)行遍歷，凡有（i,j）=(j,h)，當(dāng)h=i時則保留匹配對，否則刪除。

3.2.2 運(yùn)動估計(jì)

在雙目相機(jī)及RGB-D相機(jī)得到3D-3D特征匹配后，通常用ICP算法進(jìn)行運(yùn)動估計(jì)[8]，ICP（迭代最近點(diǎn)）算法是一種點(diǎn)集對點(diǎn)集算法，該算法可以把上一幀的點(diǎn)云集通過旋轉(zhuǎn)平移得到下一幀的點(diǎn)云集，所以ICP算法求得最小的旋轉(zhuǎn)矩陣和平移向量是關(guān)鍵，得到3D-3D的對應(yīng)關(guān)系（P,P′）可轉(zhuǎn)換為求3D點(diǎn)對的歐式變換：

?i,pi=Rp′+t。

在上述視覺已提供了特征匹配之后，求解迭代最近點(diǎn)也變得容易，將上式轉(zhuǎn)換為

ei=φi-Rpi+t。

求解最佳運(yùn)動估計(jì)問題也轉(zhuǎn)換為誤差函數(shù)最小化的問題，即求解R、t：

式中，n為最鄰近點(diǎn)對個數(shù)。

上述方程可采用SVD奇異值分解法求解旋轉(zhuǎn)矩陣R,平移向量t。

兩個點(diǎn)云集之間的旋轉(zhuǎn)矩陣為

誤差為3.1648×10-5。

3.3 綁定深度信息的RANSAC算法

使用ICP算法得到匹配點(diǎn)集的“最短路徑”之后可采用RANSAC（隨機(jī)抽樣一致性）算法對匹配對進(jìn)行提純，RANSAC算法也可以用于圖像配準(zhǔn)及其拼接[9]。RANSAC算法采用不斷迭代的方法，在一組包含外點(diǎn)的配對集中尋找最優(yōu)參數(shù)模型，該最優(yōu)參數(shù)模型通過不斷迭代更新模型，直到達(dá)到設(shè)定的迭代次數(shù)K值后停止迭代，取當(dāng)前最優(yōu)值，具體步驟如下：

隨機(jī)選取3個匹配點(diǎn)對，設(shè)目標(biāo)圖像角點(diǎn)位置(x,y)，設(shè)當(dāng)前幀圖像角度位置為（x′,y′），通過基礎(chǔ)矩陣轉(zhuǎn)化可得：

利用基礎(chǔ)矩陣H計(jì)算目標(biāo)圖像與當(dāng)前幀圖像的投影誤差，小于設(shè)定閾值的判定為內(nèi)點(diǎn)并存入內(nèi)點(diǎn)集Q，如果當(dāng)前內(nèi)點(diǎn)集Q元素大于最優(yōu)內(nèi)點(diǎn)集，則更新最優(yōu)內(nèi)點(diǎn)集。

如果迭代次數(shù)大于設(shè)定迭代次數(shù)K，則停止，否則按上述步驟重復(fù)進(jìn)行。

當(dāng)RGB-D類傳感器精度與深度信息相關(guān)的時候，誤差大小與物體到攝像頭距離成正比[10]，近距離的物體在實(shí)現(xiàn)運(yùn)動估計(jì)或圖像配準(zhǔn)時的精度往往高于遠(yuǎn)距離的物體，本文提出綁定深度信息的RANSAC三維點(diǎn)配準(zhǔn)算法，將步驟（2）設(shè)定的閾值與深度信息相關(guān)聯(lián)，近距離的深度權(quán)重高，閾值可以表示為

∈i=hdi+v。

如果只有近距離的點(diǎn)成為內(nèi)點(diǎn)，那么進(jìn)行圖像配準(zhǔn)會造成配準(zhǔn)精度低的問題，為了加強(qiáng)配準(zhǔn)可靠性、穩(wěn)定性，可適當(dāng)添加深度較大的點(diǎn)。

表1 內(nèi)點(diǎn)及可靠性

4 全局優(yōu)化方法

為了解決前端漂移等噪聲引起的誤差，需要進(jìn)行閉環(huán)檢測和圖優(yōu)化。在進(jìn)行閉環(huán)檢測與圖優(yōu)化算法前，先要引出關(guān)鍵幀的定義。相機(jī)的移動路線可以把關(guān)鍵幀的位姿作為節(jié)點(diǎn)，將位姿之間的剛體轉(zhuǎn)換關(guān)系視作為節(jié)點(diǎn)與節(jié)點(diǎn)間的連線，如圖8所示。

后續(xù)的全局優(yōu)化需基于優(yōu)質(zhì)的關(guān)鍵幀進(jìn)行才會具有高效率高精度，關(guān)鍵幀過多會造成信息冗余度大，消耗了計(jì)算機(jī)資源。總結(jié)關(guān)鍵幀的選取有以下幾點(diǎn)：關(guān)鍵幀不需要頻繁插入，距離上一次定位較近時不需要插入；關(guān)鍵幀上的內(nèi)點(diǎn)數(shù)必須大于設(shè)定閾值。如今最常用根據(jù)幀間相對距離來選擇關(guān)鍵幀，由于深度相機(jī)在機(jī)器人運(yùn)動時通過旋轉(zhuǎn)獲得的信息量較平移時帶來的變化信息大，設(shè)幀間相對距離為：

Ed=w1‖Δt2‖2+w2‖（φ,θ,ω）‖2；

w2>w1。

進(jìn)行閉環(huán)檢測是消除誤差最有效的辦法，當(dāng)機(jī)器人再次到達(dá)此前識別過的某個點(diǎn)或點(diǎn)附近，由于相機(jī)位姿隨時間的漂移，導(dǎo)致了地圖不精確，此時機(jī)器人的建圖卻到達(dá)了別的地方。在前面利用ORB算法對每幀圖片進(jìn)行了特征探測，對其聚類產(chǎn)生了視覺詞典，在離線情況下，可以采集大量的場景圖片，形成信息量豐富的視覺詞典。進(jìn)行閉環(huán)檢測時，將關(guān)鍵幀形成的特征向量在視覺詞典里進(jìn)行搜索，判斷是否發(fā)生閉環(huán)。這種方法稱為基于詞袋模型的方法（Bag of words）[11]。

采用基于位姿圖的優(yōu)化算法，位姿圖模型如圖8所示。實(shí)質(zhì)為求解最小誤差函數(shù)的過程[12-13]，總體優(yōu)化問題變成包含頂點(diǎn)與邊的目標(biāo)函數(shù)[14]：

式中：e（xi,xj,zij）為第i節(jié)點(diǎn)與第j節(jié)點(diǎn)之間位姿誤差函數(shù)；zij為xi、xj的約束條件，如果xi、xj完全約束于zij，則誤差為0；Ωij是設(shè)為對角矩陣的信息矩陣，對角陣元素大小表示該誤差的重視程度。在RGB-D SLAM環(huán)境下，該目標(biāo)函數(shù)可以設(shè)為包含旋轉(zhuǎn)矩陣R與平移向量t的函數(shù)：

5 結(jié) 語

本文研究了基于RGB-D傳感器的同步定位與建圖方法，針對ORB特征匹配算法中易出現(xiàn)的錯誤匹配進(jìn)行改進(jìn)，加入設(shè)置漢明距離小于2倍最小匹配距離達(dá)到剔除錯誤匹配的目的。對基于視覺傳感器的物體運(yùn)動估計(jì)進(jìn)行原理解釋及仿真?？梢姡赗GB-D傳感器的SLAM基礎(chǔ)上，可實(shí)現(xiàn)機(jī)器人在不同環(huán)境下的自主導(dǎo)航工作。