陳柯霖 謝守勇 陳翀 向旺 劉偉

摘要：針對(duì)丘陵山區(qū)農(nóng)業(yè)機(jī)械因體積小、種類(lèi)多，難以無(wú)人作業(yè)改造的問(wèn)題，搭建基于雙預(yù)瞄純追蹤算法的丘陵山區(qū)農(nóng)機(jī)差分導(dǎo)航系統(tǒng)，完成導(dǎo)航系統(tǒng)結(jié)構(gòu)、路徑規(guī)劃策略和追蹤控制器的設(shè)計(jì)。使用Ntrip DTU和北斗定位板卡完成差分定位。采用平滑轉(zhuǎn)向方式，提出適用于丘陵山區(qū)的全局路徑規(guī)劃策略，可有效適配不同轉(zhuǎn)彎半徑的農(nóng)機(jī)，減少地頭區(qū)域。設(shè)計(jì)雙預(yù)瞄純追蹤模糊控制器，開(kāi)展與純追蹤控制器的對(duì)比仿真試驗(yàn)，并搭建試驗(yàn)樣機(jī)完成測(cè)試。實(shí)際測(cè)試結(jié)果顯示：控制器在追蹤直線(xiàn)時(shí)，在s=2.9 m處收斂，在之后的直線(xiàn)追蹤中平均絕對(duì)誤差保持在0.01 m；在追蹤曲線(xiàn)時(shí)，兩段彎道最大誤差分別為0.08 m和0.75 m，平均絕對(duì)誤差0.04 m。

關(guān)鍵詞：丘陵山區(qū)；差分導(dǎo)航；路徑規(guī)劃；純追蹤；雙預(yù)瞄模糊控制器

中圖分類(lèi)號(hào)：S24

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

文章編號(hào)：20955553 （2024） 02018707

收稿日期：2023年2月13日 ?修回日期：2023年5月18日

基金項(xiàng)目：中國(guó)煙草總公司重慶市煙草專(zhuān)賣(mài)局（公司）科技計(jì)劃項(xiàng)目（B20211NY1317）；重慶市技術(shù)創(chuàng)新與應(yīng)用發(fā)展專(zhuān)項(xiàng)重點(diǎn)項(xiàng)目（cstc2021jscx—gksbX0065）

第一作者：陳柯霖，男，1996年生，四川成都人，碩士；研究方向?yàn)檗r(nóng)業(yè)智能裝備。Email： 1083106951@qq.com

通訊作者：謝守勇，男，1969年生，重慶人，博士，教授；研究方向?yàn)橹悄苻r(nóng)業(yè)裝備開(kāi)發(fā)。Email： zswu0310@163.com

Research on differential navigation system of agricultural machinery in hilly areas

based on dual preview pure tracking algorithm

Chen Kelin， Xie Shouyong， Chen Chong， Xiang Wang， Liu Wei

（College of Engineering and Technology， Southwest University， Chongqing， 400715， China）

Abstract：

In order to solve the problem that agricultural machinery in hilly areas is difficult to be transformed by unmanned operation due to its small size and many kinds， a differential navigation system for agricultural machinery in hilly and mountainous areas based on double preview tracking algorithm was built， and the navigation system structure， path planning strategy and tracking controller were designed. Ntrip DTU and Beidou positioning board card were used to complete differential positioning. A global path planning strategy suitable for hilly and mountainous areas was proposed by using smooth steering mode， which could effectively adapt to agricultural machinery with different turning radius and reduce field area. The dual preview pure tracking fuzzy controller was designed， and the simulation experiment was carried out compared with the pure tracking controller， and the experimental prototype was built to complete the test. The actual test results showed that the controller proposed in this paper converged?at s=2.9 m when tracking the straight line， and the average absolute error remained at 0.01 m in the following straight line tracking. When tracking the curve， the maximum error of the two curves was 0.08 m and 0.75 m respectively， and the average absolute error was 0.04 m.

Keywords：

hilly areas; differential navigation; path planning; pure tracking; double preview fuzzy controller

0 引言

傳感器、物聯(lián)網(wǎng)和云計(jì)算等技術(shù)的應(yīng)用，推動(dòng)了現(xiàn)代農(nóng)業(yè)向著智能化、信息化、精細(xì)化發(fā)展［1］，其中衛(wèi)星導(dǎo)航技術(shù)的應(yīng)用尤其廣泛［2］。根據(jù)《2021年全國(guó)農(nóng)業(yè)機(jī)械化發(fā)展統(tǒng)計(jì)公報(bào)》，2021年中國(guó)加裝北斗衛(wèi)星導(dǎo)航系統(tǒng)的農(nóng)用車(chē)輛超過(guò)60萬(wàn)臺(tái)，植保無(wú)人機(jī)保有量97931架，同比增長(zhǎng)39.22%［3］，大幅提高了作業(yè)效率和農(nóng)業(yè)智能化水平。

