《電子技術(shù)應(yīng)用》
您所在的位置:首頁 > 測試測量 > 設(shè)計(jì)應(yīng)用 > 一種基于點(diǎn)云匹配的激光雷達(dá)/IMU聯(lián)合標(biāo)定方法
一種基于點(diǎn)云匹配的激光雷達(dá)/IMU聯(lián)合標(biāo)定方法
2019年電子技術(shù)應(yīng)用第12期
吳昱晗,王蘊(yùn)寶,薛慶全,郄曉斌,李志強(qiáng)
北京航天發(fā)射技術(shù)研究所,北京100076
摘要: 針對車載激光雷達(dá)和慣性測量單元(Inertial Measurement Unit,IMU)的坐標(biāo)系標(biāo)定問題,設(shè)計(jì)了一種使用角反射器作標(biāo)志物的標(biāo)定方法。首先通過區(qū)域分割、地面濾除和標(biāo)志點(diǎn)提取的預(yù)處理方法來提取標(biāo)志點(diǎn);然后借鑒3D點(diǎn)云匹配的方法,將坐標(biāo)系聯(lián)合標(biāo)定轉(zhuǎn)化為點(diǎn)云匹配問題,使用迭代最近鄰點(diǎn)(Iterative Closest Point,ICP)算法求得兩坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣。最后,將標(biāo)定結(jié)果與基于最小二乘法的標(biāo)定結(jié)果進(jìn)行對比,結(jié)果表明使用3D點(diǎn)云匹配的標(biāo)定方法是可行的。
中圖分類號: TN958.98
文獻(xiàn)標(biāo)識(shí)碼: A
DOI:10.16157/j.issn.0258-7998.190691
中文引用格式: 吳昱晗,王蘊(yùn)寶,薛慶全,等. 一種基于點(diǎn)云匹配的激光雷達(dá)/IMU聯(lián)合標(biāo)定方法[J].電子技術(shù)應(yīng)用,2019,45(12):78-82.
英文引用格式: Wu Yuhan,Wang Yunbao,Xue Qingquan,et al. A LiDAR/IMU joint calibration method based on point cloud matching[J]. Application of Electronic Technique,2019,45(12):78-82.
A LiDAR/IMU joint calibration method based on point cloud matching
Wu Yuhan,Wang Yunbao,Xue Qingquan,Qie Xiaobin,Li Zhiqiang
Beijing Institute of Space Launch Technology,Beijing 100076,China
Abstract: In this paper, a calibration method using a corner reflector as the marker is designed for the calibration of the coordinate system of the LiDAR and inertial measurement unit(IMU). Firstly, the pre-processing methods of region segmentation, ground filtering and marker point extraction are used to extract the marker points. And then the three-dimensional point cloud matching method is used to convert the coordinate system joint calibration into the point cloud matching problem, using the iterative closest point(ICP) algorithm to obtain the coordinate transformation matrix of the two coordinate systems. The calibration results are compared with results obtained by the traditional least squares algorithm, which shows that the calibration method using 3D point cloud matching is feasible.
Key words : coordinate system parameter calibration;3D point cloud matching;vehicle LiDAR

