陳子文，熊揚凡，胡宗銳，李聰，龐有倫，楊明金※

（1. 西南大學(xué)工程技術(shù)學(xué)院，重慶 400715；2. 重慶工業(yè)職業(yè)技術(shù)學(xué)院機械工程與自動化學(xué)院，重慶 401120；3. 重慶市農(nóng)業(yè)科學(xué)院農(nóng)業(yè)機械化研究所，重慶 401329）

0 引言

農(nóng)業(yè)機械自動導(dǎo)航是精準(zhǔn)農(nóng)業(yè)的關(guān)鍵技術(shù)，可以極大減輕操作人員的勞動強度，同時提高作業(yè)質(zhì)量與作業(yè)效率[1-3]。底盤總成是移動式農(nóng)用動力機械的核心部件之一，在通用式底盤上搭載其他功能性農(nóng)具可實現(xiàn)多種農(nóng)業(yè)生產(chǎn)作業(yè)[4]。相較于普通輪式拖拉機，自走式履帶底盤體積小巧，擁有更低的接地比壓、滑轉(zhuǎn)率和滾動阻力系數(shù)，地形適應(yīng)能力廣 。尤其在復(fù)雜多變的山地和黏濕土壤的田間環(huán)境下，履帶式底盤表現(xiàn)出卓越的綜合性能[5]。

目前，農(nóng)機裝備自動導(dǎo)航研究主要集中在輪式拖拉機和電驅(qū)動力平臺，涉及的導(dǎo)航控制方法包括PID 控制[6]、模糊控制[7-8]、預(yù)瞄跟蹤控制[9]、非線性模型預(yù)測控制[10]、基于位姿偏差的轉(zhuǎn)向角線性控制[11-12]、基于運動特性的導(dǎo)航控制[13]以及反步滑動模式控制[14]等。然而，針對低成本、技術(shù)成熟，采用轉(zhuǎn)向離合器進(jìn)行轉(zhuǎn)向的油動機械換擋履帶底盤的導(dǎo)航系統(tǒng)研究較少。ZHANG等[15]針對單缸柴油履帶車導(dǎo)航提出一種基于虛擬阿克曼轉(zhuǎn)向模型的狀態(tài)反饋導(dǎo)航控制系統(tǒng)，并設(shè)計了基于脈寬調(diào)制原理的前向比例控制方法，在果園環(huán)境中的平均跟蹤誤差為5.1 cm，跟蹤誤差標(biāo)準(zhǔn)差為8.4 cm。丁幼春等[16]針對搭載油菜播種機的履帶底盤設(shè)計了免疫PID 導(dǎo)航控制器，當(dāng)行駛速度為0.5 m/s 時，其平均導(dǎo)航偏差為4.2 cm，最大偏差為11.9 cm。劉志杰等[17]針對小型履帶拖拉機，通過搭載組合導(dǎo)航系統(tǒng)，并利用神經(jīng)網(wǎng)絡(luò)提出了一種虛擬雷達(dá)模型的路徑跟蹤控制算法，當(dāng)行駛速度為0.36、0.75 m/s 時，平均橫向偏差分別為3.1、5.1 cm。吳才聰?shù)萚18]針對履帶底盤提出模糊自適應(yīng)純追蹤控制方法，水泥路面試驗表明，當(dāng)行駛速度為0.8 m/s 時，模糊自適應(yīng)純追蹤控制器最大跟蹤偏差為3.9 cm，平均絕對偏差為1.8 cm。

在農(nóng)業(yè)作業(yè)場景中，應(yīng)用于此類農(nóng)用動力機械的路徑跟蹤算法必須滿足較高的精度要求。例如，在播種、施肥和噴藥等環(huán)節(jié)，精準(zhǔn)的路徑跟蹤有助于減少重復(fù)覆蓋和遺漏區(qū)域，從而提高作業(yè)效率[19]。同時，較高的行駛速度也可縮減作業(yè)周期，以適應(yīng)農(nóng)業(yè)生產(chǎn)的季節(jié)性要求[20]。但過高的速度會導(dǎo)致導(dǎo)航系統(tǒng)的精度和穩(wěn)定性降低，因此要求跟蹤算法在保證導(dǎo)航精度的要求下盡可能提高行駛速度。另一方面，在實際農(nóng)業(yè)作業(yè)中，自動導(dǎo)航系統(tǒng)需在較短時間內(nèi)上線并開始跟蹤期望路徑。若上線距離過長，不但會降低作業(yè)效率，甚至影響整個農(nóng)業(yè)生產(chǎn)過程。然而，油動換擋履帶底盤難以進(jìn)行精準(zhǔn)的速度控制，其速度控制只能通過調(diào)整油門和機械檔位實現(xiàn)，速度滯后較嚴(yán)重，同時由于各側(cè)履帶無法單獨速度控制，因此只能進(jìn)行單一模式轉(zhuǎn)向。值得關(guān)注的是，在離合制動式履帶平臺上，每一次的轉(zhuǎn)向動作，尤其在直線跟蹤過程中，都會對車輛造成一次剛性沖擊。這主要是由于轉(zhuǎn)向時，履帶平臺的單側(cè)履帶輪鎖死，從而導(dǎo)致平臺速度瞬間改變造成的。頻繁轉(zhuǎn)向會導(dǎo)致搭載在平臺上的功能性農(nóng)具的作業(yè)精度下降，甚至無法正常工作；并且過于頻繁的糾偏可能會對農(nóng)具或履帶平臺自身造成損壞。故此類履帶平臺在農(nóng)業(yè)作業(yè)中并不適宜頻繁轉(zhuǎn)向糾偏。目前的各類路徑跟蹤算法并未充分考慮車輛在跟蹤過程中的糾偏次數(shù)，尤其是針對離合制動式履帶平臺的算法，因此，在設(shè)計適用于此類平臺的跟蹤算法時，既需要嚴(yán)格限制糾偏次數(shù)，又要確保跟蹤精度得到滿足。這一原則有助于提高農(nóng)業(yè)作業(yè)效率，減少搭載農(nóng)具的精度損失和避免農(nóng)具及履帶平臺的損壞。

