壟作模式設(shè)施植保機(jī)器人設(shè)計(jì)與導(dǎo)航系統(tǒng)試驗(yàn)

摘要：

針對(duì)設(shè)施壟作模式下空間結(jié)構(gòu)狹小、環(huán)境密閉造成機(jī)器自主作業(yè)困難等問(wèn)題，設(shè)計(jì)一種四輪獨(dú)立驅(qū)動(dòng)轉(zhuǎn)向設(shè)施植保機(jī)器人，開發(fā)基于UWB和IMU的組合導(dǎo)航控制系統(tǒng)，并對(duì)機(jī)器人進(jìn)行室內(nèi)壟間與換壟行走性能測(cè)試。首先根據(jù)設(shè)施壟作模式特點(diǎn)，確定底盤壟間行走和換壟作業(yè)模式并設(shè)計(jì)相應(yīng)的底盤結(jié)構(gòu)部件；然后基于行走作業(yè)模式與底盤結(jié)構(gòu)開發(fā)設(shè)施植保作業(yè)機(jī)器人的導(dǎo)航控制系統(tǒng)，采用UWB定位技術(shù)與IMU模塊進(jìn)行組合導(dǎo)航，以機(jī)器人的位置信息和姿態(tài)信息作為輸入，在底盤運(yùn)動(dòng)學(xué)模型的基礎(chǔ)上，采用純跟蹤算法實(shí)現(xiàn)導(dǎo)航控制。最后開展機(jī)器人室內(nèi)導(dǎo)航實(shí)地測(cè)試。結(jié)果表明，在行駛速度為0.5m/s、1.0m/s、1.5m/s時(shí)，該機(jī)器人的壟間直線行走最大橫向偏差平均值分別為0.094m、0.106m、0.148m，平均偏差分別為0.028m、0.041m、0.068m。該機(jī)器人在室內(nèi)具有較好的轉(zhuǎn)向靈活性和導(dǎo)航行走精度，能夠滿足設(shè)施壟作結(jié)構(gòu)特點(diǎn)下的行走作業(yè)要求，為自主設(shè)施植保作業(yè)機(jī)器人的研制提供參考。

關(guān)鍵詞：設(shè)施溫室;壟作模式;植保機(jī)器人;自主導(dǎo)航

中圖分類號(hào)：S625； TP273

文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：2095-5553 （2025） 03-0071-08

收稿日期：2023年9月29日" 修回日期：2023年11月13日*

基金項(xiàng)目：浙江省“尖兵”研發(fā)技術(shù)攻關(guān)項(xiàng)目（2023C02012）；浙江省“領(lǐng)雁”研發(fā)技術(shù)攻關(guān)項(xiàng)目（2023C02053）；浙江省農(nóng)業(yè)科學(xué)院成果推廣項(xiàng)目（2023R30CB001）

第一作者：馮昊棟，男，1998年生，河南鶴壁人，碩士研究生；研究方向?yàn)橹悄苻r(nóng)機(jī)裝備設(shè)計(jì)與制造。E-mail： Zhrory@126.com

通訊作者：俞國(guó)紅，男，1969年生，浙江蕭山人，碩士，高級(jí)工程師；研究方向?yàn)檗r(nóng)業(yè)機(jī)械化工程。E-mail： Yuguohong@163.com

Design and navigation system experimentation of plant protection robots in ridge planting conditions

Feng Haodong1， Zheng Hang2， 3， Zhang Yi4， Xue Xianglei2， 3， Tong Junhua1， 3， Yu Guohong2， 3

（1. School of Mechanical Engineering， Zhejiang Sci-Tech University， Hangzhou， 310018， China;

2. Institute of Agricultural Equipment， Zhejiang Academy of Agricultural Sciences， Hangzhou， 310021， China;

3. Key Laboratory of Agricultural Equipment for Hilly and Mountainous Areas in Southeastern China （Co-construction by

Ministry and Province）， Ministry of Agriculture and Rural Affairs， Hangzhou， 310021， China;

4. Zhejiang Qiantang Robot and Intelligent Equipment Research Co.， Ltd.， Hangzhou， 310002， China）

Abstract：

To solve the problems of autonomous operation in narrow and enclosed spaces under ridge planting conditions， a four-wheel independently driven and steering plant protection robot is designed， and a combined navigation control system based on UWB and IMU technologies is developed. The robot’s performance is tested for inter-ridge and ridge-changing operations indoors. First， the characteristics of the ridge planting conditions are analyzed to define the chassis inter-ridge walking and ridge-changing operation modes， and corresponding chassis structural components are designed. Based on these operation modes and the chassis structure， a navigation control system of the plant protection robot is developed. This system integrates UWB positioning technology and an IMU module for combined navigation， using robot’s positional information and attitude information as the inputs. A pure tracking algorithm is implemented on the chassis kinematic model to achieve precise navigation control. Finally， indoor navigation tests of the robot are conducted. The results show that at speeds of 0.5m/s， 1.0m/s and 1.5m/s， the maximum lateral deviations of the robot’s inter-ridge straight-line walking are 0.094m， 0.106m and 0.148m， and the average deviations are 0.028m， 0.041m and 0.068m， respectively. The robot demonstrates excellent steering flexibility and navigation accuracy indoors， meeting the operational requirements of ridge planting structures. The findings provide a valuable reference for the development of autonomous plant protection robots for facility-based operations.

Keywords：

facility greenhouse; ridge planting; plant protection robot; autonomous navigation

0 引言

