一種基于特征點(diǎn)三維信息的自然路標(biāo)提取與快速匹配方法

苑全德等

摘 要： 自然路標(biāo)提取與匹配是 vSLAM 的基礎(chǔ)。文中提出了一種基于特征點(diǎn)三維信息的自然路標(biāo)提取、局部特征描述與快速匹配方法。采用雙目視覺獲取環(huán)境圖像， 提取左右目圖像的特征點(diǎn)， 并進(jìn)行匹配。建立左攝像機(jī)坐標(biāo)系下的每個匹配點(diǎn)的三維信息， 提出視場約束規(guī)則對特征點(diǎn)進(jìn)行過濾。在此基礎(chǔ)上基于改進(jìn)的MeanShift聚類算法進(jìn)行自然路標(biāo)提取。提出一種路標(biāo)描述符， 可以快速進(jìn)行兩個聚類的匹配。該方法可以有效提取非結(jié)構(gòu)化環(huán)境中的自然路標(biāo)， 對機(jī)器人位姿估計(jì)精度要求較低。

關(guān)鍵詞： 移動機(jī)器人；自然路標(biāo)提取；路標(biāo)描述符；MeanShift

中圖分類號：TP242.6 文獻(xiàn)標(biāo)識號：A 文章編號：2095-2163（2015）01-

Abstract： Landmark extraction and matching is basis of vSLAM. A method of landmark extraction， local feature description and fast matching based on 3D information of feature points is proposed. Robot obtains images of environment via binocular vision， extracting feature points from left and right eye images， matching feature points of the two images. Three-dimensional information of each matched points under left camera coordinate system is built. Field of view constraint rule is proposed to filtering points. Then， the method of natural landmark extraction based on improved Mean Shift algorithm is discussed. The paper proposes a landmark descriptor， which can achieve fast matching of the two clustering. This method can extract natural landmarks in unstructured environment， tolerating relatively low accuracy of pose estimation.

Key words： Mobile Robot； Natural Landmark Extraction； Landmark Descriptor； MeanShift

0引 言

同時定位和地圖創(chuàng)建（Simultaneous Localization and Mapping， SLAM）是實(shí)現(xiàn)機(jī)器人自主導(dǎo)航的一個關(guān)鍵技術(shù)[1]。SLAM技術(shù)主要采用視覺[2]、激光[3]等傳感器以實(shí)現(xiàn)環(huán)境感知，尤其是基于視覺的SLAM技術(shù)（vSLAM）具有的對環(huán)境無侵性、獲取環(huán)境信息的豐富性、以及成本低廉等顯著優(yōu)勢，使其受到研究學(xué)界的廣泛關(guān)注和高度重視。

vSLAM主要采用路標(biāo)方法， 路標(biāo)的自動提取與快速匹配即是其中的基礎(chǔ)性問題。基于特征點(diǎn)的路標(biāo)的表示方法主要分為兩類。在此，可做如下具體分析：

