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

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

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

0 引言

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

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

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

1 基本原理

1.1 手眼標定模型

圖1 不同位置下里程計增量關系

(1)

(2)

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

通過計算平移部分和旋轉部分6個參數，可以得到IMU坐標系與LiDAR坐標系的六自由度變換關系。

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

圖2 只考慮旋轉分量的手眼標定模型示意圖

(3)

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

(4)

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

(5)

(6)

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

(7)

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

(8)

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

(9)

(10)

(11)

QN=UΣVT

(12)

1.2 含外參的非線性滑窗迭代優化

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

圖3 滑動窗口優化算法

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

(13)

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

(14)

(15)

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

2 改進算法

2.1 算法框架

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

2.2 旋轉外參解析解初值計算改進

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

圖5 兩個里程計增量夾角

圖6 旋轉外參解析解初值計算改進框架圖

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

(16)

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

σ3/σ4>threshold

(17)

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

2.3 非線性滑窗在線迭代優化改進

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

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

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

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

(18)

(19)

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

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

(20)

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

3 實驗分析

3.1 實驗設計

3.1.1實驗平臺與傳感器數據預處理

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

圖8 車載實驗平臺

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

3.1.2可觀性分析與運動要求

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

3.2 旋轉外參解析解初值計算改進對比實驗

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

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

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

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

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

3.2.2旋轉外參解析解多解加權方式

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

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

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

表2 不同加權方式下的外參標定誤差

3.3 非線性滑窗迭代優化改進對比實驗

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

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

(a)平移外參數

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

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

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

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

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

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

4 結論

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

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

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

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

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