融合點線特征的視覺慣性同時定位及建圖

姚建均, 李英朝, 吳楊, 唐瑞卓, 于新達, 閆宇坤

(1.哈爾濱工程大學 機電工程學院,黑龍江 哈爾濱 150001; 2.維沃移動通信有限公司,廣東 東莞 523850)

得益于計算機視覺理論的發展和傳感器設備小型化,基于同時定位及建圖 (simultaneous localization and mapping, SLAM)的移動機器人發展迅速,使得移動機器人可以在GPS信號較弱的室內進行環境感知,并實現實時的定位和建圖[1]。

自SLAM系統提出以來,眾多學者都致力于提升其精度和魯棒性。王立鵬等[2]使用深度學習增強SLAM的定位精度和語義理解能力,伍明等[3]結合目標狀態估計與機器人可觀性控制增強移動機器人定位精度,但會增加系統的計算負擔。慣性測量單元(international mathematical union, IMU)可以輔助相機進行定位并感知絕對尺度[4],形成了視覺慣性同時定位及建圖(visual-inertial simultaneous localization and mapping, VI-SLAM)框架[5-6]。Qin等[7]提出了一種基于圖優化緊耦合地融合單目相機和IMU信息的VINS-Mono框架,前端采用卡納德 (Lucas-Kanade,LK)光流跟蹤點特征,后端執行4自由度的位姿圖優化以增強全局一致性。VINS-Mono框架已經成為眾多研究的基礎[8-10],但在光照變化和低紋理場景中,特征點提取的質量和重復性嚴重下降,致使整個SLAM系統精度降低,甚至產生跟蹤丟失的問題。

因此,基于點線特征的SLAM受到關注,Wang等[11]利用梯度直方圖,提出了統計像素梯度平均向量和標準方差的描述子,Sol等[12]提出了5種不同路標直線參數化的辦法,Gomez-Ojeda等[13]提出了基于半直接法的點線特征視覺里程計PL-SVO,Ojeda等[14]在PL-SVO的基礎上,加入了局部建圖和回環檢測線程。但目前線特征提取的實時性、精度和魯棒性仍然較差。

本文基于點線特征的VI-SLAM系統,引入并改進線特征提取算法,構建點線特征篩選框架,優化特征匹配結果。將所得的線特征提取匹配算法應用于VINS-Mono框架中,得到VI-BLSD-Bow系統。在進行相機-IMU聯合標定及詞袋訓練后,使用智能小車對強光照室內及低紋理走廊環境進行實驗驗證。

1 線特征提取及匹配算法

線段檢測器(line segment detector,LSD)線特征提取算法已經廣泛應用于圖像處理及SLAM系統中[15-16],但其精度和魯棒性還有待提升[17]。本文引入LSD線特征及線帶描述符(line band discriptor,LBD)描述子,分析LSD算法隱藏參數對線特征提取的影響,提高線特征的提取精度,并設計點線特征匹配及篩選策略,進而得出可應用于SLAM系統的線特征提取算法。

1.1 線特征描述子提取

傳統的線特征匹配由于遮擋導致端點位置不明確,且旋轉運動下缺乏魯棒的幾何約束。Zhang等[18]提出了LBD線段描述子,引入全局和局部的高斯權重系數,產生更高的匹配精度。

LBD算法首先構建圖片高斯金字塔,并對每一層高斯金字塔進行EDLine線特征提取,將不同維度下的直線段一一對應,放入同一個線段向量中,減少在不同尺度下線段匹配的問題。然后在不同尺度下同一線段處建立線段支持區域LSR,并將其分割為條數為m,像素寬度為w的多組條帶。定義2個條帶方向,分別為直線方向向量dL以及順時針垂直于dL的垂直分量d⊥。然后建立梯度分布場,在線段中點建立坐標原點,以dL和d⊥作為坐標軸,將線段支持域內像素梯度分別投影到坐標系中:

g′=(gT·d⊥,gT·dL)T(g′d⊥,g′dL)

(1)

式中:g是像素梯度向量;g′是線段支持域內局部像素梯度坐標;d為距離。

為降低距離線段較遠的像素梯度對描述子的影響,在LSR第i行設置全局高斯函數fg為:

(2)

式中di表示第i個條帶距離坐標原點的距離,且δg=0.5(mw-1)。

為減小條帶間的邊界效應,再通過在Bj條帶上下2個條帶之間設置局部高斯函數fl：

(3)

式中d′k為第k行距離條帶Bj中心的距離,且δl=w。

對Bj與相鄰條帶Bj-1、Bj+1之間局部像素梯度進行求和為:

(4)

