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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

按照以下公式建立常規(guī)下?tīng)顟B(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)分別為測(cè)量噪聲和狀態(tài)噪聲，R(k)和Q(k)分別為相對(duì)應(yīng)的噪聲協(xié)方差；s(k+1)表示k+1時(shí)刻的行駛里程；s(k)表示k時(shí)刻的行駛里程；v(k+1)表示k+1時(shí)刻的速度；v(k)表示k時(shí)刻的速度；θ(k+1)表示k+1時(shí)刻的坡度值；θ(k)表示k時(shí)刻的坡度值；表示k時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣；G(k)表示k時(shí)刻的系統(tǒng)輸入矩陣；Z(k)表示k時(shí)刻的輸出向量；H(k)表示k時(shí)刻的系統(tǒng)測(cè)量矩陣；U(k)表示k時(shí)刻的輸入矩陣；Δt表示時(shí)間間隔；σ(k)表示k時(shí)刻的車輛旋轉(zhuǎn)質(zhì)量系數(shù)。

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

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

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

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

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

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

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

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

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

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

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

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

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

通過(guò)卡爾曼濾波狀態(tài)協(xié)方差按照以下公式計(jì)算狀態(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；

且

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

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

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

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

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

附圖說(shuō)明

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

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

圖2是車輛縱向動(dòng)力分析圖。

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

具體實(shí)施方式

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

實(shí)施例1

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

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

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

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

加速度a可由速度v對(duì)時(shí)間差分獲得，第k時(shí)刻的加速度可表示為：

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

變速器傳動(dòng)比i_g按照如下計(jì)算方式得到：

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

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

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

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

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

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

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

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

實(shí)施例2

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

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

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

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

加速度a可由速度v對(duì)時(shí)間差分獲得，第k時(shí)刻的加速度可表示為：

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

變速器傳動(dòng)比i_g按照如下計(jì)算方式得到：

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

σma＝F_t-F_f-F_i-F_v；

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

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

滾動(dòng)阻力的計(jì)算公式是：

F_r＝mgf_r cosθ；

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

F_i＝mg sinθ；

同理可將sinθ近似為sinθ≈θ?？諝庾枇Φ挠?jì)算公式為

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

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

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

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

常規(guī)下?tīng)顟B(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è)道路坡度沒(méi)有發(fā)生突變，則狀態(tài)方程可以描述為：

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

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

其中，

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

步驟四：

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

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

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

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

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

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

其中，

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

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

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

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

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

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

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

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

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

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

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

因?yàn)镵(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)換為：

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

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

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