基于RGB-D圖像的室內(nèi)機(jī)器人同時(shí)定位與地圖構(gòu)建

趙 宏，劉向東，楊永娟

（蘭州理工大學(xué)計(jì)算機(jī)與通信學(xué)院，蘭州 730050）

（?通信作者電子郵箱Liuxd1994@foxmail.com）

0 引言

2020 年的春天，一場(chǎng)席卷全球的COVID-19（Corona Virus Disease 2019）病毒使得人人自危。醫(yī)護(hù)人員每天和病患頻繁的接觸大大增加了交叉感染的幾率。一款具有自主導(dǎo)航功能的送藥、送餐醫(yī)療機(jī)器人能減少醫(yī)患人員之間接觸的頻率，降低醫(yī)護(hù)人員被感染的幾率。同時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）技術(shù)能夠讓機(jī)器人定位自己在未知環(huán)境中的具體位置，了解周?chē)h(huán)境的結(jié)構(gòu)，這些信息是機(jī)器人實(shí)現(xiàn)自主導(dǎo)航功能的基礎(chǔ)。自1986 年被提出之后，SLAM 技術(shù)一直是機(jī)器人領(lǐng)域的熱點(diǎn)研究問(wèn)題［1-3］。RGB-D（RGB-Depth）傳感器能有效避免單目和雙目相機(jī)恢復(fù)的三維點(diǎn)云尺度不確定與精度低的問(wèn)題?；赗GB-D傳感器的SLAM 方案，對(duì)室內(nèi)機(jī)器人自主導(dǎo)航具有重要的意義［4-5］。

基于特征點(diǎn)的視覺(jué)SLAM（Visual SLAM）［6］根據(jù)相機(jī)的種類(lèi)可以分為三類(lèi)：1）單目SLAM［7］。單目相機(jī)結(jié)構(gòu)簡(jiǎn)單、成本低廉，單目SLAM 通過(guò)相機(jī)的運(yùn)動(dòng)恢復(fù)自身的位置和周?chē)鷪?chǎng)景的結(jié)構(gòu)，但存在尺度不確定性的問(wèn)題，且恢復(fù)的是稀疏結(jié)構(gòu)。2）雙目SLAM［2］通過(guò)視差恢復(fù)場(chǎng)景的稠密結(jié)構(gòu)，但雙目相機(jī)恢復(fù)的空間三維點(diǎn)的坐標(biāo)誤差較大。3）RGB-D SLAM［8］則是借助RGB-D 相機(jī)［9］實(shí)現(xiàn)的，RGB-D 相機(jī)由彩色攝像頭和深度攝像頭組成，可以同時(shí)采集場(chǎng)景的彩色圖像（如圖1（a））和深度圖像（如圖1（b））。深度圖像是一種特殊的“灰度圖”，圖中每個(gè)像素的值代表相機(jī)和場(chǎng)景之間的距離。當(dāng)環(huán)境中不存在大量的紅外光源時(shí)，RGB-D 相機(jī)可以準(zhǔn)確地恢復(fù)出場(chǎng)景的三維點(diǎn)云（如圖1（c））。

圖1 RGB-D相機(jī)采集的圖像及生成的三維點(diǎn)云Fig.1 Images captured by RGB-D camera and generated 3D point cloud

