一種基于三維特征點集的激光雷達與相機配準方法

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

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

當前汽車制造產業正朝著電動化、智能化和互聯網化方向不斷發展。無人駕駛汽車是通過車載的相機、激光雷達和IMU(inertial measurement unit)等傳感器采集周圍環境信息，利用數據融合技術實現障礙物認知、環境認知和車輛認知等并實現三維環境重構，進而自動規劃行車路線并控制車輛到達預定目標[1]。在無人駕駛汽車研究領域，環境感知是該領域最基礎的技術之一。沒有對周圍環境的感知，車輛就無法實現自主決策和控制[2]。隨著傳感器技術及無人駕駛技術的不斷發展，激光雷達與相機已經成為了一組不可或缺的傳感器組合。

激光雷達通過向周圍環境發射激光束，然后接收從目標反射的回波，作適當處理后，可獲得目標的距離、方位、姿態和形狀等結構信息，但無法獲得周圍目標的色彩信息。相機可以獲得豐富的色彩和特征信息，可用于對行人、樹木等特征目標的檢測，但易受環境因素的干擾。因此，激光雷達與相機在功能上具有良好的互補性。在無人駕駛領域，將激光雷達與相機獲得的點云和圖像數據進行融合處理，實現對周圍環境目標的感知，使汽車自主避開障礙，防止事故的發生。而在數據融合過程中，激光雷達與相機的配準是影響數據融合效果的關鍵環節。

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

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

1 傳感器與標志

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

1.1 相機標定

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

表1 相機內參數標定結果

表2 兩鏡頭外參數標定結果

1.2 標志設置

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

2 特征點提取

為了求解相機坐標系與激光雷達坐標系間的配準參數，需要兩組對應的3D特征點集：一組在相機坐標系下，另一組在激光雷達坐標系下。一旦找到了這兩組對應的三維特征點，就能利用算法求解兩傳感器間的配準參數。試驗數據采集場景如圖1所示。

圖1 試驗設置

2.1 基于相機的特征點提取

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

圖2 相機識別ArUco標簽

2.2 基于激光雷達的特征點提取

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

RANSAC算法基本假設是，數據集中包含局內點，也包含局外點，采用不斷迭代的方法，尋找最優模型，并剔除不符合最優模型的局外點，構建一個僅由局內點組成的數據子集。其基本步驟如下[16]：

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

di<ε且σi<α

(1)

式中，ε為閾值距離；α夾角閾值。將滿足條件的點的個數，作為Lt的分數SLt。

(4) 重復執行步驟(1)～步驟(3)T次，選擇出得分最高的平面Lt*。其中T由下式決定

(2)

式中，τ為Lt*局外點所占比例的估計值；Φ為T次采樣后最優平面被選中的概率。

(5) 記錄Lt*并在P中移除計入SLt*的點。

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

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

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

dj<ξ

(3)

式中，ξ為點到直線距離閾值。將滿足條件的點的個數作為Im的分數SIm。

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

(4)

(11) 記錄Im*并在Q中移除計入SIm*的點。

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

3 配準參數求解

在得到兩組對應的三維特征點之后，相機坐標系與激光雷達坐標系間的轉換參數可以利用Kabsch算法求解得到。同時為驗證本文提出的配準方法的準確性，用本文的方法對相機與相機配準參數進行求解。

3.1 基于Kabsch算法求解配準參數

Kabsch算法用于計算兩個待配準點集最優旋轉矩陣，利用該方法求解兩點集的最優矩陣對點集旋轉后可以使均方根偏差(RMSD)最小，在旋轉已知的情況下，兩坐標系的平移參數也可以確定。設P和Q是兩個待配準點云，算法過程如下

(5)

(6)

(7)

(8)

(9)

將式(9)代入目標函數式(5)得

(10)

(11)

由矩陣跡的性質，式(11)試可簡化為

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

(12)

2tr(YTX′)

(13)

消去不包含R的項，得到新的目標函數

(14)

由X′=RX和跡的性質

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

(15)

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

