本發(fā)明涉及智能交通系統(tǒng)技術(shù)領(lǐng)域，特別是一種基于自適應(yīng)擴(kuò)展卡爾曼濾波的動態(tài)道路坡度估計方法。

背景技術(shù)：

隨著車輛智能化的發(fā)展，人們對出行駕駛的安全性、舒適性、經(jīng)濟(jì)性的需求越來越高。而除了車輛本身的動力性能和人為駕駛操作外，道路坡度也是影響車輛駕駛的一個重要因素。當(dāng)車輛經(jīng)過坡道時容易出現(xiàn)不合理的加速、減速、頻繁換擋等不合理的駕駛操作導(dǎo)致安全隱患問題，增加車輛的燃油消耗和尾氣排放，同時影響了駕駛的舒適性。因此，實時獲取準(zhǔn)確的道路坡度信息對于提高車輛安全性和燃油經(jīng)濟(jì)性具有重要意義。

目前關(guān)于道路坡度信息獲取的算法從實現(xiàn)原理上可以分為三類：基于加速度傳感器的檢測方法、結(jié)合GPS和GIS的地理信息查詢方法和基于縱向動力學(xué)模型的估計方法?；诩铀俣葌鞲衅鞯姆椒ǜ鶕?jù)加速度傳感器信號和由車輛速度計算得到運(yùn)動加速度，結(jié)合運(yùn)動學(xué)分析即可得到道路坡度角。該算法比較簡單，但是需要附加安裝傳感器設(shè)備，且容易受到設(shè)備噪聲干擾的影響，因此其應(yīng)用受到限制。結(jié)合GPS和GIS的地理信息查詢方法是根據(jù)GPS的定位技術(shù)，通過與GIS地理信息系統(tǒng)進(jìn)行電子地圖匹配的方法獲得坡度信息。然而該方法的估計精度依賴于GPS信息，且GPS信息容易因為城市高樓和隧道等的影響而容易丟失，無法獲取坡度信息。因此，目前國內(nèi)基于車輛動力學(xué)的估計方法成為研究熱點。但現(xiàn)有研究主要側(cè)重于坡度估計的實時性，忽略了因駕駛過程存在的各種不確定干擾和模型誤差，這在實際應(yīng)用中會大大降低道路坡度估計精度。

因此，需要一種基于車載數(shù)據(jù)的考慮非確定性干擾影響下的道路坡度實時估計方法。

技術(shù)實現(xiàn)要素：

本發(fā)明的目的是提出基于自適應(yīng)擴(kuò)展卡爾曼濾波的動態(tài)道路坡度估計方法；該方法針對道路坡度估計誤差及濾波發(fā)散問題，將基于車輛縱向動力學(xué)模型的常規(guī)擴(kuò)展卡爾曼濾波改進(jìn)為自適應(yīng)濾波估計，基于新息序列和坡度估計值變化率對測量協(xié)方差和過程協(xié)方差進(jìn)行動態(tài)地自適應(yīng)調(diào)整，從而提高坡度估計精度。

本發(fā)明的目的是通過以下技術(shù)方案來實現(xiàn)的：

本發(fā)明提供的基于自適應(yīng)擴(kuò)展卡爾曼濾波的道路坡度估計方法，包括以下步驟：

步驟一：通過數(shù)據(jù)采集裝置獲取車輛狀態(tài)數(shù)據(jù)，結(jié)合車輛狀態(tài)數(shù)據(jù)和車輛固有參數(shù)，計算模型相關(guān)參數(shù)：所述相關(guān)參數(shù)包括加速度a和變速器傳動比i_g；

步驟二：基于車輛動力學(xué)按照以下公式建立坡度與車輛狀態(tài)數(shù)據(jù)的關(guān)系模型：

