LiDAR/IMU緊耦合的實時定位方法

李帥鑫 李廣云 王 力 楊嘯天

載體利用所搭載傳感器的觀測信息,在未知環(huán)境中進(jìn)行實時自主定位并增量式構(gòu)建環(huán)境地圖的能力是無人系統(tǒng)的關(guān)鍵技術(shù)之一[1].激光雷達(dá)(LiDAR)因其分辨率高、抗干擾強(qiáng)、不受光照條件影響等特點(diǎn)被廣泛應(yīng)用于無人系統(tǒng)中,被稱為“無人車的眼睛”.慣性導(dǎo)航技術(shù)(Inertial navigation system,INS)是一種不受外界因素干擾的自主導(dǎo)航手段,可以在衛(wèi)星導(dǎo)航信號失鎖情況下提供載體的位姿信息.因此將二者結(jié)合實現(xiàn)自主定位與地圖構(gòu)建已成為學(xué)術(shù)界與工業(yè)界的共識.然而,對于諸如無人機(jī)、智能機(jī)器人等運(yùn)算資源有限的小型智能系統(tǒng),實現(xiàn)實時的6 自由度(Degree of freedom,DoF)位姿估計并增量式構(gòu)建環(huán)境地圖是十分困難的.一方面,由于定位與地圖構(gòu)建相互依賴.位姿估計可以為地圖構(gòu)建提供必要的先驗信息,而對已構(gòu)建地圖的重復(fù)觀測又可以消除位姿估計的不確定度[1],二者的交織使得實時定位與地圖構(gòu)建(Simultaneous localization and mapping,SLAM)問題具有極高的復(fù)雜度.另一方面,由于數(shù)據(jù)流中包含了數(shù)據(jù)量龐大的點(diǎn)云,以及高頻采樣的慣性測量單元(Inertial measurement unit,IMU)數(shù)據(jù),如何高效且充分地融合點(diǎn)云與IMU 數(shù)據(jù)也是一個難點(diǎn).

點(diǎn)云配準(zhǔn)是利用連續(xù)點(diǎn)云進(jìn)行位姿推估的核心方法,迭代最鄰近點(diǎn)算法(Iterative closest point,ICP)[2?3]是應(yīng)用最為廣泛的點(diǎn)云配準(zhǔn)算法.Surmann 等將2D LiDAR 安裝在編碼器和轉(zhuǎn)臺上獲取三維點(diǎn)云,并采用ICP 算法估計相LiDAR 的運(yùn)動[4].Moosmann 等提出基于3D LiDAR 和ICP 的解決方案,介紹了一種基于勻速運(yùn)動模型的點(diǎn)云扭曲的補(bǔ)償方法并對補(bǔ)償結(jié)果進(jìn)行了分析[5].由于這兩種方法均采用ICP 算法直接對大規(guī)模點(diǎn)云進(jìn)行處理,配準(zhǔn)效率低,實時性差;另外,這種增量式遞推方法缺少對點(diǎn)云配準(zhǔn)誤差累計的優(yōu)化后端.Droeschel 等提出多分辨率格網(wǎng)地圖(Multi-resolution,MRS)的環(huán)境表達(dá)方式,和一種基于點(diǎn)云表面元模型的概率配準(zhǔn)算法[6],并在此基礎(chǔ)上提出了一種分層優(yōu)化的后端圖優(yōu)化策略[7],實現(xiàn)對連續(xù)軌跡和地圖的優(yōu)化.但由于該方法側(cè)重強(qiáng)調(diào)地圖構(gòu)建的一致性,因此將地圖作為變量節(jié)點(diǎn)一并納入后端優(yōu)化中,運(yùn)算量較大,在小型智能系統(tǒng)上難以達(dá)到實時性.Zhang 等提出的激光雷達(dá)里程計與地圖構(gòu)建方法(LiDAR odometry and mapping,LOAM)[8?9]代表該領(lǐng)域的最高水準(zhǔn),該方法在KITTI (Karlsruhe Institute of Technology and Toyota Technological Institute)[10]評測中排名第二1http://www.cvlibs.net/datasets/kitti/eval_odometry.php.LOAM 采用點(diǎn)到邊和點(diǎn)到面的聯(lián)合配準(zhǔn)方法估計LiDAR 的相對運(yùn)動;為保證實時性,在兩線程上分別運(yùn)行高頻的激光里程計模塊和低頻的地圖構(gòu)建模塊,低頻線程接收高頻線程的位姿估計結(jié)果.Shan 等在[11]中指出LOAM 在計算資源有限的條件下效果大大下降,并基于此提出了面向無人車(Unmanned ground vehicle,UGV)的輕量級LOAM 方法(Lightweight and ground-optimized LOAM,LeGOLOAM).該方法增加點(diǎn)云分割處理模塊降低點(diǎn)云規(guī)模,采用兩步法配準(zhǔn)點(diǎn)云,并增加閉環(huán)檢測線程實現(xiàn)對累計誤差的在線修正.但這兩種方法都只是利用IMU 提供的姿態(tài)信息為點(diǎn)云配準(zhǔn)和點(diǎn)云畸變消除提供運(yùn)動估計的先驗信息,在優(yōu)化計算時并未將其作為觀測約束進(jìn)行整體優(yōu)化,數(shù)據(jù)利用不夠充分.此外,僅依靠點(diǎn)云配準(zhǔn)進(jìn)行位姿推估和地圖構(gòu)建在實際應(yīng)用中誤差累計較大,且極易出現(xiàn)配準(zhǔn)錯誤,尤其在較為缺乏結(jié)構(gòu)性特征的室外環(huán)境下或LiDAR 快速旋轉(zhuǎn)時.這對缺乏重定位能力的系統(tǒng)而言是致命的,將導(dǎo)致定位和地圖構(gòu)建的失敗.Google 2016 年發(fā)布的開源SLAM 庫Cartographer[12]采用一種分層優(yōu)化的思路.前端采用無損卡爾曼濾波(Unscented Kalman filter,UKF)算法融合多源數(shù)據(jù)進(jìn)行位姿推估并構(gòu)建子地圖.后端以子地圖為基本單元構(gòu)建優(yōu)化問題,并提出分支界定算法加速子地圖間約束的構(gòu)建.該系統(tǒng)僅在前端進(jìn)行了較為松散的數(shù)據(jù)耦合,而在后端優(yōu)化時仍是僅以點(diǎn)云的匹配為約束,數(shù)據(jù)融合不夠緊密.

