基于虛擬雷達模型的履帶拖拉機導航路徑跟蹤控制算法

劉志杰 王小樂 任志剛 毛文菊 楊福增

(1.西北農林科技大學機械與電子工程學院， 陜西楊凌 712100；2.農業農村部北方農業裝備科學觀測試驗站， 陜西楊凌 712100；3.陜西省蘋果重點實驗室， 陜西楊凌 712100)

0 引言

我國傳統果園具有種植密度大、作業通道低矮狹窄等特點，普通拖拉機難以進入進行作業，而小型履帶拖拉機具有體積小、地形適應能力強、轉向靈活等優點，應用較為廣泛[1]，但仍存在操作人員勞動強度大、工作環境惡劣等問題。在勞動力短缺、人工成本不斷提高的情況下，只有發展自動化、無人化的果園作業機械才能保證水果產業的可持續發展[2]。導航技術是農機自動化的關鍵技術之一[3-6]，路徑跟蹤作為導航控制系統的核心技術，對提高作業質量和效率起決定性作用[7-10]。因此，研究小型履帶拖拉機路徑跟蹤算法對提高果園生產自動化和智能化具有重要意義。

國內外學者針對輪式車輛的導航路徑跟蹤控制算法進行了廣泛而卓有成效的研究[7,9,11-17]。履帶車輛與輪式車輛的轉向原理不同，輪式車輛的路徑跟蹤算法難以直接應用于履帶車輛，HUANG等[18]針對差速驅動履帶機器人設計了一種前饋PID控制算法，提高了路徑跟蹤精度；賈全等[19]提出了一種基于航向預估模型的控制算法，在一定程度上降低了履帶拖拉機滑移對控制精度的影響；付拓等[20]提出了一種基于履帶轉速與虛擬驅動速度和虛擬轉向角轉換模型的控制算法，為導航控制系統構建了完整的反饋控制。

上述路徑跟蹤算法基于反饋控制的思路，以路徑偏差作為控制輸入，實現了履帶車輛路徑跟蹤，但未考慮路徑的變化趨勢對車輛控制的影響，從而導致控制精度不一定能滿足作業要求。本文以單邊制動轉向小型履帶拖拉機為試驗平臺，提出一種基于虛擬雷達模型的導航路徑跟蹤控制算法，以虛擬雷達圖描述車輛與路徑的相對位置關系，經深度神經網絡生成駕駛指令，從而控制拖拉機行駛，并進行仿真驗證和果園實車試驗，驗證該算法的有效性。

1 材料與方法

1.1 試驗平臺

為研究基于虛擬雷達模型的履帶拖拉機導航路徑跟蹤控制算法性能，以小型履帶拖拉機(1KFL-30型，富平縣悅達機械制造有限公司)為基礎搭建了試驗平臺，如圖1所示。拖拉機在行駛過程中，可通過電控液壓系統進行轉向控制，當拖拉機需要轉向時，電磁閥啟動轉向液壓缸，對應一側的牙嵌離合器分離，切斷動力輸出并制動實現差速轉向；當拖拉機停車時，兩側離合器同時分離切斷動力并制動實現停車。拖拉機主要技術參數如表1所示。

表1 履帶拖拉機主要技術參數

導航控制系統結構如圖2所示，主要由導航定位設備、上位控制系統和下位控制系統組成。導航定位設備為XN422型組合導航系統(西安北方捷瑞光電科技有限公司)，該設備由基站、移動站和數據鏈路3部分組成，能夠以100 Hz的頻率提供精度為0.03 m的位置信息和精度為0.1°的航向信息。下位控制系統部署在拖拉機上，主要功能包括：①對導航定位設備提供的拖拉機位姿信息進行轉發。②接收上位控制系統指令或遙控指令。③控制液壓系統實現拖拉機行駛。上位控制系統以便攜式計算機為硬件平臺，主要功能包括：①為仿真和試驗過程提供可視化界面。②路徑規劃。③人工駕駛拖拉機模型產生神經網絡訓練數據。④記錄仿真和試驗數據。上位和下位控制系統之間采用無線數傳電臺(E34-DTU型，成都億佰特電子科技有限公司)進行通信。

