基于特征點改進的視覺SLAM定位研究

王 偉，湯琴琴，汪先偉

(1.南京信息工程大學 自動化學院，南京 210000；2.無錫學院 軌道交通學院，江蘇 無錫 214000)

0 引言

視覺SLAM(simultaneous localization and mapping)是指通過視覺傳感器對環境進行感知，同時實現機器人或無人駕駛汽車在未知環境下的自主定位和建圖[1]。在解決機器人和自動駕駛等領域的應用問題中，視覺SLAM成為最受重視和廣泛應用的技術之一[2-4]。通過研究SLAM技術，可以為實現更加智能的、精準的機器人定位導航提供支持[5]，同時也可為構建數字地圖和城市規劃等領域提供數據支撐。

目前，ORB(oriented FAST and rotated BRIEF)算法作為一種關鍵點檢測與描述子生成算法[6]，在SLAM領域被廣泛使用。然而，ORB特征點的數量在一定程度上受限于圖像紋理信息，即圖像分辨率較低、場景光照黑暗或者曝光等極端條件、結構信息較少導致圖像平整度較高等因素使得環境的紋理信息不被突出時，難以提取足額數量的ORB特征點。因此，如何提高ORB特征點的數量和質量以提升SLAM定位精度成為當前SLAM領域需要解決的核心問題之一[7]。國內外學者們以往主要通過設立特征點檢測閾值、圖像預處理、圖像處理與閾值結合等方式來解決在低紋理場景下ORB特征點數量不足的問題，使得ORB算法更加魯棒。

為通過設置閾值增強ORB算法在低紋理環境下的魯棒性以便提取更多的特征點，Murartal等人[8]在ORB_SLAM2中設置了固定閾值，即當最大固定灰度閾值搜索不到特征點時，系統自動啟用最小固定灰度閾值進行ORB特征點檢測。Wu等人[9]采用動態閾值的思想，去除最高亮度和最低亮度的像素后計算平均亮度，通過自適應閾值選擇準則為每個點都設置專屬閾值，解決ORB對光變化的敏感性問題。焦嵩鳴等[10]則針對固定閾值提出了一種全局和局部的自適應閾值結合的思路，針對整體與局部的亮度變化兩種情況各自采取全局或半全局自適應閾值策略，并結合于局部自適應閾值，以此增加特征點提取的數量。另外，支持向量機(SVM，support vector machine)[11]和卷積神經網絡(CNN，convolutional neural networks)[12]也被提出用以預測自適應閾值，基于機器學習的自適應閾值方法需要大量訓練數據和計算資源的數量，以及對數據分布和功能有很高的要求，應用于現實有很大的局限性。這類方法的缺點：1)如果采用固定的全局或局部閾值，無法兼顧整個圖像的各個部分，難以達到理想的效果；2)兩次閾值可能會造成圖像某些區域塊的ORB特征點被重復提取，有些區域被遺忘檢測，不僅使特征點冗余，又影響大規模特征匹配的準確性；3)閾值的方法依賴圖像本身的灰度動態范圍，在低紋理環境下，灰度動態范圍往往較低，此類方法容易失效，適應性差，而且閾值受環境噪聲的干擾，閾值的取值并不準確。

所以，學者們繼而研究對圖像進行預處理，增強圖像的邊緣、角落等細節以改變低紋理場景下的灰度范圍。Cheng等人[13]通過對圖像進行低光增強處理，增強了系統在低光環境的魯棒性。陸佳嘉等人[14]使用二維高斯對圖像金字塔進行降噪，并對像素進行劃分，提高部分區域提取到特征點的概率。李憲華等人[15]通過自適應閾值伽馬變換，對低照度、低紋理的圖像塊進行增強，提高在這些低光環境下提取到的特征點的數量。Dai等人[16]提出一種圖像增強與截斷自適應閾值相結合的方法改進ORB 特征提取算法，將原始圖像轉換為灰度后通過應用高斯濾波、截斷自適應伽馬亮度調節和非銳化掩蔽增強圖像，將圖像分成若干子區域，并利用改進的大津法獲得相應閾值，以此解決了在光照過多或不足等低紋理環境中無法提取足夠特征點的問題?，F有方法：1)低紋理場景變化是復雜的，包括分辨率的高低變化、場景結構信息的差異導致平整度的不同，難以保證ORB算法在這些因素下的適應性與穩定性；2)結合額外的閾值后算法不準確性增加。

