基于激光點云特征的LiDAR-GNSS/IMU 聯合自動標定方法

秦曉輝 ，劉碩 ，常燈祥 ，李建中 ，周云水 ，謝國濤 ?

（1.湖南大學 機械與運載工程學院，湖南 長沙 410082；2.湖南大學無錫智能控制研究院，江蘇 無錫 214115）

在智能駕駛系統中，車體安裝有大量傳感器，通過多種傳感器數據的互補，用來實現定位、環境感知、決策、規劃等功能［1］.激光雷達（Light Detection and Ranging，LiDAR）掃描范圍廣泛，獲得的點云包含豐富的信息，常被用于同步定位與建圖中，但它具有運動畸變的缺點.GNSS/IMU 組合慣導系統能夠提供高頻的全局定位信息，且基于載波相位的差分全球導航衛星系統技術（RTK）可以采集厘米級的定位信息，但是GNSS數據在復雜的城市道路環境中會產生信號丟失等現象.基于上述原因，在室外環境的導航定位系統中，將激光雷達數據與GNSS/IMU數據融合是十分必要的［2-3］.

將LiDAR 數據與GNSS/IMU 數據進行融合的前提是對兩傳感器間外部參數進行精確標定，外部參數的標定方法分為手動標定和自動標定［4-6］，手動標定是通過人工測量來確定標志物的位置或者兩傳感器間的對應關系，通過此關系來提供準確的標定信息，此方法過于依賴人為操作或特殊的標定物.近年來，多種新穎的自動標定方法開始涌現，可分為基于深度學習的標定、基于運動的標定、基于特征的標定［7］和基于互信息的標定［8-9］.對于LiDAR 與GNSS/IMU 的標定問題，由于GNSS/IMU 系統只能提供運動信息和位置姿態信息，通常采用基于運動的標定方案.

基于運動的標定方法［10-14］不依賴特定的圖像或激光點云特征，只要兩傳感器均能獲得自身的運動，就能對外部參數進行標定，自動化程度較高.目前基于運動的多傳感器間的標定方法，大多是在機器人手眼標定的框架基礎上進行外參求解.當兩個傳感器剛性連接且各自的運動增量已知時，求解傳感器間的外部參數的問題，可轉換為求解AX=XB的矩陣方程的問題，其中A、B為傳感器各自的相對運動增量，X為傳感器間的外部參數，此過程稱為機器人手眼標定［15-16］，常采用兩步法和直接優化法進行求解.

基于運動的標定比較經典的是Taylor 等［17］的方法，作者通過視覺里程計計算相機的位姿，通過ICP配準來計算LiDAR 的運動，在手眼標定的框架上標定外部參數和時間偏移，并對傳感器不同采樣率引起的誤校準進行檢測和誤差補償.Xia 等［18］提供了一種LiDAR-GNSS/IMU 標定解決方案，首先利用初始的標定外參，將LiDAR、GNSS 數據投影到世界坐標系中，并通過點云配準找到點與點間最好的對應關系，用最佳對應點的歐氏距離之和來構造損失函數優化求解，但該方法魯棒性較差.

Chen 等［19］提出了一種LiDAR-GNSS/IMU 解耦的聯合標定方法，該方法將標定參數解耦為兩組，偏航角、X和Y方向平移通過手眼標定模型獲得，俯仰角、翻滾角和Z軸平移通過GNSS 在車體安裝位置手動獲得，此方法在每次標定時都要對GNSS 重新安裝，較為繁雜.Lü 等［20］提出了一種基于連續時間的LiDAR-IMU 標定方法，其利用樣條曲線獲得連續的IMU 數據，通過手眼標定對外部參數初始化，基于此初始值和連續的IMU 位姿去除激光點云的畸變，并構建點到平面的約束問題，迭代求解精確的外部參數.此方法基于NDT構建點云地圖，會產生較大累積誤差，且其只提取地圖中的面特征，在室外環境中會因為缺少“非地面的平面特征約束”導致標定精度變差.

