室內(nèi)環(huán)境下基于圖優(yōu)化的視覺慣性SLAM方法

徐曉蘇，代 維，楊 博，李 瑤，董 亞

（1. 微慣性儀表與先進導航技術教育部重點實驗室，南京 210096；2. 東南大學 儀器科學與工程學院，南京 210096）

室內(nèi)環(huán)境下基于圖優(yōu)化的視覺慣性SLAM方法

徐曉蘇1,2，代 維1,2，楊 博1,2，李 瑤1,2，董 亞1,2

（1. 微慣性儀表與先進導航技術教育部重點實驗室，南京 210096；2. 東南大學 儀器科學與工程學院，南京 210096）

基于圖優(yōu)化的即時定位與同步構圖（SLAM）方法是在視覺里程計運動估計的基礎上通過增加一個回環(huán)檢測，從而對非線性多約束進行優(yōu)化來提高定位精度。在視覺運動估計中，針對視覺特征點匹配錯誤率高的問題，提出了一種ORB特征點聚類抽樣匹配跟蹤的方法。在位姿圖優(yōu)化上，提出了一種改進型的回環(huán)檢測方法，減少了兩種錯誤匹配的可能性。最后將視覺SLAM與慣性導航進行組合，提高了系統(tǒng)的穩(wěn)定性和定位精度。使用公開的室內(nèi)SLAM測試數(shù)據(jù)集進行仿真，結果表明，該方法的定位均方根誤差在厘米級，生成的點云地圖清晰可見。

即時定位與同步構圖；圖優(yōu)化；聚類抽樣；回環(huán)檢測；組合定位

在陌生的環(huán)境中實現(xiàn)移動機器人的自主導航是機器人領域的熱點問題，SLAM被認為是實現(xiàn)這一技術的核心，最早由Stanford的Smith、Self等人在20世紀80年代提出[1]。SLAM的本質上是一個貝葉斯狀態(tài)估計的問題，可以表示為狀態(tài)方程以及觀測方程。因此傳統(tǒng)的視覺SLAM(Visual SLAM，vSLAM)方法多基于Kalman濾波[1]的方式利用自身運動狀態(tài)以及相機觀測環(huán)境中的路標進行定位。該方式當前位置僅與上一位置有關，即無法保證全局最優(yōu)。

隨著稀疏矩陣、非線性優(yōu)化等理論的發(fā)展，基于圖優(yōu)化的SLAM應運而生，其本質上是基于平滑的原理，在視覺里程計(Visual Odometry，VO)進行定位的同時建立位姿圖，并通過回環(huán)檢測建立位姿間的多約束，在后端通過非線性優(yōu)化算法來求解[2]。目前基于圖優(yōu)化的視覺SLAM在前端上存在以下問題[3]：一是視覺特征點匹配錯誤率高，導致視覺里程計估計精度下降，文獻[4]和文獻[5]通過對隨機抽樣一致性算法(RANSAC)進行預驗證、假設評價等方式進行改進；二是回環(huán)檢測方法不合理，導致位姿圖中出現(xiàn)錯誤邊約束，文獻[7]等采用分割式視覺字典的方式進行改進。

本文針對上述的問題，首先提出了一種快速特征（ORiented Brief，ORB）聚類抽樣的算法（Improved clustering Sample Consensus，ICSAC），并且做了全局最優(yōu)的驗證。其次，對視覺字典進行多尺度改進，并對關鍵幀進行回環(huán)檢測，降低了“假陰性”檢測(少檢測)的可能，在二次驗證時，采用ICSAC進行錯誤去除，避免了“假陽性”(多、錯檢測)的可能。針對視覺SLAM易受環(huán)境影響的缺點，以視覺慣性組合定位并基于視覺完成地圖的構建，從而建立視覺慣性的SLAM模型。

1 視覺里程計運動估計

視覺里程計通過提取圖像間的特征點進行匹配、跟蹤，從而完成對機器人三維運動的估計。在室內(nèi)環(huán)境下選用深度相機Kinect進行信息的采集。

