高魯棒性兩階段激光雷達-慣性測量單元外參在線標定算法

林 鑫 張 捷 馮景怡 孟 杰 王書亭

1.華中科技大學(xué)能源與動力工程學(xué)院，武漢，4300742.馭勢科技有限公司基礎(chǔ)平臺研發(fā)部，北京，1024003.華中科技大學(xué)機械科學(xué)與工程學(xué)院，武漢，430074

0 引言

激光雷達(laser detection and ranging, LiDAR)和慣性測量單元(inertial measurement unit, IMU)是即時定位與建圖 (simultaneous localization and mapping, SLAM)技術(shù)中常見的傳感器配置，也是自動駕駛傳感器的主流組合。LiDAR測距范圍大，精度可達厘米級，但在雨雪等惡劣環(huán)境下測量可靠性差，不適用于曠闊的環(huán)境；IMU通常分為加速度計和陀螺儀，可以提供載體的加速度和角加速度，它不依賴于環(huán)境場景，但在長時間下存在累積誤差。LiDAR與IMU在自動駕駛?cè)蝿?wù)中可以優(yōu)劣互補，因此，基于LiDAR和IMU融合的定位算法[1-3]成為近年來的研究熱點。LiDAR的定位結(jié)果基于LiDAR坐標系，而LiDAR定位過程需要校正點云的運動畸變，因此需要從IMU獲得高頻運動信息。對于所有的融合算法來說，為了提高最終輸出定位結(jié)果的精度，均需要盡可能準確地標定傳感器之間的坐標系轉(zhuǎn)換關(guān)系。幾何變換的估計過程稱為外參標定，即估計不同傳感器坐標系之間相對六自由度(即旋轉(zhuǎn)和平移)變換。人工測量傳感器之間的相對平移和旋轉(zhuǎn)是不準確的，有時也是難以實現(xiàn)的。手眼標定[4-5]是一個眾所周知的傳感器校準方法，本文將手眼標定的傳感器模型應(yīng)用到自動駕駛車輛的LiDAR與IMU傳感器組合上，LiDAR通常安裝在汽車頂部附近，IMU通常安裝在汽車后橋的垂直中心線前面。

手眼標定是一種重要的標定方法[6-8]，廣泛應(yīng)用于具有自我運動估計能力的傳感器的標定。HORAUD等[8]將手眼標定經(jīng)典公式AX=XB應(yīng)用到相機和LiDAR等更廣泛的傳感器配置中，并開發(fā)了一個通用的數(shù)學(xué)框架來解決手眼標定問題。為了標定LiDAR與IMU之間的外參，GENTIL等[9]提出了一種LiDAR-IMU傳感器系統(tǒng)外參標定的概率框架，基于IMU數(shù)據(jù)插值的預(yù)積分，可以消除三維點云的運動失真，并根據(jù)點云的點到面距離對平面進行環(huán)境擬合，構(gòu)建優(yōu)化方程。吳昱晗等[10]通過將點云轉(zhuǎn)化為全局坐標系，在逐幀聚類后匹配點云中心，并優(yōu)化殘差函數(shù)來估計最優(yōu)的六自由度外參，但由于同一類型目標點云的觀測范圍不同，聚類結(jié)果在幀與幀之間存在差異，故無法對該方法的準確性進行定量評價。楊超等[11]提出基于非線性優(yōu)化的里程計與LiDAR外參的在線標定算法，根據(jù)優(yōu)化方式求取較為精確的激光雷達位姿信息。彭夢等[12]針對2D LiDAR和攝像機最小解標定方法多解問題，在最小解標定過程中提出改進措施以提高真解命中率和標定精度。JIAO等[13]通過對齊每個傳感器的估計運動來進行自動初始化的手眼校準，然后通過最小化由特征點到平面構(gòu)成的殘差函數(shù)，使用基于外觀的方法對所得參數(shù)進行二次優(yōu)化。為了關(guān)聯(lián)IMU和LiDAR的測量數(shù)據(jù)與外參，需要在數(shù)據(jù)層面將它們聯(lián)系起來。受視覺慣性工作[14]的啟發(fā)，YE等[2]設(shè)計了緊耦合LiDAR-IMU融合的方法，使用原始IMU數(shù)據(jù)和LiDAR測量來優(yōu)化整個系統(tǒng)的狀態(tài)，其中包括可獲得初始外參的系統(tǒng)初始化和持續(xù)滑動窗口優(yōu)化外參，但是該工作沒有考慮到如何解決退化運動時的系統(tǒng)初始化。對于包含大量近似平面運動的無人駕駛汽車，IMU很難在六個自由度上都獲得足夠的激勵，這就導(dǎo)致了無人駕駛汽車中LiDAR-IMU外參標定困難。

