基于并行卡爾曼濾波的車輛環(huán)境感知方法

基于并行卡爾曼濾波的車輛環(huán)境感知方法

涂巖愷，張東風(fēng)，林明，曹輝標(biāo)(1.廈門雅迅網(wǎng)絡(luò)股份有限公司,福建廈門361008；2.鄭州宇通客車股份有限公司,河南鄭州450016)0引言智能汽車是一個(gè)集環(huán)境感知、規(guī)劃決策、多等級(jí)輔助駕駛等功能于一體的綜合系統(tǒng)。它能使車輛在行駛過(guò)程中具有實(shí)時(shí)感知自身狀態(tài)及所處地理環(huán)境的能力，能夠?yàn)槠嚨闹悄芑瘎?dòng)力性、安全性或經(jīng)濟(jì)性控制決策提供有力的參考依據(jù)。載荷與地理坡度是汽車智能優(yōu)化控制最為依賴的感知量，目前針對(duì)載荷進(jìn)行車輛優(yōu)化控制的方法有發(fā)動(dòng)機(jī)多態(tài)控制[1]、路徑優(yōu)化[2]等，針對(duì)地理坡度優(yōu)化控制的方法有自適應(yīng)巡航[3]、發(fā)動(dòng)機(jī)功率控制[4]等，還有綜合載荷與地理坡度的自動(dòng)駕駛技術(shù)[5]。傳統(tǒng)的車輛環(huán)境感知都是基于傳感器[6-7]的測(cè)量，但是高精度傳感器面臨成本高、易損壞、不易維護(hù)的問(wèn)題。目前國(guó)外開(kāi)始研究運(yùn)用軟件算法分析發(fā)動(dòng)機(jī)和儀表往車輛總線上發(fā)出的實(shí)時(shí)數(shù)據(jù)推算出車輛當(dāng)前載荷或所處地理坡度的方法。DKIM等[8]提出一種利用汽車縱向動(dòng)力學(xué)關(guān)系和迭代最小二乘法的車身質(zhì)量估算方法，但該方法易受坡度的影響。SEBSADJI等[9]提出利用擴(kuò)展卡爾曼和龍伯格觀測(cè)器結(jié)合的坡度估算方法，但是該方法以車身質(zhì)量已知為前提，在實(shí)際商用車使用條件下，車身載荷的變化是非常大的。綜合來(lái)看，目前缺少比較準(zhǔn)確的能同時(shí)求解載荷與坡度的方法。本文作者提出利用車輛CAN總線上的動(dòng)力數(shù)據(jù)，采用分步驟方式先對(duì)狀態(tài)方程進(jìn)行差分來(lái)抵消相鄰狀態(tài)的坡度因素，并采用多組卡爾曼濾波器并行迭代處理，根據(jù)收斂結(jié)果誤差最小的標(biāo)準(zhǔn)，來(lái)選擇一組濾波器求解車輛總質(zhì)量，之后再根據(jù)所求解的車輛質(zhì)量利用縱向汽車模型來(lái)實(shí)時(shí)估算汽車所處的地理坡度。1系統(tǒng)數(shù)學(xué)模型汽車縱向動(dòng)力學(xué)模型如圖1所示，汽車在行駛過(guò)程中同時(shí)受到外部力和內(nèi)部力的作用。外部力包括空氣阻力Fair、滾動(dòng)阻力Frolling和重力Fgravity；內(nèi)部力包括發(fā)動(dòng)機(jī)輸出的旋轉(zhuǎn)扭矩經(jīng)過(guò)傳動(dòng)系到達(dá)車輪上的前進(jìn)驅(qū)動(dòng)力Ftractive，以及傳動(dòng)系統(tǒng)的內(nèi)部損耗。根據(jù)圖1所示的汽車縱向動(dòng)力學(xué)模型，假設(shè)m表示汽車總質(zhì)量，a為汽車加速度，則汽車的力學(xué)平衡關(guān)系：Ftractive-Fair-Frolling-Fgravity=ma(1)汽車動(dòng)力傳動(dòng)系統(tǒng)模型如圖2所示。包括發(fā)動(dòng)機(jī)、離合器、變速器、主減速器、車輪。發(fā)動(dòng)機(jī)輸出扭矩為Te，傳動(dòng)到離合器輸出軸的扭矩為Tt，變速箱輸出軸扭矩為Tf，輪軸扭矩為Tw。發(fā)動(dòng)機(jī)輸出到變速器的扭矩Tt：(2)(3)式中：It是傳動(dòng)慣性;it是傳動(dòng)擋位比;Tf是通過(guò)變速器后的輸出扭矩。Tf通過(guò)主減速器，最終轉(zhuǎn)化為輪軸上的驅(qū)動(dòng)扭矩Tw：(4)(5)(6)車輛的加速度可以表示成車輪的旋轉(zhuǎn)加速度乘以車輪的半徑：(7)綜合上述公式可以得到牽引力的表達(dá)式：(8)式中：ug=itf/rw。汽車行駛時(shí)縱向空氣阻力：(9)式中：ρa(bǔ)ir是空氣密度;Cd是空氣阻力系數(shù);A是車正面的面積。汽車行駛時(shí)的縱向滾動(dòng)阻力：Frolling=frmgcosθ(10)式中：fr是滾動(dòng)阻力系數(shù);g是重力加速度常量;θ是道路坡度。汽車行駛時(shí)由坡度帶來(lái)的阻力：Fgravity=mgsinθ(11)將公式(8)—(11)代入到公式(1)中，并且令βr=cos[arctan(fr]，整理可得到：(12)式(12)可視為一個(gè)描述發(fā)動(dòng)機(jī)輸出扭矩、車速與質(zhì)量、坡度關(guān)系的非線性方程。后續(xù)將該方程作為狀態(tài)方程，聯(lián)立觀測(cè)方程，采用卡爾曼濾波迭代的方法，利用連續(xù)采集的車輛CAN總線動(dòng)力數(shù)據(jù)(包含扭矩Te和車速v)對(duì)汽車總質(zhì)量和坡度求解。2狀態(tài)方程與求解令Ts為車輛兩個(gè)狀態(tài)的時(shí)間間隔，v表示車輛速度，下標(biāo)k表示狀態(tài)序號(hào)，基于式(12)建立基于車輛速度的狀態(tài)方程如下：(13)如果直接采用擴(kuò)展卡爾曼濾波方法同時(shí)迭代求解式(13)狀態(tài)方程中的坡度和質(zhì)量參數(shù)，容易陷入錯(cuò)誤收斂。例如當(dāng)車輛行駛在無(wú)坡度的平原地區(qū)時(shí)，因?yàn)闊o(wú)地形變化來(lái)糾正誤差，有可能迭代收斂得出質(zhì)量偏重地形長(zhǎng)下坡、或質(zhì)量偏輕地形長(zhǎng)上坡的錯(cuò)誤結(jié)論。文中提出將狀態(tài)方程進(jìn)一步差分、分步求解的方法，先利用多組卡爾曼濾波器進(jìn)行并行計(jì)算，取收斂后預(yù)測(cè)噪聲最小的一組濾波器的質(zhì)量作為車輛當(dāng)前質(zhì)量，再反推車輛所處的地理坡度。因?yàn)檐囕v總線上速度信號(hào)和扭矩信號(hào)的發(fā)送頻率非?？欤昂髢蓚€(gè)信號(hào)周期一般在100ms以內(nèi)，因此，前后兩個(gè)狀態(tài)的坡度和道路阻力系數(shù)近似相等，即：α2φ2,k≈α2φ2,k-1，因此將式(13)前后兩個(gè)狀態(tài)相減，令Xk=vk-vk-1，整理可得抵消掉坡度參數(shù)只含質(zhì)量參數(shù)的狀態(tài)方程：Xk=AXk-1+BUk+Wk(14)式中：A=1；B=Ts；觀測(cè)量即為車輛當(dāng)前的縱向加速度，因此觀測(cè)方程：zk=HXk+Vk(15)式中：觀測(cè)矩陣H=1;Vk為觀測(cè)噪聲,方差為R。Pk表示信號(hào)協(xié)方差，即信號(hào)的預(yù)測(cè)誤差。建立卡爾曼迭代方程組如下，其中ε取一個(gè)常數(shù)初始值開(kāi)始迭代：(16)上述迭代方程組是對(duì)車輛加速度的預(yù)測(cè)求解，且方程組中的質(zhì)量參數(shù)未知，不能直接進(jìn)行運(yùn)算，需要有一個(gè)預(yù)設(shè)質(zhì)量值。由于商用車輛的車型參數(shù)中具有空載的整備質(zhì)量me與最大允許總質(zhì)量mz兩個(gè)參數(shù)，因此實(shí)質(zhì)的載重質(zhì)量應(yīng)該介于me與Mz之間。在me與mz之間每100kg取一個(gè)質(zhì)量值，作為式(16)迭代方程組中的質(zhì)量參數(shù)，這樣一共有N=(mz-me)/100組卡爾曼迭代方程組進(jìn)行并行計(jì)算。N組卡爾曼濾波器各自獨(dú)立進(jìn)行迭代運(yùn)算，如圖3所示。取所有運(yùn)算結(jié)果為收斂的濾波器，比較收斂后Pk的大小，Pk越小，表示使用當(dāng)前質(zhì)量對(duì)于加速度的預(yù)測(cè)誤差越小，說(shuō)明該質(zhì)量參數(shù)最符合車輛運(yùn)行的動(dòng)力模型，取該組卡爾曼濾波器對(duì)應(yīng)的質(zhì)量值為車輛當(dāng)前總質(zhì)量。求得車輛質(zhì)量參數(shù)后，將質(zhì)量代回公式(12)即可由車輛的扭矩信號(hào)Te及加速度a計(jì)算車輛當(dāng)前所處坡度值。3實(shí)驗(yàn)結(jié)果采用ZK6876H5Z型號(hào)客車進(jìn)行實(shí)驗(yàn)，整備質(zhì)量為4300kg，最大允許質(zhì)量為6100kg。實(shí)驗(yàn)時(shí)經(jīng)過(guò)裝載負(fù)重，實(shí)際整車質(zhì)量為5543kg。由于比較難去測(cè)繪準(zhǔn)確的道路地理坡度，因此將車輛置于汽車轉(zhuǎn)鼓實(shí)驗(yàn)臺(tái)上進(jìn)行實(shí)驗(yàn)，如圖4所示，所需地理坡度可直接在轉(zhuǎn)鼓實(shí)驗(yàn)臺(tái)上設(shè)置，將文中的環(huán)境感知算法計(jì)算得到的坡度與轉(zhuǎn)鼓所設(shè)地理坡度比較，以驗(yàn)證數(shù)據(jù)的準(zhǔn)確性。從整備質(zhì)量4300kg開(kāi)始，每100kg取一個(gè)質(zhì)量代入式(16)卡爾曼濾波器組，一共有19組濾波器并行進(jìn)行迭代計(jì)算，每組濾波器ε取一個(gè)相同常數(shù)初始值，進(jìn)行了200輪迭代后，共有4組濾波器收斂，15組濾波器不收斂。收斂的4組濾波器Pk值關(guān)系如圖5所示。可以看到，5500kg對(duì)應(yīng)的卡爾曼濾波器迭代收斂誤差最小，因此取5500kg為載重計(jì)算結(jié)果。得到質(zhì)量之后，進(jìn)一步反推車輛所處的坡度。將利用式(12)反推出的車輛所經(jīng)過(guò)的地理坡度和轉(zhuǎn)鼓設(shè)置的坡度進(jìn)行對(duì)比，結(jié)果如圖6所示，可以看出二者有比較高的重合度，說(shuō)明此方法估算的地理坡度有比較高的準(zhǔn)確性。4小結(jié)文中通過(guò)車輛的縱向動(dòng)力學(xué)關(guān)系，在建立基于速度的狀態(tài)方程基礎(chǔ)上，進(jìn)一步進(jìn)行差分處理，建立基于加速度的卡爾曼迭代計(jì)算方程，利用汽車總線信號(hào)頻率高間隔短的特點(diǎn)，先約去卡爾曼濾波器中的坡度參數(shù)，將質(zhì)