激光雷達(dá)與慣性測量單元同步融合下的園區(qū)三維建圖

馬慶祿， 汪軍豪， 張 杰， 鄒 政

（1. 重慶交通大學(xué) 交通運(yùn)輸學(xué)院， 重慶 400074；2. 同濟(jì)大學(xué) 道路與交通工程教育部重點(diǎn)實(shí)驗(yàn)室， 上海 201804）

1 引 言

隨著自動(dòng)駕駛技術(shù)的不斷發(fā)展，同步定位與建圖（Simultaneous Localization and Mapping，SLAM）技術(shù)逐步成熟。SLAM 技術(shù)是指通過車輛在未知環(huán)境中動(dòng)態(tài)構(gòu)建的空間特征從而進(jìn)行自身位姿估計(jì)，實(shí)現(xiàn)同步定位和增量式建圖的目標(biāo)。

隨著一個(gè)在未知位置和未知環(huán)境中移動(dòng)的機(jī)器人能否在建立環(huán)境地圖的同時(shí)確定他自身的位置的問題被提出，SLAM 技術(shù)不斷發(fā)展，已經(jīng)從單傳感器發(fā)展為多傳感器SLAM。SLAM技術(shù)應(yīng)用的傳感器主要包括激光雷達(dá)與視覺傳感器［1］。陳龍等［2］通過對(duì)比激光雷達(dá)SLAM 與視覺SLAM 的定位建圖效果，發(fā)現(xiàn)激光SLAM 比視覺SLAM 更具備穩(wěn)定性。有研究發(fā)現(xiàn)慣性測量單元（Inertial Measurement Unit， IMU）與3D激光雷達(dá)有很好的互補(bǔ)性［3］。Ji 等［4］提出的LOAM 算法框架通過低頻高精的后端與高頻低精的前端來提升SLAM 效能，但LOAM 算法無法使用IMU 進(jìn)行整個(gè)系統(tǒng)的優(yōu)化。Gilmar 等［5］將LOAM 算法進(jìn)行改進(jìn)，提出了適用性更好的EKF-SAM 算法，但它無法進(jìn)行回環(huán)檢測，也無法融入絕對(duì)測量數(shù)據(jù)進(jìn)行位姿校正，而且LOAM存在大場景測試中偏移的問題。Michael 等［6］提出了激光雷達(dá)與IMU 融合的Fast-SLAM 2.0 算法，該方法增強(qiáng)了設(shè)備位于不平坦區(qū)域的建圖能力。黃粒等［7］通過改進(jìn)后端優(yōu)化提高計(jì)算效率，取得了較低的軌跡誤差、絕對(duì)軌跡誤差和相對(duì)位姿誤差，提高了同步定位與建圖的精度。楊智宇等［8］利用因子圖聯(lián)合優(yōu)化共同提高位姿估計(jì)精度。王挺等［9］運(yùn)用GR-LOAM 算法，通過激光雷達(dá)和IMU 等多傳感器融合來提高建圖效果，由于傳感器本身存在精度誤差，多傳感器融合容易導(dǎo)致累計(jì)誤差的產(chǎn)生。尹芳等［10］針對(duì)模型未知的空間非合作旋轉(zhuǎn)目標(biāo)的模型重建和位姿估計(jì)問題，利用激光雷達(dá)采集的3D 點(diǎn)云，提出一種基于位姿圖優(yōu)化的SLAM 技術(shù)框架，以解決跟蹤過程中產(chǎn)生的累積誤差問題。楊林等［11］提出了一種基于激光慣性的融合SLAM 算法，實(shí)現(xiàn)了移動(dòng)機(jī)器人全局一致的同步定位與地圖構(gòu)建。針對(duì)激光雷達(dá)與IMU 融合，一些改進(jìn)SLAM 算法，如Fast-LIO［12］與Fast-LIO2［13］被提出。徐偉等［14］通過評(píng)估Fast-LIO2 算法以及其數(shù)據(jù)結(jié)構(gòu)iKDtree，體現(xiàn)Fast-LIO2 算法的優(yōu)越性。Fast-LIO2在Fast-LIO 的基礎(chǔ)上在前端采用增量KD-tree（iKD-tree），后端采用迭代誤差狀態(tài)Kalman Filter，但iKD-tree 是一個(gè)靜態(tài)數(shù)據(jù)結(jié)構(gòu)，當(dāng)需要頻繁進(jìn)行數(shù)據(jù)集更新時(shí)，需要重新構(gòu)建整個(gè)iKDtree，算法運(yùn)行效率會(huì)降低。高翔等［15］在Fast-LIO2 的基礎(chǔ)上進(jìn)行改進(jìn)，用增量體素（ivoxel）取代iKD-tree，實(shí)現(xiàn)了同等的定位和建圖精度，并取得了更快的運(yùn)行速度。馬艾強(qiáng)等［16］提出了一種面向煤礦巷道環(huán)境的激光雷達(dá)與IMU 融合的實(shí)時(shí)定位與建圖方法。徐曉蘇等［17］提出了一種面向室外環(huán)境的基于快速回環(huán)檢測的SLAM 算法，提升了回環(huán)檢測的精度與速度。陳志強(qiáng)等［18］提出了一個(gè)完整的激光雷達(dá)SLAM 框架SCLSLAM，將回環(huán)檢測模塊與掃描鄰近幀集成到緊密耦合的激光雷達(dá)慣性里程計(jì)FAST-LIO2 中。目前，有些優(yōu)秀算法缺少后端優(yōu)化與回環(huán)檢測。如果沒有回環(huán)檢測來修正軌跡估計(jì)中的誤差，SLAM 算法的軌跡會(huì)使機(jī)器人的位姿估計(jì)逐漸偏離真實(shí)軌跡，導(dǎo)致SLAM 系統(tǒng)的性能逐漸下降，甚至無法有效地完成定位和建圖任務(wù)。

