一種基于三維特征點(diǎn)集的激光雷達(dá)與相機(jī)配準(zhǔn)方法

俞德崎，李廣云，王 力，李帥鑫，宗文鵬

(信息工程大學(xué)，河南 鄭州 450001)

當(dāng)前汽車制造產(chǎn)業(yè)正朝著電動(dòng)化、智能化和互聯(lián)網(wǎng)化方向不斷發(fā)展。無(wú)人駕駛汽車是通過(guò)車載的相機(jī)、激光雷達(dá)和IMU(inertial measurement unit)等傳感器采集周圍環(huán)境信息，利用數(shù)據(jù)融合技術(shù)實(shí)現(xiàn)障礙物認(rèn)知、環(huán)境認(rèn)知和車輛認(rèn)知等并實(shí)現(xiàn)三維環(huán)境重構(gòu)，進(jìn)而自動(dòng)規(guī)劃行車路線并控制車輛到達(dá)預(yù)定目標(biāo)[1]。在無(wú)人駕駛汽車研究領(lǐng)域，環(huán)境感知是該領(lǐng)域最基礎(chǔ)的技術(shù)之一。沒(méi)有對(duì)周圍環(huán)境的感知，車輛就無(wú)法實(shí)現(xiàn)自主決策和控制[2]。隨著傳感器技術(shù)及無(wú)人駕駛技術(shù)的不斷發(fā)展，激光雷達(dá)與相機(jī)已經(jīng)成為了一組不可或缺的傳感器組合。

激光雷達(dá)通過(guò)向周圍環(huán)境發(fā)射激光束，然后接收從目標(biāo)反射的回波，作適當(dāng)處理后，可獲得目標(biāo)的距離、方位、姿態(tài)和形狀等結(jié)構(gòu)信息，但無(wú)法獲得周圍目標(biāo)的色彩信息。相機(jī)可以獲得豐富的色彩和特征信息，可用于對(duì)行人、樹(shù)木等特征目標(biāo)的檢測(cè)，但易受環(huán)境因素的干擾。因此，激光雷達(dá)與相機(jī)在功能上具有良好的互補(bǔ)性。在無(wú)人駕駛領(lǐng)域，將激光雷達(dá)與相機(jī)獲得的點(diǎn)云和圖像數(shù)據(jù)進(jìn)行融合處理，實(shí)現(xiàn)對(duì)周圍環(huán)境目標(biāo)的感知，使汽車自主避開(kāi)障礙，防止事故的發(fā)生。而在數(shù)據(jù)融合過(guò)程中，激光雷達(dá)與相機(jī)的配準(zhǔn)是影響數(shù)據(jù)融合效果的關(guān)鍵環(huán)節(jié)。

目前，在激光雷達(dá)和相機(jī)配準(zhǔn)研究領(lǐng)域，國(guó)內(nèi)外都已經(jīng)取得了一定的進(jìn)展，目前的配準(zhǔn)方法主要有以下兩類：一是基于棋盤(pán)格、3D圖形等標(biāo)志的點(diǎn)、線或形狀特征集，通過(guò)建立點(diǎn)云與圖像特征集間的對(duì)應(yīng)關(guān)系，求解配準(zhǔn)參數(shù)[3-6]；二是基于點(diǎn)云和圖像的一些共有屬性，通過(guò)互信息值計(jì)算損失函數(shù)，再利用優(yōu)化算法實(shí)現(xiàn)最佳配準(zhǔn)[2，7-9]，但這類方法只適用于傳感器間位移和旋轉(zhuǎn)較小的情況。文獻(xiàn)[10]提出了一種新的運(yùn)動(dòng)恢復(fù)結(jié)構(gòu)(structure from motion,SFM)算法用于配準(zhǔn)參數(shù)解算，文獻(xiàn)[11]利用IMU提供的運(yùn)動(dòng)信息輔助配準(zhǔn)，這兩種配準(zhǔn)方式對(duì)硬件條件有較高的要求。

本文提出一種基于激光雷達(dá)與相機(jī)數(shù)據(jù)中對(duì)應(yīng)三維特征點(diǎn)集，適用于低密度激光雷達(dá)與相機(jī)的配準(zhǔn)方法。該方法利用ArUco標(biāo)簽[12]輔助得到紙板角點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)；利用隨機(jī)抽樣一致算法(RANSAC)提取激光雷達(dá)點(diǎn)云中的紙板邊緣，以邊緣交點(diǎn)為角點(diǎn)，從而建立兩組角點(diǎn)的對(duì)應(yīng)關(guān)系；最后利用Kabsch算法求解最優(yōu)配準(zhǔn)參數(shù)。