Hentry 等［10］搭建了一套基于特征點(diǎn)的RGB-D SLAM，可以完成稠密點(diǎn)云的重建，但實(shí)時(shí)性不高。Endres［11］在迭代最近點(diǎn)（Iterative Closest Point，ICP）的基礎(chǔ)上提出了RGB-D SLAMv2 系統(tǒng)，建立了一套完整的RGB-D SLAM 系統(tǒng)，該系統(tǒng)提供了ORB（Oriented FAST and Rotated BRIEF）［12］、SIFT（Scale Invariant Feature Transform）［13］、SURF（Speeded Up Robust Features）［14］、SiftGPU［15］四種特征提取算法的接口，使用之前只需要在配置文件中進(jìn)行配置，并提供了ROS（Robot Operating System）［16］系統(tǒng)的接口，方便使用者進(jìn)行實(shí)地建模；但系統(tǒng)在運(yùn)行時(shí)有卡頓現(xiàn)象，且生成的點(diǎn)云文件占用空間存儲(chǔ)量過(guò)大，機(jī)器人在導(dǎo)航時(shí)難以應(yīng)用。文獻(xiàn)［17］中采用非線性優(yōu)化的方法求解相機(jī)姿態(tài)，準(zhǔn)確度較ICP 方法有所提升。文獻(xiàn)［18］中提出的ORB-SLAM2是ORB-SLAM［17］的升級(jí)版本，提供了單目、雙目、RGB-D 相機(jī)的接口，整個(gè)系統(tǒng)圍繞ORB 特征檢測(cè)算法建立，可以在CPU 上實(shí)時(shí)完成稀疏三維點(diǎn)云的重建，并使用三個(gè)線程完成SLAM。在目前基于特征點(diǎn)的SLAM方案中，ORB-SLAM2 擁有較高的精度和實(shí)時(shí)性，但ORBSLAM2 得到的是稀疏的點(diǎn)云地圖，無(wú)法滿足機(jī)器人導(dǎo)航和避障的需求。文獻(xiàn)［19］中在ORB-SLAM2的基礎(chǔ)上，增加了構(gòu)建稠密地圖的功能，并針對(duì)點(diǎn)云地圖難以用于機(jī)器人導(dǎo)航的問(wèn)題，提出了一種基于八叉樹(shù)的導(dǎo)航地圖構(gòu)建方法，完成機(jī)器人在未知空間的導(dǎo)航。

為彌補(bǔ)現(xiàn)有RGB-D SLAM 系統(tǒng)實(shí)時(shí)性差和精度低的問(wèn)題，本文搭建一套R(shí)GB-D SLAM系統(tǒng)，主要內(nèi)容如下：

1）對(duì)特征檢測(cè)算法進(jìn)行改進(jìn)，在ORB 特征的基礎(chǔ)上引入一種基于四叉樹(shù)的特征點(diǎn)均勻化策略，保證特征點(diǎn)的均勻分布，并在特征匹配階段結(jié)合詞袋（Bag of Words，BoW）模型［20］提升特征匹配的速度和精度；

2）為準(zhǔn)確求解相機(jī)姿態(tài)初始值，采用PnP（PerspectivenPoint）［21］+RANSAC（RANdom SAmple Consensus）［22］+非線性優(yōu)化的方法估計(jì)每幀圖像的相機(jī)姿態(tài)；

3）為避免誤差的累積，結(jié)合回環(huán)檢測(cè)和光束法平差（Bundle Adjustment，BA）［2］對(duì)相機(jī)姿態(tài)初始值進(jìn)行優(yōu)化，得到相機(jī)姿態(tài)的最優(yōu)值，并在此基礎(chǔ)上，根據(jù)點(diǎn)云數(shù)據(jù)和相機(jī)坐標(biāo)系的對(duì)應(yīng)性，將所有點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到同一個(gè)坐標(biāo)系中，得到場(chǎng)景的稠密點(diǎn)云地圖；

4）對(duì)稠密點(diǎn)云地圖進(jìn)行壓縮和轉(zhuǎn)換，得到用于機(jī)器人導(dǎo)航的八叉樹(shù)地圖［19］。

1 系統(tǒng)設(shè)計(jì)

本文系統(tǒng)實(shí)現(xiàn)主要分為：特征點(diǎn)檢測(cè)與匹配，相機(jī)姿態(tài)估計(jì)及優(yōu)化，回環(huán)檢測(cè)，地圖構(gòu)建等部分。本文系統(tǒng)整體框架如圖2所示。

圖2 本文系統(tǒng)框架Fig.2 Framework of proposed system

1.1 特征檢測(cè)與匹配

文獻(xiàn)［12］中提出的ORB 特征檢測(cè)算法由FAST（Features from Accelerated Segment Test）關(guān)鍵點(diǎn)和BRIEF（Binary Robust Independent Elementary Feature）描述子構(gòu)成。相較SIFT［13］和SURF［14］算法，ORB 具有更快的速度（三者的比較如表1 所示，檢測(cè)的特征點(diǎn)數(shù)量為1 000，數(shù)據(jù)來(lái)源于文獻(xiàn)［12］）。因此本文采用ORB算法檢測(cè)特征點(diǎn)。

表1 不同特征檢測(cè)算法時(shí)間對(duì)比Tab.1 Time comparison of different feature detection algorithm