激光SLAM 與IMU 在同步定位與構(gòu)圖導(dǎo)航應(yīng)用方面各有優(yōu)勢，有著較為良好的互補(bǔ)性，但同時(shí)定位與建圖的誤差問題始終存在，部分算法缺少后端優(yōu)化與回環(huán)檢測，導(dǎo)致算法存在全局一致性較弱或者累計(jì)誤差過大的問題。本文提出一種改進(jìn)的激光雷達(dá)與IMU 融合SLAM 方法用以提高位姿估計(jì)精度和地圖構(gòu)建精度。

2 激光雷達(dá)IMU 融合的建圖系統(tǒng)分析

2.1 SLAM 模型分析

SLAM 數(shù)學(xué)模型可以使用貝葉斯濾波理論來描述［19］。激光雷達(dá)的測量模型為距離-方位角-俯仰角模型（即距離-方位角-俯仰角模型），如式（1）所示：

其中：(x，y，z，α，ω)表示機(jī)器人位姿，(lx，ly，lz)表示地標(biāo)點(diǎn)位置，α表示測量的地標(biāo)點(diǎn)相對(duì)于機(jī)器人的方位角，ω表示激光雷達(dá)測量的地標(biāo)點(diǎn)相對(duì)于機(jī)器人的俯仰角。該測量模型是一種非線性模型，需要通過優(yōu)化算法來估計(jì)機(jī)器人位姿和地標(biāo)點(diǎn)的位置。

IMU 用于測量當(dāng)前時(shí)刻的角速度和加速度值。IMU 加速度計(jì)模型及陀螺儀模型如式（2）表示：

其中：ba和bω表示加速度計(jì)和陀螺儀的偏置向量，表示從物體坐標(biāo)系到IMU 安裝坐標(biāo)系的旋轉(zhuǎn)矩陣，na和nω表示加速度計(jì)和陀螺儀的隨機(jī)噪聲向量。

2.2 Fast-LIO2 算法

Fast-LIO2 以高效緊耦合的迭代卡爾曼濾波器（Extended Kalman Filter， EKF）為基礎(chǔ)，將原始點(diǎn)云配準(zhǔn)在地圖中，利用環(huán)境中的細(xì)微特征提高數(shù)據(jù)的準(zhǔn)確性，通過增量數(shù)據(jù)結(jié)構(gòu)iKDtree 來維護(hù)地圖。對(duì)于狀態(tài)轉(zhuǎn)移模型而言，以第一幀IMU 幀作為系統(tǒng)初始化輸入，其初始化全局坐標(biāo)系為G，并且以TL=(RL，PL)代表激光雷達(dá)和IMU 之間的外參，其運(yùn)動(dòng)學(xué)方程如公式（3）所示：