式中λ=fg(k)fl(k)為綜合高斯系數。

最后,將每一行的像素梯度和聯立在一起,得到線特征描述矩陣Bj為:

(5)

組合所有條帶的特征向量Dj,獲得LBD算法的特征描述子:

(6)

1.2 隱藏參數對線特征提取的影響

LSD算法隱藏參數包括:高斯金字塔尺度S,高斯核函數標準差SS,梯度量化值Q,主方向角度差閾值Ta,NFA檢測閾值E,外接矩形密度閾值Td,偽排序箱數I以及長度抑制因子F。目前針對LSD算法的優化工作集中于LSD算法與其他算法的融合,針對隱藏參數對LSD算法影響的研究較少。

表1是傳統LSD算法的默認參數,在此參數設計下,線段檢測的精度以及魯棒性較低,因此需要對默認參數進行改良。本文主要針對高斯金字塔尺度S、梯度量化值Q、外接矩形密度閾值Td以及長度抑制因子F參數進行優化,分析其對LSD算法檢測精度和提取速度的影響,得出適用于SLAM系統的LSD算法檢測方案。

高斯金字塔尺度S是高斯降采樣下的縮放系數,用于解決圖像中的混疊和鋸齒問題。設置NFA檢測閾值E=1.0,能在帶噪聲條件下控制每幅圖像只存在一個誤檢。圖1是針對Euroc數據集調整高斯金字塔尺度時,線特征提取時間和精度的變化情況。

圖1可知,將高斯金字塔尺度縮小到0.4～0.6 (圖像金字塔縮小至25%～36%),能減小一半的檢測時間。同時,縮小尺度因子雖然減少了使用的像素數量,但對系統的精度影響不大。因此設置優化的LSD算法中S參數為0.5。

在LSD算法計算梯度大小和方向的過程中,平滑區域內的小梯度值點對直線的形成沒有幫助。因此可以通過設置梯度閾值ρ,對梯度值進行篩選。梯度閾值ρ為:

ρ=Q/sinτ

(7)

式中τ為角度值。

作為篩選線特征區域的指標,增大Q將加快篩選速度,但是檢測到的線特征會減少。且當Q>10時,線段的形成將出現大量錯誤。為加快線特征提取速度,同時考慮誤差篩選,設置Q參數為2.8。

在LSD算法計算梯度大小和方向之后,需要采用偽排序的方式對梯度進行排序,因為梯度值越大,越有可能是邊緣。設置箱數為1 024,即將梯度值大小(0～255)劃分為1 024個等級,選取梯度最大的點作為種子點,進行區域生長。

在線段支持域形成的過程中,選擇好種子節點(梯度值最大的點)后,需要在一定區域內搜尋同性點。主方向角度差閾值將誤差容忍范圍內的像素點標記為內點,并生成這些內點的外接矩形。如圖2所示,若采用主方向角閾值篩選同性點的方式,可能會將小于這個閾值但不同的2個邊緣劃分到一個矩形當中,可以通過選定矩形內同性點占像素點的密度大小,對矩形進行分割。即當外接矩形區域內部同性點密度很高時,規定外接矩陣密度閾值Td,將大于閾值的外接矩形進行截斷,直至滿足要求。

圖2 相鄰邊緣劃分為一個矩形

密度閾值Td保證了在一個外接矩形中,同性點矩形來自一個邊緣,且矩形不被過度分割。因此,在其他參數固定的情況下,調節密度閾值在0.3～0.9變化,研究密度閾值對線段提取的影響,圖3所示為Euroc數據集實驗結果。

圖3 密度閾值對線段提取時間和精度的影響

由圖3可知,減小密度閾值,能加快線段支持區域的外接矩形分割的速度,且對系統精度影響較小。因此設置Td參數為0.6。

圖4(a)是優化高斯金字塔尺度S、梯度量化值Q、外接矩形密度閾值Td參數后的線特征提取結果。傳統LSD算法提取一幅720×480的圖片需要47.343 ms,共檢測到933條線段。優化后的LSD算法提取時間只需19.626 ms,提升了2.41倍。原因為參數S減小后,高斯金字塔利用的像素點減少,檢測時間減少。同時,Td參數的減小將加速線段支持區域外接矩形分割,檢測到的線段量減少到336條。

圖4 改良參數對LSD算法提取線段的影響

此外,圖4(b)中仍然存在細小線段,對SLAM位姿估計沒有幫助。所以應使用線段抑制策略,剔除線段長度小于一定閾值的結果。

