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

1 基本原理

    激光雷達(dá)與IMU之間存在安裝誤差角和位置誤差，因此兩個(gè)傳感器測(cè)量得到的同一組標(biāo)志點(diǎn)的三維坐標(biāo)不同，可以通過對(duì)應(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所示。

    (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_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)志物，通過移動(dòng)角反射器可獲得在雷達(dá)坐標(biāo)系下標(biāo)志物在不同位置的坐標(biāo)，并同時(shí)采用差分GPS獲得標(biāo)志物在當(dāng)前位置的地理坐標(biāo)系下的坐標(biāo)。因標(biāo)定場(chǎng)地不水平，通過車載IMU測(cè)得車輛相對(duì)地理坐標(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)系為(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)過步驟(4)中求解得到的轉(zhuǎn)換矩陣計(jì)算，轉(zhuǎn)移到雷達(dá)坐標(biāo)系中，與步驟(3)中得到的測(cè)量數(shù)據(jù)進(jìn)行對(duì)比，進(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)行對(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ì)來求得兩個(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á)探測(cè)到的點(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)。

    由上述步驟，可以得到測(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é)果基本一致，說明點(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é)果對(duì)比，結(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] 韓棟斌，徐友春，王任棟，等.基于多對(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] 余和青，劉文波，鄧釗波，等.最小二乘法視覺系統(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].中國圖象圖形學(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)