国产日韩欧美一区二区三区三州_亚洲少妇熟女av_久久久久亚洲av国产精品_波多野结衣网站一区二区_亚洲欧美色片在线91_国产亚洲精品精品国产优播av_日本一区二区三区波多野结衣 _久久国产av不卡

?

基于模型預(yù)測的插秧機(jī)路徑跟蹤控制算法

2023-01-05 03:54遲瑞娟熊澤鑫姜龍騰馬悅琦黃修煉朱曉龍
關(guān)鍵詞:實車插秧機(jī)航向

遲瑞娟 熊澤鑫 姜龍騰 馬悅琦 黃修煉 朱曉龍

(中國農(nóng)業(yè)大學(xué)工學(xué)院,北京 100083)

0 引言

農(nóng)機(jī)自動駕駛技術(shù)是實現(xiàn)精準(zhǔn)農(nóng)業(yè)與無人農(nóng)場的關(guān)鍵與基礎(chǔ),其中路徑生成及跟蹤是自動駕駛的研究重點[1-3]。路徑跟蹤是為了使車輛可以自動跟蹤由規(guī)劃算法得到的軌跡路線[4-5],其主要任務(wù)是收集各傳感器的信息以確定車輛當(dāng)前狀態(tài)(車速、位置、車身姿態(tài)等),然后根據(jù)車輛運(yùn)動學(xué)模型或動力學(xué)模型,按照所選路徑跟蹤算法,以車輛當(dāng)前行駛路徑與預(yù)設(shè)路徑的偏差為約束量,計算并輸出車輛控制參數(shù),如車速、轉(zhuǎn)向輪轉(zhuǎn)角等[6-7]。

目前,路徑跟蹤算法應(yīng)用較多的有純跟蹤控制算法[8]、PID控制算法[9]、神經(jīng)網(wǎng)絡(luò)[10]、機(jī)器學(xué)習(xí)[10]、模型預(yù)測算法[11]等。其中,羅錫文等[12]采用PID算法進(jìn)行路徑跟蹤實驗,直線路徑跟蹤精度平均值小于10 cm,但彎道誤差較大;唐小濤[13]提出純跟蹤控制算法,實現(xiàn)對路徑的跟蹤,但適應(yīng)性差,需要計算在不同車速下的前視距離;黃沛琛等[14]以固定前視距離下的仿真數(shù)據(jù)作為樣本訓(xùn)練神經(jīng)網(wǎng)絡(luò),實現(xiàn)動態(tài)調(diào)節(jié)前視距離,但是需要大量數(shù)據(jù)作為訓(xùn)練樣本。FAN等[11]將非線性模型預(yù)測控制理論應(yīng)用于車輛軌跡跟蹤,其增加了控制量的約束條件,仿真結(jié)果表明控制效果更加準(zhǔn)確、穩(wěn)定,且針對影響因素進(jìn)行了靈敏度分析,實驗表明對非線性模型預(yù)測控制器的效果影響不大。

農(nóng)機(jī)自動駕駛技術(shù)除了滿足常規(guī)道路運(yùn)輸?shù)男旭偪刂?,還要滿足不同作業(yè)情況下的行駛要求。農(nóng)田行駛和作業(yè)環(huán)境較為惡劣,算法的選取需考慮對環(huán)境變化及參數(shù)調(diào)節(jié)的依賴程度,防止當(dāng)作業(yè)環(huán)境變化時,無法準(zhǔn)確跟蹤預(yù)設(shè)路徑或無法及時調(diào)節(jié)修正算法參數(shù)[15]。雖然農(nóng)機(jī)的行駛和作業(yè)速度不高,但作業(yè)環(huán)境復(fù)雜,且需滿足不同農(nóng)藝的要求。作業(yè)過程中的行駛控制主要體現(xiàn)在對作業(yè)路徑的跟蹤精度及地頭轉(zhuǎn)彎的平順性、安全性要求高[16]。插秧機(jī)在我國應(yīng)用廣泛,插秧機(jī)特殊的工作環(huán)境和插秧農(nóng)業(yè)特點[17-18]對自動駕駛技術(shù)的需求更高。模型預(yù)測算法可提高自動駕駛響應(yīng)的及時性和控制過程的平穩(wěn)性。綜上考慮,本文以插秧機(jī)為控制對象,以模型預(yù)測算法[19-20]為基礎(chǔ)設(shè)計路徑跟蹤控制器,以泰勒級數(shù)形式對插秧機(jī)運(yùn)動學(xué)模型進(jìn)行線性化處理,利用模型預(yù)測算法求解插秧機(jī)路徑跟蹤控制序列,依次循環(huán)求解實現(xiàn)對參考路徑的跟蹤[21]。為確保算法中預(yù)測時域略高于控制時域以保證路徑跟蹤的實時性,根據(jù)插秧機(jī)車輛參數(shù)及特點增加對控制增量的約束條件,以保證插秧機(jī)跟蹤過程的平穩(wěn)性。最后通過仿真分析和實車試驗進(jìn)行驗證,以證明算法對插秧機(jī)自動駕駛控制具有較高的適應(yīng)性和平穩(wěn)性。

