龔詩雄，王旭，孔國杰，2，龔建偉

(1.北京理工大學(xué) 機(jī)械與車輛學(xué)院，北京 100081；2.北京特種車輛研究所，北京 100081)

0 引言

多車協(xié)同感知技術(shù)在網(wǎng)聯(lián)車輛或多平臺(tái)作戰(zhàn)系統(tǒng)中具有重要應(yīng)用價(jià)值[1]，相對(duì)于單車感知技術(shù)，通過多車之間的信息交互與融合，可以減小感知盲區(qū)，提升系統(tǒng)環(huán)境感知效果，并實(shí)現(xiàn)感知資源的高效利用。

在協(xié)同目標(biāo)檢測(cè)方法層面，Chen等[2]利用車輛GPS定位與IMU位姿將點(diǎn)云重建為整體點(diǎn)云數(shù)據(jù)，在此基礎(chǔ)上設(shè)計(jì)了一種稀疏卷積神經(jīng)網(wǎng)絡(luò)實(shí)現(xiàn)多車融合點(diǎn)云數(shù)據(jù)的目標(biāo)識(shí)別，能夠在車輛定位不穩(wěn)定的情況也能較為準(zhǔn)確的檢測(cè)目標(biāo)。此后，Chen等[3]在之前研究的基礎(chǔ)上將感知數(shù)據(jù)融合并利用Voxelnet進(jìn)行特征提取，對(duì)提取的特征依據(jù)不同車輛位姿進(jìn)行合并，實(shí)現(xiàn)了較為準(zhǔn)確的目標(biāo)檢測(cè)結(jié)果。Kanaki等[4]基于連續(xù)幀的柵格地圖實(shí)現(xiàn)單車對(duì)環(huán)境動(dòng)態(tài)目標(biāo)進(jìn)行檢測(cè)，并基于衛(wèi)星定位實(shí)現(xiàn)多車檢測(cè)數(shù)據(jù)進(jìn)行拼接。在協(xié)同目標(biāo)跟蹤研究層面，協(xié)同跟蹤方案設(shè)計(jì)依據(jù)多車協(xié)同數(shù)據(jù)融合方式的不同主要分為兩種：一種方案是數(shù)據(jù)級(jí)[5]融合，先將多輛車采集的數(shù)據(jù)進(jìn)行拼接融合，再進(jìn)行目標(biāo)檢測(cè)以及目標(biāo)跟蹤等操作。Kanaki等[4]基于此方案提出了一種基于規(guī)則的數(shù)據(jù)關(guān)聯(lián)方式用于多輪式機(jī)器人聯(lián)合目標(biāo)跟蹤，一定程度上解決了同一目標(biāo)在不同激光雷達(dá)量測(cè)系統(tǒng)中出現(xiàn)量測(cè)沖突的情況。另一種方案是軌跡級(jí)[6]融合，將多輛車先看做獨(dú)立個(gè)體進(jìn)行感知數(shù)據(jù)的處理，再將不同車的目標(biāo)跟蹤軌跡進(jìn)行融合。Gabb等[7]分析了多種協(xié)同目標(biāo)跟蹤的軌跡級(jí)融合方法，提出一種融合道路邊緣設(shè)備跟蹤目標(biāo)軌跡與車載設(shè)備跟蹤目標(biāo)軌跡的方法，該方法簡化了融合步驟，適用于環(huán)境信號(hào)存在干擾的情況，其融合效果在仿真環(huán)境中得到了驗(yàn)證。Ozaki等[8]采用此方案提出了一種多輪式機(jī)器人聯(lián)合的目標(biāo)跟蹤方法，機(jī)器人通過激光雷達(dá)獨(dú)立對(duì)目標(biāo)進(jìn)行跟蹤，當(dāng)兩機(jī)器人在一定距離范圍內(nèi)有交集時(shí)則融合跟蹤軌跡得到更為穩(wěn)定的跟蹤結(jié)果，一定程度上解決了單一機(jī)器人在目標(biāo)遮擋時(shí)無法跟蹤的情況。

以上方法僅考慮了環(huán)境中的動(dòng)態(tài)目標(biāo)，沒有考慮周邊的靜態(tài)環(huán)境。由于沒有建立協(xié)同感知系統(tǒng)的整體模型，導(dǎo)致目標(biāo)經(jīng)常出現(xiàn)短暫丟失現(xiàn)象。本文主要針對(duì)地面無人系統(tǒng)多車協(xié)同目標(biāo)檢測(cè)與跟蹤問題開展討論，通過對(duì)環(huán)境進(jìn)行整體建模，得到多車融合后的全局環(huán)境柵格地圖，減少靜態(tài)環(huán)境障礙物遮擋的影響，并在此基礎(chǔ)上進(jìn)行協(xié)同目標(biāo)檢測(cè)與跟蹤，得到融合后的全局動(dòng)態(tài)檢測(cè)目標(biāo)，設(shè)計(jì)并使用合適的數(shù)據(jù)關(guān)聯(lián)與級(jí)聯(lián)匹配方法，提高了檢測(cè)與跟蹤的連續(xù)性及穩(wěn)定性。

1 多車協(xié)同感知系統(tǒng)設(shè)計(jì)與建模

1.1 多車協(xié)同感知系統(tǒng)設(shè)計(jì)

