一種激光雷達和紅外攝像頭聯(lián)合標定方法

蔣進軍 茅時群 譚興福

上海振華重工(集團)有限公司

1 引言

隨著港口碼頭智能化升級改造，激光器和視覺融合技術成為當前研究和應用熱點。在傳統(tǒng)的二維目標檢測中，圖像識別已經(jīng)很成熟，通過顏色和紋理很容易分辨出物體的輪廓，特征信息很明顯。但二維圖像無法準確計算出目標物體的尺寸、距離，而通過激光雷達掃描出點云，可獲取物體的純幾何結構、距離、尺寸等相關信息。將圖像和點云融合，通過點云獲取物體尺寸、位置，然后結合圖像分割、識別目標物體，增加物體識別準確度，從而達到很好的互補。

激光器和圖像傳感器標定是融合技術必不可少的一步，標定的好壞直接關系到后續(xù)融合識別、定位的效果。在自動化龍門起重機(以下簡稱場橋)或岸邊集裝箱起重機(以下簡稱岸橋)中，小車架上通常裝有攝像頭以及激光雷達，用于檢測吊具或者集裝箱位置[1-2]，如何將攝像頭和激光雷達快速、準確進行標定，是傳感器使用過程中非常關鍵的一步[3-4]。結合現(xiàn)場實際使用情況，在吊具上貼兩張3M紙，作為標定靶，通過標定靶快速將激光雷達和攝像頭進行同步。

2 原理和方法

標定對像是近紅外攝像頭和激光雷達，均安裝在起重機小車架上。

2.1 標定原理

將激光器雷達和攝像頭同步，先通過式(1)將激光雷達坐標系變化到攝像頭坐標系，然后通過式(2)變換到圖像坐標系，最后通過式(3)變換到像素坐標系，從而將點云數(shù)據(jù)變換到圖像中，實現(xiàn)配對。

(1)

(2)

(3)

聯(lián)合上面3個公式得到公式(4)：

(4)

式中，K只與攝像機內(nèi)部結構有關,為攝像頭內(nèi)參矩陣，可以通過棋盤格標定方法得到；u、v是特征點在圖形中的像素坐標，可以直接通過圖片獲??；XL、YL、ZL為特征點在激光器雷達中的坐標，可以通過激光器雷達掃描出的點云數(shù)據(jù)中獲??；T為攝像頭相對于激光器雷達的變換矩陣,旋轉矩陣和平移向量組成，也是最終需要求取的轉換矩陣。

其中：

(5)

(6)

為了求取激光雷達到攝像頭之間的轉換關系，首先通過攝像頭和激光雷達分別獲取圖像信息和點云數(shù)據(jù)，且保證獲取圖像和點云數(shù)據(jù)時外部環(huán)境以及自身姿態(tài)沒有變化，然后分別在圖像和點云數(shù)據(jù)提取出同一特征點。攝像頭內(nèi)參矩陣可以通過棋盤格標定[5-6]出來，結合標定出來的攝像頭內(nèi)參矩陣，n個激光雷達坐標系中的特征點坐標，以及與這n個點對應在攝像頭坐標系下的像素坐標，利用EPnP[7]算法，最終求解出T。

2.2 標定方法

為準確獲得激光雷達到攝像頭坐標系的轉換矩陣，結合當前攝像頭和激光雷達的特性，需要設計一個標定靶，用于在圖像和激光同時精確識別對應數(shù)據(jù)中特征點，并進行匹配，從而求解出T。

標定靶在設計的時候，需要考慮到圖像和激光雷達之間的特征，通過在吊具上安裝兩個3M反光貼片，標定過程中攝像頭拍攝兩個3M反光貼片的圖像，激光雷達掃描3M反光貼片，然后通過算法分別在圖像和點云數(shù)據(jù)中識別3M貼紙的中心點(見圖1)。

圖1 標定靶

該標定靶既有矩形標定板的結構簡單的優(yōu)點，同時可以有效消除點云的偏差，保證識別的精度，最終保證激光器雷達和攝像頭同步的準確。

3 計算攝像頭的外參

3.1 攝像頭特征點獲取

因為此處標定工作在室外進行，為避免各種光線的干擾，攝像頭的補光燈采用的是紅外補光燈。近紅外攝像頭配有紅外濾光片，通過濾光片有效濾掉紅外波長外的光線。但在實際操作過程中，尤其是夏天陽光直射的時候，陽光的干擾相對嚴重，因此在實際標定過程中，通過將攝像頭光圈盡量調(diào)小，進一步降低太陽光的影響。獲取圖像后，圖像進行自適用二值化，然后再對圖像進行打標簽，根據(jù)標簽值將圖像分割出來，并過濾掉不符合要求的塊，再利用分割出來的圓，進行橢圓擬合圓心。

3.2 雷達獲取特征點方法

獲取點云數(shù)據(jù)時先提取標定板數(shù)據(jù)，并得到標定板所在平面方程，將點云數(shù)據(jù)投影到二維平面，然后將二維平面點轉換為圖像，在圖像中根據(jù)能量值進行二值化，提取輪廓，根據(jù)輪廓進行橢圓擬合圓心，再根據(jù)所獲取的橢圓圓心，反求圓心的3D坐標。

3.2.1 標定板數(shù)據(jù)提取

通過直通濾波，縮小點云數(shù)據(jù)大小，然后根據(jù)激光點能量值，將掃描在3M貼紙上的點提取出來，采用隨機抽樣一致性進行平面擬合。

隨機抽取一批種子點，{pi},i=1,…n，對其求得擬合平面Ax+By+Cz+D=0。向量n(A,B,C)為平面的法向量，即{pi}組成的協(xié)方差矩陣的最大特征值對應的特征向量。D為坐標原點到該平面距離，即為{pi}中心點pj和法向量的點乘。

