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

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

0 引言

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

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

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

為提高水田環(huán)境作業(yè)的水稻直播機導航控制系統(tǒng)的穩(wěn)定性和快速性，提出一種利用模糊自適應調(diào)整參數(shù)Kd的控制方法。

1 水稻直播機運動學模型

1.1 模型說明

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

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

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

1.2 狀態(tài)空間模型

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

水稻直播機模型的狀態(tài)方程[22-24]為

(1)

1.3 模型的鏈式形式

根據(jù)兩輸入系統(tǒng)的鏈式形式[25]，有

(2)

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

(3)

則式(2)可寫為

(4)

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

a1=s

(5)

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

(6)

為了便于計算，設

a2=de

(7)

(8)

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

(9)

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

(10)

1.4 控制律設計

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

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

(11)

式中Kp、Kd——系數(shù)

將式(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)為控制律輸出的期望前輪轉(zhuǎn)角。Kp和Kd的比值決定了自主導航控制系統(tǒng)的穩(wěn)定性和快速性，比值越大快速性越好，穩(wěn)定性越差；相反，比值越小，穩(wěn)定性越好，快速性變差；當兩者的比例相同時，Kp和Kd的值越大系統(tǒng)的控制越靈敏，但是由于機構(gòu)限制和控制周期的限制，機構(gòu)不一定能快速響應，同時還會引起超調(diào)。因此，Kp和Kd的值有一個合適的取值范圍。

1.5 PD最優(yōu)模型仿真

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

圖2 最優(yōu)控制仿真模型Fig.2 Optimal control simulation model

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

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

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

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

2 模糊自適應的PD控制器

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

2.1 模糊控制器設計

2.1.1輸入輸出變量模糊化

以橫向偏差de和航向偏差θe為模糊控制器的輸入，控制律參數(shù)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) 控制律參數(shù)Kd?；菊撚驗閇1,3.5]，量化等級為{1,1.63,2.25,2.83,3.5}={Z,S,M,L,VL}，比例因子取1。

2.1.2模糊規(guī)則設計原則

橫向偏差和航向偏差越小，參數(shù)Kd越大，則Kp/Kd的比值越小，增加系統(tǒng)的穩(wěn)定性。

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

2.1.3隸屬度函數(shù)

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

圖5 隸屬度函數(shù)Fig.5 Membership function

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

表1 模糊控制規(guī)則Tab.1 Fuzzy control regulation

圖6 模糊規(guī)則曲面Fig.6 Surface of fuzzy regulation

2.2 模糊自適應控制器仿真

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

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

從仿真結(jié)果可以得到，自適應調(diào)整控制參數(shù)的方法不僅使系統(tǒng)具有較好的響應速度，而且沒有超調(diào)。

3 實驗驗證

3.1 實驗平臺

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

圖8 實驗平臺Fig.8 Experiment platform

3.2 確定參數(shù)實驗

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

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

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

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

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

圖10 系統(tǒng)穩(wěn)定后的橫向偏差曲線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可得，導航控制系統(tǒng)存在一定的穩(wěn)態(tài)誤差；Kp/Kd=1/3時，標準差為0.015 6 m，小于其他兩種比值的標準差(0.025 5 m和0.024 6 m)。Kp/Kd的取值較小時，導航控制系統(tǒng)的穩(wěn)定性相對較好。

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

3.3 水泥地面實驗

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

如圖11所示，導航控制系統(tǒng)上線距離為4.3 m，無超調(diào)，穩(wěn)定后最大橫向偏差為0.039 8 m。對導航控制系統(tǒng)穩(wěn)定后(偏差見圖12)的數(shù)據(jù)進行分析，其結(jié)果如表3所示。

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

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

參數(shù)平均偏差平均絕對偏差最大偏差標準差數(shù)值-0.01810.02090.03980.0275

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

3.4 水田環(huán)境實驗

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

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

圖14 水田環(huán)境路徑跟蹤的實驗結(jié)果Fig.14 Experimental results of environmental path tracking in paddy field

圖15 系統(tǒng)穩(wěn)定后的橫向偏差曲線(水田環(huán)境)Fig.15 Lateral error after system stability (paddy field)

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

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

根據(jù)以上對比實驗，可以得出：

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

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

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

4 結(jié)論

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

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

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