基于誤匹配剔除和地面約束的視覺SLAM 算法

黃丹丹，郝文豪，楊 陽

（長春理工大學 電子信息工程學院，長春 130000）

隨著計算機視覺和機器人技術的迅猛發展，視覺同步定位與建圖（Simultaneous Localization and Mapping,SLAM）算法作為一種重要的技術手段，已經在無人駕駛、AR/VR和三維重建等領域得到廣泛應用[1,2]。前端視覺里程計和后端優化是視覺SLAM中關鍵的研究內容，前端視覺里程計主要通過對連續幀之間的特征點進行匹配和跟蹤，估計相機在三維空間中的運動；后端優化通過構建最小化重投影誤差，對相機位姿和地圖進行全局一致性調整[3]。前端特征點匹配的準確性為后端提供了優化的基礎，而后端優化又反過來優化前端估計的相機運動和地圖，使得整個SLAM系統的精度更高、魯棒性更強，并在復雜場景下保持穩定性[4]。

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

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

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

1 算法框架

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

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

圖1中四個線程協同工作，使得本文算法能夠實現對RGBD相機輸入的環境信息進行實時定位與地圖構建。特征點跟蹤線程和地面提取線程提供了前端的關鍵功能，通過特征點提取與匹配、誤匹配剔除和地面點提取等操作，為后端的局部建圖線程和回環線程提供準確的輸入數據。局部建圖線程和回環線程通過地圖點的更新和位姿優化提高定位精度和地圖的一致性。相對于ORB-SLAM2框架，本文對特征點跟蹤線程中的特征點剔除模塊進行改進以提高位姿估計的準確性，并增加了地面提取線程以提高位姿優化的魯棒性。

這種算法設計使得系統能夠充分利用RGBD相機的深度信息，從而提高對環境的感知能力。同時，通過前后端的協同優化，可以實現較高精度和魯棒性的定位與建圖。

2 特征點對誤匹配剔除

ORB-SLAM2采用RANSAC算法剔除誤匹配點，對初值敏感且參數設置復雜。針對該問題，提出一種利用Boxplot四分位算法與直方圖統計算法進行誤匹配特征點對剔除的方法，該方法能夠對匹配的特征點對進行評估，并剔除誤匹配特征點對，提高SLAM系統前端的準確性。特征點對的表達式為：

其中，P為當前幀的特征點集，與之相對應匹配幀的特征點集為Q，點集元素pi和qi為匹配的特征點；(u1i,v1i)表示當前幀第i個特征點的像素坐標；(u2i,v2i)表示匹配幀第i個特征點的像素坐標；N為匹配特征點的個數。

將特征點對的歐氏距離作為誤匹配剔除的預處理階段。特征點對的歐式距離集合L的表達式為：

采用統計學計算分析特征點對的分布特性。首先，使用Boxplot算法對預處理的歐氏距離集合進行異常值檢測與剔除。Boxplot四分位算法是一種常用的統計學數據分析方法，通過四分位數描述數據的分布情況，由上下四分位距離確定異常值的存在。該方法可以評估特征點對的歐式距離是否存在異常值，進而識別并剔除超過上下限的特征點對，減少誤匹配特征點對的干擾。對集合L中的元素從小到大進行排序，得到新的集合記為L*。設有序集合L*的第1/4個數為Lb1，第3/4個數為Lb2，則經過Boxplot算法處理后的歐式距離集合L1為：

其中，IQR為四分位距。

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

本文算法的優點在于Boxplot算法和直方圖算法可以有效互補，其中直方圖算法能夠直觀地展示數據分布情況，當異常數據較多時會由于頻數較高而將錯誤的匹配對當作正常值，而Boxplot算法能夠有效地彌補直方圖的缺陷從而更好地剔除異常值。通過綜合使用這兩種算法，能夠在SLAM算法的前端進一步提高特征點匹配的質量，有效地降低誤匹配對定位和建圖的影響。

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

3.1 地面向量描述與提取

地平面一般方程表示為：

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

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

根據針孔相機模型，3D坐標點與像素點的轉換關系為：

其中，u、v為像素坐標；fx、fy、cx、cy為相機內參；X、Y、Z為該像素坐標在相機坐標系下的3D空間坐標點云。

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

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

其中，k個點組成的點云簇記為M={mi|i=1,2 …k}；c為點云簇的中心點；n*為待求點云單位法向量；n為當前估計的點云法向量；Ω為向量歸一化函數。

式(8)表示點云簇里每個點與中心點所構成的向量與當前估計方向向量的點乘之和最小即為所求的方向向量。

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

k1}中隨機選取三個點進行平面擬合，擬合方程為：

通過構建最小化點到擬合平面的距離作為平面擬合的代價函數，代價函數為：

通過算法多次迭代選點，取代價函數最小時的平面方程作為最終擬合的平面。

3.2 加入地面約束的后端優化

后端的作用是對前端視覺里程計得到的位姿進行優化，優化的過程主要依靠地圖點約束和地面向量約束。優化因子圖如圖2所示。

