基于三維標(biāo)定板的相機-激光雷達聯(lián)合標(biāo)定方法

王 慶，譚镕軒，馮悠揚，嚴(yán) 超，孫 楊

（東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

相機和激光雷達是同時定位與建圖（Simultaneous Localization and Mapping,SLAM）領(lǐng)域最具代表性的兩類傳感器?；谙鄼C的視覺SLAM 能夠利用圖像中豐富的紋理信息，但是容易受到光線的干擾[1]；基于激光雷達的激光SLAM 能夠直接獲取測量點的深度，但是在走廊、隧道等缺少形態(tài)信息的場景易出現(xiàn)退化和失效的問題[2]。相機與激光雷達聯(lián)合感知可以有效降低成本，同時彌補各自的不足，而實現(xiàn)相機與激光雷達數(shù)據(jù)融合的前提是標(biāo)定出相機的內(nèi)參和相機與激光雷達的外參。

針對相機與激光雷達的聯(lián)合標(biāo)定算法有很多，但大多只關(guān)注相機與激光雷達的外參標(biāo)定，相機內(nèi)參需要提前標(biāo)定好。主流的相機標(biāo)定算法[3]通過平面標(biāo)志物進行標(biāo)定，陳文藝等[4]使用雙神經(jīng)網(wǎng)絡(luò)獲取棋盤格角點從世界坐標(biāo)系到相機坐標(biāo)系的映射關(guān)系，得到比傳統(tǒng)方法更高的標(biāo)定精度。王靖等[5]考慮到圓形陣列板存在的邊緣模糊和偏心問題，改進Zernike 矩和偏心誤差修正獲取更精確的圓心坐標(biāo)，提高了標(biāo)定精度。類似的研究還有很多，但大多算法采用的都是平面標(biāo)志物，在標(biāo)定過程中失去了一個維度的約束，容易出現(xiàn)過擬合問題[6]。相機與激光雷達外參標(biāo)定方法主要通過在圖像和點云中提取對應(yīng)特征，構(gòu)建約束條件求解外參。Zhou 等[7]通過檢測棋盤格在圖像和點云中的邊緣，利用直線與平面對應(yīng)的方式求解外參。Wang等[8]根據(jù)棋盤格的點云反射強度提取特征點，采用未校準(zhǔn)透視n 點(Uncalibrated Perspective-n-Point,UPnP)算法獲取外參。Pusztai 等[9]使用三面體進行標(biāo)定，通過檢測標(biāo)定板頂點建立點到點的約束獲得高精度外參，但該算法需要人工剔除背景點云并人工標(biāo)注圖像角點。Toth 等[10]通過從點云和圖像中計算球體標(biāo)志物中心，并用球體輪廓進行匹配得到外參。

除此之外，還有很多熱門的相機與激光雷達開源標(biāo)定工具。lidar_camera_calibration[11]將ArUco 標(biāo)記粘貼到兩個平面矩形板上，能夠自動標(biāo)定相機與激光雷達的外參，但需要人為地篩選出標(biāo)定板邊緣點。Autoware[12,13]使用平面棋盤格標(biāo)定板能夠同時標(biāo)定相機內(nèi)參和相機與激光雷達外參，但需要人為地選定點云特征，得到的結(jié)果穩(wěn)定性差。velo2cam_calibration[14]設(shè)計了一種包含四個ArUco 圖案和四個圓孔的平面標(biāo)定板，通過提取圓心坐標(biāo)構(gòu)建點和點對應(yīng)的約束求解外參。

上述外參標(biāo)定算法需要在點云中提取點和線特征，而主流的16 線激光雷達分辨率低，提取的點和線特征精度較低，因此這些算法對低線束激光雷達的標(biāo)定效果較差。且大部分聯(lián)合標(biāo)定算法只關(guān)注相機與激光雷達外參標(biāo)定，相機的內(nèi)參需要提前標(biāo)定好，標(biāo)定過程中存在大量人工操作，標(biāo)定步驟較為復(fù)雜。

