朱世豪，武一民

基于多傳感融合的目標(biāo)追蹤方法

朱世豪，武一民

（河北工業(yè)大學(xué) 機(jī)械工程學(xué)院 車輛工程，天津 300131）

自動駕駛已經(jīng)成為未來汽車技術(shù)發(fā)展的一個重要方向。但現(xiàn)階段自動駕駛汽車的感知精度不足已經(jīng)成為限制自動駕駛汽車應(yīng)用的一個重要因素。為解決上述問題，文章基于多傳感器信息融合理論，提出一種自適應(yīng)數(shù)據(jù)關(guān)聯(lián)方法，分別考慮傳感器的誤差特性模型、目標(biāo)的運(yùn)動狀態(tài)對數(shù)據(jù)關(guān)聯(lián)的影響實(shí)現(xiàn)雜波環(huán)境中的目標(biāo)追蹤。并實(shí)驗(yàn)驗(yàn)證方法的有效性，實(shí)驗(yàn)結(jié)果表明，文章提出的融合感知結(jié)果能夠有效地降低誤差值，且目標(biāo)軌跡追蹤方法在所有實(shí)驗(yàn)場景中能100%保證目標(biāo)編號的一致性。

多傳感器融合；目標(biāo)追蹤；自適應(yīng)跟蹤門限

前言

隨著新一輪科技革命的發(fā)展，未來汽車的研究方向也隨之發(fā)生改變，無人駕駛技術(shù)已經(jīng)成為未來汽車領(lǐng)域的一個重要研究方向。但目前尚未出現(xiàn)能夠商業(yè)化的無人駕駛，其中一個難以解決的問題在于感知精度難以滿足需求。為提高感知精度，在車載感知方面進(jìn)行了大量的研究，如 Mobus基于毫米波雷達(dá)與紅外傳感器分別實(shí)現(xiàn)融合追蹤，采用PDA算法實(shí)現(xiàn)數(shù)據(jù)關(guān)聯(lián)，將關(guān)聯(lián)結(jié)果進(jìn)行融合且采用多模型交互實(shí)現(xiàn)目標(biāo)的狀態(tài)估計，模型的權(quán)重取決于假設(shè)的可能性[1]。該方法可以實(shí)現(xiàn)多傳感器的融合感知，但采用的PDA數(shù)據(jù)關(guān)聯(lián)算法雖然方法簡單，但難以實(shí)現(xiàn)多個目標(biāo)的數(shù)據(jù)關(guān)聯(lián)。黃偉[2]、王亞麗[3]和姚遠(yuǎn)[4]均是基于毫米波雷達(dá)與攝像頭實(shí)現(xiàn)目標(biāo)的融合感知，利用AdaBoost算法作為相機(jī)的檢測算法，該方法可以極大地提高目標(biāo)的檢測效率，降低目標(biāo)的漏檢率與誤檢率，并通過融合實(shí)現(xiàn)良好的目標(biāo)追蹤。上述研究僅是針對簡單工況中的目標(biāo)追蹤，對于車輛在雜波環(huán)境下的遮擋問題并未進(jìn)行過多的研究。針對上述問題，本文以兩個攝像頭為感知原件提出一種基于多傳感器融合的目標(biāo)追蹤方法。構(gòu)建傳感器的誤差特性模型，基于傳感器的誤差特性作為權(quán)重，實(shí)現(xiàn)多傳感器的融合感知，為保證跟蹤門限包含正確量測的前提下減少雜波，基于當(dāng)前時刻傳感器的誤差特性以及目標(biāo)的運(yùn)動狀態(tài)構(gòu)建自適應(yīng)跟蹤門限，實(shí)現(xiàn)跟蹤門限的自適應(yīng)調(diào)整。

1 多傳感器的時空同步

1.1 多傳感器的時間同步