ORB 特征檢測(cè)算法由四個(gè)部分構(gòu)成：1）圖像金字塔的構(gòu)建；2）FAST特征點(diǎn)的檢測(cè)；3）特征點(diǎn)旋轉(zhuǎn)角度的計(jì)算；4）特征點(diǎn)描述子的生成。本文在此基礎(chǔ)上對(duì)ORB 算法進(jìn)行改進(jìn)，引入一種基于四叉樹(shù)的特征點(diǎn)均勻分布策略，從而使ORB 提取的特征點(diǎn)分布更加均勻。特征點(diǎn)的均勻分布步驟如下：

步驟1 對(duì)輸入的圖像構(gòu)建圖像金字塔。

步驟2 在金字塔的每一層圖像中，按照大小為30×30的像素進(jìn)行分格處理。

步驟3 在每個(gè)格子中，提取FAST角點(diǎn)。

步驟4 對(duì)所有提取到的角點(diǎn)，采用四叉樹(shù)進(jìn)行劃分，初始節(jié)點(diǎn)為1。

步驟5 計(jì)算當(dāng)前節(jié)點(diǎn)的特征點(diǎn)數(shù)量，如果大于1，在當(dāng)前節(jié)點(diǎn)繼續(xù)按照四叉樹(shù)進(jìn)行劃分；否則，當(dāng)前節(jié)點(diǎn)停止劃分。

步驟6 計(jì)算節(jié)點(diǎn)的數(shù)量，如果小于設(shè)定的需求特征點(diǎn)數(shù)N，則繼續(xù)劃分；否則，停止所有劃分。

步驟7 遍歷滿足條件的節(jié)點(diǎn)，對(duì)每個(gè)節(jié)點(diǎn)中按照特征響應(yīng)值c排序，c的計(jì)算如式（1）所示，選取特征響應(yīng)值最大的特征點(diǎn)作為代表。

其中：IP是特征點(diǎn)對(duì)應(yīng)的灰度值；Ii是以特征點(diǎn)為圓心、半徑為3的圓上標(biāo)號(hào)為1～16的像素的灰度值。

如圖3 所示，OpenCV（Open source Computer Vision library）［23］實(shí)現(xiàn)的ORB 關(guān)鍵點(diǎn)有扎堆現(xiàn)象，本文方法實(shí)現(xiàn)的ORB關(guān)鍵點(diǎn)分布均勻。

圖3 不同方法實(shí)現(xiàn)的ORB特征分布結(jié)果Fig.3 Distribution results of ORB features achieved by different methods

特征點(diǎn)檢測(cè)與匹配的速度顯著影響系統(tǒng)的實(shí)時(shí)性。本文在使用改進(jìn)的ORB 算法檢測(cè)特征點(diǎn)的基礎(chǔ)上，在特征匹配階段，引入一種基于詞袋模型［20］的特征匹配方法。首先，對(duì)輸入的圖像使用ORB 算法提取特征。然后，運(yùn)用K-means++將ORB 算法提取的圖像描述子進(jìn)行聚類(lèi)，構(gòu)建k叉樹(shù)，生成圖像的詞典。最后，當(dāng)進(jìn)行特征匹配時(shí)，從k叉樹(shù)的根節(jié)點(diǎn)出發(fā)，查找特征對(duì)應(yīng)的單詞，將圖像特征轉(zhuǎn)換成單詞向量的描述。當(dāng)對(duì)應(yīng)的單詞向量一致時(shí)，對(duì)特征進(jìn)行匹配。在該過(guò)程中，有效減少了特征點(diǎn)匹配的范圍，因此特征匹配的準(zhǔn)確率得到提升。同時(shí)，k叉樹(shù)便于查找的特性，保證了在匹配時(shí)有較快的速度。詞袋模型的生成原理如圖4 所示，最終得到一棵深度為d（d=logkN，N為特征點(diǎn)的個(gè)數(shù)）的k叉樹(shù)。

圖4 詞袋模型的生成原理示意圖Fig.4 Generation principle schematic diagram of BoW

1.2 相機(jī)姿態(tài)的求解與優(yōu)化

