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

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

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

前言

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

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

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

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

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

1.1 3階球面徑向容積準則

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

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

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

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

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

這里，若exp(-)為標準正態(tài)分布，上述高斯加權(quán)積分為

1.2 自適應(yīng)容積卡爾曼濾波ACKF

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

（1）參數(shù)初始化

（2）時間更新

首先對P進行Cholesky分解：

根據(jù)分解結(jié)果可計算容積點，得

計算傳播容積點，通過狀態(tài)轉(zhuǎn)移函數(shù)，轉(zhuǎn)換后的容積點計算為

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

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

（3）量測更新

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

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

（4）計算卡爾曼增益、時刻狀態(tài)估計和估計方差

（5）噪聲自適應(yīng)

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

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

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

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

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

圖1 自行車模型

圖2 車輛在側(cè)傾路面的重力分解

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

式中：C為前輪側(cè)偏剛度；C為后輪側(cè)偏剛度；I為車輛繞車輛質(zhì)心坐標系軸轉(zhuǎn)動慣量。

考慮到路面?zhèn)葍A角不會劇烈突變，且系統(tǒng)采樣工作周期很短，可認為sin φ?sin φ。對式（22）和式（26）進行離散化處理，得離散系統(tǒng)狀態(tài)方程：

另外，根據(jù)阿克曼轉(zhuǎn)向幾何原理，得如下方程：

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

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

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

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

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

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

綜上，本文設(shè)計如下噪聲協(xié)方差更新方程。其中，c、c、c、c、c、c、c和c通過實車調(diào)試的方法確定。

4 實車驗證

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

圖3 某SUV車型測試車

圖4 i-TESTER AVE 2000 車身姿態(tài)真值采集系統(tǒng)

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

圖5 試驗車設(shè)備連接原理

該車車身參數(shù)和輪胎參數(shù)如表1和表2所示。

表1 車身參數(shù)表

表2 輪胎參數(shù)表

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

圖6 測試道路

4.1 橫擺角速度傳感器濾波

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

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

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

表3 KF和ACKF的MAE和RMSE指標

4.2 車輛模型的估計結(jié)果

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

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

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

4.3 基于RISF的融合估計

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

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

5 結(jié)論

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

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