激光雷達(dá)和視覺(jué)傳感器組合系統(tǒng)的在線校準(zhǔn)方法

朱蒙，馬其華

（上海工程技術(shù)大學(xué)機(jī)械與汽車(chē)工程學(xué)院，上海 201620）

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

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

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

1 原理與模型

設(shè)計(jì)激光雷達(dá)-視覺(jué)傳感器組合系統(tǒng)的在線校準(zhǔn)主要思路如下：

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

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

3）將車(chē)輛行駛時(shí)得到的校準(zhǔn)參數(shù)與初始標(biāo)準(zhǔn)值進(jìn)行對(duì)比，判斷兩傳感器相對(duì)位置是否發(fā)生變化并在車(chē)輛移動(dòng)時(shí)通過(guò)調(diào)整偏移量來(lái)自動(dòng)校正校準(zhǔn)參數(shù)。

在線校準(zhǔn)流程如圖1 所示。

圖1 在線校準(zhǔn)流程

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

1.1 視覺(jué)圖像處理

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

圖2 邊緣圖像輸出

Iu,v計(jì)算公式如下：

1.2 激光點(diǎn)云處理

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

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

其中，Ri是對(duì)應(yīng)點(diǎn)Pi的坐標(biāo)。根據(jù)KITTI 數(shù)據(jù)集的閾值設(shè)置，只提取間隙值Xi大于1 m 的點(diǎn)作為邊緣點(diǎn)。

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

圖3 激光點(diǎn)云處理后提取的物體邊緣點(diǎn)

1.3 校準(zhǔn)參數(shù)評(píng)價(jià)指標(biāo)

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

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

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

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

其中，?θJ為每個(gè)數(shù)據(jù)的梯度，η是自適應(yīng)步長(zhǎng)。

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

1.4 在線校正方法

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

算法：梯度下降

2 結(jié)果與分析

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

2.1 檢測(cè)過(guò)程及分析

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

圖4 初始標(biāo)準(zhǔn)參數(shù)偏移不同長(zhǎng)度時(shí)的Pc值

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

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

2.2 校正過(guò)程及分析

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

表1 初始標(biāo)準(zhǔn)參數(shù)

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

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

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

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

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

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

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

3 結(jié)論

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