在ADAS/AD系統(tǒng)中，傳感器融合是實(shí)現(xiàn)各傳感器信息互補(bǔ)，提高目標(biāo)狀態(tài)精度，滿(mǎn)足ISO-26262 ASIL功能安全等級(jí)的重要方法，其作用于感知層，作為后續(xù)規(guī)劃與決策層，執(zhí)行層的輸入，重要性不言而喻。

　　傳感器融合一般分為數(shù)據(jù)級(jí)融合，特征級(jí)融合，決策級(jí)融合。數(shù)據(jù)級(jí)融合一般用于同類(lèi)型傳感器融合；目前常見(jiàn)的 1R1V 或 5R1V 系統(tǒng)中采用 Vision 和 Radar 的融合， 以及 AD 系統(tǒng)中采用的 LiDAR 與 Radar 融合主要是特征級(jí)融合，即對(duì)多傳感器目標(biāo)狀態(tài)信息進(jìn)行融合；決策級(jí)融合一般針對(duì)具體功能利用不同傳感器的決策與規(guī)劃結(jié)果結(jié)合優(yōu)先級(jí)在控制算法層面可以實(shí)現(xiàn)。

　　還記得之前一位做傳感器融合的德國(guó)工程師給 OEM 介紹傳感器融合直接使用了一詞“Magic”。

　　MATLAB & Simulink提供了如GNN，JPDA，TOMHT，PHD，KF/EKF/UKF, PF, IMM等多種跟蹤器及濾波器算法。

　　本文以Multiple Target Tracking點(diǎn)目標(biāo)跟蹤為例介紹下Vision和Radar融合“Magic”，后續(xù)再單獨(dú)介紹擴(kuò)展目標(biāo)跟蹤。

　　圖1所示為多目標(biāo)跟蹤系統(tǒng)框架圖，各傳感器輸出目標(biāo)探測(cè) Detections 經(jīng)數(shù)據(jù)處理作為輸入，Assignment 組件根據(jù) Gating 組件中判斷的門(mén)限范圍內(nèi)的 Detections 與 Tracks 進(jìn)行目標(biāo)關(guān)聯(lián)；Track Maintenance組件對(duì)Tracks進(jìn)行新增/更新/刪除管理；Filtering組件通過(guò)測(cè)量值進(jìn)行更新得到當(dāng)前時(shí)刻 Tracks 的狀態(tài)估計(jì)，并進(jìn)行下一時(shí)刻的預(yù)測(cè)。

　　針對(duì)點(diǎn)目標(biāo)跟蹤MATLAB提供了trackerGNN，trackerJPDA，trackerTOMHT 實(shí)現(xiàn)上述框架算法，對(duì)應(yīng)的Simulink模塊如圖2所示。可以直接在Simulink中如圖3所示使用，輸出即融合后的 Tracks，后續(xù)可以再根據(jù)具體功能選擇CIPV或ACC targets。

　　下面具體來(lái)看下各個(gè)組件與其關(guān)鍵屬性，以及在MATLAB中如何定義：

　　1. 傳感器融合輸入Detections

　　即對(duì)應(yīng)各傳感器的目標(biāo)探測(cè)信息，MATLAB通過(guò)System Object將Vision及Radar的探測(cè)Detections打包成為objectDetection，方便調(diào)試各種跟蹤器及濾波器算法，其包括如圖4所示屬性：

　　a). Measurement，不論是通過(guò)實(shí)車(chē)測(cè)試，還是通過(guò)虛擬場(chǎng)景及傳感器模型都可以得到Vision與Radar基于時(shí)間序列的探測(cè)對(duì)象數(shù)據(jù)，主要包括x/y位置信息，vx/vy速度信息。Radar也可能發(fā)出探測(cè)目標(biāo)在極坐標(biāo)系下的徑向距離rng，水平及垂直角度az，el，徑向速度rr。Vision 輸出的Detection都是針對(duì)單個(gè)目標(biāo)的，Radar 如果針對(duì)一個(gè)目標(biāo)可輸出多個(gè) Detections，則需要先進(jìn)行聚類(lèi)，如圖 3 中 Detection Clustering 模塊所示。

　　b). MeasurementNoise，非常重要的參數(shù)，和傳感器測(cè)量特性及狀態(tài)矩陣相關(guān)的對(duì)角矩陣，直接影響到最后融合狀態(tài)估計(jì)量更相信 Vision 還是 Radar，以及是更相信測(cè)量值還是預(yù)測(cè)值。例如 Rradar = diag([1,1,2,10])；Rvision = diag([2,2,1,10]);

　　c). MeasurementParameters，可用于存取各傳感器坐標(biāo)系與車(chē)輛坐標(biāo)系轉(zhuǎn)換的信息，最終輸出的 Detections 以車(chē)輛坐標(biāo)系為基準(zhǔn)。

　　2. 多目標(biāo)跟蹤門(mén)Gating及數(shù)據(jù)關(guān)聯(lián)Assignment

　　在多目標(biāo)跟蹤中，為了減少不必要的數(shù)據(jù)關(guān)聯(lián)計(jì)算，通過(guò) Tracks 上一時(shí)刻狀態(tài)預(yù)測(cè)當(dāng)前時(shí)刻狀態(tài)，建立當(dāng)前時(shí)刻 Tracks 的有效跟蹤門(mén)。利用門(mén)限來(lái)確定哪些 Detections 是 Tracks 的有效候選對(duì)象，在準(zhǔn)確性可接受的范圍內(nèi)縮小門(mén)限值可加速數(shù)據(jù)關(guān)聯(lián)。