設(shè)施農(nóng)業(yè)是一種綜合應(yīng)用現(xiàn)代生物和信息技術(shù)的高新技術(shù)產(chǎn)業(yè)，是可以實(shí)現(xiàn)農(nóng)作物高產(chǎn)、優(yōu)產(chǎn)的現(xiàn)代農(nóng)業(yè)模式，是未來(lái)農(nóng)業(yè)的發(fā)展方向［1］。相對(duì)大田種植，室內(nèi)空間封閉、高溫高濕且作物種植密度大，更容易導(dǎo)致作物病蟲害的發(fā)生。目前溫室植保基本采用人工通過(guò)背負(fù)式手動(dòng)（電動(dòng)）噴霧器，人工噴灑隨機(jī)性高，機(jī)具作業(yè)效率低、機(jī)械化水平低，且密閉環(huán)境對(duì)植保工作人員健康傷害大［2］。植保作業(yè)是保證農(nóng)作物高產(chǎn)、穩(wěn)產(chǎn)的重要環(huán)節(jié)，先進(jìn)的植保裝備和技術(shù)多處于試驗(yàn)階段還未普及，因此設(shè)施農(nóng)業(yè)的高效率、高品質(zhì)的優(yōu)勢(shì)無(wú)法充分體現(xiàn)。溫室壟間距狹窄且種植物密集，室內(nèi)空間狹小，目前市面上大多數(shù)傳統(tǒng)的自走式噴桿植保機(jī)轉(zhuǎn)向和掉頭所需空間大，操縱方便性較差，容易造成農(nóng)作物的碾壓，同時(shí)影響植保作業(yè)效率，無(wú)法直接應(yīng)用在設(shè)施環(huán)境。

自主導(dǎo)航是實(shí)現(xiàn)植保機(jī)器人在溫室內(nèi)無(wú)人化作業(yè)的基礎(chǔ)［3］，溫室內(nèi)實(shí)現(xiàn)精準(zhǔn)定位是植保機(jī)器人自主導(dǎo)航的技術(shù)基礎(chǔ)［4］。在定位方面，全球定位系統(tǒng)（GPS）被廣泛用于自動(dòng)導(dǎo)航農(nóng)機(jī)定位研究中，但大多用于室外大田環(huán)境。設(shè)施溫室遮擋物較多，GPS信號(hào)嚴(yán)重衰減，定位精度低［5］。常見的定位方法還有使用視覺傳感器或紅外線傳感器進(jìn)行機(jī)器人定位，但是受到光照條件影響較大，不適用于溫室環(huán)境下的導(dǎo)航定位［6］。因此，研發(fā)和應(yīng)用在溫室環(huán)境下靈活性高的自主植保作業(yè)機(jī)器人，對(duì)提高我國(guó)的設(shè)施作業(yè)機(jī)具自動(dòng)化程度和設(shè)施農(nóng)業(yè)生產(chǎn)效率具有重要的現(xiàn)實(shí)意義。

近年來(lái)，國(guó)內(nèi)外針對(duì)溫室智能植保作業(yè)技術(shù)與裝備開展了大量研究。管澤峰等［7］設(shè)計(jì)了電磁導(dǎo)式噴霧機(jī)器人導(dǎo)航系統(tǒng)，利用磁導(dǎo)航傳感器和測(cè)距傳感器實(shí)現(xiàn)溫室移動(dòng)平臺(tái)的協(xié)同作業(yè)行走控制，前期需要鋪設(shè)誘導(dǎo)線及磁標(biāo)等，應(yīng)用受限且成本高；侯加林等［8］設(shè)計(jì)了基于激光雷達(dá)的溫室運(yùn)輸機(jī)器人，通過(guò)在底盤的頭、尾部搭載二維激光雷達(dá)實(shí)現(xiàn)溫室環(huán)境下的定位導(dǎo)航。但激光雷達(dá)在壟作環(huán)境下工作時(shí)，激光點(diǎn)云不能準(zhǔn)確掃描田壟的點(diǎn)云信息，從而難以保障植保機(jī)運(yùn)行軌跡的正確性。UWB技術(shù)具有較高的傳輸速率和時(shí)間分辨率，其信號(hào)強(qiáng)度不受距離影響且功耗較低，視距通訊下有著較高的定位精度，已被廣泛應(yīng)用于室內(nèi)高精度定位［9］。雖然UWB定位技術(shù)能夠?qū)崿F(xiàn)溫室環(huán)境內(nèi)的高精度定位，但受遮擋影響，慣性導(dǎo)航定位不受環(huán)境限制。在溫室環(huán)境下，UWB和IMU數(shù)據(jù)的有效組合可為溫室機(jī)器人作業(yè)過(guò)程提供穩(wěn)定的高精度數(shù)據(jù)。

本文針對(duì)設(shè)施溫室空間狹小、植保機(jī)轉(zhuǎn)向困難、溫室內(nèi)定位精度差等問(wèn)題，設(shè)計(jì)一種基于UWB/IMU組合導(dǎo)航的四輪獨(dú)立轉(zhuǎn)向的植保機(jī)器人，并通過(guò)純跟蹤算法實(shí)現(xiàn)對(duì)預(yù)設(shè)軌跡的跟蹤控制，以實(shí)現(xiàn)設(shè)施溫室植保環(huán)節(jié)的自動(dòng)化、無(wú)人化作業(yè)，為設(shè)施農(nóng)業(yè)機(jī)械化、智能化發(fā)展提供技術(shù)與裝備支撐。

1 整機(jī)結(jié)構(gòu)與工作原理

1.1 設(shè)施壟作模式結(jié)構(gòu)特征

與露天田間環(huán)境不同，設(shè)施溫室為半結(jié)構(gòu)化的設(shè)施農(nóng)業(yè)環(huán)境，其由鋼體框架搭建，頂部安裝外遮陽(yáng)系統(tǒng)［10］。設(shè)施溫室可被劃分為道路區(qū)域和種植區(qū)域兩部分，其中道路區(qū)域?yàn)楣ぷ魅藛T和運(yùn)輸設(shè)備通行的水泥硬化路面，路面寬度為1.8m；種植區(qū)域?yàn)閴抛髂Ｊ?，中間分布若干金屬立柱。設(shè)施溫室環(huán)境如圖1所示。

