莊瀚洋，王曉亮，王春香，楊 明

1. 上海交通大學(xué)密西根學(xué)院，上海 200240； 2. 廣西科技大學(xué)廣西汽車零部件與整車技術(shù)重點(diǎn)實(shí)驗(yàn)室，廣西 柳州 545006； 3. 上海交通大學(xué)自動(dòng)化系，上海 200240； 4. 系統(tǒng)控制與信息處理教育部重點(diǎn)實(shí)驗(yàn)室，上海 200240； 5. 上海工業(yè)智能管控工程技術(shù)研究中心，上海 200240

智能駕駛系統(tǒng)的環(huán)境感知功能對(duì)于車輛的運(yùn)行安全起著至關(guān)重要的作用，目標(biāo)檢測(cè)與跟蹤是環(huán)境感知的核心，目標(biāo)檢測(cè)任務(wù)能夠?qū)煌▍⑴c者進(jìn)行識(shí)別，而目標(biāo)跟蹤任務(wù)則是在時(shí)間尺度上分辨是否為同一目標(biāo)并且進(jìn)行持續(xù)的狀態(tài)估計(jì)?；谌S激光雷達(dá)的目標(biāo)檢測(cè)與跟蹤算法是無人駕駛系統(tǒng)的一個(gè)重要組成部分[1]，對(duì)于路徑規(guī)劃和決策控制等高級(jí)任務(wù)具有重要意義。

現(xiàn)階段基于激光雷達(dá)的目標(biāo)檢測(cè)方法較為成熟[2]：基于先驗(yàn)規(guī)則的檢測(cè)手段，通過地面分割、聚類、生成包圍盒等流程完成目標(biāo)檢測(cè)，但穩(wěn)健性較差[3-6]；近年來基于神經(jīng)網(wǎng)絡(luò)的激光雷達(dá)目標(biāo)檢測(cè)算法趨于成熟，且具備更高的檢測(cè)精度與更好的適應(yīng)性，如PointRCNN[7]、Part-A2Net[8]、PVRCNN[9]等。而基于檢測(cè)的目標(biāo)跟蹤方法通過多幀的數(shù)據(jù)關(guān)聯(lián)與狀態(tài)估計(jì)生成目標(biāo)軌跡[10]，數(shù)據(jù)關(guān)聯(lián)旨在確定多幀數(shù)據(jù)之間多個(gè)目標(biāo)的對(duì)應(yīng)關(guān)系，是目標(biāo)跟蹤的基礎(chǔ)[11-13]；狀態(tài)估計(jì)指的是對(duì)同一目標(biāo)多幀測(cè)量結(jié)果進(jìn)行濾波，并對(duì)目標(biāo)的狀態(tài)量進(jìn)行精確估計(jì)[14-16]。

跟蹤道路上行駛的車輛是智能車輛的首要任務(wù)，車輛跟蹤算法在距離較近時(shí)能夠保持較高的精度。但在高速公路或城市快速路中，車輛速度較快、制動(dòng)距離較長，要求車輛跟蹤功能對(duì)中遠(yuǎn)距離的車輛依舊夠保持準(zhǔn)確、穩(wěn)定的跟蹤效果，對(duì)跟蹤算法提出了嚴(yán)格的要求。然而在車輛距離較遠(yuǎn)時(shí)激光雷達(dá)點(diǎn)云較為稀疏，如圖1所示，檢測(cè)結(jié)果存在較大誤差從而導(dǎo)致跟蹤算法精度下降，并且跟蹤的車輛軌跡出現(xiàn)明顯抖動(dòng)，最終影響決策系統(tǒng)的意圖判斷，產(chǎn)生安全隱患。因此，如何提升遠(yuǎn)距離下車輛跟蹤算法的精度與穩(wěn)定性，是本文擬解決的問題。

對(duì)遠(yuǎn)距離的車輛檢測(cè)與跟蹤需求發(fā)生在道路狀況良好的結(jié)構(gòu)化道路場(chǎng)景中，如高速公路或高架路等，在這種情況下絕大多數(shù)車輛的行駛軌跡是沿著車道線方向的，因此基于該先驗(yàn)假設(shè)，通過車道線的幾何結(jié)構(gòu)對(duì)車輛的行駛朝向做出約束從而提高在車輛朝向估計(jì)上的精度是行之有效的技術(shù)路線，相關(guān)的研究也從仿真與理論方面證明了其效果[17-18]。然而，在實(shí)際場(chǎng)景的應(yīng)用過程中，由于傳感器測(cè)量誤差、標(biāo)定誤差、遠(yuǎn)處點(diǎn)云稀疏等因素會(huì)導(dǎo)致遠(yuǎn)處車道線的朝向估計(jì)準(zhǔn)確性下降，因此如何有效、準(zhǔn)確地獲取車道信息是本文研究課題的核心問題。

