基于路側(cè)激光雷達(dá)的多目標(biāo)檢測與跟蹤算法

顧 晶,胡夢寬

(1.無錫學(xué)院電子信息工程學(xué)院,江蘇 無錫 214105;2.南京信息工程大學(xué)電子與信息工程學(xué)院,江蘇 南京 210044)

1 引 言

在智能交通領(lǐng)域中,使用先進(jìn)的傳感器識別和跟蹤道路上的目標(biāo),是道路交通安全的重要任務(wù)。激光雷達(dá)傳感器由于其探測距離遠(yuǎn)、精度高、光線環(huán)境適應(yīng)性好等優(yōu)點(diǎn),被廣泛應(yīng)用于自動(dòng)駕駛車輛上。如何根據(jù)自動(dòng)駕駛技術(shù),配合安裝在道路基礎(chǔ)設(shè)施上的激光雷達(dá)傳感器準(zhǔn)確檢測和跟蹤城市交叉路口的行人和車輛,使之成為無縫銜接的合作系統(tǒng),是當(dāng)今研究的熱點(diǎn)問題[1]。

國內(nèi)外許多團(tuán)隊(duì)都對三維點(diǎn)云的多目標(biāo)檢測與跟蹤方面有著深入研究。在目標(biāo)檢測上,通過利用基于密度的DBSCAN聚類[2]、超體素區(qū)域聚類[3]、連通組件聚類[4]等算法提取目標(biāo)特征信息從而實(shí)現(xiàn)目標(biāo)檢測。在目標(biāo)跟蹤上,常利用卡爾曼濾波器系列[5-6]進(jìn)行目標(biāo)狀態(tài)預(yù)測和更新,結(jié)合基于邊界交并比[7]、外觀運(yùn)動(dòng)特征以及馬氏距離[8]等為度量的常見數(shù)據(jù)關(guān)聯(lián)算法,如全局最近鄰(Global Nearest Neighbor,GNN)[9]、聯(lián)合概率數(shù)據(jù)互聯(lián)(Joint Probabilistic Data Association,JPDA)[10]以及多假設(shè)跟蹤(Multiple Hypothesis Tracking,MHT)等[11],最終實(shí)現(xiàn)三維多目標(biāo)跟蹤。

本文提出一種基于路側(cè)激光雷達(dá)的多目標(biāo)檢測與跟蹤算法,首先利用背景減除法去除大量背景和地面點(diǎn)云,同時(shí)根據(jù)邊界坐標(biāo)選擇感興趣區(qū)域,從而提高檢測精度并降低計(jì)算成本;緊接著采取先驗(yàn)旋轉(zhuǎn)變換矩陣融合5幀點(diǎn)云從而提高細(xì)小目標(biāo)的點(diǎn)云密度,然后通過基于哈希數(shù)據(jù)結(jié)構(gòu)的曲率體素聚類[12]算法高效、準(zhǔn)確的分割點(diǎn)云;之后通過雙門控濾除雜波干擾,利用歷史檢測置信度調(diào)整關(guān)聯(lián)匹配閾值和生存周期管理策略,有效提升算法的速度和關(guān)聯(lián)的準(zhǔn)確性,減少了目標(biāo)丟失和誤檢;最后應(yīng)用IMM-UKF-JPDAF的組合貝葉斯濾波器算法,保證對城市交叉口道路目標(biāo)進(jìn)行準(zhǔn)確跟蹤。實(shí)驗(yàn)結(jié)果表明,該方法具有可靠性、魯棒性和實(shí)時(shí)性。算法總體流程如圖1所示。

圖1 多目標(biāo)檢測跟蹤算法流程圖

2 目標(biāo)檢測算法

2.1 背景濾除

背景點(diǎn)云如樹木、地面和建筑物的濾除是處理城市交叉路口激光雷達(dá)數(shù)據(jù)的重要初始步驟。由于實(shí)驗(yàn)安裝的激光雷達(dá)的位置是固定的,因此選擇背景減除法將當(dāng)前獲取的點(diǎn)云幀與背景幀進(jìn)行差分運(yùn)算,同時(shí)根據(jù)邊界坐標(biāo)選擇感興趣區(qū)域,從而提取出運(yùn)動(dòng)目標(biāo)點(diǎn)云數(shù)據(jù)。圖2所示為目標(biāo)點(diǎn)云去除背景的前后對比,濾除背景后的點(diǎn)云包括機(jī)動(dòng)車、非機(jī)動(dòng)車和行人等道路目標(biāo)以及少數(shù)噪聲點(diǎn)。

