席志紅，王洪旭，韓雙全

（哈爾濱工程大學(xué)信息與通信工程學(xué)院，哈爾濱 150001）

（?通信作者電子郵箱504715799@qq.com）

0 引言

同步定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM）技術(shù)隨著智能家居機(jī)器人和特殊用途工業(yè)機(jī)器人的應(yīng)用成為當(dāng)前機(jī)器人技術(shù)領(lǐng)域的熱點(diǎn)研究之一。目前SLAM 技術(shù)已在家庭服務(wù)機(jī)器人、無人機(jī)及無人車等領(lǐng)域大顯身手，如：波士頓動(dòng)力公司研發(fā)的家庭服務(wù)機(jī)器人已經(jīng)可以實(shí)現(xiàn)簡單自主導(dǎo)航、開門、扔垃圾等日常任務(wù)；大疆公司生產(chǎn)的消費(fèi)級(jí)無人機(jī)已經(jīng)完全實(shí)現(xiàn)了視覺定位，視覺避障以及目標(biāo)跟蹤等核心技術(shù)。視覺SLAM（Visual SLAM，VSLAM）因主要采用視覺傳感器作為數(shù)據(jù)源而相比激光雷達(dá)等其他SLAM技術(shù)具有一定價(jià)格優(yōu)勢(shì)，近年來成為SLAM 研究領(lǐng)域的熱點(diǎn)方向之一。

VSLAM 技術(shù)根據(jù)對(duì)圖像信息的利用方式可以分為直接法和特征法。直接法是VSLAM研究的一重要方法，它在2011年才被提出，但是經(jīng)過幾年的迅速發(fā)展，它已經(jīng)達(dá)到了和特征點(diǎn)法同樣重要的地位。Engel 等［1］通過添加回環(huán)檢測(cè)和關(guān)鍵幀管理模塊來對(duì)半稠密視覺里程計(jì)進(jìn)行改進(jìn)，并在2014 年提出了LSD-SLAM（Large-Scale Direct monocular SLAM）系統(tǒng)，將直接法應(yīng)用到了半稠密的單目SLAM 中，并在2015 年提出了分別用于雙目相機(jī)和全景相機(jī)的LSD-SLAM［2］系統(tǒng)。該系統(tǒng)能夠構(gòu)建大尺度的，全局一致性的環(huán)境地圖，并能夠基于直接圖像配準(zhǔn)得到高度準(zhǔn)確的相機(jī)位姿。直接法中是耦合的，而在特征點(diǎn)法中則是解耦的。耦合能夠更整體性地處理數(shù)據(jù)關(guān)聯(lián)；而解耦能夠在位姿不確定的情況下，僅利用圖像信息去解數(shù)據(jù)關(guān)聯(lián)問題，所以直接法更擅長求解連續(xù)圖像的定位，而特征點(diǎn)法則更適合回環(huán)檢測(cè)與重定位［3］?；谔卣鼽c(diǎn)法的VSLAM 方法是VSLAM 研究另一個(gè)重要方向。2007 年，Klein等［4］提出了PTAM（Parallel Tracking And Mapping）系統(tǒng)，并實(shí)現(xiàn)了跟蹤與建圖過程的并行化。2015 年Mur-Artal 等［5］在完全繼承PTAM 系統(tǒng)基礎(chǔ)上提出ORB-SLAM2（Oriented FAST and Rotated BRIEF SLAM2）系統(tǒng)，該方法圍繞ORB（Oriented FAST and Rotated BRIEF）特征計(jì)算具有良好的旋轉(zhuǎn)和縮放不變性，提出了包括視覺里程計(jì)與回環(huán)檢測(cè)的詞袋字典模型，實(shí)現(xiàn)實(shí)時(shí)的回環(huán)檢測(cè)與高魯棒性的重定位，較好解決直接法的定位丟失問題。

圖1 本文SLAM系統(tǒng)框架Fig.1 SLAM system framework in this paper