多傳感器數(shù)據(jù)緊融合算法整體可分為濾波算法和平滑算法兩類[13?14].濾波算法[15?16]數(shù)據(jù)處理簡單,實時性強(qiáng),在工業(yè)界得到了廣泛應(yīng)用.但由于其本身的遞推性質(zhì),線性化誤差將不斷累積,導(dǎo)致精度下降.平滑算法包括全平滑(Full smoothing)和固定滯延平滑(Fixed-lag smoothing)兩種.全平滑[17]對所有變量整體優(yōu)化,精度高,但在SLAM 問題中隨著軌跡擴(kuò)張變量個數(shù)激增,無法滿足實時性.固定滯延平滑[18?19]是一種濾波和全優(yōu)化的折中方式,它在時間軸上設(shè)置一個隨時間滑動的固定窗口,每次僅優(yōu)化窗口內(nèi)的變量,并邊緣化其余變量.由于優(yōu)化迭代時所有變量都將被重新線性化,因此線性化累計誤差較小,精度得到保證;同時由于窗口大小固定,優(yōu)化變量個數(shù)基本不變,實時性可以滿足.該算法的缺點(diǎn)是變量邊緣化時協(xié)方差矩陣變稠密,這一定程度上增加了計算負(fù)擔(dān),影響了計算效率.Kaess 等用因子圖(Fator graph)表示變量間的關(guān)系,提出一種增量式平滑的全優(yōu)化算法[20?21],代表當(dāng)前最先進(jìn)的優(yōu)化方法.該方法將因子圖保存在貝葉斯樹中[22],當(dāng)有新的因子節(jié)點(diǎn)加入時,識別被影響的變量節(jié)點(diǎn)并僅對它們進(jìn)行優(yōu)化更新,從而維持全優(yōu)化的稀疏特性.目前基于因子圖全優(yōu)化算法的IMU數(shù)據(jù)緊融合研究主要集中在相機(jī)與IMU 的組合中[23?24],而在LiDAR 與IMU 的組合中還鮮有涉及.

綜上所述,針對小型智能系統(tǒng)的實時定位研究具有重要的現(xiàn)實意義,LiDAR/IMU 的緊耦合方案仍需進(jìn)一步探討.本文在LOAM[9]和LeGO-LOAM[11]的基礎(chǔ)上并結(jié)合前期工作[25],提出一種在未知環(huán)境下,基于LiDAR/IMU 緊耦合的實時定位方法 —Inertial-LOAM,通過在預(yù)處理、配準(zhǔn)和后端優(yōu)化等多層次的數(shù)據(jù)融合,實現(xiàn)多源數(shù)據(jù)的緊密耦合,降低軌跡漂移,增強(qiáng)系統(tǒng)在室外開闊環(huán)境和快速轉(zhuǎn)動時的魯棒性.實驗結(jié)果表明本文方法可以滿足小型智能的實時定位需求,定位性能顯著提升.

1 系統(tǒng)概述