一類以提取出的特征點(diǎn)直接作為路標(biāo)，這類方法構(gòu)建的地圖路標(biāo)的特征點(diǎn)數(shù)量較多， 進(jìn)行場景匹配的運(yùn)算量也隨之較大。重點(diǎn)成果則有：文獻(xiàn)[2]基于SIFT算法對環(huán)境圖象進(jìn)行特征提取，形成路標(biāo)并構(gòu)建分層地圖。 文獻(xiàn)[4]也提出一種基于顯著場景Bayesian Surprise的自然路標(biāo)檢測方法。并且，文獻(xiàn)[5]又使用關(guān)鍵幀Harris角點(diǎn)作為自然路標(biāo)， 實(shí)現(xiàn)了機(jī)器人的實(shí)時全局定位與導(dǎo)航。相應(yīng)地，文獻(xiàn)[6]亦同樣使用環(huán)境圖像的SIFT特征點(diǎn)作為自然路標(biāo)，利用特征點(diǎn)信息建立自然路標(biāo)數(shù)據(jù)庫，實(shí)現(xiàn)了環(huán)境地圖創(chuàng)建。針對特征點(diǎn)的提取也進(jìn)行了大量的研究，文獻(xiàn)[7]即提出了基于圖像二階多項(xiàng)擴(kuò)展式局部方向張量的興趣點(diǎn)檢測 PLOT算子，并利用雙目視覺實(shí)現(xiàn)了機(jī)器人的SLAM過程。繼而，文獻(xiàn)[8]提出一種利用雙目視覺有效提取特征點(diǎn)的無監(jiān)督算法， 該算法使用SOM對SIFT特征點(diǎn)進(jìn)行快速匹配以獲得穩(wěn)定的特征點(diǎn)。還有文獻(xiàn)[9]通過應(yīng)用Harris算法來提取角點(diǎn)。而文獻(xiàn)[10]則針對單目視覺提取的特征點(diǎn)給出了統(tǒng)一逆深度參數(shù)方法來準(zhǔn)確表達(dá)其不確定性。

另一類路標(biāo)表示法是通過對提取的特征點(diǎn)進(jìn)一步聚類， 再將獲得的聚類整體作為一個路標(biāo)。相應(yīng)成果有，文獻(xiàn)[11]提出了一種基于角點(diǎn)聚類的自然路標(biāo)局部特征提取其匹配算法。本文則基于對特征點(diǎn)聚類的思路，進(jìn)一步提出了一種基于三維信息對特征點(diǎn)進(jìn)行聚類分析形成路標(biāo)、對路標(biāo)進(jìn)行局部特征描述與快速匹配的方法。

1特征點(diǎn)及其三維信息的獲取與預(yù)處理

機(jī)器人使用參數(shù)相同的兩個攝像機(jī)獲取環(huán)境圖像，對圖像進(jìn)行特征點(diǎn)選取與過濾， 又采用雙目視覺原理獲得特征點(diǎn)對應(yīng)空間點(diǎn)的三維信息。

1.1特征點(diǎn)的選取

本文采用SURF算法對左右目攝像機(jī)獲得的圖像分別進(jìn)行特征點(diǎn)提取，基本可以達(dá)到實(shí)時處理的要求[12]。令由左目獲取的圖像標(biāo)記為， 由的特征點(diǎn)組成集合， 同理右目圖像的特征點(diǎn)組成集合， 因?yàn)閮蓴z像機(jī)是對同一場景進(jìn)行拍攝， 這就使得提取的特征點(diǎn)大致相同。為了計(jì)算特征點(diǎn)對應(yīng)空間點(diǎn)的三維信息， 需要找到左目圖象的特征點(diǎn)在右目圖像中的對應(yīng)點(diǎn)以獲得該點(diǎn)在雙目中的視差， 即某一點(diǎn)在兩幅圖像中相應(yīng)點(diǎn)的位置差。

以中的點(diǎn)為基準(zhǔn)與中的點(diǎn)進(jìn)行匹配，使之一一對應(yīng)，未獲匹配的表示該點(diǎn)在其中一目中看不到。匹配上的特征點(diǎn)、組成特征點(diǎn)對并入集合中， 記 為主特征點(diǎn)。

1.2特征點(diǎn)對應(yīng)空間點(diǎn)的三維信息的獲取

特征點(diǎn)對對應(yīng)的空間點(diǎn)記為， 由所有空間點(diǎn)組成的集合記為。攝像機(jī)坐標(biāo)系的原點(diǎn)在攝像機(jī)鏡頭的光心處。為簡化計(jì)算， 令左右鏡頭的參數(shù)相同， 兩攝像機(jī)光心在同一個平面上， 在左攝像機(jī)坐標(biāo)系下的三維坐標(biāo)信息可以利用視差原理獲得：