視覺(jué)SLAM 將整個(gè)系統(tǒng)分為前端視覺(jué)里程計(jì)（Visual Odometry，VO）［2］和后端優(yōu)化兩個(gè)部分。后端對(duì)前端輸入的相機(jī)姿態(tài)進(jìn)行優(yōu)化，從而得到相機(jī)姿態(tài)的最優(yōu)值。因此，前端估計(jì)的相機(jī)姿態(tài)初始值對(duì)于整個(gè)系統(tǒng)的精度具有顯著的影響。本文采用PnP+RANSAC+非線性優(yōu)化的方法估計(jì)相機(jī)姿態(tài)的初始值。

PnP是一種相機(jī)位姿求解方法。P3P算法［24］（圖5）是PnP中的一種，只需四對(duì)3D-2D 匹配點(diǎn)，其中：一對(duì)匹配點(diǎn)作為驗(yàn)證點(diǎn)；其余三對(duì)匹配點(diǎn)中，2D 點(diǎn)a、b、c是3D 點(diǎn)A、B、C在相機(jī)成像平面上的投影。O表示相機(jī)的光心。

圖5 P3P問(wèn)題示意圖Fig.5 Schematic diagram of P3P problem

三角形之間的對(duì)應(yīng)關(guān)系如式（2）所示。

根據(jù)余弦定理，三角形之間的關(guān)系如式（3）所示。

經(jīng)過(guò)改進(jìn)的特征匹配關(guān)系中仍然可能存在誤匹配，誤匹配會(huì)造成P3P算法的失效，因此，本文采用RANSAC 模型來(lái)增強(qiáng)P3P算法的魯棒性。

其中：(XW，YW，ZW)表示空間點(diǎn)P在世界坐標(biāo)系下的三維坐標(biāo)；(Xc，Yc，Zc)表示P點(diǎn)在相機(jī)坐標(biāo)系下的三維坐標(biāo)；T表示變換矩陣，由旋轉(zhuǎn)矩陣R和平移向量t構(gòu)成；β表示距離閾值。運(yùn)用RANSAC算法的具體步驟如下：

步驟1 隨機(jī)抽取四對(duì)匹配點(diǎn)，作為初始的內(nèi)點(diǎn)集合，通過(guò)該內(nèi)點(diǎn)集合計(jì)算出變換矩陣T。

步驟2 依次判斷剩余的點(diǎn)，計(jì)算三維坐標(biāo)點(diǎn)之間的距離，如果距離小于設(shè)定的閾值（本文設(shè)定的閾值為4.0［2］），則將當(dāng)前點(diǎn)加入到內(nèi)點(diǎn)集合中。

步驟3 重復(fù)迭代步驟M（M的值為100［2］）次，選取內(nèi)點(diǎn)樣本數(shù)量最多的一組作為最終的匹配點(diǎn)集。

為對(duì)P3P 算法求解的相機(jī)姿態(tài)進(jìn)行優(yōu)化，將相機(jī)的姿態(tài)根據(jù)BA定義成一個(gè)最小二乘問(wèn)題，采用小孔相機(jī)模型描述成像原理。小孔相機(jī)模型的定義如下：

其中：(XW，YW，ZW)是空間點(diǎn)P在世界坐標(biāo)系中的坐標(biāo)；K是攝像機(jī)的內(nèi)參；R是旋轉(zhuǎn)矩陣；t是平移向量；(u，v)是P點(diǎn)投影在成像平面上的像素坐標(biāo)。

待優(yōu)化的誤差項(xiàng)e的計(jì)算如式（6）所示：

其中：P′是投影點(diǎn)P的真實(shí)像素坐標(biāo)；s是P點(diǎn)的深度；K是攝像機(jī)的內(nèi)參；ξ是相機(jī)姿態(tài)的李代數(shù)［2］；ξ∧表示李代數(shù)的反對(duì)稱矩陣。最小化誤差項(xiàng)ξ*的計(jì)算如式（7）所示。

使用列文伯格馬夸爾特（Levenberg Marquardt，LM）方法不斷調(diào)整ξ的值，最終得到與當(dāng)前匹配關(guān)系最優(yōu)的相機(jī)姿態(tài)。在LM 方法中，使用JTJ代替海塞矩陣（Hessian Matrix），避免計(jì)算量大的問(wèn)題。ξ∧左乘擾動(dòng)量δξ后，相機(jī)姿態(tài)的雅可比矩陣JC的計(jì)算方法如式（8）所示。

其中：fx和fy分別是攝像機(jī)在水平和豎直方向的焦距；(Xc，Yc，Zc)表示P點(diǎn)在相機(jī)坐標(biāo)系下的三維坐標(biāo)。

