點線特征融合的激光雷達(dá)單目慣導(dǎo)SLAM 系統(tǒng)

崔云軒，劉桂華，余東應(yīng)，郭中遠(yuǎn)，張文凱

（西南科技大學(xué)信息工程學(xué)院，四川綿陽 621010）

0 概述

同步定位與建圖（Simultaneous Localization and Mapping，SLAM）技術(shù)在移動機器人狀態(tài)估計及環(huán)境感知領(lǐng)域起到重要作用。根據(jù)傳感器的不同，SLAM 技術(shù)分為視覺SLAM 以及激光SLAM 技術(shù)［1-2］?；谝曈XSLAM 技術(shù)的方法適用于位置識別，但容易受到低紋理、光照、劇烈運動的影響；而基于激光SLAM 技術(shù)的方法能夠捕捉遠(yuǎn)程環(huán)境的細(xì)節(jié)信息，精度較高，但會受退化場景的影響。多傳感器融合的SLAM 技術(shù)將相機、激光雷達(dá)、慣性傳感器（Inertial Measurement Unit，IMU）相結(jié)合，能夠進(jìn)一步提高SLAM 系統(tǒng)的魯棒性和精度。因此，激光雷達(dá)、相機及IMU 融合的SLAM 系統(tǒng)也越來越受到人們的關(guān)注。

目前多傳感器融合的SLAM 系統(tǒng)主要分為視覺慣性系統(tǒng)（Visual Inertial System，VIS）、雷達(dá)慣性系統(tǒng)（Lidar Inertial System，LIS）及激光雷達(dá)視覺慣性系統(tǒng)（Lidar Visual Inertial System，LVIS）。根據(jù)傳感器的直接或間接融合測量方法，相機與IMU 的融合方式主要分為松耦合和緊耦合。松耦合方法［3-4］通過兩種方式處理圖像和IMU 的測量數(shù)據(jù)，并分別估計相機和IMU 的狀態(tài)，融合得到最終結(jié)果。緊耦合方法［5-6］將相機與IMU 緊耦合，聯(lián)合優(yōu)化估計狀態(tài)，從而獲得高精度的位姿。與松耦合方法相比，緊耦合方法的魯棒性和精度更高。雖然采用緊耦合融合方式的系統(tǒng)能夠?qū)崿F(xiàn)高精度的位置估計，但依然會受到低紋理場景的影響。因此，研究人員提出PLSLAM 系統(tǒng)［7］、基于單目的點線特征系統(tǒng)［8-10］以及基于雙目的點線特征系統(tǒng)［11-12］，其中PL-SLAM 系統(tǒng)建立在ORB-SLAM 系統(tǒng)［13-14］的基礎(chǔ)上。HE等［15］提出PL-VIO 系統(tǒng)，F(xiàn)U等［16］提出PL-VINS 系統(tǒng)，通過在前端引入線特征，對點線特征聯(lián)合優(yōu)化，克服了低紋理環(huán)境下無法進(jìn)行特征跟蹤的問題。

同樣，LIS 系統(tǒng)也分為緊耦合和松耦合系統(tǒng)。ZHAO等［17］提出一種實時的激光雷達(dá)慣性里程計及建圖系統(tǒng)LIOM，該系統(tǒng)通過卡爾曼濾波融合激光雷達(dá)和IMU 數(shù)據(jù)，實現(xiàn)準(zhǔn)確的位姿估計。文獻(xiàn)［18］介紹了一種緊耦合系統(tǒng)，該系統(tǒng)將激光雷達(dá)里程計、IMU 預(yù)積分因子、GPS 因子以及回環(huán)檢測因子引入因子圖中，聯(lián)合優(yōu)化實現(xiàn)高精度、實時的移動機器人軌跡估計和地圖構(gòu)建。相比于LIOM 系統(tǒng)，該系統(tǒng)通過ISAM2 實現(xiàn)局部和全局的優(yōu)化，其魯棒性和精度更高，但仍然會受到退化場景的影響。

