劉 進(jìn)， 趙熙俊

(中國北方車輛研究所，北京 100072)

一種自主車輛橫向運(yùn)動狀態(tài)估計(jì)的方法

劉 進(jìn)， 趙熙俊

(中國北方車輛研究所，北京 100072)

采用非線性三自由度單軌模型和考慮輪胎側(cè)偏特性的非穩(wěn)態(tài)輪胎模型，建立了用于狀態(tài)估計(jì)的非線性狀態(tài)方程.結(jié)合自主車輛的現(xiàn)有傳感器配置，梳理了橫向運(yùn)動狀態(tài)的觀測(輸入)和估計(jì)(輸出).基于擴(kuò)展卡爾曼濾波提出了橫向運(yùn)動狀態(tài)估計(jì)的方法，開發(fā)了狀態(tài)估計(jì)算法，實(shí)現(xiàn)了基于MATLAB/Simulink的離線狀態(tài)估計(jì)平臺.最后，通過對典型工況的仿真模擬以及對樣車數(shù)據(jù)的離線回放驗(yàn)證了算法性能.試驗(yàn)結(jié)果表明算法能夠合理、有效、準(zhǔn)確地估計(jì)自主車輛行駛過程中的橫擺角速度、橫向速度、橫向加速度、路面附著系數(shù)等橫向運(yùn)動狀態(tài)，對車輛質(zhì)量、轉(zhuǎn)動慣量、路面附著系數(shù)的變化具有較好的魯棒性.

自主駕駛車輛；橫向運(yùn)動狀態(tài)；狀態(tài)估計(jì)；擴(kuò)展卡爾曼濾波

Abstract：A state estimation of autonomous vehicle′s lateral dynamics using extended kalman filter is presented, which adopted single-track 3 DOF non-linear vehicle model, as well as non-steady tire dynamic model. Based on the sensor configuration of common autonomous vehicles, measured and estimated states are determined exactly. Due to non-linear model behavior, the extended kalman filter is utilized for state estimation and an off-line state estimator based on MATLAB/Simulink is implemented. The performance of the proposed method is analyzed with simulation scenarios and real data replay. The experimental results show that the proposed method has good performance and is robust to key parameters of lateral dynamics.

Keywords: autonomous driving vehicles; lateral states；state estimation；extended kalman filter

行駛狀態(tài)估計(jì)是保證自主駕駛車輛安全行駛的重要技術(shù).對運(yùn)動狀態(tài)的實(shí)時(shí)準(zhǔn)確估計(jì)不僅能改善路徑跟蹤控制精度，而且能提高自主車輛的運(yùn)動控制和行駛安全性，特別是對極限工況下的動力學(xué)控制與安全行駛提供必要信息.自主駕駛車輛橫向運(yùn)動狀態(tài)估計(jì)是以現(xiàn)有傳感器配置為基礎(chǔ)，采用多源的信息融合和先進(jìn)的狀態(tài)估計(jì)算法，對橫向運(yùn)動的狀態(tài)，如橫向速度、橫擺角速度、輪胎側(cè)偏力等進(jìn)行精確估計(jì)的方法.

常用的各種狀態(tài)估計(jì)算法有Luenberger觀測器、魯棒觀測算法、滑模觀測算法、神經(jīng)網(wǎng)絡(luò)和模糊觀測算法以及各式各樣的非線性觀測器，等等.其中卡爾曼濾波理論以其獨(dú)特的迭代方法已成為主流的行駛狀態(tài)估計(jì)方法，它不僅可以實(shí)現(xiàn)在線實(shí)時(shí)計(jì)算，同時(shí)具有良好的估計(jì)精度.文獻(xiàn)[1]在建立單軌車輛非線性三自由度車輛模型基礎(chǔ)上，利用擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF)對縱向車速、橫擺角速度和質(zhì)心側(cè)偏角進(jìn)行了估計(jì)，并通過仿真和樣車試驗(yàn)進(jìn)行了驗(yàn)證.文獻(xiàn)[2]采用雙軌車輛模型，通過EKF對縱向車速、橫擺角速度、質(zhì)心側(cè)偏角以及輪胎的縱橫向力進(jìn)行了估計(jì)，且實(shí)現(xiàn)了狀態(tài)估計(jì)的在線實(shí)時(shí)系統(tǒng)，實(shí)驗(yàn)結(jié)果表明所提出的方法對模型中的參數(shù)，如質(zhì)量、轉(zhuǎn)動慣量、路面附著系數(shù)等的變化具有較好的魯棒性.然而上述方法都忽略了路面附著系數(shù)變化較大的情況，例如路面附著系數(shù)減小時(shí)對估計(jì)精度影響較大，特別是路面附著系數(shù)變化時(shí)，常常需要人工調(diào)整初始值以獲得期望的估計(jì)精度.

