一種基于三維視覺的移動(dòng)機(jī)器人定位與建圖方法

陳超 張偉偉 徐軍

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

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

中圖分類號(hào)： TN95?34; TP242? ? ? ? ? ? ? ? ? ? 文獻(xiàn)標(biāo)識(shí)碼： A? ? ? ? ? ? ? ? ? ? ? 文章編號(hào)： 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（同步定位與建圖）主要指機(jī)器人在未知環(huán)境中，通過攜帶的傳感器獲取外界信息以完成對(duì)自身位姿的估計(jì)，并構(gòu)建出周圍環(huán)境的地圖[1]。其中，以相機(jī)進(jìn)行信息采集的方法稱為視覺SLAM[2]，該方法可實(shí)現(xiàn)在復(fù)雜環(huán)境下的三維地圖構(gòu)建。

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

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

1? 位姿初估計(jì)

1.1? 橢圓化MSER檢測

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

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

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

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

1.2? 稀疏直接法位姿估計(jì)

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

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

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

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

2? 相機(jī)標(biāo)定與點(diǎn)云生成

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

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

3? 位姿精估計(jì)

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

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

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

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

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

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

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

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

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

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

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

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

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

4.1? 閉環(huán)檢測

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

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

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

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

1） 空間最近鄰ICP匹配。

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

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

2） 稀疏直接法隨機(jī)檢測。

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

4.2? 圖優(yōu)化

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

5? 實(shí)驗(yàn)結(jié)果與分析

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

5.1? 標(biāo)準(zhǔn)數(shù)據(jù)集實(shí)驗(yàn)

5.1.1? ICP算法驗(yàn)證

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

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

5.1.2? SLAM算法驗(yàn)證

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

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

圖5為三種算法在fr1/xyz場景下的建圖結(jié)果，圖6為該場景下的相機(jī)運(yùn)動(dòng)軌跡。可以看出本文算法能準(zhǔn)確構(gòu)建出環(huán)境的三維地圖，同時(shí)對(duì)相機(jī)運(yùn)動(dòng)過程中的大部分軌跡做出了較準(zhǔn)確的估計(jì)，只有小部分相機(jī)運(yùn)動(dòng)過快的地方出現(xiàn)了誤差較大的情況。

5.2? 室內(nèi)環(huán)境實(shí)驗(yàn)

5.2.1? 點(diǎn)云配準(zhǔn)

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

5.2.2? 定位與建圖

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

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

6? 結(jié)? 論

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

參考文獻(xiàn)

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