一種多線(xiàn)激光雷達(dá)與GNSS/INS系統(tǒng)標(biāo)定方法

葉玨磊,周志峰,王立端,龐正雅

(1.上海工程技術(shù)大學(xué)機(jī)械與汽車(chē)工程學(xué)院,上海201620;2.上海司南衛(wèi)星導(dǎo)航技術(shù)股份有限公司,上海201801)

1 引 言

近年來(lái),人工智能與數(shù)據(jù)科學(xué)給汽車(chē)工業(yè)帶來(lái)了革命性的進(jìn)步,智能駕駛技術(shù)得到了充分的發(fā)展?，F(xiàn)有的導(dǎo)航定位系統(tǒng)通常使用GNSS/INS組合系統(tǒng)進(jìn)行導(dǎo)航定位,衛(wèi)星導(dǎo)航系統(tǒng)通過(guò)衛(wèi)星信號(hào)在全球坐標(biāo)系內(nèi)提供速度和位置信息,慣性導(dǎo)航器件通常分為加速度計(jì)和陀螺儀,可以提供載體的方向加速度和角加速度。但GNSS與INS組合導(dǎo)航系統(tǒng)仍有其缺點(diǎn),當(dāng)GNSS在城市峽谷或者隧道中行駛時(shí),天線(xiàn)信號(hào)受到遮擋或因多路徑效應(yīng)導(dǎo)致GNSS無(wú)法定位時(shí),單一依靠慣性器件的系統(tǒng)誤差會(huì)隨著時(shí)間延續(xù)而累積,導(dǎo)致載體偏離規(guī)劃路徑。隨著激光雷達(dá)三維掃描成像技術(shù)的發(fā)展,現(xiàn)在越來(lái)越多的研究者在導(dǎo)航系統(tǒng)中加入激光雷達(dá),激光雷達(dá)具有厘米級(jí)精度,測(cè)距距離大,抗干擾能力強(qiáng),根據(jù)線(xiàn)束的多少可以測(cè)量不同的垂直角度,現(xiàn)今大多數(shù)研究是利用激光雷達(dá)的數(shù)據(jù)進(jìn)行障礙物聚類(lèi)分類(lèi),環(huán)境感知,輔助定位等[1-2]。但LiDAR獲取的雷達(dá)數(shù)據(jù)是定義于激光雷達(dá)坐標(biāo)系內(nèi),大多數(shù)應(yīng)用必須結(jié)合其他系統(tǒng)如慣性導(dǎo)航系統(tǒng)/衛(wèi)星導(dǎo)航系統(tǒng)。傳感器位姿關(guān)系的確定對(duì)多傳感器空間信息融合有直接影響,是研究多源融合導(dǎo)航定位算法的基礎(chǔ)[3]。為了加強(qiáng)導(dǎo)航定位系統(tǒng)的總體質(zhì)量和精確性,校正并標(biāo)定各個(gè)傳感器之間的關(guān)系具有重要意義。