本文以3WZF-400 汽油驅(qū)動履帶底盤為研究對象，提出一種虛擬探照燈尋徑路徑跟蹤方法，通過仿真和導(dǎo)航試驗對系統(tǒng)和算法性能進(jìn)行驗證，該方法綜合考慮離合制動式履帶平臺在跟蹤過程中的糾偏次數(shù)限制和跟蹤精度需求，旨在為此類平臺提供一種高效、穩(wěn)定的導(dǎo)航解決方案。

1 材料與方法

1.1 離合制動式履帶底盤構(gòu)成

本文以3WZF-400 汽油驅(qū)動履帶底盤（蘇州博田自動化技術(shù)有限公司生產(chǎn)）為研究對象搭建自動導(dǎo)航系統(tǒng)，如圖1 所示，主要由履帶總成、傳動系統(tǒng)和操縱系統(tǒng)組成。履帶車總長1.2 m，寬0.9 m，橡膠履帶寬0.2 m，動力由5.4 kW 單缸汽油機提供，該履帶底盤體積小，對各種果園、田地道路條件有較強適應(yīng)性。

圖1 離合制動式履帶底盤系統(tǒng)組成Fig.1 Composition of clutch-brake track chassis

履帶底盤動力傳動與轉(zhuǎn)向系統(tǒng)如圖2 所示。發(fā)動機將動力傳遞給手動換擋變速箱，離合器手柄可控制動力離合器的閉合，將動力傳遞給驅(qū)動橋，左右兩個驅(qū)動輪前端均設(shè)有常閉離合器，通過轉(zhuǎn)向手閘軋線控制離合器打開，切斷動力傳輸，實現(xiàn)停機或轉(zhuǎn)向。由于該類動力平臺通過控制離合器閉合，實現(xiàn)車輛制動或轉(zhuǎn)向，故將此類結(jié)構(gòu)和操控方式稱為離合制動式履帶底盤。為實現(xiàn)履帶底盤自動操控，對手動操控系統(tǒng)進(jìn)行升級，采用氣動系統(tǒng)代替手動操作，應(yīng)用3 組獨立控制的雙作用氣缸拉動軋線控制3個離合器的閉合，實現(xiàn)啟停、直線行駛、轉(zhuǎn)向等動作[21]。

圖2 履帶底盤轉(zhuǎn)向結(jié)構(gòu)示意圖Fig.2 Steering mechanism diagram of track chassis

1.2 導(dǎo)航控制系統(tǒng)架構(gòu)

導(dǎo)航控制系統(tǒng)采用以ARM Cortex M4 為內(nèi)核的STM30F303 VCT6 單片機作為中控器，數(shù)據(jù)接收、數(shù)據(jù)解析、運算處理等任務(wù)均由該單片機完成。全球衛(wèi)星導(dǎo)航系統(tǒng)（global navigation satellite system，GNSS）采用上海華測導(dǎo)航技術(shù)股份有限公司的P3-DU 北斗高精度定位測向接收機，搭配有載波相位差分技術(shù)（real-time kinematic，RTK），多系統(tǒng)雙頻，支持雙天線定位測向解算，在裝備有移動站接收機的基礎(chǔ)上，通過搭建基站接收機和無線電臺，實現(xiàn)RTK-GNSS 高精度定位數(shù)據(jù)的接收。導(dǎo)航控制系統(tǒng)整體架構(gòu)如圖3 所示，RTK-GNSS定位模塊利用無線電臺接收差分信號，經(jīng)差分系統(tǒng)校準(zhǔn)后，中控器采取通用異步串行通信（universal asynchronous receiver/transmitter，UART）方式獲取高精度定位數(shù)據(jù)。將解析數(shù)據(jù)與設(shè)定的期望路徑進(jìn)行比較，利用偏差跟蹤算法得到預(yù)期駕駛動作，通過控制執(zhí)行機構(gòu)以達(dá)到路徑跟蹤的目的。上位機可以監(jiān)控導(dǎo)航控制系統(tǒng)的工作狀態(tài)，其與中控器之間采用UART 方式并通過ZigBee 無線模塊進(jìn)行數(shù)據(jù)的相互傳輸。中控器將解析后的定位信息以及導(dǎo)航過程中所產(chǎn)生的各項導(dǎo)航參數(shù)反饋給上位機，在系統(tǒng)發(fā)生異常時可通過發(fā)送相關(guān)指令終止導(dǎo)航作業(yè)。在本研究中，選擇ZigBee 模塊主要考慮到其成本低、體積小、使用方便等優(yōu)勢，有利于在短時間內(nèi)實現(xiàn)系統(tǒng)原型的搭建與初步測試。

