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

(1.上海交通大學(xué)機(jī)械與動(dòng)力工程學(xué)院， 上海 200240； 2.山東五征集團(tuán)， 日照 262399)

0 引言

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

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

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

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

1 水稻直播機(jī)運(yùn)動(dòng)學(xué)模型

1.1 模型說明

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

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

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

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

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

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

(1)

1.3 模型的鏈?zhǔn)叫问?/h3>

根據(jù)兩輸入系統(tǒng)的鏈?zhǔn)叫问絒25]，有

(2)

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

(3)

則式(2)可寫為

(4)

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

a1=s

(5)

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

(6)

為了便于計(jì)算，設(shè)

a2=de

(7)

(8)

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

(9)

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

(10)

1.4 控制律設(shè)計(jì)

由于式(4)是一個(gè)線性系統(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)

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

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

(14)

式中，δ(de,θe)為控制律輸出的期望前輪轉(zhuǎn)角。Kp和Kd的比值決定了自主導(dǎo)航控制系統(tǒng)的穩(wěn)定性和快速性，比值越大快速性越好，穩(wěn)定性越差；相反，比值越小，穩(wěn)定性越好，快速性變差；當(dāng)兩者的比例相同時(shí)，Kp和Kd的值越大系統(tǒng)的控制越靈敏，但是由于機(jī)構(gòu)限制和控制周期的限制，機(jī)構(gòu)不一定能快速響應(yīng)，同時(shí)還會(huì)引起超調(diào)。因此，Kp和Kd的值有一個(gè)合適的取值范圍。

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

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

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

根據(jù)圖2的仿真模型，設(shè)定初始橫向偏差為0.5 m、航向初始偏差為0°時(shí)，水稻直播機(jī)軸距為1.06 m，直播機(jī)行駛速度為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可以看出，當(dāng)Kp/Kd=1(此時(shí)Kp=1.2，Kd=1.2)時(shí)，自主導(dǎo)航系統(tǒng)超調(diào)較小，并且可以快速穩(wěn)定地跟蹤期望路徑。當(dāng)Kp和Kd比值增大(Kp=1.2，Kd=0.8)時(shí)，自主導(dǎo)航控制系統(tǒng)的快速性變好，超調(diào)增大；當(dāng)Kp和Kd比值減小(Kp=1.2，Kd=1.6)時(shí)，系統(tǒng)超調(diào)減小，快速性變差。當(dāng)Kp和Kd比值進(jìn)一步減小(Kp=1.2，Kd=2.0)時(shí)，系統(tǒng)基本沒有超調(diào)；如圖4所示，在相同比例系數(shù)下，Kp和Kd的取值越大，系統(tǒng)的超調(diào)減小，而且快速性也好。因此，在現(xiàn)場實(shí)驗(yàn)時(shí)應(yīng)該找一組較好的控制參數(shù)，并且能夠自適應(yīng)調(diào)整。

2 模糊自適應(yīng)的PD控制器

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

2.1 模糊控制器設(shè)計(jì)

2.1.1輸入輸出變量模糊化

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

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

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

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

2.1.2模糊規(guī)則設(shè)計(jì)原則

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

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

2.1.3隸屬度函數(shù)

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

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

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

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

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

2.2 模糊自適應(yīng)控制器仿真

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

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

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

3 實(shí)驗(yàn)驗(yàn)證

3.1 實(shí)驗(yàn)平臺(tái)

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

圖8 實(shí)驗(yàn)平臺(tái)Fig.8 Experiment platform

3.2 確定參數(shù)實(shí)驗(yàn)

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

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

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

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

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

圖10 系統(tǒng)穩(wěn)定后的橫向偏差曲線Fig.10 Transverse deviation curves after system stability

Kp/Kd平均偏差平均絕對偏差最大偏差標(biāo)準(zhǔn)差1/3-0.02840.02880.06920.01561-0.02630.02860.11470.02552-0.02620.02000.09420.0246

從圖10和表2可得，導(dǎo)航控制系統(tǒng)存在一定的穩(wěn)態(tài)誤差；Kp/Kd=1/3時(shí)，標(biāo)準(zhǔn)差為0.015 6 m，小于其他兩種比值的標(biāo)準(zhǔn)差(0.025 5 m和0.024 6 m)。Kp/Kd的取值較小時(shí)，導(dǎo)航控制系統(tǒng)的穩(wěn)定性相對較好。

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

3.3 水泥地面實(shí)驗(yàn)

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

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

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

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

參數(shù)平均偏差平均絕對偏差最大偏差標(biāo)準(zhǔn)差數(shù)值-0.01810.02090.03980.0275

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

3.4 水田環(huán)境實(shí)驗(yàn)

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

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

圖14 水田環(huán)境路徑跟蹤的實(shí)驗(yà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)

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

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

根據(jù)以上對比實(shí)驗(yàn)，可以得出：

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

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

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

4 結(jié)論

(1)在建立水稻直播機(jī)運(yùn)動(dòng)學(xué)模型的基礎(chǔ)上，基于鏈?zhǔn)娇臻g狀態(tài)模型設(shè)計(jì)了PD控制器。

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

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