農(nóng)業(yè)車(chē)輛衛(wèi)星導(dǎo)航系統(tǒng)包含完整可靠的硬件基礎(chǔ)、精準(zhǔn)的路徑追蹤控制器［45］和高效的路徑規(guī)劃策略［6］，目前國(guó)內(nèi)外學(xué)者對(duì)此都有大量的研究。在追蹤控制器的研究上，柴善鵬等［7］在溫室農(nóng)機(jī)上使用粒子群優(yōu)化算法優(yōu)化純追蹤追蹤控制器，以提高控制精度。黎永鍵等［8］在東方紅拖拉機(jī)上搭建雙閉環(huán)轉(zhuǎn)向控制的自動(dòng)導(dǎo)航系統(tǒng)。王立輝等［9］依托雷沃谷神收獲機(jī)搭建自適應(yīng)路徑追蹤系統(tǒng)。刁勤晴等［10］通過(guò)雙預(yù)瞄策略解決了跟蹤大曲率路徑時(shí)車(chē)速過(guò)高的問(wèn)題。Ahn等［11］通過(guò)改變前瞻點(diǎn)與路徑的幾何位置，改進(jìn)了純追蹤算法，解決了震蕩和轉(zhuǎn)彎半徑過(guò)小問(wèn)題。靳欣宇等［12］提出了一種基于最優(yōu)預(yù)瞄時(shí)間的Stanley追蹤控制器，實(shí)現(xiàn)了復(fù)雜道路的無(wú)人駕駛。

同時(shí)，在對(duì)導(dǎo)航系統(tǒng)核心技術(shù)之一——路徑規(guī)劃的研究上［13］，翟衛(wèi)欣等［14］提出了一種基于區(qū)塊套行作業(yè)模式的路徑規(guī)劃方法。孟志軍等［15］ 根據(jù)車(chē)輛和田地的先驗(yàn)信息，選擇不同的優(yōu)化目標(biāo)計(jì)算出最優(yōu)路徑。劉剛等［16］提出了一種基于地塊平整度的全局路徑規(guī)劃方法，提高平地效率30%以上。Rahman等［17］使用N多邊形算法和凸包分裂交叉點(diǎn)法完成凸凹多邊形田地路徑規(guī)劃，減少了作物損失和工作時(shí)間。Han等［18］提出了基于X形轉(zhuǎn)向的多邊形地塊路徑規(guī)劃算法，提高了作業(yè)質(zhì)量。

上述導(dǎo)航系統(tǒng)使用的控制器和路徑規(guī)劃策略效果顯著，但主要適用于特定類(lèi)型的農(nóng)機(jī)和場(chǎng)景，硬件結(jié)構(gòu)相對(duì)零散并需要車(chē)載計(jì)算機(jī)的輔助，成本較高、缺乏靈活性。而丘陵山區(qū)農(nóng)機(jī)種類(lèi)多、農(nóng)機(jī)體積小，要求導(dǎo)航系統(tǒng)擴(kuò)展性高、成本低，便于二次開(kāi)發(fā)。為此本文基于純追蹤算法和RTK（Realtime kinematic）定位技術(shù)搭建丘陵山區(qū)小型農(nóng)機(jī)導(dǎo)航控制系統(tǒng)，完成系統(tǒng)硬件搭建，提出適用于丘陵山區(qū)的路徑規(guī)劃策略和雙預(yù)瞄純追蹤模糊控制器，并對(duì)控制器進(jìn)行仿真和試驗(yàn)，驗(yàn)證其準(zhǔn)確性和有效性。

1 系統(tǒng)設(shè)計(jì)

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

導(dǎo)航系統(tǒng)結(jié)構(gòu)由控制終端、被控端、執(zhí)行機(jī)構(gòu)控制端、定位裝置、網(wǎng)絡(luò)端組成，如圖1所示。

