黃丹丹，郝文豪，楊 陽(yáng)

（長(zhǎng)春理工大學(xué) 電子信息工程學(xué)院，長(zhǎng)春 130000）

隨著計(jì)算機(jī)視覺和機(jī)器人技術(shù)的迅猛發(fā)展，視覺同步定位與建圖（Simultaneous Localization and Mapping,SLAM）算法作為一種重要的技術(shù)手段，已經(jīng)在無(wú)人駕駛、AR/VR和三維重建等領(lǐng)域得到廣泛應(yīng)用[1,2]。前端視覺里程計(jì)和后端優(yōu)化是視覺SLAM中關(guān)鍵的研究?jī)?nèi)容，前端視覺里程計(jì)主要通過(guò)對(duì)連續(xù)幀之間的特征點(diǎn)進(jìn)行匹配和跟蹤，估計(jì)相機(jī)在三維空間中的運(yùn)動(dòng)；后端優(yōu)化通過(guò)構(gòu)建最小化重投影誤差，對(duì)相機(jī)位姿和地圖進(jìn)行全局一致性調(diào)整[3]。前端特征點(diǎn)匹配的準(zhǔn)確性為后端提供了優(yōu)化的基礎(chǔ)，而后端優(yōu)化又反過(guò)來(lái)優(yōu)化前端估計(jì)的相機(jī)運(yùn)動(dòng)和地圖，使得整個(gè)SLAM系統(tǒng)的精度更高、魯棒性更強(qiáng)，并在復(fù)雜場(chǎng)景下保持穩(wěn)定性[4]。

近年來(lái)，眾多學(xué)者在前端視覺里程計(jì)領(lǐng)域提出了基于特征點(diǎn)法的開源方案[5,6]，這些方案通常需要對(duì)初匹配的特征點(diǎn)對(duì)進(jìn)行誤匹配剔除。VINS-MONO[7]算法在計(jì)算本質(zhì)矩陣時(shí)使用隨機(jī)采樣一致性（Random Sample Consensus,RANSAC）算法剔除誤匹配特征點(diǎn)對(duì)；ORB-SLAM[8]則利用特征點(diǎn)旋轉(zhuǎn)主方向與RANSAC算法剔除誤匹配的特征點(diǎn)對(duì)；SuperPoint[9]設(shè)計(jì)了一種自監(jiān)督網(wǎng)絡(luò)框架，使用端到端的深度學(xué)習(xí)方法完成特征點(diǎn)匹配與誤匹配剔除?？偟膩?lái)說(shuō)，RANSAC算法是一種比較常用的誤匹配剔除方法，該算法是一種基于隨機(jī)采樣的算法，其結(jié)果可能對(duì)初始采樣點(diǎn)的選擇敏感，并且算法的結(jié)果依賴于參數(shù)的設(shè)置，如采樣數(shù)量、迭代次數(shù)等。因此，僅僅依賴RANSAC算法難以有效剔除誤匹配特征點(diǎn)對(duì)，需要進(jìn)一步提高特征點(diǎn)對(duì)的匹配精度來(lái)提高SLAM系統(tǒng)的準(zhǔn)確性。

在SLAM后端優(yōu)化方面，越來(lái)越多的學(xué)者引入新的特征來(lái)增強(qiáng)位姿約束[10,11]，以提高SLAM算法的魯棒性。Yang等人提出了CubeSLAM[12]算法，該算法引入物體特征，利用2D平面物體與3D物體模型構(gòu)建距離約束、角度約束以及形狀約束來(lái)優(yōu)化位姿；王立玲等人提出了基于點(diǎn)線特征視覺慣性融合的機(jī)器人SLAM算法[13]，該算法引入線特征與慣性約束來(lái)優(yōu)化位姿；晏小彬等人[14]提出了基于地平面的單目視覺輔助激光雷達(dá)SLAM系統(tǒng)，該算法使用激光雷達(dá)提取地面特征，并通過(guò)視覺與激光相輔相成優(yōu)化位姿。從以上分析可知，引入新的約束能使后端優(yōu)化結(jié)果更加收斂，防止陷入局部最優(yōu)，提高SLAM系統(tǒng)的魯棒性。