由于每個傳感器的采樣頻率不同，采樣起始時間存在差異，傳輸效率也不盡相同。為保證數(shù)據(jù)的時間同步，基于采集數(shù)據(jù)的時間戳實(shí)現(xiàn)時間同步。即保證車載相機(jī)與RT range（一種組合定位產(chǎn)品，內(nèi)含GPS、IMU）時間差的消除以及采樣頻率的統(tǒng)一。

設(shè)備時鐘分別為工控機(jī)賦予車載相機(jī)以及行車記錄儀的時間戳，該時鐘為北京時間；RT range的真值時鐘為格林尼治天文臺時間，即RT range與北京時間的差為八小時。為了保證后續(xù)數(shù)據(jù)的時間同步，考慮將格林威治時間轉(zhuǎn)換為北京時間保證數(shù)據(jù)起始時間的一致性；在實(shí)際過程中，車載相機(jī)的采樣頻率為50 Hz，RT range的采樣頻率為100 Hz，為保證有效數(shù)據(jù)的最大化利用，將車載相機(jī)的數(shù)據(jù)進(jìn)行線性插值，轉(zhuǎn)換為100 Hz，保證數(shù)據(jù)頻率的一致性。

1.2 多傳感器的空間同步

在融合感知系統(tǒng)中存在多個傳感器，每個傳感器的量測值均基于自身坐標(biāo)系，因此數(shù)據(jù)融合前需要保證局部坐標(biāo)系與全局坐標(biāo)系的統(tǒng)一。描述空間中兩個物體的相對關(guān)系時，可以分為兩個物體間的位置關(guān)系以及兩個物體的方向關(guān)系。位置關(guān)系一般使用笛卡爾位置來表示，方向關(guān)系則使用姿態(tài)來表示。本文基于已有的實(shí)驗(yàn)平臺實(shí)現(xiàn)空間同步，其中RT range內(nèi)含標(biāo)定軟件可實(shí)現(xiàn)空間同步，且實(shí)驗(yàn)平臺已完成攝像機(jī)的內(nèi)參標(biāo)定，因此只需對相機(jī)的外參進(jìn)行標(biāo)定即可實(shí)現(xiàn)空間同步。

旋轉(zhuǎn)矩陣法的核心是一個3×3的旋轉(zhuǎn)矩陣，表達(dá)式如下：

在三維空間中的任意一點(diǎn)進(jìn)行坐標(biāo)系轉(zhuǎn)換，不僅需要三個獨(dú)立的位置坐標(biāo)，還需要三個軸的旋轉(zhuǎn)角度來表示。兩個坐標(biāo)系的位置關(guān)系通過空間位置轉(zhuǎn)換即可實(shí)現(xiàn),姿態(tài)關(guān)系則通過旋轉(zhuǎn)矩陣實(shí)現(xiàn)。因此對于坐標(biāo)系的任意一點(diǎn)經(jīng)坐標(biāo)轉(zhuǎn)換為坐標(biāo)系，表達(dá)式如2所示。

2 目標(biāo)追蹤框架

2.1 目標(biāo)狀態(tài)估計

目標(biāo)的狀態(tài)估計主要包括目標(biāo)運(yùn)動狀態(tài)的預(yù)測及更新。本文基于KF作為狀態(tài)估計器。首先基于上一時刻的最優(yōu)狀態(tài)以及運(yùn)動學(xué)模型對車輛的狀態(tài)進(jìn)行預(yù)測，將預(yù)測結(jié)果與傳感器的量測進(jìn)行數(shù)據(jù)關(guān)聯(lián)，確認(rèn)目標(biāo)與量測之間的匹配關(guān)系后進(jìn)行目標(biāo)運(yùn)動狀態(tài)的更新[5-6]。

2.2 數(shù)據(jù)關(guān)聯(lián)方法

