顧及數字地圖中車道走向的車輛跟蹤增強算法

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

1. 上海交通大學密西根學院，上海 200240； 2. 廣西科技大學廣西汽車零部件與整車技術重點實驗室，廣西 柳州 545006； 3. 上海交通大學自動化系，上海 200240； 4. 系統控制與信息處理教育部重點實驗室，上海 200240； 5. 上海工業智能管控工程技術研究中心，上海 200240

智能駕駛系統的環境感知功能對于車輛的運行安全起著至關重要的作用，目標檢測與跟蹤是環境感知的核心，目標檢測任務能夠對交通參與者進行識別，而目標跟蹤任務則是在時間尺度上分辨是否為同一目標并且進行持續的狀態估計?；谌S激光雷達的目標檢測與跟蹤算法是無人駕駛系統的一個重要組成部分[1]，對于路徑規劃和決策控制等高級任務具有重要意義。

現階段基于激光雷達的目標檢測方法較為成熟[2]：基于先驗規則的檢測手段，通過地面分割、聚類、生成包圍盒等流程完成目標檢測，但穩健性較差[3-6]；近年來基于神經網絡的激光雷達目標檢測算法趨于成熟，且具備更高的檢測精度與更好的適應性，如PointRCNN[7]、Part-A2Net[8]、PVRCNN[9]等。而基于檢測的目標跟蹤方法通過多幀的數據關聯與狀態估計生成目標軌跡[10]，數據關聯旨在確定多幀數據之間多個目標的對應關系，是目標跟蹤的基礎[11-13]；狀態估計指的是對同一目標多幀測量結果進行濾波，并對目標的狀態量進行精確估計[14-16]。

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

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

圖1 不同距離上激光雷達的點云數量Fig.1 Point number of LiDAR point cloud at different distances

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

本文基于上述思路，將數字地圖中的道路信息進行提取與建模，利用道路的朝向約束來增強車輛跟蹤。本文在KITTI數據集上展開[25]，在檢測的基線方法上選擇PVRCNN來進行，因為該方法是目前KITTI排行榜上效果最好的三維目標檢測算法之一，基于該方法進行研究不僅能提供較為穩定的檢測結果，而且也能體現道路的朝向約束對車輛跟蹤方法的提升。由于KITTI數據集本身不提供與之配套的高精度地圖，本文通過OpenStreetMap這一開源的地圖數據平臺進行車道線的先驗信息提取，所提出的方法僅使用道路的朝向信息作為約束為后續的目標跟蹤提供增強，而不需要考慮車道線在空間中的位置、車道線的個數、寬度等信息。這種方法具有良好的地圖數據類型兼容性，而且在使用更準確的高精地圖時會達到更好的效果。

本文的系統架構如圖2所示，首先通過車輛定位信息首先獲取前方道路的OSM地圖數據，然后對激光雷達點云數據中的車道線信息進行提取，并結合車道線與OSM地圖進行道路建模以獲取道路的朝向信息作為車輛的朝向約束；此外，PVRCNN方法用于對激光雷達點云數據進行車輛的檢測，檢測的車輛位姿結果與朝向約束共同用于車輛跟蹤。

圖2 本文的系統架構Fig.2 System architecture of the proposed method

1 基于數字地圖融合的道路建模

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

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

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

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

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

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

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

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

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

(1)

(2)

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

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

車輛的跟蹤方法基于卡爾曼濾波框架[15]，包含目標狀態的預測與更新兩部分，但由于空間中存在多個目標車輛，每次進行目標狀態更新時需要將預測結果與當前幀的檢測結果進行對應，因此在預測與更新過程中引入數據關聯模塊進行目標的對應。在完成基于檢測結果的目標狀態更新后，會將目標的朝向狀態依據車道約束再次進行更新，完成當前幀的車輛跟蹤。整個跟蹤任務的流程如圖7所示，分為4個主要部分，其中數字編號對應的是本節中的小節編號。

圖3 車道線點云結果Fig.3 Lane marker detection result from point cloud

圖4 KITTI數據集中某一場景對應的OSM數據Fig.4 The OSM data of a scene in KITTI dataset

圖5 OSM道路投影到車道線特征點的結果Fig.5 The projection of OSM lane data on the lane marker features