根據(jù)上述分析，本文從SLAM系統(tǒng)前端的特征點(diǎn)匹配以及后端的位姿優(yōu)化入手，提高算法對(duì)未知環(huán)境定位與建圖的準(zhǔn)確性與魯棒性。在視覺SLAM前端設(shè)計(jì)特征點(diǎn)對(duì)剔除策略，進(jìn)一步保證特征點(diǎn)匹配的準(zhǔn)確性，提高SLAM算法的精度；在后端利用RGBD（Red Green Blue Depth）相機(jī)獲取的深度數(shù)據(jù)提取地面向量信息，并將地面約束引入后端優(yōu)化，抑制解算結(jié)果在z軸上的偏移，提高優(yōu)化精度，從而增強(qiáng)系統(tǒng)的魯棒性。

1 算法框架

本文提出的SLAM算法利用RGBD相機(jī)獲取環(huán)境信息，并在ORB-SLAM2框架的基礎(chǔ)上進(jìn)行算法開發(fā)，整個(gè)算法的處理流程如圖1所示。算法主要由四個(gè)線程組成，分別是：特征點(diǎn)跟蹤線程、地面提取線程、局部建圖線程和回環(huán)線程，虛線部分為本文新增算法。其中，特征點(diǎn)跟蹤線程和地面提取線程屬于SLAM算法的前端部分，局部建圖線程和回環(huán)線程屬于SLAM算法的后端優(yōu)化部分。特征點(diǎn)跟蹤線程包括特征點(diǎn)的提取、匹配、誤匹配剔除以及計(jì)算相鄰幀之間的相機(jī)位姿變換；地面提取線程則負(fù)責(zé)提取地面點(diǎn)，并計(jì)算地面向量；局部建圖線程用于更新地圖點(diǎn)，并對(duì)前端位姿進(jìn)行優(yōu)化；回環(huán)線程則負(fù)責(zé)閉環(huán)檢測(cè)和閉環(huán)矯正等任務(wù)。

圖1 算法流程圖Fig.1 Algorithm flow chart

圖1中四個(gè)線程協(xié)同工作，使得本文算法能夠?qū)崿F(xiàn)對(duì)RGBD相機(jī)輸入的環(huán)境信息進(jìn)行實(shí)時(shí)定位與地圖構(gòu)建。特征點(diǎn)跟蹤線程和地面提取線程提供了前端的關(guān)鍵功能，通過(guò)特征點(diǎn)提取與匹配、誤匹配剔除和地面點(diǎn)提取等操作，為后端的局部建圖線程和回環(huán)線程提供準(zhǔn)確的輸入數(shù)據(jù)。局部建圖線程和回環(huán)線程通過(guò)地圖點(diǎn)的更新和位姿優(yōu)化提高定位精度和地圖的一致性。相對(duì)于ORB-SLAM2框架，本文對(duì)特征點(diǎn)跟蹤線程中的特征點(diǎn)剔除模塊進(jìn)行改進(jìn)以提高位姿估計(jì)的準(zhǔn)確性，并增加了地面提取線程以提高位姿優(yōu)化的魯棒性。

這種算法設(shè)計(jì)使得系統(tǒng)能夠充分利用RGBD相機(jī)的深度信息，從而提高對(duì)環(huán)境的感知能力。同時(shí)，通過(guò)前后端的協(xié)同優(yōu)化，可以實(shí)現(xiàn)較高精度和魯棒性的定位與建圖。

2 特征點(diǎn)對(duì)誤匹配剔除

ORB-SLAM2采用RANSAC算法剔除誤匹配點(diǎn)，對(duì)初值敏感且參數(shù)設(shè)置復(fù)雜。針對(duì)該問題，提出一種利用Boxplot四分位算法與直方圖統(tǒng)計(jì)算法進(jìn)行誤匹配特征點(diǎn)對(duì)剔除的方法，該方法能夠?qū)ζヅ涞奶卣鼽c(diǎn)對(duì)進(jìn)行評(píng)估，并剔除誤匹配特征點(diǎn)對(duì)，提高SLAM系統(tǒng)前端的準(zhǔn)確性。特征點(diǎn)對(duì)的表達(dá)式為：

其中，P為當(dāng)前幀的特征點(diǎn)集，與之相對(duì)應(yīng)匹配幀的特征點(diǎn)集為Q，點(diǎn)集元素pi和qi為匹配的特征點(diǎn)；(u1i,v1i)表示當(dāng)前幀第i個(gè)特征點(diǎn)的像素坐標(biāo)；(u2i,v2i)表示匹配幀第i個(gè)特征點(diǎn)的像素坐標(biāo)；N為匹配特征點(diǎn)的個(gè)數(shù)。