系統(tǒng)被控端芯片采用STM32F407VET6，移植freeRTOS（Free Real Time Operating System），支持任務(wù)創(chuàng)建，享有獨(dú)立的堆棧空間，通過(guò)時(shí)間片流轉(zhuǎn)和全搶占式調(diào)度，從宏觀上實(shí)現(xiàn)多線(xiàn)程、多優(yōu)先級(jí)處理［19］。系統(tǒng)數(shù)據(jù)儲(chǔ)存由SD卡和可擦可編程只讀存儲(chǔ)器（EEPROM）組成，SD卡用以?xún)?chǔ)存導(dǎo)航路點(diǎn)，移植FATFS文件系統(tǒng)［20］，可完成路徑點(diǎn)文件（夾）的創(chuàng)建、寫(xiě)入、刪除、搜索，EEPROM儲(chǔ)存用戶(hù)設(shè)備參數(shù)。串口屏作為人機(jī)交互界面，通SI24R1射頻模組與被控端連接，完成遠(yuǎn)程通信。使用4G模組接入網(wǎng)絡(luò)，提高系統(tǒng)可擴(kuò)展性，配置TCP（Transmission Control Protocol）通道工作模式，進(jìn)行數(shù)據(jù)透?jìng)?，由socket網(wǎng)絡(luò)編程搭建服務(wù)器，可在服務(wù)器端完成數(shù)據(jù)上傳、儲(chǔ)存、處理、下載、可視化等功能，并為復(fù)雜算法提供空間和算力。定位裝置使用北斗高精度定位定向板卡UB482，定位精度1cm，航向角精度0.2°/1m基線(xiàn)。由千尋DTU（Data Transfer unit）接收服務(wù)商固定站的差分?jǐn)?shù)據(jù)，無(wú)需現(xiàn)場(chǎng)搭建固定站。數(shù)據(jù)以NMEA消息協(xié)議傳輸至系統(tǒng)解碼，獲取當(dāng)前坐標(biāo)、速度、航向角。執(zhí)行單元由STM32F103VET6作為主控，由RS232接口與系統(tǒng)通信并預(yù)留CAN通訊接口。根據(jù)系統(tǒng)功能需要設(shè)計(jì)PCB主板，并將大小控制在13cm×15cm，符合輕量化設(shè)計(jì)，更加適用于小型農(nóng)機(jī)。

1.2 任務(wù)創(chuàng)建與數(shù)據(jù)處理

系統(tǒng)依托freeRTOS，創(chuàng)建四種任務(wù)類(lèi)型。數(shù)據(jù)傳輸類(lèi)任務(wù)用于整合外部通信接口，與系統(tǒng)內(nèi)部建立通道；數(shù)據(jù)處理類(lèi)任務(wù)用于預(yù)處理定位數(shù)據(jù)及解析命令、根據(jù)命令調(diào)用路徑規(guī)劃和路徑追蹤功能；數(shù)據(jù)存儲(chǔ)類(lèi)任務(wù)用于完成數(shù)據(jù)的存儲(chǔ)和調(diào)用；人機(jī)交互類(lèi)任務(wù)為用戶(hù)提供觸摸屏輸入界面以及操作按鍵。采用串口DMA（Direct Memory Access）中斷接收定位數(shù)據(jù)，接收與解碼輪轉(zhuǎn)，避免數(shù)據(jù)丟失問(wèn)題。在中斷中引入二進(jìn)制信號(hào)量，完成數(shù)據(jù)接收與解碼任務(wù)的同步，建立消息隊(duì)列等待其他任務(wù)對(duì)定位數(shù)據(jù)請(qǐng)求。以“$cmd+命令符+文件名+#”規(guī)則建立命令，在數(shù)據(jù)處理任務(wù)中解析字符串，設(shè)置事件組中對(duì)應(yīng)位實(shí)現(xiàn)多任務(wù)之間的同步。

2 路徑規(guī)劃策略

丘陵山區(qū)田塊面積相對(duì)較小，邊緣規(guī)整度低，為提高土地使用效率，提出一種轉(zhuǎn)彎地頭小、對(duì)車(chē)輛適應(yīng)性高、生成路徑規(guī)整度好的規(guī)劃策略。將田塊分為自主作業(yè)區(qū)和遙控作業(yè)區(qū)，在自主作業(yè)區(qū)使用跨行次數(shù)nc來(lái)量化通勤路線(xiàn)長(zhǎng)度，以減小地頭轉(zhuǎn)彎區(qū)域?yàn)橹鳎瑴p少通勤路線(xiàn)長(zhǎng)度為輔規(guī)劃路徑。在遙控作業(yè)區(qū)，對(duì)人工遙控作業(yè)產(chǎn)生的路點(diǎn)全程采集并存儲(chǔ)，可供自主作業(yè)二次調(diào)用。

2.1 作業(yè)區(qū)和作業(yè)帶劃分

在獲取經(jīng)緯度信息后，因其單位跨度較大，將其轉(zhuǎn)化為通用墨卡爾投影（UTM）下的位置信息。通過(guò)四點(diǎn)圈定田塊，規(guī)整為四邊形，分別計(jì)算四邊與對(duì)邊端點(diǎn)的最大距離，選擇其中最小距離d3對(duì)應(yīng)的邊l3作為起始邊，以作業(yè)寬幅w等距切分起始邊最短鄰邊l1，生成n條作業(yè)帶，使得作業(yè)條數(shù)和轉(zhuǎn)彎次數(shù)最少，作業(yè)帶中心線(xiàn)即為自主作業(yè)路徑雛形，同時(shí)劃出了自主和遙控作業(yè)區(qū)，如圖2所示。

2.2 自主作業(yè)區(qū)轉(zhuǎn)向方式及路徑規(guī)劃