1.2 履帶拖拉機運動學模型

履帶車輛的轉向方式為差速轉向，依靠行進時兩側履帶的不同轉速來改變車輛的行駛方向。本研究所使用的履帶拖拉機只能實現單邊制動轉向，即轉向時內側履帶速度為0，外側速度保持不變，其轉彎半徑為固定值。

為簡化運動學分析過程，假設：①拖拉機僅在水平面上運動。②兩側履帶完全一樣且忽略履帶寬度對行駛軌跡的影響。③整機質心和幾何中心重合。④兩側履帶在運動過程中不發生側滑。

履帶拖拉機運動學模型如圖3所示，O′為拖拉機質心位置。根據運動學原理，履帶拖拉機的運動學方程為

(1)

式中 (x,y,θ)——拖拉機位姿

B——拖拉機軌距寬度

vL——拖拉機左側履帶速度

vR——拖拉機右側履帶速度

當vL=vR≠0時，左右兩側履帶速度相等，拖拉機直線行駛；當vL=0或vR=0時，即左側或右側履帶速度為0時，拖拉機向左或向右轉向。

1.3 虛擬雷達模型描述

目前農業機械導航路徑跟蹤控制一般根據車輛當前位置相對于規劃路徑的橫向和航向偏差，按照某種控制策略計算車輛的期望轉向角，使車輛沿預定路線行駛[4,6]。但履帶車輛沒有輪式車輛的前輪轉向角概念[20]，因此輪式車輛的導航路徑跟蹤控制算法難以直接應用于履帶車輛；同時，橫向和航向偏差雖然可以反映車輛相對于規劃路徑的偏差情況，但其計算過程較為復雜且無法反映路徑的走向變化趨勢。

本研究針對現有路徑跟蹤算法的不足提出一種基于虛擬雷達模型的導航路徑跟蹤控制算法。該算法借鑒人類駕駛經驗，結合雷達掃描原理和圖像識別的思路，使用虛擬雷達圖描述拖拉機與路徑的相對位置關系，運用深度神經網絡對虛擬雷達圖進行識別并生成對應的拖拉機行駛操縱指令以實現導航路徑跟蹤控制。

1.3.1虛擬雷達模型原理

駕駛員駕駛車輛勻速沿著車道行駛時(圖4)，其用來進行轉向操作的參照物并不是車道的幾何中線，而是左右兩條車道邊線，通過判斷前方視野區的車道邊線與車輛的相對位置以及車道的形狀和走向變化趨勢(圖4中的曲線AB和DC)來轉動方向盤從而控制車輛沿著車道行駛。

通過對人類駕駛經驗的借鑒，本研究將農機的規劃路徑進行偏移，形成虛擬路徑邊界AB和DC，拖拉機在自動駕駛過程中不再以橫向和航向偏差為控制輸入，而是以拖拉機“看到”的封閉圖形ABCD的形狀來控制拖拉機轉向，ABCD組成的封閉圖形不僅可以反映自身和路徑的相對位置而且可以提供路徑和走向趨勢信息。如何讓拖拉機“看到”虛擬路徑邊界成為本算法實現的核心問題之一。真實無線電雷達的工作原理是通過向目標發射電磁波信號，然后接收目標反射回來的信號(目標回波)，進而獲得目標的輪廓、位置信息[21]。如圖5所示，lmax表示雷達最大探測距離，l表示目標距離，θ表示目標方向。借鑒這一原理，本研究通過算法模擬雷達掃描目標的過程，實現拖拉機“看到”虛擬路徑邊界組成的封閉圖形ABCD。

為了便于實現所提出的算法，對虛擬雷達模型做出如下約定：

(1)虛擬雷達只是通過算法模擬雷達掃描目標的過程，不涉及真實世界的雷達。

(2)為降低控制算法的復雜度，虛擬雷達具有有限的掃描分辨率和有限的掃描距離。

(3)虛擬雷達只對虛擬路徑邊界有探測作用。

1.3.2虛擬雷達圖生成