1.1 圖像配準

Kinect上裝有RGB攝像頭和深度攝像頭，分別用來采集彩色圖像和深度圖像。由于安裝的位置不同，兩幅圖像在同一時間得到的圖像會有細微的差別，其中的差別可以用一組旋轉矩陣和平移向量表示，因此要實現(xiàn)圖像間的配準。以OpenNI+OpenCV為平臺，兩幅圖像的配準如圖1所示。

圖1 深度圖與RGB圖配準Fig.1 Registration of RGB and depth map

在配準后的圖像中，每個RGB圖上的像素點都對應其深度值。如圖2，記像素坐標系中的點(Se2)為(u, v, d)，三維世界坐標系中的點(Se3)坐標為(xd,yd,zd)，s為深度單位，d=zd·s，fu、fv分別是沿像素平面兩軸方向的焦距，f為像素歸一化參數(shù)，考慮到相機自身的畸變引入小角度θ進行修正，u0、v0是相機的光圈中心。稱為相機外參，可以通過標定來確定。由像素點對應空間點可以表示為：

圖2 像素與三維坐標對應關系Fig.2 Relationship of pixel and 3D-coordinate

1.2 運動估計

ORB特征方法是在2011年國際計算機視覺大會(International Conference of Computer Vision，ICCV)上提出的一種新型的方法[8]，其相對于傳統(tǒng)的SIFT，SURF最為顯著的特點就是速度快。ORB方法通過FAST特征提取和BREIF特征匹配來完成的，但是其缺乏旋轉不變性和尺度不變性。為了增加其旋轉特性，考慮特征點O的鄰域為S，在其鄰域內(nèi)計算其質心位置C，以特征點為起點，質心為終點構建向量。圖像特征點鄰域S內(nèi)的矩可以表示為

式中，I是像素點的灰度值，r表示鄰域半徑。

鄰域質心位置可以表示為

因此在BREIF特征描述時，對其鄰域內(nèi)的n個點按其主方向進行旋轉：

為了增加其尺度不變性，可以參考SIFT特征提取中圖像金字塔的方式，將一張圖像降采樣處理，對n維的尺度空間中的描述子分別匹配，則改進的描述子可以表示為

利用上述描述子，對兩幀圖像(F1, F2)匹配，得到對應的兩組特征點記為

由于環(huán)境中各項誤差的存在，將上述問題轉化為

ORB特征匹配的結果如圖3(a)所示，由于誤匹配較多，因此求解出來的無法用于兩幀間的運動估計。傳統(tǒng)的RANSAC方法可以進行部分錯誤去除，但是由于取樣點的隨機性，會產(chǎn)生效率低下、易退化等缺點。針對此，本文結合圖像的先驗信息提出一種ICSAC算法用于降低匹配錯誤率，具體如下：

步驟1 對匹配的兩幀圖像提取ORB特征，并基于空間一致性的SCRNMSAC濾波器[9]進行點集的預處理。

步驟2 基于圖像的光流主成分進行譜聚類，用S表示相似矩陣，構建譜矩陣，其中，sij為n點相似性分數(shù)，。對譜矩陣分解得到的k個特征向量聚類，其中k為聚類數(shù)，得到關于n個點集的超球面。選取點σ為圓心，r為半徑，其中r=Ix-Iy表示光流閾值均勻選取點集，從而構成抽樣的最小集。

步驟3 由于估計的內(nèi)點之間存在聯(lián)系，因此在較大的聚類點中含有內(nèi)點的可能性最大，因而估計的最小集樣本點均在較大類中選取。根據(jù)最小集估計兩幀之間的，建立模型，并用超球面邊緣的點對模型作預驗證，去除明顯不符合的模型。

步驟5 給定數(shù)值矩陣A，其行表示估計點約束的關系，通過觀測矩陣的秩rA使用本方法不斷迭代前次的內(nèi)點并增加此約束[10]，作為退化補償，并選取部分非聚類點進行模型驗證，避免由于聚類性導致的局部最優(yōu)，最終選取內(nèi)點最多的模型。

