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

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

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

前言

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

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

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

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

1 跟蹤系統(tǒng)設(shè)計

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

圖1 跟蹤系統(tǒng)流程圖

1.1 檢測結(jié)果獲取

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

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

1.2 雙波門GNN關(guān)聯(lián)算法

在常用的數(shù)據(jù)關(guān)聯(lián)算法中，GNN算法是一種不考慮干擾的關(guān)聯(lián)算法，直接將濾波算法預(yù)測結(jié)果與最相似的檢測目標關(guān)聯(lián)。而JPDA與MHT綜合考慮了環(huán)境中的雜波干擾，與GNN算法相比，優(yōu)勢是關(guān)聯(lián)精準更高，劣勢是速度較慢。

激光雷達原始點云數(shù)據(jù)量龐大，每幀多達數(shù)萬個檢測點，但經(jīng)過目標檢測后，跟蹤算法的輸入僅有數(shù)十個檢測框。環(huán)境干擾對跟蹤算法的影響已降到較低水平，在這種情況下GNN算法結(jié)構(gòu)簡單、速度較快的優(yōu)勢便體現(xiàn)出來，因此本文中選擇GNN關(guān)聯(lián)算法作為基礎(chǔ)的關(guān)聯(lián)算法。

1.2.1 關(guān)聯(lián)向量與關(guān)聯(lián)距離

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

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

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

1.2.2 雙波門算法

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

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

圖2 雙波門示意圖

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

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

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

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

1.2.3 關(guān)聯(lián)匹配

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

圖3 關(guān)聯(lián)示意圖

1.3 IMM跟蹤器設(shè)計

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

1.3.1 運動模型的選定

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

第j種運動狀態(tài)方程在k時刻為

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

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

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

1.3.2 狀態(tài)交互

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

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

1.3.3 KF濾波狀態(tài)更新

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

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

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

1.3.4 最終估計

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

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

式中c為歸一化常數(shù)。

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

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

1.4 軌跡管理

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

2 實驗

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

2.1 離線實驗

2.1.1 數(shù)據(jù)集

本文中采用KITTI的跟蹤數(shù)據(jù)集，其數(shù)據(jù)集分為包含真值數(shù)據(jù)的21個序列共8 008幀訓(xùn)練數(shù)據(jù)和不包含真值數(shù)據(jù)的29個序列共11 095幀測試數(shù)據(jù)。本文中對訓(xùn)練數(shù)據(jù)進行了訓(xùn)練集和驗證集的分割，并依據(jù)相關(guān)文獻［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切換數(shù)（ID switch，IDS）和跟蹤軌跡的中斷數(shù)（fragmentations，F(xiàn)RAG）等。

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

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

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

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

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

2.1.3 實驗結(jié)果

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

表1 跟蹤結(jié)果

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

圖4 跟蹤結(jié)果

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

2.2 對比實驗

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

2.2.1 不同的關(guān)聯(lián)算法

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

表2 關(guān)聯(lián)算法對比

2.2.2 不同的關(guān)聯(lián)向量與關(guān)聯(lián)距離

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

表3 關(guān)聯(lián)向量與關(guān)聯(lián)距離對比

2.2.3 不同的軌跡管理閾值設(shè)置

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

表4 軌跡管理模塊設(shè)計對比

2.2.4 不同波門算法

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

表5 不同波門算法對比

2.3 實車驗證

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

圖5 無人車實驗平臺

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

圖6 實車數(shù)據(jù)驗證

3 結(jié)論

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