農(nóng)業(yè)機(jī)械自動導(dǎo)航控制系統(tǒng)設(shè)計(jì)

楊保香

(西安航空職業(yè)技術(shù)學(xué)院，陜西 西安 710089)

伴隨著信息科技的不斷發(fā)展與應(yīng)用，精準(zhǔn)農(nóng)業(yè)領(lǐng)域發(fā)生著日新月異的變化。在此背景下，自動導(dǎo)航等先進(jìn)技術(shù)的研究與實(shí)踐，已經(jīng)成為現(xiàn)代智能化農(nóng)業(yè)發(fā)展的必然趨勢，并被廣泛應(yīng)用在田間作業(yè)中，如土地平整、自動噴藥等。在自動導(dǎo)航定位中，精準(zhǔn)定位和路徑控制是其關(guān)鍵的技術(shù)。在自動定位方面，GPS、GLONASS、北斗和伽利略等定位系統(tǒng)是當(dāng)前主流的定位方式，并已成為自動導(dǎo)航應(yīng)用的重點(diǎn)。如張碩等[1]提出了一種基于GNSS(global navigation satellite system，全球?qū)Ш叫l(wèi)星系統(tǒng))/MIMU/DR 組合的拖拉機(jī)自動定位方法，并結(jié)合PID控制方法，大大提高了拖拉機(jī)運(yùn)行的精度。而在自動導(dǎo)航的路徑控制方面，目前的研究通常集中在PID模糊控制、最優(yōu)控制、模糊控制、滑膜控制、純追蹤模型理論等。如熊中剛[2]以高速插秧機(jī)作為研究對象，提出一種基于模糊PID控制的自動轉(zhuǎn)向方法，并采用直線和曲線路徑跟蹤的方法，完成對高速插秧機(jī)路徑和轉(zhuǎn)向的控制；張聞宇等[3]提出一種基于SVR算法的逆向農(nóng)機(jī)導(dǎo)航純追蹤方法，大大提高了純追蹤模型的動態(tài)性能；孟慶寬等[4]在PID控制中引入改進(jìn)粒子算法對參數(shù)進(jìn)行優(yōu)化，改進(jìn)后自動導(dǎo)航精度顯著高于傳統(tǒng)的控制方法。在實(shí)際應(yīng)用中，考慮到農(nóng)田環(huán)境的多樣性，農(nóng)業(yè)機(jī)械導(dǎo)航的精度還需要進(jìn)一步改進(jìn)，并且不同的算法也都存在不同的問題，如PID控制時(shí)，其魯棒性較差；模糊控制過程中，對大曲率轉(zhuǎn)彎作業(yè)的導(dǎo)航效果不佳[5]；粒子群算法的計(jì)算相對復(fù)雜；傳統(tǒng)的追蹤模型很難實(shí)現(xiàn)對農(nóng)業(yè)機(jī)械狀態(tài)的自適應(yīng)調(diào)整[6-7]。對此，筆者提出一種基于GNSS的農(nóng)業(yè)機(jī)械導(dǎo)航系統(tǒng)，并對系統(tǒng)進(jìn)行了測試。

1 系統(tǒng)整體架構(gòu)搭建

本文設(shè)計(jì)的目的是通過硬件和軟件的搭建，實(shí)現(xiàn)對農(nóng)業(yè)機(jī)械的自動導(dǎo)航，進(jìn)而使得農(nóng)業(yè)機(jī)械在作業(yè)的過程中，能嚴(yán)格按照設(shè)定的路徑進(jìn)行作業(yè)，減少路徑運(yùn)行的誤差。系統(tǒng)的整體架構(gòu)設(shè)計(jì)如圖1所示。

圖1 系統(tǒng)整體架構(gòu)搭建

