基于卷積神經(jīng)網(wǎng)絡(luò)的車(chē)輛位姿估計(jì)算法

周蘇 鄭淼 鄭守建 金杰

（同濟(jì)大學(xué)，上海 201804）

主題詞：卷積神經(jīng)網(wǎng)絡(luò) 位姿估計(jì) 單目視覺(jué) 自動(dòng)駕駛

1 前言

目標(biāo)的位姿估計(jì)方法大致可分為基于激光雷達(dá)和基于視覺(jué)的2 類(lèi)方法，其中，基于視覺(jué)的方法又分為單目視覺(jué)方法和雙目視覺(jué)方法。以激光雷達(dá)作為傳感器的位姿估計(jì)方法[1]利用三維點(diǎn)云數(shù)據(jù)結(jié)合深度學(xué)習(xí)對(duì)車(chē)輛目標(biāo)的位姿信息進(jìn)行估計(jì)，檢測(cè)精度高，魯棒性好，但往往面臨成本、體積和滿(mǎn)足車(chē)規(guī)級(jí)要求等問(wèn)題。基于雙目視覺(jué)的方法[2-4]利用左、右圖像的視差對(duì)不同距離的目標(biāo)提取相應(yīng)的特征，且基線(xiàn)越長(zhǎng)，測(cè)量距離越遠(yuǎn)，精度雖高，卻很難滿(mǎn)足實(shí)時(shí)性要求。相比之下，單目視覺(jué)以其成本低、速度快等優(yōu)點(diǎn)在自動(dòng)駕駛場(chǎng)景中有著廣泛的應(yīng)用。

在目標(biāo)的位姿估計(jì)中，目標(biāo)的距離、語(yǔ)義信息和姿態(tài)信息是不可或缺的。在距離檢測(cè)方面，單目視覺(jué)方法一般采取標(biāo)定的方式進(jìn)行距離估計(jì)[5-6]，即利用先驗(yàn)信息，以目標(biāo)的檢測(cè)框底邊來(lái)估計(jì)目標(biāo)的距離。對(duì)于目標(biāo)姿態(tài)和尺度信息，單目視覺(jué)更多采用估計(jì)的方式，其中，對(duì)目標(biāo)的語(yǔ)義特征進(jìn)行建模用于估計(jì)目標(biāo)姿態(tài)，目標(biāo)的參考基準(zhǔn)可用于估計(jì)尺度信息[7]。例如，Roddick[8]基于ResNet50網(wǎng)絡(luò)，對(duì)輸入圖片進(jìn)行特征提取，并對(duì)特征進(jìn)行正交變換處理，解決了透視效果影響的視角問(wèn)題。但一方面該深度學(xué)習(xí)方法訓(xùn)練消耗較大，另一方面，其所搭建的神經(jīng)網(wǎng)絡(luò)提取特征困難，導(dǎo)致目標(biāo)尺度估計(jì)的準(zhǔn)確率低；而在傳統(tǒng)的基于單目攝像頭的解決方案，如單目同時(shí)定位與地圖構(gòu)建（Simutaneous Location and Mapping，SLAM）[9]算法中，除了單目相機(jī)的圖像信號(hào)，還會(huì)利用慣性傳感器，通過(guò)積分預(yù)算提供尺度信號(hào)和速度信號(hào)。傳統(tǒng)方案往往需要利用更多的傳感器來(lái)解決單目視覺(jué)從二維信號(hào)恢復(fù)到三維信號(hào)的尺度問(wèn)題，計(jì)算量倍增。

針對(duì)現(xiàn)有單目視覺(jué)方案存在的問(wèn)題，本文提出一種純單目視覺(jué)算法，通過(guò)設(shè)計(jì)卷積神經(jīng)網(wǎng)絡(luò)對(duì)車(chē)輛目標(biāo)的尺寸、姿態(tài)角以及3D 中心點(diǎn)進(jìn)行估計(jì)。基于一階段檢測(cè)算法對(duì)車(chē)輛目標(biāo)進(jìn)行定位，訓(xùn)練一個(gè)二級(jí)位姿估計(jì)網(wǎng)絡(luò)用于車(chē)輛目標(biāo)的位姿估計(jì)，將檢測(cè)算法和位姿算法相結(jié)合以解決交通場(chǎng)景中車(chē)輛目標(biāo)位姿估計(jì)的問(wèn)題。