本文設(shè)計(jì)一種基于激光雷達(dá)的多車協(xié)同感知系統(tǒng)，協(xié)同方案設(shè)計(jì)如圖1所示。本文采用結(jié)果級(jí)融合的集中式方案，系統(tǒng)中每輛車都搭載有處理環(huán)境感知數(shù)據(jù)的運(yùn)算平臺(tái)，能夠?qū)?fù)雜環(huán)境進(jìn)行表征。為了能夠獲得環(huán)境的全局表征數(shù)據(jù)，將每輛車的環(huán)境表征數(shù)據(jù)上傳至部署在指揮車上的主控端，執(zhí)行環(huán)境表征數(shù)據(jù)的融合。在得到全局環(huán)境表征地圖后，可以在此基礎(chǔ)上完成其他功能模塊的設(shè)計(jì)，如多車協(xié)同定位模塊[9]、多車協(xié)同建圖[10]模塊及多車協(xié)同目標(biāo)檢測(cè)與跟蹤模塊[11]等。本文在此基礎(chǔ)上設(shè)計(jì)了一種多車協(xié)同目標(biāo)檢測(cè)與跟蹤方法，主控端在獲得融合結(jié)果后，環(huán)境表征融合數(shù)據(jù)與動(dòng)態(tài)目標(biāo)跟蹤結(jié)果可以為主控端的協(xié)同規(guī)劃與協(xié)同任務(wù)[12-13]布置提供環(huán)境信息，而下發(fā)的跟蹤目標(biāo)狀態(tài)并與車端的環(huán)境表征數(shù)據(jù)結(jié)合可以為車端局部路徑規(guī)劃[14]提供環(huán)境數(shù)據(jù)。

1.2 多車協(xié)同感知系統(tǒng)建模

1.2.1 多車時(shí)間對(duì)準(zhǔn)

由于每輛車由于上位機(jī)硬件時(shí)鐘存在不一致的情況，使得多輛車在同一時(shí)刻的探測(cè)數(shù)據(jù)存在不同的時(shí)間戳，導(dǎo)致數(shù)據(jù)傳至主控端進(jìn)行融合操作時(shí)無法利用時(shí)間戳將同一時(shí)刻感知數(shù)據(jù)進(jìn)行融合，故采用圖2所示GPS授時(shí)方法來統(tǒng)一不同平臺(tái)的時(shí)間戳。

系統(tǒng)中每輛車均接收GPS信號(hào)的統(tǒng)一時(shí)間戳作為車輛本地時(shí)間，并將時(shí)間戳賦予協(xié)同系統(tǒng)中用于交互傳輸?shù)臄?shù)據(jù)，從而使得同一時(shí)刻不同平臺(tái)得到的感知數(shù)據(jù)具有相同的時(shí)間戳。進(jìn)一步地，主控端通過多線程與隊(duì)列的方式實(shí)現(xiàn)各平臺(tái)數(shù)據(jù)的時(shí)間同步。首先，各平臺(tái)的感知數(shù)據(jù)發(fā)送至主控端后均被放入隊(duì)列中進(jìn)行緩存。接著，提取各隊(duì)列頭部消息的時(shí)間戳進(jìn)行比對(duì)，以其中時(shí)間戳最晚的消息作為同步基準(zhǔn)，其他隊(duì)列數(shù)據(jù)的時(shí)間戳與該基準(zhǔn)通過最近鄰的方式進(jìn)行同步。

1.2.2 多車空間對(duì)準(zhǔn)

由于每輛車的傳感器數(shù)據(jù)是以本車自身坐標(biāo)系作為基準(zhǔn)，導(dǎo)致數(shù)據(jù)傳至主控端進(jìn)行融合操作時(shí)無法將基于不同坐標(biāo)系的數(shù)據(jù)進(jìn)行融合，故需要將多輛車的數(shù)據(jù)轉(zhuǎn)換到同一坐標(biāo)系下。在多車空間對(duì)準(zhǔn)方面，單車部分首先要有穩(wěn)定的定位結(jié)果。本文利用衛(wèi)星差分定位技術(shù)提供的經(jīng)緯度與航向信息將車輛位置轉(zhuǎn)至全局坐標(biāo)系UTM[15]。

在獲得多輛車在全局坐標(biāo)系下各自的位置后，由于UTM平面坐標(biāo)系數(shù)值較大，不利于將不同車輛局部坐標(biāo)系下的柵格狀態(tài)或目標(biāo)位置信息進(jìn)行融合。因此，本文利用UTM坐標(biāo)系作為全局坐標(biāo)系實(shí)現(xiàn)車間局部坐標(biāo)系的變換對(duì)準(zhǔn)，即以某一輛車的坐標(biāo)系為基準(zhǔn)，其余車輛均對(duì)準(zhǔn)至該坐標(biāo)系下進(jìn)行感知數(shù)據(jù)的融合，如圖3所示。圖3中，坐標(biāo)原點(diǎn)O為地球中央子午線與赤道的交點(diǎn)，N為子午線所在軸北向，E為赤道所在軸東向，R21和Rn1表示車2和車n到車1的旋轉(zhuǎn)矩陣，T21和Tn1表示車2和車n到車1的平移矩陣。

對(duì)于全局坐標(biāo)系下兩個(gè)局部坐標(biāo)系間的變換矩陣求解，設(shè)定絕對(duì)坐標(biāo)系的原點(diǎn)OU，先將設(shè)定為融合坐標(biāo)系的某一車輛坐標(biāo)系轉(zhuǎn)至全局坐標(biāo)系下，如(1)式：

(1)

其余車輛也將車輛局部坐標(biāo)系轉(zhuǎn)至全局坐標(biāo)系下：

(2)

在得到所有車輛的變換矩陣后，由于最終的目的是將其余車輛坐標(biāo)系轉(zhuǎn)換至某一固定融合用車輛坐標(biāo)系下，還需要將其余車輛坐標(biāo)系利用(3)式進(jìn)行轉(zhuǎn)變。