圖3 導(dǎo)航控制系統(tǒng)架構(gòu)Fig.3 Structure of navigation control system

2 路徑跟蹤控制方法

2.1 履帶底盤運動學(xué)模型

根據(jù)離合制動式履帶底盤的工作原理，只能依靠單側(cè)履帶輪完全制動來實現(xiàn)轉(zhuǎn)向，即轉(zhuǎn)向時內(nèi)側(cè)履帶的驅(qū)動輪角速度為零，外側(cè)履帶輪角速度不變，且履帶底盤作業(yè)過程中驅(qū)動輪的速度無法隨意調(diào)節(jié)，在導(dǎo)航平面坐標(biāo)系X-O-Y（n系）中建立履帶車運動學(xué)模型，如圖4 所示，G(x,y)為履帶車幾何中心坐標(biāo)?？紤]到農(nóng)業(yè)作業(yè)中一般行駛速度較低，可做如下假設(shè)：1）履帶底盤質(zhì)心與幾何中心重合；2）履帶底盤在作業(yè)過程中非制動時履帶的帶速恒定；3）忽略雙側(cè)履帶的滑移和滑轉(zhuǎn) 。

圖4 離合制動式履帶底盤運動學(xué)模型Fig.4 Kinematic model of clutch-brake track chassis

定義履帶底盤的航向角ψ為Y軸正方向順時針旋轉(zhuǎn)至速度v的角度，轉(zhuǎn)向角φ的大小與轉(zhuǎn)向?qū)?yīng)的圓心角相等，其值與轉(zhuǎn)向持續(xù)時間有關(guān)。根據(jù)運動學(xué)原理有：

式中rl表示左側(cè)履帶幾何中心相對于旋轉(zhuǎn)中心o的距離，m。由于右轉(zhuǎn)時右側(cè)履帶完全制動，即：

將式（5）帶入式（3）可得：

由式（6）可看出，履帶底盤的旋轉(zhuǎn)中心位于履帶的幾何中心處，車輛進(jìn)行轉(zhuǎn)向時，旋轉(zhuǎn)中心在左右兩側(cè)履帶的幾何中心之間切換，故兩側(cè)履帶可獨立控制轉(zhuǎn)速和轉(zhuǎn)向的差速模型不適用于此類履帶底盤。

2.2 虛擬探照燈尋徑跟蹤算法原理

路徑跟蹤的實質(zhì)是通過控制車輛的運動減少車輛與期望路徑之間的偏差，在農(nóng)機導(dǎo)航作業(yè)中，作業(yè)路徑通常由直線段構(gòu)成。為提高作業(yè)質(zhì)量和效率，要求農(nóng)機自動導(dǎo)航系統(tǒng)上線快速穩(wěn)定、直線跟蹤精度高以及對顛簸復(fù)雜路面抗干擾性好[22]。目前大多數(shù)跟蹤算法按照某種控制策略計算車輛的前輪期望轉(zhuǎn)角，從而通過改變前輪轉(zhuǎn)角控制車輛運動軌跡的曲率，如純跟蹤算法[23]、Stanley 算法[24]、MPC 算法[25-26]。對于差速轉(zhuǎn)向的履帶底盤，可以通過改變雙側(cè)履帶的速度調(diào)整履帶底盤運動軌跡的曲率，所以可利用轉(zhuǎn)換輪式車輛的跟蹤算法實現(xiàn)對目標(biāo)路徑的跟蹤控制。而對于離合制動式履帶底盤，雙側(cè)履帶的帶速不可隨意調(diào)節(jié)，同時受到速度傳感器缺失和路面環(huán)境等因素影響，利用轉(zhuǎn)向時間間接獲取期望轉(zhuǎn)向角會產(chǎn)生較大誤差。此外，為避免履帶底盤在跟蹤過程中頻繁轉(zhuǎn)向糾偏導(dǎo)致的作業(yè)精度下降、農(nóng)具以及履帶底盤自身損壞，應(yīng)對糾偏次數(shù)嚴(yán)格限定。

本文提出的虛擬探照燈尋徑跟蹤（virtual searchlight pathfinding tracking，VSPT）算法假設(shè)履帶底盤搭載一種光線射程為無限遠(yuǎn)的虛擬探照燈，并設(shè)定一個位于期望路徑及其延長線上且不斷運動的假想目標(biāo)點C，通過模擬探照燈搜索物體的方式建立履帶底盤不斷追逐目標(biāo)點的路徑跟蹤方法。假定該虛擬探照燈安裝于履帶底盤幾何中心處，其視域角為γ，且光束投射方向與履帶底盤航向一致。圖5 所示，若目標(biāo)點C位于虛擬探照燈視域內(nèi)，則履帶底盤直線前進(jìn)，反之，目標(biāo)點C位于虛擬探照燈視域范圍外，履帶底盤通過轉(zhuǎn)向?qū)ふ以撃繕?biāo)點并使其位于視域內(nèi)。

