基于無模型自適應控制的設施園藝自動導航車輛路徑跟蹤

陳娟 姚立健 徐麗君 顏松

摘要：為實現設施園藝內農業機械的自動導航，提出一種基于無模型自適應控制（model-free adaptive control，簡稱MFAC）車輛路徑跟蹤方法。采用4個超寬帶（UWB）基站組建室內無線定位系統，并與WT901C光纖陀螺儀組合使用，獲取試驗樣車與期望路徑之間的橫向偏差和航向偏差。建立預瞄偏差模型，并采用無模型自適應控制器來動態確定純追蹤算法中的前視距離，最終實現路徑跟蹤。對比試驗結果表明，在4種不同的初始狀態下進行直線跟蹤時，本研究方法的平均偏差分別為0.102、0.079、0.181、0.084 m，達到穩定狀態后，其穩態偏差變化范圍在0.013～0.033 m之間;在矩形路徑跟蹤時，平均偏差為0.139 m，本研究方法明顯優于采用固定前視距離的純追蹤模型，滿足農業設施內狹窄空間中運輸作業的精度要求，可為設施園藝農機導航提供新思路。

關鍵詞：無模型自適應控制;自動導航;超寬帶;設施園藝;路徑跟蹤

中圖分類號：TP242.6 ??文獻標志碼： A

文章編號：1002-1302（2022）08-0187-06

近年來我國設施園藝產業發展迅速，總面積現已位列世界首位[1]。在設施園藝生產中，農產品在設施內的運輸時間約占整個作業時間20%以上[2]，因此研發能自動導航運輸的裝備成為設施園藝研究的熱點之一。

目前，位姿獲取與路徑跟蹤控制是農機自動導航的2個關鍵技術。全球定位系統（GPS）[3-6]、機器視覺[7-9]作為傳統的定位手段經常被使用在田間農機位姿解算、作物行導航線提取等導航研究中。但在農業設施內無法接收到清晰的GPS信號，而機器視覺的實時性和全局路徑規劃能力還有待提高。超寬帶（UWB）是利用納秒級或亞納秒級脈寬實現短距高速傳輸數據的無線通信技術，具有時間分辨率高、發送功率小等優點，被越來越多地運用到室內高精度定位研究中。如戴波等通過對UWB技術的靜態、動態定位性能進行試驗與分析，提出基于多時段協同定位方法，解決倉庫貨物堆垛遮擋、金屬貨罐和液體等干擾的問題[10]。符世琛等提出一種基于UWB測距技術的掘進機自主定位導向方法，可有效擴大掘進機的測量范圍，減少工人井下作業時間[11]。林相澤等采用信息到達時間差（time difference of arrival，簡稱 TDOA）技術以及動靜態優化算法，為設施環境下農用車輛精細作業提供了準確定位[12]。

在路徑跟蹤控制方面，純追蹤算法[13]因其控制參數少、預見性強等優點獲得廣泛應用。研究人員采用模糊控制[14]、線性方程[15]等方法動態調整前視距離，以提高路徑跟蹤精度。在模糊控制中，隸屬度函數的構建對專家經驗依賴較大，因此很難精準確定。對于線性方程算法，其數學表達式只能滿足當前假設條件，而這些假設條件在實際環境中存在一定局限性。無模型自適應控制（model-free adaptive control，簡稱MFAC）是一種利用動態線性化模型解決復雜非線性系統的控制器，其控制過程無需構建復雜的數學模型，僅僅利用受控系統的I/O數據驅動，具有控制算法難度低、無需假設條件等優點，現已廣泛應用在無人駕駛[16]、自動泊車系統[17]以及多自由度機器人外骨骼控制[18]等領域。

因此，本研究在前述研究基礎上，采用超寬帶技術構建室內定位系統以精確獲取車輛橫向偏差和航向偏差，通過預瞄偏差模型對純追蹤算法進行改進，并提出一種基于MFAC的前視距離動態確定方法并驗證其路徑跟蹤方面的有效性。

1 試驗樣車結構