系統(tǒng)接收3D LiDAR 的原始點(diǎn)云和IMU 輸出的角速度及加速度數(shù)據(jù),輸出連續(xù)位姿估計和環(huán)境地圖.需要說明的是,LiDAR/IMU 的外參數(shù)需預(yù)先標(biāo)定,在后續(xù)數(shù)據(jù)處理中作為已知量.傳感器需時間對齊,一般可由IMU 向LiDAR 輸出PPS(Pulse per second)脈沖和推薦定位信息(GPRMC),實現(xiàn)同步觸發(fā);也可利用IMU 的高頻輸出特性,對帶有時間戳的數(shù)據(jù)進(jìn)行濾波和內(nèi)插實現(xiàn).

系統(tǒng)整體分為五個部分:1)數(shù)據(jù)預(yù)處理.將原始點(diǎn)云投影為深度圖像,并進(jìn)行快速的地面點(diǎn)及目標(biāo)分割剔除野點(diǎn),并從分割后的點(diǎn)云中提取特征點(diǎn);同時,利用IMU 積分得到的相對運(yùn)動估值對特征點(diǎn)進(jìn)行畸變校正;2)激光里程計.將連續(xù)時刻的特征點(diǎn)配準(zhǔn),估計LiDAR 相對運(yùn)動,并據(jù)此再次校正特征點(diǎn);3)地圖構(gòu)建.將校正后的特征點(diǎn)與局部地圖配準(zhǔn),優(yōu)化位姿估計結(jié)果,并更新局部地圖;4)閉合回環(huán).檢測軌跡是否閉合,并將閉合處的點(diǎn)云配準(zhǔn)結(jié)果作為閉環(huán)約束關(guān)系;5)因子圖優(yōu)化.系統(tǒng)維護(hù)一個全局的因子圖,各模塊向因子圖中插入IMU 預(yù)積分因子、配準(zhǔn)因子和閉環(huán)因子,每當(dāng)插入新的因子節(jié)點(diǎn),優(yōu)化計算一次.為保證實時性,系統(tǒng)在三個線程上并行運(yùn)行,其中預(yù)處理和激光里程計占用主線程,地圖構(gòu)建和閉合回環(huán)各占一個分線程.Inertial-LOAM 系統(tǒng)框架圖如圖1.

圖1 系統(tǒng)框架圖Fig.1 The overview of the system

2 LiDAR/IMU 數(shù)據(jù)融合方法

2.1 基于因子圖優(yōu)化的狀態(tài)估計

載體坐標(biāo)系定義為 B,與IMU 坐標(biāo)系保持一致;世界坐標(biāo)系定義為 W,原點(diǎn)為系統(tǒng)初始化時的載體系中心,z軸方向與世界坐標(biāo)系下的重力方向?qū)R;假設(shè)LiDAR 第i次掃描的起始時刻為ti,掃描得到的全部點(diǎn)云為Pi,其中任意一點(diǎn)記為Bpn∈Pi;IMU 在 [ti,ti+1]內(nèi)采集的數(shù)據(jù)為I(i,i+1),由于IMU輸出頻率高于LiDAR,I(i,i+1)中包含了N組 B 下的加速度和角速度,n∈N.系統(tǒng)在ti時刻的狀態(tài)包括姿態(tài),位置,速度和IMU 零偏

其中,位姿變換構(gòu)成特殊歐氏群[26][Ri,ti]∈SE(3);速度為歐氏空間下的三維向量vi∈R3;零偏項由陀螺儀零偏和加速度計零偏構(gòu)成bi=.需要說明的是,本文側(cè)重于研究系統(tǒng)的定位方法,因此地圖點(diǎn)不作為狀態(tài)量進(jìn)行優(yōu)化,以保證后端優(yōu)化的運(yùn)算效率.此外,所有LiDAR 的狀態(tài)量過于龐大,且部分狀態(tài)量十分冗余(如:LiDAR靜止),若將其全部作為狀態(tài)量進(jìn)行優(yōu)化,一方面很快就會超出計算機(jī)的運(yùn)算能力,另一方面也會造成運(yùn)算資源浪費(fèi).因此,在數(shù)據(jù)處理中僅選取具有代表性的關(guān)鍵幀作為優(yōu)化估計的狀態(tài)量.定義Kk為tk時刻的所有關(guān)鍵幀,其對應(yīng)的狀態(tài)量為;其對應(yīng)的所有觀測量為.

在這些假設(shè)和定義下,狀態(tài)估計問題可描述為:在給定觀測信息Zk和先驗信息p(X0)的條件下,估計X0的后驗概率問題,即

由于觀測量已知,在聯(lián)合概率分布中將其作為參數(shù)而非隨機(jī)變量.根據(jù)馬爾科夫性質(zhì),Pi僅與ti時刻的狀態(tài)xi有關(guān),則式(2)可分解為

變量因子的最大后驗概率(Maximum a posteriori,MAP)可由式(3)推得

式中r表示觀測模型與實際觀測的殘差,是關(guān)于狀態(tài)量Xk的函數(shù),Σ為對應(yīng)的協(xié)方差矩陣.后文將對式(4)中各項的具體形式做詳細(xì)說明.