其中，F(xiàn)_t為車輛驅(qū)動力，F(xiàn)_f為滾動阻力，F(xiàn)_i為坡度阻力，F(xiàn)_v為空氣阻力，F(xiàn)_j為加速阻力；m為汽車質(zhì)量；i_g為變速器傳動比；i₀是傳速器傳動比，η為機(jī)械效率，f_r為滾動阻力，ρ是空氣密度；C_D是空氣阻力系數(shù)；A為車輛正向迎風(fēng)面積，v為車速；θ為道路坡度；

步驟三：基于坡度與車輛狀態(tài)數(shù)據(jù)的關(guān)系模型，構(gòu)建擴(kuò)展卡爾曼濾波估計模型；

步驟四：將卡爾曼濾波估計模型改進(jìn)為自適應(yīng)擴(kuò)展卡爾曼濾波算法模型。

進(jìn)一步，所述自適應(yīng)擴(kuò)展卡爾曼濾波算法模型設(shè)置有自適應(yīng)調(diào)整模塊，所述自適應(yīng)調(diào)整模塊是通過測量協(xié)方差和過程協(xié)方差來實現(xiàn)動態(tài)自適應(yīng)調(diào)整，形成自適應(yīng)卡爾曼濾波算法。

進(jìn)一步，所述步驟三中的擴(kuò)展卡爾曼濾波估計模型是通過以下步驟來實現(xiàn)的：

1)按照以下公式將關(guān)系模型轉(zhuǎn)化成離散狀態(tài)空間的形式；

按照以下公式建立常規(guī)下狀態(tài)空間：

其中，是指狀態(tài)變量，u∈R是系統(tǒng)輸入，y∈R^m是系統(tǒng)輸出，A∈R^n×n,B∈Rⁿ,C∈R^m×n表示系統(tǒng)參數(shù)矩陣；

按照以下公式建立狀態(tài)方程：

式中，表示車輛行駛里程的微分(變化率)；表示速度的微分(變化率)；σ表示車輛旋轉(zhuǎn)質(zhì)量系數(shù)；表示坡度的微分(變化率)；r表示車輪半徑；

按照以下公式將狀態(tài)方程離散化為離散狀態(tài)空間：

其中，

式中，w(k)和v(k)分別為測量噪聲和狀態(tài)噪聲，R(k)和Q(k)分別為相對應(yīng)的噪聲協(xié)方差；s(k+1)表示k+1時刻的行駛里程；s(k)表示k時刻的行駛里程；v(k+1)表示k+1時刻的速度；v(k)表示k時刻的速度；θ(k+1)表示k+1時刻的坡度值；θ(k)表示k時刻的坡度值；表示k時刻的狀態(tài)轉(zhuǎn)移矩陣；G(k)表示k時刻的系統(tǒng)輸入矩陣；Z(k)表示k時刻的輸出向量；H(k)表示k時刻的系統(tǒng)測量矩陣；U(k)表示k時刻的輸入矩陣；Δt表示時間間隔；σ(k)表示k時刻的車輛旋轉(zhuǎn)質(zhì)量系數(shù)。

進(jìn)一步，所述步驟四中的自適應(yīng)擴(kuò)展卡爾曼濾波算法模型中包括計算當(dāng)前時刻的狀態(tài)值和輸出值，所述當(dāng)前時刻的狀態(tài)值和輸出值是按照以下步驟來實現(xiàn)的：

1)利用基于上一時刻的狀態(tài)值按照以下公式計算預(yù)測當(dāng)前時刻的狀態(tài)值和輸出值：

式中，是k時刻的狀態(tài)預(yù)測值；是k時刻的狀態(tài)估計值；P(k)是狀態(tài)協(xié)方差；δ(k)為漸消因子；

所述漸消因子δ(k)按照以下公式計算：

δ(k)＝e^c(k-1)；

其中，定義c(k-1)表示指數(shù)修正因子。

進(jìn)一步，所述步驟四中的自適應(yīng)擴(kuò)展卡爾曼濾波算法模型中包括自適應(yīng)模塊，所述自適應(yīng)模塊包括以下步驟：

1)利用當(dāng)前時刻的輸出值按照以下公式計算當(dāng)前時刻的估計值：