基于上述方案的不足之處，且受呂佳俊LiDARIMU 標定方案［20］中的啟發，針對室外環境下的LiDAR-GNSS/IMU 標定問題，提出一種融合激光雷達點云特征的LiDAR-GNSS/IMU 聯合自動標定方法，此方法通過激光里程計構建全局點云地圖，對地圖提取線特征聚類和面特征體素，通過線特征聚類、面特征體素關聯并結合運動約束，構建外部參數聯合標定最小二乘優化問題，求解LiDAR-GNSS/IMU間精確的外部參數.此方法通過回環約束優化地圖，減小累積誤差，提升點云特征提取的精度，引入線特征約束，提升在缺少點云面特征室外場景下的標定精度和穩定性，并通過實驗對比驗證了此方法的有效性.

1 標定方案概述

本文的標定算法流程如圖1 所示.首先車輛采集能形成單一閉環的激光點云數據和GNSS/IMU 數據；對點云數據采用激光里程計計算每一激光幀的位姿，并通過回環檢測構建位姿圖優化問題；對激光里程計數據和GNSS/IMU 數據分別求取相對位姿，通過手眼標定計算初始外部參數，并用此參數和GNSS/IMU 位姿對每一幀的點云去畸變；利用去畸變后的點云和回環約束后的位姿構建面特征地圖和線特征地圖，并對兩類地圖分別檢測“面特征體素”和“線特征聚類”；將每一幀去畸變后的激光點通過回環約束后的位姿與“面特征體素”和“線特征聚類”進行所屬關系關聯.構建以外部參數為優化量的最小二乘問題，并加入點線關聯的距離殘差、點面關聯的距離殘差和手眼標定殘差，通過高斯-牛頓法進行求解，以得到精確的外部參數.

圖1 LiDAR-GNSS/IMU 自動標定算法流程圖Fig.1 LiDAR-GNSS/IMU automatic calibration algorithm flow chart

2 算法設計

本節對LiDAR-GNSS/IMU 標定系統所包含的各個核心流程進行設計與理論分析，包括兩類激光特征點云地圖的構建、外部參數的初始化、點云地圖中的特征提取、融合激光點云特征與運動優化問題的構建.

2.1 激光里程計與地圖的構建

本文激光里程計是基于LOAM［21］的改進算法，首先對多線激光雷達數據線束劃分，對一幀點云進行聚類，并對每一線束的點云進行特征提取；對相鄰兩幀進行特征匹配，推算當前幀的位姿并判斷是否為關鍵幀；基于關鍵幀建立局部點云地圖，進行當前關鍵幀與局部地圖的匹配，得到精確的里程計；進行起始幀與結束幀的閉環檢測，構建位姿圖優化問題求得校正后的里程計位姿.

2.1.1 特征點云提取

特征點云提取參照了LOAM 算法［21］中激光點曲率計算方法，首先剔除遮擋點和斜面點這兩類不穩定點，并通過一線中的鄰域點計算當前幀每一個激光點的曲率，曲率計算表達式為：

通過曲率c的值對每一線的激光點進行排序，將大于角點閾值的歸類為線特征點，將小于面點閾值的歸類為面特征點.之后采用LeGO-LOAM 算法［22］中的點云聚類思想，將樹葉點等不穩定特征點剔除，得到每一幀激光點云中的線特征點與面特征點.

2.1.2 回環檢測與地圖構建

由于誤差的累積，當激光里程計回到原點附近時，構建的地圖具有重影，因此通過回環檢測構建位姿圖優化問題，得到全局位姿的最優解來減小累積誤差.如圖2 所示，由于激光里程計累積誤差較小，本文算法以距離為判定條件，在原點附近檢測兩幀間距離是否滿足閉環約束條件.對約束的先前幀構建局部地圖，采用點云ICP 的方法，進行幀到局部地圖的配準，并根據ICP 中最近點距離的總和是否小于設定的閾值進一步判斷當前回環是否精確.

