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

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

(1. 山東農業大學機械與電子工程學院，山東泰安，271018；2. 山東省園藝機械與裝備重點實驗室，山東泰安，271018)

0 引言

水稻在我國種植廣泛，是我國重要的經濟作物。但是，我國插秧機自動駕駛技術水平較低，主要依靠傳統機械化作業，影響了水稻的產量和質量。插秧機自動駕駛的關鍵為自動駕駛控制算法，插秧機自動駕駛控制算法的效果影響了插秧機自動作業的質量和效率，為此，插秧機自動駕駛控制算法的研究有著重要意義[1-2]。

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

針對路徑跟蹤控制算法使用復雜的問題，本文以井關PZ60型插秧機為研究平臺，采用RTK-GPS/BDS定位技術，設計了一種基于線性時變模型預測控制的路徑跟蹤算法。通過在MATLAB/Simulink仿真與水田試驗相結合的方法，對該路徑跟蹤控制算法進行驗證分析。

1 插秧機運動學模型與模型線性化

1.1 插秧機運動學模型

井關PZ60型插秧機，車體長3.18 m，四輪驅動，發動機總功率13.2 kW，前輪外徑0.65 m，后輪外徑0.9 m，軸距1.1 m。

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

圖1 插秧機運動學模型

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

以插秧機后軸中心為參考點，建立插秧機運動學模型

(1)

式中：x——橫向偏差；

y——插秧機Y方向行進距離。

1.2 模型線性化與離散化

由式(1)可知，插秧機運動學模型是一個非線性系統，進行模型預測控制時計算復雜、計算速度較慢、實時性不好，而對插秧機進行控制時要求具有較好的實時性，所以對該模型進行線性化。本文采用反饋線性化，具有良好的近似性。

將模型轉化為控制系統的一般形式

(2)

式中：ξ——狀態量；

u——控制量。

在參考狀態ξr處泰勒級數展開，并忽略高階項只保留一階項，得到

(3)

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

(4)

由于模型是連續的，還需要進行離散化處理，本文采用前向歐拉進行離散化，離散后得到線性時變模型

(5)

式中：T——采樣時間；

Ak,t、Bk,t——線性時變矩陣。

2 模型預測路徑跟蹤控制器設計

模型預測控制(Model Predictive Control，MPC)是工業領域應用較廣泛的優化控制算法，實現過程采用預測模型、滾動優化和反饋校正的控制策略[11]。具有較好的魯棒性，并且對控制系統模型的精確度要求不高。

2.1 預測模型

設定t時刻的狀態量為

(6)

從而可以得到狀態空間表達式

(7)

其中：

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

令Np為系統預測時域，表示在[t,t+Np]時域內預測系統的狀態量，Nc為系統控制時域，表示在[t,t+Nc]時域內系統的控制序列[12]，那么系統t時刻預測模型表示為

(8)

式中：Y(t)——系統預測輸出矩陣；

ψt——系統狀態量預測參數；

θt——控制增量序列預測參數；

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

2.2 滾動優化

滾動優化的目標是為了使當前的狀態量盡快收斂到參考值，并且控制量盡可能小，使插秧機能夠快速平穩的路徑跟蹤，通過目標函數的最優化實現[13]。本文采用加權平方和的方法，并引入松弛因子ε，防止系統出現無可行解，目標函數表示為

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

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

(9)

式中：Q——預測時域權重矩陣；

R——控制時域權重矩陣；

Yref——參考的狀態；

ρ——權值系數。

目標函數第一項表示插秧機跟蹤預定義路徑迅速響應的能力，第二項表示插秧機控制量穩定變化的能力[14]。

對目標函數求解時，將目標函數轉化為一個二次規劃(Quadratic Programming，QP)問題[15]。定義一個偏差e，e的表達式為

(10)

聯立式(8)、式(9)和式(10)并整理，得到優化后的目標函數

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

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

(11)

進一步整理得到二次規劃標準形式

(12)

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

fT——控制增量系數矩陣；

P——常量。

在實際系統中，還要考慮系統控制量和控制增量的約束條件，約束條件表示為

umin≤u≤umax

Δumin≤Δu≤Δumax

(13)

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

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

2.3 反饋校正

本文采用內點法進行求解，完成每個控制周期求解，得到控制時域內t時刻控制輸入增量：ΔUt=[Δut,Δut+1,…,Δut+Nc-1]T，根據模型預測控制原理，將控制序列第一個元素作用于系統，實施控制輸入量：u(t)=u(t-1)+Δut，系統執行這一控制量，根據當前狀態預測下一周期的控制增量序列，反復循環這一過程，實現最優控制。

3 仿真驗證

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

圖2 系統仿真模型

圖3 Stanley控制仿真模型

根據插秧機工作方式，分別對直線路徑跟蹤、曲線路徑跟蹤和S型路徑跟蹤進行仿真試驗。

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

(a) 直線路徑仿真結果

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

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

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

(a) 直線路徑仿真結果

最后，根據插秧機的工作路徑，設置一條S型路徑，對比不同算法和不同速度的路徑跟蹤效果，仿真結果如圖6所示。

(a) 不同算法仿真結果

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

綜上所述，上述模型預測控制器橫向偏差較小，可以對規劃路徑實現很好地跟蹤。

4 田間試驗

為驗證本文所設計控制器實際控制效果，結合農藝要求于2021年4月中旬在濟寧恒盛農機專業合作社水田進行試驗。采用AGI-4接收機、NX300電動方向盤和424A16A090霍爾式轉角傳感器，改造井關PZ60型插秧機，搭建插秧機自動駕駛試驗平臺，如圖7所示。控制器采用STM32單片機，電動方向盤與控制器STM32單片機之間采用CANOPEN協議通信，在試驗田內測試模型預測控制器性能。

圖7 水田自動駕駛試驗平臺

在合作社指定地塊進行測試，首先對試驗平臺進行硬件校正，然后將差分基站立在地頭，使用手持式儀器獲取目標地塊坐標，對目標作業地塊進行預定義路徑規劃，最后使用本文設計的控制器對預定義路徑進行路徑跟蹤，并進行自動駕駛作業。對自動駕駛過程中的橫向偏差數據進行統計，統計結果如圖8所示。

從圖8可以看出，自動駕駛插秧機在直線作業段的行駛速度約為1 m/s，路徑跟蹤橫向偏差的最大值為2.02 cm，平均值為1.03 cm；轉彎段路徑跟蹤最大橫向偏差為3.02 cm，平均值為2.09 cm，橫向偏差最大誤差出現在路徑轉彎處。由于插秧機在直線段作業，轉彎段不作業，保證插秧機直線行駛精度，可獲得較好的插秧效果。因此，插秧機自動駕駛的田間試驗主要針對直線作業段進行誤差統計分析。試驗結果表明：自動駕駛控制系統可以較準確地實現對預定義路徑的跟蹤，自動駕駛效果良好。

圖8 水田自動駕駛試驗橫向偏差

5 結論

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

2) 以井關PZ60型插秧機試驗平臺，采用AGI-4接收機、NX300電動方向盤和424A16A090霍爾式轉角傳感器等搭建插秧機自動駕駛硬件系統；進行了插秧機自動駕駛作業試驗，田間試驗結果表明：在插秧機行進速度約為1 m/s時，直線段跟蹤誤差最大為2.02 cm，滿足自動駕駛的精度要求。