1 車輛運(yùn)動學(xué)模型及基于模型預(yù)測的路徑跟蹤控制器設(shè)計

插秧機(jī)運(yùn)動學(xué)模型比較復(fù)雜,為了簡化模型和便于控制,忽略懸架特性,忽略輪胎與地面的側(cè)向滑動,且假設(shè)插秧機(jī)左右輪胎在任意時刻都擁有相同的轉(zhuǎn)向角和轉(zhuǎn)速,則可將前后車輪分別集中于一點,插秧機(jī)即可簡化為二輪車模型[22]。從而減少控制器的在線計算量,提高控制系統(tǒng)的實時性。

車輛運(yùn)動學(xué)模型如圖1所示,(xF,yF)與(xR,yR)分別為車輛前軸中心與后軸中心的坐標(biāo)、δ為前輪(轉(zhuǎn)向輪)轉(zhuǎn)角、θ為車輛航向角、v為車輛后軸速度、L為軸距,在地面固定坐標(biāo)系XOY下可得車輛運(yùn)動學(xué)方程為

圖1 車輛運(yùn)動學(xué)模型

(1)

由式(1)可知,插秧機(jī)路徑跟蹤控制器可看作是由控制量u=(v,δ)和狀態(tài)量p=(x,y,θ)組成的控制系統(tǒng)。其一般形式可表示為

(2)

基于運(yùn)動學(xué)方程可知,該控制系統(tǒng)為非線性控制系統(tǒng),無法直接運(yùn)用模型預(yù)測控制算法[23],故需對上述控制系統(tǒng)進(jìn)行近似線性化處理。參考軌跡任一點處皆滿足式(2)的一般形式,各參數(shù)添加下標(biāo)r代表參考位置的各參考量,則式(2)于參考位置的一般形式可表達(dá)為

(3)

式(3)在任意參考軌跡點處進(jìn)行泰勒級數(shù)展開,忽略高階項,得到

(4)

將式(4)與式(3)相減,可得自動駕駛農(nóng)機(jī)誤差模型即線性表達(dá)式為

(5)

為了能夠?qū)⒉逖頇C(jī)二輪車模型應(yīng)用于模型預(yù)測控制器的設(shè)計,利用前向歐拉公式[24],將上述線性化系統(tǒng)進(jìn)行離散化處理可得

(6)

其中

式中k——任一時刻T——采樣時間

得到誤差模型式(6)后,需基于自動駕駛插秧機(jī)線性誤差模型設(shè)計目標(biāo)函數(shù),預(yù)測插秧機(jī)未來時刻的輸出。且為了更好地約束控制增量,設(shè)

(7)

式中ξ(k|t)——假設(shè)k時刻新的狀態(tài)量

則可推導(dǎo)出下一時刻ξ(k+1|t)的表達(dá)式為

(8)

ξ(k+1|t)=Aktξ(k|t)+BktΔu(k|t)

(9)

其中

式中 Δu(k|t)——k時刻的控制增量

n——狀態(tài)量維度

m——控制量維度t——時間參數(shù)

I——單位矩陣O——零矩陣

