如何用自動駕駛系統工具箱設計和驗證多目標跟蹤演算法

新智駕按：2017 年，MathWorks 推出自動駕駛系統工具箱（ADST），藉助這個新的工具箱提供的一系列設計和驗證工具，能夠更快速地開發感知演算法，並簡化演算法的驗證工作。

本文內容是 MathWorks 應用工程師王鴻鈞在雷鋒網與中國汽車工程學會組織的公開課上的分享。由雷鋒網編輯與整理。

本期公開課要點：

自動駕駛系統工具箱簡介

多目標跟蹤原理

使用自動駕駛系統工具箱設計多目標跟蹤演算法

使用基準真值標註工具分析計算結果

演算法的實時驗證技術

我們來回顧一下主動安全和自動駕駛系統的開發流程。

首先，我們會有一個系統的需求。然後，我們會在系統需求的基礎上進行感知演算法與控制演算法的設計。感知演算法包括了圖象、雷達、GPS 以及一系列感測器的採集、目標檢測以及感知演算法。控制演算法主要包括控制策略與控制執行部分。

開發完演算法之後，我們需要進行系統的集成、測試和驗證，然後自動生成代碼，最後進行代碼的測試和系統最終確認驗證。

感測器融合與多目標跟蹤的演算法，其實是處於感知演算法和控制演算法中間的部分，對於控制工程師來說，我們需要對感知的感測器信息進行處理，得到用於控制的輸入信息。這就是我們的工作。

MathWorks 在最新的 2017A 版本中推出了一個全新的自動駕駛系統工具箱，它希望為從事自動駕駛以及主動安全系統設計的工程師提供一系列加速自動駕駛系統及主動安全系統開發的工具。

主要包括三方面：

第一，測試與驗證工具，包括真實值標籤與場景生成；

第二，演算法開發工具，包括感測器融合與目標跟蹤，和視覺系統設計工具；

第三，可視化工具，包括鳥瞰圖工具，以及雷達點雲工具。

目標跟蹤原理

什麼是目標跟蹤？要進行控制，首先必須從真實的世界採集到一定的信息，根據信息進行閉環控制。

我們要從真實的世界進行感知，感知部件包括一系列感測器，比如雷達、視覺、激光雷達等等。然後，我們需要在感知的基礎上，對真實的世界進行估計。

這就是我們要介紹的目標跟蹤等相關演算法和感測器融合。

先介紹一些概念：

真實值指的是物體的實際運動。檢測指的是通過感測器獲得的含有分類屬性的一系列目標列表，檢測通常是根據時間的序列來完成的。

比如上圖右版部分 6 個時刻，我們分別檢測到了一系列目標：紅色和藍色代表車輛，綠色是行人。當然，檢測的時候還可能出現一些問題，比如黑色的點就是我們沒有辦法對它進行分類的目標，黃色的點屬於虛景，但是我們在實際檢測的時候是不知道的，所以我們也把它歸類為沒有分類的目標。

其實從 T0 時刻一直到 T2、T3、T4、T5 時刻，我們對目標檢測情況進行一系列分析，通過跟蹤演算法對物體進行實際運動估計。

在 T0 時刻，我們成功檢測到了藍色的車輛與綠色的行人，那麼我們就將它作為車輛與行人跟蹤。但是我們沒有檢測到紅色車輛，所以我們添加了兩個灰色的點進行試探性跟蹤。

在 T1 時刻，我們觀測到了紅色車輛，所以我們現在確認了灰色跟蹤當中其中一個是車輛，但另一個沒有確認的跟蹤，我們把它刪除了。

T3 時刻，我們並沒有觀測到行人，但是我們通過預測，把被遮擋的行人目標預測出來，也把它作為跟蹤的一部分。

T4 時刻，紅色和藍色的車輛離開了觀測範圍，所以我們對離開的目標位置也進行了預測。

最後，紅色和藍色的車輛都離開了觀測範圍，行人也穿過了馬路，於是我們完成了對這三個物體的跟蹤，我們將跟蹤從我們跟蹤的列表當中刪除。

如果以上介紹過於抽象，我們在 2017A 自動駕駛系統工具箱當中提供了一個前向碰撞預警的 Demo。你們可以通過這個 Demo 來形象地了解到跟蹤器是如何工作的。

視頻當中通過右側的鳥瞰圖可以發現藍色的是視覺目標，紅色是雷達目標，方塊形狀是我們跟蹤的目標。

我們會對車輛進行檢測，然後將最近的最有可能發生碰撞的目標作為 MIO（最重要目標），防撞預警系統會對它進行監測並進行告警。

