王 肖，李克強(qiáng)，王建強(qiáng)，徐友春

(1.清華大學(xué)，汽車安全與節(jié)能國(guó)家重點(diǎn)實(shí)驗(yàn)室，北京 100084; 2.軍事交通學(xué)院軍用車輛系，天津 300161)

2016179

基于三維激光雷達(dá)的智能車輛目標(biāo)參數(shù)辨識(shí)*

王 肖1,2，李克強(qiáng)1，王建強(qiáng)1，徐友春2

(1.清華大學(xué)，汽車安全與節(jié)能國(guó)家重點(diǎn)實(shí)驗(yàn)室，北京 100084; 2.軍事交通學(xué)院軍用車輛系，天津 300161)

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

智能車輛；三維激光雷達(dá)；參數(shù)辨識(shí)；動(dòng)態(tài)參考點(diǎn)；幾何形狀變化速率

前言

三維激光雷達(dá)能捕獲目標(biāo)物基本形狀特征和豐富局部細(xì)節(jié)，且具有可靠性和測(cè)量精度高等優(yōu)點(diǎn)，被廣泛應(yīng)用在智能車輛環(huán)境感知中。作為感知的重要內(nèi)容，車輛目標(biāo)的位姿、運(yùn)動(dòng)速度和幾何形狀是智能車輛路徑規(guī)劃和決策算法的基礎(chǔ)。由于三維激光雷達(dá)固有特性，目標(biāo)點(diǎn)云分布受到觀測(cè)點(diǎn)位置和自遮擋等因素的影響，使參數(shù)難以精確計(jì)算。

國(guó)內(nèi)外學(xué)者對(duì)車輛目標(biāo)參數(shù)計(jì)算做了許多研究。文獻(xiàn)[1]中綜合利用盒子模型和點(diǎn)模型對(duì)動(dòng)態(tài)障礙目標(biāo)進(jìn)行形狀及運(yùn)動(dòng)學(xué)屬性描述，其中盒子模型主要用于車輛目標(biāo)，其參數(shù)包括目標(biāo)位置、行駛方向和速度等，而點(diǎn)模型用于描述行人等非車輛目標(biāo)。文獻(xiàn)[2]中建立了描述車輛目標(biāo)長(zhǎng)、寬和中心點(diǎn)位置的幾何模型，考慮到觀測(cè)位置改變對(duì)目標(biāo)點(diǎn)云帶來(lái)的影響，提出一種中心點(diǎn)更新算法來(lái)進(jìn)行校正。文獻(xiàn)[3]在以上基礎(chǔ)上建立了包含目標(biāo)中心點(diǎn)位置、形狀、行駛方向和速度的盒子模型，同樣采用中心點(diǎn)更新算法來(lái)彌補(bǔ)觀測(cè)位置變化和遮擋可能帶來(lái)的影響。

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

車輛目標(biāo)速度是目標(biāo)跟蹤研究的重要內(nèi)容。跟蹤算法主要研究點(diǎn)為運(yùn)動(dòng)模型、數(shù)據(jù)關(guān)聯(lián)和濾波算法。常速度模型是最簡(jiǎn)單、應(yīng)用最廣泛的運(yùn)動(dòng)模型[2-3,6]，其線性特性使之可直接應(yīng)用卡爾曼濾波器進(jìn)行數(shù)據(jù)預(yù)測(cè)和更新[3]。而粒子濾波由于其適用于非線性運(yùn)動(dòng)模型非單高斯?fàn)顟B(tài)假設(shè)中，也得到一定應(yīng)用[2,6]。數(shù)據(jù)關(guān)聯(lián)算法主要包括最近鄰(NN)[7]、全局最近鄰(GNN)[8]、聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)(JPDA)[9]和多假設(shè)跟蹤算法(MHT)[10]等，其中最近鄰算法由于其簡(jiǎn)單高效，適合于應(yīng)用在目標(biāo)稀疏場(chǎng)景中，而聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)和多假設(shè)跟蹤算法雖能提高復(fù)雜場(chǎng)景下關(guān)聯(lián)準(zhǔn)確率，但其效率和資源耗費(fèi)等問(wèn)題突出。

以上研究結(jié)果表明：(1)車輛目標(biāo)位置通常利用包圍體中心點(diǎn)來(lái)表示，但由于觀測(cè)點(diǎn)位置和自遮擋因素影響，使中心點(diǎn)位置難以精確計(jì)算，這導(dǎo)致后續(xù)目標(biāo)速度計(jì)算誤差較大；(2)車輛目標(biāo)航向角研究較少，由于點(diǎn)云分布的復(fù)雜性，常規(guī)擬合方法效果較差；(3)車輛幾何形狀通常采用固定的先驗(yàn)參數(shù)。

