汪 偉， 貝紹軼， 張?zhí)m春， 汪永志

(江蘇理工學(xué)院汽車與交通工程學(xué)院， 江蘇 常州 213001)

基于蟻群優(yōu)化FEKF算法的汽車狀態(tài)估計

汪 偉， 貝紹軼， 張?zhí)m春， 汪永志

(江蘇理工學(xué)院汽車與交通工程學(xué)院， 江蘇 常州 213001)

針對汽車狀態(tài)估計過程中觀測噪聲時變問題， 提出一種雙重迭代自適應(yīng)濾波算法—蟻群優(yōu)化模糊邏輯擴展卡爾曼濾波(FEKF)算法. 建立考慮Fiala輪胎模型的汽車二自由度非線性動力學(xué)模型， 利用模糊邏輯對擴展卡爾曼濾波(EKF)算法估計過程中的觀測噪聲水平進行在線修正， 同時引入蟻群優(yōu)化算法對模糊邏輯中的輸入輸出隸屬度函數(shù)進行優(yōu)化， 得到的雙重迭代算法對處理強時變觀測噪聲水平下濾波估計過程具有很強的自適應(yīng)性. 最后通過建立虛擬仿真試驗來驗證該蟻群優(yōu)化FEKF新算法的估計精度， 結(jié)果顯示, 蟻群優(yōu)化FEKF算法相比較于FEKF算法估計精度更高， 魯棒性更強.

汽車動力學(xué)； 狀態(tài)估計； 蟻群優(yōu)化算法； 模糊邏輯； EKF算法； 虛擬試驗

0 引言

汽車當(dāng)前的行駛狀態(tài)作為很多電子控制系統(tǒng)的輸入條件， 比如ABS、 ESP等系統(tǒng)都需要準(zhǔn)確知道當(dāng)前的汽車車速、 橫擺角速度、 質(zhì)心側(cè)偏角等參數(shù). 因此， 如何實時準(zhǔn)確地獲取上述參數(shù)對汽車穩(wěn)定控制系統(tǒng)正常工作起著至關(guān)重要的作用. 上述參數(shù)的獲取雖然可以通過試驗相關(guān)傳感器直接測量獲得， 但是其試驗的相關(guān)費用昂貴， 且傳感器的精度要求很高， 不適用于配置在量產(chǎn)車上. 因此， 國內(nèi)外對于上述參數(shù)的獲取主要以設(shè)計汽車估計器方式進行研究， 這些研究主要集中在估計算法， 主要目的是針對汽車穩(wěn)定控制系統(tǒng)中的控制邏輯量進行實時準(zhǔn)確的獲取, 從而進一步對危險工況下汽車的行駛狀態(tài)進行干預(yù).

針對汽車估計算法的研究主要有卡爾曼濾波(KF)[1]、 擴展卡爾曼濾波(EKF)[2-3]、 容積卡爾曼濾波(CKF)[4]、 無跡卡爾曼濾波(UKF)[5]、 模糊邏輯[6]、 狀態(tài)觀測器[7]、 神經(jīng)網(wǎng)絡(luò)[8-9]等. 這些研究方法中，常常設(shè)定觀測噪聲為定值噪聲， 即該噪聲在濾波參數(shù)設(shè)定時為常數(shù). 但從實際情況出發(fā)， 觀測噪聲會隨著外界干擾而發(fā)生變化， 呈現(xiàn)較強的不確定性， 這樣必然會導(dǎo)致濾波估計產(chǎn)生誤差甚至發(fā)散. 模糊邏輯作為具有處理不確定性問題能力的方法被引入到卡爾曼濾波當(dāng)中[10]， 用于在線估計觀測噪聲水平， 可以較準(zhǔn)確地跟蹤觀測噪聲的變化， 保證了濾波算法的實時估計精度.

通過建立考慮輪胎模型的汽車非線性二自由度動力學(xué)模型， 運用EKF算法進行濾波估計， 同時將模糊邏輯加入到EKF算法當(dāng)中， 對EKF濾波過程中的觀測噪聲水平進行在線預(yù)測與修正. 模糊邏輯算法通過設(shè)計的模糊系統(tǒng)實時在線得到量測新息的實際方差與理論方差的差值， 進一步調(diào)整量測噪聲矩陣， 在此過程中將用先驗知識產(chǎn)生輸入輸出的隸屬度函數(shù). 由于采用先驗知識獲取函數(shù)， 所以該函數(shù)不具備最優(yōu)特性， 此時引入蟻群算法可以彌補模糊邏輯算法中的不足， 通過選擇合適的目標(biāo)函數(shù)對輸入輸出的隸屬度函數(shù)進行雙重迭代優(yōu)化， 得到的蟻群優(yōu)化FEKF算法在汽車狀態(tài)估計過程中可以很大程度地提高估計精度和魯棒性.