多目標跟蹤演算法

現在我們看到的是前向碰撞預警系統演算法的框圖，我們可以通過雷達目標與視覺目標進行感測器的融合以及卡爾曼濾波完成跟蹤器的工作。

然後，我們會對跟蹤到的目標進行威脅評估，最後得到 MIO，進行防撞預警。預警的結果還可以輸入給 AEB（自動剎車系統）來進行控制處理。

我們可以藉助計算機視覺工具箱當中代價矩陣的方法將雷達目標與視覺目標進行配對：根據配對的結果將匹配的雷達與視覺目標進行融合。

我們可以通過以下的函數來實現：

實際的跟蹤演算法會通過卡爾曼濾波來實現。卡爾曼濾波的含義或者是公式可能有些人比較熟悉，它就是通過前狀態和協方差來對當前的狀態進行預測，通過當前的測量值去更新當前的狀態，將更新的狀態進行輸出。

在計算機視覺工具箱當中同樣提供了卡爾曼濾波的函數。

實際多目標的跟蹤，遠遠比一個卡爾曼濾波器更為複雜，因為它涉及到了我們提到的感測器融合，以及多目標跟蹤管理，包括跟蹤的初始化、確認、預測。

比如，剛才提到的可能會有一些未確認的目標，但是我們同樣也要對這些目標進行跟蹤。同時有些目標會被遮擋，我們要對被遮擋的目標進行預測。所以現在我們看到的就是一個完整的多目標跟蹤器的邏輯。

上圖，分別是目標檢測數據需要包括的信息、目標跟蹤數據能夠提供的信息。

這些信息包括了時間測量值、測量雜訊、感測器編號、目標 ID、目標屬性等等。我們最後得到的跟蹤數據包括了跟蹤的 ID、時間、年齡、狀態、協方差、確認標誌、遮擋標誌、目標類別的 ID 以及目標屬性（如果大家對以上的信息還有疑問的話，在我們的幫助文件里會有更詳細的描述）。

介紹一下多目標跟蹤器的層級，一共分為三個層級。

第一，頂層與用戶的交互界面，它同時負責分配多個檢測到多個跟蹤，進行這樣一個匹配，以及維護跟蹤的列表，進行新建或者是刪除跟蹤。

第二，對單個跟蹤來說，會處理對單個跟蹤的多個檢測，就是完成感測器融合的演算法，同時也會負責單個跟蹤的確認與刪除，以及維護跟蹤的 ID 分類等信息。

第三，跟蹤濾波器，用戶自定義的濾波器，負責處理測量產生的濾波器更新，以及維護濾波器的狀態和協方差。

對於非線性的擴展卡爾曼濾波或者無界卡爾曼濾波，用戶還需要定義這一時刻的狀態轉移函數與測量函數，它取決於用戶對目標模型的分析情況。

比如，目標運動狀態，是否是勻速、勻加速或者是其他運動狀態。

同時用戶還需要配置跟蹤器的其他屬性，包括確認跟蹤的閾值、被遮擋的更新次數、感測器的數量以及最大數量的跟蹤數量等等。

簡單介紹檢測到跟蹤的分配以及感測器的融合是如何實現的。

圖一顯示的是目標前一時刻的狀態，圖二顯示的是跟蹤器對目標當前位置的預測，圖三是根據跟蹤器對目標當前位置的預測，將檢測到的一些目標的信息分配到跟蹤的目標。

比如，可能有雷達的測量數據、視覺的測量數據，就是採用代駕矩陣的方法來進行分配的。

最後一步，進行感測器融合的計算，獲得當前目標的位置，更新當前的狀態。

對於跟蹤的預測以及狀態更新，我們提供了三種濾波的演算法，分別是：KF 線性卡爾曼濾波、EKF 擴展卡爾曼濾波、UKU 無界卡爾曼濾波。

我們預設了三種濾波器的初始化函數，分別針對於勻速目標、勻加速目標和勻轉向速目標。同時用戶也可以添加自定義的濾波器初始化函數，這通常用於非線性的濾波器。

對於 EKF 和 UKF 來說，用戶還需要定義它的運動函數和測量函數。同樣，我們預先設置了勻加速、勻速、勻轉向速度這三種情況，對於其他一些更複雜的情況，用戶可以根據公式添加自定義模型。

單個跟蹤來說，有一個生命周期，一共分為三個步驟。

