車輛質(zhì)心側(cè)偏角容積卡爾曼濾波軟測(cè)量方法

張 婧，張芝雨，季 曄

（1.許昌職業(yè)技術(shù)學(xué)院機(jī)電與汽車工程學(xué)院，河南 許昌 461000；2.洛陽(yáng)理工學(xué)院機(jī)械工程學(xué)院，河南 洛陽(yáng) 471023）

1 引言

近年來(lái)，隨著汽車制造業(yè)的發(fā)展，汽車產(chǎn)業(yè)技術(shù)逐步壯大與成熟，汽車產(chǎn)品力日益提升，業(yè)內(nèi)競(jìng)爭(zhēng)也愈發(fā)激烈。伴隨而來(lái)的是整個(gè)行業(yè)對(duì)于行業(yè)鏈中產(chǎn)品品質(zhì)的精益求精，以及人們對(duì)于乘坐體驗(yàn)要求的日益提高。此外，汽車的智能制造與智能控制技術(shù)也得到了空前的關(guān)注于大力發(fā)展［1-2］。智能車集成了諸多的先進(jìn)控制技術(shù)，具有更高的道路利用率與操縱穩(wěn)定性，尤其是L3級(jí)別的智能車輛技術(shù)是當(dāng)前的研究熱點(diǎn)之一。在車輛控制技術(shù)應(yīng)用中，準(zhǔn)確可靠的環(huán)境感知信息與車輛自身行駛狀態(tài)信息是決定車輛動(dòng)力學(xué)控制性能好壞的關(guān)鍵一環(huán)［3-4］，因此，進(jìn)行智能車行駛狀態(tài)估計(jì)方面的研究具有非常重要的理論價(jià)值與實(shí)踐意義。

卡爾曼濾波及其諸多的改進(jìn)形式在車輛參數(shù)與車輛行駛狀態(tài)估計(jì)的研究中得到了廣泛的應(yīng)用［5］，是一種普遍且有效的估計(jì)手段。文獻(xiàn)［6］在考慮車輛模型的非線性特性基礎(chǔ)上，結(jié)合自適應(yīng)神經(jīng)模糊推理方法與無(wú)跡卡爾曼濾波提出了一種車輛質(zhì)心側(cè)偏角估計(jì)方法，利用低成本傳感器得到了準(zhǔn)確的車輛狀態(tài)估計(jì)量。文獻(xiàn)［7］設(shè)計(jì)了一種車輛狀態(tài)擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）估計(jì)方法，并結(jié)合自適應(yīng)輔助濾波的方式，通過(guò)誤差迭代提高了估計(jì)精度。文獻(xiàn)［8］考慮了非高斯噪聲與輪胎的非線性，提出了一種將輔助粒子濾波與迭代擴(kuò)展卡爾曼濾波相結(jié)合的估計(jì)方法。觀察現(xiàn)有研究可知，在進(jìn)行車輛行駛狀態(tài)濾波估計(jì)中，通常將由慣導(dǎo)裝置采集到的車輛橫擺角速度或縱橫向加速度作為測(cè)量方程的更新輸入量［9-10］。然而，依賴單個(gè)裝置或傳感器測(cè)量所得的狀態(tài)信息一旦出現(xiàn)故障或者受到外界未知干擾，容易對(duì)估計(jì)結(jié)果造成較大的偏差。四輪輪速耦合關(guān)系與車輛行駛狀態(tài)息息相關(guān)，且輪速傳感器價(jià)格相對(duì)較低，輪速信息相對(duì)較易獲得，若將其應(yīng)用到車輛狀態(tài)濾波估計(jì)中，可以憑借測(cè)量信息的冗余度來(lái)提高整個(gè)估計(jì)系統(tǒng)的可靠性，因此具有較大的研究?jī)r(jià)值與空間。

