丘陵山地果園行間運輸車自動跟隨系統研究

摘要：針對當前丘陵山地地區果園機械智能化程度低、采摘和運輸過程效率低的問題，設計一種基于自研履帶式運輸車的自動跟隨方法，采用基于AOA（入射方向角）的定位原理，利用UWB雙基站定位技術，由兩基站接收電子標簽信號，獲得其方向角。并通過擴展卡爾曼濾波法計算出電子標簽的準確位置，實現運輸車對標簽的精確定位跟隨。在MATLAB中仿真分析，結果表明擴展卡爾曼濾波-AOA定位法平均距離誤差為0.21 m，平均角度誤差為1.45°。在平地和丘陵果園兩種工況下做試驗驗證，結果表明：兩種工況下平均距離誤差均小于30 cm，證明該方法滿足自動跟隨需求。

關鍵字：履帶式運輸車；自動跟隨；雙基站定位技術；入射方向角；擴展卡爾曼濾波

中圖分類號：S229+.1" " " 文獻標識碼：A" " " 文章編號：2095?5553 （2024） 09?0178?07

Research on automatic following system of inter?row transporter

in hilly and mountainous orchards

Zheng Tuo Yuan Lifeng Qi Dezhong Yan Xingxing Yang Wenguang

（1. Hubei Agricultural Machinery Engineering Research and Design Institute， Wuhan， 430068， China; 2. Hubei Provincial

Agricultural Machinery Equipment Intelligent Engineering Technology Research Center， Wuhan， 430068， China）

Abstract： Aiming at the problems of low degree of mechanical intelligence and low efficiency of picking and transportation process in hilly and mountainous areas， this paper designs an automatic following method based on self?developed crawler transporter， adopts the positioning principle based on AOA （angle of incidence）， and uses UWB dual base station positioning technology to receive the electronic tag signal and obtain its direction angle. And by extending the Kalman filter method， the accurate position of the electronic tag is calculated to realize the precise positioning and following of the tag by the transport vehicle. In the simulation analysis in MATLAB， it is concluded that the average distance error of the extended Kalman filter-AOA positioning method is 0.21 m and the average angle error is 1.45°. The results show that the average distance error is less than 30 cm under the two working conditions of flat land and hilly orchard， which proves that the proposed method meets the requirements of automatic following.

Keywords： tracked transporter; automatic following; dual base station positioning technology; angle of incidence direction; extended Kalman filtering

0 引言

我國是水果種植和消費大國，種植面積和產量均位于世界前列，種類超過50余種，隨著近年來國家的政策扶持，各種惠農政策實施，使水果種植規模得到了大發展。根據調查研究，2022年我國水果總產量為312 962.4 kt，種植面積已經達到了13 009.53 khm2，從2012—2022年這十年間水果產量增加了41.67%，種植面積增加了18.4%[1]，且主要集中在丘陵山地地區。可見水果產業是我國農業經濟的重要支柱產業。但我國果園生產工具機械化和智能化水平較低，很多地區最主要的作業方式依舊是人工作業，加上丘陵山地地區交通不便、道路情況復雜，人工作業效率低下、危險性高。因此解決果園生產裝備智能化問題，將對林果產業的發展具有重要作用。在果園產業智能化問題中，最主要的是采摘和運輸過程的智能化。目前國內果園采摘過程大部分依舊是人工采摘，采用扁擔挑籮筐的方式，邊采摘邊移動，采摘過程緩慢；運輸方式大多都是采用輪式拖拉機進行運輸，這種方式雖然運輸量大，但是也存在很多問題。首先，輪式拖拉機體積較大，通過性差，而果園的果樹之間間距一般比較小，容易損傷果樹枝葉；其次，丘陵山地地區地形復雜，坡度較大，輪式拖拉機在這種路況中行走和轉向較為困難，容易發生危險；第三，輪式拖拉機智能化程度低，對水果的保護措施較少，在運輸水果過程中容易損傷果實，造成經濟損失。從各種實踐數據中表明，水果的品質對其價格影響很大，采摘前的水果品質主要受氣候、自然災害、病蟲害等影響，屬于不可控因素。采摘和運輸過程中的損傷屬于可控因素[2]。據調查，在水果采摘和運輸過程中造成的經濟損失，每年可達上百億元人民幣。

