基于虛擬探照燈的離合制動履帶底盤尋徑跟蹤

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

（1. 西南大學工程技術學院，重慶 400715；2. 重慶工業職業技術學院機械工程與自動化學院，重慶 401120；3. 重慶市農業科學院農業機械化研究所，重慶 401329）

0 引言

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

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

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

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

1 材料與方法

1.1 離合制動式履帶底盤構成

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

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

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

圖2 履帶底盤轉向結構示意圖Fig.2 Steering mechanism diagram of track chassis

1.2 導航控制系統架構

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

圖3 導航控制系統架構Fig.3 Structure of navigation control system

2 路徑跟蹤控制方法

2.1 履帶底盤運動學模型

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

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

定義履帶底盤的航向角ψ為Y軸正方向順時針旋轉至速度v的角度，轉向角φ的大小與轉向對應的圓心角相等，其值與轉向持續時間有關。根據運動學原理有：

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

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

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

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

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

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

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

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

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

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

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

式中k2表示目標增益，s-1。

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

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

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

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

2.3 基于數據流圖的軟件設計及算法實現

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

圖7 數據流圖Fig.7 Data flow diagram

通過DMA 傳輸，結合空閑中斷與循環隊列的數據接收方式的優勢在于系統進行工作運算的同時能夠進行數據的接收。每次進行數據提取時均能捕捉到最新序列的數據幀，提高了定位數據與實際位置的吻合度；省略了CPU 數據接收流程，提高了系統運算的效率和穩定性。

3 仿真與現場試驗

3.1 評價指標

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

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

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

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

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

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

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

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

圖9 糾偏次數統計Fig.9 Statistical of deviation correction

3.2 仿真試驗

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

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

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

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

3.3 現場試驗

3.3.1 試驗方法

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

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

3.3.2 算法參數可行性驗證

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

表2 現場試驗結果Table 2 Field test result

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

3.3.3 初始位姿參數導航試驗

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

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

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

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

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

3.3.4 運動參數導航試驗

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

圖12 運動參數變化試驗結果Fig.12 Test results of variation of motion parameter

試驗結果表明，隨履帶底盤的速度提升VSPT 算法導航效果變差，但從整體上看，在不同速度情況下若選取相匹配的較優控制參數，仍有較好的導航精度與穩定性，控制參數k1、k2與履帶底盤的運行速度有強相關性。

3.3.5 路況對比試驗

為驗證VSPT 算法對不同路況的適應能力，選取試驗田土路面與水泥路面進行對比試驗，試驗現場如圖13所示，試驗結果如表2 中試驗號12～15。

圖13 現場試驗路況Fig.13 Road condition of field test

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

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

4 結論

1）提出離合制動式履帶平臺的概念，搭建了基于STM30F303 VCT6 控制器的RTK-GNSS 平臺自動導航系統，實現履帶平臺自動啟停、轉向控制，以及方位信號獲取與動態監測功能。

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

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