基于NDT 配準與輪式里程計的激光雷達運動畸變補償算法①

陳強 陳海波 張瀝化

(浙江工業大學信息工程學院 杭州310023)

同時定位與地圖構建(simultaneous localization and mapping,SLAM)目前普遍運用于移動機器人領域,其被認為是實現避障、路徑規劃、導航等應用的核心環節[1-3]。激光雷達因能提供精準可靠的測距信息,且不易受光照環境影響,所以被廣泛應用于SLAM 測距[4-5]。目前激光雷達一般是旋轉結構,導致其會逐步掃描周圍空間。因此,激光雷達在掃描期間的任何移動都會導致運動畸變現象[6],影響SLAM算法輸出的機器人位姿精度與構建地圖的質量。

激光雷達的掃描配準是激光里程計算法的基礎。當激光雷達快速運動時,運動畸變的點云成為影響激光里程計精度的主要因素。研究者們通過對雷達的運動估計從而處理運動畸變現象。迭代最近點(iterated closest point,ICP)[7]用于估計點云之間的剛性變換,并未考慮掃描中的運動畸變現象。Hong 等人[8]在ICP 算法的基礎上引入雷達速度的估計,實現對雷達運動畸變補償。如文獻[9]和[10]所示,恒速模型是解決運動畸變的方法之一。此外,Bosse 和Zlot[10]提出了一種基于控制點的連續狀態表示,并在控制點之間進行位姿插值,從而恢復運動估計。Furgale 等人[11]引入時間基函數的線性組合對運動軌跡建立連續狀態模型。與傳統離散模型相比,該框架能提供更準確的估計。Anderson 和Barfoot[12]介紹了另一種連續時間軌跡狀態估計方法,其關鍵思想是在離散最大后驗估計上使用計算簡便的高斯過程回歸,實現查詢時間點的狀態。

以上算法處理運動畸變的效果取決于假設模型的準確性,而運動模型的假設并不能代表系統的真實運動,因此在實際應用中比較困難。不同于傳統純估計運動方案,當前基于傳感器的外部運動估計也是處理運動畸變的重要發展方向。Kuramachi 等人[13]提出激光雷達與慣性傳感器相結合的方法,添加三自由度慣性信息從而實現點云校正和位置預測,對旋轉運動具有較好的魯棒性。Geneva 等人[14]給出了一種基于新平面表示的方法,該算法將激光雷達的原始數據與慣性測量單元(inertial measurement unit,IMU)的運動信息融合,從而估計系統軌跡。Levinson 等人[15]引入輪式里程計輔助的方法,借助輪式里程計提供的位置信息獲得激光雷達在掃描周期內的位姿,實現校正點云和雷達校準。Gentil等人[16]介紹了一種三維激光雷達和IMU 外部校準的算法,利用IMU 連續表示的預積分來描述系統在任何時間點的運動。該方法在不依賴任何顯示運動模型的條件下補償運動畸變。

綜上所述,基于傳感器輔助的解決方案取得了一定的研究成果。IMU 傳感器具有較高的角速度測量精度,但線加速度精度以及二次積分在局部的精度較低,而輪式里程計具有較高的局部位姿測量精度。在輪式里程計輔助方法的基礎上,本文開展進一步研究并提出基于正態分布變換(normal distributions transform,NDT)配準與輪式里程計的激光雷達運動畸變補償算法。本文算法通過輪式里程計外部運動估計獲得低準確度的雷達運動狀態,并設計基于NDT 點云配準算法的輪式里程計誤差處理方法,優化運動畸變補償。實驗結果表明,本文算法可以實現對激光雷達運動畸變的精準補償,有效提升SLAM 系統的準確性。

1 運動畸變與符號定義

1.1 運動畸變描述

激光雷達是通過電機驅動測距核心進行旋轉,從而完成對周圍環境的掃描測距。受限于電機的旋轉速率,激光雷達掃描頻率是恒定的。因此一幀掃描內的各個激光束并不是同時完成測量,而是隨著電機旋轉,按照時間順序依次測量。圖1 是激光雷達運動畸變示意圖。當裝載激光雷達的機器人處于靜止狀態,雷達獲取的數據并不會產生誤差。但在SLAM 系統中機器人通常處于運動狀態,且激光雷達運動速度一般高于掃描速度,若機器人在掃描期間內運動,而雷達仍默認在同一位置收集一幀數據,則掃描結果將產生畸變,這一現象稱為激光雷達的運動畸變。激光雷達運動畸變會降低SLAM 系統的精確性。為減小運動畸變對SLAM 的影響,本文算法實現精準補償運動畸變。

圖1 激光雷達運動畸變

1.2 符號與定義

