基于激光雷達的3D實時車輛跟蹤*

王 海，李 洋，蔡英鳳，孫 愷，陳 龍

（1.江蘇大學汽車與交通工程學院，鎮江 212013；2.江蘇大學汽車工程研究院，鎮江 212013；3.上海禾賽科技股份有限公司，上海 201702）

前言

隨著智能汽車的不斷發展，其對周圍環境的感知需求日益增加。在環境感知算法中，多目標跟蹤算法是重要的組成部分之一，其為智能汽車的軌跡規劃與決策提供了必要的環境信息。相較于智能車輛上的攝像頭與毫米波雷達，激光雷達具有感知精度高、對光照和天氣適應能力強等特點，受到學術界和工業界的廣泛關注。

跟蹤算法主要目的是將前后幀目標檢測結果進行匹配，進而實現對目標狀態的連續感知。現有3D多目標跟蹤算法由數據關聯算法結合濾波算法組成。數據關聯算法主要解決目標間的關聯問題，濾波算法主要解決目標的狀態估計和軌跡更新問題。常見的數據關聯算法有聯合概率數據互聯（joint probabilistic data association，JPDA）［1］、多假設跟蹤（multiple hypothesis tracking，MHT）［2］、全局最近鄰關聯（global nearest neighbor，GNN）［3］等。常見的濾波算法有卡爾曼濾波（Kalman filter，KF）［4-5］系列、粒子濾波（particle filter，PF）［6］以及適應目標機動工況的交互多模型算法（interacting multiple model，IMM）［7］等。

現有的3D多目標跟蹤方法，例如文獻［8］～文獻［12］中基于檢測的跟蹤，將檢測算法與跟蹤算法耦合，整體精度較高。但由于跟蹤算法精度依賴于檢測算法的精度，將檢測算法與跟蹤算法耦合會掩蓋跟蹤算法本身的精度，無法確定是由于先進的感知算法帶來的跟蹤性能提升還是由于跟蹤算法的改進帶來的性能提升。同時這樣的耦合所帶來的性能提升是以整個系統的復雜性和計算成本為代價的，例如文獻［8］～文獻［13］中方法具有較好的性能精度，但其系統的整體復雜度較高，無法滿足高實時性的需求。

針對上述問題，本文中提出了一種基于激光雷達的3D實時車輛跟蹤方法。使用GNN算法作為數據關聯算法，并通過雙波門設計，提升了算法精度與速度。同時為消除檢測與跟蹤算法的耦合關系，采用目標位置信息與目標體積信息作為關聯向量，并使用馬氏距離作為關聯距離以提升關聯的準確性。最后使用3D IMM?KF算法，保證對機動目標的跟蹤，實現了一個魯棒性強、實時性好的跟蹤系統。

1 跟蹤系統設計

本文中提出的基于激光雷達的3D實時車輛跟蹤系統，由3個部分組成：（1）使用雙波門GNN的前后幀目標關聯模塊；（2）使用3DKF實現的IMM跟蹤濾波器；（3）結合歷史軌跡信息的跟蹤軌跡生成和消亡管理器。其整體系統流程如圖1所示。

圖1 跟蹤系統流程圖

1.1 檢測結果獲取

隨著智能車輛檢測算法的不斷發展，不斷涌現出性能優異的檢測算法，例如文獻［14］～文獻［16］。本文中采用文獻［16］中所提出的檢測算法，未進行任何改動。該檢測算法并不具有領先的性能，目前其精度在KITTI的基準測試榜單中為75.64%，相較于主流檢測方法有5%以上的精度差距。采用該算法是因為其算法開源且傳播廣泛，能更加體現出所提跟蹤算法的綜合優勢。

同時為保證跟蹤算法的通用性，本文中不使用除3D檢測框結果以外的信息。對于k時刻的檢測結果z（k），有z（k）=（x，y，z，l，w，h，θ），其中（x，y，z）為3D檢測框中心點的三維坐標，（l，w，h）為3D檢測框的長寬高，θ為3D檢測框朝向角。

1.2 雙波門GNN關聯算法