設(shè)施溫室內(nèi)葉菜類種植的壟型多為寬平壟，如圖2所示，該壟型壟距為1450mm，壟溝寬300mm，壟頂寬1050mm，壟高150mm。植保機(jī)械作業(yè)過(guò)程需要跨壟行走，目前設(shè)施蔬菜起壟主要依靠人工操作機(jī)械起壟，壟溝直線度誤差大，影響植保機(jī)械的壟間通過(guò)性，同時(shí)溫室內(nèi)只有道路區(qū)域可進(jìn)行掉頭和換壟作業(yè)，需要植保機(jī)具有較好的靈活轉(zhuǎn)向性。

1.2 整機(jī)結(jié)構(gòu)

根據(jù)上述設(shè)施溫室結(jié)構(gòu)和轉(zhuǎn)向要求，設(shè)計(jì)一款可全方位行駛的植保作業(yè)機(jī)器人。該機(jī)器人主要由噴霧模塊、獨(dú)立驅(qū)動(dòng)/轉(zhuǎn)向模塊、導(dǎo)航模塊以及控制模塊組成?？傮w結(jié)構(gòu)如圖3所示。其中噴霧模塊由折疊噴桿、電推桿、隔膜泵、噴頭等組成；獨(dú)立驅(qū)動(dòng)/轉(zhuǎn)向模塊由車輪、轉(zhuǎn)向電機(jī)、蝸輪蝸桿減速器、角度傳感器、驅(qū)動(dòng)電機(jī)等組成，如圖4所示；導(dǎo)航模塊由上位機(jī)、UWB模塊、IMU傳感器、移動(dòng)電源、安裝支架等組成；控制模塊由STM32F407單片機(jī)、電機(jī)驅(qū)動(dòng)器、繼電器，串口顯示器等組成，其控制系統(tǒng)操作界面如圖5所示。植保機(jī)有兩種控制模式，分別是遙控模式和自主導(dǎo)航模式。其中遙控模式是通過(guò)遙控器發(fā)出信號(hào)，控制器接收信號(hào)后通過(guò)控制電機(jī)驅(qū)動(dòng)器和相應(yīng)繼電器遠(yuǎn)程實(shí)現(xiàn)植保機(jī)的前進(jìn)、后退、轉(zhuǎn)向、噴霧打開/關(guān)閉、噴桿折疊等功能。當(dāng)植保機(jī)到達(dá)指定位置時(shí)，通過(guò)切換操作界面啟動(dòng)自主模式，植保機(jī)開始跟蹤預(yù)定軌跡，上位機(jī)實(shí)時(shí)處理偏差并將驅(qū)動(dòng)信號(hào)傳輸至底盤控制系統(tǒng)實(shí)現(xiàn)導(dǎo)航行走作業(yè)。

1.3 工作原理

設(shè)施植保機(jī)器人控制系統(tǒng)結(jié)構(gòu)如圖6所示。操作人員通過(guò)遙控器控制植保機(jī)到達(dá)溫室道路區(qū)域的指定位置后，植保機(jī)切換到自主模式，然后上位機(jī)發(fā)出指令，噴桿自動(dòng)展開并噴霧，植保機(jī)開始跟蹤預(yù)定軌跡。上位機(jī)通過(guò)UWB輸入位置信息和IMU輸入的航向信息計(jì)算出橫向偏差和航向偏差，通過(guò)純跟蹤算法計(jì)算出車身角速度發(fā)送至底盤控制器；上位機(jī)與底盤控制器通過(guò)CAN總線協(xié)議進(jìn)行通訊，控制器根據(jù)接收到的車身角速度通過(guò)運(yùn)動(dòng)學(xué)模型計(jì)算出四輪的轉(zhuǎn)角和速度，通過(guò)CAN接口與電機(jī)驅(qū)動(dòng)器傳輸數(shù)據(jù)，實(shí)時(shí)調(diào)整車輪轉(zhuǎn)向，實(shí)現(xiàn)植保機(jī)沿作業(yè)軌跡導(dǎo)航行駛。STM32通過(guò)I/O接口為噴霧模塊提供控制信號(hào)，完成噴霧啟?？刂?、噴桿折疊控制。遙控模式下，遙控器信號(hào)以RS485通信方式發(fā)送遙控信號(hào)到底盤控制器。

設(shè)施植保機(jī)器人的主要技術(shù)參數(shù)如表１所示。

2 溫室調(diào)頭、換壟工況與運(yùn)動(dòng)學(xué)模型

2.1 溫室調(diào)頭換壟工況分析與路徑規(guī)劃

當(dāng)植保機(jī)在進(jìn)入溫室后需要在狹窄道路區(qū)域進(jìn)行調(diào)轉(zhuǎn)車頭動(dòng)作和換行作業(yè)。設(shè)計(jì)的四輪獨(dú)立轉(zhuǎn)向植保機(jī)可實(shí)現(xiàn)原地轉(zhuǎn)向和平移換行作業(yè)。如圖7所示，當(dāng)植保機(jī)行駛至位置點(diǎn)A時(shí)，切換到自主模式。上位機(jī)打開串口，開始接收UWB定位數(shù)據(jù)和IMU數(shù)據(jù)，植保機(jī)開始跟蹤AB段軌跡，當(dāng)?shù)竭_(dá)B處時(shí)，控制器控制植保機(jī)的各車輪轉(zhuǎn)動(dòng)至特定角度，以原地轉(zhuǎn)向的方式實(shí)現(xiàn)車頭旋轉(zhuǎn)90°，令車頭與噴霧作業(yè)路徑平行，然后轉(zhuǎn)向輪回正，植保機(jī)繼續(xù)沿著既定作業(yè)路徑BC跟蹤。當(dāng)植保機(jī)行駛至C點(diǎn)時(shí)，車輪反向行駛，植保機(jī)開始回航。當(dāng)植保機(jī)回航至B點(diǎn)時(shí)，控制器發(fā)出指令，控制底盤的各輪相同方向轉(zhuǎn)動(dòng)90°，以一種平行運(yùn)動(dòng)的方式實(shí)現(xiàn)換壟作業(yè)，此時(shí)車頭方向仍與路徑BC保持平行，當(dāng)植保機(jī)移動(dòng)到下一直線跟蹤路徑起點(diǎn)D時(shí)，轉(zhuǎn)向輪回正。同理，按上述控制方法完成剩余的既定作業(yè)路徑，當(dāng)植保機(jī)回航行駛至點(diǎn)F點(diǎn)后再平移運(yùn)動(dòng)到補(bǔ)給點(diǎn)處完成作業(yè)任務(wù)，植保機(jī)停止所有運(yùn)動(dòng)。