本文針對現(xiàn)有標(biāo)定算法存在的問題，設(shè)計一種由三個棋盤格制作的三維標(biāo)定板，能夠?qū)崿F(xiàn)相機內(nèi)參和相機與激光雷達外參的聯(lián)合標(biāo)定。相比平面標(biāo)定方法，三維標(biāo)定方法在內(nèi)參標(biāo)定過程中增加一個維度的約束，可有效提高相機內(nèi)參標(biāo)定的精度和穩(wěn)定性。使用圖像和點云中的平面特征構(gòu)建約束方程，在低線束激光雷達中也能提取出準(zhǔn)確的平面特征，只需在算法初始化階段從激光點云中選取三個種子點，即可自動獲取高精度的相機內(nèi)參和相機與激光雷達外參。

1 標(biāo)定原理

相機與激光雷達的聯(lián)合標(biāo)定就是估計相機內(nèi)參矩陣K和相機與激光雷達之間的剛體變換矩陣Tcl的過程。主流相機內(nèi)參標(biāo)定方法通過平面棋盤格的角點=(Xi,Yi,Zi,1)T和相機像素坐標(biāo)系下的對應(yīng)點=(ui,vi,1)T建立一一對應(yīng)關(guān)系：

其中，fx和fy是相機焦距，cx和cy為相機主點坐標(biāo)，Tcw為相機在世界坐標(biāo)系下的位姿矩陣，Rcw為位姿Tcw中的旋轉(zhuǎn)矩陣，tcw為位姿Tcw中的平移矩陣，s為點在相機坐標(biāo)系下的深度。

使用至少四組數(shù)據(jù)即可估計相機內(nèi)參K，相機標(biāo)定通過重投影誤差計算標(biāo)定精度，計算公式如下：

其中，|||·||2是向量的2 范數(shù)。

常用的平面標(biāo)定方法通過計算與擬合目標(biāo)(平面標(biāo)志物)的重投影誤差來證明其方法的有效性，但對于整個場景的擬合效果較差，存在對標(biāo)志物過擬合的現(xiàn)象，而三維標(biāo)志物能夠有效克服過擬合問題，因此本文通過三維標(biāo)定板進行相機內(nèi)參標(biāo)定。

在已知相機內(nèi)參K的前提下可以通過透視n點(Perspective-n-Point,PnP)算法獲取外參。根據(jù)Geiger等[15]的工作，外參標(biāo)定性能測試可通過線性平移誤差et和角度誤差er進行評估：

其中，Rgt和tgt分別為旋轉(zhuǎn)矩陣和平移矩陣的真值，tr(·)為矩陣的跡。

由于低線束激光雷達的點線特征提取精度差，導(dǎo)致現(xiàn)有算法在低線束激光雷達的標(biāo)定效果較差。本文采用三維標(biāo)定板，通過提取平面特征，構(gòu)建平面對應(yīng)的約束方程計算外參，從而使得相機與低線束激光雷達也能獲取精確的外參。

2 基于三維標(biāo)定板的聯(lián)合標(biāo)定算法

本文設(shè)計的三維標(biāo)定板如圖1 所示，標(biāo)定板三個面相互垂直，每個面放置一個棋盤格圖案，事先使用全站儀測量得到棋盤格角點的三維坐標(biāo)。

圖1 三維標(biāo)定板Fig.1 3D calibration plate

2.1 相機內(nèi)參標(biāo)定

令P=KTcw，式(1)可以轉(zhuǎn)換為：

通過式(6)能構(gòu)造如下方程：

對式(7)使用奇異值分解（Singular Value Decomposition,SVD）得到投影矩陣P的解。

投影矩陣可以分解為P=，對矩陣H進行正交三角分解（Orthogonal Triangular Decomposition）即可得到矩陣K和矩陣Rcw，內(nèi)參矩陣K最后一個元素值為1，令K=K/K33進行歸一化，最后計算平移矩陣tcw=K-1h。

上述方法得到的解沒有考慮到相機畸變的影響，只能作為初值，通過列文伯格-馬夸爾特(Levenberg-Marquardt,LM)算法同時優(yōu)化相機每一幀的位姿Tcw以及內(nèi)參矩陣K和畸變系數(shù)k1、k2，構(gòu)造代價函數(shù)如下：

2.2 相機-激光外參標(biāo)定

2.2.1 圖像平面提取

