李 洋，程廣偉，樊順濤，郭占正，徐立友

(1.河南科技大學(xué) 車輛與交通工程學(xué)院，河南 洛陽 471003；2.洛陽理工學(xué)院 智能制造學(xué)院，河南 洛陽 471023)

移動(dòng)機(jī)器人在物流、教育、醫(yī)療、服務(wù)、餐飲等行業(yè)的推廣與應(yīng)用，對(duì)即時(shí)定位與地圖構(gòu)建(Simultaneous Localization and Mapping，SLAM)提出了更高的精度要求[1-2]。激光雷達(dá)、深度相機(jī)、毫米波雷達(dá)以及慣性測(cè)量單元(Inertial Measurement Unit，IMU)等是獲取周圍環(huán)境信息和實(shí)現(xiàn)SLAM技術(shù)的主要傳感器[3]。Zhang等采用高頻低精度的里程計(jì)進(jìn)行運(yùn)動(dòng)估計(jì)，利用低頻特征點(diǎn)云進(jìn)行運(yùn)動(dòng)匹配去矯正定位構(gòu)建地圖，建立了3D激光SLAM方案LOAM[4]。Deschaud針對(duì)采集點(diǎn)云數(shù)據(jù)量大小和稀疏性的問題，提出了單幀激光點(diǎn)云匹配模型的IMLS-SLAM方案[5]。Shan等針對(duì)小型無人車輛在多變地形實(shí)時(shí)運(yùn)行采集的數(shù)據(jù)畸變問題，對(duì)地面點(diǎn)進(jìn)行分類和提取，使用點(diǎn)云聚類算法剔除噪聲點(diǎn)，兩步L-M優(yōu)化定位精度，提出了LeGO-LOAM方案[6]。Shan等針對(duì)單一傳感器在大場(chǎng)景同步定位與建圖時(shí)高漂移的問題，通過因子圖耦合雷達(dá)慣性里程計(jì)，提出了LIO-SAM方案[7]，確保了系統(tǒng)的實(shí)時(shí)性，對(duì)復(fù)雜多變和快速旋轉(zhuǎn)等大場(chǎng)景掃描有更好的魯棒性。

綜上所述，單一激光雷達(dá)提取的原始特征點(diǎn)云去求解幀間運(yùn)動(dòng)時(shí)會(huì)產(chǎn)生位姿估計(jì)失準(zhǔn)現(xiàn)象，運(yùn)用多傳感器信息融合提高SLAM精度的研究很有必要。本文擬以激光雷達(dá)點(diǎn)云為主，IMU信息為輔，開展激光雷達(dá)與IMU信息融合的研究與檢驗(yàn)。

1 回環(huán)檢測(cè)插值算法概述

回環(huán)檢測(cè)插值算法框架如圖1所示。IMU數(shù)據(jù)用于狀態(tài)估計(jì)和預(yù)積分，利用IMU狀態(tài)估計(jì)的結(jié)果對(duì)激光雷達(dá)失真點(diǎn)云進(jìn)行運(yùn)動(dòng)補(bǔ)償，更新矯正自身位姿，對(duì)運(yùn)動(dòng)補(bǔ)償后的點(diǎn)云進(jìn)行特征提取與匹配。在激光里程計(jì)中，通過選取關(guān)鍵幀減少系統(tǒng)計(jì)算壓力，采用基于關(guān)鍵幀的建圖方法，保證系統(tǒng)實(shí)時(shí)運(yùn)行。在后端增加回環(huán)約束，降低系統(tǒng)長(zhǎng)期運(yùn)行的累計(jì)誤差。利用預(yù)積分誤差約束、特征點(diǎn)匹配約束和回環(huán)約束構(gòu)建整體代價(jià)函數(shù)，并進(jìn)行非線性優(yōu)化求解，優(yōu)化整體誤差，保持全局一致性，輸出準(zhǔn)確的定位與建圖信息。

2 回環(huán)檢測(cè)插值算法的構(gòu)建

2.1 IMU狀態(tài)估計(jì)和預(yù)積分

IMU用于測(cè)量移動(dòng)機(jī)器人的三軸加速度與角速度信息[8]：