1 非線性二自由度汽車動力學(xué)模型

建立汽車動力學(xué)模型僅考慮沿y軸的側(cè)向運動與繞z軸的橫擺運動兩個自由度， 且汽車沿x軸前進的速度vx視為不變； 這樣實際汽車便簡化成兩輪摩托車模型[11]， 具體如圖1所示.

汽車運動微分方程

(1)

(2)

整車質(zhì)心側(cè)偏角

(3)

由式(2)、 (3)整理可得

(4)

將式(1)和(4)作為狀態(tài)方程， 狀態(tài)變量為x=[ωr,β]T；

觀測方程

(5)

前后輪側(cè)偏角

(6)

(7)

式中各符號代表的含義詳見文獻[11].

由于上述模型中不考慮輪胎縱向力， 而需要模擬輪胎側(cè)向力， 在此采用對側(cè)向力模擬較為準(zhǔn)確的“修正Fiala模型”， 該模型能夠準(zhǔn)確地描述輪胎的非線性側(cè)偏特性[12]. “修正Fiala模型”的具體擬合公式如下：

側(cè)向力公式

(8)

回正力矩公式

(9)

2 EKF算法濾波過程

對于上述所建立的非線性狀態(tài)空間模型， 從數(shù)學(xué)意義上可以抽象為以下非線性方程， 式(10)為狀態(tài)方程， 式(11)為觀測方程[13].

(10)

(11)

(12)

(13)

該系統(tǒng)的噪聲統(tǒng)計特性：E{wk}=0，E{υk}=0

其中:δkj是克羅內(nèi)克函數(shù)， 即，

濾波過程如下：

1)狀態(tài)預(yù)測:

(14)

2)方差預(yù)測:

(15)

3)濾波增益:

(16)

4)方差更新:

(17)

5)狀態(tài)更新:

(18)

3 蟻群優(yōu)化FEKF算法

3.1 FEKF算法設(shè)計

(19)

式中:n為自適應(yīng)調(diào)整因子.

k時刻新息的實際方差與理論方差的差值被定義為ek：

(20)

建立的模糊控制器是以自適應(yīng)調(diào)整因子n為輸出， 以新息的實際方差值與理論方差值的差值為輸入. 在最優(yōu)模糊控制情況下， 新息的理論方差值應(yīng)近似等于實際方差值. 根據(jù)上述思想設(shè)計模糊控制器, 基本的步驟如圖2所示.

定義輸入的模糊子集為：L0<0，E0≈0，M0>0.

定義輸出的模糊子集為：l1<1，e1≈1，m0>1.

采用三角形隸屬度函數(shù)作為輸入輸出的隸屬度函數(shù)， 根據(jù)先驗知識及試驗經(jīng)驗可以總結(jié)輸入輸出的隸屬度函數(shù)具體如圖3、 4所示.

模糊邏輯規(guī)則為：

1) 若Ratiok∈L0， 則n∈l1； 2) 若Ratiok∈E0， 則n∈e1； 3) 若Ratiok∈M0， 則n∈m1.

3.2 蟻群算法優(yōu)化隸屬度函數(shù)

上述FEKF算法設(shè)計過程中， 輸入輸出已經(jīng)明確， 模糊邏輯規(guī)則可以通過試驗及仿真進行總結(jié)， 所以模糊邏輯規(guī)則準(zhǔn)確性較好， 但隸屬度函數(shù)是一個待優(yōu)化的準(zhǔn)變量， 其常規(guī)是根據(jù)先驗知識和試驗結(jié)果進行總結(jié)， 所以隸屬度函數(shù)的優(yōu)越性與試驗的次數(shù)以及試驗的條件息息相關(guān)， 所以本文致力于引入蟻群算法進行隸屬度函數(shù)的優(yōu)化， 以增加最終濾波的估計精度.

Dorigo受螞蟻覓食時的路徑選擇行為啟發(fā)提出了蟻群算法[15- 16]. 螞蟻路徑選擇是通過行走不同的地點而轉(zhuǎn)移的，t時刻螞蟻從位置i向位置j的轉(zhuǎn)移概率Mij(t)為：

