四輪轉(zhuǎn)向噴桿噴霧機(jī)平移換行導(dǎo)航控制系統(tǒng)設(shè)計(jì)與試驗(yàn)

周志艷 余 鑫 梁樂彬 向 穎 陳羽立 羅錫文

(1.華南農(nóng)業(yè)大學(xué)工程學(xué)院, 廣州 510642; 2.廣東省農(nóng)業(yè)人工智能重點(diǎn)實(shí)驗(yàn)室, 廣州 510642;3.嶺南現(xiàn)代農(nóng)業(yè)科學(xué)與技術(shù)廣東省實(shí)驗(yàn)室, 廣州 510642; 4.廣東省農(nóng)業(yè)航空應(yīng)用工程技術(shù)研究中心, 廣州 510642;5.華南農(nóng)業(yè)大學(xué)南方農(nóng)業(yè)機(jī)械與裝備關(guān)鍵技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室, 廣州 510642)

0 引言

農(nóng)機(jī)自動(dòng)導(dǎo)航控制可以減輕農(nóng)機(jī)操作人員勞動(dòng)強(qiáng)度、提高工作效率,是智慧農(nóng)業(yè)中智能農(nóng)機(jī)的重要功能[1-2]。在農(nóng)機(jī)自動(dòng)導(dǎo)航作業(yè)過程中,地頭轉(zhuǎn)向和換行是導(dǎo)航控制中的重要環(huán)節(jié),為實(shí)現(xiàn)農(nóng)機(jī)換行作業(yè),常用地頭轉(zhuǎn)向方式有弓形、梨形、魚尾形轉(zhuǎn)向[3-5]。張聞?dòng)畹萚6]設(shè)計(jì)了雙切圓虛線模型實(shí)現(xiàn)了地頭轉(zhuǎn)彎與直線跟蹤間的銜接;黎永鍵等[7]根據(jù)拖拉機(jī)轉(zhuǎn)向半徑選擇一種跨行轉(zhuǎn)向的掉頭方式;楊洋等[8]將作業(yè)田塊規(guī)劃為播種區(qū)域與地頭轉(zhuǎn)向區(qū)域,通過地頭轉(zhuǎn)向區(qū)域進(jìn)行轉(zhuǎn)彎、換行等動(dòng)作。上述地頭轉(zhuǎn)向方式依賴于規(guī)劃的曲線路徑,其靈活性較低,且跟蹤時(shí)易出現(xiàn)控制超調(diào)等情況。

目前噴霧機(jī)大多采用前輪轉(zhuǎn)向,部分采用四輪轉(zhuǎn)向[9-11],前輪轉(zhuǎn)向模型雖簡單,但部分農(nóng)機(jī)由于整車質(zhì)量和體積較大,較大轉(zhuǎn)向半徑使前輪轉(zhuǎn)向在換行、轉(zhuǎn)場時(shí)較困難,四輪轉(zhuǎn)向與前輪轉(zhuǎn)向相比雖減小了轉(zhuǎn)向半徑,但在農(nóng)機(jī)轉(zhuǎn)向換行區(qū)域空間小的情況下,通過性仍較差。文獻(xiàn)[12-13]設(shè)計(jì)多模式的地頭轉(zhuǎn)向方式,實(shí)現(xiàn)兩輪、四輪的協(xié)調(diào)配合,但適用性有待驗(yàn)證;徐琪蒙等[14]設(shè)計(jì)了差速-四輪轉(zhuǎn)向耦合的轉(zhuǎn)向控制方法,但平臺(tái)裝置設(shè)計(jì)較復(fù)雜、適應(yīng)性低;劉慧等[15]通過四輪差速帶動(dòng)整個(gè)機(jī)具橋臂轉(zhuǎn)動(dòng)實(shí)現(xiàn)自轉(zhuǎn)向的結(jié)構(gòu)也較復(fù)雜;李翊寧等[16]設(shè)計(jì)的四輪獨(dú)立柔性底盤,可利用底盤橫向移動(dòng)的運(yùn)動(dòng)方式實(shí)現(xiàn)調(diào)頭換行,但樣機(jī)在田間行駛工況有待實(shí)地驗(yàn)證;楊美鏡等[17]設(shè)計(jì)的四輪獨(dú)立轉(zhuǎn)向控制系統(tǒng),可實(shí)現(xiàn)全方位行駛、原地轉(zhuǎn)向等運(yùn)動(dòng)模式,但仍處于設(shè)計(jì)的初步階段,未進(jìn)行實(shí)地試驗(yàn)。

周志艷等[18]提出了一種旋翼懸浮式噴桿噴霧作業(yè)方法,單側(cè)最大寬幅可達(dá)20 m以上,寬幅噴桿噴霧機(jī)在進(jìn)行地頭轉(zhuǎn)向和換行時(shí),若采用傳統(tǒng)的車頭調(diào)頭方式,由于噴桿轉(zhuǎn)彎半徑大,通常有兩種處理方式:一是需要較大的凈空才能完成調(diào)頭動(dòng)作,但大部分農(nóng)田環(huán)境下,調(diào)頭空間有限,特別是幅寬超過20 m時(shí),比較困難;二是調(diào)頭前進(jìn)行噴桿收卷或折疊,待噴桿噴霧機(jī)車頭完成調(diào)頭和對行后再展開,整個(gè)動(dòng)作耗時(shí)較長,影響作業(yè)效率。

為解決上述寬幅噴桿噴霧機(jī)地頭轉(zhuǎn)向和換行的問題,本文擬提出一種基于四輪轉(zhuǎn)向噴桿噴霧機(jī)的平移換行方法,設(shè)計(jì)配套的平移換行導(dǎo)航控制算法,搭建自動(dòng)導(dǎo)航控制的軟硬件系統(tǒng)和測試平臺(tái),并進(jìn)行傳統(tǒng)PID與單神經(jīng)元PID兩種導(dǎo)航控制算法的對比試驗(yàn),從而優(yōu)選出精度高、穩(wěn)定性好的地頭平移換行導(dǎo)航控制算法,為寬幅噴桿噴霧機(jī)的地頭轉(zhuǎn)向和換行提供解決方案。

1 地頭平移換行工況與導(dǎo)航控制策略