　　由于傳感器存在誤報(bào)，漏報(bào)以及測(cè)量誤差，同時(shí)也缺乏多目標(biāo)環(huán)境的先驗(yàn)知識(shí)，可能多個(gè) Detections 在同一個(gè) Track 的門(mén)限內(nèi)或者一個(gè) Detection 在多個(gè) Tracks 的門(mén)限內(nèi)。如圖 6 所示 O1~O4 是 Detections，T1~T3 是 Tracks，因此需要數(shù)據(jù)關(guān)聯(lián)將 Vision 及 Radar 的探測(cè) Detections 分別與 Tracks 進(jìn)行關(guān)聯(lián)或新建 Track。

　　圖2中Simulink模塊對(duì)應(yīng)的數(shù)據(jù)關(guān)聯(lián)算法即全局最近鄰算法，聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)，多假設(shè)跟蹤數(shù)據(jù)關(guān)聯(lián)。例如全局最近鄰（GNN）數(shù)據(jù)關(guān)聯(lián)算法：將門(mén)限內(nèi)的一個(gè)Detection最多與一個(gè)Track關(guān)聯(lián)，以使所有關(guān)聯(lián)結(jié)果的總代價(jià)距離J 最小。

　　Cij 表示Detection i 與Track j之間的馬氏距離，不同類(lèi)別的探測(cè)目標(biāo)（例如車(chē)輛和行人）之間，可以將其代價(jià)距離設(shè)置為較大值而不進(jìn)行關(guān)聯(lián)。對(duì)于 2-D GNN 分配問(wèn)題，可以使用assignmunkres，assignauction，assignjv 等算法進(jìn)行求解。

　　3. 目標(biāo)跟蹤管理 Track Maintenance

　　根據(jù)數(shù)據(jù)關(guān)聯(lián)的結(jié)果，對(duì)有 detections 關(guān)聯(lián)的 tracks 進(jìn)行更新；將沒(méi)有關(guān)聯(lián)的探測(cè) Detections 開(kāi)始創(chuàng)建新的 Tracks，且對(duì)假設(shè)性的 Tracks 進(jìn)行更新確認(rèn)；對(duì)沒(méi)有 Detections 關(guān)聯(lián)的 Tracks 進(jìn)行沒(méi)有更新的預(yù)測(cè)或刪除。

　　根據(jù)使用的關(guān)聯(lián)算法可以選擇不同的目標(biāo)跟蹤管理方法：trackerGNN 和 trackerJPDA 可以使用基于歷史數(shù)據(jù)的方法（History Logic）確認(rèn)，例如某個(gè) Track 在 N 次更新中，至少有M次更新分配了 Detection，則可以確認(rèn)。trackerGNN 和 trackerTOMHT 可以基于打分的方法（Track Score Logic）進(jìn)行確認(rèn)。

　　4. 濾波器 Filtering

　　雖然 Vision 和 Radar 可以對(duì)目標(biāo)運(yùn)動(dòng)狀態(tài)量進(jìn)行測(cè)量，但由于噪聲的存在，無(wú)法得到真值。實(shí)際項(xiàng)目中可以通過(guò)濾波對(duì)目標(biāo)狀態(tài)進(jìn)行估計(jì)，盡量的逼近于真值。卡爾曼濾波根據(jù)狀態(tài)方程和觀測(cè)方程以遞推的方式利用上一時(shí)刻的狀態(tài)預(yù)測(cè)當(dāng)前時(shí)刻的狀態(tài)，并通過(guò)當(dāng)前時(shí)刻的測(cè)量值對(duì)預(yù)測(cè)值進(jìn)行更新得到當(dāng)前時(shí)刻狀態(tài)的最小方差估計(jì)。

　　圖7為卡爾曼濾波算法公式，是針對(duì)零均值高斯噪聲的純時(shí)域?yàn)V波器。

　　那在 MATLAB 中如何根據(jù)下面公式創(chuàng)建一個(gè)卡爾曼濾波器？

　　步驟 1：需定義目標(biāo)運(yùn)動(dòng)模型及對(duì)應(yīng)的狀態(tài)轉(zhuǎn)移矩陣F，如常見(jiàn)勻加速模型或機(jī)動(dòng)模型。

　　步驟 2：定義過(guò)程噪聲 Q， sigma 為加速度變化率的最大值：

　　步驟 3：定義測(cè)量模型矩陣H，結(jié)合 Detections 從各傳感器獲得的測(cè)量值和狀態(tài)轉(zhuǎn)移矩陣而定：

　　步驟 4: 根據(jù) Detections 輸入狀態(tài)和測(cè)量噪聲確定目標(biāo)初始狀態(tài)及初始狀態(tài)的協(xié)方差矩陣，每個(gè)時(shí)刻狀態(tài)的不確定性都是通過(guò)狀態(tài)協(xié)方差P矩陣（stateCov）來(lái)表示：

　　步驟 5：結(jié)合步驟1~4 創(chuàng)建濾波器初始化函數(shù)，用于新建 track：

　　還可嘗試更多濾波器：Alpha-Beta 濾波/trackingABF, 卡爾曼/擴(kuò)展卡爾曼/無(wú)跡卡爾曼/容積卡爾曼濾波/ trackingKF/trackingEKF/trackingUKF/trackingCKF，高斯和濾波/trackingGSF，粒子濾波/trackingPF，交互多模式濾波/trackingIMM 等。

　　完成上述各組件后就可創(chuàng)建用于多目標(biāo)跟蹤器 trackerGNN / trackerJPDA / trackerTOMHT，例如：

　　最后，可以使用 MATLAB 創(chuàng)建的多目標(biāo)跟蹤器或 Simulink 模塊，可以自動(dòng)生成 C/C++ 代碼進(jìn)行部署實(shí)現(xiàn)算法實(shí)時(shí)驗(yàn)證。