基于MPC的插秧機(jī)路徑跟蹤控制算法研究*

王玉亮，李漢卿，陳兆英，石紹軍，陳國防，王金星

(1. 山東農(nóng)業(yè)大學(xué)機(jī)械與電子工程學(xué)院，山東泰安，271018；2. 山東省園藝機(jī)械與裝備重點(diǎn)實(shí)驗(yàn)室，山東泰安，271018)

0 引言

水稻在我國種植廣泛，是我國重要的經(jīng)濟(jì)作物。但是，我國插秧機(jī)自動(dòng)駕駛技術(shù)水平較低，主要依靠傳統(tǒng)機(jī)械化作業(yè)，影響了水稻的產(chǎn)量和質(zhì)量。插秧機(jī)自動(dòng)駕駛的關(guān)鍵為自動(dòng)駕駛控制算法，插秧機(jī)自動(dòng)駕駛控制算法的效果影響了插秧機(jī)自動(dòng)作業(yè)的質(zhì)量和效率，為此，插秧機(jī)自動(dòng)駕駛控制算法的研究有著重要意義[1-2]。

國內(nèi)外學(xué)者對(duì)農(nóng)機(jī)自動(dòng)駕駛控制進(jìn)行了深入研究。O’Connor等[3]以約翰迪爾7800拖拉機(jī)為試驗(yàn)平臺(tái)，設(shè)計(jì)了一種適用于大田環(huán)境的CDGPS轉(zhuǎn)向控制系統(tǒng)，試驗(yàn)結(jié)果表明直線行駛跟蹤效果良好。美國相關(guān)公司也推出了各自的自動(dòng)導(dǎo)航控制系統(tǒng)，并且具有自動(dòng)調(diào)節(jié)姿態(tài)等功能[4-5]。日本各農(nóng)機(jī)研究單位對(duì)插秧機(jī)自動(dòng)駕駛系統(tǒng)進(jìn)行研發(fā)，研發(fā)出一套完整的基于RTK-GPS導(dǎo)航技術(shù)的水田直播機(jī)，水田實(shí)際作業(yè)效果良好[6]。李革等[7]采用GPS和慣性導(dǎo)航組合定位方式，使用純追蹤控制，開發(fā)了插秧機(jī)自動(dòng)駕駛導(dǎo)航控制系統(tǒng)，試驗(yàn)結(jié)果表明跟蹤效果良好。李逃昌等[8]提出的模糊自適應(yīng)純追蹤模型的農(nóng)業(yè)機(jī)械路徑跟蹤方法，根據(jù)專家經(jīng)驗(yàn)制定的模糊控制規(guī)則，但是很難快速修正，經(jīng)試驗(yàn)跟蹤誤差較大。偉利國等[9]改裝XDNZ630型插秧機(jī)，增加轉(zhuǎn)向機(jī)構(gòu)和變速機(jī)構(gòu)，采用DGPS技術(shù)，使用PID控制算法，實(shí)現(xiàn)了直線導(dǎo)航及地頭轉(zhuǎn)彎，試驗(yàn)結(jié)果表明跟蹤效果良好。插秧機(jī)自動(dòng)駕駛路徑使用較多的跟蹤控制算法包括純追蹤算法和PID控制算法，純追蹤模型結(jié)構(gòu)簡單，但是前視距離調(diào)整較為復(fù)雜；PID控制算法魯棒性較好，但是參數(shù)調(diào)節(jié)過程較為復(fù)雜。

針對(duì)路徑跟蹤控制算法使用復(fù)雜的問題，本文以井關(guān)PZ60型插秧機(jī)為研究平臺(tái)，采用RTK-GPS/BDS定位技術(shù)，設(shè)計(jì)了一種基于線性時(shí)變模型預(yù)測(cè)控制的路徑跟蹤算法。通過在MATLAB/Simulink仿真與水田試驗(yàn)相結(jié)合的方法，對(duì)該路徑跟蹤控制算法進(jìn)行驗(yàn)證分析。

1 插秧機(jī)運(yùn)動(dòng)學(xué)模型與模型線性化