1.1 地頭平移換行工況分析與路徑規(guī)劃

農(nóng)機(jī)在地塊完成對既定作業(yè)路徑跟蹤時(shí),需要在地頭進(jìn)行換行作業(yè)。本文設(shè)計(jì)的四輪轉(zhuǎn)向平移換行方式通過跟蹤預(yù)先設(shè)定的直線換行路徑實(shí)現(xiàn)換行作業(yè):如圖1a所示,當(dāng)作業(yè)車行至換行位置點(diǎn)B時(shí),導(dǎo)航控制器發(fā)出指令,控制底盤的各轉(zhuǎn)向輪轉(zhuǎn)動(dòng)至特定角度,以一種平行移動(dòng)的方式實(shí)現(xiàn)換行作業(yè)。當(dāng)作業(yè)車移動(dòng)至下一直線跟蹤路徑起點(diǎn)C時(shí),轉(zhuǎn)向輪回正,繼續(xù)進(jìn)行原定行壟的直線導(dǎo)航跟蹤作業(yè),而換行過程中,機(jī)身姿態(tài)基本不變化,同理,繼續(xù)導(dǎo)航控制完成剩下的既定作業(yè)路徑,行至點(diǎn)G停止作業(yè)。特別是在不規(guī)則地塊邊界,如圖1b所示的斜角邊界地況時(shí),本設(shè)計(jì)也可通過預(yù)先設(shè)定換行路徑BC、FG處自動(dòng)控制轉(zhuǎn)向輪轉(zhuǎn)動(dòng)至對應(yīng)轉(zhuǎn)向角度,以一種斜向移動(dòng)的姿態(tài)實(shí)現(xiàn)換行作業(yè),且整個(gè)導(dǎo)航作業(yè)過程中,車身姿態(tài)基本保持不變。綜上,當(dāng)作業(yè)底盤需要換行作業(yè)時(shí),如行至圖中BC、DE、FG段處,均可以實(shí)現(xiàn)底盤平移換行控制,該換行方式簡單靈活,能縮短換行時(shí)間,避免換行時(shí)由于轉(zhuǎn)向半徑大而需要較大的轉(zhuǎn)向空間的問題,有較強(qiáng)的適用性。

圖1 不同地塊行駛工況

結(jié)合上述作業(yè)底盤的運(yùn)動(dòng)方式,可規(guī)劃噴桿噴霧機(jī)田間作業(yè)的導(dǎo)航路徑(以常規(guī)方形地塊為例),如圖1所示:坐標(biāo)點(diǎn)A～G為行駛預(yù)設(shè)軌跡點(diǎn),導(dǎo)航控制系統(tǒng)將兩坐標(biāo)點(diǎn)之間直線區(qū)域進(jìn)行線性化處理,生成序列導(dǎo)航軌跡點(diǎn)。作業(yè)地塊的路徑包括噴霧作業(yè)路徑和非噴霧作業(yè)路徑。噴霧機(jī)在噴霧作業(yè)路徑時(shí),自動(dòng)啟動(dòng)水泵,均勻噴出藥液,在非作業(yè)路徑時(shí)自動(dòng)關(guān)閉水泵,停止噴藥。圖中箭頭方向表示噴霧機(jī)行駛方向,噴霧機(jī)從起點(diǎn)A進(jìn)入地塊開始導(dǎo)航作業(yè),沿著既定作業(yè)路徑自動(dòng)跟蹤。在換行位置時(shí),轉(zhuǎn)向電機(jī)帶動(dòng)轉(zhuǎn)向輪旋轉(zhuǎn)90°,驅(qū)動(dòng)電機(jī)按目標(biāo)轉(zhuǎn)速運(yùn)轉(zhuǎn),此時(shí)車頭方向仍與路徑AB方向保持平行,作業(yè)底盤平行移動(dòng)實(shí)現(xiàn)換行,換行完成后到達(dá)點(diǎn)C,底盤轉(zhuǎn)向輪回正,車身以倒退的方式完成路徑CD的直線跟蹤,實(shí)現(xiàn)了作業(yè)行之間的銜接。同理,按上述控制方法完成剩余的既定作業(yè)路徑,行至點(diǎn)H完成作業(yè)任務(wù),噴霧機(jī)停止所有運(yùn)動(dòng)。

1.2 四輪轉(zhuǎn)向噴霧機(jī)運(yùn)動(dòng)學(xué)模型

文獻(xiàn)[19]提出輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)可簡化為剛體的平面運(yùn)動(dòng),則可分解為剛體的平行移動(dòng)和定軸轉(zhuǎn)動(dòng),本文噴霧機(jī)作業(yè)底盤運(yùn)動(dòng)可簡化為剛體的平面運(yùn)動(dòng),為進(jìn)一步獲取任意狀態(tài)下四輪轉(zhuǎn)向噴霧機(jī)的運(yùn)動(dòng)學(xué)模型,假設(shè):噴霧機(jī)底盤在轉(zhuǎn)動(dòng)過程中各車輪轉(zhuǎn)向角速度和阻力系數(shù)不變,各車輪之間轉(zhuǎn)動(dòng)互不影響;噴桿噴霧機(jī)的質(zhì)心和幾何中心重合;車輪在行駛或轉(zhuǎn)動(dòng)過程中與接觸面的滑移忽略不計(jì),則可建立如圖2所示的運(yùn)動(dòng)學(xué)模型。

圖2 四輪轉(zhuǎn)向噴霧機(jī)的運(yùn)動(dòng)學(xué)模型

四輪轉(zhuǎn)向噴霧機(jī)底盤運(yùn)動(dòng)學(xué)方程[20]為

(1)

式中 (X,Y)——機(jī)體坐標(biāo)

φ——機(jī)身航向角,(°)

vx——機(jī)身前進(jìn)正方向速度,m/s

vy——機(jī)身前進(jìn)正方向垂直方向速度,m/s

ω——機(jī)身逆時(shí)針轉(zhuǎn)動(dòng)角速度,rad/s

由導(dǎo)航控制器計(jì)算出對應(yīng)控制量,實(shí)現(xiàn)對四輪轉(zhuǎn)向噴霧機(jī)運(yùn)動(dòng)控制。

