中圖分類號：U469.5 文獻標志碼：A

Abstract：Aiming at the problem that some key dynamical states of tractor semi-trailer cannot be measured and the values ofsensors are interfered by random factors such as engine vibration noise，an improved particle filter is proposed toestimatethe dynamical states of the driving tractor semi-trailerinreal-time.This paper establishes a17 degrees of freedom dynamical modelof tractor semi-trailerfirst.Bycombining theparticle filter principleand the adaptive geneticalgorithm to enhance the particle diversity，the piecewise proposal distribution function is designed，and the systematic resampling method is used to suppress the particle regression.The in-time and accurate estimation of longitudinal speed，lateral speed，yawrate，and other states of tractor semi-trailer was realized.A hardware-in-the-loop（HIL）simulation test platform was builtto verifythealgorithm under dierent conditions.The testing results show that compared with the unscented particle filter algorithm，the improved particle filter algorithm proposed in this papercan realize the state estimationof the whole vehicle under both ideal and random noise environments，and has higher estimation accuracy.

Key Words：vehicle engineering;state estimation;particle filter;tractor semi-trailer

半掛汽車列車由于能夠在運輸驛站快速更換掛車，避免了貨物裝卸的等待時間，已成為道路運輸的主力車型.相比于四輪車輛，半掛汽車列車的車身更長，裝載后重心位置更高，因此存在更多的失穩(wěn)形式及更大的失穩(wěn)可能性[1-2].半掛汽車列車的整車穩(wěn)定性控制已成為自前車輛領域的研究熱點，而行駛過程中的動力學狀態(tài)實時獲取是影響穩(wěn)定性控制效果的關鍵因素.鑒于某些關鍵狀態(tài)難以通過現有的車載傳感器進行測量，研究人員已經開始采用光學傳感器來應對這一挑戰(zhàn)，例如利用攝像機來測量半掛汽車列車的鉸接角度等[3.然而，該類傳感器顯著提升了整車的制造成本[4]，更實用的解決方案是通過可測量得到的部分狀態(tài)信息結合數字濾波技術對不可測狀態(tài)進行估計.

目前估計方法主要分為試驗法和模型法5.試驗法包括參數回歸方法、神經網絡、基于規(guī)則的方法等.人工神經網絡已被應用于估算車輛的側傾角度-7.非線性最小二乘法作為參數回歸方法已被用于半掛汽車列車質量估計[8-9].試驗法雖然在實現手段上較為簡便，但是需要進行大量的預試驗，獲得足夠的數據集后方能保證估計精度.此外，大量的數據集對于整車控制器的存儲空間也提出了較高要求[.與試驗法相比，模型法無須預先標定，對整車控制器的存儲空間需求也較低.在眾多試驗法中，基于卡爾曼濾波原理的估計方法應用最為廣泛.標準卡爾曼濾波要求系統狀態(tài)方程是線性的，無法直接應用于車輛系統.擴展卡爾曼濾波理論通過雅可比矩陣的形式將非線性系統線性化來實現對非線性系統的估計，但是雅可比矩陣的復雜程度由系統狀態(tài)空間維度決定，當系統狀態(tài)空間維度較高時，其較為復雜，且線性化形式的系統與原始非線性系統誤差較大，這對于估計的精度和速度有一定的不良影響[11-12].無跡卡爾曼濾波（unscented Kalman filter，UKF）和容積卡爾曼濾波器（cubatureKalman filter，CKF）能夠應對高維非線性系統的狀態(tài)估計問題.UKF基于無跡變換生成樣本點，以此對非線性函數的概率密度分布進行近似[13-15].CKF則利用球面徑向規(guī)則求解后驗期望的積分近似[16-17].兩種方法均運用在車輛狀態(tài)估計問題中.但是UKF和CKF要求系統滿足高斯假設，而半掛汽車列車行駛過程中的發(fā)動機振動、車身振動等因素在傳感器信號中引發(fā)的噪聲屬于隨機噪聲，無法通過UKF或CKF濾除.針對此問題，粒子濾波（particlefilter，PF）將蒙特卡羅方法與貝葉斯估計相結合，通過粒子群逼近被估計狀態(tài)的后驗分布，對于受隨機噪聲影響的非線性系統具有更好的估計效果[18].標準粒子濾波算法在若干次迭代后會出現大量粒子權重過低，只有少量粒子具有非零權重的問題，研究人員稱之為粒子貧化現象．無跡粒子濾波（unscentedparticlefilter，UPF）通過UKF得到建議密度函數來逼近后驗分布，以達到降低粒子退化的目的，但是建議密度函數的準確性仍然受到系統噪聲不確定性的影響，當UPF應用于車輛時，隨機噪聲的影響導致其估計效果不一定優(yōu)于傳統 PF^[19-21] ：

目前已有通過智能優(yōu)化算法的手段避免粒子貧化現象的先例[22]，本文提出一種IPF（improved particlefilter）狀態(tài)估計算法，使用自適應遺傳算法對粒子進行自適應交叉、變異、遺傳操作，以保障粒子多樣性，通過設計分段提議分布函數加強粒子權值區(qū)分，并采用系統重采樣方法避免對權重較低粒子的盲目丟棄，以此形成改進的粒子濾波算法，并將其應用于解決半掛汽車列車的狀態(tài)實時估計問題中.本文搭建硬件在環(huán)仿真試驗臺架對算法進行驗證，與作對照的UPF算法相比，理想環(huán)境及隨機噪聲環(huán)境下的改進粒子濾波算法均能實現對半掛汽車列車行駛過程中的狀態(tài)實時估計，并保證一定的估計精度.

1半掛汽車列車動力學建模

針對三軸牽引車與三軸掛車鉸接的六軸半掛汽車列車，在綜合考慮模型精度與計算量的基礎上，本文搭建如圖1所示的17自由度整車動力學模型，17個自由度分別為牽引車的縱向、側向、橫擺自由度，掛車的橫擺自由度，牽引車前輪的轉向自由度以及12個輪胎各自的轉動自由度.圖1中： 0₁，O₂ 分別為牽引車及掛車質心位置，車輛坐標系分別為X₁O₁Y₁，X₂O₂Y₂ ， F_xi 和 為各輪胎受到的縱向力與側向力，輪胎下標i為 1，2，…，12 ，在圖1中標注出了輪胎標號及各物理量的方向.

1.1牽引車動力學方程

牽引車的縱向、側向、橫擺運動可由如下公式

式中： v_x1?v_y1 和 γ₁ 分別為牽引車縱向、側向速度和橫擺角速度；8為前輪轉角； F_ix，F_ix 分別為掛車對牽引車鉸接力在側向、縱向的分力； m₁ 為牽引車總質量； a₁ 為牽引車質心到前軸的距離； b₂ 為牽引車質心到牽引車2軸的距離； b₃ 為牽引車質心到牽引車3軸的距離； t_f1 為牽引車前軸輪距； t_r1 為牽引車后軸輪距； I_zl 為牽引車總轉動慣量； l_p 為牽引車質心到鉸接點距離.

1.2掛車動力學方程

掛車的縱向、側向、橫擺運動可由如下公式描述：

式中： v_x2、v_y2 和 γ₂ 分別為掛車縱向、側向速度和橫擺角速度； F_jx^′?F_jy^′ 分別為牽引車對牽引車鉸接力在側向、縱向的分力； a₂ 為掛車質心到鉸接點的距離； b₄ 為掛車質心到掛車1軸的距離； b_s 為掛車質心到掛車2軸的距離； b₆ 為掛車質心到掛車3軸的距離； t_r2 為掛車后軸輪距； m₂ 為掛車總質量； I_z2 為掛車總轉動慣量.

1.3鉸接點動力學方程

牽引車與掛車之間通過鞍座形成鉸接，兩者的縱向運動、側向運動、橫擺運動在鉸接點處存在一定的約束關系，約束方程表達如下：

式中： θ 為鉸接角.

對式（7）式（8）求導可得掛車縱側向加速度：

1.4輪胎動力學方程

輪胎側向力與側偏角的關系存在線性與非線性區(qū)域，側偏角小于 5^° 時為線性，大于 5^° 時為非線性.魔術公式對不同側偏角范圍內輪胎側向力的擬合精度較好[23].本文使用魔術公式對輪胎的縱向力與側向力進行計算，非聯合滑移工況下的輪胎縱向力與側向力如下：

式中：i為各輪胎對應編號； B_i 為各輪胎剛度因子； C_i 為各輪胎的形狀因子； D_zi 為各輪胎的峰值輪胎力因子； E_i 為各輪胎的曲率因子，上述因子通過試驗數據擬合魔術公式即可得出； s_i 為輪胎滑移率； α_i 為輪胎側偏角.12輪的滑移率依次定義為：

12輪的側偏角依次定義為：

輪胎的動力學方程為：

式中： I_i?R_i?ω_i?T_ii，T_bi 分別為對應下標車輪 i 的轉動慣

量、滾動半徑、角速度、驅動力矩和制動力矩.在聯合滑移工況下，輪胎的縱向力和側向力方程為：

2IPF狀態(tài)估計算法

基于上一章建立的17自由度動力學模型的半掛汽車列車狀態(tài)估計算法原理如圖2所示.半掛汽車列車目前通用的傳感器包括輪速傳感器和分別安裝在牽引車和掛車上的慣性導航傳感器，用于測量各輪胎轉速、牽引車和掛車的縱向加速度、側向加速度、橫擺角速度，結合上述傳感器測量值對整車行駛狀態(tài)進行實時估計.

前輪轉角 前輪轉角 狀態(tài)動7由度 縱向加速度 傳感器 量量 改進粒估計值滑移率 輪胎力 速度車 算法魔術公式輪胎模型 各車輪轉速輪速傳感器整車模型 狀態(tài)觀測量

標準粒子濾波算法根據重要性概率密度函數對上一時刻估計的系統狀態(tài)進行隨機抽樣，在得到附帶對應權值的隨機樣本后，根據狀態(tài)方程對系統觀測變量進行預測，然后根據當前時刻的實際觀測結果對權值進行調節(jié)，使粒子狀態(tài)獲得更新并逼近真實狀態(tài)的后驗分布，最終將粒子的加權均值作為估計結果進行輸出.與其他的估計方法相比，粒子濾波估計不要求狀態(tài)轉移函數及觀測函數是線性的，也不需要噪聲滿足正態(tài)分布，但是其迭代過程中存在著粒子退化的問題.本文使用自適應遺傳算法對粒子群進行多樣化操作，以提升粒子多樣性，抑制粒子退化趨勢，此外，采用系統重采樣方法進一步增加粒子多樣性.最終得到的改進粒子濾波算法流程如圖3所示.

具體的實施方法如下：

1）初始化和粒子采樣，在初始采樣時刻依據先

驗分布選擇粒子.

式中： x_{s，swarm，0} 為車輛狀態(tài)估計過程中的初始狀態(tài)粒子群； x_s，0 為狀態(tài)初始向量； N 為粒子數； ζ 為根據經驗設定的各個狀態(tài)變量的粒子散布范圍.

2）權重更新.執(zhí)行更新過程重要性評價，通過狀態(tài)方程對車輛狀態(tài)進行預測：

式中 ?f 為半掛汽車列車的狀態(tài)函數； u_k-1 為上一采樣時刻輸入.分別更新各個粒子的均值和標準差：

式中： 和 σ_s（i） 分別為第 i 個粒子群的加權均值和標準差； w_{s，swarm，k-1} 為上一采樣時刻粒子群權重.已有文獻通過設計提議分布函數的方法來提升粒子濾波性能[24]，借鑒該思想，本文通過分段提議分布函數區(qū)分不同權重粒子之間的差距以進一步提升濾波效果，如圖4所示，粒子越接近粒子群期望值時，其權重越大.

基于此，對粒子的更新過程權重及其歸一化權重計算如下：

分段提議分布函數中，參數 k₁=0.4，k₂=2.5，k₃=3

更新權重過程中，如圖3所示，閾值設定為初始粒子散布范圍的 k_s 倍，該閾值過大會導致粒子過快貧化，而過小則會導致算法無法收斂，本文閾值取0.02.當粒子群的標準差低于設定閾值時，在粒子群期望不變的前提下通過自適應遺傳算法對其進行選擇、交叉、變異操作，三種操作概率分別為 p_sel ， p_co p_mu. 選擇操作如下：

交叉操作如下：

式中： x_f 及 x_m 分別為交叉公粒子及交叉母粒子，利用交叉操作后的粒子計算后驗概率分布，若大于 p （z_k|x_m，k） 及 p（z_k|x_f，k） 則選擇該生成粒子，否則選擇后驗概率分布較大的原始粒子.

變異操作如下：

式中： x_mu 為變異粒子，利用變異操作后的粒子計算后驗概率分布，若大于 p（z_k|x_mu） 則選擇該生成粒子，否則選擇原始粒子.上述過程中， k_sel=0.5 ， k_co=0.5 k_mu=1.5. 自適應遺傳算法通過粒子的權重調節(jié)交叉

操作和遺傳操作的概率：

式中： w_co，1 及 w_co，2 分別為參與交叉操作的粒子權重；w_mu 為參與遺傳操作的粒子權重； w_max，w_min 及 w_avg 分別為最大權值、最小權值及權值均值.

然后進行觀測過程重要性評價，將粒子代入系統觀測方程后求得觀測粒子群，并以傳感器信號作為參考對觀測粒子群進行重要性評價，求解觀測過程的粒子群權重 w_{s，swarm，obs} ，最終對粒子群權重進行歸一化，如下所示：

式中： w_{s，swarm，i} 為粒子群歸一化后的權重.

3）輸出和重采樣.

得到權重后輸出濾波結果：

通過粒子群有效粒子數判斷其是否已退化到需要進行重采樣，有效粒子數計算方法如下：

當 N_efflt;2N/3 時，認為粒子群發(fā)生退化，使用系統重采樣方法，其原理如圖5所示，其偽代碼如圖6所示，

系統重采樣方法在按照權重劃分的區(qū)間中使用隨機數進行分層后采樣.其能夠確保采樣覆蓋所有權重區(qū)以及大權重區(qū)的更高采樣頻率.

3硬件在環(huán)仿真試驗驗證

為驗證本文所提出IPF估計方法的有效性，本文搭建了半掛汽車列車硬件在環(huán)試驗平臺，如圖7所示，在LabVIEW環(huán)境下編寫IPF估計算法并植入快速控制原型NIcRIO-9038中.NIcRIO-9038通過CAN信號方式采集NIPXI中牽引車及掛車的加速度等信息，并通過計算機實時顯示控制器前面板以及采集得到的信號.NIPXI中運行Trucksim半掛汽車列車模型，整車關鍵參數如表1所示.

基于所搭建臺架仿真試驗，對本文所提出的IPF估計效果進行驗證，硬件在環(huán)仿真試驗的算法采樣時間為 0.01s ，設置UPF作為對照，采用均方根誤差（root mean square error，RMSE）對IPF和UPF的估計精度進行表征.

3.1理想環(huán)境下估計效果驗證

驗證IPF在理想行駛環(huán)境下對半掛汽車列車進 行實時狀態(tài)估計的有效性.雙移線滑行工況下，半

掛汽車列車初始速度為 72km/h ，附著系數為0.8.兩種工況的仿真結果如圖8所示.

理想條件仿真的RMSE如表2所示，綜合各個狀態(tài)估計曲線可看出，在無噪聲的理想環(huán)境下，設計的IPF、UPF均能夠完成狀態(tài)估計.

3.2隨機噪聲干擾條件下估計效果驗證

驗證IPF在隨機噪聲環(huán)境下的估計效果，由于輪速傳感器和前輪轉角傳感器在工程實踐中往往已在傳感電路中采用濾波手段以保證其信號可用性，因此本文只在牽引車慣性導航器及掛車慣性導航器中加入隨機噪聲信號對車輛的實際行駛環(huán)境進行模擬，以此進行硬件在環(huán)仿真試驗.具體地，在橫縱向加速度信號中加入 1Hz 幅值 0.5m/s² 的車輛低頻振動模擬噪聲 ，50Hz 幅值 0.25m/s² 的車輛中頻振動模擬噪聲、 1000Hz 幅值 0.02m/s² 的高頻環(huán)境模擬噪聲；在橫擺角速度信號中加入 1Hz 幅值 0.4rad/s 的車輛低頻振動模擬噪聲 .50Hz 幅值 0.3rad/s 的車輛中頻振動模擬噪聲 .1000Hz 幅值 0.002rad/s 的高頻環(huán)境模擬噪聲.雙移線滑行工況下的仿真結果如圖9所示，估計的RMSE如表3所示.

由圖9及表3可看出，IPF能夠保持狀態(tài)及參數 估計結果收斂較好.UPF的狀態(tài)估計結果發(fā)散，而

IPF估計的整體精度較高.分析產生誤差的原因是UPF通過無跡卡爾曼濾波獲得狀態(tài)的后驗分布，當傳感器信號包含較大的隨機噪聲時，由于不滿足高斯分布，其無法發(fā)揮原有的濾波性能.隨機噪聲環(huán)境下的試驗結果驗證了IPF在隨機噪聲環(huán)境下的魯棒性和有效性.

4結論

1）基于半掛汽車列車及魔術公式輪胎模型搭建了半掛汽車列車的17自由度動力學模型，采用目前通用的車載傳感器參數作為測量值，實現對牽引車縱向車速、側向車速、橫擺角、橫擺角速度、鉸接角、鉸接角速度的估計，具有成本低、易于實施的特點，為解決半掛汽車列車的狀態(tài)估計問題提供一種思路.

2）基于自適應遺傳算法，結合粒子濾波提升粒子多樣性，通過設計分段提議分布函數提升粒子權值差距，使用系統重采樣進一步抑制粒子貧化趨勢，綜合上述手段對標準粒子濾波進行改進并搭建硬件在環(huán)仿真試驗平臺進行驗證.硬件在環(huán)仿真試驗表明了基于IPF算法的半掛汽車列車狀態(tài)估計在理想環(huán)境和環(huán)境隨機噪聲環(huán)境下均能保持較高的估計精度.

參考文獻

[1]DINGNG，SHIXB，ZHANGYP，etal．Analysis of bifurcation and stability fora tractor semi-trailerin planar motion[J]. Vehicle SystemDynamics，2014，52（12）：1729-1751.

[2] MENGFY，SHI SM，BAI MH，etal.Dissipation of energy analysisapproachforvehicleplanemotionstability[J].Vehicle SystemDynamics，2022，60（12）：4035-4058.

[3] DESAXE C，CEBON D. Camera-based articulation angle sensing for heavygoods vehicles[J].IEEE Transactionson Vehicular Technology，2021，70（8）：7522-7535.

[4] 邢德鑫，魏民祥，趙萬忠，等，基于自適應容積粒子濾波的車 輛狀態(tài)估計[J]．南京航空航天大學學報，2020，52（3）： 445-453. XINGDX，WEIMX，ZHAO WZ，et al. Vehicle state estimation based onadaptive cubature particle filtering［J]．Journal of Nanjing University of Aeronauticsamp; Astronautics，2O2O，52（3）： 445-453.（inChinese）

[5]HABIBNEJAD KORAYEM A，KHAJEPOUR A，FIDAN B.A reviewonvehicle-trailer state and parameter estimation[J]. IEEE Transactions on Intelligent Transportation Systems，2022， 23（7）：5993-6010.

[6]GARCIA GUZMAN J，PRIETO GONZALEZ L，PAJARES REDONDOJ，etal．Real-time vehicle roll angleestimation based on neural networks in IoT low-cost devices[J]．Sensors， 2018，18（7）： 2188.

[7]CHU Z Z，ZHU D Q，YANG S X. Observer-based adaptive neural network trajectory tracking control forremotelyoperated vehicle[J]. IEEE Transactions on Neural Networks and Learning Systems，2017，28（7）：1633-1645.

[8]KOBER W，HIRSCHBERG W. On-board payload identification for commercial vehicles[C]//2Oo6 IEEE International Conference onMechatronics.July3-5，2O06，Budapest，Hungary.IEEE， 2006：144-149.

[9]KORAYEM A H，KHAJEPOUR A，FIDAN B. Trailer mass estimation using system model-based and machine learning approaches [J]． IEEE Transactions on Vehicular Technology， 2020，69（11）：12536-12546.

[10]PETSCHNIGG C，PILZ J. Uncertainty estimation in deep neural networks for point cloud segmentation in factory planning［J]. Modelling，2021，2（1）：1-17.