圖2 點云幀回環檢測Fig.2 Point cloud frame loopback detection

在滿足回環的激光幀對中，選擇ICP 配準后最近點距離總和最小的一對作為回環幀約束，并構建位姿圖優化問題，通過高斯-牛頓法進行求解得到優化后的里程計位姿.通過回環約束后的位姿，將2.1.1節中每一幀的線特征點與面特征點構建點云地圖，得到面特征點云地圖和線特征點云地圖，如圖3 所示，紅色為線特征點云地圖，藍色為面特征點云地圖.

圖3 特征點云地圖Fig.3 Feature Points Cloud Map

圖4 基于運動的外參初始化Fig.4 Initialization of external parameters based on motion

2.2 基于運動的外參初始化

求解此方程，得到從GNSS/IMU 坐標系到LiDAR坐標系下的變換，即傳感器間的外部參數.

對于LiDAR 與GNSS/IMU 間的手眼標定問題，由于激光里程計的計算頻率較高，兩關鍵幀間的相對運動較小，相鄰關鍵幀構建的手眼標定問題不具有穩定性.本文通過對相對運動進行累積，當平移和旋轉大于設定的閾值時，對此時的LiDAR 和GNSS/IMU 相對運動進行選用，在里程計累積誤差較小的同時，也保證有一定的運動激勵.

選擇所有符合上述條件的n組手眼標定方程組，形成一組關于求解的超定方程組，公式如下：

對于此超定方程構建最小二乘優化問題，把每一項手眼標定方程作為殘差加入優化目標函數中.

式中：ri為構建每一項的殘差.將n組殘差的馬氏范數累加得到總體的無約束最小二乘優化目標函數如下：

式中：ρi(·)為核函數；F() 為構建的目標函數，對此目標函數采用高斯-牛頓法求解，得到外部參數的最優估計.

2.3 特征提取與關聯

在本文的特征提取算法中，首先對2.1.2 節獲得的面特征點云地圖和線特征點云地圖分別進行“面特征體素”和“線特征聚類”的提取，并把所有關鍵幀中的每一點通過位姿圖優化后的位姿轉到地圖坐標系下，計算其與體素和聚類的關聯.

2.3.1 面特征體素提取與關聯

本文的面特征提取算法［16］首先將面特征點云地圖劃分為三維體素網格，對每一體素網格內的點云計算其協方差矩陣，并進行特征值分解，得到從小到大的三個特征值e0、e1、e2，通過式（6）計算此體素網格內點云的平整程度［23］：

當網格內點云的越趨近于平面時，λ值越接近1.由于激光里程計具有一定的誤差，本文認為當λ值大于0.7 時，此體素網格內的點云滿足面特征，此體素網格被歸類為“面特征體素”，如圖5 所示，面特征點云地圖中包含多個“面特征體素”.

對“面特征體素”中的點云，采用隨機抽樣一致性算法擬合出面特征平面方程Ax+By+Cz+D=0 的4 個參數[A，B，C，D]，并將所有關鍵幀中的面特征點通過位姿圖優化后的位姿轉到地圖坐標系下，基于每一激光點與體素網格的所屬關系和點到平面的距離，篩選出每一“面特征體素”所關聯的所有關鍵幀中激光點，然后對每一“面特征體素”所關聯的激光點降采樣，完成面特征關聯.

2.3.2 線特征聚類提取與關聯

在線特征聚類提取的算法中，本文首先對線特征點云地圖進行離群點濾除，然后采用歐式聚類分割，將線特征點云地圖劃分成多個點云聚類；對于每一聚類內的點云計算其協方差矩陣，并對協方差矩陣進行特征值分解，得到從小到大的三個特征值e0、e1、e2，通過三個特征值之間的關系，提出式（7）來計算線特征點云聚類的直線度：

