水稻直播機自動駕駛模糊自適應控制方法

張 雁 李彥明 劉翔鵬 陶建峰 劉成良 李瑞川

(1.上海交通大學機械與動力工程學院， 上海 200240； 2.山東五征集團， 日照 262399)

0 引言

路徑跟蹤控制方法是農機導航系統中的核心技術，是眾多學者研究的重點和熱點。

根據模型特點，路徑跟蹤控制算法可以分為3種[1]：基于運動學模型的路徑跟蹤控制方法、基于動力學模型的路徑跟蹤控制方法以及與模型無關的路徑跟蹤控制方法。不同的方法應用場景不同，同時，3種方法也各有交匯和融合。基于運動學模型的路徑跟蹤控制方法是在簡化的二輪車模型的基礎上提出的，通常采用最優控制、魯棒控制等設計導航控制器[2-5]；基于動力學模型的路徑跟蹤控制方法是建立在牛頓第二定律的基礎上設計導航控制器的[6-7]；與模型無關的路徑跟蹤控制方法是根據模糊控制[8-10]、神經網絡[11-13]或PID控制[14-15]等設計導航控制器。此外，純追蹤模型也受到了很多學者的重點關注，其只有一個控制參數——前視距離，很多學者提出自適應調整該參數的方法來提高導航控制系統的性能[16-19]。

YOSHISADA等[20]在插秧機平臺上設計了控制器，最大誤差不超過12 cm。偉利國[21]采用滑模自校正控制方法在插秧機上進行了現場實驗，直線路徑跟蹤精度不大于10 cm。李逃昌[4]采用模糊控制調整前視距離的純追蹤模型方法在插秧機平臺上進行了實驗，結果表明，系統具有一定的快速性，但存在一定程度的超調。

為提高水田環境作業的水稻直播機導航控制系統的穩定性和快速性，提出一種利用模糊自適應調整參數Kd的控制方法。

1 水稻直播機運動學模型

1.1 模型說明

在不考慮車胎與地面的相互作用下，假設車輛不發生側滑俯仰等情況，把水稻直播機看作是兩輪車模型[22]。

水稻直播機追蹤期望路徑的示意圖如圖1所示，圖中C為設定跟蹤的期望路徑，M為后輪軸中心，即車體的控制點，P點為車體離目標路徑最近點；假設P點是唯一的，當水稻直播機始終靠近P點時，假設是可以滿足的；s是沿著C方向在M點處的曲線坐標，C(s)表示期望路徑C在這一點的曲率；de為相對于期望路徑的橫向位置誤差。本文規定當水稻直播機在期望路徑的右側時橫向位置誤差為正(de>0)，當水稻直播機在期望路徑左側時橫向偏差為負(de<0)；θe為相對于期望路徑的航向偏差；δ為前輪期望轉角；L為車輪軸距；V是行進速度。

圖1 水稻直播機與期望路徑的位置關系Fig.1 Position relationship between rice planting machinery and expected path

1.2 狀態空間模型

為了便于計算，現提出如下假設：控制變量為農機車速和前輪期望轉角；直播機為剛性體； 1-deC(s)≠0；θe≠π/2。1-deC(s)≠0表明此時后輪軸中心與曲線坐標的曲率中心重合，直播機靠近路徑行駛時，這種情況不會發生。

水稻直播機模型的狀態方程[22-24]為

(1)

1.3 模型的鏈式形式

根據兩輸入系統的鏈式形式[25]，有

(2)

設A=[a1a2a3]T和M=[m1m2]T分別為狀態變量和控制變量。為了更好地說明式(2)是線性的，設

(3)

則式(2)可寫為

(4)

則式(4)是一個線性系統。由于控制律的參數獨立于水稻直播機速度，驅使線性系統變化的變量a1在水稻直播機行駛的路徑上是平均分布的；因此，可以設定

a1=s

(5)

為了簡化計算，通過式(5)可以將非線性農機模型(式(1))轉換成鏈式形式(式(2)和式(4))。為了與鏈式模型(式(2))保持一致，控制變量m1被定義為

(6)

為了便于計算，設

a2=de

(7)

(8)

a3=(1-deC(s))tanθe

(9)