計算所有點pi到平面距離d，d小于閾值dth則為局內(nèi)點，否則為局外點。

(7)

重復進行迭代，直到找到最優(yōu)解。最優(yōu)解對應A，B，C即為平面法向量。

3.2.2 二維坐標投影

由于三維空間點并不完全在一個平面點，因此需要將三維空間的點投影到二維坐標點。p0為平面上任意一點，p為標定板點云，p′為p在平面上的投影點，則投影公式為：

(8)

3.2.3 三維中心點求解

根據(jù)激光器掃描點的能量值，將圖像進行二值化，在二值化圖像提取輪廓圖，進行邊緣提取，然后利用這些邊緣擬合橢圓方式求取圓心，求得中心點(xp,yp)。

(9)

(10)

將中心點x′p和y′p代入平面方程，反向求取z′p，得到三維中心點坐標。

3.3 EPnP求解T

利用n個激光雷達坐標系中的特征點坐標，以及與這n個點對應在攝像頭坐標系下的像素坐標，采用EPnP方法來計算出攝像頭的外參矩陣T。EPnP算法核心原理是某一坐標系中3D坐標表示為一組虛擬的控制點的加權和。一般控制點選取個數(shù)為4個，且4個控制點不共面。激光雷達坐標系中任意一個特征點可由4個控制點來表示，同時攝像頭坐標坐標系中任意特征點也可以由4個控制點來表示。求出攝像頭坐標中4個控制點坐標后，從而最終計算出攝像頭的外參矩陣。在文中的公式中，分別用上標w和c表示在激光雷達中的坐標和攝像頭坐標系中的坐標。

3.3.1 控制點基本原理

激光雷達中，每個點可以由4個控制點表示：

(11)

式中，aij為加權系數(shù)。一旦控制點確定后，且滿足4個控制點不共面前提，aij，j=1,2,3,4是唯一確定的。在攝像頭坐標系中，存在同樣的加權和關系：

(12)

(13)

進一步可得，

(14)

(15)

已知控制點，則可以求出每個控制點加權系數(shù)：

(16)

3.3.2 控制點選擇

理論上，控制點選取只要可逆即可。在激光器雷達坐標系中，選擇中心作為第一個控制點：

(17)

3D參考點集為：

(18)

記ATA的特征值為λc,i，i=1,2,3，對應的特征向量為vc,i，i=1,2,3，剩余3個控制點按照式(19)來計算：

(19)

3.3.3 求取控制點在攝像頭坐標系中的坐標

在攝像頭坐標系中，每個特征點同樣存在如下加權關系：

(20)

(21)

(22)

(23)

Mx=0

(24)

(25)

式中，vi是M的N個零特征值對應的特征向量。對于第i個控制點：

(26)

在激光器雷達坐標系中控制點，通過外參矩陣轉換到攝像頭坐標中后，兩兩之間的距離還是保持不變。引入這個約束條件：

(27)

(28)

該方程是關于{βk}k=1,…N的二次方程，主要特點是沒有{βk}k=1,…N相關一次項，在求解時，將二次項βiβj替換為βij，將方程變?yōu)殛P于{βij}k=1,…N的線性方程。通過4個控制點，共可以得到6個方程，通過6個方程求解{βij}k=1,…N，然后通過{βij}k=1,…N再求取{βk}k=1,…N。

3.3.4 計算激光雷達到攝像頭坐標系的轉換矩陣

控制點在攝像頭坐標系的坐標為：

(29)

計算每個特征點在攝像頭坐標系下的坐標：

(30)

(31)

(32)

計算H：

H=BTA

(33)

計算H的SVD分解：

H=UΣVT

(34)

求取旋轉矩陣：

R=UVT

(35)

求取平移t：

(36)

4 標定與測試

4.1 標定

在不同位置，激光雷達和攝像頭分別采集標定板數(shù)據(jù)，共獲取20組數(shù)據(jù)。通過這20組數(shù)據(jù)，來計算T。具體步驟如下：

(1)利用棋盤格，將攝像頭內(nèi)參進行標定，得到矩陣K。

(2)采集數(shù)據(jù)，將吊具停止不同高度，然后再分別在對應高度上采集攝像頭和激光雷達的數(shù)據(jù)，共采取20組數(shù)據(jù)，每組數(shù)據(jù)中有2個特征點，并將采集的數(shù)據(jù)按順序命名，保證圖像和點云數(shù)據(jù)是一一對應的。

(3)獲取點云中的40個特征點。

(4)獲取圖像中的40個特征點。

(5)根據(jù)40個特征點，采用EPnP，計算出矩陣T。標定得到的旋轉矩陣如下：

(37)

4.2 標定效果測試

在不同高度另外采集10組測試數(shù)據(jù)，在圖像和點云數(shù)據(jù)中，分別計算3M貼紙的圓心，然后根據(jù)獲取的T矩陣，以及攝像頭的內(nèi)參矩陣，將激光雷達坐標中的3M貼紙的3D坐標轉換到攝像頭中的像素坐標系，來驗證激光雷達和攝像頭的融合標定的精度，見表1。

表1 激光雷達和攝像頭測試效果

根據(jù)表1測試效果，激光器雷達中特征點投影變換到像素坐標系后，最大誤差在5個像素左右。標定靶離激光雷達越遠，相對誤差越大。

5 結語

該方法可以快速精確地將激光雷達和攝像頭進行同步標定。在實際使用過程中，如果還需進一步提高標定精度及抗干擾能力，可以繼續(xù)增加標定特征點數(shù)目或者增加點云數(shù)據(jù)量，提高點云特征點精度，最終降低標定誤差。實際測試過程中，該標定方案可靠，精度高，完全滿足應用要求。