果園運輸機械的發展歷史中，國外起步較早且一直處于領先地位，尤其以歐美國家為代表。其中一個重要原因是美洲與國內的地理環境和種植方式有較大差異。美洲地區大部分為平原，種植面積為連續的大片區域，因此運輸機械一般以大型輪式運輸機、重型貨車和半掛式牽引汽車為主。丘陵山地地區地形復雜，路面起伏較大，考慮到作業人員和作業機械的安全，在20世紀末期，西方國家便研制出一款車身可自動調平的山地拖拉機，最大調平角度可達27°；美國的公司研發出一款型號為CR9070山地拖拉機，該拖拉機采用齒輪齒條相互配合的方式來調整兩側的輪胎高度，利用蝸輪蝸桿傳動帶動主動輪旋轉，并利用其自鎖效應固定齒輪高度，可以實現車身的自動調平[3]。在定位跟隨領域，國外學者也做了大量研究。日本生研研究中心研發的基于電磁誘導的自動導航系統，具體工作方式是：將電纜埋藏在機器行走的路線上，機器通過磁傳感器計算修正偏差，進而完成自動導航；美國卡內基梅隆大學研發的農機導航系統，利用GPS、三軸陀螺儀、攝像頭、雷達等多種儀器組合，通過傳感器對環境信息的識別完成農機的自動定位導航。這兩種方式雖然定位精度較高，但是技術復雜、經濟成本高，因此很難進行大規模使用。沈鳳梅等[4]基于改進的人工勢場法，利用滾動視窗原理，將環境中的大型障礙物通過邊緣化處理，實現了移動機器人自主導航和避障功能。章小兵等[5]采用將空間分區的方法，并基于多超聲波傳感器定位方法，可以實現移動機器人在室內的精確定位和避障。

雖然目前在機器人定位跟隨方面已經有大量學者做了研究，但是研究方向主要集中在平原地區和室內移動機器人領域，對丘陵果園行間運輸機械的研究較少。因此本文將針對丘陵果園行間運輸車發展現狀，設計一種基于擴展卡爾曼濾波-AOA的自動定位跟隨方法，并對其進行仿真分析和試驗驗證。

1 理論研究

目前的UWB定位技術[6]中，應用最廣泛的方法有TOA（基于到達時間）、TDOA（基于到達時間差）和AOA（基于到達角度）等。在本文中，信號的入射方向角利用帶天線矩陣的UWB基站模塊自行測出，并通過端口將角度數據發送回主控器。主控器對角度信息解算后即可得到標簽位置。

1.1 定位方法

1.1.1 TOA/TDOA定位

TOA定位原理是將基站和移動標簽之間信號的傳輸時間轉化為距離，該種定位方法需要三個基站進行組合來滿足果園運輸車按照指定路線運動的要求。定位原理圖如圖1所示。

圖1中，B1、B2、B3為基站，L為移動標簽。定位原理為：L分別向3個基站發送信號，3個基站根據接收到信號的時間得到3個不同半徑的圓，圓的交點即是標簽的位置。假設發射源位置為（x，y），傳感器節點的坐標為（xi，yi），i=1，2，3。則幾何關系滿足

由于TOA定位完全依賴于到達時間，因此最明顯的缺點是對系統時間同步性要求高，且需要基站數量較多，當外界干擾時基站圓不能交于L點會導致無法完成運輸小車的精準定位。

TDOA定位法在TOA的基礎上進行改進，先計算各基站接收信號的時間差，之后根據數學算法求解距離[7， 8]，相對于TOA定位精度更高。

1.1.2 AOA定位

AOA定位是基于到達角度的兩基站定位方式，由兩基站獲取信號的入射方向角，其交點即是標簽位置。定位原理如圖2所示。

圖2中，標簽L到基站B1的入射角為α，到B2的入射角為β，假設B1坐標為（x1，y1），B2坐標為（x2，y2），L坐標為（x，y），則滿足關系如式（2）所示。

AOA定位[9]相對TOA和TDOA的優點是定位原理簡單，且只需兩個基站即可完成定位。因此在本文中采用AOA雙基站定位技術。

