基于RISF融合的車輛橫擺角速度估計*

廖尉華，何智成，蔣祖堅，余天龍，何逸波

（1.上汽通用五菱汽車股份有限公司，柳州 545007；2.湖南大學，汽車車身先進設計制造國家重點實驗室，長沙 410082）

前言

汽車高級駕駛輔助系統，需要獲取準確的車輛橫擺角速度，用于車輛行駛安全狀態評估、路徑預測和速度控制等，以實現車輛自動駕駛過程的安全性要求和舒適性要求。

使用傳感器測量獲得橫擺角速度是最常見的方法，但作為大規模量產的車載低成本零件，其測量信號受其性能和環境影響（如噪聲、零漂等），需進行處理后才能使用，如卡爾曼濾波器KF，在處理非線性系統問題時其魯棒性較差。再者，在濾波過程中，缺少參數的自適應環節，濾波的精度不會太高。另外，傳感器作為微機電系統，其量測結果相比于實際值具有一定的滯后性。除了傳感器方法外，另一類方法是通過建立車輛物理模型進行估計。文獻［7］和文獻［8］中建立了線性2 自由度車輛模型，使用橫向加速度作為輸入對橫擺角速度進行估計，但這種線性模型不能反映實際車輛的非線性運動特征，估計值必然會有誤差。文獻［9］中建立了3 自由度非線性汽車動力學模型，仿真測試取得較好效果，但未考慮實際道路影響。

當然，橫擺角速度等車身姿態估計亦有采用多源傳感信息融合的估計方法。文獻［5］和文獻［10］均使用了線性融合方法。文獻［9］中使用了自適應無跡卡爾曼濾波AUKF（adaptive unscented Kalman filter）的融合算法，但因為UKF算法存在狀態不穩定和發散的潛在風險，產業界一般較少使用。上述這些融合方法，雖然在仿真測試和標準路面測試中取得較好的效果，但較少考慮實際道路狀況的影響，在實際道路上的效果有待驗證。

當前，ESP 控制器已成為乘用車標配，該系統配有車身運動感知傳感器，可通過整車信號網絡發送給駕駛輔助控制器，例如輪速傳感器、前輪轉角傳感器、橫向加速度傳感器以及橫擺角速度傳感器等。針對上述車輛橫擺角速度估計存在的問題，本文中綜合傳感濾波和模型估計兩類方法，充分利用車載運動感知傳感器，首先使用估計效果更優的ACKF算法對傳感器進行濾波。ACKF 作為貝葉斯濾波框架下的次優估計算法，理論上比同框架下的KF、EKF 等具有更優的估計效果。然后通過建立車輛3自由度動力學模型，并考慮實際車輛行駛道路的傾斜角情況，使用車速、前輪轉角和橫向加速度信息，估計出橫擺角速度。最后基于RISF 多源傳感信息融合方法融合上述濾波值和模型估計值。

1 基于ACKF的傳感器量測值濾波

1.1 3階球面徑向容積準則

貝葉斯濾波從理論上給出了非線性濾波的最優估計。在高斯分布假設下，貝葉斯濾波的關鍵問題是計算形如“非線性函數×高斯概率密度函數”的高斯加權積分，該積分可轉化成如式（1）所示的積分式。其中：∈R是系統狀態向量；()是任意非線性函數。

文獻［13］中把式（1）轉換成球徑積分，即令=，=1，∈[0，∞)。為半徑，為方向向量。得

式中：U={∈R|=1}為球體表面；(·)為積分域U的微元；()由單位加權函數()=1 的球面積分定義。

徑向積分由m點高斯積分準則計算，球面積分由m點球面準則計算，則可得到(m× m)點的球面-徑向容積準則積分式：

本文使用3 階球面徑向容積準則求解式（4）。在式（4）中，m=1，m=2，總共包含2容積點，是R的系統狀態維數。

這里，若exp(-)為標準正態分布，上述高斯加權積分為

1.2 自適應容積卡爾曼濾波ACKF

算法輸入為時刻的量測值z和(-1)時刻的估計結果x、P，輸出為x、P，則ACKF的步驟如下。

（1）參數初始化

（2）時間更新

首先對P進行Cholesky分解：

根據分解結果可計算容積點，得

計算傳播容積點，通過狀態轉移函數，轉換后的容積點計算為

式中：=1，2，…=2。

計算時間更新步的先驗估計和誤差協方差：

（3）量測更新

計算容積點及傳播容積點：

計算量測更新步的先驗估計、新息協方差和互協方差：

（4）計算卡爾曼增益、時刻狀態估計和估計方差

（5）噪聲自適應