將特征點(diǎn)對(duì)的歐氏距離作為誤匹配剔除的預(yù)處理階段。特征點(diǎn)對(duì)的歐式距離集合L的表達(dá)式為：

采用統(tǒng)計(jì)學(xué)計(jì)算分析特征點(diǎn)對(duì)的分布特性。首先，使用Boxplot算法對(duì)預(yù)處理的歐氏距離集合進(jìn)行異常值檢測(cè)與剔除。Boxplot四分位算法是一種常用的統(tǒng)計(jì)學(xué)數(shù)據(jù)分析方法，通過(guò)四分位數(shù)描述數(shù)據(jù)的分布情況，由上下四分位距離確定異常值的存在。該方法可以評(píng)估特征點(diǎn)對(duì)的歐式距離是否存在異常值，進(jìn)而識(shí)別并剔除超過(guò)上下限的特征點(diǎn)對(duì)，減少誤匹配特征點(diǎn)對(duì)的干擾。對(duì)集合L中的元素從小到大進(jìn)行排序，得到新的集合記為L(zhǎng)*。設(shè)有序集合L*的第1/4個(gè)數(shù)為L(zhǎng)b1，第3/4個(gè)數(shù)為L(zhǎng)b2，則經(jīng)過(guò)Boxplot算法處理后的歐式距離集合L1為：

其中，IQR為四分位距。

其次，采用直方圖算法分析特征點(diǎn)對(duì)歐氏距離L1的數(shù)據(jù)分布情況。在特征點(diǎn)對(duì)匹配較好的情況下，所有特征點(diǎn)對(duì)的歐氏距離應(yīng)相對(duì)集中在一定范圍內(nèi)；當(dāng)出現(xiàn)誤匹配時(shí)，歐氏距離的變化范圍就會(huì)增大。為此，本文算法設(shè)定固定的直方圖組數(shù)，將特征點(diǎn)對(duì)的歐式距離集合數(shù)據(jù)放置于直方圖中，并計(jì)算每個(gè)區(qū)間內(nèi)數(shù)據(jù)點(diǎn)的數(shù)量或頻率。相對(duì)集中的數(shù)據(jù)被認(rèn)為匹配關(guān)系較好，分布于直方圖邊緣區(qū)間的數(shù)據(jù)被認(rèn)為匹配關(guān)系較差，并對(duì)直方圖按不同區(qū)間數(shù)據(jù)個(gè)數(shù)進(jìn)行排序。為了防止過(guò)多地剔除特征點(diǎn)對(duì)，本文算法提取前80%的數(shù)據(jù)作為正確的匹配點(diǎn)，剔除其余特征點(diǎn)對(duì)。

本文算法的優(yōu)點(diǎn)在于Boxplot算法和直方圖算法可以有效互補(bǔ)，其中直方圖算法能夠直觀地展示數(shù)據(jù)分布情況，當(dāng)異常數(shù)據(jù)較多時(shí)會(huì)由于頻數(shù)較高而將錯(cuò)誤的匹配對(duì)當(dāng)作正常值，而Boxplot算法能夠有效地彌補(bǔ)直方圖的缺陷從而更好地剔除異常值。通過(guò)綜合使用這兩種算法，能夠在SLAM算法的前端進(jìn)一步提高特征點(diǎn)匹配的質(zhì)量，有效地降低誤匹配對(duì)定位和建圖的影響。

3 地面向量提取與地面約束

3.1 地面向量描述與提取

地平面一般方程表示為：

其中，n為地面向量；G為地面上的三維空間點(diǎn)；d為平面方程的截距。

為此設(shè)地面參數(shù)為π=[nTd]T，設(shè)π(w)為世界坐標(biāo)系地面參數(shù)，π(c)為相機(jī)坐標(biāo)系地面參數(shù)，Rwc為相機(jī)坐標(biāo)系地面參數(shù)到世界坐標(biāo)系的旋轉(zhuǎn)矩陣，twc為相機(jī)坐標(biāo)系地面參數(shù)到世界坐標(biāo)系的平移向量，則可以用式(6)描述地面參數(shù)在不同坐標(biāo)系下的轉(zhuǎn)換關(guān)系：

根據(jù)針孔相機(jī)模型，3D坐標(biāo)點(diǎn)與像素點(diǎn)的轉(zhuǎn)換關(guān)系為：