該系統(tǒng)包含可編程邏輯控制器(PLC)、前輪轉(zhuǎn)角檢測裝置、方向控制裝置、GNSS定位裝置。具體的控制原理圖如圖2所示。首先通過GNSS對農(nóng)業(yè)機(jī)械進(jìn)行定位，同時(shí)結(jié)合對前輪轉(zhuǎn)角的測量，將數(shù)據(jù)發(fā)送給PLC控制模塊。PLC控制模塊在完成對偏差的計(jì)算后，將指令發(fā)布給方向控制系統(tǒng)，完成對運(yùn)行路徑的修正。

圖2 系統(tǒng)控制原理圖

2 兩輪拖拉機(jī)運(yùn)動學(xué)模型構(gòu)建

2.1 運(yùn)動學(xué)模型構(gòu)建

圖3為兩輪拖拉機(jī)運(yùn)動簡化模型，即將兩輪拖拉機(jī)的運(yùn)動簡化到一個(gè)二維平面上。為方便建模，采用三自由度的建模方法，即建模時(shí)只考慮拖拉機(jī)的橫向、縱向和橫擺3個(gè)自由度，忽略拖拉機(jī)運(yùn)動過程中出現(xiàn)的滑移，同時(shí)假設(shè)輪胎側(cè)偏角度為小角度。

圖3 兩輪拖拉機(jī)運(yùn)動簡化模型

在圖3中，XOY為導(dǎo)航坐標(biāo)系；O為導(dǎo)航坐標(biāo)系的原點(diǎn)；xCy為車身坐標(biāo)系；C點(diǎn)為拖拉機(jī)質(zhì)心；R為拖拉機(jī)的轉(zhuǎn)向半徑；Fx1,Fy1分別為拖拉機(jī)的前輪在縱向和側(cè)向方向上的作用力；Fx2,Fy2分別為后輪在縱向和側(cè)向上的作用力；vC,v1,v2分別為拖拉機(jī)質(zhì)心、前輪、后輪的速度；β,α1,α2分別為質(zhì)心、后輪側(cè)偏角、前輪側(cè)偏角；δ為前輪的轉(zhuǎn)向角；θ為拖拉機(jī)的行駛方向角；φC,φe分別為質(zhì)心在慣性坐標(biāo)系之下的航向角度和偏差；φP為P點(diǎn)的航向角；L為前視距離；yP為P點(diǎn)在橫向位置出現(xiàn)的偏差；a,b為質(zhì)心到前后輪的距離。

(1)

(2)

式中：ω為拖拉機(jī)的橫向擺角；ρP為轉(zhuǎn)向曲率。

β無法直接得出，若質(zhì)心沿x軸方向的速度為vx，沿y軸方向的速度為vy，則β為：

(3)

2.2 直線路徑跟蹤偏差模型

結(jié)合兩輪拖拉機(jī)運(yùn)動簡化模型，可以得到拖拉機(jī)-路徑動力學(xué)模型。

(4)

為保證拖拉機(jī)能夠沿著直線進(jìn)行跟蹤，將式(4)進(jìn)一步變形，從而可得：

(5)

式中：A11，A12，A21，A22，B1，B2為拖拉機(jī)-路徑動力學(xué)模型狀態(tài)方程的系數(shù)。

為實(shí)現(xiàn)對拖拉機(jī)直線路徑的實(shí)時(shí)追蹤，對式(7)進(jìn)行線性變換，從而可以得到直線路徑跟蹤偏差方程：

(6)

3 自動導(dǎo)航軟件設(shè)計(jì)

本文構(gòu)建的自動導(dǎo)航軟件包括上機(jī)位和下機(jī)位。其中上機(jī)位在工控機(jī)中運(yùn)行，主要負(fù)責(zé)數(shù)據(jù)的存儲、收發(fā)等；下機(jī)位在PLC中運(yùn)行，主要接收上機(jī)位發(fā)送的控制命令，并對導(dǎo)航控制進(jìn)行反饋。

3.1 上機(jī)位軟件實(shí)現(xiàn)