第一，跟蹤的初始化，我們會從從沒有分配到跟蹤的檢測創建一個新的跟蹤，這就說明我們檢測到了一個新的目標，同時我們會創建和初始化濾波器。

第二，跟蹤得到了確認，我們會將每個時刻的分配、檢測到跟蹤。通過檢測到的信息我們對跟蹤進行修正，同時跟蹤也會計算一個預測，這個就是典型的卡爾曼濾波演算法。

第三，如果一段時間內沒有觀測到跟蹤目標，那麼我們會判斷是否刪除這個跟蹤，經過一個閾值，如果在閾值的範圍之外，還是沒有觀測到跟蹤，那麼我們將其刪除。

對於非線性濾波器來說，我們還會定期地修正它的測量函數與狀態轉移函數。

這是一個行人目標跟蹤的案例，我們從案例中可以很清晰地看到一個跟蹤的生命周期：它是如何被初始化和如何確認的，在被遮擋的時候如何進行預測以及最後如何進行刪除。

真實值標籤工具

我們在自動駕駛系統工具箱當中還提供了很多其他的工具，包括了一系列的演算法驗證工具，比如說真實值標籤工具。

在 2012 版本的 Matlab2012B 之後，有一個 APP 的功能：提供了一系列的應用程序，這包括了我們在自動駕駛系統工具箱當中提供的真實值標籤應用程序。

如上述右圖所示，我們可以通過這個工具，在一段視頻當中添加真實值的標籤，將目標通過標籤的形式記錄下來，可以使用矩形的目標或者是直線的目標。

右圖中，我們使用真實值標籤標記了卡車、汽車、交通指示牌、車道線等等。

按通常理解，真實值標籤一般是用來訓練目標檢測的演算法以及評估目標檢測的演算法。在這裡，我們還可以通過真實值標籤的工具，去驗證、跟蹤和控制演算法。

我們可以通過真實值標籤去對視頻進行處理，人工產生一些視覺目標，通過採集的記錄的或者是實驗室產生的雷達目標對它進行融合，通過多目標跟蹤器與控制演算法對整個系統的功能進行一個模擬工作。

使用真實值標籤工具添加真實值標籤的方法共分為五個步驟：

第一，載入視頻。

第二，定義標籤的類型。比如，我們可以定義車輛、車道線等目標，定義環境，比如說天氣等全局的標籤。

第三，通過時間軸的拖動，可以對每一幀進行標籤的繪製。

第四，我們可以使用部分自動演算法，為我們添加標籤，包括中間幀的線性插補、KLT 目標跟蹤以及 ACF 車輛檢測這些自動化演算法，可以逐幀地繪製選定的標籤。

第五，我們通過手動的微調得到最終結果，將數據導出到文件或者是工作空間。

演算法的實時驗證

設計完成之後，就需要進行演算法驗證，我們可能會通過一些實車的測試進行驗證，也可以在實驗室里通過模擬方式，在實驗室里進行可視化的驗證。

Matlab 當中提供的一些數據導入的程序，以及一些可視化的應用，為模擬系統的搭建提供了便利。

比如，我們可以通過這些數據介面的操作函數，導入 excel 工作表、視頻文件、CAN 匯流排數據，我們也可以通過自動駕駛系統工具箱當中提供的函數來生成鳥瞰圖以及雷達點雲圖。

上圖就是一個典型的模擬環境，上面是整個系統的架構，下面是模擬的硬體設備，我們採用一台目標機來運行車輛與環境的模型，然後另一台運行感知與控制演算法，完成一個硬體再迴路的模擬工作。

通過這個視頻，我們所有的演算法都是能夠自動生成 C 語言代碼的，我們將生成的代碼下載到目標計算機當中，就可以很容易完成硬體再迴路的模擬。

對演算法進行實車的測試，也是必不可少的一個環節。我們在各種不同的路況下，可以對車載系統進行實車測試，這個系統可以是一台筆記本電腦，也可以是一台實時的模擬計算機。

我們通過 USB、CAN 匯流排、乙太網等等，將它與各個感測器進行連接，就可以測試我們的目標跟蹤，以及控制演算法。

我們可以看到，在整個系統的設計與驗證的過程中，MATLAB、Simulink 提供了一個通用的軟體設計與硬體設施的平台，其中就包括使用 MATLAB 設計感知演算法和 Simulink 來設計控制演算法，將這些演算法自動生成 C 語言代碼，以及使用 Simulink Real-Time 完成演算法的實時模擬工作，進行系統的驗證。

喜歡這篇文章嗎？立刻分享出去讓更多人知道吧！