基于蟻群優化UKF算法的汽車狀態估計

張鳳嬌　魏民祥　趙萬忠

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

基于蟻群優化UKF算法的汽車狀態估計

張鳳嬌1,2魏民祥1趙萬忠1

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

針對汽車狀態估計中過程噪聲和觀測噪聲的時變特性，提出一種新的自適應濾波算法。該算法基于三自由度非線性汽車動力學模型，在利用UKF對汽車狀態量進行估計的同時，引入蟻群優化算法，根據目標函數對過程噪聲和觀測噪聲進行尋優，實現了過程噪聲和觀測噪聲的自適應作用，估計精度的大幅提高。虛擬實驗驗證了蟻群優化UKF算法的魯棒性和精度。研究結果對汽車主動控制系統的開發具有重大的理論指導意義。

車輛工程；蟻群優化算法；UKF算法；狀態估計；虛擬試驗

0　引言

隨著汽車工業的飛速發展，汽車主動安全技術越來越受到人們的關注。對汽車主動安全的研究幾乎都需要獲知汽車的當前行駛狀態，例如汽車的當前車速、側向加速度、橫擺角速度和質心側偏角等重要參數。對于上述關鍵狀態參數的獲取，目前研究中用得較多的方法是軟測量，即基于算法進行估計。

目前汽車狀態估計算法主要有線性卡爾曼濾波(KF)[1]、擴展卡爾曼濾波(EKF)[2-3]、Unscented卡爾曼濾波(UKF)[4-5]、神經網絡[6-7]、狀態觀測器[8]、模糊邏輯[9]等，這些方法都是用于對汽車控制系統中的關鍵控制變量(包括質心側偏角、側向速度、橫擺角速度等)進行估計與預測的。

上述算法中,KF僅適用于線性系統，EKF雖然適用于非線性系統，但其對非線性系統的近似簡化降低了估計結果的精度，而UKF能很好地解決非線性系統濾波估計的問題。但從濾波參數方面進行考慮，上述常規算法中，過程噪聲和觀測噪聲的統計特性在濾波過程中根據先驗值預先設為定值，若噪聲水平改變，將會使得估計精度降低甚至導致濾波發散。

為了提高基于非線性汽車動力學模型狀態估計算法的魯棒性與估計精度，本文提出一種基于蟻群優化算法的UKF狀態估計自適應濾波算法，該算法將蟻群算法與UKF算法進行巧妙結合，運用蟻群算法的尋優功能，通過適當地選擇目標函數對過程噪聲和觀測噪聲的協方差矩陣進行尋優運算，實現了算法的自適應，提高了算法的魯棒性和估計精度。

1　汽車的非線性動力學模型

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

圖1　三自由度汽車動力學模型

汽車的運動微分方程為

(1)

(2)

(3)

(4)

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

將上述方程整理后可得如下狀態方程、觀測方程：

(5)

(6)

式中，Iz為整車對z軸的轉動慣量。

2　蟻群優化UKF算法

2.1蟻群優化算法

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

(7)

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

由式(7)可知，轉移概率與ηij和τij成正比。同時在尋優過程中，信息素是可以揮發的，定義ρ∈[0,1)為信息素殘留系數，表示信息素物質的持久性，則1-ρ為信息素的揮發度。一次循環完成后，t+1時刻蟻群在所移動路徑上的信息素強度τzδ(t+1)按照下式進行更新調整:

(8)

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

2.2UKF算法

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

(9)

(10)

(2)計算Sigma點。根據式(9)和式(10)產生2n+1個Sigma點組成的矩陣χk：

χk=

(11)

k=0,1,2,…

式中，λ為調節參數。

(3)時間更新過程。由非線性系統狀態方程對各個Sigma點進行非線性變換：

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

(12)

狀態預測：

(13)

方差陣預測：

(14)

由觀測方程對各Sigma點進行非線性變換：

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

(15)

(4)觀測更新。求系統輸出的方差陣：

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

(16)

計算協方差陣Pxk+1|yk+1：

(17)

濾波增益：

(18)

方差更新：

(19)

狀態更新：

(20)

2.3用于UKF算法優化的蟻群算法目標函數確定

定義目標函數如下：

(21)

3　蟻群優化UKF算法用于汽車狀態估計的實現過程

根據UKF算法，結合汽車狀態方程，可以實現UKF對汽車狀態量進行估計。加入蟻群優化算法后，可以對過程噪聲協方差矩陣和觀測噪聲協方差矩陣進行在線優化，提高估計結果的精度，具體流程如圖2所示。

圖2　算法的實施流程

4　基于虛擬試驗的算法驗證

采用Carsim與Simulink聯合仿真平臺模擬典型過渡工況下的汽車操縱響應。仿真模擬中采用某轎車的整車參數進行建模，整車的參數如下：m=1100kg，Iz=1720kg·m2，a=1.22m，b=1.28m，k1=-1.6×105N/rad，k2=-1.8×105N/rad，試驗工況為雙移線變道試驗，試驗車速為80km/h，采樣時間為0.02s。