針對退化場景的問題，文獻(xiàn)［19］對緊耦合系統(tǒng)進(jìn)行改進(jìn)，將激光雷達(dá)、相機以及IMU 緊耦合，實現(xiàn)了LVIS 系統(tǒng)，該系統(tǒng)包含VIS 系統(tǒng)和LIS 系統(tǒng)，VIS系統(tǒng)采用VINS-MONO 系統(tǒng)的方法［20］，而LIS 系統(tǒng)依 據(jù)LIO-SAM 系統(tǒng)的方法［18］，將2 個子系統(tǒng)緊耦合，完成系統(tǒng)初始化、狀態(tài)估計和回環(huán)檢測，當(dāng)遇到低紋理或退化場景時會進(jìn)行故障檢測，每次故障檢測均會重新進(jìn)行初始化，這樣雖然能夠阻止系統(tǒng)跟蹤失敗，但會對定位精度產(chǎn)生影響。

本文結(jié)合點線特征的方法對LVI-SAM 系統(tǒng)進(jìn)行改進(jìn)，在VIS 系統(tǒng)前端提取與匹配點線特征，并在后端聯(lián)合進(jìn)行BA 優(yōu)化［16］，實現(xiàn)點線融合。通過短線剔除策略篩選長線特征，落實穩(wěn)定有效的線特征跟蹤。在LIS 系統(tǒng)中，將線特征的端點與激光雷達(dá)深度進(jìn)行關(guān)聯(lián)，完成線特征的深度融合，從而獲得準(zhǔn)確的空間線特征，進(jìn)一步提高系統(tǒng)的定位精度。

1 本文系統(tǒng)框架

本文在LVI-SAM 系統(tǒng)的基礎(chǔ)上實現(xiàn)PL2VISLAM 系統(tǒng)，系統(tǒng)的整體框架如圖1 所示，該系統(tǒng)融合了視覺慣導(dǎo)緊耦合系統(tǒng)（Point-Line with Visual Inertial System，PLVIS）和LIS 系統(tǒng)，PLVIS 系統(tǒng)包括相機IMU 初始化、特征提取與匹配、位姿跟蹤、局部地圖構(gòu)建及回環(huán)檢測。LIS 系統(tǒng)包括雷達(dá)、IMU 初始化、點云提取與匹配、局部建圖及回環(huán)檢測。相比于LVI-SAM 系統(tǒng)，本文系統(tǒng)在PLVIS 系統(tǒng)初始化階段以及前端里程計部分融合了點線特征的提取與匹配，后端采用滑動窗口優(yōu)化算法限制匹配的點特征與線特征的數(shù)量。

相較于VINS-MONO 系統(tǒng)，本文系統(tǒng)在初始化階段增加了線特征，促進(jìn)系統(tǒng)的初始化進(jìn)程。通過重投影誤差對點線特征進(jìn)行優(yōu)化，從而獲得三維點線特征，同時以LIS 系統(tǒng)的初始化成功與否作為整體系統(tǒng)初始化是否成功的判斷依據(jù)。初始化成功后根據(jù)構(gòu)建的靜態(tài)局部地圖實現(xiàn)連續(xù)幀的位姿估計。

在PLVIS 系統(tǒng)跟蹤的過程中，同時進(jìn)行點線特征與關(guān)鍵幀的選擇，如果點線特征跟蹤的關(guān)鍵幀數(shù)量超過設(shè)定的閾值a，即可判定為跟蹤成功，并將其添加到全局地圖中，點線特征跟蹤數(shù)量的閾值表達(dá)式如式（1）所示：

其中：p是觀測到的點特征；l是線特征。

關(guān)鍵幀的數(shù)量則使用一個滑動窗口來維護(hù)，通過以上方式能夠?qū)崿F(xiàn)PLVIS 系統(tǒng)的局部跟蹤過程。

最后使用DBOW2 詞袋模型［21］實現(xiàn)PLVIS 系統(tǒng)回環(huán)檢測，并聯(lián)合LIS 系統(tǒng)實現(xiàn)整個PL2VI-SLAM系統(tǒng)的回環(huán)檢測。

2 PL2VI-SLAM 系統(tǒng)

PL2VI-SLAM 系統(tǒng)由PLVIS 視覺慣性系統(tǒng)以及LIS 雷達(dá)慣性系統(tǒng)組成。其中：PLVIS 系統(tǒng)包括點線特征的提取與匹配、IMU 預(yù)積分、點線特征深度關(guān)聯(lián)以及回環(huán)檢測；LIS 系統(tǒng)包括點云特征的提取與匹配、IMU 預(yù)積分以及回環(huán)檢測。

2.1 視覺點線特征提取與匹配