在常用的數據關聯算法中，GNN算法是一種不考慮干擾的關聯算法，直接將濾波算法預測結果與最相似的檢測目標關聯。而JPDA與MHT綜合考慮了環境中的雜波干擾，與GNN算法相比，優勢是關聯精準更高，劣勢是速度較慢。

激光雷達原始點云數據量龐大，每幀多達數萬個檢測點，但經過目標檢測后，跟蹤算法的輸入僅有數十個檢測框。環境干擾對跟蹤算法的影響已降到較低水平，在這種情況下GNN算法結構簡單、速度較快的優勢便體現出來，因此本文中選擇GNN關聯算法作為基礎的關聯算法。

1.2.1 關聯向量與關聯距離

在GNN關聯算法中，如何綜合評價前后幀目標間的相似度對關聯算法十分重要，文獻［11］～文獻［17］中使用的關聯向量不包含物體的3D信息，文獻［18］中使用的關聯向量除物體3D信息外還包含目標點云的反射強度信息。要提升相似度計算的全面性和綜合性，需盡可能增多關聯向量中包含的特征信息，但為保證跟蹤算法的通用性，一些特殊的關聯信息應該予以排除。由上述兩點考慮，本文中選擇（x，y，z，l，w，h）作為關聯向量。對于朝向角θ，其滿足上述兩點的要求，但是由于3D檢測對于物體的朝向信息容易發生混亂，實驗過程中發現關聯向量中加入朝向角θ，反而會導致跟蹤精度的下降，因此在關聯向量中去除朝向角θ。

對于關聯距離的計算方式，文獻［11］和文獻［12］中選用歐式距離進行計算。歐氏距離計算方便，直接反映了兩個物體間的空間距離。然而歐式距離對關聯向量中的各個參數進行等權重的計算，無法評判不同參數的不同重要性，因此本文中選用以協方差作為權重的馬氏距離進行計算［19］，從而能更好區分目標間的相似度。馬氏距離dm為

式中：v（k）為z(k)-z(k∣k-1)，表示新息；S（k）為v（k）的協方差。

1.2.2 雙波門算法

如圖1所示，檢測結果在進行關聯匹配前，需要進行波門的驗證。波門是以預測狀態為中心的判定區域，只有落入波門內的檢測，才會執行后續關聯算法。而GNN算法匹配邏輯簡單，波門設置方式對算法精度影響較大。以往算法只設置一道波門進行濾波，導致對目標距離信息的判定不夠全面，同時在單波門的情況下對所有目標進行馬氏距離的計算判定會耗費大量算力，導致算法實時性下降。

針對以上問題，提出雙波門濾波方法，其結構如圖2所示。首先使用歐式距離γe作為閾值，進行歐式波門的篩選。此次篩選僅對空間坐標（x，y，z）進行篩選，將在空間距離上大于閾值的觀測值進行去除。

圖2 雙波門示意圖

式中v'（k）為只包含（x，y，z）信息的新息。

隨后使用馬氏距離作為馬氏波門的閾值γm，對滿足歐式波門的檢測結果再進行馬氏距離的計算，濾除符合歐式波門但不符合馬氏波門的檢測。

式中：v''（k）為v（k）去除朝向角θ的新息；S''（k）為S（k）去除朝向角θ對應維度后的協方差。

通過進行雙波門設置可有效濾除雜波干擾，提升算法精度。在算法速度上，通過歐式波門設置減少了較為費時的馬氏距離計算次數，同時雙波門設置的兩次濾波減少了最終馬氏波門內需進行關聯匹配的目標數量，減少了關聯匹配的計算次數，從兩方面綜合提升了算法速度。

1.2.3 關聯匹配

在進行雙波門濾波后，對滿足雙波門閾值的檢測進行關聯匹配。若幾個較近的軌跡波門內不存在相互干擾的檢測結果，則使用波門內馬氏距離最近的檢測目標作為匹配目標。如存在如圖3所示的復雜情況，即預測位置P2其馬氏波門內的檢測z2與預測位置P1的距離最近，檢測z3與預測位置P3距離最近的情況，則使用匈牙利算法作為匹配算法，將其代價矩陣定義為C m（nm為已有軌跡數，n為所有檢測數），其中元素為cij。

