擾動下農用運輸車輛路徑跟蹤控制器設計與試驗

劉正鐸 張萬枝 呂釗欽 鄭文秀 穆桂脂 程向勛

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

0 引言

自動駕駛技術在農業機械研究中的應用[1-8]越來越受到學者們的重視。農用運輸車輛需要快速、準確地跟蹤到期望路徑。現階段農用運輸車主要是人工駕駛，不僅工作效率低，而且還加大了勞動人員的工作強度。文獻[8]利用GPS與電子羅盤實現了拖拉機的自動導航，結果表明，平均偏差率為14.38%，最大偏差率為31.10%,GPS信號接收效果及定位精度均不理想。這是由于果樹的枝葉遮擋GPS接收機，使其不能穩定接收衛星信號而造成的。為提高定位精度，RTK-DGPS高精度自動導航系統在實際農業生產中得到廣泛應用[9-12]，文獻[13]采用 RTK-DGPS定位技術，設計了東方紅X-804型拖拉機的自動導航系統，通過田間試驗驗證了控制系統的精確性和可靠性。文獻[14]通過改進純追蹤模型的控制方法，在仿真條件下得到了較好的跟蹤結果，但是其控制器的魯棒穩定性較差，沒有一套完善的參數調節標準，難以應用于實際生產。文獻[15]基于最小二乘法實現了車輛的參數辨識，基于GNSS設計了路徑跟蹤模糊控制器,但是在實際應用過程中跟蹤偏差較大。文獻[16]利用模糊控制調整純追蹤模型的前視距離，設計了水稻插秧機的路徑跟蹤控制器，試驗證明，該方法在速度0.3 m/s與1 m/s的情況下具有很好的適應性。

由此可以看出，由于田間的作業環境復雜(樹木的枝葉遮擋信號、高溫、沙塵、地面不平等)，農用車輛的路徑跟蹤問題難點主要在于跟蹤精度與跟蹤穩定性上[17-19]。

模型預測控制被廣泛應用于車輛自動導航[20]、主動前輪轉向[21]等方面，它能及時彌補模型失配、時變、干擾等因素引起的不確定性。文獻[22-24]從理論角度詳細闡述了將線性模型預測控制器應用到汽車導航的可能性并進行了相關的仿真試驗，但其中并未涉及控制器的魯棒性分析及證明，更沒有通過實物驗證。針對上述問題，本文將模型預測控制與農用運輸車輛相結合，設計導航控制器，實現農用運輸車輛的路徑跟蹤。本文所述導航控制器能夠通過內部積分器實時計算農用車輛的位姿信息，實現對所設計路徑的有效跟蹤，通過滾動優化實時反饋，增強控制系統穩定性，減少對外部環境的依賴。

1 路徑跟蹤的運動學模型

對農用運輸車進行運動學分析，建立如圖1所示的模型。該模型前輪轉向，后輪驅動。在整個運動學分析過程中，將農用運輸車視為剛體，運行在水平面上。為了確定車輛在整個運行軌跡中的位姿，建立導航坐標系。選取農用運輸車后軸中心點為參考點并定義小車的位姿信息(x,y,φ)，其中(x,y)為農用運輸車后軸中心坐標，φ為農用運輸車航向角。在不考慮車輛側傾、俯仰、側滑等運動的情況下，可將低速行駛的農用運輸車簡化為二自由度的運動學模型[7]。其運動學方程為

(1)

式中l——軸距δ——前輪轉角

v——后軸中心速度

圖1 農用運輸車的運動學模型Fig.1 Kinematic model of agricultural transport vehicle

2 線性模型預測控制器的設計

2.1 線性誤差模型

為進一步提高定位精度，減少定位過程中的外界干擾，通過控制器中的內置積分器來處理傳感器收集到的速度與轉角信息，通過對速度與轉角的不間斷積分得到農用運輸車的實時位姿為

(2)

式中Xt、Yt、φt——t時刻農用車的實際位姿

x0、y0、φ0——農用車的初始位姿

用r表示目標軌跡，xr、yr、φr為目標位姿，vr、δr為目標速度和目標前輪轉角，農用運輸車的線性誤差模型可以表示為

(3)

其中，控制增量為

Δv=v-vrΔδ=δ-δr

狀態偏差為

xe=x-xrye=y-yrφe=φ-φr

為將該模型應用到控制器，對式(3)進行離散化處理并寫成狀態空間方程形式

(4)

其中

ξt=(x,y,φ)ut=(v,δ)

式中ut(k)——t時刻遞推k次系統控制量

ξt(k)——t時刻遞推k次系統狀態量

Ak,t——t時刻遞推k次系統矩陣