許多學(xué)者對(duì)此課題做了不同的研究:文獻(xiàn)[4]提出了一種用于多光束激光雷達(dá)掃描儀的自主外部參數(shù)校準(zhǔn)技術(shù)。該方法將外部參數(shù)校準(zhǔn)程序分解為兩部分:通過(guò)地平面估算俯仰角和橫滾角引起的旋轉(zhuǎn)矩陣;通過(guò)在一系列姿態(tài)中匹配桿狀障礙物來(lái)計(jì)算由航向角控制的旋轉(zhuǎn)矩陣。文獻(xiàn)[5]提出了一種多波束激光雷達(dá)傳感器標(biāo)定參數(shù)的自動(dòng)優(yōu)化方法,該方法定義了一個(gè)能量函數(shù)來(lái)懲罰遠(yuǎn)離局部平面的點(diǎn)來(lái)優(yōu)化校正模型的不同參數(shù)。文獻(xiàn)[6]提出了一種將三維激光掃描技術(shù)與位置、姿態(tài)確定技術(shù)相結(jié)合的標(biāo)定方法。文獻(xiàn)[3]、[7]針對(duì)激光雷達(dá)的掃描中心和掃描光束大多不可見(jiàn)的問(wèn)題,提出首先使用太陽(yáng)能電池來(lái)找到激光雷達(dá)掃描線(xiàn)的準(zhǔn)確位置,用玻璃反射和撞擊激光流,得到控制點(diǎn)在激光雷達(dá)坐標(biāo)上的坐標(biāo),最后利用全站儀測(cè)得其地心地固坐標(biāo)后,最后采用改進(jìn)的Gauss-Newton方法建立三維坐標(biāo)變換模型計(jì)算激光雷達(dá)的外部參數(shù)。文獻(xiàn)[8]使用直線(xiàn)行駛車(chē)輛的三維激光雷達(dá)和GPS/INS組合導(dǎo)航系統(tǒng)數(shù)據(jù),用于選擇具有相似距離和相反方向的點(diǎn)云以形成點(diǎn)云對(duì)?；谶B續(xù)多對(duì)點(diǎn)云同時(shí)匹配,遍歷多步參數(shù)區(qū)域迭代的方法獲取外部參數(shù)。但標(biāo)定過(guò)程中沒(méi)有考慮高度參數(shù)。在文獻(xiàn)[9]中提出的激光雷達(dá)內(nèi)部參數(shù)兩步標(biāo)定法,采用標(biāo)靶場(chǎng)對(duì)激光雷達(dá)實(shí)現(xiàn)全視場(chǎng)的角度標(biāo)定,再利用基線(xiàn)場(chǎng)完成距離標(biāo)定。這種分離了角度與距離的標(biāo)定,降低了兩者的耦合性。在文獻(xiàn)[10]中通過(guò)三維激光掃描測(cè)量系統(tǒng)(LS)與多臺(tái)經(jīng)緯度測(cè)量?jī)x(TMS)對(duì)激光雷達(dá)/慣導(dǎo)/GNSS進(jìn)行了室內(nèi)安置參數(shù)標(biāo)定方法研究,但連接桿和天線(xiàn)表面材質(zhì)會(huì)對(duì)反射率的測(cè)量精度產(chǎn)生影響。文獻(xiàn)[11]中采用分步標(biāo)定法對(duì)三維激光雷達(dá)安置參數(shù)進(jìn)行標(biāo)定,先完成對(duì)激光雷達(dá)俯仰角、橫滾角和縱向位移的標(biāo)定,再以此為基礎(chǔ)對(duì)同一標(biāo)定目標(biāo)進(jìn)行中心聚類(lèi),在二維面內(nèi)對(duì)此目標(biāo)進(jìn)行直線(xiàn)擬合,根據(jù)斜率計(jì)算航向角,由此得到安置參數(shù),但對(duì)地面點(diǎn)云和標(biāo)定桿提取的精確度會(huì)影響標(biāo)定算法的精度和效率。

基于以上所述,本文提出了一種標(biāo)定方法:先對(duì)初始標(biāo)定參數(shù)進(jìn)行粗略測(cè)量后通過(guò)分步迭代法迭代初始參數(shù)附近的值,并基于KD樹(shù)優(yōu)化的DBSCAN將轉(zhuǎn)換到唯一坐標(biāo)系下的相鄰幀點(diǎn)云集位置重合度進(jìn)行評(píng)價(jià),尋求最準(zhǔn)確標(biāo)定參數(shù)。最后使用ICP算法對(duì)不同分步步長(zhǎng)求解,得到的最優(yōu)標(biāo)定結(jié)果進(jìn)行了對(duì)比和驗(yàn)證。此標(biāo)定模型來(lái)源于地理參考模型并與環(huán)境中的目標(biāo)點(diǎn)云的狀態(tài)有關(guān),標(biāo)定參數(shù)包括激光雷達(dá)與慣導(dǎo)載體坐標(biāo)系的桿臂量和歐拉角。相比于傳統(tǒng)標(biāo)定測(cè)量方法需使用經(jīng)緯儀或全站儀,且對(duì)目標(biāo)匹配物要求較高,標(biāo)定算法只使用到了激光雷達(dá)與INS/GNSS的觀(guān)測(cè)數(shù)據(jù),程序?qū)?zhí)行整個(gè)標(biāo)定過(guò)程并輸出最優(yōu)標(biāo)定結(jié)果。

2 標(biāo)定方法

2.1 坐標(biāo)系關(guān)系

地理參考模型如圖1所示,激光雷達(dá)坐標(biāo)系為l系,原點(diǎn)位于激光雷達(dá)結(jié)構(gòu)中心,X軸指向激光雷達(dá)的正前方,Y軸垂直于X軸指向雷達(dá)左側(cè),Z軸垂直于X,Y軸指向上,IMU載體坐標(biāo)系定義為b系,b系原點(diǎn)通常取慣性?xún)xIMU的測(cè)量中心,X軸指向運(yùn)動(dòng)載體右方,Y軸朝向載體前進(jìn)方向,Z軸垂直于X,Y軸指向上方。當(dāng)?shù)厮阶鴺?biāo)系定義為N系(N,E,D),N指向地理北方向,E指向地理東方向,D順著重力指向地面。因?yàn)镚NSS接收機(jī)輸出的大地經(jīng)度和緯度和高度表示的大地坐標(biāo)(L,B,H)是一種橢球面上的坐標(biāo),不能直接應(yīng)用于導(dǎo)航定位計(jì)算[12],因此將其轉(zhuǎn)換為空間直角坐標(biāo)系m系。

