林 鑫 張 捷 馮景怡 孟 杰 王書(shū)亭

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

0 引言

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

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

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

1 基本原理

1.1 手眼標(biāo)定模型

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

(1)

(2)

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

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

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

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

(3)

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

(4)

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

(5)

(6)

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

(7)

[qv]×為虛部向量的反對(duì)稱(chēng)矩陣：

(8)

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

(9)

(10)

(11)

QN=UΣVT

(12)

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

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

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

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

(13)

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

(14)

(15)

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

2 改進(jìn)算法

2.1 算法框架

圖4 兩階段LiDAR-IMU外參標(biāo)定流程框架

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

2.2.1剔除外點(diǎn)的手眼標(biāo)定模型

圖5 兩個(gè)里程計(jì)增量夾角

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

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

(16)

2.2.2手眼標(biāo)定模型求解的判斷條件改進(jìn)

σ3/σ4>threshold

(17)

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

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

2.3.1兩階段LiDAR-IMU外參標(biāo)定算法框架圖

圖7 兩階段LiDAR-IMU外參標(biāo)定算法框架圖

2.3.2退化運(yùn)動(dòng)時(shí)的固定外參策略

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

(18)

(19)

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

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

(20)

若rcost大于閾值，說(shuō)明錯(cuò)誤的歷史約束比較大，則固定外參。進(jìn)一步地，若rcost遠(yuǎn)大于閾值，說(shuō)明錯(cuò)誤的歷史約束過(guò)大了，則固定外參的同時(shí)重置歷史約束殘差。

3 實(shí)驗(yàn)分析

3.1 實(shí)驗(yàn)設(shè)計(jì)

3.1.1實(shí)驗(yàn)平臺(tái)與傳感器數(shù)據(jù)預(yù)處理

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

圖8 車(chē)載實(shí)驗(yàn)平臺(tái)

在獲取LiDAR和IMU兩個(gè)傳感器的數(shù)據(jù)信息后，首先做的是數(shù)據(jù)同步。LiDAR點(diǎn)云頻率為10 Hz，即獲取的位置信息更新頻率為10 Hz，而IMU點(diǎn)云頻率數(shù)據(jù)為200 Hz，兩者均采用UNIX時(shí)間戳機(jī)制，記錄結(jié)果自1970年1月1日(UTC時(shí)間)開(kāi)始。因?yàn)椴荒鼙ＷC找到具有相同時(shí)間戳的數(shù)據(jù)，因此通過(guò)時(shí)間插值來(lái)實(shí)現(xiàn)同步對(duì)齊。同步對(duì)齊后，可以得到同一時(shí)間序列的兩個(gè)傳感器的位姿信息。

3.1.2可觀性分析與運(yùn)動(dòng)要求

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

3.2 旋轉(zhuǎn)外參解析解初值計(jì)算改進(jìn)對(duì)比實(shí)驗(yàn)

3.2.1改進(jìn)后的手眼標(biāo)定方程組窗口大小

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

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

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

圖9 剔除外點(diǎn)算法與原始算法誤差分布對(duì)比

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

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

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

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

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

3.3 非線性滑窗迭代優(yōu)化改進(jìn)對(duì)比實(shí)驗(yàn)

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

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

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

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

3.4 3D點(diǎn)云重建可視化綜合驗(yàn)證

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

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

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

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

4 結(jié)論

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

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

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

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

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