基于關鍵幀的雙模式車輛激光SLAM

中圖分類號：TP399 文獻標志碼：A

Abstract：Simultaneous localization and mapping（SLAM）technology is widely used inthe field of autonomous driving，where accuracyand computational eficiencyare the two most important indicators.However，traditional LiDAR odometry faces challenges inaccurately and eficiently extracting keyframes，resulting inanexcess of redundant frames during map construction. Additionally，the majority of LiDAR odometry systems require aligning each frame to the map，which imposes a substantial computational burden.This paper proposes a dual-mode LiDAR odometry and mapping method based on keyframes.Bycomputing the feature similarity between two point clouds and comparing it with a motion-adaptive threshold，keyframes are extracted.Subsequently，diferent registration algorithmsare applied to keyframes and non-keyframes to minimize computational resource consumption. Furthermore，a weight function is calculated using point horizontal distance information and integrated into the weighted pose constraints.The SLAM system proposedundergoes extensive testing on the KITTI dataset and real vehicles.The results from KITTI sequences OO-1O demonstrate a translational error of only 0.56% and a rotational error of O.O021 degree/m.In terms of real-time performance，compared with F-LOAM，our algorithm improves average speed by 26.5% ，and even outperforms lightweight systemLeGO-LOAM.

Key Words：autonomous driving; keyframe; simultaneous localization and mapping （SLAM）；dual-mode

隨著自動駕駛的興起，SLAM技術受到了廣泛關注，SLAM依靠多種傳感器感知外部環境，解算位姿實現精確的實時定位，是自動駕駛技術的重要一環.現階段，SLAM在各種定位環境中得到越來越廣泛的應用，特別是在城市、峽谷等GNSS信號差的場景.與基于視覺的SLAM相比，激光雷達具有高精度和不受光線明暗變化影響等優點，在自動駕駛領域越來越受青睞，已經成為自動駕駛車輛的標配傳感器.近年來，出現了許多優秀的激光雷達SLAM算法[1-3].