綜上，當(dāng)植保機(jī)需要掉頭運(yùn)動(dòng)時(shí)，如行至B點(diǎn)處，可以實(shí)現(xiàn)原地轉(zhuǎn)向控制。當(dāng)植保機(jī)需要換壟作業(yè)時(shí)，如行至D處，可實(shí)現(xiàn)平移換壟控制。原地轉(zhuǎn)向和平移換壟方式簡(jiǎn)單靈活，能縮短轉(zhuǎn)向和換行時(shí)間，解決狹小溫室內(nèi)需要較大轉(zhuǎn)向空間的問(wèn)題，有較強(qiáng)的適用性。

植保機(jī)自主導(dǎo)航模式工作時(shí)，上位機(jī)發(fā)出控制信號(hào)，植保機(jī)開始跟蹤提前規(guī)劃好的行駛路徑，如圖7所示，行駛路徑包括噴霧作業(yè)路徑、回航路徑和換行路徑，AB、BD、DF為預(yù)設(shè)換行路徑，BC、DE、FG為噴霧作業(yè)路徑；CB、ED、GF、IH為回航路徑。植保機(jī)在噴霧作業(yè)路徑時(shí)，自動(dòng)啟動(dòng)水泵并噴出藥液，在橫移換壟路徑和回航路徑時(shí)，自動(dòng)關(guān)閉水泵并停止噴藥。圖7中箭頭方向表示植保機(jī)行駛方向。

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

運(yùn)動(dòng)學(xué)模型是植保機(jī)運(yùn)動(dòng)控制的基礎(chǔ)［15］。設(shè)計(jì)的植保機(jī)由4個(gè)直流電機(jī)實(shí)現(xiàn)驅(qū)動(dòng)，由4個(gè)轉(zhuǎn)向電機(jī)提供轉(zhuǎn)向力矩實(shí)現(xiàn)轉(zhuǎn)向。在噴霧作業(yè)過(guò)程中，底盤能按照上述所規(guī)劃的路徑行駛，誤差小且軌跡平滑。每個(gè)獨(dú)立驅(qū)動(dòng)模塊的車輪轉(zhuǎn)向角度和車輪的速度必須相互協(xié)調(diào)，否則會(huì)導(dǎo)致車體變形。為滿足以上要求，需要建立底盤運(yùn)動(dòng)學(xué)模型。

基于阿克曼原理的低速轉(zhuǎn)向模型是農(nóng)用底盤常用的轉(zhuǎn)向模式［12］。為保持實(shí)時(shí)控制效果，使瞬心預(yù)設(shè)在底盤中軸線的延長(zhǎng)線上。簡(jiǎn)化后的低速轉(zhuǎn)向模型如圖8所示，底盤低速向左繞旋轉(zhuǎn)中心O′旋轉(zhuǎn)。四輪轉(zhuǎn)向底盤的運(yùn)動(dòng)分解關(guān)系推導(dǎo)如式（1）～式（4）所示。

α1=α3=arctanL2R－B

（1）

α2=α4=arctanL2R+B

（2）

v1=v2=ωR－B22+L24

（3）

v3=v4=ωR+B22+L24

（4）

式中： L——底盤的軸距；

B——底盤的輪距；

vi——4個(gè)車輪的速度，i=1，2，3，4；

αi——4個(gè)車輪的轉(zhuǎn)角，i=1，2，3，4；

R——繞瞬心O′旋轉(zhuǎn)半徑；

ω——底盤角速度。

當(dāng)植保機(jī)底盤進(jìn)行原地轉(zhuǎn)向運(yùn)動(dòng)（圖9）時(shí)，底盤形心即為旋轉(zhuǎn)中心，各輪的旋轉(zhuǎn)半徑和速度相同，且每個(gè)車輪的速度與豎直方向的夾角相同，并根據(jù)底盤的輪距和軸距，可由三角函數(shù)得出，即為O。

tanα=BL

（5）

由式（5）可得

α=arctanBL=0.736rad

（6）

當(dāng)斜行（圖10）時(shí)各輪的轉(zhuǎn)速相同，偏轉(zhuǎn)角相同且不為0°，即

v1=v2=v3=v4

（7）

α1=α2=α3=α4

（8）

當(dāng)橫移換壟運(yùn)動(dòng)時(shí)，偏轉(zhuǎn)角轉(zhuǎn)至90°，即

α1=α2=α3=α4=90°

（9）

3 導(dǎo)航控制策略

3.1 導(dǎo)航功能實(shí)現(xiàn)原理

植保機(jī)導(dǎo)航系統(tǒng)組成如圖11所示，包括上位機(jī)、UWB定位模塊、IMU傳感器、角度傳感器和四輪轉(zhuǎn)向植保機(jī)。