假設(shè)平面在三維直角坐標(biāo)系中的方程為nxx+nyy+nzz+d=0，定義平面模型由平面的單位法向量n=(nx,ny,nz)T和平面到坐標(biāo)系原點的距離d組成，使用最小二乘法計算標(biāo)定板的三個平面在世界坐標(biāo)系下的平面模型，利用式(8)計算得到的位姿Rcw和tcw可以構(gòu)造的關(guān)系如下：

2.2.2 激光點云平面提取

激光平面提取算法中常用點的幾何曲率和法線方向作為判斷依據(jù)，但由于機械式激光雷達點云在垂直和水平方向上的分布是不均勻的，單幀點云估計出來的曲率和法線精度很低，導(dǎo)致平面提取結(jié)果較差。

由于激光雷達的水平分辨率高，在單幀點云中能夠獲取較高精度線曲率，很多激光SLAM 算法[16]通過線曲率提取點云特征。本文通過計算線曲率的方式代替幾何曲率，在第一幀激光點云中，標(biāo)注每個標(biāo)定板平面內(nèi)的中心作為種子點，通過區(qū)域生長算法擴散到平面邊緣得到待提取的平面。后續(xù)每一幀點云通過正態(tài)分布變換(Normal Distributions Transform,NDT)算法將第一幀中的種子點變換到后續(xù)幀的點云中，取距離變換后的與種子點最近的點作為后續(xù)幀的種子點，因此只需在算法初始化的階段標(biāo)注三個種子點，即可自動實現(xiàn)點云平面提取。點云平面提取步驟描述如下：

(1)如果是第一幀，按照固定順序在標(biāo)定板每個平面選擇一個種子點。如果是后續(xù)幀，根據(jù)當(dāng)前幀的相對位姿，將第一幀的種子點轉(zhuǎn)換到當(dāng)前幀中，取最近的一個點作為當(dāng)前幀的種子點；

(2)將點云按線束劃分為Lk(k=1,2...n)，對每個線束Lk的點云按照角度進行排序；

(3)從種子點X(k,seed)開始計算當(dāng)前線束Lk中點的曲率c，計算公式如下：

其中，X(k,i)表示線束Lk中要計算的當(dāng)前點，X(k,j)表示X(k,i)的近鄰點，S表示線束Lk中點X(k,i)前后5 個點組成的點集，kS表示點集S的數(shù)量。

如果當(dāng)前點的曲率c小于閾值，則認(rèn)為是平面點，繼續(xù)計算周圍點的曲率；如果當(dāng)前點的曲率c大于閾值，則認(rèn)為已經(jīng)擴散到邊緣。當(dāng)前線束提取完畢后，搜索線束Lk-1和Lk+1中距離種子點X(k,seed)最近的點,如果與種子點X(k,seed)的距離小于閾值，則認(rèn)為該線束中存在平面點，將最近點作為起始點計算線曲率，通過這種方式判斷每個線束是否有平面點，得到完整的平面提取結(jié)果。最后對提取的平面點云采用隨機抽樣一致性(Random Sample Consensus,RANSAC)算法剔除離群點，得到精確的平面模型。

重復(fù)步驟(1)-(3)直到處理完所有數(shù)據(jù)，得到每一幀的平面提取結(jié)果。

2.2.3 外參估計

將式(12)中的誤差項展開：

式(13)中第一項不含旋轉(zhuǎn)矩陣Rcl，第二項中=I，實際待優(yōu)化的誤差項為：

根據(jù)式(14)可以定義矩陣W為：

對矩陣W進行SVD 分解：

其中，U和V為矩陣W分解得到的酉矩陣，∑為矩陣W分解得到的對角矩陣。

旋轉(zhuǎn)矩陣的最優(yōu)估計值為Rcl=UVT。對于平移矩陣tcl構(gòu)建如下方程：

對式(17)構(gòu)成的超定方程組進行SVD 分解即可得到平移矩陣的最小二乘解，將計算得到的外參初值再帶入到LM 算法中進行優(yōu)化得到高精度的外參。

3 實驗結(jié)果與分析