果園運輸車基站安裝位置以及定位標簽如圖3所示。

1.2 濾波方法

普通卡爾曼濾波算法基于系統為線性高斯模型，實際應用中，大多數系統為非線性系統，因此需要對普通卡爾曼濾波算法做優化處理，引入擴展卡爾曼濾波（EKF），擴展卡爾曼濾波算法的核心是：對濾波值處理，通過將非線性系統用泰勒級數展開，并省略二階及二階以上項，得到一個近似的線性化高斯模型。再利用卡爾曼濾波完成系統狀態估計。系統方程如式（3）所示。

引入擴展卡爾曼濾波后，由非線性函數計算系統狀態預測和觀測值預測，線性卡爾曼濾波中狀態轉移矩陣A和觀測矩陣H則由f和h函數的雅克比迭代矩陣代替。假設系統狀態有n維，則

1.3 數學建模

前文可知該研究采用AOA定位方法，AOA獲取角度的原理是基于基站接收標簽信號的相位差計算角度，在本文中，方向角由帶天線陣列的基站自行測出。運輸車跟隨定位可轉化為二維平面定位，跟隨模型如圖4所示。

圖4中，假設點劃線代表移動標簽的運動軌跡，M代表某一時刻標簽的位置，（x，y）代表該時刻標簽的坐標，為未知量，（vx，vy）代表標簽的移動速度。A、B分別為運輸車的兩個基站且AB連線平行于X軸，A基站坐標為（x1，y1），B基站坐標為（x2，y2），M與A的夾角為α，與B的夾角為β。A、B基站的坐標及夾角α、β均為已知量。根據擴展卡爾曼濾波算法[11]，假設采樣周期為T，標簽勻速運動，可得到系統狀態模型為

2 仿真分析

2.1 距離誤差分析

在MatLab中建立仿真模型，得到兩組隨機距離誤差如圖5所示。由圖5可知，在隨機的兩次仿真中，最小二乘法濾波效果最差，普通卡爾曼濾波法優化效果適中，但誤差跳動范圍大，穩定性差。擴展卡爾曼濾波法誤差優化效果最好，各方法距離誤差平均值與均方差對比如表1所示。

由表1可知，普通卡爾曼濾波法相對于最小二乘法平均距離誤差優化55.8%，均方差優63.4%，擴展卡爾曼濾波與卡爾曼濾波相比，距離誤差平均值優化82.7%，均方差優化86.5%。

綜上所述可看出，在距離誤差對比中，擴展卡爾曼濾波法優化效果相比普通卡爾曼濾波法有很大改善。根據高陽[13]的研究，距離誤差小于0.3 m為高精度定位。由圖5可看出，兩次仿真中擴展卡爾曼濾波法距離誤差基本保持在0.3 m以下，證明該方法定位效果良好。

2.2 角度誤差分析

角度誤差[14]是衡量定位算法精確度的一個重要標準，兩組隨機角度誤差仿真如圖6所示。

由表2可知，普通卡爾曼濾波法[15]與最小二乘法相比，平均值優化6.9%。均方差優化10.4%；擴展卡爾曼濾波法與普通卡爾曼濾波法相比，角度誤差平均值優化16.2%，均方差優化38.8%。從圖6中可以看出，最小二乘法和普通卡爾曼濾波法角度誤差不穩定，跳動過大。擴展卡爾曼濾波法在前15個節點波動較大，之后趨于穩定，誤差保持在1.6°左右。對比可知擴展卡爾曼濾波法效果最佳。

2.3 軌跡分析

由圖7可看出，在兩次仿真中，擴展卡爾曼濾波真實軌跡與原始軌跡基本重合，定位精度最高。卡爾曼濾波法的真實軌跡沿著原始軌跡上下波動，穩定性較差。雖然最小二乘法仿真軌跡也在實際軌跡線上下跳動，但是誤差過大，效果最差。

通過對距離誤差、角度誤差、軌跡誤差進行仿真分析，結果證明擴展卡爾曼濾波的優化效果最優。后續將進行試驗驗證擴展卡爾曼濾波-AOA法的定位效果。

3 試驗驗證

3.1 試驗方法