為實(shí)現(xiàn)1.1節(jié)中的平移換行控制要求,需要進(jìn)行相關(guān)控制量的運(yùn)算分析?；谠撨\(yùn)動(dòng)學(xué)模型,得到導(dǎo)航控制器的輸出控制量,即底盤的矢量和運(yùn)動(dòng);為執(zhí)行該輸出控制量,須通過底盤控制系統(tǒng)控制轉(zhuǎn)向電機(jī)與驅(qū)動(dòng)輪電機(jī)來實(shí)現(xiàn)車輛行駛的控制,即底盤轉(zhuǎn)向輪的矢量分運(yùn)動(dòng);基于矢量運(yùn)算、阿克曼轉(zhuǎn)向等理論,建立底盤各轉(zhuǎn)向輪的轉(zhuǎn)向角、行駛速度與導(dǎo)航控制量間的等效轉(zhuǎn)換關(guān)系,用上述控制量變化來達(dá)到導(dǎo)航控制器輸出控制量所要求的控制效果,從而實(shí)現(xiàn)直線導(dǎo)航、平移換行導(dǎo)航,繼而實(shí)現(xiàn)整個(gè)既定作業(yè)路徑的跟蹤控制,其等效轉(zhuǎn)換公式為

(2)

(3)

式中a——車輛左右輪距,取1.83 m

b——車輛前后軸距,取2.56 m

θ1——左前輪轉(zhuǎn)向角,(°)

θ2——左后輪轉(zhuǎn)向角,(°)

θ3——右后輪轉(zhuǎn)向角,(°)

θ4——右前輪轉(zhuǎn)向角,(°)

v1——左前驅(qū)動(dòng)輪速度,m/s

v2——左后驅(qū)動(dòng)輪速度,m/s

v3——右后驅(qū)動(dòng)輪速度,m/s

v4——右前驅(qū)動(dòng)輪速度,m/s

基于運(yùn)動(dòng)學(xué)模型,可通過底盤轉(zhuǎn)向輪的矢量分運(yùn)動(dòng)實(shí)現(xiàn)對噴霧機(jī)位置坐標(biāo)變化與機(jī)身姿態(tài)的調(diào)整。實(shí)際作業(yè)過程中,受路面影響致使機(jī)身發(fā)生偏航,可將航向偏差、距離偏差作為導(dǎo)航控制器的輸入,輸出控制量實(shí)現(xiàn)對4個(gè)轉(zhuǎn)向輪的獨(dú)立控制,從而控制車身回正,繼續(xù)進(jìn)行既定作業(yè)路徑的跟蹤控制。

1.3 基于有限狀態(tài)機(jī)的自動(dòng)作業(yè)控制策略

采用有限狀態(tài)機(jī)[21-22]將田間作業(yè)過程簡化為根據(jù)位置信息使噴桿噴霧機(jī)在不同階段呈現(xiàn)不同狀態(tài)的過程,根據(jù)實(shí)際作業(yè)要求與有限狀態(tài)機(jī)的基本控制策略,設(shè)計(jì)噴桿噴霧機(jī)的系統(tǒng)狀態(tài)如表1所示。

表1 噴桿噴霧機(jī)系統(tǒng)狀態(tài)

在工控機(jī)上規(guī)劃好既定作業(yè)路徑后,啟動(dòng)開始作業(yè)命令,導(dǎo)航控制器根據(jù) RTK定位模塊獲取當(dāng)前車輛的位置信息,觸發(fā)不同的系統(tǒng)狀態(tài),實(shí)現(xiàn)對噴霧機(jī)的狀態(tài)控制,工作流程如下:當(dāng)狀態(tài)機(jī)接收到定位信息后,進(jìn)入 Start 狀態(tài)啟動(dòng)自動(dòng)作業(yè)系統(tǒng),當(dāng)噴霧機(jī)觸發(fā)事件非噴霧路徑時(shí),進(jìn)入非噴霧狀態(tài) S1,此時(shí),噴霧機(jī)直線導(dǎo)航行駛,但噴霧機(jī)的水泵不工作;當(dāng)噴霧機(jī)觸發(fā)事件噴霧路徑時(shí),進(jìn)入噴霧狀態(tài) S2,此時(shí),噴霧機(jī)直線導(dǎo)航行駛,并且水泵自動(dòng)啟動(dòng),開始噴霧;當(dāng)噴霧機(jī)觸發(fā)事件換行位置點(diǎn)時(shí),進(jìn)入平移換行狀態(tài) S3,噴霧機(jī)的控制系統(tǒng)控制四輪轉(zhuǎn)向底盤進(jìn)行平移換行,水泵不工作;當(dāng)噴霧機(jī)觸發(fā)作業(yè)任務(wù)結(jié)束點(diǎn)時(shí),進(jìn)入完成作業(yè)狀態(tài) Stop,噴霧機(jī)停止,水泵關(guān)閉,完成作業(yè);當(dāng)噴霧機(jī)觸發(fā)事件信號(hào)異常時(shí),立即停止,水泵停止工作;綜上,通過位置信息變化切換不同作業(yè)狀態(tài),實(shí)現(xiàn)噴霧機(jī)對規(guī)劃路徑的精確跟蹤,并完成特定路徑的噴霧作業(yè),實(shí)現(xiàn)該平臺(tái)的自動(dòng)作業(yè)控制。

2 四輪轉(zhuǎn)向噴桿噴霧機(jī)導(dǎo)航控制系統(tǒng)設(shè)計(jì)

2.1 導(dǎo)航系統(tǒng)平臺(tái)

噴桿噴霧機(jī)導(dǎo)航系統(tǒng)組成如圖3所示,包括北斗載波相位動(dòng)態(tài)實(shí)時(shí)差分RTK(Real time kinematic)定位模塊、北斗接收天線、工控機(jī)、角度傳感器及固定裝置、姿態(tài)傳感器和四輪轉(zhuǎn)向噴桿噴霧機(jī)。

