吳瓊 時(shí)利 謝欣燕 岳麗姣

（安徽江淮汽車集團(tuán)股份有限公司技術(shù)中心，汽車智能網(wǎng)聯(lián)技術(shù)安徽省重點(diǎn)實(shí)驗(yàn)室，合肥 230601）

主題詞：激光雷達(dá) 相機(jī) 在線自動標(biāo)定 無人駕駛車輛

1 前言

激光雷達(dá)和相機(jī)是當(dāng)前無人駕駛汽車環(huán)境感知使用的最主要的兩類傳感器，但二者獲取的不同形態(tài)的信息單獨(dú)利用通常會引發(fā)沖突，故相機(jī)和激光雷達(dá)的置信融合是當(dāng)前無人駕駛汽車環(huán)境感知領(lǐng)域的重要研究方向[1-4]。信息的置信融合不僅可以避免沖突問題，還可以提高環(huán)境要素感知的置信度。其中，相機(jī)和激光雷達(dá)間的外部參數(shù)標(biāo)定是實(shí)現(xiàn)信息魯棒融合的一個(gè)重要環(huán)節(jié)。

國內(nèi)外科研工作者針對相機(jī)與激光雷達(dá)信息融合問題，提出了大量的自動或半自動外部參數(shù)標(biāo)定方法。Ranjith Unnikrishnan等[5]在Zhang等[6]提出的相機(jī)與二維激光雷達(dá)標(biāo)定方法的基礎(chǔ)上，提出了三維激光雷達(dá)與相機(jī)外部參數(shù)標(biāo)定方法。A.Sergio等[7]提出一種利用圓形目標(biāo)物求解相機(jī)與四線激光雷達(dá)間坐標(biāo)映射關(guān)系的方法，該方法能夠準(zhǔn)確估計(jì)圓形目標(biāo)物的位姿。Scaramuzza等[8]提出了一種基于自然環(huán)境特征的激光雷達(dá)與相機(jī)外部參數(shù)在線標(biāo)定方法，但需要手動選擇兩類傳感器的配準(zhǔn)點(diǎn)。Gaurav Pandey[9]提出一種共有信息最大化的無目標(biāo)外部參數(shù)標(biāo)定方法，該方法通過場景反射率使共有區(qū)域最大化，并通過克拉美羅下界（Cramer-Rao Lower Bound，CRLB）消除標(biāo)定誤差，但是該方法過度依賴反射率信息。

目前，相機(jī)與激光雷達(dá)外部參數(shù)標(biāo)定方法大都為手動或半自動化標(biāo)定，不僅過程繁瑣，而且人為因素對標(biāo)定誤差的影響較大。針對上述問題，本文提出一種面向無人駕駛汽車車載激光雷達(dá)與相機(jī)的在線快速自動標(biāo)定方法，以安裝視覺標(biāo)定板的試驗(yàn)車為標(biāo)定目標(biāo)，從圖形和三維激光雷達(dá)數(shù)據(jù)中快速識別試驗(yàn)車，進(jìn)而求解相機(jī)與激光雷達(dá)之間的外部參數(shù)矩陣。

2 問題描述

相機(jī)和激光雷達(dá)之間的外部參數(shù)標(biāo)定問題實(shí)際上是相機(jī)坐標(biāo)系與激光雷達(dá)坐標(biāo)系之間的剛性轉(zhuǎn)換問題。在無人駕駛平臺上，相機(jī)以一定的傾斜角度安裝在前部，將前方的三維物體映射到二維圖像中，64線激光雷達(dá)水平安裝在平臺頂端，二者相對位置和姿態(tài)關(guān)系固定不變，故在相機(jī)和激光雷達(dá)的視場公共區(qū)域內(nèi)，每個(gè)三維激光點(diǎn)在圖像中始終存在一個(gè)像素點(diǎn)與之對應(yīng)。因此，相機(jī)和激光雷達(dá)之間的外部參數(shù)標(biāo)定問題即三維激光點(diǎn)與二維圖像像素點(diǎn)之間的映射矩陣求解問題：

式中，(x,y,z)為三維激光點(diǎn)的坐標(biāo)；(u,v)為三維激光點(diǎn)對應(yīng)的二維圖像像素坐標(biāo)；T為3×4矩陣，即所求的映射關(guān)系矩陣。

