基于三維激光雷達的智能車輛目標參數辨識*

王 肖，李克強，王建強，徐友春

(1.清華大學，汽車安全與節能國家重點實驗室，北京 100084; 2.軍事交通學院軍用車輛系，天津 300161)

2016179

基于三維激光雷達的智能車輛目標參數辨識*

王 肖1,2，李克強1，王建強1，徐友春2

(1.清華大學，汽車安全與節能國家重點實驗室，北京 100084; 2.軍事交通學院軍用車輛系，天津 300161)

智能車周圍車輛目標參數(位姿、運動速度和幾何形狀)對智能車路徑規劃和決策算法而言至關重要。選取三維激光雷達作為傳感器，對車輛目標位置、航向角、速度和幾何形狀進行計算和濾波。針對車輛目標點云受觀測點位置和自遮擋等因素的影響，提出一種動態參考點模型用于計算目標速度；對于目標航向角，采用先點云分塊聚類后主成分擬合的思路來提高航向角精度；提出一種基于幾何形狀變化速率的濾波算法來解決幾何形狀不易測量和復雜多變等問題。分別就車輛目標速度、航向角和幾何形狀進行實驗分析，結果證明基于本文方法的參數計算結果能獲得較高精度，滿足智能車環境感知要求。

智能車輛；三維激光雷達；參數辨識；動態參考點；幾何形狀變化速率

前言

三維激光雷達能捕獲目標物基本形狀特征和豐富局部細節，且具有可靠性和測量精度高等優點，被廣泛應用在智能車輛環境感知中。作為感知的重要內容，車輛目標的位姿、運動速度和幾何形狀是智能車輛路徑規劃和決策算法的基礎。由于三維激光雷達固有特性，目標點云分布受到觀測點位置和自遮擋等因素的影響，使參數難以精確計算。

國內外學者對車輛目標參數計算做了許多研究。文獻[1]中綜合利用盒子模型和點模型對動態障礙目標進行形狀及運動學屬性描述，其中盒子模型主要用于車輛目標，其參數包括目標位置、行駛方向和速度等，而點模型用于描述行人等非車輛目標。文獻[2]中建立了描述車輛目標長、寬和中心點位置的幾何模型，考慮到觀測位置改變對目標點云帶來的影響，提出一種中心點更新算法來進行校正。文獻[3]在以上基礎上建立了包含目標中心點位置、形狀、行駛方向和速度的盒子模型，同樣采用中心點更新算法來彌補觀測位置變化和遮擋可能帶來的影響。

以上研究主要集中于目標中心位置計算上，而未涉及目標姿態的計算。文獻[4]在柵格單元和連通區域標記基礎上，提出一種二維包圍體(Bounding-box)來容納所有各個連通區內柵格單元，而后采用主成分分析計算包圍體內點云的特性向量，選用對應特征值較大的特征向量方向作為包圍體的行駛方向。文獻[5]在此基礎上，改進采用定向包圍體來對目標進行描述。首先采用統計分析方法去除異值點云，而后找出剩余點云主要成分分布方向。考慮目標自遮擋特性，主成分分析算法往往不能獲得目標真實運動方向，因此采用隨機抽樣一致性算法來擬合點云分布的直線模型。該方法關鍵步驟在于如何去除異值點云，但文中沒有給出具體步驟。

車輛目標速度是目標跟蹤研究的重要內容。跟蹤算法主要研究點為運動模型、數據關聯和濾波算法。常速度模型是最簡單、應用最廣泛的運動模型[2-3,6]，其線性特性使之可直接應用卡爾曼濾波器進行數據預測和更新[3]。而粒子濾波由于其適用于非線性運動模型非單高斯狀態假設中，也得到一定應用[2,6]。數據關聯算法主要包括最近鄰(NN)[7]、全局最近鄰(GNN)[8]、聯合概率數據關聯(JPDA)[9]和多假設跟蹤算法(MHT)[10]等，其中最近鄰算法由于其簡單高效，適合于應用在目標稀疏場景中，而聯合概率數據關聯和多假設跟蹤算法雖能提高復雜場景下關聯準確率，但其效率和資源耗費等問題突出。

以上研究結果表明：(1)車輛目標位置通常利用包圍體中心點來表示，但由于觀測點位置和自遮擋因素影響，使中心點位置難以精確計算，這導致后續目標速度計算誤差較大；(2)車輛目標航向角研究較少，由于點云分布的復雜性，常規擬合方法效果較差；(3)車輛幾何形狀通常采用固定的先驗參數。