此外，關鍵幀選擇在視覺SLAM算法中也扮演重要角色，關鍵幀選擇策略的質量直接影響著算法的定位精度和運行效率[17-19]。傳統的關鍵幀選擇策略通常只考慮了時間因素，根據距離閾值、角度閾值或關鍵幀密度等指標進行篩選[20]，將一些劣質的關鍵幀納入隊列，難以針對不同類型場景進行調整導致定位精度下降、模型計算量大。

為解決上述問題，本文利用多尺度分析與局部灰度的特征檢測為一般ORB特征點添加尺度與旋轉的描述，利用基于高斯模糊的圖像信息增強方法加強ORB算法在低紋理環境下的穩定性，象限分割圖像以使特征點均勻分布，并在只考慮時間因素的傳統關鍵幀選擇機制中兼容了特征點數量因素，降低定位誤差。

1 視覺SLAM定位方法

基于特征點的視覺SLAM的定位過程如圖1所示。

圖1 視覺SLAM定位方法

1)相機調整參數之后利用ORB特征提取器檢測圖像特征點，在當前幀與上一幀之間通過隨機采樣一致性算法(RANSAC)[21]尋找匹配的特征點。

2)通過特征點坐標差異計算相機運動。

3)但是為了提升系統響應速度，降低系統定位誤差，通過選擇合適的關鍵幀，使系統只保留最必要的信息進行位姿估計。

4)基于詞袋模型構成系統的閉環，用于減少定位過程中的累計誤差。閉環檢測[22]比較當前幀與之前幀，找到或代表同一區域的特征，計算相似度，并在相似度高于一定閾值時確立閉環。

5)根據運動估計獲得的相對位置信息，進行后端的全局優化，使誤差最小化。

本文將特征點看作視覺SLAM系統正式進行定位的輸入數據，對圖像中的特征點加以描述，并克服傳統ORB算法在低紋理的環境下的不穩定性，過濾劣質關鍵幀，降低視覺SLAM的定位誤差。

2 特征點提取過程以及改進

特征點對視覺SLAM定位的重要性是顯著的。在視覺SLAM中，特征點是用于進行相鄰幀間匹配的關鍵點，它們被用來計算相機的運動和場景的深度信息。特征點數量越充足時，視覺SLAM算法在定位中就有越多可用的信息。在視覺SLAM定位中，需要依靠匹配當前幀與先前幀之間的特征點來獲取相機的運動估計。在低紋理、低分辨的環境中，傳統ORB算法容易失效導致特征點數量不足，如果特征點數量太少，匹配誤差就會增加，從而導致相機姿態估計不準確或者無法成功匹配，進而導致整個SLAM系統定位失效。

2.1 傳統ORB特征點提取

傳統ORB特征點提取方法使用FAST角點檢測器尋找圖像中明顯的突變點，并將這些點組成候選關鍵點，原理如圖2所示，比較圓心點O且半徑等于3的圓上16個連續像素點與O的灰度差來提取FAST點。若圓周上連續著M個(M一般取12)像素點的灰度值與O的差已超規定閾值T，則判定被選中的圓心O作為特征點。詳細過程如下：

圖2 FAST角點提取示意圖

1)在圖像上選取一點O，灰度值假設為Io，以O為圓心且3為半徑作圓，共有16個點。

2)設定灰度閾值T，比如T=IO× 0.2。

3)若圓上有連續12個點的灰度值大于T+IO或小于T+IO，判斷O為關鍵點。

接著計算每個關鍵點的描述子，描述子是用來描述關鍵點周圍區域的一系列二進制特征的向量，在ORB算法當中，使用BRIEF算法生成每個關鍵點的描述子，即在公式(1)中：P(x)、P(y)分別為點x、y處的灰度值大小。

(1)

其具體思想是在特征點P周圍以特定模式選取N對點，比較灰度值的大小(比如x，y)。若x比y大，則取1，反之就取0。

故BRIEF描述子可以表示為：

(2)

2.2 改進的ORB特征點提取過程