上述內(nèi)容介紹了系統(tǒng)前端相機(jī)姿態(tài)的求解方法。由于噪聲的存在，上一時(shí)刻的誤差會(huì)傳遞到下一時(shí)刻，隨著系統(tǒng)的運(yùn)行，誤差會(huì)不斷地積累，最終造成系統(tǒng)的崩潰。為減小誤差累積，系統(tǒng)在后端將前端輸入的所有相機(jī)姿態(tài)和三維點(diǎn)進(jìn)行更大范圍的優(yōu)化。后端局部BA 優(yōu)化的計(jì)算如式（5）所示，空間三維點(diǎn)的雅可比矩陣JP的計(jì)算方法如式（9）所示。

1.3 回環(huán)檢測(cè)

回環(huán)檢測(cè)的目的是讓機(jī)器人在運(yùn)動(dòng)的過(guò)程中識(shí)別出曾經(jīng)到過(guò)的地方，從而在采集的圖像數(shù)據(jù)中添加一個(gè)有間隔時(shí)間的約束，進(jìn)而減小誤差的積累，提高重建系統(tǒng)的精度。本文采用基于詞袋模型［20］的回環(huán)檢測(cè)方法，判斷圖像之間的相似性。該方法首先將圖像用詞袋模型轉(zhuǎn)換成單詞向量，然后通過(guò)對(duì)單詞向量的比較判斷兩幀圖像之間的相似性。兩幅圖像fA和fB之間的相似性計(jì)算如式（10）所示：

回環(huán)檢測(cè)的具體步驟如下：

步驟1 輸入圖像。

步驟2 計(jì)算特征點(diǎn)和描述子。

步驟3 從k叉樹(shù)的根節(jié)點(diǎn)出發(fā)，查找描述子對(duì)應(yīng)的單詞，生成單詞向量。

步驟4 和關(guān)鍵幀序列中的圖像進(jìn)行對(duì)比，計(jì)算圖像之間的相似性。

步驟5 如果滿足設(shè)定的閾值（本文設(shè)定的閾值為0.32），則將當(dāng)前幀設(shè)置為回環(huán)幀候選幀；否則，返回步驟1。

步驟6 計(jì)算當(dāng)前幀和回環(huán)候選幀的特征匹配數(shù)量（本文設(shè)定的閾值為45）。如果滿足閾值，則判定當(dāng)前幀和當(dāng)前回環(huán)候選幀產(chǎn)生回環(huán)；否則，返回步驟1。

步驟7 在當(dāng)前幀和回環(huán)幀中計(jì)算相機(jī)姿態(tài)，更新當(dāng)前幀的狀態(tài)。

1.4 導(dǎo)航地圖

受深度相機(jī)量程和視角的限制，使用傳感器每次僅僅能獲取一部分待重建的場(chǎng)景，因此需要通過(guò)不同角度來(lái)掃描，最終根據(jù)深度圖和相機(jī)坐標(biāo)系的對(duì)應(yīng)關(guān)系，使用優(yōu)化后的相機(jī)姿態(tài)Ri和ti將各個(gè)角度下的深度圖恢復(fù)的點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到同一個(gè)坐標(biāo)系中，得到場(chǎng)景的稠密點(diǎn)云地圖。

但稠密點(diǎn)云地圖的文件存儲(chǔ)量過(guò)大，包含了大量對(duì)于機(jī)器人導(dǎo)航無(wú)用的信息，且無(wú)法處理動(dòng)態(tài)的物體，因此難以應(yīng)用于機(jī)器人導(dǎo)航。八叉樹(shù)地圖是一種靈活的、壓縮的、能實(shí)時(shí)更新的地圖形式，相較點(diǎn)云地圖更有利于機(jī)器人的導(dǎo)航。八叉樹(shù)的生成原理如圖6所示。

圖6 八叉樹(shù)生成原理示意圖Fig.6 Generation principle schematic diagram of octree

八叉樹(shù)的每個(gè)葉子節(jié)點(diǎn)中存儲(chǔ)是否被點(diǎn)云占據(jù)的概率值，即表示機(jī)器人在某個(gè)地方能不能通過(guò)。對(duì)于第n個(gè)葉子節(jié)點(diǎn)，設(shè)系統(tǒng)在Ti狀態(tài)下觀測(cè)到的數(shù)據(jù)為zi，則第n個(gè)葉子節(jié)點(diǎn)存儲(chǔ)的概率值Poct為：