2.2 數(shù)據(jù)預(yù)處理

1)點(diǎn)云數(shù)據(jù)處理

本文采用前期工作[25]中的運(yùn)動模型估計方法校正點(diǎn)云,即根據(jù)預(yù)積分得到的相對運(yùn)動估計將點(diǎn)云統(tǒng)一至同一時刻載體坐標(biāo)系下.圖2 為KITTI數(shù)據(jù)集[10]的處理實例.點(diǎn)云的特征提取主要分為以下步驟:

步驟 1.將無序的點(diǎn)云P轉(zhuǎn)為有序的深度圖,以提升點(diǎn)云搜索速度.的行數(shù)r為LiDAR 線陣的行數(shù),列數(shù)c根據(jù)深度圖水平角分辨率設(shè)定.中每個像素對應(yīng)一個或幾個點(diǎn)云中的點(diǎn),像素值取最遠(yuǎn)點(diǎn)的深度值.如圖2 (c),深度圖大小為 64×860,每個像素約包含5 個點(diǎn).

步驟 2.不同于LeGO-LOAM[11]中的地面點(diǎn)分割方法,本文采用基于角度圖像的分割策略.首先根據(jù)垂直方向和水平方向的坐標(biāo)差 Δx和 Δz計算深度圖上同一列相鄰行的兩點(diǎn)A和B間的垂直夾角α(如圖2 (d)),構(gòu)成夾角圖像,并對其采用Savitsky-Golay 濾波算法平滑處理得到如圖2 (e)所示的平滑后夾角圖像.對于車載點(diǎn)云,一般可以認(rèn)為滿足α接近于0 的像素是地面點(diǎn)所對應(yīng)的區(qū)域;采用廣度優(yōu)先算法(Breadth-first search,BFS)算法在上搜索小于閾值的點(diǎn),將其標(biāo)記為地面點(diǎn).

步驟 3.對剔除地面點(diǎn)后的深度圖進(jìn)行目標(biāo)分割.當(dāng)某一激光光束OP與兩激光點(diǎn)連線P Q所構(gòu)成的夾角較小時,認(rèn)為兩激光點(diǎn)P和Q位于不同目標(biāo)上.循環(huán)搜索生成如圖2 (f)的分割圖像,并將中點(diǎn)數(shù)較少的目標(biāo)作為野點(diǎn)剔除.

圖2 點(diǎn)云分割示例Fig.2 Example of point cloud segmentation

步驟 4.將地面和目標(biāo)分割后的部分作為特征圖,并在其上進(jìn)行特征提取,提取方法與文獻(xiàn)[8]中相同.根據(jù)各點(diǎn)深度值di計算某一點(diǎn)在其同行鄰域點(diǎn)S中的粗糙度θ:

并將θ較大的非地面點(diǎn)標(biāo)記為邊緣點(diǎn)PE,將θ較小的標(biāo)記為平面點(diǎn)PF.從PE中選取nE個θ最大的點(diǎn)構(gòu)成,從PF中選取nF個θ最小且為地面點(diǎn)的點(diǎn)構(gòu)成.

2)IMU 數(shù)據(jù)處理

IMU 的觀測模型為:

其中,η～N(0,Σ)表示觀測噪聲,為 B 到W的旋轉(zhuǎn)矩陣,Wg為重力加速度.根據(jù)IMU 的動力學(xué)模型,采用離散化積分方法對在IMU 采樣間隔時間 Δt內(nèi)積分:

式中‘∧’符號表示將三維向量映射為反對稱矩陣,由于IMU 輸出頻率很高,狀態(tài)估計時若直接將IMU 采樣時刻對應(yīng)的全部位姿作為變量節(jié)點(diǎn)插入因子圖進(jìn)行優(yōu)化,無疑是不現(xiàn)實的[21].通常通過預(yù)積分處理,將高頻輸出的加速度和角速度觀測量轉(zhuǎn)化為狀態(tài)量間的位姿變換,構(gòu)成關(guān)鍵幀狀態(tài)量之間的約束因子[23],從而將所有IMU 觀測量轉(zhuǎn)化為一個預(yù)積分觀測量,傳感器采樣頻率與IMU 因子關(guān)系如圖3.

圖3 IMU 與LiDAR 的頻率關(guān)系Fig.3 Frequencies of IMU and LiDAR

根據(jù)式(8)～(10)定義關(guān)鍵幀xi與xj間的運(yùn)動增量:

為方便書寫保證公式簡潔,上式省略了左下標(biāo)的坐標(biāo)系標(biāo)注,并將 (·)(ti)簡寫為(·)i;其中Δtij=.假設(shè)關(guān)鍵幀xi與xj間的IMU 零偏保持不變,則可由式(11)～(13)可得預(yù)積分觀測模型[13]:

2.3 激光里程計

因此,將式(19)分別代入式(17)和式(18),得:

令d最小,利用Levenberg-Maquardt 算法即可求解旋轉(zhuǎn)量和平移量的估值.

