基于動態前視距離純追蹤模型的溫室農機路徑跟蹤研究*

柴善鵬，姚立健，徐麗君，陳欽漢，徐濤濤，楊巖坤

(1.浙江農林大學光機電工程學院，杭州市，311300；2.浙江農林大學集賢學院，杭州市，311300)

0 引言

自動導航能顯著提升農機作業效率和質量，是精準農業體系中的重要組成部分[1-2]。純追蹤模型是一種模擬人駕駛行為的幾何方法，具有控制參數少，預見性強等優點，在農機導航控制中得到廣泛應用[3-5]。

設施農業環境道路狹窄，需要農機有精準的路徑跟蹤能力[6-8]。作為純追蹤模型唯一可調參數，前視距離的選取會影響路徑跟蹤的效果。

Petrinec等[9]通過試驗證明，前視距離取輪距大小時能得到較好的跟蹤效果，但得到的精度有限。黃沛琛等[10]通過BP神經網絡動態調整前視距離，但此方法需要大量的優質樣本，并且容易陷入局部最優。汪小旵等[11]利用模糊控制來自適應調整前視距離，但把農機做恒速處理，僅以農機位姿偏差作為模糊控制器的輸入，缺乏對速度變化的適應性。張聞宇等[12]在調整前視距離視雖然考慮了速度的影響，但是提出的經驗公式并不適合其他農機，需要通過大量試驗才能確定公式參數。

上述方法調整前視距離需要調整的參數較多，并且控制算法的實時性有待提高。PSO算法是一種智能仿生算法，具有調整參數少，收斂速度快等優點[13-15]，被廣泛應用在函數優化以及系統控制等領域。王爾申等[16]將PSO算法引入到多星座的組合導航選星過程中，試驗表明該方法能減少選星時間并提高搜索結果的準確性。唐傳茵等[17]利用PSO算法對模型預測控制器進行優化，并在不同速度區間和工況下進行了試驗，試驗驗證該方法提升了路徑跟蹤效果。綜上可知，PSO算法利于求取導航過程中控制參數的最優值。

本文采用一種雙參數(橫向偏差d與航向偏差θ)定量的方法來分析車體偏差，根據實際需求設計了動態權重來調整兩種偏差對整體偏差的影響。為進一步提升路徑跟蹤的行駛效率和精準性，提出一種通過PSO算法動態確定純追蹤模型前視距離的路徑跟蹤算法，構建速度控制函數，依據車體的偏差程度對車體進行變速控制。并利用樣機試驗驗證本文方法的有效性。

1 試驗材料

1.1 試驗樣機

試驗樣機為純電動四輪獨立轉向/驅動機構，由鋰電池(24 V、20 Ah)供電，如圖1所示。車身的結構尺寸(長×寬×高)為120 cm×70 cm×70 cm，左右輪距為54 cm，前后輪距為104 cm。采用轉向舵機(DH-03X，120°/s，38 N/m)實現各輪轉向，最大轉向角為±90°，轉向角0°作為車輪的初始狀態，車輪逆時針偏轉時轉向角為正，反之轉向角為負。由驅動器AQMD6015BLS驅動4個輪轂電機實現樣機的行駛。樣機的位姿狀態信息分別由UWB無線定位系統和九軸電子陀螺儀(WT901C)提供，位姿信息誤差范圍分別為±5 cm和±0.1°。主控器(STM32F103ZET6)接收、處理數據及發送控制指令。

圖1 試驗樣機Fig.1 Test prototype1.電子陀螺儀 2.舵機 3.移動標簽 4.UWB基站 5.電機驅動器 6.單片機 7.鋰電池 8.輪轂電機

1.2 樣機控制系統

主控器接收UWB模塊發來的試驗樣機位置坐標(x，y)和電子陀螺儀傳來的航向角φ，換算成車體相對于期望路徑的位姿偏差信息d和θ。采用PSO算法并根據車體偏差信息確定最優前視距離。

將位姿信息和對應的最優前視距離LD輸入到純追蹤模型，計算輪轉角δ，并通過速度控制函數g(λ)確定實時車速v。計算樣機行駛以及轉向所需的PWM信號，輸入電機控制器以及舵機，執行相應行駛和轉向動作，實現路徑跟蹤。系統控制流程如圖2所示。