2 位姿估計(jì)算法模型基礎(chǔ)

2.1 單目相機(jī)模型

采用如圖1 所示的針孔相機(jī)模型。其中，Ow-Xw-Yw-Zw為世界坐標(biāo)系。分別以相機(jī)光心Oc、圖像平面中心O1和圖像平面右上點(diǎn)O2為坐標(biāo)原點(diǎn)構(gòu)成相機(jī)坐標(biāo)系Oc-Xc-Yc-Zc、圖像坐標(biāo)系O1-x-y和像素平面坐標(biāo)系O2-u-v，f為相機(jī)焦距，(Xcp,Ycp,Zcp)為點(diǎn)P在相機(jī)坐標(biāo)系下的坐標(biāo)，(xp,yp)為點(diǎn)P在圖像坐標(biāo)系下的坐標(biāo)。小孔成像原理描述了物相間的關(guān)系。

圖1 針孔相機(jī)模型

如圖1所示，u軸向右與x軸平行，v軸向下與y軸平行。因此，像素坐標(biāo)(up,vp)和圖像坐標(biāo)(xp,yp)滿(mǎn)足：

式中，α、β為縮放因子；cx、cy為平移因子。

令αf=fx、βf=fy，齊次坐標(biāo)的矩陣形式為：

式中，Pc=(Xcp,Ycp,Zcp)為點(diǎn)P的相機(jī)坐標(biāo)向量；K為相機(jī)內(nèi)參矩陣，可利用張正友標(biāo)定法[10]標(biāo)定。

由于相機(jī)在運(yùn)動(dòng)，所以P的相機(jī)坐標(biāo)應(yīng)該由它的世界坐標(biāo)向量Pw，根據(jù)相機(jī)當(dāng)前的位姿變換得到：

式中，R為旋轉(zhuǎn)向量；T為平移向量。

2.2 車(chē)輛尺度建模

在進(jìn)行車(chē)輛位姿估計(jì)前，需要通過(guò)檢測(cè)網(wǎng)絡(luò)得到目標(biāo)的類(lèi)別和檢測(cè)框大小。不同類(lèi)別目標(biāo)的3D尺度相差較大，對(duì)于不同類(lèi)別分別取均值。針對(duì)每個(gè)類(lèi)別目標(biāo)的尺度差值，以檢測(cè)網(wǎng)絡(luò)為估計(jì)模型，以類(lèi)別作為先驗(yàn)信息，降低網(wǎng)絡(luò)的學(xué)習(xí)難度。在KITTI數(shù)據(jù)集上對(duì)不同類(lèi)別目標(biāo)的3D尺度進(jìn)行統(tǒng)計(jì)，結(jié)果如表1所示。

表1 不同車(chē)輛類(lèi)別尺寸均值 m

尺度的損失值取平方差，則損失函數(shù)為：

式中，Dj_gt、Dj_avg分別為對(duì)應(yīng)目標(biāo)類(lèi)別的3個(gè)尺度的真值、均值；δj為網(wǎng)絡(luò)對(duì)應(yīng)的3個(gè)尺度的輸出；j=1,2,3分別表示長(zhǎng)、寬、高3個(gè)尺度。

網(wǎng)絡(luò)訓(xùn)練完成后，目標(biāo)相應(yīng)類(lèi)別的預(yù)測(cè)值Dj_pre(j=1,2,3)為：

2.3 車(chē)輛姿態(tài)角建模

目標(biāo)的姿態(tài)角模型如圖2所示，目標(biāo)的姿態(tài)由觀察角θray、當(dāng)?shù)匚恢媒铅纫约跋鄼C(jī)與目標(biāo)中心連線(xiàn)方向與目標(biāo)行駛方向之間的夾角θl描述。其中θray由目標(biāo)在圖像中的位置直接計(jì)算得到。θ相同但距離不同的目標(biāo)經(jīng)過(guò)裁剪后的輸入是不同的。因此，對(duì)位姿角建模時(shí)不能對(duì)θ直接建模，應(yīng)以如圖2 所示的θl進(jìn)行輸出建模。θl、θray和θ應(yīng)滿(mǎn)足：