其中，e(k)是估計系統(tǒng)輸出值與測量值的新息值；K(k)為卡爾曼濾波增益，為狀態(tài)協(xié)方差的修正值；

2)基于新息值對測量誤差協(xié)方差和狀態(tài)誤差協(xié)方差按照以下公式進(jìn)行在線調(diào)整估計：

NV(k)＝R(k)+H(k)P(k)H(k)^T；

其中，R(k)為測量噪聲協(xié)方差；NK(k)定義為新息誤差協(xié)方差，所述新息誤差協(xié)方差按照以下公式進(jìn)行計算：

其中，N代表新息序列的時間窗；k表示當(dāng)前k時刻；i表示在時間窗內(nèi)的第i個序列值；

通過卡爾曼濾波狀態(tài)協(xié)方差按照以下公式計算狀態(tài)誤差協(xié)方差：

K(k)NV(k)K(k)^T＝K(k)NV(k)(P(k)H(k)^TNV(k)^-1)^T＝K(k)H(k)P(k)^T；

且

式中，表示先驗估計值協(xié)方差，p(k)表示后驗估計值協(xié)方差。

進(jìn)一步，所述狀態(tài)噪聲協(xié)方差按照以下公式進(jìn)行計算：

由于采用了上述技術(shù)方案，本發(fā)明具有如下的優(yōu)點：

本發(fā)明提供的基于自適應(yīng)擴(kuò)展卡爾曼濾波的動態(tài)道路坡度估計方法，利用了能快速收斂和實時估計的自適應(yīng)擴(kuò)展卡爾曼濾波算法，通過車輛駕駛狀態(tài)數(shù)據(jù)動態(tài)、實時估計道路坡度，從而為駕駛員提供實時的道路坡度信息，為自動駕駛輔助決策、綠色駕駛、及自動變速器換擋控制提供重要的依據(jù)，從而實現(xiàn)安全、經(jīng)濟(jì)、舒適駕駛。本方法在不影響坡度估計實時性前提下，不但能克服因模型誤差而導(dǎo)致的估計發(fā)散問題，同時，還能動態(tài)地調(diào)整卡爾曼濾波估計中的測量噪聲協(xié)方差和過程噪聲協(xié)方差，進(jìn)而提高在不確定性動態(tài)噪聲影響下坡度估計的精度和適用范圍。

本發(fā)明的其他優(yōu)點、目標(biāo)和特征在某種程度上將在隨后的說明書中進(jìn)行闡述，并且在某種程度上，基于對下文的考察研究對本領(lǐng)域技術(shù)人員而言將是顯而易見的，或者可以從本發(fā)明的實踐中得到教導(dǎo)。本發(fā)明的目標(biāo)和其他優(yōu)點可以通過下面的說明書來實現(xiàn)和獲得。

附圖說明

本發(fā)明的附圖說明如下。

圖1是本發(fā)明采用的基于OBD-II接口的數(shù)據(jù)采集框架圖。

圖2是車輛縱向動力分析圖。

圖3是本發(fā)明采用的自適應(yīng)擴(kuò)展卡爾曼濾波估計方法流程圖。

具體實施方式

下面結(jié)合附圖和實施例對本發(fā)明作進(jìn)一步說明。

實施例1

如圖所示，本實施例提供的基于自適應(yīng)擴(kuò)展卡爾曼濾波的動態(tài)道路坡度估計方法，包括以下四個步驟：

步驟一：通過數(shù)據(jù)采集裝置獲取車輛狀態(tài)數(shù)據(jù)，結(jié)合車輛狀態(tài)數(shù)據(jù)和車輛固有參數(shù)，計算模型的其他相關(guān)參數(shù)：

1)通過OBD-II接口獲取實時的車輛狀態(tài)數(shù)據(jù)，包括動機(jī)轉(zhuǎn)矩T、車速v、發(fā)動機(jī)轉(zhuǎn)速n、油門開度Th、剎車信號Br、檔位信息Ge并通過藍(lán)牙設(shè)備存儲到移動設(shè)備終端。