針對(duì)以上問(wèn)題，本文中提出一種動(dòng)態(tài)參考點(diǎn)模型來(lái)計(jì)算目標(biāo)速度，相對(duì)于中心點(diǎn)，該模型參考點(diǎn)具有可直接觀測(cè)、動(dòng)態(tài)切換等優(yōu)點(diǎn)；在目標(biāo)航向角計(jì)算中，采用分塊聚類、主成分?jǐn)M合的方法來(lái)提高參數(shù)精度；提出一種幾何形狀變化速率方法，并結(jié)合卡爾曼濾波器來(lái)估計(jì)車輛實(shí)時(shí)幾何參數(shù)。最后基于最近鄰關(guān)聯(lián)算法和在線卡爾曼濾波器對(duì)目標(biāo)參數(shù)進(jìn)行跟蹤優(yōu)化。通過(guò)速度、航向角和幾何形狀3組實(shí)驗(yàn)對(duì)以上方法進(jìn)行驗(yàn)證和分析。

1 參數(shù)表示

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

圖1 定向包圍體參數(shù)示意圖

2 參數(shù)計(jì)算

2.1 位姿計(jì)算

已知t時(shí)刻目標(biāo)位姿表示為Xt=(xrt,yrt,φt)，其中參考點(diǎn)xrt,yrt用于計(jì)算目標(biāo)速度。通常參考點(diǎn)選取包圍體中心，但由于受觀測(cè)點(diǎn)位置和遮擋等因素影響，中心點(diǎn)坐標(biāo)往往無(wú)法精確計(jì)算，這使后續(xù)以中心點(diǎn)位置差分方法計(jì)算出的目標(biāo)速度嚴(yán)重失真。為此提出一種不受觀測(cè)位置和目標(biāo)自遮擋影響的動(dòng)態(tài)參考點(diǎn)模型。

目標(biāo)航向角φt同樣受觀測(cè)位置和自遮擋的影響，為此提出一種先分塊聚類、后主成分?jǐn)M合的方法來(lái)提高航向角精度。

2.1.1 動(dòng)態(tài)參考點(diǎn)模型

點(diǎn)云分布特性導(dǎo)致難以使用包圍體中心作為參考點(diǎn)，但無(wú)論目標(biāo)處于什么位置，總可以找到一個(gè)可直接觀測(cè)的包圍體角點(diǎn)并將此作為參考點(diǎn)(暫不考慮目標(biāo)間相互遮擋的影響)，同時(shí)隨著位置的改變?cè)搮⒖键c(diǎn)可以動(dòng)態(tài)更新，這便是動(dòng)態(tài)參考點(diǎn)模型的基本原理[11]。

參考點(diǎn)選擇和更新的基本思想見(jiàn)圖2。將雷達(dá)坐標(biāo)系識(shí)別區(qū)域分為A1,A12,A2,A23,A3,A34,A4和A148個(gè)矩形子區(qū)域。可知在Ai=1,2,3,4區(qū)域包圍體由于自遮擋導(dǎo)致部分尾部和側(cè)部數(shù)據(jù)丟失，稱此區(qū)域?yàn)樽哉趽鯀^(qū)。而在區(qū)域A12和A34則能夠獲取較準(zhǔn)確的車輛長(zhǎng)度參數(shù)，區(qū)域A14和A23則能夠獲取較準(zhǔn)確寬度參數(shù)，稱此區(qū)域?yàn)榘胱哉趽鯀^(qū)。動(dòng)態(tài)參考點(diǎn)選擇遵循以下準(zhǔn)則。

圖2 動(dòng)態(tài)參考點(diǎn)選擇示意圖

(1) 初始化動(dòng)態(tài)參考點(diǎn) 當(dāng)t時(shí)刻目標(biāo)車輛位于Ai=1,2,3,4區(qū)時(shí)，選擇定向包圍體與自車最近鄰角點(diǎn)作為參考點(diǎn)(xrt,yrt)，如圖中位于A1區(qū)中的角點(diǎn)p1。

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

(3) 參考點(diǎn)切換 當(dāng)包圍體進(jìn)入新的自遮擋區(qū)時(shí)，則更新動(dòng)態(tài)參考點(diǎn)。例如當(dāng)包圍體從A12至A2時(shí)，參考點(diǎn)從p1更換為p2，同理當(dāng)從A14至A4時(shí)，參考點(diǎn)從p1更換為p4。