設X={xi}(i=1,…,n) 為一幀點云集合,xi表示第i個點的二維坐標向量,Xk表示第k幀點云,x(k,i)表示第k幀中的第i個點。記t為當前時間戳,則是第k幀的起始時間,由于激光雷達連續掃描的特性,故第k幀的結束時間與k +1 幀的起始時間是相等的。激光雷達坐標系{L} 是一個二維坐標系,其原點位于激光雷達的幾何中心,第k幀雷達坐標系{Lk} 中的第i個點表示為。p=[p1p2p3]T=[x y θ]T表示位姿向量。世界坐標系{W} 是一個二維坐標系,在初始位置與{L} 重合,第k幀世界坐標系{Wk} 中輪式里程計ti k時刻的位姿表示為。

2 運動畸變補償算法設計

2.1 輪式里程計運動估計

假定雷達運動的角速度和線速度隨時間平滑且連續。隨著旋轉激光雷達在空間中移動,每個掃描點在不同運動位姿下收集。為對激光雷達運動畸變進行補償,故為雷達收集的每個點匹配準確的運動位姿。輪式里程計數據輸出頻率較高,因此采用輪式里程計外部運動估計方式計算任一時刻的雷達運動位姿[17]。

世界坐標系為W,雷達坐標系L。第k幀起始時間與終止時間分別為和,輪式里程計在對應時間區間內的測量位姿為{pi},i=1、…、m,表示第k幀世界坐標系中輪式里程計位姿集合,p(k,i)是在區間內時刻的位姿。是第k幀對應的輪式里程計位姿。借助線性插值可得時刻的雷達估計位姿,如式(1)所示。

如果激光雷達在掃描周期內處于靜止狀態,則每個點的測量坐標系相同。由于雷達一般在移動中掃描,故Xk中每一點在不同的位姿下采集。運動中雷達位姿用一個3 ×3 的變換矩陣T表示,如式(2)所示。

其中,R為2 ×2 的旋轉矩陣,t為1 ×2 的平移向量。

對不同幀間坐標系的點x(k,i)使用匹配的輪式里程計位姿進行幀間變換從而校正畸變的點云。如圖2 幀間變換所示,激光雷達在時刻采集的點使用式(3)映射到雷達的起始幀中。

圖2 幀間變換

輪式里程計高頻測量的外部運動估計使運動畸變得到一定程度補償。但輪式里程計存在漂移,無法準確反映真實運動的情況[18-19]。由于傳感器的噪聲是不可忽略的,故基于式(1)的輪式里程計輔助方法只能獲得低精確度的雷達運動估計。為提高雷達運動估計的準確性,本文算法將基于輪式里程計漂移對當前雷達運動估計位姿進一步優化。

2.2 優化運動畸變補償

基于點云配準的激光里程計具有高精度的特點。與ICP 激光點云配準算法相比,NDT 算法對初始值不敏感,且匹配結果更精確,但不需要消耗大量計算作為代價求解估計值[20]。輪式里程計的測量誤差通常服從線性分布模型,因此針對輪式里程計漂移的影響,本文將輪式里程計外部運動估計與NDT 點云配準算法相結合。通過輪式里程計校正部分運動畸變,然后運用NDT 配準算法進行點云配準,將幀間匹配的結果作為精確值,從而處理輪式里程計的漂移。

NDT 將每次掃描中離散的二維點云轉換成二維平面內定義的分段連續可微的概率密度,且概率密度由易于計算的正態分布組成,然后通過相鄰掃描幀間匹配,獲得最優運動估計[21]。首先,機器人周圍的二維空間被規劃地分為大小相同的單元。其次,收集在二維空間內第k幀點云Xk,找出包含掃描點的單元,分別計算該單元中樣本x(k,i)的均值qk和協方差矩陣∑k。最后,建立該單元中掃描點樣本概率密度p(x) 的正態分布模型N(qk,∑k):

其中,p為待估計的位姿向量參數。

由于優化問題一般被描述為最小化問題,故將函數s(p) 表示為-s(p)。Newton 法可以尋找令函數最優的參數p。采用Newton 法迭代求解方程,方程如式(6)所示。

其中,H和g分別是s(p) 的Hessian 矩陣和梯度向量。為簡化公式,令梯度g中的元素gi為

等式(6)的解是增量Δp,在每次迭代中將Δp添加到當前估計位姿:

通過對當前幀估計位姿迭代優化,計算出精準位姿參數p。將p引入式(1),給出如式(10)形式。

其中,n表示第k幀中點x(k,i)的數量,p′表示輪式里程計位姿增量。

針對函數s(p) 的求解,初始值采用輪式里程計估計的當前幀位姿,利用Newton 法對位姿迭代優化,由式(6)～(9)迭代至收斂或滿足最大迭代次數,計算比較精準的當前幀位姿p。與文獻[15]方法不同的是,本文算法考慮了輪式里程計漂移對雷達運動估計的影響,為降低輪式里程計累積誤差的影響,如式(10)所示,由NDT 配準算法計算的位姿p作為準確值,將其與輪式里程計位姿p′的差值作為漂移估計值。輪式里程計的測量誤差服從線性分布,將誤差均勻分布到每幀,減小輪式里程計漂移對雷達運動估計的干擾,優化運動畸變補償效果,進而降低SLAM 系統軌跡累積誤差并生成全局一致地圖。