圖1 不同距離上激光雷達(dá)的點(diǎn)云數(shù)量Fig.1 Point number of LiDAR point cloud at different distances

在現(xiàn)階段的研究中，車道線建模已經(jīng)是較為成熟的技術(shù)手段，如二值圖像[19]、多項(xiàng)式[20-21]、樣條曲線[22-23]等，但簡(jiǎn)單的直線模型無法處理復(fù)雜彎道，而復(fù)雜的模型又容易過擬合，均無法有效應(yīng)對(duì)任務(wù)要求。智能駕駛車輛的決策與規(guī)劃所依賴的數(shù)字地圖基本是必不可少的部分[24]，無論是OpenStreetMap(OSM)這類拓?fù)涞貓D，或是OpenDRIVE這類高精地圖，都能夠提供基本的道路幾何結(jié)構(gòu)信息。

本文基于上述思路，將數(shù)字地圖中的道路信息進(jìn)行提取與建模，利用道路的朝向約束來增強(qiáng)車輛跟蹤。本文在KITTI數(shù)據(jù)集上展開[25]，在檢測(cè)的基線方法上選擇PVRCNN來進(jìn)行，因?yàn)樵摲椒ㄊ悄壳癒ITTI排行榜上效果最好的三維目標(biāo)檢測(cè)算法之一，基于該方法進(jìn)行研究不僅能提供較為穩(wěn)定的檢測(cè)結(jié)果，而且也能體現(xiàn)道路的朝向約束對(duì)車輛跟蹤方法的提升。由于KITTI數(shù)據(jù)集本身不提供與之配套的高精度地圖，本文通過OpenStreetMap這一開源的地圖數(shù)據(jù)平臺(tái)進(jìn)行車道線的先驗(yàn)信息提取，所提出的方法僅使用道路的朝向信息作為約束為后續(xù)的目標(biāo)跟蹤提供增強(qiáng)，而不需要考慮車道線在空間中的位置、車道線的個(gè)數(shù)、寬度等信息。這種方法具有良好的地圖數(shù)據(jù)類型兼容性，而且在使用更準(zhǔn)確的高精地圖時(shí)會(huì)達(dá)到更好的效果。

本文的系統(tǒng)架構(gòu)如圖2所示，首先通過車輛定位信息首先獲取前方道路的OSM地圖數(shù)據(jù)，然后對(duì)激光雷達(dá)點(diǎn)云數(shù)據(jù)中的車道線信息進(jìn)行提取，并結(jié)合車道線與OSM地圖進(jìn)行道路建模以獲取道路的朝向信息作為車輛的朝向約束；此外，PVRCNN方法用于對(duì)激光雷達(dá)點(diǎn)云數(shù)據(jù)進(jìn)行車輛的檢測(cè)，檢測(cè)的車輛位姿結(jié)果與朝向約束共同用于車輛跟蹤。

圖2 本文的系統(tǒng)架構(gòu)Fig.2 System architecture of the proposed method

1 基于數(shù)字地圖融合的道路建模

1.1 基于反射率的車道線特征提取

用于KITTI數(shù)據(jù)集制作的激光雷達(dá)為Velodyne的64線激光雷達(dá)，所得到的點(diǎn)云數(shù)據(jù)包含三維坐標(biāo)以及反射率等4個(gè)維度的信息。其中，反射率信息能夠在一定程度上反映物體的表面特性，道路上的瀝青等黑色物體的反射率低，而車道線標(biāo)記等白色物體的反射率高，因此本文采用激光強(qiáng)度信息來提取車道線[26]。

車道線的主要組成是虛線，照射到車道線上的點(diǎn)云數(shù)量較少，而場(chǎng)景中的車輛又會(huì)對(duì)車道線產(chǎn)生遮擋，使車道線上的點(diǎn)云變得更加稀疏。因此，從單幀點(diǎn)云中獲取車道線信息較為有限，如圖3(a)所示，對(duì)后續(xù)的車道線擬合造成了較大困難，針對(duì)這一問題，本文采用多幀點(diǎn)云疊加的方式提高密度。根據(jù)KITTI數(shù)據(jù)集提供的傳感器標(biāo)定參數(shù)和車輛定位信息，首先，根據(jù)標(biāo)定參數(shù)將道路點(diǎn)云從激光雷達(dá)坐標(biāo)系轉(zhuǎn)到車輛坐標(biāo)系；然后，根據(jù)定位信息將其轉(zhuǎn)換到世界坐標(biāo)系；最后，對(duì)世界坐標(biāo)系下的點(diǎn)云進(jìn)行疊加。多幀點(diǎn)云疊加的結(jié)果如圖3(b)所示。由圖3(b)可以看出，疊加后的點(diǎn)云相比于單幀點(diǎn)云更加稠密，車道線比之前也更加清晰。

