激光雷達和視覺傳感器組合系統(tǒng)的在線校準方法

朱蒙，馬其華

（上海工程技術(shù)大學機械與汽車工程學院，上海 201620）

激光雷達和視覺傳感器組合系統(tǒng)[1-2]因其超強的感知能力被廣泛應用于自動駕駛領(lǐng)域[3]。傳感器的檢測準確性直接影響自動駕駛車輛的行車安全性[4]，傳感器之間的相互校準[5-7]是保障其可靠性的重要手段之一。

在行車過程中采用的在線校準方法[8-10]，不需要輔助標記物，通過對目標對象的快速細化變換，實現(xiàn)及時檢驗并校準。文獻[11]使用激光雷達反射圖像進行校準，該方法需要大量場景數(shù)據(jù)，計算難度大。文獻[12]并未使用所有激光雷達點的反射率信息，而是引入了兩種使用深度信息的實時算法。在文獻[12]的基礎上，文獻[13-14]設計了一個類似的校準系統(tǒng)。新穎之處在于邊緣檢測的數(shù)據(jù)預處理中，通過VICP 算法[15]對失真的激光雷達數(shù)據(jù)進行運動校正，以進一步提高精度。文獻[16]發(fā)現(xiàn)估計激光雷達和圖像邊緣之間轉(zhuǎn)換的魯棒性至關(guān)重要，提出了一種低開銷的穩(wěn)健模型來評估穩(wěn)定性。

為解決激光雷達和視覺傳感器這一組合傳感系統(tǒng)在工作狀態(tài)下的在線校準問題，通過兩個傳感器測取同一目標物的點云與圖像數(shù)據(jù)進行邊緣匹配，確定是否存在傳感器位置偏差的情況，并根據(jù)偏差情況采取不同的應急或校準方式進行處理。

1 原理與模型

設計激光雷達-視覺傳感器組合系統(tǒng)的在線校準主要思路如下：

1）激光雷達和視覺傳感器初始狀態(tài)正常。根據(jù)KITTI 數(shù)據(jù)集中提供的傳感器標準安裝位置計算兩傳感器之間校準參數(shù)，作為初始標準值。

2）在車輛行駛的任意環(huán)境中隨時間捕獲一系列相應的激光點和相機圖像，將激光點投影至相機圖像，根據(jù)同一物體的激光雷達點和圖像邊緣的匹配原理，自動計算兩傳感器在此刻位置關(guān)系，得到校準參數(shù)。

3）將車輛行駛時得到的校準參數(shù)與初始標準值進行對比，判斷兩傳感器相對位置是否發(fā)生變化并在車輛移動時通過調(diào)整偏移量來自動校正校準參數(shù)。

在線校準流程如圖1 所示。

圖1 在線校準流程

分別對激光雷達點云數(shù)據(jù)和視覺傳感器輸出的圖像數(shù)據(jù)進行處理，獲取邊緣激光點和圖像邊緣，計算兩傳感器之間的校準參數(shù)。之后進行在線檢測，根據(jù)邊緣匹配得到的變換矩陣寫為成本函數(shù)，利用成本函數(shù)計算Pc值（成本函數(shù)最差百分比），若0.8＞Pc＞0，則說明校準參數(shù)發(fā)生較大偏差，此時傳感器做應急處理。若1＞Pc＞0.8，則校準參數(shù)發(fā)生較小擾動，此時進行在線校正，應用梯度下降法優(yōu)化成本函數(shù)并不斷獲取新的校準參數(shù)。

1.1 視覺圖像處理

對于圖像傳感器獲得的在時間t內(nèi)拍攝的n幀圖像中的每一幀圖像，都要進行灰度轉(zhuǎn)換；應用邊緣檢測來生成邊緣圖像并輸出，如圖2 所示；利用逆距離變換使得當前圖像幀的每個像素值Iu,v都獲得一個到最近邊緣像素的距離值。該距離值表示這是一條邊的概率，使用歸一化使距離值在0～1 內(nèi)。

圖2 邊緣圖像輸出

Iu,v計算公式如下：

1.2 激光點云處理

由于原始激光點云數(shù)量大、雜亂無序、噪聲多，需要對激光點云進行預處理。首先將激光雷達點投影到同一時刻的圖像中，然后保留投影在圖像中且可以找到的相同對象上的點，丟棄其他點。

對于光束的每個點Pi，相鄰點是來自同一光束的左側(cè)Pi-1點和右側(cè)Pi+1點。點Pi的距離間隙值為：