1.1 插秧機(jī)運(yùn)動(dòng)學(xué)模型

井關(guān)PZ60型插秧機(jī)，車體長3.18 m，四輪驅(qū)動(dòng)，發(fā)動(dòng)機(jī)總功率13.2 kW，前輪外徑0.65 m，后輪外徑0.9 m，軸距1.1 m。

插秧機(jī)作業(yè)時(shí)速度較慢，可以忽略在行進(jìn)過程中輪胎側(cè)滑、車身側(cè)傾等影響穩(wěn)定性的因素，并且假定水田地面平坦，插秧機(jī)行進(jìn)速度恒定，采用運(yùn)動(dòng)學(xué)模型[10]設(shè)計(jì)路徑跟蹤控制器具有良好的控制性能。插秧機(jī)運(yùn)動(dòng)學(xué)模型如圖1所示。

圖1 插秧機(jī)運(yùn)動(dòng)學(xué)模型

圖1中(Xf，Yf)為前軸中心位置坐標(biāo)；(Xr，Yr)為后軸中心位置坐標(biāo)；vr為插秧機(jī)后軸中心速度，vf為插秧機(jī)前軸中心速度，δf為插秧機(jī)前輪轉(zhuǎn)角，φ為插秧機(jī)航向角，l為插秧機(jī)前后輪軸距。

以插秧機(jī)后軸中心為參考點(diǎn)，建立插秧機(jī)運(yùn)動(dòng)學(xué)模型

(1)

式中：x——橫向偏差；

y——插秧機(jī)Y方向行進(jìn)距離。

1.2 模型線性化與離散化

由式(1)可知，插秧機(jī)運(yùn)動(dòng)學(xué)模型是一個(gè)非線性系統(tǒng)，進(jìn)行模型預(yù)測(cè)控制時(shí)計(jì)算復(fù)雜、計(jì)算速度較慢、實(shí)時(shí)性不好，而對(duì)插秧機(jī)進(jìn)行控制時(shí)要求具有較好的實(shí)時(shí)性，所以對(duì)該模型進(jìn)行線性化。本文采用反饋線性化，具有良好的近似性。

將模型轉(zhuǎn)化為控制系統(tǒng)的一般形式

(2)

式中：ξ——狀態(tài)量；

u——控制量。

在參考狀態(tài)ξr處泰勒級(jí)數(shù)展開，并忽略高階項(xiàng)只保留一階項(xiàng)，得到

(3)

將式(3)與式(2)做差，得到線性化結(jié)果

(4)

由于模型是連續(xù)的，還需要進(jìn)行離散化處理，本文采用前向歐拉進(jìn)行離散化，離散后得到線性時(shí)變模型

(5)

式中：T——采樣時(shí)間；

Ak,t、Bk,t——線性時(shí)變矩陣。

2 模型預(yù)測(cè)路徑跟蹤控制器設(shè)計(jì)

模型預(yù)測(cè)控制(Model Predictive Control，MPC)是工業(yè)領(lǐng)域應(yīng)用較廣泛的優(yōu)化控制算法，實(shí)現(xiàn)過程采用預(yù)測(cè)模型、滾動(dòng)優(yōu)化和反饋校正的控制策略[11]。具有較好的魯棒性，并且對(duì)控制系統(tǒng)模型的精確度要求不高。

2.1 預(yù)測(cè)模型

設(shè)定t時(shí)刻的狀態(tài)量為

(6)

從而可以得到狀態(tài)空間表達(dá)式

(7)

其中：

式中：Δu——控制增量。

令Np為系統(tǒng)預(yù)測(cè)時(shí)域，表示在[t,t+Np]時(shí)域內(nèi)預(yù)測(cè)系統(tǒng)的狀態(tài)量，Nc為系統(tǒng)控制時(shí)域，表示在[t,t+Nc]時(shí)域內(nèi)系統(tǒng)的控制序列[12]，那么系統(tǒng)t時(shí)刻預(yù)測(cè)模型表示為

(8)

式中：Y(t)——系統(tǒng)預(yù)測(cè)輸出矩陣；