圖2 優化因子圖Fig.2 Optimization factor graph

構建地圖點約束時，采用ORB-SLAM2進行地圖點的生成與剔除，利用每個特征點的像素坐標與其相對應的地圖點坐標構建觀測方程：

其中，γ為像素坐標，即γ=(u,v)；Tcw為待優化位姿；V為像素坐標對應的空間坐標；h函數為世界坐標系到像素坐標系的轉換方程，即

其中，S為地圖點深度值；K為相機內參矩陣。

為此，一個特征點的像素點與其地圖點構成一個觀測方程，設共有j對特征點與其地圖點相匹配，則能夠提供j個觀測方程，組成一個超定方程。

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

當地面平整時，由于地面的平面法向量具有固定的方向和高度信息，后端可以使用地面向量約束相鄰幀之間的相機運動，從而提高相機位姿的估計精度。構建地面向量約束時，利用當前幀采集的地面信息與實際地面信息建立角度誤差與距離誤差方程。

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

后端優化總的約束方程為：

4 實驗分析

4.1 實驗環境

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

4.2 特征點對誤匹配剔除算法的性能測試

為了測試誤匹配剔除功能的魯棒性，本文分別在靜態和動態環境下評估算法的性能。選取了TUM數據集中的靜態子序列：fr2_360_hemisphere、fr2_360_kidnap、fr2_desk、fr2_large_with_loop 以及TUM數據集中的動態子序列：fr3_walking_halfsphere。這些數據集子序列均提供了真實軌跡作為參考，以確保評估的準確性。在評估算法性能時，本文采用了絕對軌跡誤差（Absolute Trajectory Error,ATE）和相對位姿誤差（Relative Pose Error,RPE）作為評價指標。ATE衡量了估計軌跡與真實軌跡之間的整體誤差，計算公式為：

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

RPE則關注相鄰幀之間的相對位姿誤差，計算公式為：

測試時，將原始ORB-SLAM2算法計算的特征點對與加入誤匹配機制后剔除的特征點對進行圖像化顯示。原始ORB-SLAM2算法與增加誤匹配特征點對剔除機制的本文算法在靜態環境下的對比效果如圖3所示，性能對比如表1所示；動態環境下兩種算法的對比效果如圖4所示，性能對比如表2所示。

表1 靜態環境下本文算法與ORB-SLAM2 性能對比Tab.1 Performance comparison of the proposed algorithm with ORB-SLAM2 in static environment

表2 動態環境下本文算法與ORB-SLAM2 性能對比Tab.2 Performance comparison of the proposed algorithm with ORB-SLAM2 in dynamic environment

圖3 靜態環境算法對比效果Fig.3 Comparison effect of algorithms in static environment

圖4 動態環境算法對比效果Fig.4 Comparison effect of algorithms in dynamic environment

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

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

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

從表2可以看出，與ORB-SLAM2算法相比，本文算法的ATE減小了19.72%，RPE減小了18.18%。此外，與靜態環境相比，動態環境下剔除的誤匹配特征點對提升了6.78%，表明了本文算法在動態環境中也有很好的魯棒性。

4.3 增加地面約束的后端優化性能測試

本文首先對數據集中的圖像序列進行地面提取測試，地面提取流程圖如圖5所示。

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

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

圖6 位姿對比Fig.6 Comparison of postures

從圖6可以看出，在z軸軌跡中，ORB-SLAM2算法相對于真值軌跡不穩定，會發生漂移，波動較大，而本文算法與真值軌跡更加貼合，更加穩定。本文將地平面向量引入后端優化過程，系統可以更精確地調整相機位姿，確保地面點在不同幀中保持一致性，從而有效減小地面區域的漂移問題。地面約束在后端優化中促使地圖點云與相機位姿更加緊密地匹配，提升了SLAM系統的準確性和魯棒性。

4.4 本文SLAM 算法的性能測試

實驗對原始ORB-SLAM2算法與本文算法性能分別進行測試，并在CollaborativeSLAM數據集與實際環境下進行多次測試。CollaborativeSLAM數據集包含了不同的室內場景，并且數據集在多個環境下進行錄制，具有較高的挑戰性。取連續30次測量的中位數作為算法性能的最終結果，通過這種方式可以獲得更可靠且準確的實驗結果，排除算法運行中由于隨機性引起的偶然誤差。本文算法與ORB-SLAM2算法的運行軌跡對比如圖7所示，兩種算法在實際場景中的測試結果如圖8所示。

圖7 軌跡對比Fig.7 Track comparison

圖8 實際環境算法對比Fig.8 Comparison of algorithms for real-world environment

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

本文算法與ORB-SLAM2算法在TUM數據集序列上的測試結果如表3所示。

表3 本文算法與ORB-SLAM2 算法在不同數據集測試結果（單位：米）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%，驗證了本文算法能夠提高SLAM系統精度。

5 結論

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