ORB-SLAM2 系統(tǒng)在特征匹配時(shí)采用隨機(jī)抽樣一致（RANdom SAmple Consensus，RANSAC）算法［6］，該算法在圖像匹配上應(yīng)用廣泛。在二值化互信息篩選ORB 特征點(diǎn)得到的集合中選擇內(nèi)點(diǎn)，內(nèi)點(diǎn)比例高，采用RANSAC算法能快速找到最優(yōu)估計(jì)［7］?；贠RB特征進(jìn)行的RANSAC改進(jìn)算法來進(jìn)行圖像拼接，在已有特征點(diǎn)基礎(chǔ)上根據(jù)內(nèi)點(diǎn)計(jì)算變換矩陣H，有效節(jié)省匹配時(shí)間［8］。在RANSAC 算法基礎(chǔ)上改進(jìn)的漸進(jìn)一致采樣法（PROgressive SAmple Consensus，PROSAC）算法也得到廣泛應(yīng)用。將PROSAC 算法用于剔除圖形匹配異常值，在保證數(shù)據(jù)精度基礎(chǔ)上有效提高了采樣效率，節(jié)省計(jì)算時(shí)間［9］。但由于RANSAC 算法的隨機(jī)性特點(diǎn)不得不通過提高迭代次數(shù)來提高得到正確模型參數(shù)的概率，進(jìn)而導(dǎo)致了該算法的低效。本文在ORB-SLAM2 系統(tǒng)基礎(chǔ)上在特征匹配時(shí)采用PROSAC 算法替代RANSAC 算法［10］，PROSAC 算法是對(duì)經(jīng)典的RANSAC 算法的一種優(yōu)化［11］。相比經(jīng)典的RANSAC 方法均勻地從整個(gè)集合中采樣，PROSAC 方法是從不斷增大的最佳對(duì)應(yīng)點(diǎn)集合中進(jìn)行采樣的，所以這種方法可以節(jié)省計(jì)算量，提高運(yùn)行速度。另一方面，ORB-SLAM2系統(tǒng)只利用稀疏點(diǎn)云地圖，而稀疏點(diǎn)云地圖并不能在機(jī)器人導(dǎo)航中使用［12］，所以本文在原有方法基礎(chǔ)上添加稠密點(diǎn)云地圖［13］與八叉樹地圖［14-15］的建立線程，無論稠密點(diǎn)云地圖還是八叉樹地圖都可以用于機(jī)器人導(dǎo)航與路徑規(guī)劃。

通過利用TUM（Technische Universit?t München）數(shù)據(jù)集的實(shí)驗(yàn)結(jié)果顯示，本文方法與ORB-SLAM2系統(tǒng)對(duì)比，本文系統(tǒng)在均方根誤差（Root Mean Square Error，RMSE）僅相差0.003 2 m，在進(jìn)行誤匹配對(duì)比實(shí)驗(yàn)時(shí)，在CPU 為Intel i7，內(nèi)存為16 GB 平臺(tái)上，RANSAC 算法處理圖片速度14 fps（frames per second）左右，PROSAC算法處理速度26 fps左右，圖片處理速度是原來的185%。

1 本文SLAM系統(tǒng)

ORB-SLAM系統(tǒng)基于ORB特征實(shí)現(xiàn)，包含跟蹤定位、局部地圖構(gòu)建、回環(huán)檢測(cè)三個(gè)主線程和全局BA（Bundle Adjustment）優(yōu)化線程，在該系統(tǒng)基礎(chǔ)上，本文用PROSAC 算法替代原RANSAC算法，并添加建立稠密點(diǎn)云地圖和八叉樹地圖線程到ORB-SLAM2框架內(nèi)，本文系統(tǒng)的技術(shù)框架圖如圖1所示。

1.1 系統(tǒng)線程

跟蹤線程 這一部分主要工作是從RGB-D 圖像中提取ORB 特征，根據(jù)提取ORB 特征與上一幀進(jìn)行匹配或者通過全局重定位初始化位姿，并利用PROSAC 算法進(jìn)行誤匹配的剔除，然后跟蹤已經(jīng)重建的局部地圖優(yōu)化位姿，再確定當(dāng)前幀是否為新的關(guān)鍵幀。