圖2 樣機控制系統原理圖Fig.2 Schematic diagram of prototype control system

2 純追蹤模型

純追蹤模型是一種直觀的幾何追蹤模型，其示意圖如圖3所示，P1P2為跟蹤路徑，以車體幾何中心點A點為基準點，橫向偏差d即為A點到跟蹤路徑的投影點B的距離，車身方向AD與跟蹤路徑的夾角為航向偏差θ，在P1P2上找到一點C，使得BC等于前視距離LD，則點C即為預瞄點，根據純追蹤模型思想，可規劃一段以O為圓心從A點到C點的圓弧，該圓弧半徑R為車體轉向半徑，根據幾何運算可得移動輪轉角。

圖3 純追蹤模型Fig.3 Pure pursuit model

由圖3中幾何關系可得

(1)

根據四輪阿克曼低速轉向模型可得車體轉向時四個移動輪的轉角[18]，為保證控制效果，對模型進行簡化，解算過程中令車體同側前后移動輪轉角大小相等，偏轉方向相反。

結合四輪轉向模型得各輪轉角

(2)

式中：δfr——右前輪轉角；

δfl——左前輪轉角；

δrr——右后輪轉角；

δrl——左后輪轉角，轉角正負號代表偏轉方向；

L——車體的前后輪距；

W——車體的左右輪距。

純追蹤模型的核心是確定一個合適的前視距離，前視距離對路徑跟蹤質量有著直接的影響，固定前視距離純追蹤模型對車體在在不同偏差下的自適應性較差。下文將采用一種改進型的粒子群算法基于車體偏差程度對前視距離LD進行動態調整。

車體路徑跟蹤過程中需要根據車體偏差程度來動態調整前視距離[19]。車體偏差程度的界定主要依靠個人經驗，缺少定量分析方法。因此本文采用一種雙參數定量的方法來分析車體偏差程度。定義車體當前的d和θ如圖3所示。當車體幾何中心定位點位于期望路徑前進方向的左側時d為正，反之d為負，當車體航向角相對期望路徑逆時針偏轉時θ為正，反之θ為負。根據實際作業環境將d值域設置為[-2 m，2 m]，θ值域設為[-60°，60°]，在計算中若d或θ超出值域時按邊界值處理。則車體偏差

(3)

式中：m——權重。

(4)

式中：a——權重函數的比例系數；

b——權重函數的常數變量；

n——權重函數的冪次。

λ可表示車體的偏差程度，其值域為[-1，1]，|λ|越接近0，偏差程度越小，反之越大。

為實現精準的路徑跟蹤應更重視d的變化。在λ計算中不能忽略任一偏差的因素，m取值范圍為(0，1)。若a，b設置較大，則m值較大，θ在λ計算中權重過小，反之，m值較小，d在λ計算中權重過小。由于λ計算中需兼顧d和θ的權重，故設置權重函數的參數a=0.4，b=0.3。

如圖4所示，當權重函數的冪次n<1時，m隨d遞增較快，則在λ計算中d所占權重增大較快，權重變化趨勢體現了對d的重視，因此選擇n<1，設置n=0.8，反映λ大小的高程圖如圖5所示。

圖4 權重函數示意圖Fig.4 Schematic diagram of weight function

圖5 λ的高程圖Fig.5 Elevation map of λ

3 PSO算法

前視距離是純追蹤算法中唯一可變參數，它直接影響車體路徑追蹤的效果。本文通過PSO算法實時確定最優前視距離。

PSO算法是一種高效的全局尋優算法，本文將前視距離LD視為粒子，設定粒子總數為30，前視距離的值作為粒子i的位置xi(i=1,2,…30)，粒子i以一定的速度ei在約束的LD范圍[0.5 m,2 m]內進行搜索，通過適應度函數F評價xi效果，即衡量LD的路徑跟蹤效果，xi在變化過程中逐漸趨于全局最優位置，最終確定一個最優的LDi。

PSO算法進化公式