圖2 背景濾除

2.2 點(diǎn)云融合

實(shí)驗(yàn)選用非重復(fù)掃描模式的激光雷達(dá),其即時(shí)采樣的數(shù)據(jù)存在點(diǎn)云密度低和運(yùn)動(dòng)畸變的問題。因此本文采取融合連續(xù)5幀點(diǎn)云的策略,根據(jù)旋轉(zhuǎn)角度和平移距離將前4幀點(diǎn)云變換到當(dāng)前時(shí)刻點(diǎn)云的位置,從而增強(qiáng)了點(diǎn)云密度,校正了運(yùn)動(dòng)畸變。圖3為簡單拼接5幀點(diǎn)云和采用先驗(yàn)旋轉(zhuǎn)平移矩陣融合5幀點(diǎn)云的對比圖。從圖3中看出,目標(biāo)點(diǎn)云通過變換融合后密度更大,并且在一定程度上校正了其運(yùn)動(dòng)畸變。先驗(yàn)旋轉(zhuǎn)平移矩陣如:

(1)

圖3 點(diǎn)云融合

式中,R為旋轉(zhuǎn)矩陣,表示為先繞X軸方向旋轉(zhuǎn)θx度,再繞Y軸方向旋轉(zhuǎn)θy度,最終繞Z軸方向旋轉(zhuǎn)θz度而得到;T為平移矩陣,dx、dy、dz表示為在各自坐標(biāo)軸方向上的位移量。

2.3 目標(biāo)聚類

作為目標(biāo)檢測的一個(gè)環(huán)節(jié),點(diǎn)云聚類的效果直接影響檢測的質(zhì)量?，F(xiàn)有的方法不能同時(shí)滿足速度和精度,因此本文選擇曲率體素聚類(CVC)算法分割點(diǎn)云,包括四個(gè)步驟。首先,將笛卡爾坐標(biāo)轉(zhuǎn)換成球面坐標(biāo),隨后構(gòu)建Hash表將體素索引與其內(nèi)點(diǎn)的索引相聯(lián)系,緊接著通過Hash表找到附近9個(gè)體素內(nèi)的相鄰點(diǎn),最后將其合并為一個(gè)簇。球坐標(biāo)與直角坐標(biāo)關(guān)系轉(zhuǎn)化如式:

(2)

式中:r為原點(diǎn)到某點(diǎn)的距離;θ為仰角;φ為方位角。

本文根據(jù)不同區(qū)域設(shè)置不同的閾值α,有效改進(jìn)并提高了CVC算法的聚類效果,其聚類分割的速度比常規(guī)方法快,且具有正確區(qū)分相鄰電動(dòng)車和人群的優(yōu)勢。圖4為使用改進(jìn)曲率體素聚類算法的點(diǎn)云聚類效果圖,不同灰度的點(diǎn)代表不同的道路目標(biāo)。

圖4 改進(jìn)曲率體素聚類效果

3 目標(biāo)跟蹤算法

通過上述目標(biāo)檢測算法提取道路目標(biāo)位姿信息Det(t)作為目標(biāo)跟蹤器的輸入,對于t時(shí)刻的位姿信息Det(t),有Det(t)=(x,y,z,l,w,h,θ),其中(x,y,z)是包圍盒中心點(diǎn)坐標(biāo),(l,w,h)為包圍盒的長寬高,θ為包圍盒的偏航角。

3.1 IMM-UKF算法

在城市交叉路口復(fù)雜道路環(huán)境下,機(jī)動(dòng)目標(biāo)具有上下/左右直行、左轉(zhuǎn)彎和右轉(zhuǎn)彎等運(yùn)動(dòng)狀態(tài),此時(shí)采用單一運(yùn)動(dòng)模型卡爾曼濾波器無法解決非線性運(yùn)動(dòng)問題。因此,本文使用交互式多模型無損卡爾曼濾波即IMM-UKF算法進(jìn)行目標(biāo)跟蹤。

