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