ψt——系統(tǒng)狀態(tài)量預(yù)測(cè)參數(shù)；

θt——控制增量序列預(yù)測(cè)參數(shù)；

ΔU(t)——控制增量序列。

2.2 滾動(dòng)優(yōu)化

滾動(dòng)優(yōu)化的目標(biāo)是為了使當(dāng)前的狀態(tài)量盡快收斂到參考值，并且控制量盡可能小，使插秧機(jī)能夠快速平穩(wěn)的路徑跟蹤，通過目標(biāo)函數(shù)的最優(yōu)化實(shí)現(xiàn)[13]。本文采用加權(quán)平方和的方法，并引入松弛因子ε，防止系統(tǒng)出現(xiàn)無可行解，目標(biāo)函數(shù)表示為

J(k)=(Y-Yref)TQ(Y-Yref)+

[ΔU(t)]TR[ΔU(t)]+ρε2

(9)

式中：Q——預(yù)測(cè)時(shí)域權(quán)重矩陣；

R——控制時(shí)域權(quán)重矩陣；

Yref——參考的狀態(tài)；

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

目標(biāo)函數(shù)第一項(xiàng)表示插秧機(jī)跟蹤預(yù)定義路徑迅速響應(yīng)的能力，第二項(xiàng)表示插秧機(jī)控制量穩(wěn)定變化的能力[14]。

對(duì)目標(biāo)函數(shù)求解時(shí)，將目標(biāo)函數(shù)轉(zhuǎn)化為一個(gè)二次規(guī)劃(Quadratic Programming，QP)問題[15]。定義一個(gè)偏差e，e的表達(dá)式為

(10)

聯(lián)立式(8)、式(9)和式(10)并整理，得到優(yōu)化后的目標(biāo)函數(shù)

J(k)=[ΔU(t)]T(θTQθ+R)[ΔU(t)]+

2e2Qθ[ΔU(t)]+P

(11)

進(jìn)一步整理得到二次規(guī)劃標(biāo)準(zhǔn)形式

(12)

式中：H——正定Hessian矩陣；

fT——控制增量系數(shù)矩陣；

P——常量。

在實(shí)際系統(tǒng)中，還要考慮系統(tǒng)控制量和控制增量的約束條件，約束條件表示為

umin≤u≤umax

Δumin≤Δu≤Δumax

(13)

式中：umin、umax——控制量約束的最值;

Δumin、Δumax——控制增量約束的最值。

2.3 反饋校正

本文采用內(nèi)點(diǎn)法進(jìn)行求解，完成每個(gè)控制周期求解，得到控制時(shí)域內(nèi)t時(shí)刻控制輸入增量：ΔUt=[Δut,Δut+1,…,Δut+Nc-1]T，根據(jù)模型預(yù)測(cè)控制原理，將控制序列第一個(gè)元素作用于系統(tǒng)，實(shí)施控制輸入量：u(t)=u(t-1)+Δut，系統(tǒng)執(zhí)行這一控制量，根據(jù)當(dāng)前狀態(tài)預(yù)測(cè)下一周期的控制增量序列，反復(fù)循環(huán)這一過程，實(shí)現(xiàn)最優(yōu)控制。

3 仿真驗(yàn)證

在MATLAB/Simulink環(huán)境下，對(duì)上述模型預(yù)測(cè)控制器建立系統(tǒng)仿真，根據(jù)使用計(jì)算機(jī)的配置，設(shè)定控制器參數(shù)Np=30，Nc=10，T=0.1 s，ε=10，插秧機(jī)速度1 m/s、軸距1.1 m，系統(tǒng)仿真圖如圖2所示。為說明模型預(yù)測(cè)控制器的性能，將此模型預(yù)測(cè)控制器與路徑跟蹤效果良好的Stanley控制算法作比較。在MATLAB/Simulink環(huán)境下搭建Stanley控制算法模型，如圖3所示。

圖2 系統(tǒng)仿真模型

圖3 Stanley控制仿真模型

根據(jù)插秧機(jī)工作方式，分別對(duì)直線路徑跟蹤、曲線路徑跟蹤和S型路徑跟蹤進(jìn)行仿真試驗(yàn)。