(21)

式中: 各符號代表的含義詳見文獻[15-16].

由式(21)可知， 蟻群路徑選擇過程中的尋優(yōu)， 可以揮發(fā)信息素作用， 在此定義ρ∈[0， 1)為信息素殘留系數(shù)， 表示信息素物質(zhì)的持久性， 則1-ρ為信息素的揮發(fā)度. 一次循環(huán)完成后，t+1時刻蟻群在所移動路徑上的信息素強度τzδ(t+1)按照下式進行更新調(diào)整，

(22)

式中： Δτij(t，t+1)為第p只螞蟻在本次循環(huán)過程中留在路徑(i，j)上的信息素. 路徑越短， 信息素釋放就越多.

對于上述模糊邏輯規(guī)則， 通過實驗數(shù)據(jù)以及先驗知識可以總結(jié)得到， 且具有較高可信度. 但是對于輸入輸出的隸屬度函數(shù)則需要盡可能的準(zhǔn)確， 因為該兩函數(shù)將直接影響模糊輸出的數(shù)值, 即自適應(yīng)調(diào)整因子α的大小. 所以, 在此采用蟻群算法對隸屬度函數(shù)進行優(yōu)化， 尋求最優(yōu)的隸屬度函數(shù)、 根據(jù)節(jié)3.1中采用的三角形隸屬度函數(shù)， 對于三角形上頂點， 根據(jù)先驗知識可以獲取， 僅需要對下頂點進行尋優(yōu)， 以獲取最優(yōu)的隸屬度函數(shù).

根據(jù)新息的實際方差設(shè)計目標(biāo)函數(shù)：

(23)

蟻群算法優(yōu)化過程如下：

1) 參數(shù)初始化， 包括時間初始化， 及迭代次數(shù)Niter初始化；

2) 設(shè)定螞蟻總數(shù)為m， 每個螞蟻都置于初始化鄰域內(nèi)， 按照式(21)的概率向相應(yīng)鄰域移動；

3) 計算每只螞蟻的目標(biāo)函數(shù)， 記錄當(dāng)前隸屬度函數(shù)最優(yōu)解；

4) 按照式(22)更新信息素強度；

5) 若迭代次數(shù)達到設(shè)定次數(shù)， 則停止迭代轉(zhuǎn)到步驟；

6) 若未達到， 轉(zhuǎn)到2)繼續(xù)迭代；

7) 輸出優(yōu)化后的隸屬度函數(shù).

通過上述蟻群算法的尋優(yōu)功能， 可以得出優(yōu)越的輸入及輸出的隸屬度函數(shù)， 從而增加自適應(yīng)調(diào)整因子n的精度， 最終增加濾波精度.

4 虛擬試驗驗證

為得到最優(yōu)的隸屬度函數(shù)， 在不同定值噪聲水平下， 進行多次濾波估計試驗， 然后通過蟻群優(yōu)化算法按照設(shè)定的目標(biāo)函數(shù)進行尋優(yōu). 得到比先驗知識更加優(yōu)越的輸入輸出隸屬度函數(shù)后， 將優(yōu)化后的FEKF算法應(yīng)用到時變的觀測噪聲濾波過程中， 進行虛擬試驗驗證.

4.1 隸屬度函數(shù)的優(yōu)化仿真

采用Carsim與Simulink聯(lián)合仿真平臺建立虛擬試驗平臺來模擬幾種典型過渡工況下的汽車運動狀態(tài)響應(yīng). 仿真模擬中采用某型轎車的整車參數(shù)進行建模， 整車的參數(shù)如下，m=1 100 kg；Iz=1 720 kg·m2；a=1.22 m；b=1.28 m；k1=180 kN·rad；k2=-160 kN·rad-1； 試驗選擇蛇形連續(xù)避讓路徑， 汽車以80 km·h-1的試驗車速， 采樣時間為0.02 s.

對觀測量ay分三次加入定值高斯白噪聲， 觀測噪聲的協(xié)方差矩陣R分別為0.001、 0.01、 0.05. 在定值觀測噪聲狀態(tài)下， 進行三次濾波估計試驗仿真.

通過三次仿真試驗結(jié)果綜合得出最優(yōu)的輸入輸出的隸屬度函數(shù)如圖5、 6所示.