2.4 地圖構(gòu)建

LOAM 和LeGO-LOAM 中并未定義局部地圖,只是根據(jù)垂直角可視范圍,以傳感器為中心搜索與索引特征點(diǎn)可能存在匹配的地圖點(diǎn).這種粗略的搜索方法過于依賴閾值:搜索得到的地圖點(diǎn)過多,則配準(zhǔn)效率低下;若地圖點(diǎn)過少,則可能造成地圖點(diǎn)與特征點(diǎn)對周圍環(huán)境表達(dá)的不一致,部分可觀測的地圖點(diǎn)缺失,導(dǎo)致配準(zhǔn)精度下降.

與文獻(xiàn)[27?28]類似,本文提出采用以傳感器為中心的多尺度地圖模型建立局部地圖Mk,并定義Mk的參考系與當(dāng)前關(guān)鍵幀一致mk=xi,地圖構(gòu)建模塊每運(yùn)行一次,便插入一個關(guān)鍵幀.局部地圖共分三層,各層?xùn)鸥翊笮〔灰?相互嵌套.柵格中的點(diǎn)存儲于環(huán)形容器中,保持局部地圖中點(diǎn)個數(shù)不變,從而保持地圖配準(zhǔn)模塊的處理速度相對恒定.點(diǎn)坐標(biāo)保存為點(diǎn)在柵格內(nèi)的相對位置,以便于局部地圖坐標(biāo)系變化時點(diǎn)坐標(biāo)更新.局部地圖的示意圖如圖4,大小不一的三種柵格分別表示不同分辨率的格網(wǎng)地圖;柵格中的點(diǎn)表示該格網(wǎng)中存儲的點(diǎn)云;當(dāng)載體運(yùn)動時局部地圖隨之移動,局部地圖的中心由mk?1的原點(diǎn)Oold向mk的原點(diǎn)Onew移動,局部地圖向前進(jìn)方向添加?xùn)鸥?并剔除遠(yuǎn)離方向的柵格.

圖4 局部地圖示意圖Fig.4 Demonstration for the local map

這種局部地圖表示方法有兩個特點(diǎn):1)由于局部地圖始終保持以載體為中心,使得地圖中的點(diǎn)大部分位于LiDAR 的可視范圍內(nèi),避免了對不可視地圖點(diǎn)的搜索.2)多分辨率格網(wǎng)地圖更符合LiDAR 放射狀采樣的性質(zhì),近處點(diǎn)云密集,而遠(yuǎn)處相對稀疏.隨著移動LiDAR 的移動高分辨率柵格內(nèi)的點(diǎn)逐漸增加并穩(wěn)定,能夠更準(zhǔn)確地刻畫環(huán)境的真實面貌,避免地圖點(diǎn)與特征點(diǎn)不一致性的產(chǎn)生.這也是與其他格網(wǎng)地圖表達(dá)點(diǎn)云方法[27?28]的最主要區(qū)別.

2.5 閉合回環(huán)

閉合回環(huán)模塊檢測載體的軌跡是否形成閉合,即是檢測載體是否回到之前的位置.若構(gòu)成閉環(huán)則利用GICP 算法將當(dāng)前的特征點(diǎn)云與閉環(huán)處的特征點(diǎn)云配準(zhǔn),得到相對位姿關(guān)系,構(gòu)成閉環(huán)約束因子,并將其插入因子圖中.閉環(huán)檢測根據(jù)當(dāng)前關(guān)鍵幀位置與其余關(guān)鍵幀間的距離判斷:將關(guān)鍵幀列表保存于KD 樹中,以半徑R搜索當(dāng)前關(guān)鍵幀的相鄰關(guān)鍵幀,并根據(jù)采樣時間判斷是否為相鄰時刻的關(guān)鍵幀.

本文提出采用大閉環(huán)與小閉環(huán)結(jié)合的方式,大閉環(huán)表示機(jī)器人回到之前的位置,可以建立閉環(huán)約束修正全局誤差累計;而小閉環(huán)則表示間隔關(guān)鍵幀之間的共視關(guān)系,可以為轉(zhuǎn)角位置提供更多約束,從而提升激光里程計對LiDAR 旋轉(zhuǎn)運(yùn)動的魯棒性.

2.6 因子圖優(yōu)化

因子圖優(yōu)化模塊在系統(tǒng)中維護(hù)一個全局因子圖,因子圖結(jié)構(gòu)如圖5.圖中右側(cè)為系統(tǒng)實際運(yùn)行時構(gòu)建的因子圖,左側(cè)是對其結(jié)構(gòu)的抽象化說明.假設(shè)給定初始狀態(tài)X0,對應(yīng)式(4)中的.地圖構(gòu)建模塊向因子圖插入配準(zhǔn)因子,對應(yīng)式(4)中的:

圖5 因子圖結(jié)構(gòu)Fig.5 Structure of the factor graph

