基于模型預測的插秧機路徑跟蹤控制算法

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

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

0 引言

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

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

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

1 車輛運動學模型及基于模型預測的路徑跟蹤控制器設計

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

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

圖1 車輛運動學模型

(1)

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

(2)

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

(3)

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

(4)

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

(5)

為了能夠將插秧機二輪車模型應用于模型預測控制器的設計，利用前向歐拉公式[24]，將上述線性化系統(tǒng)進行離散化處理可得

(6)

其中

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

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

(7)

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

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

(8)

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

(9)

其中

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

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

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

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

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

(10)

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

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

(11)

其中

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

NP——預測時域長度

Nc——控制時域長度

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

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

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

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

(12)

轉換為

Umin≤AΔUt+Ut≤Umax

(13)

其中

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

(14)

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

R、Q——權重矩陣

ρ——權重系數(shù)

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

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

(15)

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

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

(16)

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

2 路徑跟蹤誤差模型

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

圖2 路徑跟蹤誤差模型

(17)

式中xcr——平面坐標中車輛后軸中心x軸坐標，m

ycr——平面坐標中車輛后軸中心y軸坐標，m

xS——投影點S處x軸坐標，m

yS——投影點S處y軸坐標，m

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

Φ=57.3(θ-θroad)

(18)

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

3 路徑跟蹤算法仿真

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

3.1 直線跟蹤仿真

直線路徑跟蹤仿真基本設定為：插秧機初始狀態(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。其余模型預測算法參數(shù)如表1所示，在Matlab中編寫相關程序，對無人駕駛車輛的直線路徑跟蹤過程進行仿真。

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

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

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

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

3.2 曲線跟蹤仿真

曲線參考路徑設定為

(19)

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

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

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

dP=vxNPT

(20)

式中dP——預測距離

vx——插秧機縱向車速

在采樣時間及縱向車速不變時，預測距離主要與預測時域有關。若預測距離過大，將增大跟蹤誤差權重，影響插秧機跟蹤精度，若預測距離過小，控制量及增量前后時刻相差較大，導致車輛無法及時準確轉向。另外，本文針對控制對象所控車速為插秧機后軸實時車速，在插秧機轉向時，無法保證縱向速度不變，且預測時域為固定值，故插秧機轉向動作發(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°。結果表明，所設路徑跟蹤控制器可用于曲線追蹤，但設計地頭轉向曲線時，應避免連續(xù)換向曲線設計。

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

4 實車試驗

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

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

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

圖7 平臺實物圖

采集插秧機實際行走的經(jīng)緯度數(shù)據(jù)，將參考路徑及實車行走的經(jīng)緯度數(shù)據(jù)轉換為墨卡托投影下的平面坐標數(shù)據(jù)，并將所有平面坐標數(shù)據(jù)減去各自第1組平面坐標，繪制實車試驗路徑跟蹤曲線。插秧機由(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%，表明跟蹤過程的實時航向轉換變化平緩。

圖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)模型預測算法可成功應用于自動駕駛農(nóng)機的路徑跟蹤，且對路徑追蹤的過程中橫向、航向偏差值較小且變化平穩(wěn)。

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

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

6 結論

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

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

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