2.1.2 基于分塊聚類、主成分?jǐn)M合的航向角計(jì)算

在復(fù)雜行駛環(huán)境中，目標(biāo)經(jīng)常出現(xiàn)切入、轉(zhuǎn)向等行為，這要求智能車必須能夠快速、準(zhǔn)確地識(shí)別其行駛航向角。

目標(biāo)航向角計(jì)算有兩種思路：一是利用參考點(diǎn)變化方向來(lái)計(jì)算；二是根據(jù)點(diǎn)云分布特征來(lái)分析。第一種方法缺點(diǎn)是無(wú)法分析靜態(tài)和低速目標(biāo)，且難以適用于車輛轉(zhuǎn)向場(chǎng)景中。

本研究采用第二種方法進(jìn)行研究，其主要思路是轉(zhuǎn)化為二維平面上點(diǎn)云主成分方向直線擬合問(wèn)題。采用隨機(jī)抽樣一致性算法[5]來(lái)擬合點(diǎn)云的直線模型，但該方法受點(diǎn)云分布影響較大?？紤]到車輛點(diǎn)云平面形狀主要呈“L”和“I”型，如圖3所示，如果能夠?qū)⒈碚骱较蚪堑闹饕糠痔崛〕鰜?lái)作為感興趣區(qū)域(ROI)，并基于該區(qū)域進(jìn)行擬合，這無(wú)疑比利用整體點(diǎn)云擬合精度更高。

圖3 “L”和“I”型點(diǎn)云感興趣區(qū)域

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

圖4 “L”和“I”型點(diǎn)云航向角計(jì)算過(guò)程

在點(diǎn)云類型計(jì)算中(CalculateType函數(shù))，考慮到點(diǎn)云分布主要受目標(biāo)位姿影響，則可用如下概率來(lái)描述：

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

(1)

式中：Mt為目標(biāo)分布類型(“L”或“I”)；Xt為t時(shí)刻目標(biāo)位姿?；谀繕?biāo)位置與航向角獨(dú)立性假設(shè)，可得

表1 基于GMM和RANSAC

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

(2)

由于此時(shí)刻目標(biāo)方位φt未知，因此采用點(diǎn)云分布方差之比τt來(lái)近似替代，視點(diǎn)云為二維高斯分布，則有

(3)

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

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

(4)

對(duì)于p(Mt|xrt,yrt)，由于難以用概率密度函數(shù)來(lái)精確表示，因此采用如下離散經(jīng)驗(yàn)值：

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

對(duì)于p(M|τt)同樣采用如下經(jīng)驗(yàn)值：

最終計(jì)算兩種目標(biāo)類型的概率：

(5)

(6)

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

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

2.2 速度計(jì)算

目標(biāo)速度可直接利用前后時(shí)刻的參考點(diǎn)位置進(jìn)行差分：

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

(7)

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

(8)

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

2.3 幾何形狀參數(shù)

車輛幾何形狀參數(shù)難以用精確的模型進(jìn)行描述，需要依靠濾波算法進(jìn)行優(yōu)化，在3.3節(jié)將對(duì)幾何參數(shù)濾波進(jìn)行研究，本節(jié)首先引入幾何形狀變化速率的概念。

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

vl=(lt-lt-1)/T

(9)

vw=(wt-wt-1)/T

(10)

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

3 參數(shù)濾波

3.1 數(shù)據(jù)關(guān)聯(lián)

數(shù)據(jù)關(guān)聯(lián)是將當(dāng)前時(shí)刻某一觀測(cè)目標(biāo)同上一時(shí)刻建立的目標(biāo)航跡集合中相似度最大的目標(biāo)進(jìn)行匹配的過(guò)程，通常采用目標(biāo)位置作為相似度判斷準(zhǔn)則。本研究采用簡(jiǎn)單、高效的最近鄰方法作為實(shí)現(xiàn)算法。

最近鄰方法的基本思想是將與被跟蹤目標(biāo)預(yù)測(cè)位置最近的有效回波作為候選回波：

(11)

目標(biāo)下一時(shí)刻位置需要利用運(yùn)動(dòng)模型進(jìn)行預(yù)測(cè)，這將在下節(jié)進(jìn)行研究。

3.2 目標(biāo)姿態(tài)與速度濾波