圖6 與地圖融合后的道路建模結果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 目標狀態預測

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

(3)

對于目標車輛的運動，狀態向量的微分表達式為

(4)

假設離散的時間步k和連續時間值tk相關，離散的時間步k+1與連續時間值tk+1相關，tk和tk+1之間的時間差為Δt，則狀態轉移方程為

(5)

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

(6)

(7)

(8)

由于CTRV模型的狀態轉移方程是非線性的，因此需要利用泰勒展開對非線性模型進行近似，計算雅克比矩陣J，這里將角標k移除掉以減少公式長度

(9)

最終得到擴展卡爾曼濾波的預測方程則是

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

(10)

Pk+1|k=JPkJT+Q

(11)

式中，J為上述狀態轉移方程的雅可比矩陣；P為狀態協方差矩陣，其初始值一般由人為指定；Q為過程噪聲協方差矩陣。Q的計算方式為

(12)

2.2 目標數據關聯

由于場景中一般存在多個目標，需要通過數據關聯將預測結果與檢測結果進行匹配。本文選擇全局最近鄰進行數據關聯，采用數據關聯矩陣描述目標關聯問題，采用匈牙利算法尋找最優匹配。數據關聯矩陣是一種通過二維矩陣來描述兩幀目標關聯問題的方法，其存儲的是目標之間的相似度[33]。使用當前測量構建維度為m的檢測列表，上一幀預測的目標構建維度為n為預測列表，則數據關聯矩陣

(13)

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

(14)

式中，wij為置換矩陣中的元素，wij=1表示第i個檢測目標和第j個預測目標是同一個目標，wij=0表示第i個檢測目標和第j個預測目標不是同一個目標。對于上述線性分配問題，采用匈牙利算法[34]進行求解便可獲取相鄰幀間的數據關聯矩陣，得到檢測結果間的最大相似度對應關系。依據這一關聯矩陣，能夠將每一輛車在的預測結果與正確的檢測結果相匹配，為后續狀態更新過程提供數據一致性保障。

2.3 目標狀態更新

基于數據關聯的結果，提取同一車輛的預測結果與檢測結果，并進行目標狀態的更新，此時的更新方程為

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方法進行目標檢測得到的測量向量；H為測量矩陣；R為測量噪聲協方差矩陣，由傳感器的用戶手冊得到；K為卡爾曼增益；I為單位矩陣。式(15)、式(16)、式(17)構成了本文中目標跟蹤算法中的卡爾曼濾波更新流程。

在目標距離較遠時，由于檢測結果的不穩定導致目標狀態更新時存在較大誤差，而且這種誤差會始終存在，從而導致目標跟蹤的軌跡產生抖動。因此2.4節中在擴展卡爾曼濾波的基礎上，利用道路約束作為偽測量對車輛的朝向進行二次更新，以提升跟蹤效果。

2.4 目標朝向狀態更新

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

(18)

式中，l為目標包圍盒的長度；δx和δy分別為兩個方向上的位置誤差，同時假設目標真實框與目標檢測框的旋轉中心為目標坐標系的原點。

圖8 目標位置誤差計算Fig.8 Object position error calculation

在計算得到目標朝向誤差和位置誤差后，根據誤差的大小對測量噪聲協方差矩陣Ro進行動態調整

(19)

(20)

式中，R11等4個參數為測量噪聲協方差矩陣Ro左上角的兩維方陣。圖9是兩個不同朝向誤差目標對應的位置噪聲協方差示意圖，可以看出對于不同朝向誤差的目標，本文所提出的方法可以對其位置噪聲進行實時估計，更加符合真實情況。

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

由于本文方法假設目標朝向與車道線的方向相同，因此將依據道路建模所得的道路方向作為測量結果對目標的朝向θ進行更新