根據(jù)車(chē)輛轉(zhuǎn)彎半徑r和作業(yè)條數(shù)n，分割地頭區(qū)域。本文自主作業(yè)區(qū)采用平滑轉(zhuǎn)向［11］，在轉(zhuǎn)彎半徑較大時(shí)，使用跨行平滑轉(zhuǎn)向代替常用的Ω形轉(zhuǎn)向掉頭，見(jiàn)圖3。

（a） Ω形轉(zhuǎn)向

（b） 平滑轉(zhuǎn)向

由幾何關(guān)系可得，平滑轉(zhuǎn)向?qū)D(zhuǎn)彎地頭區(qū)域要求長(zhǎng)度與α和r有關(guān)，當(dāng)邊界為斜邊時(shí)，根據(jù)相切的兩直線(xiàn)生成轉(zhuǎn)向圓弧，轉(zhuǎn)彎區(qū)域長(zhǎng)度

h1=r·sin（90°－α）+w2

（1）

而常用的Ω形轉(zhuǎn)向，轉(zhuǎn)彎區(qū)域長(zhǎng)度

h2=2rsinarccosw+2r4r+r+w2

（2）

由式（1）和式（2）知，平滑轉(zhuǎn)向相較于常用的Ω形轉(zhuǎn)向，占用地頭轉(zhuǎn)彎區(qū)域更少。

當(dāng)r≤1/2w時(shí)，無(wú)需跨行轉(zhuǎn)向，地頭轉(zhuǎn)彎區(qū)域較小，以梭行法［11］生成作業(yè)路徑，能夠減少通勤路線(xiàn)長(zhǎng)度，此時(shí)跨行次數(shù)nc=n，如圖4（a）所示。

當(dāng)r>1/2w時(shí)，需轉(zhuǎn)向跨行，使用結(jié)行法規(guī)劃路徑，如圖4（b）所示。結(jié)行法最小作業(yè)條數(shù)ns與轉(zhuǎn)彎半徑r、作業(yè)寬幅w相關(guān)，按式（3）計(jì)算，在相同轉(zhuǎn)彎半徑和作業(yè)寬幅下，該方法較于常用的套行法［11］，每條作業(yè)帶所消耗的通勤路徑更短，且要求的最小作業(yè)條數(shù)更小，擴(kuò)大了適用范圍，如圖4（c）所示，在轉(zhuǎn)彎最小跨行數(shù)都為2且車(chē)輛同側(cè)進(jìn)出的狀態(tài)下，滿(mǎn)足結(jié)行法的最小作業(yè)行數(shù)為4，而套行法為6。對(duì)于結(jié)行法，其跨行次數(shù)nc按式（4）計(jì)算。

ns=2×2rw r≥12w

（3）

nc=4∑n2i=1i－3

（4）

由式知當(dāng)作業(yè)條數(shù)為4時(shí)，nc最小，作業(yè)條數(shù)增多時(shí)，nc增速上升，為減小通勤路線(xiàn)長(zhǎng)度，將其分組，如圖4（d）所示。以最小作業(yè)條數(shù)為一組，將n條作業(yè)路徑分為gs個(gè)最小組和1個(gè)作業(yè)條數(shù)為nf剩余組。其中g(shù)s和nf分別按式（5）、式（6）計(jì)算。

gs=nns－1

（5）

nf=n－gs·ns

（6）

（a） 梭行法，r

（b） 結(jié)行法，n=4

（c） 結(jié)行與套行對(duì)比

（d） 組合法

3 雙預(yù)瞄純追蹤模糊控制器設(shè)計(jì)

3.1 農(nóng)機(jī)運(yùn)動(dòng)模型分析

對(duì)路徑進(jìn)行精確追蹤是導(dǎo)航系統(tǒng)的主要功能，而實(shí)現(xiàn)這一功能需要對(duì)農(nóng)業(yè)機(jī)械轉(zhuǎn)向模型進(jìn)行分析，丘陵山區(qū)常用的農(nóng)機(jī)運(yùn)動(dòng)轉(zhuǎn)向模型為阿克曼轉(zhuǎn)向和履帶差速轉(zhuǎn)向兩類(lèi)。在質(zhì)心和幾何中心重合且不計(jì)車(chē)輪滑移的條件下對(duì)兩類(lèi)模型分析，如圖5所示。

（a） 阿克曼轉(zhuǎn)向模型

（b） 履帶差速轉(zhuǎn)向模型

對(duì)農(nóng)機(jī)轉(zhuǎn)向模型進(jìn)行運(yùn)動(dòng)學(xué)分析，將阿克曼轉(zhuǎn)向模型簡(jiǎn)化成自行車(chē)模型，如圖5（a）所示，可得轉(zhuǎn)彎半徑與前輪航向角關(guān)系如式（7）所示。

tanβ2=dr

（7）