最后，可以得到控制變量m2的表達式為

(10)

1.4 控制律設計

由于式(4)是一個線性系統，則根據線性系統控制理論，可以得到控制律為

m3=-Kda3-Kpa2(Kp、Kd∈R+)

(11)

式中Kp、Kd——系數

將式(11)代入式(4)中得

(12)

式(12)表示a2和a3均收斂于零，即橫向偏差和航向偏差都收斂于零。則將式(11)代入式(3)、(6)、(10)，可以得到控制變量

(13)

水稻直播機跟蹤直線時有C(s)=0，式(13)便可以簡化為

δ(de,θe)=arctan(Lcos3θe(-Kdtanθe-Kpde))

(14)

式中，δ(de,θe)為控制律輸出的期望前輪轉角。Kp和Kd的比值決定了自主導航控制系統的穩定性和快速性，比值越大快速性越好，穩定性越差；相反，比值越小，穩定性越好，快速性變差；當兩者的比例相同時，Kp和Kd的值越大系統的控制越靈敏，但是由于機構限制和控制周期的限制，機構不一定能快速響應，同時還會引起超調。因此，Kp和Kd的值有一個合適的取值范圍。

1.5 PD最優模型仿真

由式(14)可以建立基于狀態控制的最優控制仿真模型，如圖2所示，其中f(u)即為式(14)所示的控制律。Saturation模塊表示機構的限制，即前輪轉角的值限制為車輛轉向機構允許的范圍。

圖2 最優控制仿真模型Fig.2 Optimal control simulation model

根據圖2的仿真模型，設定初始橫向偏差為0.5 m、航向初始偏差為0°時，水稻直播機軸距為1.06 m，直播機行駛速度為1 m/s，可得如圖3、4所示的橫向偏差曲線。

圖3 不同比例系數的橫向偏差曲線Fig.3 Lateral deviation curves of different coefficient ratios

圖4 相同比例系數的橫向偏差曲線Fig.4 Lateral deviation curves of the same coefficient ratio

從圖3可以看出，當Kp/Kd=1(此時Kp=1.2，Kd=1.2)時，自主導航系統超調較小，并且可以快速穩定地跟蹤期望路徑。當Kp和Kd比值增大(Kp=1.2，Kd=0.8)時，自主導航控制系統的快速性變好，超調增大；當Kp和Kd比值減小(Kp=1.2，Kd=1.6)時，系統超調減小，快速性變差。當Kp和Kd比值進一步減小(Kp=1.2，Kd=2.0)時，系統基本沒有超調；如圖4所示，在相同比例系數下，Kp和Kd的取值越大，系統的超調減小，而且快速性也好。因此，在現場實驗時應該找一組較好的控制參數，并且能夠自適應調整。

2 模糊自適應的PD控制器

根據1.4節和1.5節的結論，當Kp和Kd比值越大快速性越好，穩定性越差；相反，比值越小，穩定性越好，快速性變差。則Kp固定為1.2，然后利用模糊方法的自適應調整控制律參數Kd，這樣可以提高自主導航控制系統的穩定性和快速性，為此提出了一種模糊自適應的調整控制律參數的控制方法。

2.1 模糊控制器設計

2.1.1輸入輸出變量模糊化

以橫向偏差de和航向偏差θe為模糊控制器的輸入，控制律參數Kd為其輸出；首先對輸入輸出變量進行模糊化。

(1) 橫向偏差為de，基本論域為[-0.5 m,0.5 m]，量化等級為{-5,-2.5,0,2.5,5}={NB,NM,Z,PM,PB}，量化因子取10。

(2) 航向偏差為θe，基本論域為[-30°,30°]，量化等級為{-5,-2.5,0,2.5,5}={NB,NM,Z,PM,PB}，量化因子取1/6。

(3) 控制律參數Kd。基本論域為[1,3.5]，量化等級為{1,1.63,2.25,2.83,3.5}={Z,S,M,L,VL}，比例因子取1。

2.1.2模糊規則設計原則

橫向偏差和航向偏差越小，參數Kd越大，則Kp/Kd的比值越小，增加系統的穩定性。

橫向偏差越大，或者航向偏差越大，參數Kd越小，則Kp/Kd的比值越大，增加系統的響應速度。

