基于擴展卡爾曼濾波算法的港口智能無人內集卡狀態估計研究

楊 拯 曾小偉 雷丁瑞 黃雨晴

1 中國交通物資有限公司 2 武漢航科物流有限公司 3 武漢理工大學交通與物流工程學院

1 引言

港口內集裝箱卡車運輸效率是影響港口作業效率的關鍵因素之一,準確實時地獲取智能無人內集卡的狀態參數,是保證其行駛安全穩定的前提。但無人內集卡的有些參數,難以測量甚至不可測量,需要尋找一種更好的獲取參數的方法[1]。吳志成等采用卡爾曼濾波算法,建立車輛坐標系搭建二自由度線性車輛模型,對轉向系統所需的質心側偏角和橫速度2個參數進行仿真估計[2];安秋蘭等首先在Matlab軟件的Simulink中設計擴展卡爾曼濾波器,接著通過Carsim軟件模擬車輛模型并輸出Simulink的算法中需要的狀態參數,最后通過2個軟件聯合仿真來檢驗算法的準確性以及魯棒性[3];郝亮等采用擴展卡爾曼濾波算法的基本理論,并使用現階段可以使用的測量儀器如傳感器測得的所需的觀測參數,實現在汽車行駛狀態下對車輛狀態參數的較為精確的估計[4];Liu等主要針對四輪驅動的車輛,采用擴展卡爾曼濾波的相關原理與最小模型誤差的相關理論設計出一種新型算法,通過與擴展卡爾曼濾波算法比較可以明顯發現參數估計的精確度提升[5];Chen采用擴展卡爾曼濾波算法在車輛行駛過程中對車輛的側偏角進行研究,通過不同的路面參數以及工況設計的設置進行算法的驗證[6];劉麗等建立車輛三自由度系統模型以及采用輪胎模型,研究縱向加速度對車輛系統穩定性的影響[7];張航星等主要對車輛的狀態參數以及路面附著系數進行估計,在雙移線工況下采用擴展卡爾曼濾波算法的相關理論,在不同的路面附著系數條件下進行仿真分析[8]。

目前,國內主要針對二軸汽車的狀態參數估計算法進行研究,對港口多軸智能無人內集卡狀態參數估計相關算法的研究比較少,同時相關研究中參數估計算法的所需參數較少,模型建立不夠完善。為此,針對港口多軸智能無人內集卡進行研究,建立內集卡動力學模型和魔術輪胎模型并設計擴展卡爾曼濾波算法,對貨車狀態參數進行估計,并通過Simulink軟件與Trucksim軟件聯合仿真,驗證擴展卡爾曼濾波算法在港口內集卡狀態參數估計的實時性與準確性。

2 狀態估計模型的建立

2.1 動力學模型

為保證擴展卡爾曼濾波算法狀態參數估計的準確性,需建立合適的內集卡動力學模型,對智能無人內集卡結構進行整合,將多軸內集卡簡化為三軸模型,并對三軸內集卡進行受力分析,以此為基礎建立內集卡車體坐標系模型,并以此設計擴展卡爾曼濾波器(見圖1)。坐標系中的坐標原點選擇在內集卡的車體質心,坐標軸X的正方向是內集卡的前進方向,坐標軸Y在內集卡的車體質心處與坐標軸X垂直。除此之外,還需作以下假設:忽略內集卡行駛過程中的俯仰運動和側傾運動以及垂直運動;忽略空氣阻力等影響;忽略車輛懸梁影響;認為轉向系統是剛性的。

圖中VX為內集卡縱向速度,VY為內集卡側向速度,a、b、c為內集卡前中后三軸到質心的距離,δ為輪胎轉向角,γ為內集卡橫擺角速度,FXL1、FXL2、FXL3分別為內集卡1軸、2軸、3軸的左側輪胎的縱向力,FYL1、FYL2、FYL3分別為內集卡1軸、2軸、3軸的左側輪胎的側向力,FXR1、FXR2、FXR3分別為內集卡1軸、2軸、3軸的右側輪胎的縱向力,FYR1、FYR2、FYR3分別為內集卡1軸、2軸、3軸的右側輪胎的側向力,T1、T2、T3分別為內集卡車輪之間的距離。受力分析可得到狀態方程:

(1)

內集卡的質心側偏角可由內集卡縱向速度與側向速度計算獲得:

(2)

到此內集卡所需的狀態參數變量VX、γ、β的相應的方程已經建立。

2.2 輪胎模型

車輛行駛過程中大部分力都是通過輪胎進行傳遞,建立較為準確的輪胎模型對內集卡模型有關鍵的作用。魔術輪胎模型是以三角函數組合的形式來擬合輪胎試驗數據,得出一套可以同時表達縱向力和側向力的輪胎模型。在研究中使用的是Pacejka89輪胎模型。

在穩態純縱滑工況的條件下,內集卡行駛過程中的魔術輪胎Pacejka89模型的縱向力表達式為:

FX0=DXsin(CXarctan(BXSX0-

EX(BXSX0-arctan(BXSX0))))+SVX

(3)

式中,SX0=SX+SHX為縱向力組合自變量;SX為輪胎縱向滑動率,SX>0時為驅動滑轉率,SX<0時為制動滑移率;CX為縱向力曲線形狀因子;DX為縱向力峰值因子;BX為縱向力曲線剛度因子;BXCXDX為縱向力零點處的縱向剛度;EX為縱向力曲率因子;SVX為縱向力曲線垂直偏移因子;SHX為縱向力曲線水平偏移因子。

根據建立的縱向力方程,可知根據輪胎縱向滑移率與其垂向載荷可以估算輪胎模型的縱向力,并以此為基礎構建車輛動力學方程,將方程的輸出結果應用到算法之中進行仿真。