針對以上問題，本文中提出一種動態參考點模型來計算目標速度，相對于中心點，該模型參考點具有可直接觀測、動態切換等優點；在目標航向角計算中，采用分塊聚類、主成分擬合的方法來提高參數精度；提出一種幾何形狀變化速率方法，并結合卡爾曼濾波器來估計車輛實時幾何參數。最后基于最近鄰關聯算法和在線卡爾曼濾波器對目標參數進行跟蹤優化。通過速度、航向角和幾何形狀3組實驗對以上方法進行驗證和分析。

1 參數表示

車輛目標采用矩形定向包圍體(oriented bounding box, OBB)表示，如圖1所示。在雷達坐標系Oxy下，t時刻目標參數可表示為Bt=(Xt,vt,Gt)，其中Xt=(xrt,yrt,φt)為位姿模型，xrt,yrt表示目標參考點坐標，航向角φt為行駛方向同x軸夾角；vt=(vxt,vyt)表示目標沿兩個坐標軸行駛速度；Gt=(lt,wt,xct,yct)為目標幾何形狀模型，其中lt和wt為t時刻測量的包圍體長和寬，xct和yct為包圍體中心點坐標。

圖1 定向包圍體參數示意圖

2 參數計算

2.1 位姿計算

已知t時刻目標位姿表示為Xt=(xrt,yrt,φt)，其中參考點xrt,yrt用于計算目標速度。通常參考點選取包圍體中心，但由于受觀測點位置和遮擋等因素影響，中心點坐標往往無法精確計算，這使后續以中心點位置差分方法計算出的目標速度嚴重失真。為此提出一種不受觀測位置和目標自遮擋影響的動態參考點模型。

目標航向角φt同樣受觀測位置和自遮擋的影響，為此提出一種先分塊聚類、后主成分擬合的方法來提高航向角精度。

2.1.1 動態參考點模型

點云分布特性導致難以使用包圍體中心作為參考點，但無論目標處于什么位置，總可以找到一個可直接觀測的包圍體角點并將此作為參考點(暫不考慮目標間相互遮擋的影響)，同時隨著位置的改變該參考點可以動態更新，這便是動態參考點模型的基本原理[11]。

參考點選擇和更新的基本思想見圖2。將雷達坐標系識別區域分為A1,A12,A2,A23,A3,A34,A4和A148個矩形子區域。可知在Ai=1,2,3,4區域包圍體由于自遮擋導致部分尾部和側部數據丟失，稱此區域為自遮擋區。而在區域A12和A34則能夠獲取較準確的車輛長度參數，區域A14和A23則能夠獲取較準確寬度參數，稱此區域為半自遮擋區。動態參考點選擇遵循以下準則。

圖2 動態參考點選擇示意圖

(1) 初始化動態參考點 當t時刻目標車輛位于Ai=1,2,3,4區時，選擇定向包圍體與自車最近鄰角點作為參考點(xrt,yrt)，如圖中位于A1區中的角點p1。

(2) 參考點保持 當定向包圍體當前時刻仍處于自遮擋區時，則參考點(xrt,yrt)仍為上一時刻包圍體角點；當包圍體上一時刻和當前時刻皆位于自遮擋區時，則參考點(xrt,yrt)仍為上一時刻包圍體角點，如圖中A1～A12參考點保持不變。

(3) 參考點切換 當包圍體進入新的自遮擋區時，則更新動態參考點。例如當包圍體從A12至A2時，參考點從p1更換為p2，同理當從A14至A4時，參考點從p1更換為p4。

2.1.2 基于分塊聚類、主成分擬合的航向角計算

在復雜行駛環境中，目標經常出現切入、轉向等行為，這要求智能車必須能夠快速、準確地識別其行駛航向角。

目標航向角計算有兩種思路：一是利用參考點變化方向來計算；二是根據點云分布特征來分析。第一種方法缺點是無法分析靜態和低速目標，且難以適用于車輛轉向場景中。

本研究采用第二種方法進行研究，其主要思路是轉化為二維平面上點云主成分方向直線擬合問題。采用隨機抽樣一致性算法[5]來擬合點云的直線模型，但該方法受點云分布影響較大。考慮到車輛點云平面形狀主要呈“L”和“I”型，如圖3所示，如果能夠將表征航向角的主要部分提取出來作為感興趣區域(ROI)，并基于該區域進行擬合，這無疑比利用整體點云擬合精度更高。

圖3 “L”和“I”型點云感興趣區域