圖5 基于虛擬探照燈的履帶底盤轉(zhuǎn)向示意圖Fig.5 Track chassis steering diagram based on virtual searchlight

設(shè)履帶底盤導(dǎo)航路徑坐標(biāo)點序列A1，A2，···，Ai，···，An，如 圖6 所示，以AiAi+1為期望路徑進(jìn)行分析，視域角的大小影響導(dǎo)航作業(yè)的穩(wěn)定性，當(dāng)履帶底盤逐漸靠近期望路徑時，視域角γ不變，由于視場變小，則視域內(nèi)可見的期望路徑變短，類比于真實場景中駕駛員視野變窄，此時算法對橫向偏差極度敏感，將不斷調(diào)整航向?qū)е孪到y(tǒng)振蕩。同時在橫向偏差較小的情況下，若視域角γ過大，也會導(dǎo)致導(dǎo)航誤差增加，系統(tǒng)穩(wěn)定性下降。故本文提出一種基于橫向偏差指數(shù)的視域角動態(tài)變化方法，視域角反比于橫向偏差d的λ次方，指數(shù)λ可用于調(diào)節(jié)視域角的變化速率，具體計算式為

圖6 路徑跟蹤模型Fig.6 Path tracking model

式中k1表示視域增益，rad·m；d表示橫向偏差，m；λ表示橫向偏差指數(shù)。

規(guī)定目標(biāo)點始終于車輛前方，即位于直線QAi+1及履帶底盤幾何中心G在期望路徑AiAi+1上的投影點Q向Ai+1點方向的延長線上。目標(biāo)點C的位置控制履帶底盤向期望路徑靠近的迫切程度。低速行駛時履帶底盤目標(biāo)點靠近當(dāng)前位置，履帶底盤應(yīng)盡快接近期望路徑，高速行駛時則應(yīng)避免很小的橫向偏差引起跟蹤振蕩。因此將直線QC的長度m與履帶底盤行駛速度之間設(shè)置為正比例關(guān)系，計算式為

式中k2表示目標(biāo)增益，s-1。

若直接利用點C坐標(biāo)與視域方程判斷目標(biāo)點C是否位于視域范圍內(nèi)，則運算過程較為復(fù)雜繁瑣。為簡化計算過程，本文采用期望航向偏差角α作為中間參數(shù)進(jìn)行求解，α可表示為

由式（9）可知，α與橫向偏差有關(guān)，d愈大，α愈趨近于0.5π，反之，α趨近于0。

如圖6 所示，若位于G點與G'點時履帶底盤參數(shù)ψ，θi，α，d等相等，則β絕對值相等。而G點位于有向線段AiAi+1左側(cè)，G'點位于右側(cè)，顯然絕對值相等的β不能夠準(zhǔn)確描述上述情況。為賦予β更多物理含義，定義直線DE的長度為履帶平臺的視野范圍，若視野范圍大于0，則航向偏差角β為正值，反之，航向偏差角β為負(fù)值。

履帶底盤航向與假想目標(biāo)點之間的夾角δ可直接指導(dǎo)履帶底盤的駕駛動作，根據(jù)α和β的取值，可通過VSPT 算法進(jìn)行求解。為保證履帶底盤通過轉(zhuǎn)向搜尋目標(biāo)點時路徑最短，即轉(zhuǎn)角最小，δ絕對值應(yīng)小于π，且轉(zhuǎn)彎方向總是朝著δ絕對值逐漸減小的方向。若δ為正，目標(biāo)點C位于履帶底盤右側(cè)，需向右轉(zhuǎn)向；若δ為負(fù)，則目標(biāo)點C位于履帶底盤左側(cè)，向左轉(zhuǎn)向。

2.3 基于數(shù)據(jù)流圖的軟件設(shè)計及算法實現(xiàn)

數(shù)據(jù)流圖（data flow diagram，DFD）是描述系統(tǒng)信息流以及數(shù)據(jù)變換流程的圖形[27]，具有描述系統(tǒng)邏輯結(jié)構(gòu)的能力[28]。建立的DFD 如圖7 所示，數(shù)據(jù)源包括上位機、RTK-GNSS 定位系統(tǒng)以及空閑中斷服務(wù)函數(shù)。上位機提供用戶預(yù)設(shè)的基于WGS84 坐標(biāo)系（e系）下的期望路徑坐標(biāo)序列和原點坐標(biāo)，系統(tǒng)將兩者做坐標(biāo)轉(zhuǎn)換[26]，得到n系下的期望路徑坐標(biāo)。RTK-GNSS 定位系統(tǒng)通過標(biāo)準(zhǔn)串口實時輸出不定長數(shù)據(jù)。為解決不定長數(shù)據(jù)穩(wěn)定接收同時提高系統(tǒng)工作效率，采用直接存儲器訪問（direct memory access，DMA）方式，結(jié)合串口空閑中斷和循環(huán)隊列策略進(jìn)行數(shù)據(jù)接收，通過數(shù)據(jù)解析以及坐標(biāo)轉(zhuǎn)換得到履帶底盤航向、速度以及n系下的定位坐標(biāo)。將數(shù)據(jù)傳入VSPT 算法后解算出行駛動作并傳遞給執(zhí)行機構(gòu)，從而完成履帶底盤的自動駕駛。