激光雷達(dá)點(diǎn)云的反射率強(qiáng)度值，不僅與物體表面的反射率有關(guān)，也與物體距離、激光入射角有很大的關(guān)系[27]，因此會(huì)導(dǎo)致不同線束的激光照射到同一車道線上產(chǎn)生不同的反射率強(qiáng)度值。為了保證同一車道線上點(diǎn)云強(qiáng)度值的一致性，本文引入一種無監(jiān)督的點(diǎn)云強(qiáng)度修正方法[28]，為每一個(gè)激光束計(jì)算了一個(gè)響應(yīng)函數(shù)，處理其中的非線性關(guān)系。將疊加后的點(diǎn)云投影到鳥瞰圖上的二維柵格中，其中每個(gè)柵格存儲(chǔ)了該處點(diǎn)云的強(qiáng)度值和激光束編號(hào)。在二維柵格中尋找所有包含激光束為i并且強(qiáng)度值為I的點(diǎn)的柵格，然后計(jì)算這些柵格中除了屬于第i個(gè)激光束以外的所有點(diǎn)強(qiáng)度的平均值，并將其作為真實(shí)的反射率強(qiáng)度值。對(duì)于每一個(gè)激光束返回的每一個(gè)強(qiáng)度值均進(jìn)行上述計(jì)算得到真實(shí)強(qiáng)度值，同時(shí)將這種對(duì)應(yīng)關(guān)系存儲(chǔ)在一個(gè)二維查找表中方便后續(xù)查找。本文所使用的三維激光雷達(dá)為64線，而強(qiáng)度返回值的取值范圍為0～0.99、分辨率為0.01，因此二維查找表的大小為64×100?；谶@個(gè)查找表，能夠?qū)⒚總€(gè)點(diǎn)根據(jù)其所屬的激光束和強(qiáng)度值進(jìn)行查表，使用表中存儲(chǔ)的強(qiáng)度值對(duì)原始的強(qiáng)度值進(jìn)行修正。點(diǎn)云強(qiáng)度標(biāo)定的結(jié)果如圖3(c)所示，標(biāo)定后的車道線與地面之間的反射率強(qiáng)度對(duì)比更強(qiáng)、噪聲更少，有助于車道線特征的提取。

車道線與地面之間的反射率強(qiáng)度值在經(jīng)過強(qiáng)度標(biāo)定后呈現(xiàn)較為明顯的二分類趨勢(shì)，因此只需要確定一個(gè)強(qiáng)度閾值就足以進(jìn)行分割。對(duì)于強(qiáng)度閾值的確定，本文采用大津法[29]對(duì)三維點(diǎn)云投影到地面的二維圖像進(jìn)行處理，將每個(gè)點(diǎn)的反射率強(qiáng)度值轉(zhuǎn)化為像素值，求解出強(qiáng)度閾值。最終依據(jù)該閾值對(duì)點(diǎn)云進(jìn)行二值化分割，得到反射率強(qiáng)度較高的點(diǎn)云，并將其視作車道線。分割結(jié)果如圖3(d)所示，道路上的車道線均被正確檢測(cè)出來。由于不同場(chǎng)景下道路和車道線的材質(zhì)不同，道路磨損情況也不同，因此需要對(duì)數(shù)據(jù)中的每一個(gè)場(chǎng)景單獨(dú)進(jìn)行一次強(qiáng)度標(biāo)定和閾值計(jì)算。

1.2 車道線與地圖的融合建模

車輛所在場(chǎng)景周圍的數(shù)字地圖是依據(jù)車輛的導(dǎo)航信息從OpenStreetMap(https:∥www.openstreetmap.org/)中得到的。圖4展示了KITTI數(shù)據(jù)集中某一場(chǎng)景對(duì)應(yīng)的OSM數(shù)據(jù)，其由3個(gè)基本單元組成：節(jié)點(diǎn)(nodes)、路(ways)和關(guān)系(relations)。其中，節(jié)點(diǎn)包含了地理坐標(biāo)點(diǎn)的GPS坐標(biāo)和可用標(biāo)簽列表；路由2～2000個(gè)節(jié)點(diǎn)構(gòu)成，主要用于表示道路、河流、鐵路等；關(guān)系用于描述兩個(gè)或者多個(gè)基本單元的關(guān)系，以形成更加復(fù)雜的結(jié)構(gòu)。

