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

李帥鑫 李廣云 王 力 楊嘯天

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

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

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

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

1 系統概述

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

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

圖1 系統框架圖Fig.1 The overview of the system

2 LiDAR/IMU 數據融合方法

2.1 基于因子圖優化的狀態估計

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

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

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

由于觀測量已知,在聯合概率分布中將其作為參數而非隨機變量.根據馬爾科夫性質,Pi僅與ti時刻的狀態xi有關,則式(2)可分解為

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

式中r表示觀測模型與實際觀測的殘差,是關于狀態量Xk的函數,Σ為對應的協方差矩陣.后文將對式(4)中各項的具體形式做詳細說明.

2.2 數據預處理

1)點云數據處理

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

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

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

步驟 3.對剔除地面點后的深度圖進行目標分割.當某一激光光束OP與兩激光點連線P Q所構成的夾角較小時,認為兩激光點P和Q位于不同目標上.循環搜索生成如圖2 (f)的分割圖像,并將中點數較少的目標作為野點剔除.

圖2 點云分割示例Fig.2 Example of point cloud segmentation

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

并將θ較大的非地面點標記為邊緣點PE,將θ較小的標記為平面點PF.從PE中選取nE個θ最大的點構成,從PF中選取nF個θ最小且為地面點的點構成.

2)IMU 數據處理

IMU 的觀測模型為:

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

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

圖3 IMU 與LiDAR 的頻率關系Fig.3 Frequencies of IMU and LiDAR

根據式(8)～(10)定義關鍵幀xi與xj間的運動增量:

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

2.3 激光里程計

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

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

2.4 地圖構建

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

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

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

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

2.5 閉合回環

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

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

2.6 因子圖優化

因子圖優化模塊在系統中維護一個全局因子圖,因子圖結構如圖5.圖中右側為系統實際運行時構建的因子圖,左側是對其結構的抽象化說明.假設給定初始狀態X0,對應式(4)中的.地圖構建模塊向因子圖插入配準因子,對應式(4)中的:

圖5 因子圖結構Fig.5 Structure of the factor graph

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

式中?表示 se(3)中位姿變換的逆運算.每當因子圖中插入新的節點,就對整個因子圖優化計算一次,更新當前時刻的位姿估計,實現點云數據和IMU數據的深度融合.公式推導參考文獻[13].

3 實驗與分析

3.1 實驗及設備介紹

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

圖6 數據采集平臺Fig.6 Data collection platform

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

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

3.2 運行效率對比及閉環優化結果分析

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

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

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

圖7 系統運行時間對比Fig.7 Comparison of time cost of two systems

3.3 定位精度分析

在IMU 和LiDAR 數據融合時,對傳感器觀測量的信任度由觀測量的質量決定.因此需要預先標定IMU,統計其阿倫方差量,以確定對角速度和加速度觀測值的信任程度.

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

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

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

表1 累計誤差結果Table 1 Error accumulation result

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

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

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

圖8 閉環優化效果Fig.8 Performance of loop optimization

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

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

4 結論

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

采用實測數據對系統進行全面測試和評估,可以得到以下結論:

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

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

3)在結構特征不明顯的開闊室外環境,LeGOLOAM 等方法無法進行可靠的位姿估計,而Inertial-LOAM 仍能得到準確的定位結果;同時,在快速直角轉彎處,Inertial-LOAM 較依賴點云配準的LeGO-LOAM 具有更為穩定可靠的表現.