陳超 張偉偉 徐軍

摘? 要： 針對移動機器人三維視覺SLAM（同步定位與建圖）中定位精度低、實時性差等問題，提出一種基于由初到精的位姿估計和雙重閉環(huán)策略的SLAM方法。首先對MSER（最大穩(wěn)定極值區(qū)）檢測算法進行橢圓擬合化處理并提取出圖像中的ROI（感興趣區(qū)）;然后從ROI中提取出稀疏像素點并使用直接法得到初始位姿變換參數(shù);接著結(jié)合改進的基于八叉樹結(jié)構(gòu)的ICP（迭代最近點）對相機位姿進行精估計;再結(jié)合關(guān)鍵幀選擇機制提出一種雙重閉環(huán)檢測方法為構(gòu)建的位姿圖添加約束;最后通過g2o圖優(yōu)化框架對位姿圖進行優(yōu)化并完成點云的拼接。通過NYU和TUM標準數(shù)據(jù)集驗證了算法的實時性與有效性，室內(nèi)實驗結(jié)果表明，在復(fù)雜環(huán)境下也能利用該方法進行準確的位姿估計，并構(gòu)建出環(huán)境的三維點云地圖。

關(guān)鍵詞： SLAM; 移動機器人; 三維視覺; 位姿估計; 閉環(huán)檢測; 點云拼接

中圖分類號： TN95?34; TP242? ? ? ? ? ? ? ? ? ? 文獻標識碼： A? ? ? ? ? ? ? ? ? ? ? 文章編號： 1004?373X（2020）06?0034?05

A method of mobile robot localization and mapping based on 3D vision

CHEN Chao， ZHANG Weiwei， XU Jun

（School of Mechanical Engineering， Jiangsu University of Science and Technology， Zhenjiang 212000， China）

Abstract： A SLAM method based on the initial to precise pose estimation and double closed?loop strategy is proposed to improve the positioning accuracy and real?time performance in the 3D vision SLAM （synchronous localization and mapping） of mobile robot. The ellipse fitting processing is performed for the MSER （maximally stable extremal region） detection algorithm and the ROI （region of interest） in the image is extracted， and then the sparse pixels are extracted from ROI and the initial pose transformation parameters are obtained by means of the direct method. The camera pose is accurately estimated in combination with the improved ICP （iterative nearest point） based on octree structure. A double closed?loop detection method is proposed in combination with the key frame selection mechanism to add constraints to the constructed pose map. By means of the g2o map optimization framework， the pose map is optimized and the splicing of point cloud is completed. The real?time performance and effectiveness of the algorithm is verified with the NYU and TUM standard data sets. The results of indoor experiments show that in a complex environment， this method can also be used to accurately estimate the pose and construct a 3D point cloud map of the environment.

Keywords： SLAM; mobile robot; 3D vision; pose estimation; closed?loop detection; point cloud splicing

0? 引? 言

SLAM（同步定位與建圖）主要指機器人在未知環(huán)境中，通過攜帶的傳感器獲取外界信息以完成對自身位姿的估計，并構(gòu)建出周圍環(huán)境的地圖[1]。其中，以相機進行信息采集的方法稱為視覺SLAM[2]，該方法可實現(xiàn)在復(fù)雜環(huán)境下的三維地圖構(gòu)建。

視覺SLAM大體分為兩種，即特征點法與直接法。Artal 等人開源的ORB?SLAM2算法采用多線程處理[3]，雖能構(gòu)建相對稠密的地圖，但算法的運行效率較低。相較之下，直接法通過對像素進行操作，能較好地應(yīng)對特征點缺失的情況。文獻[4]提出一種稀疏直接SLAM算法DSO，該算法通過優(yōu)化相機內(nèi)參、位姿和地圖幾何參數(shù)，可完成定位并構(gòu)建半稠密的地圖，但算法運行于單目相機且實時性較差。2010年誕生的Kinect，是微軟公司推出的一款能同時采集彩色圖像和深度數(shù)據(jù)的相機[5]，利用它機器人能獲取更加詳細的三維信息，以進行更加精確的定位和三維地圖構(gòu)建。文獻[6]通過校準、融合不同位置的Kinect傳感器的信息來進行三維地圖構(gòu)建，但由于數(shù)據(jù)的獲取與處理量過于龐大，使得系統(tǒng)的運行效率下降。KinectFusion通過點云配準進行位姿估計與三維地圖構(gòu)建[7]，但對硬件要求較高，需要通過配置GPU來實現(xiàn)，因此無法在一般平臺上實時運行。