(1)

圖1 回環(huán)檢測(cè)插值算法框架

在一段連續(xù)時(shí)間[i，j]內(nèi)，若要得到j(luò)時(shí)刻狀態(tài)量(PVQ，位置、速度、姿態(tài))，可對(duì)i時(shí)刻的狀態(tài)量進(jìn)行積分。積分結(jié)果作為j時(shí)刻狀態(tài)量估計(jì)的初始值，用于下一時(shí)刻的狀態(tài)估計(jì)。由于IMU更新頻率較快，一段時(shí)間內(nèi)得到的數(shù)據(jù)很多，每次位姿優(yōu)化更新后需要重新積分，通過預(yù)積分的方法減少計(jì)算時(shí)間，將IMU的積分模型轉(zhuǎn)化為預(yù)積分模型[9]：

(2)

式(2)中的積分部分是預(yù)積分量，為i到j(luò)時(shí)刻IMU數(shù)據(jù)直接積分的結(jié)果。利用式(3)表示預(yù)積分量，得到的預(yù)積分量只與IMU的測(cè)量值有關(guān)，是j時(shí)刻相對(duì)于i時(shí)刻的狀態(tài)。

(3)

式中：αbibj、βbibj和γbibj分別為i時(shí)刻到j(luò)時(shí)刻的位置、速度和姿態(tài)預(yù)積分量。

為約束IMU相鄰時(shí)刻之間的運(yùn)動(dòng)，將上一時(shí)刻IMU預(yù)積分量作為下一時(shí)刻測(cè)量值，對(duì)這兩相鄰時(shí)刻做預(yù)積分可得IMU預(yù)積分誤差模型：

(4)

式中：φp、φq和φv分別為IMU預(yù)積分約束的位置、姿態(tài)和速度殘差；φba和φbg分別為加速度和角速度偏差。

構(gòu)建IMU預(yù)積分誤差約束如下：

(5)

2.2 運(yùn)動(dòng)補(bǔ)償

移動(dòng)機(jī)器人運(yùn)動(dòng)過程中受環(huán)境約束，不能保持勻速行駛狀態(tài)，通過激光雷達(dá)獲取周圍環(huán)境的點(diǎn)云信息，常常存在三維點(diǎn)云失真的現(xiàn)象[10]，可利用IMU的狀態(tài)信息對(duì)失真點(diǎn)云進(jìn)行補(bǔ)償。綜合考慮激光雷達(dá)和IMU接收數(shù)據(jù)時(shí)間的不同，進(jìn)行時(shí)間配準(zhǔn)后，使用線性插值的方法進(jìn)行補(bǔ)償，線性插值原理如圖2所示。

圖2 線性插值原理圖

在線性插值過程中，利用式(6)進(jìn)行IMU數(shù)據(jù)的位姿解算，得到兩幀點(diǎn)云之間的位姿轉(zhuǎn)換矩陣，利用式(7)對(duì)失真點(diǎn)云進(jìn)行補(bǔ)償。

(6)

(7)

2.3 特征提取與匹配

提取特征點(diǎn)云的過程中，使用粗糙度c作為提取的評(píng)價(jià)指標(biāo)。遍歷每個(gè)運(yùn)動(dòng)補(bǔ)償后的點(diǎn)云及其周圍點(diǎn)的坐標(biāo)，使用式(8)來計(jì)算點(diǎn)云粗糙度c。c值按從小到大排序，選擇c值最大的點(diǎn)作為邊緣點(diǎn)，該類點(diǎn)云在三維空間發(fā)生尖銳拐角變化；選擇c值最小的點(diǎn)作為平面點(diǎn)，該類點(diǎn)云在三維空間變化平緩。假設(shè)S是激光雷達(dá)在同一掃描中返回的連續(xù)點(diǎn)A的集合，則粗糙度c表達(dá)式為：

(8)

特征點(diǎn)提取完成后，對(duì)特征點(diǎn)建立匹配關(guān)系。邊緣特征點(diǎn)匹配時(shí)，選取當(dāng)前幀中邊緣特征點(diǎn)A，查找上一幀中與點(diǎn)A距離最近的點(diǎn)B，查找與點(diǎn)B同一幀中相鄰掃描線上的點(diǎn)G，點(diǎn)A與直線BG建立匹配關(guān)系如圖3所示。