0 引言

    當(dāng)今導(dǎo)航定位領(lǐng)域,使用慣性導(dǎo)航系統(tǒng)(Inertial Navigation System,INS)+里程計(jì)(Odometry,OD)+高程計(jì)的組合定位方式進(jìn)行定位是主流的自主定位手段,但是該系統(tǒng)本身存在的誤差會(huì)隨著時(shí)間不斷發(fā)散,需要靠其他傳感器來進(jìn)行輔助定位。三維點(diǎn)云激光雷達(dá)定位作為一個(gè)新興的手段,具有不受光線影響、分辨率高、測量距離遠(yuǎn)的優(yōu)點(diǎn)。激光雷達(dá)和慣性導(dǎo)航進(jìn)行組合的定位方式也是當(dāng)今實(shí)現(xiàn)無人駕駛的主流技術(shù)途徑之一。為了滿足該系統(tǒng)的定位精度,傳感器之間的參數(shù)標(biāo)定至關(guān)重要,參數(shù)標(biāo)定精度直接影響融合定位結(jié)果。

    最初,激光雷達(dá)主要利用外部的經(jīng)緯儀和測距儀直接進(jìn)行輪廓測量[1],以此得到標(biāo)定參數(shù),但是這種方法過于繁瑣且精度較低。目前已經(jīng)提出了多種針對激光雷達(dá)位姿的標(biāo)定方法。程金龍[2]采用三面靶標(biāo)的激光雷達(dá)外參數(shù)標(biāo)定的方法,使用隨機(jī)采樣一致性算法完成了平面分割和同名向量的提取,最后解出標(biāo)定參數(shù);韓正勇[3]提出了一種可以在采樣幀數(shù)比較少的情況下獲得較高精度的參數(shù)矩陣的方法,該方法采用棋盤面對應(yīng)性的性質(zhì),將坐標(biāo)系標(biāo)定問題轉(zhuǎn)換為三維空間中旋轉(zhuǎn)和縮放矩陣的求解問題;韓棟斌[4]提出了一種在非理想?yún)?shù)初值的條件下依然可以獲得較好標(biāo)定結(jié)果的方法,該方法采用多對點(diǎn)云同時(shí)匹配迭代生成外參數(shù)來進(jìn)行參數(shù)解算。

    最小二乘法作為處理空間坐標(biāo)組合轉(zhuǎn)換的經(jīng)典方法之一,被廣泛應(yīng)用于多種傳感器的系統(tǒng)坐標(biāo)標(biāo)定[5-8]。針對最小二乘法標(biāo)定的改進(jìn)也在不斷進(jìn)行[9]。趙立峰[10]將整體最小二乘法引入了坐標(biāo)轉(zhuǎn)換中,提出了一種迭代算法,降低低精度點(diǎn)的影響;楊仕平[11]提出在標(biāo)志點(diǎn)數(shù)目在4個(gè)及以上,且在兩套坐標(biāo)系下均存在隨機(jī)誤差的情況下,采用多元整體最小二乘法進(jìn)行解算,提高算法精度。

    隨著即時(shí)定位與地圖構(gòu)建(Simultaneous Localization and Mapping,SLAM)技術(shù)的發(fā)展,基于3D點(diǎn)云匹配技術(shù)的點(diǎn)云拼接方法日趨成熟,3D點(diǎn)云匹配的思路可以運(yùn)用到坐標(biāo)系標(biāo)定的問題中,通過點(diǎn)云匹配方法求解兩個(gè)坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣。常用的3D點(diǎn)云匹配方法有ICP、正態(tài)分布變換(Normal Distributions Transform,NDT)等算法。點(diǎn)云配準(zhǔn)方法有很多種,目前比較普遍的處理方式是基于點(diǎn)-點(diǎn)匹配的迭代最近點(diǎn)方法(ICP)、進(jìn)一步提取特征進(jìn)行特征匹配的改進(jìn)ICP算法以及采用概率模型描述點(diǎn)云正態(tài)分布的NDT算法[12]。原始的ICP算法由Besl等提出[13],核心思想為計(jì)算使得匹配點(diǎn)對歐氏距離和最小的坐標(biāo)變換矩陣。針對常規(guī)ICP算法的缺陷,國內(nèi)外的學(xué)者也提出了大量的改進(jìn)算法,如MINGUEZ J[14]提出了一種新的距離尺度函數(shù),同時(shí)考慮到平移和選轉(zhuǎn),解決了旋轉(zhuǎn)誤差的問題。NDT算法是BIBER P[15]提出的,這種算法完全基于概率模型進(jìn)行匹配。

    本文中將提出一種基于點(diǎn)云匹配思想的車載激光雷達(dá)/IMU聯(lián)合標(biāo)定的方法,并與基于最小二乘法的標(biāo)定結(jié)果進(jìn)行對比。

1 基本原理

    激光雷達(dá)與IMU之間存在安裝誤差角和位置誤差,因此兩個(gè)傳感器測量得到的同一組標(biāo)志點(diǎn)的三維坐標(biāo)不同,可以通過對應(yīng)坐標(biāo)點(diǎn)的關(guān)系來計(jì)算得到坐標(biāo)系之間的轉(zhuǎn)換矩陣,完成激光雷達(dá)/IMU坐標(biāo)系的聯(lián)合標(biāo)定。

    兩坐標(biāo)系下三維坐標(biāo)的關(guān)系模型如圖1所示。

ck2-t1.gif

    (oX1Y1Z1)為坐標(biāo)系M,(OX2Y2Z2)為坐標(biāo)系N,標(biāo)志點(diǎn)在兩個(gè)坐標(biāo)系之間的坐標(biāo)分別為(x1,y1,z1)、(x2,y2,z2),兩者之間的坐標(biāo)變換矩陣為T3D為4×4矩陣,由旋轉(zhuǎn)矩陣R和平移矩陣T組成。

ck2-gs1-2.gif

    坐標(biāo)系N相對坐標(biāo)系M的歐拉角為俯仰角θ、橫滾角γ、方位角Ψ,相對于軸向的平移量為tx、ty、tz,則:

ck2-gs3-5.gif

2 標(biāo)定方案

2.1 總體方案

    本文采用具有多面反射、反射強(qiáng)度高、高度可調(diào)等優(yōu)點(diǎn)的角反射器作為標(biāo)志物,通過移動(dòng)角反射器可獲得在雷達(dá)坐標(biāo)系下標(biāo)志物在不同位置的坐標(biāo),并同時(shí)采用差分GPS獲得標(biāo)志物在當(dāng)前位置的地理坐標(biāo)系下的坐標(biāo)。因標(biāo)定場地不水平,通過車載IMU測得車輛相對地理坐標(biāo)系的姿態(tài)角及IMU處的地理坐標(biāo)系坐標(biāo),通過坐標(biāo)變換將標(biāo)志物在地理坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換到IMU坐標(biāo)系下。坐標(biāo)系描述如下:地理坐標(biāo)系為(OX1Y1Z1),IMU坐標(biāo)系為(OX2Y2Z2),激光雷達(dá)坐標(biāo)系為(oX3Y3Z3)。標(biāo)定系統(tǒng)總體方案示意圖如圖2所示。

ck2-t2.gif

    IMU和激光雷達(dá)坐標(biāo)系的參數(shù)標(biāo)定流程如圖3所示,標(biāo)定步驟如下:

ck2-t3.gif

    (1)采集地理坐標(biāo)系標(biāo)志點(diǎn)的原始數(shù)據(jù)和激光雷達(dá)坐標(biāo)系下的點(diǎn)云數(shù)據(jù)。

    (2)因?yàn)镮MU和差分全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite Systems,GNSS)的原點(diǎn)基本重合,可使用IMU傳感器測量得到的姿態(tài)角計(jì)算IMU與地理坐標(biāo)系之間的轉(zhuǎn)換矩陣,求解IMU坐標(biāo)系下標(biāo)志點(diǎn)的坐標(biāo)數(shù)據(jù)。

    (3)對激光雷達(dá)測量的原始點(diǎn)云數(shù)據(jù)進(jìn)行預(yù)處理,找到標(biāo)志點(diǎn)對應(yīng)的點(diǎn)云坐標(biāo)。

    (4)對IMU和雷達(dá)坐標(biāo)系下的標(biāo)志點(diǎn)數(shù)據(jù)進(jìn)行數(shù)據(jù)擬合,求得坐標(biāo)系轉(zhuǎn)換矩陣。

    (5)將步驟(2)得到的IMU坐標(biāo)系下的標(biāo)志點(diǎn)坐標(biāo)經(jīng)過步驟(4)中求解得到的轉(zhuǎn)換矩陣計(jì)算,轉(zhuǎn)移到雷達(dá)坐標(biāo)系中,與步驟(3)中得到的測量數(shù)據(jù)進(jìn)行對比,進(jìn)行誤差分析。

    至此標(biāo)定方法轉(zhuǎn)變?yōu)橥ㄟ^數(shù)據(jù)擬合、優(yōu)化的方法求解IMU坐標(biāo)系與雷達(dá)坐標(biāo)系之間坐標(biāo)轉(zhuǎn)換矩陣。傳統(tǒng)的方法是采用最小二乘法求解。隨著SLAM技術(shù)的發(fā)展,基于3D點(diǎn)云匹配技術(shù)的點(diǎn)云拼接方法日趨成熟。本文借鑒3D點(diǎn)云匹配思路,將這一標(biāo)定問題轉(zhuǎn)換為兩幀點(diǎn)云拼接的問題,進(jìn)而通過點(diǎn)云匹配方法求解兩個(gè)坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣。常用的3D點(diǎn)云匹配方法有ICP、NDT等算法,本文選擇ICP算法來求解坐標(biāo)轉(zhuǎn)換矩陣,并與最小二乘法求解結(jié)果進(jìn)行對比。

2.2 最小二乘法基本原理

    使用最小二乘法進(jìn)行系統(tǒng)坐標(biāo)標(biāo)定,求解的坐標(biāo)轉(zhuǎn)換公式如下:

ck2-gs6-7.gif

    由此方法能夠得到使三維坐標(biāo)誤差最小的坐標(biāo)系轉(zhuǎn)換矩陣。