3.1.1 目標(biāo)運(yùn)動(dòng)模型

常見運(yùn)動(dòng)模型有勻速模型(Constant Velocity,CV)、勻加速模型(Constant Acceleration,CA)、恒定轉(zhuǎn)彎率和速度模型(Constant Turn Rate and Velocity,CTRV)等[13]。本文根據(jù)實(shí)際道路場景選擇CV和CTRV作為IMM-UKF算法的子模型,則其離散運(yùn)動(dòng)狀態(tài)空間模型可以表示為:

(3)

CV線性運(yùn)動(dòng)模型假定目標(biāo)是直線運(yùn)動(dòng)的,并不考慮物體的轉(zhuǎn)彎,將k時(shí)刻的目標(biāo)狀態(tài)X(k)=[xk,yk,vk,θk]T推算到k+1時(shí)刻,只有位置發(fā)生改變,其狀態(tài)轉(zhuǎn)移函數(shù)為:

(4)

(5)

3.1.2 算法遞推過程

IMM-UKF算法過程包括4個(gè)步驟:交互輸入、模型條件濾波、模型概率更新和估計(jì)融合[14]。該算法從k-1到k時(shí)刻的遞推過程如下:

(1)狀態(tài)交互:根據(jù)上一時(shí)刻融合估計(jì)的結(jié)果對模型初始化,得到當(dāng)前時(shí)刻狀態(tài)向量和協(xié)方差矩陣作為UKF的輸入。定義IMM模型之間通過馬爾可夫轉(zhuǎn)移概率矩陣P進(jìn)行交互,其中pmn為P的第m行n列元素,表示為從模型m切換到模型n的概率。

(6)

(7)

(8)

(9)

(10)

(11)

(12)

(3)模型概率更新:計(jì)算模型n的似然概率:

(13)

(14)

(4)融合估計(jì):計(jì)算與更新后的模型概率結(jié)合的k時(shí)刻融合狀態(tài)向量的估計(jì)值與融合誤差協(xié)方差矩陣,其表達(dá)式如式(15)和式(16)所示:

(15)

(16)

3.2 自適應(yīng)雙門控JPDA關(guān)聯(lián)算法

為了準(zhǔn)確跟蹤道路上的多目標(biāo),本文引入JPDA算法,并將IMM-UKF算法融入到其算法框架中,因此可以在目標(biāo)運(yùn)動(dòng)狀態(tài)發(fā)生改變時(shí)仍具有較高的跟蹤精度。該算法主要包括產(chǎn)生確認(rèn)矩陣、生成聯(lián)合事件、計(jì)算關(guān)聯(lián)概率、獲取綜合觀測值等步驟,其中,計(jì)算關(guān)聯(lián)概率是關(guān)鍵步驟之一,表達(dá)式如下:

(17)

(18)

(19)

為了有效濾除雜波干擾,提升跟蹤精度,在檢測結(jié)果進(jìn)行最終JPDA關(guān)聯(lián)匹配前設(shè)置自適應(yīng)閾值雙門控驗(yàn)證,雙門控示意圖如圖5所示。初級門控以歐氏距離為度量,在歐式空間距離上小于閾值ρec的目標(biāo)才能進(jìn)入次級門控;次級門控以馬氏距離為度量,當(dāng)目標(biāo)和檢測目標(biāo)間的馬氏距離小于閾值ρma時(shí)才能進(jìn)入最終關(guān)聯(lián)匹配。馬氏距離通過在歐氏距離的基礎(chǔ)上添加協(xié)方差權(quán)重,可以更好地區(qū)分目標(biāo)間的相似度。歐式距離和馬氏距離的表達(dá)式如下:

(20)

圖5 雙門控示意圖

(21)