(3)

至此就完成了協(xié)同感知系統(tǒng)中的多車空間對(duì)準(zhǔn)。

1.2.3 多車協(xié)同感知環(huán)境模型構(gòu)建

如圖4所示，在得到多車協(xié)同感知系統(tǒng)中車輛之間的時(shí)間、空間相對(duì)關(guān)系后，為減小因?yàn)槊^(qū)或遮擋問題導(dǎo)致被遮擋車輛無法感知到前方環(huán)境中靜態(tài)障礙物的情況，有效增大被遮擋車輛的感知范圍，本文采用D-S證據(jù)理論[16]將多車間的環(huán)境柵格地圖進(jìn)行融合得到全局環(huán)境柵格地圖。

同時(shí)，為能夠準(zhǔn)確且全面地表征檢測(cè)結(jié)果，提高計(jì)算效率，采用二維占據(jù)柵格地圖[17]的形式來對(duì)點(diǎn)云進(jìn)行壓縮表征處理，結(jié)果如圖5所示。

如圖6所示，將不同車輛的激光雷達(dá)構(gòu)建的柵格圖進(jìn)行融合，以確定最終融合后的柵格圖中每個(gè)柵格的狀態(tài)，從而實(shí)現(xiàn)多車傳感器融合的環(huán)境表征柵格圖。根據(jù)在單車環(huán)境柵格地圖中，其狀態(tài)主要包含了占據(jù)和空閑兩類，即狀態(tài)空間為Θ={O,F}。進(jìn)一步地，D-S理論則將狀態(tài)空間擴(kuò)展為命題集合2Θ={O,F,Θ,?}，因此對(duì)于每一個(gè)柵格狀態(tài)，D-S證據(jù)理論會(huì)計(jì)算其基本概率分配函數(shù)，得到狀態(tài)空間中這4個(gè)狀態(tài)的信度為[m(F),m(O),m(Θ),m(?))]，這4個(gè)狀態(tài)分別為空閑、占據(jù)、未知以及空集。

此外，還需要針對(duì)單車表征柵格的不同狀態(tài)設(shè)定基本概率分配函數(shù)，當(dāng)單車表征的柵格為占據(jù)時(shí)，設(shè)定分配函數(shù)為

m(F)=0,m(O)=0.94,m(Θ)=0.06,m(?)=0

(4)

當(dāng)單車表征的柵格為空閑時(shí)，設(shè)定分配函數(shù)為

m(F)=0.88,m(O)=0,m(Θ)=0.12,m(?)=0

(5)

當(dāng)單車表征柵格為未知時(shí)，設(shè)定為

m(F)=0,m(O)=0,m(Θ)=1,m(?)=0

(6)

則每輛車所獲得的柵格圖狀態(tài)均有一個(gè)基本信任度集，即

Si={mi(F),mi(O),mi(Θ),mi(?)}i=1,2,…,n

(7)

式中：n為系統(tǒng)中無人車的數(shù)量。

利用上述信任度集對(duì)兩車獲取的柵格狀態(tài)進(jìn)行融合，需要符合上述D-S合成規(guī)則：

(8)

獲得狀態(tài)集S={m(F),m(O),m(Θ),m(?)} 后，需要對(duì)狀態(tài)集進(jìn)行判斷以得到最終多車數(shù)據(jù)融合后的柵格狀態(tài)。此處設(shè)m(F)>0.5時(shí)柵格狀態(tài)為空閑；m(O)>0.5時(shí)柵格狀態(tài)為占據(jù)；m(Θ)>0.5時(shí)柵格狀態(tài)為未知。

圖7(a)所示為兩車點(diǎn)云數(shù)據(jù)。圖7(b)所示為融合柵格圖數(shù)據(jù)。由圖7可以看出，融合后的圖能夠更全面地表征環(huán)境，對(duì)于橘色車輛，前方由于藍(lán)色車的遮擋而無法對(duì)前向障礙物有很好的探測(cè)，而融合后的表征圖則對(duì)全局障礙物進(jìn)行了顯示，利用藍(lán)色車的結(jié)果對(duì)橘色車的探測(cè)不足進(jìn)行了補(bǔ)充。

2 多車協(xié)同目標(biāo)檢測(cè)與跟蹤

多車協(xié)同目標(biāo)檢測(cè)與跟蹤框架如圖8所示，各車先采用基于雷達(dá)鳥瞰圖的目標(biāo)檢測(cè)、得到檢測(cè)目標(biāo)，然后將檢測(cè)結(jié)果與環(huán)境柵格地圖結(jié)合獲取環(huán)境中的動(dòng)態(tài)目標(biāo)，對(duì)多車檢測(cè)目標(biāo)采用非極大值抑制的方法解決檢測(cè)沖突，然后采用聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)和卡爾曼濾波方法，并設(shè)計(jì)了合理的跟蹤管理方法，完成目標(biāo)跟蹤。最后將協(xié)同檢測(cè)后的結(jié)果下發(fā)給各車。

2.1 目標(biāo)檢測(cè)

基于點(diǎn)云深度學(xué)習(xí)的目標(biāo)檢測(cè)由點(diǎn)云表征方式的不同進(jìn)而產(chǎn)生了許多不同的方法，本文采用一種基于激光雷達(dá)鳥瞰圖BEV(Bird’s Eye View)的目標(biāo)檢測(cè)方法BirdNet+[18]，具體網(wǎng)絡(luò)結(jié)構(gòu)如圖9 所示。

在實(shí)際車輛采集的激光雷達(dá)數(shù)據(jù)上的檢測(cè)結(jié)果如圖10所示。