ei(s+1)=ωei(s)+c1r1(s)[ji(s)-xi(s)]+c2r2(s)[jg(s)-xi(s)]

(5)

xi(s+1)=xi(s)+ei(s+1)

(6)

式中：s——迭代次數；

ω——慣性權重系數；

ji——微粒個體所經歷的最好位置；

jg——所有微粒群體所經歷的最好位置；

c1、c2——加速因子；

r1、r2——(0，1)之間的隨機數。

3.1 PSO算法慣性權值改進

慣性權重ω具有平衡粒子群全局和局部搜索能力，當ω較大時，粒子群全局搜索能力較強；反之則粒子群局部搜索能力較強。PSO算法中ω的調整主要采用隨著迭代而線性減小的方法，但由于PSO算法早期收斂較快，如果在初期搜索找不到最好的值，那么隨著ω的減小，就會陷入局部極值，并且在PSO算法后期ω遞減較慢，粒子群局部搜索能力弱，因此慣性權重系數ω線性減小的方法求解效率差。

為克服典型線性遞減策略的局限性，采用正弦函數動態調整ω，計算公式如式(7)所示。

(7)

式中：ω(s)——第s次迭代對應的ω；

smax——最大迭代次數，smax=100；

ωmax——ω最大值，根據實際需求設定ωmax=1；

ωmin——ω最小值，根據實際需求設定ωmin=0.4。

由式(7)可得不同迭代次數時ω變化曲線，如圖6所示。

圖6 慣性權重變化曲線Fig.6 Inertial weight change curve

采用正弦函數非線性遞減策略，ω在初始迭代的時候遞減速度緩慢，有利于粒子群在初始迭代時尋找滿足條件的最優值，粒子群不會過快的收斂到局部搜索，這樣可避免算法陷入局部最優值；在迭代中后期，ω遞減速度快，粒子群具有更好的局部搜索能力，因此能提升求解效率。仿真試驗在MATLAB R2018環境下完成，通過多次對比仿真試驗，當ω采用正弦函數遞減時，PSO算法每次求解平均耗時130 ms，當ω采用線性函數遞減時，PSO算法每次求解平均耗時200 ms。ω采用正弦函數遞減方法能提升算法的求解速度。

3.2 適應度函數設計

適應度函數F的設計是粒子群算法的核心[20]，它直接影響PSO算法是否能找到最優解。采用純追蹤算法進行路徑跟蹤時，車體在一定時間內的偏差修正程度可作為判斷前視距離選取優劣的指標。車體偏差程度采用上文提出的|λ|，定量分析值能更加精準的顯示每個采樣時刻車體的偏差程度。在控制領域常采用時間乘以誤差絕對值積分(Integral time absolute error，ITAE)作為評價控制系統性能優劣的指標，利用ITAE指標設計的系統具有超調量小、阻尼適中、動態性能良好等優點。因此本文采用|λ|的ITAE作為路徑跟蹤的性能指標函數f(λ)。前視距離選取的越優，λ減小越快，即在一定工作時間內車體在中后期的λ越小，因此f(λ)計算中隨采樣次序變化逐漸加大λ權重能更好的反饋出車體λ的修正速度?？紤]計算實時性，設計性能指標函數

(8)

式中：Δt——采樣間隔0.1 s；

λ(p)——在第p次采樣時刻的|λ|值；

p——采樣次數。

f(λ)值越小說明車體位姿修正效果更好，選取的前視距離效果更優；而粒子群算法中個體的F數值越大說明解更優，即選取的前視距離效果更優，因此將f(λ)的倒數作為F。

(9)

3.3 PSO算法設置及流程

在實際應用中，由于硬件平臺配置限制，PSO算法在硬件平臺上進行大量迭代運算將增加算法計算時間，影響控制系統實時性。因此要設置合理的參數以提高粒子群收斂速度，降低算法耗費時間。本文設置PSO算法最大迭代次數為100，粒子數個數為30，學習因子c1、c2均為1，ω初值設為1。本文通過PSO算法實時確定前視距離步驟如下。

Step1：初始化粒子群，確定初始粒子的位置和速度。設定粒子群個數i為30，在前視距離范圍[0.5 m，2 m]內均勻生成前視距離矩陣[LD1LD2…LD30]，在粒子速度范圍[0.1 m/s，0.5 m/s]內均勻產生ei。