假設(shè)未來預(yù)測時域內(nèi)插秧機(jī)狀態(tài)為ξ(k+1|t)、ξ(k+2|t)、ξ(k+3|t)、…、ξ(k+n|t),則類比于式(9),可推導(dǎo)出k+2、k+3、…、k+n時刻的狀態(tài)方程表達(dá)式為

(10)

由遞推規(guī)律可得自動駕駛系統(tǒng)預(yù)測輸出表達(dá)式[25]為

Y(k)=Ψξ(k|t)+HΔU(k|t)

(11)

其中

式中C——比例系數(shù)

NP——預(yù)測時域長度

Nc——控制時域長度

由式(11)可知,若已知當(dāng)前時刻的狀態(tài)量和控制時域Nc內(nèi)的控制增量,可以得到預(yù)測時域NP的系統(tǒng)輸出量且一般情況下滿足NP≥Nc。

在目標(biāo)函數(shù)中,控制增量的變化對下一時刻預(yù)測狀態(tài)有極大影響,需依據(jù)插秧機(jī)自身車輛特性對控制增量進(jìn)行約束,即

Δumin(k+i)≤Δu(k+i)≤Δumax(k+i)

(i=0,1,2,…,Nc-1)

(12)

轉(zhuǎn)換為

Umin≤AΔUt+Ut≤Umax

(13)

其中

為減少算法運(yùn)行時間及便于程序的編寫,將目標(biāo)函數(shù)轉(zhuǎn)換為標(biāo)準(zhǔn)二次型形式并結(jié)合約束條件,求解最優(yōu)方案為

(14)

式中eT——預(yù)測時域內(nèi)的跟蹤誤差

R、Q——權(quán)重矩陣

ρ——權(quán)重系數(shù)

求解式(14)后得到控制時域內(nèi)一系列控制輸入增量為

ΔUNc=[ΔukΔuk+1… Δuk+Nc-1]T

(15)

將控制序列中的第一個元素作為實際的控制輸入增量作用于系統(tǒng),即

u(k)=u(k-1)+Δuk

(16)

每周期依次循環(huán),重復(fù)上述計算過程,用于預(yù)測插秧機(jī)未來時刻的輸出,實現(xiàn)插秧機(jī)對預(yù)設(shè)路徑的跟蹤控制,結(jié)合約束條件限制,對每個采樣周期內(nèi)的控制增量進(jìn)行限制,避免被控系統(tǒng)控制量突變的現(xiàn)象,實現(xiàn)快速且平穩(wěn)地追蹤[26]。

2 路徑跟蹤誤差模型

考慮農(nóng)藝水平的要求,一方面需確保自動駕駛插秧機(jī)路徑跟蹤精度,另一方面需考慮路徑跟蹤過程中的平穩(wěn)性?;谀P皖A(yù)測算法設(shè)計自動駕駛路徑跟蹤控制器,以橫向偏差d和航向偏差Φ作為評判路徑跟蹤效果的因素。路徑跟蹤誤差模型如圖2所示,橫向偏差定義為車輛后軸中心(xcr,ycr)與其在期望路徑上的投影點S(xS,yS)之間的距離,規(guī)定車輛在參考路徑右側(cè)為正,計算式為

圖2 路徑跟蹤誤差模型

(17)

式中xcr——平面坐標(biāo)中車輛后軸中心x軸坐標(biāo),m

ycr——平面坐標(biāo)中車輛后軸中心y軸坐標(biāo),m

xS——投影點S處x軸坐標(biāo),m

yS——投影點S處y軸坐標(biāo),m

先找到期望路徑上述投影點S(xS,yS)處的切線,航向偏差計算式為插秧機(jī)本身航向與該切線的絕對角度之差,規(guī)定順時針方向為正,計算式為

Φ=57.3(θ-θroad)

(18)

式中θroad——投影點S處的切線航向角,rad

3 路徑跟蹤算法仿真

在進(jìn)行實車試驗之前,通過仿真對所設(shè)路徑跟蹤算法進(jìn)行分析。利用Matlab進(jìn)行算法仿真,因插秧機(jī)在田中行走的路徑可大致分為直線作業(yè)路徑、地頭轉(zhuǎn)向路徑,在仿真環(huán)境中分別設(shè)計直線與曲線的參考路徑,在不同速度下測試所設(shè)路徑跟蹤控制系統(tǒng)對不同路徑的跟蹤能力。