為了解決無人駕駛汽車中傳感器噪聲和退化運動導(dǎo)致LiDAR-IMU外參標定失敗的問題，本文提出了一種高魯棒性兩階段LiDAR-IMU外參在線標定算法。第一階段是旋轉(zhuǎn)外參解析解初值計算，通過剔除外點來去除退化的傳感器數(shù)據(jù)，先對預(yù)數(shù)據(jù)集篩選出具有一定的旋轉(zhuǎn)增量激勵，并且IMU里程計旋轉(zhuǎn)增量和LiDAR里程計旋轉(zhuǎn)增量中滿足角度差小于閾值的、離散的里程計對來構(gòu)建只包含旋轉(zhuǎn)分量的手眼標定方程組，同時對整個數(shù)據(jù)集以滑動窗口的形式，對手眼標定方程組多次求解解析解，最后以奇異值分解(SVD)中的第三大與第四大奇異值的比值來進行多解加權(quán)。第二階段是非線性滑窗在線迭代優(yōu)化，最小化包含外參的殘差函數(shù)，以旋轉(zhuǎn)解析解為初值滑動窗口迭代優(yōu)化六自由度外參，使外參快速收斂，并在退化運動和錯誤歷史約束過大時固定外參，避免外參退化。本文標定算法對不完美和退化的傳感器數(shù)據(jù)具有魯棒性，在無初值情況下也能在線標定外參。

1 基本原理

1.1 手眼標定模型

圖1 不同位置下里程計增量關(guān)系

(1)

(2)

式中，α為偏航角(yaw)；β為俯仰角(pitch)；γ為橫搖角(roll)；c表示cos；s表示sin。

通過計算平移部分和旋轉(zhuǎn)部分6個參數(shù)，可以得到IMU坐標系與LiDAR坐標系的六自由度變換關(guān)系。

在本文的外參標定算法中，只考慮對應(yīng)的IMU里程計增量和LiDAR里程計增量的旋轉(zhuǎn)部分。在系統(tǒng)還未初始化時，通常會忽略IMU的偏差，這個偏差包括加速度計的偏差和陀螺儀的偏差。IMU作為一個包含旋轉(zhuǎn)狀態(tài)和平移狀態(tài)的估計器，其旋轉(zhuǎn)量是陀螺儀數(shù)據(jù)的一階積分，而平移量是加速度計數(shù)據(jù)的二階積分，因此，在加速度計的偏差和陀螺儀的偏差都沒有獲得修正時，二階積分求得的平移量遠遠大于一階積分所得的旋轉(zhuǎn)量。同時，依據(jù)傳感器特性，陀螺儀偏差相對于加速度計偏差更為穩(wěn)定，累積誤差也更小，因此，對于IMU而言，旋轉(zhuǎn)量相對于平移量的累積誤差更小。

圖2 只考慮旋轉(zhuǎn)分量的手眼標定模型示意圖

(3)

將式(3)用四元數(shù)表示為

(4)

其中，?表示兩個四元數(shù)之間的乘法運算。由式(4)可得

(5)

(6)

其中，qw為四元數(shù)實部，四元數(shù)q和其虛部qv分別表示為

(7)

[qv]×為虛部向量的反對稱矩陣：

(8)