試驗樣車由鋰電池（48 V、20 Ah）供電，采用前輪轉向、后輪驅動的4輪結構，輪距和軸距分別為550、840 mm（圖1）。前輪由L298N驅動器驅動電動推桿實現轉向，后輪電機和AQMD3608BLS驅動器驅動實現車輛行駛。通過霍爾編碼器將電機的位移量轉換成脈沖數，來檢測前輪轉向角（α）和后輪速度（v），進而實現閉環控制。選用4個UWB無線傳感器（I-UWB LPS PA型）組建室內定位系統，并與WT901C光纖陀螺儀（測量范圍-180°～180°，測量精度為0.1°）組合使用，獲得樣車的位置和姿態信息。主控制器采用STM32F767IGT6芯片，接收UWB定位系統和光纖陀螺儀傳來的位姿信息，換算成樣車相對于期望路徑的橫向偏差（d）和航向偏差（θ）。根據純追蹤算法計算樣車前輪轉向角（α），從而達到設施內道路路徑跟蹤的目的。

2 樣車位姿解算

在試驗樣車自動導航中，需要實時獲取車輛位置和姿態信息。車輛的姿態信息由WT901C光纖陀螺儀直接讀取，位置信息則通過UWB定位系統獲取，具體解算流程如下：采用4個UWB無線傳感器（基站）組建室內無線定位坐標系（圖2）。將移動信標安裝在試驗樣車上，當樣車在定位系統中移動時信標與4個基站建立通訊，根據信號到達時間（time of arrival，簡稱TOA）原理[10]，信標到每個基站的距離（r）可由如下方程組表示：

（x1-x）2+（y1-y）2=r21;

（x2-x）2+（y2-y）2=r22;

（x3-x）2+（y3-y）2=r23;

（x4-x）2+（y4-y）2=r24。（1）

式中：（x1，y1）、（x2，y2）、（x3，y3）、（x4，y4）分別為基站1、2、3、4的坐標;（x，y）為移動信標的坐標。r1、r2、r3、r4為各基站到信標的距離，其計算方式如下：

ri=（xi-x）2+（yi-y）2=cti。（2）

式中：c為光速，3×108 m/s;ti為信號在移動標簽到

第i個基站之間的傳輸時間，i=1，2，3，4。將式（1）中的后3個方程分別與第1個方程相減，可得：

[x y]T=Z·C。（3）

式中：Z=2x1-x2 y1-y2

x1-x3 y1-y3

x1-x4 y1-y4-1;

C=r22-r12+x12+y12-x22-y22

r32-r12+x12+y12-x32-y32

r42-r12+x12+y12-x42-y42。

式（3）為超靜定方程組，其中各基站的坐標（xi，yi）及移動標簽到各定位基站的距離（ri）均可以由UWB定位系統獲得。采用加權最小二乘法（weighted least squares，簡稱 WLS）[19]解該超靜定方程組，得到移動標簽坐標[x，y]T的最優解。[x，y]T的定位精度與基站的分布、標簽的位置、障礙物等密切相關。經過反復調試，當4個基站組成15 m×20 m的矩形時，此區域內能獲得良好的定位精度。

3 路徑跟蹤控制

3.1 預瞄偏差模型

自動導航是通過對樣車橫向控制來實現路徑跟蹤。參考駕駛員習慣，本研究引入預瞄點和前視距離的概念。如圖3所示，B點為車輛后輪中心（A）到期望跟蹤直線P1P2的投影，設C點為車輛預瞄點，根據弧長預瞄方法[20]，BC即為前視距離（L）。

圖3中，β為車身方向與AC連線的夾角，定義該角度為預瞄偏差角，并設當車在跟蹤路徑的左側時角度為正。根據幾何知識，預瞄偏差模型的前視距離（L）為

L=dtan（β+θ）。（4）

式中：d為橫向偏差，m;θ為航向偏差，°。橫向偏差（d）和航向偏差（θ）的正負號定義如下：在路徑跟蹤中，橫向偏差d為左正右負;根據時針運行方向，航向偏差θ為逆正順負。由式（4）可知，前視距離L由橫向偏差d、航向偏差θ和預瞄偏差角β決定，θ、d可由光纖陀螺儀和UWB定位系統得到，因此預瞄偏差角β是影響前視距離L的唯一參數。

3.2 純追蹤原理

純追蹤算法[13]是一種基于幾何模型的路徑跟蹤方法，通過設置預瞄點計算前輪轉向角α，并規劃車輛從當前位置到預瞄點的圓弧軌跡（如圖3中的圓弧AC）來實現路徑跟蹤，該方法簡單直觀且具有預見性，在計算過程中無需考慮車輛的動力學模型。由純追蹤原理，可得前輪轉向角α為

α=-acrtan2l（dcosθ+Lsinθ）L2+d2。（5）

