一種基于三維視覺的移動機器人定位與建圖方法

陳超 張偉偉 徐軍

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

關鍵詞： SLAM; 移動機器人; 三維視覺; 位姿估計; 閉環檢測; 點云拼接

中圖分類號： 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（同步定位與建圖）主要指機器人在未知環境中，通過攜帶的傳感器獲取外界信息以完成對自身位姿的估計，并構建出周圍環境的地圖[1]。其中，以相機進行信息采集的方法稱為視覺SLAM[2]，該方法可實現在復雜環境下的三維地圖構建。

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

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

1? 位姿初估計

1.1? 橢圓化MSER檢測

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

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

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

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

1.2? 稀疏直接法位姿估計

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

式中，[e]為標量誤差，誤差的二范數[minξJ（ξ）=e2]作為優化的目標函數。對于[N]個空間點集[Pi]，優化函數變為：

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

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

2? 相機標定與點云生成

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

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

3? 位姿精估計

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

1） 建立基于八叉樹模型的點云索引結構。其基本思想是：構建包圍點云數據的最小外接包圍盒，沿3個坐標軸方向對包圍盒進行八等分。直到葉節點（子立方體）滿足規定的閾值;否則，需要對葉節點繼續八等分。剖分示意圖如圖2所示。

通過唯一對應任意一節點與一八進制數完成對葉節點的編碼管理，即：

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

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

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

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

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

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

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

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

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

4? 閉環與優化

4.1? 閉環檢測

為了保證閉環檢測與圖優化結構的稀疏性，必須要定義關鍵幀。本文采取如下方法進行關鍵幀判別：

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

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

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

1） 空間最近鄰ICP匹配。

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

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

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

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

4.2? 圖優化

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

5? 實驗結果與分析

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

5.1? 標準數據集實驗

5.1.1? ICP算法驗證

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

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

5.1.2? SLAM算法驗證

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

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

圖5為三種算法在fr1/xyz場景下的建圖結果，圖6為該場景下的相機運動軌跡。可以看出本文算法能準確構建出環境的三維地圖，同時對相機運動過程中的大部分軌跡做出了較準確的估計，只有小部分相機運動過快的地方出現了誤差較大的情況。

5.2? 室內環境實驗

5.2.1? 點云配準

圖7所示為在實際場景下本文ICP與經典ICP的配準結果。可以看出，本文算法在最終配準精度上相較于傳統算法有明顯提高。圖8為配準過程的誤差收斂速度曲線，由于本文算法采用基于樹的索引結構，因此誤差收斂的更快;同時，引入迭代初值也使得最終的迭代次數更少，配準效率大大提高。

5.2.2? 定位與建圖

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

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

6? 結? 論

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

參考文獻

[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.