在ADAS/AD系統(tǒng)中，傳感器融合是實現(xiàn)各傳感器信息互補，提高目標狀態(tài)精度，滿足ISO-26262 ASIL功能安全等級的重要方法，其作用于感知層，作為后續(xù)規(guī)劃與決策層，執(zhí)行層的輸入，重要性不言而喻。
傳感器融合一般分為數(shù)據(jù)級融合，特征級融合，決策級融合。數(shù)據(jù)級融合一般用于同類型傳感器融合；目前常見的 1R1V 或 5R1V 系統(tǒng)中采用 Vision 和 Radar 的融合， 以及 AD 系統(tǒng)中采用的 LiDAR 與 Radar 融合主要是特征級融合，即對多傳感器目標狀態(tài)信息進行融合；決策級融合一般針對具體功能利用不同傳感器的決策與規(guī)劃結(jié)果結(jié)合優(yōu)先級在控制算法層面可以實現(xiàn)。
還記得之前一位做傳感器融合的德國工程師給 OEM 介紹傳感器融合直接使用了一詞“Magic”。
MATLAB & Simulink提供了如GNN，JPDA，TOMHT，PHD，KF/EKF/UKF, PF, IMM等多種跟蹤器及濾波器算法。
本文以Multiple Target Tracking點目標跟蹤為例介紹下Vision和Radar融合“Magic”，后續(xù)再單獨介紹擴展目標跟蹤。
圖1所示為多目標跟蹤系統(tǒng)框架圖，各傳感器輸出目標探測 Detections 經(jīng)數(shù)據(jù)處理作為輸入，Assignment 組件根據(jù) Gating 組件中判斷的門限范圍內(nèi)的 Detections 與 Tracks 進行目標關聯(lián)；Track Maintenance組件對Tracks進行新增/更新/刪除管理；Filtering組件通過測量值進行更新得到當前時刻 Tracks 的狀態(tài)估計，并進行下一時刻的預測。
針對點目標跟蹤MATLAB提供了trackerGNN，trackerJPDA，trackerTOMHT 實現(xiàn)上述框架算法，對應的Simulink模塊如圖2所示。可以直接在Simulink中如圖3所示使用，輸出即融合后的 Tracks，后續(xù)可以再根據(jù)具體功能選擇CIPV或ACC targets。
下面具體來看下各個組件與其關鍵屬性，以及在MATLAB中如何定義：
1. 傳感器融合輸入Detections
即對應各傳感器的目標探測信息，MATLAB通過System Object將Vision及Radar的探測Detections打包成為objectDetection，方便調(diào)試各種跟蹤器及濾波器算法，其包括如圖4所示屬性：
a). Measurement，不論是通過實車測試，還是通過虛擬場景及傳感器模型都可以得到Vision與Radar基于時間序列的探測對象數(shù)據(jù)，主要包括x/y位置信息，vx/vy速度信息。Radar也可能發(fā)出探測目標在極坐標系下的徑向距離rng，水平及垂直角度az，el，徑向速度rr。Vision 輸出的Detection都是針對單個目標的，Radar 如果針對一個目標可輸出多個 Detections，則需要先進行聚類，如圖 3 中 Detection Clustering 模塊所示。
b). MeasurementNoise，非常重要的參數(shù)，和傳感器測量特性及狀態(tài)矩陣相關的對角矩陣，直接影響到最后融合狀態(tài)估計量更相信 Vision 還是 Radar，以及是更相信測量值還是預測值。例如 Rradar = diag([1,1,2,10])；Rvision = diag([2,2,1,10]);
c). MeasurementParameters，可用于存取各傳感器坐標系與車輛坐標系轉(zhuǎn)換的信息，最終輸出的 Detections 以車輛坐標系為基準。
2. 多目標跟蹤門Gating及數(shù)據(jù)關聯(lián)Assignment
在多目標跟蹤中，為了減少不必要的數(shù)據(jù)關聯(lián)計算，通過 Tracks 上一時刻狀態(tài)預測當前時刻狀態(tài)，建立當前時刻 Tracks 的有效跟蹤門。利用門限來確定哪些 Detections 是 Tracks 的有效候選對象，在準確性可接受的范圍內(nèi)縮小門限值可加速數(shù)據(jù)關聯(lián)。
由于傳感器存在誤報，漏報以及測量誤差，同時也缺乏多目標環(huán)境的先驗知識，可能多個 Detections 在同一個 Track 的門限內(nèi)或者一個 Detection 在多個 Tracks 的門限內(nèi)。如圖 6 所示 O1~O4 是 Detections，T1~T3 是 Tracks，因此需要數(shù)據(jù)關聯(lián)將 Vision 及 Radar 的探測 Detections 分別與 Tracks 進行關聯(lián)或新建 Track。
圖2中Simulink模塊對應的數(shù)據(jù)關聯(lián)算法即全局最近鄰算法，聯(lián)合概率數(shù)據(jù)關聯(lián)，多假設跟蹤數(shù)據(jù)關聯(lián)。例如全局最近鄰（GNN）數(shù)據(jù)關聯(lián)算法：將門限內(nèi)的一個Detection最多與一個Track關聯(lián)，以使所有關聯(lián)結(jié)果的總代價距離J 最小。
Cij 表示Detection i 與Track j之間的馬氏距離，不同類別的探測目標（例如車輛和行人）之間，可以將其代價距離設置為較大值而不進行關聯(lián)。對于 2-D GNN 分配問題，可以使用assignmunkres，assignauction，assignjv 等算法進行求解。
3. 目標跟蹤管理 Track Maintenance
根據(jù)數(shù)據(jù)關聯(lián)的結(jié)果，對有 detections 關聯(lián)的 tracks 進行更新；將沒有關聯(lián)的探測 Detections 開始創(chuàng)建新的 Tracks，且對假設性的 Tracks 進行更新確認；對沒有 Detections 關聯(lián)的 Tracks 進行沒有更新的預測或刪除。
根據(jù)使用的關聯(lián)算法可以選擇不同的目標跟蹤管理方法：trackerGNN 和 trackerJPDA 可以使用基于歷史數(shù)據(jù)的方法（History Logic）確認，例如某個 Track 在 N 次更新中，至少有M次更新分配了 Detection，則可以確認。trackerGNN 和 trackerTOMHT 可以基于打分的方法（Track Score Logic）進行確認。
4. 濾波器 Filtering
雖然 Vision 和 Radar 可以對目標運動狀態(tài)量進行測量，但由于噪聲的存在，無法得到真值。實際項目中可以通過濾波對目標狀態(tài)進行估計，盡量的逼近于真值。卡爾曼濾波根據(jù)狀態(tài)方程和觀測方程以遞推的方式利用上一時刻的狀態(tài)預測當前時刻的狀態(tài)，并通過當前時刻的測量值對預測值進行更新得到當前時刻狀態(tài)的最小方差估計。
圖7為卡爾曼濾波算法公式，是針對零均值高斯噪聲的純時域濾波器。
那在 MATLAB 中如何根據(jù)下面公式創(chuàng)建一個卡爾曼濾波器？
步驟 1：需定義目標運動模型及對應的狀態(tài)轉(zhuǎn)移矩陣F，如常見勻加速模型或機動模型。
步驟 2：定義過程噪聲 Q， sigma 為加速度變化率的最大值：
步驟 3：定義測量模型矩陣H，結(jié)合 Detections 從各傳感器獲得的測量值和狀態(tài)轉(zhuǎn)移矩陣而定：
步驟 4: 根據(jù) Detections 輸入狀態(tài)和測量噪聲確定目標初始狀態(tài)及初始狀態(tài)的協(xié)方差矩陣，每個時刻狀態(tài)的不確定性都是通過狀態(tài)協(xié)方差P矩陣（stateCov）來表示：
步驟 5：結(jié)合步驟1~4 創(chuàng)建濾波器初始化函數(shù)，用于新建 track：
還可嘗試更多濾波器：Alpha-Beta 濾波/trackingABF, 卡爾曼/擴展卡爾曼/無跡卡爾曼/容積卡爾曼濾波/ trackingKF/trackingEKF/trackingUKF/trackingCKF，高斯和濾波/trackingGSF，粒子濾波/trackingPF，交互多模式濾波/trackingIMM 等。
完成上述各組件后就可創(chuàng)建用于多目標跟蹤器 trackerGNN / trackerJPDA / trackerTOMHT，例如：
最后，可以使用 MATLAB 創(chuàng)建的多目標跟蹤器或 Simulink 模塊，可以自動生成 C/C++ 代碼進行部署實現(xiàn)算法實時驗證。
