基于雙自適應無跡卡爾曼濾波的半掛車狀態估計

周兵，李濤，吳曉建，雷富強

（1.湖南大學汽車車身先進設計制造國家重點實驗室，湖南長沙 410082；2.南昌大學機電工程學院，江西南昌 330031）

車輛狀態信息對車輛的穩定性控制、主動安全控制，乃至無人駕駛控制有十分重要的意義［1］.實際應用中，輪速和加速度等車輛狀態參量，可通過車載傳感器直接測量，但諸如質心側偏角和鉸接角等狀態量，或難以直接測量，或需昂貴傳感器才能測量［2-3］.為解決上述問題，利用經濟性好的車載傳感器、結合車輛動力學和運動學原理、運用智能算法估計出所需狀態量的“軟測量”技術得到重視［4］.

車輛狀態估計常用的算法主要有卡爾曼濾波（KF）［5-6］、擴展卡爾曼濾波（EKF）［7-9］、無跡卡爾曼濾波（UKF）［10-11］、神經網絡［12-14］和深度學習［15］等.通過分析上述狀態估計算法，經典卡爾曼濾波算法（KF）在車輛表現出復雜非線性時的估計精度低，具有局限性；擴展卡爾曼濾波算法（EKF）可針對性改善非線性系統的估計問題，但仍只是近似處理，其雅可比矩陣和協方差矩陣計算復雜；無跡卡爾曼濾波（UKF）能很好地適用于非線性系統，但傳統的UKF算法，其過程噪聲和測量噪聲的統計特性預先設為定值，與實際不符，且定值化處理會降低估計精度，甚至導致濾波發散.針對過程噪聲和測量噪聲統計特性的問題，周聰［16］利用基于虛擬噪聲補償技術的非線性自適應濾波算法對汽車的行駛狀態進行估計.文獻［17］考慮在不同路面激勵水平情況下，定義了不同的噪聲協方差，提出了自適應無跡卡爾曼濾波狀態參數估計算法，該算法能夠適應不同路面的變化，具有較強的穩定性.張鳳嬌等［18］引入蟻群優化算法對汽車狀態進行估計，降低噪聲的干擾，提高了估計精度.周衛琪等［19］結合UKF 算法與遺傳算法對汽車狀態參數進行估計，實現了噪聲的自適應調節，有效提高了估計精度.張一西等［20］提出一種基于蟻獅算法的無跡卡爾曼濾波狀態參數估計器，采用蟻獅優化算法（ALO）對噪聲協方差矩陣進行尋優，提高估計的精度.需要說明的是，目前對于帶有掛車的車輛進行狀態估計的資料較少，張不揚等［21］在傳統UKF 基礎上，加入了具有自適應性作用的遺忘因子，對重型半掛車橫擺角速度和鉸接角進行了估計.

以上文獻對車輛狀態估計做了積極的研究工作，但在以下幾方面有待繼續深入：

1）針對帶有掛車這類復雜鉸接車輛的狀態估計，現有研究較少聚焦，尤其是考慮牽引-半掛車高維自由度、強非線性特性的狀態估計.

2）上述估計方法都以UKF 為基礎，屬于無限增長記憶型濾波，在進行k時刻的最優估計時，要用到k時刻之前的所有數據，因此隨著時間的推移，容易造成誤差累積，可能導致濾波發散［22］.

3）上述方法對估計初值要求較高，當初值不準確時，可能導致估計效果不佳，而在斷電再恢復的情形，這種情況更加明顯.

4）許多狀態估計是在標準工況下進行，當出現ABS 控制制動和大轉角等特殊的情形，由于輪速變化劇烈，對應的輪速傳感器所測值誤差大，此時很難實現準確的狀態估計.

本文以半掛車輛為研究對象，針對狀態估計過程中存在的上述累計誤差、初值敏感、ABS 介入等問題，提出一種雙自適應無跡卡爾曼濾（FFUKF）估計算法，可對強非線性動力學系統同時實時估計出其縱向/側向速度、橫擺角速度和鉸接角，且比普通模糊自適應無跡卡爾曼（FUKF）有更高的精度.

1 車輛動力學模型建立

1.1 整車模型