其中：PI和RI分別表示IMU 在全局坐標(biāo)系下的位置和姿態(tài)，na和nω分別表示測量值am和ωm中的噪聲；nba和ba表示am中的零偏，bω和nbw表示ωm的零偏，其離散方程為：

其中：x表示狀態(tài)，u表示輸入，w表示噪聲。利用反向傳播修正掃描幀的運(yùn)動(dòng)，見式（5）所示：

其中：k表示激光雷達(dá)掃描幀中的索引；pj表示第k個(gè)掃描幀在激光雷達(dá)局部坐標(biāo)系以及掃描幀結(jié)束時(shí)間的采樣點(diǎn)，每個(gè)測量點(diǎn)pj都包含了nj的距離和方向；剔除噪聲后即是激光雷達(dá)局部坐標(biāo)系中真正的點(diǎn)；TLk表示相應(yīng)的激光雷達(dá)姿態(tài)，TL表示外參；表示對(duì)應(yīng)平面的法向量；qj表示該平面的一個(gè)點(diǎn)。

基于上述的狀態(tài)轉(zhuǎn)移模型和觀測模型，使用迭代卡爾曼濾波。當(dāng)IMU 數(shù)據(jù)到來時(shí)則前向傳播，過程噪聲wi=0，前向傳播狀態(tài)和協(xié)方差如下：

3 改進(jìn)的激光雷達(dá)IMU 融合建圖算法

3.1 Fast-LIO2 算法的后端優(yōu)化

對(duì)Fast-LIO2 算法引入GTSAM 后端圖優(yōu)化，增強(qiáng)激光雷達(dá)與IMU 的融合精度以提高SLAM 構(gòu)圖的全局一致性。GTSAM 為解決SLAM 中出現(xiàn)的非線性優(yōu)化問題提供了一個(gè)框架，其優(yōu)化過程為調(diào)用因子圖以及相應(yīng)圖節(jié)點(diǎn)的值，計(jì)算因子（觀測約束）的殘差，因子對(duì)相關(guān)圖節(jié)點(diǎn)的雅可比矩陣，完成迭代優(yōu)化，以提高自動(dòng)駕駛車輛的定位精度和穩(wěn)定性。改進(jìn)算法的因子圖結(jié)構(gòu)如圖1 所示。

在GTSAM 中，IMU 預(yù)積分因子是一種重要的優(yōu)化因子，車輛在t+Δt時(shí)刻的速度vt+Δt、位置pt+Δt和旋轉(zhuǎn)Rt+Δt的計(jì)算公式如下：

IMU 預(yù)積分提供IMU 預(yù)積分因子，建立IMU 預(yù)積分因子首先建立IMU 模型，如式（11）所示：

其中：ωb表示IMU 坐標(biāo)系中角速度的真實(shí)值，aw表示加速度真實(shí)值，bg，ba表示IMU 偏置，ng，na表示測量值的噪聲，p表示位置，v表示速度，q表示姿態(tài)。根據(jù)積分關(guān)系，得到迭代公式：

根據(jù)i時(shí)刻的位置pwbi，速度以及姿態(tài)qwbi進(jìn)行積分得到j(luò)時(shí)刻以及qwbj。定義預(yù)積分量并且移項(xiàng)整理，構(gòu)建殘差并離散化，根據(jù)EKF 進(jìn)行泰勒展開可得：

其中：F，G分別表示方程f(xk-1，uk-1)在x?k-1處噪聲項(xiàng)為0 處的雅可比矩陣。預(yù)積分量的協(xié)方差矩陣可以通過F，G雅可比矩陣進(jìn)行傳播。

激光里程因子是一種基于激光數(shù)據(jù)的因子，對(duì)于特征點(diǎn)云與其對(duì)應(yīng)的邊緣或平面特征之間的距離為：

使用主成分分析（Pricipal Component Analysis， PCA）提取點(diǎn)、線和平面特征。定義幾何特征的局部線性度σ1D、平面度σ2D和曲率σc分別為：

其中：Tt和Tt+k分別表示車輛在時(shí)間步t和t+k時(shí)的位姿變換矩陣，Xi表示地圖上的點(diǎn)。