對于N對相對旋轉(zhuǎn)的測量值，可獲得超定的線性方程：

(9)

(10)

(11)

QN=UΣVT

(12)

1.2 含外參的非線性滑窗迭代優(yōu)化

在即時定位與建圖過程中，隨著關(guān)鍵幀和路標點的增多，后端優(yōu)化的計算效率會不斷下降，為了避免這種情況，使用滑動窗口算法將待優(yōu)化的關(guān)鍵幀的數(shù)量限制在一定范圍內(nèi)，以控制后端優(yōu)化的規(guī)模，固定一些歷史軌跡，僅對當前時刻附近的一些軌跡進行優(yōu)化，如圖3所示。

圖3 滑動窗口優(yōu)化算法

當新的測量約束出現(xiàn)時，滑動窗口將包含新的狀態(tài)，并在窗口中忽略最老的狀態(tài)。通過最小化以馬氏范數(shù)表示的代價函數(shù)：

(13)

得到狀態(tài)χ的最大后驗估計。其中，rp(χ)表示邊緣化先驗殘差，rl(m,χ)表示相對LiDAR掃描的點到線面約束的殘差，rb(z,χ)表示IMU預(yù)積分約束的殘差。滑動窗口內(nèi)所有的狀態(tài)量定義為χ：

(14)

(15)

代價函數(shù)(式(13))為非線性最小二乘形式，采用高斯-牛頓算法求解。

2 改進算法

2.1 算法框架

圖4 兩階段LiDAR-IMU外參標定流程框架

2.2 旋轉(zhuǎn)外參解析解初值計算改進

2.2.1剔除外點的手眼標定模型

圖5 兩個里程計增量夾角

圖6 旋轉(zhuǎn)外參解析解初值計算改進框架圖

另外，在只考慮旋轉(zhuǎn)部分手眼標定模型中，觀察旋轉(zhuǎn)部分約束方程發(fā)現(xiàn)，當傳感器的運動接近直線運動時，旋轉(zhuǎn)部分的矩陣R趨近于單位矩陣，當R恰好等于單位矩陣時，旋轉(zhuǎn)部分的約束方程退化，即有

(16)

2.2.2手眼標定模型求解的判斷條件改進

σ3/σ4>threshold

(17)

與原始算法中單純以第三大奇異值σ3>0.25的絕對閾值相比，考慮到最佳最小二乘解的求解條件，奇異值σ3/σ4比值的相對閾值threshold對包含退化運動的數(shù)據(jù)更具魯棒性，更能反映手眼方程組內(nèi)的運動狀態(tài)。

2.3 非線性滑窗在線迭代優(yōu)化改進

2.3.1兩階段LiDAR-IMU外參標定算法框架圖

圖7 兩階段LiDAR-IMU外參標定算法框架圖

2.3.2退化運動時的固定外參策略

非線性滑窗在線迭代優(yōu)化階段也會遇到一些較為平緩的運動序列，為此，本文設(shè)置了在遇到退化運動(如靜止或者勻速運動)時觸發(fā)固定外參的策略，即

(18)

(19)

當pmean小于一定的閾值時，說明此時的運動狀態(tài)是靜止的或者勻速的，屬于退化運動，這時外參的某些狀態(tài)量處于不可觀情況。為了避免第一階段獲得的精確的解析解初始值在迭代優(yōu)化過程中被帶偏，陷入局部極小值，需要進行固定外參的操作。第二個條件是在系統(tǒng)剛初始化成功時，初始的點云地圖還未收斂，點云的匹配效果較差，出現(xiàn)較多的誤匹配，導(dǎo)致優(yōu)化中含有錯誤的歷史殘差，在這時迭代優(yōu)化外參，將導(dǎo)致解析解初始值偏離，而這部分歷史殘差應(yīng)當先由窗口內(nèi)的IMU狀態(tài)來進行調(diào)整，直到系統(tǒng)較為穩(wěn)定時再開始啟動微調(diào)外參。具體方案為判斷rcost的值：

rcost=rp(χ)/(rl(m,χ)+rb(z,χ))

