基于視覺和距離傳感器的SLAM和導航方法的探新

花罡辰

（東京工業(yè)大學綜合理工學研究科，日本 橫濱 226-8503）

基于視覺和距離傳感器的SLAM和導航方法的探新

花罡辰

（東京工業(yè)大學綜合理工學研究科，日本 橫濱 226-8503）

SLAM(Simultaneous Localization and Mapping)被廣泛應(yīng)用于生成地圖和機器人導航領(lǐng)域。基于視覺特征的SLAM是一種低成本的解決方案，且能夠提取環(huán)境的豐富的特征點用于機器人導航，但是，現(xiàn)實世界的可視特征往往是動態(tài)的，很多基于視覺特征的SLAM方法都不能排除環(huán)境中移動物體的影響，比如：在餐廳或購物中心的行人等等。ICGM(Incremental Center of Gravity Matching)是一個通過圖像隊列之間特征點的幾何結(jié)構(gòu)互相匹配而得到穩(wěn)定特征點的方法，基于ICGM的SLAM能有效地排除環(huán)境中移動物體的影響。本研究在前人研究的基礎(chǔ)上提出了一種新的算法，利用ICGM的方法提取環(huán)境穩(wěn)定的視覺特征，結(jié)合Kinect距離傳感器，在高度動態(tài)環(huán)境中實現(xiàn)了較高精度的機器人自動導航。并通過實驗檢驗了這種算法，實驗結(jié)果顯示，本方法的速度能夠達到實時處理的要求，與此前研究的其它方法相比，這種方法能夠達到更高的精度，所提出的SLAM和導航系統(tǒng)更加接近實用要求。

SLAM；計算機視覺；機器人技術(shù)

1 關(guān)聯(lián)研究

SLAM對多種機器人系統(tǒng)都是必不可少的，自動導航對于機器人系統(tǒng)來說也非常重要。

在SLAM和機器人導航領(lǐng)域，近年來有非常多的先進研究，其核心目標是提高機器人導航系統(tǒng)的速度、精度和可靠性。然而，在現(xiàn)實使用中，機器人周圍的工作環(huán)境往往是動態(tài)的，因此，“如何在動態(tài)的環(huán)境中提高位置識別精度”，成為當前研究的熱點。

大多數(shù)基于圖像特征的SLAM或?qū)Ш椒椒?，是提取SIFT(Scale-Invariant Feature Transform)或SURF(Speeded Up Robust Feature)特征作為定位時的視覺識別標志，但是，如果使用SIFT或SURF，SLAM或?qū)Ш较到y(tǒng)就會把移動中的物體，比如行人、汽車等，作為定位時的視覺識別標志，顯然會對定位的效率產(chǎn)生不好的影響。

KawewongA.et al[2,3]提出了位置不變穩(wěn)定特征值PIRF (Position Invariant Robust Feature)。PIRF 是一個通過圖像隊列之間特征點的幾何結(jié)構(gòu)互相匹配而得到穩(wěn)定特征點的方法。作為定位時的視覺識別標志，它能一定程度排除掉動態(tài)物體對導航精度的影響?；赑IRF，Morioka H.et al.[4]提出了一種基于PIRF的SLAM和機器人導航方法，即3D-PIRF。3D-PIRF從單眼全方位相機的連續(xù)圖像中中提取PIRF特征，并結(jié)合機器人里程計的信息推算出所有PIRF特征的空間3D坐標。把這些PIRF特征和空間3D坐標學習后，構(gòu)建一個混合地圖(Hybrid Map)。便能進行導航。導航時，利用8點方法可以計算出機器人的全局位置。

雖然PIRF是一種較為有效的安定特征點提取方法。但是HUAGangchen et al[1].提出了一種比PIRF更有效的安定特征點提取方法，被稱作ICGM。對比PIRF，ICGM在利用圖像隊列之間特征點互相匹配的基礎(chǔ)上還利用了特征點的幾何結(jié)構(gòu)。把ICGM應(yīng)用在只基于圖像的在高度動態(tài)環(huán)境下的SLAM中可以達到比PIRF更高的精度。

本研究基于ICGM加上新穎的Kinect傳感器，以期實現(xiàn)比3D-PIRF更好的SLAM和機器人導航系統(tǒng)。

本方法實現(xiàn)機器人自動導航分為兩個步驟，學習階段和導航階段。SLAM即為學習階段。導航階段機器人利用SLAM的數(shù)據(jù)進行自動導航。

圖1是本研究的基本結(jié)構(gòu)：

圖1 研究的基本結(jié)構(gòu)Fig.1 Basic structure of the proposed method