因本文關注半掛車縱向/側向速度、橫擺角速度以及鉸接角等狀態量的估計，且使之適用于ABS 控制工況，故分別建立牽引車和掛車的縱向、側向和橫擺自由度，以及6 個車輪的旋轉自由度.根據上述情況，建立了掛車和牽引車的12 自由度車輛模型，車輛動力學分析俯視示意圖如圖1所示.

圖1 12自由度半掛車動力學模型Fig.1 Twelve-degree-of-freedom dynamic model of semi-trailer

車輛運動微分方程如下：

（1）牽引車模型

縱向運動方程：

側向運動方程：

橫擺運動方程：

（2）半掛車模型

縱向運動方程：

側向運動方程：

橫擺運動方程：

（3）鉸接點受力關系

牽引車和半掛車通過鞍座鉸接，鉸接點所受到的縱向力與橫向力的關系如圖2所示.

圖2 鉸接點受力圖Fig.2 Force diagram of hinge point

牽引點的縱向力和側向力的坐標變換關系如下：

（4）6個車輪的力矩平衡方程

上列各式中，下標1、2 分別代表牽引車和掛車；fl、fr、rl、rr、l2、r2 分別代表6 個車輪.各符號含義見表1.

表1 符號含義Tab.1 Meaning of symbols

1.2 輪胎模型

魔術公式輪胎模型具有較高的擬合精度、良好的魯棒性和極限狀況下較高的置信度，并以一套形式相同的公式就可以描述等優點，被廣泛使用［23］.本文采取“魔術公式”輪胎模型來求取輪胎力，模型對應關系如圖3所示，其表達式為：

圖3 魔術輪胎模型Fig.3 Magic tire model

式中：X為輸入變量；Y為輸出變量；B為剛度系數；C為曲線形狀系數，控制了正弦函數的范圍，故決定了曲線的形狀；D為曲線峰值；E控制了曲線峰值的曲率.具體各系數的值與輪胎載荷、車輪外傾角及路面附著條件等因素有關.

各輪滑移率通過下式得到：

側偏角通過下式得到：

式中，κ(ij)為各車輪的滑移率；α(ij)為各車輪的側偏角；v(ij)為各輪心速度.

2 雙自適應UKF設計

本文的估計量為掛車與牽引車的縱向速度、側向速度、橫擺角速度以及掛車與牽引車的鉸接角.為精確估計，選取縱向、側向加速度和橫擺角加速度為測量量，以方向盤轉角和車輪輸入力矩為控制輸入.依據狀態估計的方法，將上述整車動力學模型狀態和測量方程寫成標準形式為：

狀態向量：x(k)=[vx，vy，γ，vx2，vy2，γ2，θ]T

測量向量：

z(k)=[ax，ay，γ，ax2，ay2，γ2，ωfl，ωfr，ωrl，ωrr，ωl2，ωr2]T

控制輸入：u(k)=[δ，Tfl，Tfr，Trl，Trr，Tl2，Tr2]T

式中：x（k）和z（k）分別為系統狀態向量和測量向量；u（k）為輸入向量；W（k）和V（k）分別為過程噪聲和測量噪聲；f和h分別為非線性狀態函數和觀測函數；Tfl、Tfr、Trl、Trr、Tl2、Tr2對應6 個車輪的轉矩輸入，具體通過下式獲得：

2.1 模糊自適應UKF估計算法

2.1.1 UKF算法

無跡卡爾曼（Unscented Kalman Filter，UKF）在卡爾曼線性濾波框架的基礎上，采用無跡變換處理均值和協方差的非線性傳遞問題，在估計點附近確定采樣點，用這些樣本點來逼近狀態的后驗概率密度，這樣是對非線性函數的概率密度分布進行近似，所得到的均值和協方差至少具有泰勒展開式的2 階精度［24］，具體無跡卡爾曼濾波方法見圖4.

圖4 UKF流程圖Fig.4 The flow chart of UKF

2.1.2 噪聲自適應模糊算法

實際車輛在行駛過程中，外界情況復雜且多變，測量噪聲協方差陣R是變量，需要動態調整［25］.可通過模糊控制動態調整R，使狀態估計結果更為準確.