數(shù)據(jù)預(yù)處理模塊計算相鄰關(guān)鍵幀之間的IMU 預(yù)積分,并向因子圖插入IMU 預(yù)積分因子,對應(yīng)式(4)中的:

式中?表示 se(3)中位姿變換的逆運(yùn)算.每當(dāng)因子圖中插入新的節(jié)點(diǎn),就對整個因子圖優(yōu)化計算一次,更新當(dāng)前時刻的位姿估計,實現(xiàn)點(diǎn)云數(shù)據(jù)和IMU數(shù)據(jù)的深度融合.公式推導(dǎo)參考文獻(xiàn)[13].

3 實驗與分析

3.1 實驗及設(shè)備介紹

為客觀全面地評估本文所提方法的性能,共進(jìn)行兩組實驗:1)采用文獻(xiàn)[11]公開的兩組數(shù)據(jù)集1#和3#對系統(tǒng)運(yùn)行速度、閉環(huán)優(yōu)化效果進(jìn)行評價和對比;2)分別采用文獻(xiàn)[11]公開的數(shù)據(jù)集2#和實測數(shù)據(jù),根據(jù)回到起始點(diǎn)的位姿偏差定量評估系統(tǒng)定位精度.數(shù)據(jù)采集平臺為搭載了Xsens MTi-G-710 IMU 和Velodyne VLP-16 LiDAR 的地面移動機(jī)器人(如圖6),數(shù)據(jù)采集頻率分別為200 Hz 和10 Hz,兩傳感器采用軟同步方式進(jìn)行時間對齊.數(shù)據(jù)采集環(huán)境為室內(nèi)地下停車場和室外開闊廣場(如圖10 (c)和10 (d)),數(shù)據(jù)采集時間約為1 000 s,平臺運(yùn)動速度約為1 m/s,總距離約為1 km.系統(tǒng)采用C++實現(xiàn),在機(jī)器人操作系統(tǒng)(Robot operation system,ROS)中運(yùn)行.小型智能地面機(jī)器人的數(shù)據(jù)處理器為4 GB 內(nèi)存并搭載Intel i5 1.8 GHz CPU 的終端(如圖6).

圖6 數(shù)據(jù)采集平臺Fig.6 Data collection platform

在數(shù)據(jù)融合前,需要對系統(tǒng)進(jìn)行初始化,其主要目的是:1)獲得IMU 與LiDAR 之間的相對位姿,即傳感器外參數(shù);2)計算載體系 B 下的重力加速度Bg,從而得到載體坐標(biāo)系 B 到世界坐標(biāo)系W的轉(zhuǎn)換關(guān)系.結(jié)合本文對坐標(biāo)系統(tǒng)的定義,在本實驗中,Bg采用文獻(xiàn)[23]中所述方法計算得到,而后將其旋轉(zhuǎn)至 W 系下得到旋轉(zhuǎn)關(guān)系;為保證精度,LiDAR/IMU 外參數(shù)的標(biāo)定在結(jié)構(gòu)性較好且設(shè)置了特殊靶標(biāo)的實驗室預(yù)先進(jìn)行,數(shù)據(jù)處理中作為已知量.基本原理是分別采用激光雷達(dá)里程計和IMU 預(yù)積分得到一段時間內(nèi)相鄰狀態(tài)間的約束,從而構(gòu)建關(guān)于的優(yōu)化方程:

用特殊氏式群[26][Ri,ti]∈SE(3)表達(dá)位姿變換,即可在流形上利用L-M 算法求解外參的估值.一般而言,傳感器外參數(shù)在數(shù)據(jù)采集過程中保持不變,因此在后續(xù)數(shù)據(jù)處理中可將其作為固定量.

3.2 運(yùn)行效率對比及閉環(huán)優(yōu)化結(jié)果分析

分別統(tǒng)計LeGO-LOAM 和本文Inertial-LOAM對1# 和3# 兩組數(shù)據(jù)集的單幀數(shù)據(jù)處理時間,包括數(shù)據(jù)預(yù)處理,激光里程計,地圖構(gòu)建,優(yōu)化計算等各模塊及系統(tǒng)總體的單幀處理時長(如圖7),比較并分析兩系統(tǒng)數(shù)據(jù)處理的速度和效率.

從圖中可以看出:兩系統(tǒng)總處理時間的95 %以上都小于LiDAR 的采樣間隔0.1 s,說明系統(tǒng)滿足實時處理的要求;由于Inertial-LOAM 采用多分辨率局部地圖,地圖構(gòu)建模塊避免了對全局地圖點(diǎn)的搜索,因此平均處理速度更快.而在數(shù)據(jù)預(yù)處理時,由于增加了IMU 數(shù)據(jù)的預(yù)積分計算,該模塊處理時間略有增加;另外,Inertial-LOAM 的優(yōu)化計算時間并未明顯增加,說明融合IMU 數(shù)據(jù)后并未過多增加系統(tǒng)的計算負(fù)擔(dān).