虛擬雷達對虛擬路徑邊界的掃描可以反映出拖拉機的路徑偏移情況和路徑的走向變化，將虛擬雷達對虛擬邊界的掃描結果定義為虛擬雷達圖。為了方便結合雷達掃描原理，對人類控制車輛轉向的原理做了進一步拓展，圖4中封閉圖像不再僅限于視野前方，而是圍繞拖拉機一周，如圖6所示；具體生成虛擬雷達圖的流程如圖7所示。

如圖6所示，設參考路徑的直線部分起點為點S(xs,ys)，終點為E(xe,ye)，虛擬路徑寬度為d，則參考路徑可描述為

A0x+B0y+C0=0

(2)

路徑邊界方程為

A1x+B1y+C1=0

(3)

A2x+B2y+C2=0

(4)

其中

A0=A1=A2=ys-ye

式中A0、A1、A2、B0、B1、B2、C0、C1、C2——直線方程系數

(5)

式中i——虛擬雷達探測距離個數

j——兩側虛擬邊界，取1或2

當Ajcos(θ+ir)+Bjsin(θ+ir)或探測距離l(i)<0以及探測距離超過虛擬雷達的最大探測距離lmax時，l(i)=lmax，否則雷達的探測距離l(i)=min(l1,l2)，其中l1、l2分別表示到兩側虛擬邊界的距離，最終對探測距離l進行歸一化即可得到虛擬雷達的探測結果。

以直線路徑為例，典型的虛擬雷達探測圖對應的車輛和規劃路徑的相對位置關系如圖8和圖9所示，圖8中拖拉機的橫向偏差均小于虛擬路徑邊界，圖9中的橫向偏差已超出虛擬路徑邊界，圖中綠色圖形為虛擬雷達探測到的圖像，其由4條線段構成，其中直線段為探測到的虛擬路徑邊界，圓弧段為雷達的最大探測邊界。

1.4 虛擬雷達模型導航路徑跟蹤控制算法實現

1.4.1基本思路

導航路徑跟蹤控制的實質是建立車輛和規劃路徑的偏差與行駛指令的映射關系[22]。分析目前傳統果園廣泛使用的小型履帶拖拉機的功能和作業特點，其前行時其操作指令僅有直行、左轉、右轉和停車等4種，所要建立的導航路徑跟蹤控制算法的任務就是將4種操作指令與虛擬雷達圖建立起映射關系。隨著計算機性能不斷提升，深度學習在諸如圖像分類、智能駕駛等領域有著越來越廣泛的應用[23]，同時Google、Microsoft、百度等公司紛紛研發并開源深度學習框架，進一步減少了使用深度神經網絡的學習成本，加快了深度學習在各個領域的應用推廣。

本研究通過利用深度神經網絡對輸入的虛擬雷達圖進行識別并輸出對應的行駛操作指令。算法整體結構如圖10所示，輸入為虛擬雷達圖，輸出為拖拉機4種備選轉向指令(TL為左轉、GS為直行、TR為右轉、ST為停車)的概率，選取其中概率最大的備選轉向指令為拖拉機的轉向控制指令。

本研究使用Python 3.5和Google公司的開源深度學習程序框架TensorFlow(GPU版本)搭建神經網絡，運行環境為Windows 10系統。具體為使用keras中的Sequential模型搭建網絡，網絡結構如圖11所示，包括輸入層、2個全連接隱藏層和1個輸出層；其中輸入層有360個神經元(由虛擬雷達分辨率決定)，2個隱藏層分別有700個和500個神經元，激活函數為線性整流函數(Rectified linear unit, ReLU)，表達式為

(6)

輸出層有4個神經元，分別對應拖拉機行駛的4種行駛狀態(左轉、右轉、直行和停車)，激活函數為Softmax函數，表達式為

(7)

式中yj——輸入到Softmax之前的每個單元的值

yk——輸入到Softmax之前的第k個單元的值

Softmax(y)k——第k個單元經過Softmax函數處理后的概率分布