Bk,t——t時刻遞推k次輸入矩陣

ηt(k)——t時刻遞推k次系統輸出量

k——離散變量T——采樣周期

t——采樣時刻

2.2 誤差模型魯棒性分析

農用運輸車輛在田間行駛時會受到各種各樣的擾動，為使農用運輸車穩定性達到使用要求，對式(3)進行魯棒性分析。

定義新的狀態變量為

(5)

于是將證明式(3)的收斂性轉換為證明式(5)的收斂性。定義李雅普諾夫函數為

(6)

求導得

(7)

其中Δv、Δδ的取值與控制器參數采樣周期T有關，為了便于證明,令

(8)

將式(8)代入式(7)得

(9)

根據李雅普諾夫第二法可以得出當T>0時，V、φe收斂，于是可以得到xe、ye、φe收斂且收斂于0。

農用運輸車輛在田間行駛過程中的各種擾動，最終都可以歸結為沿x方向的偏差dx與沿y方向的偏差dy，將dx、dy代入式(3)得

(10)

將式(8)、(10)代入式(7)得

(11)

用楊氏不等式對式(11)右側進行處理得

由以上分析可知當采樣周期T>0時，農用運輸車在沒有外界干擾情況下即可實現路徑跟蹤，當采樣周期0

2.3 預測模型

為得到系統的預測模型，構建狀態變量為

λ(k|t)=[ξ(k|t)u(k-1|t)]

(12)

將式(12)代入式(4)得到新的狀態空間表達式

(13)

式中n——狀態量維度m——控制量維度

ΔU——控制增量

Im——m維單位矩陣

O——零矩陣

從而得到輸出方程即系統的預測模型為

Γ(t)=ψξ(t|t)+ΘΔU(t)

(14)

其中

式中 Δu——系統控制量增量

Nc——系統控制時域

Np——系統預測時域

2.4 設計目標函數

為將路徑跟蹤問題轉換為二次規劃問題，構造目標函數[25-26]。通過權重系數將系統狀態量偏差與控制量偏差關聯起來，設置狀態量與控制量的約束條件，通過有效集法把每步迭代中有效約束作為等式約束進行求解，重復此過程尋得最優解，同時引入松弛因子防止系統出現無可行解現象。目標函數為

(15)

其中 ΔΓ=Γ-ΓrΓmin≤Γ(t)≤Γmax

ΔΓmin≤ΔΓ(t)≤ΔΓmax
Γr(k)=[ηr(t+1|t) …ηr(t+Np|t)]T
umin≤u(t+k)≤umax
Δumin≤Δu(t+k)≤Δumax

式中ρ——權值系數ε——松弛因子

Q——權重矩陣R——權重系數

J——目標函數

Γmin、Γmax——狀態變量約束最值

ΔΓmin、ΔΓmax——狀態變量增量約束最值

umin、umax——控制量約束最值

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

2.5 滾動優化

文中采用內點法進行求解，將t時刻控制輸入量的第1個元素作為實際控制量。在t+1時刻，重復以上控制過程實現最優控制。

3 控制器仿真與分析

3.1 仿真路徑

圖2為農用無人駕駛車輛從O點到G點的參考軌跡。農用車輛從O點進入，對其進行噴藥或果實運輸，G點為終點。其中A、B、C、D、E、F、G、H、J、K、L為路徑方程改變的節點，在一定程度上其各項參數可以反映出控制器對該路段的路徑跟蹤性能。其中定義A點為路徑跟蹤動態響應階段最大超調量的坐標點。定義F點為擾動測試點，在此位置加入擾動以測試控制器的魯棒穩定性。基于Matlab平臺進行模型預測控制器設計，從采樣周期T、橫向偏差擾動Yr、前輪轉角擾動δfr等3方面進行仿真分析。

圖2 參考路徑Fig.2 Reference path

根據路徑信息(參考路徑的坐標范圍與小車轉向時前輪轉角的取值范圍設定目標函數的極值約束)與目標速度設置控制器參數如下:l=1;Q=diag(100,100,100),R=100;umin=[-3.2 m/s -0.5 rad]T,umax=[3.2 m/s 0.5 rad]T；Δumin=[-0.05 m/s -0.47 rad]T, Δumax=[0.05 m/s 0.47 rad]T;Γmin=[-5 -1 -0.5]T,Γmax=[5 1 0.5]T;ΔΓmin=[-0.05 -0.05 -0.008 2]T,ΔΓmax=[0.05 0.05 0.008 2]T。

3.2 采樣周期對路徑跟蹤的影響

