楊 拯 曾小偉 雷丁瑞 黃雨晴

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

1 引言

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

目前,國(guó)內(nèi)主要針對(duì)二軸汽車(chē)的狀態(tài)參數(shù)估計(jì)算法進(jìn)行研究,對(duì)港口多軸智能無(wú)人內(nèi)集卡狀態(tài)參數(shù)估計(jì)相關(guān)算法的研究比較少,同時(shí)相關(guān)研究中參數(shù)估計(jì)算法的所需參數(shù)較少,模型建立不夠完善。為此,針對(duì)港口多軸智能無(wú)人內(nèi)集卡進(jìn)行研究,建立內(nèi)集卡動(dòng)力學(xué)模型和魔術(shù)輪胎模型并設(shè)計(jì)擴(kuò)展卡爾曼濾波算法,對(duì)貨車(chē)狀態(tài)參數(shù)進(jìn)行估計(jì),并通過(guò)Simulink軟件與Trucksim軟件聯(lián)合仿真,驗(yàn)證擴(kuò)展卡爾曼濾波算法在港口內(nèi)集卡狀態(tài)參數(shù)估計(jì)的實(shí)時(shí)性與準(zhǔn)確性。

2 狀態(tài)估計(jì)模型的建立

2.1 動(dòng)力學(xué)模型

為保證擴(kuò)展卡爾曼濾波算法狀態(tài)參數(shù)估計(jì)的準(zhǔn)確性,需建立合適的內(nèi)集卡動(dòng)力學(xué)模型,對(duì)智能無(wú)人內(nèi)集卡結(jié)構(gòu)進(jìn)行整合,將多軸內(nèi)集卡簡(jiǎn)化為三軸模型,并對(duì)三軸內(nèi)集卡進(jìn)行受力分析,以此為基礎(chǔ)建立內(nèi)集卡車(chē)體坐標(biāo)系模型,并以此設(shè)計(jì)擴(kuò)展卡爾曼濾波器(見(jiàn)圖1)。坐標(biāo)系中的坐標(biāo)原點(diǎn)選擇在內(nèi)集卡的車(chē)體質(zhì)心,坐標(biāo)軸X的正方向是內(nèi)集卡的前進(jìn)方向,坐標(biāo)軸Y在內(nèi)集卡的車(chē)體質(zhì)心處與坐標(biāo)軸X垂直。除此之外,還需作以下假設(shè):忽略?xún)?nèi)集卡行駛過(guò)程中的俯仰運(yùn)動(dòng)和側(cè)傾運(yùn)動(dòng)以及垂直運(yùn)動(dòng);忽略空氣阻力等影響;忽略車(chē)輛懸梁影響;認(rèn)為轉(zhuǎn)向系統(tǒng)是剛性的。

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

(1)

內(nèi)集卡的質(zhì)心側(cè)偏角可由內(nèi)集卡縱向速度與側(cè)向速度計(jì)算獲得:

(2)

到此內(nèi)集卡所需的狀態(tài)參數(shù)變量VX、γ、β的相應(yīng)的方程已經(jīng)建立。

2.2 輪胎模型

車(chē)輛行駛過(guò)程中大部分力都是通過(guò)輪胎進(jìn)行傳遞,建立較為準(zhǔn)確的輪胎模型對(duì)內(nèi)集卡模型有關(guān)鍵的作用。魔術(shù)輪胎模型是以三角函數(shù)組合的形式來(lái)擬合輪胎試驗(yàn)數(shù)據(jù),得出一套可以同時(shí)表達(dá)縱向力和側(cè)向力的輪胎模型。在研究中使用的是Pacejka89輪胎模型。

在穩(wěn)態(tài)純縱滑工況的條件下,內(nèi)集卡行駛過(guò)程中的魔術(shù)輪胎Pacejka89模型的縱向力表達(dá)式為:

FX0=DXsin(CXarctan(BXSX0-

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

(3)

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

根據(jù)建立的縱向力方程,可知根據(jù)輪胎縱向滑移率與其垂向載荷可以估算輪胎模型的縱向力,并以此為基礎(chǔ)構(gòu)建車(chē)輛動(dòng)力學(xué)方程,將方程的輸出結(jié)果應(yīng)用到算法之中進(jìn)行仿真。

在穩(wěn)態(tài)純側(cè)偏工況的條件下,在內(nèi)集卡行駛過(guò)程中的魔術(shù)輪胎Pacejka89模型的側(cè)向力表達(dá)式為:

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

arctan(BYaY))))+SVY

(4)

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