首先，對(duì)比兩種控制器在相同路徑、相同速度下的路徑跟蹤效果，仿真結(jié)果如圖4所示。

(a) 直線路徑仿真結(jié)果

設(shè)置直線路徑如圖4(a)所示實(shí)線，通過仿真結(jié)果可知，在相同速度下，本文設(shè)計(jì)的MPC控制器響應(yīng)速度快、超調(diào)量小且很快就趨于穩(wěn)定，Stanley控制算法響應(yīng)速度較慢、超調(diào)量大，在設(shè)定值附近振蕩，一直未能收斂，穩(wěn)定性較差；設(shè)置曲線路徑如圖4(b)所示實(shí)線，通過仿真結(jié)果可知，在相同速度下，本文設(shè)計(jì)的MPC控制器跟蹤路徑與預(yù)定義路徑偏差很小，路徑跟蹤效果較好。

然后，對(duì)比本文設(shè)計(jì)的MPC控制器在相同路徑、不同速度下的路徑跟蹤效果，仿真結(jié)果如圖5所示。

設(shè)置直線路徑如圖5(a)所示實(shí)線，通過仿真結(jié)果可知，當(dāng)速度設(shè)置在1 m/s時(shí)MPC控制器響應(yīng)速度快、超調(diào)量小、穩(wěn)定性好，當(dāng)速度為1.5 m/s時(shí)，相比于速度1 m/s時(shí)超調(diào)量增大，但是相應(yīng)速度也較快，在經(jīng)過較短的調(diào)整時(shí)間后，系統(tǒng)同樣趨于穩(wěn)定；設(shè)置曲線路徑如圖5(b)所示實(shí)線，通過仿真結(jié)果可知，當(dāng)速度不同時(shí)，MPC控制器都可以較好地逼近于預(yù)定義曲線，路徑跟蹤效果較好。

(a) 直線路徑仿真結(jié)果

最后，根據(jù)插秧機(jī)的工作路徑，設(shè)置一條S型路徑，對(duì)比不同算法和不同速度的路徑跟蹤效果，仿真結(jié)果如圖6所示。

(a) 不同算法仿真結(jié)果

相同速度、不同算法仿真結(jié)果如圖6(a)所示，當(dāng)速度設(shè)置在1 m/s時(shí)，本文設(shè)計(jì)的MPC控制器跟蹤路徑與預(yù)定義路徑偏差很小，路徑跟蹤效果較好，Stanley控制算法跟蹤路徑與預(yù)定義路徑偏差較大，路徑跟蹤效果較差。相同算法、不同速度仿真結(jié)果如圖6(b)所示，設(shè)置速度在1 m/s和1.5 m/s時(shí)，MPC控制器都可以較好地逼近于預(yù)定義曲線，速度1 m/s時(shí)，直線段最大誤差為1.04 cm，平均誤差為0.50 cm，轉(zhuǎn)彎段最大誤差1.38 cm，平均誤差0.97 cm，路徑跟蹤效果較好；速度1.5 m/s時(shí)，直線段最大誤差為1.12 cm，平均誤差為0.63 cm，轉(zhuǎn)彎段最大誤差1.65 cm，平均誤差1.37 cm，路徑跟蹤效果較好。

綜上所述，上述模型預(yù)測(cè)控制器橫向偏差較小，可以對(duì)規(guī)劃路徑實(shí)現(xiàn)很好地跟蹤。

4 田間試驗(yàn)

為驗(yàn)證本文所設(shè)計(jì)控制器實(shí)際控制效果，結(jié)合農(nóng)藝要求于2021年4月中旬在濟(jì)寧恒盛農(nóng)機(jī)專業(yè)合作社水田進(jìn)行試驗(yàn)。采用AGI-4接收機(jī)、NX300電動(dòng)方向盤和424A16A090霍爾式轉(zhuǎn)角傳感器，改造井關(guān)PZ60型插秧機(jī)，搭建插秧機(jī)自動(dòng)駕駛試驗(yàn)平臺(tái)，如圖7所示。控制器采用STM32單片機(jī)，電動(dòng)方向盤與控制器STM32單片機(jī)之間采用CANOPEN協(xié)議通信，在試驗(yàn)田內(nèi)測(cè)試模型預(yù)測(cè)控制器性能。

