廖尉華，何智成，蔣祖堅(jiān)，余天龍，何逸波

（1.上汽通用五菱汽車(chē)股份有限公司，柳州 545007；2.湖南大學(xué)，汽車(chē)車(chē)身先進(jìn)設(shè)計(jì)制造國(guó)家重點(diǎn)實(shí)驗(yàn)室，長(zhǎng)沙 410082）

前言

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

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

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

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

1 基于ACKF的傳感器量測(cè)值濾波

1.1 3階球面徑向容積準(zhǔn)則

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

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

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

徑向積分由m點(diǎn)高斯積分準(zhǔn)則計(jì)算，球面積分由m點(diǎn)球面準(zhǔn)則計(jì)算，則可得到(m× m)點(diǎn)的球面-徑向容積準(zhǔn)則積分式：

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

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

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

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

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

（2）時(shí)間更新

首先對(duì)P進(jìn)行Cholesky分解：

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

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

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

計(jì)算時(shí)間更新步的先驗(yàn)估計(jì)和誤差協(xié)方差：

（3）量測(cè)更新

計(jì)算容積點(diǎn)及傳播容積點(diǎn)：

計(jì)算量測(cè)更新步的先驗(yàn)估計(jì)、新息協(xié)方差和互協(xié)方差：

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

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

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

更新過(guò)程噪聲Q和測(cè)量噪聲R：

2 基于考慮路傾角自行車(chē)模型的車(chē)輛橫擺角速度估計(jì)

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

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

圖1 自行車(chē)模型

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

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

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

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

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

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

值得一提的是，模型中的車(chē)速是根據(jù)輪速信號(hào)獲得，由于車(chē)速估計(jì)不是本文重點(diǎn)，所以本文中使用最簡(jiǎn)單的輪速平均法估計(jì)，當(dāng)有更好的車(chē)速估計(jì)方法時(shí)，亦能得到更好的橫擺角速度估計(jì)。

3 基于RISF的橫擺角速度融合估計(jì)

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

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

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

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

4 實(shí)車(chē)驗(yàn)證

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

圖3 某SUV車(chē)型測(cè)試車(chē)

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

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

圖5 試驗(yàn)車(chē)設(shè)備連接原理

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

表1 車(chē)身參數(shù)表

表2 輪胎參數(shù)表

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

圖6 測(cè)試道路

4.1 橫擺角速度傳感器濾波

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

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

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

表3 KF和ACKF的MAE和RMSE指標(biāo)

4.2 車(chē)輛模型的估計(jì)結(jié)果

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

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

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

4.3 基于RISF的融合估計(jì)

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

圖9 基于RISF的橫擺角速度融合估計(jì)

5 結(jié)論

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

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