數(shù)據(jù)關(guān)聯(lián)是基于上一時刻的最優(yōu)狀態(tài)，采用運(yùn)動學(xué)模型對目標(biāo)的狀態(tài)進(jìn)行預(yù)測，并以狀態(tài)預(yù)測值作為跟蹤門限的中心。在實(shí)際應(yīng)用，跟蹤門限的大小及形狀會對追蹤效果產(chǎn)生不同的影響，跟蹤門限的形狀應(yīng)符合追蹤目標(biāo)的運(yùn)動特性，因此本文采用橢圓形跟蹤門。除此之外，在追蹤過程中，如跟蹤門限過大，會導(dǎo)致落入跟蹤門限內(nèi)的量測過多，出現(xiàn)多個無關(guān)量測與干擾噪聲，將對追蹤精度產(chǎn)生影響，若在追蹤過程中，跟蹤門限過小則會導(dǎo)致跟蹤門限內(nèi)無量測，數(shù)據(jù)關(guān)聯(lián)失敗。因此本文綜合考慮追蹤過程中的多種因素，實(shí)現(xiàn)對跟蹤門限實(shí)時修正[7]。

還需要確定跟蹤門限的面積，而橢圓形跟蹤門限的面積在于確定橢圓的長軸及短軸。研究表明，量測的傳遞過程存在兩種誤差：一是由于傳感器的性能導(dǎo)致量測值會存在隨機(jī)誤差，該隨機(jī)誤差無法避免，但該誤差會導(dǎo)致正確目標(biāo)的量測無法落入跟蹤門限內(nèi)，因此以傳感器的誤差特性統(tǒng)計結(jié)果為依據(jù)，改變跟蹤門限的大小，盡量保證有效量測落入跟蹤門限內(nèi)，二是車輛狀態(tài)估計的運(yùn)動模型是線性模型，由于采樣間隔較短，可以認(rèn)為在短時間內(nèi)車輛的運(yùn)動狀態(tài)不會變化，但仍然會存在一定的誤差，所以需要考慮車輛運(yùn)動狀態(tài)的不確定性導(dǎo)致的誤差?；谏鲜鰞蓚€要素構(gòu)建自適應(yīng)跟蹤門限。

12=2?MAX (φ(())∪?(w()))

12=2?MAX((())∪?(w())) （3）

其中12、12分別為橢圓跟蹤門限的長軸與短軸，φ(())與?(w())分別表示縱向的車輛運(yùn)動特性以及縱向的傳感器誤差特性；φ(())與?(w())分別表示橫向的車輛運(yùn)動特性以及橫向的傳感器誤差特性。

（1）傳感器誤差特性的建模方法。

目前車載相機(jī)的性能差別較大，因此如何正確描述傳感器的誤差特性成為構(gòu)建跟蹤門限的一大難題，針對上述問題采用如下方法解決：

1）確定傳感器的有效感知范圍，在感知范圍內(nèi)統(tǒng)計隨著目標(biāo)車輛縱向相對距離的增加，傳感器的量測值與真值間的誤差。

2）統(tǒng)計所有工況的數(shù)據(jù)，確定隨著縱向相對距離的變大，傳感器的縱向相對距離的隨機(jī)誤差、橫向相對距離的隨機(jī)誤差。

3）將統(tǒng)計結(jié)果擬合為非線性函數(shù)。擬合結(jié)果如圖1與2所示。

圖1 縱向動態(tài)誤差擬合圖

圖2 橫向動態(tài)誤差擬合圖

（2）目標(biāo)運(yùn)動狀態(tài)的計算方法。

以常加速度模型對目標(biāo)狀態(tài)進(jìn)行估計，但實(shí)際行駛中，由于采樣間隔的不確定性以及目標(biāo)運(yùn)動狀態(tài)的不確定性，因此需要考慮目標(biāo)的運(yùn)動誤差對自適應(yīng)跟蹤門限的影響，主要該表達(dá)式如下所示：

縱向位移：

表示采樣間隔，v橫向相對速度，為10 m/s2。

橫向位移：

v為縱向相對速度，而橫向的加速能力為0.4。

（3）誤差限制因子的計算方法。