其中，Ri是對應點Pi的坐標。根據(jù)KITTI 數(shù)據(jù)集的閾值設置，只提取間隙值Xi大于1 m 的點作為邊緣點。

為了進一步改善邊緣點的提取結(jié)果，應用中值濾波器去除隨機噪聲但保留清晰的特征。如圖3 所示，使用RANSAC 線模型后，物體線條被清晰地提取出來，如電線桿、樹干、鐵軌的輪廓，并去除樹點等輪廓。

圖3 激光點云處理后提取的物體邊緣點

1.3 校準參數(shù)評價指標

組合系統(tǒng)的輸出結(jié)果為激光點云和圖像的兩者邊緣，根據(jù)邊緣匹配原理找到其3D 點云與2D 像素關(guān)系，通過矩陣變換計算在線校準參數(shù)。利用激光雷達坐標中的邊緣3D 點云X=[x,y,z,1]T與2D 圖像像素p=[u,v,1]T對應關(guān)系，計算其旋轉(zhuǎn)矩陣R和平移向量T。轉(zhuǎn)換矩陣計算公式如下：

其中，K是相機的固有參數(shù)，包括焦距(fu,fv)、原點(u0,v0) 。R[I/T]是具有平移T和旋轉(zhuǎn)R的外在矩陣，其中，三個為平移參數(shù)，三個為旋轉(zhuǎn)參數(shù)（統(tǒng)稱校準參數(shù)）。

為了最大化成本函數(shù)，給定當前校準參數(shù)θ，在窗口大小為n的數(shù)據(jù)幀中，對于每一幀圖像找到所有激光雷達邊緣點。對于每個邊緣點找到其對應的投影像素，使用雙線性插值用于計算激光雷達點到圖像像素投影過程中的值，即成本函數(shù)值。由于該函數(shù)不是凸函數(shù)，因此不可能全局搜索，即可能存在多個最優(yōu)成本值。成本函數(shù)如下：

為解決成本函數(shù)可能存在多個最優(yōu)值這一缺陷，添加一個約束以使用網(wǎng)格搜索方法驗證校準參數(shù)的正確性。Pc的計算公式如下：

其中，?θJ為每個數(shù)據(jù)的梯度，η是自適應步長。

由于校準參數(shù)越準確，成本函數(shù)值越大，因此，可用Pc（計算最差成本函數(shù)百分比），評估校準參數(shù)的正確性。由文獻[12]可知，當Pc＜0.8，校準參數(shù)發(fā)生較大錯誤，1＞Pc＞0.8，校準參數(shù)發(fā)生較小擾動。

1.4 在線校正方法

對于校準參數(shù)發(fā)生較小擾動的參數(shù)，采用梯度下降法迭代優(yōu)化成本函數(shù)，不斷實時更新校準參數(shù)。假設成本函數(shù)在一個小的局部范圍內(nèi)是可微的，因不可能將圖像值和點云范圍直接寫入成本函數(shù)，即它是通過數(shù)值梯度近似估計的。算法如下：

算法：梯度下降

2 結(jié)果與分析

實驗使用KITTI 數(shù)據(jù)集進行測試，KITTI 數(shù)據(jù)集是利用組裝的設備齊全的車輛對實際交通場景進行數(shù)據(jù)采集獲得的公開數(shù)據(jù)集。該數(shù)據(jù)集包含豐富多樣的傳感器數(shù)據(jù)，有雙目相機、64 線激光雷達、GPS/IMU 組合導航定位系統(tǒng)，基本滿足對圖像、點云和定位數(shù)據(jù)的需求，大量的標定真值包括檢測2D 和3D包圍框、跟蹤軌跡等。該實驗基于C++開發(fā)的機器人操作系統(tǒng)（ROS）、Ubuntu18.04，使用的庫包括點云庫（PCL）、OpenCV 等。

2.1 檢測過程及分析

由上節(jié)可知，最差成本函數(shù)百分比Pc可用來判斷參數(shù)是否發(fā)生偏差。通過對標準參數(shù)施加不同的位置偏移量驗證其檢驗有效性，偏移量在[-10 cm,10 cm]或[-2°,2°]之間。結(jié)果如圖4 所示。首先，算法在不到0.05 s 內(nèi)快速響應；其次，當偏移量大于4 cm或偏移小于-6 cm 時，Pc＜0.8。偏移總量在10 cm 附近時，Pc值趨近穩(wěn)定值1。由于平移偏移參數(shù)絕對值大于10 cm 時，算法檢測其錯誤的 時間較長，因此，將平移偏移參數(shù)閾值設置為10 cm。