式中：β為點云聚類的直線度，當聚類的點云越接近直線時，直線度β值越接近1，本文認為直線度β值大于0.65 的點云聚類符合線特征，如圖6 所示為線特征地圖檢測出的所有線特征聚類.

圖6 線特征點云聚類Fig.6 Line feature point cloud cluster

對滿足直線度的線特征聚類中的點云，計算點云的最小包圍盒，并采用隨機抽樣一致性算法擬合出線特征的表示參數[O，a0]，其中O是直線上一點的坐標，a0是直線的方向向量.之后將所有關鍵幀的線特征點云通過位姿圖優化后的位姿轉到地圖坐標系下，基于每一激光點與點云包圍盒的所屬關系和點到直線的距離，分類出每一“線特征聚類”所關聯的所有關鍵幀中激光點，然后將所關聯的激光點云降采樣，完成線特征關聯.

2.4 聯合優化求解

2.4.1 聯合優化問題構建

在完成特征提取與關聯后，把每一“面特征體素”所關聯的激光點和每一“線特征聚類”所關聯的激光點通過位姿圖優化后的位姿，轉到地圖坐標系下，轉換公式為：

式中：PL是對應點在激光雷達關鍵幀下的坐標；是當前幀的GNSS/IMU 到GNSS 世界坐標系下的變換矩陣；PM是通過GNSS/IMU 位姿和外參轉換到激光世界坐標系（地圖坐標系）下的面特征激光點.

對屬于面特征體素的激光點，計算此點到“面特征體素”點云所擬合平面的距離dM，公式如下：

其中，A、B、C、D為“面特征體素”內擬合平面方程Ax+By+Cz+D=0 的參數.

對屬于線特征聚類的激光點，計算此點到線特征聚類點云所擬合直線的距離dL，公式如下：

其中，O、a0分別為“線特征聚類”所擬合直線上一點的坐標和其方向向量.

通過外部參數轉化后，點到對應直線的距離和點到對應平面的距離能夠反映外參標定結果的精確程度，因此在2.2節構建的無約束最小二乘優化目標函數中，加入點到線距離的殘差和點到平面距離的殘差，以提高標定結果的準確性與魯棒性，總體優化目標函數如下：

式中：rhe為手眼標定所構建的殘差；rp為點到平面距離所構成的殘差；rl為點到直線距離所構成的殘差；ρi(·)、ρj(·)、ρk(·)分別為上述各殘差的核函數，采用高斯-牛頓法最小化目標函數進行求解，可以得到優化后的精確外部參數

2.4.2 迭代優化求解

3 實驗結果與分析

3.1 實驗平臺與設備

本算法的實驗平臺如圖7 所示，該實驗車輛安裝有16 線旋轉式機械激光雷達、融合載波相位差分（RTK）的GNSS/IMU 組合慣性導航設備.多線激光雷達采集激光點云數據，采樣頻率為10 Hz，點云精度可達厘米級；組合導航系統采集GNSS/IMU 位姿和軌跡數據，頻率為50 Hz，定位精度小于2 cm，姿態精度可達0.02°.

圖7 實驗平臺Fig.7 Experimental platform

激光雷達坐標系原點在雷達機械結構幾何中心，坐標系的X軸沿著車輛行進方向向前，坐標系的Y軸垂直于車輛行進方向向左，Z軸垂直于地面向上.GNSS/IMU 坐標系原點在車輛后軸中心，坐標系的X軸垂直于車輛行進方向向右，坐標系的Y軸沿著車輛前進方向向后，Z軸直于地面向上.

3.2 實驗場地與數據

本文在4 處場地進行實驗，分別為室外小型停車場、丁字路口、十字路口、室外園區，如圖8 所示，4處場地均具有良好的GNSS 信號和豐富的三維激光點云特征.實驗時，車輛在行駛中進行旋轉和平移運動，并分別采集形成單一閉環的激光點云數據和GNSS/IMU位姿軌跡數據.