如圖11所示，最終將檢測(cè)結(jié)果進(jìn)行坐標(biāo)與尺度變換投影至動(dòng)態(tài)柵格圖以區(qū)分環(huán)境中的動(dòng)態(tài)目標(biāo)與靜態(tài)目標(biāo)。

2.2 多車目標(biāo)融合規(guī)則

由于采取了不同車輛獨(dú)立對(duì)環(huán)境進(jìn)行障礙物檢測(cè)的方式，兩車的檢測(cè)結(jié)果在感知量測(cè)重疊的范圍內(nèi)會(huì)出現(xiàn)重復(fù)檢測(cè)而產(chǎn)生沖突，為此需要采取一定的方式來解決多目標(biāo)檢測(cè)重疊的問題，采取的流程如圖12所示。

兩車數(shù)據(jù)傳到主控端后，主控端利用協(xié)同架構(gòu)內(nèi)車輛的定位信息首先率除掉協(xié)同架構(gòu)內(nèi)車輛相互檢測(cè)的結(jié)果。在濾除架構(gòu)中車輛的檢測(cè)結(jié)果后，采用NMS的方式對(duì)多車檢測(cè)同一目標(biāo)結(jié)果進(jìn)行濾除。當(dāng)兩個(gè)檢測(cè)框的交并比(IOU)大于一定閾值時(shí)，表明這兩個(gè)目標(biāo)屬于同一個(gè)目標(biāo)，此時(shí)保存目標(biāo)檢測(cè)概率較大的那一個(gè)以此來濾除檢測(cè)沖突。

IOU的原理如圖13所示，具體計(jì)算見(9)式。

(9)

式中：A、B為矩形的面積。

此外，為保證沖突目標(biāo)能夠正確篩出，本文不僅使用了IOU來計(jì)算兩個(gè)檢測(cè)目標(biāo)是否沖突，還加入了歐式距離度量來測(cè)試兩個(gè)目標(biāo)間的距離。兩目標(biāo)中心點(diǎn)歐式距離計(jì)算如(10)式：

(10)

當(dāng)兩個(gè)邊界框的IOU大于一定值并且兩邊界框中心點(diǎn)距離小于一定閾值時(shí)，表明這兩個(gè)目標(biāo)存在檢測(cè)沖突，濾除檢測(cè)概率較小的一個(gè)。最終多車檢測(cè)沖突處理的結(jié)果如圖14所示，圖14(a)為未進(jìn)行目標(biāo)沖突匹配的結(jié)果，深橘色為車1檢測(cè)結(jié)果，深綠色為車2檢測(cè)結(jié)果，可以看到在兩車數(shù)據(jù)進(jìn)行融合后，部分目標(biāo)產(chǎn)生了重疊，如紅圈部分。利用上述匹配規(guī)則進(jìn)行目標(biāo)沖突消除得到的結(jié)果如圖14(b)所示，可以看到車2檢測(cè)的車1結(jié)果被濾除，兩車檢測(cè)的環(huán)境目標(biāo)結(jié)果也無沖突，其中綠色為行人，紫色為車輛。

2.3 目標(biāo)跟蹤

2.3.1 多目標(biāo)數(shù)據(jù)關(guān)聯(lián)

聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)(JPDA)[19]方法利用跟蹤軌跡周圍一定范圍內(nèi)的觀測(cè)值，計(jì)算每一個(gè)觀測(cè)與目標(biāo)軌跡的關(guān)聯(lián)概率，實(shí)現(xiàn)聯(lián)合概率加權(quán)更新目標(biāo)軌跡狀態(tài)。對(duì)于觀測(cè)與目標(biāo)軌跡匹配的限定范圍，本文采用卡方檢驗(yàn)跟蹤門的方式來限定跟蹤軌跡目標(biāo)的關(guān)聯(lián)范圍。

當(dāng)量測(cè)與軌跡預(yù)測(cè)目標(biāo)之間的馬氏距離滿足狀態(tài)量維數(shù)對(duì)應(yīng)的卡方檢驗(yàn)值的要求[20]，如(11)式，并且量測(cè)與軌跡目標(biāo)間的歐式距離也滿足一定閾值的情況下，表明這個(gè)量測(cè)與軌跡預(yù)測(cè)目標(biāo)是有關(guān)聯(lián)的。

(11)

式中：Zi(k)為k時(shí)刻編號(hào)i的某一觀測(cè)值；S(k)為k時(shí)刻量測(cè)估計(jì)分布的協(xié)方差；chi2inv為卡方檢驗(yàn)函數(shù)。

關(guān)聯(lián)矩陣構(gòu)成示意圖如圖15所示。最終利用上述判定條件對(duì)各觀測(cè)值與軌跡進(jìn)行匹配，生成類似如下圖右側(cè)的關(guān)聯(lián)矩陣。矩陣的行數(shù)為當(dāng)前量測(cè)數(shù)量，而列數(shù)為跟蹤軌跡目標(biāo)的數(shù)量加1，即矩陣的第1列，為虛警類。虛警是指在跟蹤過程中對(duì)軌跡狀態(tài)估計(jì)存在干擾的部分。這里假設(shè)每一個(gè)量測(cè)都與虛警相關(guān)聯(lián)，即每一個(gè)量測(cè)都可能是虛警而不是實(shí)際的跟蹤軌跡量測(cè)。圖15中，坐標(biāo)系原點(diǎn)o表示車輛后軸中心位置，坐標(biāo)系oxy平行于地面，車輛前進(jìn)方向?yàn)閥軸正方向，垂直于車輛前進(jìn)方向朝右為x軸正方向，Ω表示關(guān)聯(lián)矩陣。

