[發明專利]一種基于激光雷達的動態多目標跟蹤方法

有效

• 申請號: 202110467582.4
• 申請日: 2021-04-28
• 公開（公告）號: CN113160280B
• 公開（公告）日: 2022-07-08
• 發明（設計）人: 劉斌;王紅星;方瑋;黃鄭;李志敏;朱潔;邱佳月;宋煜;孟悅;賴際舟
• 申請（專利權）人: 江蘇方天電力技術有限公司;南京航空航天大學
• 主分類號: G06T7/277
• 分類號: G06T7/277;G06T7/246;G06V10/762;G06K9/62
• 代理公司: 南京鐘山專利代理有限公司 32252
• 代理人: 戴朝榮

1.一種基于激光雷達的動態多目標跟蹤方法，其特征在于，包括如下步驟：

步驟一：獲取原始激光雷達點云并進行聚類，然后對聚類后的結果進行目標識別檢測，并顯示檢測結果；

步驟二：基于目標類別的不同，建立不同的狀態方程表達式，即運動狀態空間模型，并根據上一時刻的系統狀態和所述運動狀態空間模型，預測當前時刻的系統狀態；

步驟三：對已跟蹤目標和當前時刻新檢測的目標進行關聯：

步驟四：通過計算，判斷已跟蹤目標和當前時刻新檢測的目標的關聯結果，基于所述關聯結果構建關聯矩陣matrix_assign_valid；

步驟五：對于每一個已跟蹤目標，若在所述關聯矩陣matrix_assign_valid中成功關聯到當前時刻新檢測的目標m_j，則針對不同類別目標分別進行多模態卡爾曼濾波，然后輸出已跟蹤目標的運動信息；

若未關聯到當前時刻新檢測的目標m_j，則進行目標出現與消失的判斷：對當前時刻新檢測的目標，若沒有被關聯上，將用于生成當前時刻新檢測的目標，對已跟蹤目標如果沒有被關聯到一個當前時刻新檢測的目標，將未關聯目標從已跟蹤目標集合中刪除；

所述目標包括低速目標，所述低速目標的運動狀態空間模型表示為

其中，p_x、p_y分別表示低速目標在x、y方向的位置分量，v_x、v_y分別表示低速目標在x、y方向的速度分量；

所述低速目標的運動模型的狀態轉移函數表示為

其中，Δt表示上一時刻到當前時刻的時間間隔，為上一時刻t的運動狀態，為當前時刻t+Δt的運動狀態。

2.根據權利要求1所述的基于激光雷達的動態多目標跟蹤方法，其特征在于，所述目標還包括高速目標，所述高速目標的運動狀態空間模型表示為

其中，x、y分別表示高速目標在x、y方向的位置分量，v為前向運動速度，為偏航角，即追蹤的目標在當前目標坐標系下與x軸的夾角，逆時針方向為正，取值范圍是[0,2π)，ω是偏航角速度；

所述高速目標的運動模型的狀態轉移函數表示為

其中，為上一時刻t的運動狀態，為當前時刻t+Δt的運動狀態。

3.根據權利要求1所述的基于激光雷達的動態多目標跟蹤方法，其特征在于，所述步驟三中，使用匈牙利算法對已跟蹤目標和當前時刻新檢測的目標進行關聯。

4.根據權利要求3所述的基于激光雷達的動態多目標跟蹤方法，其特征在于，記所述已跟蹤目標為n個，所述當前時刻新檢測的目標為m個，構建一個n×m維的得分矩陣score_matrix，所述得分矩陣score_matrix內各元素滿足如下條件：

其中，max_dist為設定的最大距離閾值，表示目標預測位置和實際量測位置之間的最大偏差；dist(i，j)表示已跟蹤目標預測位置和當前時刻新檢測的目標位置之間的距離，dist(i，j)＝||trac_i.pose-m_j.pose||；min(dist(i，j)，max_dist)表示取兩者的最小值；

trac_i.pose表示直到上一時刻目標的位置，即已跟蹤目標預測位置，m_j.pose表示當前時刻目標的位置，即當前時刻新檢測的目標位置。

5.根據權利要求4所述的基于激光雷達的動態多目標跟蹤方法，其特征在于，采用匈牙利算法求解得分矩陣score_matrix，找到已跟蹤目標和當前時刻新檢測的目標之間的最優分配關系，記最終的求解結果為關聯矩陣matrix_assign_k×2，

即將每個trac_i最優地關聯到一個m_j，找到k對對應關系，且必然有k≤min(n，m)。