自動駕駛車輛的計算平臺需要同時支持感知、規控、定位、控制等多方面算力需求，其計算資源有限，能夠支持SLAM的算力很少，因此實時性是SLAM的關鍵指標之一.最初，廣泛使用的經典迭代最近點（iterativeclosestpoint，ICP）算法[4]及其變體被用于直接對齊原始點云，這些方法統稱為基于ICP的方法[5-7.ICP算法最小化對齊誤差，迭代優化相對位姿使得點到點的誤差和最小.然而，ICP算法處理的是原始點云，計算代價大且對噪聲敏感.此外，傳統的ICP方法及其變體通過構建k維 （k-d） 樹進行最近鄰搜索，其計算成本隨地圖規模的擴大而不斷增加.FAST-LIO2[8直接進行點云配準，不依賴特征提取，并引入了一種ikd-Tree結構，確保計算資源需求不會隨著地圖大小的增加而呈線性增長.

LOAM的提出開辟了基于特征的激光SLAM方法.在基于特征的方法[10-12]中，LOAM基于點的局部平滑度選擇邊緣和表面特征，通過特征點對構造優化問題，顯著減少了待匹配點的數量.LOAM將高頻率的幀間匹配和低頻率的幀到地圖配準相結合，并引入了點到線和點到面殘差建立優化模型，顯著提高了特征法 SLAM的準確性和效率.Lio-mapping[13]對滑動窗口內的所有幀執行幀到地圖的配準，實時性低下.F-LOAM[4簡化了LOAM方法，消除高頻率的幀間匹配，直接采用單幀到局部地圖的配準來進行位姿優化.與LOAM相比，F-LOAM在保持精度的同時，平均處理速度提高了3倍.MULLS[15]提出了一種基于點、線和面特征的多尺度最小二乘優化方法，這是一種高效且低漂移的3D激光SLAM系統.

值得注意的是，上述所有方法都忽視了關鍵幀的篩選，地圖規模增長較快，這會影響回環檢測和后端優化的計算效率.關鍵幀策略旨在從一系列普通幀中選擇一個幀作為局部幀的代表.一個良好的關鍵幀策略可以評估點云和特征的質量，篩選特征穩定豐富的幀作為關鍵幀.通過關鍵幀策略可以過濾掉冗余和不穩定幀，這可以防止不相關或錯誤的數據滲入優化過程，從而影響定位和建圖的準確性.此外，關鍵幀策略可以減少待優化的幀的數量，從而提高閉環檢測的準確率和效率.在后端優化過程中，關鍵幀充當了局部普通幀的代表，大幅壓縮了后端優化的規模，在保證后端優化精度的同時，也大大減輕了計算壓力.

然而，激光點云是無序的，并且缺乏環境紋理信息，故提取關鍵幀是一個艱巨的挑戰，而現有的相關工作明顯不足.因此，一些激光SLAM系統不會檢測關鍵幀，或者主要依賴粗糙的幀間角度和平移變化來確定關鍵幀，這種方法缺乏精度和穩定性，并且難以適應不同工況.LeGO-LOAM是一種輕量級的LiDARSLAM，它采用聚類算法進行點云分割，并引入兩步列文伯格-馬夸爾特優化方法.此外，它還采用了粗略的關鍵幀策略，提高了其實時性能.然而，LeGO-LOAM將所有距離超過 0.3m 的幀都標記為關鍵幀，關鍵幀提取效果較差，對LeGO-LOAM的定位精度具有負面影響.在LIO-SAM[1中，當空間距離和旋轉變化超過固定閾值時，當前幀被指定為新的關鍵幀，只有關鍵幀才被納入后端因子地圖的優化中.盡管該算法引人了關鍵幀策略以提升計算效率和優化效果，但在不同工況下，連續幀之間的平移與旋轉變化幅度存在較大差異，因此難以確定一個在不同工況下均適用的統一閾值.在LILI- ?0M^[18] 中，考慮了當前幀與局部特征地圖之間的特征重疊率.如果重疊率低于 60% ，或者當前幀與最新關鍵幀之間的時間差超過了定義的閾值，那么當前幀就被指定為新的關鍵幀.如上所述，現有的激光SLAM缺乏穩定有效的關鍵幀提取方法，這是影響SLAM定位精度和計算效率的關鍵因素.Shen等[9]提出了一種基于幀之間語義差異的自適應選擇策略，其效果依賴于語義分割精度，泛化性較差，在不同場景難以自適應應用，并且語義分割對算力要求更高 .Zu 等[20]提出了一種基于視覺的關鍵幀動態閾值設置方法，利用統計理論分析各幀之間的姿態差來表征攝像機的運動特性，采用多層模糊推理機制進行聯合判斷，得出關鍵幀篩選的動態閾值，應用在ORB-SALM3系統自適應選擇關鍵幀.InfoLa-SLAM[21]提出了一種使用費舍爾信息矩陣去度量當前幀和上一關鍵幀配準誤差，平均信息值越大，表示配準誤差越小，以此篩選關鍵幀.該方法需要先進行配準，需要容忍一定的配準誤差才能保證稀疏有效的關鍵幀，精度和實時性難以平衡.

另外，位姿優化模型會給所有點對分配相同的權重，盡管并非所有的幾何約束對位姿優化的影響都相同[22].F-LOAM優先對具有較高局部平滑度的邊緣特征和具有較低局部平滑度的平面特征進行匹配，相應地分配更高的權重進行加權優化.KFS-LIO[23提出了一個特征評估函數來衡量各個點的可信度，權衡它們對殘差的貢獻.WiCRF224基于運動可觀測性提出了一種加權方法，提高了系統的魯棒性和準確性.ROI-cloud25提出了一種基于點云的體素化方法，并通過粒子濾波選擇感興趣的區域進行注冊，將處理限制在感興趣區域內的點上.

針對上述問題，我們提出了一種基于關鍵幀的雙模式車輛激光SLAM，衡量幀間特征差異以篩選關鍵幀，并基于關鍵幀檢測結果執行雙模式的激光里程計配準算法.受到Scan-Context26]和 Inis^[27] 的啟發，引入了一種利用點云中的邊緣和平面特征的新型關鍵幀提取方法.首先，我們引入二維特征矩陣（fea-turematrix，FM）的概念，FM封裝了單幀點云的幾何結構特征信息，FM之間的差異量化了兩幀之間特征變化的程度.因此，通過計算當前幀和最新關鍵幀的FM之間的相似度，即可量化幀間的特征差異以進行關鍵幀篩選.此外，因為不穩定工況下的特征變化顯著，所以引入運動自適應閾值與特征相似度比較，過濾不穩定工況下的幀.依據關鍵幀的檢測結果，相鄰的非關鍵幀和關鍵幀之間存在大量特征重疊，依靠最近的關鍵幀進行配準足以提供足夠的約束.因此，我們設計了一種雙模式配準方法，對非關鍵幀僅執行幀與幀的配準，對關鍵幀僅執行幀與局部地圖的配準.在點云配準的優化模型中，遠距離點對旋轉具有更加顯著的約束，故融合點的水平距離作為殘差權重，實現加權優化.本文的主要貢獻如下：

1）提出了一種新穎的激光點云關鍵幀提取方法，該方法綜合考慮了幾何特征相似性和運動自適應閥值，以提取穩定且合適的關鍵幀.

2）提出了基于關鍵幀的雙模式激光里程計算法.在非關鍵幀時，僅執行幀間匹配，以獲得更高的計算效率；而在關鍵幀時，則執行幀到局部地圖的配準，以確保系統的定位精度.

3）利用點云中得出的水平距離數據，為遠距離點賦予更高的權重，這些點對旋轉具有更強的約束.

1系統概述

本文提出了一種基于關鍵幀的雙模式激光SLAM框架，如圖1所示，主要包括特征提取、關鍵幀檢測、運動自適應閾值、雙模式配準和加權約束模塊.

系統每獲取一幀激光點云，就先提取邊緣和平面特征.將所提取的三維特征投影到二維平面上，由于激光雷達的掃描特性是從中心向外 360^° 掃描，故投影結果是一個圓形.將圓形展開構造得到特征矩陣（FM），矩陣的每個單元存儲該位置所有點的高度信息并升序排列.當前幀的FM構造完成后，將與最新關鍵幀的FM進行匹配，計算兩個FM的相似度，并與運動自適應閾值比較以篩選關鍵幀.基于關鍵幀檢測結果，執行雙模式的激光里程計配準算法，并融入點的水平信息進行加權優化位姿.

首先進行特征提取.激光雷達是多線束的，每一線激光都會掃描周圍環境，最終所有線束的掃描結果匯聚得到一幀完整的激光點云.此外，激光雷達的水平分辨率遠高于豎直分辨率，因此針對激光雷達每一線點云都提取特征點，計算每個點的局部平滑度以篩選線特征點和面特征點，如式（1）所示，

式中： s 是 P_i 在同一行中的鄰近點集合，試驗中集合的大小設置為 10;P_j 是 P_i 的鄰近點； σ 是 P_i 的局部平滑度.

根據所計算的點的局部平滑度，從每個激光線束中選擇具有最高局部平滑度的前20個點作為線特征點.為了提升特征提取的速度，我們丟棄了LOAM中的弱特征點，將除線特征點外的所有點均視為面特征點.

2關鍵幀檢測

一個合理的關鍵幀策略可以消除冗余和不穩定的特征，同時保留豐富的環境信息.然而，現有的激光SLAM中的關鍵幀檢測方法十分粗糙，大都依據空間位置和旋轉約束判定，這對定位精度產生不利影響.與傳統方法相比，我們利用提取的邊緣和平面特征進行關鍵幀選擇，直接衡量兩幀的特征差異，這將包含更加穩定和豐富的特征.

首先，對提取的線面特征進行下采樣，以減少激光點數量.再將線面特征點投影到二維水平面，將投影得到的圓形平面展開為特征矩陣（FM），FM中的每個單元格容納一個特征向量（FV），存儲投影到該單元格的點的高度信息.每當訂閱新的點云，計算FM并將其與最新關鍵幀的FM進行匹配，計算它們的特征相似度.隨后，將特征相似度與運動自適應閾值進行比較以篩選關鍵幀.自適應閾值基于運動信息解算，以避免將不穩定工況下的點云納入關鍵幀，在本文的后續討論中，當前幀的FM表示為 c ，而最新關鍵幀的FM表示為L.

2.1點云投影

投影沿著特征點云的高度方向進行，得到一個圓形平面，將其沿徑向和切向方向等間隔地分割成單元格，如圖2所示.在徑向和切向方向上，分割的每個單元格分別對應于FM的行索引和列索引.圖2（a）為特征點云的俯視圖，特征在徑向和切向均勻地劃分為N_r 和 N_c 個單元，分別對應于圖2（b）矩陣的行數和列數.點云的最大水平感知距離 L_max 設置為 90m. 在試驗中，特征矩陣的徑向分辨率為 1m ，切向分辨率為2^° .圖2（b）中矩陣單元顏色代表該位置存儲的激光點數量.

圖2特征點云投影

Fig.2 The projection of features

需要注意的是，投影是基于車輛當前位姿的局部坐標系進行的，可避免引入累積定位誤差.然而，激光雷達的機械旋轉特性使得每幀點云存在運動畸變，單幀內的點不在同一位置采集.因此，引入勻速運動模型將所有點與當前幀的起始位置對齊.

Iris將點沿高度方向劃分為八個部分，并以漢明距離的形式存儲點的高度，而本文則將每個點分配到其相應的特征向量中，以保留一幀中的所有特征.此外，鑒于特征相似性計算僅涉及最新的關鍵幀，計算成本保持在較低水平.

2.2特征相似度計算

如圖2（b）所示，每個FM包含 N_c×N_r 個FV，每個FV存儲該位置對應的點的高度，因此FM反映了環境的幾何結構.而 C 和 L 具有相同索引的FV之間的相似度反映了當前幀和最新關鍵幀之間幾何特征變化的程度.最終，通過對 C 和 L 的所有對應FV的特征相似度進行加權計算，即可求出兩幀之間特征差異度.

FM的維度是固定的，取決于單元格分辨率，但FV的維度并不固定，這取決于單元格中點的數量.為了衡量兩個FV之間的距離，將維度較低的FV補充為高度為0的點，以確保它們具有相同的維度.此外，將每個FV按降序排列可以確保高度較大的點成對出現，高處的點所蘊含的環境信息更豐富.對所有訂閱到的點云都執行上述算法，以獲取其FM和FV.

遍歷當前幀的FM，將每個FV與最新關鍵幀的FM相同索引處的FV進行匹配.根據FV的維度，將計算兩個FV之間差異的方法分為三種情況：第一種，參與匹配的FV都為空，表示該位置沒有特征點，相似度設為默認值1；第二種，僅有一個FV為空，表明該位置的特征差異巨大，相似度設置為0;第三種，兩個FV都存儲了點，計算兩者余弦距離作為特征相似度，這反映了特征向量間的差異.最終，FV的特征相似度計算如下：

式中：i和j分別是FM的行和列索引； C_ij 和 L_ij 分別是 C 和 L 在 （i，j） 處的FV; s（i，j） 是 C 和 L 在 （i，j） 處的特 征相似度.兩個FV之間的相似度越高， s（i，j） 越大.

單個位置的FV之間的差異計算如上所示.綜合考慮所有FV之間的特征相似度 s（i，j） ，即可得到全

局特征距離，從而量化兩幀之間的差異，如式（3） 所示.

式中： d 為加權得到的特征距離； n 是 C 和 L 中相同索引處的FV不同時為空的計數 .c 和 L 之間的相似度越高， d 越小.

隨著車輛的移動，新的特征將更多地出現在遠處，故更高的權重被分配給更遠的點，以更靈敏地感知特征變化.

當車輛保持靜止，環境特征基本保持不變，各FV中的特征維持不變，所解算出的特征距離 d 很小.在動態場景下，動態物體可能會影響局部FV的特征分布，但我們的方法考慮了 C 和L中的全局FV差異，占比更大的靜態物體起決定作用，對局部動態物體具有魯棒性.隨著車輛移動，不斷感知到新的環境特征， d 逐漸增大.一旦 d 超過特征距離閾值，就表明當前幀與最新關鍵幀之間存在顯著差異.傳統的關鍵幀提取方法通常為監測空間距離和旋轉變化，然而該方法很難建立一個能夠有效適應不同車速和工況下的變化閾值.相反，我們所提出的方法直接聚集于幀間的特征差異，這是兩幀能否配準良好的根本.只要環境特征發生變化，就應當被視為關鍵幀，受車速和工況影響小.

2.3運動自適應閾值

關鍵幀的質量直接影響著激光里程計和建圖的精度，因此必須保證所篩選關鍵幀特征的穩定和健壯.如前所述，本文采用的關鍵幀選取策略是通過計算兩幀之間的特征距離，并與閾值比較來選取的.然而，在不穩定的工作條件下，例如車輛遇到減速帶時，受到大幅沖擊，其提取的特征與上一關鍵幀會出現顯著不同，計算的特征差異會很大.因為該幀實際包含的環境特征是不穩定的，所以不應將其納為關鍵幀，否則會引入錯誤的地圖信息，而且錯誤是不可逆轉的，這對建圖和隨后的定位都會產生永久影響.因此，我們結合運動信息構建自適應特征閾值，避免將不穩定工況下捕獲的幀包含到地圖中.

旋轉變化對運動強度的感知更敏感，因此我們通過計算兩個最新幀之間的歐拉角來評估運動工況，如式（4）所示.

t={t₁，ΔR?t₂

式中：t是計算得到的自適應特征距離閾值； t₁ 是穩定情況下的固定閾值； t₂ 是角度變化閾值； ΔR 是最新兩幀之間歐拉角的模長.在所有試驗中， t₁ 為 0.6，t₂ 為1.5^° ：

對于具有大幅旋轉變化的工作條件，特征距離的閾值會相應提升，以避免將劇烈運動時刻包括為關鍵幀.另外，這種自適應調節不會影響正常工況下的關鍵幀選擇.

3基于關鍵幀的配準算法

基于關鍵幀檢測結果，我們將激光里程計分為幀與幀配準和幀與地圖配準兩種模式.對關鍵幀進行幀與地圖配準，對非關鍵幀進行幀與幀配準.對于線特征點，采用點線ICP計算殘差；對于面特征點，采用點面ICP計算殘差，如式（5）和式（6所示.

式中 ：f_e（P_i） 和 f_s（P_i） 分別表示點到線的殘差和點到平面的殘差； P_i 是在局部坐標系中表示的邊緣或平面特征點； τ 是當前幀的全局位姿，通過該位姿可將點 P_i 變換至世界坐標系下； P₁，P₂，P₃ 是全局坐標系下與 TP_i 的最近點.

3.1雙模式配準

我們依據幀間的特征差異程度篩選關鍵幀.對于非關鍵幀，這意味著當前幀與最新關鍵幀之間的特征差異較小，即存在大量的特征重疊，因此通過幀到幀的配準可以提供足夠的幾何約束.將當前幀與最新關鍵幀特征點云進行配準，在速度上遠遠快于與地圖的配準.另外，我們是將當前幀與最新關鍵幀對齊，而不是與普通幀對齊.因為普通幀僅通過幀到幀的配準進行對齊，并未通過幀到地圖的配準進行進一步優化，因此比關鍵幀包含更多的不確定性.

對于關鍵幀，這意味著當前幀觀察到的特征與最新關鍵幀中的特征顯著不同，即車輛在當前時刻檢測到了很多新的環境特征.在這種情況下，僅依靠幀與幀的配準難以提供足夠的幾何約束.因此，當前幀必須與信息更加豐富的局部地圖進行對齊，以構建更有效的幾何約束.子圖的大小是車輛在全局地圖中以當前位置為中心的 100m 區域.

在這兩種配準模式之間切換，可以在保證足夠精度的同時顯著減少計算成本并提升實時性.

3.2加權約束

激光雷達在捕獲環境信息時受到真實世界物體的水平距離的影響，因此遠處特征表現出稀疏性，近處特征表現出稠密性，如圖2（b）所示.特征的非均勻分布導致近處點相對遠處點的權重更高，然而，遠處點對旋轉的約束更強，因此應該增強遠處特征構建的約束.針對這一挑戰，我們利用水平距離信息構建加權約束，為遠處特征分配更高的權重，如式（7）所示.每個殘差被分配一個適當的權重，通過最小化這些殘差的總和來優化位姿，如式（8）所示.

式中： r 是特征點的水平距離； r_min 是激光雷達的最小有效使用距離； r_max 是激光雷達感知的最大距離； T 是要優化的全局位姿 ;f_e（P_i） 表示點到線的殘差 ;f_s（P_j） 表示點到平面的殘差； w_i 和 w_j 是點的權重； P_e 和 P_s 分別是當前幀的邊緣特征和平面特征的集合.

3.3地圖更新

位姿求解完成后，將進行地圖的更新，我們會在世界坐標系內維護一個全局地圖.具體來說，只會使用從關鍵幀中提取的特征來更新這個全局地圖，而與非關鍵幀相關的特征在完成位姿估計后被丟棄.這種策略明顯減小了地圖的規模，從而提高了SLAM的計算效率.此外，在內存資源有限的計算平臺上，這種方法還顯著降低了內存消耗.

我們依據關鍵幀的全局位姿將其點云轉換到世界坐標系中，注冊到一個體素化的全局地圖上.此外，關鍵幀的特征點云和位姿是分開存儲的，這樣可以方便地與回環檢測和后端優化模塊集成.另外，由于關鍵幀的數量明顯少于普通幀，回環檢測和后端優化的效率都會顯著提升.本文采用了ScanContext的方法執行回環檢測.

4試驗測試

本文通過計算定位的精度和耗時來驗證系統的綜合性能.定位精度采用平均平移誤差ATE（aver-age translation error，單位是 % ）和平均旋轉誤差ARE（averagerotationalerror，單位是 ^°/100m 評估，計算方法由KITTI數據集定義.本文在公開數據集KITTI和實車上都做了測試，并與當前杰出的激光SLAM系統進行比較.鑒于本文是基于LOAM框架實現的，故選擇了一些基于LOAM的方案進行比較分析，這包括LOAM[9]、A-LOAM、F-LOAM[14]和 LeGO-LOAM[16.為更全面地展示本文所提出方法的有效性，我們還與LOAM系列之外的杰出方法進行了比較，包括 SuMa++^[28] 和MULLS[15].我們的代碼完全使用 C++ 編寫，并在機器人操作系統ROSNoetic和Ubuntu20.04LTS上運行.為了準確對比計算成本，所有試驗均在一臺裝有Inteli5-12500H、2.50GHz處理器的筆記本電腦上進行.

4.1KITTI數據集測試

KITTI數據集是SLAM領域中最具權威性的數據集之一，被廣泛用于測試各種SLAM算法，包括ORB-SLAM[29]、LOAM、F-LOAM、MULLS等.KITTI平臺配備了兩個灰度相機、兩個彩色相機、一臺Velodyne64線激光雷達和一套GPS導航系統.我們在KITTI的00～10數據集序列上進行了試驗驗證，涵蓋了超過23000幀的數據，車輛行駛距離達 22000m ，最高速度達 96km/h. 此外，還建立了一組試驗包括回環檢測和后端優化模塊，作為完整的SLAM系統.我們將本系統在部分序列的軌跡繪制出來，并與KITTI的真實軌跡進行比較，如圖3所示.KLOAM-LO代表無回環檢測的系統，KLOAM-SLAM代表具備回環檢測功能的系統.圖3（a）～（c）分別表示KITTI的00、05、07序列.

圖3車輛定位軌跡 Fig.3The trajectoryof vehiclelocalization

所有試驗結果如表1所示，K-LOAM的平均精度排名第二，ATE僅為 0.56% ，ARE為 0.0021（^°）/m 此外，在序列02中，K-LOAM具有最高精度.

為驗證本系統的實時性，我們計算了所有方法在KITTI的00一10序列上每幀的平均計算時間，如表2所示.本系統展現出最高的處理速度，每幀處理時間為 38.5ms .而LeGO-LOAM是一種輕量級激光SLAM，其速度與我們的速度相當.然而，我們僅考慮了其前端計算，因為其后端優化在額外的線程中運行.雖然這不影響實時性能，但會消耗額外的計算資源.LeGO-LOAM的后端較慢，在我們的測試中，該線程的平均執行時間在 200～300ms 之間.此外，它每跳過固定數量的幀才執行一次后端優化，這對定位精度產生了不利影響，如表1所示.我們將本文中的關鍵幀檢測方法替換為 LIO-SAM^[17] 的傳統關鍵幀檢測方法，用 K-LOAM/nk 表示.該組試驗結果的實時性大幅下降，平均每幀處理時間增加為 53.5ms ，這是因為傳統的關鍵幀檢測引人了大量冗余幀，增加了計算負擔.另外，在所測試的方法中，從是否使用關鍵幀的角度來看，基于關鍵幀的方法表現出更高的計

表1KITTI數據集定位誤差ATE/ARE

Tab.1 ThelocalizationerrorATE/AREonKITTIdataset

注：1）ATE表示平均平移誤差，單位是%；ARE表示平均旋轉誤差，單位是 （^°）/（100m）.2）nc 表示沒有加權約束的試驗組;nk表示使用傳統關鍵幀策略的試驗組;ns表示沒有運動自適應閾值的試驗組，詳細信息在4.3節消融試驗中展示.表格中的定位精度，有下劃線的數據表示最高精度，藍色數據表示第二位精度.試驗組中，*表示該組包含回環檢測，△表示該組采用了關鍵幀檢測.

算效率.

4.2實車測試

我們的數據是配備了VLP-32c激光雷達和NPOS220GNSS/IMU系統的車輛收集的，如圖4所示.圖4（a）為試驗車輛，圖4（b）和圖4（c）為試驗環境，圖4（d）為試驗場景中存在回環的建圖效果.試驗場景涵蓋了郊區、繁華的城市區域和公園，包括大量動態障礙物.值得注意的是，序列00和01是在城市道路上以平均時速 50km/h 采集的，而序列02是在狹窄街道上收集的，序列03是在公園內獲取的，平均時速為25km/h. 此外，激光點云和GNSS/IMU數據的采集速率均為 10Hz ，使用RTK-GNSS/IMU作為定位真值.

表2平均計算時間Tab.2 Averagecalculationtime

圖4實車試驗場景與效果

Fig.4Actual vehicle testscenariosand effects

我們對K-LOAM、F-LOAM、A-LOAM和MULLS進行了測試.如表3所示，K-LOAM表現出最高的定位精度，ATE為 0.56% .此外，K-LOAM具有最快的平均每幀計算時間，達到 25Hz 圖4（d）提供了K-

LOAM實時構建的序列03的地圖的可視化表示.如該圖所示，樹木、道路和建筑物清晰和明確的輪廓印證了本文方法在復雜的戶外環境中實現高精度定位和地圖繪制的能力．

表3實車試驗結果的ATE/平均計算時間

Tab.3ATE/averagetimeconsumptionfromactualvehicletestresult

注：1）ATE表示平均平移誤差，單位是%;2）平均計算時間的單位是ms

4.3消融試驗

為了驗證本文算法的有效性，我們設置了三組消融試驗，分別評估關鍵幀檢測、動態自適應閾值和加權約束算法的有效性.每組試驗中僅調整一個方法，其余方法保持不變，獨立評估該方法的影響.

4.3.1關鍵幀檢測方法

為了評估本文提出的關鍵幀檢測方法與傳統方法之間的優劣，我們引人了一組消融試驗，標記為nk.在這組試驗中，關鍵幀檢測方法被替換為LIO-SAM所采用的傳統方法，該方法基于空間距離和角度變化過濾關鍵幀.而KITTI數據集中不同序列的速度和工況差異很大，僅使用固定的距離和角度閾值很難精確地選擇關鍵幀.試驗結果如表1和表2所示，與K-LOAM相比，nk的精度和效率都顯著降低，證實了本文基于特征變化篩選關鍵幀的方法的有效性和穩健性.值得注意的是，01序列的精度下降最為顯著，因為它是在高速公路場景中錄制的，最高速度為 96km/h

4.3.2運動自適應閾值

本組消融試驗旨在評估本文提出的自適應閾值方法應對劇烈運動場景的效果.在這組試驗中，使用固定閾值而不考慮運動信息，用ns表示.試驗結果如表1所示，與KLOAM相比，ns未過濾不穩定的幀，幾乎所有序列的精度均有所下降，其中在運動最為劇烈的高速公路場景（01序列）下降最明顯.此外，過濾掉不穩定關鍵幀，可以減少關鍵幀數量，從而提高了計算效率.盡管如此，仍會受到路面顛簸的影響，因此本文算法適用于城市平整路面.

4.3.3加權約束

為驗證本文提出的加權約束算法的有效性，設置消融試驗取消加權函數，這組試驗被標記為nc.如表1所示，試驗結果明確表明，缺乏加權約束的方法表現出較低的定位精度，證實了本文的加權約束算法對精度提升的幫助.

表4算法移植到其他系統的ATE/ARE

Tab.4 ATE/AREwhentransplantingalgorithmstoanothersystem

4.4在其他SLAM系統的表現

為進一步評估本文提出的關鍵幀策略的健壯性，我們將本文算法應用于經典的A-LOAM框架.框架的差異導致無法完整地將本文的工作整合到A-LOAM中.這是因為：將關鍵幀檢測方法移植到A-LOAM中，需要對關鍵幀同時執行幀間匹配和幀與地圖配準，對非關鍵幀僅進行幀間匹配.另外，A-LOAM的點云存儲在一個體素化的大地圖中，而不是保留單個幀的點云.因此，非關鍵幀只能與最新的普通幀匹配，而不是關鍵幀，這對幀間匹配的精度有不利影響.

如表4所示，試驗仍然在KITTI數據集上進行，該組試驗標記為A-LOAM/k.與A-LOAM的結果相比，A-LOAM/k的平均精度提升了 9.1% 其中，A-LOAM在KITTI的02序列上表現出退化現象[30]，定位精度顯著下降.相反，本文的關鍵幀系統在該系統退化時仍表現出強大的健壯性和出色的性能.在02序列上，A-LOAM/k的定位精度顯著提高，達到了29.8% 更重要的是，我們的關鍵幀系統顯著提高了A-LOAM在所有序列上的速度，平均提升了 33.6%

5結論

本文提出了一種基于關鍵幀的雙模式實時激光SLAM，創新性地直接衡量兩幀之間的特征差異，以篩選關鍵幀.通過提取每幀的邊緣和平面特征以構建特征矩陣（FM）和特征向量（FV），計算FM之間的相似度，并與運動自適應閾值比較完成關鍵幀檢測.該關鍵幀策略顯著減小了地圖的規模并提升了系統實時性.另外，本文設計了雙模式激光里程計，基于關鍵幀檢測的結果執行不同的配準算法.對于關鍵幀，需要與局部地圖對齊以獲得更豐富的約束，而非關鍵幀只需與關鍵幀對齊以減少系統計算資源的消耗.此外，本文結合點的水平距離信息構造加權約束，提高了定位的精度.K-LOAM在KITTI公共數據集和實車數據集上進行了全面測試，與F-LOAM、MULLS、 SuMa++ 和其他最新方法相比，表現出高精度和優越的實時性能.

參考文獻

[1] CHENX，MILIOTOA，PALAZZOLOE，etal.SuMa：efficient LiDAR-based semantic SLAM[C]//2019 IEEE/RSJ International ConferenceonIntelligentRobotsand Systems（IROS）. Macau， China.IEEE，2019：4530-4537.

[2] WANG H，WANG C，XIEL H.Intensity-SLAM：intensity assisted localization and mapping for large scale environment[J]. IEEERobotics and AutomationLetters，2021，6（2）：1715-1721.

[3] ZHENG X，ZHU JK．Traj-LO：in defense of LiDAR-only odometryusingan effective continuous-time trajectory[J].IEEE Robotics and AutomationLetters，2024，9（2）：1961-1968.

[4]BESL PJ，MCKAY N D. A method for registration of 3-D shapes [J].IEEE Transactions on Pattern Analysisand Machine Intelligence，1992，14（2）：239-256.

[5]VIZZO I，GUADAGNINO T，MERSCH B，et al． KISS-ICP： in defense of point-to-point ICP-simple，accurate，and robust registration if done the right way[J].IEEE Roboticsand Automation Letters，2023，8（2）：1029-1036.

[6]CHEN K，LOPEZ B T，AGHA-MOHAMMADI A A，et al. Direct LiDARodometry：fast localization with dense point clouds[J]. IEEE Robotics and Automation Letters，2022，7（2）：2000-2007.

[7]DELLENBACH P，DESCHAUD JE，JACQUET B，et al． CTICP： real-time elastic LiDAR odometry with loop closure [C]// 2022 International Conference on Robotics and Automation （ICRA）.Philadelphia，PA，USA.IEEE，2022：5580-5586.

[8]XU W，CAI Y X，HE D J，et al. FAST-LIO2：fast direct LiDARinertial odometry[J].IEEE Transactions on Robotics，2022， 38（4）：2053-2073.

[9]ZHANG J，SINGH S. LOAM： lidar odometry and mapping in real-time [C]//Robotics：Science and Systems X. Robotics： Science and Systems Foundation，2014，2（9）：1-9.

[10]CHEN SB，MA H，JIANG C H，et al. NDT-LOAM：a real-time lidar odometry and mappng with weighted NDT and LFA[J]. IEEE Sensors Journal，2022，22（4）：3660-3671.

[11]ALI W，LIU PL，YING R D，et al.A feature based laser SLAM using rasterized images of 3D point cloud[J]． IEEE Sensors Journal，2021，21（21） ：24422-24430.

[12]ZHENG X，ZHU JK．Efficient LiDAR odometry for autonomous driving[J]．IEEE Robotics and Automation Letters， 2021，6（4） 8458-8465.

[13]YE HY，CHEN YY，LIU M. Tightly coupled 3D lidar inertial odometry and mapping [C]//2O19 International Conference on Robotics and Automation （ICRA）.Montreal，QC，Canada． IEEE， 2019：3144-3150.

[14]WANG H，WANGC，CHENCL，et al. F-LOAM： fast LiDAR odometryandmapping [C]//2021IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）． Prague， Czech Republic． IEEE，2021：4390-4396.

[15]PAN Y，XIAO PC，HEY J，et al. MULLS：versatile LiDAR SLAM via multi-metric linear least square[C]/2021 IEEE International Conference on Roboticsand Automation（ICRA）. Xi'an，China．IEEE，2021：11633-11640.

[16] SHAN T X，ENGLOT B. LeGO-LOAM：lightweight and groundoptimized lidar odometry and mapping on variable terrain [C]/ 2018IEEE/RSJ International Conference on Intelligent Robots and Systems（IROS）.Madrid，Spain.IEEE，2018：4758-4765.

[17]SHANTX，ENGLOTB，MEYERSD，etal.LIO-SAM： tightlycoupled lidar inertial odometry via smoothing and mapping[C]/ 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. Las Vegas，NV，USA. IEEE，2020：5135- 5142.

[18]LIKL，LIM，HANEBECKUD.Towards high-performance solid-state-LiDAR-inertial odometry and mapping［J].IEEE RoboticsandAutomationLetters，2021，6（3）：5167-5174.

[19]SHENBK，XIEWM，PENGXD，et al.LIO-SAM ⁺⁺ ： a lidarinertial semantic SLAM with association optimizationand keyframe selection[J]. Sensors，2024，24（23）：7546.

[20]ZULN，WEICR，SUNQQ，etal.Adaptive keyframe selection strategy of visual SLAM incomplex poses[J].IEEE Sensors Journal，2025，25（1）：1756-1767.

[21]LINY，DONGHQ，YEWT，et al. InfoLa-SLAM：efficient lidarbased lightweight simultaneous localization and mapping with information-based keyframe selection and landmarks assisted relocalization[J].RemoteSensing，2023，15（18）：4627.

[22]DUANYF，PENGJ，ZHANG Y，etal.PFilter：building persistent maps through feature filtering for fast and accurate LiDAR-basedSLAM[C]//2O22IEEE/RSJInternational Conference on Intelligent Robotsand Systems（IROS）.Kyoto， Japan.IEEE，2022：11087-11093.

[23]LI W，HU Y，HAN Y H，et al.KFS-LIO： key-feature selection forlightweightlidarinertialodometry[C]//2O21IEEE International Conference on Robotics and Automation （ICRA）. Xi'an，China.IEEE，2021：5042-5048.

[24]CHANGDX，HUANGSJ，ZHANGRB，etal.WiCRF2：multiweighted LiDAR odometryand mappingwith motion observability features[J].IEEESensors Journal，2023，23（17）：20236- 20246.

[25]ZHOU ZB，YANG M，WANG CX，et al. ROI-cloud：a key regionextraction method forLiDAR odometryandlocalization [C]//202O IEEE International Conference on Roboticsand Automation（ICRA）.Paris，France.IEEE，202O：3312-3318.

[26]KIM G，KIM A.Scan context： egocentric spatial descriptor for placerecognition within 3D pointcloud map[C]//2O18 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）．Madrid，Spain．IEEE，2018：4802-4809.

[27]WANG Y，SUN ZZ，XUCZ，et al.LiDAR iris for loop-closure detection [C]//2O2O IEEE/RSJInternational Conferenceon Intelligent Robots andSystems（IROS）.LasVegas，NV，USA. IEEE，2020：5769-5775.

[28]CHENX，MILIOTOA，PALAZZOLOE，et al.SuMa++：efficient LiDAR-based semantic SLAM[C]//2O19 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）.Macau， China.IEEE，2019：4530-4537.

[29]MUR-ARTALR，MONTIELJMM，TARDOSJD.ORB-SLAM： aversatile and accurate monocular SLAM system[J].IEEE TransactionsonRobotics，2015，31（5）：1147-1163.

[30]ZHANGJ，KAESS M，SINGH S. On degeneracy of optimizationbased state estimation problems[C]//2O16 IEEE International Conference on Roboticsand Automation （ICRA）.Stockholm， Sweden. IEEE，2016：809-816.