圖1右邊所示為SLAM和導航機器人的硬件，機器人上端安裝有一臺Kinect傳感器。左上為Kinect傳感器得到的環(huán)境的距離信息（暖色代表離傳感器距離近，冷色代表遠）和圖像信息。Kinect傳感器能夠同時獲取環(huán)境中的距離信息和圖像信息。左下為SLAM系統(tǒng)計算出的機器人的移動路徑。

2 ICGM簡介

ICGM(Incremental Center of Gravity Matching) 是一個通過圖像隊列之間特征點的幾何結(jié)構(gòu)互相匹配而得到穩(wěn)定特征點的方法。

其基本思路如圖2所示。假設(shè)Ia和Ib是在相同地點不同時刻拍攝的兩張照片。

圖2：Incremental Center of Gravity Matching的基本思路Fig.2 Basic idea of Incremental Center of Gravity Matching

圖中的(a,a'),(b,b'),(c,c'),(d,d'),(e,e')為已經(jīng)被匹配上的特征點（SURF或SIFT）對。(a,a'),(b,b'),(c,c'),(e,e')之間的匹配是穩(wěn)定的匹配，(d,d')是不穩(wěn)定的匹配。換句話說(a,a'),(b,b'),(c,c'),(e,e')在到拍攝的時間段內(nèi)，位置沒有大的變化，所以(a,a'),(b,b'),(c,c'),(e,e')是穩(wěn)定的。而(d,d')的位置變化了，所以(d,d')之間的匹配是不穩(wěn)定的。

