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

劉 進， 趙熙俊

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

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

劉 進， 趙熙俊

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

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

自主駕駛車輛；橫向運動狀態(tài)；狀態(tài)估計；擴展卡爾曼濾波

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

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

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

1 問題的描述

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

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

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

(1)

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

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

考慮到側(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)計算.顯然，改進的魔術(shù)公式可描述不同路面附著條件下的輪胎側(cè)偏特性.

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

(4)

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

2 橫向運動狀態(tài)估計的擴展卡爾曼濾波器設(shè)計

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

(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)為過程噪聲和測量噪聲，且為相互獨立、均值為零的高斯白噪聲.

對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)的計算如下，

(10)

(11)

(12)

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

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

(13)

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

圖2 EKF的計算過程

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

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

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

表1 車輛參數(shù)

3.1 仿真試驗

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

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

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

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

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

3.2 樣車試驗

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

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

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

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

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

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

圖6 EKF樣車試驗性能

4 結(jié)束語

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

[1] 宗長富, 潘 釗, 胡 丹, 等. 基于擴展卡爾曼濾波的信息融合技術(shù)在車輛狀態(tài)估計中的應(yīng)用[J]. 機械工程學(xué)報, 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版. 陳蔭三, 余 強,譯. 北京:清華大學(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.

劉 進(1971-)，男，研究員，研究方向為地面無人平臺技術(shù).

趙熙俊(1984-)，男，副研究員，研究方向為地面無人平臺自主技術(shù).