基于此思想提出先利用混合高斯(GMM)算法將點云分成兩部分并從中選取感興趣區域，而后采用隨機抽樣一致性(RANSAC)算法對感興趣區域點云進行擬合，最后再計算航向角。圖4為計算過程示意圖。具體算法如表1所示。

圖4 “L”和“I”型點云航向角計算過程

在點云類型計算中(CalculateType函數)，考慮到點云分布主要受目標位姿影響，則可用如下概率來描述：

p(Mt|Xt)=p(Mt|xrt,yrt,φt)

(1)

式中：Mt為目標分布類型(“L”或“I”)；Xt為t時刻目標位姿。基于目標位置與航向角獨立性假設，可得

表1 基于GMM和RANSAC

p(Mt|Xt)=p(Mt|xrt,yrt)p(Mt|φt)

(2)

由于此時刻目標方位φt未知，因此采用點云分布方差之比τt來近似替代，視點云為二維高斯分布，則有

(3)

式中σmax和σmin分別為最大和最小正態方差。則可得

p(Mt|Xt)=p(Mt|xrt,yrt)p(Mt|τt)

(4)

對于p(Mt|xrt,yrt)，由于難以用概率密度函數來精確表示，因此采用如下離散經驗值：

式中：R1為圖2中的并集A14∪A23；R2為剩余區域。

對于p(M|τt)同樣采用如下經驗值：

最終計算兩種目標類型的概率：

(5)

(6)

如果p(M=“L”)>p(M=“I”)則選擇“L”模型，反之選擇“I”模型。

在第二步聚類算法中，首先采用Kmeans算法進行初始聚類，而后利用GMM算法提升聚類精度。在第三步中則采用RANSAC算法對聚類出的兩部分進行直線擬合。最終結合點云類型在擬合直線基礎上計算出目標航向角：如對于“L”模型點云，則直接選擇較扁平部分(可以兩部分高斯模型的方差比作為選擇依據)的擬合直線作為航向角方向；對于“I”模型，可選擇近鄰部分擬合直線的正交直線作為航向角方向。

2.2 速度計算

目標速度可直接利用前后時刻的參考點位置進行差分：

vxt=(xrt-xrt-1+δ(t-tc)(±l))/T

(7)

vyt=(yrt-yrt-1+δ(t-tc)(±w))/T

(8)

式中T為時刻間隔。為防止參考點切換造成速度突變，設置單位沖擊函數項δ(t-tc)，其中tc為參考點更換時刻。

2.3 幾何形狀參數

車輛幾何形狀參數難以用精確的模型進行描述，需要依靠濾波算法進行優化，在3.3節將對幾何參數濾波進行研究，本節首先引入幾何形狀變化速率的概念。

幾何形狀變化速率表示單位時間內幾何參數的變化程度，如圖5所示。假定車輛真實幾何尺寸由t-1時刻的實線和虛線兩部分組成，受觀測位置影響，在t-1時刻觀測長寬分別為lt-1和wt-1，t時刻觀測長寬分別為lt和wt，則車輛長、寬變化速率分別為

vl=(lt-lt-1)/T

(9)

vw=(wt-wt-1)/T

(10)

圖5 幾何形狀變化速率示意圖

3 參數濾波

3.1 數據關聯

數據關聯是將當前時刻某一觀測目標同上一時刻建立的目標航跡集合中相似度最大的目標進行匹配的過程，通常采用目標位置作為相似度判斷準則。本研究采用簡單、高效的最近鄰方法作為實現算法。

最近鄰方法的基本思想是將與被跟蹤目標預測位置最近的有效回波作為候選回波：

(11)

目標下一時刻位置需要利用運動模型進行預測，這將在下節進行研究。

3.2 目標姿態與速度濾波

車輛運動模型包括常速度、常加速度和協調式轉向模型等。為簡化計算，本研究將車輛運動視為質點平移，不考慮轉向運動的影響，采用常速度模型作為基本研究模型，則有

(12)

對于目標方位角，采用常橫擺角速度模型，則有

(13)

在線濾波基本步驟為：(1)狀態預測，基于上一時刻的觀測和式(12)，對當前時刻目標位置進行預估，可通過調節噪聲方差來改變預估值；(2)目標前后幀關聯，基于最近鄰關聯方法，查找同目標預估位置最近的當前觀測，可通過設置適當的關聯門限來提高效率和關聯準確率；(3)狀態更新，通過調節增益值來獲取最優濾波結果。

3.3 目標幾何形狀參數的濾波