導(dǎo)航系統(tǒng)的移動(dòng)平臺(tái)——噴桿噴霧機(jī)為山東魯虹農(nóng)業(yè)科技股份有限公司生產(chǎn)的 3WP-100A型植保機(jī)器人,其技術(shù)指標(biāo)如表2所示。為提高其在田間移動(dòng)的靈活性,在原有電控系統(tǒng)上進(jìn)行改造升級,可對四輪運(yùn)動(dòng)底盤實(shí)現(xiàn)獨(dú)立轉(zhuǎn)向控制,可實(shí)現(xiàn)橫向、斜向移動(dòng)等動(dòng)作。

表2 魯虹3WP-100A型植保機(jī)器人主要技術(shù)指標(biāo)

2.2 導(dǎo)航系統(tǒng)結(jié)構(gòu)組成

四輪轉(zhuǎn)向噴霧機(jī)導(dǎo)航控制系統(tǒng)結(jié)構(gòu)如圖4所示,由位姿定位系統(tǒng)、車載工控機(jī)、底盤控制系統(tǒng)和噴桿噴霧機(jī)組成。其中,位姿定位系統(tǒng)中的RTK定位模塊(u-blox F9P GNSS模塊)提供實(shí)時(shí)位置信息,WT-901C485多級聯(lián)九軸姿態(tài)傳感器(動(dòng)態(tài)精度±0.1°)用來實(shí)時(shí)檢測車輛姿態(tài)信息;車載工控機(jī)(研華ARK-3500P intel i5 8256U、CPU 4核心、8 GB 內(nèi)存和64 GB存儲(chǔ)器)運(yùn)行基于ROS(Robot operating system)的導(dǎo)航控制系統(tǒng),由位姿定位系統(tǒng)獲取作業(yè)車位置、姿態(tài)信息后,根據(jù)已設(shè)定的作業(yè)航點(diǎn)坐標(biāo),進(jìn)行數(shù)據(jù)處理和運(yùn)行導(dǎo)航算法,并通過USB端口向底盤控制系統(tǒng)發(fā)送控制指令。底盤控制系統(tǒng)接收車載工控機(jī)的指令后,控制各個(gè)電機(jī)運(yùn)轉(zhuǎn)以實(shí)現(xiàn)底盤的直線與轉(zhuǎn)向運(yùn)動(dòng)以及噴霧啟停等功能。JY-ME01型高精度角度傳感器(精度為±0.01°)檢測轉(zhuǎn)向輪實(shí)際轉(zhuǎn)向角,實(shí)現(xiàn)對轉(zhuǎn)向輪的精確控制。

圖4 四輪轉(zhuǎn)向噴霧機(jī)控制系統(tǒng)結(jié)構(gòu)框圖

2.3 轉(zhuǎn)向角檢測裝置設(shè)計(jì)

轉(zhuǎn)向系統(tǒng)性能影響噴霧機(jī)實(shí)際運(yùn)行狀況,噴霧機(jī)轉(zhuǎn)向輪轉(zhuǎn)向角的測量是轉(zhuǎn)向控制系統(tǒng)中重要組成部分,直接影響轉(zhuǎn)向性能[23]。本文設(shè)計(jì)的轉(zhuǎn)向角檢測裝置主要由減速電機(jī)、主動(dòng)齒輪、從動(dòng)齒輪、旋轉(zhuǎn)編碼器、數(shù)據(jù)采集板和支架等構(gòu)成,裝配安裝示意圖如圖5所示,主要部件參數(shù)如表3所示。

表3 轉(zhuǎn)向角檢測裝置主要部件參數(shù)

圖5 轉(zhuǎn)向角檢測裝配圖

通過加裝外部編碼器構(gòu)成反饋校正,實(shí)現(xiàn)對車輪實(shí)際轉(zhuǎn)動(dòng)角度的精確控制,工作原理為:當(dāng)噴霧機(jī)接收到底盤控制系統(tǒng)發(fā)送的轉(zhuǎn)向指令時(shí),電機(jī)驅(qū)動(dòng)器控制減速電機(jī)轉(zhuǎn)動(dòng),帶動(dòng)車輪轉(zhuǎn)向軸旋轉(zhuǎn),使主動(dòng)齒輪帶動(dòng)從動(dòng)齒輪旋轉(zhuǎn),與從動(dòng)齒輪同軸連接的旋轉(zhuǎn)編碼器轉(zhuǎn)動(dòng),數(shù)據(jù)采集板實(shí)時(shí)記錄編碼器的數(shù)據(jù),并通過CAN數(shù)據(jù)端口回傳至底盤控制系統(tǒng),通過與目標(biāo)角度的偏差來進(jìn)行反饋校正,進(jìn)而精準(zhǔn)控制實(shí)際的轉(zhuǎn)向角度。

2.4 轉(zhuǎn)向系統(tǒng)測試

為驗(yàn)證轉(zhuǎn)向檢測裝置及其配套系統(tǒng)的可靠性,對車輪的轉(zhuǎn)向角進(jìn)行角度測量試驗(yàn)。試驗(yàn)過程中通過STM32單片機(jī)的軟件開發(fā)系統(tǒng)Keil u5發(fā)出指定角度命令,單片機(jī)收到指令后控制電機(jī)運(yùn)轉(zhuǎn),配套的編碼器則回傳數(shù)據(jù),并通過串口助手輸出實(shí)際轉(zhuǎn)向角。為了檢測轉(zhuǎn)向系統(tǒng)的穩(wěn)定性,對各轉(zhuǎn)向輪分別進(jìn)行基準(zhǔn)轉(zhuǎn)向角30°、60°、90°的方波跟隨測試。每次試驗(yàn)重復(fù)3 次,取3次試驗(yàn)的平均值。圖6顯示了各車輪轉(zhuǎn)角變化的試驗(yàn)結(jié)果,結(jié)果表明:30°基準(zhǔn)角方波跟隨測試中上升時(shí)間不超過0.78 s,跟隨誤差不超過0.50°,60°基準(zhǔn)角方波跟隨測試中上升時(shí)間不超過1.58 s,跟隨誤差不超過1.23°,90°基準(zhǔn)角方波跟隨測試中上升時(shí)間不超過2.35 s,跟隨誤差不超過1.98°。試驗(yàn)表明各轉(zhuǎn)向輪實(shí)際響應(yīng)性能好,該轉(zhuǎn)向系統(tǒng)有較好的穩(wěn)定性,滿足實(shí)際運(yùn)動(dòng)需求。

