文獻(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.
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所示。
(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組成。
坐標(biāo)系N相對坐標(biāo)系M的歐拉角為俯仰角θ、橫滾角γ、方位角Ψ,相對于軸向的平移量為tx、ty、tz,則:
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所示。
IMU和激光雷達(dá)坐標(biāo)系的參數(shù)標(biāo)定流程如圖3所示,標(biāo)定步驟如下:
(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)換公式如下:
由此方法能夠得到使三維坐標(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)提取算法的主要流程如下:
最終得到的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所示。
由上述計(jì)算結(jié)果可知,誤差在0.3 m范圍內(nèi),距離誤差的平均值為0.167 m,誤差的標(biāo)準(zhǔn)差為0.068 8 m。最小二乘法計(jì)算結(jié)果與實(shí)際測量坐標(biāo)對比如圖4所示。
根據(jù)三維點(diǎn)云擬合的ICP算法,得到的x、y、z點(diǎn)云坐標(biāo)與測量坐標(biāo)的誤差如表2所示。
由上述計(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所示。
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)