本文試驗分為兩種工況，第一種為平地試驗，地點選擇平坦的水泥路面，試驗現場如圖8所示。

具體試驗步驟如下：首先確定工作人員的移動路線（曲線），在路線上每隔10 m做一個標記，作為原始路線。運輸車在后方定位跟隨時，每到達一個標記附近時，做另一個標記，作為運輸車定位的實際路線。之后測量原始路線標記與實際路線標記之間的偏差值作為評定定位效果好壞的標準。試驗時，工作人員移動的距離為150 m，共標記15個點位。兩種工況分別做兩次測試，各得到兩組試驗值。

3.2 試驗結果

平地試驗測得的偏差數據如圖11所示。試驗數據中，最大誤差為26 cm，最小誤差為18 cm。兩組平地試驗的誤差平均值在22 cm左右，均小于30 cm；與仿真平均值相比，誤差率均小于10%，表明該算法滿足在平地工作的需求。

丘陵果園試驗測得的偏差數據如圖12所示。在丘陵芒果園地區試驗[16， 17]時，最大誤差為28 cm，最小誤差為20 cm，平均誤差值保持在24 cm附近，均小于30 cm。表明該方法滿足丘陵果園使用需求。但與平地試驗相比，在丘陵果園試驗時誤差值略高。

對比兩種工況的測試結果，分析丘陵果園定位精度略低于在平坦路面精度的原因是：丘陵果園地區環境條件較為復雜，路面不平度相比平地較大，且果樹枝葉對信號的傳輸也有一定干擾。但從總體上看，擴展卡爾曼濾波-AOA定位法應用于自動跟隨系統時，在平坦路面和丘陵果園環境下均可滿足實際需求。

4 結論

1） 構建運輸車二維平面定位模型，采用AOA雙基站定位技術對丘陵山地果園行間運輸車的自動跟隨系統進行設計，通過對系統模型進行狀態預測與更新，最終計算出電子移動標簽的位置。

2） 與普通卡爾曼濾波法、最小二乘法作對比，對擴展卡爾曼濾波-AOA定位法進行仿真分析。距離誤差基本保持在0.3 m以下，與普通卡爾曼濾波法相比距離誤差平均值優化82.7%，均方差優化86.5%；角度誤差保持在16°左右，與普通卡爾曼濾波法相比角度誤差平均值優化16.2%，均方差優化38.8%，仿真結果表明，該定位方法效果良好。

3） 在平坦的水泥路面和丘陵果園中分別對運輸車進行自動跟隨試驗。平地試驗的誤差平均值在22 cm左右；丘陵果園試驗平均誤差值保持在24 cm附近，均小于30 cm。試驗結果表明，擴展卡爾曼濾波-AOA定位法應用于自動跟隨系統時，在平坦路面和丘陵果園環境下均可滿足實際需求。

參 考 文 獻

[ 1 ] 郭林. 丘陵山地果園運輸車減振降損性能研究[D]. 武漢： 湖北工業大學， 2021.

Guo Lin. The vibration and damage reduction properties study of orchard transporter in hilly mountainous areas [D]. Wuhan： Hubei University of Technology， 2021.

[ 2 ] 鄧秀新. 關于我國水果產業發展若干問題的思考[J]. 果樹學報， 2021， 38（1）： 121-127.

Deng Xiuxin. Thoughts on the development of China's fruit industry [J]. Journal of Fruit Science， 2021， 38（1）： 121-127.

[ 3 ] 吳瓊， 周然. 運輸振動對水果貯藏品質影響的研究進展[J]. 食品工業科技， 2017， 38（11）： 356-362， 368.

Wu Qiong， Zhou Ran. Advances in research of effects of transportation vibration on storage quality of fruits [J]. Science and Technology of Food Industry， 2017， 38（11）： 356-362， 368.

[ 4 ] 沈鳳梅， 吳隆. 基于改進人工勢場法的移動機器人自主導航和避障研究[J]. 制造業自動化， 2013， 35（23）： 28-30， 39.

Shen Fengmei， Wu Long. Study of mobile robot's autonomous navigation and obstacle avoidance based on improved artiicial potential ield algorithm [J]. Manufacturing Automation， 2013， 35（23）： 28-30， 39.