其中，u、v為像素坐標(biāo)；fx、fy、cx、cy為相機(jī)內(nèi)參；X、Y、Z為該像素坐標(biāo)在相機(jī)坐標(biāo)系下的3D空間坐標(biāo)點(diǎn)云。

通過(guò)式(7)可以將RGBD相機(jī)采集的深度圖恢復(fù)成像素點(diǎn)在相機(jī)坐標(biāo)系下的3D坐標(biāo)點(diǎn)云。設(shè)相機(jī)坐標(biāo)系符合右手定則，坐標(biāo)系x方向向右，y方向向前，z方向向上。當(dāng)深度圖轉(zhuǎn)換為3D點(diǎn)后，會(huì)得到大量的點(diǎn)云數(shù)據(jù)，但是將所有的3D點(diǎn)進(jìn)行平面擬合可能會(huì)導(dǎo)致計(jì)算復(fù)雜度較高，特別是在相機(jī)分辨率較大的情況下。為了提高算法的計(jì)算效率和性能，本文算法僅選取z軸負(fù)方向上的3D點(diǎn)云用于地面向量擬合。

由于地面特征的法線真值必然位于垂直方向，為了得到更準(zhǔn)確的地面點(diǎn)云，根據(jù)每個(gè)點(diǎn)云的法向向量，剔除掉非垂直法線向量的點(diǎn)云，然而單純利用一個(gè)點(diǎn)云數(shù)據(jù)無(wú)法得出該點(diǎn)云的法線向量。為此，本文采用最近鄰搜索算法搜索該點(diǎn)云附近的點(diǎn)云作為點(diǎn)云簇，構(gòu)建點(diǎn)云簇與待求方向向量的約束方程。

其中，k個(gè)點(diǎn)組成的點(diǎn)云簇記為M={mi|i=1,2 …k}；c為點(diǎn)云簇的中心點(diǎn)；n*為待求點(diǎn)云單位法向量；n為當(dāng)前估計(jì)的點(diǎn)云法向量；Ω為向量歸一化函數(shù)。

式(8)表示點(diǎn)云簇里每個(gè)點(diǎn)與中心點(diǎn)所構(gòu)成的向量與當(dāng)前估計(jì)方向向量的點(diǎn)乘之和最小即為所求的方向向量。

隨后對(duì)剔除掉非垂直法線向量的點(diǎn)云進(jìn)行平面擬合，為了提高擬合平面的魯棒性，基于RANSAC算法，從地面垂直法向點(diǎn)云數(shù)據(jù)G*={gi|i=1,2…

k1}中隨機(jī)選取三個(gè)點(diǎn)進(jìn)行平面擬合，擬合方程為：

通過(guò)構(gòu)建最小化點(diǎn)到擬合平面的距離作為平面擬合的代價(jià)函數(shù)，代價(jià)函數(shù)為：

通過(guò)算法多次迭代選點(diǎn)，取代價(jià)函數(shù)最小時(shí)的平面方程作為最終擬合的平面。

3.2 加入地面約束的后端優(yōu)化

后端的作用是對(duì)前端視覺里程計(jì)得到的位姿進(jìn)行優(yōu)化，優(yōu)化的過(guò)程主要依靠地圖點(diǎn)約束和地面向量約束。優(yōu)化因子圖如圖2所示。

圖2 優(yōu)化因子圖Fig.2 Optimization factor graph

構(gòu)建地圖點(diǎn)約束時(shí)，采用ORB-SLAM2進(jìn)行地圖點(diǎn)的生成與剔除，利用每個(gè)特征點(diǎn)的像素坐標(biāo)與其相對(duì)應(yīng)的地圖點(diǎn)坐標(biāo)構(gòu)建觀測(cè)方程：

其中，γ為像素坐標(biāo)，即γ=(u,v)；Tcw為待優(yōu)化位姿；V為像素坐標(biāo)對(duì)應(yīng)的空間坐標(biāo)；h函數(shù)為世界坐標(biāo)系到像素坐標(biāo)系的轉(zhuǎn)換方程，即

其中，S為地圖點(diǎn)深度值；K為相機(jī)內(nèi)參矩陣。

為此，一個(gè)特征點(diǎn)的像素點(diǎn)與其地圖點(diǎn)構(gòu)成一個(gè)觀測(cè)方程，設(shè)共有j對(duì)特征點(diǎn)與其地圖點(diǎn)相匹配，則能夠提供j個(gè)觀測(cè)方程，組成一個(gè)超定方程。

