高適應性激光雷達SLAM

黃 瑞，張 軼

(四川大學計算機學院 成都 610065)

最近幾年來，由于自動駕駛及自主機器人的迅速發展，作為其基礎核心技術的時定位與地圖構建技術 (simultaneous localization and mapping, SLAM)獲得了較多的關注[1-4]。且由于硬件的發展，特別是以多線激光為代表的高精度空間傳感器的出現，SLAM 系統在導航精度魯棒性上都有了較大突破。得益于激光雷達能夠獲取高精度的距離信息、不受光照變化的影響，且能夠提供豐富的結構化信息用于位姿估計，近年來涌現了眾多的基于激光雷達的SLAM 算法[5-8]。這些算法的穩定性、定位精度、運算實時性相較基于視覺或視覺與慣性測量單元 (inertial measurement unit, IMU) 融合的方案都有了較大的提升。但基于激光雷達的算法也面臨一些其他的問題，如：空間結構信息較少時系統穩定性及精度問題、運動畸變的矯正、動態場景問題等。

為了適應復雜多變的周圍環境和無規律的自身運動，需要一種兼具魯棒性和準確性的定位與建圖的方法。在過去的十多年中，基于視覺和單線激光的定位方法取得了較多的成果[2,6-8]；而最近幾年，基于多線激光雷達的SLAM 受到了廣泛的關注[8-10]。但是現有的激光雷達SLAM 算法在平面結構較多且空間狹小的環境中定位時，會受到結構化約束信息較少而帶來的位姿更新誤差的影響。更重要的是，如果激光雷達本身進行劇烈運動，其將表現出與結構化信息較少相似的特點：定位誤差較大且建圖不精確。在劇烈運動過程中，相比處于結構化信息較少的環境，系統更容易積累，這就導致了其定位和建圖都會產生更大的誤差。

在應對激光雷達劇烈運動方面的文章較少，且都是關于多傳感器融合的[11-15]。其通常做法是，使用激光雷達和慣性傳感器(inertial measurement unit,IMU)緊耦合，相互之間提供約束，共同估計自身位姿。這種方法面對劇烈運動時誤差累積小、精度高[16-17]，但是存在以下兩個問題：

1)多傳感器之間的時間同步較難[17-18]，只能在設備使用前，通過靜態方法估計，然后在后續優化問題中作為常量添加；

2)由于多傳感器緊耦合框架復雜，且嚴重依賴于傳感器的自身特性[19-21]，最終會對系統穩定性造成較大影響。

本文只使用激光雷達，免去了時間同步誤差及多傳感器外參標定誤差的影響，同時可以保證在劇烈運動場景中誤差與多傳感器緊耦合的誤差處于同一量級。實驗中發現單純使用激光雷達在特殊場景中定位精度低，且主要是以下兩方面的原因：1)迭代優化過程中，固定的退化處理門限不適應動態的場景變化；2)過大噪聲導致前端里程計不準，從而使得點云畸變矯正出現問題。本文主要對這兩方面進行改進。

1 算法描述

本文提出一種適應性強、在各種環境及運動狀態下都有較高精度及魯棒性的算法。主要從兩方面改進：1)迭代優化過程中，不再固定退化門限，以動態門限和靜態門限結合的方式，綜合處理點云退化的情況；2)在最終執行ICP (就近點迭代)之前，使用之前已有的局部地圖，進一步進行優化位姿后再一次進行畸變矯正，從而使得最終優化的時候引入更少的噪聲，對應1)中較平緩的進行門限變化，保證普通場景中門限變化是由于所處環境變化而非偶發性的運動狀態變化導致。

1.1 退化處理相關

文獻[22]通過添加擾動找到退化方向，并通過統計分析，確定一個退化值的門限，小于這個退化門限的特征值都可以認為在這個方向上有退化。本文繼承這種處理方式，但是采用了更加靈活的方式減小噪聲的影響。

在非線性優化中，高斯牛頓法是最常用的優化算法，其思路是在函數的局部極值點附近，對函數進行二階泰勒展開。由于泰勒展開的二次項中海森矩陣的二階偏導計算復雜，所以一般用一階雅可比矩陣計算得出。最終得到一個線性方程組：