2)基于相應(yīng)的車輛固有參數(shù)(輪胎滾動半徑r、主減速器傳動比i₀、道路滾動阻力系數(shù)f、傳動系機(jī)械效率η、車輛空氣阻力系數(shù)C_d、車輛正向迎風(fēng)面積A)，計算模型中其他參數(shù)信息。

加速度a可由速度v對時間差分獲得，第k時刻的加速度可表示為：

其中，Δt為數(shù)據(jù)采集軟件模塊的采樣周期；

變速器傳動比i_g按照如下計算方式得到：

步驟二：基于車輛動力學(xué)建立坡度與車輛狀態(tài)數(shù)據(jù)的關(guān)系模型；

車輛在行駛過程中受到滾動阻力，空氣阻力和坡度阻力的作用，其車輛縱向動力與道路坡度i存在的關(guān)系:

其中，F(xiàn)_t為車輛驅(qū)動力，F(xiàn)_f為滾動阻力，F(xiàn)_i為坡度阻力，F(xiàn)_v為空氣阻力，F(xiàn)_j為加速阻力；m為汽車質(zhì)量、i_g為變速器傳動比、i₀是傳速器傳動比，η為機(jī)械效率，f_r為滾動阻力，ρ是空氣密度、C_D是空氣阻力系數(shù)；

步驟三：基于坡度與車輛狀態(tài)數(shù)據(jù)的關(guān)系模型，構(gòu)建擴(kuò)展卡爾曼濾波估計模型；

1)將關(guān)系模型轉(zhuǎn)化成離散狀態(tài)空間的形式；

2)基于狀態(tài)空間方程建立擴(kuò)展卡爾曼濾波估計模型；

步驟四：改進(jìn)為自適應(yīng)擴(kuò)展卡爾曼濾波算法模型

在常規(guī)卡爾曼濾波中增加自適應(yīng)調(diào)整部分，包括增加漸消因子來增強(qiáng)新數(shù)據(jù)的作用而削弱舊數(shù)據(jù)對估計的影響、基于新息系列和坡度估計值變化率對測量協(xié)方差和過程協(xié)方差進(jìn)行動態(tài)地自適應(yīng)調(diào)整，形成自適應(yīng)卡爾曼濾波算法，進(jìn)而提高坡度估計精度。

實施例2

本實施例進(jìn)一步的詳細(xì)描述；通過OpenXC采集車輛狀態(tài)，運(yùn)用自適應(yīng)擴(kuò)展卡爾曼濾波算法實現(xiàn)道路坡度的動態(tài)估計；下面對四個步驟進(jìn)行詳細(xì)的說明：

步驟一：通過OpenXC數(shù)據(jù)采集裝置獲取車輛狀態(tài)數(shù)據(jù)，基結(jié)合車輛狀態(tài)數(shù)據(jù)和車輛固有參數(shù)，計算模型的其他相關(guān)參數(shù)：

1)附圖1是車內(nèi)傳感器獲取的車輛狀態(tài)數(shù)據(jù)傳輸及獲取的過程。為了實現(xiàn)汽車的智能化控制，目前各大汽車都采用CAN總線連接車內(nèi)部所具有的控制器、執(zhí)行器以及傳感器，并將數(shù)據(jù)統(tǒng)一傳輸?shù)絆BD-II接口。實施例中采用美國福特汽車公司提供的OpenXC插入到OBD-II接口，然后開發(fā)基于數(shù)據(jù)采集軟件APP，通過藍(lán)牙設(shè)備接收實時的車輛狀態(tài)數(shù)據(jù)(轉(zhuǎn)矩T、車速v、發(fā)動機(jī)轉(zhuǎn)速n、油門開度Th、剎車信號Br、檔位信息Ge)，存于手機(jī)移動終端。

2)基于相應(yīng)的車輛固有參數(shù)(輪胎滾動半徑r、主減速器傳動比i₀、道路滾動阻力系數(shù)f、傳動系機(jī)械效率η、車輛空氣阻力系數(shù)C_d、車輛正向迎風(fēng)面積A)，計算模型中其他參數(shù)信息。

