基于蟻群優(yōu)化UKF算法的汽車(chē)狀態(tài)估計(jì)

張鳳嬌　魏民祥　趙萬(wàn)忠

1.南京航空航天大學(xué)，南京，210016　　2.常州工學(xué)院，常州，213002

基于蟻群優(yōu)化UKF算法的汽車(chē)狀態(tài)估計(jì)

張鳳嬌1,2魏民祥1趙萬(wàn)忠1

1.南京航空航天大學(xué)，南京，2100162.常州工學(xué)院，常州，213002

針對(duì)汽車(chē)狀態(tài)估計(jì)中過(guò)程噪聲和觀測(cè)噪聲的時(shí)變特性，提出一種新的自適應(yīng)濾波算法。該算法基于三自由度非線性汽車(chē)動(dòng)力學(xué)模型，在利用UKF對(duì)汽車(chē)狀態(tài)量進(jìn)行估計(jì)的同時(shí)，引入蟻群優(yōu)化算法，根據(jù)目標(biāo)函數(shù)對(duì)過(guò)程噪聲和觀測(cè)噪聲進(jìn)行尋優(yōu)，實(shí)現(xiàn)了過(guò)程噪聲和觀測(cè)噪聲的自適應(yīng)作用，估計(jì)精度的大幅提高。虛擬實(shí)驗(yàn)驗(yàn)證了蟻群優(yōu)化UKF算法的魯棒性和精度。研究結(jié)果對(duì)汽車(chē)主動(dòng)控制系統(tǒng)的開(kāi)發(fā)具有重大的理論指導(dǎo)意義。

車(chē)輛工程；蟻群優(yōu)化算法；UKF算法；狀態(tài)估計(jì)；虛擬試驗(yàn)

0　引言

隨著汽車(chē)工業(yè)的飛速發(fā)展，汽車(chē)主動(dòng)安全技術(shù)越來(lái)越受到人們的關(guān)注。對(duì)汽車(chē)主動(dòng)安全的研究幾乎都需要獲知汽車(chē)的當(dāng)前行駛狀態(tài)，例如汽車(chē)的當(dāng)前車(chē)速、側(cè)向加速度、橫擺角速度和質(zhì)心側(cè)偏角等重要參數(shù)。對(duì)于上述關(guān)鍵狀態(tài)參數(shù)的獲取，目前研究中用得較多的方法是軟測(cè)量，即基于算法進(jìn)行估計(jì)。

目前汽車(chē)狀態(tài)估計(jì)算法主要有線性卡爾曼濾波(KF)[1]、擴(kuò)展卡爾曼濾波(EKF)[2-3]、Unscented卡爾曼濾波(UKF)[4-5]、神經(jīng)網(wǎng)絡(luò)[6-7]、狀態(tài)觀測(cè)器[8]、模糊邏輯[9]等，這些方法都是用于對(duì)汽車(chē)控制系統(tǒng)中的關(guān)鍵控制變量(包括質(zhì)心側(cè)偏角、側(cè)向速度、橫擺角速度等)進(jìn)行估計(jì)與預(yù)測(cè)的。

上述算法中,KF僅適用于線性系統(tǒng)，EKF雖然適用于非線性系統(tǒng)，但其對(duì)非線性系統(tǒng)的近似簡(jiǎn)化降低了估計(jì)結(jié)果的精度，而UKF能很好地解決非線性系統(tǒng)濾波估計(jì)的問(wèn)題。但從濾波參數(shù)方面進(jìn)行考慮，上述常規(guī)算法中，過(guò)程噪聲和觀測(cè)噪聲的統(tǒng)計(jì)特性在濾波過(guò)程中根據(jù)先驗(yàn)值預(yù)先設(shè)為定值，若噪聲水平改變，將會(huì)使得估計(jì)精度降低甚至導(dǎo)致濾波發(fā)散。

為了提高基于非線性汽車(chē)動(dòng)力學(xué)模型狀態(tài)估計(jì)算法的魯棒性與估計(jì)精度，本文提出一種基于蟻群優(yōu)化算法的UKF狀態(tài)估計(jì)自適應(yīng)濾波算法，該算法將蟻群算法與UKF算法進(jìn)行巧妙結(jié)合，運(yùn)用蟻群算法的尋優(yōu)功能，通過(guò)適當(dāng)?shù)剡x擇目標(biāo)函數(shù)對(duì)過(guò)程噪聲和觀測(cè)噪聲的協(xié)方差矩陣進(jìn)行尋優(yōu)運(yùn)算，實(shí)現(xiàn)了算法的自適應(yīng)，提高了算法的魯棒性和估計(jì)精度。