基于稀疏體素的近鄰結(jié)構(gòu)（incremental Voxels， iVox）與LIO 算法更加匹配［15］，而iVox 中偽希爾伯特空間曲線（Pseudo-Hilbert Curve，PHC）在降低點(diǎn)云配準(zhǔn)耗時(shí)的同時(shí)不影響SLAM的精度表現(xiàn)。因此，通過給iVox 局部地圖添加最近最少使用緩存（Least Recently Used， LRU），以克服遍歷整個(gè)iVox 帶來的時(shí)效問題。通過調(diào)整iVox 局部地圖的容量閾值，并在添加新地圖數(shù)據(jù)時(shí)篩選出使用頻率較小的體素以移除，以降低iVox 遍歷的復(fù)雜度。

3.2 SLAM 算法評(píng)價(jià)指標(biāo)

視覺里程計(jì)（Estimation， Validation， and Optimization， EVO）評(píng)價(jià)工具是眾多學(xué)者廣泛認(rèn)可的SLAM 系統(tǒng)性能評(píng)價(jià)工具［20］，主要評(píng)價(jià)參數(shù)為絕對(duì)位姿誤差（Absolute Pose Error， APE）與相對(duì)位姿誤差（Relative Pose Error， RPE）。APE 可以直觀地反映SLAM 算法精度和軌跡的全局一致性。如下：

其中：V表示位姿對(duì)之間的相對(duì)平移向量，N表示位姿個(gè)數(shù)，Tt，i和Te，i分別表示真實(shí)軌跡和SLAM 系統(tǒng)估計(jì)出的軌跡，Eg，i表示為誤差求解出不包含位姿旋轉(zhuǎn)角的APE 參數(shù)，如下：

其中trans 表示變量平移部分。RPE 反映SLAM算法的局部精度，計(jì)算相隔Δt時(shí)間的兩幀之間位姿之差，代表相應(yīng)軌跡段的相對(duì)誤差，如下：

其中：EAP誤差的均方根誤差（Root Mean Squared Error， RMSE）、誤差均值（EMean）、最大誤差（EMax）以及最小誤差（EMin）用于評(píng)價(jià)預(yù)測值與真實(shí)值之間的差異，分別為：

此外有學(xué)者提出眾多類似于點(diǎn)云在距離真值一定距離處的點(diǎn)的累積百分比或離散程度來評(píng)價(jià)點(diǎn)云建圖性能［4］。

4 實(shí)驗(yàn)及結(jié)果分析

4.1 激光雷達(dá)IMU 融合建圖

實(shí)驗(yàn)平臺(tái)為Apollo D-KIT-lite 自動(dòng)駕駛開發(fā)套件，采用的開發(fā)套件以及建圖算法框架如圖2 所示。實(shí)驗(yàn)場地選取為學(xué)校園區(qū)第一教學(xué)樓外。改進(jìn)算法建圖過程與生成點(diǎn)云地圖的部分細(xì)節(jié)如圖3 所示。改進(jìn)后生成的總體地圖效果優(yōu)異。同一數(shù)據(jù)集分別采用原始算法與改進(jìn)算法進(jìn)行建圖，生成的點(diǎn)云地圖與地圖細(xì)節(jié)對(duì)比如圖4 所示。

圖2 激光雷達(dá)IMU 融合建圖實(shí)驗(yàn)平臺(tái)與系統(tǒng)框架Fig.2 Experimental platform and system framework for mapping based on fusion of lidar and IMU

圖3 改進(jìn)算法的建圖過程與效果Fig.3 Improved algorithm building process and renderings

圖4 Fast-LIO2 改進(jìn)前后的建圖對(duì)比Fig.4 Comparison of rendering before and after improvement of Fast-LIO2

對(duì)比圖4（a）與圖4（b）可以明顯看出，改進(jìn)算法的整體建圖效果優(yōu)于原算法。如圖4（c）所示，原算法點(diǎn)云重疊導(dǎo)致地圖邊緣點(diǎn)云厚度異常，改進(jìn)算法的輪廓纖細(xì)清晰，如圖4（d）所示。在地面點(diǎn)云上，圖4（e）出現(xiàn)點(diǎn)云運(yùn)動(dòng)畸變，圖4（f）地面平整。