基于2.3節提出的幾何形狀變化速率模型，可建立狀態方程：

(14)

同樣可利用線性卡爾曼濾波器對各參數進行濾波。需要注意的是，vl和vw大于0說明目標觀測長寬遞增變化，這意味著觀測值越來越接近真實值；而vl和vw小于0則意味著由于自遮擋影響而導致失真。因此目標幾何形狀的卡爾曼濾波適用于幾何形狀變化速率大于0的情況下，當變化速率小于0時，應采取形狀參數的保優操作以確保逼近真實值，如圖6所示。

圖6 幾何參數濾波邏輯

4 實驗分析

圖7為實驗平臺，其中傳感器包括1臺Velodyne HDL-64E三維激光雷達、4個外置和1個內置相機。相機用于記錄車外和駕駛員圖像，雷達用于采集環境數據，其基本參數為：水平視場360°，垂直視場26.8°，角度分辨率0.09°，測量距離約120m。

圖7 實驗平臺

實驗主要通過目標的速度、航向角和幾何形狀計算來驗證本文所提方法，其中涉及的算法參數和預設值如表2所示。

4.1 速度實驗分析

由于對未知速度的車輛進行分析無法得到定量的實驗結果，因此本實驗設置包含定速運動目標和靜止目標兩組實驗場景，如表3所示。

圖8為場景1實驗結果。圖中上方為目標車輛 分別位于智能車左后側、左側和左前側的相機和雷達圖，雷達圖中心實心矩形表示智能車；圖中下方為實驗曲線。由圖可見，車輛目標估計速度總體平穩，數值在67～74km/h范圍內浮動，誤差控制在7%以內。值得注意的是，當目標從左后側行駛至左前側時，參考點發生了改變，但如圖中虛線橢圓中標注，目標速度仍非常平穩，未發生大的突變。

表2 算法參數和預設值

表3 速度實驗場景設置

圖8 場景1實驗結果

圖9為場景2實驗結果。由圖可見：車輛目標估計速度總體平穩，數值在-2～3km/h范圍內浮動，基本可認為車輛處在靜止狀態；同時由虛線橢圓標注處可知，在參考點切換時刻車輛速度估計仍能夠保持平穩。

圖9 場景2實驗結果

由以上兩組實驗可知，結合動態參考點模型和濾波算法的速度估計在準確度和平穩性方面均能取得較滿意效果，能夠較好地反映目標真實運動速度；另外參考點切換時刻速度沒有跳變現象，這說明式(7)和式(8)中沖擊函數項起到關鍵作用。

4.2 航向角分析

航向角實驗場景設置見表4。

表4 航向角實驗場景

圖10 目標車輛換道切入場景航向角實驗結果

圖10為實驗結果。由圖可見：在目標車輛切入過程中，航向角絕對值從0開始逐漸增大(實驗中右轉航向角為負，左轉為正)；當目標逐漸進入智能車所在車道時，航向角開始變小；當目標車輛完全進入目標車道并直行時，其航向角又回歸為0，整個過程的航向角變化同實際情況基本相符。在目標處于切入的各個階段中，其點云形狀雖然發生較大變化，但基于本研究提出的分塊聚類、主成分擬合的方法依然能夠得出較理想的結果。同時整個切入過程的航向角比較平滑，證明濾波算法能夠有效地去除識別噪聲干擾。

4.3 幾何形狀分析

幾何形狀實驗場景采用表3中1號場景，即目標車輛從智能車左后側逐漸行駛至左前側。

圖11為實驗結果曲線。由圖可見：當目標處于側后方時，由于受自遮擋影響，其長寬尚不能反映目標真實情況，隨著目標逐漸靠近智能車，遮擋影響變小，其長寬也逐漸增加；當到達第25幀左右，寬度值基本達到最理性觀測值，到達50幀左右，長度也達到最理想觀測值；當車輛繼續向前時，由于自遮擋現象再次出現，長寬觀測值將變小，但由于算法的保優機制，避免了車輛形狀由大變小的邏輯錯誤，長寬值將保持不變。值得注意的是，車輛寬度開始保持不變點(25幀)早于長度保持不變點(50幀)出現，這是由于當目標位于自遮擋區(圖2中A1區)時便能夠獲取較精確的寬度，而繼續前進至半遮擋區(圖2中A12區)才能獲取較精確的長度，這也從另一方面驗證了計算結果的合理性。

圖11 目標車輛換道切入的幾何形狀實驗結果

5 結論