上機(jī)位主要負(fù)責(zé)將系統(tǒng)采集到的GNSS數(shù)據(jù)和PLC數(shù)據(jù)等進(jìn)行分析，然后顯示出拖拉機(jī)的路徑、姿態(tài)，以及相關(guān)的偏差角度信息。具體功能模塊如圖4所示。

圖4 上機(jī)位功能模塊

上機(jī)位軟件的數(shù)據(jù)處理流程設(shè)計(jì)如圖5所示。

圖5 上機(jī)位軟件數(shù)據(jù)處理流程

在上機(jī)位軟件數(shù)據(jù)處理流程中，通過多線程串口通信的方式完成對不同數(shù)據(jù)的收發(fā)，以保證數(shù)據(jù)通信的可靠性，同時(shí)提取GNSS 坐標(biāo)，在對坐標(biāo)進(jìn)行變換以后，完成路徑的搜索，最終發(fā)送相關(guān)的控制指令。

3.2 下機(jī)位軟件實(shí)現(xiàn)

對于下機(jī)位軟件的實(shí)現(xiàn)采用C++編程和MFC技術(shù)框架。下機(jī)位的功能模塊如圖6所示，包括通信模塊、控制模塊和AD/DA模塊3個(gè)部分。

圖6 下機(jī)位功能模塊

4 系統(tǒng)測試

為驗(yàn)證系統(tǒng)的可行性，以雷沃重工股份有限公司的TG1254拖拉機(jī)作為載體，搭建和開發(fā)該導(dǎo)航測試平臺。具體試驗(yàn)方案為：以拖拉機(jī)直線路徑模型中的前視距離L處航位偏差為零作為目標(biāo)，以實(shí)時(shí)采集到的拖拉機(jī)位姿信息來計(jì)算橫向位置偏差和航向角偏差，并將其與實(shí)際的結(jié)果進(jìn)行比較，進(jìn)而評價(jià)導(dǎo)航控制的優(yōu)劣。同時(shí)設(shè)定前視距離為4.5m，采樣頻率和控制頻率都為5Hz，采樣時(shí)間為60s。在試驗(yàn)之前，結(jié)合農(nóng)田的實(shí)際情況規(guī)劃出一條直線的路徑，并結(jié)合導(dǎo)航系統(tǒng)標(biāo)定起點(diǎn)和終點(diǎn)的坐標(biāo)，進(jìn)而得到目標(biāo)路線。然后拖拉機(jī)靠近該路線，并啟動自動導(dǎo)航模式。試驗(yàn)過程中，結(jié)合實(shí)時(shí)采集到的拖拉機(jī)位置坐標(biāo)，計(jì)算出其實(shí)時(shí)橫向位置偏差和航向角偏差。

設(shè)定拖拉機(jī)的速度v分別為0.6m/s、0.9m/s，從而可以得到如圖7所示的測試結(jié)果。

圖7 目標(biāo)路徑與實(shí)際運(yùn)動軌跡

通過統(tǒng)計(jì)可以得到航向軌跡的最大誤差、最小誤差和均方根誤差,具體結(jié)果見表1。

表1 直線路徑導(dǎo)航試驗(yàn)結(jié)果

將本文構(gòu)建的方法與傳統(tǒng)的PID控制算法進(jìn)行比較，可以得到如表2所示的誤差對比。

表2 本文控制方法與模糊控制算法誤差比較

5 結(jié)束語

本文構(gòu)建的拖拉機(jī)運(yùn)動路徑系統(tǒng)，可實(shí)時(shí)完成對路徑的自動導(dǎo)航。在導(dǎo)航中，系統(tǒng)可根據(jù)采集到的信息，自動調(diào)整偏差和方位角。而在本文構(gòu)建的控制算法中，經(jīng)過測試發(fā)現(xiàn)，車輛的速度越大，與目標(biāo)路徑之間的偏差越大。由此可以看出，本文構(gòu)建的導(dǎo)航系統(tǒng)在農(nóng)用拖拉機(jī)導(dǎo)航作業(yè)上具有一定的實(shí)用價(jià)值。