首先，每一幀圖像采用KLT 稀疏光流法［22］跟蹤前一幀點特征，并提取當(dāng)前幀新的點特征，使每幀圖像中的點特征數(shù)量不少于100 個，然后在2 個相鄰特征之間設(shè)置最小像素分離值來實現(xiàn)統(tǒng)一的特征分布。根據(jù)RANSAC 算法的基本矩陣模型［23］，能夠檢驗特征的離群值，并剔除離群值大的特征點。通過離群值檢驗的點將投影到單位球體上，然后延伸到深度歸一化平面上，獲得特征點的最終位置。

由于OpenCV 庫中的LSD 算法包含大量的短線特征，計算代價較大，若單純使用LSD 算法，很難滿足實時性要求。因此采用線特征長度篩選策略，保留主線特征而剔除不必要的短線特征，既能滿足實時性要求，又能提高位姿精度。

長度拒絕策略是通過定義特征線的最小長度來拒絕短線特征，線特征長度的最低閾值表達(dá)式如式（2）所示：

其中：(WI,HI)表示輸入圖像的最小長度或高度；u是比例因子，u越大，拒絕的線特征長度就越長，本文采用PL-VINS 系統(tǒng)的拒絕策略，使u=0.125。線特征提取完成后，分 別采用LBD 算法［24］和KNN算法［25］來描述和匹配線特征。對于不必要的線特征，用以下2 種方法剔除：

1）通過KNN 算法計算線特征之間的漢明距離，如果大于30，則剔除，否則保留。

2）測試所匹配的線特征間的角度是否小于0.1 rad，如果是，則保留，否則剔除。

經(jīng)過以上2 種方法對線特征進(jìn)行處理后，發(fā)現(xiàn)其速度至少是LSD 算法的3 倍。

2.2 點線特征殘差

匹配完點線特征后，需要構(gòu)建點線特征的殘差模型。本文依據(jù)VINS-MONO 系統(tǒng)的方法構(gòu)建點的殘差。定義一個單位球體上的相機測量殘差，對于第i幀首次觀察到的第n個特征，該特征對應(yīng)的第j幀觀測的特征殘差為：

由于視覺殘差的自由度為2，因此，將殘差向量投影到切平面上，如圖2 所示。其中，b1、b2是任意選擇的正交基，它們跨越正切平面。

圖2 點特征殘差示意圖Fig.2 Schematic diagram of point feature residuals

首先建立圖像平面上的線特征對應(yīng)關(guān)系，然后通過三角化這些對應(yīng)關(guān)系來初始化世界坐標(biāo)系中的空間線lw，其端點為M、N，如圖3 所示。

圖3 線特征的重投影誤差示意圖Fig.3 Schematic diagram of reprojection error of line features

線特征的重投影誤差是根據(jù)點線距離進(jìn)行建模的。首先定義空間線，然后定義線的幾何變換，給定一個世界坐標(biāo)系到相機坐標(biāo)系的變換矩陣Qcw=[Rcw,tcw]，Rcw和tcw分別表示旋轉(zhuǎn)矩陣與平移向量。根據(jù)變換矩陣Qcw，可以得到lw從世界坐標(biāo)系變換到相機坐標(biāo)系下的坐標(biāo)lc表達(dá)式：

將lc轉(zhuǎn)換到圖像平面上，獲得投影線l［26］：

其中：Kl表示線投影矩陣；nc可從式（4）獲得。假設(shè)lw表示由第i幀ci觀測到的第j個空間線lj，則線特征的重新投影誤差可定義為：

其中：d(p,l)表示點到直線的距離函數(shù)；p為端點M'、N'的坐標(biāo)；l1、l2、l3均為直線方程系數(shù)；p是線特—征中點的齊次坐標(biāo)。

2.3 PLVIS 子系統(tǒng)

無論是子系統(tǒng)的初始化還是狀態(tài)估計，都離不開IMU 積分模型。通過IMU 積分可以獲得機器人在任意時刻的位置、速度以及姿態(tài)，從而為本文系統(tǒng)提供高精度的狀態(tài)估計。而在IMU 積分的過程中，系統(tǒng)會受到加速度和陀螺儀偏置的影響，如果每次計算都要將IMU 坐標(biāo)系轉(zhuǎn)換到世界坐標(biāo)系，計算量將非常大，嚴(yán)重影響系統(tǒng)的實時性，因此本文采用IMU 預(yù)積分模型［27］。

