關(guān)鍵詞：自動(dòng)導(dǎo)航；路徑跟蹤；預(yù)瞄模型；模糊PID；路徑優(yōu)化；農(nóng)機(jī)導(dǎo)航

0 引言

農(nóng)業(yè)現(xiàn)代化迅速發(fā)展，對(duì)農(nóng)業(yè)機(jī)械自動(dòng)駕駛技術(shù)的要求逐漸提高，而農(nóng)機(jī)路徑跟蹤控制是實(shí)現(xiàn)農(nóng)機(jī)自動(dòng)駕駛的關(guān)鍵技術(shù)[1-2]。常用的農(nóng)機(jī)路徑跟蹤控制方法有模型預(yù)測(cè)控制[3-5]、純追蹤控制[6-7]、PID控制[8-9]及模糊控制[10-11]等。

張華強(qiáng)等[12]將一種改進(jìn)純追蹤模型用于農(nóng)機(jī)路徑跟蹤控制，該方法顯著提高了農(nóng)機(jī)在作業(yè)時(shí)的直線行駛精度。張智剛等[13]開發(fā)一種基于預(yù)瞄跟隨的路徑跟蹤方法進(jìn)行導(dǎo)航控制，能夠滿足農(nóng)業(yè)機(jī)械路徑跟蹤的精度要求。王輝等[14]提出一種基于預(yù)瞄追蹤模型的方法，可以有效提高農(nóng)機(jī)導(dǎo)航系統(tǒng)的性能，并且具有較好的抗干擾能力。

PID控制方法常被應(yīng)用于工程領(lǐng)域，但其控制參數(shù)的整定較為復(fù)雜，通常需要通過大量試驗(yàn)來確定合適的參數(shù)取值。張雁等[15]在PID控制器的基礎(chǔ)上，使用模糊控制調(diào)整微分增益，提高了農(nóng)機(jī)跟蹤速度與系統(tǒng)穩(wěn)定性。尹奇輝等[16]以預(yù)瞄控制為基礎(chǔ)，采用模糊神經(jīng)網(wǎng)絡(luò)調(diào)節(jié)PID系數(shù)，該方法調(diào)節(jié)時(shí)間短、穩(wěn)定性好。

本研究考慮到單一預(yù)瞄控制自適應(yīng)性相對(duì)不足的問題，將預(yù)瞄模型生成的預(yù)瞄航向角偏差作為航向角變化量的目標(biāo)值引入農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型，得到一種可隨速度變化的前輪轉(zhuǎn)角控制方法，并將模糊PID方法加入對(duì)此角度偏差的反饋控制，試驗(yàn)驗(yàn)證本方法可有效提高系統(tǒng)性能。

1 改進(jìn)型模糊PID控制器設(shè)計(jì)

在地面坐標(biāo)系（xOy）下創(chuàng)建預(yù)瞄模型，如圖1所示。定義A點(diǎn)為農(nóng)機(jī)車體中心，過點(diǎn)A向曲線路徑在C點(diǎn)的切線作垂線，垂足為點(diǎn)B，并稱直線AC為預(yù)瞄直線，定義d為線段BC的長(zhǎng)度，稱作預(yù)瞄距離。e定義為線段AB的長(zhǎng)度，稱作橫向偏差，本研究將橫向偏差定義為一個(gè)有符號(hào)數(shù)，若橫向偏差e為正數(shù)，即農(nóng)機(jī)在目標(biāo)路徑右側(cè)；若橫向偏差e為負(fù)數(shù)，即農(nóng)機(jī)在目標(biāo)路徑左側(cè)。Φr為目標(biāo)路徑切線的航向角，Φc為農(nóng)機(jī)機(jī)體的當(dāng)前航向角；Φe為農(nóng)機(jī)機(jī)體當(dāng)前航向角與目標(biāo)路徑航向角之差，稱作航向角偏差；θ為預(yù)瞄直線與目標(biāo)路徑切點(diǎn)連線的夾角，稱作橫向偏差角；Φ為農(nóng)機(jī)車體航向所在直線與預(yù)瞄直線之間的夾角，稱作預(yù)瞄航向角偏差。δ為農(nóng)機(jī)前輪轉(zhuǎn)角，本研究定義前輪轉(zhuǎn)角為有符號(hào)數(shù)，若前輪轉(zhuǎn)角δ為正數(shù)，即前輪相對(duì)中心位置偏左；若前輪轉(zhuǎn)角δ為負(fù)數(shù)，即前輪相對(duì)中心位置偏右。