圖2 目標(biāo)姿態(tài)角模型

θl的取值范圍為[0,2π)，對(duì)其直接估計(jì)相對(duì)困難。因此，先對(duì)目標(biāo)的θl進(jìn)行分類(lèi)，再對(duì)目標(biāo)采用小角度回歸方式估計(jì)姿態(tài)角。設(shè)θl的類(lèi)別數(shù)量為n，則每個(gè)類(lèi)別的角度范圍的大小為2π∕n，第i個(gè)類(lèi)別的角度范圍為[i·2π∕n,(i+1)·2π∕n),i∈{0,1,…,n-1}。θl的類(lèi)別置信度的損失函數(shù)為：

θl的實(shí)際角度偏移為：

設(shè)偏移損失函數(shù)為：

化簡(jiǎn)并去除比例系數(shù)后的損失函數(shù)為：

采用上述形式的損失函數(shù)，其導(dǎo)函數(shù)較為平滑，有利于網(wǎng)絡(luò)優(yōu)化。當(dāng)θl偏移的絕對(duì)值較大時(shí)，梯度較大，能有效抑制離群點(diǎn)。

2.4 車(chē)輛3D Box建模

車(chē)輛三維邊框（3D Bounding Box）可用{a,b,c,w,h,d,Y}表述，其中，(a,b,c)為3D Box的中心坐標(biāo)，{w,h,d}和Y分別為車(chē)輛的長(zhǎng)、高、寬和航向角，其值由網(wǎng)絡(luò)模型輸出。航向角Y和姿態(tài)角θ滿(mǎn)足Y=π∕2-θ。車(chē)輛3D Box建模采用如下假設(shè)：車(chē)輛的翻滾角和俯仰角均為0；車(chē)輛中心與3D Box中心完全重合。

采用檢測(cè)網(wǎng)絡(luò)能夠獲得精確的車(chē)輛2D檢測(cè)框（2D Box）。因此，構(gòu)建2D Box和3D Box之間的投影關(guān)系，是車(chē)輛3D Box建模的關(guān)鍵。如圖3所示，A、B、C、D4個(gè)點(diǎn)為目標(biāo)在圖像中的2D Box角點(diǎn)，編號(hào)為0～7的8個(gè)點(diǎn)為目標(biāo)在圖像上的3D Box 角點(diǎn)，交叉線(xiàn)所在面表示目標(biāo)的朝向。假設(shè)候選3D Box的尺度為{w,h,d}，其在同原點(diǎn)的世界坐標(biāo)系中的坐標(biāo)分別為：

3D Box 的每個(gè)角至少能投射到2D Box 的1 條邊上。如圖3 的中點(diǎn)0，其投影到2D Box 的左邊之上，由式（3）可知，兩點(diǎn)坐標(biāo)滿(mǎn)足：

圖3 3D Box與2D Box之間投影關(guān)系

對(duì)于其他點(diǎn)也能得到類(lèi)似方程，即4 個(gè)2D Box 與3D Box 之間的約束關(guān)系。4 個(gè)方程均為類(lèi)似Ax=0 的線(xiàn)性方程，可以通過(guò)奇異值分解（Singular Value Decomposition，SVD）[11]求得相應(yīng)的平移向量T。一組4 個(gè)點(diǎn)到4條邊的約束關(guān)系得到一組平移向量，再將平移向量T與物體的3D 尺度代入式（3），就可以得到預(yù)測(cè)的2D Box和檢測(cè)網(wǎng)絡(luò)計(jì)算的2D Box的誤差。

3D Box 的點(diǎn)和2D Box 的邊之間的關(guān)系無(wú)法直接獲取，因此，實(shí)際計(jì)算過(guò)程中，取多組點(diǎn)對(duì)的計(jì)算結(jié)果中誤差值對(duì)應(yīng)最小的平移向量作為最后的計(jì)算結(jié)果，圖4 所示為平移向量T的計(jì)算流程。其中，2D Box 用{xmin,ymin,xmax,ymax}表示，目標(biāo)尺度用{w,h,d}表示，S表示以目標(biāo)車(chē)輛3D Box 中心點(diǎn)為坐標(biāo)系所對(duì)應(yīng)頂點(diǎn)的坐標(biāo)，初始投影誤差E設(shè)置為一個(gè)浮點(diǎn)數(shù)范圍內(nèi)的極大值。

