1 關聯研究
SLAM對多種機器人系統都是必不可少的，自動導航對于機器人系統來說也非常重要。在SLAM和機器人導航領域，近年來有非常多的先進研究，其核心目標是提高機器人導航系統的速度、精度和可靠性。然而，在現實使用中，機器人周圍的工作環境往往是動態的，因此，“如何在動態的環境中提高位置識別精度”，成為當前研究的熱點。
大多數基于圖像特征的SLAM或導航方法，是提取SIFT(Scale-Invariant Feature Transform)或SURF(Speeded Up Robust Feature)特征作為定位時的視覺識別標志，但是，如果使用SIFT或SURF，SLAM或導航系統就會把移動中的物體，比如行人、汽車等，作為定位時的視覺識別標志，顯然會對定位的效率產生不好的影響。
KawewongA.et al [2,3] 提出了位置不變穩定特征值PIRF (Position Invariant Robust Feature)。PIRF 是一個通過圖像隊列之間特征點的幾何結構互相匹配而得到穩定特征點的方法。作為定位時的視覺識別標志，它能一定程度排除掉動態物體對導航精度的影響。基于PIRF，Morioka H. et al. [4] 提出了一種基于PIRF的SLAM和機器人導航方法，即3D-PIRF。3D-PIRF從單眼全方位相機的連續圖像中中提取PIRF特征，并結合機器人里程計的信息推算出所有PIRF特征的空間3D坐標。把這些PIRF特征和空間3D坐標學習后，構建一個混合地圖(Hybrid Map)。便能進行導航。導航時，利用8點方法可以計算出機器人的全局位置。
雖然PIRF是一種較為有效的安定特征點提取方法。但是HUAGangchen et al [1] . 提出了一種比PIRF更有效的安定特征點提取方法，被稱作ICGM。對比PIRF，ICGM在利用圖像隊列之間特征點互相匹配的基礎上還利用了特征點的幾何結構。把ICGM應用在只基于圖像的在高度動態環境下的SLAM中可以達到比PIRF更高的精度。
本研究基于ICGM加上新穎的Kinect傳感器，以期實現比3D-PIRF更好的SLAM和機器人導航系統。
本方法實現機器人自動導航分為兩個步驟，學習階段和導航階段。SLAM即為學習階段。導航階段機器人利用SLAM的數據進行自動導航。
圖1是本研究的基本結構：
圖1右邊所示為SLAM和導航機器人的硬件，機器人上端安裝有一臺Kinect傳感器。左上為Kinect傳感器得到的環境的距離信息(暖色代表離傳感器距離近，冷色代表遠)和圖像信息。Kinect傳感器能夠同時獲取環境中的距離信息和圖像信息。左下為SLAM系統計算出的機器人的移動路徑。
2 ICGM簡介
ICGM(Incremental Center of Gravity Matching) 是一個通過圖像隊列之間特征點的幾何結構互相匹配而得到穩定特征點的方法。
其基本思路如圖2所示。假設Ia 和Ib 是在相同地點不同時刻拍攝的兩張照片。
圖中的(a,a'), (b,b'),(c,c'),(d,d'),(e,e')為已經被匹配上的特征點(SURF或SIFT)對。(a,a'), (b,b'), (c,c'), (e,e')之間的匹配是穩定的匹配，(d,d')是不穩定的匹配。換句話說(a,a'), (b,b'), (c,c'), (e,e')在到拍攝的時間段內，位置沒有大的變化，所以(a,a'), (b,b'), (c,c'),(e,e')是穩定的。而(d,d')的位置變化了，所以(d,d')之間的匹配是不穩定的。
以上所提到的穩定和不穩定的概念皆是時間尺度上的概念。假設Ia拍攝到Ib拍攝經過時間t，那么(d,d')在t內是不穩定的。
本研究利用ICGM方法用來排除不穩定的匹配。
RoG為重心向量的差的模和重心向量的模的和的比例。當式(2)成立時，一個特征點匹配對即被認為不穩定，反之則穩定。
實際的ICGM方法會首先隨機地提取初始N個匹配對，分別計算他們的重心。再用(1)計算初始N個點的RoG，用式(2)判斷它們穩定與否。當且僅當初始的N點都穩定時，才進入下一步計算。
得到N個初始的穩定的特征點后，通過它們的重心來測試剩下特征點匹配對的穩定度。假設如圖2，(a,b,c)和(a',b',c')是初始的3個穩定的匹配對。然后，(e,e')是被識別為穩定，ICGM會重新計算重心，用(a,b,c,e)和(a',b',c',e')的重心來計算剩下的匹配對的穩定度。因為(d,d')不穩定，所以(d,d')會被直接排除，重心也不會被重新計算。ICGM最后能夠排除所有不穩定的特征點，而留下穩定的特征點。
3 基于ICGM的SLAM和機器人導航
本研究在學習SLAM階段會構建一個混合地圖。導航階段使用混合地圖進行自動導航。
3.1 混合SLAM和混合地圖的構建(學習階段)
目前有很多SLAM都是只基于視覺信息的(vision-only)，但只基于視覺信息的SLAM只能夠獲取系統當前的大概位置。而對于機器人導航，精確的6自由度的全局姿態是必要。6自由度的全局姿態即機器人在地圖中的3維位置(x，y，z)和3維的機器人的全局的的角度姿態(roll，pitch，yaw)。為了獲取機器人6自由度的全局姿態，我們開發了一種基于vision-only SLAM的混合SLAM。利用混合SLAM我們能夠構建混合地圖。
本方法的混合地圖是基于可視里程器構建的。可視里程器通過計算3D點云間的6D剛性變換 [6] 實現。
一幀kinect數據先通過ICGM提取穩定特征點，然后通過kinect的距離數據算出穩定特征點的空間3D坐標，這些帶有空間3D坐標的穩定特征點生成一個3D點云。我們把這樣的一個點云叫做一個數據模型Mx 。我們能夠連續得到一個Mx 的集合M。
通過計算兩個Mx間的6D剛性變換，我們能夠得到他們之間的相對3D姿態變換R。計算R的過程中我們利用RANSAC [5] 來取得最佳模型以獲取最高精度。
如圖5所示，隨著學習路徑的變長，里程計的誤差會漸漸累計，導致很大的誤差。學習過程中我們必須控制誤差的累計。
本方法利用ICGM的vision-only SLAM [1] 檢測閉環。檢測閉環是指通過可視特征檢測當前的位置是不是一個之前已經來過的位置。
檢測出閉環后，利用vision-only SLAM取得3個最好的數據模型，Mbest，Msecond_best，Mthird_best。
3.2 基于混合地圖的自動導航(導航階段)
自動導航仍舊基于閉環檢測。步驟和學習階段類似，但是是基于學習階段的混合地圖實現定位。通常會把設置為1以達到快速的位置識別的響應。
機器人導航時同樣利用ICGM的vision-only SLAM檢測閉環，然后獲取最佳的和。
4 實驗
本實驗包括兩個階段，學習階段和導航階段。如圖4，這個實驗是在東京工業大學食堂進行的。食堂大小為20*20m。我們在晚上8點的時候檢測自己所提出的方法。這時，食堂大概有70人。他們在這個環境中自由地進餐或走動。這是一個高度動態的環境。
在學習階段，我們使用了游戲手柄控制機器人。機器人在移動過程中實時記錄里程計和kinect的圖像及距離信息。我們控制機器人繞了兩圈。在學習的終點，我們讓機器人回到學習的原點。總的移動的距離大概為80米。在整個過程中，機器人記錄了6739幀數據。
雖然環境是高度動態的，但由于ICGM特征點的穩定性，學習結果：kinect的可視里程計的成功率為97.6%，所以機器人路徑有97.6%由kinect的數據算出。閉環檢出率為63.8%。
圖5為機器人里程計計算出的路徑，顯然地，機器人里程計計算出的路徑誤差會漸漸累計，導致極大的誤差。圖5為通過本方法學習的路徑。由于環境較大，取得真實數據很難，所以較難做出精確的誤差評價。但因為本方法通過檢出閉環修正了路徑，誤差累計的問題顯然被較好地解決了。
表1為閉環檢出率的對比。相比3D-PIRF [3] ，本方法的閉環檢出率大大提高了。
在導航階段，我們選取了一段長度為12米的路徑作為規劃路徑。此路徑包含在學習路徑中。我們基于上一學習階段構建的混合地圖進行導航。導航階段每幀平均計算時間為321ms。機器人移動速度為125mm/s，用地板磚的分界線作為誤差評判基準，移動階段對于規劃路徑的平均偏移為60mm，最大偏移為92mm。
表2為本方法和3D-PIRF [3] 的導航結果的對比。請注意，本方法運用了GPU加速，而3D-PIRF沒有，所以計算速度的數據僅供參考。
5 總結
本研究提出了一個基于混合使用視覺特征和距離傳感器的SLAM和機器人導航方法，它能在高度動態的環境中穩定工作。實驗結果顯示，本方法的速度能夠達到實時處理的要求，相比前人研究的方法，這種方法能夠達到更高的精度，所提出的SLAM和導航系統更加接近實用要求。
在高度動態環境中工作是多種機器人必備的能力。如果機器人能利用一般的家用傳感器，就能大大降低機器人制造成本。微軟公司的Kinect是一種高速高精度且低成本的傳感器，它非常適合未來的商用機器人。另外，除了機器人，一些車輛也能使用Kinect實現自動導航，巨大的應用需求給本方法提供了廣闊的前景。
參考文獻
[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., andHasegawaO.. Position-invariant 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.. Vision-based 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.