建圖線程 這一部分主要完成局部地圖構(gòu)建。包括對(duì)關(guān)鍵幀的插入，驗(yàn)證最近生成的稀疏點(diǎn)云地圖點(diǎn)并進(jìn)行篩選，然后生成新的地圖點(diǎn)，使用局部BA（Local BA）優(yōu)化，最后再對(duì)插入的關(guān)鍵幀進(jìn)行篩選，去除多余的關(guān)鍵幀。

稠密點(diǎn)云地圖與八叉樹地圖構(gòu)建線程 對(duì)稀疏點(diǎn)云地圖的稀疏點(diǎn)進(jìn)行提取，根據(jù)稀疏點(diǎn)云進(jìn)行稠密點(diǎn)地圖的構(gòu)建，并在此基礎(chǔ)上構(gòu)建八叉樹地圖。在對(duì)關(guān)鍵幀進(jìn)行插入和刪除時(shí)也對(duì)建立的稠密點(diǎn)云地圖與八叉樹地圖進(jìn)行地圖占有概率更新。

閉環(huán)檢測(cè)線程 這一部分主要分為兩個(gè)過程，分別是閉環(huán)探測(cè)和閉環(huán)校正：閉環(huán)檢測(cè)先使用詞袋模型進(jìn)行探測(cè)，然后通過Sim3 算法計(jì)算相似變換；閉環(huán)校正主要是閉環(huán)融合和必要回環(huán)的圖優(yōu)化。

1.2 PROSAC算法原理

PROSAC 算法是對(duì)經(jīng)典的RANSAC 算法中采樣的一種優(yōu)化。相比經(jīng)典的RANSAC 方法均勻地從整個(gè)集合中采樣，PROSAC 方法是從不斷增大的最佳對(duì)應(yīng)點(diǎn)集合中進(jìn)行采樣，根據(jù)相似性進(jìn)行排序的半隨機(jī)采樣不會(huì)比完全隨機(jī)采樣的效果更差，所以這種方法可以在保持原有方法的魯棒性基礎(chǔ)上，獲得與采用RANSAC 方法的內(nèi)點(diǎn)基本相同的結(jié)果，并使計(jì)算速度更快。

PROCAS 方法的成立假設(shè)：具有更高相似性的數(shù)據(jù)點(diǎn)更可能是類內(nèi)點(diǎn)。

引入評(píng)價(jià)函數(shù)q(uj)值：N個(gè)數(shù)據(jù)點(diǎn)的集合記為uN，根據(jù)評(píng)價(jià)函數(shù)，uN內(nèi)數(shù)據(jù)點(diǎn)（如ui，uj）被降序滿足條件：

根據(jù)特征匹配點(diǎn)的歐氏距離的比值β，對(duì)評(píng)價(jià)函數(shù)q(uj)定量表示，表達(dá)式如下：

式中：dmin表示最小歐氏距離，dmin2表示次小歐氏距離。比值越小，表示特征點(diǎn)匹配質(zhì)量越好，即評(píng)價(jià)函數(shù)值越大；反之亦然。

將評(píng)價(jià)函數(shù)值最大的前n 個(gè)數(shù)據(jù)點(diǎn)集合記為un，并記集合M，M中元素個(gè)數(shù)為m。

由于PROSAC 算法不是從所有數(shù)據(jù)點(diǎn)中進(jìn)行隨機(jī)采樣，而是需要先對(duì)數(shù)據(jù)點(diǎn)進(jìn)行排序，然后在評(píng)價(jià)函數(shù)值較高的數(shù)據(jù)點(diǎn)子集中進(jìn)行隨機(jī)采樣，這個(gè)子集被稱為假設(shè)生成集，并且這個(gè)假設(shè)生成集的元素?cái)?shù)量是不斷增大的，所以PROSAC 算法需要確定生成數(shù)據(jù)集大小和采樣過程的停止策略。

1）確定數(shù)據(jù)集大小。

生長函數(shù)用來確定假設(shè)生成集的大小，它既不能過分依賴評(píng)價(jià)函數(shù)的預(yù)排序作用，也不能地對(duì)所有數(shù)據(jù)點(diǎn)做相同處理。所以用P(uj)表示數(shù)據(jù)點(diǎn)uj是正確匹配點(diǎn)的概率，對(duì)這個(gè)概率和評(píng)價(jià)函數(shù)之間的聯(lián)系做單調(diào)性假設(shè)：