式中：l為車輛軸距，m。

由式（5）可知，前視距離L決定前輪轉向角α的大小。前視距離較大時，車輛會沿數個曲率較小的弧線慢慢靠近期望路徑，振蕩小但收斂快速性較差;當前視距離較小時，車輛會沿曲率較大的弧線快速靠近期望路徑，但車輛會產生振蕩。因此，可以通過控制β不斷收斂來獲得動態的前視距離L，進而控制前輪轉向角α來調整車輛位姿逐步向期望路徑收斂。本研究將設計一種MFAC算法，根據實時預瞄偏差β信息對前視距離L進行動態調整。

4 無模型自適應控制系統

4.1 控制系統框架

如圖4所示，β*（k）為當前時刻（k）預瞄偏差角的期望輸出，通常為0°。通過位姿和轉角傳感器獲取車輛前一時刻（k-1）時的狀態數據β（k-1）、α（k-1），與β*（k）比對后，輸入MFAC，產生當前時刻（k）的預瞄偏差角[β（k）]，將β（k）帶入預瞄偏差模型[式（4）]，就可求出此刻最合適的前視距離 [L（k）]，再將前視距離帶入式（5），就可得到此刻理想的前輪轉角[α*（k）]，以此控制車輛調整姿態。不斷循環便可將車輛逐步收斂到期望路徑上。因此，子系統MFAC是整個控制系統設計的重點。

4.2 MFAC設計

由式（4）和式（5）可知，前輪轉向角α和預瞄偏差角β存在非線性關系，可用一般離散時間描述 k+1時刻的β值：

β（k+1）=f[β（k），…，β（k-kβ），α（k），…，α（k-kα）]。（6）

式中：f（·）是未知非線性函數;kα和kβ是須追溯參數的時間寬度。

根據文獻[21]，式（6）須滿足以下3種假設：

假設1，子系統MFAC輸入輸出變量α和β均須可測、可控、有邊界，且輸出變量β會由系統輸入信號驅動迭代逐漸接近期望輸出信號β*;

假設2，f（·）關于輸入信號中α（k）的偏導數是連續的;

假設3，子系統MFAC滿足廣義Lipschitz相關條件。

因此符合基于緊格式的動態線性化模型，使得子系統MFAC轉換為

Δβ（k+1）=（k）Δα（k）。（7）

式中：Δα（k）=α（k）-α（k-1）;Δβ（k+1）=β（k+1）-β（k）;（k）為偽偏導數，|（k）|≤b，且b是一個正數。

為保證前輪轉向角α急劇轉向引起車輛失穩以及消除路徑跟蹤誤差，本研究引入輸入準則函數和估計準則函數，公式如下：

J[α（k）]=|β*（k+1）-β（k+1）|2+λ|α（k）-α（k+1）|2;（8）

J[（k）]=|β（k）-β（k-1）-（k）Δα（k-1）|2+μ|（k）-（k-1）|2。（9）

式中：μ>0;λ>0;β*為期望輸出。將式（7）代入工（8）中，對α（k）求導等于0;同時式（9）對 （k） 求極值，得其近似估計值：

^（k）=^（k-1）+η[Δβ（k）-^（k-1）Δα（k-1）]Δα（k-1）μ+|Δα（k-1）|2;（10）

α（k）=α（k-1）+（k）[β*（k+1）-β（k）]λ+|（k）|2。（11）

式中：步長因子η∈（0，2]。

為了使估計算法具有較好的適應性和收斂性，（k）須滿足如下約束條件

|（k）|2≤ε‖Δα（k-1）≤ε;

sign[（k）]≠sign[（1）];

（k）=（1）。（12）

式中：ε取正數最小值。由式（10）、式（11）和式（12）可知，該系統需要設定起始時刻的前視距離 L（1）、估計偽偏導 ^（1）以及λ、μ、η。

5 試驗結果與討論

5.1 實車試驗

為檢驗本研究算法的有效性，在浙江農林大學官塘農場溫室中進行實車試驗。試驗環境及場地布置如圖1和圖2所示。試驗樣車的采樣頻率和車速分別為10 Hz、1 m/s。采用最佳固定前視距離（經計算機反復仿真，取0.8 m）的純追蹤模型（簡稱L-fixed法）進行對比試驗。對各性能指標的定義如下：平均偏差是從試驗開始到結束所有橫向偏差絕對值的平均值，m;平均標準差是各個偏差偏離平均偏差距離的均值，m;穩定距離是指樣車首次收斂至穩定點（距離期望路徑±0.02 m以內）時樣車所行駛的縱向距離，m;調整時間是指從初始狀態行駛到穩定點所需的時間，s;穩定距離和調整時間這2個指標反映路徑跟蹤的收斂速度;穩態偏差是指從穩定點向后的平均橫向偏差，m，該指標反映了路徑跟蹤精度。