圖7 水田自動(dòng)駕駛試驗(yàn)平臺(tái)

在合作社指定地塊進(jìn)行測(cè)試，首先對(duì)試驗(yàn)平臺(tái)進(jìn)行硬件校正，然后將差分基站立在地頭，使用手持式儀器獲取目標(biāo)地塊坐標(biāo)，對(duì)目標(biāo)作業(yè)地塊進(jìn)行預(yù)定義路徑規(guī)劃，最后使用本文設(shè)計(jì)的控制器對(duì)預(yù)定義路徑進(jìn)行路徑跟蹤，并進(jìn)行自動(dòng)駕駛作業(yè)。對(duì)自動(dòng)駕駛過程中的橫向偏差數(shù)據(jù)進(jìn)行統(tǒng)計(jì)，統(tǒng)計(jì)結(jié)果如圖8所示。

從圖8可以看出，自動(dòng)駕駛插秧機(jī)在直線作業(yè)段的行駛速度約為1 m/s，路徑跟蹤橫向偏差的最大值為2.02 cm，平均值為1.03 cm；轉(zhuǎn)彎段路徑跟蹤最大橫向偏差為3.02 cm，平均值為2.09 cm，橫向偏差最大誤差出現(xiàn)在路徑轉(zhuǎn)彎處。由于插秧機(jī)在直線段作業(yè)，轉(zhuǎn)彎段不作業(yè)，保證插秧機(jī)直線行駛精度，可獲得較好的插秧效果。因此，插秧機(jī)自動(dòng)駕駛的田間試驗(yàn)主要針對(duì)直線作業(yè)段進(jìn)行誤差統(tǒng)計(jì)分析。試驗(yàn)結(jié)果表明：自動(dòng)駕駛控制系統(tǒng)可以較準(zhǔn)確地實(shí)現(xiàn)對(duì)預(yù)定義路徑的跟蹤，自動(dòng)駕駛效果良好。

圖8 水田自動(dòng)駕駛試驗(yàn)橫向偏差

5 結(jié)論

1) 在MATLAB/Simulink環(huán)境下，基于模型預(yù)測(cè)控制方法，針對(duì)PZ60型插秧機(jī)建立了運(yùn)動(dòng)學(xué)模型，設(shè)計(jì)了插秧機(jī)路徑跟蹤模型預(yù)測(cè)控制器，并進(jìn)行了仿真模擬試驗(yàn)。將本文設(shè)計(jì)的MPC控制器與Stanley控制算法在直線與曲線路徑進(jìn)行了對(duì)比試驗(yàn)，試驗(yàn)結(jié)果表明，本文設(shè)計(jì)的MPC控制器響應(yīng)速度較快且超調(diào)量小；根據(jù)插秧機(jī)工作方式設(shè)置了一條S型路徑進(jìn)行了路徑跟蹤試驗(yàn)，試驗(yàn)結(jié)果表明，1 m/s速度下進(jìn)行S型路徑跟蹤，最大跟蹤誤差為1.38 cm，并且最大誤差集中在轉(zhuǎn)彎段，直線段跟蹤誤差較小，說明控制器具有良好的跟蹤性能，并且具有較好的穩(wěn)定性。

2) 以井關(guān)PZ60型插秧機(jī)試驗(yàn)平臺(tái)，采用AGI-4接收機(jī)、NX300電動(dòng)方向盤和424A16A090霍爾式轉(zhuǎn)角傳感器等搭建插秧機(jī)自動(dòng)駕駛硬件系統(tǒng)；進(jìn)行了插秧機(jī)自動(dòng)駕駛作業(yè)試驗(yàn)，田間試驗(yàn)結(jié)果表明：在插秧機(jī)行進(jìn)速度約為1 m/s時(shí)，直線段跟蹤誤差最大為2.02 cm，滿足自動(dòng)駕駛的精度要求。