一種三維激光雷達與雙目相機的聯合標定方法

陳凱陽，李 航，彭 程，李楊龍

（河南科技大學機電工程學院，河南洛陽 471003）

0 引言

在移動機器人、無人駕駛汽車等領域多傳感器數據融合有著廣泛應用［1-2］。與單一的傳感器系統相比，多傳感器融合系統可以提供更加精確和豐富的環境信息，完成更高級別的任務，如目標檢測、自主定位和路徑規劃等［3-5］。激光雷達傳感器與攝像機傳感器的聯合應用非常普遍。三維激光雷達抗干擾能力強、探測范圍廣，能準確獲取探測目標的距離和角度信息［6］。攝像機體積小、成本低，可以獲得目標的形狀和顏色信息。兩種傳感器在功能上可以互補［7］。但在多傳感器系統中，激光雷達與攝像機安裝位置不同，獲得的傳感器測量信息無法匹配，空間一致性差。因此，為了得到準確可靠的點云和圖像數據，在多傳感器數據融合之前必須完成空間坐標系的標定。

三維激光雷達和雙目相機聯合標定，實質上是獲取激光雷達坐標系與圖像像素坐標系位姿的相互轉換關系。通過該方法，可以將兩個傳感器檢測到的空間信息進行關聯與融合。Zhang 等［8］提出一種基于單線激光雷達與攝像機的標定方法，通過對標定板多角度檢測，以標定板的平面法向量建立約束關系，通過非線性的優化方法來求解標定參數。雖然該方法實現了單線激光雷達與攝像機的標定，但步驟繁瑣，計算量大；Vasconcelos 等［9］和Zhou［10］使用多角度下標定板的數據構建三點透視（P3P）問題，構造出虛擬的三面體，減少標定板的檢測次數，但存在多重解問題；Park 等［11］使用白色菱形板作為標定板，以菱形板的頂點作為特征點在32 線激光雷達中擬合出特征點位置。但該方法局限性大，不適用于掃描層數較少的激光雷達傳感器；賈子永等［12］和劉振等［13］使用一塊梯形標定板進行聯合標定，該標定板組合了等腰直角三角板和正方形棋盤格標定板，雖然只需要采集一次數據就可以完成聯合標定，但其標定板加工難度大；Tian 等［14］使用棋盤格三面體對相機和2D 激光測距儀分別進行標定，利用三點建立約束方程求解傳感器之間的位置關系，精度較高但是需要大量的仿真和實驗。綜上，為了降低聯合標定難度，需要研究一種更有效的方法。

本文提出一種基于空間圓心的聯合標定方法。通過使用棋盤格標定板，得到雙目相機的內外參數；設計一種圓孔標定板，以標定板中的圓心作為特征點，建立三維激光雷達坐標系與圖像像素坐標系之間的約束關系，計算并優化兩個坐標系之間的旋轉平移矩陣，實現三維激光雷達和雙目相機的聯合標定。該方法計算速度快、測量精度高，提高了聯合標定的準確性。將三維激光雷達和雙目相機傳感器搭建的聯合標定裝置進行實驗，得到合理的聯合標定參數和測量結果。

1 聯合標定原理

1.1 相關坐標系

以激光雷達與攝像機的聯合標定為例，聯合標定過程存在4 種坐標系［15］：世界坐標系、激光雷達坐標系、攝像機坐標系和圖像坐標系，如圖1 所示。

Fig.1 Relationship of each coordinate system圖1 各坐標系關系

（1）世界坐標系OW-XYZ。是由用戶定義的三維空間坐標系，主要用來描述相機的位置。在Zhang［16］標定法中，以棋盤格標定板中第一個棋盤格的頂點作為世界坐標系的坐標。

（2）激光雷達坐標系OL-XLYLZL。其原點OL位于激光雷達的掃描中心，空間中任意一點P在OL-XLYLZL中表示為（XL，YL，ZL）。

（3）攝像機坐標系OC-XCYCZC。以攝像機的光學中心為坐標原點，ZC軸與攝像機光軸重合，且以攝像的方向為ZC軸正向，攝像機的焦距為f。在OC-XCYCZC中P點的坐標表示為（XC，YC，ZC）。