(16)

令M=VTRU則有

(17)

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

M=I?VTRU=I?R=VUT

(18)

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

tr(YTX′)=d1M11+d2M22+d3M33

(19)

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

當式(19)有第二大值時M11=M22=1，M33=-1，綜上則最優矩陣為

R=UCVT

(20)

式中，C為校正矩陣

(21)

3.2 相機與相機的配準

本文的主要目的是獲得激光雷達與相機準確的配準參數，但由于旋轉參數在實際試驗過程中測量不便，為了驗證本文方法解算的配準參數的準確性，利用文中的配準方法，求解兩臺RGB-D相機C1和C2配準參數,配準過程如下：

(1) 執行以上配準步驟，求解C1與激光雷達(LiDAR)的配準參數TLiDAR-to-C1。

(2) 求解C2與激光雷達的配準參數TLiDAR-to-C2。

(3) 利用二者間的關系，可以得到C1到C2的配準參數

(22)

(4) 利用所求配準參數，融合點云。得到相機與相機的配準參數，利用該結果對深度相機的點云進行數據融合。如果配準參數準確，那么點云數據可以很好地對準；如果配準參數不夠準確，融合結果將會出現較大偏差。

4 試驗與結果分析

利用Velodyne VLP16激光雷達采集的點云數據和KinectV2深度相機獲取的點云數據來測試方法的可行性與準確性。相機與激光雷達的點云數據如圖3和圖4所示。

圖3 相機點云

圖4 激光雷達點云

首先，驗證本文配準方法的可行性，在激光雷達與相機采集數據時，確保激光雷達與相機的位置固定。為減少噪聲數據對試驗結果的影響，提高試驗結果的精度，固定傳感器位置采集多組數據求解多組配準參數，以配準參數的平均值作為最終的配準參數結果，即

(23)

改變激光雷達與相機的位置，重復以上試驗步驟求解配準參數，結果如圖5所示。

圖5 平移參數

由圖5可以看出，平移參數的的變化率隨著采集次數的增加而不斷減小，最終達到一個穩定的值。當采集次數小于15次時，平移參數的變化率減小幅度較大。當采集次數大于15次時，平移參數仍有波動，但幅度很小。當采集次數大于20次時，平移參數處于一個穩定的值，將該值作為平移參數的準確值，作為最終結果。配準參數最終結果見表3和表4。

表3 位置1配準參數結果

表4 位置2配準參數結果

為了檢驗方法的可行性，利用卷尺人工測量激光雷達與相機的平移參數作為概略值；由于旋轉參數很難人工測量，本文未測量旋轉參數概略值。將試驗結果與人工測量結果進行比較。由表3、表4可知，試驗結果與人工測量結果的誤差在2 cm以內，由此可知該方法是可行的；由于人工測量的參數值只是概略值，因此2 cm并不是試驗結果的最終精度。

為了進一步驗證該方法所求配準參數的準確性，用本文方法求解相機與相機的配準參數，將所求配準結果用于相機點云數據融合，點云融合前后分別如圖6和圖7所示。

圖6 點云數據融合前

圖7 點云數據融合后

由圖6、圖7可知，點云數據融合前，兩組點云數據存在明顯的偏差；利用所求配準參數對兩組點云進行數據融合之后，點云數據得到很好的對準，幾乎沒有偏差。這說明本文方法所求的配準參數具有很好的準確性。

5 結 語

本文對無人駕駛技術中的激光雷達與相機的配準問題進行了研究，提出了基于3D特征點集的配準方法。在完成相機內標定和鏡頭外標定之后，利用ArUco標簽提供的坐標系轉換關系和RANSAC算法擬合標志邊緣提取角點，獲得角點在兩傳感器坐標系下的坐標，從而建立兩組對應特征角點的關系，最后基于Kabsch算法求解最優配準參數。試驗中，將試驗結果與人工測量結果進行對比證明了方法的可行性；通過點云數據融合的質量證明了方法的準確性。結果表明本文方法可以很好地實現激光雷達與相機的配準。