3.1 直線跟蹤仿真

直線路徑跟蹤仿真基本設(shè)定為:插秧機(jī)初始狀態(tài)為X0=0.5 m,Y0=1.0 m,δ=0°,分別以期望縱向速度0.5、1.0、1.5 m/s跟蹤參考路徑,直線參考路徑為直線Y=X,其中X、Y取值范圍均為0~20 m,控制周期為50 ms。其余模型預(yù)測算法參數(shù)如表1所示,在Matlab中編寫相關(guān)程序,對無人駕駛車輛的直線路徑跟蹤過程進(jìn)行仿真。

表1 模型預(yù)測算法控制參數(shù)

3種車速下直線路徑跟蹤結(jié)果如圖3a所示,仿真路線都可迅速跟蹤上期望軌跡。圖3b為圖3a中紅色矩形位置偏差較大值的局部放大圖,因為初始位置及初始狀態(tài)與期望值存在偏差,為正常的追蹤過程。跟蹤過程中實時偏差如圖4所示。圖4a為橫向偏差,在大約X=6 m后誤差逐漸趨向平穩(wěn),跟蹤偏差在0.020 m之內(nèi);圖4b為3種速度下對應(yīng)的航向偏差,航向偏差較大值出現(xiàn)在初始跟蹤處,最大值為0.500°,在大約X=7 m后偏差值趨向穩(wěn)定,偏差在0.080°以內(nèi)。由仿真結(jié)果可知,控制器不僅實現(xiàn)了較高精度跟蹤期望直線軌跡的功能,也保證了跟蹤過程中的平穩(wěn)性。

圖3 直線參考軌跡與實際軌跡

圖4 3種速度下橫向、航向偏差曲線(直線跟蹤)

3.2 曲線跟蹤仿真

曲線參考路徑設(shè)定為

(19)

將插秧機(jī)的初始狀態(tài)設(shè)為X0=-1.0 m,Y0=0,δ=0.5°;行車速度分別設(shè)為0.5、1.0、1.5 m/s;采樣控制周期設(shè)為50 ms;控制增量約束及模糊控制器的基本參數(shù)設(shè)置參照直線路徑仿真試驗。如圖5a中參考路徑所示,預(yù)設(shè)曲線設(shè)計為“S”形,目的是驗證不同速度下路徑跟蹤控制器對曲線路徑的跟蹤能力。

圖5 曲線參考軌跡與實際軌跡

曲線路徑整體跟蹤效果如圖5a所示,3種速度下對曲線跟蹤在起始點處、中間換向轉(zhuǎn)向處及目標(biāo)終止點位置出現(xiàn)較大偏差,其余位置跟蹤平穩(wěn)且偏差較小。起始處偏差較大是因為初始狀態(tài)值與期望值存在偏差,終止點處偏差較大是因為預(yù)測時域大于控制時域部分,所做出的預(yù)測值超出預(yù)設(shè)參考路徑,都在可接受范圍之內(nèi)。圖5b為圖5a中紅色矩形所示位置的局部放大圖,偏差較大。由文獻(xiàn)[27]可知,預(yù)測算法中參數(shù)預(yù)測時域NP對路徑跟蹤精度和穩(wěn)定性有極大影響,即

dP=vxNPT

(20)

式中dP——預(yù)測距離

vx——插秧機(jī)縱向車速

在采樣時間及縱向車速不變時,預(yù)測距離主要與預(yù)測時域有關(guān)。若預(yù)測距離過大,將增大跟蹤誤差權(quán)重,影響插秧機(jī)跟蹤精度,若預(yù)測距離過小,控制量及增量前后時刻相差較大,導(dǎo)致車輛無法及時準(zhǔn)確轉(zhuǎn)向。另外,本文針對控制對象所控車速為插秧機(jī)后軸實時車速,在插秧機(jī)轉(zhuǎn)向時,無法保證縱向速度不變,且預(yù)測時域為固定值,故插秧機(jī)轉(zhuǎn)向動作發(fā)生時,無法實時調(diào)參兼顧穩(wěn)定性與精度。

