劉 雷,柏艷紅,孫志毅,趙兵洋

(太原科技大學(xué)電子信息工程學(xué)院,山西 太原 030024)

1 引 言

利用環(huán)境感知傳感器實現(xiàn)的三維重建技術(shù),廣泛應(yīng)用于無人駕駛、測繪、物流、醫(yī)療等領(lǐng)域。點云場景三維重建采用多線激光雷達,通過同步定位與建圖(SLAM)技術(shù)來構(gòu)建高精度地圖[1-3],在衛(wèi)星信號弱的場景下,依靠智能算法與高精度地圖的融合實現(xiàn)定位。點云配準(zhǔn)技術(shù)是激光三維重建的數(shù)據(jù)關(guān)聯(lián),位姿解算,場景拼接中最關(guān)鍵的技術(shù)。

實際應(yīng)用中,多線激光雷達在移動載體上使用,首要解決的就是點云畸變校正問題。一些學(xué)者采用了額外傳感器,比如編碼器、慣性傳感器、相機來校正點云[4-6],取得了良好的效果。其次是機器人的定位誤差隨時間累計增大的問題,一些學(xué)者提出了增加回環(huán)約束因子來優(yōu)化全局位姿軌跡[7-8]。還存在特征約束不足造成激光里程計漂移問題,有學(xué)者將相機和激光雷達數(shù)據(jù)融合,彌補了不同傳感器的缺陷,提高了定位系統(tǒng)魯棒性[9-12]?？偠灾?提高點云配準(zhǔn)精度是該領(lǐng)域一直追求的目標(biāo)。

本文針對單激光雷達多幀點云配準(zhǔn)的位姿估計不準(zhǔn)確、累計誤差較大等問題,在激光雷達測量系統(tǒng)中引入IMU,研究點云和IMU位姿融合的配準(zhǔn)方法。

2 引入IMU的多幀點云配準(zhǔn)

多幀點云配準(zhǔn)時,將起始時刻點云作為基坐標(biāo)系和基礎(chǔ)點云,每個時刻的點云與基礎(chǔ)點云配準(zhǔn)后作為新的基礎(chǔ)點云。當(dāng)基礎(chǔ)點云數(shù)量龐大時,為了提高計算效率,只采用最近的幾幀數(shù)據(jù)進行配準(zhǔn)。

每兩幀點云配準(zhǔn)流程如圖1所示。原始點云數(shù)據(jù)經(jīng)過去無效點(NaN)后,將點云數(shù)據(jù)和IMU預(yù)積分位姿時間上對齊來校正單幀畸變點云,經(jīng)過投影、聚類后提取的曲率特征點,將IMU雷達位姿作為配準(zhǔn)的初值,構(gòu)建包含配準(zhǔn)誤差和IMU預(yù)積分誤差的聯(lián)合優(yōu)化方程來求解準(zhǔn)確的雷達位姿。

圖1 點云配準(zhǔn)流程圖

雷達位姿經(jīng)過坐標(biāo)變換,將不同時刻下點云場景配準(zhǔn)到基坐標(biāo)下形成點云地圖,認(rèn)為地圖為帶有歷史場景數(shù)據(jù)的集合。

2.1 IMU預(yù)積分

IMU傳感器可以獲得IMU坐標(biāo)系b下的三軸角速度和加速度信息,對三軸的加速度和角速度信息進行積分就可以得到任意時刻IMU相對于世界坐標(biāo)系w的速度、位置及姿態(tài)信息。由于IMU零偏和噪聲影響,長時間積分誤差較大,采用短時間內(nèi)預(yù)積分結(jié)果作為測量位姿。

預(yù)積分計算公式為:

(1)

(2)

(3)

預(yù)積分誤差導(dǎo)致IMU測量位姿不精確。誤差計算如下式:

(4)

2.2 特征點提取與匹配

本文的點云特征點提取方法,參考了文獻[7]中的Lego-Loam計算每個點處的曲率特征。設(shè)置曲率閾值為 0.1。將不同掃描線上的點按曲率從大到小進行排序,曲率排序前40的點作為邊緣特征點,曲率排序后80的點作為平面特征點。

提取了邊緣和平面特征點,下面將對相鄰兩幀點云中的特征點進行匹配,優(yōu)化求解兩幀間的位姿。配準(zhǔn)誤差的距離觀測值,采用當(dāng)前幀tk時刻數(shù)據(jù)中邊緣點到上一幀tk-1時刻數(shù)據(jù)中兩邊緣點點確定直線距離de和平面點到上一幀數(shù)據(jù)中的三個平面點所確定的平面的距離dp。計算公式如下:

(5)

3 點云畸變校正

激光雷達在運動中獲取點云,一幀點云中的激光點的坐標(biāo)系不同,產(chǎn)生點云畸變。為解決這個問題,需要計算雷達的運動量,在對應(yīng)的激光點上補償這個運動量。