Ω=[ωjt]，ωjt的含義如(12)式，j=1,2，…,mk;t=0,1,…,q，j表示矩陣的行，t表示矩陣的列，mk為量測(cè)的個(gè)數(shù)，q為跟蹤軌跡個(gè)數(shù)。

(12)

在得到關(guān)聯(lián)矩陣后，利用關(guān)聯(lián)矩陣求解得到每一個(gè)觀測(cè)值與目標(biāo)軌跡的關(guān)聯(lián)概率如(13)式所示。

(13)

在得到目標(biāo)與量測(cè)相互關(guān)聯(lián)概率后，需要利用量測(cè)值的概率加權(quán)對(duì)軌跡狀態(tài)進(jìn)行更新，即需要利用狀態(tài)估計(jì)濾波器的估計(jì)結(jié)果與量測(cè)值實(shí)現(xiàn)濾波器最終的狀態(tài)值估計(jì)?？柭鼮V波如(14)式所示：

k|k-1=AXk-1|k-1+qk

k|k-1=Hk|k-1

(14)

式中：Xk-1|k-1為k-1時(shí)刻的系統(tǒng)狀態(tài)向量；k|k-1為根據(jù)k-1時(shí)刻的系統(tǒng)狀態(tài)量得到的k時(shí)刻估計(jì)狀態(tài)向量；A為狀態(tài)轉(zhuǎn)移矩陣；qk為k時(shí)刻系統(tǒng)噪聲；k|k-1為根據(jù)k-1時(shí)刻的系統(tǒng)狀態(tài)量得到的k時(shí)刻量測(cè)估計(jì)值；H為觀測(cè)矩陣。

令vjt(k)表示第j個(gè)量測(cè)與t個(gè)目標(biāo)量測(cè)估計(jì)間的差值，即(15)式：

(15)

進(jìn)一步地，對(duì)于目標(biāo)t在k時(shí)刻與所有量測(cè)的差值的和，有

(16)

得到上述結(jié)果后，JPDA令狀態(tài)更新如下：

(17)

式中：Kt(k)為目標(biāo)t在k時(shí)刻的卡爾曼增益[21]。

vt(k)vt(k)T]Kt(k)T

(18)

2.3.2 跟蹤管理

2.3.2.1 目標(biāo)跟蹤初始化與剔除

目標(biāo)跟蹤初始化與剔除的主要功能是維持當(dāng)前跟蹤軌跡的穩(wěn)定性與可靠度，保證跟蹤軌跡的連續(xù)性，對(duì)同一目標(biāo)不產(chǎn)生過多的跟蹤Nid跳變，能夠?qū)Ω欆壽E狀態(tài)進(jìn)行劃分，對(duì)誤檢目標(biāo)進(jìn)行判斷。

圖16所示為本文所設(shè)計(jì)的跟蹤管理方案，其中Nhit表示目標(biāo)被匹配的次數(shù)，Nage表示當(dāng)前跟蹤軌跡未被匹配的次數(shù)。首先，對(duì)于第1幀檢測(cè)結(jié)果，跟蹤管理模塊不對(duì)目標(biāo)軌跡進(jìn)行初始化，而是保存檢測(cè)結(jié)果與下一時(shí)刻檢測(cè)結(jié)果，利用匈牙利算法進(jìn)行匹配，只有匹配上的目標(biāo)才進(jìn)行跟蹤初始化。初始化后的跟蹤目標(biāo)狀態(tài)設(shè)置為Sunconfirmed，表明對(duì)當(dāng)前跟蹤的信任程度，當(dāng)該跟蹤目標(biāo)在后續(xù)又被匹配上時(shí)，跟蹤目標(biāo)的Nhit值加1，當(dāng)Nhit>3時(shí)，跟蹤目標(biāo)狀態(tài)轉(zhuǎn)為Sconfirmed，表明該跟蹤目標(biāo)狀態(tài)穩(wěn)定，如圖16中的①所示。對(duì)于狀態(tài)為Sconfirmed的目標(biāo)將其與觀測(cè)進(jìn)行JPDA方法關(guān)聯(lián)，而Sunconfirmed的跟蹤目標(biāo)則與剩下未匹配的觀測(cè)目標(biāo)進(jìn)行匈牙利算法數(shù)據(jù)關(guān)聯(lián)，這樣能夠在一定程度上保證穩(wěn)定狀態(tài)跟蹤目標(biāo)的跟蹤穩(wěn)定性，也能夠?qū)Σ环€(wěn)定狀態(tài)跟蹤是否誤檢進(jìn)行快速辨別，并降低算法的運(yùn)算消耗。

進(jìn)一步地，當(dāng)跟蹤目標(biāo)未在當(dāng)前獲得匹配觀測(cè)目標(biāo)時(shí)，對(duì)于Sunconfirmed狀態(tài)的跟蹤目標(biāo)，其狀態(tài)轉(zhuǎn)為Sdelete，表明不穩(wěn)定的跟蹤在匹配無效后會(huì)被直接刪除，如圖16中的②所示；對(duì)于Sconfirmed狀態(tài)的跟蹤，跟蹤管理模塊會(huì)進(jìn)行保留，并在一段時(shí)間內(nèi)對(duì)該跟蹤狀態(tài)進(jìn)行預(yù)測(cè)維持，每進(jìn)行一次狀態(tài)預(yù)測(cè)，Nage值就會(huì)加1，直到該跟蹤目標(biāo)獲得匹配，Nage值清零；當(dāng)該跟蹤目標(biāo)連續(xù)多幀未獲得匹配，即Nage值大于一定值時(shí)，該跟蹤狀態(tài)轉(zhuǎn)為Sdelete，如圖16中的③所示。這樣設(shè)計(jì)跟蹤目標(biāo)剔除方案的好處在于，對(duì)于穩(wěn)定狀態(tài)的跟蹤，能夠在該跟蹤目標(biāo)由于漏檢或遮擋在觀測(cè)時(shí)消失幾幀后還繼續(xù)進(jìn)行關(guān)聯(lián)匹配，保證了跟蹤Nid的穩(wěn)定以及跟蹤軌跡的完整程度；對(duì)于不穩(wěn)定狀態(tài)的跟蹤，則能夠較快速地判別該目標(biāo)是否為誤檢以剔除，降低算法整體的運(yùn)算負(fù)擔(dān)。