OSM中每個(gè)道路所包含的節(jié)點(diǎn)數(shù)量由道路的復(fù)雜度決定：通常直道由2、3個(gè)點(diǎn)組成，而彎道則可能由數(shù)十個(gè)點(diǎn)組成。通過激光雷達(dá)點(diǎn)云數(shù)據(jù)提取的車道線特征點(diǎn)和OSM數(shù)據(jù)都相對(duì)于世界坐標(biāo)系，因此只需要將OSM數(shù)據(jù)中除了道路以外的其他幾何信息刪除，然后投影到車道線特征點(diǎn)上即可，結(jié)果如圖5所示，其中黃色實(shí)線為OSM中的道路，白色點(diǎn)云為車道線特征點(diǎn)，三原色坐標(biāo)軸中的紅色直線指向車輛前進(jìn)方向。圖5中顯示了自車前后25 m范圍內(nèi)的車道線特征點(diǎn)，其中自車前方的車道線特征點(diǎn)非常稀疏、有效距離只有10 m左右，但是OSM道路不存在視距限制，因此將其作為車道模型，既與道路形狀高度一致又不會(huì)產(chǎn)生過擬合的問題。

由圖5可以看出，OSM的道路形狀與真實(shí)的道路形狀有一定的一致性，但也存在著誤差，為了消除誤差，需要對(duì)OSM道路進(jìn)行旋轉(zhuǎn)平移變換。由于車輛目標(biāo)一般是在二維平面上運(yùn)動(dòng)，因此本文的旋轉(zhuǎn)平移忽略了高度方向上的變換。與無人駕駛中使用的高精地圖不同，OSM中不包含實(shí)際場(chǎng)景中的車道級(jí)信息，而只包含了道路的幾何形狀。本文僅使用道路的朝向信息來增強(qiáng)目標(biāo)跟蹤效果，不考慮車道線在空間中的位置關(guān)系，因此平移變換也被省略，而只需進(jìn)行旋轉(zhuǎn)變換，最終使OSM道路與特征提取的朝向一致即可。

(1)

(2)

式中，oi,j為第i段中第j簇所代表的車道線的主方向；ci為第i段中聚類出的簇的個(gè)數(shù)。車道線建模的結(jié)果如圖6所示，其中不同顏色的點(diǎn)屬于不同的段。與圖5相比，經(jīng)過旋轉(zhuǎn)后的OSM道路與真實(shí)道路基本一致。

2 融合道路模型的車輛跟蹤

車輛的跟蹤方法基于卡爾曼濾波框架[15]，包含目標(biāo)狀態(tài)的預(yù)測(cè)與更新兩部分，但由于空間中存在多個(gè)目標(biāo)車輛，每次進(jìn)行目標(biāo)狀態(tài)更新時(shí)需要將預(yù)測(cè)結(jié)果與當(dāng)前幀的檢測(cè)結(jié)果進(jìn)行對(duì)應(yīng)，因此在預(yù)測(cè)與更新過程中引入數(shù)據(jù)關(guān)聯(lián)模塊進(jìn)行目標(biāo)的對(duì)應(yīng)。在完成基于檢測(cè)結(jié)果的目標(biāo)狀態(tài)更新后，會(huì)將目標(biāo)的朝向狀態(tài)依據(jù)車道約束再次進(jìn)行更新，完成當(dāng)前幀的車輛跟蹤。整個(gè)跟蹤任務(wù)的流程如圖7所示，分為4個(gè)主要部分，其中數(shù)字編號(hào)對(duì)應(yīng)的是本節(jié)中的小節(jié)編號(hào)。

圖3 車道線點(diǎn)云結(jié)果Fig.3 Lane marker detection result from point cloud

圖4 KITTI數(shù)據(jù)集中某一場(chǎng)景對(duì)應(yīng)的OSM數(shù)據(jù)Fig.4 The OSM data of a scene in KITTI dataset

圖5 OSM道路投影到車道線特征點(diǎn)的結(jié)果Fig.5 The projection of OSM lane data on the lane marker features

圖6 與地圖融合后的道路建模結(jié)果Fig.6 The road modeling result by fusing the lane marker detection and digital map

圖7 車輛跟蹤算法Fig.7 The multi-vehicle tracking algorithm

2.1 目標(biāo)狀態(tài)預(yù)測(cè)

在三維目標(biāo)檢測(cè)任務(wù)中，目標(biāo)表示為(x,y,z,l,w,h,θ)，其中(x,y,z)為目標(biāo)在三維空間中的位置，(l,w,h)為目標(biāo)的長寬高，θ為目標(biāo)的朝向。在城市道路環(huán)境中，目標(biāo)一般沿著地平面運(yùn)動(dòng)，z方向的運(yùn)動(dòng)可以忽略。同時(shí)在目標(biāo)跟蹤過程中，目標(biāo)的長寬高變化不明顯、無須對(duì)其進(jìn)行濾波。因此，由檢測(cè)任務(wù)得到的目標(biāo)狀態(tài)量即為(x,y,θ)。

(3)

對(duì)于目標(biāo)車輛的運(yùn)動(dòng)，狀態(tài)向量的微分表達(dá)式為

(4)

假設(shè)離散的時(shí)間步k和連續(xù)時(shí)間值tk相關(guān)，離散的時(shí)間步k+1與連續(xù)時(shí)間值tk+1相關(guān)，tk和tk+1之間的時(shí)間差為Δt，則狀態(tài)轉(zhuǎn)移方程為