(a)匹配示意 (b)匹配關(guān)系

根據(jù)點(diǎn)到直線的特征匹配關(guān)系，通過點(diǎn)到直線的特征距離對(duì)匹配過程進(jìn)行約束：

(9)

平面特征點(diǎn)匹配時(shí)，選取當(dāng)前幀中平面特征點(diǎn)A，查找上一幀中與點(diǎn)A距離最近的點(diǎn)B，分別查找與點(diǎn)B同一幀中的同一掃描線和相鄰掃描線上距離最近的點(diǎn)G和點(diǎn)H，點(diǎn)A與平面BGH建立的匹配關(guān)系如圖4所示。

(a)匹配示意 (b)匹配關(guān)系

根據(jù)點(diǎn)到平面的特征匹配關(guān)系，通過點(diǎn)到平面的特征距離對(duì)匹配過程進(jìn)行約束：

(10)

2.4 關(guān)鍵幀選取與建圖

移動(dòng)機(jī)器人實(shí)時(shí)構(gòu)建較大環(huán)境地圖對(duì)系統(tǒng)計(jì)算能力有較高要求，通過采用選取關(guān)鍵幀的策略，降低計(jì)算壓力。選取關(guān)鍵幀過多，不能達(dá)到減少計(jì)算時(shí)間的目的；選取關(guān)鍵幀過于稀疏，會(huì)增大幀間匹配的誤差。針對(duì)以上問題，采用如下關(guān)鍵幀選取方法：① 第一幀點(diǎn)云作為關(guān)鍵幀；② 相鄰兩關(guān)鍵幀平移距離ΔX超過設(shè)定閾值；③ 相鄰兩關(guān)鍵幀時(shí)間間隔Δt超過設(shè)定閾值；④ 相鄰關(guān)鍵幀旋轉(zhuǎn)ΔR超過設(shè)定閾值。選取關(guān)鍵幀的標(biāo)準(zhǔn)方程：

(11)

式中：Δx(a，a+1)，Δy(a，a+1)，Δz(a，a+1)分別為a關(guān)鍵幀的x，y，z軸到a+1關(guān)鍵幀的x，y，z軸的距離。

(12)

2.5 回環(huán)檢測(cè)

回環(huán)檢測(cè)可以減小傳感器長(zhǎng)時(shí)間運(yùn)行對(duì)系統(tǒng)產(chǎn)生的累計(jì)誤差，在系統(tǒng)中增加回環(huán)檢測(cè)以提升系統(tǒng)的全局定位精度?；丨h(huán)場(chǎng)景中，當(dāng)前關(guān)鍵幀和歷史關(guān)鍵幀的相同環(huán)境特征較多，使用ICP算法對(duì)其進(jìn)行匹配并建立位姿約束。判定存在回環(huán)的條件為：① 當(dāng)前關(guān)鍵幀序列與歷史關(guān)鍵幀序列之差大于設(shè)定閾值；② 當(dāng)前關(guān)鍵幀與歷史關(guān)鍵幀之間的軌跡距離ΔL小于設(shè)定閾值；③ 當(dāng)前關(guān)鍵幀與歷史關(guān)鍵幀的時(shí)間間隔ΔtR大于設(shè)定閾值。回環(huán)判定方程如式(13)所示。使用式(14)對(duì)存在回環(huán)的兩關(guān)鍵幀進(jìn)行位姿約束，建立回環(huán)約束模型，更新當(dāng)前運(yùn)動(dòng)位姿并進(jìn)行建圖。

(13)

式中：ΔO為當(dāng)前關(guān)鍵幀序列與歷史關(guān)鍵幀序列之差；Op，a為第a個(gè)關(guān)鍵幀序列；vab為a關(guān)鍵幀到b關(guān)鍵幀的行駛速度。

(14)

2.6 誤差優(yōu)化

為了有效解決大規(guī)模同步定位與建圖，減小因系統(tǒng)誤差對(duì)建圖帶來的影響，構(gòu)建整體代價(jià)函數(shù)：