實際上，圖像中的物體可能會以不同尺度或角度出現，故提出一種多尺度分析方法，在不同尺度下檢測物體的特征，即通過對原始圖像進行重復縮減或擴大操作，生成不同分辨率的圖像，每縮放一次，圖像的分辨率就會下降，并在這些不同尺度下進行特征提取，使得ORB特征點具備尺度不變性。如圖3所示，本文將原始圖像作為待縮圖，長為W0，寬為H0，以1.2為尺度系數進行縮放，第n次得到的圖像長為Wn，寬為Hn，面積為Sn。

圖3 多尺度縮放示意圖

圖4 圖像信息增強前后灰度直方圖對比

Wn與W0的關系為：

(3)

Hn與H0的關系為：

(4)

故Sn可表示為：

(5)

由于本文以1.2為尺度系數，所以每縮放一次后的圖像面積是上一次的69.44%。

ORB算法添加了通過平移模擬旋轉的過程來應對BRIEF描述子對旋轉不太敏感的問題，本文引入基于局部灰度分布的特征點檢測方法以提高ORB算法對旋轉的穩定性，即以特征點為原點構建局部坐標系，在構建好的坐標系中，通過對鄰域內每個像素的灰度加權平均，計算出鄰域內灰度值的中心位置，最后用特征點指向灰度質心的特征向量代表此特征點的方向。具體步驟如下：

1)定義某圖像塊A的矩定義為：

(6)

其中：I(i，j)表示在位置(i，j)處的像素灰度值，M與N分別表示圖像塊A的寬度與高度，p、q表示矩陣階數，Cx與Cy分別表示圖像塊A灰度質心坐標，即圖像塊中所有像素的坐標的加權平均值。

2)圖像塊A灰度質心C的坐標公式為：

(7)

3)由于p，q={0，1}，所以C的坐標可以進一步表示為：

(8)

4)將圖像塊A幾何中心O與質心相連得到方向向量，將特征點方向定義為：

(9)

在描述子的基礎上，經過多尺度縮放和灰度質心方向描述，ORB特征點更加穩健，同時增強算法的性能和穩定性。從角點提取過程可以看出，ORB的提取依賴角點周圍像素的差異程度，高紋理的圖像像素之間的變化明顯大于低紋理的圖像，故而更容易提取到特征點。因此，基于擴大圖像灰度動態范圍的思想，在角點檢測前提出一種基于高斯模糊的圖像信息增強方法，提升圖像灰度動態范圍，突出圖像的紋理信息，從而增加可提取的ORB特征點數量。

圖像信息增強方法首先對原圖像進行3個高斯模糊，得到3個不同尺度的模糊圖像。因為高斯模糊會平滑掉圖像中的細節信息，所以在每個模糊圖像與原圖之間做差，獲得這些細節信息，并通過一定的組合公式將其融合到原圖像中，進而增強原圖像的信息表達能力，形成更高動態范圍的灰度圖像。具體步驟如下：

1)定義待處理圖像為I*(x，y)，選擇標準差分別為σ1、σ2、σ3的高斯核函數G1(x，y)、G2(x，y)、G3(x，y)，函數表達式如(10)所示，其中σ1=1.0，σ2=2.0，σ2=4.0表示模糊程度，x和y表示在高斯核函數的中心點的偏移量。

(10)

2)利用3個不同尺度的高斯核函數分別平滑處理I*(x，y)，得到處理后的圖像稱為B1(x，y)、B2(x，y)、B3(x，y)，公式如下：

(11)

3)如公式(12)所示，將所得的B1(x，y)、B2(x，y)、B3(x，y)分別與原圖I*(x，y)做差，獲得3個不同層次的細節信息，記作D1(x，y)、D2(x，y)、D3(x，y)。

(12)

4)選擇w1，w2，w3三個加權系數，通過組合函數得到信息增強后的圖像D*(x，y)。組合函數的公式如下：

D*(x，y)=(1-w1×sgn(D1(x，y)))×D1(x，y)+w2×D2(x，y)+w3×D3(x，y)

(13)

多次實驗對比后得到w1=w2=0.25，w3=0.5時效果最佳。