根據(jù)相機(jī)的小孔成像原理可知：

式中，fx、fy分別為相機(jī)在x方向和y方向的焦距；(cx,cy)為相機(jī)的主點(diǎn)位置；γ為相機(jī)的畸變參數(shù)，可以通過MATLAB相機(jī)標(biāo)定工具標(biāo)定得到；R為世界坐標(biāo)系到相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣；t為世界坐標(biāo)系原點(diǎn)在相機(jī)坐標(biāo)系中的位置矩陣。

因此，可認(rèn)為相機(jī)的內(nèi)部參數(shù)為已知條件，三維激光點(diǎn)和圖像像素之間的映射關(guān)系求解問題可以轉(zhuǎn)換為相機(jī)與激光雷達(dá)之間的旋轉(zhuǎn)和平移關(guān)系求解問題。通過對若干對一致點(diǎn)構(gòu)成的線性方程進(jìn)行求解，可以獲得其最小二乘解。

3 標(biāo)定

3.1 標(biāo)定方法

考慮到在線標(biāo)定的動態(tài)性和目標(biāo)的唯一性要求，本文利用視覺標(biāo)記板代替?zhèn)鹘y(tǒng)標(biāo)定中的棋盤格，以便于視覺識別。同時(shí)，相機(jī)和激光雷達(dá)采集數(shù)據(jù)存在異步性，所以對t1時(shí)刻采集的圖像數(shù)據(jù)和t2時(shí)刻采集的激光點(diǎn)云數(shù)據(jù)分開進(jìn)行處理。當(dāng)試驗(yàn)車輛和標(biāo)記板（Marker）均成功識別后，輸出標(biāo)記板相對于相機(jī)坐標(biāo)系的位姿和試驗(yàn)車輛相對于激光雷達(dá)的位姿，可認(rèn)為這兩個(gè)位姿在世界坐標(biāo)系中是等價(jià)的，因此提取一致點(diǎn)構(gòu)成線性方程組，求解后通過評價(jià)函數(shù)判斷是否滿足收斂條件，若滿足，則終止，否則將一致點(diǎn)帶入方程組迭代求解，直至滿足最優(yōu)條件。算法流程如圖1所示。

圖1 在線標(biāo)定算法流程

3.2 基于視覺標(biāo)記板的車輛位姿識別

受虛擬現(xiàn)實(shí)技術(shù)啟發(fā)，已知幾何尺寸的共面4個(gè)點(diǎn)可以確定該面在相機(jī)坐標(biāo)系中的位置和姿態(tài)[6]。本文通過識別標(biāo)記板，利用坐標(biāo)轉(zhuǎn)換快速獲取試驗(yàn)車輛在相機(jī)坐標(biāo)系下的位姿關(guān)系，避免了基于深度學(xué)習(xí)方法的車輛識別在時(shí)間上的消耗問題與傳統(tǒng)雙面測量的誤檢問題。以標(biāo)記板中心為坐標(biāo)原點(diǎn)，標(biāo)記板平面為X-Y面，則標(biāo)記板4個(gè)頂點(diǎn)在標(biāo)記板坐標(biāo)系中的坐標(biāo)分別為(l/2,l/2,0)、(-l/2,l/2,0)、(l/2,-l/2,0)、(-l/2,-l/2,0)。其中，l為標(biāo)記板的物理邊長。標(biāo)記板坐標(biāo)系與相機(jī)坐標(biāo)系之間的坐標(biāo)轉(zhuǎn)換關(guān)系如圖2所示。

圖2 標(biāo)記板坐標(biāo)系與相機(jī)坐標(biāo)系的轉(zhuǎn)換關(guān)系

根據(jù)小孔成像原理，空間坐標(biāo)和圖像坐標(biāo)之間的映射關(guān)系為：