式中，P為變換矩陣，是由n個特征值對應的n個線性無關的特征向量按照特征值的對應關系所組成的矩陣；A為對角線上存在零元素的對角矩陣，表示在基空間中對弱約束維度屏蔽的線性變換；B是A的相似矩陣，處在優化更新量的空間中，即B變換是更新量所在空間中對應的線性變換。A,B兩個變換在不同基空間中描述了同一件事情：即對弱約束維度的屏蔽。

本文迭代過程中用到的3 種海森矩陣特征向量組成的矩陣：

而考慮弱約束的完整更新量為：

1.2 畸變矯正

圖1 畸變矯正不好時，輸入給后端ICP的點云會出現較大畸變

通常算法采用了一個默認的假設：前端假設激光雷達是勻速運動的。因此這種假設在一般情況下表現較好，但是當位姿在短時間內變化較大時，就會失去精度。而這一步解算出來的位姿會作為運動畸變的矯正參數，由此導致畸變矯正出問題，是輸入給后端優化的參數的進一步惡化，如圖1所示。這種未很好矯正畸變的點云在后端精細化的ICP 中會引入較大噪聲，使得迭代優化過程中即使有較好的位姿初始值，也會受到較大的噪聲影響，帶來的噪聲又同時會對退化的判斷造成影響。因此本文在輸入后端進行全局優化之前，先根據已有局部地圖對位姿做一遍修正，之后用修正的位姿送入后端進一步迭代優化。

1.3 算法整體流程

在上述算法中，引入動態門限：當動態門限D2增大時，表示當前噪聲影響較大，需要在下一次迭代過程中剔除更多的噪聲影響；當動態門限D2減小時，表示當前噪聲的影響并不大，可以在下一次迭代過程中剔除更少的噪聲影響。同時，由于激光雷達測量的環境結構信息具有時間上的連續性，動態門限D2的引入能更好的反映當前時間段所處環境的結構信息豐富與否，同時在面對復雜多變的環境時有更好的適應性。整體的算法流程如圖2 所示。

圖2 算法整體流程圖

該系統首先進行幀間匹配，此時并不考慮幀間迭代過程中的噪聲與退化，因為基于幀間對應掃描線匹配的位姿迭代足夠魯棒。之后進入局部地圖優化模塊，若出現退化，則按照算法1 屏蔽特征向量，同時判斷當前動態積累的退化幀數是否大于5(表示動態累計退化時間大于0.5 s)。若count 大于5，則需要局部優化的位姿進行第二次畸變矯正。將點云送入當前維護的局部地圖，再次進行位姿的迭代優化。

該算法在多個層級上減小噪聲對SLAM 系統的影響，保證系統在各種環境，各種運動狀態中都有較好的定位與建圖精度。同時，當設備并未處于持續性的劇烈運動中時，系統并不進行局部地圖優化，保證了系統整體的計算量較低。

2 實驗與分析

2.1 室內公開數據集測試

對比本文算法、LOAM 算法和LIO 算法的實現數據，數據集[17]為公開的室內無人機采集的數據集，表1 為數據集中fast1 數據的對比。

表1 3 種算法的定位精度統計

由上表可以看出，本文提出的算法相比于同樣只使用激光雷達的LOAM 算法，在定位精度上有了很大的提升。同時相較于使用激光雷達和IMU緊耦合的LIO 算法，本文提出的算法在定位精度上也沒有太大的差距。進一步的，本文提出的算法在室內激烈運動場景中，對平移量的相對誤差控制得較好。在誤差水平上，整體跟LIO 處于同一個量級。但本算法的旋轉量相對誤差優勢不明顯，整體誤差水平和LOAM 接近。因為LIO 使用了IMU 與激光雷達的緊耦合，更容易獲得較為精確的旋轉信息，而本文算法旋轉信息的估計本質上還是依靠幀間匹配，相對于同樣使用幀間匹配約束的位移信息更加不好約束，因此，圖3a 和圖3b 中，本文算法軌跡相對于LIO 算法軌跡表現出更明顯的在真值附近來回震蕩的效果。