圖1 地理參考模型

由圖1可以推出以下公式:

(1)

(2)

(3)

(4)

2.2 數(shù)據(jù)采集與處理

2.3 分步迭代法計(jì)算評(píng)價(jià)系數(shù)

Mmax=

f(Xbest,Ybest,Zbest,Rollbest,Pitchbest,Yawbest)

(4)

在這里我們采用分步迭代法不斷調(diào)整相對(duì)位移向量(XΔ,YΔ,ZΔ,Roll,Pitch,Yaw)的值,一次遍歷一個(gè)參數(shù)。全局遍歷法需要對(duì)數(shù)據(jù)的最小步長(zhǎng)單位進(jìn)行遍歷,若數(shù)據(jù)量過(guò)于龐大,則對(duì)處理系統(tǒng)而言壓力較大且耗時(shí)較長(zhǎng)。分步迭代法先在區(qū)間內(nèi)求出局部最優(yōu)解,再在局部最優(yōu)解周邊尋求全局最優(yōu)解,與全局遍歷法相比加快了遍歷速度,提高了效率。

圖2 相鄰點(diǎn)云幀特征物點(diǎn)云的重合

3 實(shí)驗(yàn)設(shè)計(jì)與分析

3.1 實(shí)驗(yàn)設(shè)計(jì)

本次實(shí)驗(yàn)采用速騰聚創(chuàng)的16線(xiàn)激光雷達(dá)和上海司南衛(wèi)星導(dǎo)航技術(shù)股份有限公司的GNSS/INS接收機(jī)M600-mini來(lái)采集標(biāo)定環(huán)境數(shù)據(jù)。圖3是在本次實(shí)驗(yàn)中使用的實(shí)驗(yàn)平臺(tái)。表1與表2給出了接收機(jī)和激光雷達(dá)的具體參數(shù)。對(duì)于接收機(jī)和激光雷達(dá)點(diǎn)云PCD的數(shù)據(jù),統(tǒng)一接入基于Ubuntu的ROS(機(jī)器人操作系統(tǒng))中,使用C++語(yǔ)言編寫(xiě)本文的算法程序。調(diào)用PCL庫(kù)中的函數(shù)并根據(jù)高度差值濾掉低于激光雷達(dá)高度的所有地面點(diǎn)云,優(yōu)化點(diǎn)云的大小,便于篩選出目標(biāo)物點(diǎn)云,使本文的目標(biāo)物點(diǎn)云數(shù)據(jù)更加精確。

表1 司南M600-mini接收機(jī)參數(shù)

為了減少實(shí)驗(yàn)的誤差源,標(biāo)定測(cè)試場(chǎng)景在上方無(wú)遮擋的場(chǎng)景下進(jìn)行,保證載波相位差分(RTK)信號(hào)較好。圖4即為通過(guò)三維激光雷達(dá)點(diǎn)云擬合出的行駛環(huán)境三維地圖,矩形所示部分是行駛的道路,圖5為圖4中間白色矩形部分放大后的道路點(diǎn)云圖,在路的兩端有很多樹(shù)木,具有固定特征有助于提取出其結(jié)構(gòu)用于標(biāo)定,可以有助于對(duì)相鄰幀的點(diǎn)云重合度進(jìn)行評(píng)價(jià)并可幫助建立三維高精度地圖。本文在當(dāng)前場(chǎng)景下的車(chē)速為10 km/h,保證了點(diǎn)云的連續(xù)性與密集性。

表2 速騰聚創(chuàng)16線(xiàn)激光雷達(dá)參數(shù)

圖3 標(biāo)定所用測(cè)量系統(tǒng)

圖4 標(biāo)定測(cè)試場(chǎng)地地圖

圖5 某路段道路環(huán)境點(diǎn)云圖

3.2 校準(zhǔn)結(jié)果與分析