（4）圖像坐標系。一般分為圖像物理坐標系和圖像像素坐標系［17］。圖像物理坐標系OC′-XC′YC′是以攝像機光軸與成像平面的交點OC′為坐標原點的直角坐標系（一般以實際物理尺度為單位，如mm），其中XC′軸、YC′軸分別與圖像像素坐標系的U、V軸平行，圖像像素坐標系O-UV的坐標原點為圖像的左上角，以像素為單位［18］，U、V分別表示每個像素在數字圖像中的列數和行數。

1.2 坐標系之間的轉換關系

（1）攝像機坐標系與激光雷達坐標系關系。激光雷達坐標系與攝像機坐標系之間的位姿關系可以分解為繞坐標原點OC的旋轉和平移，計算公式如下：

其中，R為旋轉矩陣，代表兩個坐標系之間的角位移關系；α、β和γ分別為激光雷達坐標系在攝像機坐標系中沿XC、YC和ZC軸方向的偏轉角度；T是平移矩陣，表示激光雷達坐標系原點在攝像機坐標系中的坐標，代表兩個坐標系之間的相對位置關系［19］。

（2）攝像機坐標系與圖像坐標系關系。從攝像機坐標系到圖像物理坐標系，是從三維空間坐標系到二維平面坐標系的轉換。攝像機將物點P成像到像素平面上的P′點，根據三角形相似原理，P′點的物理坐標與P點（XC，YC，ZC）的關系如式（2）所示：

將公式（2）用矩陣表示：

P′點的圖像物理坐標（XC′，YC′）與像素坐標（u，v）的轉換關系如式（4）所示：

將式（4）用矩陣表示：

其中，dx、dy、r、u0和v0為攝像機的內部參數；dx、dy分別為攝像機的單個像素在XC′和YC′方向上的物理尺寸；r是傾斜因子，在一般標準相機中為0；（u0，v0）是圖像主點坐標，即攝像機光軸與物理成像平面相交點的像素坐標［20］。

由式（3）和式（5）可得出攝像機坐標系與圖像像素坐標系之間的轉換關系：

其中，fx=f/dx和fy=f/dy分別表示軸和軸方向上的等效焦距［21］。

（3）激光雷達坐標系與圖像像素坐標系的關系。由式（1）和式（6）可以得到激光雷達坐標系與圖像像素坐標系的轉換關系為：

由以上公式可以得出，聯合標定中的未知量為攝像機的內部參數、激光雷達坐標系與攝像機坐標系之間的旋轉矩陣R和平移矩陣T。

2 激光雷達和雙目相機聯合標定算法

本文使用的激光雷達和雙目相機聯合標定裝置如圖2所示。因為存在加工和安裝誤差，旋轉矩陣R和平移矩陣T的真實值與設計值相差較大，因此需要更加準確的結果來進行傳感器之間的數據融合。

Fig.2 Joint calibration device of 3D lidar and stereo camera圖2 三維激光雷達與雙目相機的聯合標定裝置

2.1 聯合標定方法設計

聯合標定實驗中使用的標定板如圖3 所示。首先，使用圖3（a）中的棋盤格標定板對雙目相機進行標定，得到雙目相機的內外參數；然后，制作一個如圖3（b）所示的圓孔標定板，以標定板上圓孔的圓心作為特征點，雙目相機和激光雷達分別檢測標定板上圓孔的位置，計算激光雷達坐標系與攝像機坐標系之間的旋轉矩陣R和平移矩陣T，完成聯合標定。

Fig.3 Calibration board used to calibrate internal parameters（a）and external parameters（b）圖3 用于標定內部參數（a）和外部參數（b）的標定板

2.2 聯合標定流程

聯合標定步驟如下：①首先對雙目相機進行標定，獲取雙目相機的內外參數；②雙目相機采集圓孔標定板圖像，提取二維圖像中圓孔的圓心坐標ci(ui,vi)以及半徑ri(i=1,2,3,4)；③激光雷達掃描圓孔標定板，獲取點云中圓心坐標以及半徑Ri；④將標定板中的4 個圓心作為特征點，以激光雷達坐標系的圓心坐標Ci轉換到圖像坐標系的圓心坐標ci建立約束關系，計算激光雷達坐標系到攝像機坐標系的旋轉平移矩陣。

2.2.1 粗標定估算平移矩陣T