目標(biāo)跟蹤最終效果如圖17所示。由圖17可以看到，目標(biāo)檢測(cè)在t時(shí)刻未能檢測(cè)出車輛前方Nid為1的移動(dòng)目標(biāo)，但是由于跟蹤管理保留了該目標(biāo)的運(yùn)動(dòng)軌跡，其在這一幀通過預(yù)測(cè)得出了該目標(biāo)當(dāng)前的位置。當(dāng)t+1時(shí)刻再次檢測(cè)到該目標(biāo)時(shí)，保留的軌跡預(yù)測(cè)值與該目標(biāo)進(jìn)行匹配，賦予該目標(biāo)當(dāng)前軌跡的Nid，使得對(duì)該目標(biāo)的跟蹤保持了連續(xù)性，不會(huì)由于檢測(cè)器誤檢的情況丟失跟蹤目標(biāo)。

2.3.2.2 目標(biāo)跟蹤結(jié)果剪枝

由于JPDA算法對(duì)于跟蹤目標(biāo)的狀態(tài)更新利用了多個(gè)與跟蹤目標(biāo)滿足關(guān)聯(lián)關(guān)系的觀測(cè)目標(biāo)，跟蹤目標(biāo)與實(shí)際觀測(cè)并不是一一對(duì)應(yīng)的關(guān)系。但實(shí)際跟蹤目標(biāo)的狀態(tài)只能表示環(huán)境中的某一個(gè)運(yùn)動(dòng)物體，因此需要將該跟蹤狀態(tài)與環(huán)境中的目標(biāo)進(jìn)行一一關(guān)聯(lián)。

如圖18所示，本文采取目標(biāo)跟蹤值與環(huán)境觀測(cè)值的歐式距離構(gòu)建代價(jià)矩陣，利用匈牙利算法求解的方式實(shí)現(xiàn)跟蹤目標(biāo)與環(huán)境目標(biāo)一一對(duì)應(yīng)，以實(shí)現(xiàn)跟蹤目標(biāo)結(jié)果的剪枝。

此外，為保證跟蹤目標(biāo)尺寸與航向的穩(wěn)定，需要對(duì)剪枝后與跟蹤目標(biāo)關(guān)聯(lián)的目標(biāo)尺寸依據(jù)跟蹤目標(biāo)歷史尺寸進(jìn)行更新?？紤]到在連續(xù)時(shí)刻內(nèi)，目標(biāo)的航向、尺寸不存在較大變化，當(dāng)檢測(cè)目標(biāo)尺寸變動(dòng)或目標(biāo)檢測(cè)航向與跟蹤歷史目標(biāo)的偏差較大，則不采用當(dāng)前檢測(cè)的尺寸或航向，而采用原有值對(duì)跟蹤目標(biāo)尺寸進(jìn)行顯示，如圖19所示，可以看出對(duì)于當(dāng)前幀的檢測(cè)其航向與實(shí)際偏差較大，因此不采用當(dāng)前檢測(cè)結(jié)果，而采用歷史幀的結(jié)果對(duì)當(dāng)前的檢測(cè)結(jié)果進(jìn)行描述，從而降低了檢測(cè)目標(biāo)尺寸表述不準(zhǔn)確的情況。

最終，跟蹤結(jié)果下發(fā)至協(xié)同架構(gòu)中的每輛車，其中包含了目標(biāo)的位置、速度以及Nid。

3 試驗(yàn)及結(jié)果分析

3.1 多車協(xié)同動(dòng)目標(biāo)跟蹤靜態(tài)試驗(yàn)

靜態(tài)試驗(yàn)的目的主要為了測(cè)試靜態(tài)下多車協(xié)同感知相對(duì)于單車感知在感知范圍上的優(yōu)勢(shì)以及對(duì)受到障礙物遮擋的目標(biāo)跟蹤的穩(wěn)定性。

靜態(tài)試驗(yàn)設(shè)計(jì)如圖20(a)所示，兩協(xié)同車輛朝向夾角約90°，靜止停放在地面上，為體現(xiàn)協(xié)同在傳感器量測(cè)范圍上的優(yōu)勢(shì)，設(shè)定兩車間距離約為40 m，目標(biāo)車輛與目標(biāo)人員從約為兩車朝向連線構(gòu)成的角度的角平分線方向相向而行，路線為綠色線所示，當(dāng)車輛與人走出兩車的感知量測(cè)范圍時(shí)試驗(yàn)結(jié)束。實(shí)際測(cè)試數(shù)據(jù)如圖20(b)所示。

