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

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

1 引 言

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

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

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

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

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

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

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

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

2.1 IMU預(yù)積分

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

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

(1)

(2)

(3)

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

(4)

2.2 特征點(diǎn)提取與匹配

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

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

(5)

3 點(diǎn)云畸變校正

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

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

圖2 IMU線性插值示意圖

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

(6)

(7)

4 點(diǎn)云配準(zhǔn)

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

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

(8)

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

(9)

5 實(shí)驗(yàn)研究

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

圖3 雷達(dá)在平面上的運(yùn)動(dòng)軌跡估計(jì)圖

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

圖4 雷達(dá)位姿估計(jì)結(jié)果

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

圖5 真實(shí)場(chǎng)景下動(dòng)態(tài)點(diǎn)云配準(zhǔn)結(jié)果對(duì)比

6 結(jié) 論

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