因為激光雷達和雙目相機的安裝位置已經確定，兩個坐標系的三軸建立在同一方向上，所以首先計算平移矩陣。根據旋轉不變性將式（5）改寫為：

此時式（8）中的未知變量為平移矩陣T，通過激光雷達點云和二維圖像中檢測到的圓孔半徑和圓心坐標來估算平移矩陣T。

式（12）中N為提供的數據點對數，使用多對數據可以提高計算精度。

2.2.2 優化聯合標定參數

因為聯合標定裝置安裝有誤差，為了提高標定參數精度，在粗標定階段得到平移矩陣之后需要對旋轉矩陣進行優化。分別取激光雷達和雙目相機檢測到的特征點建立特征點集合bl和bc，式（13）為優化函數：

其中，wi>0(i=1,2…n)代表每個特征點權重，等式左側的R和T代表具有最優解的旋轉平移矩陣。將特征點中心化：

分別以中心化的特征點集xi和yi建立矩陣X和Y，進行協方差計算：

其中，W=diag（w1，w2，w3…wn）是一個對角矩陣。將協方差矩陣S進行奇異值分解（Singular Value Decomposition，SVD）得到U∑VT，式（13）中輸入的旋轉矩陣R通過式（17）得到：

3 激光雷達和雙目相機聯合標定實驗

3.1 實驗設備及環境

聯合標定實驗采用速騰聚創的三維激光雷達（RS-Li-DAR-16），該雷達為16 線激光雷達，探測范圍為20cm～150m，水平視角為360°，水平方向采樣間隔為0.36°；垂直視角為-15°～15°，垂直方向采樣間隔為2°。雙目相機采用FpgaLena 可變基線雙目相機，基線調整范圍（即雙目中心距）是2.5cm～20.5cm，采用USB3.0 通信協議，單目相機的分辨率為1 280×720。

實驗使用的棋盤格標定板型號為GP400-12×9，外形尺寸為400×300mm，棋盤格的尺寸精度為±0.01mm；圓孔標定板的外形尺寸為400×400mm，圓的半徑為50mm，加工精度為0.1mm，棋盤格標定板和圓孔標定板的精度均可達到環境感知所需要的精度。設置激光雷達和雙目相機距離標定板的距離約為1 000～2 000mm，如圖4 所示。在實驗過程中保持激光雷達和雙目相機系統的位置固定，不斷調整標定板位置，采集多組數據進行實驗。

3.2 實驗方案設計

Fig.4 Joint calibration experiment scene圖4 聯合標定實驗場景

在聯合標定實驗中，首先對雙目相機傳感器進行標定，得到雙目相機的內外參數；然后進行如圖4 所示的聯合標定實驗。調整標定板到聯合標定裝置的距離約為1 100mm、1 300mm、1 700mm，同時使用雙目相機和激光雷達依次采集3 組數據，對實驗采集到的原始數據先進行處理，得到圖像坐標系和激光雷達坐標系下圓孔中心點的坐標，再進行傳感器聯合標定求解并驗證，確保聯合標定參數的正確。聯合標定實驗過程中需保證雙目相機的硬件參數和布置方式不變，保持雙目相機與激光雷達的相對位置、光照強度等環境因素不變，確保聯合標定實驗在相同的環境下進行。

3.2.1 二維圖像中的圓檢測

相機獲取的圖像如圖5（a）所示，首先將其轉化為圖5（b）中的灰度圖像gray，使用公式（18）和公式（19）的Sobel邊緣檢測算法來創建邊緣圖像E，結果如圖5（c）所示；然后基于OpenCV-python 進行霍夫變換，在邊緣圖像E中檢測圓擬合出圓心，結果如圖5（d）所示。

Fig.5 The circle detected from the camera image圖5 從相機圖像中檢測到的圓

3.2.2 點云中的圓檢測

通過三維激光雷達得到的原始點云數據如圖6（a）所示。在進行圓心檢測之前，首先需要從原始點云數據中提取出圓孔標定板的點云數據，如圖6（b）和圖6（c）所示。

Fig.6 Lidar point cloud data圖6 激光雷達點云數據