改變長度抑制因子F,圖5是線段提取時間和系統精度變化情況。圖5(a)是長度抑制因子對算法提取速度的影響,易知長度抑制策略并不影響LSD算法提取線段的時間。圖5(b)顯示長度抑制因子在0.04～0.2能改善系統精度,在0.2～0.6對系統精度影響不大,大于0.6后,減小的線特征影響系統的整體精度。由此可知,長度抑制因子越大,系統檢測到的線段越少,即越少的線段約束被添加進后端優化中,系統精度將有所降低。因此設置長度抑制因子參數為0.1。

圖5 長度抑制因子對線段提取時間和精度的影響

長度抑制因子能調節進入后端優化的線條數量,圖6是改變長度抑制因子對線段提取條數的影響。圖6(a)中,在長度抑制因子為0.08時,線段檢測數量減少為82條,基本不存在細小線段。長度抑制因子大于0.2后,參與后端優化的線特征減少,系統精度下降。圖6(b)中,長度抑制因子大于0.7后,線段只能檢測到1條直線,相當于只加入點特征進行優化,精度下降明顯。

圖6 不同長度抑制因子提取線段條數

綜上,減小高斯金字塔尺度S、減小外接矩陣密度閾值Td和增大梯度量化值Q能加快LSD算法,而長度抑制因子F能篩選進入后端優化的線段數量。針對不同參數進行實驗,表2是4組參數下LSD算法的檢測結果,其中參數1條件下系統無法正常運行。綜合考慮LSD算法提取速度和系統精度,最終選定S=0.5,Q=2.8,Td=0.6,F=0.1作為LSD線特征提取算法的參數。

表2 不同參數對LSD算法的影響

1.3 點線特征匹配與篩選

提取點線特征后,需要借助描述子進行匹配。誤匹配將影響位姿解算,降低系統魯棒性,因此匹配關系的篩選就尤為重要。本文級聯交叉驗證、KNN-Matching算法以及RANSAC (隨機樣本一致性方法)3種算法[19-21]進行點線誤匹配的剔除。

在點特征的提取與匹配方面,本文提取加速分段試驗特征(features from accelerated segment test,FAST)角點,匹配二值魯棒獨立初等特征(binary robust independent elementary features,BRIEF)描述子。圖7(a)是提取點特征,并根據漢明距離進行暴力匹配的結果,匹配關系為422對。

圖7 點特征匹配及篩選結果

圖7(b)是將匹配結果的最小距離與最大距離比值大于0.6的點剔除的結果,匹配關系為323對。圖7(c)進一步計算重投影誤差,并使用RANSAC過濾的結果,匹配關系為292對。結果顯示,交叉驗證、距離篩選、RANSAC迭代的組合匹配策略可以很好的降低關鍵幀之間特征點的誤匹配數量。

線特征匹配采用LBD二進制描述子,在加入全局和局部高斯權重系數后,添加線段局部外觀約束和幾何約束,加速特征的匹配。

計算LBD描述子后,首先計算二進制描述子之間的漢明距離,圖8(a)是根據漢明距離暴力匹配的結果,出現了較多誤匹配,匹配關系為65對。為減少誤匹配數目,首先采用KNN將最優匹配距離與次優匹配距離比值小于2/3的匹配剔除,再對優化后的匹配采用交叉驗證。圖8(b)是優化后特征線匹配結果,匹配關系為55對。

圖8 線特征匹配及篩選結果

2 VI-BLSD-Bow系統框架

基于VINS-Mono框架,并引入本文設計的點線特征提取和匹配算法,圖9為本文搭建的VI-BLSD-Bow系統原理。首先進行點線特征提取和篩選,進行單目相機和IMU聯合初始化建立初始地圖;然后在后端構建包含先驗、點線重投影誤差以及IMU誤差的滑動窗口因子圖優化框架;再基于漢明距離匹配進行時間和空間一致性的檢測,確定回環候選幀;最后進行4自由度位姿圖優化,得到全局一致的軌跡和地圖。SLAM系統的前端采用普呂克坐標參數化線特征的旋轉和重投影變換,在后端采用普呂克坐標的正交形式進行參數優化。

圖9 VI-BLSD-Bow系統原理圖

本文搭建的VI-BLSD-Bow系統內部每個線程具體的功能為:

1) 點線特征提取及初始化輸入單目圖像和IMU測量信息,構建移動機器人運動模型,計算IMU預積分,提取本文設計的點線特征并跟蹤,進行初步的位姿解算和誤匹配的剔除,為后端優化提供良好初值。

2) 單目聯合IMU初始化。

基于IMU預積分計算重投影殘差和偏置殘差,通過優化的方式恢復重力方向及絕對尺度,進行單目相機和IMU聯合初始化,更新對應的移動機器人位姿,確認聯合初始化完成程度。