1　汽車(chē)的非線性動(dòng)力學(xué)模型

考慮沿x軸的縱向運(yùn)動(dòng)、沿y軸的側(cè)向運(yùn)動(dòng)以及沿z軸的橫擺運(yùn)動(dòng)，建立包括橫擺、側(cè)向、縱向在內(nèi)的狀態(tài)量相互耦合的三自由度非線性模型，限定汽車(chē)側(cè)向加速度在0.4g以下，這樣輪胎的側(cè)偏特性處在線性范圍，且假設(shè)驅(qū)動(dòng)力不大，不考慮地面切向力對(duì)輪胎側(cè)偏特性的影響，忽略左右輪輪胎由于載荷的變化而引起輪胎特性的變化以及回正力矩的作用，沒(méi)有空氣動(dòng)力的作用，所得模型如圖1所示[10]。

圖1　三自由度汽車(chē)動(dòng)力學(xué)模型

汽車(chē)的運(yùn)動(dòng)微分方程為

(1)

(2)

(3)

(4)

式中，m為整車(chē)質(zhì)量；δ為前輪轉(zhuǎn)角；ax、ay分別表示質(zhì)心處縱向加速度和側(cè)向加速度；β為質(zhì)心側(cè)偏角；ωr為橫擺角速度；a、b為質(zhì)心分別到前后軸的距離；vx、vy為質(zhì)心處縱向速度和側(cè)向速度；k1、k2分別為前后軸側(cè)偏剛度。

將上述方程整理后可得如下?tīng)顟B(tài)方程、觀測(cè)方程：

(5)

(6)

式中，Iz為整車(chē)對(duì)z軸的轉(zhuǎn)動(dòng)慣量。

2　蟻群優(yōu)化UKF算法

2.1蟻群優(yōu)化算法

蟻群算法是意大利學(xué)者Dorigo受螞蟻覓食時(shí)的路徑選擇行為啟發(fā)提出的。螞蟻通過(guò)行走不同的地點(diǎn)轉(zhuǎn)移，t時(shí)刻螞蟻從位置i向位置j的轉(zhuǎn)移概率Mij(t)為[11-12]：

(7)

式中，τij(t)為t時(shí)刻螞蟻i鄰域內(nèi)的信息素軌跡強(qiáng)度；ηij為螞蟻由位置i轉(zhuǎn)移到位置j的啟發(fā)程度，也稱(chēng)為能見(jiàn)度；r為螞蟻允許到達(dá)的位置；A為螞蟻下一步可以選擇的位置集合；α表示軌跡的相對(duì)重要性，反映蟻群在運(yùn)動(dòng)過(guò)程中所積累的信息對(duì)蟻群整體運(yùn)動(dòng)的影響，該數(shù)值越大，該螞蟻越傾向于選擇其他螞蟻經(jīng)過(guò)的路徑；γ表示能見(jiàn)度的相對(duì)重要性。

由式(7)可知，轉(zhuǎn)移概率與ηij和τij成正比。同時(shí)在尋優(yōu)過(guò)程中，信息素是可以揮發(fā)的，定義ρ∈[0,1)為信息素殘留系數(shù)，表示信息素物質(zhì)的持久性，則1-ρ為信息素的揮發(fā)度。一次循環(huán)完成后，t+1時(shí)刻蟻群在所移動(dòng)路徑上的信息素強(qiáng)度τzδ(t+1)按照下式進(jìn)行更新調(diào)整:

(8)

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

2.2UKF算法

(1)均值和方差的初始化：

(9)

(10)

(2)計(jì)算Sigma點(diǎn)。根據(jù)式(9)和式(10)產(chǎn)生2n+1個(gè)Sigma點(diǎn)組成的矩陣χk：

χk=

(11)

k=0,1,2,…

式中，λ為調(diào)節(jié)參數(shù)。

(3)時(shí)間更新過(guò)程。由非線性系統(tǒng)狀態(tài)方程對(duì)各個(gè)Sigma點(diǎn)進(jìn)行非線性變換：

χk+1|k=f(χk|k,uk)

(12)

狀態(tài)預(yù)測(cè)：

(13)

方差陣預(yù)測(cè)：

(14)