此時(shí)若令前輪轉(zhuǎn)角δ等于Φ即可達(dá)到跟蹤目的。但在實(shí)際農(nóng)田工作環(huán)境下，農(nóng)機(jī)速度會(huì)受到外界很多因素的影響，從而影響控制效果。因此為了提高控制質(zhì)量，本研究將速度參量引入控制律中。如圖2所示，構(gòu)建農(nóng)機(jī)兩輪運(yùn)動(dòng)學(xué)模型，其中L為軸距，v為瞬時(shí)速度，R為瞬時(shí)轉(zhuǎn)彎半徑。

當(dāng)航向角變化量ΔΦc等于預(yù)瞄航向角偏差Φ時(shí)，此前輪轉(zhuǎn)角不僅可以使農(nóng)機(jī)跟蹤目標(biāo)路徑，還在不同速度下都具有一定的自適應(yīng)性。因此，將式（4）中預(yù)瞄航向角偏差Φ引入式（8），得到一種改善的控制律為

在真實(shí)農(nóng)田環(huán)境下，當(dāng)農(nóng)機(jī)與預(yù)定路徑距離較遠(yuǎn)時(shí)，為提高系統(tǒng)的快速性，以較大的轉(zhuǎn)向角逼近目標(biāo)路徑；當(dāng)農(nóng)機(jī)與目標(biāo)路徑距離適中時(shí)，為提高農(nóng)機(jī)在轉(zhuǎn)向時(shí)的穩(wěn)定性與跟蹤路徑曲線的平滑性，以柔性轉(zhuǎn)向角逼近目標(biāo)路徑；當(dāng)農(nóng)機(jī)與目標(biāo)路徑距離較小時(shí)，以較小的轉(zhuǎn)向角逼近目標(biāo)路徑。因此為了使快速性和穩(wěn)定性達(dá)到平衡，引入一種路徑優(yōu)化方法為

2 模糊PID參數(shù)整定方法

2.1 變量模糊化與量化因子選取

模糊控制器的輸入量設(shè)定為預(yù)瞄航向角偏差Φ及預(yù)瞄航向角偏差變化率ΔΦ，模糊控制器的輸出量設(shè)定為ΔKp、ΔKi和ΔKd3個(gè)增量系數(shù)。

為了對(duì)輸入變量進(jìn)行模糊化，將其模糊論域設(shè)定為[?3.0，?2.0，?1.0，0，1.0，2.0，3.0]，分別與[NB、NM、NS、ZR、PS、PM、PB]7個(gè)模糊語言值對(duì)應(yīng)。預(yù)瞄航向角偏差的基本論域被設(shè)定為[?90°，90°]，量化因子設(shè)定為1/30。預(yù)瞄航向角偏差變化率的基本論域設(shè)定為[?90°，90°]。量化因子設(shè)定為1/30。

設(shè)定輸出變量模糊論域的范圍分別為[?0.100，?0.066，?0.033，0，0.033，0.066，0.100]、[?0.015，?0.010，?0.005，0，0.005，0.010，0.015]、[?0.070，?0.046，?0.023，0，0.023，0.046，0.070]，分別與[NB、NM、NS、ZR、PS、PM、PB]7個(gè)模糊語言值對(duì)應(yīng)。比例控制變量ΔKp、積分控制變量ΔKi、微分控制變量ΔKd的基本論域根據(jù)一組整定完成的初始PID參數(shù)確定，分別為[?0.1，0.1]、[?0.015，0.015]和[?0.07，0.07]。比例因子都為1。

2.2 模糊規(guī)則制定

控制參數(shù)整定原則：當(dāng)偏差為較大值時(shí)，為滿足系統(tǒng)快速性，需要對(duì)Kp、Kd取較大的值，通常取Ki=0。當(dāng)偏差為中間值時(shí)，在滿足系統(tǒng)響應(yīng)速度的情況下，還要使系統(tǒng)的超調(diào)小，需降低Kp值，此時(shí)Kd對(duì)系統(tǒng)影響大，需降低Kd值，Ki取值適中。當(dāng)偏差是較小值時(shí)，需要適當(dāng)減小Kp和Ki的值。為加強(qiáng)控制系統(tǒng)抗擾動(dòng)能力，避免系統(tǒng)到達(dá)目標(biāo)值后產(chǎn)生振蕩，當(dāng)變化率較大時(shí)，減小Kd值；反之，增加Kd值。模糊規(guī)則如表1、表2和表3所示。

2.3 解模糊化

在處理解模糊化過程中，本研究采用重心法，計(jì)算公式為

3 仿真研究