圖6為3種速度下曲線仿真跟蹤過程中的實時偏差曲線。由圖6可知,3種速度下橫向偏差較大值出現(xiàn)在X=-1 m、X=1 m、X=3 m附近,最大值出現(xiàn)在X=1 m附近,最大值為0.580 m,平均值為0.022 m。航向偏差最大值出現(xiàn)在X=1 m附近,最大值為10.570°,平均值為0.699°。結(jié)果表明,所設(shè)路徑跟蹤控制器可用于曲線追蹤,但設(shè)計地頭轉(zhuǎn)向曲線時,應(yīng)避免連續(xù)換向曲線設(shè)計。

圖6 3種速度下橫向、航向偏差曲線(曲線跟蹤)

4 實車試驗

為了驗證所建立的基于模型預(yù)測算法的路徑跟蹤控制器的魯棒性及跟蹤精度,以改裝后的洋馬VP6E型水稻插秧機(jī)為試驗平臺,插秧機(jī)參數(shù)如表2所示。

表2 洋馬VP6E型插秧機(jī)參數(shù)

平臺實物如圖7所示,改裝原插秧機(jī)中的轉(zhuǎn)向系統(tǒng)、行走系統(tǒng)、制動系統(tǒng):以前輪轉(zhuǎn)角傳感器為轉(zhuǎn)向的信息反饋傳感器,以無刷直流電機(jī)為動力源,通過齒輪傳動將動力傳輸至方向盤及轉(zhuǎn)向軸,實現(xiàn)自主轉(zhuǎn)向功能,以電推桿模擬駕駛員踩、放踏板的動作。同時,該試驗平臺加入RTK-GPS設(shè)備來實現(xiàn)高精度的定位導(dǎo)航功能,增加避障模塊,實時檢測障礙物[28]以保證自動駕駛插秧機(jī)的作業(yè)安全。

圖7 平臺實物圖

采集插秧機(jī)實際行走的經(jīng)緯度數(shù)據(jù),將參考路徑及實車行走的經(jīng)緯度數(shù)據(jù)轉(zhuǎn)換為墨卡托投影下的平面坐標(biāo)數(shù)據(jù),并將所有平面坐標(biāo)數(shù)據(jù)減去各自第1組平面坐標(biāo),繪制實車試驗路徑跟蹤曲線。插秧機(jī)由(0,0)出發(fā)至終點(14 m,4 m),由圖8可得 3段直線的跟蹤效果較好,兩段曲線跟蹤偏差較大,實時偏差如圖9~12所示。

圖8 實車試驗路徑跟蹤曲線

圖9 實車試驗直線橫向偏差

圖9為實車試驗跟蹤3段直線路徑的實時橫向偏差,由表3可得,對直線跟蹤的最大橫向偏差為0.110 m,平均橫向偏差為0.021 m,各組數(shù)據(jù)小于0.02 m占比不小于69%,表明跟蹤過程的平穩(wěn)性較高;圖10為實車試驗跟蹤3段直線路徑的實時航向偏差,由表3可得,航向偏差最大值為10.949°,平均航向偏差為6.187°,小于5°偏差占比不小于71%,表明跟蹤過程的實時航向轉(zhuǎn)換變化平緩。

圖10 實車試驗直線航向偏差

表3 實車試驗直線跟蹤能力

圖11為實車試驗跟蹤2段曲線路徑的實時橫向偏差。由表4可得,橫向偏差最大值為0.951 m,最小值為0.030 m,平均橫向偏差為0.450 m,偏差小于0.04 m占比不小于43%;圖12為實車試驗跟蹤2段曲線路徑的實時航向偏差,由表4可得,航向偏差最大值為33.391°,最小值為0.091°,平均航向偏差為10.107°,偏差小于7°占比不小于50%。

圖11 實車試驗曲線橫向偏差

圖12 實車試驗曲線航向偏差

表4 實車試驗曲線跟蹤能力

5 討論

(1)模型預(yù)測算法可成功應(yīng)用于自動駕駛農(nóng)機(jī)的路徑跟蹤,且對路徑追蹤的過程中橫向、航向偏差值較小且變化平穩(wěn)。