計算卡爾曼濾波協方差的理論值：

計算實際殘差的協方差：

式中：M為平滑窗口.

確定模糊控制輸入輸出：

式中：tr［］為矩陣的跡.

定義輸入輸出模糊集：

輸入：e={NB，NS，Z，PS，PB}

ec={NB，NS，Z，PS，PB}

輸出：ε={NB，NM，NS，Z，PS，PM，PB}

建立模糊規則如表2所示

表2 測量噪聲模糊規則Tab.2 Fuzzy rules of measuring noise

當無跡卡爾曼協方差越準確，殘差實際值與理論值之差ek越接近0，相反，其絕對值越大.故將ek和其變化率eck定義為模糊控制輸入，定義ε為模糊控制輸出，再通過下式得到自適應后的測量噪聲協方差陣Rf.

通過動態調整測量噪聲協方差陣Rf，使UKF 中系統預測的協方差動態調整，進而完成自適應濾波功能，達到使狀態估計更準確和穩定的目的.

2.2 車輪等效車速估計算法

車輛在正常行駛中，車輪做純滾動的情況占居較大的比例，且目前車輪轉速的測量較準確.故這里通過模糊控制自適應判斷穩定單輪，以車輪轉速為輸入，利用穩定輪轉速，等效計算出整車車速.

2.2.1 等效車速計算

當車輪處于穩定純滾動狀態時，車輪輪速與車速存在運動學關系.分別利用各純滾動輪的輪速，計算出相應的車輛縱向速度，再利用最小二乘法計算得到等效車輛縱向速度，計算如下：

式中：λmin為最小殘差平方和；為等效車輛縱向速度；為各車輪處速度；R為各車輪滾動半徑.

2.2.2 車輪穩定性判斷

上述為車輪純滾動計算得到的車輛速度，實際上在緊急制動和加速等工況車輪并非純滾動，需對其進行穩定性判斷，篩選出穩定的車輪再用2.2.1 計算等效車速.這里采用基于車輪速度和加速度兩種方法，綜合判斷各個車輪的穩定狀態，同時引入模糊控制自適應調節，提高估計器的適應性.

（1）基于速度判斷

以測量的車輪轉速為輸入，通過汽車動力學關系求得輪胎的滑移率，依據各車輪的滑移率是否小于滑移率容差來判斷車輪是否做純滾動運動.

式中，v(ij)為車輛輪心速度；為滑移率容差.

（2）基于加速度判斷

以測量的車輪轉速和車輛加速度為輸入，通過車輪轉速算出車輛的等效加速度，與（1）類似，該值與傳感器測得的車輛加速度求容差，判斷各車輪是否做純滾動運動.

式中：ax(ij)為等效的車輛加速度；為測量的車輛加速度；為加速度容差.

2.2.3 容差自適應模糊算法

同樣由于實際外界情況復雜，不同附著路面、急劇的轉向、驅動與制動等對（1）（2）影響巨大［26］，故通過模糊控制原理動態調整容差，以適應在ABS 等復雜工況，提高狀態估計的準確性.

（1）確定模糊控制輸入輸出

這里以車輛轉角δ及其斜率δc和車輪轉矩T(ij)及其斜率Tc(ij)為模糊控制輸入，以車輪滑移率容差和車輛加速度容差為模糊控制輸出.

（2）定義輸入輸出模糊集

輸入：δ={NB，NS，Z，PS，PB}

δc={NB，NS，Z，PS，PB}

T(ij)={NB，NS，Z，PS，PB}

Tc(ij)={NB，NS，Z，PS，PB}

（3）建立模糊規則，如表3所示.

表3 容差模糊規則Tab.3 Fuzzy rules of tolerance

在急劇的轉角和驅動與制動的情況下，適當的減小容差，以提高判斷車輪穩定的準確性，提高估計器的精度和適應性.本估計器后續可擴展估計出路面附著系數，該值亦可做為模糊控制的輸入，進一步改善估計效果.

2.3 雙自適應UKF算法