由觀測(cè)方程對(duì)各Sigma點(diǎn)進(jìn)行非線性變換：

ψi,k+1|k=h(χi,k+1|k)

(15)

(4)觀測(cè)更新。求系統(tǒng)輸出的方差陣：

(ψi,k+1|k-yk+1|k)T+R

(16)

計(jì)算協(xié)方差陣Pxk+1|yk+1：

(17)

濾波增益：

(18)

方差更新：

(19)

狀態(tài)更新：

(20)

2.3用于UKF算法優(yōu)化的蟻群算法目標(biāo)函數(shù)確定

定義目標(biāo)函數(shù)如下：

(21)

3　蟻群優(yōu)化UKF算法用于汽車(chē)狀態(tài)估計(jì)的實(shí)現(xiàn)過(guò)程

根據(jù)UKF算法，結(jié)合汽車(chē)狀態(tài)方程，可以實(shí)現(xiàn)UKF對(duì)汽車(chē)狀態(tài)量進(jìn)行估計(jì)。加入蟻群優(yōu)化算法后，可以對(duì)過(guò)程噪聲協(xié)方差矩陣和觀測(cè)噪聲協(xié)方差矩陣進(jìn)行在線優(yōu)化，提高估計(jì)結(jié)果的精度，具體流程如圖2所示。

圖2　算法的實(shí)施流程

4　基于虛擬試驗(yàn)的算法驗(yàn)證

采用Carsim與Simulink聯(lián)合仿真平臺(tái)模擬典型過(guò)渡工況下的汽車(chē)操縱響應(yīng)。仿真模擬中采用某轎車(chē)的整車(chē)參數(shù)進(jìn)行建模，整車(chē)的參數(shù)如下：m=1100kg，Iz=1720kg·m2，a=1.22m，b=1.28m，k1=-1.6×105N/rad，k2=-1.8×105N/rad，試驗(yàn)工況為雙移線變道試驗(yàn)，試驗(yàn)車(chē)速為80km/h，采樣時(shí)間為0.02s。

狀態(tài)變量初始值為[0022.22]T，過(guò)程噪聲協(xié)方差矩陣初始值為Q=diag(1,1,0.1)，觀測(cè)噪聲協(xié)方差初始值為R=0.001。

首先基于常規(guī)UKF算法進(jìn)行濾波估計(jì)，給觀測(cè)量ay加入定值高斯白噪聲。估計(jì)結(jié)果的對(duì)比如圖3～圖5所示。

圖3　橫擺角速度估計(jì)值

圖4　質(zhì)心側(cè)偏角估計(jì)值

圖5　縱向車(chē)速估計(jì)值

通過(guò)圖3～圖5可以看出，在定值高斯白噪聲的干預(yù)下，常規(guī)UKF算法可以較好地跟蹤橫擺角速度、質(zhì)心側(cè)偏角和縱向車(chē)速，估計(jì)誤差最大偏差值約為4.2%以?xún)?nèi)。

為充分檢驗(yàn)算法的魯棒性與估計(jì)精度，給觀測(cè)量ay加入時(shí)變的高斯白噪聲。信息素殘留系數(shù)ρ=0.61。

綜合圖6～圖8可以看出，單純采用UKF算法時(shí)，由于過(guò)程噪聲和觀測(cè)噪聲在濾波過(guò)程中設(shè)定為定值，不會(huì)自動(dòng)更新。而試驗(yàn)過(guò)程中輸入的是時(shí)變的噪聲，因此橫擺角速度、質(zhì)心側(cè)偏角和縱向車(chē)速的估計(jì)值出現(xiàn)較大偏差，在幅值處偏差達(dá)最大，無(wú)法較準(zhǔn)確地跟蹤狀態(tài)量。而加入蟻群算法后，通過(guò)其尋優(yōu)的作用，根據(jù)設(shè)定的目標(biāo)函數(shù)對(duì)過(guò)程噪聲和觀測(cè)噪聲協(xié)方差矩陣進(jìn)行調(diào)整，使輸出誤差最小。通過(guò)綜合對(duì)比，蟻群優(yōu)化UKF算法的狀態(tài)估計(jì)值最優(yōu)能準(zhǔn)確地跟蹤汽車(chē)當(dāng)前狀態(tài)，說(shuō)明蟻群算法的加入可以彌補(bǔ)UKF算法的弱點(diǎn)，從而可以獲得更好的魯棒性與估計(jì)精度。