Step2：計算每個粒子的適應度。根據車體的λ，通過F確定每個粒子適應度，即每個前視距離的路徑跟蹤效果。

Step3：比較每個粒子與個體最優位置ji適應度，若粒子適應度更好，則將該粒子作為個體最優位置。

Step4：比較每個粒子與全局最優位置jg適應度，若粒子適應度更好，則將該粒子作為全局最優位置。

Step5：根據粒子群進化公式對粒子的位置和速度進行進化。在達到最大迭代次數前，重復步驟2～5；達到最大迭代次數后，停止運算，輸出全局最優位置，即最優前視距離。

3.4 速度控制函數

車速v對路徑跟蹤的精準性和導航效率有著較大的影響，當車體從較大的偏差程度下收斂至期望路徑過程中，若v過大則車體自身位姿偏差變化較快，轉向控制可能有一定滯后性，在路徑跟蹤過程中會影響收斂速度以及跟蹤精度，并且車體的側向加速度較大，車體有失穩的趨勢。當車體逐漸收斂至期望路徑時，車體偏差程度較小，則可以適當提升v以提升行駛效率。因此在路徑跟蹤過程中可根據車體偏差程度λ動態調整車速，通過歸一化線性關系可求出在不同λ的車速。設計速度控制函數

(10)

式中：vmax——最大行駛速度；

vmin——最小行駛速度；

|λmax|——偏差程度的最大值。

4 試驗與結果分析

4.1 路徑跟蹤試驗

為驗證本文算法有效性，于2021年4-6月在浙江農林大學農學實驗基地一間溫室內對樣機做路徑跟蹤試驗，試驗場地如圖7所示。試驗樣機前進速度分為恒速和變速，根據實際需求變速范圍設置為[0.3 m/s,1.5 m/s],恒速的車速設為0.6 m/s。試驗采用10 m 直線路徑跟蹤，采樣頻率為5 Hz。

圖7 實車試驗圖Fig.7 Real vehicle test chart

試驗1：為檢驗本文算法對不同偏差程度的適應性，設計3種不同偏差初始狀態(d，θ)來測試直線跟蹤效果，d與θ的數值選取是基于上文提出的λ，可使初始狀態的偏差程度分為大、中、小三種情形，分別以恒速下傳統的固定前視距離純追蹤模型和改進后的動態前視距離純追蹤模型進行對比試驗，傳統純追蹤的固定前視距離由計算機仿真獲得。跟蹤軌跡如圖8所示。

(a)初始狀態(1 m,90°)跟蹤軌跡

試驗2：為檢驗變速控制對路徑跟蹤效果的影響，采用試驗1的3種不同初始狀態來測試直線跟蹤效果，分別按變速動態前視距離純追蹤模型和恒速動態前視距離純追蹤模型進行對比試驗。跟蹤軌跡如圖9所示。對圖8和圖9數據進行統計可得表1。

(a)初始狀態(1 m,90°)跟蹤軌跡

表1 直線跟蹤試驗結果統計Tab.1 Statics of linear tracking test results

4.2 試驗效果指標

以路徑跟蹤的平均偏差，最大偏差，穩定距離，穩態偏差，導航全程時間等指標來衡量路徑跟蹤質量，平均偏差是從導航開始到結束所有橫向偏差的均值；最大偏差是指整個試驗過程中最大的橫向偏差；穩定距離是指樣機從初始狀態收斂到橫向偏差穩定小于0.1 m 的穩定點所走過的水平距離；穩態偏差指樣機到達穩定點后的橫向偏差平均值。平均偏差與穩態偏差是反映路徑跟蹤精度的重要指標。穩定距離反映路徑跟蹤的收斂速度，導航全程時間反映了路徑跟蹤的效率。

4.3 試驗結果分析

4.3.1 路徑跟蹤精度