1 傳感器與標(biāo)志

本文使用Velodyne VLP16型激光雷達(dá)和KinectV2 RGB-D相機(jī)作為試驗(yàn)的傳感器組合對(duì)提出的配準(zhǔn)方法進(jìn)行研究。Velodyne VLP16型共有16條掃描線，水平視場(chǎng)360°，垂直視場(chǎng)30°，測(cè)量距離可以達(dá)到100 m，每秒可接受30萬(wàn)個(gè)點(diǎn)云數(shù)據(jù)。KinectV2 RGB-D相機(jī)有彩色和紅外兩個(gè)鏡頭，紅外鏡頭可以取得目標(biāo)的深度信息，水平視場(chǎng)70°，垂直視場(chǎng)60°，距離檢測(cè)距離0.5～4.5 m，每秒可傳輸30幀圖像。在對(duì)激光雷達(dá)與相機(jī)進(jìn)行配準(zhǔn)前必須先對(duì)相機(jī)內(nèi)參數(shù)進(jìn)行標(biāo)定。

1.1 相機(jī)標(biāo)定

RGB-D相機(jī)標(biāo)定主要包括3部分：彩色鏡頭內(nèi)參數(shù)標(biāo)定、紅外鏡頭內(nèi)參數(shù)標(biāo)定，以及彩色與紅外鏡頭安置參數(shù)標(biāo)定。相機(jī)鏡頭的內(nèi)參數(shù)標(biāo)定可采用張正友標(biāo)定法[13]。棋盤(pán)格采用尺寸為7×9的網(wǎng)格，網(wǎng)格邊長(zhǎng)40 mm，從不同視角對(duì)棋盤(pán)板拍攝50次，拍攝原則是以不同距離不同視角拍攝，保證靶標(biāo)在圖像的各個(gè)區(qū)域。已知鏡頭內(nèi)參數(shù)后，可按照雙目相機(jī)標(biāo)定方法解算兩鏡頭外參數(shù)。表1為相機(jī)內(nèi)標(biāo)定結(jié)果，(fu,fv,u0,v0)為內(nèi)方位元素，k1、k2為徑向畸變系數(shù)，p1、p2為切向畸變系數(shù)。表2為彩色鏡頭和紅外鏡頭外標(biāo)定結(jié)果。

表1 相機(jī)內(nèi)參數(shù)標(biāo)定結(jié)果

表2 兩鏡頭外參數(shù)標(biāo)定結(jié)果

1.2 標(biāo)志設(shè)置

測(cè)量標(biāo)志的選取原則：保證標(biāo)志存在相機(jī)與激光雷達(dá)都能簡(jiǎn)單識(shí)別和檢測(cè)到的特征。矩形紙板是一種簡(jiǎn)單有效的標(biāo)志，并且可以裁成任意尺寸，重量輕，易于懸掛。由于本文使用的低密度Velodyne VLP16激光雷達(dá)只有16根掃描線，如果紙板尺寸太小或掃描距離太遠(yuǎn)會(huì)導(dǎo)致只有2～3根掃描線打在紙板，那么每個(gè)邊緣只有2～3個(gè)點(diǎn)，這樣在點(diǎn)云數(shù)據(jù)中很難擬合出紙板的邊緣。因此，紙板尺寸及掃描距離必須合適。經(jīng)過(guò)前期多次測(cè)試，確定了較合適的紙板尺寸和掃描距離。文中使用的是邊長(zhǎng)為50 cm的正方形紙板，掃描距離為2 m，保證有足夠的掃描線打在紙板上。

2 特征點(diǎn)提取

為了求解相機(jī)坐標(biāo)系與激光雷達(dá)坐標(biāo)系間的配準(zhǔn)參數(shù)，需要兩組對(duì)應(yīng)的3D特征點(diǎn)集：一組在相機(jī)坐標(biāo)系下，另一組在激光雷達(dá)坐標(biāo)系下。一旦找到了這兩組對(duì)應(yīng)的三維特征點(diǎn)，就能利用算法求解兩傳感器間的配準(zhǔn)參數(shù)。試驗(yàn)數(shù)據(jù)采集場(chǎng)景如圖1所示。

圖1 試驗(yàn)設(shè)置

2.1 基于相機(jī)的特征點(diǎn)提取