式中，(xc,yc)為標(biāo)記板頂點(diǎn)在圖像坐標(biāo)系中的坐標(biāo)；(xm,ym,zm)為頂點(diǎn)在標(biāo)記板坐標(biāo)系中的坐標(biāo)，通常取zm=0；A為相機(jī)的內(nèi)部參數(shù)矩陣；M為透視投影變換矩陣；Tcm為相機(jī)坐標(biāo)系到標(biāo)記板坐標(biāo)系的轉(zhuǎn)換矩陣，可以通過OpenCV 開源庫[10]中的solvePnP 函數(shù)求解得到；r11=cosβcosθ；r12=cosβsinθ；r13=-sinβ；r21=-cosαsinθ+sinφsinβcosθ；r22=cosφcosθ+sinαsinβsinθ；r23=sinφcosβ；r31=sinφsinθ+cosαsinβcosθ；r32=-sinαcosθ+cosφsinβsinθ；r33=cosαcosβ；φ、β、θ分別為相機(jī)坐標(biāo)系與標(biāo)記板坐標(biāo)系之間的偏航角、俯仰角和翻滾角；tx、ty、tz分別為相機(jī)在標(biāo)記板坐標(biāo)系中x軸、y軸及z軸的空間坐標(biāo)。

3.3 基于激光雷達(dá)的車輛位姿識別

激光雷達(dá)能否正確識別試驗(yàn)車輛的位姿是標(biāo)定試驗(yàn)成敗的關(guān)鍵因素，以最小外包立方體中的姿態(tài)表示，主要通過分割、聚類、車輛識別、試驗(yàn)車輛跟蹤等幾個(gè)關(guān)鍵步驟實(shí)現(xiàn)。

3.3.1 分割

由于標(biāo)定試驗(yàn)在城區(qū)結(jié)構(gòu)化環(huán)境下進(jìn)行，本文采用文獻(xiàn)[11]中提出的基于線段擬合的快速路面分割算法，建立了三維點(diǎn)云數(shù)據(jù)的極坐標(biāo)柵格圖，將數(shù)據(jù)點(diǎn)從三維空間(x,y,z)映射到二維空間(,z)，然后在每個(gè)柵格中選擇高度最低的點(diǎn)作為地面候選點(diǎn)，對同屬于一個(gè)扇區(qū)的所有柵格的候選點(diǎn)進(jìn)行線段擬合，將擬合線段作為候選地面，計(jì)算所有數(shù)據(jù)點(diǎn)到擬合線段的距離，根據(jù)距離大小和線段斜率區(qū)分障礙物點(diǎn)與地面點(diǎn)。

3.3.2 聚類

得到當(dāng)前場景的路面信息之后，本文通過K-means[12]聚類算法對濾除地面激光點(diǎn)后的三維點(diǎn)云數(shù)據(jù)進(jìn)行聚類。

3.3.3 車輛識別

車輛目標(biāo)識別主要是在激光雷達(dá)點(diǎn)云數(shù)據(jù)中將其區(qū)別開來，以提高相機(jī)與激光雷達(dá)的識別一致性，如圖3 所示。得到聚類結(jié)果后，利用1 個(gè)面與地面重合的外接立方體近似車輛，該立方體的姿態(tài)即為當(dāng)前聚類的姿態(tài)。車輛識別過程遵從以下原則：由于小型車輛的幾何尺寸可視為已知，故外接立方體的高度應(yīng)在1.4～2.0 m 范圍內(nèi)，寬度應(yīng)小于2.0 m，長度不大于5.0 m；車輛在激光雷達(dá)的側(cè)前方，其三維點(diǎn)云數(shù)據(jù)投影到二維地面，通常都呈現(xiàn)L 型[13]。

圖3 前方的車輛目標(biāo)識別

3.3.4 試驗(yàn)車輛跟蹤

在得到當(dāng)前場景中的車輛目標(biāo)信息后，創(chuàng)建車輛目標(biāo)動態(tài)狀態(tài)列表來存儲識別到的信息[13-14]。車輛狀態(tài)用六元組s={編號,位置,時(shí)間,姿態(tài),加速度,置信度}表示，并且根據(jù)識別結(jié)果實(shí)時(shí)更新狀態(tài)列表。由于標(biāo)定試驗(yàn)車的狀態(tài)在上一時(shí)刻已知，故可以利用最大關(guān)聯(lián)法將上一幀的試驗(yàn)車狀態(tài)信息與當(dāng)前識別的車輛狀態(tài)列表進(jìn)行關(guān)聯(lián)匹配，從而在包含眾多車輛目標(biāo)的雷達(dá)數(shù)據(jù)中精準(zhǔn)聚類。在關(guān)聯(lián)匹配的過程中需要考慮試驗(yàn)車輛本身的運(yùn)動學(xué)特性：