對(duì)于履帶式農(nóng)機(jī)，將其等效為二輪差速模型，如圖5（b）所示，其中虛擬輪距dv與實(shí)際工況變化的參數(shù)相關(guān)［21］，根據(jù)運(yùn)動(dòng)學(xué)分析可得式（8）。

r=v2dr=（v1+v3）×dv2（v3－v1）

（8）

由以上分析得，阿克曼轉(zhuǎn)向和履帶差速轉(zhuǎn)向可分別通過(guò)改變前輪轉(zhuǎn)向角和左右履帶速度來(lái)獲取期望的轉(zhuǎn)彎半徑。為此系統(tǒng)采用通過(guò)改變轉(zhuǎn)彎半徑來(lái)追蹤目標(biāo)的純追蹤算法，該算法對(duì)追蹤路徑的連續(xù)性要求低，系統(tǒng)路徑可采用點(diǎn)間隔0.2m的路點(diǎn)形式存儲(chǔ)，加入雙預(yù)瞄模糊控制器提高追蹤精度。面對(duì)不同種類(lèi)的農(nóng)機(jī)，只需要修改執(zhí)行機(jī)構(gòu)端程序完成轉(zhuǎn)彎半徑的控制以適配系統(tǒng)。

3.2 雙預(yù)瞄純追蹤幾何模型分析

純追蹤算法是通過(guò)車(chē)輛的橫向偏差和到預(yù)瞄點(diǎn)的航向偏差計(jì)算出期望轉(zhuǎn)彎半徑，對(duì)純追蹤模型進(jìn)行分析如圖6所示。

通過(guò)分析可得式（9）。

ldsinα=rsinπ－2α2

（9）

簡(jiǎn)化可得半徑與前視距離的關(guān)系如式（10）所示。

r=ld2sinα

（10）

由式（10）可知，純追蹤法所得的期望轉(zhuǎn)彎半徑r與ld、α有關(guān)。其中α根據(jù)實(shí)際位置計(jì)算得知，ld為系統(tǒng)設(shè)定值，并且追蹤性能極大地取決于ld的大小。當(dāng)ld較小時(shí)，跟蹤更加精準(zhǔn)，但過(guò)小的前視距離也會(huì)帶來(lái)震蕩，而較大的前視距離意味著跟蹤軌跡越平滑，但在彎曲道路上，跟蹤精度會(huì)下降。

3.3 雙預(yù)瞄純追蹤模糊控制器設(shè)計(jì)

確定一個(gè)合適的前視距離，提高追蹤精度，更加有利于農(nóng)機(jī)在丘陵山區(qū)作業(yè)。本文使用雙預(yù)瞄和模糊算法建立純追蹤控制器。第一預(yù)瞄點(diǎn)對(duì)道路曲折度進(jìn)行評(píng)估，算法從最近路點(diǎn)開(kāi)始向前搜索路點(diǎn)直到最大前視距離位置，計(jì)算搜索到的相鄰路點(diǎn)連線(xiàn)的平均夾角大小，用于評(píng)估道路曲折度，第二預(yù)瞄點(diǎn)即為由前視距離確定的跟蹤目標(biāo)點(diǎn)，見(jiàn)圖6。模糊控制器輸入為橫向偏差和曲折度，輸出為前視距離，對(duì)輸入輸出模糊化。

橫向偏差論域?yàn)?2～2 m，量化等級(jí)負(fù)大（NB），負(fù)中（NM），負(fù)?。∟S），零（ZO），正?。≒S），正中（PM），正大（PB）。

道路曲折度論域 0°～8°，量化等級(jí)超?。╒S），小（S），中（M），大（B），超大（VB）。

前視距離論域0.5～2.5 m，量化等級(jí)超?。╒S），小（S），中（M），大（B），超大（VB）。