加速度a可由速度v對時間差分獲得，第k時刻的加速度可表示為：

其中，Δt為OpenXC數(shù)據(jù)數(shù)據(jù)采集軟件模塊的采樣周期；

變速器傳動比i_g按照如下計算方式得到：

步驟二：基于車輛動力學(xué)建立坡度與車輛狀態(tài)數(shù)據(jù)的關(guān)系模型，車輛縱向動力學(xué)模型如下：

σma＝F_t-F_f-F_i-F_v；

其中，F(xiàn)_t為車輛驅(qū)動力，F(xiàn)_f為滾動阻力，F(xiàn)_i為坡度阻力，F(xiàn)_aero為空氣阻力，F(xiàn)_j為加速阻力。

驅(qū)動力對應(yīng)的關(guān)系式為：

滾動阻力的計算公式是：

F_r＝mgf_r cosθ；

由道路設(shè)計規(guī)范標(biāo)準(zhǔn)可以知道，道路坡度設(shè)計中一般不超過10度，因此為了模型簡化，可以將cosθ近似等于1。坡度阻力的跟重力和坡度大小相關(guān)，關(guān)系式為

F_i＝mg sinθ；

同理可將sinθ近似為sinθ≈θ?？諝庾枇Φ挠嬎愎綖?/p>

其中，m為汽車質(zhì)量、i_g為變速器傳動比、i₀是傳速器傳動比，η為機(jī)械效率，f_r為滾動阻力，ρ是空氣密度、C_D是空氣阻力系數(shù)；

由以上式子，即可得到車輛狀態(tài)數(shù)據(jù)與坡度的關(guān)系模型：

步驟三：基于坡度與車輛狀態(tài)數(shù)據(jù)的關(guān)系模型，構(gòu)建擴(kuò)展卡爾曼濾波估計模型。

1)將關(guān)系模型轉(zhuǎn)化成離散狀態(tài)空間的形式；

常規(guī)下狀態(tài)空間描述的表達(dá)式為：

其中，是指狀態(tài)變量，u∈R是系統(tǒng)輸入，y∈R^m是系統(tǒng)輸出，A∈R^n×n,B∈Rⁿ,C∈R^m×n表示系統(tǒng)參數(shù)矩陣；

假設(shè)道路坡度沒有發(fā)生突變，則狀態(tài)方程可以描述為：

為了實現(xiàn)道路坡度的實時遞推估計，將狀態(tài)方程離散化

以上是車輛最后實現(xiàn)離散狀態(tài)空間描述

其中，

式中，w(k)和v(k)分別為測量噪聲和狀態(tài)噪聲，其相對應(yīng)的噪聲協(xié)方差分別為R(k)和Q(k)。

步驟四：

由以上建立的常規(guī)擴(kuò)展卡爾曼濾波算法，是在假設(shè)系統(tǒng)的測量噪聲和系統(tǒng)噪聲在濾波過程中是已知的且為定值的基礎(chǔ)上。然而車輛在行駛過程中的駕駛操作策略及周圍環(huán)境不斷變化的，其噪聲統(tǒng)計特性也會變化，同時構(gòu)建的部分模型參數(shù)不可測或隨著路況環(huán)境發(fā)生變化，導(dǎo)致估計結(jié)果誤差變大甚至引起濾波發(fā)散。

為了防止濾波發(fā)散且提高道路坡度估計精度，附圖3中表示的是自適應(yīng)擴(kuò)展卡爾曼濾波算法的流程圖，卡爾曼濾波算法包括兩個計算過程：時間更新和測量更新。時間更新方程向前推算先驗狀態(tài)估計值和先驗誤差協(xié)方差；測量更新方程將先驗狀態(tài)估計和測量變量結(jié)合產(chǎn)生狀態(tài)的后驗估計并更新估計狀態(tài)的后驗估計誤差協(xié)方差。由上一時刻的狀態(tài)估計值和當(dāng)前測量值便可估算下一時刻的坡度估計值，如此遞推。本實施例中的自適應(yīng)是指基于卡爾曼濾波新息在坡度估計的過程中動態(tài)地調(diào)整量測噪聲協(xié)方差矩陣及誤差協(xié)方差陣。