為了測試標(biāo)定方法的穩(wěn)定性與精度，用小覓相機和VLP-16 激光雷達搭建實驗平臺，基于ROS 操作系統(tǒng)在Linux 平臺上實行，進行相機與激光雷達聯(lián)合標(biāo)定實驗。小覓相機的分辨率為752×480，相機基線距離為12 cm，內(nèi)參值為fx=368.03像素，fy=368.12像素，cx=377.36像素，cy=247.23像素。VLP-16 激光雷達的測距范圍為50 m，測距精度為2 cm，棋盤格尺寸設(shè)置為420 mm×297 mm，棋盤格內(nèi)角點的世界坐標(biāo)通過NTS-340R6A 型號全站儀精確測量，全站儀水平角、垂直角置零后，將全站儀坐標(biāo)系作為世界坐標(biāo)系，傳感器的坐標(biāo)系建立如圖2 所示。

圖2 聯(lián)合標(biāo)定實驗平臺Fig.2 Joint calibration experimental platform

3.1 相機內(nèi)參標(biāo)定實驗

實驗過程中，在三維標(biāo)定板內(nèi)隨機取一半的特征點采用三維標(biāo)定方法進行內(nèi)參標(biāo)定，參與標(biāo)定的特征點計算內(nèi)符合重投影誤差，量化標(biāo)定方法的擬合精度，未參與標(biāo)定的點計算外符合重投影誤差，量化標(biāo)定方法對整個場景的擬合精度。隨機選定三維標(biāo)定板中的一個棋盤格圖案，采用平面標(biāo)定方法進行內(nèi)參標(biāo)定，并計算內(nèi)符合重投影誤差，未參與標(biāo)定的棋盤格角點計算外符合重投影誤差，每組數(shù)據(jù)采集20 張圖像。10組實測數(shù)據(jù)標(biāo)定結(jié)果的平均值與真值進行對比，計算內(nèi)參標(biāo)定誤差，表1 統(tǒng)計了10 組實測數(shù)據(jù)計算得到的平均重投影誤差和相機內(nèi)參誤差。

表1 內(nèi)參標(biāo)定實驗誤差Tab.1 Error of intrinsic calibration experiments

由表1 可知，相機焦距fx、fy和相機主點坐標(biāo)cx、cy的三維標(biāo)定方法精度要優(yōu)于平面標(biāo)定方法。

從重投影誤差來看，三維標(biāo)定方法的內(nèi)符合誤差大于平面標(biāo)定方法，但外符合重投影誤差要遠小于平面標(biāo)定方法。三維標(biāo)定方法的內(nèi)符合誤差和外符合誤差在同一水平，說明三維標(biāo)定方法能夠有效克服過擬合問題。而平面標(biāo)定方法的外符合誤差和內(nèi)符合誤差差異較大，說明平面標(biāo)定方法會對標(biāo)定數(shù)據(jù)過擬合，從而導(dǎo)致對整個場景的擬合效果較差，因此采用三維標(biāo)定方法能得到更精確的相機內(nèi)參。圖3 為采用平面標(biāo)定方法與三維標(biāo)定方法校正后的圖像，其中平面標(biāo)定方法校正后的圖像中門框存在彎曲的部分，而三維標(biāo)定方法的畸變校正結(jié)果明顯優(yōu)于平面標(biāo)定方法的校正結(jié)果。

圖3 相機校準(zhǔn)結(jié)果Fig.3 Camera calibration results

3.2 相機-激光雷達外參標(biāo)定實驗

為了驗證點云平面提取算法的性能，對區(qū)域生長算法和本文的方法進行測試，并通過Polyworks 工具人工提取平面點云進行對比。圖4 為兩種點云平面提取方法以及人工提取的結(jié)果，平面提取結(jié)果通過不同顏色進行標(biāo)注。

如圖4(a)所示，設(shè)置計算法向量和曲率的近鄰點數(shù)為kn=10時，由于點云在空間上分布不均勻，區(qū)域生長算法計算的曲率和法線精度較低，區(qū)域生長算法在單幀激光點云中無法提取正確的平面點云。設(shè)置kn=50時，如圖4(b)所示，區(qū)域生長算法能夠得到平面提取結(jié)果，但三個標(biāo)定板的連接處的點云沒有被提取。由圖4(d)可以看出，設(shè)置kn=10時，除了少數(shù)邊緣點和離群點沒有提取，本文方法能夠獲取穩(wěn)定的提取結(jié)果。將圖4(c)中Polyworks 獲取的標(biāo)定板點云作為真值，區(qū)域生長算法(kn=50)的召回率為87.97%，但kn設(shè)置越大，參與計算的點云數(shù)量越多，算法會更耗時，而本文方法在kn=10的條件下即可將召回率提升為98.77%，能夠獲取準(zhǔn)確的標(biāo)定板平面點云，且不需要在所有平面分割結(jié)果中搜索和匹配對應(yīng)平面特征，耗時更短。