在基于半掛汽車的12 自由度非線性動力學模型的基礎上，通過測量的輪速、車輛加速度和方向盤轉角等信息，首先利用模糊控制自適應調整速度和加速度容差，綜合判斷篩選出穩定狀態的車輪，結合動力學原理，通過輪速算出等效縱向車速，此為估計器1；然后模糊控制自適應調整測量噪聲，利用無跡卡爾曼估計出鉸接角和車速，此為估計器2；最后通過卡爾曼算法，以估計器1 縱向車速為測量量，估計器2 縱向速度為狀態量，融合二者所估計的結果，實時估計出車輛的縱向/側向速度、橫擺角速度和掛車與牽引車的鉸接角，具體流程如圖5.

圖5 估計器流程圖Fig.5 Flow chart of estimator

如果估計器1 判斷得到全部車輪均為非穩定狀態，輸出上狀態車速；如存在穩定車輪，則會在估計器2的基礎上融入估計器1的結果，由于引入了新的測量值，且為非積分估計，可以有效克服估計器2 的累計誤差，也因此對整體估計器的初值的準確性要求低，即使出現斷電再恢復等特殊情況，依然可以較精確地重新估計出狀態量.

3 仿真驗證

本文采用Simulink 與TruckSim 聯合仿真平臺，以TruckSim 輸出的值為參考值，驗證本算法的有效性.在TruckSim 模型中選用有ABS 制動的半掛車輛，添加掛車后，將方向盤轉角和制動力矩做為輸入.TruckSim 輸出的車輪轉角和車輪轉矩為估計算法的輸入；縱向、側向加速度、橫擺角速度以及車輪轉速為估計器的測量值，表4為整車參數.

表4 整車參數表Tab.4 Parameters of complete vehicle

雙自適應無跡卡爾曼估計器（FFUKF）的狀態變量初始值為[50/3.6，0，0，0]T，過程噪聲協方差矩陣初始值為Q=diag[0.01，0.01，0.01，0.1]，觀測噪聲協方差初始值為R=1，采樣時間為0.001 s.為進一步驗證所提出算法的精確性，本文與普通模糊自適應無跡卡爾曼估計器（FUKF）估計的結果進行對比分析.下面分別進行兩種工況仿真分析，在不同控制下對估計器進行驗證.

3.1 角階躍工況

依據國標GB/T 6323—2014 設置方向盤轉角階躍輸入仿真.初始車速為50 km/h，直線行駛4.5 s 后方向盤轉動90°并固定數秒，在8 s 時，給牽引車后輪制動泵以30 MPa的壓強，進行緊急制動，并保持2 s，方向盤轉角和制動設置如圖6 所示，牽引車后輪的輪速如圖7所示.

圖6 方向盤轉角和制動泵輸入Fig.6 Steering wheel angle and brake pump input

圖7 牽引車后輪輪速（左）Fig.7 Wheel speed of rear wheel of tractor（left）

方向盤轉角階躍輸入仿真工況中，本文所提雙自適應無跡卡爾曼算法（FFUKF）和普通模糊自適應無跡卡爾曼算法（FUKF）估計出的牽引車與掛車的縱向、側向速度和橫擺角速度以及二者的鉸接角分別與TruckSim的輸出值進行對比，如圖8-14所示.

圖8 牽引車縱向速度Fig.8 Longitudinal speed of tractor

通過仿真結果對比可知：

圖9 掛車縱向速度Fig.9 Longitudinal speed of trailer

圖10 牽引車側向速度Fig.10 Lateral speed of tractor

圖11 掛車側向速度Fig.11 Lateral speed of trailer

圖12 牽引車橫擺角速度Fig.12 Yaw rate of tractor

圖13 掛車橫擺角速度Fig.13 Yaw rate of trailer

圖14 牽引車與掛車鉸接角Fig.14 Articulation angle between tractor and trailer