為展現(xiàn)優(yōu)化前后隸屬度函數(shù)對最終估計結(jié)果的影響， 噪聲協(xié)方差選取為0.01水平時的估計對比結(jié)果如圖7、 8所示. 從圖7可以看出， 在定值噪聲水平下， 優(yōu)化前后隸屬度函數(shù)對橫擺角速度的估計精度影響不大， 都可以較好地跟蹤理論曲線， 在峰值處出現(xiàn)一定的偏差， 這個與EKF算法本身的線性化近似誤差有很大關(guān)系. 同理， 從圖8看出， 質(zhì)心側(cè)偏角的估計精度在優(yōu)化前后也相差甚微， 但是從峰值處看來， 優(yōu)化后得到的質(zhì)心側(cè)偏角估計值更加貼近理論曲線， 展示了優(yōu)化后隸屬度函數(shù)的優(yōu)越性.

4.2 時變噪聲水平下虛擬試驗

為觀測量ay加入時變高斯白噪聲(文中加入的是均值為零， 方差隨時間發(fā)生變化的噪聲)， 該噪聲協(xié)方差按照變振幅正弦曲線進行變化， 正弦曲線振幅每隔一個周期變化一次， 按照給定0.01、 0.006、 0.035為振幅的時變噪聲順序進行分析， 正弦曲線周期為2 s， 如圖9所示為時變噪聲變化曲線圖.

加入時變噪聲后， 所得的狀態(tài)估計結(jié)果如圖10、 11所示. 從圖10、 11可以看出， 強時變噪聲對估計結(jié)果產(chǎn)生很大影響， 基于先驗知識的FEKF算法中的隸屬度函數(shù)無法較準(zhǔn)確的跟蹤理論曲線， 在峰值處產(chǎn)生很大的偏差. 基于蟻群算法優(yōu)化后的隸屬度函數(shù)， 其大大提高了橫擺角速度和質(zhì)心側(cè)偏角的估計精度， 具有較高的估計精度及很強的魯棒性.

通過定量比較FEKF算法和蟻群優(yōu)化FEKF算法的估計精度， 給出兩種算法的估計值相對于理論值(理論值來自于軟件聯(lián)合仿真得出的數(shù)據(jù))的平均絕對誤差(MAE)和均方根誤差(RSME)， 同時將4.1中定值噪聲水平下的估計結(jié)果也一并加入比較， 如表1所示.

從表1可以看出， 在定值噪聲水平下， 橫擺角速度和質(zhì)心側(cè)偏角的平均絕對誤差(MAE)和均方根誤差(RSME)指標(biāo)在優(yōu)化前后相差不大， 且優(yōu)化后的算法即蟻群優(yōu)化FEKF算法較優(yōu)化前的FEKF算法估計效果稍好.

在加入強時變噪聲下， 優(yōu)化前的FEKF算法的誤差值成倍地增加， 估計精度急劇變差； 而蟻群算法優(yōu)化得到的FEKF算法則表現(xiàn)出很好的魯棒性和估計精度. 其中質(zhì)心側(cè)偏角的MAE指標(biāo)(0.002 2)幾乎可以達到與定值噪聲水平下蟻群優(yōu)化FEKF算法(0.001 7)相當(dāng)?shù)男Ч?

表1 不同算法的MAE和RSME指標(biāo)Tab.1 MAE and RSME index of different algorithms

5 結(jié)語

1) 提出蟻群優(yōu)化FEKF的汽車狀態(tài)估計算法， FEKF可以提高對時變觀測噪聲的自適應(yīng)性， 但是其輸入輸出的隸屬度函數(shù)由于通過先驗知識獲得， 無法達到最優(yōu)狀況， 此時蟻群算法的引入就彌補了這個缺點， 并且進一步提高了FEKF算法的自適應(yīng)性和估計精度.

2) 通過虛擬試驗結(jié)果對比可以看出， FEKF算法可以實現(xiàn)濾波的自適應(yīng)， 但在強時變噪聲水平下， 蟻群優(yōu)化FEKF算法則表現(xiàn)出了更強的魯棒性和估計精度.

[1] CHEN B C， HSIEH F C. Sideslip angle estimation using extended Kalman filter[J]. Vehicle System Dynamics， 2008， 46(sup): 353-364.

[2] 宗長富， 胡丹， 楊肖， 等. 基于擴展Kalman濾波的汽車行駛狀態(tài)估計[J]. 吉林大學(xué)學(xué)報(工學(xué)版)， 2009， 39(1): 7-11.

[3] 郭孔輝， 付皓， 丁海濤. 基于擴展卡爾曼濾波的汽車質(zhì)心側(cè)偏角估計[J]. 汽車技術(shù)， 2009(4): 1-3.