在得到圓孔標定板的點云數據后對其進行濾波處理，剔除其中的噪聲和漂移點，并分離出圓孔標定板的前平面El，如圖7（a）所示。使用隨機抽樣一致算法（Random sample consensus，RANSAC）重新擬合標定板，得到更加精確的標定板平面，RANSAC 算法得到的平面方程表達式為：

其中，d為激光原點到標定板的距離。

為了準確查找圓的中心，將El(xl,yl,zl)中所有的點重投影到標定板上，得到如圖7（b）所示的平面。

Fig.7 Front plane of the calibration plate in the lidar point cloud圖7 激光雷達點云中標定板前平面

通過在Ehp中擬合圓的模型，檢測標定板上的4 個圓。每檢測到一個圓，就刪除到圓心一定距離內的點，然后在剩余點中繼續檢測圓，直到檢測到所有的圓，再驗證相鄰圓的圓心距離與半徑大小。如果成功，則圓檢測算法停止，使用式（9）-式（12）求解平移矩陣T，否則丟棄當前檢測到的圓返回到初始值，重新檢測。

Fig.8 Circles detected in the point cloud圖8 點云中檢測到的圓

使用的RANSAC 算法步驟如下：①設置期望值、迭代次數和距離閾值，隨機選取原始數據的3 個點；②對選擇出的點擬合一個模型；③計算所有其他點到模型的距離；④統計模型中所有內點的個數（距離小于閾值）；⑤如果模型中內點個數小于設置的期望值則丟棄當前模型，繼續迭代；⑥最終選擇內點個數最多的模型為最優結果。

3.3 實驗結果

3.3.1 雙目相機標定

雙目相機的標定結果如表1 所示，包括左目和右目相機的內部參數矩陣M、右目相機到左目相機旋轉矩陣RC和平移向量TC。雙目相機標定實驗誤差如圖9 所示，其中總體標準差為0.09 像素。

Table 1 Calibration results of stereo camera表1 雙目相機的標定結果

Fig.9 Calibration error of stereo camera圖9 雙目相機標定誤差

3.3.2 聯合標定

聯合標定實驗中，使用圓孔標定板測得3 組數據如表2所示，每組數據包含4 個圓心和半徑，(xl,yl,zl)和Ri是激光雷達坐標系中檢測的圓心坐標及半徑，單位為m；(u,v)和ri是圖像像素坐標系中檢測的圓心坐標及半徑，單位為像素（pixel）。

Table 2 Detection results of circles in lidar point clouds and images表2 雷達點云和圖像中圓的檢測結果

表3 所示的聯合標定參數中包含手工測量參數，以及由公式（8）-（12）求出的粗標定參數。手工是直接測量的，而粗標定是假設在沒有旋轉角度的情況下進行，都存在著較大誤差。

Table 3 Joint calibration parameters表3 聯合標定參數

按照聯合標定算法中公式（13）、（14）、（15）、（16）進行優化求解，優化后的聯合標定參數如下：

為驗證公式（22）和公式（23）中聯合標定結果的準確性，使用公式（7）將表2 中激光雷達的點云坐標投影到圖像上，并計算出投影誤差。為了直觀展示出激光雷達的點云坐標在圖像上的投影誤差，其誤差絕對值柱狀圖如圖10 所示，其中最大投影誤差為2.71 像素，最小投影誤差為1.4 像素，其中有10 組分布在1.4～2.0 像素區間中，12 組平均投影誤差為1.86 像素，標準差為0.41 像素，證明公式（22）和公式（23）中聯合標定參數是準確可靠的。

Fig.10 Projection error of lidar point cloud coordinates on the image圖10 激光雷達點云坐標在圖像上的投影誤差

4 結語

無人駕駛汽車的環境感知系統使用多傳感器融合的方法提高感知精度，但首先需要知道傳感器之間的位姿參數才能將傳感器的測量信息關聯。本文以三維激光雷達和雙目相機為研究對象，提出一種聯合標定方法，建立了激光雷達坐標系與雙目相機坐標系的轉換關系，并實現了激光點云數據與圖像像素級關聯。在聯合標定過程中只需對圓孔標定板采集3 次數據，降低了特征點的提取難度，簡化了標定過程。但本文使用低線束的激光雷達，垂直方向分辨率較低，遠距離目標信息較少。下一步將研究激光雷達和雙目相機特征級的融合方法，以提高測量環境信息的精度。