根據(jù)閉環(huán)優(yōu)化前后的地圖偏差定性評價閉環(huán)優(yōu)化對系統(tǒng)定位和建圖精度的提升作用,1# 和3#兩組數(shù)據(jù)閉環(huán)優(yōu)化前后所構(gòu)建的地圖及軌跡如圖7.從圖7 (a)可以明顯看出,閉環(huán)優(yōu)化可以有效處理大閉環(huán)造成的累計誤差,對軌跡精度和地圖一致性的提升具有重要作用;圖7(b)的激光里程計誤差累計不明顯,因此閉環(huán)優(yōu)化作用效果不甚顯著.采用Inertial-LOAM 方法和文獻(xiàn)[11]中1# 和3# 數(shù)據(jù),構(gòu)建的地圖和軌跡的結(jié)果如圖10 (a)和10 (b).

圖7 系統(tǒng)運(yùn)行時間對比Fig.7 Comparison of time cost of two systems

3.3 定位精度分析

在IMU 和LiDAR 數(shù)據(jù)融合時,對傳感器觀測量的信任度由觀測量的質(zhì)量決定.因此需要預(yù)先標(biāo)定IMU,統(tǒng)計其阿倫方差量,以確定對角速度和加速度觀測值的信任程度.

實驗時,在起點(diǎn)附近架設(shè)高精度全站儀,并在機(jī)器人上粘貼標(biāo)志點(diǎn).每當(dāng)測量平臺回到起點(diǎn)附近時,用高精度全站儀觀測標(biāo)志點(diǎn),并以其位姿變換作為相對變換的參考值GT.估計的位姿Wx的精度由累計誤差的均方根(Root mean square error,RMSE)表示:

其中,?表示位姿變換的逆運(yùn)算.采用起終點(diǎn)相同的2# 數(shù)據(jù),分別對Cartographer[12],LeGO-LOAM[11],LOAM[8],IMU 和本文所提出的Inertial-LOAM(以下分別簡稱為C,LL,L,I,和IL)進(jìn)行精度對比,并說明IMU 在數(shù)據(jù)融合中的作用;采用實測的室內(nèi)停車場、室外開闊廣場數(shù)據(jù)對比IL,LL,L 和C 的精度,分析本文方法的性能和優(yōu)勢.誤差累計結(jié)果如表1.需要說明的是,為客觀評價系統(tǒng)漂移程度,實驗結(jié)果均未進(jìn)行閉環(huán)優(yōu)化,LL,L 和C 均為加入IMU 觀測后的結(jié)果,誤差累計均為各系統(tǒng)處理5 次的平均值.

2# 數(shù)據(jù)的采集環(huán)境與3#相同,特征豐富但機(jī)器人運(yùn)動較快.由表1 可以看出,僅利用IMU 積分的位姿推估誤差累計嚴(yán)重,無法得到正確的結(jié)果.因此,在后續(xù)實驗中不再將其作為對比對象.盡管IMU 位置漂移嚴(yán)重,但姿態(tài)偏差相對較小.說明IMU 能提供一個較好的姿態(tài)約束,而這正是LiDAR 里程計所缺少的,間接反映了激光里程計與IMU 耦合的優(yōu)勢.

表1 累計誤差結(jié)果Table 1 Error accumulation result

在室外特征豐富但運(yùn)動較快的情況下,L、LL和IL 均能得到分米級的定位結(jié)果而C 精度較差.LL 和IL 在數(shù)據(jù)預(yù)處理階段對特征點(diǎn)云進(jìn)行了目標(biāo)分割,剔除了大量主要特征以外的野點(diǎn),避免其對配準(zhǔn)精度的影響,因此較L 的精度有所提升,其中IL 最優(yōu).由于C 將三維點(diǎn)云投影至二維格網(wǎng)中,它對類似墻面等結(jié)構(gòu)性較強(qiáng)的環(huán)境有較好的適應(yīng)性,而室外諸如樹木、車輛等特征不能保證較好的配準(zhǔn)精度.因此,即便在前端融合了IMU 數(shù)據(jù),最終仍產(chǎn)生較大誤差.

在相對狹小的室內(nèi)環(huán)境中,C、LL 與IL 的位姿估計精度基本都在0.05 m 左右,其中C 方法最優(yōu),較L 提升近一倍,四種方法的定位精度均能達(dá)到厘米量級.由于室內(nèi)相對狹小,路面平坦,且包含大量的墻面及邊沿線等明顯的結(jié)構(gòu)特征,點(diǎn)云配準(zhǔn)精度高,在IMU/LiDAR 數(shù)據(jù)融合解算時占據(jù)更大權(quán)重,因此IMU 對定位結(jié)果的作用較小.室內(nèi)環(huán)境中IL 地圖和軌跡結(jié)果如圖10 (c).