F=argmin∑‖rX‖2+∑‖rDe+Dp‖2+∑‖rf‖2

(15)

式中：rX為IMU預(yù)積分誤差約束；rDe+Dp為特征點(diǎn)匹配約束；rf為回環(huán)約束。

整體代價(jià)函數(shù)為非線性函數(shù)，使用L-M(Levenberg-Marquard)算法[12]進(jìn)行非線性優(yōu)化并求解，使得優(yōu)化后的誤差結(jié)果最小，輸出的定位保持全局一致性。

3 實(shí)驗(yàn)驗(yàn)證與結(jié)果分析

3.1 實(shí)驗(yàn)流程

(1)典型場(chǎng)景選?。哼x取國(guó)際上自動(dòng)駕駛場(chǎng)景KITTI數(shù)據(jù)集[13]的00、05、09三個(gè)序列，其中序列00和05的場(chǎng)景為道路轉(zhuǎn)換很多、少量動(dòng)態(tài)物體和樹木、大量住宅和停泊汽車；序列09的場(chǎng)景為上下坡、彎道和少量樹木。

(2)算法驗(yàn)證：使用KITTI官方提供的kitti2bag工具將序列00、05、09轉(zhuǎn)換為bag文件，運(yùn)行SLAM算法并跑通轉(zhuǎn)化過的bag文件。

(3)結(jié)果分析：獲取KITTI數(shù)據(jù)集官網(wǎng)提供的GNSS行駛軌跡真值，輸出回環(huán)檢測(cè)插值算法運(yùn)行3個(gè)序列的位姿數(shù)據(jù)，從行駛軌跡對(duì)比和絕對(duì)軌跡誤差對(duì)回環(huán)檢測(cè)插值算法做定量分析。通過可視化工具Rviz對(duì)KITTI數(shù)據(jù)集環(huán)境進(jìn)行三維點(diǎn)云重現(xiàn)，并做定性分析。

3.2 定量實(shí)驗(yàn)結(jié)果與分析

回環(huán)檢測(cè)插值算法和LOAM分別運(yùn)行序列00、05和09得到的運(yùn)動(dòng)軌跡結(jié)果與軌跡真值對(duì)比如圖5所示，GT_00灰色線為KITTI的運(yùn)動(dòng)軌跡真值，OURS藍(lán)色線為回環(huán)檢測(cè)插值算法運(yùn)行KITTI的運(yùn)動(dòng)軌跡，LOAM橙色線為L(zhǎng)OAM運(yùn)行KITTI的運(yùn)動(dòng)軌跡?；丨h(huán)檢測(cè)插值算法運(yùn)行的軌跡更接近軌跡真值。在轉(zhuǎn)換很多的大型場(chǎng)景00和05中，回環(huán)檢測(cè)插值算法表現(xiàn)更加穩(wěn)定，誤差更小。在變化較少的序列09中，回環(huán)檢測(cè)插值算法和LOAM表現(xiàn)整體相當(dāng)，急轉(zhuǎn)彎處本算法略有優(yōu)勢(shì)。

(a)序列00軌跡對(duì)比 (b)序列05軌跡對(duì)比 (c)序列09軌跡對(duì)比

利用絕對(duì)軌跡誤差評(píng)測(cè)不同算法運(yùn)行KITTI數(shù)據(jù)集的全局地圖一致性。采集回環(huán)檢測(cè)插值算法和LOAM運(yùn)行序列00、05、09位姿估計(jì)值，利用evo工具與真值位姿相比，得到不同算法運(yùn)行序列00、05、09的絕對(duì)軌跡誤差，分別如圖6～圖8所示，APE灰色線為絕對(duì)軌跡誤差曲線，mean橙色線為誤差均值，median綠色線為中值誤差，rmse紅色線為均方根誤差，std紫色區(qū)域?yàn)闃?biāo)準(zhǔn)差。統(tǒng)計(jì)了不同算法運(yùn)行不同序列得出的絕對(duì)軌跡誤差各項(xiàng)結(jié)果，如表1所示。

