王 超,王立勇,蘇清華,丁炳超,張 政,賈曉亮
(1.北京信息科技大學(xué) 現(xiàn)代測控技術(shù)教育部重點實驗室,北京 100192;2.陸軍裝備部駐北京地區(qū)軍事代表局駐臨汾地區(qū)軍事代表室,山西 臨汾 041000)
隨著人工智能技術(shù)高速發(fā)展,無人駕駛車輛已成為各高校與機(jī)構(gòu)的重點研究對象,其主要包含3個關(guān)鍵技術(shù):環(huán)境感知、決策規(guī)劃及車輛控制[1]。其中,環(huán)境感知是實現(xiàn)車輛自主行駛的先決條件,它模擬了人類駕駛員對自身周圍環(huán)境的理解,如在結(jié)構(gòu)化道路中,無人車輛需感知環(huán)境,使其保持在車道線以內(nèi)行駛,對此,有學(xué)者提出針對車道線的可行使區(qū)域檢測方法[2-4]。而路況環(huán)境是復(fù)雜多變的,其中臨時道路作為一種特殊路況之一,常在道路保養(yǎng)與維護(hù)、發(fā)生交通事故以及道路臨時管制等情況下使用交通路錐臨時搭建,以避免事故進(jìn)一步擴(kuò)大及交通引流。另外,為了驅(qū)動無人駕駛技術(shù)的發(fā)展,國內(nèi)外高校與研究機(jī)構(gòu)舉辦的無人駕駛方程式大賽均以交通路錐作為臨時牽引車道[5-8]。針對上述特定場景,常規(guī)的車道線識別方法面臨諸多挑戰(zhàn),對此,有部分學(xué)者提出了解決方案,如李剛等[9]使用四邊形重心的幾何特性確定路錐車道內(nèi)可行駛路徑點,并采用3次樣條插值擬合路徑曲線,但該方法估算出的車輛可行駛路徑較短,且沒有考慮可行駛區(qū)域的安全冗余范圍。史林波等[10]使用Hough變換的方法檢測路錐車道線,根據(jù)檢測結(jié)果擬合車輛的目標(biāo)行駛路徑,此方法計算復(fù)雜度較高,且對彎道檢測魯棒性不強(qiáng)。Wang等[11]使用特征點法對場景視覺特征點進(jìn)行提取并匹配,根據(jù)匹配結(jié)果確定各點之間的幾何約束,并從中分割出可行駛道路,此方法依賴于視覺特征點的提取與匹配,易受環(huán)境光照、載體旋轉(zhuǎn)等影響,且道路檢測結(jié)果中包含較多非道路成分,安全冗余度不高。
針對上述問題,本文提出一種改進(jìn)Delaunay三角剖分算法,通過實時更新與優(yōu)化Delaunay三角網(wǎng)的權(quán)重及損失值,濾除損失值總和不滿足條件閾值的冗余三角邊,從而提升道路檢測準(zhǔn)確率,降低車輛行駛危險。
本文以交通錐桶搭建的臨時道路作為研究對象,參考中國大學(xué)生無人方程式大賽規(guī)則并結(jié)合實際道路應(yīng)用[10],以紅、藍(lán)2種顏色錐桶分別標(biāo)記臨時道路左、右兩側(cè)邊界。交通錐桶長、寬、高分別為20、20、30 cm,橫向間距為3 m,縱向間距為3.8 m,如圖1所示。
圖1 臨時道路場景
交通錐桶的檢測分為3部分,分別為:從相機(jī)采集的彩色圖像中獲取交通錐桶2D顏色信息,從激光雷達(dá)獲取的點云數(shù)據(jù)中獲取交通錐桶3D點云信息,以及2D、3D交通錐桶數(shù)據(jù)融合。
為提取彩色圖像中的交通錐桶的顏色信息,訓(xùn)練檢測錐桶的機(jī)器學(xué)習(xí)模型,首先,將通過相機(jī)采集的紅、藍(lán)色交通錐桶圖像數(shù)據(jù)集使用LabelImg軟件分別進(jìn)行手工標(biāo)注;然后,選取8 000張道路圖像按照7∶2∶1的比例分別構(gòu)建訓(xùn)練集(5 600張)、測試集(1 600張)及驗證集(800張);最后,基于訓(xùn)練集數(shù)據(jù)訓(xùn)練YOLOv4目標(biāo)檢測網(wǎng)絡(luò)[12],直至該網(wǎng)絡(luò)收斂并可實現(xiàn)對道路中不同顏色錐桶實時檢測,獲取錐桶顏色分類結(jié)果及圖像檢測框,如圖2(a)所示。
基于視覺的深度估計方法易受光照、車輛震動等因素影響,無法高精度感知錐桶位置,因此本文采用激光雷達(dá)對道路場景實時掃描,從而獲取錐桶的準(zhǔn)確位置。由于地面不平等因素,運動狀態(tài)下激光雷達(dá)采集的點云數(shù)據(jù)中包含大量地面噪聲點云,且呈現(xiàn)為紋理狀,對后續(xù)錐桶點云聚類帶來干擾,因此,通過線性模型對地面進(jìn)行平面擬合,將原始點云數(shù)據(jù)中的地面噪聲濾除,如式(1)所示,式中,A、B、C、D為平面擬合參數(shù),x、y、z為原始點云坐標(biāo)。
Ax+By+Cz+D=0
(1)
使用RANSAC方法[13]對式(1)進(jìn)行參數(shù)估計,并判斷原始點云到該平面距離是否滿足條件,若點云中滿足該點到平面的正交投影距離小于給定閾值,則該點云屬于地面,將其濾除并保留地面以上的點云。最后,采用歐幾里得算法對錐桶點云進(jìn)行聚類分割。由于該方法會對所有相鄰點云計算歐式距離,運算量較大,為提高算法整體運算效率,在聚類前使用體素柵格法對障礙物點云進(jìn)行降采樣,其中體素柵格閾值設(shè)為0.01,確保在減少點云數(shù)量的同時保留其形狀特征。
由于相機(jī)與激光雷達(dá)對場景的感知是松耦合過程,因此,需要將相機(jī)與激光雷達(dá)進(jìn)行聯(lián)合標(biāo)定,進(jìn)而確定兩者在空間內(nèi)的投影關(guān)系,從而實現(xiàn)圖像中交通錐桶的顏色信息與激光點云中交通錐桶的點云信息的數(shù)據(jù)融合。本文通過構(gòu)建關(guān)于重投影殘差的PnP模型[14],如式(2)所示。
(2)
式中:e為重投影殘差;Iv為圖像中第v個像素坐標(biāo);Pv為第v個點云坐標(biāo),且Iv與Pv均為先驗數(shù)據(jù);sv為第v個點云深度值;K為相機(jī)內(nèi)參矩陣。
為計算投影矩陣T,采用高斯-牛頓非線性優(yōu)化方法對式(2)進(jìn)行迭代優(yōu)化,優(yōu)化過程中存在重投影殘差關(guān)于投影矩陣T的微分運算,由于T是由正交旋轉(zhuǎn)矩陣R和平移向量t所構(gòu)成的特殊歐氏群,其加法運算不具封閉性,因此本文采用李代數(shù)左乘擾動模型實現(xiàn)對投影矩陣T的無約束優(yōu)化。待優(yōu)化完成后從T中取出旋轉(zhuǎn)矩陣R和平移向量t,實現(xiàn)交通錐桶3D點云投影到2D圖像,如式(3)所示,式中Pr為實時檢測到的錐桶3D點云坐標(biāo),Ir為點云投影到圖像中的2D像素坐標(biāo)。
Ir=K(RPr+t)
(3)
由于傳感器標(biāo)定存在誤差,無法確保交通錐桶點云投影后的像素坐標(biāo)與圖像交通錐桶坐標(biāo)重合,因此,通過統(tǒng)計每一個交通錐桶檢測框內(nèi)Ir的數(shù)量來決定融合是否有效,若數(shù)量滿足給定閾值,則對滿足條件的Pr求質(zhì)心并作為該錐桶最終的3D點云坐標(biāo),如圖2(b)所示。
圖2 交通錐桶檢測
Delaunay三角剖分可將離散的交通錐桶集合連接成互不交叉的三角網(wǎng)。該三角網(wǎng)滿足空外接圓準(zhǔn)則和最小角最大準(zhǔn)則,避免產(chǎn)生狹長三角形[15-16],從而確保從錐桶集合中任意交通錐桶位置開始建網(wǎng),最終得到的Delaunay三角網(wǎng)是唯一的。本文以所檢測交通錐桶集合中每個錐桶個體作為三角形頂點,如圖3(a)所示,各頂點位置由交通錐桶點云坐標(biāo)描述,采用逐點插入法[17]構(gòu)建Delaunay三角網(wǎng),具體步驟如下:
1) 創(chuàng)建一個虛擬平面,確保其與道路平面平行,在該平面內(nèi)構(gòu)建一個虛擬三角形包圍盒,確保能將所有交通錐桶包含在包圍盒以內(nèi)。
2) 在虛擬三角形包圍盒內(nèi)插入一點,將該點分別與虛擬三角形各頂點相連,構(gòu)成初始三角網(wǎng),隨后將交通錐桶點集中所有點依次插入到當(dāng)前的三角網(wǎng)中,并記錄包含該點的三角形位置。
3) 在確定插入點位置后,將該點與其所在三角形3個頂點相連,根據(jù)空外接圓準(zhǔn)則刪除構(gòu)成影響域凸包內(nèi)三角形公共邊,最終生成如圖3(b)所示的Delaunay三角網(wǎng)。
由圖3可知,由紅、藍(lán)交通錐桶作為路標(biāo)引導(dǎo)的臨時道路中,對各交通錐桶進(jìn)行Delaunay三角剖分可確定其內(nèi)部的幾何關(guān)系,提取出紅色交通錐桶之間所連接的三角邊即可構(gòu)建出臨時道路左車道線,同理,也可確定藍(lán)色交通錐桶構(gòu)成的右車道線,連接紅、藍(lán)之間所連接的三角邊中點可作為車輛可行駛路徑。
圖3 Delaunay三角剖分示意圖
由于場景的復(fù)雜性以及算法的局限性,無法確保對交通錐桶檢測效果達(dá)到100%準(zhǔn)確率,因此在實際交通錐桶檢測過程中可能出現(xiàn)錯檢,如圖3(a)中,當(dāng)藍(lán)色交通錐桶引導(dǎo)的車道下側(cè)出現(xiàn)異常點,該點為交通錐桶誤識別結(jié)果,而導(dǎo)致在構(gòu)建圖3(b)中Delaunay三角網(wǎng)時出現(xiàn)多條冗余三角邊。此外,當(dāng)檢測到彎道處的交通錐桶,如圖3(b)中的紅色交通錐桶,在對其進(jìn)行Delaunay三角剖分時也會產(chǎn)生冗余三角邊。因此,從Delaunay三角網(wǎng)中直接規(guī)劃車道線與可行駛路徑的錯誤率將增加。
為消除因冗余三角邊數(shù)量的增加而導(dǎo)致無法穩(wěn)定規(guī)劃出車道線與可行駛路徑的問題,本文提出一種針對Delaunay三角網(wǎng)冗余三角邊的濾波器,在不影響Delaunay三角網(wǎng)內(nèi)部幾何約束的前提下實現(xiàn)冗余三角邊濾除。
該算法首先對Delaunay三角網(wǎng)進(jìn)行均值濾波,可避免因交通錐桶擺放位置間距不規(guī)律導(dǎo)致后續(xù)計算三角邊損失值無法收斂的問題。對如圖3(b)所得Delaunay三角網(wǎng),計算紅、藍(lán)交通錐桶連接的三角邊長度總和并求其均值Ms,如式(4)所示。
(4)
式中:n為三角邊數(shù)量;(Xri,Yri)為第i個三角邊對應(yīng)紅色交通錐桶位置坐標(biāo);(Xbi,Ybi)為第i個三角邊對應(yīng)藍(lán)色交通錐桶位置坐標(biāo)。
同種顏色且位置相鄰的交通錐桶所連接三角邊長度總和dr、db,及其均值Md,如式(5)—(7)所示。
(5)
(6)
(7)
式中:m、k分別為紅、藍(lán)色交通錐桶數(shù)量;dr為所有紅色且位置相鄰的2個交通錐桶連線長度總和;db為所有藍(lán)色且位置相鄰的2個交通錐桶連線長度總和;(Xru,Yru)與(Xr(u+1),Yr(u+1))為紅色且位置相鄰的交通錐桶位置坐標(biāo);(Xbj,Ybj)與(Xb(j+1),Yb(j+1))為藍(lán)色且位置相鄰的交通錐桶位置坐標(biāo)。
根據(jù)Ms和Md可計算出2種不同顏色交通錐桶構(gòu)成三角邊的長度損失值W,以及同種顏色且位置相鄰的錐桶所構(gòu)成三角邊的長度損失值D,W和D分別反映了Delaunay三角網(wǎng)中橫向及縱向噪聲約束規(guī)模,如式(8)、(9)所示,其中,(Xr,Yr)與(Xb,Yb)表示不同顏色交通錐桶所構(gòu)成三角邊對應(yīng)紅色、藍(lán)色交通錐桶位置坐標(biāo),(Xm1,Ym1)與(Xm2,Ym2)表示不同顏色交通錐桶構(gòu)成三角邊中2個相鄰三角邊的中點坐標(biāo),也即路徑點坐標(biāo)。
(8)
(9)
根據(jù)路徑點坐標(biāo)可得到3個相鄰路徑點所構(gòu)成三角形的各邊長度:aq,bq,cq,如圖4所示,并以此計算路徑夾角θq,如式(10)所示。為從Delaunay三角網(wǎng)中獲取路徑的損失信息,需對路徑夾角進(jìn)行編碼,得到路徑夾角損失值θ,如式(11)所示,其中,Q為Delaunay三角網(wǎng)中路徑夾角個數(shù)。
(10)
(11)
圖4 路徑夾角示意圖
聯(lián)立式(8)、(9)及(11),計算Delaunay三角邊總損失值,如式(12)所示。
(12)
式中:Nc為交通錐桶誤檢數(shù)量;Wc、Wk、Ww、Wd分別表示錐桶誤檢縮放系數(shù)、路徑角度縮放系數(shù)、車道寬度縮放系數(shù)、路徑長度縮放系數(shù)。
通過設(shè)定閾值γ1,將損失值L大于γ1的三角邊進(jìn)行濾除,從而有效降低冗余三角邊的數(shù)量。為避免在不同環(huán)境下出現(xiàn)非冗余三角邊的誤濾除,需根據(jù)交通錐桶擺放位置的前后及左右間距變化實時更新閾值γ1,如式(13)所示。
(13)
對濾除冗余三角邊后的Delaunay三角網(wǎng),可根據(jù)三角網(wǎng)內(nèi)各頂點顏色特征及其關(guān)聯(lián)的點云坐標(biāo)構(gòu)建車道線與可行駛路徑,即在三角網(wǎng)內(nèi)依次連接紅色錐桶,得到左車道線;同理,對藍(lán)色錐桶處理得到右車道線,左右車道線之間的區(qū)域為車輛可行駛區(qū)域,如圖5所示。每當(dāng)獲取到新的錐桶數(shù)據(jù)時需對原Delaunay三角網(wǎng)進(jìn)行更新,并對更新后的三角網(wǎng)再次構(gòu)建車道線及可行駛路徑。更新時需對插入交通錐桶后所構(gòu)成的新三角邊進(jìn)行空圓特性檢測,遍歷新Delaunay三角網(wǎng),并計算各三角邊損失值。該過程本質(zhì)上是對整體Delaunay三角網(wǎng)進(jìn)行全局更新,當(dāng)出現(xiàn)大量交通錐桶數(shù)據(jù)時會導(dǎo)致運算量增大,對后續(xù)可行駛路徑及車道線的規(guī)劃效率造成一定影響。
圖5 路徑夾角示意圖
為保證算法整體運行效率,當(dāng)檢測到新的交通錐桶數(shù)據(jù)時,采用滑動窗口法對Delaunay三角網(wǎng)進(jìn)行局部優(yōu)化,具體實施步驟如圖6所示。在更新Delaunay三角網(wǎng)時,需對部分歷史錐桶數(shù)據(jù)進(jìn)行濾除,通過判斷錐桶3D點云坐標(biāo)與車輛的相對位置,將車輛后方的交通錐桶數(shù)據(jù)進(jìn)行濾除,避免其參與Delaunay三角剖分,確保后續(xù)只對車輛前方滑動窗口內(nèi)的交通錐桶數(shù)據(jù)進(jìn)行優(yōu)化。
優(yōu)化過程中需確保新插入的交通錐桶數(shù)據(jù)在滑動窗口內(nèi)與歷史幀交通錐桶數(shù)據(jù)產(chǎn)生關(guān)聯(lián),由于冗余三角邊的濾除過程是由單獨的線程負(fù)責(zé)計算,因此可將該線程所得到的三角邊均值Ms與Md作為歷史幀交通錐桶數(shù)據(jù),隨后對更新后的Delaunay三角網(wǎng)計算標(biāo)準(zhǔn)差損失值。
圖6 Delaunay三角網(wǎng)局部優(yōu)化流程
由紅、藍(lán)2種不同顏色交通錐桶構(gòu)成三角邊的標(biāo)準(zhǔn)差損失值Lw如式(14)所示。
(14)
式中:wg為2種不同顏色交通錐桶構(gòu)成三角邊中第g個三角邊的長度;G為2種不同顏色交通錐桶構(gòu)成三角邊的數(shù)量。
由同種顏色且位置相鄰的交通錐桶所構(gòu)成三角邊的標(biāo)準(zhǔn)差損失值Lb如式(15)所示。
(15)
式中:re為紅色相鄰交通錐桶所構(gòu)成三角邊中第e個三角邊長度;bt為藍(lán)色相鄰交通錐桶所構(gòu)成三角邊中第t個三角邊長度;E和B分別為紅色相鄰交通錐桶、藍(lán)色相鄰交通錐桶所構(gòu)成三角邊的數(shù)量。
根據(jù)Lw及Lb可計算三角網(wǎng)局部優(yōu)化損失值Lwb,如式(16)所示。
Lwb=αLw+βLb
(16)
式中:α、β分別為路徑標(biāo)準(zhǔn)差縮放系數(shù)、車道標(biāo)準(zhǔn)差縮放系數(shù)。
新插入的交通錐桶將作為新的頂點并關(guān)聯(lián)多個三角邊,通過多次計算局部優(yōu)化損失值Lwb,選取損失值最小的三角邊中點作為路徑更新量,并將其與前一幀路徑點相連,同時,將該三角邊2個端點根據(jù)交通錐桶顏色特征分別與前一幀車道線端點相連,作為車道線更新量,如圖7所示。
圖7 車道線及路徑更新示意圖
實車實驗過程中,采用無人方程式賽車作為移動平臺,如圖8所示。無人方程式賽車車身軸距為1.6 m,搭載RS-LDAR-32激光雷達(dá)1臺,ZED雙目立體相機(jī)1臺,RTK組合導(dǎo)航模塊1套,英偉達(dá)Jetson Xavier一臺。32線激光雷達(dá)的測距范圍為0.4~200 m,可感知道路上的交通錐桶距離;ZED相機(jī)分辨率為1 280×720,圖像采集頻率為60 Hz,用于實時獲取道路圖像信息;RTK組合導(dǎo)航模塊用于車輛實時定位;Jetson Xavier作為車輛嵌入式計算機(jī)運行環(huán)境感知、道路檢測程序。
圖8 無人方程式賽車
將Delaunay三角網(wǎng)濾波算法中的縮放系數(shù)Wc、Wk、Ww、Wd分別設(shè)置為1、0.5、0.25、0.25;三角網(wǎng)局部優(yōu)化縮放系數(shù)α、β分別為1.5、0.2。由紅、藍(lán)2種顏色交通錐桶構(gòu)建的臨時道路長度約103 m,車輛行駛速度為10 km/h。為驗證算法的有效性,在正常光照環(huán)境下對臨時道路中直行、右轉(zhuǎn)及左轉(zhuǎn)車道進(jìn)行實車實驗,如圖9所示。
圖9 正常光照環(huán)境下臨時道路檢測結(jié)果
在圖9(a)直行車道場景中,受遠(yuǎn)處右側(cè)車道交通錐桶的影響,Delaunay三角網(wǎng)增加了4條冗余三角邊,導(dǎo)致Delaunay三角剖分算法無法穩(wěn)定規(guī)劃出車道線,且計算可行駛路徑時產(chǎn)生錯誤偏移,如圖9(b)所示。在圖9(d)右轉(zhuǎn)車道及圖9(g)左轉(zhuǎn)車道2種場景中,受錐桶誤識別以及彎道同側(cè)交通錐桶的影響,Delaunay三角剖分算法均無法正常檢測出車道線與可行駛路徑。然而,通過增加三角網(wǎng)濾波和局部優(yōu)化,直行、右轉(zhuǎn)及左轉(zhuǎn)路試的實時車道檢測和可行駛路徑規(guī)劃均可快速、準(zhǔn)確地生成,如圖9(c)、(f)及(i)中所示。
為驗證算法的魯棒性,在不同光照環(huán)境下進(jìn)行實車實驗,如圖10所示。傳統(tǒng)Delaunay三角剖分算法無法正確規(guī)劃出可行駛路徑,且車道線檢測不穩(wěn)定,如圖10(b)及(e)中所示。然而,改進(jìn)Delaunay三角剖分算法在弱光及逆光場景下均可穩(wěn)定、準(zhǔn)確檢測出車道線與可行駛路徑,如圖10(c)及(f)中所示,因此,本文算法具有良好的魯棒性。
圖10 不同光照環(huán)境下臨時道路檢測結(jié)果
為進(jìn)一步驗證本文算法的準(zhǔn)確性,使用RTK組合導(dǎo)航對上述臨時道路進(jìn)行數(shù)據(jù)采集。本實驗中,首先,由人工駕駛無人方程式賽車以10 km/h的速度完成整個賽道行駛,駕駛過程中,車輛盡可能處于道路正中間,RTK組合導(dǎo)航、激光雷達(dá)及相機(jī)共同實時記錄車輛行駛的GPS數(shù)據(jù)、場景障礙物點云信息以及交通錐桶圖像信息。其次,通過對數(shù)據(jù)集中GPS數(shù)據(jù)進(jìn)行路徑可視化,可得到車輛真實行駛軌跡。隨后,基于該數(shù)據(jù)集中的障礙物點云與道路圖像數(shù)據(jù),使用本文算法完成道路檢測和可行駛路徑計算。最后,通過與人工駕駛的GPS路徑比較,判斷改進(jìn)Delaunay三角剖分算法是否可行,實驗結(jié)果如圖11所示。
圖11 可行駛路徑與GPS路徑對比
在同樣的環(huán)境下,表1給出不同算法實驗對比結(jié)果。由表1可知,文本算法所檢測可行駛路徑絕對軌跡誤差為0.2 m,相比其他算法均有所降低。另外,本文算法處理每幀數(shù)據(jù)平均耗時為35.4 ms,路徑檢測長度為8.4 m,準(zhǔn)確率為97%,在滿足實時性的同時提高了可行駛路徑檢測長度及檢測準(zhǔn)確率。
表1 不同算法實驗對比
本文提出一種改進(jìn)Delaunay三角剖分算法,用于檢測由交通錐桶引導(dǎo)的臨時道路,在實車實驗中規(guī)劃出車道線及車輛可行駛路徑,得出的主要結(jié)論如下:
1) 提出一種Delaunay三角網(wǎng)濾波算法,根據(jù)路況變化實現(xiàn)Delaunay三角網(wǎng)權(quán)重與損失值的實時計算,有效減少Delaunay三角網(wǎng)內(nèi)的噪聲約束,降低車道線與可行駛路徑檢測誤差。
2) 提出一種Delaunay三角網(wǎng)局部優(yōu)化策略,實時計算三角網(wǎng)局部優(yōu)化損失值,有效濾除交通錐桶信息中的無效數(shù)據(jù),實現(xiàn)車道線與可行駛路徑的快速規(guī)劃與實時更新。
3) 實車實驗結(jié)果表明,本文所提出的改進(jìn)Delaunay三角剖分算法平均耗時為35.4 ms,檢測可行駛路徑的絕對軌跡誤差為0.2 m、準(zhǔn)確率為97%,滿足實時性要求,降低了路徑檢測誤差,提高了路徑檢測準(zhǔn)確率。