根據(jù)車(chē)輛駕駛經(jīng)驗(yàn)，當(dāng)橫向偏差較大、道路曲折時(shí)，減小前視距離，預(yù)瞄近點(diǎn)，以快速逼近路線(xiàn)，更精準(zhǔn)的跟蹤路徑。當(dāng)橫向偏差小、道路端直時(shí)，增大前視距離，使行駛路徑更加平滑?；谏鲜鲴{駛經(jīng)驗(yàn)，經(jīng)過(guò)專(zhuān)家經(jīng)驗(yàn)設(shè)計(jì)的三維模糊控制器共有35條控制規(guī)則，如表1所示，模糊控制曲面如圖7所示。

4 路點(diǎn)追蹤仿真

為測(cè)試雙預(yù)瞄純追蹤模糊控制器的可靠性，建立阿克曼轉(zhuǎn)向模型，設(shè)定車(chē)輛軸距1m。取固定前視距離1.6m和控制器輸出的動(dòng)態(tài)前視距離，使用Python編寫(xiě)控制器程序，根據(jù)上述路徑規(guī)劃策略特點(diǎn)生成追蹤路點(diǎn)，完成直線(xiàn)和曲線(xiàn)追蹤仿真實(shí)驗(yàn)。

4.1 直線(xiàn)追蹤仿真

以起點(diǎn)（0，1）、終點(diǎn)（0，13），點(diǎn)間隔0.2m生成直線(xiàn)路點(diǎn)，車(chē)輛起始位置（0，0），航向角0°，追蹤路點(diǎn)，仿真結(jié)果如圖8、圖9所示。

直線(xiàn)路徑曲折度為0，從起點(diǎn)開(kāi)始橫向誤差較大，此時(shí)控制器輸出較短的前視距離，使車(chē)輛快速逼近路徑，在s=2.65m時(shí)處接近路點(diǎn)，使追蹤誤差減少到0.1m以?xún)?nèi)，固定前視距離則在s=4.2m時(shí)誤差接近0.1m。動(dòng)態(tài)距離輸出最大反向誤差約為0.04m，固定距離最大反向誤差約為0.03m，差值0.01m，但使用動(dòng)態(tài)距離可快速地將誤差逼近到小范圍內(nèi)，以滿(mǎn)足作業(yè)要求。隨著橫向誤差減小，控制器輸出較長(zhǎng)的前視距離，車(chē)輛平緩靠近路點(diǎn)，并在s=7m時(shí)將追蹤誤差減小到0.01m以?xún)?nèi)。

4.2 曲線(xiàn)追蹤仿真

以起點(diǎn)（0，-2），終點(diǎn)（6，-2），半徑2.5m生成兩段曲線(xiàn)路點(diǎn)。車(chē)輛起點(diǎn)（0，0），航向角90°，追蹤路點(diǎn)，仿真結(jié)果如圖10、圖11所示。

在初始階段，車(chē)輛在路徑上且航向與路徑一致，固定距離和動(dòng)態(tài)距離跟蹤精度差距小。當(dāng)要進(jìn)入曲線(xiàn)路點(diǎn)時(shí)，第一預(yù)瞄點(diǎn)向前搜索，計(jì)算出道路曲折度變大，控制器提前輸出一個(gè)較小的前視距離以更精確地追蹤路點(diǎn)。而固定距離在道路變曲折時(shí)，未作出相應(yīng)改變，導(dǎo)致轉(zhuǎn)彎半徑變小，從而偏離路點(diǎn)。

車(chē)輛在s=2m處進(jìn)入曲線(xiàn)路徑，誤差持續(xù)變大，在第一段曲線(xiàn)路徑和第二段曲線(xiàn)路徑中，使用固定距離的追蹤誤差分別在s=3.7m和s=9.3m時(shí)達(dá)到最大值0.22m和0.13m，動(dòng)態(tài)距離則在s=3.8m和s=8.3m時(shí)達(dá)到最大值，分別為0.05m和0.06m。

通過(guò)仿真數(shù)據(jù)知，相比于固定前視距離，使用動(dòng)態(tài)前視距離追蹤直線(xiàn)路點(diǎn)能夠更快收斂，在追蹤曲線(xiàn)路點(diǎn)時(shí)，精度更高。

5 路點(diǎn)追蹤試驗(yàn)

為驗(yàn)證系統(tǒng)和追蹤算法的可行性，改裝實(shí)驗(yàn)室現(xiàn)有小型播種機(jī)作為試驗(yàn)樣機(jī)，采樣實(shí)際UTM坐標(biāo)（390 431.23，3 395 672.39）并生成作業(yè)直線(xiàn)與兩段曲線(xiàn)路徑點(diǎn)。在速度1m/s，誤差1m，航向角0°的初始狀態(tài)下分別使用固定和動(dòng)態(tài)前視距離完成追蹤試驗(yàn)，試驗(yàn)結(jié)果如圖12、圖13所示。

結(jié)果表明，使用固定前視距離追蹤時(shí)，車(chē)輛在s=4.2m處逼近直線(xiàn)路徑，并產(chǎn)生0.01m的反向誤差，在追蹤曲線(xiàn)路點(diǎn)階段，追蹤第一段和第二段曲線(xiàn)路點(diǎn)最大誤差分別為0.16m和0.11m，平均絕對(duì)誤差為0.09m，并在回到之后直線(xiàn)路點(diǎn)時(shí)，產(chǎn)生0.08m的反向誤差。

而使用動(dòng)態(tài)前視距離追蹤時(shí)，車(chē)輛在s=2.9m處逼近直線(xiàn)路徑，產(chǎn)生0.02m的反向誤差，在追蹤曲線(xiàn)路點(diǎn)時(shí)，第一段和第二段曲線(xiàn)路點(diǎn)最大誤差為0.08m和0.075m，平均絕對(duì)誤差為0.04m。并在回到之后直線(xiàn)路徑時(shí)，產(chǎn)生0.02m的反向誤差。兩者在追蹤直線(xiàn)誤差穩(wěn)定后平均絕對(duì)誤差均為0.01m。