若過程噪聲和量測噪聲能夠精確定義，卡爾曼濾波可獲得最優估計。由式（10）、式（15）、式（17）和式（18）可知，和影響了卡爾曼濾波增益的大小，決定了估計值在過程和量測信息間的權重。文獻［4］研究了4 種自適應卡爾曼濾波方法，其中的協方差匹配法應用最廣，且效果較好。本文中參照文獻［4］，采用移動窗口內的新息序列對和矩陣進行自適應更新。

更新過程噪聲Q和測量噪聲R：

2 基于考慮路傾角自行車模型的車輛橫擺角速度估計

車輛在水平路面行駛時，可通過建立自行（兩輪）車模型得到車輛橫向加速度與車輛橫擺角速度之間的關系。但當車輛在側傾路面行駛時，橫向加速度傳感器的量測值包含了車輛橫向運動信息和重力沿側傾路面的分力信息，因此不能直接用于橫擺角速度的估計。雖然車輛轉彎時，由于向心力導致車身橫擺角速度和底盤懸架橫擺角速度不等，但當車輛處于側傾路面彎道時，重力沿側傾路面的分力提供了部分向心加速度，車身相對底盤懸架的側擺程度降低。另外，本文所述方法主要面向智能輔助駕駛功能的舒適性問題，例如高速領航功能和車道保持功能等。當這些功能開啟時，車輛處于自動駕駛模式，系統會控制車輛按照設定速度沿著所在車道的中心線行駛。因此，本文中不考慮車輛在側傾路面彎道上變道的情況，駕駛員主動變道所產生的車身嚴重側擺不在本文考慮范圍內。綜上所述，本文中不考慮車身側擺影響，認為車身橫擺角速度等于底盤懸架系統的橫擺角速度。

盡管自行車模型是簡化的車輛模型，但模型中的參數，滿足考慮側傾路面問題的研究。本文中建立的模型如圖1 和圖2 所示。是車輛相對于大地坐標系的航向角，是汽車質量，是車輛質心，是車輛質心速度，是車輛質心滑移角，是重力加速度，是前輪轉向角，是車輛質心到前輪中心距離，是車輛質心到后輪中心距離，是側傾路面角。

圖1 自行車模型

圖2 車輛在側傾路面的重力分解

車輛在側傾路面彎道上行駛時，前輪轉向角較小，滿足車輛模型的小角度假設?？纱_定車輛質心橫向受力平衡、質心處滑移角速度和軸力矩平衡的微分方程，分別為

式中：C為前輪側偏剛度；C為后輪側偏剛度；I為車輛繞車輛質心坐標系軸轉動慣量。

考慮到路面側傾角不會劇烈突變，且系統采樣工作周期很短，可認為sin φ?sin φ。對式（22）和式（26）進行離散化處理，得離散系統狀態方程：

另外，根據阿克曼轉向幾何原理，得如下方程：

上述式（27）和式（28）可作為卡爾曼濾波的時間更新步，式（30）和式（31）作為量測更新步。遞推過程與第2節相同。

值得一提的是，模型中的車速是根據輪速信號獲得，由于車速估計不是本文重點，所以本文中使用最簡單的輪速平均法估計，當有更好的車速估計方法時，亦能得到更好的橫擺角速度估計。

3 基于RISF的橫擺角速度融合估計

可靠指標傳感器融合（reliability indexed sensor fusion，RISF）是一種卡爾曼濾波框架下的多源傳感信息融合方法。該方法認為，可把卡爾曼濾波的量測噪聲矩陣和過程噪聲矩陣視為各傳感器的可靠指標RI（reliability index），根據不同的場景條件依靠工程經驗調試的方法確定兩噪聲矩陣的值。

式中是量測空間到觀測空間的變換矩陣，=。

根據工程經驗，當車輛橫擺角速度傳感器值較小時，由于低信噪比和零漂因素的存在，傳感器量測值的精確度較差。在這種情況下，卡爾曼濾波更新步結果的置信度應降低，即傳感器噪聲矩陣的值取大些；當車輛前輪轉向角較大時，由于輪胎的側滑、變形等因素，導致小角度假設不成立，因此卡爾曼濾波預測步結果的置信度應降低，即系統過程噪聲的值取大些；當車輛橫向加速度傳感器測量值較小時，對應的過程噪聲的值取大些。

綜上，本文設計如下噪聲協方差更新方程。其中，c、c、c、c、c、c、c和c通過實車調試的方法確定。

4 實車驗證