UWB定位模塊包括1個(gè)UWB定位標(biāo)簽、4個(gè)UWB定位基站、移動(dòng)電源和基站安裝支架。其中，UWB定位基站通過(guò)安裝支架固定于定位區(qū)域邊緣作為定位參考點(diǎn)。UWB定位標(biāo)簽為1個(gè)UWB無(wú)線傳感器，安裝于植保機(jī)車身頂部對(duì)稱軸線上并通過(guò)串口與上位機(jī)通訊，UWB標(biāo)簽通過(guò)與定位基站進(jìn)行無(wú)線通信，獲得與各定位基站間的距離信息。植保機(jī)機(jī)身上安裝IMU傳感器并通過(guò)串口與上位機(jī)通訊，用于實(shí)時(shí)測(cè)量車身的航向信息。

首先確定預(yù)設(shè)軌跡，將該軌跡以二維數(shù)組的形式存儲(chǔ)到上位機(jī)中，和上位機(jī)相連的UWB標(biāo)簽實(shí)時(shí)接收與定位基站的距離信息，據(jù)此可計(jì)算出植保機(jī)當(dāng)前的位置坐標(biāo)，再由前視距離來(lái)獲得軌跡跟蹤的目標(biāo)點(diǎn)，通過(guò)分析IMU采集到的當(dāng)前姿態(tài)信息得出航向信息。從而確定目標(biāo)方向，通過(guò)純跟蹤算法計(jì)算出植保機(jī)角速度并發(fā)送至底盤控制器，控制器根據(jù)運(yùn)動(dòng)學(xué)模型計(jì)算出四輪的轉(zhuǎn)角和速度，實(shí)現(xiàn)植保機(jī)導(dǎo)航（圖12）。

3.2 純跟蹤模型

目前農(nóng)業(yè)機(jī)械導(dǎo)航控制方法為PID控制［13］、模糊控制［14］、模糊PID控制［15］、純跟蹤控制［16］和最優(yōu)控制［17］等。PID控制可靠性高，但控制參數(shù)整定繁瑣復(fù)雜。模糊控制需要根據(jù)專家經(jīng)驗(yàn)制定其控制規(guī)則，追蹤誤差一般較大，難以快速修正?；谏窠?jīng)網(wǎng)絡(luò)的最優(yōu)控制非常依賴訓(xùn)練樣本，需要大量訓(xùn)練樣本才能保證路徑追蹤效果。

設(shè)施內(nèi)空間狹小，障礙物多，在提前規(guī)劃好底盤行駛軌跡的同時(shí)需要控制算法具有響應(yīng)速度快、魯棒性好的優(yōu)點(diǎn)。植保機(jī)室內(nèi)作業(yè)軌跡多為直線軌跡，路徑較單一，選用簡(jiǎn)單的控制算法就能滿足設(shè)施內(nèi)導(dǎo)航要求，并節(jié)省上位機(jī)程序處理的時(shí)間。純跟蹤算法主要是通過(guò)對(duì)底盤進(jìn)行運(yùn)動(dòng)學(xué)建模建立直觀的幾何模型進(jìn)行公式推導(dǎo)，只需要調(diào)整參數(shù)找到最合適的前視距離就可以達(dá)到良好的跟蹤效果。因此采用純跟蹤控制來(lái)解決路徑跟蹤問(wèn)題。

純跟蹤模型如圖13所示。AB線為跟蹤路徑，底盤質(zhì)點(diǎn)為C點(diǎn)，過(guò)C點(diǎn)作AB垂線，垂足為D，從D點(diǎn)沿AB線一定距離為預(yù)瞄點(diǎn)P。過(guò)C點(diǎn)作Y軸的垂線，并與CP的中垂線相交于點(diǎn)O，即為期望轉(zhuǎn)向中心。

CP轉(zhuǎn)向半徑和圓心角分別為R和β，則有幾何關(guān)系

β=2α

（10）

Ldsinβ=Rsinπ2－α

（11）

整理得

R=Ld2sinα

（12）

Ld=（xP－xC）2+（yP－yC）2

（13）

式中： （xP，yP）——

預(yù)瞄點(diǎn)P在UWB坐標(biāo)系下的坐標(biāo)；

（xC，yC）——

植保機(jī)質(zhì)心C點(diǎn)的坐標(biāo)，可通過(guò)UWB測(cè)距值計(jì)算出具體位置。

由圖13可以推導(dǎo)出角速度的表達(dá)式為

ω=2LX·vLd2

（14）

式中： ω——質(zhì)點(diǎn)處的角速度；

v——控制點(diǎn)處的線速度；

Ld——前視距離；

LX——

預(yù)瞄點(diǎn)在坐標(biāo)系下的X軸向距離。

結(jié)合推導(dǎo)出的運(yùn)動(dòng)分解公式，可以將質(zhì)點(diǎn)處的線速度和角速度分解為4個(gè)車輪的轉(zhuǎn)速、轉(zhuǎn)角控制量。

橫向偏差和航向偏差都是在UWB和IMU信息解析計(jì)算得到的數(shù)值。由圖13可知，前視距離Ld為直角△CDP的斜邊CP，其中直角邊DC為橫向偏差d，DP為重要的控制模型參數(shù)，經(jīng)實(shí)際測(cè)試取定值1.5m。

3.3 位置解算算法

將UWB定位基站安裝在同一高度，采用TW—TOF雙向飛行時(shí)間法測(cè)量UWB定位標(biāo)簽與UWB定位基站間的距離［18］，根據(jù)標(biāo)簽與各定位基站的距離以及各基站的位置坐標(biāo)，得到平面4點(diǎn)的定位方程組，如式（15）所示。

（x－x0）2+（y－y0）2=d02

（x－x1）2+（y－y1）2=d12

（x－x2）2+（y－y2）2=d22

（x－x3）2+（y－y3）2=d32

（15）

式中： （xi，yi）——

基站坐標(biāo)，i=1，2，3;

（x，y）——標(biāo)簽坐標(biāo)；

d0、d1、d2、d3——

標(biāo)簽到基站Ai的測(cè)距值。