為保證跟蹤門限的有效性，結(jié)合目標(biāo)特性對跟蹤門先進(jìn)行約束。即最小跟蹤門限應(yīng)滿足一般車輛尺寸，即寬度為1.8 m，長度4.8 m；且不同傳感器誤差特性相差較大，為防止傳感器的誤差過大導(dǎo)致跟蹤門限過大，結(jié)合國家法規(guī)車道線的寬度不超過3.75 m為依據(jù)構(gòu)建跟蹤門限短軸的自適應(yīng)上限，即橫向相對誤差最大值不可超過3.75 m，由傳感器特性獲取該橫向相對誤差值對應(yīng)的縱向相對距離，該縱向相對距離所對應(yīng)的傳感器縱向誤差即為傳感器縱向相對誤差的最大值，如圖3所示。

圖3 誤差限制因子

當(dāng)目標(biāo)與自車距離較近，此時誤差將達(dá)到極小值，而目標(biāo)尺寸將成為構(gòu)建跟蹤門限不可忽略的因素；當(dāng)目標(biāo)車與自車距離較遠(yuǎn)，此實(shí)量測值出現(xiàn)較大誤差，則該量測值已經(jīng)無法表示目標(biāo)當(dāng)前時刻的狀態(tài)，即使在跟蹤門限的邊緣出現(xiàn)量測是由該目標(biāo)產(chǎn)生，由于傳感器性能導(dǎo)致該量測的偏移，則該量測已不具備確定目標(biāo)狀態(tài)的能力，因?yàn)樵摿繙y已偏離真實(shí)目標(biāo)過大，若關(guān)聯(lián)成功該量測將會影響目標(biāo)追蹤的精度。因此需要對誤差進(jìn)行自適應(yīng)矯正。

誤差的限制因子的表達(dá)式如下所示：

自適應(yīng)限制因子反映了該算法對于量測誤差的最大容忍限度。其中?(w())與?(w())分別為由于傳感器誤差特性對自適應(yīng)跟蹤門限的短軸以及長軸的影響，當(dāng)傳感器的橫向相對誤差小于3.75米時，則表明該量測誤差表明傳感器的性能正常，產(chǎn)生的誤差在允許范圍內(nèi)，可以用于構(gòu)建自適應(yīng)跟蹤門限，對最終的追蹤結(jié)果會產(chǎn)生積極的影響；但傳感器的橫向相對距離大于3.75米時，表明傳感器的誤差已經(jīng)無法產(chǎn)生有效量測，此時的跟蹤門限為最大跟蹤門限，若跟蹤門限內(nèi)仍不存在量測，則該方法失效。

綜合考慮車載感知系統(tǒng)的誤差特性以及目標(biāo)運(yùn)動狀態(tài)的不確定性來構(gòu)建自適應(yīng)跟蹤門限，確保在數(shù)據(jù)關(guān)聯(lián)時正確量測落入跟蹤門限內(nèi)且減少雜波的落入。本文由于統(tǒng)計的最大誤差來構(gòu)建誤差特性曲線，因此導(dǎo)致誤差值較大。

3 實(shí)驗(yàn)驗(yàn)證

3.1 實(shí)驗(yàn)步驟

（1）三車保持同一直線，其中自車、目標(biāo)車2、目標(biāo)車1中相鄰兩車之間車輛間距20 m。

（2）三車同時緩慢起步，加速至實(shí)驗(yàn)要求車速，期間盡量保持起始車距不變，且實(shí)驗(yàn)數(shù)據(jù)能正確保存，實(shí)驗(yàn)準(zhǔn)備就緒。

（3）目標(biāo)車2先向右側(cè)緩慢變道，變道成功后，緩慢回到原車道，期間盡量保持目標(biāo)車1與目標(biāo)車2間縱向間距不變。

（4）重復(fù)步驟3。該車速下實(shí)驗(yàn)完成。行駛至初始位置靠邊停車（或掉頭）。

（5）改變實(shí)驗(yàn)車速，重復(fù)步驟1—5。