本文中基于某SUV 車型進行驗證（見圖3），在測試車上安裝中汽院智能網聯的i-TESTER AVE 2100 測試系統，用于獲取車輛運動姿態真值，如圖4所示。該系統配備有i-TESTER主機、組合定位模塊等，包括GPS-RTK 和IMU 等傳感器，可精確測量車身姿態參數。該系統可把測量數據通過CAN 方式發出。另外使用Speedgoat 公司的Mobile 型快速控制原型機RCP（見圖5）實時運行Matlab/Simulink 算法模型，該原型機可通過網線與上位機PC 連接通信。

圖3 某SUV車型測試車

圖4 i-TESTER AVE 2000 車身姿態真值采集系統

測試車設備連接原理如圖5 所示，智能駕駛控制器與ESP 控制器通過車載網關交互信號，使用CAN 通信方式。在智能輔助駕駛控制器和網關之間，制作三通線，連接Speedgoat 實時機。真值測試系統的組合定位傳感器安裝在車身上，系統主機與上位機PC 通過CANoe 連接。最后Speedgoat 實時機的運行結果和i-TESTER 真值系統測量結果匯總在上位機PC上顯示。

圖5 試驗車設備連接原理

該車車身參數和輪胎參數如表1和表2所示。

表1 車身參數表

表2 輪胎參數表

根據JTG B01—2014《公路工程技術標準》，高等級彎道橫坡坡度設計推薦為2%～3%。本文中選取柳州市北進路自南向北一段較長彎路作為測試道路，該彎道的設計側傾坡度為3%，如圖6 所示。測試車輛以（65±2）km/h的車速行駛，對所設計算法進行驗證。

圖6 測試道路

4.1 橫擺角速度傳感器濾波

測試彎道從入彎到出彎長約550 m，整個過程約30 s。根據采集的數據結果，如圖7（a）所示，車載傳感器量測值與i-TESTER 測試系統輸出真值曲線的走勢基本一致。同時使用傳統KF 算法和本文設計的ACKF 算法，對車載傳感器的輸出濾波進行處理，結果如圖7（b）所示。

圖7 車載橫擺角速度傳感器輸出及濾波

濾波曲線的走勢表明，在傳感信號振蕩處，ACKF 比KF 具有更好的消抖表現；在傳感信號持續上升過程中，ACKF 也比KF 表現出更好的跟隨性。另外，把i-TESTER 測試系統輸出值作為參考真值，使用均方誤差（MAE）和均方根誤差（RMSE）對KF和ACKF 兩種算法的估計精度進行定量比較，結果如表3 所示。從表中數據可看出，雖然車載傳感器與i-TESTER測量系統存在固有誤差，導致兩濾波算法的MAE 和RMSE 結果相差不大，但ACKF 算法的MAE 和RMSE 依舊比KF 的小，反映了ACKF 更好的估計精度。

表3 KF和ACKF的MAE和RMSE指標

4.2 車輛模型的估計結果

基于3 自由度自行車模型的橫擺角速度估計如圖8 所示，綠色曲線為模型估計值，其走勢基本與真值測試系統的輸出曲線保持一致。轉動轉向盤是產生車輛橫擺角速度的原因，而傳感器本身是一個微機電系統，因此理論上模型估計值應比傳感器量測值“早一些”。實際的測試結果也驗證了這一現象，即圖8 中的綠色曲線均比真值測試系統的紅色曲線和車載傳感器濾波黑色曲線“靠左”。

圖8 基于車輛模型的橫擺角速度和道路側傾角估計

藍色曲線為道路側傾角估計，基本能反映出道路側傾角，在0.03 rad值附近。

4.3 基于RISF的融合估計

圖9 中藍色曲線為最終的RISF 融合曲線，相比于傳感器濾波曲線和模型估計曲線，融合曲線更加平滑，波峰波谷區域亦消除了傳感器濾波曲線的抖動。另外，融合曲線相比于傳感器濾波曲線“靠左”，說明融合曲線更能反映出車輛實時真實的姿態值。

圖9 基于RISF的橫擺角速度融合估計

5 結論

本文中設計了一種基于RISF 多源傳感信息融合的車輛橫擺角速度估計方法。在RISF 自適應卡爾曼濾波算法框架下，首先使用ACKF 算法獲得了較好的橫擺角速度傳感器濾波值，然后建立考慮道路側傾角的自行車模型，通過動力學公式估計出橫擺角速度，最后在RISF-AKF框架下進行融合。

實車道路數據表明，RISF 融合曲線相比于傳感器濾波曲線，消除了波峰波谷區域的信號抖動，與此同時引入了模型估計對車輛真實橫擺角速度的預測信息，解決了使用單一傳感器信息源帶來的橫擺角速度估計波動大、量測滯后等問題。