ORB算法對角點的響應較為敏感，所以ORB算法提取的特征點往往集中在圖像角點等邊緣區域。此外，由于采用了在不同尺度空間下檢測和描述特征點，使得特征點分布較為集中，因為在不同尺度下相鄰像素的信息是高度相關的，因此在不同尺度下提取到的特征點往往存在冗余。本文通過對圖像進行象限劃分結合非極大值抑制的方法過濾部分冗余的特征點，使特征點分布均勻化，減小計算量，提高匹配效率，進而提升視覺SLAM的定位精度。

如圖5所示，將圖像劃分為4個面積相等的矩形象限，并作為4個原始象限N1、N2、N3和N4，統計每個象限中的特征點的總數。對于每一個象限，若包含的特征點數量大于1，則作進一步細分，即將該象限分成4個子象限，繼續對子象限進行特征點數量的統計與劃分，直到每個象限中只包含1或0個特征點，停止劃分。對于已經劃分出來的象限進行后處理，將每個象限中的所有特征點按照響應強度進行排序，并保留最強的一個特征點，其余的特征點將被舍棄。在實際應用中，由于特征點可能會堆積在一起，導致檢測結果出現重疊，因此需要設定一個參數k，限制象限的最大劃分次數。當劃分次數已達k時，對于仍然包含多個特征點的象限，只保留其中響應強度最高的一個特征點。

圖5 象限劃分原理圖

總之，通過多尺度分析方法及局部灰度特征檢測的方法使得FAST角點具備尺度和旋轉的描述，利用基于高斯模糊的信息增強預處理視覺傳感器抓取的圖像，解決ORB算法提取特征點數量存在不足的缺陷，并對圖像進行象限分割融合非極大值抑制將冗余特征點剔除，使特征點分布均勻化。具體的改進后ORB特征點提取流程如圖6所示。

圖6 改進的ORB特征點提取過程

3 關鍵幀選擇機制

視覺SLAM系統將前后兩幀圖像里提取到的特征點進行匹配，得到它們在兩幅圖像中的對應關系，通過對這些點進行幾何變換，估計前一幀到當前幀之間的相對運動，包括平移與旋轉。如圖7所示，O1和O2是兩個視覺相機中心，后一幀KF2的特征點P2是前一幀KF1的特征點P1通過匹配得到的對應點，O1P1與O2P2交于點P，并構成一個極平面，O1O2為基線，線與KF1、KF2交于點e1、e2，極平面與KF1、KF2交于極線l1、l2。

圖7 相對運動估計示意圖

假設KF1幀與KF2幀對應的旋轉和平移向量分別是R、t，點P、P1和P2三者之間的關系如式(14)所示：

(14)

取Z=1歸一化平面，聯解(14)中的兩個式子，可以得到式(15)：

(15)

其中，記E=t^R，E為本質矩陣(Essential Matrix)，記F=K-TEK-1，F為基礎矩陣(Fundamental Matrix)，E和F之間相差了相機內參K。

將式(15)寫成本質矩陣E的形式，可以進一步改寫為式(16)。E為一個三行三列的矩陣，(u1，v1，1)和(u2，v2，1)分別是P1和P2歸一化平面上的投影。

(16)

通過八點法，可以求解本質矩陣E，通過本質矩陣進行奇異值分解求解旋轉矩陣R和平移向量t。即通過P1和P2的匹配關系計算本質矩陣，奇異值分解得到旋轉矩陣和平移向量，這些矩陣和向量描述了相機之間的運動。

利用相對運動信息更新位姿估計，生成最終的軌跡。但如果所有的幀都用來做匹配和運動估計，會導致計算量巨大。同時，由于兩幀之間或存在很多相似之處，使用所有幀進行匹配可能造成冗余信息，降低SLAM系統的定位精度。故需要選擇能夠提高獨特信息的幀作為關鍵幀，減少系統計算量和提升系統的效率。關鍵幀要選取能夠代表整個場景的幀，以保證位置更加精確，選取描述信息能力強的幀以提高定位精度，同時時間間隔要適當。一般的關鍵幀選擇機制沒有著重考慮圖像中的冗余信息，即便提取出的特征不同，但由于圖像內容相似，所選幀之間信息重疊，導致匹配優化時間過長。為了剔除冗余的劣質關鍵幀，本文提出一種綜合時間因素和特征點數量因素的關鍵幀選擇機制。