圖6　加入時(shí)變?cè)肼暫髾M擺角速度估計(jì)值

圖7　加入時(shí)變?cè)肼暫筚|(zhì)心側(cè)偏角估計(jì)值對(duì)比

圖8　加入時(shí)變?cè)肼暫罂v向車(chē)速估計(jì)值對(duì)比

通過(guò)計(jì)算分析，加入時(shí)變?cè)肼暫?，UKF算法的估計(jì)誤差最大達(dá)到了9.96%，而蟻群優(yōu)化UKF算法則將估計(jì)誤差縮小到了4.52%，與定值噪聲狀態(tài)下UKF算法估計(jì)精度相當(dāng)，有效地說(shuō)明了上述結(jié)論的正確性。

為了進(jìn)一步定量地比較兩種算法的估計(jì)精度，給出了估計(jì)值相對(duì)于實(shí)際值的平均絕對(duì)誤差(MAE)和均方根誤差(RSME)，如表1、表2所示。

表1　算法的MAE指標(biāo)

表2　算法的RSME指標(biāo)

由表1和表2可以看出，隨著算法的改進(jìn)以及蟻群算法的加入，橫擺角速度、質(zhì)心側(cè)偏角和縱向車(chē)速的MAE和RSME指標(biāo)明顯逐步變好，即呈現(xiàn)下降趨勢(shì)；表1、表2進(jìn)一步從量的對(duì)比上說(shuō)明蟻群優(yōu)化算法的優(yōu)越性，對(duì)汽車(chē)穩(wěn)定性控制系統(tǒng)的研究具有指導(dǎo)意義。

5　結(jié)語(yǔ)

(1)提出將蟻群算法與UKF相結(jié)合進(jìn)行汽車(chē)狀態(tài)估計(jì)，UKF算法可以針對(duì)非線性系統(tǒng)進(jìn)行狀態(tài)估計(jì)，但是不能適應(yīng)時(shí)變的過(guò)程噪聲和觀測(cè)噪聲，從而產(chǎn)生較大偏差;隨著蟻群算法的加入，其尋優(yōu)作用逐漸將估計(jì)誤差縮小，達(dá)到與定值噪聲狀態(tài)下估計(jì)精度相當(dāng)?shù)男Ч?/p>

(2)通過(guò)虛擬試驗(yàn)驗(yàn)證表明，蟻群優(yōu)化UKF算法的估計(jì)結(jié)果可以很好地跟蹤虛擬試驗(yàn)值，優(yōu)于單純的UKF算法，具有較好地估計(jì)精度及魯棒性。

后續(xù)研究將致力于蟻群算法的改進(jìn)，并應(yīng)用到更復(fù)雜的多自由度非線性汽車(chē)模型中，為汽車(chē)穩(wěn)定性控制系統(tǒng)的研究提供理論指導(dǎo)。

[1]Chen B C, Hsieh F C. Sideslip Angle Estimation Using Extended Kalman Filter[J]. Vehicle System Dynamics, 2008, 46(S): 353-364.

[2]李旭，宋翔，張為公. 基于擴(kuò)展卡爾曼濾波的車(chē)輛狀態(tài)可靠估計(jì)[J]. 東南大學(xué)學(xué)報(bào)(自然科學(xué)版)，2014，44 (4): 740-744.

Li Xu, Song Xiang, Zhang Weigong. Reliable Vehicle State Estimation Based on Improved Extended Kalman Filter[J].Journal of Southeast University(Natural Science Edition), 2014, 44(4): 740-744.

[3]宗長(zhǎng)富，胡丹，楊肖，等. 基于擴(kuò)展Kalman濾波的汽車(chē)行駛狀態(tài)估計(jì)[J]. 吉林大學(xué)學(xué)報(bào)(工學(xué)版)，2009，39(1): 7-11.

Zong Changfu, Hu Dan, Yang Xiao, et al. Vehicle Driving State Estimation Based on Extended Kalman Filter[J]. Journal of Jilin University(Engineering and Technology Edition), 2009, 39(1): 7-11.

[4]趙又群，林棻. 基于UKF算法的汽車(chē)狀態(tài)估計(jì)[J]. 中國(guó)機(jī)械工程，2010，21(5): 615-619.

Zhao Youqun, Lin Fen. Vehicle State Estimation Based on Unscented Kalman Filter Algorithm[J]. China Mechanical Engineering, 2010, 21(5): 615-619.