Softmax回歸本身可作為一個學習算法來優化分類結果，而TensorFlow中去掉了Softmax回歸的一些參數，使它僅作為輸出層的一個額外處理層，將神經網絡的輸出變為一個概率分布輸出。

1.4.2訓練數據準備

訓練數據的獲取算法是通過人工在仿真平臺駕駛履帶拖拉機(基于運動學模型式(1))沿規劃路徑行駛，記錄人的操作指令，并與該時刻的虛擬雷達圖配對存儲，以此作為神經網絡的訓練數據。

在仿真平臺獲取訓練數據時，規劃路徑為多段折線路徑，拖拉機的橫向偏差控制在[-1.000 m，1.000 m]，航向偏差控制在[-60°，60°]，共采集數據32 000組，包括5 055組直行、13 560組左轉、13 385組右轉；考慮到實際試驗過程中可能出現定位漂移，導致虛擬雷達掃描出現圖9c的情況，因此人為加入2 000組停車指令；將數據按照8∶1∶1分為訓練集、驗證集和測試集。

基于草原絲路文物資源的鄉村旅游產業開發缺乏統一部署，具有文物資源地區各行其是，未用系統眼光看待問題，解決問題，開發模式分散。無論是從橫向上看還是從縱向上看，鄉村旅游產業與相關產業融合度都比較低，導致相關產品附加值低，宣傳力度不夠。產品部門間協作不強，產業鏈條較短，產業關聯效果不顯著。產業處于起步階段，發展機制仍需完善。

1.4.3網絡訓練

網絡訓練過程中，初始學習率設為0.000 1，選用TensorFlow提供的Adam自適應學習率優化器[24]對訓練過程中的學習率進行優化，Batch size為10，迭代次數為180，Dropout為0.02，損失函數為稀疏分類交叉熵函數(Sparse categorical cross entropy)，公式為

(8)

式中M——類別數量

yc——指示變量，若該類和樣本的類別相同則為1，否則為0

pc——觀測樣本屬于類別c的預測概率

訓練所用計算機硬件配置為中央處理器(CPU)i5-8300H，主頻2.3 GHz；GTX-1050Ti顯卡(GPU)；16 GB內存，DDR4，2666 MHz。訓練過程中訓練集和驗證集損失函數變化如圖12a所示，訓練集和驗證集的精度變化如圖12b所示，網絡在測試集精度為0.990 8。

2 仿真與試驗結果分析

2.1 仿真驗證

仿真規劃路徑為U形折線路徑，長20 m，寬4 m(路徑總長約42.83 m)，路徑起始點為(0，-0.5 m)，航向角為180°，拖拉機運動學模型初始橫向偏差為0.5 m，航向角為180°。分別對2個速度0.36、0.75 m/s進行仿真驗證，仿真過程中上位控制系統通信、顯示、數據記錄等功能正常，仿真結果如圖14所示。當仿真速度為0.36 m/s時，路徑跟蹤最大橫向偏差0.103 m，平均橫向偏差0.01 m，標準差0.01 m；當仿真速度為0.75 m/s時，路徑跟蹤最大橫向偏差0.108 m，平均橫向偏差0.016 m，標準差0.016 m。仿真結果表明，基于虛擬雷達模型的導航路徑跟蹤控制算法在原理上可行，可以進行導航路徑跟蹤控制，并且具有較高的路徑跟蹤精度，能夠滿足實際作業需求。

2.2 實車試驗

為進一步驗證本文路徑跟蹤算法的有效性，以目前廣泛使用的模糊控制算法為對比進行了實車試驗。模糊控制算法以橫向偏差和航向偏差作為輸入，期望轉向角作為輸出；橫向偏差論域為[-1.200 m，1.200 m]，航向偏差和期望轉向角論域均為[-60°，60°]，規定左側橫向偏差為負、右側為正，航向偏差和期望轉向角逆時針為正、順時針為負；輸入輸出變量均劃分為7 個模糊集，采用三角隸屬度函數對變量進行模糊化，重心法進行反模糊化。對于單邊制動履帶拖拉機沒有期望轉向角的問題，參照文獻[25-26]對控制輸出的期望角進行處理，當期望轉向角絕對值小于3°時對應直行指令，其余情況根據期望轉向角的正、負情況對應左、右轉指令。

