黃 瑞，張 軼

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

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

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

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

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

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

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

1 算法描述

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

1.1 退化處理相關(guān)

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

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

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

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

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

1.2 畸變矯正

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

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

1.3 算法整體流程

在上述算法中，引入動態(tài)門限：當(dāng)動態(tài)門限D(zhuǎn)2增大時，表示當(dāng)前噪聲影響較大，需要在下一次迭代過程中剔除更多的噪聲影響；當(dāng)動態(tài)門限D(zhuǎn)2減小時，表示當(dāng)前噪聲的影響并不大，可以在下一次迭代過程中剔除更少的噪聲影響。同時，由于激光雷達測量的環(huán)境結(jié)構(gòu)信息具有時間上的連續(xù)性，動態(tài)門限D(zhuǎn)2的引入能更好的反映當(dāng)前時間段所處環(huán)境的結(jié)構(gòu)信息豐富與否，同時在面對復(fù)雜多變的環(huán)境時有更好的適應(yīng)性。整體的算法流程如圖2 所示。

圖2 算法整體流程圖

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

該算法在多個層級上減小噪聲對SLAM 系統(tǒng)的影響，保證系統(tǒng)在各種環(huán)境，各種運動狀態(tài)中都有較好的定位與建圖精度。同時，當(dāng)設(shè)備并未處于持續(xù)性的劇烈運動中時，系統(tǒng)并不進行局部地圖優(yōu)化，保證了系統(tǒng)整體的計算量較低。

2 實驗與分析

2.1 室內(nèi)公開數(shù)據(jù)集測試

對比本文算法、LOAM 算法和LIO 算法的實現(xiàn)數(shù)據(jù)，數(shù)據(jù)集[17]為公開的室內(nèi)無人機采集的數(shù)據(jù)集，表1 為數(shù)據(jù)集中fast1 數(shù)據(jù)的對比。

表1 3 種算法的定位精度統(tǒng)計

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

圖3 室內(nèi)軌跡誤差對比圖

2.2 自有設(shè)備數(shù)據(jù)采集測試

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

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

圖4 測試環(huán)境為樓道即狹長走廊的測試結(jié)果

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

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

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

2.3 室外公開數(shù)據(jù)集測試

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

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

圖6 室外公開數(shù)據(jù)集軌跡對比圖

3 結(jié) 束 語

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