用Tn表示中數(shù)據(jù)點(diǎn)僅來源于評(píng)價(jià)函數(shù)最好的前n個(gè)數(shù)據(jù)點(diǎn)集合un的平均數(shù)，則有：

最終它們之間的遞歸關(guān)系為：

Tn個(gè)采樣點(diǎn)集合只包含un的數(shù)據(jù)點(diǎn)，Tn+1個(gè)采樣點(diǎn)集合包含un+1的數(shù)據(jù)點(diǎn)。由于un+1=un∪{un+1}（un+1是一個(gè)數(shù)據(jù)點(diǎn)），所以Tn+1個(gè)采樣點(diǎn)集合包括1 個(gè)數(shù)據(jù)點(diǎn)un+1和m -1個(gè)來源于un的數(shù)據(jù)點(diǎn)。因此Tn+1采樣點(diǎn)集合是在un中隨機(jī)抽取m -1個(gè)采樣點(diǎn)，在此基礎(chǔ)上從m 開始到N -1隨機(jī)取值獲得數(shù)據(jù)點(diǎn)un+1，因此可以快速得到采樣點(diǎn)排序后集合

由于Tn值一般是非整數(shù)，所以令且有：

所以生長函數(shù)可以被定義為：

式中：n表示采樣次數(shù)大于等于t次時(shí)最小的n值。

2）采樣過程的停止策略。

如果集合un*中的類內(nèi)點(diǎn)數(shù)In*滿足下面條件，PROSAC 算法就停止運(yùn)算：

非隨機(jī)性 n*個(gè)數(shù)據(jù)點(diǎn)中的類內(nèi)點(diǎn)恰好也是任意一個(gè)錯(cuò)誤模型的類內(nèi)點(diǎn)的概率小于Ψ（一般為5%）；

極大性 存在一個(gè)解在un*中有比In*更多的內(nèi)點(diǎn)情況，并且在這種情況下k 次采樣后還未被發(fā)現(xiàn)的概率小于η0（一般小于5%）。

這兩個(gè)條件中非隨機(jī)性可以防止由類外點(diǎn)產(chǎn)生的模型也與算法相同。隨機(jī)類內(nèi)點(diǎn)集合的元素個(gè)數(shù)的分布是二項(xiàng)分布：

其中β 為與一個(gè)錯(cuò)誤模型相對(duì)應(yīng)的點(diǎn)不在采樣集合中的概率，并且該采樣點(diǎn)包含一個(gè)類外點(diǎn)。

在un*中發(fā)現(xiàn)一個(gè)非隨機(jī)的結(jié)果必須要滿足

極大性定義了需要多少次采樣才能保證解的置信度，這同樣也是RANSAC 算法唯一的停止策略。從un中采樣得到的m個(gè)點(diǎn)都來自類內(nèi)點(diǎn)的概率為：

其中：In是un中類內(nèi)點(diǎn)的數(shù)量，εn=In/n 表示類內(nèi)點(diǎn)數(shù)的比例。那么k次采樣也都是非類內(nèi)點(diǎn)的概率為(g(k) ≤n)：

給定概率閾值η0后，采樣次數(shù)需要滿足下列的條件：

算法流程如下所示。

輸入 最大迭代次數(shù)Im，判斷是否為內(nèi)點(diǎn)的誤差極限δ，設(shè)定內(nèi)點(diǎn)數(shù)目閾值T，判斷是否為內(nèi)點(diǎn)的誤差閾值ε；

輸出 單應(yīng)矩陣H。

1）計(jì)算特征點(diǎn)的最小歐氏距離dmin，求解歐氏距離比值β。

1）根據(jù)假設(shè)生成集g(t)確定n的值。

2）根據(jù)β衡量匹配質(zhì)量，根據(jù)匹配質(zhì)量選取前n個(gè)特征點(diǎn)進(jìn)行降序排列。

3）選取排序后4 個(gè)點(diǎn)為一組，計(jì)算每組匹配質(zhì)量之和，再根據(jù)質(zhì)量之和進(jìn)行排序。