以上所提到的穩(wěn)定和不穩(wěn)定的概念皆是時間尺度上的概念。假設(shè)Ia拍攝到Ib拍攝經(jīng)過時間t，那么(d,d')在t內(nèi)是不穩(wěn)定的。

本研究利用ICGM方法用來排除不穩(wěn)定的匹配。

首先假設(shè)系統(tǒng)已知(a,a'),(b,b'),(c,c')的匹配是穩(wěn)定的。然后分別在Ia和Ib中計算a,b,c和a',b',c'的重心，得到o,o'?；趏,o'我們能夠計算出重心向量（從已知重心到其他特征點的向量）：。顯然地，但是。我們能夠利用兩張圖的重心向量的關(guān)系來判斷一個特征點的匹配對是否穩(wěn)定。

因此，通過合理地設(shè)置一個閾值可以區(qū)分一個匹配對是否穩(wěn)定。

實際使用時，ICGM方法使用了差值比例RoG（ratio of difference）來計算匹配對的穩(wěn)定度。

RoG為重心向量的差的模和重心向量的模的和的比例。當式(2)成立時，一個特征點匹配對即被認為不穩(wěn)定，反之則穩(wěn)定。

實際的ICGM方法會首先隨機地提取初始N個匹配對，分別計算他們的重心。再用(1)計算初始N個點的RoG，用式(2)判斷它們穩(wěn)定與否。當且僅當初始的N點都穩(wěn)定時，才進入下一步計算。

得到N個初始的穩(wěn)定的特征點后，通過它們的重心來測試剩下特征點匹配對的穩(wěn)定度。

假設(shè)如圖2，(a,b,c)和(a',b',c')是初始的3個穩(wěn)定的匹配對。然后，(e,e')是被識別為穩(wěn)定，ICGM會重新計算重心，用(a,b,c,e)和(a',b',c',e')的重心來計算剩下的匹配對的穩(wěn)定度。因為(d,d')不穩(wěn)定，所以(d,d')會被直接排除，重心也不會被重新計算。

ICGM最后能夠排除所有不穩(wěn)定的特征點，而留下穩(wěn)定的特征點。

以上描述了ICGM排除不穩(wěn)定匹配對的方法。如圖3所示，假設(shè)為機器人在時間軸上連續(xù)的圖片，在SLAM和導航中，我們分別在和，之間計算ICGM。假設(shè)A為計算ICGM后，的穩(wěn)定特征點。假設(shè)B為計算ICGM后，的穩(wěn)定特征點。我們?nèi)和B的并集作為的穩(wěn)定的特征點。

圖3 基于ICGM從連續(xù)的圖像隊列中提取穩(wěn)定特征值Fig.3 Robust feature extraction method based on ICGM

3 基于ICGM的SLAM和機器人導航

本研究在學習SLAM階段會構(gòu)建一個混合地圖。導航階段使用混合地圖進行自動導航。

3.1 混合SLAM和混合地圖的構(gòu)建（學習階段）

目前有很多SLAM都是只基于視覺信息的（vision-only），但只基于視覺信息的SLAM只能夠獲取系統(tǒng)當前的大概位置。而對于機器人導航，精確的6自由度的全局姿態(tài)是必要。6自由度的全局姿態(tài)即機器人在地圖中的3維位置（x，y，z）和3維的機器人的·全局的的角度姿態(tài)（roll，pitch，yaw）。

為了獲取機器人6自由度的全局姿態(tài)，我們開發(fā)了一種基于vision-only SLAM的混合SLAM。利用混合SLAM我們能夠構(gòu)建混合地圖。

本方法的混合地圖是基于可視里程器構(gòu)建的?？梢暲锍唐魍ㄟ^計算3D點云間的6D剛性變換[6]實現(xiàn)。

一幀kinect數(shù)據(jù)先通過ICGM提取穩(wěn)定特征點，然后通過kinect的距離數(shù)據(jù)算出穩(wěn)定特征點的空間3D坐標，這些帶有空間3D坐標的穩(wěn)定特征點生成一個3D點云。我們把這樣的一個點云叫做一個數(shù)據(jù)模型。我們能夠連續(xù)得到一個的集合M。

如圖5所示，隨著學習路徑的變長，里程計的誤差會漸漸累計，導致很大的誤差。學習過程中我們必須控制誤差的累計。

本方法利用ICGM的vision-only SLAM[1]檢測閉環(huán)。檢測閉環(huán)是指通過可視特征檢測當前的位置是不是一個之前已經(jīng)來過的位置。

檢測出閉環(huán)后，利用vision-only SLAM取得3個最好的數(shù)據(jù)模型，Mbest，Msecond_best，Mthird_ best。

然后我們嘗試分別計算當前的數(shù)據(jù)模型Mcurrent和Mbest，Msecond_best，Mthird_best之間的剛性變換。假如都成功了，我們能得到3個相對姿態(tài)。假如都失敗了，我們會認為此次閉環(huán)檢測失敗了。

然后根據(jù)RANSAC建模的結(jié)果好壞，我們從上面3個相對姿態(tài)選取一個最好的相對姿態(tài)和最好的閉環(huán)位置。進一步可以得到當前模型的通過閉環(huán)檢測得到的準確的全局位置：。此為消除了累計誤差之后的當前位置。

我們通過最小化(5)式來最優(yōu)化current時刻之前的路徑。式（5）中表示current時刻之前學習的6自由度的全局姿態(tài)。

3.2 基于混合地圖的自動導航（導航階段）

自動導航仍舊基于閉環(huán)檢測。步驟和學習階段類似，但是是基于學習階段的混合地圖實現(xiàn)定位。通常會把設(shè)置為1以達到快速的位置識別的響應(yīng)。

機器人導航時同樣利用ICGM的vision-only SLAM檢測閉環(huán)，然后獲取最佳的和。

4 實驗

本實驗包括兩個階段，學習階段和導航階段。

如圖4，這個實驗是在東京工業(yè)大學食堂進行的。食堂大小為20*20m。我們在晚上8點的時候檢測自己所提出的方法。這時，食堂大概有70人。他們在這個環(huán)境中自由地進餐或走動。這是一個高度動態(tài)的環(huán)境。

圖4 經(jīng)過ICGM處理的結(jié)果:行人被ICGM作為動態(tài)物體排除了Fig.4 ICGM's processing result:pedestrians are ignored

在學習階段，我們使用了游戲手柄控制機器人。機器人在移動過程中實時記錄里程計和kinect的圖像及距離信息。我們控制機器人繞了兩圈。在學習的終點，我們讓機器人回到學習的原點?？偟囊苿拥木嚯x大概為80米。在整個過程中，機器人記錄了6739幀數(shù)據(jù)。

雖然環(huán)境是高度動態(tài)的，但由于ICGM特征點的穩(wěn)定性，學習結(jié)果：kinect的可視里程計的成功率為97.6%，所以機器人路徑有97.6%由kinect的數(shù)據(jù)算出。閉環(huán)檢出率為63.8%。

圖5為機器人里程計計算出的路徑，顯然地，機器人里程計計算出的路徑誤差會漸漸累計，導致極大的誤差。圖5為通過本方法學習的路徑。

由于環(huán)境較大，取得真實數(shù)據(jù)很難，所以較難做出精確的誤差評價。但因為本方法通過檢出閉環(huán)修正了路徑，誤差累計的問題顯然被較好地解決了。

圖5 單純的里程計計算出的機器人路徑Fig.5 Route calculated only by robot's odometry

圖6 通過檢出閉環(huán)修正了的路徑。紅色代表閉環(huán)被檢出的位置Fig.6 Corrected route based on loop-closure detection.Red points represent loop-closure detected locations

表1為閉環(huán)檢出率的對比。相比3D-PIRF[3]，本方法的閉環(huán)檢出率大大提高了。

表1 閉環(huán)檢出率的對比Tab.1 Comparisons of loop-closure detected ratio

在導航階段，我們選取了一段長度為12米的路徑作為規(guī)劃路徑。此路徑包含在學習路徑中。我們基于上一學習階段構(gòu)建的混合地圖進行導航。

導航階段每幀平均計算時間為321ms。機器人移動速度為125mm/s，用地板磚的分界線作為誤差評判基準，移動階段對于規(guī)劃路徑的平均偏移為60mm，最大偏移為92mm。

表2為本方法和3D-PIRF[3]的導航結(jié)果的對比。請注意，本方法運用了GPU加速，而3D-PIRF沒有，所以計算速度的數(shù)據(jù)僅供參考。

表2 導航結(jié)果對比Tab.2 Comparisons of navigation's results

5 總結(jié)

本研究提出了一個基于混合使用視覺特征和距離傳感器的SLAM和機器人導航方法，它能在高度動態(tài)的環(huán)境中穩(wěn)定工作。實驗結(jié)果顯示，本方法的速度能夠達到實時處理的要求，相比前人研究的方法，這種方法能夠達到更高的精度，所提出的SLAM和導航系統(tǒng)更加接近實用要求。

在高度動態(tài)環(huán)境中工作是多種機器人必備的能力。如果機器人能利用一般的家用傳感器，就能大大降低機器人制造成本。微軟公司的Kinect是一種高速高精度且低成本的傳感器，它非常適合未來的商用機器人。另外，除了機器人，一些車輛也能使用Kinect實現(xiàn)自動導航，巨大的應(yīng)用需求給本方法提供了廣闊的前景。

（

）

[1]HUAGangchen,HasegawaO.,A Robust Visual-Feature-Extraction Method for Simultaneous Localization and Mapping in Public Outdoor Environment[J].Vol.19 No.1,2015,11-22.

[2]Kawewong A.,Tangruamsub S.,and HasegawaO..Positioninvariant robustfeatures for long-term recognition of dynamic outdoor scenes[J].IEICE Transactionson Information and Systems,2010,E93-D(9):2587-2601.

[3]Kawewong A.,Tongprasit N.,Tangruamsub S.,and HasegawaO..Online incremental appearance-based slam in highly dynamic environments[J].Int.J.of Robotics Research,2011,30(1):33-55.

[4]MoriokaH.,Sangkyu Y.,and HasegawaO..Visionbased mobile robot's slam and navigation in crowded environments[R].IEEE/RSJ International Conferenceon Intelligent Robots and Systems(IROS),2011.

[5]FischlerM.A.and Bolles R.C..Random sample consensus:aparadigm for model fitting with applications to image analysis and automated cartography[J].Commun.ACM,1981,24(6):381-395.

[6]Berthold K.P.Horn.Closed-form solution of absolute orientation using unit quaternions[J].JOSA A,1987,4:629-642.

Anexploration on visionbased SLAM and navigation method combining range finder

HUA Gangchen
（Tokyo institute of technology,Interdisciplinary Graduate School of Science and Engineering,Yokohama 226-8503,Japan）

Simultaneous Localization and Mapping(SLAM) is widely used for generation of maps for autonomous robotic navigation etc.SLAM based on visual features is a kind of low cost solution.Besides,SLAM based on visual features have plenty information for matching and recognition.But appearance of actual world is always dynamic.Many kinds of vision- based SLAM method are not robust to influence of unstable objects such as walking humans in cafeterias or shopping malls.ICGM(Incremental Center of Gravity Matching) is method which extracts static visual features from images sequence based on feature points geometric structure.SLAMs based on ICGM can exclude bad influences of dynamic objects.This proposed method is using ICGM to extract static features and combines range finderinformation.Thus,the proposed method can achieve high accuracy robotic navigation in highly dynamic environment.We run our approach in a crowed cafeteria.The result shows that by using proposed method SLAM and navigation can be achievedfast enough for real-time processing.Comparing to previous,the proposed method's precision is higher.Proposed method is more suitable for actual needs.

Simultaneous Localization and Mapping(SLAM);computer vision;robotics

TP242

A

1672-6332（2015）01-0083-06

【責任編輯：楊立衡】

2015-03-10

花罡辰（1988-），男（漢），江西撫州人，博士，主要研究方向：智能科學系統(tǒng)。E-mail:huagangchen1988@gmail.com