點線特征融合的激光雷達單目慣導SLAM 系統

崔云軒，劉桂華，余東應，郭中遠，張文凱

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

0 概述

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

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

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

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

本文結合點線特征的方法對LVI-SAM 系統進行改進，在VIS 系統前端提取與匹配點線特征，并在后端聯合進行BA 優化［16］，實現點線融合。通過短線剔除策略篩選長線特征，落實穩定有效的線特征跟蹤。在LIS 系統中，將線特征的端點與激光雷達深度進行關聯，完成線特征的深度融合，從而獲得準確的空間線特征，進一步提高系統的定位精度。

1 本文系統框架

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

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

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

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

關鍵幀的數量則使用一個滑動窗口來維護，通過以上方式能夠實現PLVIS 系統的局部跟蹤過程。

最后使用DBOW2 詞袋模型［21］實現PLVIS 系統回環檢測，并聯合LIS 系統實現整個PL2VI-SLAM系統的回環檢測。

2 PL2VI-SLAM 系統

PL2VI-SLAM 系統由PLVIS 視覺慣性系統以及LIS 雷達慣性系統組成。其中：PLVIS 系統包括點線特征的提取與匹配、IMU 預積分、點線特征深度關聯以及回環檢測；LIS 系統包括點云特征的提取與匹配、IMU 預積分以及回環檢測。

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

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

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

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

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

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

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

經過以上2 種方法對線特征進行處理后，發現其速度至少是LSD 算法的3 倍。

2.2 點線特征殘差

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

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

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

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

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

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

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

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

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

2.3 PLVIS 子系統

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

對于PLVIS 子系統，當初始化完成后，將激光雷達測量的特征深度值關聯到視覺里程計中，從而獲得一個用于特征深度估計的關鍵幀。將關鍵幀對應的不同時刻的狀態定義為：

其中：R是李群表示的旋轉矩陣；p、v、b是經過IMU預積分獲得的位置、速度以及偏置。在IMU 預積分過程中，將關鍵幀的狀態加入到滑動窗口中，執行BA 優化，從而獲得IMU 坐標系到世界坐標系的變換Q=[R|p]。

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

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

根據IMU 測量值，可以得到相機在不同時刻的位置、速度以及旋轉狀態的表達式如下所示：

其中：Rt是IMU 坐標系到世界坐標系的旋轉變換矩陣，可以理解為的轉置。

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

圖4 視覺點線特征的深度關聯Fig.4 Visual point-line feature depth association

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

圖5 深度特征關聯前后的效果圖Fig.5 Effect drawing before and after depth feature association

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

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

2.4 LIS 子系統

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

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

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

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

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

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

3 實驗與結果分析

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

3.1 線特征匹配效果

基于EUROC 的V1_03 數據集，選取相鄰2 個關鍵幀對線特征進行匹配，匹配結果如圖6 所示（彩色效果見《計算機工程》官網HTML 版本）。

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

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

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

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

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

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

3.2 jackal 數據集

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

圖8 不同系統在jackal 數據集下的軌跡對比Fig.8 Trajectory comparison of different systems under jackal date set

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

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

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

由表2 可知，與其他系統相比，本文系統的位置誤差和旋轉誤差均最小。

3.3 handled 數據集

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

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

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

圖10 不同系統在handled 數據集下的軌跡對比Fig.10 Trajectory comparison of different systems under handled date set

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

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

由表3 可知，在有退化場景的handled 數據集下，本文系統的定位精度優于其他先進的開源SLAM 系統，驗證了本文系統在退化場景下有更好的效果。

3.4 長走廊數據集

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

圖11 低紋理區域Fig.11 Low textured area

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

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

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

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

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

圖13 不同系統在長走廊數據集下的軌跡Fig.13 Trajectories of different systems under long corridor data set

不同系統的端到端誤差對比結果如表4 所示。可以看出，LIO-SAM 和LVI-SAM 系統的軌跡均跟蹤失敗。雖然VINS-MONO 系統跟蹤成功，但最終的位置和起點的位置相差8.87 m，角度誤差為183.36°。而本文系統的位置誤差僅為0.88 m，角度誤差為5.79°，說明本文系統的跟蹤軌跡更接近真實軌跡。

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

最后，本文采用文獻［18］的耗時評估方法，在長走廊數據集下評估各系統處理一幀數據所消耗的平均時間，結果如表5 所示。可以看出，當雷達的旋轉頻率為10 Hz 時，本文系統處理一幀數據所消耗的平均時間為56.83 ms，比LIO-SAM 系統快19.22 ms。因此，本文系統更能滿足實時性要求。

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

4 結束語

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