基于自適應卡爾曼算法的重型半掛車狀態估計研究*

張不揚 宗長富 王化吉 鄭宏宇

（吉林大學 汽車仿真與控制國家重點實驗室）

1 前言

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

針對重型半掛車的橫擺和折疊兩種常見穩定性問題，建立了用于估算橫擺角速度和半掛車折疊角的半掛車三軸簡化動力學模型，并用商業軟件Trucksim對簡化模型進行了驗證。

2 重型半掛車三軸簡化動力學模型

本文用來估算模型橫擺角速度和折疊角的動力學模型是重型半掛車三軸簡化動力學模型，該模型以前輪轉角作為輸入，模型假設如下：

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

b. 忽略車輛的俯仰運動；

c. 忽略空氣阻力作用；

d. 忽略輪胎特性隨負載不同發生的變化；

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

重型半掛車三軸簡化動力學模型如圖1所示，其中，r1、r2分別為牽引車和掛車的橫擺角速度，定義逆時針方向為其正方向，則r1、r2可表示為：

由于所用模型假設轉角輸入較小，則牽引車質心處的側向加速度為：

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

整車的側向力平衡方程如下：

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

牽引車質心處力矩平衡方程為：

鉸接點處得到的掛車力矩平衡方程如下：

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

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

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

3 自適應卡爾曼濾波算法

系統狀態方程與觀測方程為：

式中，x（k）與 x（k+1）為 k 與 k+1 時刻的狀態矢量；y（k+1）為 k+1 時刻的觀測矢量；V（k+1）為 k+1 時刻的觀測噪聲；H為觀測矩陣，H=[0 1 0 0]；W為系統噪聲。

自適應卡爾曼濾波算法分為3部分，即在原有的時間更新和測量更新中加入了具有自適應性的自適應更新部分。

a.濾波時間更新部分

狀態預測方程：

誤差協方差預測：

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

b.濾波測量更新部分

增益方程：

濾波方程：

誤差協方差更新：

c. 濾波自適應更新部分噪聲均值、自協方差矩陣估計計算：

4 仿真結果分析

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

表1 車輛模型參數

表2 車輪側偏剛度 （rad）－1

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

5 結束語

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

1 余卓平，高曉杰.車輛行駛過程中的狀態估計問題綜述.機械工程學報，2009，45（5）:20～33.

2 宗長富，胡丹，楊肖，潘釗.基于擴展卡爾曼濾波的汽車行駛狀態估計.吉林大學學報工學版，2009，39（1）:17～11.

3 胡丹.基于雙擴展卡爾曼濾波的汽車狀態及路面附著系數估計算法研究：[學位論文].長春：吉林大學，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算法及其改進算法的研究：[學位論文].長沙：中南大學，2009.

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

7 高越，高振海，李向瑜.基于自適應Kalman濾波的汽車橫擺角速度測量算法.江蘇大學學報，2005，26（1）:24～27.

8 鄧志紅，王躍鵬，付夢印，肖烜.激光陀螺漂移數據的自適應 Kalman 濾波.傳感器與微系統，2007，26（2）.

9 朱天軍.基于改進TTR重型車輛側翻預警及多目標穩定控制算法研究：[學位論文].長春：吉林大學，2009.