圖6 方波信號(hào)跟隨測試結(jié)果

3 地頭平移換行導(dǎo)航控制算法

3.1 導(dǎo)航控制原理與PID控制器設(shè)計(jì)

目前,農(nóng)業(yè)機(jī)械導(dǎo)航控制方法主要有PID控制、模糊控制、純追蹤控制、神經(jīng)網(wǎng)絡(luò)控制和最優(yōu)控制等[24-25]。其中,PID控制器結(jié)構(gòu)簡單、不需要精確系統(tǒng)模型,在控制系統(tǒng)中得到廣泛的應(yīng)用[26]。本文基于PID控制設(shè)計(jì)的導(dǎo)航控制系統(tǒng)基本原理如圖7所示。

圖7 導(dǎo)航控制原理圖

由操作人員預(yù)先設(shè)定作業(yè)航點(diǎn)坐標(biāo),導(dǎo)航控制系統(tǒng)通過RTK定位模塊獲取當(dāng)前作業(yè)車的經(jīng)緯度坐標(biāo),通過滑動(dòng)濾波算法減小動(dòng)態(tài)定位誤差,并進(jìn)行坐標(biāo)轉(zhuǎn)換,最終得到噴霧機(jī)在平面直角坐標(biāo)系下的XOY坐標(biāo)。導(dǎo)航控制系統(tǒng)自動(dòng)計(jì)算當(dāng)前位置與目標(biāo)航點(diǎn)的距離偏差e,通過姿態(tài)傳感器獲取當(dāng)前的航向角θ,與目標(biāo)航向角ψ的差值作為航向偏差dθ。以距離偏差e、航向偏差dθ為輸入控制量,通過導(dǎo)航控制器可計(jì)算出速度、角速度控制量,結(jié)合1.2節(jié)運(yùn)動(dòng)學(xué)模型與等效轉(zhuǎn)換公式實(shí)現(xiàn)四輪轉(zhuǎn)向噴桿噴霧機(jī)的自動(dòng)導(dǎo)航跟蹤控制。

定位模塊實(shí)時(shí)采集噴霧機(jī)的經(jīng)緯度坐標(biāo)并通過高斯投影公式轉(zhuǎn)換為(X0,Y0)。將首個(gè)航跡點(diǎn)坐標(biāo)(X0,Y0)設(shè)為平面直角坐標(biāo)系的原點(diǎn),則車輛行駛時(shí)任意時(shí)刻的坐標(biāo)為

(4)

式中 (X,Y)——作業(yè)車的經(jīng)緯度坐標(biāo)

(Xk,Yk)——以(X0,Y0)為原點(diǎn)的平面直角坐標(biāo)系下的坐標(biāo)

同理,將規(guī)劃的既定作業(yè)路徑相關(guān)坐標(biāo)進(jìn)行高斯投影,并根據(jù)公式(4)得出以初始點(diǎn)為原點(diǎn)坐標(biāo)系下一組以時(shí)間為序列的有序軌跡坐標(biāo)。根據(jù)時(shí)間順序選取第k時(shí)刻某點(diǎn)坐標(biāo)(Xgk,Ygk),得到第k時(shí)刻當(dāng)前坐標(biāo)與目標(biāo)點(diǎn)之間的距離偏差ek為

(5)

同時(shí)得出航向角偏差dθk為

dθk=θk-ψ

(6)

式中θk——當(dāng)前航向角,(°)

在本設(shè)計(jì)中,由于要求車身姿態(tài)基本保持不變,設(shè)定目標(biāo)航向角ψ=0°。

在上述基礎(chǔ)上分別設(shè)計(jì)位置、角度PID控制器

(7)

式中Kp——常規(guī)PID控制器比例控制系數(shù)

Ki——常規(guī)PID控制器積分控制系數(shù)

Kd——常規(guī)PID控制器微分控制系數(shù)

dθk-1——k-1時(shí)刻的角度偏差,(°)

ek-1——k-1時(shí)刻的距離偏差,m

其中,位置PID控制器以距離偏差為輸入量,輸出速度控制量uv。角度PID控制器以航行偏差作為輸入量,輸出角度控制量uω。

速度控制量uv根據(jù)當(dāng)前航向角θk進(jìn)行解算,得到在車體坐標(biāo)系下車輛軸向速度控制量ux、uy為

(8)

通過上述控制量ux、uy、uω,結(jié)合1.2節(jié)所述的運(yùn)動(dòng)控制方法,得到具體的車輪行進(jìn)速度控制量和轉(zhuǎn)向的角速度控制量,實(shí)現(xiàn)對四輪轉(zhuǎn)向噴桿噴霧機(jī)的控制。

3.2 單神經(jīng)元PID控制器設(shè)計(jì)

當(dāng)被控對象受到較大干擾且具有大慣性時(shí),常規(guī)PID控制效果不好,其抗干擾能力較差。文獻(xiàn)[27-28]提出一種利用模糊自適應(yīng)調(diào)整PID參數(shù)的控制方法,提高水稻插秧機(jī)、直播機(jī)導(dǎo)航控制系統(tǒng)的穩(wěn)定性和快速響應(yīng)性。丁幼春等[29]設(shè)計(jì)出將免疫算法與PID控制器相結(jié)合的控制方法,提升了響應(yīng)速度,減小了跟隨偏差。文獻(xiàn)[30-31]設(shè)計(jì)了單神經(jīng)元PID控制器,使其具有超調(diào)小和進(jìn)入穩(wěn)態(tài)快的特點(diǎn),單神經(jīng)元是構(gòu)成神經(jīng)網(wǎng)絡(luò)的基本單位,具有自學(xué)習(xí)和自適應(yīng)能力,結(jié)構(gòu)簡單易于計(jì)算,基于單神經(jīng)元自適應(yīng)調(diào)整功能[32],將PID控制與單神經(jīng)元結(jié)合可實(shí)現(xiàn)對PID控制器參數(shù)的在線調(diào)節(jié),設(shè)計(jì)了單神經(jīng)元PID控制器,通過PID參數(shù)的在線調(diào)節(jié)實(shí)現(xiàn)噴霧機(jī)田間自動(dòng)導(dǎo)航作業(yè),該控制器結(jié)構(gòu)如圖8所示。圖中,Δu(t)為當(dāng)前控制增量,Z-1為上一時(shí)刻的控制量。