由圖6～圖8和表1得出：序列00中，回環(huán)檢測(cè)插值算法的均方根誤差比LOAM減少了5.08 m，相比下降了23.5%；序列05中，回環(huán)檢測(cè)插值算法的均方根誤差比LOAM減少了5.98 m，相比下降了56.5%；序列09中，回環(huán)檢測(cè)插值算法的均方根誤差比LOAM減少了2.43 m，相比下降了12%。在絕對(duì)軌跡誤差中其他各項(xiàng)統(tǒng)計(jì)均有所下降。由此可見有回環(huán)檢測(cè)的回環(huán)檢測(cè)插值算法比無回環(huán)檢測(cè)的LOAM更有優(yōu)勢(shì)，減少了系統(tǒng)的累計(jì)誤差，對(duì)構(gòu)建全局一致地圖有正向作用。

表1 不同方法運(yùn)行KITTI的絕對(duì)軌跡誤差結(jié)果統(tǒng)計(jì)

(a)LOAM (b)回環(huán)檢測(cè)插值算法

(a)LOAM (b)回環(huán)檢測(cè)插值算法

(a)LOAM (b)回環(huán)檢測(cè)插值算法

3.3 定性實(shí)驗(yàn)結(jié)果與分析

為了評(píng)測(cè)算法還原三維真實(shí)場(chǎng)景，將回環(huán)檢測(cè)插值算法和LOAM運(yùn)行序列00、05、09，利用Rviz對(duì)不同算法構(gòu)建的環(huán)境地圖進(jìn)行三維點(diǎn)云重現(xiàn)。不同算法構(gòu)建序列00、05、09的三維點(diǎn)云地圖分別如圖9、圖10和圖11所示。

圖9和圖10中LOAM構(gòu)建的點(diǎn)云地圖出現(xiàn)了明顯漂移，地圖重影較大，回環(huán)檢測(cè)插值算法很少有重影，十字路口和和轉(zhuǎn)彎處的地圖依舊清晰，構(gòu)建地圖更接近真實(shí)環(huán)境。圖11中道路轉(zhuǎn)彎角度大，LOAM算法構(gòu)建的地圖有明顯殘影，轉(zhuǎn)彎位置的處理較差，與LOAM算法相比，回環(huán)檢測(cè)插值算法在轉(zhuǎn)彎處構(gòu)建地圖更清晰?；丨h(huán)檢測(cè)插值算法增加IMU約束和回環(huán)約束減小了地圖漂移，轉(zhuǎn)彎細(xì)節(jié)處理更好，周圍環(huán)境的三維重現(xiàn)效果更優(yōu)。

(a)LOAM (b)回環(huán)檢測(cè)插值算法

(a)LOAM (b)回環(huán)檢測(cè)插值算法

4 結(jié) 語

針對(duì)僅使用單一激光雷達(dá)提取的原始點(diǎn)云求解幀間運(yùn)動(dòng)變換導(dǎo)致自身位姿估計(jì)不準(zhǔn)和定位精度低的問題，增加IMU預(yù)積分信息，利用線性插值對(duì)失真點(diǎn)云進(jìn)行運(yùn)動(dòng)補(bǔ)償，以矯正自身運(yùn)動(dòng)位姿并提升定位準(zhǔn)確度。在KITTI數(shù)據(jù)集中的實(shí)驗(yàn)結(jié)果表明：與LOAM相比，回環(huán)檢測(cè)插值算法運(yùn)行KITTI的軌跡更接近軌跡真值，定位精度更高。移動(dòng)機(jī)器人在構(gòu)建大型地圖時(shí)，傳感器長(zhǎng)時(shí)間運(yùn)行會(huì)增大系統(tǒng)的累計(jì)誤差，使定位精度下降，增加回環(huán)檢測(cè)環(huán)，對(duì)確存在回環(huán)的場(chǎng)景建立回環(huán)約束，構(gòu)建整體誤差函數(shù)優(yōu)化系統(tǒng)全局誤差。與LOAM相比，回環(huán)檢測(cè)插值算法在數(shù)據(jù)集上運(yùn)行的絕對(duì)軌跡誤差更小，還原周圍環(huán)境真實(shí)度更高，地圖全局一致性更優(yōu)。