由于IMU數(shù)據(jù)和點云數(shù)據(jù)獲取方式不同,導(dǎo)致算法在接收處理兩種數(shù)據(jù)的時間不統(tǒng)一,采用點云當(dāng)前時刻和IMU前后兩次最近預(yù)積分位姿進行線性插值實現(xiàn)時間對齊。插值方式如圖2所示。

圖2 IMU線性插值示意圖

t1,tn為當(dāng)前幀點云激光點的起始和結(jié)束時刻,t0,t2為起始時刻最近的兩次IMU測量位姿,記位置為p0,p2,姿態(tài)為q0,q2。tn-1,tn+1為結(jié)束時刻后最近的兩次IMU位姿,記位置為pn-1,pn+1,姿態(tài)為qn-1,qn+1。則當(dāng)前幀點云起止時刻的雷達位姿可由IMU的預(yù)積分位姿進行線性插值獲得,插值計算公式如下:

(6)

(7)

4 點云配準(zhǔn)

IMU誤差僅來自預(yù)積分的位置、速度、姿態(tài)、加速度計偏置及陀螺儀偏置誤差。由于激光點云配準(zhǔn)的誤差約束無法提供高精度的橫滾角和俯仰角的狀態(tài)估計。引入IMU傳感器狀態(tài),提供幀間配準(zhǔn)初始值,在點云配準(zhǔn)誤差方程中加入IMU誤差項,構(gòu)建聯(lián)合優(yōu)化的誤差函數(shù)。

由點到直線的距離d(e,i),點到平面的距離d(p,h)作為兩幀配準(zhǔn)誤差的觀測值。故有兩幀點云配準(zhǔn)誤差如下式:

(8)

式中,N為單幀點云提取的特征點總數(shù);ne為單幀提取的邊緣特征點;nq為單幀提取的平面特征點。構(gòu)建雷達和IMU融合的誤差函數(shù)F(TkL):

(9)

5 實驗研究

實驗方式為手持16線激光雷達,水平移動,步行速度約0.5 m/s,由于實驗條件有限,難以獲得雷達的真實運動軌跡,為了計算里程計算法誤差,采用閉環(huán)實驗驗證。將雷達運動起點和終點選為同一點,通過算法計算后,用起點與終點的位移差來表示里程計的準(zhǔn)確度。由于采集數(shù)據(jù)時雷達高度幾乎不變,只繪制雷達在x,y平面上運動軌跡,如圖3所示。Lidar-IMU方法軌跡起點坐標(biāo)(0.019,0.014,0.011),終點坐標(biāo)(0.019,0.012,0.042),誤差為3.3 %。單激光雷達方法估計位姿軌跡不能閉環(huán)。

圖3 雷達在平面上的運動軌跡估計圖

圖4為雷達位姿狀態(tài)估計的結(jié)果。分析可得,僅采用激光雷達點云配準(zhǔn)估計雷達位姿的方法,在z軸,y軸,x軸均存在較大程度的漂移,在雷達運動一周回到起始點時仍然漂移。采用IMU預(yù)積分提供初始位姿的Lidar-IMU方法估計的位姿較為準(zhǔn)確,雷達回到起始點后位姿不再漂移。

圖4 雷達位姿估計結(jié)果

實驗地點是某樓道走廊場景,如圖5(a)所示。如圖5(c)所示,僅用Lidar多幀融合得到的點云地圖出現(xiàn)大量重影。采用Lidar-IMU去畸變?nèi)诤系玫降狞c云地圖重影現(xiàn)象明顯減少,如圖5(d)所示,電梯口的輪廓比較清晰,墻面整體比較薄,說明采用IMU提供初始位姿動態(tài)配準(zhǔn)的誤差更小。對融合后的點云地圖進行網(wǎng)格化渲染,效果如圖5(b)所示。

圖5 真實場景下動態(tài)點云配準(zhǔn)結(jié)果對比

6 結(jié) 論

本文利用IMU預(yù)積分提供高精度的橫滾角和俯仰角的狀態(tài)約束,結(jié)合激光點云配準(zhǔn)誤差構(gòu)建聯(lián)合優(yōu)化的誤差函數(shù),采用高斯牛頓方法迭代求解優(yōu)化的雷達位姿。在實際場景實驗中驗證,未使用IMU去畸變?nèi)诤系狞c云地圖存在大量重影現(xiàn)象,使用IMU去畸變?nèi)诤系牡玫降狞c云地圖重影現(xiàn)象明顯減少。分析里程計狀態(tài)估計精度,單激光雷達位姿估計方法的回環(huán)實驗不能閉環(huán),且存在較大程度漂移。本文Lidar-IMU回環(huán)實驗精度為3.3 %,在橫滾角和俯仰角狀態(tài)估計上更準(zhǔn)確。