圖3(a)(b)(c)分別表在相同時間下同一圖片特征點經(jīng)過旋轉后的原始匹配、經(jīng)過RANSAC改進后的匹配，以及經(jīng)過ICSAC后的匹配。相比RANSAC，在同時間下的ICSAC算法匹配的特征點體現(xiàn)出聚類性及正確性。

經(jīng)過ICSAC去除后的特征點即可用于三維運動估計。由于像素點與三維坐標點的關系已經(jīng)由上文得到，因此通過像素坐標的旋轉、平移變換即可估計三維空間的運動。用表示機器人的實際視覺位姿信息，CI、SO分別表示相機內(nèi)外參數(shù)矩陣，則有：

求得旋轉矩陣R3×3即為機器人視覺姿態(tài)矩陣，即為機器人的位置。

圖3(a) 原始匹配Fig.3(a) Original matching

圖3(b) RANSAC改進匹配Fig.3(b) Matching improved by RANSAC

圖3(c) ICSAC改進匹配Fig.3(c) Matching improved by ICSAC

2 視覺SLAM優(yōu)化

2.1 回環(huán)檢測

位姿圖是指機器人的位置、姿態(tài)構成的拓撲圖[2]，其頂點表示位姿，邊表示約束關系，表示為，其中：，，qi為姿態(tài)四元數(shù)。

根據(jù)第1節(jié)建立的視覺里程計雖然可以進行定位，但是在長時間的運動中會產(chǎn)生累計誤差。如果機器人在運動中回到之前的某個位置，這樣的信息就可以用來優(yōu)化機器人的運動，表現(xiàn)在位姿圖上就是在原有的鏈式位姿圖上增加了新的邊約束。傳統(tǒng)的回環(huán)檢測主要通過建立“視覺字典”[11]來聚類，將描述子抽象為“視覺單詞”，根據(jù)相鄰幀的單詞進行分類查找然后匹配。但是由于匹配的錯誤，可能會產(chǎn)生“假性”檢測，且“假陽性”檢測對整個系統(tǒng)的危害更大。由于移動過程中圖像的尺度變換是最重要的變換，因此提出了一種多尺度視覺字典并將ICSAC算法應用于其中的回環(huán)檢測方法，從而降低兩種“假性”檢測的可能性，并且回環(huán)檢測針對關鍵幀，降低了位姿圖的冗余性。該方法具體描述為：

步驟1 初始化關鍵幀序列I*：加入數(shù)據(jù)集第一幀圖像I0，對于新幀Ij，利用ICSAC判斷：①是否可以匹配；②匹配內(nèi)點數(shù)小于閾值。若同時滿足則該幀稱為回環(huán)檢測關鍵幀，記得到的關鍵幀集合為

式中，?1、?2表示兩次判斷的閾值。

步驟3 根據(jù)每個葉子節(jié)點在整個視覺字典中的頻率高低定義其區(qū)分度：ω1,i=lg(M Mi)，其中，M表示字典中所有的圖像數(shù)，Mi表至少含有節(jié)點i中的一個描述子的圖像數(shù)。

步驟4 對于某幀圖像I*j，其中節(jié)點i出現(xiàn)的頻率定義為

2.2 非線性優(yōu)化

當回環(huán)檢測新建出的位姿圖的約束邊時，即邊Ti,j有誤差，因此可以優(yōu)化不一致性誤差：

其中，x*是x的估計值。在優(yōu)化過程中，根據(jù)前端給定初始值，通過目標函數(shù)對x的梯度：

3 視覺慣性組合定位

上述視覺SLAM定位較為精準，但是易受到環(huán)境特別是光照等影響，穩(wěn)定性有所不足。將上述視覺位姿信息與慣性進行組合定位，一方面可以在視覺失鎖的情況下保持系統(tǒng)的穩(wěn)定性，另一方面可以通過組合濾波的方式提高定位精度。以慣導東北天坐標系對應視覺XYZ坐標系建立組合系統(tǒng)，以表示視覺向慣性的轉化矩陣，則慣導與視覺的導航數(shù)據(jù)變換關系可以表示為：