然而,當(dāng)門控閾值設(shè)置過小時(shí),進(jìn)入波門內(nèi)的目標(biāo)減少,隨著時(shí)間的推移,可能會(huì)丟失跟蹤目標(biāo)的軌跡;當(dāng)門控閾值設(shè)置過大時(shí),進(jìn)入波門內(nèi)的目標(biāo)增多,則造成算法的復(fù)雜度增大,算法運(yùn)行時(shí)間變長。針對此問題,提出自適應(yīng)門控閾值調(diào)整算法,如圖6所示。針對多次跟蹤丟失的目標(biāo),通過5幀內(nèi)歷史檢測置信度的變化,以目標(biāo)量測預(yù)測為中心,逐步擴(kuò)大或減小目標(biāo)的搜索范圍,保證目標(biāo)關(guān)聯(lián)更加穩(wěn)定,進(jìn)一步提升跟蹤精度。

圖6 自適應(yīng)門控閾值調(diào)整示意圖

3.3 自適應(yīng)生存周期管理策略

由于目標(biāo)檢測算法的誤檢和漏檢問題普遍存在,需要建立軌跡管理模塊管理軌跡的生成和刪除。即當(dāng)目標(biāo)被連續(xù)檢測到Fmin幀后才判定新軌跡的生成,同樣地,當(dāng)目標(biāo)未能成功匹配上Fmax幀后才判定軌跡的消亡并將其刪除。文獻(xiàn)[7]指出,若采用固定的生存周期管理策略,將出現(xiàn)錯(cuò)誤的跟蹤軌跡。因此。本文采取文獻(xiàn)[7]中的自適應(yīng)生存周期管理策略,通過歷史檢測置信度,動(dòng)態(tài)調(diào)整最大生存周期,表達(dá)式如下:

FAmax=Fmax×σ(α·score+β)

(22)

式中,FAmax表示根據(jù)置信度計(jì)算后的生存周期;Fmax為最大生存周期,σ(·)表示Sigmoid非線性函數(shù);α和β為尺度系數(shù)和偏移系數(shù);score為歷史檢測置信度。由于Sigmoid函數(shù)的曲線特性,當(dāng)目標(biāo)的置信度越高時(shí),其生存周期也會(huì)越長,從而實(shí)現(xiàn)生存周期的動(dòng)態(tài)調(diào)整,顯著減少了目標(biāo)丟失和誤檢。

4 實(shí)驗(yàn)結(jié)果與分析

4.1 實(shí)驗(yàn)準(zhǔn)備與評價(jià)指標(biāo)

4.1.1 路側(cè)激光雷達(dá)數(shù)據(jù)采集系統(tǒng)

本文使用大疆旗下的Livox Mid-40激光雷達(dá),其最大探測距離可達(dá)260 m,有效視場角為38.4°,距離精度為2 cm。實(shí)驗(yàn)選擇在Visual Studio 2019,i9-12900H CPU @ 2.50 GHz,16 GB內(nèi)存的Windows11系統(tǒng)環(huán)境下進(jìn)行。將Mid-40固定于城市交叉路口上空5 m處收集道路目標(biāo)點(diǎn)云數(shù)據(jù),同時(shí)安裝攝像頭獲取同步視頻用于實(shí)驗(yàn)結(jié)果分析。設(shè)備由智能機(jī)箱接入電源和網(wǎng)絡(luò),道路點(diǎn)云通過筆記本上的Livox Viewer軟件實(shí)時(shí)顯示并錄制保存。路側(cè)激光雷達(dá)數(shù)據(jù)信息采集系統(tǒng)如圖7所示。實(shí)驗(yàn)采集了60 min共36000幀激光雷達(dá)點(diǎn)云數(shù)據(jù),選擇其中2000幀分為8個(gè)序列作為實(shí)驗(yàn),數(shù)據(jù)包括機(jī)動(dòng)車、非機(jī)動(dòng)車、行人等不同目標(biāo)類別。

圖7 路側(cè)激光雷達(dá)點(diǎn)云信息采集系統(tǒng)

4.1.2 實(shí)驗(yàn)場地

本文實(shí)驗(yàn)場地設(shè)在無錫市新吳區(qū)的某道路交叉路口處,該路口視野開闊,車流量和人流量較大。將設(shè)備安裝于監(jiān)控立桿上,調(diào)整角度使攝像頭和激光雷達(dá)掃描區(qū)域?yàn)樗南蚨嘬嚨?如圖8所示。圖8(a)為攝像頭所拍的實(shí)時(shí)交通場景1快照,圖8(b)為(a)對應(yīng)的場景1激光雷達(dá)圖;圖8(c)為攝像頭所拍的實(shí)時(shí)交通場景2快照,圖8(d)為(c)對應(yīng)的場景2激光雷達(dá)圖。