(2)插秧機(jī)為低速車輛,算法可應(yīng)用于農(nóng)機(jī)不同速度狀態(tài)下,且不同速度下跟蹤結(jié)果變化較小,表明算法魯棒性較好。

(3)實車試驗直線部分的跟蹤效果略好于曲線部分,仿真試驗跟蹤效果略好于實車試驗。經(jīng)分析實車試驗相比于仿真試驗需要考慮機(jī)器及導(dǎo)航設(shè)備誤差等因素,該因素為導(dǎo)致跟蹤結(jié)果偏差較大的主要原因;在實車試驗中,曲線試驗誤差大于直線試驗誤差,但追蹤過程變化平穩(wěn),橫向偏差稍大但穩(wěn)定于約0.4 m。原因為:如曲線仿真結(jié)論處所述,預(yù)測時域的選取應(yīng)與插秧機(jī)縱向速度、橫向速度有關(guān)。在直線行駛時,車輛后軸速度理論值等于縱向速度且不變,故預(yù)測時域NP雖為固定值,但對直線路徑跟蹤精度影響較小。當(dāng)插秧機(jī)進(jìn)行轉(zhuǎn)向動作時,縱向速度實時變化,固定的預(yù)測時域NP導(dǎo)致預(yù)測距離dP實時變化,將很大程度上影響插秧機(jī)對曲線路徑的跟蹤精度及轉(zhuǎn)向的及時性。另外,轉(zhuǎn)向策略的選取也需考慮插秧機(jī)本身轉(zhuǎn)向特性。

6 結(jié)論

(1)基于模型預(yù)測算法建立了路徑跟蹤控制器,在參考路徑點采用泰勒級數(shù)形式線性化農(nóng)機(jī)自動駕駛誤差模型,建立控制目標(biāo)函數(shù),在控制過程中,增加控制量極限約束,保證對路徑跟蹤的平穩(wěn)性。

(2)利用Matlab建立3種速度下的仿真試驗,結(jié)果表明,農(nóng)機(jī)針對直線跟蹤的橫向偏差可穩(wěn)定在0.020 m以內(nèi),航向偏差可穩(wěn)定在0.080°以內(nèi)。針對曲線跟蹤平均橫向偏差為0.022 m,平均航向偏差為0.699°,表明所設(shè)計算法可適用于實車路徑跟蹤。

(3)以插秧機(jī)為試驗平臺進(jìn)行實車試驗,試驗結(jié)果表明,該控制器用于自動駕駛插秧機(jī)直線路徑跟蹤時的平均橫向偏差為0.021 m、平均航向偏差為6.187°,曲線路徑跟蹤時的平均橫向偏差為0.450 m、平均航向偏差為10.107°。提出的路徑跟蹤控制算法可成功應(yīng)用于自動駕駛插秧機(jī)對作業(yè)行的路徑跟蹤,且具有較好的魯棒性及跟蹤性能。

猜你喜歡
實車插秧機(jī)航向
水稻插秧機(jī)發(fā)展綜述
實車碰撞試驗牽引系統(tǒng)鋼絲繩疲勞損傷問題分析
風(fēng)浪干擾條件下艦船航向保持非線性控制系統(tǒng)
基于手機(jī)藍(lán)牙的奧迪A4L發(fā)動機(jī)故障設(shè)置系統(tǒng)設(shè)計
知坐標(biāo),明航向
基于WiFi的奧迪A4L發(fā)動機(jī)故障設(shè)置系統(tǒng)
插秧機(jī)的使用與維修
考慮幾何限制的航向道模式設(shè)計
超智能插秧機(jī)
稻田
宁武县| 双柏县| 高尔夫| 彭阳县| 叙永县| 鹤峰县| 大厂| 诏安县| 年辖:市辖区| 夏津县| 陇西县| 大同市| 临泉县| 保康县| 兴宁市| 宁远县| 武胜县| 阜康市| 九龙城区| 浦东新区| 甘南县| 延吉市| 阿勒泰市| 天全县| 霍山县| 定边县| 台前县| 伊金霍洛旗| 金门县| 甘孜| 商河县| 海晏县| 兰西县| 申扎县| 肇源县| 饶平县| 阿拉尔市| 余干县| 克山县| 长丰县| 乌兰浩特市|