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

陳子文，熊揚(yáng)凡，胡宗銳，李聰，龐有倫，楊明金※

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

0 引言

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

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

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

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

1 材料與方法

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

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

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

履帶底盤動(dòng)力傳動(dòng)與轉(zhuǎn)向系統(tǒng)如圖2 所示。發(fā)動(dòng)機(jī)將動(dòng)力傳遞給手動(dòng)換擋變速箱，離合器手柄可控制動(dòng)力離合器的閉合，將動(dòng)力傳遞給驅(qū)動(dòng)橋，左右兩個(gè)驅(qū)動(dòng)輪前端均設(shè)有常閉離合器，通過轉(zhuǎn)向手閘軋線控制離合器打開，切斷動(dòng)力傳輸，實(shí)現(xiàn)停機(jī)或轉(zhuǎn)向。由于該類動(dòng)力平臺(tái)通過控制離合器閉合，實(shí)現(xiàn)車輛制動(dòng)或轉(zhuǎn)向，故將此類結(jié)構(gòu)和操控方式稱為離合制動(dòng)式履帶底盤。為實(shí)現(xiàn)履帶底盤自動(dòng)操控，對(duì)手動(dòng)操控系統(tǒng)進(jìn)行升級(jí)，采用氣動(dòng)系統(tǒng)代替手動(dòng)操作，應(yīng)用3 組獨(dú)立控制的雙作用氣缸拉動(dòng)軋線控制3個(gè)離合器的閉合，實(shí)現(xiàn)啟停、直線行駛、轉(zhuǎn)向等動(dòng)作[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 單片機(jī)作為中控器，數(shù)據(jù)接收、數(shù)據(jù)解析、運(yùn)算處理等任務(wù)均由該單片機(jī)完成。全球衛(wèi)星導(dǎo)航系統(tǒng)（global navigation satellite system，GNSS）采用上海華測(cè)導(dǎo)航技術(shù)股份有限公司的P3-DU 北斗高精度定位測(cè)向接收機(jī)，搭配有載波相位差分技術(shù)（real-time kinematic，RTK），多系統(tǒng)雙頻，支持雙天線定位測(cè)向解算，在裝備有移動(dòng)站接收機(jī)的基礎(chǔ)上，通過搭建基站接收機(jī)和無線電臺(tái)，實(shí)現(xiàn)RTK-GNSS 高精度定位數(shù)據(jù)的接收。導(dǎo)航控制系統(tǒng)整體架構(gòu)如圖3 所示，RTK-GNSS定位模塊利用無線電臺(tái)接收差分信號(hào)，經(jīng)差分系統(tǒng)校準(zhǔn)后，中控器采取通用異步串行通信（universal asynchronous receiver/transmitter，UART）方式獲取高精度定位數(shù)據(jù)。將解析數(shù)據(jù)與設(shè)定的期望路徑進(jìn)行比較，利用偏差跟蹤算法得到預(yù)期駕駛動(dòng)作，通過控制執(zhí)行機(jī)構(gòu)以達(dá)到路徑跟蹤的目的。上位機(jī)可以監(jiān)控導(dǎo)航控制系統(tǒng)的工作狀態(tài)，其與中控器之間采用UART 方式并通過ZigBee 無線模塊進(jìn)行數(shù)據(jù)的相互傳輸。中控器將解析后的定位信息以及導(dǎo)航過程中所產(chǎn)生的各項(xiàng)導(dǎo)航參數(shù)反饋給上位機(jī)，在系統(tǒng)發(fā)生異常時(shí)可通過發(fā)送相關(guān)指令終止導(dǎo)航作業(yè)。在本研究中，選擇ZigBee 模塊主要考慮到其成本低、體積小、使用方便等優(yōu)勢(shì)，有利于在短時(shí)間內(nèi)實(shí)現(xiàn)系統(tǒng)原型的搭建與初步測(cè)試。

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

2 路徑跟蹤控制方法

2.1 履帶底盤運(yùn)動(dòng)學(xué)模型

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3 仿真與現(xiàn)場(chǎng)試驗(yàn)

3.1 評(píng)價(jià)指標(biāo)

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

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

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

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

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

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

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

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

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

3.2 仿真試驗(yàn)

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

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

仿真試驗(yàn)發(fā)現(xiàn)，控制參數(shù)k1、k2的取值對(duì)導(dǎo)航效果有重要影響。因此在λ取值確定后，需初步評(píng)估k1、k2的合理值，為現(xiàn)場(chǎng)試驗(yàn)提供依據(jù)。圖10 為上線過程和直線行駛過程的仿真軌跡。根據(jù)圖10 和表1 可知，視域增益k1對(duì)上線距離影響較小，但對(duì)直線跟蹤過程中l(wèi)dm的大小有重要影響，在合理范圍內(nèi)視域增益k1增大則ldm變小。目標(biāo)增益k2對(duì)上線距離有重要影響，隨著k2的增大，上線距離先減小后增大，這是由于過大的k2會(huì)引起較大的超調(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)場(chǎng)試驗(yàn)

3.3.1 試驗(yàn)方法

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

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

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

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

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

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

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

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

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

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

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

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

3.3.4 運(yùn)動(dòng)參數(shù)導(dǎo)航試驗(yàn)

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

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

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

3.3.5 路況對(duì)比試驗(yàn)

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

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

試驗(yàn)中初始航向偏差為25°，初始橫向偏距為0.5 m，λ=1/4，速度為0.4 m/s 時(shí)，選用仿真所得較優(yōu)值k1=0.005 rad·m、k2=6.0 s-1。水泥路面試驗(yàn)以試驗(yàn)號(hào)1、8、11 為代表，控制初始位姿不變，保證速度擋位、k1以及k2各水平對(duì)應(yīng)，以表2 中試驗(yàn)號(hào)12、14、15 的數(shù)據(jù)進(jìn)行對(duì)比（平均上線距離為1.85 m，橫向偏差絕對(duì)值的均值、標(biāo)準(zhǔn)差和均方根分別為0.82、0.57 偏差絕對(duì)值的均值、標(biāo)準(zhǔn)差和0.98 cm，航向偏差絕對(duì)值的均值、標(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，此時(shí)可獲得較好的導(dǎo)航效果。因此，算法參數(shù)的取值不受到路況的影響，僅與運(yùn)行速度有關(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)機(jī)導(dǎo)航作業(yè)需求。后續(xù)將深入探索不同速度感知下，k1和k2自動(dòng)獲取和匹配，為實(shí)現(xiàn)最佳導(dǎo)航效果提供便捷的控制算法。

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

4 結(jié)論

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

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

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