2.1.3隸屬度函數

橫向偏差、航向偏差以及輸出變量控制參數Kd的隸屬度函數均為高斯函數，橫向偏差的隸屬度函數如圖5所示。

圖5 隸屬度函數Fig.5 Membership function

根據量化等級和模糊規則可以設計模糊控制規則表，如表1和圖6所示。

表1 模糊控制規則Tab.1 Fuzzy control regulation

圖6 模糊規則曲面Fig.6 Surface of fuzzy regulation

2.2 模糊自適應控制器仿真

另外，在相同的條件下，進行了模糊自適應調整控制參數Kp和Kd的比值和固定Kp與Kd比值(Kp=1.2，Kd=1.2)的仿真實驗，結果如圖7所示。

圖7 不同方法的橫向偏差對比Fig.7 Comparison of lateral deviation of different methods

從仿真結果可以得到，自適應調整控制參數的方法不僅使系統具有較好的響應速度，而且沒有超調。

3 實驗驗證

3.1 實驗平臺

以上海世達爾公司的洋馬VP6型水稻直播機為實驗平臺，如圖8所示，經機電一體化改造后，搭載由北斗導航GNSS定位系統、自動轉向系統、導航主控制器、轉向角檢測模塊組成的自主導航控制系統。分別在上海松江農業推廣站試驗田附近的水泥地面上和水田環境下進行了驗證實驗。

圖8 實驗平臺Fig.8 Experiment platform

3.2 確定參數實驗

參考仿真模型的數據即Kp=1.2，Kd=1.8，首先在水泥地面上對控制參數進行了整定。設定期望跟蹤路徑為直線AB，并設定水稻直播機所在位置距離直線AB的初始橫向偏差為0.50 m，航向偏差為0°；主變速手柄被設定在前進擋位，由變速踏板控制器設定行走速度恒為0.8 m/s，更改Kp與Kd比值，導航控制系統的橫向偏差曲線如圖9所示。

圖9 不同比例系數的橫向偏差曲線Fig.9 Transverse deviation curves of different ratios of Kp/Kd

圖11 水泥地面路徑跟蹤的實驗結果Fig.11 Track curve and lateral deviation curve of cement experiment

從圖9可以看出，在一定范圍內，Kp/Kd的比值越大，導航系統的響應時間越短，響應速度越快，上線距離越短；同時，超調也越大；Kp/Kd的比值越小，導航控制系統的響應速度越慢，上線距離越長；但超調小，甚至沒有超調(Kp/Kd=1/3)時，系統的穩定性較好，精度較高。這與2.2節仿真的結論是一致的。

進一步處理上面的數據，可以得到導航控制系統穩定后(誤差見圖10)不同Kp/Kd比值的跟蹤效果如表2所示。

圖10 系統穩定后的橫向偏差曲線Fig.10 Transverse deviation curves after system stability

Kp/Kd平均偏差平均絕對偏差最大偏差標準差1/3-0.02840.02880.06920.01561-0.02630.02860.11470.02552-0.02620.02000.09420.0246

從圖10和表2可得，導航控制系統存在一定的穩態誤差；Kp/Kd=1/3時，標準差為0.015 6 m，小于其他兩種比值的標準差(0.025 5 m和0.024 6 m)。Kp/Kd的取值較小時，導航控制系統的穩定性相對較好。

最后，經過反復測試，確定了參數Kp和Kd的取值范圍分別為[0.8,2.1]和[1,3.5]。根據第2節設計的模糊自適應控制器，分別進行了水泥地面和水田環境的路徑跟蹤實驗。

3.3 水泥地面實驗

首先，在水泥地面實驗時，直播機掛載播種器，設定期望跟蹤路徑為直線AB，并設定直播機所在位置距離直線AB的初始橫向偏差為0.50 m，航向偏差為0°；主變速手柄被設定在前進擋位，由變速踏板控制器設定行走速度為0.8 m/s。

如圖11所示，導航控制系統上線距離為4.3 m，無超調，穩定后最大橫向偏差為0.039 8 m。對導航控制系統穩定后(偏差見圖12)的數據進行分析，其結果如表3所示。