本研究提出一種自主駕駛車輛的橫向運(yùn)動狀態(tài)估計(jì)方法，由環(huán)境感知對路面識別分類的結(jié)果給出的附著系數(shù)初始值，采用擴(kuò)展卡爾曼濾波方法估計(jì)地面附著系數(shù)以及橫向運(yùn)動狀態(tài).這種方法大大提高了自主駕駛車輛在復(fù)雜工況行駛過程中狀態(tài)估計(jì)的精度和可靠性，且所設(shè)計(jì)的狀態(tài)估計(jì)算法對參數(shù)變化具有一定的魯棒性，保證了當(dāng)環(huán)境感知對路面附著條件的識別不十分精確時(shí)的狀態(tài)估計(jì)精度.

1 問題的描述

充分利用自主駕駛車輛上安裝的傳感器有助于更好地解決橫向運(yùn)動狀態(tài)估計(jì)問題.通過原車的CAN總線獲取縱向車速u和前輪偏角δF，通過慣性測量單元(Inertial Measurement Unit，IMU)獲取縱向加速度ax、橫向加速度ay、橫擺角速度ωr.因此，本文研究的橫向運(yùn)動狀態(tài)估計(jì)算法的觀測與估計(jì)，即輸入與輸出，如圖1所示.所需估計(jì)的橫向運(yùn)動狀態(tài)變量包含縱向速度u、橫向速度υ、橫擺角速度ωr、縱向加速度ax、前后軸側(cè)偏力FytF和FytR,等等.狀態(tài)估計(jì)的輸入為直接可以測量的狀態(tài)，如縱向車速u、橫擺角速度ωr、前輪偏角δF、縱向加速度ax和橫向加速度ay.

圖1 橫向運(yùn)動狀態(tài)估計(jì)算法輸入-輸出

假設(shè)自主車輛行駛在平坦路面上，只考慮縱向、橫向和橫擺運(yùn)動.結(jié)合輸入輸出關(guān)系，建立三自由度橫擺動力學(xué)非線性模型，

(1)

式中：m表示車輛質(zhì)量；Iz表示繞z軸的轉(zhuǎn)動慣量；lF為質(zhì)心到前軸的距離；lR為質(zhì)心到后軸的距離.

式(1)表明，非線性不僅表現(xiàn)在橫擺運(yùn)動的耦合關(guān)系上，更表現(xiàn)在非線性輪胎側(cè)偏力上.特別是車輛行駛在極限工況時(shí)，輪胎側(cè)偏力明顯表現(xiàn)為輪胎側(cè)偏角的非線性函數(shù)，這將導(dǎo)致車輛橫向動力學(xué)模型的強(qiáng)非線性.

考慮到側(cè)偏動特性，前后軸輪胎的側(cè)偏力非穩(wěn)態(tài)特性為[3]

(2)

結(jié)合魔術(shù)公式中的系數(shù)和附著系數(shù)之間關(guān)系的描述[5]，純側(cè)偏工況下的魔術(shù)公式描述的輪胎側(cè)偏力為

(3)

為前后軸側(cè)偏角，對于前輪轉(zhuǎn)向車輛按照αF=δF-arctan(tanβ+lFωr/u)、αR=-arctan(tanβ-lRωr/u)計(jì)算.顯然，改進(jìn)的魔術(shù)公式可描述不同路面附著條件下的輪胎側(cè)偏特性.

根據(jù)對前后軸的輪胎側(cè)偏力估計(jì)，可計(jì)算得到橫向力的利用附著系數(shù)，即前后軸的橫向力與垂向載荷的比值[4]，