(20)

若rcost大于閾值，說明錯誤的歷史約束比較大，則固定外參。進一步地，若rcost遠大于閾值，說明錯誤的歷史約束過大了，則固定外參的同時重置歷史約束殘差。

3 實驗分析

3.1 實驗設(shè)計

3.1.1實驗平臺與傳感器數(shù)據(jù)預(yù)處理

如圖8所示，該實驗車輛含有一個Velodyne VLP-16型號的激光雷達與一個Xsens MTi-100型號的IMU。在車輛視圖中，激光雷達坐標系是l系統(tǒng)，l系統(tǒng)原點位于激光雷達結(jié)構(gòu)的中心，它的X軸指向車輛前進方向，其Y軸垂直于X軸指向車輛前進方向的左邊，Z軸垂直于X、Y軸向上。IMU載體坐標系的定義是b系統(tǒng)，b系統(tǒng)原點位于IMU的中心，它的X軸也是指向車輛前進方向，其Y軸指向車輛前進方向的左邊，Z軸垂直于X、Y軸向上。為方便觀察和比較，在后續(xù)的實驗結(jié)果分析時，將使用yaw、pitch和roll來分析LiDAR-IMU之間的旋轉(zhuǎn)，并將之前所有可用的外參的平均值作為后續(xù)標定實驗的目標值。

圖8 車載實驗平臺

在獲取LiDAR和IMU兩個傳感器的數(shù)據(jù)信息后，首先做的是數(shù)據(jù)同步。LiDAR點云頻率為10 Hz，即獲取的位置信息更新頻率為10 Hz，而IMU點云頻率數(shù)據(jù)為200 Hz，兩者均采用UNIX時間戳機制，記錄結(jié)果自1970年1月1日(UTC時間)開始。因為不能保證找到具有相同時間戳的數(shù)據(jù)，因此通過時間插值來實現(xiàn)同步對齊。同步對齊后，可以得到同一時間序列的兩個傳感器的位姿信息。

3.1.2可觀性分析與運動要求

對于IMU傳感器來說，各狀態(tài)量耦合程度較高，直觀上難以確定各狀態(tài)量是否能夠得到有效估計，通過可觀性分析[15]可有效掌握系統(tǒng)對各狀態(tài)量的估計情況，同時有助于提出相應(yīng)的改進方案。為了獲得可靠的平移校準，需要至少兩軸旋轉(zhuǎn)以及隨機平移。單軸旋轉(zhuǎn)將導(dǎo)致平移校準沿旋轉(zhuǎn)軸變得不可觀?？煽康目臻g校準的最小運動要求是至少一個全局軸平移和雙軸旋轉(zhuǎn)，為了最大限度地使標定過程中LiDAR-IMU外參的各個量可觀，車輛需要有X、Y軸上的旋轉(zhuǎn)運動，即包含roll和pitch分量上的運動激勵，因此，采集的預(yù)數(shù)據(jù)集會包含“∞”環(huán)的運動序列，并含有上下斜坡的運動序列，使其包含Z軸方向的運動激勵，最后預(yù)數(shù)據(jù)集還需要超過3 min的駕駛時間，以確保在旋轉(zhuǎn)外參解析解初值計算階段有滿足本文算法篩選的可用IMU數(shù)據(jù)進行校準，盡量避免IMU數(shù)據(jù)完全退化，確保LiDAR-IMU外參參數(shù)的六自由度可觀性。

3.2 旋轉(zhuǎn)外參解析解初值計算改進對比實驗

3.2.1改進后的手眼標定方程組窗口大小

表1 不同手眼標定方程組窗口大小下外參標定誤差