縱向速度估計中，自適應無跡卡爾曼（FFUKF）和模糊自適應無跡卡爾曼（FUKF）算法在未有轉角輸入前（0～4.5 s）對掛車和牽引車均有較高的估計精度，但是將曲線比例放大后，可以對比看出FUKF 仍然有一定誤差，FFUKF算法的估計精度更高，在有轉角輸入后（4.5～12 s），FUKF 算法對牽引車縱向速度的估計的誤差逐漸增大，而FFUKF 算法能一直保持較高的估計精度，其中牽引車在11 s時由8.356 m/s下降到8.217 m/s，與參考值8.218 m/s 相比，誤差由1.679%縮小到0.012%；側向速度估計中，二者均有一定誤差，但FUKF 算法的估計誤差相對較小，尤其牽引車的側向速度估計，可明顯觀察出FFUKF 估計誤差在逐漸向參考值靠近，其中牽引車在11 s 時由0.209 m/s下降到0.197 m/s，與參考值0.188 m/s相比，誤差由11.170%縮小到4.787%；橫擺角速度以及牽引車與掛車的鉸接角估計中，自適應無跡卡爾曼（FFUKF）和FUKF 算法對鉸接角的估計均有較高的精度，放大后可看出，FFUKF算法的估計值更接近參考值，尤其是鉸接角基本與參考值重合.

3.2 雙移線工況

設置雙移線工況仿真，運動軌跡如圖15 所示，參考ISO3888-1—1999 標準［27］，考慮半掛車車速略低乘用車，本文設置初始車速為50 km/h，直線行駛50 m后開始轉向，方向盤轉角如圖16所示.

圖15 雙移線運動軌跡Fig.15 Trajectory of double shift line

圖16 方向盤轉角Fig.16 Turning angle of steering wheel

雙移線仿真工況中，本文所提雙自適應無跡卡爾曼算法（FFUKF）和模糊自適應無跡卡爾曼算法（FUKF）估計出的牽引車與掛車的縱向、側向速度和橫擺角速度以及二者的鉸接角分別與TruckSim 的輸出值進行對比，如圖17-23所示.

圖17 牽引車縱向速度Fig.17 Longitudinal speed of tractor

圖18 掛車縱向速度Fig.18 Longitudinal speed of trailer

圖19 牽引車側向速度Fig.19 Lateral speed of tractor

圖20 掛車側向速度Fig.20 Lateral speed of trailer

圖21 牽引車橫擺角速度Fig.21 Yaw rate of tractor

圖22 掛車橫擺角速度Fig.22 Yaw rate of trailer

圖23 牽引車與掛車鉸接角Fig.23 Articulation angle between tractor and trailer

通過仿真結果對比可知，在縱向速度和橫擺角速度估計中，雙自適應無跡卡爾曼（FFUKF）算法的估計精度均高于模糊自適應無跡卡爾曼（FUKF）算法，尤其在牽引車縱向速度估計中，雙自適應無跡卡爾曼（FFUKF）的估計精度優勢最為突出，其中牽引車在9 s 時由13.72 m/s 上升到13.745 m/s，與參考值13.75 m/s 相比，誤差由0.218%縮小到0.036%.在掛車的側向速度估計中，FUKF 算法也具有較高精度，但將曲線比例放大后，看出本文所提算法仍然具有更好的估計精度.牽引車與掛車的鉸接角估計中，在10 s之后，明顯對比出FFUKF算法的精度更高.

4 結論與展望

1）本文以半掛車輛為研究對象，搭建車輛12 自由度非線性動力學模型和輪胎模型，利用模糊控制自適應調整滑移率容差和測量噪聲，在UKF 算法的基礎上，提出雙自適應無跡卡爾曼估計算法（FFUKF），可以實現實時估計出車輛的縱向、側向速度、橫擺角速度和掛車與牽引車的鉸接角的目的.

2）在Simulink/TruckSim 聯合環境下進行多工況實驗，驗證所提出的雙自適應無跡卡爾曼估計算法（FFUKF）具有良好的魯棒性，降低估計對估計初值準確性的依賴性，且在有ABS控制輸入的情況，仍可以更精確地對車速和鉸接角進行實時估計.

3）本文所提算法只在縱向速度估計中引入新的測量量，有效克服了縱向速度的累計誤差，減少了由于縱向速度的累計誤差對側向速度和橫擺角速度的影響，但在側向速度和橫擺角速度的估計里面暫時未引入新的量，不能根本上消除側向速度和橫擺角速度的累計誤差，后續需要進一步的研究擴展.其次本估計方法用到了模糊控制，其在實際應用需要根據具體情況進行調整.