采用最小二乘求解超定方程(13)的解，T*表示最小二乘的解。

當(dāng)?shù)孛嫫秸麜r(shí)，由于地面的平面法向量具有固定的方向和高度信息，后端可以使用地面向量約束相鄰幀之間的相機(jī)運(yùn)動(dòng)，從而提高相機(jī)位姿的估計(jì)精度。構(gòu)建地面向量約束時(shí)，利用當(dāng)前幀采集的地面信息與實(shí)際地面信息建立角度誤差與距離誤差方程。

其中，α為角度因子；β為距離因子；nw和dw為實(shí)際地面的平面參數(shù)；nc和dc為當(dāng)前幀擬合地面的平面參數(shù)。

后端優(yōu)化總的約束方程為：

4 實(shí)驗(yàn)分析

4.1 實(shí)驗(yàn)環(huán)境

本文實(shí)驗(yàn)使用的硬件環(huán)境為搭載Intel i9-12900KF處理器的計(jì)算平臺(tái)，軟件環(huán)境為Ubuntu 20.04操作系統(tǒng)，并使用C++14編程語(yǔ)言在ORB-SLAM2框架下進(jìn)行算法代碼開發(fā)，平臺(tái)沒有使用GPU加速。本文選用TUM RGB-D數(shù)據(jù)集[15]與CollaborativeSLAM數(shù)據(jù)集[16]作為輸入數(shù)據(jù)，TUM RGB-D數(shù)據(jù)集是一個(gè)多功能的RGBD數(shù)據(jù)集，包含多個(gè)室內(nèi)和室外場(chǎng)景，涵蓋靜態(tài)和動(dòng)態(tài)場(chǎng)景，提供了豐富多樣的環(huán)境和運(yùn)動(dòng)情況；CollaborativeSLAM數(shù)據(jù)集提供了大范圍場(chǎng)景下的RGBD數(shù)據(jù)序列，這兩個(gè)數(shù)據(jù)集均提供了準(zhǔn)確的真值信息和同步采集的RGB圖像和深度圖像，并附帶精確的時(shí)間戳信息。在實(shí)際環(huán)境中，利用Orbbec RGBD相機(jī)進(jìn)行數(shù)據(jù)采集，驗(yàn)證本文提出的算法。

4.2 特征點(diǎn)對(duì)誤匹配剔除算法的性能測(cè)試

為了測(cè)試誤匹配剔除功能的魯棒性，本文分別在靜態(tài)和動(dòng)態(tài)環(huán)境下評(píng)估算法的性能。選取了TUM數(shù)據(jù)集中的靜態(tài)子序列：fr2_360_hemisphere、fr2_360_kidnap、fr2_desk、fr2_large_with_loop 以及TUM數(shù)據(jù)集中的動(dòng)態(tài)子序列：fr3_walking_halfsphere。這些數(shù)據(jù)集子序列均提供了真實(shí)軌跡作為參考，以確保評(píng)估的準(zhǔn)確性。在評(píng)估算法性能時(shí)，本文采用了絕對(duì)軌跡誤差（Absolute Trajectory Error,ATE）和相對(duì)位姿誤差（Relative Pose Error,RPE）作為評(píng)價(jià)指標(biāo)。ATE衡量了估計(jì)軌跡與真實(shí)軌跡之間的整體誤差，計(jì)算公式為：

其中，Nf為幀數(shù)；Tgt,i和Test,i分別表示第i幀的真實(shí)軌跡與估計(jì)軌跡位姿；?表示反對(duì)稱矩陣到向量的映射。

RPE則關(guān)注相鄰幀之間的相對(duì)位姿誤差，計(jì)算公式為：

測(cè)試時(shí)，將原始ORB-SLAM2算法計(jì)算的特征點(diǎn)對(duì)與加入誤匹配機(jī)制后剔除的特征點(diǎn)對(duì)進(jìn)行圖像化顯示。原始ORB-SLAM2算法與增加誤匹配特征點(diǎn)對(duì)剔除機(jī)制的本文算法在靜態(tài)環(huán)境下的對(duì)比效果如圖3所示，性能對(duì)比如表1所示；動(dòng)態(tài)環(huán)境下兩種算法的對(duì)比效果如圖4所示，性能對(duì)比如表2所示。