對(duì)式（15）簡(jiǎn)化可得到關(guān)于標(biāo)簽坐標(biāo)（x，y）的線性方程組，其矩陣表示形式為

Ax=B

（16）

其中x=xi yiT。

式中： x——標(biāo)簽位置坐標(biāo)。

其中矩陣A和矩陣B具體形式如式（17）和式（18）所示。

A=

2（x1－x0）2（y1－y0）

2（x2－x1）2（y2－y1）

2（x3－x2）2（y3－y2）

（17）

B=

x12－x02+y12－y02+d12－d02

x22－x12+y22－y12+d22－d12

x32－x22+y32－y22+d32－d22

（18）

使用最小二乘法求解式（19），可得到基于當(dāng)前UWB標(biāo)簽的二位坐標(biāo)計(jì)算值x。

x=（ATA）－1ATB

（19）

3.4 卡爾曼濾波預(yù)處理UWB測(cè)距值

設(shè)施溫室內(nèi)存在若干金屬支柱，植保機(jī)移動(dòng)過(guò)程中，安裝在機(jī)身上的UWB標(biāo)簽與安裝在溫室內(nèi)的UWB定位基站之間由于金屬立柱的遮擋會(huì)產(chǎn)生非視距誤差，同時(shí)由于傳感器自身屬性會(huì)產(chǎn)生隨機(jī)系統(tǒng)誤差，所以植保機(jī)在溫室作業(yè)過(guò)程中獲得的UWB測(cè)距數(shù)據(jù)需要進(jìn)行預(yù)處理。

卡爾曼濾波器用于預(yù)處理測(cè)距值，并抑制導(dǎo)致UWB距離值突變的隨機(jī)系統(tǒng)誤差和一些非視距距離誤差。使用t-1和t的測(cè)距值的差值作為測(cè)量向量，將x1=0設(shè)置為初始狀態(tài)，并建立離散狀態(tài)模型用于測(cè)距值濾波。

xt=Azxt-1+Wt

zt=Hxt+Vt

（20）

式中： Az——系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣，Az=

1dt

01

；

H——系統(tǒng)測(cè)量矩陣，并且H=［1 0］；

Wt、Vt——

當(dāng)前的過(guò)程噪聲和測(cè)量噪聲，Wt～N（0，Qt），Vt～N（0，Rt）。

基于離散狀態(tài)模型的先驗(yàn)估計(jì)產(chǎn)生預(yù)測(cè)狀態(tài)x-t和預(yù)測(cè)協(xié)方差P-t。

x-t=Azxt-1

（21）

P-t=AzPt-1AzT+Qt

（22）

式中： xt-1——瞬時(shí)t-1最佳狀態(tài)估計(jì)；

Qt——瞬時(shí)t過(guò)程噪聲的協(xié)方差矩陣。

卡爾曼增益Kt從系統(tǒng)中獲得，如式（23）所示。

Kt=P－tH（HP－tHT+Rt）－1

（23）

式（23）表明，預(yù)測(cè)協(xié)方差P－t與卡爾曼增益Kt呈正相關(guān)，觀測(cè)噪聲Rt與卡爾曼增益Kt呈負(fù)相關(guān)。因此，當(dāng)測(cè)量矢量誤差增大時(shí)，當(dāng)前時(shí)刻的卡爾曼增益Kt將減小。更新狀態(tài)x^t和協(xié)方差Pt產(chǎn)生結(jié)果如式（24）和式（25）所示。

x^=x^t－1+Kt［zt－H（x－t－x^t－1）］

（24）

Pt=（1－H·Kt）P－t

（25）

x^t表示當(dāng)前狀態(tài)下的最優(yōu)估計(jì)，并且x^t和Pt將在時(shí)刻t+1參與先驗(yàn)估計(jì)，直到濾波結(jié)束。

4 試驗(yàn)與結(jié)果分析

4.1 自動(dòng)導(dǎo)航作業(yè)試驗(yàn)

為檢驗(yàn)植保機(jī)的自動(dòng)導(dǎo)航作業(yè)系統(tǒng)的穩(wěn)定性，在浙江省農(nóng)業(yè)科學(xué)院楊渡試驗(yàn)基地進(jìn)行試驗(yàn)。試驗(yàn)材料：設(shè)施植保機(jī)、上位機(jī)、UWB定位模塊、IMU傳感器等。

溫室內(nèi)的試驗(yàn)場(chǎng)地為四邊形壟田區(qū)域（尺寸為20m×20m）。UWB定位基站位于試驗(yàn)場(chǎng)地4個(gè)角落，UWB基站安裝高度為2m。建立導(dǎo)航坐標(biāo)系，各基站坐標(biāo)分別為A0（0，0）、A1（20，0）、A2（0，20）、A3（20，20），單位為m。UWB定位標(biāo)簽安裝在植保機(jī)頂部對(duì)稱軸線上。試驗(yàn)前進(jìn)行上位機(jī)、傳感器、控制電源等各控制系統(tǒng)部件的安裝和檢查，設(shè)定噴霧路徑、回航路徑和換行路徑，植保機(jī)分別按照0.5m/s、1.0m/s、1.5m/s的速度行駛，采樣頻率為10Hz。植保機(jī)出發(fā)的初始位置為A0（5，5），初始橫向、航向偏差均為0°，沿指定方向行駛。同一初始狀態(tài)的試驗(yàn)重復(fù)3次，各數(shù)據(jù)求平均值。上位機(jī)記錄實(shí)際跟蹤軌跡并和規(guī)劃軌跡進(jìn)行比較，計(jì)算橫向偏差。植保機(jī)實(shí)際規(guī)劃軌跡如圖14所示。

4.2 試驗(yàn)結(jié)果分析