EVO 對(duì)SLAM 效果進(jìn)行對(duì)比，如圖5 所示（彩圖見期刊電子版）。數(shù)據(jù)集中的GPS 定位信息為時(shí)間戳經(jīng)緯度高程信息，需要通過解算與對(duì)齊（所有傳感器數(shù)據(jù)通過相應(yīng)的TF 變化到同一坐標(biāo)系）后才能用于地面真值作為后續(xù)SLAM 算法位姿估計(jì)的基準(zhǔn)。圖5（b）、圖5（c）與圖5（d）中虛線為GPS 差分得到的真實(shí)軌跡參考、藍(lán)色為改進(jìn)前Fast-LIO2 算法、綠色與紅色為改進(jìn)后算法（其中綠色未將回環(huán)檢測因子加入圖優(yōu)化、紅色為加入回環(huán)檢測），可以發(fā)現(xiàn)改進(jìn)算法與原算法相比，無論是否增加回環(huán)檢測，改進(jìn)算法的軌跡估計(jì)更接近GPS 提供的軌跡真值。

圖5 SLAM 位姿估計(jì)Fig.5 SLAM pose estimation

4.2 實(shí)驗(yàn)評(píng)價(jià)分析

通過APE 與RPE 對(duì)改進(jìn)前后的融合算法進(jìn)行對(duì)比分析，結(jié)果如圖6 所示。

圖6 SLAM 軌跡估計(jì)的誤差分析Fig.6 Error analysis of SLAM trajectory estimation

對(duì)比圖6（a）和圖6（b），改進(jìn)后算法的位姿估計(jì)誤差變低。圖6（c）和圖6（d）表明，改進(jìn)算法將絕對(duì)誤差的Mean、RMSE、誤差中位數(shù)（midian）以及標(biāo)準(zhǔn)差（std）限定在更小的范圍內(nèi)；而圖6（e）和圖6（f）表明，RPE 誤差所代表的局部誤差在算法改進(jìn)前后沒有明顯區(qū)別。

為了更好地增強(qiáng)園區(qū)的三維建圖效果，通過添加回環(huán)檢測對(duì)點(diǎn)云地圖進(jìn)行回環(huán)修正消除累計(jì)誤差，利用APE 與RPE 對(duì)添加回環(huán)檢測的改進(jìn)后融合算法進(jìn)行分析，結(jié)果如圖7 所示。

對(duì)比圖6 和圖7，在改進(jìn)算法中添加回環(huán)檢測后位姿估計(jì)誤差變低，Mean，RMSE，midian 以及std 范圍變小。根據(jù)APE 誤差進(jìn)行分布對(duì)比分析與誤差箱型圖，得到具體的誤差分布規(guī)律，APE 誤差分布如圖8 所示。

由圖8（a）可知，改進(jìn)算法比原算法的APE誤差分布更為平均，沒有出現(xiàn)極其突出的誤差。由圖8（b）可知，改進(jìn)算法沒有存在異常值并將誤差分布限制在更小的范圍內(nèi)。對(duì)錄制的數(shù)據(jù)集進(jìn)行bag 包的倍速播放再進(jìn)行重復(fù)的誤差分析，如圖9 所示。

圖9 倍速實(shí)驗(yàn)的APE 誤差對(duì)比Fig.9 Comparison of APE error of double speed experiment

圖9 分析得到，改進(jìn)前算法的各項(xiàng)誤差受倍速實(shí)驗(yàn)干擾時(shí)的影響較大，而改進(jìn)后算法的誤差都較為穩(wěn)定。以z軸高度為顏色映射能直觀地看出建圖的高程效果，如圖10 所示。以z軸為分割映射，調(diào)整點(diǎn)云顏色賦值為z軸并將藍(lán)色設(shè)置為點(diǎn)云z軸坐標(biāo)最低于1.5 m 處的點(diǎn)云，總體上隨著高度變高顏色由藍(lán)色變?yōu)榧t色，這樣的操作可以確定藍(lán)色部分為地面及其接近地面的點(diǎn)云，紅色部分為其他高處環(huán)境點(diǎn)云，改進(jìn)后算法點(diǎn)云在水平基準(zhǔn)0 m 存在更多分布（SLAM 開始時(shí)，以起點(diǎn)處為高程為0）。