[ 5 ] 章小兵， 宋愛國， 唐鴻儒. 一種主要基于多超聲波傳感器定位的新方法[J]. 制造業自動化， 2006（1）： 29-32.

Zhang Xiaobing， Song Aiguo， Tang Hongru. A new method of localization mainly on the basis of multi ultrasonic sensors [J]. Manufacturing Automation， 2006（1）： 29-32.

[ 6 ] ünal ?. Integration of ZigBee based GPS receiver to CAN network for precision farming applications [J]. Peer?to?Peer Networking and Applications， 2020， 13（5）： 1-12.

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

Lin Xiangze， Wang Xiang， Lin Caixin， et al. Location information collection and optimization for agricultural vehicles based on UWB [J]. Transactions of the Chinese Society for Agricultural Machinery， 2018， 49（10）： 23-29， 45.

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

Yao Lijian， Santosh K Pitla， Yang Zidong， et al. Path tracking of mobile platform in agricultural facilities based on ultra wideband wireless positioning [J]. Transactions of the Chinese Society of Agricultural Engineering， 2019， 35（2）： 17-24.

[ 9 ] 解彬彬， 劉繼展， 何蒙， 等. 改進AOA模式的大田農機無人駕駛導航參數檢測系統設計[J]. 農業工程學報， 2021， 37（14）： 40-51.

Xie Binbin， Liu Jizhan， He Meng， et al. Design of detection system for the unmanned navigation parameters of field agricultural machines based on improved AOA mode [J]. Transactions of the Chinese Society of Agricultural Engineering， 2021， 37（14）： 40-51.

[10] 任昊譽， 郭晨霞， 楊瑞峰. 卡爾曼濾波提高UWB測距精度研究[J]. 電子測量技術， 2021， 44（18）： 111-115.

Ren Haoyu， Guo Chenxia， Yang Ruifeng. Research on improving of UWB ranging accuracy by Kalman filter [J]. Electronic Measurement Technology， 2021， 44（18）： 111-115.

[11] 王嫣， 常曉鵬， 張建平. 基于EKF算法的UWB和ZigBee測量技術的混合運動目標定位[J]. 應用科學學報， 2019， 37（6）： 815-824.

Wang Yan， Chang Xiaopeng， Zhang Jianping. Hybrid moving object localization with EKF based on measurement techniques of UWB and ZigBee [J]. Journal of Applied Sciences， 2019， 37（6）： 815-824.

[12] Jin K. UWB localization algorithm based on BP neural network compensation extended Kalmanfilter [J]. Journal of Physics： Conference Series， 2021， 1885（4）.

[13] 高陽. 用于AOA室內定位的UWB設備的設計與實現[D]. 成都： 電子科技大學， 2021.

Gao Yang. Design and implementation of UWB equipment for AOA indoor positioning [D]. Chengdu： University of Electronic Science and Technology of China， 2021.

[14] 喬少杰， 韓楠， 朱新文， 等. 基于卡爾曼濾波的動態軌跡預測算法[J]. 電子學報， 2018， 46（2）： 418-423.

Qiao Shaojie， Han Nan， Zhu Xinwen， et al. A dynamic trajectory prediction algorithm based on Kalman filter [J]. Acta Electronica Sinica， 2018， 46（2）： 418-423.

[15] 張桀， 沈重. 聯合TDOA改進算法和卡爾曼濾波的UWB室內定位研究[J]. 現代電子技術， 2016， 39（13）： 1-5.

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

Wang Xiaochan， Lu Wei， Chen Man， et al. Automatic following system for greenhouse harvesting transportation based on adaptive pure pursuit model [J]. Transactions of the Chinese Society for Agricultural Machinery， 2016， 47（12）： 8-13.

[17] 胡煉， 林潮興， 羅錫文， 等. 農機具自動調平控制系統設計與試驗[J]. 農業工程學報， 2015， 31（8）： 15-20.

Hu Lian， Lin Chaoxing， Luo Xiwen， et al. Design and experiment on auto leveling control system of agricultural implements [J]. Transactions of the Chinese Society of Agricultural Engineering， 2015， 31（8）： 15-20.