本文采用一種增強(qiáng)現(xiàn)實(shí)(AR)標(biāo)簽，ArUco是一種特殊的編碼模式，有助于標(biāo)簽本身的檢測(cè)和糾錯(cuò)，將標(biāo)簽用A4紙打印并粘貼在硬紙板上作為標(biāo)志。在紙板尺寸和標(biāo)簽位置已知的情況下，紙板角點(diǎn)的位置可以通過(guò)計(jì)算得到。相機(jī)可以識(shí)別ArUco標(biāo)簽，并得到標(biāo)簽中心到相機(jī)坐標(biāo)系的轉(zhuǎn)換參數(shù)，如圖2所示，具體使用方法可參考文獻(xiàn)[14]，在標(biāo)簽尺寸位置已知的情況下，利用轉(zhuǎn)換參數(shù)可以將角點(diǎn)從標(biāo)簽坐標(biāo)系轉(zhuǎn)換到相機(jī)坐標(biāo)系下，從而得到角點(diǎn)在相機(jī)坐標(biāo)系下的三維坐標(biāo)。

圖2 相機(jī)識(shí)別ArUco標(biāo)簽

2.2 基于激光雷達(dá)的特征點(diǎn)提取

激光雷達(dá)掃描線是與地面平行的水平線，因此如果矩形紙板以邊緣平行地面方式放置，那么激光雷達(dá)只能掃到與地面垂直的2個(gè)邊緣。為了避免這種情況，將紙板以邊緣與地面大約呈45°的方式懸掛，這樣可以掃到4個(gè)邊緣。利用RANSAC算法[15]擬合點(diǎn)云中的邊緣線。

RANSAC算法基本假設(shè)是，數(shù)據(jù)集中包含局內(nèi)點(diǎn)，也包含局外點(diǎn)，采用不斷迭代的方法，尋找最優(yōu)模型，并剔除不符合最優(yōu)模型的局外點(diǎn)，構(gòu)建一個(gè)僅由局內(nèi)點(diǎn)組成的數(shù)據(jù)子集。其基本步驟如下[16]：

(3) 計(jì)算點(diǎn)云P中其余點(diǎn)pi到Lt的距離di及與nt的夾角σi是否滿足條件

di<ε且σi<α

(1)

式中，ε為閾值距離；α夾角閾值。將滿足條件的點(diǎn)的個(gè)數(shù)，作為L(zhǎng)t的分?jǐn)?shù)SLt。

(4) 重復(fù)執(zhí)行步驟(1)～步驟(3)T次，選擇出得分最高的平面Lt*。其中T由下式?jīng)Q定

(2)

式中，τ為L(zhǎng)t*局外點(diǎn)所占比例的估計(jì)值；Φ為T(mén)次采樣后最優(yōu)平面被選中的概率。

(5) 記錄Lt*并在P中移除計(jì)入SLt*的點(diǎn)。

(6) 重復(fù)步驟(1)～步驟(5)，直至無(wú)法從剩余的點(diǎn)中選擇出一個(gè)得分至少為閾值S0的平面。

(7) 將每個(gè)平面L擬合的點(diǎn)投影到L上，并將區(qū)域的邊界點(diǎn)作為邊緣線的候選點(diǎn)集Q。

(9) 計(jì)算Q中其余點(diǎn)qj到Im的距離dj，檢查是否滿足條件

dj<ξ

(3)

式中，ξ為點(diǎn)到直線距離閾值。將滿足條件的點(diǎn)的個(gè)數(shù)作為Im的分?jǐn)?shù)SIm。

(10) 重復(fù)步驟(8)～步驟(9)M次，選擇出得分最高的直線Im*，其中M由下式確定

(4)

(11) 記錄Im*并在Q中移除計(jì)入SIm*的點(diǎn)。

在擬合出紙板4條邊緣線之后，可以解得4條邊緣的線方程，以它們的交點(diǎn)作為紙板角點(diǎn)。若求出的邊緣線方程無(wú)交點(diǎn)，則把兩邊緣線距離最短處連線的中點(diǎn)近似為角點(diǎn)。

3 配準(zhǔn)參數(shù)求解

在得到兩組對(duì)應(yīng)的三維特征點(diǎn)之后，相機(jī)坐標(biāo)系與激光雷達(dá)坐標(biāo)系間的轉(zhuǎn)換參數(shù)可以利用Kabsch算法求解得到。同時(shí)為驗(yàn)證本文提出的配準(zhǔn)方法的準(zhǔn)確性，用本文的方法對(duì)相機(jī)與相機(jī)配準(zhǔn)參數(shù)進(jìn)行求解。

3.1 基于Kabsch算法求解配準(zhǔn)參數(shù)