5.1.1 試驗1 樣車分別從4種不同的初始狀態[（d，θ）分別為（0.7 m，-70°）、（-0.8 m，80°）、（0.9 m，-20°）、（-0.5 m，40°）]出發，跟蹤全長為15 m的直線路徑。本研究方法（簡稱L-MFAC法）中，系統初始時刻[L（1）]取最佳固定前視距離，即為0.8 m，估計偽偏導^=0.5，λ=18，μ=1，η=1。L-MFAC法和L-fixed法直線追蹤的對比試驗軌跡與數據統計分別如圖5和表1所示。

5.1.2 試驗2 為進一步驗證樣車在設施園藝狹小空間中轉向能力，設計了19 m×9 m的矩形跟蹤路線。出發的初始位置為（0，0），依次經過點（19，0）、點（19，9）、點（0，9）后回到點（0，0），初始橫向、航向偏差均為0°（圖6）。考慮到系統的響應時間及試驗樣機的速度，設定距離下一條路徑不足 2.0 m 時更新路徑。矩形追蹤的對比試驗結果與數據統計分別如圖6和表2所示。

5.2 試驗結果與分析

如圖5、表1所示，在UWB定位系統位姿偏差可測范圍內，4種狀態下本研究基于MFAC的純追蹤模型直線跟蹤的平均偏差分別為0.102、0.079、0.181、0.084 m，而固定前視距離L-fixed純追蹤模型平均偏差分別為0.141、0.122、0.198、0.118 m。當試驗樣機行駛到穩定狀態時，本研究 L-MFAC 法的穩態偏差分別為0.013、0.015、0.018、0.033 m，而作為對比的L-fixed法分別為0.067、0.038、0.024、0.081 m。由此可知，L-MFAC法平均能夠降低一半以上的穩態誤差率;在試驗2中，本研究的L-MFAC法和對比的L-fixed法平均偏差分別為0.139、0.265 m（圖6、表2），表明本研究方法具有更好的跟蹤精度。

由表1可知，本研究的L-MFAC法和L-fixed法路徑跟蹤穩態偏差的標準差分別為0.008、0.010、0.013、0.018 m和0.017、0.013、0.019、0.023 m，前者均值為0.012 m，而后者均值為 0.018 m，前者明顯小于后者。說明本研究基于MFAC的純追蹤模型轉向較為平緩且穩定，可以提高運輸車在設施園藝抗側翻性能。

收斂性能的快慢是檢驗試驗樣機路徑跟蹤的重要性能之一，即調整時間是表明受控系統的收斂性能。由表1可知，本研究所用L-MFAC法的調整時間為5.6、6.1、7.6、6.8 s，對應的穩定距離分別為4.210、4.970、6.110、5.590 m。而L-fixed法調整時間分別為6.6、6.4、10.4、8.1 s，對應穩定距離分別為5.570、5.020、8.800、6.260 m。可知，本研究所用L-MFAC法確定前視距離具有較好的適應性，收斂效果要優于作為對比的L-fixed法。

由表2可知，本研究的L-MFAC法平均偏差的標準差為0.111m，而L-fixed法平均偏差的標準差為0.208 m，穩定性能提高了46.6%。同時由圖6可知，最大偏差均出現在轉彎處，這是由于試驗樣機存在最小轉彎半徑且更換當前路徑，因此試驗樣機需要重新收斂至最新路徑。

6 總結與展望

本研究采用UWB無線傳感器組建室內定位系統，并與光纖陀螺儀組合使用，精準獲取移動平臺的位姿偏差信息，能夠解決在設施園藝環境下農業機械的定位與導航難題。

利用無模型自適應算法方法動態確定純追蹤模型的前視距離，并設計直線跟蹤和矩形路徑跟蹤2個試驗。試驗結果表明，在直線路徑跟蹤下，其穩態偏差均值為0.020 m;在矩形路徑跟蹤下，平均偏差和標準差分別為0.139、0.111 m，可以滿足園藝設施內作業平臺的導航要求。