圖3 室內軌跡誤差對比圖

2.2 自有設備數據采集測試

本實驗采用自行錄制的數據，硬件如圖4a 所示，無人小車搭載rslidar-16、GPS、相機及毫米波雷達(用于后期自主導航等用途)，測試中故意制造較大的角度變化。在這種場景下，迭代優化過程將會出現退化，原始的退化控制算法將造成某些方向上的不可控。

可以看出，圖4a 在優化過程中，角度優化量出現了較大的誤差。為了分析以上過程，畫出了各種情況下的海森矩陣的特征向量矩陣及相似變換矩陣。

圖4 測試環境為樓道即狹長走廊的測試結果

各種情況下的特征向量矩陣及相似矩陣如圖5 所示。其中，5a 表示受到較好約束的迭代過程，此時每行每列都有明顯的極大值，只在平面運動(即F和L)上有少許噪聲。但是對結果影響不大，因為平面運動中，平面上的兩個軸會相互影響。5b 表示受到了嚴重的噪聲影響，此時P,Y,F列與V1,V4,V5行都 沒有 出 現明 顯的極大值。此時紅線上方代表固定門限判定的退化維度，此時為forward 方向。圖5c 對應圖5a效果，在方向維度出現了較大誤差，即出現了偽退化的情況。

圖5 各種情況下的特征向量矩陣及相似矩陣

圖5d 表示真實退化情況下算法估計的結果。退化維度(即V4行)的更新量不是由其自身(即F列)主導的，而是通過所有更新維度對其進行估計。與圖5d 不同的是，圖5c 中最后更新量依舊取決于F的原始更新量(即[V4,F]依舊是所在行與列的極大值)，但此時的海森矩陣的特征向量矩陣已經屏蔽掉了F維度，這也造成了圖5b 中V1維度在其他維度上的投影也被屏蔽了。此時5b 中第二大值[V1,P]也受到了較大影響，導致其在pitch 角度的更新量受到嚴重影響，即對應圖4b 中所示的軌跡和建圖誤差。綜上，該場景下，迭代優化不應該被判斷為退化，這也是本文提出動態門限的作用，最終的結果如圖4b 所示，此時更新量正常，沒有出現嚴重的建圖誤差。

2.3 室外公開數據集測試

實驗使用室外公開數據集[23]，該數據集使用20 Hz、16 線激光雷達，結果如圖6 所示。該數據集中傳感器處于長期的平面運動中，期間經過城市街景和樹木較多的類森林路段，周圍環境復雜多變，可以較好地評價算法性能。

在這個數據集中，LIO 的表現較為糟糕，其垂直方向的軌跡出現了較大誤差。因為激光雷達和IMU 緊耦合的框架不適合長期的平面運動，IMU 某些軸的未充分激勵很容易導致將隨機誤差引入系統中。LOAM 的表現稍好于LIO，但是依舊存在較大誤差。由于該數據集設置20 Hz 的采集頻率，造成單幀點云數量減少一半，有用約束減少，系統容易出現退化，特別是在后半程樹木較多的場景下，結構化信息約束雜亂，噪聲較多，容易出現退化。而本文算法計算的軌跡非常接近真值，表明本文算法在室外開闊且多變的場景中，依舊有較好的表現。

圖6 室外公開數據集軌跡對比圖

3 結 束 語

本文采用多層級多分支的數據處理流程，最終達到了在室內劇烈運動場景和室外開闊且又變化的場景都有較高精度的效果。改進的退化算法保證系統能盡可能減小多種場景下的噪聲影響，同時有選擇的進入局部地圖優化，既保證了設備在劇烈運動時能通過更多的信息優化自身位姿，又使得在普通場景中，系統不會消耗太多計算資源。最后，實驗數據表明本文提出的算法流程，能夠兼顧多種運動場景和多種運動狀態，在復雜的環境和運動中保持較高的定位和建圖精度。