測(cè)試可視化結(jié)果如圖21所示。圖21中顯示了試驗(yàn)?zāi)繕?biāo)的軌跡跟蹤情況，其中綠色與藍(lán)色方框?yàn)閰f(xié)同架構(gòu)下的車輛，紅色為所跟蹤目標(biāo)車輛軌跡，淺藍(lán)色為跟蹤行人目標(biāo)軌跡。從圖21(b)中可以發(fā)現(xiàn)：當(dāng)行人與車輛相向而行并在場(chǎng)地某一位置相遇產(chǎn)生交互時(shí)，行人由于受到車輛的遮擋，對(duì)于藍(lán)色位置的車輛其感知系統(tǒng)無法對(duì)行人進(jìn)行檢測(cè)，而綠色位置車輛則仍舊可以對(duì)目標(biāo)行人進(jìn)行探測(cè)，交互過程持續(xù)時(shí)間約1 s左右；當(dāng)車輛與人結(jié)束交互時(shí)，藍(lán)色位置車輛再次可以探測(cè)到行人目標(biāo)。在整個(gè)交互過程中，行人在協(xié)同架構(gòu)下始終能夠被探測(cè)，因此其跟蹤軌跡保持連續(xù)，未發(fā)生Nid跳變，也未出現(xiàn)漏檢；目標(biāo)車輛由于始終被協(xié)同架構(gòu)下兩車探測(cè)，未出現(xiàn)漏檢，跟蹤較穩(wěn)定，也未出現(xiàn)Nid跳變。

進(jìn)一步地，對(duì)跟蹤目標(biāo)的狀態(tài)進(jìn)行分析，目標(biāo)車輛檢測(cè)器輸出結(jié)果與跟蹤器輸出狀態(tài)及文獻(xiàn)[7]的軌跡級(jí)融合法比較結(jié)果如圖22所示。目標(biāo)行人的狀態(tài)比較結(jié)果如圖23所示。圖22(c)和圖23(c)中的速度曲線，藍(lán)色為通過檢測(cè)位置直接計(jì)算得出的結(jié)果，紅色為跟蹤器輸出狀態(tài)結(jié)果。

由圖22(a)、圖22(b)與圖23(a)、圖23(b)可以看出：靜態(tài)情況下，本文所提檢測(cè)器檢測(cè)結(jié)果較為穩(wěn)定，無論是行人或車輛目標(biāo)定位均較為準(zhǔn)確，與真值相差較??；協(xié)同情況下的跟蹤器輸出位置狀態(tài)結(jié)果與檢測(cè)結(jié)果十分貼合，曲線整體過渡平滑，誤差較小，具體均方根誤差結(jié)果如表1所示。其中行人的狀態(tài)估計(jì)誤差相對(duì)于車輛的估計(jì)誤差較小，這是因?yàn)檐囕v目標(biāo)較大，目標(biāo)檢測(cè)結(jié)果易產(chǎn)生尺寸上的變動(dòng)，使得車輛幾何中心位置的估計(jì)產(chǎn)生偏差；而行人目標(biāo)較小，檢測(cè)結(jié)果的尺寸變動(dòng)較小，目標(biāo)中心位置不會(huì)產(chǎn)生較大變動(dòng)，使得整體狀態(tài)估計(jì)較為準(zhǔn)確。

文獻(xiàn)[7]的軌跡級(jí)融合方法得到的目標(biāo)跟蹤狀態(tài)相對(duì)于目標(biāo)真值波動(dòng)較大，這是因?yàn)椴煌囕v在不同視角對(duì)同一目標(biāo)的檢測(cè)結(jié)果存在偏差，造成不同車輛對(duì)該目標(biāo)的狀態(tài)估計(jì)也存在偏差，使得最終不同軌跡融合獲得的目標(biāo)狀態(tài)估計(jì)不穩(wěn)定，其狀態(tài)估計(jì)結(jié)果如表1所示，相對(duì)于本文的方法，目標(biāo)位置的估計(jì)偏差較大。由圖22(c)和圖23(c)的速度曲線可以看出，通過直接計(jì)算得出的速度曲線跳變較為劇烈，而跟蹤器輸出結(jié)果曲線較為平滑，更符合目標(biāo)的運(yùn)動(dòng)規(guī)律。

表1 靜態(tài)試驗(yàn)?zāi)繕?biāo)位置估計(jì)均方根誤差

3.2 多車協(xié)同動(dòng)目標(biāo)跟蹤動(dòng)態(tài)試驗(yàn)

動(dòng)態(tài)試驗(yàn)的目的主要是為了驗(yàn)證多車協(xié)同感知框架在協(xié)同車輛行駛過程中對(duì)目標(biāo)跟蹤的穩(wěn)定性。

動(dòng)態(tài)試驗(yàn)設(shè)計(jì)如圖24(a)所示，當(dāng)車輛以隊(duì)列行駛時(shí)，最后方車輛由于受到前車的遮擋而無法獲取目標(biāo)車輛信息，而中間的車輛則可以感知到前車信息，以此來測(cè)驗(yàn)多車協(xié)同框架在動(dòng)態(tài)場(chǎng)景下受到目標(biāo)遮擋時(shí)的目標(biāo)檢測(cè)能力；當(dāng)后車加速超越前方兩車隊(duì)列時(shí)，其傳感器逐漸不受前車遮擋從而檢測(cè)到目標(biāo)車輛，這時(shí)兩輛車均能感知到目標(biāo)車輛，以此來測(cè)試動(dòng)態(tài)行駛下多車檢測(cè)數(shù)據(jù)融合的性能。