(5)

對(duì)式(5)中的積分進(jìn)行求解，可得

(6)

(7)

(8)

由于CTRV模型的狀態(tài)轉(zhuǎn)移方程是非線性的，因此需要利用泰勒展開對(duì)非線性模型進(jìn)行近似，計(jì)算雅克比矩陣J，這里將角標(biāo)k移除掉以減少公式長度

(9)

最終得到擴(kuò)展卡爾曼濾波的預(yù)測(cè)方程則是

Xk+1|k=g(Xk)+E

(10)

Pk+1|k=JPkJT+Q

(11)

式中，J為上述狀態(tài)轉(zhuǎn)移方程的雅可比矩陣；P為狀態(tài)協(xié)方差矩陣，其初始值一般由人為指定；Q為過程噪聲協(xié)方差矩陣。Q的計(jì)算方式為

(12)

2.2 目標(biāo)數(shù)據(jù)關(guān)聯(lián)

由于場(chǎng)景中一般存在多個(gè)目標(biāo)，需要通過數(shù)據(jù)關(guān)聯(lián)將預(yù)測(cè)結(jié)果與檢測(cè)結(jié)果進(jìn)行匹配。本文選擇全局最近鄰進(jìn)行數(shù)據(jù)關(guān)聯(lián)，采用數(shù)據(jù)關(guān)聯(lián)矩陣描述目標(biāo)關(guān)聯(lián)問題，采用匈牙利算法尋找最優(yōu)匹配。數(shù)據(jù)關(guān)聯(lián)矩陣是一種通過二維矩陣來描述兩幀目標(biāo)關(guān)聯(lián)問題的方法，其存儲(chǔ)的是目標(biāo)之間的相似度[33]。使用當(dāng)前測(cè)量構(gòu)建維度為m的檢測(cè)列表，上一幀預(yù)測(cè)的目標(biāo)構(gòu)建維度為n為預(yù)測(cè)列表，則數(shù)據(jù)關(guān)聯(lián)矩陣

(13)

式中，C為數(shù)據(jù)關(guān)聯(lián)矩陣；元素cij表示第m個(gè)檢測(cè)目標(biāo)與第n個(gè)預(yù)測(cè)目標(biāo)的相似度，該相似度通過兩個(gè)目標(biāo)包圍盒的三維交并比(intersection of union，IoU)來計(jì)算。由于目標(biāo)在時(shí)間序列上的唯一性，對(duì)于第i個(gè)檢測(cè)目標(biāo)，最多只有一個(gè)預(yù)測(cè)目標(biāo)與之對(duì)應(yīng)，同樣對(duì)于第j個(gè)預(yù)測(cè)目標(biāo)，最多只有一個(gè)檢測(cè)目標(biāo)與之對(duì)應(yīng)，因此數(shù)據(jù)關(guān)聯(lián)問題也可以被看作線性分配問題。優(yōu)化目標(biāo)就是找出數(shù)據(jù)關(guān)聯(lián)矩陣中的一組對(duì)應(yīng)關(guān)系使相似度的和最大，其數(shù)學(xué)形式為

(14)

式中，wij為置換矩陣中的元素，wij=1表示第i個(gè)檢測(cè)目標(biāo)和第j個(gè)預(yù)測(cè)目標(biāo)是同一個(gè)目標(biāo)，wij=0表示第i個(gè)檢測(cè)目標(biāo)和第j個(gè)預(yù)測(cè)目標(biāo)不是同一個(gè)目標(biāo)。對(duì)于上述線性分配問題，采用匈牙利算法[34]進(jìn)行求解便可獲取相鄰幀間的數(shù)據(jù)關(guān)聯(lián)矩陣，得到檢測(cè)結(jié)果間的最大相似度對(duì)應(yīng)關(guān)系。依據(jù)這一關(guān)聯(lián)矩陣，能夠?qū)⒚恳惠v車在的預(yù)測(cè)結(jié)果與正確的檢測(cè)結(jié)果相匹配，為后續(xù)狀態(tài)更新過程提供數(shù)據(jù)一致性保障。

2.3 目標(biāo)狀態(tài)更新

基于數(shù)據(jù)關(guān)聯(lián)的結(jié)果，提取同一車輛的預(yù)測(cè)結(jié)果與檢測(cè)結(jié)果，并進(jìn)行目標(biāo)狀態(tài)的更新，此時(shí)的更新方程為

Kk+1=Pk+1|kHT(HPk+1|kHT+R)-1

(15)

Xk+1=Xk+1|k+Kk+1(zk+1-HXk+1|k)

(16)

Pk+1=(I-Kk+1H)Pk+1|k

(17)