針對上述問題，本文提出結(jié)合特征法與直接法對相機位姿進行初估計，并將初始參數(shù)代入改進的ICP進行進一步的精確估計，以此獲得最終的位姿軌跡。針對建圖過程中的累計誤差，結(jié)合關(guān)鍵幀策略提出一種雙重閉環(huán)檢測方法為構(gòu)建的位姿圖添加約束，并對位姿圖進行優(yōu)化，完成點云的拼接，從而得到全局一致的軌跡和地圖。最后，通過實驗對本文算法進行了驗證和分析。

1? 位姿初估計

1.1? 橢圓化MSER檢測

本文使用MSER算子[8]只對灰度圖中的ROI（感興趣區(qū)）進行提取。該算子是一種典型的ROI算子，描述的是圖像中與相鄰區(qū)域具有較高區(qū)分度的區(qū)域且具備良好的仿射不變性與抗噪性。由于MSER提取的區(qū)域具有不規(guī)則性，因此本文進行了橢圓擬合處理，公式為：

在灰度圖中，不規(guī)則區(qū)域內(nèi)點的坐標[（x，y）]用向量[X]表示，[R]表示MSER，[R]表示MSER中像素集合的勢。式（2）為式（1）降維后得到的協(xié)方差矩陣。

式中，[D（x）]，[D（y）]，[COV（x，y）]分別為MSER中點集的橫、縱坐標方差及兩者的協(xié)方差，式（2）所需各分量的均值由式（3）給出。

通過計算可得該協(xié)方差矩陣的特征向量和特征根，由此可確定擬合橢圓的長短軸方向和幅值，圖1中 [a1]和 [a2]分別為長軸和短軸的幅值，[α]代表長軸與 [x]的夾角。

1.2? 稀疏直接法位姿估計

由于MSER區(qū)域中像素灰度相對變化較小，因此本文提出選取橢圓化MSER區(qū)域中心點[（E（x），E（y））]處的像素，通過直接法跟蹤像素實現(xiàn)對幀間位姿的初步估計。直接法基于灰度不變假設(shè)，通過最小化兩像素的光度誤差來進行位姿優(yōu)化：

式中，[e]為標量誤差，誤差的二范數(shù)[minξJ（ξ）=e2]作為優(yōu)化的目標函數(shù)。對于[N]個空間點集[Pi]，優(yōu)化函數(shù)變?yōu)椋?/p>

此處需要知道[e]隨相機位姿[ξ]如何變化，通過使用李代數(shù)的擾動模型，得到誤差函數(shù)的雅克比矩陣：

接著利用高斯牛頓法計算增量，進行迭代求解，得到相機位姿變換矩陣記為[Tini]，并作為之后點云迭代配準的初值。

2? 相機標定與點云生成

采用文獻[9]中的標定方法對 Kinect進行標定，以此獲得相機的內(nèi)參數(shù)矩陣，完成校準后，圖像中任意點的三維坐標可根據(jù)對應(yīng)的深度信息得到，本文將提取出的MSER區(qū)域中所有像素點對應(yīng)到深度圖，以此得到三維點云數(shù)據(jù)。對于深度圖中的任意像素點[（xd，yd）]，其三維坐標計算如下：

式中：[cxd]和[cyd]為圖像的光心坐標;[fxd]和[fyd]為相機的焦距，由標定可得。

3? 位姿精估計

對于點云模型P和Q，經(jīng)典ICP算法通過構(gòu)建點對間的誤差函數(shù)并使用迭代方式計算兩模型的變換關(guān)系，但存在迭代耗時、配準誤差大等問題。本文結(jié)合八叉樹結(jié)構(gòu)對傳統(tǒng)ICP進行了改進，具體步驟如下：