本文中主要對基于三維激光雷達的車輛目標參數進行研究。在參數計算中，提出動態參考點模型來計算目標速度，以消除中心參考點方法受點云分布復雜性影響；對于目標航向角，提出一種先分塊聚類，后主成分擬合的方法來提高精度；采用線性卡爾曼濾波器對目標位置、速度和航向角進行優化。針 對目標幾何形狀難以精確計算的問題，提出幾何形狀變化速率模型并應用至濾波中。分別就車輛目標速度、航向角和幾何形狀進行實驗分析，結果表明：運動目標速度估計誤差控制在7%范圍內，靜態目標速度估計接近于0；目標換道切入整個過程的航向角變化同實際情況基本相符，同時整個過程的航向角比較平滑；目標幾何形狀參數能夠逐步逼近于真實值，保優機制能夠保證獲得穩定形狀參數。

本研究尚未考慮目標間相互遮擋場景，此時目標點云分布將更加復雜，從而導致參考角點將難以獲取，幾何形狀變化無規律等問題。下一步將重點對此問題進行研究、完善。

[1] FERGUSON D, DARMS M, URMSON C, et al. Detection, prediction, and avoidance of dynamic obstacles in urban environments[C]. Intelligent Vehicles Symposium,2008 IEEE. IEEE,2008:1149-1154.

[2] PETROVSKAYA A, THRUN S. Model based vehicle detection and tracking for autonomous urban driving[J]. Autonomous Robots,2009,26(2-3):123-139.

[3] 楊飛,朱株,龔小謹,等.基于三維激光雷達的動態障礙實時檢測與跟蹤[J].浙江大學學報(工學版),2012,46(9).

[4] HIMMELSBACH M. LIDAR-based 3D object perception[C]. Proceedings of 1st International Workshop on Cognition for Technical Systems,2008: Munich.

[5] HIMMELSBACH M, WUENSCHE H. Tracking and classification of arbitrary objects with bottom-up/top-down detection[C]. Intelligent Vehicles Symposium (IV),2012 IEEE.

[6] 諶彤童.三維激光雷達在自主車環境感知中的應用研究[D].長沙:國防科技大學,2011.

[7] LI X R, BAR-SHALOM Y. Tracking in clutter with nearest neighbor filters: analysis and performance[J]. Aerospace and Electronic Systems, IEEE Transactions on,1996,32(3):995-1010.

[8] BLACKRNAN S, HOUSE A. Design and analysis of modern tracking systems[M]. Boston, MA: Artech House,1999.

[9] BAR-SHALOM Y, DAUM F, et al. The probabilistic data association filter[J]. Control Systems, IEEE,2009,29(6):82-100.

[10] BLACKMAN S S. Multiple hypothesis tracking for multiple target tracking[J]. Aerospace and Electronic Systems Magazine, IEEE,2004,19(1):5-18.

[11] 王肖,王建強,李克強,等.智能車輛3-D點云快速分割方法[J].清華大學學報(自然科學版),2014,54(11).

Parameter Identification of Intelligent Vehicle Target Based on 3D Laser Radar

Wang Xiao1,2, Li Keqiang1, Wang Jianqiang1& Xu Youchun2

1.TsinghuaUniversity,StateKeyLaboratoryofAutomotiveSafetyandEnergy,Beijing100084;2.AutomobileEngineeringDepartment，MilitaryTransportationUniversity,Tianjin300161

The parameters of vehicle targets surrounding intelligent vehicle, including the position and pose, movement velocity and geometric shape are important for the path planning and decision making algorithms of intelligent vehicle. With 3D laser radar chosen as sensor, the position, azimuth angle, velocity and geometric shape of vehicle targets are calculated and filtered. In view of that the point cloud of vehicle targets is affected by the position of observation point and self-occlusion, a dynamic reference point model is proposed for calculating target velocity. A scheme of block clustering first then principal component fitting is adopted for increasing the accuracy of azimuth angle, and a filtering algorithm based on the changing rate of geometric shape is proposed to tackle the problem that geometric shape is complex, changeable and difficult to measure. Finally experimental analyse are conducted on the speed, azimuth angle and geometric shape of vehicle targets repsectively with a result verifying that with the parameters calculated by the method proposed, the higher accuracy can be achieved and the requirements of environmental perception for intelligent vehicle can be met.

intelligent vehicle; 3D laser radar; parameter identification; dynamic reference point; changing rate of geometric shape

*國家自然科學基金(51175290)資助。

原稿收到日期為2015年7月20日，修改稿收到日期為2015年8月28日。