假設系統(tǒng)滿足小失準角模型，即

視覺慣性組合系統(tǒng)狀態(tài)方程為：

視覺慣性組合系統(tǒng)的觀測方程為

4 實驗驗證分析

以視覺SLAM公開數(shù)據(jù)集TUM的freiburg1_xyz和freiburg1_desk為測試樣本，其內(nèi)容是室內(nèi)環(huán)境下1m3空間內(nèi)利用30Hz頻率深度相機采集的多幀彩色RGB圖像及對應的深度圖，并且該數(shù)據(jù)集中含有真實運動軌跡，是通過16個高精度相機以100Hz頻率從不同角度拍攝并計算得到，此公開數(shù)據(jù)集特別適合用來測定算法。為了避免幀間的冗余性，并且由于數(shù)據(jù)集大小不同，分別選100幀和200幀作為關鍵幀進行驗證，數(shù)據(jù)集中已經(jīng)做好了圖像的配準，仿真時可略去。分別采用傳統(tǒng)RANSAC的ORB特征提取匹配跟蹤方法和ICSAC的ORB方法，建立了視覺里程計模型，其視覺X方向定位誤差如圖4所示。

分析圖4可以得到以下結論：

1）兩種方法的視覺里程計模型的定位誤差均隨著時間不斷增加，即含有累計誤差，符合一般里程計的誤差模型。

2）使用ICSAC算法里程計誤差增長平穩(wěn)，說明該方法所用聚類性特征點估計具有一定的穩(wěn)定性。

3）在室內(nèi)環(huán)境下的兩個數(shù)據(jù)集使用ICSAC相較于RANSAC的定位精度提高約15%，說明了本文提出的ICSAC算法對特征點去除錯誤匹配的正確性。

圖4(a) fr1_xyz改進前后視覺里程計定位誤差Fig.4(a) Position error of visual odometerof fr1_xyz

圖4(b) fr1_desk改進前后視覺里程計定位誤差Fig.4(b) Position error of visual odometer of fr1_desk

以SLAM公開數(shù)據(jù)集TUM和NYU為測試樣本，以Ubuntu14.04為實驗平臺，基于改進的視覺回環(huán)檢測方法對位姿圖進行優(yōu)化，可以得到如圖5所示的回環(huán)檢測后的位姿圖。

優(yōu)化后的位姿圖呈現(xiàn)環(huán)狀，因此可以用于多約束目標函數(shù)非線性優(yōu)化。

以TUM數(shù)據(jù)集的freiburg1_xy、freiburg1_desk以及NYU數(shù)據(jù)集v2為例，慣性仿真選取的陀螺零漂0.006 (°)/h，隨機漂移0.006 (°)/h1/2，加表零偏50μg，分別給出三種方式（①未經(jīng)過回環(huán)檢測的視覺里程計與慣性組合（VO-INS）；②經(jīng)過回環(huán)檢測的視覺SLAM與慣導組合（vSLAM-INS）；③改進回環(huán)檢測下的視覺SLAM與慣性組合（I-vSLAM-INS））下的東向定位誤差，并構建了地圖（其中NYUv2數(shù)據(jù)集無真實軌跡），如圖6所示。

由上述定位誤差圖可知，在fr1_xyz中，I-vSLAMINS算法的單向定位最大誤差為1.035cm，單向定位均方根誤差(RMSE)為0.418cm，則三方向總體定位RMSE約為。fr1_desk中，I-vSLAM-INS算法的單向定位最大誤差為1.247cm，單向RMSE為0.554cm，則三方向總體定位RMSE約為。