只考慮時間因素時，首先要保證當前幀的內點數大于最小閾值且重疊度不高，以確保關鍵幀跟蹤指令的準確性。一旦條件滿足，就會對以下條件進行判斷，滿足條件之一，即可插入關鍵幀。

1)為了避免追蹤失效，當與上一個關鍵幀的時間間隔T超過預設的最大值MAX時，需要插入新的關鍵幀。

2)為了避免圖像重疊度過高，當與上一個插入關鍵幀的時間間隔T達到預設的最小值MIN時，且局部建圖線程空余，插入新的關鍵幀。

3)由于局部建圖線程處理能力限制，需要在該線程中保證關鍵幀隊列的幀數量不超過3個。

只考慮時間因素，在視覺相機轉彎或移動速度過慢時，幀與幀之間的信息重疊過大導致定位精度下降，針對這一問題，在考慮時間因素的基礎上，本文將特征點因素作為考量，設計下述劣質關鍵幀判斷機制，減少計算開銷，提升視覺SLAM系統的定位精度。

1)3個相鄰的關鍵幀利用隨機采樣一致性方法匹配ORB特征點。

2)分別將相鄰的兩組關鍵幀成功匹配點的數量記為Num1和Num2，相隔的兩個關鍵幀成功匹配的特征點數則記為Num3。

3)如果滿足Num3>Num1和Num3>Num2，則中間幀被認為是劣質關鍵幀。

4)剔除被選的劣質關鍵幀，則進行下一次判斷。

本文關鍵幀選擇策略偽代碼如下：

IfT>MAXThen

插入關鍵幀 or

IfT≥MINand 局部建圖線程空閑 Then

插入關鍵幀 or

If 局部建圖線程關鍵幀隊列幀數<3 Then

插入關鍵幀

If 關鍵幀庫內關鍵幀總數超過3 Then

IfNum3>Num1Then

IfNum3>Num2Then

剔除劣質的關鍵幀

EndIf

EndIf

EndIf

EndIf

EndIf

EndIf

4 實驗測試與分析

選用CPU為i7-11800H，內存16 GB的計算機，軟件平臺為基于Ubuntu18.04的ROS系統，選用Kinect V2視覺相機拍攝真實圖像和TUM數據集作為測試數據。

4.1 提取特征點的數量實驗

為驗證本文改進的ORB特征點提取方法在不同分辨率、不同光照、不同平整度等因素導致環境紋理信息不被突出時的穩定性，分別選取3組各5張圖像作為實驗對象，A1～A5圖像的分辨率分別為100、150、200、250、300 dpi，B1～B5圖像的光照程度分別為黑暗、稍暗、正常、明亮、高光，C1～C5圖像因為環境結構的差異平整度各不相同。提取效果如圖8所示，每組的第一行圖像采用傳統ORB算法提取特征點，第二行圖像利用本文提出改進ORB特征點提取策略提取特征點。

圖8 改進前后提取特征點對比

圖9 圖像分割示意圖

將傳統ORB算法提取的特征點總數記為Sum1，本文方法提取的特征點總數記為Sum2。如公式(17)所示，引入ORB特征點增長系數Q，用于對改進后的ORB特征點提取方法在不同因素下的性能表現進行評價，Q值越大，說明算法的適應性與穩定性越好。

Q=Sum2/Sum1

(17)

首先，通過表1數據可以看出，a1至c5所有圖像在使用本文改進的方法提取特征點后，特征點數量均有明顯增加，表明本文方法在不同分辨率、不同光照、不同平整度的情況下均有良好效果。

表1 圖像ORB特征點數量對比

其次，經計算，分辨率因素下的Q平均值為1.44；光照因素下的Q平均值為1.31；平整度因素下Q平均值為1.57。對于分辨率因素而言，Q與分辨率大致成反比；就光照而言，越是黑暗和高光這種極端條件下，Q值越是突出；就平整度而言，結構信息越少，平整度越高，Q值也越大。

綜上所述，本文改進后的ORB特征點能夠在紋理信息不被突出的環境下有良好的性能表現，尤其是在分辨率較低、光照極端、結構信息越少的條件下，具有良好的適應性和穩定性。

4.2 均勻度實驗