式中，Kmax為最大曲率；φmax為最大轉(zhuǎn)向角；rmin為最小轉(zhuǎn)彎半徑；L為車輛軸距；k為車輛當(dāng)前運(yùn)行軌跡曲率；φ為標(biāo)定對象車輛的當(dāng)前轉(zhuǎn)角；xt、yt分別為t時(shí)刻前方車輛在試驗(yàn)本車坐標(biāo)系中的橫向坐標(biāo)和縱向坐標(biāo)；vmax為車輛允許的最大行駛速度；t為試驗(yàn)本車識別的時(shí)間間隔；θ為標(biāo)定對象車輛的航向角。

3.4 最優(yōu)解評價(jià)

為實(shí)現(xiàn)平移和轉(zhuǎn)換矩陣最優(yōu)化，即標(biāo)記板和試驗(yàn)車三維激光雷達(dá)聚類外包立方體的后部共面，通過評估函數(shù)[1]實(shí)現(xiàn)外部參數(shù)的優(yōu)化評價(jià)：

式中，pi為第i個(gè)標(biāo)記板在相機(jī)坐標(biāo)系中的位置；O為選擇矩陣；T(x,y,z)為平移矩陣；qi,j為第i個(gè)位置的標(biāo)記板中心點(diǎn)在第j幀激光雷達(dá)數(shù)據(jù)中的坐標(biāo)；ni為第i個(gè)標(biāo)記板位置的標(biāo)記板面法向量。

4 試驗(yàn)驗(yàn)證

采用自主研發(fā)的“智能先鋒”系列無人駕駛汽車平臺，以正后方配置視覺標(biāo)記板的車輛為標(biāo)定對象，試驗(yàn)本車上安裝有Velodyne HDL 64E 激光雷達(dá)與映美精高清相機(jī)，即為待標(biāo)定的兩類傳感器，如圖4所示。

圖4 試驗(yàn)平臺

標(biāo)定試驗(yàn)在開放式的城區(qū)道路環(huán)境中進(jìn)行，首先將本文提出的方法與開源軟件Autoware[4]庫中基于棋盤格的激光雷達(dá)與相機(jī)靜態(tài)手動標(biāo)定方法進(jìn)行對比分析，結(jié)果如表1 所示。表1 中，α、β、γ分別為相機(jī)與雷達(dá)之間的俯仰角、側(cè)傾角及橫擺角，x、y、z分別為相機(jī)與激光雷達(dá)之間的空間位移關(guān)系。從表中可以看出，本文方法產(chǎn)生結(jié)果與Autoware中標(biāo)定方法的標(biāo)定結(jié)果非常接近，平均誤差雖然略大于Autoware中方法，但對最終信息融合結(jié)果影響不大，且本文方法的主要優(yōu)勢在于自動化和節(jié)約時(shí)間。為了直觀地驗(yàn)證和顯示標(biāo)定結(jié)果，選擇不同位置的三維激光點(diǎn)云數(shù)據(jù)映射到高清相機(jī)圖像中，如圖5所示。從圖5中可以看出，車輛及其邊緣得到了較好匹配，雖然周圍存在少數(shù)誤差點(diǎn)，但不影響整體的融合效果，可見，標(biāo)定結(jié)果能夠較好地滿足無人車激光雷達(dá)與相機(jī)之間的信息融合要求。

表1 相機(jī)與激光雷達(dá)的外參標(biāo)定結(jié)果

圖5 激光雷達(dá)數(shù)據(jù)與圖像映射結(jié)果

5 結(jié)束語

本文提出了一種面向無人駕駛汽車車載激光雷達(dá)與相機(jī)的外部參數(shù)快速自動標(biāo)定方法。以配置有視覺標(biāo)記板的車輛為標(biāo)定目標(biāo)，本車上相機(jī)與激光雷達(dá)之間的外部參數(shù)可以通過本文提出的標(biāo)定算法自動完成。試驗(yàn)結(jié)果表明，本文提出的標(biāo)定方法可靠，但是車輛在行駛過程中，受顛簸等因素影響，可能會增大標(biāo)定誤差。如何克服或補(bǔ)償顛簸或檢測精度等因素導(dǎo)致的標(biāo)定誤差累積問題是下一步研究的重點(diǎn)內(nèi)容。