圖8 實(shí)驗(yàn)場地

4.1.3 評價(jià)指標(biāo)

本文采用文獻(xiàn)[15]中提出的多目標(biāo)跟蹤評價(jià)指標(biāo)來量化跟蹤的效果,主要包括多目標(biāo)跟蹤準(zhǔn)確度(Multiple Object Tracking Accuracy,MOTA)、多目標(biāo)跟蹤精度(Multiple Object Tracking Precision,MOTP)、ID切換次數(shù)(ID Switches,IDS)、多數(shù)跟蹤軌跡命中率(Mostly Trackedtrajectories,MT)、多數(shù)丟失軌跡丟失率(Mostly Lost Trajectories,ML)、軌跡打斷次數(shù)(Fragmentations,FRAG)等。其中,MOTA反映誤報(bào)、錯(cuò)報(bào)和軌跡切換的目標(biāo)占跟蹤真值中的比例;MOTP用于衡量目標(biāo)位置上的精確度,主要體現(xiàn)檢測器的性能;MT表示跟蹤軌跡命中80 %以上的目標(biāo)比例。根據(jù)各指標(biāo)的含義和計(jì)算方式可以得出:MOTA、MOTP、MT的值越大表示跟蹤效果越好,而IDS、ML、FRAG的值則應(yīng)越小越好。

4.2 實(shí)驗(yàn)結(jié)果

AB3DMOT是三維多目標(biāo)跟蹤的基準(zhǔn)算法,其中3D目標(biāo)檢測使用與本文算法相同的檢測模型,目標(biāo)跟蹤則使用原有的三維卡爾曼濾波和匈牙利算法的直接組合進(jìn)行目標(biāo)的狀態(tài)估計(jì)和數(shù)據(jù)關(guān)聯(lián)。實(shí)驗(yàn)使用實(shí)測數(shù)據(jù)集中選擇的2000幀點(diǎn)云對算法性能進(jìn)行驗(yàn)證,同時(shí)與基準(zhǔn)AB3DMOT算法進(jìn)行比較,結(jié)果如表1所示。

表1 多目標(biāo)跟蹤結(jié)果

表1的結(jié)果表明,本文算法在大部分跟蹤指標(biāo)上均高于基準(zhǔn)算法,其中跟蹤準(zhǔn)確度MOTA值與基準(zhǔn)算法相比提高近20 %,命中率MT和丟失率ML以及跟蹤打斷次數(shù)FRAG指標(biāo)都有明顯提升,意味著本文算法能夠長時(shí)間穩(wěn)定的跟蹤道路目標(biāo)。在這樣的結(jié)果下,僅在實(shí)時(shí)性上低于AB3DMOT基準(zhǔn)算法,但也達(dá)到了11.12 f/s,滿足實(shí)時(shí)性的需求。分析其中的原因,可以發(fā)現(xiàn)城市交叉路口的道路目標(biāo)具有多種運(yùn)動(dòng)狀態(tài),使用卡爾曼濾波的基準(zhǔn)算法無法解決此類非線性運(yùn)動(dòng)問題,同時(shí)數(shù)據(jù)集中包含了比較多非機(jī)動(dòng)車小目標(biāo),相較于使用交并比作為關(guān)聯(lián)度量,使用本文的自適應(yīng)雙門控關(guān)聯(lián)算法能更好的關(guān)聯(lián)道路小目標(biāo),避免出現(xiàn)檢測框和預(yù)測框交集較少或無交集的問題。