圖3 關聯示意圖

1.3 IMM跟蹤器設計

現實中，實際車輛的運動如換道、超車等為機動狀態，是幾個運動模型的復合，因此只采用單一運動模型的KF濾波器無法適應該種情況。針對此類問題，使用可以應對機動目標情況的IMM算法，將多個濾波器結合起來進行跟蹤。

1.3.1 運動模型的選定

IMM算法是通過考慮多個模型的交互轉換來實現機動目標跟蹤，使用時需要選定多個模型。而常用模型如勻速模型（constant velocity，CV）、勻轉速模型（constant turn rate，CTR）、勻轉速與速度模型（constant turn rate and velocity，CTRV）等都是針對點目標的運動情況，其狀態向量中不包含3D信息，只針對2D信息進行計算。由于沒有針對3D物體的跟蹤模型，本文中針對3D物體的運動特點提出使用3DCTR模型與3DCV模型的IMM?KF算法。

第j種運動狀態方程在k時刻為

式中：x（jk）為狀態向量；F j為狀態轉移矩陣；H j為觀測矩陣；W j(k)為過程噪聲，是協方差Q j的高斯噪聲；V j(k)為觀測噪聲，是協方差R j的高斯噪聲。

在3DCV模型中k時刻的狀態量x（1k）為（xk，vx，yk，vy，zk，vz，θ，l，w，h）T，基于3D CV模型將x（1k-1）中的信息推算到k時刻，勻速推算只改變3維位置，不改變其余參數。

在3D CTR模型中k時刻的狀態量x（2k）為（xk，vx，yk，vy，zk，vz，θk，vθ，l，w，h）T，基于3D CTR模型將x（2k-1）的信息推算到k時刻，勻轉速推算只改變3D位置和朝向角θ，不改變其余參數。

1.3.2 狀態交互

定義模型間轉移概率矩陣為P，其中元素pij為從第i個運動模型轉移到第j個模型的概率。

從k-1時刻的狀態估計xj(k-1|k-1)、模型j的概率u j(k-1)計算k-1時刻混合狀態估計x0j(k-1|k-1)與混合協方差P0j(k-1|k-1)。

1.3.3 KF濾波狀態更新

對于模型j進行卡爾曼濾波狀態更新。首先，計算預測狀態與預測協方差P j(k|k-1)：

計算卡爾曼增益K j(k)：

最后，進行狀態更新，計算更新后狀態與協方差P j(k|k)：

1.3.4 最終估計

計算模型j的可能性Λj(k)：

計算模型j的概率u j(k)：

式中c為歸一化常數。

計算最終狀態估計(k|k)：

計算最終協方差估計P(k|k)：

1.4 軌跡管理

由于檢測算法會產生誤檢和漏檢，為消除其對跟蹤算法的影響，建立軌跡管理模塊對軌跡的生成和消亡進行管理。本文中采用基于歷史軌跡信息的軌跡管理器，其依托于歷史軌跡的跟蹤信息對軌跡的生成和消亡進行管理。對于軌跡的生成，由于檢測算法會出現誤檢，因此不能直接將新目標當作軌跡起始。設置閾值fs與fmin，當在連續fs幀中，新目標能夠成功關聯fmin幀，即判定新軌跡確認。通過合適的fs與fmin閾值設定，即可有效消除檢測中誤檢的影響，準確生成軌跡。對于軌跡消亡采取類似的閾值fd與fmax，在連續的fd幀中，已有軌跡fmax幀沒有關聯到檢測，即將該軌跡刪除實現軌跡的消亡。

2 實驗

為了驗證上述所提出的跟蹤方法在實際復雜環境中的跟蹤性能，在公開數據集KITTI上對算法進行評估，同時在無人車實驗平臺上進行真實道路環境中的算法驗證。

2.1 離線實驗

2.1.1 數據集