圖4 平移向量T的計(jì)算流程

3 位姿估計(jì)網(wǎng)絡(luò)構(gòu)建、訓(xùn)練及檢測(cè)

3.1 位姿估計(jì)網(wǎng)絡(luò)構(gòu)建

位姿網(wǎng)絡(luò)的輸入為一張包含較大信息量且無(wú)關(guān)噪聲較少的三通道彩色圖片，設(shè)計(jì)一種層數(shù)及通道數(shù)較少的卷積神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)，提取特征的同時(shí)使位姿網(wǎng)絡(luò)的前傳消耗不至于過(guò)大。網(wǎng)絡(luò)總體結(jié)構(gòu)如圖5 所示。

圖5 位姿網(wǎng)絡(luò)結(jié)構(gòu)示意

將檢測(cè)到的目標(biāo)區(qū)域縮放到固定大小(224,224,3)并作為位姿網(wǎng)絡(luò)的輸入。設(shè)卷積塊內(nèi)的通道數(shù)量k=8個(gè)。卷積塊由多個(gè)卷積集組成，每經(jīng)過(guò)一個(gè)卷積塊會(huì)利用步長(zhǎng)為2 的卷積操作降低特征圖的尺度，同時(shí)增加通道數(shù)量提升特征圖的感受野，作為對(duì)特征圖尺度降低的彌補(bǔ)。最后的壓平層模塊將多維輸入轉(zhuǎn)化為一維，作為卷積層到全連接層的過(guò)渡。位姿網(wǎng)絡(luò)中的尺度模塊預(yù)測(cè)目標(biāo)3D 尺度，分區(qū)模塊推測(cè)姿態(tài)角所在的角度分塊區(qū)域，方向模塊推測(cè)實(shí)際角度與區(qū)塊中的實(shí)際角度之間的偏差，3 個(gè)模塊共享骨干網(wǎng)絡(luò)的輸出特征。

3.2 位姿估計(jì)網(wǎng)絡(luò)訓(xùn)練

構(gòu)建網(wǎng)絡(luò)結(jié)構(gòu)后，通過(guò)上述目標(biāo)函數(shù)構(gòu)建網(wǎng)絡(luò)總體的損失函數(shù)，對(duì)神經(jīng)網(wǎng)絡(luò)進(jìn)行訓(xùn)練。網(wǎng)絡(luò)訓(xùn)練過(guò)程的總損失如圖6所示，包括尺度、分區(qū)和方向3個(gè)模塊，屬于多任務(wù)訓(xùn)練。任務(wù)之間根據(jù)重要程度設(shè)置不同的權(quán)重比，本文設(shè)置損失函數(shù)為：

圖6 位姿估計(jì)網(wǎng)絡(luò)訓(xùn)練損失

在網(wǎng)絡(luò)訓(xùn)練過(guò)程中，損失逐漸下降且速度變慢，直至網(wǎng)絡(luò)收斂。訓(xùn)練過(guò)程中設(shè)置批大小為32 張，學(xué)習(xí)率為0.001，在迭代周期至80 次左右時(shí)，網(wǎng)絡(luò)趨于收斂。

位姿網(wǎng)絡(luò)的可視化特征圖如圖7、圖8 所示。由圖7、圖8可以看出，淺層特征圖保留了車(chē)輛形狀和紋理信息，深層特征圖則具有更加抽象的、富含語(yǔ)義的關(guān)鍵點(diǎn)信息，如車(chē)燈、后視鏡及A 柱等有效特征。從網(wǎng)絡(luò)損失角度而言，網(wǎng)絡(luò)總體趨勢(shì)是收斂的，神經(jīng)網(wǎng)絡(luò)能夠有效提取用于降低損失的特征；從可視化的結(jié)果也可以看出，對(duì)于車(chē)輛輸入目標(biāo)，神經(jīng)網(wǎng)絡(luò)能夠有效提取相應(yīng)的特征。