圖8 單神經(jīng)元PID控制器結(jié)構(gòu)圖

控制器采用增量式PID控制算法

u(k)=u(k-1)+Δu(k)

(9)

Δu(k)=kpr1(k)+kir2(k)+kdr3(k)

(10)

式中u(k-1)——k-1時(shí)刻控制量

Δu(k)——k時(shí)刻控制增量

r1(k)——比例輸入

r2(k)——積分輸入

r3(k)——微分輸入

kp——比例控制系數(shù)

ki——微分控制系數(shù)

kd——積分控制系數(shù)

單神經(jīng)元PID控制器輸入為

(11)

式中e(k-1)——k-1時(shí)刻輸入偏差

e(k-2)——k-2時(shí)刻輸入偏差

e(k)——k時(shí)刻的輸入偏差

單神經(jīng)元控制器采用有監(jiān)督的赫布學(xué)習(xí)(Hebb learning)規(guī)則,對kp、ki和kd進(jìn)行修正為

wk(k)=ηke(k)u(k)rk(k) (k=1,2,3)

(12)

式中ηk——學(xué)習(xí)速度

相關(guān)參數(shù)整定規(guī)則參考文獻(xiàn)[30],可得在線整定規(guī)則為

(13)

(14)

式中ηp——比例學(xué)習(xí)速率

ηi——積分學(xué)習(xí)速率

ηd——微分學(xué)習(xí)速率

w1(k)——比例加權(quán)系數(shù)

w2(k)——積分加權(quán)系數(shù)

w3(k)——微分加權(quán)系數(shù)

Kc——神經(jīng)元增益系數(shù)

同理,利用式(8)進(jìn)行相關(guān)控制量的解算,結(jié)合3.1節(jié)所述的常規(guī)PID控制器以及1.2節(jié)運(yùn)動(dòng)控制方法,得到具體的車輪行進(jìn)速度控制量和轉(zhuǎn)向的角速度控制量,實(shí)現(xiàn)對四輪轉(zhuǎn)向噴桿噴霧機(jī)的優(yōu)化控制。

4 試驗(yàn)

4.1 試驗(yàn)方法

4.1.1導(dǎo)航精度評價(jià)指標(biāo)

在試驗(yàn)過程中,對搭載導(dǎo)航控制器的噴桿噴霧機(jī)行駛過程中的跟蹤偏差最大值、平均絕對偏差[29,33]進(jìn)行了統(tǒng)計(jì)分析。跟蹤偏差由點(diǎn)到直線距離求解。由RTK定位模塊可獲得當(dāng)前位置K(x0,y0),則噴桿噴霧機(jī)與設(shè)定路徑的跟蹤偏差d為

(15)

其中

(16)

導(dǎo)航精度提高百分比[22]用來表示改進(jìn)后導(dǎo)航控制器相對于原控制器導(dǎo)航精度提升的效果。計(jì)算公式為

(17)

式中M——導(dǎo)航精度提高百分比,%

Savg——常規(guī)PID控制平均值,m

Mavg——單神經(jīng)元PID控制平均值,m

分別以最大跟蹤偏差和平均跟蹤偏差作為衡量噴桿噴霧機(jī)導(dǎo)航精度指標(biāo),符號(hào)為正表示精度提高,符號(hào)為負(fù)表示精度降低。

4.1.2平移換行導(dǎo)航控制試驗(yàn)方案

為驗(yàn)證導(dǎo)航控制系統(tǒng)在不同類型的地塊實(shí)際效果以及所設(shè)計(jì)的單神經(jīng)元PID控制器對傳統(tǒng)PID控制器的導(dǎo)航精度提升效果,分別進(jìn)行硬質(zhì)平整地塊試驗(yàn)與田間試驗(yàn),其中試驗(yàn)材料有:四輪轉(zhuǎn)向平移換行噴桿噴霧機(jī)、便攜式計(jì)算機(jī)、工控機(jī)、北斗定位移動(dòng)端與固定站、卷尺、U盤、ST-LINK調(diào)試器、濕度檢測儀、土壤堅(jiān)實(shí)度測量儀、風(fēng)速儀和攝像機(jī)等。

試驗(yàn)步驟:①安裝導(dǎo)航平臺(tái)、檢查傳感器通信狀況,設(shè)置導(dǎo)航參數(shù)。②利用測量儀測量外部環(huán)境參數(shù),如天氣狀況、風(fēng)速、田間行駛時(shí)地塊的濕度與土壤緊實(shí)度等。③根據(jù)1.1節(jié)使用導(dǎo)航控制終端輸入位置坐標(biāo)自動(dòng)規(guī)劃出對應(yīng)的作業(yè)區(qū)域,確定既定作業(yè)路徑。④調(diào)試好噴桿噴霧機(jī)導(dǎo)航控制系統(tǒng)后,啟動(dòng)控制終端開始直線跟蹤導(dǎo)航作業(yè)。⑤完成直線跟蹤路徑需要換行作業(yè)時(shí),導(dǎo)航控制器輸出控制命令,實(shí)現(xiàn)平移換行動(dòng)作。⑥平移至下一條直線作業(yè)路徑起點(diǎn)時(shí),繼續(xù)進(jìn)行直線路徑的跟蹤,直至完成所有路徑的跟蹤控制后噴霧機(jī)停止。⑦記錄導(dǎo)航過程中噴桿噴霧機(jī)位置信息并及時(shí)備份,試驗(yàn)重復(fù)3次。

其中,相關(guān)控制參數(shù)為:PID的參數(shù)分別為kp=0.5、ki=0.02、kd=0.01。單神經(jīng)元PID參數(shù)分別為Kc=0.25、ηp=0.5、ηi=0.2、ηd=0.5。根據(jù)噴霧機(jī)噴桿的實(shí)際長度以及試驗(yàn)作業(yè)要求,設(shè)計(jì)對應(yīng)的作業(yè)區(qū)域,可規(guī)劃出4行。為防止噴霧機(jī)在作業(yè)時(shí)因大幅振動(dòng)顛簸造成噴桿觸地導(dǎo)致噴頭堵塞及損傷,避免行駛時(shí)發(fā)生較大滑移等情況,最終在速度0.7 m/s下進(jìn)行搭載常規(guī)PID控制器與單神經(jīng)元PID控制器的路徑跟蹤試驗(yàn)。