在室外開闊且運(yùn)動相對平緩的條件下,LL 隨運(yùn)行時間增加,誤差累計愈發(fā)嚴(yán)重,無法得到可靠的位姿估計結(jié)果;L 和C 性能優(yōu)于LL,但仍存在一定誤差累計.在不采用閉環(huán)優(yōu)化的前提下,IL 通過融合IMU 信息,顯著降低定位漂移,精度仍能達(dá)到分米量級.四者軌跡和地圖結(jié)果對比如圖9.室外空間較為開闊且結(jié)構(gòu)性特征較少,路面不夠平整造成移動機(jī)器人運(yùn)動時產(chǎn)生劇烈的震動,因此點(diǎn)云包含較多噪點(diǎn).由于未在后端優(yōu)化中融入IMU 觀測約束,且LL 為了適應(yīng)輕量級終端,提取的特征點(diǎn)云較為稀疏,導(dǎo)致配準(zhǔn)誤差較大,出現(xiàn)明顯的誤差累計;盡管L 的運(yùn)行效率不及LL,但相對稠密的點(diǎn)云保證了其配準(zhǔn)的精度,但軌跡仍不可避免的存在漂移;雖然C 在前端融合了IMU 數(shù)據(jù),但本質(zhì)上只是利用融合的數(shù)據(jù)構(gòu)建更為精確的局部地圖,以此保證更準(zhǔn)確的前端匹配,導(dǎo)致數(shù)據(jù)融合的作用在后端優(yōu)化中體現(xiàn)不明顯,不足以彌補(bǔ)連續(xù)配準(zhǔn)引起的軌跡發(fā)散問題.本文提出的IL 通過充分融合LiDAR 和IMU 的觀測信息對軌跡進(jìn)行整體優(yōu)化,有效降低了軌跡漂移,提升了位姿估計的精度.從圖9 的對比中還可以看出,融合IMU 信息后系統(tǒng)在快速直角轉(zhuǎn)彎處的表現(xiàn)顯著提升,反映了IMU 良好的姿態(tài)約束的作用.采用室外數(shù)據(jù)和IL 方法所構(gòu)建的地圖和軌跡結(jié)果如圖10 (d).

圖8 閉環(huán)優(yōu)化效果Fig.8 Performance of loop optimization

圖9 室外開闊環(huán)境IL/LL/L/C 軌跡結(jié)果對比Fig.9 Comparison of pose estimation of IL/LL/L/C in outdoor environment

圖10 Inertial-LOAM 軌跡及建圖結(jié)果Fig.10 Trajectory and mapping result of Inertial-LOAM

4 結(jié)論

本文面向小型智能平臺,針對現(xiàn)有的LiDAR/IMU SLAM 方法數(shù)據(jù)融合不充分,對開闊環(huán)境和快速旋轉(zhuǎn)魯棒性差,地圖構(gòu)建缺乏一致性的問題,提出了一種LiDAR/IMU 緊耦合的實時定位方法— Inertial-LOAM.首先在數(shù)據(jù)預(yù)處理階段采用地面分割效果更好的基于夾角圖像的地面點(diǎn)云分割方法,并對IMU 數(shù)據(jù)進(jìn)行預(yù)積分處理.其次,在地圖構(gòu)建階段定義了一個以傳感器為中心的MRS 局部地圖,提升地圖匹配的速度和精度.最后,在系統(tǒng)中增加因子圖優(yōu)化模塊,對LiDAR 和IMU 觀測數(shù)據(jù)進(jìn)行整體優(yōu)化,實現(xiàn)多源觀測值的緊密耦合.

采用實測數(shù)據(jù)對系統(tǒng)進(jìn)行全面測試和評估,可以得到以下結(jié)論:

1)IMU 預(yù)積分計算略微增加了數(shù)據(jù)預(yù)處理時間,同時IMU 預(yù)積分因子的加入使因子圖結(jié)構(gòu)更為復(fù)雜,但整體而言對系統(tǒng)的運(yùn)算負(fù)擔(dān)增加不大,可以滿足實時性要求;另外由于定義了MRS 局部地圖,避免了對全局地圖的搜索,地圖構(gòu)建模塊速度更快.

2)閉合回環(huán)對系統(tǒng)精度的提升具有重要作用.通過對整體誤差的修正,能夠有效降低誤差累計對定位結(jié)果帶來的影響,提升構(gòu)建地圖的一致性.

3)在結(jié)構(gòu)特征不明顯的開闊室外環(huán)境,LeGOLOAM 等方法無法進(jìn)行可靠的位姿估計,而Inertial-LOAM 仍能得到準(zhǔn)確的定位結(jié)果;同時,在快速直角轉(zhuǎn)彎處,Inertial-LOAM 較依賴點(diǎn)云配準(zhǔn)的LeGO-LOAM 具有更為穩(wěn)定可靠的表現(xiàn).