基于自適應(yīng)卡爾曼算法的重型半掛車狀態(tài)估計(jì)研究*

張不揚(yáng) 宗長(zhǎng)富 王化吉 鄭宏宇

（吉林大學(xué) 汽車仿真與控制國(guó)家重點(diǎn)實(shí)驗(yàn)室）

1 前言

重型半掛車主動(dòng)安全控制常用的估計(jì)算法包括卡爾曼濾波法、龍貝格觀測(cè)器、魯棒觀測(cè)器、滑模觀測(cè)器、模糊觀測(cè)器、非線性觀測(cè)器和神經(jīng)網(wǎng)絡(luò)算法等。其中，經(jīng)典卡爾曼濾波算法最為常用[1]，但其只適用于線性系統(tǒng)。文獻(xiàn)[2]和文獻(xiàn)[3]中通過(guò)擴(kuò)展卡爾曼濾波進(jìn)行了行駛狀態(tài)估計(jì)，文獻(xiàn)[4]中使用雙無(wú)軌卡爾曼濾波計(jì)算車輛狀態(tài)及路面附著系數(shù)。這些濾波算法雖然解決了非線性系統(tǒng)的問題，但仍存在濾波器性能下降、難以適應(yīng)復(fù)雜的環(huán)境變化及受觀測(cè)噪聲統(tǒng)計(jì)特性影響較大[5]等缺點(diǎn)。針對(duì)上述問題，一些學(xué)者提出了自適應(yīng)卡爾曼濾波算法，該方法能夠不斷計(jì)算、更換噪聲協(xié)方差，增強(qiáng)算法準(zhǔn)確性[6，7]。

針對(duì)重型半掛車的橫擺和折疊兩種常見穩(wěn)定性問題，建立了用于估算橫擺角速度和半掛車折疊角的半掛車三軸簡(jiǎn)化動(dòng)力學(xué)模型，并用商業(yè)軟件Trucksim對(duì)簡(jiǎn)化模型進(jìn)行了驗(yàn)證。

2 重型半掛車三軸簡(jiǎn)化動(dòng)力學(xué)模型

本文用來(lái)估算模型橫擺角速度和折疊角的動(dòng)力學(xué)模型是重型半掛車三軸簡(jiǎn)化動(dòng)力學(xué)模型，該模型以前輪轉(zhuǎn)角作為輸入，模型假設(shè)如下：

a. 牽引車速度與掛車速度相等且恒定；

b. 忽略車輛的俯仰運(yùn)動(dòng)；

c. 忽略空氣阻力作用；

d. 忽略輪胎特性隨負(fù)載不同發(fā)生的變化；

e. 忽略輪胎回正力矩作用。

重型半掛車三軸簡(jiǎn)化動(dòng)力學(xué)模型如圖1所示，其中，r1、r2分別為牽引車和掛車的橫擺角速度，定義逆時(shí)針方向?yàn)槠湔较?，則r1、r2可表示為：

由于所用模型假設(shè)轉(zhuǎn)角輸入較小，則牽引車質(zhì)心處的側(cè)向加速度為：

重型汽車模型車輪側(cè)偏角可由公式 （5）～公式（7）得到：

整車的側(cè)向力平衡方程如下：

（m1+m2）（v˙+ur）－m2（c+d）r˙－m2dq˙

牽引車質(zhì)心處力矩平衡方程為：

鉸接點(diǎn)處得到的掛車力矩平衡方程如下：

公式中，m1為牽引車質(zhì)量；m2為半掛車質(zhì)量；J1為牽引車轉(zhuǎn)動(dòng)慣量；J2為半掛車轉(zhuǎn)動(dòng)慣量；γ為牽引車與半掛車的鉸接角；q為鉸接角速度；q˙為鉸接角加速度；u為牽引車縱向速度；v為側(cè)向速度；ψ為牽引車橫擺角；a為牽引車轉(zhuǎn)向軸到質(zhì)心的距離；b為牽引車驅(qū)動(dòng)軸到質(zhì)心距離；c為鉸接點(diǎn)到牽引車質(zhì)心的距離；d為鉸接點(diǎn)到半掛車質(zhì)心的距離；e為半掛車車軸到半掛車質(zhì)心的距離；C1、C2、C3分別為車輛三軸車輪輪胎側(cè)偏剛度。

令x=[v r q γ]T，將上述微分方程整理成狀態(tài)空間形式：

模型建立后，借助Trucksim軟件進(jìn)行模型驗(yàn)證，在車速為60 km/h掃頻工況下，動(dòng)力學(xué)模型輸出與Trucksim仿真結(jié)果對(duì)比如圖2和圖3所示，可見此模型能夠描述車輛的橫擺運(yùn)動(dòng)。

3 自適應(yīng)卡爾曼濾波算法

系統(tǒng)狀態(tài)方程與觀測(cè)方程為：

式中，x（k）與 x（k+1）為 k 與 k+1 時(shí)刻的狀態(tài)矢量；y（k+1）為 k+1 時(shí)刻的觀測(cè)矢量；V（k+1）為 k+1 時(shí)刻的觀測(cè)噪聲；H為觀測(cè)矩陣，H=[0 1 0 0]；W為系統(tǒng)噪聲。