傳統(tǒng)的紋理貼圖技術(shù)確有助益，但是當(dāng)近距離觀察采用紋理貼圖技術(shù)進(jìn)行繪制的物體時(shí)，其效果就大打折扣了。利用傳統(tǒng)的紋理貼圖技術(shù)可以呈現(xiàn)出物體表面的一些凹痕和細(xì)小的裂紋，但是這些細(xì)節(jié)僅僅通過(guò)顏色變化而被表現(xiàn)出來(lái)，它們更像是被直接涂染到物體表面上而并非客觀存在的，整體的真實(shí)感太低。而現(xiàn)實(shí)中的物體表面大多并非光滑平坦，而是表現(xiàn)出無(wú)數(shù)凹凸不平的細(xì)節(jié)。

圖7 可視化特征圖

圖8 深層特征圖與輸入對(duì)比

3.3 位姿估計(jì)網(wǎng)絡(luò)算法集成與檢測(cè)

3.3.1 位姿估計(jì)網(wǎng)絡(luò)算法集成

以檢測(cè)網(wǎng)絡(luò)的輸出作為位姿估計(jì)網(wǎng)絡(luò)的輸入，算法主要分為以下幾個(gè)步驟：

a.通過(guò)目標(biāo)在圖像中的位置{xmin,ymin,xmax,ymax}和相機(jī)的內(nèi)參計(jì)算得到目標(biāo)的2D框中心c和觀察角θray：

b.將目標(biāo)圖像裁剪縮放后進(jìn)行網(wǎng)絡(luò)前傳，獲得分類(lèi)類(lèi)別置信度最高的區(qū)域m、總類(lèi)別數(shù)n和偏移角o，姿態(tài)角為：

c.根據(jù)3D Box建模方式計(jì)算目標(biāo)的平移向量T；

d.由T和目標(biāo)的3D 尺寸得到3D Box 中8 個(gè)點(diǎn)在相機(jī)坐標(biāo)系中的坐標(biāo)，根據(jù)式（3）計(jì)算每個(gè)點(diǎn)的像素坐標(biāo)，由此繪制3D Box，流程如圖9所示。

圖9 位姿估計(jì)流程示意

3.3.2 位姿估計(jì)網(wǎng)絡(luò)檢測(cè)結(jié)果可視化

試驗(yàn)所使用硬件平臺(tái)包括一臺(tái)單顯卡的臺(tái)式機(jī)和一臺(tái)帶4 塊TITAN X（PASCAL）顯卡的深度學(xué)習(xí)工作站。2 個(gè)硬件平臺(tái)的參數(shù)配置如表2 所示，硬件平臺(tái)1 主要用于模型訓(xùn)練后的系統(tǒng)結(jié)構(gòu)搭建和測(cè)試，硬件平臺(tái)2 主要用于需要大量并行運(yùn)算的卷積神經(jīng)網(wǎng)絡(luò)的模型訓(xùn)練。本文關(guān)于模型運(yùn)算速度的測(cè)試均在硬件平臺(tái)1 上完成，視頻采集設(shè)備如圖10所示。

表2 試驗(yàn)硬件平臺(tái)

圖10 視頻采集設(shè)備

對(duì)KITTI 數(shù)據(jù)集中的車(chē)輛目標(biāo)進(jìn)行可視化，結(jié)果如圖11 所示。可視化繪制過(guò)程中，目標(biāo)朝向的面添加2 條交叉的對(duì)角線(xiàn)。對(duì)于目標(biāo)部分被遮擋和被圖像邊緣截?cái)嗟那闆r，通過(guò)目標(biāo)2D Box 和圖像邊界的距離設(shè)置閾值進(jìn)行過(guò)濾。普通目標(biāo)送入位姿估計(jì)網(wǎng)絡(luò)，被過(guò)濾目標(biāo)只進(jìn)行2D Box 檢測(cè)。實(shí)際的視頻采集過(guò)程中，被過(guò)濾目標(biāo)大多在本車(chē)的相鄰車(chē)道或攝像頭視野臨界位置。

圖11 3D Box可視化結(jié)果

3.3.3 位姿估計(jì)網(wǎng)絡(luò)精度對(duì)比