[5]Antonov S, Fehn A, Kugi A. Unscented Kalman Filter for Vehicle State Estimation[J].Vehicle System Dynamics, 2011, 49(9): 1497-1520.

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

Zhang Xiaolong, Li Liang, Li Hongzhi, et al. Experimental Research on Vehicle Sideslip Angle Estimation Based on Improved RBF Neural Networks[J].Journal of Mechanical Engineering, 2010, 46(22): 105-110.

[7]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): 2005-2019.

[8]Cheli F, Melzi S, Sabbioni E. An Adaptive Observer for Sideslip Angle Estimation: Comparison with Experimental Results[C]//Proceedings of the ASME 2007 International Design Engineering Technical Conferences & Computers and Information in Engineering Conference. Nevada: ASME,2007：1193-1199.

[9]施樹(shù)明，Lupker H，Bremmer P，等. 基于模糊邏輯的車(chē)輛側(cè)偏角估計(jì)方法[J]. 汽車(chē)工程, 2005, 27(4): 426-430.

Shi Shuming, Lupker H, Bremmer P, et al. Estimation of Vehicle Side Slip Angle Based on Fuzzy Logic[J]. Automotive Engineering, 2005,27(4):426-430.

[10]郭孔輝. 汽車(chē)操縱動(dòng)力學(xué)[M]. 長(zhǎng)春: 吉林科學(xué)技術(shù)出版社, 1991.

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

Zhang Jie, Zhang Peng, Liu Guobao. Two-stage Ant Colony Algorithm Based Job Shop Scheduling with Unrelated Parallel Machines[J].Journal of Mechanical Engineering, 2013,49(6): 136-144.

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

Liu Zhen, Hu Yunan. Multi-granularity Pattern Ant Colony Optimization Algorithm and Its Application in Path Planning[J]. Journal of Central South University(Science and Technology), 2013, 44(9): 3713-3722.

[13]劉永強(qiáng)，常青，熊華鋼. 改進(jìn)蟻群算法求解時(shí)變網(wǎng)絡(luò)中最短路徑問(wèn)題[J]. 北京航空航天大學(xué)學(xué)報(bào)，2009，35(10): 1245-1248.

Liu Yongqiang, Chang Qing, Xiong Huagang. Improved Ant Colony Algorithm for Shortest Path Problem in Time-dependent Networks[J]. Journal of Beijing University of Aeronautics and Astronautics, 2009,35(10): 1245-1248.

(編輯郭偉)

Vehicle State Estimation Based on Ant Colony Optimization Algorithm

Zhang Fengjiao1,2Wei Minxiang1Zhao Wanzhong1

1.Nanjing University of Aeronautics and Astronautics,Nanjing,210016 2.Changzhou Institute of Technology,Changzhou,213002

For time-varying characteristics of process noises and observation noise problems in vehicle state estimation, a new adaptive filtering algorithm was put forward. The new adaptive filtering algorithm was based on the 3-DOF nonlinear vehicle dynamics model, when UKF algorithm was applied to estimate vehicle state, the ant colony optimization algorithm was introduced at the same time. The adaptive functions of process noises and observation noises were achieved according to optimization principles based on objective function. The estimation precision increases greatly by using the new adaptive filtering algorithm. Robustness and accuracy of the ant colony optimization UKF algorithm were verified through the virtual experiments, the results will have important theoretical significance for the development of automobile active control system.

vehicle engineering; ant colony optimization algorithm; UKF algorithm; state estimation; virtual experiment

2014-11-03

國(guó)家自然科學(xué)基金資助項(xiàng)目(51375007,51005115,51205191)；常州市應(yīng)用基礎(chǔ)研究計(jì)劃資助項(xiàng)目(CJ20159011)

U461.6DOI：10.3969/j.issn.1004-132X.2015.22.011

張鳳嬌，女，1978年生。南京航空航天大學(xué)能源與動(dòng)力學(xué)院博士研究生，常州工學(xué)院機(jī)電與車(chē)輛工程學(xué)院講師。主要研究方向?yàn)槠?chē)系統(tǒng)動(dòng)力學(xué)。發(fā)表論文8篇。魏民祥，男，1963年生。南京航空航天大學(xué)能源與動(dòng)力學(xué)院教授、博士研究生導(dǎo)師。趙萬(wàn)忠，男，1982年生。南京航空航天大學(xué)能源與動(dòng)力學(xué)院副教授。