圖9展示了多目標(biāo)檢測與跟蹤的可視化結(jié)果。圖9(a)為第100幀、107幀和155幀的RGB圖像,圖9(b)為(a)對應(yīng)的點(diǎn)云圖。在圖9(b)的第#100幀中選取斑馬線后方的道路目標(biāo)0、6、8、21、35作為觀察對象,此時(shí)目標(biāo)處于靜止?fàn)顟B(tài);在第107幀中目標(biāo)21暫時(shí)丟失,其余目標(biāo)仍保持跟蹤編號不變;在第155幀中道路目標(biāo)處于直行的交通狀態(tài),目標(biāo)6、8、35一直存在且編號未發(fā)生變化,目標(biāo)21在連續(xù)丟失4幀的情況下重新出現(xiàn)后仍被正確關(guān)聯(lián),僅電動(dòng)車目標(biāo)0在長時(shí)間檢測不到的情況下跟蹤編號發(fā)生了改變。根據(jù)這幾幀數(shù)據(jù)的可視化結(jié)果,表明本文算法具有較好的關(guān)聯(lián)能力和良好穩(wěn)定的多目標(biāo)檢測與跟蹤效果。

圖9 多目標(biāo)檢測與跟蹤可視化結(jié)果

4.3 消融實(shí)驗(yàn)

為了驗(yàn)證文中所提的自適應(yīng)雙門控算法和生存周期管理策略的有效性,在實(shí)測數(shù)據(jù)集上進(jìn)行了消融實(shí)驗(yàn)。

比較單門控、雙門控和自適應(yīng)雙門控算法的跟蹤性能,實(shí)驗(yàn)結(jié)果如表2所示。結(jié)果表明,相對于單門控,雙門控算法在MOTA指標(biāo)上有1.65 %的提升,同時(shí)降低了身份互換的次數(shù),在運(yùn)行速度上達(dá)到最優(yōu)。在雙門控算法的基礎(chǔ)上,自適應(yīng)雙門控算法在MOTA和MT上分別提升了1.44 %和1.69 %,僅在IDS和FPS上有所下降,表明其能夠執(zhí)行更加穩(wěn)健的數(shù)據(jù)關(guān)聯(lián),同時(shí)運(yùn)行速度仍優(yōu)于單門控。

表2 不同關(guān)聯(lián)匹配設(shè)置性能比較

為了方便比較有無、固定與自適應(yīng)的生存周期管理策略的跟蹤性能,設(shè)置最大生存周期Fmax值均為5,實(shí)驗(yàn)結(jié)果如表3所示。結(jié)果表明,固定生存周期管理策略相比于無設(shè)置,其MOTA、MT、ML指標(biāo)均有明顯提升,僅FRAG值比無設(shè)置時(shí)大。與固定設(shè)置相比,自適應(yīng)設(shè)置在跟蹤準(zhǔn)確度MOTA性能上提升了1.56 %,同時(shí)跟蹤打斷次數(shù)FRAG值有所降低,驗(yàn)證了自適應(yīng)生存周期管理策略能夠改善多目標(biāo)跟蹤效果,減少目標(biāo)丟失和漏檢,保證跟蹤過程中的穩(wěn)定性。

表3 生存周期管理策略設(shè)計(jì)對比

5 結(jié) 論

本文提出了一種基于路側(cè)激光雷達(dá)的多目標(biāo)檢測與跟蹤算法。首先利用背景減除法去除大量背景點(diǎn)云,極大地提高了算法的運(yùn)行速度;緊接著采取三維點(diǎn)云剛體變換融合5幀數(shù)據(jù),提高了細(xì)小目標(biāo)的點(diǎn)云密度并校正其運(yùn)動(dòng)畸變;然后利用基于哈希數(shù)據(jù)結(jié)構(gòu)的曲率體素聚類算法高效、準(zhǔn)確的分割點(diǎn)云;之后通過歷史檢測置信度,利用可變閾值的雙門控設(shè)計(jì)和生存周期管理策略,自適應(yīng)地調(diào)整數(shù)據(jù)關(guān)聯(lián)的搜索范圍和目標(biāo)最大生存周期,提升了算法的速度和關(guān)聯(lián)的準(zhǔn)確性,減少了目標(biāo)丟失和誤檢;最后利用IMM-UKF-JPDAF的組合貝葉斯濾波器算法,保證對城市交叉口復(fù)雜環(huán)境下的具有非線性隨機(jī)運(yùn)動(dòng)模型的道路目標(biāo)進(jìn)行準(zhǔn)確跟蹤。實(shí)驗(yàn)結(jié)果表明,本文算法在真實(shí)道路交通場景中具有較高的跟蹤準(zhǔn)確度和良好的實(shí)時(shí)性。