車輛運(yùn)動(dòng)模型包括常速度、常加速度和協(xié)調(diào)式轉(zhuǎn)向模型等。為簡(jiǎn)化計(jì)算，本研究將車輛運(yùn)動(dòng)視為質(zhì)點(diǎn)平移，不考慮轉(zhuǎn)向運(yùn)動(dòng)的影響，采用常速度模型作為基本研究模型，則有

(12)

對(duì)于目標(biāo)方位角，采用常橫擺角速度模型，則有

(13)

在線濾波基本步驟為：(1)狀態(tài)預(yù)測(cè)，基于上一時(shí)刻的觀測(cè)和式(12)，對(duì)當(dāng)前時(shí)刻目標(biāo)位置進(jìn)行預(yù)估，可通過(guò)調(diào)節(jié)噪聲方差來(lái)改變預(yù)估值；(2)目標(biāo)前后幀關(guān)聯(lián)，基于最近鄰關(guān)聯(lián)方法，查找同目標(biāo)預(yù)估位置最近的當(dāng)前觀測(cè)，可通過(guò)設(shè)置適當(dāng)?shù)年P(guān)聯(lián)門限來(lái)提高效率和關(guān)聯(lián)準(zhǔn)確率；(3)狀態(tài)更新，通過(guò)調(diào)節(jié)增益值來(lái)獲取最優(yōu)濾波結(jié)果。

3.3 目標(biāo)幾何形狀參數(shù)的濾波

基于2.3節(jié)提出的幾何形狀變化速率模型，可建立狀態(tài)方程：

(14)

同樣可利用線性卡爾曼濾波器對(duì)各參數(shù)進(jìn)行濾波。需要注意的是，vl和vw大于0說(shuō)明目標(biāo)觀測(cè)長(zhǎng)寬遞增變化，這意味著觀測(cè)值越來(lái)越接近真實(shí)值；而vl和vw小于0則意味著由于自遮擋影響而導(dǎo)致失真。因此目標(biāo)幾何形狀的卡爾曼濾波適用于幾何形狀變化速率大于0的情況下，當(dāng)變化速率小于0時(shí)，應(yīng)采取形狀參數(shù)的保優(yōu)操作以確保逼近真實(shí)值，如圖6所示。

圖6 幾何參數(shù)濾波邏輯

4 實(shí)驗(yàn)分析

圖7為實(shí)驗(yàn)平臺(tái)，其中傳感器包括1臺(tái)Velodyne HDL-64E三維激光雷達(dá)、4個(gè)外置和1個(gè)內(nèi)置相機(jī)。相機(jī)用于記錄車外和駕駛員圖像，雷達(dá)用于采集環(huán)境數(shù)據(jù)，其基本參數(shù)為：水平視場(chǎng)360°，垂直視場(chǎng)26.8°，角度分辨率0.09°，測(cè)量距離約120m。

圖7 實(shí)驗(yàn)平臺(tái)

實(shí)驗(yàn)主要通過(guò)目標(biāo)的速度、航向角和幾何形狀計(jì)算來(lái)驗(yàn)證本文所提方法，其中涉及的算法參數(shù)和預(yù)設(shè)值如表2所示。

4.1 速度實(shí)驗(yàn)分析

由于對(duì)未知速度的車輛進(jìn)行分析無(wú)法得到定量的實(shí)驗(yàn)結(jié)果，因此本實(shí)驗(yàn)設(shè)置包含定速運(yùn)動(dòng)目標(biāo)和靜止目標(biāo)兩組實(shí)驗(yàn)場(chǎng)景，如表3所示。

圖8為場(chǎng)景1實(shí)驗(yàn)結(jié)果。圖中上方為目標(biāo)車輛 分別位于智能車左后側(cè)、左側(cè)和左前側(cè)的相機(jī)和雷達(dá)圖，雷達(dá)圖中心實(shí)心矩形表示智能車；圖中下方為實(shí)驗(yàn)曲線。由圖可見(jiàn)，車輛目標(biāo)估計(jì)速度總體平穩(wěn)，數(shù)值在67～74km/h范圍內(nèi)浮動(dòng)，誤差控制在7%以內(nèi)。值得注意的是，當(dāng)目標(biāo)從左后側(cè)行駛至左前側(cè)時(shí)，參考點(diǎn)發(fā)生了改變，但如圖中虛線橢圓中標(biāo)注，目標(biāo)速度仍非常平穩(wěn)，未發(fā)生大的突變。

表2 算法參數(shù)和預(yù)設(shè)值

表3 速度實(shí)驗(yàn)場(chǎng)景設(shè)置