其中， 是空間點(diǎn)P在左攝像機(jī)坐標(biāo)系下的三維坐標(biāo)， b為左右攝像機(jī)的基線距，d為視差，f為焦距，u1、v1為特征點(diǎn)在左右視覺中的圖像坐標(biāo)。點(diǎn)的三維坐標(biāo)在聚類過程中將會用到。而提取路標(biāo)后、進(jìn)行存儲時， 應(yīng)將所有空間點(diǎn)在攝像機(jī)坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換為世界坐標(biāo)系下的坐標(biāo)。

2自然路標(biāo)的提取與描述

2.1基于改進(jìn)MeanShift算法的特征點(diǎn)聚類提取路標(biāo)

已建立在攝像機(jī)坐標(biāo)系下的坐標(biāo)， 根據(jù)這些點(diǎn)的相對空間位置進(jìn)行聚類分析以提取路標(biāo)。 MeanShift算法因?yàn)檫\(yùn)算量小， 計(jì)算速度快，且有一定的魯棒性，更為適合進(jìn)行特征點(diǎn)聚類。但卻需要對該算法加以改進(jìn)， 就使得算法可以根據(jù)不同特征點(diǎn)的聚合情況，相應(yīng)產(chǎn)生不同數(shù)量的聚類。

MeanShift 可以理解為一個核密度估計(jì)的過程， 即將空間點(diǎn)看作采樣得到的數(shù)據(jù)點(diǎn)， 并將單位體積內(nèi)的空間點(diǎn)數(shù)看作概率密度， 由此求取概率密度最大的點(diǎn)集合就完成了聚類。每個點(diǎn)X的概率密度可以表示為：

其中，為核密度函數(shù)，k可取高斯函數(shù)，是以點(diǎn)為圓心，即為半徑的球型區(qū)域內(nèi)的點(diǎn)，為聚類半徑， 可以看作 Parzen 窗口尺寸，則為半徑內(nèi)的空間點(diǎn)的個數(shù)。

欲求概率密度最大的點(diǎn)，可先對求導(dǎo)，使則可求得極值點(diǎn)，令，有

當(dāng)， 即可近似求得點(diǎn)概率密度的極值點(diǎn)， 而以其為中心點(diǎn)， 半徑的區(qū)域內(nèi)的點(diǎn)將形成一個聚類。其中，為閾值。

考慮到自然路標(biāo)有大有小，應(yīng)保證聚類內(nèi)最少包含個特征點(diǎn)數(shù)， 否則形成的聚類特征點(diǎn)太少， 將導(dǎo)致路標(biāo)不穩(wěn)定且不利于路標(biāo)之間的區(qū)分，而且更不利于定位。此后，在聚類過程中將自動調(diào)整半徑。如果以點(diǎn)為圓心， 半徑內(nèi)特征點(diǎn)數(shù)， 則以步長增加半徑， 直至， 可稱為增長半徑。其中，為聚類中最少特征點(diǎn)數(shù)，為聚類最大半徑。

一般情況下，聚類半徑長度未增長到最大半徑時， 特征點(diǎn)數(shù)與已經(jīng)滿足要求了， 此時聚類已經(jīng)形成， 但存在半徑再適當(dāng)擴(kuò)大仍然可能有特征點(diǎn)的情況， 為此提出聚類進(jìn)行小幅度擴(kuò)張的應(yīng)對策略。如果有距離聚類較近的點(diǎn)與聚類的距離小于， 可將聚類半徑增加以包括該點(diǎn)，并重復(fù)該過程。擴(kuò)張幅度， 即可取。 如果在下一步擴(kuò)張了的聚類范圍內(nèi)沒有更多的點(diǎn)或者超過了最大聚類半徑限制， 則隨即停止該擴(kuò)張過程。

組成路標(biāo)的特征點(diǎn)分布范圍半徑也應(yīng)該控制在一定范圍內(nèi)，這與自然環(huán)境中作為物體多會有一定尺寸限制現(xiàn)象相吻合。本文取