針對(duì)車輛質(zhì)心側(cè)偏角軟測(cè)量問(wèn)題，提出了一種基于自適應(yīng)容積卡爾曼濾波（Adaptive Cubature Kalman Filter，ACKF）的智能車質(zhì)心側(cè)偏角耦合估計(jì)方法。建立了三自由度車輛模型、輪胎模型、輪速耦合模型，并采用卡爾曼濾波進(jìn)行車輛狀態(tài)估計(jì)。結(jié)合車輛模型，設(shè)計(jì)了自適應(yīng)漸消矩陣提高增益矩陣的調(diào)節(jié)能力，并將輪速耦合關(guān)系應(yīng)用到ACKF的測(cè)量更新方程中，利用測(cè)量信息的冗余度來(lái)提高估計(jì)結(jié)果的精度與可靠性。

2 車輛動(dòng)力學(xué)模型

2.1 三自由度車輛模型

為了實(shí)現(xiàn)車輛行駛狀態(tài)估計(jì)方法的設(shè)計(jì)工作，需要定義與表征界定研究條件與范圍下的車輛動(dòng)態(tài)模型。車輛建模中所設(shè)定的建模條件與研究范圍可以表示為：考慮車輛行駛水平平面內(nèi)的動(dòng)力學(xué)狀態(tài)，研究車輛縱橫向以及橫擺方向的動(dòng)力學(xué)方程，建立坐標(biāo)系并將其表示為xoy，且將其原點(diǎn)放置于車輛質(zhì)心。坐標(biāo)系的x軸與y軸分別代表車輛運(yùn)動(dòng)的縱、橫向。常規(guī)車輛的四個(gè)車輪的輪胎型號(hào)與安裝方式、安裝工藝通常來(lái)說(shuō)是一致的，故這里設(shè)定車輛四個(gè)輪胎的特性相同。如圖1所示，從而可建立車輛的三自由度動(dòng)力學(xué)模型，且車輛動(dòng)力學(xué)方程可以表示為：

圖1 三自由度車輛模型Fig.1 Vehicle Model with 3 Degree of Freedom

式中：vx—縱向車速；vy—側(cè)向車速；γ—橫擺角速度；m—汽車質(zhì)量；Fxj和Fy（jj=1，2，3，4）—輪胎的縱向力和側(cè)向力；δ—前輪轉(zhuǎn)角；Iz—繞z軸的轉(zhuǎn)動(dòng)慣量；lf—質(zhì)心距前軸的距離；lr—質(zhì)心距后軸的距離；bf—前輪距的1/2；br—后輪距的1/2。

2.2 輪胎模型

魔術(shù)公式是目前公認(rèn)且應(yīng)用較為普遍的輪胎模型之一，對(duì)表征輪胎力具有較好效果。故這里采用成熟的基于魔術(shù)公式的輪胎模型對(duì)車輛的輪胎力進(jìn)行計(jì)算，魔術(shù)公式可表示為：

式中：Y（X）—縱向輪胎力或側(cè)向輪胎力；

X—滑移率s或輪胎側(cè)偏角α；

B—?jiǎng)偠纫蜃樱?/p>

C—曲線形狀因子；

D—峰值因子；

E—曲線曲率因子；

sh和sv—水平偏移和垂直偏移。

輪胎模型參數(shù)B、C、D、E都與輪胎的垂直載荷相關(guān)，各輪胎的垂直載荷為：

式中：Fz1、Fz2、Fz3、Fz4—對(duì)應(yīng)輪胎的垂直載荷；h—質(zhì)心高度；g—重力加速度。各輪胎側(cè)偏角為：

3 車輛質(zhì)心側(cè)偏角估計(jì)方法設(shè)計(jì)

3.1 基于ACKF的車輛狀態(tài)估計(jì)方法

針對(duì)式（1）～式（3）中的車輛模型，可將其表示成如下的離散狀態(tài)空間方程：

式中：xk—離散系統(tǒng)的狀態(tài)空間向量；yk+1—離散系統(tǒng)的測(cè)量向量；（f·）—離散系統(tǒng)的狀態(tài)轉(zhuǎn)移函數(shù)；h（·）—離散系統(tǒng)的測(cè)量函數(shù)；wk和vk—互不相關(guān)的高斯白噪聲。則式（1）～式（3）離散化結(jié)果可表示為：

式中：T—濾波迭代步長(zhǎng)。