在試驗1中，3種不同偏差下動態前視純追蹤模型路徑跟蹤平均偏差分別為51.8 cm、13.4 cm、24.8 cm，穩態偏差為5.4 cm、4.8 cm、5.1 cm，而傳統固定視距純追蹤模型平均偏差為68.7 cm、17.2 cm、38.9 cm，穩態偏差為7.2 cm、5.7 cm、6.8 cm。在試驗2中，變速動態前視純追蹤模型路徑跟蹤平均偏差分別為42.8 cm、11.1 cm、19.3 cm，穩態偏差為4.5 cm、4.3 cm、4.2 cm。上述數據證明采用動態前視純追蹤模型的路徑跟蹤精度優于采用傳統純追蹤模型，變速控制能提升路徑跟蹤精度。

4.3.2 收斂快速性

在試驗1中，3種不同偏差下動態前視純追蹤模型的穩定距離分別為394.5 cm、252.4 cm、369.2 cm，而傳統固定視距純追蹤模型的穩定距離分別為626.4 cm、313.9 cm、517.7 cm。在試驗2中，3種不同偏差下變速動態前視純追蹤模型的穩定距離分別為371.2 cm、231.6 cm、351.3 cm。上述數據顯示采用動態前視純追蹤模型的穩定距離均小于傳統固定視距純追蹤模型的穩定距離，說明本文的采用動態前視純追蹤模型顯著于提升收斂速度。變速控制對路徑跟蹤的收斂快速性也有一定程度提升。

4.3.3 路徑跟蹤穩定性

標準差反映數據的離散程度。因此可以用平均偏差以及穩態偏差的標準差能反映路徑跟蹤的穩定性，在試驗1中，3種不同偏差下動態前視純追蹤模型的平均偏差以及穩態偏差的標準差分別為 4.8 cm與1.6 cm，而傳統固定視距純追蹤模型的標準差分別為9.6 cm與3.9 cm。在試驗2中，變速動態前視純追蹤模型的平均偏差以及穩態偏差的標準差分別為4.3 cm 與1.2 cm。上述數據表明動態前視純追蹤模型路徑跟蹤穩定性優于傳統固定視距純追蹤模型，并且變速控制能一定程度提升路徑跟蹤穩定性。

4.3.4 變速控制對導航整體效果的影響

采用變速動態前視純追蹤模型控制下3種不同偏差下路徑跟蹤所需平均時間為13.2 s，而恒速下采用動態前視純追蹤模型和傳統純追蹤模型的路徑跟蹤所需平均時間分別為16.1 s和18.2 s。上述數據證明變速控制顯著提升了路徑的效率。由表1數據可知變速動態前視純追蹤的路徑跟蹤指標均為最優，這是因為車體收斂初期車速較低，有助于提升路徑跟蹤的精度和收斂效率，在逐漸收斂到期望路徑上時車速變快，有助于提升行駛效率。因此變速控制能一定程度上提升路徑追蹤的精度，收斂效率以及行駛效率。

5 結論

1)針對設施環境下農機路徑跟蹤精度問題，提出了基于PSO算法的動態前視距離純追蹤模型，根據農機的實時位姿狀態選取最優的前視距離。利用樣機進行了直線路徑跟蹤試驗，在不同初始狀態下，采用本文研究方法的路徑追蹤平均偏差為30.0 cm，平均穩定距離為338.7 cm，平均標準差為3.2 cm；采用固定視距純追蹤方法的路徑跟蹤平均偏差為41.6 cm，平均穩定距離為486.0cm，平均標準差為6.8 cm；采用本文算法的路徑跟蹤各項指標均優于傳統固定視距純追蹤模型，證明本文研究方法的有效性。

2)設計了樣機的可變速度控制函數，根據車體位姿偏差程度對樣機速度進行實時調整，利用樣機進行變速與恒速下直線路徑跟蹤的對照試驗，變速控制相比恒速控制，平均偏差降低了18.6%，平均穩定距離縮短了6.1%，平均標準差減小了10.9%，試驗結果證明了速度控制對路徑跟蹤的各項指標有一定程度提升。

3)試驗樣機整體的轉向圓心始終在車體橫向中心線的延長線上，在后續研究中，針對樣機的轉向方式多樣性進行研究，進一步發揮四輪轉向優勢，并提升路徑跟蹤的質量。