對于PLVIS 子系統(tǒng)，當(dāng)初始化完成后，將激光雷達(dá)測量的特征深度值關(guān)聯(lián)到視覺里程計中，從而獲得一個用于特征深度估計的關(guān)鍵幀。將關(guān)鍵幀對應(yīng)的不同時刻的狀態(tài)定義為：

其中：R是李群表示的旋轉(zhuǎn)矩陣；p、v、b是經(jīng)過IMU預(yù)積分獲得的位置、速度以及偏置。在IMU 預(yù)積分過程中，將關(guān)鍵幀的狀態(tài)加入到滑動窗口中，執(zhí)行BA 優(yōu)化，從而獲得IMU 坐標(biāo)系到世界坐標(biāo)系的變換Q=[R|p]。

1）初始化。對于PLVIS 系統(tǒng)，初始化的質(zhì)量在很大程度上取決于IMU 傳感器的運動和精度，當(dāng)IMU 傳感器以較小速度或勻速移動時，VINSMONO 系統(tǒng)無法進(jìn)行初始化，這是因為當(dāng)加速度不大時，尺度無法觀測，而IMU 傳感器的偏置和噪聲影響了原始加速度的測量，所以初始化能夠?qū)@些參數(shù)有良好的猜測，會使后續(xù)的迭代優(yōu)化更快收斂。相比于VINS-MONO 系統(tǒng)，本文系統(tǒng)在初始化時，首先初始化LIS 系統(tǒng)，并獲得狀態(tài)x和偏置b，通過線性插值法將狀態(tài)x和偏置b插入關(guān)鍵幀中，并作為PLVIS 系統(tǒng)的初始化猜測，從而保證初始化的速度和魯棒性。其次，為進(jìn)一步提高初始化的魯棒性，增加了線特征的初始化，使系統(tǒng)能更好地應(yīng)對低紋理情況。

2）IMU 預(yù)積分。根據(jù)IMU 傳感器的原始信息，可以定義IMU 傳感器的測量值為：

根據(jù)IMU 測量值，可以得到相機在不同時刻的位置、速度以及旋轉(zhuǎn)狀態(tài)的表達(dá)式如下所示：

其中：Rt是IMU 坐標(biāo)系到世界坐標(biāo)系的旋轉(zhuǎn)變換矩陣，可以理解為的轉(zhuǎn)置。

3）特征深度關(guān)聯(lián)。在PLVIS 系統(tǒng)初始化以及視覺里程計中，本文將LIS 系統(tǒng)中激光雷達(dá)測量的深度值作為視覺特征的深度。由于激光雷達(dá)產(chǎn)生的是稀疏的點云，因此需要通過疊加多個激光雷達(dá)幀，才能獲得特征密集的深度圖。相比于LVI-SAM 系統(tǒng)，本文系統(tǒng)不僅將點特征與深度值關(guān)聯(lián)起來，而且將線特征端點與深度值聯(lián)系起來。將點特征、線特征端點、激光雷達(dá)深度點投影到一個單位球體上。此時，降采樣激光雷達(dá)深度點云在單位球體上利用視覺特征的極坐標(biāo)搜索二維K-D 樹。找到球體上對應(yīng)距離最近的3 個深度特征，此特征深度即為相機中心到視覺特征的距離。同理，可以獲得線特征端點的深度。視覺點線特征的深度關(guān)聯(lián)示意圖如圖4 所示，其中，虛線的長度代表特征深度。

圖4 視覺點線特征的深度關(guān)聯(lián)Fig.4 Visual point-line feature depth association

投影到球體上的3 個深度特征進(jìn)行逆投影后，它們的距離可能相差甚遠(yuǎn)，如圖4（b）所示，ti時刻和tj時刻疊加的激光雷達(dá)幀，它們的深度特征間距d很大，這樣會嚴(yán)重影響視覺特征的深度估計。因此，需要檢查逆投影后的深度特征之間的距離，當(dāng)其中某個深度特征的距離超過了本文設(shè)定的閾值2 m 時，即可判斷對應(yīng)的視覺特征沒有深度，從而更有利于還原真實場景。視覺特征深度關(guān)聯(lián)的效果如圖5 所示。在圖5（b）中，綠色代表關(guān)聯(lián)的點特征，藍(lán)色代表關(guān)聯(lián)的線特征（彩色效果見《計算機工程》官網(wǎng)HTML 版本）。上述的深度特征均指激光雷達(dá)觀測到的特征。