1） 建立基于八叉樹模型的點云索引結(jié)構(gòu)。其基本思想是：構(gòu)建包圍點云數(shù)據(jù)的最小外接包圍盒，沿3個坐標軸方向?qū)Π鼑羞M行八等分。直到葉節(jié)點（子立方體）滿足規(guī)定的閾值;否則，需要對葉節(jié)點繼續(xù)八等分。剖分示意圖如圖2所示。

通過唯一對應(yīng)任意一節(jié)點與一八進制數(shù)完成對葉節(jié)點的編碼管理，即：

式中：[n]為節(jié)點深度;[qi]為八進制數(shù)， [q∈[0，7]，][i∈（0，1，2，…，i-1）]，[qi]表示該節(jié)點在兄弟節(jié)點中的編碼。這樣，從跟節(jié)點到葉節(jié)點的路徑即可由[q0]到[qi]得到。

2） 點的K鄰近快速搜索。點的K鄰近是指在點云中離該點最近的K個點，只要確定了根節(jié)點，根據(jù)點云中任意一點的坐標即可計算出其八叉樹編碼。點云中點所在立方體的編碼為：

根據(jù)周圍小立方體的空間坐標，由式（9）可計算出對應(yīng)的位置編碼，由位置編碼就可得到小立方體中對應(yīng)的點的集合。對于點云中的任意一點，搜索其所在葉節(jié)點中距離其最近的點集，并搜索周圍小立方體中的鄰近點集，通過這一方式對八叉樹中全部葉節(jié)點進行遍歷，最終得到點云中所有點的K鄰近。

3） 初始對準兩點云模型。由于構(gòu)建的模型過于理想化，因此需要一個迭代初值以避免算法陷入局部極值的情況，本文以直接法位姿粗估計得到的變換矩陣作為點到面ICP精配準的初值。目標誤差函數(shù)為：

式中：[TTk-1=Tk];[Skj]是[qki]處的切平面，[qkj=（Tk-1li）?Q]是直線[Tk-1li]與[Q]的交點，[li]是[P]中點[pi]處的向量;[pi]是點云[P]中的點;[d]表示點到平面的距離。對于[P]中的各點[pi]，求出其法向量[ni]，并令[T0=Tini]。

4） 通過四元數(shù)法可得到使目標函數(shù)最小的變換參數(shù)[T]。

5） 將[T]作用于待配準點云P，Q，求取轉(zhuǎn)換誤差，并判斷是否滿足閾值要求，若滿足，則迭代終止;否則，轉(zhuǎn)步驟2），繼續(xù)迭代。

6） 將滿足要求的變換參數(shù)[T]用于最終的點云配準。

改進的 ICP 算法流程如圖3所示，利用優(yōu)化后的變換參數(shù)[T]即可對相機位姿進行精確估計。

4? 閉環(huán)與優(yōu)化

4.1? 閉環(huán)檢測

為了保證閉環(huán)檢測與圖優(yōu)化結(jié)構(gòu)的稀疏性，必須要定義關(guān)鍵幀。本文采取如下方法進行關(guān)鍵幀判別：

1） 將相機采集的第一幀作為關(guān)鍵幀，設(shè)置一定的時間間隔限制。若當前幀獲取時間為[tc]，前一關(guān)鍵幀的獲取時間為[tp]，則要滿足[tc-tp≥εt]，令[εt=0.5 s]。

2） 用稀疏直接法估計當前幀和前一關(guān)鍵幀間相機運動，當運動量[D]大于閾值時，將該幀加入關(guān)鍵幀序列：

式中：[w1]為平移權(quán)重;[Δt]為幀間平移量;[w2]為旋轉(zhuǎn)權(quán)重;[α，β，λ]為幀間旋轉(zhuǎn)量。實驗中發(fā)現(xiàn)相機運動估計對旋轉(zhuǎn)比平移更為敏感，即當產(chǎn)生少量旋轉(zhuǎn)或者大量平移時，將新幀加入關(guān)鍵幀序列，令[w1=（0.6，0.7，0.7）]，[w2=1]。? ? ? ? ?為降低閉環(huán)檢測的計算量與判別誤差，本文提出一種雙重閉環(huán)檢測方法，具體步驟如下：