該實驗的評價指標為不同手眼標定方程組窗口大小獲得旋轉(zhuǎn)外參yaw、pitch和roll的標定結(jié)果與目標值的誤差均值，與目標值的偏差越小，該窗口大小越好。從6組不同窗口設(shè)置的標定結(jié)果中可以看出，以10幀的滑動窗口大小來求解手眼標定方程組的旋轉(zhuǎn)外參最小二乘解的誤差均值最小，最小為0.775°，對應(yīng)yaw、pitch和roll三個角的誤差為(1.190°,0.625°,0.510°)。該實驗的結(jié)論為：在以上6組的窗口大小配置中，手眼標定方程組最佳窗口大小設(shè)置為10幀，在滿足旋轉(zhuǎn)外參解析解標定結(jié)果精確度的前提下，還能實現(xiàn)以小規(guī)模(10幀大小)的手眼標定方程組來減小求解復(fù)雜度，加快求解速度。

另外，為了對比剔除外點算法和原始算法構(gòu)建的手眼標定方程旋轉(zhuǎn)外參解析解多解誤差分布情況，將它們的旋轉(zhuǎn)外參yaw、pitch和roll多解誤差繪制成圖9所示的箱線圖，從圖中可以看出，剔除外點算法的旋轉(zhuǎn)外參解析解的yaw、pitch和roll三個角的誤差外點數(shù)量要遠少于原始算法，同時誤差均值也更接近零，說明剔除外點算法的旋轉(zhuǎn)外參解析解穩(wěn)定性和精確度更高，對退化傳感器數(shù)據(jù)的魯棒性更高。

圖9 剔除外點算法與原始算法誤差分布對比

3.2.2旋轉(zhuǎn)外參解析解多解加權(quán)方式

由于第一階段改進為以滑動窗口的形式，以10幀的窗口大小多次求解旋轉(zhuǎn)外參解析解，并以奇異值分解的奇異值矩陣中的第三大與第四大奇異值σ3/σ4的比值作為閾值進行篩選解析解來判斷運動激勵，因此為了基于數(shù)據(jù)集的最大化成功率計算旋轉(zhuǎn)外參解析解，本文以奇異值σ3/σ4的比值來加權(quán)每次求解的旋轉(zhuǎn)外參解析解結(jié)果。為了將第一階段中窗口滑動過程中的所有手眼標定方程組求解的解析解都利用起來，以達到基于預(yù)數(shù)據(jù)集的最大成功率獲得最佳的旋轉(zhuǎn)外參解析解初值的目的，下面通過實驗來確定最佳的加權(quán)方式。

以奇異值σ3/σ4的比值大于2.5作為篩選閾值，采用8種不同的加權(quán)方法，分別對4組不同運動序列的數(shù)據(jù)集進行32組實驗，來探索8種不同的加權(quán)方法中，手眼標定方程組旋轉(zhuǎn)外參解析解多解的最佳加權(quán)方式，實驗結(jié)果如表2所示。

該實驗的評價指標為以不同手眼標定方程組旋轉(zhuǎn)外參解析解的加權(quán)方式獲得旋轉(zhuǎn)外參yaw、pitch和roll結(jié)果與目標值的誤差均值，誤差均值越小，說明該加權(quán)方式越好。由表2可得，由加權(quán)方式D的誤差均值最小，最小為2.578°，對應(yīng)yaw、pitch和roll三個角的誤差為(5.669°,0.799°,1.265°)，因此，解析解多解的最佳加權(quán)方式為D，即篩選多解中滿足σ3/σ4>2.5的解，并以奇異值σ3/σ4作為各個解的權(quán)重加權(quán)求和，誤差均值最小。

表2 不同加權(quán)方式下的外參標定誤差

3.3 非線性滑窗迭代優(yōu)化改進對比實驗

為了避免迭代優(yōu)化中一些退化運動造成系統(tǒng)狀態(tài)量偏離，本文在出現(xiàn)退化運動時選擇固定外參，不進行外參優(yōu)化，最大化利用求解手眼方程組的解析解第一階段提供的良好的初始值，以避免陷入優(yōu)化的局部極小值。因此，在實驗場地中含有直道和彎道的地方進行在線標定實驗，與原始算法對比，以驗證固定外參策略的有效性。

