崔雅博， 王曉婷

(開封大學(xué) 信息工程學(xué)院， 河南 開封 475004)

車輛運(yùn)動(dòng)狀態(tài)的準(zhǔn)確跟蹤是汽車輔助駕駛系統(tǒng)研究領(lǐng)域的關(guān)鍵技術(shù)之一[1-2].目前，利用GPS、車載慣導(dǎo)、雷達(dá)、紅外和機(jī)器視覺等路側(cè)傳感設(shè)備來感知道路車輛的運(yùn)動(dòng)狀態(tài)信息已形成了較為成熟的技術(shù)與方法[3].劉進(jìn)等[4]借助非線性三自由度單軌模型，并結(jié)合擴(kuò)展卡爾曼濾波，對車輛的橫向運(yùn)動(dòng)狀態(tài)進(jìn)行了估計(jì)；盤朝奉等[5]采用統(tǒng)計(jì)模型作為機(jī)動(dòng)目標(biāo)模型，引入卡爾曼濾波算法對車輛的運(yùn)動(dòng)狀態(tài)進(jìn)行估計(jì)，取得了良好的效果.然而，在實(shí)際道路交通環(huán)境下，由于傳感器局部感知信息不完整、目標(biāo)遮擋以及傳感器受環(huán)境噪聲影響等問題，采用基于單一傳感器信息源對汽車運(yùn)動(dòng)狀態(tài)進(jìn)行估計(jì)存在較大誤差，而且車輛在行駛的過程中具有勻速、加速和轉(zhuǎn)彎等不同的運(yùn)動(dòng)狀態(tài)，采用單一的運(yùn)動(dòng)模型必然會(huì)降低跟蹤的精度.因此，本文提出了基于多傳感器的信息融合方法，對車輛運(yùn)動(dòng)狀態(tài)進(jìn)行跟蹤，大大提高了汽車運(yùn)動(dòng)狀態(tài)的跟蹤性能和精度，為汽車輔助駕駛提供了強(qiáng)有力的技術(shù)支持.

1 車輛運(yùn)動(dòng)狀態(tài)描述

1.1 車輛運(yùn)動(dòng)狀態(tài)測量

由于激光雷達(dá)在測距與方位感知上具有較好的效果，本文選用激光雷達(dá)作為路側(cè)傳感器.但是單一傳感器的測量會(huì)受到環(huán)境和噪聲的影響，使其感知能力受到限制，故采用在路側(cè)分布式部署多個(gè)傳感器形成網(wǎng)絡(luò)的方法進(jìn)行數(shù)據(jù)融合處理，使不同位置的傳感器測量方位和角度不同，從而降低環(huán)境噪聲對車輛狀態(tài)估計(jì)的影響，達(dá)到優(yōu)勢互補(bǔ)的效果[6].路側(cè)傳感器需要輸出距離和方位角兩個(gè)基本測量的變量，其是實(shí)現(xiàn)車輛目標(biāo)狀態(tài)檢測和跟蹤的基本要素，路側(cè)傳感器測量模型如圖1所示.

圖1 路側(cè)傳感器測量模型Fig.1 Measurement model for roadside sensor

(1)

1.2 車輛運(yùn)動(dòng)模型

車輛運(yùn)動(dòng)模型是用來描述運(yùn)動(dòng)狀態(tài)信息隨時(shí)間的演化過程，如位置、速度、加速度和橫擺角等.要實(shí)現(xiàn)對目標(biāo)汽車的精確跟蹤和定位，關(guān)鍵在于如何有效地從觀測數(shù)據(jù)中提取目標(biāo)車輛運(yùn)動(dòng)狀態(tài)的信息，并估計(jì)目標(biāo)運(yùn)動(dòng)的狀態(tài)軌跡[7].由于道路上行駛的車輛具有機(jī)動(dòng)性，其運(yùn)動(dòng)模型在不同道路條件下是不同的，在進(jìn)行車輛狀態(tài)估計(jì)和跟蹤時(shí)必須考慮其運(yùn)動(dòng)模型的多樣性和動(dòng)態(tài)特性.假設(shè)車輛在理想的二維平面內(nèi)運(yùn)動(dòng)，通?？梢杂蓜蛩僦本€運(yùn)動(dòng)和勻速轉(zhuǎn)向運(yùn)動(dòng)來描述.設(shè)采樣間隔為T，在笛卡爾坐標(biāo)系下車輛運(yùn)動(dòng)狀態(tài)的離散模型可以表示為車輛勻速直線運(yùn)動(dòng)模型(CV model)和車輛勻速轉(zhuǎn)向運(yùn)動(dòng)模型(CT model)，其表達(dá)式分別為

(2)

(3)

1.3 多模型融合

2 多模交互UKF濾波融合估計(jì)

2.1 無跡卡爾曼濾波

考慮到汽車運(yùn)動(dòng)模型和測量模型的非線性特性，選擇無跡卡爾曼濾波作為IMM算法中的基本濾波器[9-10].非線性系統(tǒng)的一般形式為

(4)

γi，k|k-1=fk-1(ξi，k-1) (i=0，1，…，L)

(5)

(6)

(7)

χi，k|k-1=hk(ξi，k|k-1)

(8)

(9)

(10)

(11)

在獲得新的測量值zk后，進(jìn)行濾波量測更新，即

(12)

(13)

(14)

2.2 融合估計(jì)算法流程

綜合車輛運(yùn)動(dòng)的CV和CT模型，設(shè)計(jì)基于IMM的車輛運(yùn)動(dòng)狀態(tài)融合估計(jì)算法，其中每個(gè)模型的基本濾波器采用UKF濾波器，算法過程分為4個(gè)步驟：交互、濾波、模型概率更新和融合估計(jì)[12-13].