本文中采用KITTI的跟蹤數據集，其數據集分為包含真值數據的21個序列共8 008幀訓練數據和不包含真值數據的29個序列共11 095幀測試數據。本文中對訓練數據進行了訓練集和驗證集的分割，并依據相關文獻［17］，使用1、6、8、10、12、13、14、15、16、18、19序列進行驗證。

2.1.2 評價方法

在文獻［11］和文獻［12］中評價跟蹤精度所使用的速度誤差和航向角誤差，更適合單目標跟蹤，無法綜合評價多目標跟蹤的精度。因此本文中采用多目標跟蹤指標（multiple object tracking，MOT）［20-21］、包含多目標跟蹤準確度（multiple object tracking accuracy，MOTA）、多目標跟蹤精度（multiple object tracking precision，MOTP）、跟蹤ID切換數（ID switch，IDS）和跟蹤軌跡的中斷數（fragmentations，FRAG）等。

其中MOTA是綜合評價跟蹤軌跡精確性的指標，其反映了誤檢、漏檢和軌跡切換數在整體跟蹤真值中的比例。

式中：FN為漏檢數；FP為誤檢數；IDS為跟蹤ID切換數；GT為真值總數。

MOTP是評價多目標跟蹤位置精準度的指標，反應了跟蹤結果和跟蹤真值之間的差值。

式中：ck為第k幀跟蹤結果與真值的匹配結果；dk，i為第k幀真值i與匹配結果的距離，一般用IoU計算。

MOTA、MOTP值越大跟蹤結果越好，IDS、FRAG值越小跟蹤結果越好。KITTI數據集的官方評價是以2D的形式實現的，對于3D跟蹤則將其投影到2D圖像中再進行評價。這種評價方式對于3D跟蹤是不合適的，在3D跟蹤結果中微小的差異可能會導致2D的結果發生巨大的變化，因此本文中將MOTA與MOTP的評價計算改為3D匹配的方式。

2.1.3 實驗結果

使用在訓練數據中劃分出來的驗證集對算法性能進行驗證，其結果如表1所示。由表1可知，在主要的跟蹤指標MOTA上對比算法mmMOT與FANTrack高了7%，而在MOTP與FRAG指標上較最好的mmMOT算法差距并不大，僅在IDS指標上效果較差。在這樣的結果下，本文中算法實時性達到266.1 FPS，比FANTrack算法快了11.5倍，比mmMOT算法快55.4倍，所提出的跟蹤算法完全滿足智能駕駛實時性的需求。

表1 跟蹤結果

選擇KITTI訓練集序列9進行跟蹤，可視化結果如圖4所示。圖4（a）為3D視角下的跟蹤結果，分別顯示了檢測結果與跟蹤結果，同時對每個跟蹤軌跡分配了編號。圖4（b）～圖4（e）驗證了所提出的跟蹤算法對車輛目標跟蹤效果。對照右側的場景圖片，可以看到在自車前方編號為T279和T217的兩輛車輛在整個跟蹤過程中都保持了跟蹤編號不變，驗證了所提出的跟蹤算法具有穩定跟蹤目標的能力。

圖4 跟蹤結果

如圖4（b）右側的場景圖片所示，第170幀中有效的檢測目標只有T279和T217兩輛車，但檢測算法輸出了多個誤檢目標。此時，在圖4（b）左側的跟蹤結果視圖中所提出的跟蹤算法有效濾除了誤檢目標，生成了正確的軌跡。此種情況在圖4（c）～圖4（e）中也有出現，綜合驗證了所提出跟蹤算法所具有的優秀關聯能力。

2.2 對比實驗

為了驗證論文所提方法的有效性，對關聯算法、關聯向量與關聯距離、軌跡管理和波門設置在KITTI數據集上進行了對比實驗，從而驗證提出方法的有效性。

2.2.1 不同的關聯算法

對比不同的關聯算法，其結果如表2所示。從中可以看出，使用GNN使得MOTA有了極大的提升，同時也實現了IDS與FRAG最小，僅在MOTP指標上較差。正如在1.2節中所述，在獲得3D檢測結果后，干擾有了較大的衰減，因此不考慮干擾的GNN關聯算法反而更加適應此種情況，可以獲得更好的關聯結果。