根據(jù)建立的側(cè)向力方程,可根據(jù)輪胎外傾角和側(cè)偏角與其垂向載荷估算輪胎模型的側(cè)向力,并以此為基礎(chǔ)構(gòu)建車(chē)輛動(dòng)力學(xué)方程,并將方程的輸出結(jié)果應(yīng)用到算法之中進(jìn)行仿真。

本次研究所需的輪胎力輸出變量由內(nèi)集卡狀態(tài)參數(shù)輸入計(jì)算可得,輸入輸出變量的表達(dá)過(guò)程見(jiàn)圖2。

圖2 魔術(shù)輪胎模型的輸入輸出變量

3 擴(kuò)展卡爾曼濾波器設(shè)計(jì)

3.1 EKF算法

傳統(tǒng)的卡爾曼濾波是通過(guò)對(duì)輸入?yún)?shù)的一系列計(jì)算,使用貝葉斯定理計(jì)算后驗(yàn)概率分布,對(duì)狀態(tài)參數(shù)進(jìn)行較為準(zhǔn)確的估計(jì)。而擴(kuò)展卡爾曼濾波算法是將非線性函數(shù)在某一采樣估計(jì)點(diǎn)的附近進(jìn)行一階泰勒展開(kāi),在不考慮展開(kāi)式的高階項(xiàng)的條件下簡(jiǎn)化得到線性化的系統(tǒng),之后按照卡爾曼濾波算法的流程原理完成對(duì)系統(tǒng)狀態(tài)參數(shù)的最優(yōu)估計(jì)。為了對(duì)智能無(wú)人內(nèi)集卡的狀態(tài)參數(shù)進(jìn)行更為準(zhǔn)確的估計(jì),采用擴(kuò)展卡爾曼濾波算法完成所需參數(shù)的估計(jì)。

將貨車(chē)動(dòng)力學(xué)模型和魔術(shù)輪胎模型組合后,再將其離散化表示,最后形成的離散化狀態(tài)方程為:

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

(5)

觀測(cè)方程可表示為:

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

(6)

此時(shí)假定沒(méi)有控制量參數(shù)的輸入,過(guò)程噪聲是均值為0的白噪聲,觀測(cè)噪聲同樣也是均值為0的白噪聲,同時(shí)過(guò)程噪聲和觀測(cè)噪聲彼此獨(dú)立,以便進(jìn)行數(shù)據(jù)處理?？柭鼮V波算法的迭代過(guò)程如下。

(1)狀態(tài)預(yù)測(cè):

(7)

(2)誤差協(xié)方差預(yù)測(cè):

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)狀態(tài)更新:

(10)

(5)誤差協(xié)方差更新:

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

(11)

3.2 狀態(tài)轉(zhuǎn)移矩陣

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

(12)

擴(kuò)展卡爾曼濾波算法公式中需要的A矩陣如式(13)所示,即狀態(tài)轉(zhuǎn)移矩陣,為采用此算法提供了矩陣參數(shù),是設(shè)計(jì)濾波算法的基礎(chǔ)。

(13)

3.3 觀測(cè)矩陣

觀測(cè)方程是求解所需變量的方程。在貨車(chē)狀態(tài)參數(shù)估計(jì)中Z是由Vx、ay、γ三個(gè)所需變量組成,故所需的觀測(cè)方程如式(14)所示,所需的經(jīng)過(guò)離散化后的觀測(cè)矩陣如式(15)所示。擴(kuò)展卡爾曼濾波算法公式中需要的H矩陣就是此觀測(cè)矩陣,為此算法提供了矩陣參數(shù),是設(shè)計(jì)濾波算法的基礎(chǔ)。

(14)

(15)

4 仿真與結(jié)果分析

在Simulink中使用Function模塊編寫(xiě)魔術(shù)輪胎模型與擴(kuò)展卡爾曼濾波算法,得到關(guān)于貨車(chē)的狀態(tài)參數(shù)估計(jì)。將Trucksim的實(shí)際輸出值與擴(kuò)展卡爾曼濾波算法得出的估算值進(jìn)行對(duì)比,驗(yàn)證擴(kuò)展卡爾曼濾波算法的準(zhǔn)確性和參數(shù)估計(jì)的可行性。Simulink模型見(jiàn)圖3,算法所需要的部分參數(shù)見(jiàn)表1。

表1 車(chē)輛模型部分參數(shù)

圖3 聯(lián)合仿真模型

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

通過(guò)仿真,系統(tǒng)實(shí)際輸出參數(shù)與通過(guò)擴(kuò)展卡爾曼濾波算法估算出的參數(shù)之間的誤差見(jiàn)表2。

表2 真實(shí)值與估計(jì)值的比較

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

5 結(jié)語(yǔ)

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