如圖10所示，通過非線性優(yōu)化中外參迭代優(yōu)化過程的對比，觀察旋轉(zhuǎn)外參yaw、pitch和roll的變化可以得出：本文帶有固定外參策略的改進算法的旋轉(zhuǎn)和平移外參量在迭代到40幀左右便收斂至零誤差附近，而原始算法在迭代到200幀以后才逐漸收斂，且Z方向上的平移和yaw方向的旋轉(zhuǎn)誤差較大。因此，兩階段LiDAR-IMU外參在線標定算法收斂速度快于原始算法收斂速度，可有效避免外參在非線性迭代優(yōu)化中陷入局部最小值。

(a)平移外參數(shù)

(b)歐拉角旋轉(zhuǎn)外參數(shù)圖10 固定外參算法與原始算法外參變化對比

3.4 3D點云重建可視化綜合驗證

在第二階段的緊耦合SLAM過程中，除了在線標定LiDAR-IMU之間的外參，該階段也會進行點云地圖的構(gòu)建。為了直觀地比較本文改進算法的外參標定成功率和精度，與原始算法進行同一數(shù)據(jù)集的建圖效果可視化對比，分別在地點A、B中進行實驗，3D點云重建結(jié)果如圖11所示。

(a)地點A-原始算法 (b)地點A-本文算法

(c)地點B-原始算法 (d)地點B-本文算法圖11 3D點云重建可視化對比

在地點A的即時定位與建圖過程中，原始算法標定結(jié)果的誤差太大，外參陷入局部極小值，無法在迭代中恢復(fù)，構(gòu)建的點云地圖漂移，標定失敗。而本文算法標定結(jié)果的誤差小，外參迭代收斂至真值附近，構(gòu)建的點云地圖緊湊規(guī)整，標定成功。在地點B的即時定位與建圖過程中兩個算法都標定成功，但是原始算法點云地圖的左下部和右下部紅色框內(nèi)含有重影，未完全收斂，而本文算法重建的點云地圖在這兩處都收斂，地圖質(zhì)量優(yōu)于原始算法地圖質(zhì)量。因此，在本文所測試的數(shù)據(jù)集中，兩階段LiDAR-IMU外參在線標定算法對退化的傳感器數(shù)據(jù)具有魯棒性，能夠有效提高外參標定成功率，在即時定位與建圖過程中，本文算法在標定后的3D點云建圖成功率和精度均優(yōu)于原始算法。

4 結(jié)論

為了解決無人駕駛中傳感器噪聲和退化運動導(dǎo)致LiDAR-IMU外參標定失敗的問題，本文提出了一種高魯棒性兩階段LiDAR-IMU外參在線標定框架，第一階段是旋轉(zhuǎn)外參解析解初值計算，第二階段是非線性滑窗在線迭代優(yōu)化。與原始框架相比，其魯棒性改進方面可以總結(jié)為以下幾點：

(1)第一階段通過剔除外點去除退化的傳感器數(shù)據(jù)來構(gòu)建手眼標定方程組，在剔除外點算法的旋轉(zhuǎn)外參解析解多解中，yaw、pitch和roll三個角的誤差外點的數(shù)量要遠少于原始算法，提高了標定算法對包含退化傳感器數(shù)據(jù)的魯棒性。

(2)通過滑動窗口多次求解解析解初值來篩選多解中滿足相對閾值σ3/σ4>2.5的解，并以奇異值σ3/σ4作為各個解的權(quán)重加權(quán)求和，旋轉(zhuǎn)外參yaw、pitch和roll的誤差均值最小，最小為2.578°。

(3)第二階段非線性滑窗在線迭代優(yōu)化時設(shè)置了固定外參的策略，在退化運動和錯誤歷史約束過大時固定外參，避免了第一階段獲得優(yōu)化初值漂移過大，陷入局部最小值。

(4)將原始算法的三維點云重建結(jié)果與兩階段LiDAR-IMU外參在線標定算法的重建結(jié)果進行對比，本文所改進的算法對退化的傳感器數(shù)據(jù)具有魯棒性，提高了標定成功率和精度。