圖8 場(chǎng)景1實(shí)驗(yàn)結(jié)果

圖9為場(chǎng)景2實(shí)驗(yàn)結(jié)果。由圖可見(jiàn)：車輛目標(biāo)估計(jì)速度總體平穩(wěn)，數(shù)值在-2～3km/h范圍內(nèi)浮動(dòng)，基本可認(rèn)為車輛處在靜止?fàn)顟B(tài)；同時(shí)由虛線橢圓標(biāo)注處可知，在參考點(diǎn)切換時(shí)刻車輛速度估計(jì)仍能夠保持平穩(wěn)。

圖9 場(chǎng)景2實(shí)驗(yàn)結(jié)果

由以上兩組實(shí)驗(yàn)可知，結(jié)合動(dòng)態(tài)參考點(diǎn)模型和濾波算法的速度估計(jì)在準(zhǔn)確度和平穩(wěn)性方面均能取得較滿意效果，能夠較好地反映目標(biāo)真實(shí)運(yùn)動(dòng)速度；另外參考點(diǎn)切換時(shí)刻速度沒(méi)有跳變現(xiàn)象，這說(shuō)明式(7)和式(8)中沖擊函數(shù)項(xiàng)起到關(guān)鍵作用。

4.2 航向角分析

航向角實(shí)驗(yàn)場(chǎng)景設(shè)置見(jiàn)表4。

表4 航向角實(shí)驗(yàn)場(chǎng)景

圖10 目標(biāo)車輛換道切入場(chǎng)景航向角實(shí)驗(yàn)結(jié)果

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

4.3 幾何形狀分析

幾何形狀實(shí)驗(yàn)場(chǎng)景采用表3中1號(hào)場(chǎng)景，即目標(biāo)車輛從智能車左后側(cè)逐漸行駛至左前側(cè)。

圖11為實(shí)驗(yàn)結(jié)果曲線。由圖可見(jiàn)：當(dāng)目標(biāo)處于側(cè)后方時(shí)，由于受自遮擋影響，其長(zhǎng)寬尚不能反映目標(biāo)真實(shí)情況，隨著目標(biāo)逐漸靠近智能車，遮擋影響變小，其長(zhǎng)寬也逐漸增加；當(dāng)?shù)竭_(dá)第25幀左右，寬度值基本達(dá)到最理性觀測(cè)值，到達(dá)50幀左右，長(zhǎng)度也達(dá)到最理想觀測(cè)值；當(dāng)車輛繼續(xù)向前時(shí)，由于自遮擋現(xiàn)象再次出現(xiàn)，長(zhǎng)寬觀測(cè)值將變小，但由于算法的保優(yōu)機(jī)制，避免了車輛形狀由大變小的邏輯錯(cuò)誤，長(zhǎng)寬值將保持不變。值得注意的是，車輛寬度開(kāi)始保持不變點(diǎn)(25幀)早于長(zhǎng)度保持不變點(diǎn)(50幀)出現(xiàn)，這是由于當(dāng)目標(biāo)位于自遮擋區(qū)(圖2中A1區(qū))時(shí)便能夠獲取較精確的寬度，而繼續(xù)前進(jìn)至半遮擋區(qū)(圖2中A12區(qū))才能獲取較精確的長(zhǎng)度，這也從另一方面驗(yàn)證了計(jì)算結(jié)果的合理性。

圖11 目標(biāo)車輛換道切入的幾何形狀實(shí)驗(yàn)結(jié)果

5 結(jié)論

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

本研究尚未考慮目標(biāo)間相互遮擋場(chǎng)景，此時(shí)目標(biāo)點(diǎn)云分布將更加復(fù)雜，從而導(dǎo)致參考角點(diǎn)將難以獲取，幾何形狀變化無(wú)規(guī)律等問(wèn)題。下一步將重點(diǎn)對(duì)此問(wèn)題進(jìn)行研究、完善。

[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ǐn),等.基于三維激光雷達(dá)的動(dòng)態(tài)障礙實(shí)時(shí)檢測(cè)與跟蹤[J].浙江大學(xué)學(xué)報(bào)(工學(xué)版),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á)在自主車環(huán)境感知中的應(yīng)用研究[D].長(zhǎng)沙:國(guó)防科技大學(xué),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] 王肖,王建強(qiáng),李克強(qiáng),等.智能車輛3-D點(diǎn)云快速分割方法[J].清華大學(xué)學(xué)報(bào)(自然科學(xué)版),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

*國(guó)家自然科學(xué)基金(51175290)資助。

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