4.2 改進(jìn)算法對比試驗(yàn)

4.2.1硬質(zhì)平整地塊試驗(yàn)

為避免試驗(yàn)區(qū)域地勢不平而導(dǎo)致噴霧機(jī)在跟蹤行駛時(shí)出現(xiàn)滑移等影響導(dǎo)航精度的情況,在華南農(nóng)業(yè)大學(xué)燕山區(qū)操場進(jìn)行硬質(zhì)平整地塊的試驗(yàn),試驗(yàn)當(dāng)天天氣多云,自然風(fēng)速小于2.5 m/s,按照4.1.2節(jié)相關(guān)參數(shù)以及試驗(yàn)步驟進(jìn)行試驗(yàn),測試現(xiàn)場如圖9所示。

圖9 硬質(zhì)平整地塊試驗(yàn)現(xiàn)場

搭載了單神經(jīng)元PID的導(dǎo)航控制器的噴霧機(jī)在硬質(zhì)平整地塊中的實(shí)際導(dǎo)航軌跡如圖10所示,搭載常規(guī)PID控制器與單神經(jīng)元PID控制器的噴霧機(jī)在硬質(zhì)平整地塊中的平移換行跟蹤效果對比如表4所示。

表4 硬質(zhì)平整地塊平移換行跟蹤效果對比

圖10 單神經(jīng)元PID控制器實(shí)際導(dǎo)航路徑軌跡

在常規(guī)方形地塊中,搭載常規(guī)PID控制器的噴霧機(jī)在換行過程中的最大跟蹤偏差、平均絕對偏差為7.63、4.27 cm,而搭載單神經(jīng)元PID控制器的噴霧機(jī)在換行過程中的最大跟蹤偏差、平均絕對偏差為6.48、3.24 cm,由式(17)可知,單神經(jīng)元PID控制相對于常規(guī)PID控制,換行路徑的導(dǎo)航精度分別提高15.9%、24.1%;在不規(guī)則地塊中,搭載常規(guī)PID控制器的噴霧機(jī)在換行過程中的最大跟蹤偏差、平均絕對偏差為7.38、4.23 cm,而搭載單神經(jīng)元PID控制器的噴霧機(jī)在換行過程中的最大跟蹤偏差、平均絕對偏差為6.42、3.17 cm,其平移換行路徑導(dǎo)航精度分別提高13.0%、25.1%。

在相同的控制參數(shù)和試驗(yàn)場地下,一方面,在常規(guī)方形地塊與不規(guī)則地塊的平移換行跟蹤誤差基本保持一致,則表明平移換行控制系統(tǒng)有較強(qiáng)的適用性;另一方面,單神經(jīng)元PID控制與傳統(tǒng)PID控制相比,降低了最大跟蹤偏差和平均絕對偏差,表明單神經(jīng)元PID控制有較好的導(dǎo)航精度提升效果。

4.2.2田間試驗(yàn)

為進(jìn)一步驗(yàn)證四輪轉(zhuǎn)向噴桿噴霧機(jī)導(dǎo)航控制系統(tǒng)的田間工作性能,在華南農(nóng)業(yè)大學(xué)增城實(shí)驗(yàn)基地試驗(yàn)田進(jìn)行了田間自動(dòng)導(dǎo)航試驗(yàn),試驗(yàn)當(dāng)天天氣晴,自然風(fēng)速小于3.0 m/s,采用五點(diǎn)取樣法測得田間土壤含水率平均值為25.3%,土壤堅(jiān)實(shí)度平均值為987 kPa。分別進(jìn)行搭載常規(guī)PID控制器和單神經(jīng)元PID控制器的噴桿噴霧機(jī)對比試驗(yàn),試驗(yàn)步驟及相關(guān)參數(shù)與路面試驗(yàn)相同,田間試驗(yàn)現(xiàn)場如圖11所示。

圖11 田間試驗(yàn)現(xiàn)場

田間試驗(yàn)時(shí),搭載了單神經(jīng)元PID的導(dǎo)航控制器的噴霧機(jī)實(shí)際導(dǎo)航軌跡如圖12所示,搭載常規(guī)PID控制器與單神經(jīng)元PID控制器的噴霧機(jī)在田間試驗(yàn)平移換行跟蹤效果對比如表5所示。

表5 田間試驗(yàn)平移換行跟蹤效果對比

圖12 單神經(jīng)元PID控制器硬質(zhì)平整地塊試驗(yàn)導(dǎo)航軌跡

在常規(guī)方形地塊中,搭載常規(guī)PID控制器的噴霧機(jī)在換行過程中的最大跟蹤偏差、平均絕對偏差為11.01、6.66 cm,而搭載單神經(jīng)元PID控制器的噴霧機(jī)在換行過程中的最大跟蹤偏差、平均絕對偏差為8.60、4.47 cm,單神經(jīng)元PID控制相對于常規(guī)PID控制,其導(dǎo)航精度分別提高21.8%、32.8%;在不規(guī)則地塊中,搭載常規(guī)PID控制器的噴霧機(jī)在換行過程中的最大跟蹤偏差、平均絕對偏差為10.91、6.63 cm,而搭載單神經(jīng)元PID控制器在換行過程中的最大跟蹤偏差、平均絕對偏差為8.63、4.42 cm,單神經(jīng)元PID控制相對于常規(guī)PID控制,其導(dǎo)航精度分別提高20.9%、33.3%,與常規(guī)PID控制器相比,單神經(jīng)元PID控制在田間行駛時(shí)也能提升導(dǎo)航控制系統(tǒng)的精度和穩(wěn)定性,降低跟蹤偏差。表明該單神經(jīng)元PID控制有較好的路徑跟蹤提升效果,滿足田間作業(yè)要求。