表1 靜態(tài)環(huán)境下本文算法與ORB-SLAM2 性能對(duì)比Tab.1 Performance comparison of the proposed algorithm with ORB-SLAM2 in static environment

表2 動(dòng)態(tài)環(huán)境下本文算法與ORB-SLAM2 性能對(duì)比Tab.2 Performance comparison of the proposed algorithm with ORB-SLAM2 in dynamic environment

圖3 靜態(tài)環(huán)境算法對(duì)比效果Fig.3 Comparison effect of algorithms in static environment

圖4 動(dòng)態(tài)環(huán)境算法對(duì)比效果Fig.4 Comparison effect of algorithms in dynamic environment

圖3(a)為原始ORB-SLAM2 算法在靜態(tài)環(huán)境提取的特征點(diǎn)對(duì)，從圖中可以觀察到這些特征點(diǎn)對(duì)包含了誤匹配的情況，從結(jié)構(gòu)上看，誤匹配的特征點(diǎn)對(duì)與正確匹配點(diǎn)對(duì)存在明顯的角度偏差或者距離偏差。圖3(b)為本文算法檢測(cè)到的誤匹配特征點(diǎn)對(duì)，從圖中可以看出統(tǒng)計(jì)學(xué)方法能夠很好地對(duì)特征點(diǎn)對(duì)進(jìn)行異常值篩選，從而找出誤匹配特征點(diǎn)對(duì)。

表1 展示了在靜態(tài)數(shù)據(jù)序列中，本文提出的誤匹配剔除算法與原始ORB-SLAM2 的性能對(duì)比。從表中可看出在平均剔除5.43%誤匹配特征點(diǎn)對(duì)的情況下，本文算法的ATE 平均減小了17.10%，RPE 平均減小了21.67%，驗(yàn)證了剔除少量的誤匹配特征點(diǎn)對(duì)能夠提升SLAM 建圖的精度。

圖4(a)展示了原始ORB-SLAM2算法在動(dòng)態(tài)環(huán)境中提取的特征點(diǎn)對(duì)，由于受行人影響，大量誤匹配的特征點(diǎn)對(duì)出現(xiàn)在行人身上。在動(dòng)態(tài)環(huán)境中，特征點(diǎn)對(duì)可能會(huì)出現(xiàn)更大的角度變化或距離變化，從而導(dǎo)致誤匹配更加明顯，而本文提出的誤匹配剔除機(jī)制算法有效地檢測(cè)出了行人身上的誤匹配特征點(diǎn)對(duì)，如圖4(b)所示。

從表2可以看出，與ORB-SLAM2算法相比，本文算法的ATE減小了19.72%，RPE減小了18.18%。此外，與靜態(tài)環(huán)境相比，動(dòng)態(tài)環(huán)境下剔除的誤匹配特征點(diǎn)對(duì)提升了6.78%，表明了本文算法在動(dòng)態(tài)環(huán)境中也有很好的魯棒性。

4.3 增加地面約束的后端優(yōu)化性能測(cè)試

本文首先對(duì)數(shù)據(jù)集中的圖像序列進(jìn)行地面提取測(cè)試，地面提取流程圖如圖5所示。

圖5 地面提取流程可視化Fig.5 Visualization of the ground extraction process

圖5(a)為利用RGBD相機(jī)采集的RGB圖和深度圖，結(jié)合相機(jī)內(nèi)參矩陣恢復(fù)出來(lái)的RGB點(diǎn)云圖；圖5(b)通過(guò)搜索50個(gè)最近鄰點(diǎn)來(lái)擬合點(diǎn)云方向向量，圖中小線段為擬合的方向向量，從圖中可以看出擬合的方向向量與表面垂直度較好，擬合效果較好；圖5(c)為剔除掉非垂直方向向量的效果圖，從圖中可以看出本文方法能夠剔除圖5(b)中的非垂直向量，利用剩余的點(diǎn)云進(jìn)行平面提取，提取效果如圖5(d)所示。圖5(d)中的正方形平面即為提取的地面，由圖可知本文算法能夠準(zhǔn)確地提取地平面。加入地面約束后，本文算法（圖中用ours表示）與原始ORB-SLAM2算法的位姿對(duì)比如圖6所示。

圖6 位姿對(duì)比Fig.6 Comparison of postures