1)基于卡爾曼濾波算法獲取時間更新和測量更新值；

基于上一時刻的狀態(tài)值預(yù)測當(dāng)前時刻的狀態(tài)值和輸出值，計算公式為

式中，是k時刻的狀態(tài)預(yù)測值；是k時刻的狀態(tài)估計值；P(k)是狀態(tài)協(xié)方差。δ(k)為漸消因子；其目的是當(dāng)狀態(tài)估計變化比較大的情況下，增強(qiáng)新數(shù)據(jù)的作用，弱化老數(shù)據(jù)對估計的影響，縮短坡度估計收斂時間；其計算公式為：

δ(k)＝e^c(k-1)；

其中，

由以上公式可知，漸消因子δ(k)的大小取決于上一時狀態(tài)值變化的大小，當(dāng)上一時刻坡度估計值變化率大則漸消因子也增大，以此來增強(qiáng)新數(shù)據(jù)在估計中的作用。

獲取時間更新的狀態(tài)預(yù)測值后，可以利用當(dāng)前時刻的輸出值進(jìn)一步的進(jìn)行預(yù)測值更新，從而獲取當(dāng)前時刻的估計值，計算公式為：

其中，e(k)是估計系統(tǒng)輸出值與測量值的差值，又稱新息值；K(k)為卡爾曼濾波增益，為狀態(tài)協(xié)方差的修正值；

2)基于新息值對測量誤差協(xié)方差和狀態(tài)誤差協(xié)方差進(jìn)行在線調(diào)整估計；

為了提高坡度估計的精度，在擴(kuò)展卡爾曼濾波估計的基礎(chǔ)上增加自適應(yīng)模塊，用于在線修正測量誤差協(xié)方差和狀態(tài)誤差協(xié)方差，R(k)的計算公式為：

NV(k)＝R(k)+H(k)P(k)H(k)^T；

其中NK(k)定義為新息誤差協(xié)方差，如下表示：

N代表新息序列時間窗，起到數(shù)據(jù)平滑的作用。

基于以上公式，可以推導(dǎo)出測量噪聲協(xié)方差：

R(k)＝NV(k)-H(k)P(k)H(k)^T；

最后通過卡爾曼濾波狀態(tài)協(xié)方差計算公式，推導(dǎo)出狀態(tài)誤差協(xié)方差計算公式

因為K(k)NV(k)K(k)^T＝K(k)NV(k)(P(k)H(k)^TNV(k)^-1)^T＝K(k)H(k)P(k)^T；

且所以狀態(tài)噪聲協(xié)方差可以轉(zhuǎn)換為：

通過以上式子對測量噪聲協(xié)方差和狀態(tài)噪聲協(xié)方差進(jìn)行自適應(yīng)地調(diào)整，在不影響計算復(fù)雜度的情況下實現(xiàn)道路坡度的動態(tài)估計。

綜上所述，本實施例提出的基于自適應(yīng)卡爾曼濾波方法的道路坡度估計方法。與現(xiàn)有的方法相比，該方法針對復(fù)雜工況下存在的不確定性干擾，能夠自適應(yīng)地調(diào)整相關(guān)參數(shù)，進(jìn)一步提高坡度估計的魯棒性和準(zhǔn)確性。

最后說明的是，以上實施例僅用以說明本發(fā)明的技術(shù)方案而非限制，盡管參照較佳實施例對本發(fā)明進(jìn)行了詳細(xì)說明，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解，可以對本發(fā)明的技術(shù)方案進(jìn)行修改或者等同替換，而不脫離本技術(shù)方案的宗旨和范圍，其均應(yīng)涵蓋在本發(fā)明的保護(hù)范圍當(dāng)中。