式中，z為通過PVRCNN方法進(jìn)行目標(biāo)檢測(cè)得到的測(cè)量向量；H為測(cè)量矩陣；R為測(cè)量噪聲協(xié)方差矩陣，由傳感器的用戶手冊(cè)得到；K為卡爾曼增益；I為單位矩陣。式(15)、式(16)、式(17)構(gòu)成了本文中目標(biāo)跟蹤算法中的卡爾曼濾波更新流程。

在目標(biāo)距離較遠(yuǎn)時(shí)，由于檢測(cè)結(jié)果的不穩(wěn)定導(dǎo)致目標(biāo)狀態(tài)更新時(shí)存在較大誤差，而且這種誤差會(huì)始終存在，從而導(dǎo)致目標(biāo)跟蹤的軌跡產(chǎn)生抖動(dòng)。因此2.4節(jié)中在擴(kuò)展卡爾曼濾波的基礎(chǔ)上，利用道路約束作為偽測(cè)量對(duì)車輛的朝向進(jìn)行二次更新，以提升跟蹤效果。

2.4 目標(biāo)朝向狀態(tài)更新

本文假設(shè)道路上行駛的車輛基本都是沿著車道線行駛，車輛的真實(shí)朝向與車道線的朝向相同。如圖8所示，θo為車道線朝向，θd是目標(biāo)檢測(cè)的朝向，朝向誤差δθ=θo-θd。以目標(biāo)底邊的中心點(diǎn)為坐標(biāo)原點(diǎn)，車道線朝向?yàn)閤軸，車道線垂直方向?yàn)閥軸，建立目標(biāo)坐標(biāo)系，則虛擬目標(biāo)真實(shí)框(圖中虛線框)與目標(biāo)檢測(cè)框(圖中實(shí)線框)位置上的誤差為

(18)

式中，l為目標(biāo)包圍盒的長度；δx和δy分別為兩個(gè)方向上的位置誤差，同時(shí)假設(shè)目標(biāo)真實(shí)框與目標(biāo)檢測(cè)框的旋轉(zhuǎn)中心為目標(biāo)坐標(biāo)系的原點(diǎn)。

圖8 目標(biāo)位置誤差計(jì)算Fig.8 Object position error calculation

在計(jì)算得到目標(biāo)朝向誤差和位置誤差后，根據(jù)誤差的大小對(duì)測(cè)量噪聲協(xié)方差矩陣Ro進(jìn)行動(dòng)態(tài)調(diào)整

(19)

(20)

式中，R11等4個(gè)參數(shù)為測(cè)量噪聲協(xié)方差矩陣Ro左上角的兩維方陣。圖9是兩個(gè)不同朝向誤差目標(biāo)對(duì)應(yīng)的位置噪聲協(xié)方差示意圖，可以看出對(duì)于不同朝向誤差的目標(biāo)，本文所提出的方法可以對(duì)其位置噪聲進(jìn)行實(shí)時(shí)估計(jì)，更加符合真實(shí)情況。

圖9 不同朝向誤差目標(biāo)位置的噪聲協(xié)方差Fig.9 Noise covariance of object position with different orientation errors

由于本文方法假設(shè)目標(biāo)朝向與車道線的方向相同，因此將依據(jù)道路建模所得的道路方向作為測(cè)量結(jié)果對(duì)目標(biāo)的朝向θ進(jìn)行更新

(21)

Xo,k+1=Xk+1+Ko,k+1(zo,k+1-HoXk+1)

(22)

Po,k+1=(I-Ko,k+1Ho)Pk+1

(23)

式中，zo=θo是由車道線方向得到的偽測(cè)量；Ho=[0 0 1 0 0]為測(cè)量矩陣；Ro為車道線方向約束的測(cè)量噪聲方差，表征了車道線方向與目標(biāo)真實(shí)朝向的誤差分布。為了確定測(cè)量噪聲方差Ro的取值，本文對(duì)訓(xùn)練集中目標(biāo)真實(shí)朝向與車道線方向的誤差進(jìn)行了統(tǒng)計(jì)，并利用高斯分布對(duì)誤差統(tǒng)計(jì)結(jié)果進(jìn)行擬合，最終得到的高斯分布的方差就是測(cè)量噪聲方差Ro。

本文所述的目標(biāo)跟蹤算法的擴(kuò)展卡爾曼濾波流程如下。

(1) 通過式(10)和式(11)根據(jù)上一幀的狀態(tài)進(jìn)行預(yù)測(cè)。

(2) 通過式(15)、式(16)、式(17)根據(jù)目標(biāo)檢測(cè)結(jié)果對(duì)目標(biāo)的狀態(tài)進(jìn)行更新。

(3) 通過式(21)、式(22)、式(23)根據(jù)車道線約束的偽測(cè)量結(jié)果對(duì)目標(biāo)的朝向進(jìn)行更新。

(4) 迭代上述步驟(1)—步驟(3)進(jìn)行持續(xù)跟蹤。

3 試驗(yàn)結(jié)果與分析