[11] AHMADI JEYED H，GHAFFARI A. Nonlinear estimator design based on extended Kalman filter approach for state estimation of articulated heavy vehicle[J]．Proceedings of the Institution of Mechanical Engineers，PartK：Journal of Multi-bodyDynamics， 2019，233（2）：254-265.

[12]PAMIES-VILAR，FONT-LLAGUNESJM，LUGRISU，et al. Parameter identification method for a three-dimensional footground contact model[J]. Mechanism and Machine Theory，2014， 75：107-116.

[13]MORRISON G，CEBON D. Sideslip estimation for articulated heavyvehicles at the limits of adhesion[J].Vehicle System Dynamics，2016，54（11）：1601-1628.

[14］周兵，李濤，吳曉建，等.基于雙自適應無跡卡爾曼濾波的 半掛車狀態(tài)估計[J].湖南大學學報（自然科學版），2022， 49（2）：63-73. ZHOUB，LI T，WUXJ，etal.Semi-trailer state estimation based on double adaptive unscented kalman filter[J]. Journal of HunanUniversity（Natural Sciences），2022，49（2）：63-73. （in Chinese）

[15］周兵，田晨，宋義彤，等．基于路面附著系數估計的AFS控制 策略研究[J]．湖南大學學報（自然科學版），2017，44（4）： 16-22. ZHOUB，TIAN C，SONGY T，et al. Control strategy of AFS based on estimation of tire-road friction coefficient[J]. Journal of Hunan University（Natural Sciences），2017，44（4）：16-22.（in Chinese）

[16]YEJ，ZHANG Z，JINJ，et al.Estimation of tire-road friction coefficient with adaptive tire stiffness based on RC-SCKF[J]. NonlinearDynamics，2023，112：945-960.

[17]LIUYM，ZHANGRY，SHIPC，et al.Distributed electric vehicle state parameter estimationbased on the ASO-SRGHCKF algorithm[J]. IEEE Sensors Journal，2022，22（19）：18780- 18792.

[18]SUN S X，ZHANG N，WALKERP，etal． Intelligent estimation forelectric vehicle mass with unknown uncertainties based on particle filter[J].IETIntelligent Transport Systems，202O，14（5）： 463-467.

[19]LITC，CORCHADO JM，BAJO J，et al．Effectiveness of Bayesian filters：an information fusion perspective[J]. Information

Sciences，2016，329：670-689.

[20］陳昱衡，吳鴻云，邊有鋼.基于AUPF算法的水下履帶車動力學參數估計[J].湖南大學學報（自然科學版），2022，49（8）：29-35.CHENY H，WUHY，BIAN YG.Estimation of motionparameters of aunderwater track mining vehicle based onadaptiveunscented particle filter algorithm[J]. Journalof Hunan University（NaturalSciences），2022，49（8）：29-35.（inChinese）

[21]龍云澤，韋韜，封進，等.四輪驅動EV自適應抗差無跡粒子濾波狀態(tài)估計[J].湖南大學學報（自然科學版），2022，49（2）：31-37.LONGYZ，WEI T，FENGJ，et al.Estimation of adaptive robustunscented particle filter state for four-wheel driving EV[J].Journal of Hunan University（Natural Sciences），2022，49（2）：31-37.（in Chinese）

[22］韓錕，張赫．基于果蠅優(yōu)化算法改進的粒子濾波及其在目標跟蹤中的應用[J]．湖南大學學報（自然科學版），2018，45（10）：130-138.HANK，ZHANGH.Improved particlefilterbased on fruit flyoptimizationand itsapplication in target tracking[J].Journal ofHunan University（Natural Sciences），2018，45（10）：130-138.（in Chinese）

[23]BAKKERE，PACEJKAHB，LIDNERL．A new tire model withanapplication in vehicle dynamicsstudies[C]//SAE TechnicalPaper Series．SAE International，1989：101-110.

[24]ZHU JJ，WANG ZP，ZHANG L，et al.State and parameterestimation based ona modified particle filterfor an in-wheel-motor-drive electric vehicle[J].Mechanismand Machine Theory，2019，133：606-624.