(4)

利用附著系數(shù)滿足μyF≤μ、μyR≤μ，其中μ為路面附著系數(shù).

2 橫向運(yùn)動狀態(tài)估計(jì)的擴(kuò)展卡爾曼濾波器設(shè)計(jì)

卡爾曼濾波理論應(yīng)用的難點(diǎn)在于，噪聲協(xié)方差矩陣初值、系統(tǒng)噪聲協(xié)方差矩陣、測量噪聲協(xié)方差矩陣的確定.另外，車輛參數(shù)的變化，如路面附著系數(shù)、車輛質(zhì)量、轉(zhuǎn)動慣量，會直接改變所建立的車輛模型輸出，進(jìn)而影響狀態(tài)估計(jì)精度.因此，如何設(shè)計(jì)具有魯棒性的狀態(tài)估計(jì)算法是研究者們需要攻克的難題.

(5)

y(t)=h(x(t),δF)+v(t).

(6)

式中：狀態(tài)變量x分別是x1=u，x2=υ，x3=ωr，x4=ax，x5=FytF，x6=FytR；δF為輸入變量；輸出變量y分別是y1=u，y2=ωr，y3=ax，y4=ay；非線性函數(shù)f(·)和h(·)分別為

(7)

(8)

式(8)中的非線性函數(shù)f(·)和h(·)對狀態(tài)變量x和干擾變量w、v求偏導(dǎo)，得到雅克比矩陣F、H、W和V，其中F為6×6矩陣、H為4×6矩陣、W=I6×6、V=I4×6.干擾變量w(t)、v(t)為過程噪聲和測量噪聲，且為相互獨(dú)立、均值為零的高斯白噪聲.

對F離散化處理，選取采樣間隔Δt為10 ms，得到狀態(tài)轉(zhuǎn)移矩陣Φ(t)=eF(t)Δt≈I+F(t)Δt.通過求偏導(dǎo)，得到矩陣F為

(9)

其中,F5j和F6j(j=1、2、3)的計(jì)算如下，

(10)

(11)

(12)

同理，可得到F52、F53和F61、F62、F63.

對于量測方程的雅克比矩陣H為

(13)

得到了矩陣F、H和Φ后，就可以運(yùn)用EKF進(jìn)行橫向運(yùn)動狀態(tài)估計(jì).EKF分為兩個(gè)步驟：第一步驟為預(yù)測，是對狀態(tài)的時(shí)間更新過程；第二個(gè)步驟為更新或校正，通過測量值和濾波器增益對第一步驟中預(yù)測的狀態(tài)進(jìn)行更新.EKF從已知的狀態(tài)初始值x0和狀態(tài)協(xié)方差矩陣初始值P0開始進(jìn)行計(jì)算，反復(fù)執(zhí)行預(yù)測-校正步驟，故濾波過程實(shí)際上是基于狀態(tài)變化為一階馬爾科夫過程假設(shè)的“預(yù)測-校正”的反饋控制器.算法的計(jì)算過程如圖2所示.圖中x為狀態(tài)變量；u為輸入向量；y為觀測向量；Q表示激勵噪聲w(t)的協(xié)方差矩陣；R表示測量噪聲v(t)的協(xié)方差矩陣；K為卡爾曼增益；P為估計(jì)誤差方差矩陣；F和H分別按照式(9)和式(13)計(jì)算.

圖2 EKF的計(jì)算過程

3 試驗(yàn)結(jié)果與分析

為測試文中提出方法的有效性，通過MATLAB/Simulink搭建狀態(tài)估計(jì)測試環(huán)境，實(shí)現(xiàn)狀態(tài)估計(jì)算法，并通過仿真數(shù)據(jù)和樣車數(shù)據(jù)對狀態(tài)估計(jì)方法進(jìn)行離線驗(yàn)證.

在仿真分析中搭建CarSim和MATLAB/Simulink的聯(lián)合仿真平臺，用CarSim模擬車輛動力學(xué)系統(tǒng)，用噪聲模擬傳感器采集數(shù)據(jù).車輛參數(shù)如表1所示.

表1 車輛參數(shù)

3.1 仿真試驗(yàn)

