【摘要】為準(zhǔn)確估算自動(dòng)駕駛半掛車輛的牽引車側(cè)向速度，提出了一種考慮傳感器測量零偏的校正估算方法。在三自由度車輛動(dòng)力學(xué)模型的基礎(chǔ)上，將前輪轉(zhuǎn)角、橫擺角速度零偏和側(cè)向加速度零偏等擴(kuò)圍到狀態(tài)變量中，建立了測量零偏估計(jì)模型，考慮到動(dòng)力學(xué)模型失配的影響，設(shè)計(jì)了基于運(yùn)動(dòng)學(xué)模型的反饋校正機(jī)制，對(duì)側(cè)向速度和橫擺角速度的估計(jì)結(jié)果進(jìn)行反饋調(diào)節(jié)，利用平方根容積卡爾曼濾波算法實(shí)現(xiàn)該方法，并分析了估計(jì)器的可觀性，最后，基于TruckSim與MATLAB/Simulink聯(lián)合仿真平臺(tái)進(jìn)行了驗(yàn)證，結(jié)果表明：該方法不僅可以消除測量零偏的影響，還具有良好的收斂速度，性能明顯優(yōu)于未考慮測量零偏和僅考慮測量零偏的估算方法。
關(guān)鍵詞：半掛車輛 側(cè)向速度估算 測量零偏 反饋校正 平方根容積卡爾曼濾波
中圖分類號(hào)：U461.91? ?文獻(xiàn)標(biāo)志碼：A? ?DOI：
10.20104/j.cnki.1674-6546.20240121
Lateral Velocity Correction Estimation for Semi-Trailer Vehicles Considering Measurement Bias
Zhang Zhida
（Shanghai UTOPILOT Technology Company Ltd.， Shanghai 200438）
【Abstract】A correction estimation method considering sensor measurement bias was proposed to accurately estimate the lateral velocity of the tractor in autonomous semi-trailer vehicles. Based on the three-degree-of-freedom vehicle dynamics model， the measurement bias estimation model was established by expanding the front wheel angle， yaw rate bias， and lateral acceleration bias into the state variables. Taking into account the impact of dynamic model mismatch， a feedback correction mechanism based on the kinematic model was designed to adjust the estimation results of lateral velocity and yaw rate through feedback. The proposed method was implemented using the square-root cubature Kalman filter algorithm， and the observability of the estimator was analyzed. This method was verified on the joint simulation platform of TruckSim and MATLAB/Simulink. The results show that the proposed method not only eliminates the influence of measurement bias but also exhibits good convergence speed. Its performance is significantly better than estimation methods that do not consider measurement bias or only consider measurement bias.
Key words：
Semi-trailer， Lateral velocity estimation， Measurement bias， Feedback correction， Square-root cubature Kalman filter
【引用格式】張志達(dá). 考慮測量零偏的半掛車輛側(cè)向速度校正估算方法[J]. 汽車工程師， 2024（6）：
14-21.
ZHANG Z D. Lateral Velocity Correction Estimation for Semi-Trailer Vehicles Considering Measurement Bias[J]. Automotive Engineer， 2024（6）：
14-21.
1 前言
準(zhǔn)確獲取車輛狀態(tài)是自動(dòng)駕駛汽車有效決策和精準(zhǔn)控制的前提。受成本和技術(shù)限制，并非所有車輛狀態(tài)都能夠被直接、精確地測量，因此，需要借助慣性測量單元（Inertial Measurement Unit，IMU）和全球定位系統(tǒng)（Global Positioning System，GPS）等對(duì)車輛的側(cè)向速度、側(cè)偏角或橫擺角速度等狀態(tài)變量進(jìn)行實(shí)時(shí)估算[1-2]。
目前，車輛狀態(tài)估算主要通過車輛的運(yùn)動(dòng)學(xué)模型、動(dòng)力學(xué)模型結(jié)合卡爾曼濾波（Kalman Filter，KF）系列算法、遞歸最小二乘法（Recursive Least Squares，RLS）等進(jìn)行[3]。基于運(yùn)動(dòng)學(xué)模型的估算方法對(duì)車輛參數(shù)、路面附著系數(shù)和行駛工況等具有良好的魯棒性，但對(duì)傳感器偏差、數(shù)據(jù)漂移和校準(zhǔn)精度等造成的測量誤差非常敏感[4-5]。Selmanaj等[6]基于縱、橫向二自由度運(yùn)動(dòng)學(xué)模型設(shè)計(jì)縱向速度和側(cè)向速度觀測器，進(jìn)而估算了車輛側(cè)偏角。李小雨等[7]基于三自由度車輛模型和擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）算法提出了一種結(jié)合運(yùn)動(dòng)學(xué)模型和幾何方法的質(zhì)心側(cè)偏角估算方法。該方法可以提高估計(jì)精度，但需要提前獲取路面附著系數(shù)。為消除基于純運(yùn)動(dòng)學(xué)模型造成的積分偏差，Rezaeian等[8]利用基于LuGre輪胎模型得到的輪胎側(cè)偏角計(jì)算側(cè)向速度，并與基于運(yùn)動(dòng)學(xué)模型估計(jì)的側(cè)向速度進(jìn)行加權(quán)，然而，該方法中前、后輪胎側(cè)偏角的計(jì)算也需要路面附著系數(shù)。事實(shí)上，路面附著系數(shù)的測量和估算成本遠(yuǎn)高于側(cè)向速度和質(zhì)心側(cè)偏角[9]。
基于動(dòng)力學(xué)模型的車輛狀態(tài)估算方法主要依賴于受作用力和扭矩激勵(lì)的車輛模型和輪胎模型，采用的算法與基于運(yùn)動(dòng)學(xué)模型的方法基本一致。該方法受車輛和輪胎參數(shù)的影響較大，部分方案也需要提前獲知路面附著系數(shù)。耿國慶等[10]提出了一種基于二自由度動(dòng)力學(xué)模型和噪聲自適應(yīng)EKF算法的車輛狀態(tài)估算方法。Reina等[11]在假設(shè)縱向速度恒定的情況下，結(jié)合二自由度車輛模型和EKF算法，設(shè)計(jì)了用于識(shí)別質(zhì)心側(cè)偏角等狀態(tài)的估計(jì)器，該研究與文獻(xiàn)[10]類似，都將輪胎模型考慮為線性，僅適用于車輛轉(zhuǎn)向幅度較小的工況。Amin等[12]結(jié)合三自由度車輛動(dòng)力學(xué)模型和LuGre輪胎模型，提出了一種基于約束無跡卡爾曼濾波（Constrained Unscented Kalman Filter，CUKF）算法的半掛車輛側(cè)向速度估算方法，由于利用了LuGre輪胎模型，需要提前獲知路面附著系數(shù)。另外，Piyabongkarn等[13]結(jié)合動(dòng)力學(xué)模型和運(yùn)動(dòng)學(xué)模型設(shè)計(jì)了一種基于頻率加權(quán)的車輛質(zhì)心側(cè)偏角估算方法，在低頻轉(zhuǎn)向時(shí)采用動(dòng)力學(xué)模型的估算結(jié)果，在高頻轉(zhuǎn)向時(shí)采用運(yùn)動(dòng)學(xué)模型的估算結(jié)果。然而，以上研究均未綜合考慮測量零偏和參數(shù)時(shí)變的影響。
鑒于此，本文提出一種考慮傳感器測量零偏的半掛車輛側(cè)向速度校正估算方法。基于三自由度車輛動(dòng)力學(xué)模型，將前輪轉(zhuǎn)角、橫擺角速度零偏和側(cè)向加速度零偏等擴(kuò)圍到估計(jì)器的狀態(tài)變量中，建立測量零偏估算模型。針對(duì)參數(shù)時(shí)變?cè)斐傻膭?dòng)力學(xué)模型失配，設(shè)計(jì)基于運(yùn)動(dòng)學(xué)模型的反饋校正機(jī)制，對(duì)側(cè)向速度和橫擺角速度的估算結(jié)果進(jìn)行實(shí)時(shí)調(diào)節(jié)。最后，利用平方根容積卡爾曼濾波（Square-root Cubature Kalman Filter，SCKF）算法實(shí)現(xiàn)該方法，分析估計(jì)器的可觀性，并基于TruckSim和MATLAB/Simulink聯(lián)合仿真平臺(tái)，選取不同輸入和測量零偏工況測試該估算方法的有效性。
2 半掛車輛動(dòng)力學(xué)模型
忽略路面坡度的影響，假設(shè)車輛僅做平面運(yùn)動(dòng)，建立考慮半掛車輛橫向運(yùn)動(dòng)、橫擺運(yùn)動(dòng)和折疊運(yùn)動(dòng)的三自由度動(dòng)力學(xué)模型，如圖1所示。其中：m1、m2分別為牽引車、掛車的質(zhì)量，Iz1、Iz2分別為牽引車、掛車的橫擺轉(zhuǎn)動(dòng)慣量，a1、b1分別為牽引車前、后軸至其質(zhì)心的距離，c、a2分別為鉸接點(diǎn)至牽引車質(zhì)心、掛車質(zhì)心的距離，b2為掛車后軸至其質(zhì)心的距離，F(xiàn)yf、Fyr分別為牽引車前、后軸輪胎側(cè)向力，F(xiàn)yt為掛車后軸輪胎側(cè)向力，δf為牽引車的前輪轉(zhuǎn)角，vx1、vx2分別為牽引車、掛車的縱向速度，vy1、vy2分別為牽引車、掛車的側(cè)向速度，v1、v2分別為牽引車、掛車的速度矢量和，γ1、β1分別為牽引車的橫擺角速度、質(zhì)心側(cè)偏角，γ2、β2分別為掛車的橫擺角速度、質(zhì)心側(cè)偏角，θ為牽引車與掛車的相對(duì)角度，即折疊角。當(dāng)折疊角較小時(shí)，可以認(rèn)為vx1=vx2=vx，其中vx為縱向速度。
半掛車輛系統(tǒng)的動(dòng)力學(xué)方程如下：
m1ay1=Fyf+Fyr-Fyh （1）
[Iz1γ1=a1Fyf+b1Fyr-cFyh] （2）
m2ay2=Fyt+Fyh （3）
[Iz2γ2=a2Fyh-b2Fyt] （4）
式中：[ay1=vy1+vxγ1]、[ay2=vy2+vxγ2]分別為牽引車、掛車的側(cè)向加速度，F(xiàn)yh為鉸接點(diǎn)處的相互作用力。
在鉸接點(diǎn)處，有如下耦合關(guān)系[12]：
[γ2=γ1+θ] （5）
[νy2=νy1-（a2+c）γ1-a2θ-νxθ] （6）
消除耦合項(xiàng)后可得：
[Iz1γ1+m1cνy1=-m1cνxγ1+a1Fyf-b1Fyr+c（Fyf+Fyr）]（7）
[（m1+m2）νy1-m2（a2+c）γ1-m2a2θ=][-（m1+m2）νxγ1+Fyf+Fyr+Fyt] （8）
[Iz2（γ1+θ）+m1a2νy1=-m1a2νxγ1+a2（Fyf+Fyr）-b2Fyt]
（9）
利用線性輪胎模型描述牽引車前、后軸和掛車后軸的輪胎力Fyi=kiαi， i=f，r，t。其中：ki為各輪胎側(cè)偏剛度，αi為各輪胎側(cè)偏角，f、r、t分別表示牽引車前軸、牽引車后軸、掛車后軸。根據(jù)圖1的坐標(biāo)系定義，牽引車前、后輪胎和掛車后輪胎的側(cè)偏角分別為：
αf[ =δf-νy1+a1γ1νx] （10）
αr[ =b1γ1-νy1νx] （11）
αt=[θ-νy1-（a2+b2+c）γ1-（a2+b2）θνx] （12）
綜上，半掛車輛動(dòng)力學(xué)模型的微分方程為：
[Mx+Gx=Fδf] （13）
式中：x=[νy1γ1θθ]為狀態(tài)變量，M=[m1cm1+m2m1a20-Iz1m2（a2+c）Iz200-m2a2Iz20? 0001]為[x]的系數(shù)矩陣，G為x的系數(shù)矩陣，F(xiàn)=[（a1+c）kfkfa2kf0]為δf的系數(shù)矩陣。
其中，x的系數(shù)矩陣G的表達(dá)式為：
經(jīng)過整理，可得半掛車輛動(dòng)力學(xué)模型的狀態(tài)空間方程為：
[x]=Ax+Bδf （15）
式中：A=-M-1G為狀態(tài)轉(zhuǎn)移矩陣，B=M-1F為控制矩陣。
3 側(cè)向速度估計(jì)器設(shè)計(jì)
3.1 考慮測量零偏的估計(jì)模型
受成本和技術(shù)限制，并非所有車輛狀態(tài)都能夠被直接測量。部分車輛狀態(tài)參數(shù)隨時(shí)間或工況變化，增加了車輛模型的不確定性。因此，基于估計(jì)算法的虛擬觀測器成為獲取某些車輛狀態(tài)的重要手段。然而，由于模型具有非完整性，如參數(shù)時(shí)變、未建模部分和外界擾動(dòng)等，通常會(huì)引入模型失配問題。鑒于此，本文在估計(jì)牽引車側(cè)向速度時(shí)，考慮橫擺角速度和側(cè)向加速度的零偏，以降低模型非完整性的影響。所采用的測量變量為橫擺角速度、側(cè)向加速度和前輪轉(zhuǎn)角等易于獲取的狀態(tài)。結(jié)合車輛動(dòng)力學(xué)模型和運(yùn)動(dòng)學(xué)模型，定義狀態(tài)轉(zhuǎn)移方程為：
[xk=f（xk-1，uk-1）+wk-1] （16）
式中：[xk=（vyk，γk，θk，vyk，γk，θk，ayk，δfk，dγk，dayk）T]為估計(jì)器在離散時(shí)間k時(shí)刻的狀態(tài)變量，[uk-1]為控制變量，f（ ）為狀態(tài)轉(zhuǎn)移方程，wk-1為過程噪聲，dγk、dayk分別為橫擺角速度零偏、側(cè)向加速度零偏。
為便于后續(xù)說明，定義vy=vy1，γ=γ1。其中，狀態(tài)轉(zhuǎn)移方程的表達(dá)式為：
式中：T為離散時(shí)間步長。
定義側(cè)向速度估計(jì)器的測量方程為：
[zk=h（xk）+vk] （18）
其中：
[h1：γmk=γk+dγk-1h2：aymk=ayk+dayk-1h3：δfmk=δfk] （19）
式中：zk=[γmk，aymk，δfmk]T為k時(shí)刻的測量變量；
h（ ）為測量方程；
υk為測量噪聲；
γmk為橫擺角速度測量值；
aymk為側(cè)向加速度測量值；
δfmk為前輪轉(zhuǎn)角測量值，由轉(zhuǎn)向盤轉(zhuǎn)角轉(zhuǎn)換獲取。
3.2 基于運(yùn)動(dòng)學(xué)模型的反饋校正
當(dāng)半掛車輛行駛工況復(fù)雜多變時(shí)，不確定的工作環(huán)境可能會(huì)導(dǎo)致動(dòng)力學(xué)模型的失配，從而造成估算結(jié)果出現(xiàn)誤差。為進(jìn)一步降低模型非完整性的影響，在消除傳感器測量零偏的基礎(chǔ)上引入基于運(yùn)動(dòng)學(xué)模型的反饋校正，該反饋校正為基于狀態(tài)變量之間運(yùn)動(dòng)學(xué)關(guān)系以及估算值與測量值之間偏差引入的閉環(huán)調(diào)節(jié)，包括側(cè)向速度反饋調(diào)節(jié)和橫擺角速度反饋調(diào)節(jié)。
側(cè)向速度反饋調(diào)節(jié)是利用估算的側(cè)向速度與運(yùn)動(dòng)學(xué)積分得到的側(cè)向速度之間的偏差補(bǔ)償側(cè)向速度項(xiàng)。其中，側(cè)向速度偏差為：
[Δνy=νy-0t（aym-day）-νx（γm-dy）] （20）
式中：[vy]為側(cè)向速度估算結(jié)果， t為估算時(shí)間。
橫擺角速度反饋調(diào)節(jié)是利用估算的橫擺角速度與測量的橫擺角速度之間的偏差補(bǔ)償橫擺角速度項(xiàng)。其中，橫擺角速度偏差為：
[Δγ=γ-（γm-dγ）] （21）
因此，引入反饋校正后，式（17）的前2項(xiàng)可換算為：
[νyk=νyk-1+νyk-1T-kvyΔvyk-1] （22）
[γk=γk-1+γk-1T-kγΔγk-1] （23）
式中：kvy為側(cè)向速度項(xiàng)的調(diào)節(jié)增益，kγ為橫擺角速度項(xiàng)的調(diào)節(jié)增益。
3.3 SCKF估計(jì)算法
鑒于SCKF算法在估計(jì)精度和數(shù)值穩(wěn)定性方面的優(yōu)勢，選取該算法估算半掛車輛的側(cè)向速度。標(biāo)準(zhǔn)SCKF算法的實(shí)現(xiàn)過程如下[14]：
a. 初始化。給定狀態(tài)初始值[x0|0]，計(jì)算誤差協(xié)方差矩陣平方根因子的初始值[S0|0=P0|0]，其中P0|0為誤差協(xié)方差矩陣。
b. 時(shí)間更新。計(jì)算狀態(tài)容積點(diǎn)并基于狀態(tài)轉(zhuǎn)移方程傳遞：
[xik-1k-1=xk-1k-1+Sk-1k-1ξi， i=1，2，…，2nxi*kk-1=f（xik-1k-1，uk-1）] （24）
式中：ξi為容積點(diǎn)權(quán)重矩陣[[nIn，-nIn]]的第i列，In為n×n的單位矩陣，n為狀態(tài)變量的維數(shù)。
計(jì)算狀態(tài)變量的預(yù)測值及其誤差協(xié)方差矩陣的平方根因子：
[xkk-1=12ni=12nxi*kk-1Skk-1=qr（[Xi*kk-1，Qk-1]）] （25）
式中：Qk-1為過程噪聲協(xié)方差，[X*kk-1=12n[x1*kk-1-xkk-1，x2*kk-1-xkk-1，…，x2n*kk-1-xkk-1]]為加權(quán)中心矩。
c. 測量更新。更新狀態(tài)容積點(diǎn)并基于測量方程傳遞：
[xikk-1=xkk-1+Skk-1ξizikk-1=h（xikk-1）] （26）
計(jì)算測量變量的預(yù)測值及其新息協(xié)方差矩陣的平方根因子：
[zkk-1=12ni=12nzikk-1Szzkk-1=qr（[Zkk-1，Rk]）] （27）
式中：Rk為測量噪聲協(xié)方差，[Zkk-1=12n[z1kk-1-z1kk-1，z2kk-1-zkk-1，…，z2nkk-1-zkk-1]]為加權(quán)中心矩。
測量變量的協(xié)方差矩陣和互協(xié)方差矩陣分別為：
[Pzzkk-1=Szzkk-1（Szzkk-1）TPxzkk-1=Xkk-1ZTkk-1] （28）
式中：[Xkk-1=12n[x1kk-1-x1kk-1，x2kk-1-xkk-1，…，x2nkk-1-xkk-1]]為加權(quán)中心矩。
卡爾曼濾波增益矩陣為：
Kk=[Pxzkk-1（Pzzkk-1）-1] （29）
更新狀態(tài)變量估算值和誤差協(xié)方差矩陣的平方根因子為：
[xkk=xkk-1+Kk（zk-zkk-1）Skk=qr（[xkk-1Kkzk-zkk-1，KkRk]）] （30）
3.4 可觀性分析
在編程實(shí)現(xiàn)估計(jì)器前，需驗(yàn)證其可觀測性，這決定了是否可以從系統(tǒng)的輸入和外部輸出信息中正確推導(dǎo)出內(nèi)部狀態(tài)。對(duì)于非線性估計(jì)器而言，通常主要關(guān)注其在當(dāng)前狀態(tài)的局部可觀性，即利用離散線性時(shí)不變系統(tǒng)可觀性矩陣的秩來判斷非線性系統(tǒng)的局部可觀測性：
OLTI=[Hk HkFk … HkFk（n-1）T] （31）
式中：OLTI為線性時(shí)不變系統(tǒng)可觀性矩陣，若OLTI滿秩，則非線性系統(tǒng)局部可觀；
Fk、Hk分別為k時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣、測量矩陣，即f（ ）和h（ ）的雅克比矩陣。
Fk、Hk的表達(dá)式分別為：
[Hk=010000001000000010010000000100] （33）
利用MATLAB的Rank函數(shù)求解估計(jì)器可觀性矩陣OLTI的秩，輸出結(jié)果為10，證明所設(shè)計(jì)的估計(jì)器是局部可觀的。
4 仿真驗(yàn)證
為驗(yàn)證所提出的考慮測量零偏的半掛車輛側(cè)向速度校正估算方法的效果，基于TruckSim和MATLAB/Simulink在正弦輸入和階躍輸入2種工況下開展聯(lián)合仿真。其中，半掛車輛模型的主要參數(shù)為：m1=8 036 kg，Iz1=4.87×104 kg·m2，m2=22 570 kg，Iz2=3.94×105 kg·m2，a1=1.6 m，b1=2.05 m，a2=5.4 m，b2=3.4 m，c=1.5 m，kf=3.70×105 N/rad，kr=9.37×105 N/rad，kt=1.33×106 N/rad。分別在不考慮測量零偏和反饋校正、僅考慮測量零偏、同時(shí)考慮測量零偏和反饋校正的條件下開展估算，結(jié)果分別記作SCKF估計(jì)1、SCKF估計(jì)2、SCKF估計(jì)3。本文僅關(guān)注如何消除測量零偏，噪聲異常的處理采用前期研究的方法[9，15]。
4.1 正弦輸入工況
在正弦輸入仿真工況中，設(shè)置車速vx=80 km/h，前輪轉(zhuǎn)角δf輸入如圖2所示，周期為6 s。同時(shí)，設(shè)置側(cè)向加速度零偏為0.2 m/s2，用于模擬傳感器單一測量信號(hào)存在的零偏。
為評(píng)估所提出的考慮測量零偏校正估計(jì)方法的性能，選取側(cè)向速度、側(cè)向加速度、橫擺角速度、側(cè)向加速度零偏和橫擺角速度零偏的估算結(jié)果作為對(duì)比指標(biāo)。正弦輸入工況下，側(cè)向速度、側(cè)向加速度和橫擺角速度的估算結(jié)果如圖3所示。由圖3a可知，SCKF估計(jì)2和SCKF估計(jì)3可以準(zhǔn)確估算出半掛車輛的側(cè)向速度，而SCKF估計(jì)1的估算結(jié)果存在明顯偏差。這是因?yàn)镾CKF估計(jì)2和SCKF估計(jì)3在設(shè)計(jì)估計(jì)器時(shí)均考慮了傳感器的測量零偏。同時(shí)，觀察圖3a可以發(fā)現(xiàn)，考慮反饋校正的SCKF估計(jì)3的側(cè)向速度估算結(jié)果收斂速度和精度均優(yōu)于SCKF估計(jì)2。圖3b和圖3c同樣表明，SCKF估計(jì)3的估算結(jié)果更準(zhǔn)確。由于未考慮測量零偏，SCKF估計(jì)1的側(cè)向加速度估算值依然存在0.2 m/s2的偏差。因?yàn)楣浪隳Ｐ?a href="http://www.cnyhmy.com/doc/zhongkao/" target="_blank" class="keylink">中考慮了測量零偏，SCKF估計(jì)2和SCKF估計(jì)3的橫擺角速度估算結(jié)果也需要一定的收斂時(shí)間。
圖4所示為正弦輸入工況下側(cè)向加速度零偏和橫擺角速度零偏的估算結(jié)果。由圖4a和圖4b可知，SCKF估計(jì)2和SCKF估計(jì)3均能較為準(zhǔn)確地估算出側(cè)向加速度和橫擺角速度的測量零偏。由于SCKF估計(jì)3同時(shí)考慮了基于運(yùn)動(dòng)學(xué)模型的反饋校正，其收斂速度更快、估算精度更高，圖4a的前2 s更為明顯。因此，考慮測量零偏的半掛車輛側(cè)向速度校正估算方法不僅能夠估算出傳感器的測量零偏，還可以有效提高估算結(jié)果的收斂速度。
4.2 階躍輸入工況
為進(jìn)一步驗(yàn)證SCKF估計(jì)3對(duì)行駛工況和測量零偏的適應(yīng)性，進(jìn)行同時(shí)考慮多個(gè)測量零偏的階躍輸入工況仿真分析。設(shè)置車速vx=70 km/h，前輪轉(zhuǎn)角δf輸入如圖5所示。同時(shí)，設(shè)置側(cè)向加速度零偏和橫擺角速度零偏分別為0.2 m/s2和0.04 rad/s，用于模擬傳感器多個(gè)測量信號(hào)存在的零偏。
階躍輸入工況下，側(cè)向速度、側(cè)向加速度和橫擺角速度的估算結(jié)果如圖6所示。由圖6a可知，SCKF估計(jì)2和SCKF估計(jì)3同樣可以準(zhǔn)確估算出半掛車輛的側(cè)向速度。然而，SCKF估計(jì)1的估算結(jié)果不僅存在明顯的偏差，且收斂速度較慢。由圖6a可知，SCKF估計(jì)3的側(cè)向速度估算效果明顯優(yōu)于SCKF估計(jì)1和SCKF估計(jì)2。由圖6b和圖6c可知，SCKF估計(jì)2和SCKF估計(jì)3能夠有效消除側(cè)向加速度和橫擺角速度估算結(jié)果的測量零偏，由于SCKF估計(jì)1沒有考慮測量零偏，估算結(jié)果依然與其測量值一致。由圖6中的估算結(jié)果可知，SCKF估計(jì)3的整體性能最優(yōu)。
圖7所示為階躍輸入工況下側(cè)向加速度零偏和橫擺角速度零偏的估算結(jié)果。該工況下的SCKF估計(jì)2和SCKF估計(jì)3均能較為準(zhǔn)確地估算出側(cè)向加速度和橫擺角速度的測量零偏，且對(duì)測量零偏的差異均有較好的適應(yīng)性。但由圖7a和圖7b中前2 s數(shù)據(jù)可知，SCKF估計(jì)3的收斂速度和精度更好。
通過對(duì)比不同輸入工況和測量零偏的仿真結(jié)果可知，SCKF估計(jì)3可準(zhǔn)確估算出半掛車輛的側(cè)向速度，消除側(cè)向加速度和橫擺角速度的測量零偏，整體效果，尤其是收斂速度均優(yōu)于SCKF估計(jì)2。
5 結(jié)束語
本文針對(duì)自動(dòng)駕駛半掛車輛側(cè)向速度估算中的傳感器測量零偏問題，提出了一種結(jié)合動(dòng)力學(xué)模型和運(yùn)動(dòng)學(xué)模型的反饋校正估算方法。在SCKF算法框架下，利用基于車輛動(dòng)力學(xué)的估算模型消除側(cè)向加速度和橫擺角速度測量零偏的影響。為避免動(dòng)力學(xué)模型失配的影響，基于狀態(tài)變量之間的運(yùn)動(dòng)學(xué)關(guān)系，反饋調(diào)節(jié)了側(cè)向速度和橫擺角速度的估算結(jié)果。
TruckSim和MATLAB/Simulink聯(lián)合仿真結(jié)果表明，該方法能夠準(zhǔn)確估算出半掛車輛的側(cè)向速度，消除側(cè)向加速度和橫擺角速度的測量零偏，其整體性能，尤其是收斂速度，明顯優(yōu)于不考慮測量零偏和僅考慮測量零偏的估計(jì)方法。
參考文獻(xiàn)
[1]? ?SINGH K B， ARAT M A， TAHERI S. Literature Review and Fundamental Approaches for Vehicle and Tire State Estimation[J]. Vehicle System Dynamics， 2019， 57（11）：
1643-1665.
[2]? ?JIN X J， YIN G D， CHEN N. Advanced Estimation Techniques for Vehicle System Dynamic State：
A Survey[J]. Sensors， 2019， 19（19）：
4289.
[3]? ?LIU W， XIA X， XIONG L， et al. Automated Vehicle Sideslip Angle Estimation Considering Signal Measurement Characteristic[J]. IEEE Sensors Journal， 2021， 21（19）：
21675-21687.
[4]? ?WANG Z P， WU J Y， ZHANG L， et al. Vehicle Sideslip Angle Estimation for a Four-Wheel-Independent-Drive Electric Vehicle Based on a Hybrid Estimator and a Moving Polynomial Kalman Smoother[J]. Proceedings of the Institution of Mechanical Engineers， Part K Journal of Multi-Body Dynamics， 2019， 233（1）：
125-140.
[5]? ?張志達(dá). 基于車路狀態(tài)估計(jì)的智能汽車運(yùn)動(dòng)決策與控制方法研究[D]. 重慶：
重慶大學(xué)， 2021.
ZHANG Z D. Research on Motion Decision and Control of Intelligent Vehicle Based on Vehicle-Road State Estimation[D]. Chongqing：
Chongqing University， 2021.
[6]? ?SELMANAJ D， CORNO M， PANZANI G， et al. Vehicle Sideslip Estimation：
A Kinematic Based Approach[J]. Control Engineering Practice， 2017， 67（10）：
1-12.
[7]? ?李小雨， 許男， 郭孔輝. 基于運(yùn)動(dòng)學(xué)方法和運(yùn)動(dòng)幾何方法融合的質(zhì)心側(cè)偏角估計(jì)[J]. 機(jī)械工程學(xué)報(bào)， 2020， 56（2）：
121-129.
LI X Y， XU N， GUO K H. Vehicle Sideslip Angle Estimation Based on Fusion of Kinematic Method and Kinematic-Geometry Method[J]. Journal of Mechanical Engineering， 2020， 56（2）：
121-129.
[8]? REZAEIAN A， KHAJEPOUR A， MELEK W， et al. Simultaneous Vehicle Real-Time Longitudinal and Lateral Velocity Estimation[J]. IEEE Transactions on Vehicular Technology， 2017， 66（3）：
1950-1962.
[9]? ZHANG Z D， ZHNEG L， WU H， et al. An Estimation Scheme of Road Friction Coefficient Based on Novel Tyre Model and Improved SCKF[J]. Vehicle System Dynamics， 2022， 60（8）：
2775-2804.
[10] 耿國慶， 韋斌源， 江浩斌， 等. 基于NA-EKF的分布式驅(qū)動(dòng)電動(dòng)汽車行駛狀態(tài)估計(jì)研究 [J]. 汽車工程， 2018， 40（7）：
770-776.
GENG G Q， WEI B Y， JIANG H B， et al. A Research on Driving State Estimation for Distributed Drive Electric Vehicle Based on NA-EKF[J]. Automotive Engineering， 2018， 40（7）：
770-776.
[11] REINA G， MESSINA A. Vehicle Dynamics Estimation Via Augmented Extended Kalman Filtering[J]. Measurement， 2018， 133：
383-395.
[12] AMIN H K， AMIR K， BARIS F. Vehicle-Trailer Lateral Velocity Estimation using Constrained Unscented Transformation[J]. Vehicle System Dynamics， 2022， 60（3）：
1048-1075.
[13] PIYABONGKARN D， RAJAMANI R， GROGG J A， et al. Development and Experimental Evaluation of a Slip Angle Estimator for Vehicle Stability Control[J]. IEEE Transactions on Control Systems Technology， 2009， 17（1）：
78-88.
[14] ZHANG Z D， ZHENG L， LI Y N， et al. Cooperative Strategy of Trajectory Tracking and Stability Control for 4WID Autonomous Vehicles under Extreme Conditions[J]. IEEE Transactions on Vehicular Technology， 2023， 72（3）：
3105-3118.
[15] 張志達(dá)， 鄭玲， 李以農(nóng)， 等. 基于魯棒自適應(yīng)SCKF的智能汽車目標(biāo)狀態(tài)跟蹤研究[J]. 機(jī)械工程學(xué)報(bào)， 2021， 57（20）：
181-193.
ZHANG Z D， ZHENG L， LI Y N， et al. Research on Intelligent Vehicle Target State Tracking Based on Robust Adaptive SCKF[J]. Journal of Mechanical Engineering， 2021， 57（20）：
181-193.
（責(zé)任編輯 弦 歌）
修改稿收到日期為2024年5月8日。