3) 后端優化。

接受前端所有幀間的位姿信息,制定關鍵幀篩選策略,設計點線特征及相機位姿間的幾何約束,建立局部地圖的共視關系,構建包含點線特征和IMU測量信息的因子圖,優化點線誤差和IMU觀測誤差,進行邊緣化處理。

4) 回環檢測及全局位姿優化。

通過詞袋向量和關鍵幀數據庫計算圖像相似度,確定回環候選幀,進行四自由度位姿優化,剔除誤匹配后通過緊耦合的聯合優化 (bundle adjustment, BA)對齊相機位姿與回環位姿,進而對所有關鍵幀進行全局位姿優化,消除累積誤差和軌跡漂移。

線特征的端點在匹配過程中,容易受到遮擋和光照的影響,不確定性較大,不能直接通過線段端點初始化線特征。圖10是空間直線的確定原理,分別通過匹配線段的2個端點與相機光心三點確定平面,再將這2個平面相交獲得空間直線。相機坐標系原點和空間確定的平面為:

圖10 空間直線的確定

πx(x-x0)+πy(y-y0)+πz(z-z0)=0

(8)

(9)

式中:sc1和ec1為左右端點在相機歸一化平面下的坐標;ci為相機坐標系原點。

平面π1和平面π2相交所確定的空間直線,普呂克坐標表達為:

(10)

(11)

3 視覺慣性SLAM系統實驗

本文利用搭載單目相機和IMU的AGV小車進行實驗分析,使用的相機型號為Realsense D435i (不使用深度信息),IMU型號為BMI055,上位機型號為Jetson Nano。首先對單目相機和IMU進行聯合標定,減少相機畸變和IMU零偏影響。然后提取實驗環境圖像信息,針對低紋理環境重新訓練詞袋模型。最后進行室內和走廊場景實驗,針對強光照和低紋理場景,對比VI-BLSD-Bow系統和VINS-Mono系統的定位精度。

3.1 實驗結果定性分析

圖11為在強光照條件進行的室內實驗,在白墻等點特征稀少處,VINS-Mono方案會出現嚴重的軌跡漂移,甚至跟蹤丟失且重定位失敗。但在此場景下,線特征豐富且易于提取,能彌補點特征缺少帶來的影響。分析可知,引入本文設計的線特征提取算法能有效改善系統在復雜環境下,自主跟蹤和定位的能力,提高系統的魯棒性。

圖11 不同算法在室內強光照且點特征缺少條件下的表現

圖12是低紋理走廊實驗,走廊特征分布松散,光照不足,特征點較為稀少,但強約束的線特征分布均勻。與室內場景類似,在AGV小車轉彎處點特征不足,VINS-Mono系統容易產生軌跡漂移并且跟蹤丟失。線特征能迅速補充定位特征,有效提高了系統的穩定性和魯棒性。

圖12 不同算法在點特征缺少走廊條件下的表現

3.2 實驗結果定量分析

在實驗中,為了比較不同算法的實際精度,采用絕對軌跡誤差 (absolute trajectory error, ATE)進行比較。ATE比較待估計軌跡與真實軌跡的偏差,絕對軌跡誤差及絕對平移誤差分別為:

(12)

(13)

式中:Aall表示同時考慮平移和旋轉的誤差;Atrans表示僅考慮平移部分的誤差;下角標truth表示軌跡真值;下角標test表示待估計軌跡。

此外,在進行軌跡誤差分析時還可以只考慮旋轉部分的絕對軌跡誤差。針對不同算法在室內和走廊場景下采集得到的軌跡信息,表3和表4是分別計算得到的絕對軌跡誤差。

表3 室內場景下的不同算法誤差對比

表4 走廊場景下的不同算法誤差對比

由實驗結果可知,無論是否采用回環,本文設計的點線特征提取和匹配策略都可以明顯提升VINS-Mono系統的定位精度。相較于VINS-Mono系統,VI-BLSD-Bow系統在室內環境下定位精度提高了24.2%,走廊環境下精度提高了8%。

4 結論

1) 修改LSD算法隱藏參數,引入長度抑制因子,可以大幅提高線特征提取速度及精度,并篩選進入后端優化的線段數量,提升SLAM系統運行效率。

2) 指定合適的點線特征篩選框架,可以大幅降低點線特征的誤檢問題,進而提升SLAM算法的運行速度和精度。

3) 在點特征缺失條件下,引入線特征可以有效提高SLAM定位系統精度和魯棒性,在SLAM系統遇到大量低紋理墻面時,基于點線特征的系統定位精度遠高于只基于點特征的系統。