精度測(cè)試的內(nèi)容包括目標(biāo)的尺度（長(zhǎng)、寬、高）誤差，基于KITTI測(cè)試集對(duì)每個(gè)目標(biāo)根據(jù)所屬類(lèi)別在不同距離下進(jìn)行計(jì)算，每個(gè)值計(jì)算預(yù)測(cè)值與真實(shí)值之間差值的絕對(duì)值，最后在每個(gè)距離內(nèi)進(jìn)行平均，尺度和角度誤差如圖12所示。

圖12 不同距離下的尺度和角度誤差

在尺度誤差中，寬度和高度的誤差基本不超過(guò)0.09 m，長(zhǎng)度誤差稍高，但最大值不超過(guò)0.24 m。這部分誤差相對(duì)較低，原因在于車(chē)輛的長(zhǎng)、寬、高的取值范圍本身并不大，同時(shí)在估計(jì)的過(guò)程中，最終預(yù)測(cè)值是網(wǎng)絡(luò)輸出的估計(jì)偏差與先驗(yàn)的長(zhǎng)、寬、高的統(tǒng)計(jì)平均值之和，這樣也保證了最終的結(jié)果不會(huì)有很大的偏差。在角度誤差中，隨著距離的上升，誤差增大，最大值約為0.18 rad。

同時(shí)將本文算法和當(dāng)前精度最高的單目位姿估計(jì)算法子類(lèi)別感知卷積神經(jīng)網(wǎng)絡(luò)（Subcategory-aware Convolutional Neural Networks，Sub-CNN）[12]進(jìn)行精度對(duì)比。采用相機(jī)坐標(biāo)系中心到3D目標(biāo)中心點(diǎn)的距離作為距離評(píng)價(jià)指標(biāo)，誤差如圖13 所示。本文算法相對(duì)于Sub-CNN而言，在不同距離下均有更高的精度，平均誤差比Sub-CNN小0.371 2 m。

圖13 本文算法和Sub-CNN距離誤差

2 種方法采用平均方向相似性（Average Orientation Similarity，AOS）作為角度評(píng)價(jià)指標(biāo)[13]。以Sub-CNN 和本文算法估計(jì)的姿態(tài)角作為評(píng)價(jià)對(duì)象，Sub-CNN 算法平均方向相似性為85.99%，本文算法為86.14%，比Sub-CNN高約0.15百分點(diǎn)。

3.3.4 位姿估計(jì)網(wǎng)絡(luò)速度測(cè)試

位姿估計(jì)網(wǎng)絡(luò)前傳時(shí)間的統(tǒng)計(jì)結(jié)果如圖14 所示，總體分布趨近于正態(tài)分布，均值為0.019 71 s，方差為5×10-7s2，前傳時(shí)間穩(wěn)定。

圖14 前傳時(shí)間

4 結(jié)束語(yǔ)

針對(duì)目標(biāo)位姿估計(jì)問(wèn)題，本文基于單目視覺(jué)原理，提出了一種基于卷積神經(jīng)網(wǎng)絡(luò)的位姿估計(jì)算法。首先利用KITTI數(shù)據(jù)集的車(chē)輛長(zhǎng)、寬、高均值進(jìn)行計(jì)算，對(duì)長(zhǎng)、寬、高和均值的偏差進(jìn)行尺度估計(jì)；對(duì)角度采用分類(lèi)代替回歸的方式，先對(duì)角度進(jìn)行分塊的方式進(jìn)行分類(lèi)，再對(duì)分類(lèi)的偏差值進(jìn)行小角度的回歸以估計(jì)目標(biāo)的姿態(tài)角；最后根據(jù)估計(jì)出的目標(biāo)長(zhǎng)、寬、高和姿態(tài)角，利用3D Box 和2D Box 投影關(guān)系對(duì)目標(biāo)中心點(diǎn)距離進(jìn)行建模估計(jì)。對(duì)整個(gè)位姿估計(jì)網(wǎng)絡(luò)訓(xùn)練并測(cè)試，結(jié)果表明，本文所提出的車(chē)輛位姿估計(jì)網(wǎng)絡(luò)實(shí)時(shí)性較好，且在不同目標(biāo)距離下與當(dāng)前最優(yōu)的算法Sub-CNN相比，均有更高的估計(jì)精度。