圖8 標定試驗場地Fig.8 Calibration experiment site

4處場地場景大小不同，且各自所包含的線特征和面特征數量也有較大的差異，圖8（a）為室外小型停車場，停車場臨靠大樓和樹林，具有豐富的面特征和線特征；圖8（b）為丁字路口，該路口兩側具有較高的圍墻，此場景富含面特征，但缺少線特征；圖8（c）為十字路口，該場地四周被樹木和指示牌包圍，富含線特征，但缺乏除地面外的面特征；圖8（d）為室外園區，場景尺寸較大，且該場地包含線特征、面特征.在4種場地中采集數據的情況見表1.

表1 4種場地數據情況Tab.1 Data details of four sites

3.3 實驗結果與分析

3.3.1 實驗結果

本實驗平臺車輛中的GNSS/IMU 以Z軸垂直于地面的方式安裝，由于GNSS/IMU的高度數據誤差較大，在實驗中不會被使用，本文算法得到的外部標定數據有旋轉標定外部參數θyaw、θpitch、θroll和平移標定外部參數X、Y.通過傳感器在車輛上的安裝方式能夠獲得的車輛LiDAR 與GNSS/IMU 間外部參數的參考值（非真實值）見表2.

表2 外部參數參考值Tab.2 Reference value of external parameter

對4 種不同場景中采集到的多組實驗數據進行外部參數的標定，標定的部分結果見表3.

表3 外部參數標定結果Tab.3 Calibration result of external parameter

本文標定算法分為里程計與建圖、外參標定兩步驟，為分析算法的實效性，統計標定實驗中各步驟花費的時間.如表4 所示，外參標定步驟耗時短，能夠保證算法的時效性.

表4 外部參數標定時間Tab.4 Cost time of external parameter calibration s

3.3.2 算法標定準確性驗證

在獲得標定參數后，將GNSS/IMU的位姿通過此標定參數轉到LiDAR 坐標系下，通過對比轉化后得到GNSS/IMU 位姿軌跡與經過回環檢測優化后的激光里程計位姿軌跡的對齊程度，能夠反映外部參數標定結果的精確程度.

本文采用evo評估工具對算法標定準確性驗證，如圖9 所示：圖9（a）和圖9（b）的實驗數據均在室外小型停車場中采集，虛線軌跡為回環檢測后的激光里程計軌跡；橙色軌跡為使用本文算法獲得的標定值將GNSS/IMU 的位姿轉到LiDAR 坐標系下形成的軌跡；藍色為使用手眼標定的結果將GNSS/IMU的位姿轉到LiDAR坐標系形成的軌跡.

圖9 不同外參的軌跡對準情況Fig.9 Track alignment of different external parameters

通過與回環檢測軌跡進行對比，能夠發現經過本文算法轉化后的軌跡可以很好地與里程計軌跡重合，且相較于手眼標定初始化參數的轉換軌跡，本文算法在車輛轉彎處、長段平移處均有著較好的重合效果.

為進一步驗證標定結果的準確性，本文針對室外小型停車場數據1，采用evo 工具計算軌跡間的絕對位置誤差（ATE）以評估標定精度.回環優化后的軌跡和經手眼標定外參轉換后的GNSS/IMU 軌跡間的絕對位置誤差如圖10 所示，其中絕對位置誤差的均方根誤差（RMSE）為0.396 m、平均絕對誤差（MAE）為0.332 m、中值誤差為0.325 m、最大誤差為0.758 m.

圖10 手眼標定轉換后軌跡精度評估Fig.10 ATE of trajectory accuracy after hand-eye calibration

經過本文算法轉化后的GNSS/IMU 軌跡和回環優化后軌跡間的絕對軌跡誤差如圖11 所示，其中均方根誤差（RMSE）為0.232 m，平均絕對誤差（MAE）為0.201 m，中值誤差為0.199 m，最大誤差為0.460 m.