1） 空間最近鄰ICP匹配。

在以當前關(guān)鍵幀位置為中心，半徑為R=0.3 m的球內(nèi)檢測閉環(huán)，即將當前幀與球內(nèi)的其他幀進行ICP匹配。

式中：[Ikfk]為第[k]個關(guān)鍵幀;[Ii]為在第[k]和第[k+1]個關(guān)鍵幀間的數(shù)據(jù)序列;[ICP（I1，I2）]為位姿估計評價函數(shù)，若[I1]與[I2]配準成功，則[ICP（I1，I2）>0]，否則[ICP（I1，I2）≤0]。若匹配成功，則在兩關(guān)鍵幀間建立一條邊作為約束。

2） 稀疏直接法隨機檢測。

從已有關(guān)鍵幀中隨機抽取[n]幀，通過稀疏直接法估計它們與當前關(guān)鍵幀的變換矩陣，當幀間光度誤差小于閾值[eθ=0.6]時，則把該幀選取為候選閉環(huán)，然后將光度誤差最小的關(guān)鍵幀作為最終閉環(huán)，同時添加新的邊作為約束。

4.2? 圖優(yōu)化

本文采用g2o優(yōu)化庫[10]，對最終建立的位姿圖進行優(yōu)化。當優(yōu)化了整個相機運動的位姿圖后，將關(guān)鍵幀深度圖對應(yīng)的三維點云圖進行拼接，從而得到全局一致的軌跡與地圖。

5? 實驗結(jié)果與分析

為驗證本文算法的有效性，分別在NYU和TUM標準數(shù)據(jù)集以及實際室內(nèi)環(huán)境下進行了實驗。其中，室內(nèi)環(huán)境實驗以TurtleBot2移動機器人為實驗平臺，搭載ROS操作系統(tǒng)[11]，采用Kinect深度相機獲取數(shù)據(jù)，并使用筆記本電腦進行數(shù)據(jù)處理，如圖4所示。

5.1? 標準數(shù)據(jù)集實驗

5.1.1? ICP算法驗證

在NYUv2數(shù)據(jù)集上截取4段，分別為NYU_1，NYU_2，NYU_3，NYU_4，與經(jīng)典ICP算法進行對比實驗，結(jié)果如表1所示。

由表1知，經(jīng)典ICP平均配準時間為1.099 s，平均配準誤差為0.07 m;而改進的ICP平均配準時間為0.745 s，平均配準誤差為0.06 m。經(jīng)典算法耗時的主要原因是匹配點的搜索和迭代過程較慢;本文算法通過構(gòu)建八叉樹索引結(jié)構(gòu)加快了點的搜索，同時，迭代初值的引入降低了配準的誤差。

5.1.2? SLAM算法驗證

為分析本文算法的運行效果，選取了TUM/fr1的4個數(shù)據(jù)集，通過與KinectFusion[7]以及ORB?SLAM2[3]兩種算法在運行時間和軌跡RMSE（均方根誤差）的實驗，得到表2所示數(shù)據(jù)。

由表2計算得，本文算法的平均運行速度約為ORB?SLAM2的3倍，KinectFusion的1倍;軌跡誤差約為ORB?SLAM2的39%，略大于KinectFusion，可知該算法在快速運行時也能保證較小的軌跡誤差。

圖5為三種算法在fr1/xyz場景下的建圖結(jié)果，圖6為該場景下的相機運動軌跡?？梢钥闯霰疚乃惴軠蚀_構(gòu)建出環(huán)境的三維地圖，同時對相機運動過程中的大部分軌跡做出了較準確的估計，只有小部分相機運動過快的地方出現(xiàn)了誤差較大的情況。

5.2? 室內(nèi)環(huán)境實驗

5.2.1? 點云配準

圖7所示為在實際場景下本文ICP與經(jīng)典ICP的配準結(jié)果?？梢钥闯觯疚乃惴ㄔ谧罱K配準精度上相較于傳統(tǒng)算法有明顯提高。圖8為配準過程的誤差收斂速度曲線，由于本文算法采用基于樹的索引結(jié)構(gòu)，因此誤差收斂的更快;同時，引入迭代初值也使得最終的迭代次數(shù)更少，配準效率大大提高。