Kabsch算法用于計(jì)算兩個(gè)待配準(zhǔn)點(diǎn)集最優(yōu)旋轉(zhuǎn)矩陣，利用該方法求解兩點(diǎn)集的最優(yōu)矩陣對(duì)點(diǎn)集旋轉(zhuǎn)后可以使均方根偏差(RMSD)最小，在旋轉(zhuǎn)已知的情況下，兩坐標(biāo)系的平移參數(shù)也可以確定。設(shè)P和Q是兩個(gè)待配準(zhǔn)點(diǎn)云，算法過(guò)程如下

(5)

(6)

(7)

(8)

(9)

將式(9)代入目標(biāo)函數(shù)式(5)得

(10)

(11)

由矩陣跡的性質(zhì)，式(11)試可簡(jiǎn)化為

tr(X′TX′)+tr(YTY)-2tr(YTX)

(12)

2tr(YTX′)

(13)

消去不包含R的項(xiàng)，得到新的目標(biāo)函數(shù)

(14)

由X′=RX和跡的性質(zhì)

tr(YTX′)=tr(YTRX)=tr(XYTR)

(15)

利用奇異值分解有XYT=UDVT，式(15)等于

(16)

令M=VTRU則有

(17)

M是正交矩陣的乘積，也是正交矩陣，det(M)=+/-1，M中每個(gè)列向量的長(zhǎng)度等于1，向量的每個(gè)分量小于或等于1。令Mii=1，其他分量為0，則有

M=I?VTRU=I?R=VUT

(18)

R是一個(gè)旋轉(zhuǎn)矩陣，R∈SO(3)，需要確保det(R)=+1。若由式(18)得到的R，其det(R)=-1，那需要找到一個(gè)R，能使tr(YTX′)取得第二大值。

tr(YTX′)=d1M11+d2M22+d3M33

(19)

式中，d1≥d2≥d3且|Mii|≤1。

當(dāng)式(19)有第二大值時(shí)M11=M22=1，M33=-1，綜上則最優(yōu)矩陣為

R=UCVT

(20)

式中，C為校正矩陣

(21)

3.2 相機(jī)與相機(jī)的配準(zhǔn)

本文的主要目的是獲得激光雷達(dá)與相機(jī)準(zhǔn)確的配準(zhǔn)參數(shù)，但由于旋轉(zhuǎn)參數(shù)在實(shí)際試驗(yàn)過(guò)程中測(cè)量不便，為了驗(yàn)證本文方法解算的配準(zhǔn)參數(shù)的準(zhǔn)確性，利用文中的配準(zhǔn)方法，求解兩臺(tái)RGB-D相機(jī)C1和C2配準(zhǔn)參數(shù),配準(zhǔn)過(guò)程如下：

(1) 執(zhí)行以上配準(zhǔn)步驟，求解C1與激光雷達(dá)(LiDAR)的配準(zhǔn)參數(shù)TLiDAR-to-C1。

(2) 求解C2與激光雷達(dá)的配準(zhǔn)參數(shù)TLiDAR-to-C2。

(3) 利用二者間的關(guān)系，可以得到C1到C2的配準(zhǔn)參數(shù)

(22)

(4) 利用所求配準(zhǔn)參數(shù)，融合點(diǎn)云。得到相機(jī)與相機(jī)的配準(zhǔn)參數(shù)，利用該結(jié)果對(duì)深度相機(jī)的點(diǎn)云進(jìn)行數(shù)據(jù)融合。如果配準(zhǔn)參數(shù)準(zhǔn)確，那么點(diǎn)云數(shù)據(jù)可以很好地對(duì)準(zhǔn)；如果配準(zhǔn)參數(shù)不夠準(zhǔn)確，融合結(jié)果將會(huì)出現(xiàn)較大偏差。

4 試驗(yàn)與結(jié)果分析

利用Velodyne VLP16激光雷達(dá)采集的點(diǎn)云數(shù)據(jù)和KinectV2深度相機(jī)獲取的點(diǎn)云數(shù)據(jù)來(lái)測(cè)試方法的可行性與準(zhǔn)確性。相機(jī)與激光雷達(dá)的點(diǎn)云數(shù)據(jù)如圖3和圖4所示。

圖3 相機(jī)點(diǎn)云

圖4 激光雷達(dá)點(diǎn)云