通過均勻度量化，對比在真實場景下傳統ORB特征點提取方法、ORB_SLAM2和本文方法在提取特征點上均勻化的表現。

實驗采用的均勻化評價方法如下：

先對圖像進行水平、垂直、中心和四周(中心與四周面積相等)、左上至右下和左下至右上5種方式分割得到10個區域。分別計算每個區域內的特征點數量，并構建一個區域統計分布向量。再計算特征點區域統計分布的標準差，以此表示均勻度，具體如公式(18)所示。標準差越小，特征點分布越是均勻，反之亦然。

(18)

圖10 均勻度測試結果

第一行是傳統ORB算法測試結果，第二行是本文提出的改進ORB特征點提取方法的測試結果。根據式(18)量化后的結果如表2所示。

表2 均勻度量化結果圖像

通過表2計算，本文方法在均勻度性能上相較于ORB_SLAM2與傳統ORB特征點提取方法，分別提升了7.14%和20.78%，表明了改進過后的算法可使特征點分布更加均勻。

4.3 關鍵幀選擇機制效果測試

為驗證在只考慮時間因素的傳統關鍵幀選擇機制上融合特征點數量因素的方法更加有效，將本文提出的關鍵幀選擇機制移植于ORB_SLAM2，與只使用傳統關鍵幀選擇機制的ORB_SLAM2共同在TUM數據集上進行測試，在考慮了特征點數量因素之后，視覺SLAM需要處理的關鍵幀數量減少，結果如表3所示。

表3 關鍵幀數量 個

將移植前后視覺SLAM系統求解的軌跡與真實軌跡進行均方根誤差(RMSE)誤差對比，RMSE越小說明系統定位精度越高，軌跡越擬合于真實軌跡，定位精度結果如表4所示，RMSE的減少說明了剔除劣質關鍵幀可以降低視覺SLAM的定位誤差，從而證明本文的關鍵幀選擇機制可以優化關鍵幀質量。

表4 剔除劣質幀前后定位精度對比

4.4 軌跡精度測試

為驗證本文提出的改進ORB特征點提取和設計的關鍵幀選擇機制結合使用的有效性與可行性，首先，將本文提出的改進ORB特征點提取策略與ORB_SLAM2結合為ORB_V1系統，精度提升結果如表5所示。

表5 ORB_V1提升效果

在ORB_V1系統的基礎上結合本文設計的關鍵幀選擇機制，組成ORB_V2系統，精度測試結果如表6所示。

表6 ORB_V2提升效果

如圖11所示，以fr3_cabinet為例，ORB_SLAM2求解出的軌跡traj1的RMSE為0.056，在未添加本文提出的關鍵幀選擇機制時，ORB_V1求解軌跡traj2的RMSE為0.050，ORB_V2求得軌跡traj3的RMSE是0.048。相比于traj2和traj3，traj1因為環境紋理太少，一般ORB算法幾乎失效，沒有足夠的特征點用來計算相機的運動，使得估算軌跡初始階段很大程度偏離真實軌跡，且導致中途定位跟蹤失敗。traj3在traj2的基礎上，再加入了綜合時間因素和特征點因素的關鍵幀選取機制，降低了劣質關鍵幀對定位精度的影響，定位精度從10.714%進一步提升至14.286%，更貼合真實軌跡。

圖11 以fr3_cabinet為例的軌跡對比

5 結束語

本文利用多尺度分析與局部灰度的特征檢測為一般ORB特征點添加尺度與旋轉的描述，提升ORB算法的穩定性，利用基于高斯模糊的信息增強方法克服傳統ORB算法在紋理信息不被突出的環境下的不穩定性，通過對圖像進行象限分割去除多余的特征點，使特征點均勻分布，提升視覺SLAM系統的定位精度。在只考慮時間因素的傳統關鍵幀選擇機制中結合了特征點數量因素，過濾劣質關鍵幀，進一步降低定位誤差。

經計算，整個系統的定位精度誤差平均減少了14.688%。但是由于提取了更多的特征點，造成運動估計計算需要更長的運行時間，因此可考慮將深度學習與視覺SLAM結合，提升ORB算法識別的速度，或是研究基于SLAM方法的傳感器(例如激光雷達、慣性測量器件等)數據融合技術，以提高定位精度。