在穩態純側偏工況的條件下,在內集卡行駛過程中的魔術輪胎Pacejka89模型的側向力表達式為:

FY0=DYsin(CYarctan(BYaY-EY(BYaY-

arctan(BYaY))))+SVY

(4)

式中,αY=α+SHY,為側向力組合自變量;α為輪胎側偏角;CY為側向力曲線形狀因子;BY為側向力曲線剛度因子;DY為側向力峰值因子;BYCYDY為側向力零點處的側向剛度;EY為側向力曲率因子;SVY為側向力曲線垂直偏移因子;SHY為側向力曲線水平偏移因子。

根據建立的側向力方程,可根據輪胎外傾角和側偏角與其垂向載荷估算輪胎模型的側向力,并以此為基礎構建車輛動力學方程,并將方程的輸出結果應用到算法之中進行仿真。

本次研究所需的輪胎力輸出變量由內集卡狀態參數輸入計算可得,輸入輸出變量的表達過程見圖2。

圖2 魔術輪胎模型的輸入輸出變量

3 擴展卡爾曼濾波器設計

3.1 EKF算法

傳統的卡爾曼濾波是通過對輸入參數的一系列計算,使用貝葉斯定理計算后驗概率分布,對狀態參數進行較為準確的估計。而擴展卡爾曼濾波算法是將非線性函數在某一采樣估計點的附近進行一階泰勒展開,在不考慮展開式的高階項的條件下簡化得到線性化的系統,之后按照卡爾曼濾波算法的流程原理完成對系統狀態參數的最優估計。為了對智能無人內集卡的狀態參數進行更為準確的估計,采用擴展卡爾曼濾波算法完成所需參數的估計。

將貨車動力學模型和魔術輪胎模型組合后,再將其離散化表示,最后形成的離散化狀態方程為:

X(k+1)=f[k,X(k)]+G(k)W(k)BY

(5)

觀測方程可表示為:

Z(k)=h[k,X(k)]+V(k)

(6)

此時假定沒有控制量參數的輸入,過程噪聲是均值為0的白噪聲,觀測噪聲同樣也是均值為0的白噪聲,同時過程噪聲和觀測噪聲彼此獨立,以便進行數據處理?？柭鼮V波算法的迭代過程如下。

(1)狀態預測:

(7)

(2)誤差協方差預測:

P(k+1|k)=A(k+1|k)P(k|k)·

AT(k+1|k)+Q(k+1)

(8)

(3)濾波增益:

K(k+1)=P(k+1|k)HT(k+1)·

(9)

(4)狀態更新:

(10)

(5)誤差協方差更新:

P(k+1)=[I-K(k+1)H(k+1)]P(k+1|k)

(11)

3.2 狀態轉移矩陣

根據上文提到的狀態方程,令X=[VX;VY;γ]T作為狀態參數。接著將狀態參數按采樣時間離散化表示后,將當前時刻的X與前一時刻的狀態參數建立方程可得:

(12)

擴展卡爾曼濾波算法公式中需要的A矩陣如式(13)所示,即狀態轉移矩陣,為采用此算法提供了矩陣參數,是設計濾波算法的基礎。

(13)

3.3 觀測矩陣

觀測方程是求解所需變量的方程。在貨車狀態參數估計中Z是由Vx、ay、γ三個所需變量組成,故所需的觀測方程如式(14)所示,所需的經過離散化后的觀測矩陣如式(15)所示。擴展卡爾曼濾波算法公式中需要的H矩陣就是此觀測矩陣,為此算法提供了矩陣參數,是設計濾波算法的基礎。

(14)

(15)

4 仿真與結果分析

在Simulink中使用Function模塊編寫魔術輪胎模型與擴展卡爾曼濾波算法,得到關于貨車的狀態參數估計。將Trucksim的實際輸出值與擴展卡爾曼濾波算法得出的估算值進行對比,驗證擴展卡爾曼濾波算法的準確性和參數估計的可行性。Simulink模型見圖3,算法所需要的部分參數見表1。

表1 車輛模型部分參數

圖3 聯合仿真模型

為了驗證EKF算法的準確性與可行性,選擇縱向速度不變條件下蛇形工況進行仿真驗證。在Trucksim中設置仿真工況參數,其中縱向速度保持為70 km/h,采樣時間T=0.001 s,仿真時間為12 s。

通過仿真,系統實際輸出參數與通過擴展卡爾曼濾波算法估算出的參數之間的誤差見表2。

表2 真實值與估計值的比較

由表2可知,擴展卡爾曼濾波有著較好的預測效果。其中縱向速度和橫擺角速度具有較好的一致性,而質心側偏角初始的誤差較大,但隨時間變化估計值趨于穩定,可以得到較為準確的估計值。在蛇形工況下仿真出的3種參數的估算值在實時性與穩定性上較好。

5 結語

針對港口智能無人內集卡難以直接測量的行駛狀態參數的估計問題,提出了一種基于擴展卡爾曼算法的間接測量方法,構建了較為完善的魔術輪胎模型與三自由度非線性的三軸貨車模型,針對內集卡的縱向車速,橫擺角速度和質心側偏角參數進行估計。采用Simulink軟件與Trucksim軟件聯合仿真的方式,模擬內集卡的行駛工況。對比數據可以得出,利用較容易測得的輪胎力、縱向速度等參數估計難以直接測量的質心側偏角等參數,擴展卡爾曼算法有著較好的預測效果,同時預測準確性與實時性較高。該間接測量方法為智能無人內集卡狀態參數估計提供了一種可行、準確的方案,在一定程度上解決了內集卡安全行駛過程中的信息不完全問題。