容積卡爾曼濾波（CKF）相比常見(jiàn)的擴(kuò)展卡爾曼濾波降低了濾波的發(fā)散現(xiàn)象，其計(jì)算量?jī)H僅與待估計(jì)系統(tǒng)的狀態(tài)維數(shù)成正比，故相比粒子濾波其計(jì)算量又大大減小，因此近年來(lái)在諸多領(lǐng)域得到了廣泛的應(yīng)用。由于車輛行駛狀況較為復(fù)雜，在非線性干擾或者一些不確定性因素作用下，常規(guī)CKF算法的估計(jì)精度以及估計(jì)穩(wěn)定性很有可能受到影響。利用Sage濾波開(kāi)窗法，結(jié)合多重次優(yōu)漸消因子，設(shè)計(jì)ACKF算法用于車輛質(zhì)心側(cè)偏角估計(jì)，有助于提升估計(jì)效果。依據(jù)式（7）中的車輛非線性狀態(tài)空間方程，ACKF算法迭代步驟可表示為：

（1）選取初值點(diǎn)。

式中：x0—初始狀態(tài)向量；P0—誤差協(xié)方差陣。

（2）計(jì)算容積點(diǎn)。

式中：i—容積點(diǎn)編號(hào)；Sk—Pk|k通過(guò)Cholesky分解所得且有Pk|k=—容積點(diǎn)，即容積點(diǎn)集可表示為：

（3）時(shí)間更新。

容積點(diǎn)傳播，根據(jù)狀態(tài)轉(zhuǎn)移方程得到容積點(diǎn)：

狀態(tài)一步預(yù)測(cè)：

式中：Qk—wk的協(xié)方差陣。

（4）測(cè)量更新。

對(duì)Pk+1|k進(jìn)行Cholesky分解可得：

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

容積點(diǎn)傳播：

測(cè)量預(yù)測(cè)值為：

測(cè)量預(yù)測(cè)協(xié)方差矩陣和互協(xié)方差矩陣分別為：

式中：Rk—vk的協(xié)方差陣。

預(yù)測(cè)誤差εk+1|k可由實(shí)際測(cè)量值與預(yù)測(cè)測(cè)量值作差得到：

式中：yk+1—k+1時(shí)刻的實(shí)際測(cè)量值；

3.2 含自適應(yīng)漸消矩陣的濾波增益設(shè)計(jì)

式中：η—滑動(dòng)窗口寬，這里選取η=220。

觀察式（21）可知，漸消矩陣Mk+1能夠隨著測(cè)量噪聲的變化而自適應(yīng)調(diào)整。然而，隨著迭代誤差的累積，漸消矩陣Mk+1的對(duì)角線元素有可能小于1，因此需要對(duì)漸消矩陣Mk+1進(jìn)行進(jìn)一步的修正，將其設(shè)計(jì)為：

式中：(Mk+1)i—修正前漸消矩陣Mk+1的第i個(gè)主對(duì)角元素。

從而，含自適應(yīng)漸消矩陣的濾波增益可設(shè)計(jì)為：

觀察式（23）可知，當(dāng)測(cè)量噪聲異常變化時(shí)，修正漸消矩陣對(duì)應(yīng)項(xiàng)Mk′+1將同樣增加，則自適應(yīng)濾波增益將會(huì)相應(yīng)減小，從而削弱測(cè)量噪聲異常對(duì)估計(jì)結(jié)果的影響。在完成自適應(yīng)濾波增益的設(shè)計(jì)之后，繼續(xù)進(jìn)行測(cè)量更新的余下迭代步驟，即為車輛狀態(tài)估計(jì)：

以及計(jì)算車輛狀態(tài)誤差協(xié)方差矩陣：

3.3 基于多信息融合的質(zhì)心側(cè)偏角估計(jì)

根據(jù)式（9）中的車輛非線性方程，利用上節(jié)中的ACKF算法可進(jìn)行車輛狀態(tài)估計(jì)，其中狀態(tài)量為xk+1=[vx，k+1vy，k+1γk+1]T。在現(xiàn)有文獻(xiàn)中，常選取車輛側(cè)向加速度作為實(shí)際測(cè)量更新的輸入量。縱向和橫向車輛加速度可以表示為：

式中：ax和ay—車輛的縱向和橫向加速度。