5.2.2? 定位與建圖

運用本文算法可在機器人移動過程中生成三維地圖以及運動軌跡。實驗室長約7 m，寬約5 m，使用羅技游戲手柄控制機器人以0.2 m/s的速度繞室內(nèi)移動一周左右以形成閉環(huán)，如圖9所示。

從圖中可以看出，在機器人運動的同時增加了當前位姿和之前位姿間的聯(lián)系即增加隨機閉環(huán)和近距離閉環(huán)，此方法可有效消除系統(tǒng)累計誤差，進一步提高定位精度。圖10為本文算法和另外兩種算法在機器人運動60 s時的軌跡誤差對比，由于本文采用了由粗到精的位姿估計以及混合閉環(huán)檢測方法，相較于ORB?SLAM2在軌跡精度上提高了約40%，與KinectFusion較為接近。圖11為機器人運動過程中構(gòu)建的實時三維點云地圖，可以看出環(huán)境的整體結(jié)構(gòu)以及局部小場景均得到較好的描述。

6? 結(jié)? 論

本文提出一種基于由初到精的位姿估計和雙重閉環(huán)策略的SLAM方法，可有效解決移動機器人三維視覺SLAM中定位精度低、實時性差等問題。實驗結(jié)果表明，改進的基于迭代初值和八叉樹結(jié)構(gòu)的ICP方法能有效降低點云配準誤差，提高配準效率，從而使得機器人的定位精度與運行效率大大提高。此外，結(jié)合提出的閉環(huán)檢測策略，本文算法可實時有效地構(gòu)建出環(huán)境的點云地圖。受限于Kinect相機的采集距離以及動態(tài)物體的存在，本文算法對室外大場景還不大適用，今后將通過多傳感器融合和算法優(yōu)化來實現(xiàn)對室外場景的地圖構(gòu)建。

參考文獻

[1] SHAMSUDIN A U， OHNO K， HAMADA R， et al. Consistent map building in petrochemical complexes for firefighter robots using SLAM based on GPS and LIDAR [J]. ROBOMECH journal， 2018， 5（1）： 7.

[2] EADE E， MUNICH M E， FONG P. Systems and methods for VSLAM optimization： US20120121161 A1 [P]. 2012?03?06.

[3] MUR?ARTAL R， TARD?S J D. ORB?SLAM2： an open?source SLAM system for monocular， stereo， and RGB?D cameras [J]. IEEE transactions on robotics， 2017， 33（5）： 1255?1262.

[4] ENGEL J， KOLTUN V， CREMERS D. Direct sparse odometry [J]. IEEE transactions on pattern analysis and machine intelligence， 2018， 40（3）： 611?625.

[5] PENG Yanfei， PENG Jianjun， LI Jiping， et al. Design and development of the fall detection system based on point cloud [J]. Procedia computer science， 2019， 147： 271?275.

[6] MENG X R， GAO W， HU Z Y. Dense RGB?D SLAM with multiple cameras [J]. Sensors， 2018， 18（7）： 2118?2130.

[7] TENG C H， CHUO K Y， HSIEH C Y. Reconstructing three?dimensional models of objects using a Kinect sensor [J]. Visual computer， 2018， 34（11）： 1507?1523.

[8] FAN Yihua， DENG Dexiang， YAN Jia. Natural scene text detection based on maximally stable extremal region in color space [J]. Journal of computer applications， 2018， 38（1）： 264?269.

[9] LACHAT E， MACHER H， MITTET M A， et al. First experiences with Kinect V2 sensor for close range 3D modelling [J]. International archives of photogrammetry， remote sensing and spatial information sciences， 2015， 40（5）： 93?100.

[10] JIA Songmin， LI Boyang， ZHANG Guoliang， et al. Improved KinectFusion based on graph?based optimization and large loop model [C]// 2016 IEEE International Conference on Information and Automation. Ningbo： IEEE， 2016： 813?818.

[11] KOUB?A A. Robot operating system [J]. Studies in computational intelligence， 2016， 1（6）： 342?348.