一種基于三維激光雷達(dá)的目標(biāo)車輛航向角計(jì)算方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及現(xiàn)代交通智能車輛環(huán)境感知技術(shù)領(lǐng)域，特別是關(guān)于一種基于三維激光 雷達(dá)的目標(biāo)車輛航向角計(jì)算方法。
【背景技術(shù)】
[0002] 三維激光雷達(dá)能夠捕獲目標(biāo)物基本形狀特征及豐富局部細(xì)節(jié)，具有可靠性及測量 精度高等優(yōu)點(diǎn)，目前被廣泛應(yīng)用在智能車輛環(huán)境感知中。作為感知的重要內(nèi)容，車輛目標(biāo) 航向角在判斷目標(biāo)換道意圖等行為中具有重要作用，是智能車輛路徑規(guī)劃及決策算法的基 礎(chǔ)。但是由于三維激光雷達(dá)的固有特性，車輛目標(biāo)點(diǎn)云分布受到觀測點(diǎn)位置及自遮擋等因 素的影響，使得目標(biāo)航向角難以精確計(jì)算。
[0003] 當(dāng)前研究中車輛目標(biāo)主要使用軸平行包圍體（AixeAlignBoundingBox，AABB) 來表示，即無論目標(biāo)處于何種姿態(tài)，其矩形包圍體邊界一直平行于雷達(dá)坐標(biāo)軸。這種方法忽 略了車輛的航向角屬性，顯然不利于分析車輛的橫向行為，對感知而言也是不完整的。針對 這種情況，部分研究采用主成分分析方法計(jì)算航向角：首先將原始三維點(diǎn)云俯視投影降維， 而后采用主成分分析來計(jì)算二維點(diǎn)云特性向量，選用對應(yīng)特征值較大的特征向量方向作為 包圍體航向角方向。在此基礎(chǔ)上，一些研究先采用統(tǒng)計(jì)分析方法去除異值點(diǎn)云，考慮目標(biāo)自 遮擋特性，主成分分析方法往往不能獲取到目標(biāo)真實(shí)運(yùn)動方向，因此采用隨機(jī)抽樣一致性 算法來計(jì)算車輛航向角，但是實(shí)際應(yīng)用中難以去除異值點(diǎn)云，因此限制了該方法的應(yīng)用。

【發(fā)明內(nèi)容】