2.3 ICP點(diǎn)云匹配算法原理

    ICP點(diǎn)云匹配算法(迭代最近點(diǎn)算法)是最常用的三維點(diǎn)云匹配算法之一。ICP算法最早由Besl和Mckay二人提出,主要思想為計(jì)算兩個(gè)點(diǎn)云中的最近點(diǎn)對進(jìn)行匹配,然后根據(jù)配準(zhǔn)點(diǎn)對來求得兩個(gè)點(diǎn)云之間的坐標(biāo)轉(zhuǎn)換矩陣。ICP算法通常應(yīng)用于不同時(shí)刻兩組點(diǎn)云之間的匹配,從而求得載體的位姿變化。那么,可以將在IMU坐標(biāo)系及雷達(dá)坐標(biāo)系下采集到的標(biāo)志點(diǎn)點(diǎn)云數(shù)據(jù)作為不同時(shí)刻的點(diǎn)云,通過3D點(diǎn)云匹配算法求其位姿變化,即為兩坐標(biāo)系的坐標(biāo)轉(zhuǎn)換關(guān)系。

    激光雷達(dá)探測到的點(diǎn)云包含大量無關(guān)點(diǎn)云,需進(jìn)行預(yù)處理來從中提取標(biāo)志點(diǎn)。

    預(yù)處理分為3個(gè)步驟:

    (1)區(qū)域切割。根據(jù)先驗(yàn)知識(shí)只選定距離在一定范圍之內(nèi)的點(diǎn)云進(jìn)行分析。

    (2)地面濾除。將點(diǎn)云數(shù)據(jù)柵格化,本文設(shè)置柵格大小為0.3×0.3并將每個(gè)柵格內(nèi)的點(diǎn)云按照高度排序,將每個(gè)柵格中高度最低的點(diǎn)視為地面點(diǎn),將與最低點(diǎn)距離小于閾值的點(diǎn)也當(dāng)作地面點(diǎn)并一同濾掉,便于后續(xù)處理。

    (3)標(biāo)志點(diǎn)提取。地面點(diǎn)云濾除后,點(diǎn)云信息中只包含標(biāo)志物,取最高點(diǎn)當(dāng)作標(biāo)志點(diǎn)。

    由上述步驟,可以得到測量的標(biāo)志點(diǎn)在雷達(dá)坐標(biāo)系下的坐標(biāo)。

    點(diǎn)云預(yù)處理標(biāo)志點(diǎn)提取算法的主要流程如下:

ck2-sf1-2.gif

    ck2-sf1-2-x1.gif

    最終得到的T3D即轉(zhuǎn)換矩陣。

3 參數(shù)標(biāo)定實(shí)例及誤差分析

3.1 標(biāo)定實(shí)例

    使用差分GPS測得標(biāo)志點(diǎn)及車載IMU處的地理位置坐標(biāo),根據(jù)車載IMU給出的姿態(tài)角度,可以得出車載IMU坐標(biāo)系下各標(biāo)志點(diǎn)的三維坐標(biāo)。

    根據(jù)上述的最小二乘法,可得到車載慣性坐標(biāo)系轉(zhuǎn)換到車載激光雷達(dá)坐標(biāo)系的轉(zhuǎn)換矩陣,將計(jì)算得出的結(jié)果與激光雷達(dá)獲得的點(diǎn)云坐標(biāo)進(jìn)行比對,其中x、y、z的坐標(biāo)值誤差如表1所示。

ck2-b1.gif

    由上述計(jì)算結(jié)果可知,誤差在0.3 m范圍內(nèi),距離誤差的平均值為0.167 m,誤差的標(biāo)準(zhǔn)差為0.068 8 m。最小二乘法計(jì)算結(jié)果與實(shí)際測量坐標(biāo)對比如圖4所示。

ck2-t4.gif

    根據(jù)三維點(diǎn)云擬合的ICP算法,得到的x、y、z點(diǎn)云坐標(biāo)與測量坐標(biāo)的誤差如表2所示。

ck2-b2.gif

    由上述計(jì)算結(jié)果,誤差在0.4 m范圍內(nèi),距離誤差的平均值為0.197 m,誤差的標(biāo)準(zhǔn)差為0.092 m,擬合效果較好。點(diǎn)云匹配方法計(jì)算結(jié)果與實(shí)際測量坐標(biāo)對比如圖5所示。

ck2-t5.gif

3.2 實(shí)驗(yàn)結(jié)論

    使用3D點(diǎn)云匹配的方式能夠較好地估算兩個(gè)坐標(biāo)系之間的坐標(biāo)轉(zhuǎn)換矩陣,與最小二乘法標(biāo)定得出的結(jié)果基本一致,說明點(diǎn)云匹配的標(biāo)定方法是有效的,這能夠?yàn)榧す饫走_(dá)/IMU的參數(shù)聯(lián)合標(biāo)定提供一種新的思路。