將該組數據與3.2節固定Kp/Kd的數據進行對比，結果如圖13所示。由表2、3和圖13可見，模糊自適應PD控制器直線跟蹤的平均偏差和最大偏差等性能指標相對于未使用模糊調整的PD控制器均較好。模糊自適應PD控制器直線跟蹤的平均絕對偏差比前兩組控制器(Kp=1,Kd=3和Kp=1,Kd=1)小，與Kp/Kd=2的控制器的平均絕對偏差基本相當?？梢?，自適應調整控制律參數的控制方法較未使用模糊調整的固定Kp/Kd比值的控制方法超調較小，響應速度更快，跟蹤精度和穩定性也更高。

圖12 系統穩定后的橫向偏差曲線(水泥地面)Fig.12 Lateral error after system stability (cement pavement)

參數平均偏差平均絕對偏差最大偏差標準差數值-0.01810.02090.03980.0275

圖13 模糊自適應的橫向偏差曲線Fig.13 Lateral deviation curves of different Kp/Kd

3.4 水田環境實驗

按照3.2節的實驗設定和實驗步驟，對在水田環境下導航系統的路徑跟蹤效果進行了實驗。為了對比模糊自適應控制方法與傳統方法的控制效果，在水田里還進行了固定PD參數的PD控制方法和純追蹤控制方法的實驗。其中PD控制實驗中取Kp為1.2，Kd為2.2；純追蹤控制實驗中取前視距離為1.2 m。

如圖14所示，導航控制系統上線距離約為6 m，超調為0.04 m，說明導航控制系統具有較好的快速性。

圖14 水田環境路徑跟蹤的實驗結果Fig.14 Experimental results of environmental path tracking in paddy field

圖15 系統穩定后的橫向偏差曲線(水田環境)Fig.15 Lateral error after system stability (paddy field)

導航控制系統穩定后(偏差見圖15和表4)，模糊自適應PD控制的最大偏差為0.208 8 m，比固定參數的PD控制器和純追蹤控制器的最大偏差都要小。同時模糊自適應控制的平均絕對偏差為0.039 0 m，比固定參數的PD控制器和純追蹤控制器的偏差都要小，總體精度更高。模糊自適應PD控制的標準差為0.073 6 m，也比固定參數的PD控制和純追蹤控制小，路徑跟蹤更為穩定。從圖14還可以看到，模糊自適應PD算法可在復雜環境下自動調整Kd，相較于其他兩種算法，超調較小，響應較快。這與理論分析和仿真的結果是一致的。

表4 水田直線路徑跟蹤偏差Tab.4 Linear path tracking error m

根據以上對比實驗，可以得出：

(1) 從圖9、11、14、15可以看出，基于模糊自適應PD控制器的自主導航系統具有較好的響應速度、跟蹤精度和穩定性。

(2) 從表2～4可知，導航系統存在穩態誤差，這是由于車體坐標系與導航坐標系以及前輪角位移傳感器的對稱零點誤差導致的。

(3) 在水田環境作業時，由于導航控制系統沒有添加姿態補償，導致車體發生嚴重側滑側偏時，定位誤差變大，嚴重影響了導航系統的跟蹤精度和穩定性，大大降低了導航精度?？梢酝ㄟ^慣性傳感器測量車身姿態改善這一問題。

4 結論

(1)在建立水稻直播機運動學模型的基礎上，基于鏈式空間狀態模型設計了PD控制器。

(2)建立了水稻直播機的仿真模型，通過仿真得出了Kp和Kd的值，以及兩者比值對系統穩定性和響應速度的影響，基于仿真結論提出了一種模糊自適應控制方法，并通過仿真驗證了該模糊自適應控制方法的快速性、準確性和穩定性。

(3)通過現場實驗確定了控制參數的取值范圍。為了驗證方法的有效性，在水泥地面和水田環境進行了實驗，實驗結果表明，所提出的方法在水田環境下平均絕對偏差為0.039 0 m，最大偏差為0.208 8 m，相較于固定參數的PD控制器和純追蹤控制方法，具有更小的偏差和超調，響應更快，魯棒性更好，有效提高了水稻直播機在水田環境下導航控制系統的穩定性和快速性。