自適應(yīng)卡爾曼濾波算法分為3部分，即在原有的時(shí)間更新和測(cè)量更新中加入了具有自適應(yīng)性的自適應(yīng)更新部分。

a.濾波時(shí)間更新部分

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

誤差協(xié)方差預(yù)測(cè)：

式中，G=M－1A；T=M－1B；ε 是中間變量；b 為 0～1 之間的數(shù)值，為遺忘因子，能夠限制濾波的記憶長(zhǎng)度。

b.濾波測(cè)量更新部分

增益方程：

濾波方程：

誤差協(xié)方差更新：

c. 濾波自適應(yīng)更新部分噪聲均值、自協(xié)方差矩陣估計(jì)計(jì)算：

4 仿真結(jié)果分析

本文采用Trucksim中的半掛車模型運(yùn)算數(shù)據(jù)與自適應(yīng)卡爾曼濾波算法估計(jì)值進(jìn)行對(duì)比，以驗(yàn)證自適應(yīng)卡爾曼濾波算法的有效性。自適應(yīng)卡爾曼濾波狀態(tài)估計(jì)流程如圖4所示。車輛模型參數(shù)如表1和表2所列。車速分別為30 km/h與60 km/h，正弦掃頻轉(zhuǎn)向輸入工況如圖5、圖6所示。由自適應(yīng)卡爾曼濾波估計(jì)得到車速為30 km/h與60 km/h掃頻工況下的橫擺角速度、鉸接角與Trucksim仿真得到的橫擺角速度、鉸接角對(duì)比曲線分別如圖7～圖9、圖10～圖12所示。

表1 車輛模型參數(shù)

表2 車輪側(cè)偏剛度 （rad）－1

由圖7～圖9可以看出，30 km/h掃頻工況下的系統(tǒng)估計(jì)結(jié)果輸出重合度很高。圖12顯示，盡管60 km/h掃頻工況下估計(jì)結(jié)果出現(xiàn)了誤差，但與仿真結(jié)果趨勢(shì)一致，滿足估計(jì)要求。誤差的出現(xiàn)是由本文所建立重型半掛車三軸簡(jiǎn)化動(dòng)力學(xué)模型造成，即模型在低速非線性工況下能夠較好的描述車輛的實(shí)際運(yùn)動(dòng)狀況，但在高速非線性工況下，模型的輸出結(jié)果與車輛的實(shí)際狀況有一定差別。通過(guò)所設(shè)計(jì)的狀態(tài)估計(jì)器，結(jié)合該模型對(duì)車輛高速非線性工況時(shí)的狀態(tài)估計(jì)，雖然仍舊存在一定偏差，但是誤差結(jié)果在可接受范圍內(nèi)，滿足估計(jì)精度要求。

5 結(jié)束語(yǔ)

本文所提自適應(yīng)卡爾曼濾波算法，克服了普通卡爾曼算法可能出現(xiàn)的估計(jì)誤差，提高了估計(jì)精度。通過(guò)與Trucksim的仿真數(shù)據(jù)對(duì)比可以看出，本算法能夠很好的計(jì)算出正弦掃頻工況下牽引車、掛車的橫擺角速度和兩者之間的鉸接角，實(shí)現(xiàn)了以低成本獲得車輛實(shí)時(shí)橫擺角速度與鉸接角的狀態(tài)估計(jì)。

1 余卓平，高曉杰.車輛行駛過(guò)程中的狀態(tài)估計(jì)問題綜述.機(jī)械工程學(xué)報(bào)，2009，45（5）:20～33.

2 宗長(zhǎng)富，胡丹，楊肖，潘釗.基于擴(kuò)展卡爾曼濾波的汽車行駛狀態(tài)估計(jì).吉林大學(xué)學(xué)報(bào)工學(xué)版，2009，39（1）:17～11.

3 胡丹.基于雙擴(kuò)展卡爾曼濾波的汽車狀態(tài)及路面附著系數(shù)估計(jì)算法研究：[學(xué)位論文].長(zhǎng)春：吉林大學(xué)，2009.

4 LAURA R Ray.Nonlinear start and tire force estimation for advanced vehicle control.IEEE Transaction on Control System Technology，1995，3（1）:117～124.

5 劉錚.UKF算法及其改進(jìn)算法的研究：[學(xué)位論文].長(zhǎng)沙：中南大學(xué)，2009.

6 KOEHN P， ECKRICH M.Active steering—The BMW approach towards modern steering technology.SAE Paper 2004－01－1105.

7 高越，高振海，李向瑜.基于自適應(yīng)Kalman濾波的汽車橫擺角速度測(cè)量算法.江蘇大學(xué)學(xué)報(bào)，2005，26（1）:24～27.

8 鄧志紅，王躍鵬，付夢(mèng)印，肖烜.激光陀螺漂移數(shù)據(jù)的自適應(yīng) Kalman 濾波.傳感器與微系統(tǒng)，2007，26（2）.

9 朱天軍.基于改進(jìn)TTR重型車輛側(cè)翻預(yù)警及多目標(biāo)穩(wěn)定控制算法研究：[學(xué)位論文].長(zhǎng)春：吉林大學(xué)，2009.