設定控制器參數為：Nc=30，Np=60，v=2 m/s，T為0.05、0.10、0.15 s。在導航坐標系下，路徑跟蹤結果如圖3所示。

圖3 采樣周期對路徑跟蹤的影響Fig.3 Effect of sampling period on path tracking

可以看出，3種不同采樣周期下的控制器路徑跟蹤效果均較好。圖3b農用運輸車的縱向跟蹤偏差，反映農用運輸車路徑跟蹤的實時性，在AC、EG、JL直線段出現穩態縱向跟蹤偏差且偏差隨采樣周期T的增大而增大，這說明車輛在跟蹤圓形路徑時相對于仿真路徑產生了延遲，且延遲隨采樣周期的增大而增大。圖3c為農用運輸車的橫向跟蹤偏差，反映了農用運輸車的跟蹤精度，可以看出節點E、J處橫向偏差出現較大波動，這是由于路徑方程改變引起的，此時控制器需要重新預測控制量以適應新的參考路徑。圖3d反映出路徑跟蹤時農用運輸車的行駛穩定性，可以看出，在仿真條件下農用運輸車可以穩定地跟蹤參考路徑。

3.3 橫向擾動對路徑跟蹤的影響

設定控制器參數為：Np=60，Nc=30，v=2 m/s，T=0.05 s，Yr為0.5、1.0、1.5 m。在導航坐標系下，路徑跟蹤結果如圖4所示。

圖4 橫向擾動對路徑跟蹤的影響Fig.4 Effect of lateral disturbance on path tracking

當農用運輸車在果園運輸時，車輛會進行人為的橫向移動，將此移動作為擾動加入路徑分析。在節點F處加入橫向偏差擾動，可以看出，在3種不同橫向偏差的擾動作用下控制器的魯棒性均較好，隨著擾動的增大,跟蹤偏差也在增大；當擾動出現時，控制器會快速地做出反應，及時調節前輪轉角與速度，使車輛盡快回到參考路徑上行駛。

3.4 前輪轉角的擾動對路徑跟蹤的影響

設定控制器參數為：Np=60，Nc=30，v=2 m/s，T=0.03 s，δfr為5°、10°、15°。在導航坐標系下，路徑跟蹤結果如圖5所示。

圖5 前輪轉角擾動對路徑跟蹤的影響Fig.5 Effect of front wheel angle on path tracking

當農用運輸車在果園間行駛時，由于地面不平等因素會造成車輛在行駛過程中前輪轉角的改變，在F點加入前輪轉角擾動。對路徑跟蹤情況進行分析。從圖5可以看出，跟蹤偏差隨著前輪偏角擾動的增大而增大，此時控制器會快速地做出反應，及時調節前輪轉角，使車輛盡快地回到參考路徑上行駛。

4 試驗與結果分析

4.1 路徑跟蹤系統結構

自主設計的田間路徑跟蹤試驗小車如圖6所示，具體結構參數如表1所示。

路徑信息采集模塊：轉角傳感器為E6B2-CCWZ3E型編碼器，測量誤差為±0.3°；速度傳感器為JK5002D型霍爾接近開關，測速誤差±0.05 m/s。控制執行模塊：轉向驅動器為ASMT-01series大功率直線舵機控制器；轉向執行裝置為42BYGH47型步進電機驅動渦輪蝸桿式轉向裝置,其行程為80 mm,速度最快可達29 mm/s；后輪驅動器為CM2010型有刷電機控制器；后輪驅動裝置為DMW86型無刷直流電機，額定轉速為3 200 r/min，額定功率為1 000 W。路徑信息處理模塊：導航控制器的下位機為STM32F103ZET6型單片機，其主要任務是采集試驗小車轉速傳感器與前輪轉角傳感器收集來的信息并轉換成數字信號，通過Wi-Fi傳輸至導航控制器上位機戴爾Vostro計算機中的Matlab軟件，從而進行算法處理。

圖6 試驗小車Fig.6 Test car1.前輪轉角傳感器 2.速度傳感器 3.差速驅動裝置 4.前輪轉向驅動器 5.信號轉換裝置 6.蓄電池 7.后輪驅動器 8.前輪轉向裝置

參數數值/方式外形尺寸(長×寬×高)/(cm×cm×cm)120×63×33軸距/cm100最大速度/(m·s-1)3.2 質量/kg50控制方式PWM輪胎半徑/cm15

4.2 場地試驗

試驗場地為山東農業大學本部紫葉李樹林，如圖7所示。場地尺寸及樹木位置與仿真時(圖2)一致。

圖7 山東農業大學紫葉李樹林Fig.7 Prunus cerasifera forest in Shandong Agricultural University