其中，為聚類初始半徑。

確定一個聚類后，從未訪問過的特征點(diǎn)中隨機(jī)選擇一點(diǎn)作為中心，繼續(xù)尋找下一聚類，直至未訪問過的點(diǎn)的數(shù)量。

2.2聚類描述符

考慮使用一個快速索引匹配路標(biāo)的方法， 問題可以描述為已知一組形成聚類的點(diǎn)的三維坐標(biāo)， 設(shè)計(jì)一個具有唯一性和旋轉(zhuǎn)不變性的描述符， 并且該描述符應(yīng)在兩個聚類匹配過程中有充分的區(qū)分度。

本文基于空間點(diǎn)的分布來表征描述符。由聚類中心指向組成聚類的各個特征點(diǎn)形成了一組向量，向量數(shù)學(xué)原理如式（6）所示，三維空間實(shí)現(xiàn)則如圖1所示。

將聚類的區(qū)域平分若干份， 依次統(tǒng)計(jì)各個區(qū)域內(nèi)向量， 得到一個特征向量。 其對應(yīng)數(shù)學(xué)表示則如式（6）所示。

在圖2中， 以步長0.5滑動窗口， 取絕對值最大的窗口方向?yàn)橹鞣较颉Ｒ跃垲惖闹鞣较驗(yàn)榛鶞?zhǔn)方向， 圍繞聚類中心的x軸， 再取逆時針為正方向， 而將整個聚類按為跨度劃分作24個區(qū)域，并且分別重新計(jì)算各個區(qū)域的向量的模與高斯核函數(shù)的卷積， 將其組成一個24維的向量，該向量即為該聚類的描述符。

3路標(biāo)匹配過程

機(jī)器人在環(huán)境中移動時， 對獲取的圖像先進(jìn)行預(yù)處理， 估計(jì)自己的位姿， 并根據(jù)當(dāng)前位姿到地圖數(shù)據(jù)庫查詢之前機(jī)器人以該位姿觀察到的路標(biāo)。令當(dāng)前位姿為， 為保證找到路標(biāo)， 可查詢符合如下條件的路標(biāo)，具體是：， ， 。

對當(dāng)前提取的環(huán)境的特征點(diǎn)進(jìn)行聚類， 計(jì)算該聚類描述符，并與數(shù)據(jù)庫中對應(yīng)路標(biāo)的描述符進(jìn)行匹配， 在誤差允許的范圍內(nèi)如果實(shí)現(xiàn)了匹配， 說明找到了路標(biāo)， 則取出每個點(diǎn)的信息， 對已然匹配的兩個聚類中的各個點(diǎn)進(jìn)行二次匹配， 對獲得匹配的點(diǎn)的世界坐標(biāo)即可認(rèn)定為一致， 并且是以數(shù)據(jù)庫中的坐標(biāo)為基準(zhǔn)。根據(jù)機(jī)器人與每個點(diǎn)的距離、每個點(diǎn)的世界坐標(biāo)。

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

本文使配有雙目視覺的移動機(jī)器人在實(shí)驗(yàn)室環(huán)境中運(yùn)行而實(shí)現(xiàn)自然路標(biāo)的提取。圖3為左右目特征點(diǎn)提取與匹配結(jié)果，可以看到在初步的匹配后，有許多誤匹配的點(diǎn)，需要對其進(jìn)行高質(zhì)量過濾。

應(yīng)用視差約束， 視場約束等規(guī)則過濾后的結(jié)果， 剔除了大多數(shù)明顯錯誤的匹配點(diǎn)，如圖4所示。

對過濾后的特征點(diǎn)進(jìn)行聚類?設(shè)定初始半徑為25cm，半徑增長步長為10cm，聚類結(jié)果如圖5所示。為了直觀起見，將聚類得到的特征點(diǎn)對應(yīng)的圖像中的點(diǎn)標(biāo)出，如圖6所示。