首先，驗(yàn)證本文配準(zhǔn)方法的可行性，在激光雷達(dá)與相機(jī)采集數(shù)據(jù)時(shí)，確保激光雷達(dá)與相機(jī)的位置固定。為減少噪聲數(shù)據(jù)對(duì)試驗(yàn)結(jié)果的影響，提高試驗(yàn)結(jié)果的精度，固定傳感器位置采集多組數(shù)據(jù)求解多組配準(zhǔn)參數(shù)，以配準(zhǔn)參數(shù)的平均值作為最終的配準(zhǔn)參數(shù)結(jié)果，即

(23)

改變激光雷達(dá)與相機(jī)的位置，重復(fù)以上試驗(yàn)步驟求解配準(zhǔn)參數(shù)，結(jié)果如圖5所示。

圖5 平移參數(shù)

由圖5可以看出，平移參數(shù)的的變化率隨著采集次數(shù)的增加而不斷減小，最終達(dá)到一個(gè)穩(wěn)定的值。當(dāng)采集次數(shù)小于15次時(shí)，平移參數(shù)的變化率減小幅度較大。當(dāng)采集次數(shù)大于15次時(shí)，平移參數(shù)仍有波動(dòng)，但幅度很小。當(dāng)采集次數(shù)大于20次時(shí)，平移參數(shù)處于一個(gè)穩(wěn)定的值，將該值作為平移參數(shù)的準(zhǔn)確值，作為最終結(jié)果。配準(zhǔn)參數(shù)最終結(jié)果見(jiàn)表3和表4。

表3 位置1配準(zhǔn)參數(shù)結(jié)果

表4 位置2配準(zhǔn)參數(shù)結(jié)果

為了檢驗(yàn)方法的可行性，利用卷尺人工測(cè)量激光雷達(dá)與相機(jī)的平移參數(shù)作為概略值；由于旋轉(zhuǎn)參數(shù)很難人工測(cè)量，本文未測(cè)量旋轉(zhuǎn)參數(shù)概略值。將試驗(yàn)結(jié)果與人工測(cè)量結(jié)果進(jìn)行比較。由表3、表4可知，試驗(yàn)結(jié)果與人工測(cè)量結(jié)果的誤差在2 cm以內(nèi)，由此可知該方法是可行的；由于人工測(cè)量的參數(shù)值只是概略值，因此2 cm并不是試驗(yàn)結(jié)果的最終精度。

為了進(jìn)一步驗(yàn)證該方法所求配準(zhǔn)參數(shù)的準(zhǔn)確性，用本文方法求解相機(jī)與相機(jī)的配準(zhǔn)參數(shù)，將所求配準(zhǔn)結(jié)果用于相機(jī)點(diǎn)云數(shù)據(jù)融合，點(diǎn)云融合前后分別如圖6和圖7所示。

圖6 點(diǎn)云數(shù)據(jù)融合前

圖7 點(diǎn)云數(shù)據(jù)融合后

由圖6、圖7可知，點(diǎn)云數(shù)據(jù)融合前，兩組點(diǎn)云數(shù)據(jù)存在明顯的偏差；利用所求配準(zhǔn)參數(shù)對(duì)兩組點(diǎn)云進(jìn)行數(shù)據(jù)融合之后，點(diǎn)云數(shù)據(jù)得到很好的對(duì)準(zhǔn)，幾乎沒(méi)有偏差。這說(shuō)明本文方法所求的配準(zhǔn)參數(shù)具有很好的準(zhǔn)確性。

5 結(jié) 語(yǔ)

本文對(duì)無(wú)人駕駛技術(shù)中的激光雷達(dá)與相機(jī)的配準(zhǔn)問(wèn)題進(jìn)行了研究，提出了基于3D特征點(diǎn)集的配準(zhǔn)方法。在完成相機(jī)內(nèi)標(biāo)定和鏡頭外標(biāo)定之后，利用ArUco標(biāo)簽提供的坐標(biāo)系轉(zhuǎn)換關(guān)系和RANSAC算法擬合標(biāo)志邊緣提取角點(diǎn)，獲得角點(diǎn)在兩傳感器坐標(biāo)系下的坐標(biāo)，從而建立兩組對(duì)應(yīng)特征角點(diǎn)的關(guān)系，最后基于Kabsch算法求解最優(yōu)配準(zhǔn)參數(shù)。試驗(yàn)中，將試驗(yàn)結(jié)果與人工測(cè)量結(jié)果進(jìn)行對(duì)比證明了方法的可行性；通過(guò)點(diǎn)云數(shù)據(jù)融合的質(zhì)量證明了方法的準(zhǔn)確性。結(jié)果表明本文方法可以很好地實(shí)現(xiàn)激光雷達(dá)與相機(jī)的配準(zhǔn)。