對(duì)導(dǎo)航橫向偏差的平均值和最大值進(jìn)行統(tǒng)計(jì)分析。最大偏差是在整個(gè)試驗(yàn)過(guò)程中橫向偏差最大值，平均偏差是從試驗(yàn)開始到結(jié)束的橫向偏差均值。

植保機(jī)可在噴霧路徑下自動(dòng)打開噴霧功能，在換行路徑處實(shí)現(xiàn)平移換行控制，在回航路徑下植保機(jī)自動(dòng)關(guān)閉噴霧功能。不同速度下實(shí)際軌跡與規(guī)劃軌跡的橫向偏差統(tǒng)計(jì)結(jié)果如表2所示，植保機(jī)分別以0.5m/s、1.0m/s和1.5m/s的速度跟蹤規(guī)劃軌跡，最大橫向偏差平均值分別為0.094m、0.106m、0.148m，平均橫向偏差分別為0.028m、0.041m、0.068m。在不同速度下，路徑跟蹤精度基本滿足溫室植保作業(yè)要求。

從圖15可以看出，植保機(jī)在前期實(shí)際作業(yè)行駛時(shí)，橫向偏差有所增大，主要原因：植保機(jī)底盤設(shè)施內(nèi)在作業(yè)時(shí)，由于道路區(qū)域和壟作區(qū)域之間有一定的高度差，導(dǎo)致車身產(chǎn)生傾斜或滑移。植保機(jī)受到路面平整度的影響，導(dǎo)致IMU傳感器獲取車體位姿信息產(chǎn)生突變，影響導(dǎo)航性能。雖然顛簸地面不平對(duì)傳感器的定位精度造成一定影響，但是試驗(yàn)結(jié)果展現(xiàn)的導(dǎo)航精度和穩(wěn)定性對(duì)于植保作業(yè)是可以接受的，在不同速度下的導(dǎo)航精度基本滿足溫室植保作業(yè)要求。后期將持續(xù)改進(jìn)控制算法，不斷降低相關(guān)因素對(duì)導(dǎo)航控制的影響，持續(xù)提高導(dǎo)航的控制精度，以適應(yīng)更加復(fù)雜溫室結(jié)構(gòu)環(huán)境的自主移動(dòng)要求和機(jī)械化作業(yè)需求。

5 結(jié)論

1） 針對(duì)設(shè)施壟作模式下植保機(jī)轉(zhuǎn)向和換行困難的問(wèn)題，設(shè)計(jì)一種基于阿克曼轉(zhuǎn)向的設(shè)施植保機(jī)，可以實(shí)現(xiàn)原地轉(zhuǎn)向控制和平移換行控制，為解決狹小溫室內(nèi)掉頭和換行困難等問(wèn)題提供新的方案。

2） 設(shè)計(jì)的UWB/IMU組合的植保機(jī)自動(dòng)導(dǎo)航系統(tǒng)能夠按照規(guī)劃路徑完成直線行駛、平移換壟、原地轉(zhuǎn)向和自動(dòng)打開/關(guān)閉噴霧。溫室試驗(yàn)表明：當(dāng)植保機(jī)速度為0.5m/s、1.0m/s、1.5m/s時(shí)，最大橫向偏差平均值分別為0.094m、0.106m、0.148m，平均橫向偏差分別為0.028m、0.041m、0.068m。

參 考 文 獻(xiàn)

［1］ 孫錦，高洪波，田婧，等.我國(guó)設(shè)施園藝發(fā)展現(xiàn)狀與趨勢(shì)［J］.南京農(nóng)業(yè)大學(xué)學(xué)報(bào)，2019，42（4）：594-604.

Sun Jin， Gao Hongbo， Tian Jing， et al. Development status and trends of protected horticulture in China ［J］. Journal of Nanjing Agricultural University， 2019， 42（4）：594-604.

［2］ 齊飛，李愷，李邵，等.世界設(shè)施園藝智能化裝備發(fā)展對(duì)中國(guó)的啟示研究［J］. 農(nóng)業(yè)工程學(xué)報(bào)， 2019， 35（2）： 183-195.

Qi Fei， Li Kai， Li Shao， et al. Development of intelligent equipment for protected horticulture in world and enlightenment to China ［J］. Transactions of the Chinese Society of Agricultural Engineering， 2019， 35（2）：183-195.

［3］ 姬長(zhǎng)英，周俊.農(nóng)業(yè)機(jī)械導(dǎo)航技術(shù)發(fā)展分析［J］.農(nóng)業(yè)機(jī)械學(xué)報(bào)，2014，45（9）：44-54.

Ji Changying， Zhou Jun. Current situation of navigation technologies for agricultural machinery ［J］.Transactions of the Chinese Society for Agricultural Machinery， 2014， 45（9）： 44-54.

［4］ 劉成良，林洪振，李彥明，等.農(nóng)業(yè)裝備智能控制技術(shù)研究現(xiàn)狀與發(fā)展趨勢(shì)分析［J］.農(nóng)業(yè)機(jī)械學(xué)報(bào)，2020，51（1）：1-18.

Liu Chengliang， Lin Hongzhen， Li Yanming， et al. Analysis on status and development trend of intelligent control technology for agricultural equipment ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2020， 51（1）：1-18.

［5］ 林相澤，王祥，林彩鑫，等.基于超寬帶的溫室農(nóng)用車輛定位信息采集與優(yōu)化［J］.農(nóng)業(yè)機(jī)械學(xué)報(bào)，2018，49（10）：23-29，45.

Lin Xiangze， Wang Xiang， Lin Caixin， et al. Location information collection and optimization for agricultural vehicle based on UWB ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2018， 49（10）： 23-29， 45.

［6］ 李天華，吳增昊，廉憲坤，等.基于定向攝像頭的大拱棚運(yùn)輸車視覺導(dǎo)航研究［J］.農(nóng)業(yè)機(jī)械學(xué)報(bào)，2018，49（S1）：8-13.