4）在排序匹配對(duì)中選取匹配質(zhì)量最高的4 組匹配點(diǎn)，計(jì)算單應(yīng)矩陣H。

5）將上述4 組匹配點(diǎn)剔除，對(duì)其他特征點(diǎn)根據(jù)單應(yīng)矩陣計(jì)算相應(yīng)投影點(diǎn)。

6）計(jì)算其他點(diǎn)與投影點(diǎn)的誤差e，并與ε 進(jìn)行比較：如果e ＜ε，表示該點(diǎn)為內(nèi)點(diǎn)；反之為外點(diǎn)。

7）計(jì)算內(nèi)點(diǎn)數(shù)目t與內(nèi)點(diǎn)數(shù)目閾值T進(jìn)行比較：t ＞T更新內(nèi)點(diǎn)數(shù)目t；否則重復(fù)上述步驟，并對(duì)迭代次數(shù)加1。

8）用更新后t重新計(jì)算單應(yīng)矩陣H及新內(nèi)點(diǎn)。

9）若迭代次數(shù)不大于Im時(shí)滿足n ＜n*，返回H 及內(nèi)點(diǎn)集合；否則，找不到模型。

1.3 稠密點(diǎn)云地圖與八叉樹地圖的構(gòu)建

ORB-SLAM2系統(tǒng)只利用了稀疏點(diǎn)云地圖，所以本文在此基礎(chǔ)上，利用稀疏點(diǎn)云建立稠密點(diǎn)云地圖，并建立八叉樹地圖。

1）利用RGB-D圖像的信息來進(jìn)行稠密點(diǎn)云地圖的構(gòu)建。

通過定義點(diǎn)序列：X={x1，x2，…，xn}表示空間點(diǎn)的集合，其中xi=[x，y，z]表示點(diǎn)的三維坐標(biāo)，根據(jù)相機(jī)模型成像原理：

其中：C 是相機(jī)內(nèi)參，R 和t 是相機(jī)的旋轉(zhuǎn)矩陣和平移矩陣，s是深度值和實(shí)際空間距離的比例關(guān)系因子。

本文利用PCL（Point Cloud Library）軟件來處理由RGB-D圖像計(jì)算來的點(diǎn)云數(shù)據(jù)。點(diǎn)云數(shù)據(jù)點(diǎn)pi的pi.x，pi.y，pi.z可以根據(jù)深度信息計(jì)算：

其中：d 為深度圖像m 行n 列像素點(diǎn)的深度值，cx、cy、fx、fy為相機(jī)內(nèi)參。

根據(jù)ORB-SLAM2 系統(tǒng)選取的關(guān)鍵幀對(duì)應(yīng)的相機(jī)位姿與根據(jù)式（16）獲取的點(diǎn)云數(shù)據(jù)創(chuàng)建局部稠密點(diǎn)云地圖，對(duì)局部稠密點(diǎn)云地圖進(jìn)行點(diǎn)云拼接，得到全局稠密點(diǎn)云地圖。但點(diǎn)云地圖存在細(xì)節(jié)冗余導(dǎo)致其體積過大，所以在保留關(guān)鍵地圖的基礎(chǔ)上建立八叉樹地圖，可有效縮減地圖大小。

2）八叉樹地圖（octomap）是通過八叉樹結(jié)構(gòu)進(jìn)行信息存儲(chǔ)的地圖，常用于機(jī)器人導(dǎo)航等方向。

八叉樹的基本結(jié)構(gòu)如圖2 所示。八叉樹（Octree）是一種用于描述三維空間的樹狀數(shù)據(jù)結(jié)構(gòu)，八叉樹的每個(gè)節(jié)點(diǎn)表示一個(gè)正方體的體積元素，每個(gè)節(jié)點(diǎn)有8 個(gè)子節(jié)點(diǎn)，這8 個(gè)子節(jié)點(diǎn)所表示的體積元素加在一起就等于父節(jié)點(diǎn)的體積。一般將物體的中心點(diǎn)作為節(jié)點(diǎn)的分叉中心。八叉樹地圖基于八叉樹結(jié)構(gòu)對(duì)三維空間不斷切割，直到無法切割為止。八叉樹地圖的每個(gè)節(jié)點(diǎn)都保存了占有率，當(dāng)某個(gè)方塊的子節(jié)點(diǎn)都沒被占或完全被占時(shí)，則不用對(duì)該方塊繼續(xù)展開，方便數(shù)據(jù)的壓縮。