引入logit變換，有：

則式（11）變換為：

即第n個(gè)葉子節(jié)點(diǎn)的值為當(dāng)前觀測(cè)結(jié)果與上一次觀測(cè)結(jié)果的和。在系統(tǒng)運(yùn)行的過(guò)程中，根據(jù)觀測(cè)到某個(gè)節(jié)點(diǎn)被占據(jù)的情況，在初始值（Poct的初始值為0.5［19］）的基礎(chǔ)上不斷增加或減小Poct的值。父親節(jié)點(diǎn)的概率值為孩子節(jié)點(diǎn)的平均值，父親節(jié)點(diǎn)l(n)的計(jì)算如式（14）所示：

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

2.1 實(shí)驗(yàn)環(huán)境及數(shù)據(jù)集

為驗(yàn)證本文構(gòu)建的系統(tǒng)的有效性，使用TUM RGB-D數(shù)據(jù)集［25］，從系統(tǒng)運(yùn)行時(shí)間和絕對(duì)位姿誤差（Absolute Pose Error，APE）［25］兩個(gè)方面與現(xiàn)有的系統(tǒng)進(jìn)行對(duì)比實(shí)驗(yàn)。實(shí)驗(yàn)環(huán)境為：CPU 為Intel Core i5-U @42582.40 GHz；內(nèi)存為8 GB；系統(tǒng)為Windows10/Ubuntu16.04；語(yǔ)言為C/C++。選用的三個(gè)數(shù)據(jù)集序列分別是：fr1_desk2、fr1_xyz、fr1_room。

2.2 運(yùn)行時(shí)間對(duì)比

本文系統(tǒng)、ORB-SLAM2［18］、RGB-D SLAMv2［11］在運(yùn)行時(shí)間方面的對(duì)比結(jié)果如表2所示。為保證公平性，RGB-D SLAMv2中同樣使用ORB算法檢測(cè)特征點(diǎn)。

表2 不同系統(tǒng)運(yùn)行時(shí)間對(duì)比Tab.2 Running time comparison of different systems

由表2 可知，本文構(gòu)建的系統(tǒng)平均運(yùn)行時(shí)間約為ORBSLAM2［18］的1.25 倍，這是由于本文進(jìn)行稠密點(diǎn)云地圖重建造成的。ORB-SLAM2雖然快，但得到稀疏點(diǎn)云地圖不能應(yīng)用于機(jī)器人導(dǎo)航。另外，本文系統(tǒng)在RGB-D SLAMv2 的基礎(chǔ)上平均運(yùn)行時(shí)間降低了37.697%，這得益于基于詞袋模型的特征匹配方法具有較快的速度。最后，在運(yùn)行期間，場(chǎng)景的點(diǎn)云是不斷更新的，沒(méi)有卡頓現(xiàn)象，表明本文構(gòu)建的系統(tǒng)可以實(shí)時(shí)運(yùn)行。

2.3 系統(tǒng)精度對(duì)比

絕對(duì)位姿誤差（APE）描述的是每一幀相機(jī)的真實(shí)姿態(tài)和估計(jì)姿態(tài)之間的差值，可以直觀地反映系統(tǒng)的精度。對(duì)于第i幀圖像，APE的計(jì)算如式（15）所示：

其中：Qi是真實(shí)的相機(jī)姿態(tài)；Ti是系統(tǒng)估計(jì)的相機(jī)姿態(tài)；S是兩者之間的轉(zhuǎn)換矩陣。使用均方根誤差（Root Mean Square Error，RMSE）統(tǒng)計(jì)總體值，計(jì)算方法如式（16）所示：

本文系統(tǒng)、ORB-SLAM2［18］、RGB-D SLAMv2［11］在精度方面的對(duì)比結(jié)果如表3所示。

表3 不同系統(tǒng)APE對(duì)比 單位：mTab.3 APE comparison of different systems unit：m

由表3 可知，在fr1_desk2 和fr1_xyz 序列上，本文系統(tǒng)與ORB-SLAM2 精度幾乎相同。在fr1_room 序列上，本文系統(tǒng)的精度約為ORB-SLAM2 的57.89%。與RGB-D SLAMv2 的比較中，在三個(gè)序列上，本文系統(tǒng)實(shí)現(xiàn)的結(jié)果精度均優(yōu)于RGB-D SLAMv2，在RGB-D SLAMv2的基礎(chǔ)上平均提升了約56.104%。