仿真工況選取為CarSim軟件自帶的雙移線試驗(yàn)工況：1)車輛行駛速度為30 m/s；2)車輛行駛在良好路面上，路面附著系數(shù)為μ=0.9；3)自主車輛按照雙移線試驗(yàn)所規(guī)劃的路徑行駛.

雙移線仿真試驗(yàn)的結(jié)果如圖3所示.

從圖3(a)中看出，EKF估計(jì)值能夠較好地跟蹤測量值，從帶有噪聲的觀測量中估計(jì)出較為平滑的車輛狀態(tài).圖3(b)為無法直接測量的車輛橫向運(yùn)動狀態(tài).從圖中看出，EKF估計(jì)值能夠較好地跟蹤C(jī)arSim車輛模型狀態(tài)值.前后軸橫向力和橫向利用附著系數(shù)的估計(jì)值與狀態(tài)值具有一致性趨勢.特別是，υ和β的估計(jì)曲線與狀態(tài)曲線基本一致，表明EKF算法能夠很好地估計(jì)車輛橫向運(yùn)動狀態(tài).

下面分析雙移線試驗(yàn)中EKF對參數(shù)變化的魯棒性.圖4為3個(gè)參數(shù)m、Iz和μ分別取額定值的85%、100%和115%時(shí)，4個(gè)輸出量u、ωr、ax和ay的估計(jì)值與測量值之間誤差的標(biāo)準(zhǔn)差.圖中表示隨著參數(shù)的增加或減小，輸出量的估計(jì)值與測量值之間誤差的標(biāo)準(zhǔn)差也相應(yīng)增加，但是EKF算法仍能保持較好的性能.也就是說參數(shù)的變化會降低EKF算法的車輛橫向運(yùn)動狀態(tài)估計(jì)能力，但是不至于導(dǎo)致EKF發(fā)散.這表明EKF算法在雙移線試驗(yàn)工況下也對參數(shù)的有界變化具有較好的魯棒性.

但是，如果路面附著系數(shù)的變化范圍更大，例如路面附著系數(shù)降低到0.6或以下，此時(shí)EKF初值路面附著系數(shù)仍取0.9，那么EKF算法將發(fā)散.其原因是由于EKF中計(jì)算的理論輪胎模型和實(shí)際輪胎模型之間存在較大偏差，導(dǎo)致車輛所受的橫向力和橫擺力矩與實(shí)際值相差甚遠(yuǎn)，所有計(jì)算完全偏離實(shí)際情況.此時(shí)須通過環(huán)境感知對道路分類結(jié)果來修正路面附著系數(shù)的初值.

3.2 樣車試驗(yàn)

采集了所開發(fā)的自主車輛在某良好路面上自主駕駛時(shí)的行駛狀態(tài)，并通過MATLAB/Simulink環(huán)境下的EKF算法進(jìn)行離線狀態(tài)估計(jì).

圖5為基于樣車試驗(yàn)數(shù)據(jù)的離線EKF狀態(tài)估計(jì)結(jié)果.圖5(a)表示EKF估計(jì)值與樣車試驗(yàn)測量值曲線.從圖中看出，EKF估計(jì)值能較好地跟蹤輸出量.特別是縱向加速度ax和橫向加速度ay存在粗大誤差的情況下，EKF仍能獲得較好的估計(jì)結(jié)果.圖5(b)表示EKF估計(jì)值與模型狀態(tài)值的曲線.狀態(tài)值根據(jù)所采集的車速和前輪偏角的數(shù)據(jù)，通過搭建的CarSim車輛模型計(jì)算得出.狀態(tài)值主要計(jì)算無法直接測量的橫向速度υ和質(zhì)心側(cè)偏角β.從圖5看出，EKF估計(jì)值(υ和β)與狀態(tài)值的變化趨勢完全一致，表明EKF狀態(tài)估計(jì)性能較好.

圖3 雙移線工況的仿真結(jié)果

圖4 EKF對參數(shù)變化的魯棒性試驗(yàn)結(jié)果

圖5 樣車試驗(yàn)數(shù)據(jù)的離線計(jì)算結(jié)果