(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是由車道線方向得到的偽測量；Ho=[0 0 1 0 0]為測量矩陣；Ro為車道線方向約束的測量噪聲方差，表征了車道線方向與目標真實朝向的誤差分布。為了確定測量噪聲方差Ro的取值，本文對訓練集中目標真實朝向與車道線方向的誤差進行了統計，并利用高斯分布對誤差統計結果進行擬合，最終得到的高斯分布的方差就是測量噪聲方差Ro。

本文所述的目標跟蹤算法的擴展卡爾曼濾波流程如下。

(1) 通過式(10)和式(11)根據上一幀的狀態進行預測。

(2) 通過式(15)、式(16)、式(17)根據目標檢測結果對目標的狀態進行更新。

(3) 通過式(21)、式(22)、式(23)根據車道線約束的偽測量結果對目標的朝向進行更新。

(4) 迭代上述步驟(1)—步驟(3)進行持續跟蹤。

3 試驗結果與分析

3.1 試驗條件與評價指標

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

在試驗分析上，本文主要采用兩個主要的評價指標進行分析，首先是KITTI數據集所采用的多目標跟蹤準確度(multiple object tracking accuracy,MOTA)指標，主要用于車輛跟蹤算法準確性的評估，其定義如下

(24)

式中，TP(true positive)表示檢測為正樣本并且真值也是正樣本的個數；FN(false negative)表示檢測為負樣本但是真值是正樣本的個數；FP(false positive)表示檢測為正樣本但是真值是負樣本的個數；IDS(ID switch)表示目標ID切換的次數，對于一個良好的目標跟蹤系統，需要對目標進行持續跟蹤，同一目標在不同幀之間的ID應該保持一致，因此IDS越多指標越低。

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

(25)

3.2 定量試驗與分析

表1為兩個車段場景的MOTA對比，兩個場景的MOTA都得到了提升，特別是車輛比較多的0020場景，車輛之間相互遮擋嚴重，也會導致點云數較少，在基于車道朝向先驗信息對目標跟蹤進行增強后，跟蹤的效果提升效果明顯。

表1 兩個路段場景的MOTA對比

ATE評價結果見表2，加入車道朝向先驗后，兩個場景的ATE指標均有所降低，在0018場景中ATE降低了0.013 8 m，而在0020場景中ATE降低0.028 5 m。

表2 兩個路段場景的ATE對比

不同距離下的ATE指標見表3。對于0018場景，ATE指標的降低基本隨著距離的增加而增大，當目標距離為50～60 m時ATE指標降低最多為0.025 3 m，由于該場景60 m以外的車輛未標注，因此用N/A表示。對于0020場景，其ATE指標的降低幅度大于0018場景，并在60～70 m時達到最大為0.081 6 m，驗證了本文方法對于遠距離點云稀疏車輛的優勢。

表3 不同距離下兩個場景的ATE對比

對目標跟蹤方法的運行時間進行統計，結果見表4。雖然加入車道特征進行增強后，方法的運行時間有所增加，但是還是滿足實時性要求。同時，本節提取的車道結果還可以用于車道保持等其他模塊，具有較高的實用價值。

表4 兩個路段場景的運行時間對比

由上述結果可知，本文提出的車輛跟蹤增強方法不僅可以提高多目標跟蹤的準確度，同時可以減少目標跟蹤軌跡與真實軌跡的誤差，特別對于遠距離點云稀疏的車輛，其誤差降低較為明顯。雖然在運行時間方面，本文相比之前增加較多，但是可以滿足實時性要求。

3.3 定性試驗與分析

為了對增強前后車輛跟蹤軌跡進行直觀分析，對其進行了可視化。圖10顯示了目標直行場景下的對比結果。其中紅色曲線為道路朝向先驗增強前的目標跟蹤軌跡，綠色曲線為增強后的目標跟蹤軌跡。以776號目標為例，增強前目標跟蹤軌跡抖動較為嚴重，而增強后的目標跟蹤軌跡較為平滑。同時774號目標原本的跟蹤軌跡在中間位置存在突變，但是基于本文方法進行增強后，目標跟蹤軌跡更加平滑。

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

在真實的道路環境中，目標不會只沿著車道線直線行駛，而是存在換道、超車等情況。因此，本文對目標換道情況下的目標跟蹤軌跡進行了對比，結果如圖11所示。其中，102號目標從最右邊車道換道至中間車道，而在目標跟蹤增強前后的車輛軌跡基本一致。因此，本文方法在利用車道朝向先驗對目標跟蹤進行增強的同時，不會影響目標換道等復雜運動。

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

4 總 結

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

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