圖7 數(shù)據(jù)流圖Fig.7 Data flow diagram

通過DMA 傳輸，結(jié)合空閑中斷與循環(huán)隊列的數(shù)據(jù)接收方式的優(yōu)勢在于系統(tǒng)進(jìn)行工作運算的同時能夠進(jìn)行數(shù)據(jù)的接收。每次進(jìn)行數(shù)據(jù)提取時均能捕捉到最新序列的數(shù)據(jù)幀，提高了定位數(shù)據(jù)與實際位置的吻合度；省略了CPU 數(shù)據(jù)接收流程，提高了系統(tǒng)運算的效率和穩(wěn)定性。

3 仿真與現(xiàn)場試驗

3.1 評價指標(biāo)

為精確評價路徑跟蹤算法的導(dǎo)航效果，將履帶底盤跟蹤期望路徑的過程以上線點為界限分為上線過程以及直線跟蹤過程，如圖8 所示。起始點作為履帶底盤的初始位置，上線點指橫向偏差小于3.0 cm 同時航向偏差小于2.0°且與起始點相距最近的采樣點。上線過程采用上線距離作為評價指標(biāo)；直線跟蹤過程采用橫向偏差、航向偏差均值、標(biāo)準(zhǔn)差和均方根以及糾偏次數(shù)作為評價指標(biāo)。

圖8 上線過程與直線跟蹤過程Fig.8 On-line process and straight-line tracking process

上線距離為從起始點到上線點的實際軌跡在期望路徑上的投影長度，表征系統(tǒng)在偏離期望路徑時的響應(yīng)速度。

在國外，19世紀(jì)40～60年代，牛津大學(xué)使用“推廣”一詞指大學(xué)的推廣活動，即建議；而到19世紀(jì)70年代，劍橋大學(xué)將校外開展的教育活動稱之為“推廣教育”。在本世紀(jì)初，美國建立了贈地學(xué)院和合作推廣機構(gòu)，開始真正使用“農(nóng)業(yè)推廣”這一表述。1915年，美國《史密斯—來弗聯(lián)邦推廣法》頒布實施，使得農(nóng)業(yè)推廣法律化，同時也給“農(nóng)業(yè)推廣”賦予新的意義。

橫向偏差均值ldm為直線跟蹤過程中履帶平臺幾何中心位置采樣點距期望路徑橫向偏差d絕對值的均值，可表示為

式中下標(biāo)j表示采樣點序號；q表示采樣點總數(shù)；(xj,yj)表示履帶底盤在平面坐標(biāo)系下的幾何中心坐標(biāo)，m；Ai(Xi,Yi)，Ai+1(Xi+1,Yi+1)表示期望路徑兩端點在平面坐標(biāo)系下的坐標(biāo)，m。

航向偏差均值cdm為直線跟蹤過程中履帶平臺航向角ψ與θi偏差絕對值的平均值，可表示為

糾偏次數(shù)指上線后導(dǎo)航系統(tǒng)自動控制履帶底盤左、右轉(zhuǎn)向的次數(shù)，衡量算法對直線跟蹤的振蕩情況。履帶底盤在直線跟蹤中頻繁左、右轉(zhuǎn)向會導(dǎo)致車體劇烈振動，從而影響定位精度，延長導(dǎo)航時間，無法滿足作業(yè)平穩(wěn)性的需求；同時車體頻繁振蕩對機械、電機結(jié)構(gòu)的耐久性均產(chǎn)生影響。試驗中動態(tài)采集履帶底盤轉(zhuǎn)向控制信號并傳輸給上位機，采樣頻率5 Hz。如圖9，單次試驗過程中采集150個控制信號，圖中數(shù)據(jù)點代表控制器發(fā)出的右轉(zhuǎn)、左轉(zhuǎn)或直行控制信號。圖中虛線為上線時刻所對應(yīng)的控制數(shù)據(jù)序列的位置，糾偏次數(shù)為上線之后的轉(zhuǎn)向次數(shù)。由于履帶平臺轉(zhuǎn)向控制響應(yīng)具有滯后性，經(jīng)試驗驗證，單幀控制數(shù)據(jù)無法使履帶平臺轉(zhuǎn)向，判定為控制噪聲，3 幀及以上可作為判斷轉(zhuǎn)向的依據(jù)。因而定義一次糾偏為上位機接收連續(xù)3 幀及以上的轉(zhuǎn)向數(shù)據(jù)。試驗中150個控制數(shù)據(jù)點中發(fā)生了3 次糾偏。

圖9 糾偏次數(shù)統(tǒng)計Fig.9 Statistical of deviation correction

3.2 仿真試驗

為驗證虛擬探照燈尋徑跟蹤算法的導(dǎo)航效果并對指數(shù)λ、視域增益k1、目標(biāo)增益k2進(jìn)行標(biāo)定，利用MATLAB對其進(jìn)行仿真試驗。設(shè)定仿真過程中的路徑起始點坐標(biāo)為(0,0)，終點坐標(biāo)為(10 m,10 m)，履帶底盤的初始橫向偏差為0.5 m，初始航向偏差角β為25°，采樣時間為0.2 s，假設(shè)雙側(cè)履帶在非制動情況時速度恒定，為0.4 m/s，履帶軸線間距b=0.9 m。取初始k1=0.005 rad·m，k2=6.0 s-1進(jìn)行仿真試驗，試驗結(jié)果如表1。通過對比導(dǎo)航效果（試驗號1～5），選取較優(yōu)值λ=1/4。