3.1 試驗(yàn)條件與評(píng)價(jià)指標(biāo)

本文所選用的試驗(yàn)數(shù)據(jù)來源于KITTI數(shù)據(jù)集，是自動(dòng)駕駛領(lǐng)域的最為權(quán)威的數(shù)據(jù)集之一，其中的道路場(chǎng)景具有較好的代表性。由于本文所提出的方法是通過車道線提供的朝向約束提升遠(yuǎn)距離車輛跟蹤效果，而在一般路口處沒有車道線覆蓋且此時(shí)的檢測(cè)任務(wù)主要集中在近處，因此試驗(yàn)基于KITTI數(shù)據(jù)集中典型的城郊道路和高速道路展開。全部數(shù)據(jù)共約800幀：其中0018是一個(gè)彎道場(chǎng)景，0020(前500幀)是一個(gè)直道場(chǎng)景。

在試驗(yàn)分析上，本文主要采用兩個(gè)主要的評(píng)價(jià)指標(biāo)進(jìn)行分析，首先是KITTI數(shù)據(jù)集所采用的多目標(biāo)跟蹤準(zhǔn)確度(multiple object tracking accuracy,MOTA)指標(biāo)，主要用于車輛跟蹤算法準(zhǔn)確性的評(píng)估，其定義如下

(24)

式中，TP(true positive)表示檢測(cè)為正樣本并且真值也是正樣本的個(gè)數(shù)；FN(false negative)表示檢測(cè)為負(fù)樣本但是真值是正樣本的個(gè)數(shù)；FP(false positive)表示檢測(cè)為正樣本但是真值是負(fù)樣本的個(gè)數(shù)；IDS(ID switch)表示目標(biāo)ID切換的次數(shù)，對(duì)于一個(gè)良好的目標(biāo)跟蹤系統(tǒng)，需要對(duì)目標(biāo)進(jìn)行持續(xù)跟蹤，同一目標(biāo)在不同幀之間的ID應(yīng)該保持一致，因此IDS越多指標(biāo)越低。

另一個(gè)評(píng)價(jià)指標(biāo)采用平均位移誤差(average translation error，ATE)[35]，主要用于車輛跟蹤軌跡誤差的評(píng)估，其定義為

(25)

3.2 定量試驗(yàn)與分析

表1為兩個(gè)車段場(chǎng)景的MOTA對(duì)比，兩個(gè)場(chǎng)景的MOTA都得到了提升，特別是車輛比較多的0020場(chǎng)景，車輛之間相互遮擋嚴(yán)重，也會(huì)導(dǎo)致點(diǎn)云數(shù)較少，在基于車道朝向先驗(yàn)信息對(duì)目標(biāo)跟蹤進(jìn)行增強(qiáng)后，跟蹤的效果提升效果明顯。

表1 兩個(gè)路段場(chǎng)景的MOTA對(duì)比

ATE評(píng)價(jià)結(jié)果見表2，加入車道朝向先驗(yàn)后，兩個(gè)場(chǎng)景的ATE指標(biāo)均有所降低，在0018場(chǎng)景中ATE降低了0.013 8 m，而在0020場(chǎng)景中ATE降低0.028 5 m。

表2 兩個(gè)路段場(chǎng)景的ATE對(duì)比

不同距離下的ATE指標(biāo)見表3。對(duì)于0018場(chǎng)景，ATE指標(biāo)的降低基本隨著距離的增加而增大，當(dāng)目標(biāo)距離為50～60 m時(shí)ATE指標(biāo)降低最多為0.025 3 m，由于該場(chǎng)景60 m以外的車輛未標(biāo)注，因此用N/A表示。對(duì)于0020場(chǎng)景，其ATE指標(biāo)的降低幅度大于0018場(chǎng)景，并在60～70 m時(shí)達(dá)到最大為0.081 6 m，驗(yàn)證了本文方法對(duì)于遠(yuǎn)距離點(diǎn)云稀疏車輛的優(yōu)勢(shì)。

表3 不同距離下兩個(gè)場(chǎng)景的ATE對(duì)比

對(duì)目標(biāo)跟蹤方法的運(yùn)行時(shí)間進(jìn)行統(tǒng)計(jì)，結(jié)果見表4。雖然加入車道特征進(jìn)行增強(qiáng)后，方法的運(yùn)行時(shí)間有所增加，但是還是滿足實(shí)時(shí)性要求。同時(shí)，本節(jié)提取的車道結(jié)果還可以用于車道保持等其他模塊，具有較高的實(shí)用價(jià)值。

表4 兩個(gè)路段場(chǎng)景的運(yùn)行時(shí)間對(duì)比