（6）記錄實(shí)驗(yàn)數(shù)據(jù)，復(fù)查實(shí)驗(yàn)數(shù)據(jù)，確定實(shí)驗(yàn)數(shù)據(jù)的完整、有效。

圖4 實(shí)驗(yàn)示意圖

3.2 實(shí)驗(yàn)結(jié)果

為驗(yàn)證本文提出的目標(biāo)追蹤算法的性能，選取一組實(shí)驗(yàn)結(jié)果，分別為目標(biāo)車2變道遮擋場景以及自車變道遮擋場景進(jìn)行方法驗(yàn)證。首先針對車載感知結(jié)果進(jìn)行分析。分析結(jié)果表明：目標(biāo)的追蹤過程中存在目標(biāo)編號的切換以及由于目標(biāo)間的遮擋導(dǎo)致被遮擋目標(biāo)的軌跡被刪除。針對上述問題通過改進(jìn)已有的算法，將實(shí)驗(yàn)采集數(shù)據(jù)作為目標(biāo)追蹤算法的輸入。車載感知的目標(biāo)追蹤算法結(jié)果如圖5所示。

圖5分別為縱向相對距離與橫向相對距離的感知結(jié)果，其中綠色以及粉色實(shí)現(xiàn)分別為傳感器的感知結(jié)果，藍(lán)色以及紅色虛線為RT range提供的目標(biāo)真值。

圖5 縱向相對距離與橫向相對距離感知結(jié)果

上圖為實(shí)驗(yàn)1車載感知結(jié)果，由該結(jié)果可知：在0 s～4 s時對于目標(biāo)車1與目標(biāo)車2的追蹤結(jié)果正常，而4 s～10 s時目標(biāo)車2發(fā)生變道，變道后真實(shí)情況為目標(biāo)車1在感知范圍內(nèi)消失，目標(biāo)車2仍應(yīng)該被追蹤到，但傳感器的追蹤結(jié)果顯示，目標(biāo)車1被誤認(rèn)為目標(biāo)車2，目標(biāo)車2則被認(rèn)定為消失。在10 s～15 s目標(biāo)車2變道結(jié)束后對目標(biāo)車1及目標(biāo)車2的追蹤回歸正常。因此，上述工況中存在問題是在發(fā)生目標(biāo)車間的相互遮擋中發(fā)生目標(biāo)編號的不合理切換。

圖6分別為被遮擋車輛的縱向相對距離、橫向相對距離、縱向相對速度、橫向相對速度的改進(jìn)結(jié)果，其中綠色為傳感器的感知結(jié)果，黑色實(shí)線為改進(jìn)追蹤結(jié)果，紅色點(diǎn)為RT range提供的目標(biāo)真值。分析結(jié)果如下：

圖6 縱橫向相對距離與相對速度改進(jìn)結(jié)果

如圖6目標(biāo)車2向右變道短暫遮擋目標(biāo)車1后再次向左變道的結(jié)果示意圖，其中，綠線表示傳感器的量測值，在換道前以及換道后對于目標(biāo)車1實(shí)現(xiàn)了準(zhǔn)確追蹤，在被遮擋期間對目標(biāo)保持消失前的狀態(tài)且保留目標(biāo)編號，在遮擋消失后再次實(shí)現(xiàn)對于目標(biāo)的準(zhǔn)確追蹤且目標(biāo)編號沒有發(fā)生改變。

表1 實(shí)驗(yàn)結(jié)果

編號切換次數(shù)縱向平均誤差/m橫向平均誤差/m 車載感知結(jié)果41.2950.612 改進(jìn)感知結(jié)果00.9930.476

表1表明，在4組實(shí)驗(yàn)中改進(jìn)算法能夠保證目標(biāo)的持續(xù)追蹤，不會發(fā)生目標(biāo)的編號切換，通過結(jié)果表明，八組實(shí)驗(yàn)中融合感知誤差的縱向平均誤差為0.993 m，相較于車載單傳感器的縱向平均誤差降低了23.3%；橫向平均誤差為0.476 m，相較于車載單傳感器的縱向平均誤差降低了22.2%。說明改進(jìn)的追蹤結(jié)果能夠有效降低傳感器的追蹤誤差，提高追蹤的精度。