圖5 深度特征關(guān)聯(lián)前后的效果圖Fig.5 Effect drawing before and after depth feature association

4）故障檢測。PLVIS 系統(tǒng)容易受到劇烈運動、光照變化和低紋理環(huán)境影響。當(dāng)機器人經(jīng)歷劇烈運動或進(jìn)入低紋理環(huán)境時，被跟蹤特征的數(shù)量將大大減少，可能會導(dǎo)致優(yōu)化失敗，并產(chǎn)生較大的IMU 偏差。LVI-SAM 系統(tǒng)的解決辦法是當(dāng)跟蹤的特征數(shù)量低于閾值時，系統(tǒng)進(jìn)行故障檢測并重新進(jìn)行初始化。但每次初始化時，如果PLVIS 系統(tǒng)遇到低紋理場景，初始化將失敗，導(dǎo)致定位失敗，定位精度降低。因此，本文加入線特征，只有當(dāng)跟蹤的線特征數(shù)量低于閾值時才進(jìn)行故障檢測。

5）視覺回環(huán)檢測?；丨h(huán)檢測模塊利用了DBoW2 詞袋模型，當(dāng)新的關(guān)鍵幀到來時，將提取簡短的二進(jìn)制BRIEF 描述子［28］，并且將其與之前的描述子進(jìn)行匹配，如果相似度超過了設(shè)定的閾值，即可判定該幀是一個回環(huán)幀。此外，PLVIS 系統(tǒng)一旦檢測到回環(huán)，就會將此信息傳達(dá)給LIS 系統(tǒng)，并將DBoW2 詞袋模型得到的回環(huán)幀的時間戳發(fā)送給LIS系統(tǒng)進(jìn)行驗證，如果該時間戳對應(yīng)的關(guān)鍵幀在LIS系統(tǒng)里也是回環(huán)幀，則可驗證該幀為回環(huán)幀。

2.4 LIS 子系統(tǒng)

圖1 左側(cè)所示為基于LIO-SAM 系統(tǒng)實現(xiàn)的LIS子系統(tǒng)框架。LIO-SAM 系統(tǒng)中的因子圖包含了IMU 預(yù)積分因子、激光雷達(dá)里程計因子、回環(huán)檢測因子以及可選的GPS 因子。而LIS 子系統(tǒng)還包含了視覺里程計因子，并將它們添加到因子圖中，通過ISAM2 算法［29］優(yōu)化，獲得一個高精度的里程計。IMU 預(yù)積分過程在2.3 節(jié)已介紹過，在因子圖中，只需添加IMU 預(yù)積分和視覺里程計估計的狀態(tài)，并與其他因子進(jìn)行聯(lián)合優(yōu)化。因此，下文僅介紹激光雷達(dá)里程計和回環(huán)檢測。

1）激光雷達(dá)里程計約束。在LIS 子系統(tǒng)中，若要實現(xiàn)一個完整的激光雷達(dá)里程計，需要對激光雷達(dá)檢測到的點云特征進(jìn)行提取與匹配。本文采用LOAM 系統(tǒng)的特征提取方式，根據(jù)局部區(qū)域點的曲率來提取邊緣和平面的特征。若將第i幀提取的邊緣和平面特征分別表示為則該時刻提取的所有特征組成的激光雷達(dá)幀為Fi，可以表示為：

由于LIS 系統(tǒng)是基于LIO-SAM 系統(tǒng)實現(xiàn)的，其關(guān)鍵幀的選擇是通過一種簡單有效的啟發(fā)式方法：與之前的狀態(tài)xi相比，當(dāng)機器人姿態(tài)變化超過用戶定義的閾值時，則選擇激光雷達(dá)幀F(xiàn)i+1作為關(guān)鍵幀。在因子圖中，新保存的關(guān)鍵幀F(xiàn)i+1與一個新的機器人狀態(tài)節(jié)點xi+1關(guān)聯(lián)。2 個關(guān)鍵幀之間的激光雷達(dá)幀被丟棄。通過這種方式添加關(guān)鍵幀，既能平衡建圖的密度和內(nèi)存消耗，又能保持相對稀疏的因子圖，從而實現(xiàn)實時的非線性優(yōu)化。每增加一個新的關(guān)鍵幀，其距離變化和旋轉(zhuǎn)角度變化的閾值選分別為1m和10°。在此基礎(chǔ)上，使用1 個滑動窗口來包含固定數(shù)量的子關(guān)鍵幀{Fi-n,Fi-n+1,…,Fi}，子關(guān)鍵幀在世界坐標(biāo)系下的變換為{Qi-n,Qi-n+1,…,Qi}，將其轉(zhuǎn)換到世界坐標(biāo)系下，最終合并到一個體素地圖Mi中。根據(jù)所提取的邊緣特征和面特征，對應(yīng)的邊緣特征體素地圖和平面特征體素地圖可以表示為和激光雷達(dá)幀和體素地圖的關(guān)系可以表示為：