由上述結(jié)果可知，本文提出的車輛跟蹤增強(qiáng)方法不僅可以提高多目標(biāo)跟蹤的準(zhǔn)確度，同時(shí)可以減少目標(biāo)跟蹤軌跡與真實(shí)軌跡的誤差，特別對(duì)于遠(yuǎn)距離點(diǎn)云稀疏的車輛，其誤差降低較為明顯。雖然在運(yùn)行時(shí)間方面，本文相比之前增加較多，但是可以滿足實(shí)時(shí)性要求。

3.3 定性試驗(yàn)與分析

為了對(duì)增強(qiáng)前后車輛跟蹤軌跡進(jìn)行直觀分析，對(duì)其進(jìn)行了可視化。圖10顯示了目標(biāo)直行場(chǎng)景下的對(duì)比結(jié)果。其中紅色曲線為道路朝向先驗(yàn)增強(qiáng)前的目標(biāo)跟蹤軌跡，綠色曲線為增強(qiáng)后的目標(biāo)跟蹤軌跡。以776號(hào)目標(biāo)為例，增強(qiáng)前目標(biāo)跟蹤軌跡抖動(dòng)較為嚴(yán)重，而增強(qiáng)后的目標(biāo)跟蹤軌跡較為平滑。同時(shí)774號(hào)目標(biāo)原本的跟蹤軌跡在中間位置存在突變，但是基于本文方法進(jìn)行增強(qiáng)后，目標(biāo)跟蹤軌跡更加平滑。

圖10 直行車輛在增強(qiáng)前(紅色)與增強(qiáng)后(綠色)的跟蹤軌跡對(duì)比Fig.10 Comparison of tracking trajectory before (red) and after (green) enhancement for straight driving object

在真實(shí)的道路環(huán)境中，目標(biāo)不會(huì)只沿著車道線直線行駛，而是存在換道、超車等情況。因此，本文對(duì)目標(biāo)換道情況下的目標(biāo)跟蹤軌跡進(jìn)行了對(duì)比，結(jié)果如圖11所示。其中，102號(hào)目標(biāo)從最右邊車道換道至中間車道，而在目標(biāo)跟蹤增強(qiáng)前后的車輛軌跡基本一致。因此，本文方法在利用車道朝向先驗(yàn)對(duì)目標(biāo)跟蹤進(jìn)行增強(qiáng)的同時(shí)，不會(huì)影響目標(biāo)換道等復(fù)雜運(yùn)動(dòng)。

圖11 變道車輛在增強(qiáng)前(紅色)與增強(qiáng)后(綠色)的跟蹤軌跡對(duì)比Fig.11 Comparison of tracking trajectory before (red) and after (green) enhancement for lane-changing object

4 總 結(jié)

本文針對(duì)基于激光雷達(dá)的車輛跟蹤方法在遠(yuǎn)距離處精度下降以及軌跡抖動(dòng)等問題展開研究，提出了一種基于數(shù)字地圖中車道朝向先驗(yàn)的車輛跟蹤增強(qiáng)方法。首先，對(duì)多幀疊加后的點(diǎn)云進(jìn)行無監(jiān)督強(qiáng)度標(biāo)定，通過大津法計(jì)算強(qiáng)度閾值并提取車道線特征點(diǎn)，并從車輛周圍的OSM地圖數(shù)據(jù)中提取車道信息，進(jìn)行道路建模獲取道路的朝向約束；然后，通過PVRCNN方法對(duì)點(diǎn)云中車輛進(jìn)行目標(biāo)檢測(cè)，獲取車輛的位姿數(shù)據(jù)；最后，建立擴(kuò)展卡爾曼濾波器進(jìn)行數(shù)據(jù)關(guān)聯(lián)和目標(biāo)跟蹤，利用目標(biāo)檢測(cè)結(jié)果進(jìn)行車輛狀態(tài)更新之后又進(jìn)行了基于道路朝向約束的車輛朝向狀態(tài)更新，從而提升目標(biāo)跟蹤的精度、增強(qiáng)預(yù)測(cè)結(jié)果的穩(wěn)定性。

基于KITTI數(shù)據(jù)集的試驗(yàn)結(jié)果表明，本文所提出的方法在多目標(biāo)跟蹤(MOTA)指標(biāo)上提升了至少0.33%，而平均位移誤差(ATE)指標(biāo)則優(yōu)化了0.014 m，同時(shí)，在最遠(yuǎn)距離(約60 m外)的跟蹤誤差降低了0.08 m以上。本文所提出的方法充分利用了智能駕駛車輛所依賴的數(shù)字地圖，將車道線檢測(cè)的結(jié)果與地圖中的道路先驗(yàn)信息進(jìn)行融合，將地圖與車輛感知功能進(jìn)行了有機(jī)結(jié)合。未來工作將會(huì)集中于利用道路邊界等其他高精地圖中的道路信息，進(jìn)一步提升跟蹤系統(tǒng)的精度和穩(wěn)定性，并將該方法推廣至更為復(fù)雜的道路環(huán)境當(dāng)中。