狀態變量初始值為[0022.22]T，過程噪聲協方差矩陣初始值為Q=diag(1,1,0.1)，觀測噪聲協方差初始值為R=0.001。

首先基于常規UKF算法進行濾波估計，給觀測量ay加入定值高斯白噪聲。估計結果的對比如圖3～圖5所示。

圖3　橫擺角速度估計值

圖4　質心側偏角估計值

圖5　縱向車速估計值

通過圖3～圖5可以看出，在定值高斯白噪聲的干預下，常規UKF算法可以較好地跟蹤橫擺角速度、質心側偏角和縱向車速，估計誤差最大偏差值約為4.2%以內。

為充分檢驗算法的魯棒性與估計精度，給觀測量ay加入時變的高斯白噪聲。信息素殘留系數ρ=0.61。

綜合圖6～圖8可以看出，單純采用UKF算法時，由于過程噪聲和觀測噪聲在濾波過程中設定為定值，不會自動更新。而試驗過程中輸入的是時變的噪聲，因此橫擺角速度、質心側偏角和縱向車速的估計值出現較大偏差，在幅值處偏差達最大，無法較準確地跟蹤狀態量。而加入蟻群算法后，通過其尋優的作用，根據設定的目標函數對過程噪聲和觀測噪聲協方差矩陣進行調整，使輸出誤差最小。通過綜合對比，蟻群優化UKF算法的狀態估計值最優能準確地跟蹤汽車當前狀態，說明蟻群算法的加入可以彌補UKF算法的弱點，從而可以獲得更好的魯棒性與估計精度。

圖6　加入時變噪聲后橫擺角速度估計值

圖7　加入時變噪聲后質心側偏角估計值對比

圖8　加入時變噪聲后縱向車速估計值對比

通過計算分析，加入時變噪聲后，UKF算法的估計誤差最大達到了9.96%，而蟻群優化UKF算法則將估計誤差縮小到了4.52%，與定值噪聲狀態下UKF算法估計精度相當，有效地說明了上述結論的正確性。

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

表1　算法的MAE指標

表2　算法的RSME指標

由表1和表2可以看出，隨著算法的改進以及蟻群算法的加入，橫擺角速度、質心側偏角和縱向車速的MAE和RSME指標明顯逐步變好，即呈現下降趨勢；表1、表2進一步從量的對比上說明蟻群優化算法的優越性，對汽車穩定性控制系統的研究具有指導意義。

5　結語

(1)提出將蟻群算法與UKF相結合進行汽車狀態估計，UKF算法可以針對非線性系統進行狀態估計，但是不能適應時變的過程噪聲和觀測噪聲，從而產生較大偏差;隨著蟻群算法的加入，其尋優作用逐漸將估計誤差縮小，達到與定值噪聲狀態下估計精度相當的效果。

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

后續研究將致力于蟻群算法的改進，并應用到更復雜的多自由度非線性汽車模型中，為汽車穩定性控制系統的研究提供理論指導。

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

[2]李旭，宋翔，張為公. 基于擴展卡爾曼濾波的車輛狀態可靠估計[J]. 東南大學學報(自然科學版)，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]宗長富，胡丹，楊肖，等. 基于擴展Kalman濾波的汽車行駛狀態估計[J]. 吉林大學學報(工學版)，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算法的汽車狀態估計[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]張小龍，李亮，李紅志，等. 基于改進RBF網的汽車側偏角估計方法試驗研究[J]. 機械工程學報，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]施樹明，Lupker H，Bremmer P，等. 基于模糊邏輯的車輛側偏角估計方法[J]. 汽車工程, 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]郭孔輝. 汽車操縱動力學[M]. 長春: 吉林科學技術出版社, 1991.

[11]張潔，張朋，劉國寶. 基于兩階段蟻群算法的帶非等效并行機的作業車間調度[J]. 機械工程學報，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]劉振，胡云安. 一種多粒度模式蟻群算法及其在路徑規劃中的應用[J]. 中南大學學報(自然科學版)，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]劉永強，常青，熊華鋼. 改進蟻群算法求解時變網絡中最短路徑問題[J]. 北京航空航天大學學報，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

國家自然科學基金資助項目(51375007,51005115,51205191)；常州市應用基礎研究計劃資助項目(CJ20159011)

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

張鳳嬌，女，1978年生。南京航空航天大學能源與動力學院博士研究生，常州工學院機電與車輛工程學院講師。主要研究方向為汽車系統動力學。發表論文8篇。魏民祥，男，1963年生。南京航空航天大學能源與動力學院教授、博士研究生導師。趙萬忠，男，1982年生。南京航空航天大學能源與動力學院副教授。