4 結(jié)論

    本文設(shè)計(jì)利用角反射器作為標(biāo)志物的車載激光雷達(dá)與IMU坐標(biāo)系的標(biāo)定方法,通過區(qū)域分割、地面濾除、標(biāo)志點(diǎn)提取的方法提取標(biāo)志點(diǎn),借鑒3D點(diǎn)云匹配算法的思路,將激光雷達(dá)與IMU坐標(biāo)系標(biāo)定問題轉(zhuǎn)換為點(diǎn)云匹配問題,通過ICP算法迭代求解兩者之間坐標(biāo)變換矩陣,并與最小二乘法結(jié)果對比,結(jié)果基本一致,說明基于點(diǎn)云匹配思想的標(biāo)定方法是有效的。

參考文獻(xiàn)

[1] KRABILL W B,WRIGHT C W,SWIFT R N.Airborne laser mapping of assateague national seashore beach[J].Photogrammetic Engineering and Remote Sensing,2000,66(1):65-71.

[2] 程金龍,馮瑩,曹毓,等.車載激光雷達(dá)外參數(shù)的標(biāo)定方法[J].光電工程,2013,40(12):89-94.

[3] 韓正勇,卜春光,劉宸.一種針孔相機(jī)與三維激光雷達(dá)外參標(biāo)定方法[J].傳感器與微系統(tǒng),2018,37(4):9-12,16.

[4] 韓棟斌,徐友春,王任棟,等.基于多對點(diǎn)云匹配的三維激光雷達(dá)外參數(shù)標(biāo)定[J].激光與光電子學(xué)進(jìn)展,2018,55(2):455-462.

[5] 熊爍,葉伯生,蔣明.機(jī)器人工具坐標(biāo)系標(biāo)定算法研究[J].機(jī)械與電子,2012(6):60-63.

[6] 余和青,劉文波,鄧釗波,等.最小二乘法視覺系統(tǒng)標(biāo)定研究[J].日用電器,2014(12):61-64.

[7] 段建民,王昶人,任璐,等.基于多層激光雷達(dá)的可行駛區(qū)域信息提取算法[J].電子技術(shù)應(yīng)用,2017,43(10):78-82.

[8] 王建中,楊璐.基于GPS定向測姿系統(tǒng)的研究[J].電子技術(shù)應(yīng)用,2016,42(9):14-18.

[9] 蔣從元,楊杰.基于分布式多跳誤差估計(jì)目標(biāo)位置感知算法[J].電子技術(shù)應(yīng)用,2017,43(11):95-98.

[10] 趙立峰.整體最小二乘法及其在坐標(biāo)轉(zhuǎn)換中的應(yīng)用[J].林業(yè)科技情報(bào),2011,43(4):114-115.

[11] 楊仕平,范東明,龍玉春.基于整體最小二乘法的任意旋轉(zhuǎn)角度三維坐標(biāo)轉(zhuǎn)換[J].大地測量與地球動(dòng)力學(xué),2013,33(2):114-119.

[12] 韓明瑞.基于激光LiDAR的室外移動(dòng)機(jī)器人三維定位與建圖[D].南京:東南大學(xué),2016.

[13] ZHANG Z.Iterative point matching for registration of free-form curves and surfaces[J].International Journal of Computer Vision,1994,13(2):119-152.

[14] MINGUEZ J,MONTESANO L,LAMIRAUX F.Metric-based iterative closest point scan matching for sensor displacement estimation[J].IEEE Transactions on Robotics,2006,22(5):1048-1054.

[15] BIBER P,STRASSER W. The normal distributions transform:a new approach to laser scan matching[C].Proceedings of 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems,2003(IROS 2003),2003,3:2743-2748.

[16] 沈海平,達(dá)飛鵬,雷家勇.基于最小二乘法的點(diǎn)云數(shù)據(jù)拼接研究[J].中國圖象圖形學(xué)報(bào),2005(9):1112-1116.

[17] 鄭忠陽.基于車載激光雷達(dá)的點(diǎn)云配準(zhǔn)算法及應(yīng)用技術(shù)研究[D].長沙:國防科學(xué)技術(shù)大學(xué),2015.



作者信息:

吳昱晗,王蘊(yùn)寶,薛慶全,郄曉斌,李志強(qiáng)

(北京航天發(fā)射技術(shù)研究所,北京100076)

此內(nèi)容為AET網(wǎng)站原創(chuàng),未經(jīng)授權(quán)禁止轉(zhuǎn)載。