表1 路徑跟蹤仿真試驗結(jié)果Table 1 Path-tracking simulation test results

仿真試驗發(fā)現(xiàn)，控制參數(shù)k1、k2的取值對導(dǎo)航效果有重要影響。因此在λ取值確定后，需初步評估k1、k2的合理值，為現(xiàn)場試驗提供依據(jù)。圖10 為上線過程和直線行駛過程的仿真軌跡。根據(jù)圖10 和表1 可知，視域增益k1對上線距離影響較小，但對直線跟蹤過程中l(wèi)dm的大小有重要影響，在合理范圍內(nèi)視域增益k1增大則ldm變小。目標(biāo)增益k2對上線距離有重要影響，隨著k2的增大，上線距離先減小后增大，這是由于過大的k2會引起較大的超調(diào)，降低履帶底盤的上線性能，且k2越小軌跡越平滑，反之愈發(fā)激進(jìn)。仿真結(jié)果表明：虛擬探照燈尋徑的跟蹤算法具有較高的跟蹤精度，能夠滿足導(dǎo)航要求。通過比較導(dǎo)航效果，最終選取λ的較優(yōu)值為1/4，k1的較優(yōu)值為0.005 rad·m，k2為6.0 s-1。

圖10 路徑跟蹤仿真軌跡Fig.10 Path tracking simulation trajectory

3.3 現(xiàn)場試驗

3.3.1 試驗方法

本試驗旨在探究并優(yōu)化VSPT 算法對于直線跟蹤的性能。試驗在西南大學(xué)內(nèi)水泥路和試驗田土路開展，試驗時間為2022 年12 月8 日至9 日。利用華測高精度RTK-GNSS 系統(tǒng)（數(shù)據(jù)更新頻率為5 Hz）采集試驗場地中兩點坐標(biāo)，并將其連線作為期望路徑，設(shè)置其中一點作為當(dāng)?shù)厮阶鴺?biāo)系下的坐標(biāo)原點，導(dǎo)航系統(tǒng)運行時控制器將經(jīng)緯度轉(zhuǎn)換為n系下的坐標(biāo)，并實時向上位機發(fā)送履帶底盤的當(dāng)前坐標(biāo)、橫向偏差、航向偏差等導(dǎo)航參數(shù)，以控制履帶底盤導(dǎo)航運行。

考慮到作業(yè)環(huán)境以及作業(yè)參數(shù)的復(fù)雜性，為全面驗證本文提出算法的導(dǎo)航效果，首先進(jìn)行算法參數(shù)驗證，然后針對不同初始參數(shù)、運動參數(shù)和路況情況進(jìn)行對比試驗。算法參數(shù)驗證試驗以仿真結(jié)果為依據(jù)，通過實地試驗驗證導(dǎo)航效果。初始參數(shù)試驗通過設(shè)置履帶平臺不同初始位姿，包括初始橫向偏差和初始航向偏差，驗證導(dǎo)航算法對不同初始狀態(tài)的導(dǎo)航效果，試驗中選取初始橫向偏差分別為0.5、1.0 和1.5 m，初始航向偏差角選取±25°。運動參數(shù)試驗為不同運動速度下的導(dǎo)航試驗，旨在驗證算法對不同速度的適應(yīng)性，并分析速度對導(dǎo)航效果的影響，選取低速（1 檔）、中速（2 檔）、高速（3 擋）3 種檔位，對應(yīng)的實測速度分別為0.4、0.8 和1.2 m/s。考慮到履帶底盤的應(yīng)用場景，選擇水泥路面和具有雜草覆蓋的土路進(jìn)行路況對比試驗。

3.3.2 算法參數(shù)可行性驗證

利用上文仿真試驗所標(biāo)定的算法參數(shù)進(jìn)行現(xiàn)場實驗，水泥路面采用低速1 檔，設(shè)置初始橫向偏差0.5 m 以及初始航向偏差角25°，取控制參數(shù)k1=0.005 rad·m、k2=6.0 s-1、λ=1/4，該組試驗重復(fù)5 次，結(jié)果取均值，結(jié)果見表2 中試驗號1。

表2 現(xiàn)場試驗結(jié)果Table 2 Field test result

較仿真結(jié)果相比，現(xiàn)場試驗各項評價指標(biāo)均有所增加，上線距離增大0.52 m，橫向和航向偏差絕對值均值分別增大0.35 cm 和1.34°。分析其主要原因為仿真運動學(xué)模型中未考慮履帶底盤的滑移、滑轉(zhuǎn)以及定位系統(tǒng)的噪聲等誤差，同時執(zhí)行機構(gòu)存在滯后現(xiàn)象。根據(jù)現(xiàn)場試驗結(jié)果，仿真獲取的控制參數(shù)可實現(xiàn)較好的導(dǎo)航效果，實際上線距離為1.44 m，橫偏均值為0.41 cm，且航偏均值為1.81°。