在輪胎模型中，側(cè)向加速度也與輪胎力的計(jì)算結(jié)果息息相關(guān)，一旦車輛的慣導(dǎo)裝置出現(xiàn)了難以預(yù)知的偏差，很容易造成誤差自我循環(huán)以及誤差累積，從而使得濾波估計(jì)結(jié)果逐漸偏離實(shí)際值。四個(gè)車輪的輪速之間滿足如下關(guān)系：

為了避免這一問(wèn)題，將式（27）中的輪速耦合關(guān)系應(yīng)用到測(cè)量更新迭代中，結(jié)合縱橫向加速度的測(cè)量信息，將濾波器的測(cè)量狀態(tài)量設(shè)定為yk=[ax，k ay，k n1，k n2，k n3，k n4，k]T，從而可充分利用測(cè)量信息的冗余度，基于多信息融合的方式提高估計(jì)結(jié)果的可靠性。利用上節(jié)的ACKF 算法，可得車輛行駛狀態(tài)估計(jì)結(jié)果，從而車輛質(zhì)心側(cè)偏角估計(jì)值為：

4 仿真驗(yàn)證

為了驗(yàn)證這里所設(shè)計(jì)的基于ACKF的智能車質(zhì)心側(cè)偏角耦合估計(jì)方法的估計(jì)性能，基于CarSim和Simulink軟件搭建了聯(lián)合仿真模型并進(jìn)行了仿真試驗(yàn)，然后將EKF的仿真估計(jì)結(jié)果與ACKF的仿真估計(jì)結(jié)果進(jìn)行對(duì)比分析。試驗(yàn)中仿真參數(shù)，如表1所示。

表1 車輛參數(shù)Tab.1 Vehicle Parameters

4.1 雙移線工況

雙移線仿真工況，如圖2所示。仿真工況具體設(shè)定為：車速25m/s，道路附著系數(shù)1.0，濾波迭代步長(zhǎng)0.001s，狀態(tài)初值x0=[ 25 0 0]T，誤差協(xié)方差矩陣P=eye( 3 )×103，測(cè)量噪聲協(xié)方差矩陣Q=eye( 6 )×103。仿真結(jié)果，如圖3所示。其中，縱向車速、側(cè)向車速、橫擺角速度和質(zhì)心側(cè)偏角，如圖3（a）～圖3（d）所示。在整體估計(jì)趨勢(shì)上，EKF和ACKF都能對(duì)車輛行駛狀態(tài)進(jìn)行很好地跟蹤。但根據(jù)圖3（a）、圖3（b）、圖3（d）以及圖3（c）的局部放大圖可知，在車輛行駛狀態(tài)的峰值處，ACKF的估計(jì)精度明顯高于EKF。

圖2 雙移線工況Fig.2 Double Lane Changes Manoeuvre

圖3 雙移線工況估計(jì)效果對(duì)比Fig.3 Comparison of Estimation Effects in DLC Manoeuvre

為進(jìn)一步定量化地體現(xiàn)ACKF對(duì)估計(jì)精度的提升效果，采用估計(jì)結(jié)果的誤差平均值（Average Error，AE）和誤差均方根值（Root Mean Square Error，RMSE）來(lái)表征提出方法的估計(jì)效果（下同），計(jì)算公式為：

式中：n—采樣常數(shù)；

xi—第i個(gè)采樣時(shí)刻的車輛實(shí)際狀態(tài)與估計(jì)狀態(tài)。

雙移線工況下估計(jì)結(jié)果的量化對(duì)比分析，如表2所示。由表2可知，EKF的AE和RMSE都小于ACKF，說(shuō)明ACKF提高了整體估計(jì)結(jié)果的精度和穩(wěn)定性。按照AE來(lái)評(píng)價(jià)估計(jì)精度提升效果，計(jì)算可得，相比EKF來(lái)說(shuō)，ACKF對(duì)于縱向車速、側(cè)向車速、橫擺角速度和質(zhì)心側(cè)偏角的估計(jì)精度分別提升了0.20%，17.60%，4.23%，17.28%，取其平均值為整體估計(jì)精度提升比，即9.83%。