圖2 八叉樹原理Fig.2 Principle of octree

八叉樹地圖構(gòu)建具體原理如下所示：

假設(shè)1 表示節(jié)點(diǎn)被完全占據(jù)，0 表示節(jié)點(diǎn)是空白的，則節(jié)點(diǎn)的占據(jù)狀態(tài)可以用0～1 的浮點(diǎn)數(shù)x 來表示。為了防止x 超過0～1的范圍，可對(duì)x進(jìn)行對(duì)數(shù)變換，其公式如下所示：

這樣將y ∈(-∞，+∞)轉(zhuǎn)換到x ∈[0，1]。用轉(zhuǎn)換概率數(shù)值y來表示節(jié)點(diǎn)是否被占據(jù)，繼而通過逆變換即可得到概率x的值，0.5 表示未確定，越接近1 表示該節(jié)點(diǎn)被占據(jù)的可能性越高。占有概率更新公式如下：

其中：P(n)為節(jié)點(diǎn)的先驗(yàn)概率，P(n|zT)為第n 個(gè)節(jié)點(diǎn)基于T 時(shí)刻的后驗(yàn)概率為1 到T 時(shí)間段的觀測(cè)集合，P(n|z1：T)為第n 個(gè)節(jié)點(diǎn)在1 到T 時(shí)間段觀測(cè)集合的后驗(yàn)概率。通過式（17）對(duì)P(n)進(jìn)行對(duì)數(shù)變換，則式（19）可以轉(zhuǎn)化為：

通過上述公式，不斷對(duì)當(dāng)前的觀測(cè)數(shù)據(jù)進(jìn)行相關(guān)融合，實(shí)現(xiàn)節(jié)點(diǎn)的不斷更新，從而實(shí)現(xiàn)八叉樹地圖的構(gòu)建。

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

2.1 RANSAC算法與PROSAC算法的匹配對(duì)比實(shí)驗(yàn)

為驗(yàn)證基于ORB特征的圖片在采用RANSAC與PROSAC算法情況下誤匹配剔除的效果，本文設(shè)置兩組單張圖片對(duì)比實(shí)驗(yàn)。實(shí)驗(yàn)平臺(tái)：系統(tǒng)Ubuntu16.04；CPU Intel-i7；內(nèi)存16 GB，用Kinect2 相機(jī)采集實(shí)驗(yàn)室真實(shí)環(huán)境的RGD-B 圖片，將匹配結(jié)果用Opencv 軟件進(jìn)行效果顯示。實(shí)驗(yàn)共做6 組，結(jié)果相似，只選取其中一組實(shí)驗(yàn)結(jié)果展示如圖3。

圖3 兩種匹配算法效果對(duì)比Fig.3 Comparison of matching effects of two algorithms

從圖3中可以看出，基于ORB特征的直接匹配，匹配對(duì)較多，結(jié)果存在很多誤匹配，而基于ORB 特征的RANSAC 與PROSAC 算法進(jìn)行剔除誤匹配后的匹配對(duì)明顯減少。用圖3圖片的匹配時(shí)間與誤匹配剔除時(shí)間作為指標(biāo)對(duì)RANSAC算法與PROSAC 算法進(jìn)行比較，結(jié)果如表1 所示，兩種算法獲得內(nèi)點(diǎn)數(shù)基本相同，但采用PROSAC 算法無論匹配總時(shí)間與誤匹配剔除時(shí)間都大幅度減少，這在實(shí)時(shí)VSLAM 系統(tǒng)中有很大作用。

表1 RANSAC與PROSAC算法對(duì)相同圖片的匹配時(shí)間和誤匹配剔除時(shí)間Tab.1 Matching time and mismatch elimination time of RANSAC and PROSAC algorithms on the same image

2.2 選取RGB-D數(shù)據(jù)集與誤差分析分析標(biāo)準(zhǔn)