3.3.3 初始位姿參數(shù)導(dǎo)航試驗

該試驗探究在不同初始位姿參數(shù)下算法的導(dǎo)航效果，試驗在水泥路面上進(jìn)行，以低速1 檔速度運動，設(shè)置3水平初始橫向偏距因素和2 水平航向偏距因素，初始航向偏差±25°，表示初始航向角相比期望路徑的航向角偏離25°，±代表航向偏差的方向，+表示車頭朝向期望路徑，-表示車頭背向期望路徑，每組試驗重復(fù)5 次，結(jié)果取均值，試驗結(jié)果見表2 中試驗序號1～6。

試驗結(jié)果表明，在不同初始位姿下VSPT 算法均能達(dá)到較好的導(dǎo)航精度，平均上線距離為1.64 m，直線跟蹤過程中橫向偏差絕對值的平均值、標(biāo)準(zhǔn)差和均方根的平均值分別為0.44、0.31 和0.56 cm，航向偏差絕對值的平均值、標(biāo)準(zhǔn)差和均方根分別為1.57°、0.55°和1.58°，平均糾偏次數(shù)為3.5 次。在保證導(dǎo)航精度和導(dǎo)航效率的前提下，算法可允許合理范圍內(nèi)的波動。

圖11 為不同初始參數(shù)導(dǎo)航效果對比，可見，初始橫向偏差增大導(dǎo)致上線距離略微增加，初始航偏設(shè)置為-25°時，車輛調(diào)整航向所需時間更長。如圖11a、11b 所示，在不同初始位姿下，橫向偏差與航向偏差都能被快速消除，在初始橫偏為1.5 m 的情況下，上線距離小于2 m。由圖11c、11 d 可看出，履帶平臺的上線過程中，無超調(diào)量，振蕩較小。履帶底盤在上線和直線跟蹤過程中軌跡不夠平滑主要由履帶底盤的運動特性導(dǎo)致，離合制動式履帶平臺無法實現(xiàn)平滑轉(zhuǎn)向，履帶底盤分兩步完成上線，分別是快速接近和矯正過程。

圖11 不同初始位姿參數(shù)的試驗結(jié)果Fig.11 Test results of different initial posture parameters

由圖11b 可見，航向偏差出現(xiàn)最大值前均屬于快速接近過程，之后航向偏差逐漸減少到零點附近，這個過程是航向偏差的矯正過程，故在圖11c 中上線過程軌跡會出現(xiàn)折點，后續(xù)試驗發(fā)現(xiàn)，降低目標(biāo)增益k2，可使折點處更加平滑，但會明顯增加上線距離，降低直線跟蹤效率。直線跟蹤過程波動較小，出現(xiàn)的波動主要由于機器震動所導(dǎo)致差分導(dǎo)航系統(tǒng)信號跳動引起。

3.3.4 運動參數(shù)導(dǎo)航試驗

為驗證該算法在履帶底盤不同速度下的導(dǎo)航效果，進(jìn)行不同檔位下3個速度水平的單因素試驗，試驗中初始橫向偏距為0.5 m，初始航向偏差為25°，試驗在水泥路面上進(jìn)行。試驗結(jié)果見表2 中試驗號7～11，以1 號試驗作為對照組。1 號和7 號試驗除了速度外，其余參數(shù)均相同，對比可知，提高速度使上線距離略微減小，但導(dǎo)航精度明顯下降，橫向偏差和航向偏差均值相比低速分別增大156.10%、11.05%，且糾偏次數(shù)增大到23 次。由圖12 中可明顯看出直線跟蹤過程中的振蕩情況。此時提高k1降低k2（見表2 中試驗號8）可得到處于2 檔速度下的較優(yōu)控制參數(shù)k1=0.010 rad·m、k2=6.0 s-1，能有效減小上線后的糾偏次數(shù)且能減少跟蹤誤差，從而提高導(dǎo)航精度。若進(jìn)一步提高k1降低k2（見表2 試驗序號9），將犧牲部分導(dǎo)航精度，進(jìn)一步降低糾偏次數(shù)，以減少系統(tǒng)振蕩?？梢妅2對上線性能有明顯影響，提高k2可明顯減少上線距離，這與仿真結(jié)果一致。在高速3 擋（1.2 m/s）下，取k1=0.015 rad·m、k2=4.0 s-1可獲得更好的導(dǎo)航效果。對試驗號1、8、11 的試驗結(jié)果求平均，可得平均上線距離為1.54 m，橫向偏差絕對值的均值、標(biāo)準(zhǔn)差和均方根分別為0.73、0.57 和0.97 cm，航向偏差絕對值的均值、標(biāo)準(zhǔn)差和均方根分別為1.47°、0.79°和1.59°，平均糾偏次數(shù)為4.7。

圖12 運動參數(shù)變化試驗結(jié)果Fig.12 Test results of variation of motion parameter

試驗結(jié)果表明，隨履帶底盤的速度提升VSPT 算法導(dǎo)航效果變差，但從整體上看，在不同速度情況下若選取相匹配的較優(yōu)控制參數(shù)，仍有較好的導(dǎo)航精度與穩(wěn)定性，控制參數(shù)k1、k2與履帶底盤的運行速度有強相關(guān)性。