盡管激光雷達(dá)可以很好地提供深度特征，然而如果遇到退化場景，也會誤匹配。不僅會影響LIS 系統(tǒng)的狀態(tài)估計，也會影響PLVIS 系統(tǒng)的狀態(tài)估計。因此，本文利用文獻(xiàn)［30］提出的方法，將掃描匹配中的非線性問題表示成迭代求解的線性問題：

其中：A和b由線性化位姿Q得到，如果AT×A獲得的最小特征值超過了設(shè)定的閾值，說明分解Q后的矩陣A是非奇異的，此時LIS 系統(tǒng)會發(fā)送故障，使當(dāng)前幀的激光雷達(dá)里程計因子不能添加到因子圖中。

2）回環(huán)檢測?；丨h(huán)檢測是根據(jù)點云之間的歐幾里得距離實現(xiàn)的，能通過ISAM2 算法集成到因子圖中。當(dāng)一個新的狀態(tài)xi到來時，首先通過歐式距離找到與xi狀態(tài)最為接近的候選狀態(tài)xk(k=1,2,…,i-1)，然后將關(guān)鍵幀F(xiàn)i與Fk轉(zhuǎn)換到世界坐標(biāo)系下，得到相對變換ΔTi,k，并將其添加到因子圖中，最后通過前端掃描匹配法對Fi和{Fk-j,Fk-j+1,…,Fk+j-1,…,Fk+j}進(jìn)行匹配。在LIS 系統(tǒng)中，j=12，歐氏距離設(shè)置為15 m。

3 實驗與結(jié)果分析

首先選用EUROC 數(shù)據(jù)集對線特征匹配效果進(jìn)行分析，然后使用LVI-SAM 系統(tǒng)提供的手持式handled 數(shù)據(jù)集以及移動小車jackal 數(shù)據(jù)集進(jìn)行定位精度分析。最后，為驗證本文系統(tǒng)的有效性，將realsensD435i 相機、xsens 慣導(dǎo)傳感器以及VLP16 線激光雷達(dá)剛性連接在一起，并標(biāo)定它們之間的外參，手持傳感器錄制一個長走廊的數(shù)據(jù)集，并對端到端的誤差進(jìn)行評估。本文選用的數(shù)據(jù)集包含低紋理和退化場景。實驗設(shè)備配置為4核i7處理器，8 GB 內(nèi)存，GTX1060，ubuntu16.04 操作系統(tǒng)。

3.1 線特征匹配效果

基于EUROC 的V1_03 數(shù)據(jù)集，選取相鄰2 個關(guān)鍵幀對線特征進(jìn)行匹配，匹配結(jié)果如圖6 所示（彩色效果見《計算機工程》官網(wǎng)HTML 版本）。

圖6 不同算法的匹配結(jié)果對比Fig.6 Comparison of matching results of different algorithms

從圖6 可以看出，當(dāng)場景中的紋理較少時，LSD算法雖然能夠進(jìn)行線特征匹配，但會存在大量的短線特征與誤匹配，如圖中的黃線所示。而本文算法能剔除短線特征，在提高計算速度的同時也減少了誤匹配，效果明顯比LSD 算法好。對線特征匹配數(shù)量及耗時進(jìn)行對比分析，結(jié)果如表1 所示。

表1 匹配耗時及匹配數(shù)量對比Table 1 Comparison of matching time and matching quantity

由表1 可知，LSD 算法匹配耗時最長，而本文算法保留了主線特征，且匹配速度是LSD 算法的3 倍。

本文系統(tǒng)在運行jackal 數(shù)據(jù)集的過程中，其三維可視化效果如圖7 所示（彩色效果見《計算機工程》官網(wǎng)HTML 版本）。其中，紅色的線是三維線特征，紫色和白色的點是局部視覺特征，周圍的彩色點云是不同距離上的激光雷達(dá)掃描的點云。