由圖6(a)(b)(c)所示點云地圖可以看出，改進后的視覺回環(huán)檢測算法可以使人物、環(huán)境輪廓更加清楚。結合各類開源SLAM算法精度（ORB-SLAM，RGBD-SLAM）以及本文的各個算法可以得到TUM的5個不同數(shù)據(jù)集的定位RMSE，如表1所示。

圖5(a) TUM位姿圖檢測后Fig.5(a) TUM pose-graph after detection

圖5(b) NYU位姿圖檢測后Fig.5(b) NYU pose-graph after detection

圖6(a) fr1_xyz三種方式的構圖與定位誤差Fig.6(a) fr1_xyz mapping and positional error by 3 methods

圖6(b) fr1_desk三種方式的構圖與定位誤差Fig.6(b) fr1_desk mapping and positional error by 3 methods

圖6(c) NYUv2三種方式的構圖Fig.6(c) NYUv2 mapping by 3 methods

本文提出的在視覺里程計運動估計和視覺回環(huán)檢測中的兩個改進方法在表1中體現(xiàn)在：①利用ICSAC算法的視覺里程計定位精度相比RANSAC提高了約15%。②改進回環(huán)檢測的視覺SLAM相比于傳統(tǒng)方法精度提高10%～15%，相比于不回環(huán)檢測精度提高40%以上，說明了視覺回環(huán)檢測的重要性，同時說明改進方法的正確性。

本文最終建立的視覺慣性SLAM方法I-vSLAM-INS相比于一些開源項目的定位精度更高，并且可以在視覺受環(huán)境影響失鎖狀態(tài)下使得機器人能夠穩(wěn)定工作，具有一定的實用價值。

表1 各種算法的定位均方根誤差Tab.1 RMSE of various algorithms cm

5 結 論

本文針對SLAM視覺運動估計中特征點錯誤匹配的問題提出了一種ORB特征聚類抽樣估計的算法ICSAC，利用圖像的先驗知識加速算法并基于聚類性特征對運動進行估計從而提高了精度，建立了視覺里程計模型并加以驗證。針對回環(huán)檢測，以多尺度的視覺字典減少了“假陰性”匹配的機率，并基于ICSAC做幀間匹配減少了“假陽性”匹配機率，提高了回環(huán)檢測的準確性。針對視覺SLAM定位的不穩(wěn)定性，將視覺SLAM與慣性導航進行組合定位，最終建立了視覺慣性SLAM模型。

通過不同的SLAM公開數(shù)據(jù)集的仿真實驗，驗證了本文兩個改進方法的正確性。同時將本文建立的視覺慣性SLAM與已有開源SLAM項目對比，在定位精度和穩(wěn)定性上具有提升，說明了本文的研究具有一定的價值。本文在SLAM未來的研究方向上打算利用視覺慣性的緊組合系統(tǒng)進行定位[13]，在地圖創(chuàng)建上利用深度學習等方法建立環(huán)境語義地圖。

（References）：

[1] Smith R C, Cheeseman P. On the representation and estimation of spatial uncertainty[J]. International Journal of Robotics Research, 1986, 5(4): 56-68.

[2] Thrun S, Montemerlo M. The graph SLAM algorithm with applications to large-scale mapping of urban structures[J]. International Journal of Robotics Research, 2006,25(5-6): 403-429.

[3] 梁明杰, 閔華清, 羅榮華. 基于圖優(yōu)化的同時定位與地圖創(chuàng)建綜述[J]. 機器人, 2013, 35(4): 500-512.Liang Ming-jie, Min Hua-qing, Luo Rong-hua. Graphbased SLAM: A survey[J]. Robot, 2013, 35(4): 500-512.

[4] 羅文超, 劉國棟, 楊海燕. SIFT和改進的RANSAC算法在圖像配準中的應用[J]. 計算機工程應用, 2013,(15): 147-149, 156.Luo Wen-chao, Liu Guo-dong, Yang Hai-yan. Application of SIFT and advanced RANSAC algorithm on image registration[J]. Computer Engineering and Applications,2013, 49(15): 147-149, 156.