3.3.5 路況對比試驗

為驗證VSPT 算法對不同路況的適應(yīng)能力，選取試驗田土路面與水泥路面進(jìn)行對比試驗，試驗現(xiàn)場如圖13所示，試驗結(jié)果如表2 中試驗號12～15。

圖13 現(xiàn)場試驗路況Fig.13 Road condition of field test

試驗中初始航向偏差為25°，初始橫向偏距為0.5 m，λ=1/4，速度為0.4 m/s 時，選用仿真所得較優(yōu)值k1=0.005 rad·m、k2=6.0 s-1。水泥路面試驗以試驗號1、8、11 為代表，控制初始位姿不變，保證速度擋位、k1以及k2各水平對應(yīng)，以表2 中試驗號12、14、15 的數(shù)據(jù)進(jìn)行對比（平均上線距離為1.85 m，橫向偏差絕對值的均值、標(biāo)準(zhǔn)差和均方根分別為0.82、0.57 偏差絕對值的均值、標(biāo)準(zhǔn)差和0.98 cm，航向偏差絕對值的均值、標(biāo)準(zhǔn)差和均方根分別為1.35°、0.86°和1.55°，平均糾偏次數(shù)為2.7 次），土路面導(dǎo)航結(jié)果與水泥路面相似，在0.8 m/s速度下，k1和k2的較優(yōu)值為0.010 rad·m、k2=5.0 s-1，此時可獲得較好的導(dǎo)航效果。因此，算法參數(shù)的取值不受到路況的影響，僅與運行速度有關(guān)。2 種路況下，由3種速度下平均導(dǎo)航指標(biāo)數(shù)據(jù)可見，土路面相比水泥路面上線距離增大0.31 m，直線跟蹤過程中平均橫向和航向偏差區(qū)別不大，平均糾偏次數(shù)減少2 次。主要原因是土路面具有更高的路面附著系數(shù)，轉(zhuǎn)向更平穩(wěn)，較少出現(xiàn)過渡轉(zhuǎn)向情況，故糾偏次數(shù)和航向偏差均有所降低?？傮w來看，VSPT 算法適用土路面的導(dǎo)航工況，具有良好的魯棒性，能夠滿足農(nóng)機導(dǎo)航作業(yè)需求。后續(xù)將深入探索不同速度感知下，k1和k2自動獲取和匹配，為實現(xiàn)最佳導(dǎo)航效果提供便捷的控制算法。

本研究提出的VSPT 路徑跟蹤算法依據(jù)離合制動式履帶底盤的運動特性，主要用于直線路徑跟蹤，該類底盤缺點是無法實現(xiàn)轉(zhuǎn)彎半徑可變，優(yōu)點是轉(zhuǎn)彎半徑小，因此更適用于折角或原地掉頭的轉(zhuǎn)向方式。VSPT 算法針對直線路徑上的目標(biāo)點進(jìn)行判定，曲線跟蹤需要進(jìn)行適當(dāng)修正和參數(shù)調(diào)整，后續(xù)將在阿克曼轉(zhuǎn)向和差速轉(zhuǎn)向底盤上深入開展。

4 結(jié)論

1）提出離合制動式履帶平臺的概念，搭建了基于STM30F303 VCT6 控制器的RTK-GNSS 平臺自動導(dǎo)航系統(tǒng)，實現(xiàn)履帶平臺自動啟停、轉(zhuǎn)向控制，以及方位信號獲取與動態(tài)監(jiān)測功能。

2）建立履帶平臺運動學(xué)模型，提出虛擬探照燈尋徑跟蹤算法，引入橫向偏差指數(shù)λ、視域增益k1，目標(biāo)增益k2，針對橫向偏差和速度變化引起的跟蹤振蕩問題，提出基于橫向偏差指數(shù)的視域角動態(tài)獲取方法和基于速度正比例關(guān)系的假想目標(biāo)點判斷方法。MATLAB 仿真中發(fā)現(xiàn)當(dāng)運動速度為0.4 m/s、λ、k1和k2分別為1/4、0.005 rad·m 和6 s-1。

3）針對算法的導(dǎo)航效果進(jìn)行仿真驗證，并對不同初始位置、運動參數(shù)和運動場景進(jìn)行對比試驗。以仿真參數(shù)作為依據(jù)，在0.4 m/s 速度下進(jìn)行水泥路面導(dǎo)航試驗，6 種不同初始位姿下，平均上線距離為1.64 m，平均橫向偏差和航向偏差為0.44 cm 和1.57°。不同速度導(dǎo)航試驗表明，提高速度會導(dǎo)致導(dǎo)航精度降低，適當(dāng)增大k1并減小k2值可實現(xiàn)較好的導(dǎo)航效果，水泥路面時，3 種速度下獲得平均橫向和航向偏差為0.75 cm 和1.05°，平均糾偏次數(shù)為4.7 次。土路面和水泥路面導(dǎo)航對比試驗表明，由于土路面附著系數(shù)增加，轉(zhuǎn)向相對平穩(wěn)，糾偏次數(shù)和航向偏差均有所降低，但相同參數(shù)下2 種路況導(dǎo)航效果接近，算法具有良好的適應(yīng)性。