周曉春 梁 軍 陳 龍 王亞飛 龔進峰
1.江蘇大學(xué)汽車工程研究院,鎮(zhèn)江,2120132.上海交通大學(xué)機械與動力工程學(xué)院,上海,200240 3.中國汽車技術(shù)研究中心汽車工程研究院,天津,300300
高級駕駛輔助系統(tǒng)[1](advanced driver assistance system, ADAS)以及行車主動服務(wù)系統(tǒng)[2](driving active service system, DASS)的發(fā)展推動著汽車安全與服務(wù)的智能化。智能網(wǎng)聯(lián)車(intelligent connected vehicle, ICV)已成為汽車行業(yè)未來的主要發(fā)展方向,而環(huán)境感知傳感器[3](environmental sensing sensor, ESS)作為ICV的重要組成部分,在智能出行過程中發(fā)揮著重要作用。針對單點探測存在因環(huán)境條件及噪聲干擾導(dǎo)致的感知精度低的局限[4],國內(nèi)外學(xué)者重點研究了多傳感器信息融合,信息融合具體可分為數(shù)據(jù)級、特征級、決策級三部分[5]。文獻[6]提出了一種基于傳感器融合的障礙物檢測和識別系統(tǒng),使用毫米波雷達檢測障礙物的位置和速度,并利用邊界框回歸算法精確定位和識別障礙物,結(jié)果顯示該系統(tǒng)的檢出率高達91.6%。文獻[7]提出了一種基于改進的聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)的車用多傳感器跟蹤融合算法并進行實車試驗,結(jié)果表明前方檢測目標(biāo)能被穩(wěn)定跟蹤且具備較好的檢測精度。文獻[8-9]設(shè)計了兩種不同的卡爾曼濾波器,分別對雷達及攝像機探測目標(biāo)進行跟蹤并基于貝葉斯估計進行目標(biāo)融合,結(jié)果表明卡爾曼濾波可顯著降低測量噪聲且融合算法可提高估計精度。
上述研究都是基于固定傳感器組合的,未考慮融合網(wǎng)絡(luò)中傳感器的動態(tài)可擴展,無法實現(xiàn)傳感器的即插即用(plug-and-play,PnP),包括組件PnP、功能PnP、通信PnP。當(dāng)新增ESS以及ESS失效更新時,需進行重復(fù)繁瑣的組網(wǎng)流程,包括搜索、識別及標(biāo)定。針對上述組網(wǎng)難點,文獻[10]提出了一種支持PnP的物聯(lián)網(wǎng)(internet of things, IoT)設(shè)備的架構(gòu)方案,即SensPnP,可實現(xiàn)傳感器與IoT設(shè)備的無縫集成,并提出了可用于IoT自動管理所接傳感器驅(qū)動程序的算法,結(jié)果表明此方案在降低成本的同時還可提高效率。文獻[11]通過設(shè)計SpaceFibre光網(wǎng)絡(luò)的網(wǎng)絡(luò)管理器并利用寬度遍歷算法檢索網(wǎng)絡(luò)設(shè)備,從而實現(xiàn)節(jié)點設(shè)備在總線網(wǎng)絡(luò)的即插即用,并通過仿真試驗證明了算法具有更優(yōu)的分包機制及網(wǎng)絡(luò)延時。
在上述文獻基礎(chǔ)上,本文提出了一種面向PnP傳感器組網(wǎng)模型,通過網(wǎng)絡(luò)自搜索(self-search,SS)-自識別(self-identification,SI)-自標(biāo)定(self-calibration,SC)(簡稱“S-SIC”)實現(xiàn)ESS在ADAS域的自動接入與主動標(biāo)定,并基于K-means聚類與動態(tài)加權(quán)融合算法(簡稱“K-m.AW”)準(zhǔn)確預(yù)測出目標(biāo)軌跡融合坐標(biāo)集。該模型對降低組網(wǎng)難度,實現(xiàn)ESS即插即用,提高ADAS感知系統(tǒng)魯棒性,建立ICV即插即用嵌入式軟件平臺具有重要意義。
圖1所示為S-SIC模型框架,包括SS、SI及SC三部分。SS模型通過深度優(yōu)先搜索規(guī)劃(depth-first search, DFS)算法搜索即插ESS,并給定ESS目標(biāo)ID作為下一次搜索的依據(jù)。SI模型利用搜索路徑將ESS基礎(chǔ)配置信息推送至域控制器,用于該ESS信息記錄以及資源協(xié)調(diào)。SC模型以車體坐標(biāo)系(vehicle body coordinate system, VBCS)為轉(zhuǎn)換媒介,分別建立相機系統(tǒng)(camera system, CS)、毫米波雷達坐標(biāo)系(millimeter wave radar coordinate system, MWRCS)、激光雷達坐標(biāo)系(lidar coordinate system, LCS)的轉(zhuǎn)換矩陣,實現(xiàn)三者之間的外參求解以及位置姿態(tài)誤差補償。
圖1 S-SIC模型框架
1.1.1初始化
新ESS節(jié)點接入總線網(wǎng)絡(luò)后,域控制器DCU將自動創(chuàng)建一個Visited數(shù)組,用于記錄所有被訪問的ESS節(jié)點,其中DCU本身為初始節(jié)點。
1.1.2模型建立
將ICV上的ESS與ADAS域控制器DCU分別用字符Si與C表示。字符C表示DCU,是負責(zé)搜索的發(fā)起節(jié)點;Si(i=1,2,…,n)表示車輛上已有的ESS節(jié)點;Sm(m=n+1)表示目標(biāo)節(jié)點,等待被訪問。圖2所示為基于DFS算法的ESS搜索模型(六節(jié)點),黑色圓圈表示搜索的發(fā)起節(jié)點以及訪問節(jié)點,灰色圓圈表示搜索的待訪問節(jié)點,而白色圓圈則表示尚未被訪問的節(jié)點。每進行一次搜索則代表一處節(jié)點被訪問,當(dāng)目標(biāo)節(jié)點被訪問完成后則停止搜索。表1顯示了搜索完畢后各ESS節(jié)點參數(shù)值的變化情況。當(dāng)除DCU的ESS參數(shù)值全為0時,表示搜索未開始;當(dāng)ESS節(jié)點出現(xiàn)0-1變化時,表示搜索正在進行;當(dāng)參數(shù)值全為1時,則表示搜索完成。具體算法流程如下:
圖2 基于DFS的ICV傳感器搜索模型(六節(jié)點)
表1 ESS參數(shù)值變化過程
(1)初始化DCU節(jié)點C與各ESS節(jié)點Si,其中節(jié)點C作為搜索發(fā)起節(jié)點,其值為1,其他未被訪問節(jié)點的值為0。
(2)DCU節(jié)點C首次搜尋其鄰近未被訪問的ESS節(jié)點Sj,并對該節(jié)點進行訪問,此時節(jié)點Sj值由0變化到1;以節(jié)點Sj為新起始節(jié)點進行下一輪搜索,直至訪問過的節(jié)點不再有未被訪問的鄰近節(jié)點為止。
(3)返回前一個已訪問過的且擁有未被訪問鄰近點的節(jié)點,繼續(xù)訪問該節(jié)點的下一個未被訪問的鄰接節(jié)點。
(4)重復(fù)步驟(2)、步驟(3),直至目標(biāo)節(jié)點Sn被訪問,搜索結(jié)束。
流程圖見圖3。
ESS經(jīng)總線搜索后推送本身基礎(chǔ)配置信息報文至域控制器,域控制器在獲取ESS的當(dāng)前狀態(tài)參數(shù)后確定其功能及服務(wù),表2所示為部分典型基礎(chǔ)識別參數(shù)。為實現(xiàn)可即插即用的ESS組網(wǎng),DCU須識別即插ESS位置及方位等預(yù)設(shè)參數(shù)。因存在行車振動、復(fù)雜天氣等不可控性以及人為因素,使得部分參數(shù)(如時序精度Ac、采樣率Sr等參數(shù))產(chǎn)生區(qū)域波動進而直接影響決策級融合的準(zhǔn)確性,如權(quán)值分配Wd及數(shù)據(jù)更新Du。適應(yīng)K-m.AW決策級融合算法的二次參數(shù)Ac定義如下:
表2 典型基礎(chǔ)參數(shù)識別列表
(1)
Sr={α,β,γ,…}
(2)
式中,Me為ESS在[T1,T2]時間序列下的最大誤差;Fm為ESS滿程測量范圍;α,β,γ,…為Sr的多觸發(fā)狀態(tài)。
多傳感器標(biāo)定[12-13]是指通過一定方法尋找不同種類或是相同種類下不同探測角度傳感器在時空上的對應(yīng)關(guān)系,即求解傳感器間的外參[14]。NOBRE等[15]提出了一種在預(yù)先不知道系統(tǒng)初始狀態(tài)、無需標(biāo)定目標(biāo)或特殊初始化序列的狀態(tài)下,對多傳感器的內(nèi)外標(biāo)定參數(shù)進行定時自標(biāo)定和變化檢測的系統(tǒng)解決方案,并以相機IMU外部標(biāo)定為對象驗證了該系統(tǒng)的高可靠性。PANDEY等[16]提出了一種基于信息論、以數(shù)據(jù)驅(qū)動且無需任何基準(zhǔn)物的自標(biāo)定算法,并實驗驗證了該算法在機器人應(yīng)用中的魯棒性和準(zhǔn)確性。同理,在ICV上,ESS坐標(biāo)系隨外界變化的不確定性會導(dǎo)致外參在預(yù)定范圍部分微調(diào),對該微調(diào)進行補償是非常必要的。如圖4所示,Apollo 3.5系統(tǒng)存在多種ESS且預(yù)安裝位置分散,角度多變。研究ESS特征差異及其輸出參數(shù)所依賴的坐標(biāo)系成為確定轉(zhuǎn)換矩陣函數(shù)及補償算法的重點。為適應(yīng)ESS即插即用及K-m.AW算法,選擇VBCS為中間坐標(biāo)系作為轉(zhuǎn)換媒介,其他坐標(biāo)系涉及CS、MWRCS以及LCS等ESS坐標(biāo)系。
圖4 Apollo 3.5多傳感器安裝位置和坐標(biāo)系的俯視圖
1.3.1CS與VBCS轉(zhuǎn)換
VBCS是描述ICV當(dāng)前道路場景及交通路況最直觀的坐標(biāo)體系,空間坐標(biāo)可表示為(XV,YV,ZV),其中,原點為后車橋中心位置,XV指向車輛右側(cè),YV指向車輛前方,ZV指向天空。CS為一種泛稱,根據(jù)光心、圖像位置的不同又可分為像素坐標(biāo)系、圖像坐標(biāo)系以及相機坐標(biāo)系。像素坐標(biāo)系(pixel coordinate system, PCS)可表示為(u,v),其中原點為生成圖像的左上方,u指向圖像的右側(cè),v指向圖像的下方;圖像坐標(biāo)系(image coordinate system, ICS)可表示為(x,y),其中,原點為相機光軸與圖像坐標(biāo)系的交點處,x指向圖像的右側(cè),y指向圖像的下方;相機坐標(biāo)系(camera coordinate system, CCS)可表示為(XC,YC,ZC),其中,原點OC為相機光心,XC指向光心即相機右側(cè),YC指向光心上方,ZC指向光心前方。轉(zhuǎn)換過程如下:
(1)PCS轉(zhuǎn)化為ICS,存在平移與伸縮變換(相機內(nèi)CCD傳感器的存在),不涉及旋轉(zhuǎn)變換,即
(3)
(2)ICS轉(zhuǎn)換為CCS,存在空間維數(shù)的變化,利用透視投影實現(xiàn)轉(zhuǎn)化過程,即
(4)
式中,f0為相機焦距,描述光心到圖像中心的距離。
(3)CCS轉(zhuǎn)化為VBCS,存在旋轉(zhuǎn)變換及平移變換,利用旋轉(zhuǎn)矩陣R和平移矩陣T實現(xiàn)變換,即
(5)
R=R1R2R3
(6)
(7)
(8)
式中,R1、R2、R3為繞XC、YC、ZC軸旋轉(zhuǎn)θ、φ、ω角度的矩陣。
綜上,轉(zhuǎn)換矩陣為
[XVYVZV1]T=
(9)
1.3.2LCS與VBCS轉(zhuǎn)換
VBCS轉(zhuǎn)換同上,不再贅述。LCS空間坐標(biāo)表示為(XL,YL,ZL),其中,原點OL為雷達中心,XL指向輸出電纜的反方向,若雷達安裝在車輛后方,則YL指向車輛右側(cè),ZL指向天空。LCS與VBCS只存在旋轉(zhuǎn)及平移變換,參考1.3.1節(jié),轉(zhuǎn)換矩陣如下:
(10)
1.3.3MWRCS與VBCS轉(zhuǎn)換
VBCS轉(zhuǎn)換同上,不再贅述。MWRCS與上述兩種ESS的主要區(qū)別在于它是基于二維極坐標(biāo)系的目標(biāo)檢測,原點OM為雷達掃射平面的中心點,LM為監(jiān)測范圍內(nèi)目標(biāo)物與中心點的相對距離,αM為目標(biāo)物與中心點的相對角度。直角坐標(biāo)系轉(zhuǎn)換如下:
(11)
式中,Y0為轉(zhuǎn)換后XMOMZM平面至XVOVZV平面的相對距離。
故轉(zhuǎn)換矩陣如下:
(12)
1.3.4誤差補償算法設(shè)計
為補償行車過程中因外力因素產(chǎn)生的傳感器偏移,設(shè)計一種基于坐標(biāo)反算的補償算法,具體如下:
(13)
式中,dx、dy、dz、dθ、dφ、dω為相對預(yù)設(shè)位置的偏移量;(Xij,Yij,Zij)、(Xik,Yik,Zik)為待補償模塊i在探測范圍內(nèi)探測到的第j、k個目標(biāo);n為模塊總探測數(shù)。
如表3所示,決策級信息融合在通信帶寬、環(huán)境要求、向量維數(shù)、計算資源方面體現(xiàn)出其他融合方式無可比擬的優(yōu)勢。目前,常見的用于決策級數(shù)據(jù)融合估計算法的有貝葉斯估計[17]、D-S證據(jù)理論[18]和模糊集理論[19]等。然而,貝葉斯估計屬于參數(shù)化估計,其結(jié)果的準(zhǔn)確性很大程度依賴先期假設(shè)的概率分布,若假設(shè)存在偏差,會嚴(yán)重影響最后的目標(biāo)估計;D-S證據(jù)理論雖可避免求解先驗概率,但在證據(jù)獲取及計算量方面存在困難;模糊集理論通過引入隸屬函數(shù)對數(shù)字化信息進行寬松建模,但算法直觀性較差且運算相對復(fù)雜。針對上述不足,本文利用K-means聚類與動態(tài)加權(quán)函數(shù)(K-m.AW)進行信息融合估計。
表3 三類信息融合方法比較
聚類屬于無監(jiān)督學(xué)習(xí),相比于分類,聚類不依賴預(yù)定義的類和類標(biāo)號的訓(xùn)練實例[20]。在ICV環(huán)境感知過程中,ESS會生成一系列均勻離散的軌跡預(yù)測點,且不可避免地會造成預(yù)測偏差,無法保證感知精度,通過K-means聚類算法[21]的多次聚類可對預(yù)測集進行預(yù)測值分類并形成多類簇。在此基礎(chǔ)上,利用基于時間序列的動態(tài)加權(quán)函數(shù)對簇預(yù)測均值進行權(quán)值分配并確定融合目標(biāo)點坐標(biāo),如圖5所示。
圖5 基于K-m.AW融合算法的目標(biāo)定位估計流程
(1)假設(shè)第i(i=1,2,…,n)個ESS在tu(u=1,2,…,N)時刻的目標(biāo)坐標(biāo)值為Ki(tu),在tc(c=1,2,…,N)時刻,從Camera(CA)、Rader(RA)、Lider(LI)預(yù)測集群中隨機選取3個元素作為初始預(yù)測簇中心,記為K′1、K′2、K′3。
(2)利用ADAS域控制器計算全部ESS預(yù)測對象Ki(tc)到三類簇中心K′1、K′2、K′3的相異度(dissimilarity, DI),并將全部元素劃歸至相異度最低的簇。由于預(yù)測值僅存在三維坐標(biāo)值的差異,故該相異度可利用不同ESS預(yù)測對象至K′1、K′2、K′3的歐幾里得距離(Euclid distance, ED)來區(qū)別,定義如下:
(14)
m=1,2,3
(3)根據(jù)聚類結(jié)果,重新計算3個新簇中心,記為K″1、K″2、K″3,其三維坐標(biāo)值由簇內(nèi)預(yù)測值的平均數(shù)得到。
(4)將預(yù)測集中全部元素根據(jù)新簇中心點K″1、K″2、K″3重新聚類。
(5)重復(fù)步驟(4),直到聚類結(jié)果不再變化,將結(jié)果輸出,此時三類簇中心分別為K1、K2、K3。
tc時刻,三類簇中心平均值為
(15)
(16)
累計三類簇在時間序列上前tc時測量值的平均誤差為
(17)
式中,y為前tc時的測量次數(shù)。
由式(14)可知,在tc時刻三類簇測量誤差的標(biāo)準(zhǔn)差為
(18)
式(18)所求標(biāo)準(zhǔn)差近似為簇中心的均方差,由此可知在tc時刻,K1、K2、K3在加權(quán)融合時的權(quán)值為
(19)
由此可知最終預(yù)測點為
(20)
為驗證ESS組網(wǎng)模型效果,包括1.1節(jié)SS算法的準(zhǔn)確性、實時性以及1.3節(jié)SP矩陣的補償性能,采用PreScan傳感器模型中主流ESS,在相同場景下進行搜索實驗并人為設(shè)置ESS位置、姿態(tài)誤差以測試SP矩陣補償性能,記錄搜索結(jié)果并標(biāo)定誤差,具體流程如下。
(1)選取PreScan中4類ESS,包括Camera(CA)、Rader(RA)、Lider(LI)、Ultrasonic(UI),并進行編號,記為N1、N2、N3、N4。
(2)將CA接入ESS網(wǎng)絡(luò),利用DFS算法進行全局搜索,若搜索成功,則記錄時間T;反之記錄時間為∞。重復(fù)搜索10次,記錄搜索成功次數(shù)δ1、搜索失敗次數(shù)δ2及時間Ti(i=1,2,…,10)。
(21)
(22)
j=1,2,3,4
(4)為模擬環(huán)境及車體造成的影響,人為設(shè)置4類ESS定誤差ΔF,p個探測目標(biāo)(p≥2),基于補償算法求解誤差補償值?F,ESS參數(shù)設(shè)置如表4所示,統(tǒng)計相對誤差Re,并取相對平均誤差M(Re)為評價層標(biāo)準(zhǔn),定義如下:
表4 ESS參數(shù)設(shè)置
(23)
(24)
式中,ΔF[n]為矩陣ΔF=[ΔxΔyΔzΔθΔφΔω]的第n個參數(shù);F[n]為預(yù)設(shè)位置姿態(tài)矩陣?F=[?x?y?z?θ?φ?ω]的第n個參數(shù);N(Re)為Re數(shù)組量,取N(Re)=6。
實驗框架如圖6所示。
圖6 ESS組網(wǎng)實驗框架
表5 10組實驗搜索結(jié)果
(a)攝像頭CA
補償誤差結(jié)果如圖8所示,其中:
圖8 ESS相對誤差Re(SC)
(25)
n=1,2,…,6
符合標(biāo)準(zhǔn)設(shè)定,使得補償算法在預(yù)測?F=[?x?y?z?θ?φ?ω]多維度上具備良好的補償性能且矩陣M處于M(Re)標(biāo)準(zhǔn)值中段,使自標(biāo)定SC存在一定的冗余性,保證誤差在可控范圍。其中,矩陣M為
(26)
為驗證面向PnP的K-m.AW算法的性能,采用PreScan-Simulink仿真平臺,利用PreScan傳感器模型中典型的CA、LI、RA三類ESS搭配在特定場景及工況下進行前方目標(biāo)軌跡估計,場景設(shè)置見圖9,包含直道S1、彎道S2、上下坡S3三類典型場景,實驗參數(shù)設(shè)置如表6所示。
表6 實驗參數(shù)設(shè)置表
圖9 PreScan場景示意圖
為模擬環(huán)境要素對RA、LI的影響,實驗引入高斯噪聲G,其中,方位角GaAzimuth=0.80°,方程GElevtion=0.50°,范圍GRange=0.7 m。
為比較算法的適用程度,在相同場景工況下采用兩類對比算法,包括貝葉斯估計、模糊集理論,其中,先驗概率P(i)={0.1,0.3,0.6},i=CA,RA,LI,隸屬度函數(shù)置為LR型。
如圖9所示,目標(biāo)車輛(intention vehicle, IV)依照場景及工況設(shè)置由路段節(jié)點1行駛至節(jié)點7,其中,1~2、3~4、5~6為直道場景;2~3、6~7為彎道場景;4~5為上下坡場景。ICV搭載上述配置傳感器進行前方目標(biāo)車輛的軌跡估計,并保證目標(biāo)車輛全程在ICV監(jiān)測范圍內(nèi),從Simulink后端采集目標(biāo)車輛的實際行車軌跡及預(yù)測軌跡。為測試即插即用組網(wǎng)模型在軌跡估計方面的有效性及優(yōu)越性,利用上述隨機模塊在全路段接入(connect, C)或斷連(disconnect, D)三類模塊,并統(tǒng)計有效測試結(jié)果及實驗過程中三類ESS在相同時序前Tc不同場景下的平均誤差率Ar|t=Tc及不同時序前Tc同一場景下的平均誤差率Ar|t=Tc,i(i=1,2,…,n),匹配多場景下的Tc和Ar的最優(yōu)區(qū)間。
(a)CA直道場景(A1) (b)CA彎道場景(B1)(c)CA上下坡場景(C1)
表7 PnP統(tǒng)計數(shù)量及平均誤差率
圖11描述了10 s內(nèi)Tc與Ar的匹配值及局部最優(yōu)區(qū)間,結(jié)果顯示直道場景下Tc對Ar的影響最小,表明該類場景下ESS探測的誤差較小且趨近穩(wěn)定;而彎道及上下坡場景下不同Tc對Ar影響較為明顯,其中ESS探測彎道及上下坡場景的Tc的局部最優(yōu)區(qū)間分別為[2.1 s,3.3 s]、[1.6 s,2.8 s],表明在彎道及上下坡的場景工況下ESS探測平均誤差較直道場景工況波動較大,可推斷時間序列中初始時間戳誤差估計數(shù)據(jù)集對當(dāng)前時間戳的融合誤差影響較為明顯,應(yīng)降低其所占權(quán)重,而末端時間戳的誤差數(shù)據(jù)集影響較小,所占權(quán)重應(yīng)適當(dāng)增加。結(jié)果驗證了在不同場景下K-m.AW算法在動態(tài)權(quán)值選取時的合理性。
(a)直道場景
(1)建立了S-SIC組網(wǎng)模型,通過自搜索(SS)-自識別(SI)-自標(biāo)定(SC)實現(xiàn)環(huán)境感知傳感器(ESS)的組網(wǎng)流程與即插即用,并在此基礎(chǔ)上提出了基于時間序列上平均誤差的K-m.AW融合算法,提高了目標(biāo)軌跡估計的準(zhǔn)確性。
(2)以S-SIC模型為基礎(chǔ),依據(jù)4類PreScan常用ESS進行了即插即用的SS及SC補償實驗,并利用直道-彎道-上下坡進行了多場景的融合估計驗證,結(jié)果表明,該模型可有效提高ESS的組網(wǎng)效率,搜索成功率與搜索平均時間控制在92%和1.79s;以前方單目標(biāo)軌跡估計為例,該算法在融合估計中較對比算法體現(xiàn)出較為明顯的優(yōu)勢;實驗統(tǒng)計出的多場景Tc最優(yōu)區(qū)間對算法動態(tài)權(quán)值的選取具有重要參考價值;同時在多場景中隨機實現(xiàn)即插即用,進一步驗證了ESS組網(wǎng)模型的適用性及K-m.AW算法在軌跡估計中的有效性。
(3)在后續(xù)研究中,可進行多種類、多數(shù)量ESS橫縱向?qū)Ρ葘嶒炄ヲ炞CESS組網(wǎng)模型的穩(wěn)定性以及分析研究K-m.AW融合算法在多角度、多目標(biāo)軌跡估計預(yù)測的準(zhǔn)確性及可靠性。