需要注意的是，由于機(jī)器人位姿不同， 導(dǎo)致對同一個物體的觀察角度出現(xiàn)差別， 當(dāng)觀察角度差別較大時， 會出現(xiàn)聚類結(jié)果不同的現(xiàn)象，因?yàn)槲矬w的不同側(cè)面的特征點(diǎn)不可能完全相同。如果在匹配過程中，使用原有路標(biāo)的靠近聚類中心的特征點(diǎn)進(jìn)行引導(dǎo)聚類，無法聚類成功或者無法匹配成功，說明需要建立新的路標(biāo)，并更新路標(biāo)庫。

5結(jié)束語

利用圖像特征點(diǎn)的三維坐標(biāo)信息進(jìn)行聚類， 可以有效地將空間距離相近的點(diǎn)歸為一個路標(biāo)， 在機(jī)器人由于位姿變化導(dǎo)致視角不同時， 仍然可以較大概率匹配到原先觀察到的路標(biāo)。 經(jīng)過改進(jìn)的Mean Shift算法根據(jù)為不同的場景產(chǎn)生合適的數(shù)量不等的路標(biāo)， 可以很好地適應(yīng)復(fù)雜環(huán)境。 本文提出的路標(biāo)描述符可以有效提高路標(biāo)匹配速度， 避免重復(fù)存儲相同路標(biāo)。

參考文獻(xiàn)：

[1] Kortenkamp D， Bonasso R P， Murphy R. AI-based mobile robots： case studies of successful robot systems [M]. Cambridge： MIT Press， 1998.

[2]黃慶成， 洪炳镕， 厲茂海， 等. 基于主動環(huán)形閉合約束的移動機(jī)器人分層同時定位和地圖創(chuàng)建[J]. 計(jì)算機(jī)研究與發(fā)展， 2007， 44（4）： 636-642.

[3] 馮肖維，方明倫，何永義，等. 移動機(jī)器人自然路標(biāo)特征提取方法[J]. 機(jī)器人， 2010， 32（4）： 540-546.

[4] 錢堃，馬旭東，戴先中，等. 基于顯著場景Bayesian Surprise的移動機(jī)器人自然路標(biāo)檢測[J]. 模式識別與人工智能， 2013， 26（6）：571-576.

[5] XU X D， LUO Y， HAO H G. Vision-based mobile robot localization using natural landmarks[C]// Systems and Informatics （ICSAI）， 2012 International Conference on， Chicago， USA： IEEE， 2012： 2012 – 2015.

[6] SE S， LOWE D G， et al. Vision-based global localization and mapping for mobile robots [J]. Robotics， IEEE Transactions on. 2005， 21（3）： 364-375.

[7]林睿.基于圖像特征點(diǎn)的移動機(jī)器人立體視覺SLAM研究[D]. 哈爾濱： 哈爾濱工業(yè)大學(xué)， 2012.

[8] SHARMA K， MOON I， et al. Extraction of visual landmarks using improved feature matching technique for stereo vision applications [J]. IETE Technical Review. 2012， 29（6）： 473-481.

[9] MOZOS ? M， GIL A， et al. Interest Point Detectors for Visual SLAM[C]// 12th Conference of the Spanish Association for Artificial Intelligence， Springer Berlin Heidelberg. 2007： 170-179.

[10] MONTIEL J M M， CIVERA J， DAVISON A J. Unified inverse depth parametrization for monocular SLAM[C]// Proceedings of Robotics：Science and Systems. Cambridge， Massachusetts ：MIT Press， 2006.

[11] 蔡自興，王勇，王璐.基于角點(diǎn)聚類的移動機(jī)器人自然路標(biāo)檢測與識別[J].智能系統(tǒng)學(xué)報，2006，1（1）： 52-56.

[12] BAY H， ESS A， et al. Speeded-up robust features （SURF） [J]. Computer Vision and Image Understanding. 2008， 110（3）： 346-359.