測(cè)試的可視化結(jié)果如圖25所示。由圖25可以看到，協(xié)同架構(gòu)下目標(biāo)獲得了連續(xù)穩(wěn)定的跟蹤路徑，未發(fā)生Nid跳變。由圖25(a)可見，對(duì)于隊(duì)列跟車中位于隊(duì)尾的車輛，其在受前車遮擋而無法對(duì)所跟蹤目標(biāo)進(jìn)行檢測(cè)的情況下也能夠獲得目標(biāo)的位置與狀態(tài)信息。如圖25(b)所示，當(dāng)該車進(jìn)行超車操作時(shí)，該車輛檢測(cè)到目標(biāo)與原有目標(biāo)經(jīng)過處理并沒有出現(xiàn)多個(gè)檢測(cè)目標(biāo)發(fā)生沖突的情況，框架下所跟蹤的目標(biāo)仍舊十分穩(wěn)定，沒有出現(xiàn)Nid切換以及漏檢的情況。

進(jìn)一步地，對(duì)跟蹤器獲取的跟蹤目標(biāo)狀態(tài)進(jìn)行分析，其中檢測(cè)器檢測(cè)結(jié)果與跟蹤器輸出狀態(tài)結(jié)果如圖26所示。

圖26(a)為目標(biāo)沿x軸位置的變化，可以看出在動(dòng)態(tài)情況下由于目標(biāo)檢測(cè)模塊存在一定的不穩(wěn)定性，整體目標(biāo)的位置隨著時(shí)間變化出現(xiàn)了一定的波動(dòng)，而實(shí)際上在短時(shí)間內(nèi)目標(biāo)的運(yùn)動(dòng)應(yīng)是連續(xù)的，跟蹤器的輸出結(jié)果相對(duì)于原有檢測(cè)結(jié)果更為平滑。但狀態(tài)估計(jì)結(jié)果與真值仍存在一定的偏差，如表2所示，其偏差相對(duì)于靜態(tài)試驗(yàn)的偏差要小，這是因?yàn)樵诟囘^程中，打在前方車輛的點(diǎn)云較為密集，檢測(cè)器對(duì)前方車輛的尺寸估計(jì)較為準(zhǔn)確，其幾何中心未產(chǎn)生較大偏差。

從圖26(b)中可以看到，文獻(xiàn)[7]的軌跡級(jí)融合方法在250～350幀間的目標(biāo)位置估計(jì)產(chǎn)生了較大抖動(dòng)，該階段是在后車超越前車時(shí)兩車同時(shí)檢測(cè)到目標(biāo)車輛的時(shí)刻，此時(shí)兩車均對(duì)目標(biāo)產(chǎn)生跟蹤結(jié)果并融合。該階段目標(biāo)的位置估計(jì)產(chǎn)生抖動(dòng)的原因在于后車超越前車的過程中后車會(huì)逐漸檢測(cè)到目標(biāo)車輛，但該車輛檢測(cè)到的目標(biāo)較為不穩(wěn)定，目標(biāo)的尺寸與幾何中心位置隨著車輛的運(yùn)動(dòng)產(chǎn)生一定的變化，使得該過程中兩車跟蹤結(jié)果融合后得到的目標(biāo)狀態(tài)估計(jì)值不穩(wěn)定，具體偏差如表2所示。

表2 動(dòng)態(tài)試驗(yàn)?zāi)繕?biāo)位置估計(jì)均方根誤差

此外，對(duì)于速度估計(jì)(見圖24(c))，跟蹤器輸出的狀態(tài)估計(jì)結(jié)果相對(duì)于計(jì)算結(jié)果更加連續(xù)平滑，比較符合目標(biāo)的實(shí)際運(yùn)動(dòng)速度變化。

4 結(jié)論

本文針對(duì)單車傳感器因存在視野盲區(qū)及目標(biāo)受到遮擋導(dǎo)致的目標(biāo)跟蹤不連續(xù)不穩(wěn)定問題開展討論，得出主要結(jié)論如下：

1)提出了一種多車協(xié)同感知的結(jié)果級(jí)融合系統(tǒng)模型，相對(duì)于單車感知在環(huán)境表征上能夠獲得更全面的環(huán)境信息，有利于目標(biāo)的協(xié)同檢測(cè)與跟蹤。

2)提出了一種多車協(xié)同目標(biāo)檢測(cè)方法，設(shè)計(jì)了合理的目標(biāo)融合沖突解決方案，得到了準(zhǔn)確的協(xié)同目標(biāo)融合結(jié)果。

3)提出了一種級(jí)聯(lián)動(dòng)態(tài)目標(biāo)匹配與跟蹤管理方法，跟蹤器輸出位置狀態(tài)結(jié)果與檢測(cè)結(jié)果誤差較小，能夠?qū)λ櫮繕?biāo)的狀態(tài)進(jìn)行準(zhǔn)確估計(jì)；目標(biāo)檢測(cè)與跟蹤更為穩(wěn)定，當(dāng)出現(xiàn)目標(biāo)遮擋時(shí)未發(fā)生Nid跳變，跟蹤軌跡仍保持連續(xù)，也未出現(xiàn)漏檢。

本文在目標(biāo)檢測(cè)精度方面并沒有做相應(yīng)的研究，未來可以設(shè)計(jì)相關(guān)試驗(yàn)來驗(yàn)證協(xié)同框架對(duì)檢測(cè)精度提升的優(yōu)越性；在車輛定位方面采用的是定位精度較高的差分定位，未來可以對(duì)車輛定位存在偏差時(shí)的多車協(xié)同定位方法進(jìn)行研究；在試驗(yàn)設(shè)計(jì)方面，由于起伏地形會(huì)使得本車姿態(tài)不時(shí)發(fā)生較大的變化，這對(duì)多目標(biāo)協(xié)同探測(cè)時(shí)的時(shí)空一致性提出了更高的要求，未來可以開展后續(xù)的相關(guān)研究工作。