實車試驗場地為西北農林科技大學國家柿種質資源圃，果樹行距為2.4 m，試驗現場如圖15所示。考慮到拖拉機在果園作業過程中需要進行直線跟蹤和調頭換行，因此規劃路徑設計為20.0 m×2.4 m的U形折線路徑，試驗速度設定為0.36、0.75 m/s，為保證試驗數據有效性，每組試驗重復3次(對試驗結果中的橫向和航向偏差均取絕對值)。路徑跟蹤試驗效果如圖16～19所示，重復試驗的數據統計如表2所示。

表2 路徑跟蹤試驗數據

由圖16～19可知，在速度相同的條件下，基于虛擬雷達模型的路徑跟蹤算法與基于模糊控制的路徑跟蹤算法相比，試驗路徑全程的整體橫向偏差更小；在轉向路徑和換行后直線路徑的起始部分有更好的路徑跟蹤精度和行駛穩定性。

對比2種控制算法的直線路徑部分(U形路徑0～20 m)跟蹤精度，結果如表3所示，在速度0.36 m/s時，模糊控制在最大橫向偏差控制和拖拉機行駛穩定性方面(標準差)好于基于虛擬雷達模型的路徑跟蹤算法，但新算法平均橫向偏差降低了8.51%；在速度0.75 m/s時，本研究所提出算法在橫向偏差、行駛穩定性等方面均優于模糊控制，最大橫向偏差、平均橫向偏差和標準差分別降低了7.10%、5.55%、10.26%。

表3 直線路徑跟蹤精度對比

對比2種導航路徑跟蹤控制算法的U形路徑跟蹤精度，結果如表4所示，新算法的所有路徑跟蹤指標均優于模糊控制，最大橫向偏差最高降低了36.33%，平均橫向偏差最高降低了27.91%，標準差最高降低了28.00%。表明本文提出的算法在U形路徑的整體路徑跟蹤性能明顯好于基于模糊控制的跟蹤算法。

表4 U形路徑跟蹤精度對比

對比2種算法U形路徑和直線路徑的跟蹤精度，結果如表5所示，模糊控制的U形路徑和直線路徑跟蹤精度指標相差較大，特別是最大橫向偏差，速度0.36 m/s和0.75 m/s的U形路徑分別比直線路徑高56.14%和63.94%，而研究所提算法的相關指標均比較相近，最大差異出現在0.75 m/s時的最大橫向偏差，U形路徑比直線路徑僅高出12.35%。表明新算法在轉向時的偏差控制能力優于模糊控制，有較好的轉向路徑適應性。

表5 U形和直線路徑跟蹤精度對比

3 結論

(1)為提高傳統果園廣泛使用的小型履帶式拖拉機導航路徑跟蹤控制精度和行駛穩定性，提出了一種基于虛擬雷達模型的導航路徑跟蹤控制算法，該算法簡化了現有路徑跟蹤控制算法的輸入計算過程，并且不依賴于復雜控制理論和數學模型，易于應用。

(2)以果園作業過程中典型U形路徑為例，對所提出的跟蹤控制算法進行了仿真驗證。當速度為0.36 m/s時，路徑跟蹤最大橫向偏差為0.103 m；當速度為0.75 m/s時，最大橫向偏差為0.108 m。說明該算法在原理上可以實現導航路徑跟蹤控制，且具有較高的控制精度，可以滿足果園導航作業需求。

(3)在速度為0.36 m/s和0.75 m/s下采用基于虛擬雷達模型的控制算法和目前廣泛使用的模糊控制算法進行了實車對比試驗。結果表明：與模糊控制算法相比，本文算法最大橫向偏差分別減小了15.73%和36.33%，平均橫向偏差分別減小了27.91%和19.05%，標準差分別減少了21.88%和28.00%。說明該算法的整體性能優于基于模糊控制的路徑跟蹤算法，有效提高了導航路徑跟蹤精度和履帶拖拉機自動駕駛的穩定性，對果園典型作業路徑有更好的適應性。