表2 雙移線工況估計(jì)結(jié)果定量分析Tab.2 Quantitative Analysis of Estimation Results in Double Lane Change Manoeuvre

4.2 J-turn工況

為了進(jìn)一步驗(yàn)證所提出的估計(jì)方法在復(fù)雜工況下的估計(jì)效果，進(jìn)行了變車速低附著急轉(zhuǎn)向情況下的仿真試驗(yàn)。J-turn工況仿真如圖4所示。仿真工況具體設(shè)定為：道路附著系數(shù)0.5，濾波迭代步長(zhǎng)0.001s，狀態(tài)初值x0=[ 10 0 0]T，誤差協(xié)方差矩陣P=eye( 3 )×103，測(cè)量噪聲協(xié)方差矩陣Q=eye（6）×103。仿真結(jié)果，如圖4所示。其中，縱向車速、側(cè)向車速、橫擺角速度和質(zhì)心側(cè)偏角，如圖4（a）～圖4（d）所示。通過(guò)圖4（a）中局部放大圖和圖4（c）可知，ACKF 的縱向車速和橫擺角速度估計(jì)效果優(yōu)于EKF。由圖4（b）和圖4（d）可知，相比EKF，ACKF對(duì)側(cè)向車速和質(zhì)心側(cè)偏角的估計(jì)精度提升較為明顯。相比CarSim輸出的實(shí)際車輛狀態(tài)，ACKF的估計(jì)結(jié)果存在一些滯后，這主要是由于ACKF計(jì)算量更大導(dǎo)致的。而且相比側(cè)向車速和質(zhì)心側(cè)偏角的量級(jí)來(lái)說(shuō)，這種滯后導(dǎo)致的估計(jì)誤差是非常小且在可容許范圍內(nèi)的。圖4中的仿真結(jié)果表明：在車輛行駛狀態(tài)變化劇烈、行駛工況較為嚴(yán)峻的情況下，所提出的基于ACKF的耦合估計(jì)方法仍能保持精確的估計(jì)性能，驗(yàn)證了所提出方法的抗干擾性與可靠性。按照式（32）、式（33）計(jì)算J-turn工況下的AE值和RMSE值，所得結(jié)果，如表3所示。可知J-turn工況下ACKF的AE值和RMSE值都小于EKF。相比EKF來(lái)說(shuō)，ACKF對(duì)于縱向車速、側(cè)向車速、橫擺角速度和質(zhì)心側(cè)偏角的估計(jì)精度分別提升了0.47%，12.52%，3.06%，12.44%，整體估計(jì)精度提高了7.12%。從而，所提出估計(jì)方法的估計(jì)效果得到了驗(yàn)證。

圖4 J-turn工況Fig.4 J-turn Manoeuvre

圖5 J-turn工況估計(jì)效果對(duì)比Fig.5 Comparison of Estimation Effects in J-turn Manoeuvre

表3 J-turn工況估計(jì)結(jié)果定量分析Tab.3 Quantitative Analysis of Estimation Results in J-turn Manoeuvre

5 結(jié)論

（1）針對(duì)車輛質(zhì)心側(cè)偏角估計(jì)問(wèn)題，建立了車輛三自由度動(dòng)力學(xué)模型、輪胎模型和輪速耦合模型，并基于ACKF設(shè)計(jì)了車輛質(zhì)心側(cè)偏角耦合估計(jì)方法。（2）設(shè)計(jì)了含自適應(yīng)漸消矩陣濾波增益的ACKF，來(lái)提高濾波過(guò)程中的噪聲自適應(yīng)性，并利將輪速耦合關(guān)系應(yīng)用到濾波測(cè)量更新中，提高了估計(jì)結(jié)果的可靠性。進(jìn)行了基于CarSim 和Simulink 聯(lián)合仿真模型的仿真試驗(yàn)，并將EKF估計(jì)結(jié)果與ACKF進(jìn)行對(duì)比分析。（3）結(jié)果表明：相比EKF，所提出的估計(jì)方法能夠?qū)崿F(xiàn)更為精確車輛狀態(tài)估計(jì)，具有更好的實(shí)時(shí)性與可靠性，整體估計(jì)精度分別提升了9.83%和7.12%。