初始時，車輛中心位置與路徑起點重合。設定控制器參數為v=2 m/s、Np=60、Nc=30、T=0.05 s。為了更好地驗證本文所述控制器的魯棒性，向節點F處添加前輪轉角擾動δfr為5°、10°、15°。試驗結果如圖8所示。

由圖8可以看出，場地試驗中小車在跟蹤參考路徑時，直線路段偏差較小，圓形路段與加入前輪轉角擾動時偏差較大，試驗小車可以有效地跟蹤參考路徑。其具體數據如表2所示。

由試驗結果可以看出，不同轉角擾動下的最大橫向偏差主要出現在節點C處與添加擾動節點F處，分別為19.56、20.97、23.89 cm，出現較大偏差一方面是因為受到試驗條件的制約：小車各個機構之間的鏈接間隙、輪胎打滑、數據傳輸與處理的延遲、上位機 (計算機上的Matlab)與下位機(STM32單片機) 之間的耦合等；另一方面是因為路徑方程改變的位置大都處于直線路徑的末端，此時小車在行駛過程中已經有了較大的累積偏差，再加上路徑方程的突然改變，致使控制器無法在短時間內解出最優控制量。

圖8 場地試驗中前輪轉角擾動對路徑跟蹤的影響Fig.8 Effect of front wheel angle disturbance on path tracking in field test

cm

通過試驗可以看出,該控制器對于直線路段有著較好的控制作用，在控制精度上，v=2 m/s時直線段最大橫向偏差為10.57 cm，均值保持在8.49 cm以內；在跟蹤實時性上，最大縱向偏差為25.77 cm，均值保持在18.47 cm以內。試驗小車跟蹤圓弧段的偏差較大，最大橫向偏差為22.83 cm,最大縱向偏差為39.42 cm，這主要是由于控制器中采用線性模型作為預測方程造成的，在跟蹤圓形路徑時，受輪胎側偏、車輛質心變化等因素的影響，小車非線性特性突出，此時控制器依然采用線性模型預測小車下一時刻運動，與實際情況差別較大，造成了較大的跟蹤偏差。

在添加前輪轉角擾動的節點F處，橫向偏差隨擾動的增大而增大，當δfr=15°時，橫向偏差最大為23.89 cm，縱向偏差最大為62.53 cm,說明試驗小車在跟蹤精度上偏離參考軌跡23.89 cm，在跟蹤實時性上，落后仿真路徑62.53 cm。由圖8可以看出，此時控制器可以迅速起到調節作用，使小車回到參考軌跡繼續行駛。

由此可以看出本文所述控制器無論是在行駛穩定性上，還是在跟蹤精度上，都可以滿足農用運輸車輛對路徑跟蹤的要求。

4.3 對比分析

與現有研究進行對比，文獻[27-28]在未使用外部定位的情況下實現了拖拉機的自動導航，減少了控制器對外部環境的依賴。其中文獻[27]結合最優控制理論，設計了基于速度和轉向角的雙參數最優控制算法，橫向偏差最大值為12 cm；文獻[28]設計一種基于指數趨近律的滑模變結構控制器，速度為2 m/s時，橫向偏差最大值為11 cm。分析可知：本文采用的線性模型預測控制方法，在跟蹤精度上要優于文獻[27-28]，同時利用農用車輛的縱向跟蹤偏差分析了路徑跟蹤的實時性，并對控制器的魯棒性做了詳細研究，通過仿真與試驗驗證了農用車路徑跟蹤在添加了外部擾動情況下的行駛穩定性。

5 結論

(1)針對農用運輸車輛作業環境復雜，路徑跟蹤穩定性較差等問題，結合線性模型預測控制理論設計了農機自動導航控制器。該控制器通過內部積分器實現了農用車的定位，通過引入松弛因子解決了運算過程中出現非可行性解的問題，從而實現農機設備的自動導航。

(2)通過構建車輛誤差模型的李雅普諾夫函數，得到系統采樣周期T與車輛魯棒穩定性的關系，并通過仿真與試驗驗證了本文所述控制器在添加外界擾動的情況下依然可以有效跟蹤參考路線。

(3)在仿真中，分析了采樣周期T、橫向擾動Yr、前輪轉角擾動δfr對路徑跟蹤穩定性的影響；在場地試驗中，v=2 m/s無擾動情況下試驗小車最大橫向偏差為22.83 cm,均值為8.49 cm，基本實現了農用車輛的自動導航;前輪轉角δfr擾動下的最大橫向跟蹤偏差為23.89 cm，均值為8.49 cm,偏差隨擾動的增大而增大，控制器可以迅速起到調節作用，使小車回到參考軌跡繼續行駛。