圖11 經本文算法外參轉換后的軌跡間誤差Fig.11 The trajectory error after extrinsic parameter transformation using our method

本文算法轉換后的軌跡與回環優化后軌跡間ATE 的均方根誤差、均值誤差等都顯著小于手眼標定結果，兩者全時間周期內ATE 對比結果如圖12 所示，可以體現算法的有效性與準確性.

圖12 軌跡間ATE對比結果Fig.12 Result of ATE comparison between tracks

3.3.3 算法穩定性分析

為了分析本文算法在不同場景下求解的穩定性，將圖8 前三種環境中求得的多組標定外部參數的θyaw、X、Y值分別繪制成圖13 所示的箱線圖，通過對比標定參數的分布情況能夠體現本文算法在不同點云特征環境下求解的穩定性.圖13 中的紅色箱體為本文算法求解的標定參數，藍色箱體為手眼標定求解的標定參數，X軸為兩標定算法在場地1、場地2、場地3中求解結果的分布表現.

圖13 多場景標定算法穩定性分析Fig.13 Stability analysis of multi-scene calibration algorithm

從圖13 中可以看出，在富含線面特征的場景、富含線特征但缺乏面特征的場景和富含面特征但缺乏線特征的三個場景中，相較于手眼標定，本文的標定算法求得的標定結果分布更為集中，且一致性較好，能體現本文的標定算法在多種類型的場景中，標定穩定性和精度均高于手眼標定.

為分析本文算法在大場景下的穩定性和精度，本文在圖8（d）所示的大型室外園區場景進行標定實驗，此場地路徑全長411 m.圖14（a）所示為手眼標定外參轉換軌跡、本文算法外參轉換軌跡和回環檢測后軌跡.如圖14（b）所示，將本文算法ATE 與手眼標定算法ATE 對比，本文標定算法轉換軌跡ATE 的均方根誤差為0.549 m，手眼標定算法轉換軌跡ATE 的均方根誤差為1.054 m，可見本文標定算法軌跡與回環后軌跡間誤差更小，能體現算法在大場景下的有效性和精度.

圖14 大型場景（場地4）算法穩定性分析Fig.14 Algorithm stability analysis in large sites（site 4）

為分析算法在缺乏面特征的場景中的穩定性與精度，本文在場地3 中對是否使用線特征約束做出分析，標定結果如圖15 所示.僅使用面特征約束優化的外參轉換軌跡相對于回環后里程計軌跡有大幅度的偏移；使用線特征約束與面特征約束的軌跡，與回環后里程計軌跡有較好的重合效果，能夠體現該算法可以很好地解決在面特征缺失的室外場景下的自動標定問題.

總體而言，本文所提出的基于激光點云特征的LiDAR-GNSS/IMU 聯合自動標定算法的求解穩定性和精度均優于手眼標定算法，且在缺少點云特征的室外場景下，有較好的魯棒性.

4 結論

針對室外LiDAR 與GNSS/IMU 標定中僅依賴運動約束導致的約束不足、標定精度差、魯棒性低的問題，本文設計了一種基于激光點云特征的LiDARGNSS/IMU 聯合自動標定方法，利用回環檢測優化后的位姿構建兩類特征點云地圖并將激光點與相應的特征進行關聯，在基于運動約束的基礎上，增加了面特征和線特征約束，以提高算法的精確性和魯棒性.此方法不需要特殊的標志物和繁雜的人工測量，僅需要激光點云數據和GNSS/IMU 數據即可自動標定出兩傳感器間的外部參數，便于實際的應用.此算法仍有一些不足之處，此標定算法的激光里程計模塊只通過閉環檢測與位姿圖優化消除重影以提高點云地圖的全局一致性，沒有耦合GNSS/IMU 的真實位置數據進一步提升激光里程計的精度，從而影響標定結果的準確性，這也會在后續的研究中做出改進.