三個(gè)圖像序列上，APE 的實(shí)時(shí)變化情況分別如圖7所示。

圖7 絕對(duì)位姿誤差Fig.7 Absolute pose error

三個(gè)系統(tǒng)和真實(shí)軌跡的對(duì)比結(jié)果分別如圖8 所示（其中g(shù)roundtruth是真實(shí)軌跡，proposed為本文系統(tǒng)）。

圖8 相機(jī)軌跡對(duì)比Fig.8 Comparison of camera trajectory

由圖8 可以看出，和真實(shí)軌跡的對(duì)比也印證了系統(tǒng)的精度，本文系統(tǒng)和ORB-SLAM2生成的軌跡和真實(shí)軌跡的貼合程度較高，而RGB-D SLAMv2生成的軌跡偏離程度較大。

2.4 八叉樹(shù)地圖

本文系統(tǒng)在fr1_room 序列上得到的稠密點(diǎn)云地圖如圖9所示，稠密點(diǎn)云地圖經(jīng)過(guò)八叉樹(shù)壓縮得到的3D 導(dǎo)航地圖（分辨率為0.05 m）如圖10 所示。雖然，本文選擇的系統(tǒng)精度衡量標(biāo)準(zhǔn)不能直接衡量生成地圖的質(zhì)量，但是相機(jī)坐標(biāo)系和單幀點(diǎn)云數(shù)據(jù)具有對(duì)應(yīng)關(guān)系，相機(jī)姿態(tài)的偏差程度直接決定了點(diǎn)云地圖的偏差程度。

圖9 稠密點(diǎn)云地圖Fig.9 Dense point cloud map

圖10 3D導(dǎo)航地圖Fig.10 3D navigation map

三個(gè)圖像序列上稠密點(diǎn)云地圖和八叉樹(shù)地圖文件的存儲(chǔ)量對(duì)比如表4所示。

表4 地圖文件存儲(chǔ)量對(duì)比Tab.4 Comparison of map file size

由表4 可知，經(jīng)過(guò)八叉樹(shù)壓縮后的稠密點(diǎn)云地圖的文件大小顯著減小，平均大小約為原來(lái)稠密點(diǎn)云地圖大小的3.217%。在機(jī)器人自主導(dǎo)航時(shí)，較小的地圖文件，便于機(jī)器人攜帶和加載。

3 結(jié)語(yǔ)

本文基于RGB-D圖像，根據(jù)視覺(jué)SLAM 的相關(guān)技術(shù)原理，建立了一套便于機(jī)器人導(dǎo)航的八叉樹(shù)地圖。在TUM RGB-D數(shù)據(jù)集上的實(shí)驗(yàn)結(jié)果表明，本文構(gòu)建的RGB-D SLAM 系統(tǒng)在運(yùn)行速度和精度上要優(yōu)于RGB-D SLAMv2［11］系統(tǒng)，在RGB-D SLAMv2 的基礎(chǔ)上，平均運(yùn)行速度降低約37.697%，平均精度提升約56.104%。與目前具有代表性的ORB-SLAM2［18］系統(tǒng)相比，ORB-SLAM2的平均運(yùn)行速度約為本文系統(tǒng)速度的1.25倍，這是本文系統(tǒng)進(jìn)行稠密點(diǎn)云地圖重建造成的。就系統(tǒng)的精度而言，在fr1_desk2 和fr1_xyz 序列上兩個(gè)系統(tǒng)幾乎相同。在fr1_room 序列上，本文構(gòu)建的RGB-D SLAM 系統(tǒng)精度約為ORB-SLAM2的57.89%。綜合而言，本文構(gòu)建的RGB-D SLAM系統(tǒng)具有較高的實(shí)時(shí)性和精度。另外，建立的基于八叉樹(shù)的導(dǎo)航地圖，平均約為稠密點(diǎn)云文件存儲(chǔ)量的3.217%，滿足了機(jī)器人對(duì)導(dǎo)航地圖的需要。在接下來(lái)的工作中將探索機(jī)器人導(dǎo)航時(shí)，如何在八叉樹(shù)地圖上進(jìn)行路徑規(guī)劃與避障。