1) 交互.設(shè)第i個(gè)模型在有效的情況下，將由上一時(shí)刻各濾波器估計(jì)混合的結(jié)果作為該模型濾波器的輸入.設(shè)k-1和k時(shí)的匹配模型分別為j和i，則混合概率的表達(dá)式為

(15)

(16)

重初始化的狀態(tài)與協(xié)方差按混合估計(jì)分別為

(17)

(18)

2) 濾波.濾波計(jì)算步驟和通常情況下的卡爾曼濾波過程一致，本文采用UKF濾波器，包括預(yù)測UKFp和更新UKFu，其表達(dá)式為

(19)

(20)

3) 模型概率更新.計(jì)算每個(gè)模型的測量似然函數(shù)[14]，即

(21)

(22)

4) 融合估計(jì).計(jì)算k時(shí)刻聯(lián)合估計(jì)與聯(lián)合估計(jì)誤差協(xié)方差矩陣，其表達(dá)式為

(23)

(24)

3 仿真實(shí)驗(yàn)與分析

3.1 實(shí)驗(yàn)設(shè)計(jì)

為了驗(yàn)證算法的性能，選擇交通仿真軟件CARSIM進(jìn)行仿真實(shí)驗(yàn)，該仿真平臺(tái)軟件可以模擬各種無線通信環(huán)境和通信協(xié)議，提供了多種傳感器模型.采用軟件提供的仿真功能模塊構(gòu)建路側(cè)傳感網(wǎng)，模擬車路通信功能，預(yù)先設(shè)定目標(biāo)車輛的行駛路線和運(yùn)動(dòng)參數(shù)，圖2為仿真場景圖.

圖2 仿真場景Fig.2 Simulation scenarios

仿真場景設(shè)定為FHWA公路的某一路段，包括一段直線行駛道路和彎道行駛道路.設(shè)置路側(cè)傳感網(wǎng)由3個(gè)激光雷達(dá)構(gòu)成，分別位于交叉不同方位以覆蓋整個(gè)行駛區(qū)域.雷達(dá)掃描角度范圍和分辨率分別設(shè)定為70°和0.2°，探測最大距離為70 m，采樣率為20 Hz(即計(jì)算步長為0.05 s)，測距和方位角測量噪聲的分辨率分別為0.05 m和0.05 rad.假定實(shí)驗(yàn)中僅采用一輛目標(biāo)車輛作為狀態(tài)跟蹤的對象，車輛從圖2中A點(diǎn)出發(fā)，途經(jīng)一段彎道并轉(zhuǎn)向到達(dá)B點(diǎn)，算法中模型轉(zhuǎn)移概率矩陣設(shè)為

(25)

(26)

式中，lt和rt分別為采樣點(diǎn)t(t=1，2，…，n)上的估計(jì)值和真實(shí)值.

3.2 單模型跟蹤結(jié)果

單一模型情況下車輛運(yùn)動(dòng)只考慮CV模型，濾波器仍然采用UKF.圖3a為軌跡的跟蹤結(jié)果，其中，實(shí)線為車輛經(jīng)過的真實(shí)軌跡，虛線為估計(jì)值.圖3b為速度絕對值的跟蹤結(jié)果，其中，實(shí)線為真實(shí)速度值曲線，虛線為估計(jì)值.

圖3 單一CV模型跟蹤結(jié)果Fig.3 Tracking results by single CV model

由圖3可知，采用單一CV模型進(jìn)行估計(jì)時(shí)，位置誤差為RMSE-Positionmodel-1=11.62 m；速度誤差為RMSE-Speedmodel-1=14.87 m/s.位置和速度估計(jì)出現(xiàn)偏差較大的時(shí)段正是車輛行駛至交叉口并開始轉(zhuǎn)向后的一段時(shí)間，表明CV模型并不能很好地刻畫車輛轉(zhuǎn)向這一運(yùn)動(dòng)趨勢.

3.3 多模型跟蹤結(jié)果

聯(lián)合CV和CT兩個(gè)模型，利用本文提出的多模型融合估計(jì)算法對車輛運(yùn)動(dòng)狀態(tài)進(jìn)行跟蹤，結(jié)果如圖4所示.圖4a為軌跡的跟蹤結(jié)果，其中，實(shí)線為車輛經(jīng)過的真實(shí)軌跡，虛線為估計(jì)值.圖4b為速度絕對值跟蹤結(jié)果，其中，實(shí)線為真實(shí)速度值，虛線為估計(jì)值.

圖4 多模型數(shù)據(jù)融合算法跟蹤結(jié)果Fig.4 Tracking results by multi-model data fusion algorithm

由圖4可知，采用多模型算法可得，位置誤差為RMSE-PositionIMM=1.53 m；速度誤差為RMSE-SpeedIMM=3.18 m/s.對比圖3、4的結(jié)果可以看到，采用多模型的狀態(tài)跟蹤方法與使用單模型的跟蹤方法相比，位置和速度值的估計(jì)精度均有大幅提高，跟蹤誤差分別降低了86.8%和78.6%.

4 結(jié) 論

采用交互多模型設(shè)計(jì)的車輛運(yùn)動(dòng)狀態(tài)估計(jì)方法能夠根據(jù)運(yùn)動(dòng)狀態(tài)的變化實(shí)現(xiàn)模型之間的自動(dòng)切換，通過UKF濾波的融合估計(jì)大大提高了車輛行駛過程中運(yùn)動(dòng)狀態(tài)參數(shù)的精度.仿真結(jié)果表明：本文算法具有良好的適應(yīng)性，與單一模型的跟蹤方法相比，位置和速度值的跟蹤誤差分別降低了86.8%和78.6%，為汽車的安全和輔助駕駛提供了強(qiáng)有力的技術(shù)支持.