Li Tianhua， Wu Zenghao， Lian Xiankun， et al. Navigation line detection for greenhouse carrier vehicle based on fixed direction camera ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2018， 49（S1）： 8-13.

［7］ 管澤峰. 溫室作業(yè)車輛行走控制技術(shù)與裝備［D］.鎮(zhèn)江：江蘇大學(xué)，2017.

［8］ 侯加林，蒲文洋，李天華，等.雙激光雷達(dá)溫室運(yùn)輸機(jī)器人導(dǎo)航系統(tǒng)研制［J］.農(nóng)業(yè)工程學(xué)報(bào)， 2020， 36（14）：80-88.

Hou Jialin， Pu Wenyang， Li Tianhua， et al. Development of dual-lidar navigation system for greenhouse transportation robot ［J］. Transactions of the Chinese Society of Agricultural Engineering， 2020， 36（14）：80-88.

［9］ 趙辰彥，姚立健，楊自棟，等.基于無(wú)線定位的溫室生產(chǎn)平臺(tái)自動(dòng)導(dǎo)航［J］.浙江農(nóng)林大學(xué)學(xué)報(bào)，2020，37（3）：578-586.

Zhao Chenyan， Yao Lijian， Yang Zidong， et al. Automatic navigation of production platform in greenhouse based on wireless positioning ［J］. Journal of Zhejiang A amp; F University， 2020， 37（3）： 578-586.

［10］ 畢松，張國(guó)軒，李志軍，等.基于測(cè)距值修正的溫室植保機(jī)器人定位方法［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2023， 54（8）： 347-358.

Bi Song， Zhang Guoxuan， Li Zhijun， et al. Positioning method of greenhouse plant protection robot based on distance measurement value correction ［J］.Transactions of the Chinese Society for Agricultural Machinery， 2023， 54（8）： 347-358.

［11］ 景亮，張亞飛，沈躍，等.四輪獨(dú)立驅(qū)動(dòng)高地隙無(wú)人噴霧機(jī)軌跡跟蹤自適應(yīng)控制［J］.農(nóng)業(yè)機(jī)械學(xué)報(bào)，2021，52（12）：408-416.

Jing Liang， Zhang Yafei， Shen Yue， et al. Adaptive trajectory tracking control of 4WID high clearance unmanned sprayer ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2021， 52（12）： 408-416.

［12］ 何杰，滿忠賢，胡煉，等.履帶式花生聯(lián)合收獲機(jī)路徑跟蹤控制方法與試驗(yàn)［J］. 農(nóng)業(yè)工程學(xué)報(bào)， 2023， 39（1）： 9-17.

He Jie， Man Zhongxian， Hu Lian， et al. Path tracking control method and experiments for the crawler-mounted peanut combine harvester ［J］. Transactions of the Chinese Society of Agricultural Engineering， 2023， 39（1）： 9-17.

［13］ 丁幼春，何志博，夏中州，等.小型履帶式油菜播種機(jī)導(dǎo)航免疫PID控制器設(shè)計(jì)［J］.農(nóng)業(yè)工程學(xué)報(bào)，2019，35（7）：12-20.

Ding Youchun， He Zhibo， Xia Zhongzhou， et al. Design of navigation immune controller of small crawler-type rape seeder ［J］. Transactions of the Chinese Society of Agricultural Engineering， 2019， 35（7）：12-20.

［14］ 白如月，汪小旵，魯偉，等.施藥機(jī)器人對(duì)行施藥系統(tǒng)的設(shè)計(jì)與試驗(yàn)［J］. 華南農(nóng)業(yè)大學(xué)學(xué)報(bào)， 2018， 39（5）： 101-109.

Bai Ruyue， Wang Xiaochan， Lu Wei， et al. Design and experiment of row-following pesticide spraying system by robot ［J］. Journal of South China Agricultural University， 2018， 39（5）： 101-109.

［15］ 劉偉健，羅錫文，曾山，等.基于模糊PID控制的再生稻自適應(yīng)仿形割臺(tái)性能試驗(yàn)與分析［J］. 農(nóng)業(yè)工程學(xué)報(bào)， 2022， 38（10）：1-9.

Liu Weijian， Luo Xiwen， Zeng Shan， et al. Performance test and analysis of the self-adaptive profiling header forratooning ricebased on fuzzy PID control ［J］. Transactions of the Chinese Society of Agricultural Engineering， 2022，38（10）：1-9.

［16］ 姚立健，Santosh K Pitla，楊自棟，等.基于超寬帶無(wú)線定位的農(nóng)業(yè)設(shè)施內(nèi)移動(dòng)平臺(tái)路徑跟蹤研究［J］.農(nóng)業(yè)工程學(xué)報(bào)，2019，35（2）：17-24.

Yao Lijian， Santosh K Pitla， Yang Zidong， et al.Path tracking of mobile platform in agricultural facilities based on ultra wideband wireless positioning ［J］.Transactions of the Chinese Society of Agricultural Engineering，2019，35（2）：17-24.

［17］ 韓科立， 朱忠祥， 毛恩榮， 等. 基于最優(yōu)控制的導(dǎo)航拖拉機(jī)速度與航向聯(lián)合控制方法［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2013， 44（2）： 165-170.

Han Keli， Zhu Zhongxiang， Mao Enrong， et al. Joint control method of speed and heading of navigation tractor based on optimal control ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2013， 44（2）： 165-170.

［18］ 繆希仁，范建威，江灝，等.基站異常情況下基于改進(jìn)極限學(xué)習(xí)機(jī)的超寬帶室內(nèi)定位方法［J］.傳感技術(shù)學(xué)報(bào)， 2020， 33（10）： 1457-1466.

Miao Xiren， Fan Jianwei， Jiang Hao， et al. Method of UWB indoor location based on improved extreme learning machine in case of anchor abnormity ［J］.Chinese Journal of Sensors and Actuators，2020，33（10）：1457-1466.