6 結(jié)論

1） ?針對(duì)丘陵山區(qū)小型農(nóng)機(jī)無(wú)人作業(yè)改造難及改造成本高的問(wèn)題，完成了輕量化導(dǎo)航系統(tǒng)的控制主板和遠(yuǎn)程控制終端的設(shè)計(jì)，并將主板控制在13cm×15cm大小。

2） 根據(jù)丘陵山地田塊特點(diǎn)，提出了平滑轉(zhuǎn)向配合梭行法及結(jié)行法的路徑規(guī)劃策略，通過(guò)公式分析得出，該方法地頭面積更小，可適用于不同轉(zhuǎn)彎半徑的農(nóng)機(jī)。

3） 設(shè)計(jì)出了雙預(yù)瞄純追蹤模糊控制器并進(jìn)行仿真和試驗(yàn)，結(jié)果表明使用雙預(yù)瞄純追蹤模糊控制器追蹤直線(xiàn)路段時(shí)，在s=2.9m收斂到目標(biāo)點(diǎn)，相比于純追蹤控制器，提前了1.55m，且趨于穩(wěn)定后，追蹤的平均絕對(duì)誤差僅有0.01m。在進(jìn)入曲線(xiàn)路段時(shí)，追蹤第一段和第二段的誤差分別為0.08m和0.075m，相比于純追蹤控制器，分別減少了0.08m和0.035m，有效地提高了系統(tǒng)的收斂速度，實(shí)現(xiàn)了農(nóng)機(jī)對(duì)路徑的精確追蹤。

參 考 文 獻(xiàn)