采用Cloudcompare 對(duì)改進(jìn)前后算法生成的點(diǎn)云地圖進(jìn)行ICP 配準(zhǔn)后，選取基準(zhǔn)面并將截取的基準(zhǔn)面點(diǎn)云分割切片，操作流程如圖11 所示。將點(diǎn)云信息輸出，去除反射強(qiáng)度時(shí)間戳等信息后只保留位置坐標(biāo)信息進(jìn)行分析，擬合線點(diǎn)云離散分布如圖12 所示（彩圖見期刊電子版）。

圖11 擬合線信息提取過程Fig.11 Process of fitting line information extraction

圖12 點(diǎn)云的離散性分布分析Fig. 12 Discrete distribution analysis of point cloud

點(diǎn)云的擬合誤差（位置坐標(biāo)誤差）是指擬合后的點(diǎn)云與實(shí)際點(diǎn)云之間的位置差異。位置坐標(biāo)誤差是一個(gè)相對(duì)值，它表示擬合后的點(diǎn)云與實(shí)際點(diǎn)云之間的差異。根據(jù)圖12 的擬合線點(diǎn)云集合可以看出，大部分改進(jìn)后的擬合線周圍點(diǎn)云（藍(lán)色）比改進(jìn)前算法點(diǎn)云（紅色）在它們各自擬合線附近有更多分布，同時(shí)分布區(qū)間更小更密集，集中在擬合線附近。通過分析誤差也可以得出同樣的結(jié)論，擬合線點(diǎn)云分布的各類誤差與離散程度參數(shù)見表1。

表1 擬合線誤差的分析結(jié)果Tab.1 Analysis result of fitting line error

如表1 所示，改進(jìn)后的算法在各個(gè)擬合線上的點(diǎn)云偏離擬合線的均方差（S（σ）′）和殘差平方和（RSS′）相比于原算法的S（σ）和RSS 上都有改善，同時(shí)在評(píng)價(jià)偏離程度的參數(shù)上，改進(jìn)算法的R2更接近1，表明在建圖細(xì)節(jié)上改進(jìn)算法比原算法的點(diǎn)云地圖更加平整規(guī)律，一致性更好。

選取KITTI 數(shù)據(jù)集00，05，07，09 序號(hào)數(shù)據(jù)集進(jìn)行仿真，所有場景對(duì)比APE 情況如表2 所示。在4 個(gè)KITTI 數(shù)據(jù)集上改進(jìn)算法均優(yōu)于其他算法，縮小了APE 的誤差區(qū)間，改進(jìn)算法的Mean 相較于原Fast-LIO2 算法，在KITTI 數(shù)據(jù)集00，05，07 和09 號(hào)場景下分別降低了12.09%，8.36%，7.50% 和 12.21%，平 均 下 降 了10.04%，改進(jìn)算法的RMSE 相較于原算法在00，05，07 和09 號(hào)場景下分別降低了12.56%，14.56%，8.85% 和 12.19%，平均下降了12.04%。在選取的學(xué)校園區(qū)實(shí)地場景下，改進(jìn)算法的Mean 較原算法降低了11.04%，改進(jìn)算法的RMSE′較原算法降低了17.35%。

表2 算法APE 誤差對(duì)比Tab.2 Comparison of APE error

5 結(jié) 論

本文將激光雷達(dá)與IMU 融合進(jìn)行建圖，通過添加IMU 因子和激光雷達(dá)里程計(jì)因子進(jìn)行后端優(yōu)化，引入ICP 回環(huán)檢測對(duì)點(diǎn)云地圖進(jìn)行回環(huán)修正消除累計(jì)誤差，利用iVox 結(jié)構(gòu)改進(jìn)Fast-LIO2 采用的iKD-tree 結(jié)構(gòu)，通過APE 與RPE 進(jìn)行評(píng)價(jià)分析。選取開源數(shù)據(jù)集與實(shí)地場景進(jìn)行建圖實(shí)驗(yàn)，改進(jìn)算法在KITTI 數(shù)據(jù)集00，05，07 和09 號(hào)場景下的平均誤差分別降低了12.09%，8.36%，7.50% 和12.21%，平均下降了10.04%，均方誤差方面相較于原算法分別 降 低 了 12.56%，14.56%，8.85% 和12.19%，平均下降了12.04%。在選取的學(xué)校園區(qū)實(shí)地場景下，改進(jìn)算法誤差均值較原算法降低了11.04%，均方根誤差較原算法降低了17.35%。