圖7 線特征三維可視化的效果Fig.7 Effect of 3D visualization of line features

3.2 jackal 數(shù)據(jù)集

在jackal數(shù)據(jù)集中，以GPS 軌跡作為groundtruth，將本文系統(tǒng)分別和LVI-SAM、LIO-SAM 及VINSMONO 系統(tǒng)進(jìn)行軌跡對比（在進(jìn)行jackal 和handled數(shù)據(jù)集實驗時，LIO-SAM 系統(tǒng)和LVI-SAM 系統(tǒng)均加了GPS 因子），結(jié)果如圖8 所示。

圖8 不同系統(tǒng)在jackal 數(shù)據(jù)集下的軌跡對比Fig.8 Trajectory comparison of different systems under jackal date set

圖8對應(yīng)的GPS軌跡中有部分震蕩，原因是該部分在實際情況中受到了樹木的遮擋。由圖8可以看出，VINSMONO系統(tǒng)在運行一段時間后跟蹤丟失，而本文系統(tǒng)能夠有效實現(xiàn)位姿跟蹤，且相比于LIO-SAM 系統(tǒng)和LVISAM 系統(tǒng)，本文系統(tǒng)的軌跡更接近GPS groundtruth。

jackal數(shù)據(jù)集由移動小車收集，且起點與終點位置一樣，運動穩(wěn)定。本文分析了不同系統(tǒng)間端到端的誤差，使用位置誤差和旋轉(zhuǎn)誤差進(jìn)行評估，兩個誤差越接近0，定位精度越高，結(jié)果如表2 所示。

表2 不同系統(tǒng)的端到端誤差對比Table 2 Comparison of end-to-end error of different systems

由表2 可知，與其他系統(tǒng)相比，本文系統(tǒng)的位置誤差和旋轉(zhuǎn)誤差均最小。

3.3 handled 數(shù)據(jù)集

handled 數(shù)據(jù)集是操作者手持傳感器錄制的，相比于jackal 數(shù)據(jù)集，操作者的運動更劇烈，且有多處是退化場景。如圖9 所示，當(dāng)激光雷達(dá)與地面垂直或處于空曠地帶時，激光雷達(dá)將無法提取足夠的點云，導(dǎo)致掃描匹配失敗，從而使里程計跟蹤失敗。

圖9 兩種退化場景的示意圖Fig.9 Schematic diagram of two degradation scenarios

本文對比了不同系統(tǒng)在handled 數(shù)據(jù)集下的軌跡，結(jié)果如圖10 所示。由圖10 可知，與其他系統(tǒng)相比，本文系統(tǒng)和LVI-SAM 系統(tǒng)的軌跡更接近groundtruth。

圖10 不同系統(tǒng)在handled 數(shù)據(jù)集下的軌跡對比Fig.10 Trajectory comparison of different systems under handled date set

使用絕對軌跡誤差（Absolute Trajectory Error，ATE）和相對軌跡誤差（Relative Pose Error，RPE）中的均方根誤差（Root Mean Squared Error，RMSE）來評估不同系統(tǒng)的全局定位精度，結(jié)果如表3 所示。

表3 不同系統(tǒng)的定位精度對比Table 3 Comparison of positioning accuracy of different systems m

由表3 可知，在有退化場景的handled 數(shù)據(jù)集下，本文系統(tǒng)的定位精度優(yōu)于其他先進(jìn)的開源SLAM 系統(tǒng)，驗證了本文系統(tǒng)在退化場景下有更好的效果。

3.4 長走廊數(shù)據(jù)集

為驗證本文系統(tǒng)的有效性，本文錄制了具有低紋理與退化場景的長走廊數(shù)據(jù)集。在長走廊的另一端，特意對傳感器進(jìn)行旋轉(zhuǎn)，將激光雷達(dá)掃描線與地面垂直，并讓相機觀測低紋理區(qū)域，低紋理區(qū)域如圖11 所示。

圖11 低紋理區(qū)域Fig.11 Low textured area

此外，本文進(jìn)行了線特征匹配，結(jié)果如圖12（a）和圖12（b）所示。其中，圖12（a）對應(yīng)未退化場景，圖12（b）對應(yīng)低紋理退化場景，退化場景下的激光雷達(dá)掃描線是與地面垂直的。

圖12 低紋理場景下的線特征匹配結(jié)果Fig.12 Line feature matching results under low texture scenes