［1］姬長(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.

［2］鐘銀， 薛夢(mèng)琦， 袁洪良. 智能農(nóng)機(jī)GNSS/INS組合導(dǎo)航系統(tǒng)設(shè)計(jì)［J］. 農(nóng)業(yè)工程學(xué)報(bào)， 2021， 37（9）： 40-46.

Zhong Yin， Xue Mengqi， Yuan Hongliang. Design of the GNSS/INS integrated navigation system for intelligent agricultural machinery ［J］. Transactions of the Chinese Society of Agricultural Engineering， 2021， 37（9）： 40-46.

［3］農(nóng)業(yè)農(nóng)村部農(nóng)業(yè)機(jī)械化管理司. 2021年全國(guó)農(nóng)業(yè)機(jī)械化統(tǒng)計(jì)年報(bào)［M］. 2022.

［4］黎永鍵， 趙祚喜， 黃培奎， 等. 基于CAN總線(xiàn)的拖拉機(jī)導(dǎo)航控制系統(tǒng)設(shè)計(jì)與試驗(yàn)［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2016， 47（S1）： 35-42.

Li Yongjian， Zhao Zuoxi， Huang Peikui， et al. Design and experiment of navigation control system for tractor based on CAN bus ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2016， 47（S1）： 35-42.

［5］李革， 王宇， 郭劉粉， 等. 插秧機(jī)導(dǎo)航路徑跟蹤改進(jìn)純追蹤算法［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2018， 49（5）： 21-26.

Li Ge， Wang Yu， Guo Liufen， et al. Improved pure pursuit algorithm for rice transplanter path tracking ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2018， 49（5）： 21-26.

［6］劉洋成， 耿端陽(yáng)， 蘭玉彬， 等. 基于自動(dòng)導(dǎo)航的農(nóng)業(yè)裝備全覆蓋路徑規(guī)劃研究進(jìn)展［J］. 中國(guó)農(nóng)機(jī)化學(xué)報(bào)， 2020， 41（11）： 185-192.

Liu Yangcheng， Geng Duanyang， Lan Yubin， et al. Research progress of agricultural equipment full coverage path planning based on automatic navigation ［J］. Journal of chinese Agricultural Mechanization， 2020， 41（11）： 185-192.

［7］柴善鵬， 姚立健， 徐麗君， 等. 基于動(dòng)態(tài)前視距離純追蹤模型的溫室農(nóng)機(jī)路徑跟蹤研究［J］. 中國(guó)農(nóng)機(jī)化學(xué)報(bào)， 2021， 42（11）： 58-64， 79.

Chai Shanpeng， Yao Lijian， Xu Lijun， et al. Research on greenhouse agricultural machinery path tracking based on dynamic look ahead distance pure pursuit model ［J］. Journal of Chinese Agricultural Mechanization， 2021， 42（11）： 58-64， 79.

［8］黎永鍵， 趙祚喜， 黃培奎， 等. 基于DGPS與雙閉環(huán)控制的拖拉機(jī)自動(dòng)導(dǎo)航系統(tǒng)［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2017， 48（2）： 11-19.

Li Yongjian， Zhao Zuoxi， Huang Peikui， et al. Automatic navigation system of tractor based on DGPS and double closedloop steering control ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2017， 48（2）： 11-19．

［9］王立輝， 石佳晨， 王樂(lè)剛， 等. 智能收獲機(jī)定位和自適應(yīng)路徑追蹤方法［J］. 導(dǎo)航定位學(xué)報(bào)， 2020， 8（6）： 29-36.

Wang Lihui， Shi Jiachen， Wang Legang， et al. A location and adaptive path tracking methods for intelligent harvesters ［J］. Journal of Navigation and Positioning， 2020， 8（6）： 29-36.

［10］刁勤晴， 張雅妮， 朱凌云. 雙預(yù)瞄點(diǎn)智能車(chē)大曲率路徑的橫縱向模糊控制［J］. 中國(guó)機(jī)械工程， 2019， 30（12）： 1445-1452.

Diao Qinqing， Zhang Yani， Zhu Lingyun. A lateral and longitudinal fuzzy control of intelligent vehicles with double preview points for large curvature roads ［J］. China Mechanical Engineering， 2019， 30（12）： 1445-1452.

［11］Ahn J， Shin S， Kim M， et al. Accurate path tracking by adjusting lookahead point in pure pursuit method ［J］. International journal of automotive technology， 2021， 22： 119-129.

［12］靳欣宇， 張軍， 劉元盛， 等. 基于Stanley算法的自適應(yīng)最優(yōu)預(yù)瞄模型研究［J］. 計(jì)算機(jī)工程， 2018， 44（7）： 42-46.

Jin Xinyu， Zhang Jun， Liu Yuansheng， et al. Research on adaptive optimal preview model based on Stanley algorithm ［J］．Computer Engineering， 2018， 44（7）： 42-46．

［13］周俊， 何永強(qiáng). 農(nóng)業(yè)機(jī)械導(dǎo)航路徑規(guī)劃研究進(jìn)展［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2021， 52（9）： 1-14.

Zhou Jun， He Yongqiang. Research progress on navigation path planning of agricultural machinery ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2021， 52（9）： 1-14.

［14］翟衛(wèi)欣， 王東旭， 陳智博， 等. 無(wú)人駕駛農(nóng)機(jī)自主作業(yè)路徑規(guī)劃方法［J］. 農(nóng)業(yè)工程學(xué)報(bào)， 2021， 37（16）： 1-7.

Zhai Weixin， Wang Dongxu， Chen Zhibo， et al. Autonomous operation path planning method for unmanned agricultural machinery ［J］. Transactions of the Chinese Society of Agricultural Engineering， 2021， 37（16）： 1-7.

［15］孟志軍， 劉卉， 王華， 等. 農(nóng)田作業(yè)機(jī)械路徑優(yōu)化方法［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2012， 43（6）： 147-152.

Meng Zhijun， Liu Hui， Wang Hua， et al. Optimal path planning for agricultural machinery ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2012， 43（6）： 147-152.

［16］劉剛， 康熙， 夏友祥， 等. 基于GNSS農(nóng)田平整全局路徑規(guī)劃方法與試驗(yàn)［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2018， 49（5）： 27-33.

Liu Gang， Kang Xi， Xia Youxiang， et al. Global path planning algorithm and experiment based on GNSScontrolled precise land leveling system ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2018， 49（5）： 27-33.

［17］Rahman M M， Ishii K， Noguchi N. Optimum harvesting area of convex and concave polygon field for path planning of robot combine harvester ［J］. Intelligent Service Robotics， 2019， 12： 167-179.

［18］Han X， Kim H J， Jeon C W， et al. Design and field testing of a polygonal paddy infield path planner for unmanned tillage operations ［J］. Computers and Electronics in Agriculture， 2021， 191： 106567.

［19］熊一鵬， 岳偉. FreeRTOS多任務(wù)調(diào)度機(jī)制在監(jiān)控單元中的應(yīng)用［J］. 單片機(jī)與嵌入式系統(tǒng)應(yīng)用， 2021， 21（9）： 64-66.

Xiong Yipeng， Yue Wei. Application of multitask scheduling mechanism in FreeRTOS system in monitoring unit ［J］. Microcontrollers & Embedded Systems， 2021， 21（9）： 64-66.

［20］高兆強(qiáng)， 王紹舉， 徐偉， 等. FATFS在嵌入式操作系統(tǒng)FreeRTOS中的移植與應(yīng)用［J］. 現(xiàn)代電子技術(shù)， 2017， 40（6）： 65-67， 70.

Gao Zhaoqiang， Wang Shaoju， Xu Wei， et al. Transplantation and application of FATFS in embedded operating system FreeRTOS ［J］. Modern Electronics Technique， 2017， 40（6）： 65-67， 70.

［21］Wang T， Wu Y， Liang J， et al. Analysis and experimental kinematics of a skidsteering wheeled robot based on a laser scanner sensor ［J］. Sensors， 2015， 15（5）： 9681-9702.