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ā)散,，需要靠其他傳感器來(lái)進(jìn)行輔助定位,。三維點(diǎn)云激光雷達(dá)定位作為一個(gè)新興的手段，具有不受光線影響,、分辨率高,、測(cè)量距離遠(yuǎn)的優(yōu)點(diǎn)。激光雷達(dá)和慣性導(dǎo)航進(jìn)行組合的定位方式也是當(dāng)今實(shí)現(xiàn)無(wú)人駕駛的主流技術(shù)途徑之一,。為了滿足該系統(tǒng)的定位精度,，傳感器之間的參數(shù)標(biāo)定至關(guān)重要，參數(shù)標(biāo)定精度直接影響融合定位結(jié)果,。

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

    最小二乘法作為處理空間坐標(biāo)組合轉(zhuǎn)換的經(jīng)典方法之一，被廣泛應(yīng)用于多種傳感器的系統(tǒng)坐標(biāo)標(biāo)定^[5-8]。針對(duì)最小二乘法標(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)定的問(wèn)題中，通過(guò)點(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)對(duì)歐氏距離和最小的坐標(biāo)變換矩陣。針對(duì)常規(guī)ICP算法的缺陷,，國(guó)內(nèi)外的學(xué)者也提出了大量的改進(jìn)算法,，如MINGUEZ J^[14]提出了一種新的距離尺度函數(shù)，同時(shí)考慮到平移和選轉(zhuǎn),，解決了旋轉(zhuǎn)誤差的問(wèn)題,。NDT算法是BIBER P^[15]提出的，這種算法完全基于概率模型進(jìn)行匹配,。

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

1 基本原理

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

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

    (oX₁Y₁Z₁)為坐標(biāo)系M，(OX₂Y₂Z₂)為坐標(biāo)系N,，標(biāo)志點(diǎn)在兩個(gè)坐標(biāo)系之間的坐標(biāo)分別為(x₁,，y₁,，z₁)、(x₂,，y₂,，z₂)，兩者之間的坐標(biāo)變換矩陣為T(mén)_3D為4×4矩陣,，由旋轉(zhuǎn)矩陣R和平移矩陣T組成,。

    坐標(biāo)系N相對(duì)坐標(biāo)系M的歐拉角為俯仰角θ、橫滾角γ,、方位角Ψ,，相對(duì)于軸向的平移量為t_x、t_y,、t_z,，則：

2 標(biāo)定方案

2.1 總體方案

    本文采用具有多面反射、反射強(qiáng)度高,、高度可調(diào)等優(yōu)點(diǎn)的角反射器作為標(biāo)志物,，通過(guò)移動(dòng)角反射器可獲得在雷達(dá)坐標(biāo)系下標(biāo)志物在不同位置的坐標(biāo)，并同時(shí)采用差分GPS獲得標(biāo)志物在當(dāng)前位置的地理坐標(biāo)系下的坐標(biāo),。因標(biāo)定場(chǎng)地不水平，通過(guò)車載IMU測(cè)得車輛相對(duì)地理坐標(biāo)系的姿態(tài)角及IMU處的地理坐標(biāo)系坐標(biāo),，通過(guò)坐標(biāo)變換將標(biāo)志物在地理坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換到IMU坐標(biāo)系下,。坐標(biāo)系描述如下：地理坐標(biāo)系為(OX₁Y₁Z₁)，IMU坐標(biāo)系為(OX₂Y₂Z₂),，激光雷達(dá)坐標(biāo)系為(oX₃Y₃Z₃),。標(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傳感器測(cè)量得到的姿態(tài)角計(jì)算IMU與地理坐標(biāo)系之間的轉(zhuǎn)換矩陣,，求解IMU坐標(biāo)系下標(biāo)志點(diǎn)的坐標(biāo)數(shù)據(jù)。

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

    (4)對(duì)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)過(guò)步驟(4)中求解得到的轉(zhuǎn)換矩陣計(jì)算,，轉(zhuǎn)移到雷達(dá)坐標(biāo)系中，與步驟(3)中得到的測(cè)量數(shù)據(jù)進(jìn)行對(duì)比,，進(jìn)行誤差分析,。

    至此標(biāo)定方法轉(zhuǎn)變?yōu)橥ㄟ^(guò)數(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)定問(wèn)題轉(zhuǎn)換為兩幀點(diǎn)云拼接的問(wèn)題,，進(jìn)而通過(guò)點(diǎn)云匹配方法求解兩個(gè)坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣。常用的3D點(diǎn)云匹配方法有ICP,、NDT等算法,，本文選擇ICP算法來(lái)求解坐標(biāo)轉(zhuǎn)換矩陣，并與最小二乘法求解結(jié)果進(jìn)行對(duì)比,。

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)對(duì)進(jìn)行匹配，然后根據(jù)配準(zhǔn)點(diǎn)對(duì)來(lái)求得兩個(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)云,，通過(guò)3D點(diǎn)云匹配算法求其位姿變化，即為兩坐標(biāo)系的坐標(biāo)轉(zhuǎn)換關(guān)系,。

    激光雷達(dá)探測(cè)到的點(diǎn)云包含大量無(wú)關(guān)點(diǎn)云,，需進(jìn)行預(yù)處理來(lái)從中提取標(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)。

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

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

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

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

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

    使用差分GPS測(cè)得標(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)行比對(duì),，其中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í)際測(cè)量坐標(biāo)對(duì)比如圖4所示,。

    根據(jù)三維點(diǎn)云擬合的ICP算法,，得到的x、y,、z點(diǎn)云坐標(biāo)與測(cè)量坐標(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í)際測(cè)量坐標(biāo)對(duì)比如圖5所示。

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

    使用3D點(diǎn)云匹配的方式能夠較好地估算兩個(gè)坐標(biāo)系之間的坐標(biāo)轉(zhuǎn)換矩陣,，與最小二乘法標(biāo)定得出的結(jié)果基本一致,，說(shuō)明點(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)定方法,，通過(guò)區(qū)域分割、地面濾除,、標(biāo)志點(diǎn)提取的方法提取標(biāo)志點(diǎn),，借鑒3D點(diǎn)云匹配算法的思路，將激光雷達(dá)與IMU坐標(biāo)系標(biāo)定問(wèn)題轉(zhuǎn)換為點(diǎn)云匹配問(wèn)題,，通過(guò)ICP算法迭代求解兩者之間坐標(biāo)變換矩陣,，并與最小二乘法結(jié)果對(duì)比，結(jié)果基本一致,，說(shuō)明基于點(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] 韓棟斌,，徐友春,，王任棟，等.基于多對(duì)點(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] 余和青，劉文波,，鄧釗波,，等.最小二乘法視覺(jué)系統(tǒng)標(biāo)定研究[J].日用電器，2014(12)：61-64.

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

[8] 王建中，楊璐.基于GPS定向測(cè)姿系統(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].大地測(cè)量與地球動(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].中國(guó)圖象圖形學(xué)報(bào)，2005(9)：1112-1116.

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

作者信息:

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

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