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

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

1 引 言

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

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

基于以上所述,本文提出了一種標(biāo)定方法:先對初始標(biāo)定參數(shù)進(jìn)行粗略測量后通過分步迭代法迭代初始參數(shù)附近的值,并基于KD樹優(yōu)化的DBSCAN將轉(zhuǎn)換到唯一坐標(biāo)系下的相鄰幀點(diǎn)云集位置重合度進(jìn)行評價,尋求最準(zhǔn)確標(biāo)定參數(shù)。最后使用ICP算法對不同分步步長求解,得到的最優(yōu)標(biāo)定結(jié)果進(jìn)行了對比和驗證。此標(biāo)定模型來源于地理參考模型并與環(huán)境中的目標(biāo)點(diǎn)云的狀態(tài)有關(guān),標(biāo)定參數(shù)包括激光雷達(dá)與慣導(dǎo)載體坐標(biāo)系的桿臂量和歐拉角。相比于傳統(tǒng)標(biāo)定測量方法需使用經(jīng)緯儀或全站儀,且對目標(biāo)匹配物要求較高,標(biāo)定算法只使用到了激光雷達(dá)與INS/GNSS的觀測數(shù)據(jù),程序?qū)?zhí)行整個標(biāo)定過程并輸出最優(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)通常取慣性儀IMU的測量中心,X軸指向運(yùn)動載體右方,Y軸朝向載體前進(jìn)方向,Z軸垂直于X,Y軸指向上方。當(dāng)?shù)厮阶鴺?biāo)系定義為N系(N,E,D),N指向地理北方向,E指向地理東方向,D順著重力指向地面。因為GNSS接收機(jī)輸出的大地經(jīng)度和緯度和高度表示的大地坐標(biāo)(L,B,H)是一種橢球面上的坐標(biāo),不能直接應(yīng)用于導(dǎo)航定位計算[12],因此將其轉(zhuǎn)換為空間直角坐標(biāo)系m系。

圖1 地理參考模型

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

(1)

(2)

(3)

(4)

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

2.3 分步迭代法計算評價系數(shù)

Mmax=

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

(4)

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

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

3 實驗設(shè)計與分析

3.1 實驗設(shè)計

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

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

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

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

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

圖4 標(biāo)定測試場地地圖

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

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

對于初始安置參數(shù),采用不同的分步步長來驗證本文采取的分步步長是否為精確度更優(yōu)的步長。因為激光雷達(dá)與接收機(jī)都置于車載體平面上,因此Pitch角與Roll角的值不會相差太大,而Yaw偏航角的值會因安裝方式的不同而差異巨大,因此我們設(shè)計對Yaw角加大遍歷的區(qū)間。步長設(shè)計如表3所示。

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

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

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

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

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

圖6 匹配度得分對比圖

4 結(jié) 語

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