4 結(jié)論

為解決自動駕駛的感知問題，針對單傳感器的感知范圍有限、感知精度不足的問題。提出了一種多傳感器數(shù)據(jù)融合方法。根據(jù)先驗(yàn)信息構(gòu)建傳感器的誤差特性模型，并以傳感器誤差作為權(quán)重實(shí)現(xiàn)數(shù)據(jù)融合；針對雜波環(huán)境中目標(biāo)追蹤不穩(wěn)定的問題，提出一種基于自適應(yīng)跟蹤門限的聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)方法。通過實(shí)驗(yàn)表明，相較于單傳感器的感知結(jié)果，融合感知結(jié)果在縱向相對距離平均誤差為0.993 m，相較于原有算法誤差降低了23.3%，橫向相對距離平均誤差為0.476 m，相較于原有方法誤差降低了22.2%，且能夠100%實(shí)現(xiàn)目標(biāo)的準(zhǔn)確追蹤。

[1] Mobus R, Kolbe U. Multi-target multi-object tracking, sensor fusion of radar and infrared[C].Intelligent Vehicles Symposium. 2004:732-737.

[2] Otto C,Gerber W,Leon F P,et al.A Joint Integrated Probabilistic Data Association Filter for pedestrian tracking across blind regions using monocular camera and radar[C].Intelligent Vehi- cles Symposium. IEEE,2012:636-641.

[3] 黃偉.基于雷達(dá)和機(jī)器視覺的車輛前方障礙物檢測系統(tǒng)設(shè)計與實(shí)現(xiàn)[D].武漢:武漢理工大學(xué),2010.

[4] 王亞麗.基于毫米波雷達(dá)與機(jī)器視覺融合的前方車輛檢測研究[D].吉林:吉林大學(xué),2013.

[5] 姚遠(yuǎn).碰撞預(yù)判系統(tǒng)中車輛的圖像雷達(dá)聯(lián)合識別方法[D].北京:清華大學(xué),2013.

[6] 麥新晨,楊明,王春香,等.一種基于多傳感器融合的車輛檢測與追蹤方法[J].上海交通大學(xué)學(xué)報,2011(7):1012-1016.

[7] Yu J,Li S E,Liu C,et al.Dynamical tracking of surrounding objects for road vehicles using linearlyarrayed ultrasonic sensors [C].Intelligent Vehicles Symposium (IV). IEEE,2016:72-77.

Target Tracking Method Based on Multi-sensor Fusion

ZHU Shihao, WU Yimin

( Vehicle Engineering, School of Mechanical Engineering, Hebei University of Technology, Tianjin 300131 )

Autonomous driving has become an important direction for the development of future automotive technology. However, the lack of perception accuracy of selfdriving cars at this stage has become an important factor restricting the application of selfdriving cars. In order to solve the above problems, this paper proposes an adaptive data association method based on the theory of multisensor information fusion, which considers the influence of the sensor's error characteristic model and the target's motion state on the data association to achieve target tracking in a cluttered environment. Design experi- ments to verify the effectiveness of the method. The experimental results show that the fusion sensing results proposed in this paper can effectively reduce the error value, and the target trajectory tracking method can 100% guarantee the consistency of the target number in all experimental scenarios.

Multi-sensor fusion; Target tracking; Adaptive tracking gate

TP212.6

A

1671-7988(2021)24-38-05

TP212.6

A

1671-7988(2021)24-38-05

10.16638/j.cnki.1671-7988.2021.024.009

朱世豪，碩士研究生，就讀于河北工業(yè)大學(xué)機(jī)械工程學(xué)院車輛工程系，研究方向：目標(biāo)追蹤。