圖4 點云平面提取效果Fig.4 Point cloud plane extraction effect

由于真實世界無法準(zhǔn)確地獲取相機與激光雷達的外參，本文在GAZEBO 仿真平臺進行對比測試，基于實測數(shù)據(jù)對本文的方法進行驗證。仿真平臺搭載Bumblebee xb3 相機和VLP-16 激光雷達，兩者的噪聲符合高斯模型ε～，相機的像素強度誤差為=0.007m，激光雷達的測距誤差為=0.008m，相機與激光雷達之間的平移量設(shè)置為tx=0m，ty=-0.29m，tz=-0.15m，旋轉(zhuǎn)角設(shè)置為θroll=0 rad，θpitch=0.1rad，θyaw=0 rad。

采集了10組仿真數(shù)據(jù)進行相機與激光雷達外參標(biāo)定的精度測試。表2 統(tǒng)計了本文方法與Velo2cam_calibration算法的平均平移誤差et和平均角度誤差er。在仿真實驗中，本文方法的平均平移誤差et和平均角度誤差er均小于Velo2cam_calibration，說明本文的標(biāo)定方法對平移矩陣和旋轉(zhuǎn)矩陣的估計精度要優(yōu)于Velo2cam_calibration。

表2 仿真測試結(jié)果Tab.2 Simulation test results

在實際測試中，根據(jù)黃強等[17]的工作，引入相機左目到右目的外參作為真值，分別計算相機左目與激光雷達的外參以及相機右目與激光雷達的外參，從而得到相機右目到左目的外參估計值：

將式(18)得到的結(jié)果與雙目相機外參真值進行對比來評估標(biāo)定算法在實測數(shù)據(jù)中的性能。通過10 組數(shù)據(jù)計算得到的平移誤差et和角度誤差er如表3 所示。

表3 實測數(shù)據(jù)結(jié)果Tab.3 Measured data results

由表3 可知，在實測數(shù)據(jù)中，本文方法的平均標(biāo)定誤差為et=1.63cm 和er=0.022rad,相比于Velo2cam_calibration 算法，外參標(biāo)定的平均平移誤差減小了39.4%，平均旋轉(zhuǎn)誤差減小了37.1%。

為了驗證在現(xiàn)實世界中的有效性，將激光雷達點云投影到圖像上，并通過激光點云與圖像數(shù)據(jù)融合進行三維重建。圖5 繪制了停車場環(huán)境下點云投影圖，并對局部區(qū)域進行了特寫處理，圖6 繪制了運用多幀點云和圖像紋理融合得到的彩色點云圖。

圖5 點云投影效果Fig.5 Point cloud projection effect

圖6 點云重建效果Fig.6 Point cloud reconstruction effect

從圖5 和圖6 可以看出，激光點云與圖像中的物體邊緣能夠?qū)R，重建的彩色點云圖色彩紋理與點云的形狀吻合，本文提出的方法具備實用價值。

4 結(jié)論

本文針對平面標(biāo)定方法存在的過擬合問題，通過三維標(biāo)定板校準(zhǔn)相機內(nèi)參。針對單幀激光點云提取效果差的問題，設(shè)計了一種基于線曲率的平面提取方法；最后通過圖像與激光點云中對應(yīng)的平面特征一一對應(yīng)的方式計算外參，提高相機與低線束激光雷達的標(biāo)定精度。實驗結(jié)果表明：基于三維標(biāo)定板的相機內(nèi)參標(biāo)定相比平面標(biāo)定板更精確。同時，通過仿真和實測數(shù)據(jù)進行實驗，本文采用的外參標(biāo)定方法可有效提高相機與低線束激光雷達的標(biāo)定精度，與velo2cam_calibration 算法相比，外參標(biāo)定的平均平移誤差減小了39.4%，平均旋轉(zhuǎn)誤差減小了37.1%。但目前本文方法的外參標(biāo)定精度受限于圖像的平面特征提取精度，后續(xù)還要對圖像的平面特征提取方法進行優(yōu)化。