超寬帶定位精度易受到外界環境的影響，而在本研究中只考慮移動標簽與定位基站之間無遮擋的情況，屬于理想使用環境，在后續研究中，可針對有遮擋物時的定位測量，分析不同遮擋物對定位精度的控制。另外，可考慮選用4輪獨立驅動獨立轉向新型結構，以減少大角度轉向時的車體打滑現象。

參考文獻：

[1]齊 飛，魏曉明，張躍峰. 中國設施園藝裝備技術發展現狀與未來研究方向[J]. 農業工程學報，2017，33（24）：1-9.

[2]Jang W J，Lewis G，Hoachuck J，et al. Vibration-reducing path tracking control for a strawberry transport robot[C]//American Society of Agricultural and Biological Engineers .Proceedings of 2014 ASABE and CSBE/SCGAB annual international meeting. Montreal，2014：141914011.

[3]劉柯楠，吳普特，朱德蘭，等. 基于GPS的太陽能平移式噴灌機自主導航系統設計與試驗[J]. 農業工程學報，2016，32（16）：89-94.

[4]郭 娜，胡靜濤，王 鶴. 基于GPS導航的插秧機作業控制系統[J]. 農業機械學報，2013，44（1）：200-204.

[5]黎永鍵，趙祚喜，高俊文. 基于GPS/SINS組合的農業導航定位系統設計與研究[J]. 農機化研究，2014，36（3）：16-22.

[6]Yin X，Du J，Noguchi N，et al. Development of autonomous navigation system for rice transplanter[J]. International Journal of Agricultural and Biological Engineering，2018，11（6）：89-94.

[7]李云伍，徐俊杰，王銘楓，等. 丘陵山區田間道路自主行駛轉運車及其視覺導航系統研制[J]. 農業工程學報，2019，35（1）：52-61.

[8]Lee I N，Lee K H，Lee J H，et al. Autonomous greenhouse sprayer navigation using automatic tracking algorithm[J]. Applied Engineering in Agriculture，2015，31（1）：17-21.

[9]孟慶寬，何 潔，仇瑞承，等. 基于機器視覺的自然環境下作物行識別與導航線提取[J]. 光學學報，2014，34（7）：180-186.

[10]戴 波，呂 昕，劉學君，等. 基于改進的超寬帶危化品倉儲堆垛定位技術[J]. 上海交通大學學報，2016，50（6）：963-967，974.

[11]符世琛，李一鳴，楊健健，等. 基于超寬帶技術的掘進機自主定位定向方法研究[J]. 煤炭學報，2015，40（11）：2603-2610.

[12]林相澤，王 祥，林彩鑫，等. 基于超寬帶的溫室農用車輛定位信息采集與優化[J]. 農業機械學報，2018，49（10）：23-29，45.

[13]姚立健，Pitla S K，楊自棟，等. 基于超寬帶無線定位的農業設施內移動平臺路徑跟蹤研究[J]. 農業工程學報，2019，35（2）：17-24.

[14]汪小旵，魯 偉，陳 滿，等. 基于改進純追蹤模型的溫室采摘運輸自動跟隨系統[J]. 農業機械學報，2016，47（12）：8-13.

[15]李 革，王 宇，郭劉粉，等. 插秧機導航路徑跟蹤改進純追蹤算法[J]. 農業機械學報，2018，49（5）：21-26.

[16]王建邦，袁智勇，陳 波，等. 變電站巡檢機器人數據驅動無模型自適應控制[J]. 電測與儀表，2019，56（19）：114-120.

[17]侯忠生，董航瑞，金尚泰.基于坐標補償的自動泊車系統無模型自適應控制[J]. 自動化學報，2015，41（4）：823-831.

[18]Wang X F，Li X，Wang J H，et al. Data-driven model-free adaptive sliding mode control for the multi degree-of-freedom robotic exoskeleton[J]. Information Sciences，2016，327：246-257.

[19]劉書靜，羅海勇，吳 彬，等. 基于最小二乘測距定位算法信標最優部署模型[J]. 計算機學報，2013，36（3）：546-556.

[20]李 爽，徐延海，陳 靜，等. 基于弧長預瞄的車輛側向跟蹤控制研究[J]. 汽車工程，2019，41（6）：668-675.

[21]王衛紅，侯忠生，霍海波，等. 基于數據驅動方法的控制器設計及其參數整定[J]. 系統科學與數學，2010，30（6）：792-805.