本文選用公開TUM 數(shù)據(jù)集中的RGB-D 相關(guān)數(shù)據(jù)在前文提到過平臺(tái)進(jìn)行實(shí)驗(yàn)。ORB-SLAM2 系統(tǒng)在相機(jī)快速運(yùn)動(dòng)時(shí)容易出現(xiàn)跟蹤丟失情況，即無法正確對(duì)相機(jī)位姿進(jìn)行估計(jì)與定位，所以引入回環(huán)檢測(cè)來減小累計(jì)誤差?；诖藛栴}，本文選取有回環(huán)和無回環(huán)的兩組RGB-D 數(shù)據(jù)進(jìn)行對(duì)比實(shí)驗(yàn)，分別是freiburg2_large_no_loop（下文簡稱fr2_np）和freiburg2_large_with_loop（下文簡稱fr2_wp）。fr2_np 是一組時(shí)長112.37 s，軌跡長度26.086 m序列；fr2_wp是一組時(shí)長173.19 s，軌跡長度39.111 m 序列。通常采用TUM 數(shù)據(jù)集進(jìn)行相關(guān)實(shí)驗(yàn)時(shí)，會(huì)采用以下兩個(gè)指標(biāo)作為系統(tǒng)誤差評(píng)判的標(biāo)準(zhǔn)。

1）絕對(duì)軌跡誤差（Absolute Trajectory Error，ATE）。直接計(jì)算相機(jī)位姿的真實(shí)值與SLAM 系統(tǒng)的估計(jì)值之間的差，代表軌跡的全局一致性，該標(biāo)準(zhǔn)非常適合于評(píng)估視覺SLAM 系統(tǒng)的性能。

2）相對(duì)位姿誤差（Relative Pose Error，RPE）。用于計(jì)算相同兩個(gè)時(shí)間戳上的位姿變化量的差。在用時(shí)間戳對(duì)齊之后，真實(shí)位姿和估計(jì)位姿均每隔一段相同時(shí)間計(jì)算位姿的變化量，然后對(duì)該變化量做差，以獲得相對(duì)位姿誤差，該標(biāo)準(zhǔn)適合于估計(jì)系統(tǒng)的漂移。

2.3 實(shí)驗(yàn)結(jié)果及誤差分析

對(duì)于絕對(duì)軌跡誤差與相對(duì)位姿誤差進(jìn)行分析，采用評(píng)價(jià)指標(biāo)是均方根誤差、平均誤差（Mean）、中值誤差（Median）和標(biāo)準(zhǔn)偏差（Standard Deviation，Std Dev），其中RMSE 反映估計(jì)值與真實(shí)值之間的偏差，平均誤差反映所有估計(jì)誤差的平均水平，中值誤差代表所有誤差的中等水平，標(biāo)準(zhǔn)偏差（Std Dev）反映系統(tǒng)軌跡估計(jì)的離散程度。通過以上幾種誤差分析可以綜合地反映系統(tǒng)估計(jì)的軌跡與真實(shí)軌跡值的誤差，并可以借此分析系統(tǒng)的穩(wěn)定性和可靠性。表2 是ORB-SLAM2 系統(tǒng)與本文系統(tǒng)在ATE 與RPE 分析上的對(duì)比。為進(jìn)一步分析在相對(duì)位姿誤差上ORB-SLAM2系統(tǒng)與本文系統(tǒng)的差別，本文將兩種方法的估計(jì)軌跡與實(shí)際軌跡進(jìn)行對(duì)比，并繪制相對(duì)位姿誤差圖如圖4和絕對(duì)軌跡誤差圖如圖5。

圖4 fr2_np與fr2_wp序列中ORB-SLAM2與本文系統(tǒng)的相對(duì)位姿誤差Fig.4 Relative pose error of ORB-SLAM2 and the proposed system for fr2_np and fr2_wp sequences

圖5 fr2_np與fr2_wp序列中ORB-SLAM2與本文系統(tǒng)的絕對(duì)軌跡誤差Fig.5 Absolute trajectory error of ORB-SLAM2 and the proposed system for fr2_np and fr2_wp sequences