[0004] 針對上述問題，本發(fā)明的目的是提供一種檢測精度較高、可靠性較強(qiáng)的基于三維 激光雷達(dá)的目標(biāo)車輛航向角計(jì)算方法。
[0005] 為實(shí)現(xiàn)上述目的，本發(fā)明采取以下技術(shù)方案：一種基于三維激光雷達(dá)的目標(biāo)車輛 航向角計(jì)算方法，其特征在于包括以下步驟：1)采用安裝在自車頂部的三維激光雷達(dá)采集 目標(biāo)車輛，得到目標(biāo)車輛的點(diǎn)云數(shù)據(jù)；2)在雷達(dá)坐標(biāo)系中建立目標(biāo)車輛某時(shí)刻所對應(yīng)的航 向角參數(shù)模型；3)根據(jù)目標(biāo)車輛的點(diǎn)云數(shù)據(jù)計(jì)算目標(biāo)車輛的分布類型；4)根據(jù)目標(biāo)車輛的 分布類型分別對目標(biāo)車輛點(diǎn)云進(jìn)行聚類，獲得感興趣區(qū)域，其中，感興趣區(qū)域指的是表征航 向角的主要部分；5)根據(jù)聚類結(jié)果按照目標(biāo)車輛的分布類型分別將兩類點(diǎn)云成分進(jìn)行直 線擬合；6)結(jié)合目標(biāo)車輛的分布類型及相應(yīng)的兩類點(diǎn)云成分直線擬合結(jié)果計(jì)算目標(biāo)車輛 的航向角。
[0006] 進(jìn)一步，對獲取的目標(biāo)車輛的航向角采用線性卡爾曼濾波器進(jìn)行濾波。
[0007] 進(jìn)一步，步驟3)根據(jù)目標(biāo)車輛的點(diǎn)云數(shù)據(jù)計(jì)算目標(biāo)車輛的分布類型，具體過程 為：目標(biāo)車輛的形狀類型采用如下概率模型進(jìn)行描述：
[0008]
[0009] 式中，Mt為目標(biāo)車輛的分布類型，X,為t時(shí)刻目標(biāo)車輛的位姿；基于目標(biāo)車輛的目 標(biāo)位置與航向角獨(dú)立性假設(shè)，可得：
[0010]
[0011] 由于此時(shí)刻目標(biāo)方位叭未知，因此采用點(diǎn)云分布方差之比Tt來近似替代，其計(jì) 算方法為：視點(diǎn)云為二維高斯分布，則有：
[0013] 式中，〇 _，〇 _分別為最大、最小正態(tài)方差，則可得：
[0014] p(MtIXt)=p(MtIxt,yt)p(Mt |xt)
[0015] 對于p(MtIxt，yt)，采用如下離散概率經(jīng)驗(yàn)值：
_7] 式中，A1 =li2,3,4區(qū)域包圍體為自遮擋區(qū)，A12,A34A14,A23區(qū)域?yàn)榘胱哉趽鯀^(qū)；Rl=A14UA23,R2 為剩余區(qū)域A1,A12,A2,A3,A34,厶4的并集；
[0018] 對于p(M|Tt)采用如下經(jīng)驗(yàn)值：
[0020] 最終采用下式計(jì)算兩種目標(biāo)類型的概率：
[0023] 如果p(M= 〃L〃)>p(M= 〃1〃）則認(rèn)為目標(biāo)車輛的分布類型為〃L"，反之則認(rèn)為目標(biāo) 車輛的分布類型為〃I"。
[0024] 進(jìn)一步，步驟4)根據(jù)目標(biāo)車輛的分布類型分別對目標(biāo)車輛點(diǎn)云進(jìn)行聚類，獲得感 興趣區(qū)域，具體過程為：4. 1)基于K均值算法對某一分布類型的目標(biāo)車輛點(diǎn)云進(jìn)行初始聚 類；4.2)采用混合高斯算法對初始聚類后的目標(biāo)車輛點(diǎn)云進(jìn)行再聚類，提取得到相應(yīng)分布 類型的感興趣區(qū)域；4. 3)經(jīng)過再聚類后，從聚類結(jié)果中選擇一類較優(yōu)的點(diǎn)云進(jìn)行航向角計(jì) 算，具體選擇過程為：對于"L"型點(diǎn)云，將兩類點(diǎn)云視為二維正態(tài)分布，分別計(jì)算其方差比 值，并選擇方差比值較大的一類作為感興趣區(qū)域點(diǎn)云；對于"I"型點(diǎn)云，選擇靠近自車的部 分作為感興趣點(diǎn)云，如果目標(biāo)車輛位于自車前方，則選擇其尾部點(diǎn)云；如果位于后方則選擇 車頭部分點(diǎn)云。
[0025] 進(jìn)一步，步驟6)結(jié)合目標(biāo)車輛的分布類型及相應(yīng)的兩類點(diǎn)云成分直線擬合結(jié)果 計(jì)算目標(biāo)車輛的航向角，具體過程為：6. 1)對于"L"型目標(biāo)車輛，直接以感興趣區(qū)域內(nèi)點(diǎn)云 擬合的直線作為航向角方向；6.2)對于"I"型目標(biāo)車輛，以感興趣區(qū)域內(nèi)點(diǎn)云擬合直線的 正交直線作為航向角方向。
[0026] 本發(fā)明由于采取以上技術(shù)方案，其具有以下優(yōu)點(diǎn)：1、本發(fā)明根據(jù)目標(biāo)車輛的分布 類型分別對目標(biāo)車輛點(diǎn)云進(jìn)行聚類，獲得感興趣區(qū)域，根據(jù)聚類結(jié)果按照目標(biāo)車輛的分布 類型分別將兩類點(diǎn)云成分進(jìn)行直線擬合；并結(jié)合目標(biāo)車輛的分布類型及相應(yīng)的兩類點(diǎn)云 成分直線擬合結(jié)果計(jì)算目標(biāo)車輛的航向角，由于本發(fā)明針對目標(biāo)點(diǎn)云采用先分塊聚類，后 進(jìn)行主成分?jǐn)M合，因此受目標(biāo)自遮擋影響較小，能夠有效提高航向角計(jì)算精度，檢測精度較 高。2、本發(fā)明對獲取的目標(biāo)車輛的航向角采用線性卡爾曼濾波器進(jìn)行濾波，因此能夠避免 受點(diǎn)云分布復(fù)雜性影響，進(jìn)而得到穩(wěn)定、可靠的航向角。本發(fā)明可以廣泛應(yīng)用于智能車環(huán)境 感知技術(shù)領(lǐng)域。
【附圖說明】
[0027] 圖1是本發(fā)明的目標(biāo)車輛航向角計(jì)算方法流程示意圖；
[0028] 圖2是本發(fā)明的目標(biāo)航向角參數(shù)示意圖；
[0029] 圖3是本發(fā)明的目標(biāo)形狀類型示意圖，其中，圖（a)是ROI為"L"型示意圖，圖（b) 是ROI為"I"型示意圖；
[0030] 圖4是本發(fā)明的目標(biāo)車輛位置分布示意圖，OBB指的是目標(biāo)車輛；
[0031] 圖5是本發(fā)明的點(diǎn)云聚類流程示意圖；
[0032] 圖6是本發(fā)明的航向角計(jì)算流程示例示意圖，其中，圖（a)是"L"模型點(diǎn)云航向角 計(jì)算流程示意圖，圖（b)是"I"模型點(diǎn)云航向角計(jì)算流程示意圖。
【具體實(shí)施方式】
[0033] 以下結(jié)合附圖來對本發(fā)明進(jìn)行詳細(xì)的描繪。然而應(yīng)當(dāng)理解，附圖的提供僅為了更 好地理解本發(fā)明，它們不應(yīng)該理解成對本發(fā)明的限制。
[0034] 如圖1所示，本發(fā)明的基于三維激光雷達(dá)的目標(biāo)車輛航向角計(jì)算方法，包括以下 步驟：
[0035] 1、采用安裝在自車頂部的三維激光雷達(dá)采集目標(biāo)車輛，得到目標(biāo)車輛的點(diǎn)云數(shù) 據(jù)。
[0036] 2、在雷達(dá)坐標(biāo)系中建立目標(biāo)車輛某時(shí)刻所對應(yīng)的航向角參數(shù)模型。
[0037] 如圖2所示，本發(fā)明中目標(biāo)車輛采用矩形定向包圍體表示，在雷達(dá)坐標(biāo)系Oxy下， t時(shí)刻目標(biāo)車輛的參數(shù)可以表示為：
[0038] Bt= (XtlGt)
[0039] 式中，K= 外）為目標(biāo)車輛位姿模型，xt，7,為目標(biāo)車輛中心參考點(diǎn)坐標(biāo)， 單位為m;供為目標(biāo)車輛航向角，即目標(biāo)車輛行駛方向同X軸夾角，單位為度；Gt=(It，wt) 為目標(biāo)車輛幾何形狀模型，其中，lt，WtSt時(shí)刻測量的矩形定向包圍體長寬，單位為m。
[0040] 3、根據(jù)目標(biāo)車輛的點(diǎn)云數(shù)據(jù)計(jì)算目標(biāo)車輛的分布類型。
[0041] 如圖3所示，目標(biāo)車輛點(diǎn)云數(shù)據(jù)的平面形狀主要呈"L"及"I"型。由于點(diǎn)云分布主 要受目標(biāo)車輛的位置、姿態(tài)影響，因此目標(biāo)車輛的形狀類型采用如下概率模型進(jìn)行描述：
[0042]
[0043] 式中，MtS目標(biāo)車輛的分布類型（"L"或"I"型），XtSt時(shí)刻目標(biāo)車輛的位姿。
[0044]基于目標(biāo)車輛的目標(biāo)位置與航向角獨(dú)立性假設(shè)，可得：
[0045]
[0046] 由于此時(shí)刻目標(biāo)方位趴未知，因此采用點(diǎn)云分布方差之比Tt來近似替代，其計(jì) 算方法為：視點(diǎn)云為二維高斯分布，則有：
[0048] 式中，〇 _，〇 _分別為最大、最小正態(tài)方差，則可得：
[0049] p(MtIXt) =p(MtIxt,yt)p(Mt |xt)
[0050] 對于p(Mt|xt，yt)，由于難以用概率密度函數(shù)來精確表示，本發(fā)明采用如下離散概 率經(jīng)驗(yàn)值：
[0052] 如圖4所示，根據(jù)實(shí)際監(jiān)測區(qū)域和矩形格子經(jīng)驗(yàn)參數(shù)大小將雷達(dá)坐標(biāo)系識別區(qū)域 分為A1, A12, A2, A23, A3, A34, A4, A14八個(gè)矩形子區(qū)域，具體劃分過程可以根據(jù)需要進(jìn)行，在此不 進(jìn)行贅述。在A1 = 區(qū)域包圍體由于自遮擋導(dǎo)致部分尾部和側(cè)部數(shù)據(jù)丟失，稱此區(qū)域?yàn)?自遮擋區(qū)；區(qū)域A12, A34則能夠獲取較準(zhǔn)確的車輛長度參數(shù)，區(qū)域A 14, A23則能夠獲取較準(zhǔn)確 寬度參數(shù)，稱此區(qū)域?yàn)榘胱哉趽鯀^(qū)，Rl為A14, A23的并集，即Rl =A14U A23, R2為剩余區(qū)域 (A1, A12, A2, A3, A34, A4)并集。
[0053] 對于p(M|Tt)，本發(fā)明同樣采用如下經(jīng)驗(yàn)值：
[0055] 最終可以采用下式計(jì)算兩種目標(biāo)類型的