為驗(yàn)證本文算法有效性，聯(lián)合CarSim和Simulink進(jìn)行仿真分析。

對(duì)預(yù)瞄模型方法與本研究?jī)?yōu)化方法下的農(nóng)機(jī)路徑跟蹤進(jìn)行對(duì)比仿真試驗(yàn)。以坐標(biāo)原點(diǎn)為農(nóng)機(jī)起始點(diǎn)，設(shè)定初始橫向偏差e=3m，初始航向角偏差Φe=0°、預(yù)瞄距離d=2m、采集周期T=0.05s。以東方紅LX904型拖拉機(jī)作為仿真對(duì)象，其軸距L=2.185m。其他控制參數(shù)不變情況下，設(shè)置車速v分別為1.5、2.0和3.0m/s，驗(yàn)證跟蹤效果。

由圖4可知，在不同速度下，本研究?jī)?yōu)化方法相較預(yù)瞄方法，對(duì)目標(biāo)路徑有較好跟蹤效果，在跟蹤精度不變的情況下，提高了系統(tǒng)的快速性。在v=1.5m/s時(shí)農(nóng)機(jī)上線行駛距離16.16m，相較于預(yù)瞄模型方法的24.17m，提高了33.14%；在v=2.0m/s時(shí)農(nóng)機(jī)上線行駛距離15.02m，相較于預(yù)瞄模型方法的23.07m，提高了34.89%；v=3.0m/s時(shí)農(nóng)機(jī)上線行駛距離13.72m，相較于預(yù)瞄模型方法的20.12m，提高了31.81%。

由圖5可知，在不同速度下，本研究?jī)?yōu)化方法對(duì)農(nóng)機(jī)橫向偏差的處理都有較好的效果。隨著速度的增加，系統(tǒng)沒有超調(diào)，并且響應(yīng)速度上升，上線時(shí)間（農(nóng)機(jī)與目標(biāo)路徑的橫向偏差第1次lt;0.05m的時(shí)間）變短。在v=1.5m/s時(shí)農(nóng)機(jī)上線時(shí)間10.85s，相較于預(yù)瞄模型方法下的14.75s，縮短了26.44%；在v=2.0m/s時(shí)農(nóng)機(jī)上線時(shí)間8.35s，相較于預(yù)瞄模型方法下的11.20s，縮短了25.44%；在v=3.0m/s時(shí)農(nóng)機(jī)上線時(shí)間5.15s，相較于預(yù)瞄模型方法下的7.10s，縮短了27.46%。

考慮農(nóng)機(jī)實(shí)際運(yùn)行工況，進(jìn)一步將本研究?jī)?yōu)化算法在圓形曲線下進(jìn)行跟蹤驗(yàn)證。由圖6和圖7可知，農(nóng)機(jī)在不同速度下對(duì)曲線跟蹤時(shí)的行駛軌跡相對(duì)穩(wěn)定，在車速v分別為1.5、2.0和3.0m/s時(shí)的上線距離分別為5.05、4.98和4.93m；上線時(shí)間分別為10.90、7.50和5.15s；橫向偏差的最大超調(diào)分別為0.075、0.062和0.052m；穩(wěn)定后的平均橫向偏差分別為0.013、0.015和0.018m。

仿真結(jié)果表明，在路徑跟蹤的進(jìn)程中，不同速度下本研究?jī)?yōu)化方法都具有一定的自適應(yīng)性；同時(shí)路徑優(yōu)化算法讓系統(tǒng)在偏差較小時(shí)對(duì)輪角處理更為快速，使得農(nóng)機(jī)上線距離更短；模糊PID因其實(shí)時(shí)動(dòng)態(tài)調(diào)整控制參數(shù)的優(yōu)越性，使得偏差收斂快，控制平滑。

4 結(jié)束語

采用PID控制方法對(duì)預(yù)瞄模型生成的預(yù)瞄航向角偏差進(jìn)行反饋校正實(shí)現(xiàn)路徑跟蹤控制；使用模糊控制實(shí)時(shí)動(dòng)態(tài)調(diào)整控制參數(shù)；同時(shí)加入一種路徑優(yōu)化算法，保證農(nóng)機(jī)上線穩(wěn)定性，提高了農(nóng)機(jī)上線快速性。仿真分析表明，不同速度下本研究?jī)?yōu)化方法都具有一定的自適應(yīng)性，可使農(nóng)機(jī)具有較好的跟蹤精度及較短的上線時(shí)間。本研究?jī)?yōu)化方法對(duì)于農(nóng)機(jī)跟蹤性能的提高表現(xiàn)出良好效果。