從圖6可以看出，在z軸軌跡中，ORB-SLAM2算法相對(duì)于真值軌跡不穩(wěn)定，會(huì)發(fā)生漂移，波動(dòng)較大，而本文算法與真值軌跡更加貼合，更加穩(wěn)定。本文將地平面向量引入后端優(yōu)化過(guò)程，系統(tǒng)可以更精確地調(diào)整相機(jī)位姿，確保地面點(diǎn)在不同幀中保持一致性，從而有效減小地面區(qū)域的漂移問題。地面約束在后端優(yōu)化中促使地圖點(diǎn)云與相機(jī)位姿更加緊密地匹配，提升了SLAM系統(tǒng)的準(zhǔn)確性和魯棒性。

4.4 本文SLAM 算法的性能測(cè)試

實(shí)驗(yàn)對(duì)原始ORB-SLAM2算法與本文算法性能分別進(jìn)行測(cè)試，并在CollaborativeSLAM數(shù)據(jù)集與實(shí)際環(huán)境下進(jìn)行多次測(cè)試。CollaborativeSLAM數(shù)據(jù)集包含了不同的室內(nèi)場(chǎng)景，并且數(shù)據(jù)集在多個(gè)環(huán)境下進(jìn)行錄制，具有較高的挑戰(zhàn)性。取連續(xù)30次測(cè)量的中位數(shù)作為算法性能的最終結(jié)果，通過(guò)這種方式可以獲得更可靠且準(zhǔn)確的實(shí)驗(yàn)結(jié)果，排除算法運(yùn)行中由于隨機(jī)性引起的偶然誤差。本文算法與ORB-SLAM2算法的運(yùn)行軌跡對(duì)比如圖7所示，兩種算法在實(shí)際場(chǎng)景中的測(cè)試結(jié)果如圖8所示。

圖7 軌跡對(duì)比Fig.7 Track comparison

圖8 實(shí)際環(huán)境算法對(duì)比Fig.8 Comparison of algorithms for real-world environment

從圖7可以看出，本文算法的效果與真值軌跡更貼合。圖8為實(shí)際場(chǎng)景中本文算法和ORB-SLAM2算法的對(duì)比，實(shí)際場(chǎng)景是從起點(diǎn)出發(fā)行駛一圈后再回到出發(fā)點(diǎn)。由圖8可知，本文算法的軌跡路徑為封閉曲線，與實(shí)際位姿變化情況相符；ORB-SLAM2算法路徑未封閉，軌跡誤差相對(duì)本文算法較大。

本文算法與ORB-SLAM2算法在TUM數(shù)據(jù)集序列上的測(cè)試結(jié)果如表3所示。

表3 本文算法與ORB-SLAM2 算法在不同數(shù)據(jù)集測(cè)試結(jié)果（單位：米）Tab.3 Test results of the proposed algorithm and ORB-SLAM2 algorithm on different datasets (unit: m)

由表 3 可知，與ORB-SLAM2算法相比，使用誤匹配剔除算法的ATE平均減小了17.62%，RPE平均減小了20.97%；使用地面約束算法的ATE平均減小了39.20%，RPE平均減小了44.20%，驗(yàn)證了本文算法能夠提高SLAM系統(tǒng)精度。

5 結(jié)論

本文提出了一種基于誤匹配剔除和地面約束的SLAM算法，該算法基于ORB-SLAM2框架。在前端視覺里程計(jì)階段，采用Boxplot四分位算法和直方圖統(tǒng)計(jì)算法識(shí)別和剔除誤匹配的特征點(diǎn)對(duì)，從而降低前端視覺里程計(jì)的誤差，使得前端能夠提供更精確的初始估計(jì)位姿，為后續(xù)優(yōu)化奠定了堅(jiān)實(shí)的基礎(chǔ)；在后端優(yōu)化階段，通過(guò)計(jì)算點(diǎn)云方向向量剔除非垂直的地面向量以進(jìn)行地面擬合，并將地面約束引入后端優(yōu)化方程，使得SLAM系統(tǒng)更加魯棒。在實(shí)驗(yàn)部分，本文在靜態(tài)數(shù)據(jù)集與動(dòng)態(tài)數(shù)據(jù)集上分別測(cè)試了誤匹配剔除機(jī)制的效果，并對(duì)地面進(jìn)行擬合，最后給出了該算法在不同數(shù)據(jù)集上的測(cè)試效果。由測(cè)試結(jié)果可以看出，增加了誤匹配剔除機(jī)制與地面約束有效地優(yōu)化了前端和后端，能夠提升SLAM系統(tǒng)的準(zhǔn)確性與魯棒性。