圖4 初始標準參數(shù)偏移不同長度時的Pc值

對旋轉(zhuǎn)角度偏移（Roll、Pitch、Yaw）從+2°至-2°進行試驗，保持平移坐標不變，結(jié)果如圖5 所示。首先，算法在不到0.05 s 內(nèi)快速響應；其次，當旋轉(zhuǎn)偏移大于+0.25°或小于-0.25°時，Pc＜0.8。Pc值穩(wěn)定接近1 的區(qū)間約為0.5°。因此，算法可以檢測到大于0.5°的旋轉(zhuǎn)角度偏移。因旋轉(zhuǎn)偏移絕對值大于2°時，算法檢測其錯誤的時間較長，因此，將旋轉(zhuǎn)偏移參數(shù)閾值設置為2°進行測試。

圖5 初始標準參數(shù)旋轉(zhuǎn)不同角度時的Pc值

2.2 校正過程及分析

由上節(jié)可知，在線檢測算法可以在較短時間內(nèi)檢測出距離初始標準值大于10 cm 的平移偏移和大于0.5°的旋轉(zhuǎn)偏移。而對于小于10 cm 和0.5°的校準參數(shù)，使用在線校正算法來修正參數(shù)。實驗是通過改變初始標準值來完成的，設置平移偏移的范圍為[-10 cm,10 cm]，間隔為2 cm，添加所有三個平移參數(shù)（X、Y、Z）上。對于修改旋轉(zhuǎn)參數(shù)的實驗，設置旋轉(zhuǎn)偏移的范圍為[-0.5°,0.5°]，被添加到所有三個旋轉(zhuǎn)參數(shù)（Roll 水平、Pitch 俯仰、Yaw 偏航）上，如表1所示。

表1 初始標準參數(shù)

為驗證算法的普遍適用性，進行了不同的平移和旋轉(zhuǎn)實驗。對于平移，表2 顯示了從-10 cm 到10 cm 的不同初始平移偏移的校準參數(shù)在8 s 內(nèi)更新的新校準參數(shù)。對于旋轉(zhuǎn)，表3 顯示了從-0.5°到0.5°的不同初始旋轉(zhuǎn)偏移在8 s 內(nèi)穩(wěn)定的校準值。

表2 不同初始平移偏移參數(shù)更新后的穩(wěn)定值

表3 不同初始旋轉(zhuǎn)偏移參數(shù)更新后的穩(wěn)定值

圖6 顯示了新校準參數(shù)的平移和旋轉(zhuǎn)的RMSE（均方根誤差）。由圖6 可知，t=5.172 s 后，所有參數(shù)都處于穩(wěn)定狀態(tài)。平移偏移在[-4 cm,4 cm]以內(nèi)，新校準參數(shù)的均方根誤差小于0.08 cm，誤差較小。平移偏移在[-4 cm,2 cm]之間，新校準參數(shù)的均方根誤差為0.04 cm 左右，收斂于初始標準值。因此，該算法能夠?qū)崿F(xiàn)的平移校準范圍為6 cm。

圖6 不同平移偏移值參數(shù)的均方根誤差

圖7 顯示了新校準參數(shù)的平移和旋轉(zhuǎn)參數(shù)的均方根誤差。更新后的校準參數(shù)在t=5.172 s 處穩(wěn)定。考慮均方根誤差，整體平移偏移均方根誤差小于0.09 cm，整體旋轉(zhuǎn)偏移誤差為0.476°，對于[-0.125°,0.125°]的均方根誤差，大約是0.05°。因此，該算法能校正的角度是0.25°。此外還測試了絕對值大于0.5°的旋轉(zhuǎn)偏移，測試結(jié)果發(fā)現(xiàn)，旋轉(zhuǎn)偏移大于0.5°，超出局部最優(yōu)范圍，算法無法更新校準參數(shù)，因為成本函數(shù)和Pc值無法保持高位。

圖7 不同旋轉(zhuǎn)偏移值參數(shù)的均方根誤差

3 結(jié)論

基于激光點云和圖像的邊緣匹配原理自動計算校準參數(shù)，通過變換矩陣定義的成本函數(shù)進行校準參數(shù)檢測，最后通過梯度下降法優(yōu)化成本函數(shù)以不斷獲取新的校準參數(shù)，實現(xiàn)校準參數(shù)校正。因此，在傳感器緩慢漂移或旋轉(zhuǎn)的情況下，算法可以實時檢測和校正變化，不僅不會中斷車輛感知系統(tǒng)的正常運行，還可以顯著提高車輛在未知惡劣環(huán)境中的靈活性和適用性。