可以看出本文系統(tǒng)在剔除誤匹配時(shí)將時(shí)間縮短為原ORB-SLAM2 系統(tǒng)的一半的基礎(chǔ)上，保持與ORB-SLAM2 基本一致的絕對(duì)軌跡誤差和相對(duì)位姿誤差。從表2 也可以得到，在RMSE 的比較上，本文系統(tǒng)與ORB-SLAM2系統(tǒng)的ATE 差值在0.01 m 以內(nèi)，平移漂移RPE 差值在0.02 m 以內(nèi)，旋轉(zhuǎn)漂移RPE差值也在0.5°內(nèi)，基本相同。

表2 ORB-SLAM2系統(tǒng)與本文系統(tǒng)絕對(duì)軌跡誤差與相對(duì)位姿誤差對(duì)比Tab.2 Comparison of absolute trajectory error and relative pose error between ORB-SLAM2 system and the proposed system

2.4 稠密點(diǎn)云地圖與八叉樹地圖

在ORB-SLAM2系統(tǒng)中構(gòu)建的是稀疏點(diǎn)云地圖，可以用于機(jī)器人定位，卻無法進(jìn)一步應(yīng)用于機(jī)器人導(dǎo)航和機(jī)器人路徑規(guī)劃。本文在ORB-SLAM2 系統(tǒng)基礎(chǔ)上基于RGB-D 圖像信息進(jìn)行稠密點(diǎn)云地圖構(gòu)建和八叉樹地圖構(gòu)建。在ORB-SLAM2系統(tǒng)中已將構(gòu)建了稀疏點(diǎn)云地圖并提取了關(guān)鍵幀，所以本文在此基礎(chǔ)上對(duì)稀疏點(diǎn)云進(jìn)行拼接，并基于關(guān)鍵幀的加入和剔除進(jìn)行了稠密點(diǎn)云地圖的局部加入與剔除，在不影響機(jī)器人定位的情況下成功建立了稠密點(diǎn)云地圖與八叉樹地圖，為進(jìn)一步在機(jī)器人領(lǐng)域應(yīng)用做出了一定貢獻(xiàn)。因?yàn)閒r2_np 與fr2_wp數(shù)據(jù)集是同一場(chǎng)景下的數(shù)據(jù)，所以建圖效果相同，下面只列出fr2_np的建圖結(jié)果。

圖6 fr2_np數(shù)據(jù)集稠密點(diǎn)云地圖和八叉樹地圖Fig.6 Dense point cloud map and octree map of fr2_np dataset

從圖6（a）可以看出，整個(gè)數(shù)據(jù)集的信息保留完整，其中桌子、椅子和墻面等細(xì)節(jié)可以清楚地看到，說明稠密點(diǎn)云地圖的構(gòu)圖效果表現(xiàn)良好，可以應(yīng)用于機(jī)器人的進(jìn)一步導(dǎo)航等途徑。但與圖6（b）比較發(fā)現(xiàn)，稠密點(diǎn)云地圖存在細(xì)節(jié)過多，和部分冗余的情況，而八叉樹地圖在保留基本信息的基礎(chǔ)上減少了冗余，縮小地圖體積，同樣表現(xiàn)良好。

3 結(jié)語

基于ORB 特征進(jìn)行軌跡估計(jì)與位姿估計(jì)時(shí)，快速有效的誤匹配剔除過程十分重要，為此本文在ORB-SLAM2系統(tǒng)的基礎(chǔ)上，采用PROSAC 算法替換原ORB-SALM2 框架中的RANSAC 算法進(jìn)行誤匹配的剔除，在保證與ORB-SLAM2系統(tǒng)絕對(duì)軌跡誤差和相對(duì)位姿誤差基本一致的情況下有效提高了誤匹配剔除速率。另外，本文針對(duì)ORB-SLAM2系統(tǒng)只有稀疏點(diǎn)云地圖的不足，添加了稠密點(diǎn)云的構(gòu)建與八叉樹地圖的構(gòu)建環(huán)節(jié)，可以直接用于機(jī)器人的導(dǎo)航與路徑規(guī)劃，這是稀疏點(diǎn)云地圖無法提供的。在下一步的工作中，將研究如何構(gòu)建語義地圖與語義地圖的定位。