對(duì)于初始安置參數(shù),采用不同的分步步長(zhǎng)來(lái)驗(yàn)證本文采取的分步步長(zhǎng)是否為精確度更優(yōu)的步長(zhǎng)。因?yàn)榧す饫走_(dá)與接收機(jī)都置于車(chē)載體平面上,因此Pitch角與Roll角的值不會(huì)相差太大,而Yaw偏航角的值會(huì)因安裝方式的不同而差異巨大,因此我們?cè)O(shè)計(jì)對(duì)Yaw角加大遍歷的區(qū)間。步長(zhǎng)設(shè)計(jì)如表3所示。

表3 分步迭代法局部最優(yōu)與全局最優(yōu)步長(zhǎng)設(shè)計(jì)

在這個(gè)實(shí)驗(yàn)中,通過(guò)采集激光雷達(dá)數(shù)據(jù)與GNSS/INS的數(shù)據(jù)來(lái)實(shí)施校準(zhǔn)。首先通過(guò)測(cè)量得到初始安置參數(shù)。表4展示了安置初始參數(shù)桿臂量與旋轉(zhuǎn)角,加上實(shí)施算法后實(shí)現(xiàn)的校準(zhǔn)結(jié)果?？梢?jiàn)桿臂量和旋轉(zhuǎn)角與初始值相比有了很大的變化。如果我們不進(jìn)行桿臂量和旋轉(zhuǎn)角的優(yōu)化校準(zhǔn),那么激光雷達(dá)獲取的數(shù)據(jù)都是不可靠的并且會(huì)嚴(yán)重影響激光雷達(dá)點(diǎn)云幀之間的配準(zhǔn)。

表4 初始測(cè)量安置參數(shù)與實(shí)驗(yàn)計(jì)算得最優(yōu)標(biāo)定參數(shù)

表5 ICP算法驗(yàn)證最優(yōu)外參的重合結(jié)果

圖6是經(jīng)過(guò)實(shí)驗(yàn)一、二、三得到的安置參數(shù)與初始安置參數(shù)的得分對(duì)比。因?yàn)槠ヅ涞梅衷叫≌f(shuō)明匹配效果越好,我們對(duì)得分依次轉(zhuǎn)換為倒數(shù)。經(jīng)過(guò)ICP算法對(duì)標(biāo)定后的結(jié)果驗(yàn)證, 本文算法確實(shí)提升了激光雷達(dá)系中目標(biāo)物轉(zhuǎn)到空間直角坐標(biāo)系下的轉(zhuǎn)換結(jié)果的精度。實(shí)驗(yàn)一與實(shí)驗(yàn)二相比擴(kuò)大了初次迭代的區(qū)間范圍,但兩次實(shí)驗(yàn)的得分結(jié)果相近,證明最優(yōu)解是存在區(qū)間范圍的,超過(guò)這個(gè)區(qū)間范圍的迭代是無(wú)意義的。實(shí)驗(yàn)一與實(shí)驗(yàn)三對(duì)比,實(shí)驗(yàn)三縮小了迭代步長(zhǎng),但得到的點(diǎn)云匹配結(jié)果是最高的,且耗時(shí)最長(zhǎng)。

圖6 匹配度得分對(duì)比圖

4 結(jié) 語(yǔ)

多波束激光雷達(dá)在機(jī)器人、智能駕駛領(lǐng)域的重要性正在與日俱增,強(qiáng)大的3D感知能力使其在3D-SLAM中無(wú)法替代,將來(lái)也會(huì)有更多針對(duì)于激光雷達(dá)應(yīng)用的研究。對(duì)于任何傳感器,校準(zhǔn)是生成有效數(shù)據(jù)之前最重要的過(guò)程之一。為了提升激光雷達(dá)與GNSS/INS之間的傳感器相關(guān)度和傳感器組合的表現(xiàn),本文提出一種計(jì)算最優(yōu)安置參數(shù)的標(biāo)定方法,對(duì)初始安置參數(shù)進(jìn)行測(cè)量后通過(guò)分步迭代法迭代初始參數(shù)附近的值,基于KD樹(shù)優(yōu)化的DBSCAN算法評(píng)價(jià)相鄰幀點(diǎn)云重合度以得到最準(zhǔn)確的安置參數(shù)。并利用ICP算法對(duì)得到的最優(yōu)安置參數(shù)進(jìn)行了驗(yàn)證和評(píng)估,實(shí)驗(yàn)測(cè)試結(jié)果表明:①安置參數(shù)在步長(zhǎng)減小的情況下精度會(huì)得到一定的提高。②本文采用的算法相比初始估計(jì)外參明顯提高了標(biāo)定精度,具有可靠性與優(yōu)越性。后期的工作將著重于改進(jìn)本文的算法達(dá)到精度的量化。