表2 關聯算法對比

2.2.2 不同的關聯向量與關聯距離

對關聯向量與關聯距離的不同設置進行對比，其結果如表3所示。從表3中第1組與第4組對比可以看出，馬氏距離可以實現更高的MOTA精度。馬氏距離可以更好地描述關聯向量不同信息間的重要程度，使得對目標間距離的評價更加準確。而后3組的實驗對比顯示了單純的增大關聯向量維度并不能帶來MOTA精度的提高。第2組與第4組對比可以看出目標3D體積信息的添加對關聯是有益的。而第3組與第4組對比，朝向角θ的加入，并不能提高MOTA精度。本文中認為3D檢測算法對目標的朝向角敏感度較低，對于同一目標在不同幀中的檢測結果，其朝向角大小可能會發生180°的突變。朝向角發生180°突變，其檢測框位置不會發生變化，然而3D跟蹤的關聯距離會產生較大的變化，從而導致關聯失敗，因此在關聯向量中去除朝向角θ信息反而能帶來MOTA精度的提高。

表3 關聯向量與關聯距離對比

2.2.3 不同的軌跡管理閾值設置

在軌跡管理模塊中對比了不同的fs與fmin、fd與fmax參數值對精度的影響，其結果如表4所示。對比表4后3組實驗可知，增大對歷史軌跡的判斷范圍，即使軌跡生成和消亡的總體判定比fmin/fs與fmax/fd從2/3下降到3/5，仍可減小IDS與FRAG。而在合適的fs與fd設置下，將fmin/fs與fmax/fd升至1，如第1組實驗，即可獲得最小的IDS與FRAG。

表4 軌跡管理模塊設計對比

2.2.4 不同波門算法

對比所提出的雙波門算法與單波門算法的性能，其結果如表5所示。從表5可知，雙波門算法比單波門算法在MOTA指標上有約3%的提高，在MOTP指標上也有1.68%的提升，僅在IDS與FRAG上有所下降。除了精度上的提高，在算法運行速度上，由于雙波門算法減小了跟蹤算法的計算量，使跟蹤速度有了較大的提升，FPS上漲了32.92%。

表5 不同波門算法對比

2.3 實車驗證

使用如圖5所示的無人車實驗平臺，在實際道路上對跟蹤算法進行算法驗證，其結果如圖6所示。

圖5 無人車實驗平臺

圖6分別展示了兩車相近、兩車相離、兩車發生遮擋、兩車遮擋結束4個場景的實景圖和跟蹤結果。圖6（a）和圖6（b）展示了T2車輛與T4車輛兩車逐漸相近、隨后T2車輛加速駛離的過程。在這個過程中兩車跟蹤框無相交，且跟蹤ID沒有發生交換，驗證了所提出跟蹤算法在目標相近時的多目標數據關聯能力。圖6（c）和圖6（d）展示了T4車輛被T1車輛遮擋導致檢測結果消失的情況下，所提出跟蹤算法依然能夠保證對T4車輛的持續跟蹤，確保在遮擋結束后跟蹤ID不發生變化。同時，在圖6（d）中，在T3車輛被T1車輛遮擋導致檢測結果消失情況下，T3車輛仍存在的跟蹤結果也證明了所提出跟蹤算法具有的對遮擋目標的持續跟蹤能力。整個跟蹤過程驗證了所提出的跟蹤算法所具有的良好性能和魯棒性。

圖6 實車數據驗證

3 結論

本文中提出了一種基于激光雷達的3D實時車輛跟蹤算法。針對3D檢測結果雜波較少的特點，提出了雙波門GNN作為數據關聯算法，有效提升了跟蹤精度和跟蹤速度。同時對關聯向量和關聯距離進行了優化，在保證了算法的通用性的前提下提升了跟蹤算法的性能。對于3D機動目標的運動情況，提出了使用3D卡爾曼濾波的IMM算法，保證了對機動目標的跟蹤性能。所提出算法在公開數據集KITTI與無人車實驗平臺上經過了驗證，表明了算法的高精度和良好的實時性。