由于長走廊數(shù)據(jù)集中傳感器的起始位置和終端位置一樣，且終端的激光雷達(dá)坐標(biāo)系在初始的激光雷達(dá)坐標(biāo)系的x軸方向上旋轉(zhuǎn)了180°，因此本文通過評估各個系統(tǒng)的端到端誤差，從而進(jìn)一步驗證本文系統(tǒng)的有效性。設(shè)起點位置為（0，0，0），終點位置為（x，y，z），則終點位置與起點位置之間的誤差可以使用如式（21）所示的三維空間歐氏距離表示：

角度誤差為起點位置和終點位置的歐拉角之差：

此外，在長走廊數(shù)據(jù)集下對比了各個系統(tǒng)的軌跡，結(jié)果如圖13 所示。可以看出，本文系統(tǒng)可以很好地還原真實軌跡情況，能夠保持起點和終點位置一致，而LVI-SAM 和LIO-SAM 系統(tǒng)在激光雷達(dá)進(jìn)行垂直翻轉(zhuǎn)時均發(fā)生了漂移。雖然VINS-MONO 系統(tǒng)的軌跡完整，但在相機翻轉(zhuǎn)的時候掃到了低紋理區(qū)域，其軌跡發(fā)生了偏移，導(dǎo)致終點和起點位置不一致。

圖13 不同系統(tǒng)在長走廊數(shù)據(jù)集下的軌跡Fig.13 Trajectories of different systems under long corridor data set

不同系統(tǒng)的端到端誤差對比結(jié)果如表4 所示?？梢钥闯觯琇IO-SAM 和LVI-SAM 系統(tǒng)的軌跡均跟蹤失敗。雖然VINS-MONO 系統(tǒng)跟蹤成功，但最終的位置和起點的位置相差8.87 m，角度誤差為183.36°。而本文系統(tǒng)的位置誤差僅為0.88 m，角度誤差為5.79°，說明本文系統(tǒng)的跟蹤軌跡更接近真實軌跡。

表4 不同系統(tǒng)的端到端誤差對比Table 4 Comparison of end-to-end error of different system

最后，本文采用文獻(xiàn)［18］的耗時評估方法，在長走廊數(shù)據(jù)集下評估各系統(tǒng)處理一幀數(shù)據(jù)所消耗的平均時間，結(jié)果如表5 所示?？梢钥闯觯?dāng)雷達(dá)的旋轉(zhuǎn)頻率為10 Hz 時，本文系統(tǒng)處理一幀數(shù)據(jù)所消耗的平均時間為56.83 ms，比LIO-SAM 系統(tǒng)快19.22 ms。因此，本文系統(tǒng)更能滿足實時性要求。

表5 不同系統(tǒng)處理一幀數(shù)據(jù)消耗的平均時間對比Table 5 Comparison of average time consumed by different systems to process one frame date

4 結(jié)束語

本文提出一種視覺點線特征融合的激光雷達(dá)單目慣導(dǎo)SLAM 系統(tǒng)。該系統(tǒng)主要由LIS 系統(tǒng)及點線融合的PLVIS 系統(tǒng)組成，2 個子系統(tǒng)以緊耦合的方式構(gòu)建了一個完整的激光雷達(dá)視覺慣性里程計，能夠有效應(yīng)對低紋理和退化場景。在初始化階段，將LIS子系統(tǒng)的初始化成功與否作為判斷依據(jù)，提升系統(tǒng)的魯棒性，并在狀態(tài)跟蹤階段觀測線特征，提升整個系統(tǒng)在退化場景的跟蹤能力。最后，PLVIS 子系統(tǒng)將回環(huán)檢測幀傳遞給LIS 子系統(tǒng)，進(jìn)一步修正系統(tǒng)軌跡。在jackal、handled 以及自制的長走廊數(shù)據(jù)集上的實驗結(jié)果表明，與VINS-MONO、LIO-SAM、LVI-SAM 系統(tǒng)相比，本文系統(tǒng)具有更高的定位精度和魯棒性。但該系統(tǒng)會由于場景中存在動態(tài)物體而導(dǎo)致相機跟蹤失敗。因此，下一步將考慮結(jié)合深度學(xué)習(xí)，剔除場景中的動態(tài)物體，實現(xiàn)更加魯棒的位姿跟蹤，使該系統(tǒng)適用于復(fù)雜環(huán)境。