[4] 魏喜慶， 宋申民. 無模型容積卡爾曼濾波及其應(yīng)用[J]. 控制與決策， 2013， 28: 770-773.

[5] ANTONOV S， FEHN A， KUGI A. Unscented Kalman filter for vehicle state estimation[J]. Vehicle System Dynamics， 2011， 49(9): 1 497-1 520.

[6] 施樹明， LUPKER Henk， BREMMER Paul， 等. 基于模糊邏輯的車輛側(cè)偏角估計方法[J]. 汽車工程， 2005， 27(4): 426-430.

[7]楊福廣， 李貽斌， 阮久宏， 等. 基于擴張狀態(tài)觀測器的路面附著系數(shù)實時估計[J]. 農(nóng)業(yè)機械學(xué)報， 2010， 41(8)： 6-15.

[8] 張小龍， 李亮， 李紅志， 等. 基于改進RBF網(wǎng)的汽車側(cè)偏角估計方法試驗研究[J]. 機械工程學(xué)報， 2010， 46(22): 105-110.

[9] MELZI S， SABBIONI E. On the vehicle sideslip angle estimation through neural networks: numerical and experimental results[J]. Mechanical Systems and Signal Processing， 2011， 25(6): 2 005-2 019.

[10] 黃超， 林棻. S-修正的自適應(yīng)卡爾曼濾波與模糊卡爾曼濾波相結(jié)合的汽車狀態(tài)估計算法[J]. 中國機械工程， 2013， 24(20): 2 831-2 835.

[11] 余志生. 汽車?yán)碚揫M]. 北京: 機械工業(yè)出版社， 2009.

[12] 郭孔輝. 汽車操縱動力學(xué)[M]. 長春: 吉林科學(xué)技術(shù)出版社， 1991.

[13] 宋文堯， 張牙. 卡爾曼濾波[M]. 北京： 科學(xué)出版社， 1991.

[14] 肖志濤， 趙培培， 李士心. 基于INS/GPS組合導(dǎo)航的自適應(yīng)模糊卡爾曼濾波[J]. 中國慣性技術(shù)學(xué)報. 2010， 18(2): 195-198.

[15] 張潔， 張朋， 劉國寶. 基于兩階段蟻群算法的帶非等效并行機的作業(yè)車間調(diào)度[J]. 機械工程學(xué)報， 2013， 49(6): 136-144.

[16] 劉振， 胡云安. 一種多粒度模式蟻群算法及其在路徑規(guī)劃中的應(yīng)用[J]. 中南大學(xué)學(xué)報(自然科學(xué)版)， 2013， 44(9): 3 713-3 722.

(責(zé)任編輯： 蔣培玉)

Vehicle state estimation based on ant colony optimization FEKF algorithm

WANG Wei， BEI Shaoyi， ZHANG Lanchun， WANG Yongzhi

(School of Automotive and Traffic Engineering, Jiangsu University of Technology， Changzhou, Jiangsu 213001， China)

For time-varying problem of observation noise problem in vehicle state estimation, a new dual iterative adaptive filtering algorithm named the FEKF algorithm is put forward. The vehicle two freedom degrees of dynamics model based on the nonlinear Fiala tire model was established. The fuzzy logic was used on the online correction for the estimation process of the EKF algorithm. Ant colony optimization algorithm was introduced to optimize the input and output membership function in the fuzzy logic operations. So the dual iterative algorithm was obtained. The dual iterative algorithm for dealing with strong time-varying noise levels under the filtering estimation has a strong adaptability. Robustness and accuracy of the ant colony optimization FEKF algorithm compared to the FEKF algorithm is verified through the virtual experiment.

vehicle dynamics; state estimation; ant colony optimization algorithm; fuzzy logic; EKF algorithm; virtual experiment

10.7631/issn.1000-2243.2016.05.0703

1000-2243(2016)05-0703-07

2015-11-16

汪偉(1984-)， 博士， 講師， 主要從事汽車系統(tǒng)動力學(xué)研究， nuaawangwei@126.com

國家自然科學(xué)基金資助項目(51305175); 江蘇省自然科學(xué)基金資助項目(BK2012586); 江蘇省“六大人才高峰”資助項目(2012-ZBZZ-023，2013-ZBZZ-039); 江蘇省“333工程”培養(yǎng)資金資助項目(BRA2015365); 江蘇理工學(xué)院人才引進資助項目( KYY14041).

U461.6

A