[5] 曲天偉, 安波, 陳桂蘭. 改進的RANSAC算法在圖像配準中的應用[J]. 計算應用, 2010(7): 1849-1851, 1872.Qu Tian-wei, An Bo, Chen Gui-lan. Application of improved RANSAC algorithm to image registration[J]. Computer application, 2010(7): 1849-1851, 1872.

[6] 劉春燕. 基于多源信息融合的行人航位推算室內(nèi)定位方法[J]. 中國慣性技術學報, 2016, 24(2): 208-214.Liu Chun-yan. Indoor localization method based on pedestrian dead reckoning aided by multi-source fusion[J]. Journal of Chinese Inertial Technology, 2016, 24(2): 208-214.

[7] Henry P, Krainin M, Herbst E, et al. RGB-D mapping:Using Kinect-style depth cameras for dense 3D modeling of indoor environments[J]. International Journal of Robotics Research, 2012, 31(5): 647-663.

[8] Rublee E, Rabaud V, Konolige K, et al. ORB: An efficient alternative to SIFT or SURF[C]//International Conference of Computer Vision. 2011: 2564-2571.

[9] Sattler T, Leibe B, Kobbelt L. SCRAMSAC: Improving RANSAC’s efficiency with a spatial consistency filter[C]//12th IEEE International Conference on Computer Vision.2015: 2090-2097.

[10] Raguram R, Chum O, Pollefeys M, et al. USAC: A universal framework for random sample consensus[J]. IEEE Transactions on Pattern Analysis & Machine Intelligence,2013, 35(8): 2022-2038.

[11] Liu Y, Cormack L K, Bovik A C. Statistical modeling of 3-D natural scenes with application to Bayesian stereosis[J]. IEEE Transactions on Image Processing, 2011,20(9): 2515-2530.

[12] Skibinski S, Weichert F. Parametric fusion of complex landmark observations present within the road network by utilizing bundle-adjustment-based full-SLAM[C]//IEEE Conference Publications. 2016: 917-926.

[13] 王云舒, 劉建業(yè), 曾慶化, 等. 結構光輔助的慣性/視覺室內(nèi)導航三維環(huán)境重構方法[J]. 中國慣性技術學報,2016, 24(1): 51-58.Wang Yun-shu, Liu Jian-ye, Zeng Qing-hua, et al. 3D environment restructure method with structured light for indoor vision/inertial navigation[J]. Journal of Chinese Inertial Technology, 2016, 24(1): 51-58.

Visual-aid inertial SLAM method based on graph optimization in indoor

XU Xiao-su1,2, DAI Wei1,2, YANG Bo1,2, LI Yao1,2, DONG Ya1,2
(1. Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology, Ministry of Education, Nanjing 210096, China; 2. School of Instrument Science and Engineering, Southeast University, Nanjing 210096, China)

To realize the accurate autonomous navigation of mobile robot in unfamiliar environment, a graph-based SLAM method based on the visual odometry is proposed, which adds loop detection into the motion estimation of visual odometry and thus optimizes the nonlinear constraints to improve the positioning accuracy. In the motion estimation of visual odometry, a cluster sampling matching track method based on ORB feature is be proposed to deal with the high mismatch rate. In the pose graph optimization, an improved loop detection method is proposed to reduce the mismatch possibility. Finally, the inertial navigation system is combined with the visual SLAM to further improve the steady and accuracy of the system. The simulations using open indoor SLAM test datasets show that the locating RMSE of the proposed method is in centimeter level, and the generated point cloud map is clearly visible.

simultaneous localization and mapping (SLAM); graph optimization; cluster sampling; loop detection; integrated location

U666.1

：A

1005-6734(2017)03-0313-07

10.13695/j.cnki.12-1222/o3.2017.03.007

2017-03-11；

：2017-05-20

國家自然科學基金項目（51175082，61473085）

徐曉蘇（1961—），男，博士生導師，從事測控技術與導航定位領域的研究。E-mail: xxs@seu.edu.cn