從上述數(shù)據(jù)可以看出,噴霧機(jī)在田間實(shí)際作業(yè)行駛時(shí),橫向偏差與硬質(zhì)平整地塊相比均有所增大,主要原因?yàn)?噴桿噴霧機(jī)在田間作業(yè)時(shí),運(yùn)動(dòng)底盤會(huì)受到土地平整度、堅(jiān)實(shí)度和含水率等外界客觀因素的影響,致使行駛和轉(zhuǎn)向阻力增大或發(fā)生輪胎打滑等現(xiàn)象,從而導(dǎo)致噴霧機(jī)運(yùn)動(dòng)特性隨之發(fā)生變化,運(yùn)動(dòng)性能下降;噴霧機(jī)在田間行駛時(shí),車身易傾斜或產(chǎn)生滑移,導(dǎo)致傳感器獲取的車體位姿信息產(chǎn)生突變值,影響控制性能,跟蹤誤差隨之增大。此外,單天線 RTK、姿態(tài)傳感器易受地塊不平等因素影響造成定位與姿態(tài)與實(shí)際位置存在一定的誤差,后期將改進(jìn)方案采用雙天線或其他組合導(dǎo)航方案降低單個(gè)傳感器對跟蹤誤差的影響。由上述試驗(yàn)可得出,在相同條件和參數(shù)下,搭載了單神經(jīng)元PID 控制器的噴霧機(jī)試驗(yàn)效果相比搭載了常規(guī) PID 控制器噴霧機(jī)效果好,單神經(jīng)元PID 控制器具有自學(xué)習(xí)和自適應(yīng)能力,當(dāng)受到較大干擾時(shí)能夠迅速收斂,具有降低跟蹤誤差和抗干擾能力較強(qiáng)等特點(diǎn),因此噴桿噴霧機(jī)在不同作業(yè)環(huán)境下均具有較好的控制效果。

5 結(jié)論

(1)針對寬幅噴桿噴霧機(jī)地頭轉(zhuǎn)向和換行的問題,設(shè)計(jì)了一種四輪轉(zhuǎn)向的平移換行控制方法,為解決傳統(tǒng)噴桿噴霧機(jī)在轉(zhuǎn)彎、換行及調(diào)頭過程中需要較大轉(zhuǎn)向區(qū)域、轉(zhuǎn)向半徑大、易碾壓作物等問題提供新的方案。

(2)基于四輪轉(zhuǎn)向平移換行控制方法,設(shè)計(jì)了配套的自動(dòng)導(dǎo)航控制系統(tǒng),該系統(tǒng)以四輪轉(zhuǎn)向噴桿噴霧機(jī)運(yùn)動(dòng)底盤為基礎(chǔ),采用RTK定位模塊與姿態(tài)傳感器進(jìn)行組合導(dǎo)航,獲取四輪轉(zhuǎn)向噴桿噴霧機(jī)的位置和航向信息為輸入,設(shè)計(jì)了基于四輪轉(zhuǎn)向運(yùn)動(dòng)學(xué)模型的導(dǎo)航控制器,并結(jié)合作業(yè)要求設(shè)計(jì)了基于有限狀態(tài)機(jī)的自動(dòng)作業(yè)控制策略。

(3)針對常規(guī)PID控制器抗干擾能力差的缺陷,設(shè)計(jì)了基于單神經(jīng)元PID控制器以提高導(dǎo)航控制精度,并對所設(shè)計(jì)的導(dǎo)航控制器進(jìn)行硬質(zhì)平整地塊試驗(yàn),試驗(yàn)結(jié)果為:在常規(guī)方形地塊中,搭載常規(guī)PID控制器的噴霧機(jī)在平移換行過程中的最大跟蹤偏差、平均絕對偏差為7.63、4.27 cm,而搭載單神經(jīng)元PID控制器的噴霧機(jī)在平移換行過程中的的最大跟蹤偏差、平均絕對偏差為6.48、3.24 cm,單神經(jīng)元PID控制相對于常規(guī)PID控制,其在換行路徑的導(dǎo)航精度分別提高15.9%、24.1%;在不規(guī)則地塊中,搭載常規(guī)PID控制器的噴霧機(jī)在平移換行過程中的最大跟蹤偏差、平均絕對偏差為7.38、4.23 cm,而搭載單神經(jīng)元PID控制器的噴霧機(jī)在平移換行過程中的最大跟蹤偏差、平均絕對偏差為6.42、3.17 cm,換行路徑的導(dǎo)航精度分別提高13.0%、25.1%,表現(xiàn)出較強(qiáng)的適用性與較好的導(dǎo)航精度提升效果。

(4)為進(jìn)一步驗(yàn)證四輪轉(zhuǎn)向噴桿噴霧機(jī)導(dǎo)航控制系統(tǒng)的田間工作性能,對所設(shè)計(jì)的導(dǎo)航控制器進(jìn)行田間試驗(yàn),試驗(yàn)結(jié)果為:在常規(guī)方形地塊中,搭載常規(guī)PID控制器的噴霧機(jī)在平移換行過程中的最大跟蹤偏差、平均絕對偏差為11.01、6.66 cm,而搭載單神經(jīng)元PID控制器的噴霧機(jī)在平移換行過程中的最大跟蹤偏差、平均絕對偏差為8.60、4.47 cm,單神經(jīng)元PID控制相對于常規(guī)PID控制,其平移換行的導(dǎo)航精度分別提高21.8%、32.8%;在不規(guī)則地塊中,搭載常規(guī)PID控制器的噴霧機(jī)在平移換行過程中最大跟蹤偏差、平均絕對偏差為10.91、6.63 cm,而搭載單神經(jīng)元PID控制器的噴霧機(jī)在平移換行過程中的最大跟蹤偏差、平均絕對偏差為8.63、4.42 cm,單神經(jīng)元PID控制相對于常規(guī)PID控制,其平移換行導(dǎo)航精度也分別提高20.9%、33.3%,有較好的提升效果。硬質(zhì)平整地面和田間試驗(yàn)充分表明該單神經(jīng)元PID控制與傳統(tǒng)PID控制相比,能降低最大跟蹤偏差、平均絕對偏差,提高了導(dǎo)航系統(tǒng)精度和穩(wěn)定性,有較強(qiáng)的提升效果,滿足實(shí)際作業(yè)要求。