圖6表示樣車試驗(yàn)測量值與EKF狀態(tài)估計(jì)值之間誤差的標(biāo)準(zhǔn)差.從圖6中看出，車速u和橫擺角速度ωr誤差的標(biāo)準(zhǔn)差較小，而縱向加速度ax和橫向加速度ay誤差的標(biāo)準(zhǔn)差較大.這是由傳感器的精度和噪聲引起的.加速度傳感器的測量噪聲比較大，因此EKF平滑后誤差的標(biāo)準(zhǔn)差自然就大.圖6的結(jié)果表明樣車試驗(yàn)的EKF估計(jì)具有良好的性能.

圖6 EKF樣車試驗(yàn)性能

4 結(jié)束語

基于縱橫向耦合運(yùn)動的三自由度非線性車輛單軌模型和非穩(wěn)態(tài)輪胎模型，以自主車輛現(xiàn)有傳感器的測量信息為基礎(chǔ)，運(yùn)用擴(kuò)展卡爾曼濾波理論，研究了一種自主車輛橫向運(yùn)動狀態(tài)估計(jì)方法，通過仿真測試和樣車試驗(yàn)驗(yàn)證了算法性能.試驗(yàn)數(shù)據(jù)表明，該方法對車輛質(zhì)量、轉(zhuǎn)動慣量、路面附著系數(shù)在-15%～+15%范圍內(nèi)變化時(shí)能夠合理、準(zhǔn)確地估計(jì)橫擺角速度、橫向速度、前后軸側(cè)偏力等橫向運(yùn)動狀態(tài).實(shí)際上，路面附著系數(shù)在-50%～+50%范圍內(nèi)變化時(shí)，所提出的狀態(tài)估計(jì)方法也都具有較好的魯棒性，能夠準(zhǔn)確估計(jì)車輛橫向運(yùn)動狀態(tài)；路面附著系數(shù)變化范圍超過50%以上時(shí)，由于輪胎模型和實(shí)際輪胎側(cè)偏力之間的較大偏差會導(dǎo)致本研究提出的EKF不收斂，此時(shí)須利用自主車輛環(huán)境感知系統(tǒng)對路面材質(zhì)和附著特性進(jìn)行識別，為狀態(tài)估計(jì)提供更合理的初值.只要能夠保證初值給出的附著系數(shù)不超過真值的50%，那么所提出的算法能就保證橫向運(yùn)動狀態(tài)估計(jì)的準(zhǔn)確度.所提出的狀態(tài)估計(jì)方法為車體運(yùn)動控制、行駛安全性控制、路徑跟蹤控制、在線故障診斷提供反饋信息，有助于提高自主車輛的控制性能和行駛安全性.

[1] 宗長富, 潘 釗, 胡 丹, 等. 基于擴(kuò)展卡爾曼濾波的信息融合技術(shù)在車輛狀態(tài)估計(jì)中的應(yīng)用[J]. 機(jī)械工程學(xué)報(bào), 2009, 45(10): 272-277.

[2] Doumiati M, Victorino A.C, Charara A, et al. Onboard Real-Time Estimation of Vehicle Lateral Tire-Road Forces and Sideslip Angle[J]. IEEE/ASME Transaction on Mechatronics, 2011, 16(4): 601-614.

[3] Rill G. First Order Tire Dynamics[C]//European Conference on Computational Mechanics Solids, Struct-ures and Coupled Problems in Engineering. Lisbon: 2006: 1-9.

[4] Mitschke M, Wallentowitz H.汽車動力學(xué)[M].4版. 陳蔭三, 余 強(qiáng),譯. 北京:清華大學(xué)出版社 2009: 5-42.

[5] Jagt PVD, Parsons AW. Road Surface Correction of Tire Test Data[J]. Vehicle System Dynamics, 1996, 25(2): 147-165.

StateEstimationofLateralDynamicsforAutonomousVehicles

LIU Jin， ZHAO Xi-jun

(China North Vehicle Research Institute, Beijing 100072, China)

U270.1+1

A

1009-4687(2017)03-0001-07

2017-06-01.

劉 進(jìn)(1971-)，男，研究員，研究方向?yàn)榈孛鏌o人平臺技術(shù).

趙熙俊(1984-)，男，副研究員，研究方向?yàn)榈孛鏌o人平臺自主技術(shù).