3 實驗結果與分析

本文采用文獻[22]提供的公開數據集,并進行真實場景實驗對算法測試和評估。本文實驗所用計算機配置為:中央處理器為Intel i5-7300HQ,主頻2.5 GHz,內存8 GB,操作系統為Ubuntu 18.04。為證明本文算法的有效性,將其與文獻[15]提出的輪式里程計輔助方法對比。由于數據集來自室內且不包含地面真值,所以將軌跡閉合和環境建圖效果作為評估算法性能的指標。軌跡閉合指機器人運動起點和終點為同一位置,從而令運動軌跡形成閉合回路[8,23]。在真實場景實驗中,將算法估計軌跡與軌跡真值對比,并分析軌跡誤差和建圖效果。

3.1 數據集實驗

數據集1 中激光掃描數據源于頻率為40 Hz、運動速度為0.27 m?s-1的激光雷達,數據集2 中激光雷達頻率為30 Hz、運動速度為0.75 m?s-1,所有數據集均包含高頻輪式里程計信息。數據集1 室內環境平面由2 條直線走廊、1 條小圓弧和1 個半徑約為8.5 m 的半圓組成。數據集2 室內環境面積為41 m×34 m。

圖3 和圖4 是本文算法和文獻[15]算法使用不同數據集生成的運動軌跡。圖3中,對比算法軌跡起點與終點間運動估計偏差量為0.15 m,而本文算法的偏差量是0.05 m。圖4 中,本文算法起點與終點間偏差量是0.35 m,而對比算法偏差量為0.50 m。通過圖3 和圖4 軌跡終點偏差量對比,可以看出本文方法優化了運動軌跡估計,主要表現在偏差量下降,軌跡閉合效果更優。文獻[15]算法與本文算法的建圖效果如圖5 和6 所示,圖中所標注的位置顯示對比算法在構建地圖中存在建圖不一致現象。由圖5 和6 可以看出,本文算法的建圖質量優于文獻[15]算法,產生較好地圖構建效果。

圖3 數據集1 軌跡

圖4 數據集2 軌跡

圖5 數據集1 地圖

圖6 數據集2 地圖

與對比算法相比,本文算法利用NDT 配準算法處理輪式里程計漂移,減小了輪式里程計累積誤差影響,優化了運動畸變補償效果,降低了軌跡累積誤差并生成全局一致地圖。

3.2 真實場景實驗

機器人平臺如圖7 所示,移動機器人采用差速驅動方式,底盤部分向上位機反饋輪式里程計信息,且搭載掃描頻率為10 Hz的思嵐A1 激光雷達。機器人利用機器人操作系統(robot operating system,ROS)的分布式通訊實現底盤控制和傳感器數據獲取。

圖7 實驗平臺

構建地圖場景如圖8 所示,場景1 和場景2 的環境面積分別為9 m×11 m 和43 m×55 m。雷達掃描期間機器人的運動速度為0.45 m?s-1。通過控制機器人移動路徑直至返回起點,使其運動軌跡形成完整的閉合回路。

圖8 實驗場景

圖9 和10 是文獻[15]算法和本文算法在不同場景中生成的機器人運動軌跡。算法軌跡與真實軌跡的誤差結果如表1 所示,誤差度量方式采用絕對平移誤差。由表1 誤差結果分析可得,在場景1 和場景2 中,本文算法相對于對比算法的誤差分別降低了0.15 m 和0.48 m。由此可得本文算法的運動軌跡估計效果優于文獻[15]。圖11 和12 分別是由文獻[15]算法和本文算法生成的地圖。從圖中可以看出,與對比算法生成的地圖相比,本文算法降低了輪式里程計漂移對雷達運動估計干擾,實現了精準補償運動畸變,進而創建出全局一致性更高的地圖。

表1 絕對平移誤差對比

圖9 場景1 軌跡

圖10 場景2 軌跡

圖11 場景1 地圖

圖12 場景2 地圖

4 結論

本文提出了一種基于NDT 配準與輪式里程計的運動畸變補償算法,旨在解決激光雷達掃描中存在的運動畸變現象。利用輪式里程計以低精確度高頻測量的方式估計雷達運動,補償部分運動畸變;設計一種基于NDT 配準算法的誤差處理方法,減小了輪式里程計漂移的影響,提高了雷達運動估計精度,優化了運動畸變補償效果。實驗結果表明,本文算法可使軌跡取得低漂移的精確度水平,并生成全局一致地圖,實現對SLAM 系統準確性的提升。