基于EKF的農機智能體自主導航算法研究

孟福軍 岳勝如

摘要：為獲得更加準確、全面、實時的農田障礙物信息，提高農業機械智能體自主導航定位的精度，提出一種基于北斗系統和視覺導航的組合定位方法。針對農田環境，選擇BDS、視覺CCD為外部傳感器，設計一種基于擴展卡爾曼濾波器（EKF）的數據融合算法，該算法融合了BDS和視覺傳感器數據，實時定位農機智能體的位置。系統通過對導航角度和行駛進度進行跟蹤，完成絕對定位。通過機器視覺圖像處理，獲取導航基準和作業目標信息，完成相對定位。通過試驗驗證該算法的有效性，并通過卡爾曼濾波算法（KF）的成果進行對比分析。結果表明：濾波后的路徑更平滑，抖動偏差減小，坐標數據比KF濾波結果更穩定、更平滑。此外，距離的平均誤差可以從濾波前的0.119 5 m降低到濾波后的0.07 0 m，有效地降低了過程噪聲。且位置偏差在±0.1 m以內，精度較高，提升了農機智能體自主導航的定位精度。

關鍵詞：農機智能體；障礙物檢測；EKF；BDS

中圖分類號：S126： TP273

文獻標識碼：A

文章編號：2095-5553 （2023） 04-0181-07

Abstract： In order to obtain more accurate， comprehensive and real-time farmland obstacle information and improve the autonomous navigation and positioning accuracy of agricultural machinery agents， a combined positioning method based on Beidou system and visual navigation was proposed. In this paper， a data fusion algorithm based on extended Kalman filter （EKF） is designed， which selects BDS and visual CCD as external sensors. The algorithm integrates BDS and visual sensor data to locate the real-time location of the agent. The system completes absolute positioning by tracking navigation angle and driving progress. Through machine vision image processing， navigation reference and target information are obtained to complete relative positioning. The effectiveness of the algorithm is verified by experiments， and the results of Kalman filter （KF） are compared and analyzed. The results show that the filtered path is smoother， the jitter deviation is reduced， and the coordinate data is more stable and smoother than the KF filtering result. In addition， the mean distance error can be reduced from 0.119 5 m before filtering to 0.070 m after filtering， effectively reducing the process noise. And the position deviation is within ±0.1 m， the accuracy is higher， which can improve the positioning accuracy of intelligent agricultural machinery autonomous navigation.

Keywords： agricultural machinery intelligent agent; obstacle detection; EKF; BDS

0 引言

在野外環境中，通過整合各種不同的導航傳感器信息，彼此借鑒各自的優點，以獲得比單一傳感器更好的導航性能，對復雜環境、光照等周圍環境的感知，可以實現農機智能體（農業智能機器人）的實時準確定位［1-3］。使得智能體能完成多項作業，廣泛應用于農耕、播種、施肥、噴施等農業生產過程中。通過傳感器獲取周圍環境的信息，可以有效地繞過障礙物，農機智能體可以自主完成從起點到目標點的路徑選擇［4-6］。華南農業大學研制的拖拉機自動導航定位控制系統在拖拉機速度為2.88 km/h時，拖拉機的跟蹤誤差在0.3 m［7-8］；中國農業大學研制的拖拉機自動導航控制系統能實時地進行數據處理和定位導航，并且能夠較好地進行路線追蹤，最大橫向偏差為11 cm［9-10］。我國自主研制BDS可在全球范圍內提供全天候、全天時的高精度、高可靠性的即時定位服務，在使用成本和安全性上均優勝GPS定位系統［11-14］。針對單一傳感器在智能車輛環境感知中的局限性，薛金林等［15］提出一種基于攝像機與激光雷達信息融合的農業自主車輛前方障礙物檢測方法，在機器視覺的基礎上融入激光雷達數據，可以更好地排除非障礙物的干擾，實現了障礙物的完整檢出。在以上研究基礎上，本文基于BDS和視覺CCD傳感器進行組合導航，結合EKF算法，借助多傳感器融合，進行了一種農機智能體自主導航算法的研究。

1 基于BDS和視覺CCD的多傳感器信息融合原理

1.1 組合導航模型

首先利用BDS系統采集驅動速度、航向角和絕對位置信息，在農機智能體上安裝CCD攝像機，利用圖像處理算法識別作物行的相對位置，在位置信息的基礎上，生成導航參考線，獲得導航行程中的特征點；然后將獲得的信息進行坐標轉換，實現坐標系統一，并由融合后的數據生成新的數據；最后以相對坐標變換來計算出農田障礙物的相對位姿。圖1給出了導航模型示意圖。

1.2 建立坐標系及坐標轉換模型

1）? 坐標系的建立。如圖2所示，主要由農機智能體本體坐標系OB-XBYBZB、視覺坐標系o-xyz和地理坐標系O-XYZ組成。BDS提供的是CGCS2000坐標，在數據融合前，需將CGCS2000大地坐標換算為平面坐標，本文采用的是高斯平面直角坐標［9］。視覺坐標系坐標原點為農機智能體前端地面上中間的投影點、航向相反的方向為x軸，y軸與x軸垂直而建立的右手坐標系。

根據圖4、圖5和圖6，EKF未返回的數據具有大而不均勻的路徑抖動和大的處理噪聲。從圖4可以看出，EKF濾波后的位置誤差較觀測值變小很多；圖6中濾波運動軌跡較測量運動軌跡更加平滑，所得的坐標數據更加穩定。從圖5可以看出，與KF算法相比，EKF算法后的數據更平滑，濾波效果更好，更接近真值。從上述結果可以看出，融合兩種傳感器的EKF定位算法的定位效果更好。

3.1.2 長度試驗

首先在地面上距離試驗儀器位置M點間隔1 m、5 m、10 m、20 m、50 m和100 m的地點分別標記為S點、P點、Q點、R點、L點、X點。智能小車沿直線移動，并分別記錄在M、S、P、Q、R、L、X點上。將濾波前后各位置的經緯度坐標轉換為平面坐標，然后用坐標反算得到兩點之間的間距。表1顯示濾波前后計算長度和實測長度的誤差對比。

從表1中可以看出，最大誤差從濾波前的0.184 m降到濾波后的0.108 m，最小誤差由0.055 m降到0.032 m，且平均誤差由0.119 5 m降到0.070 m；可以清晰看出較卡爾曼濾波計算成果更接近于真實值。結合兩個試驗的結果，與卡爾曼濾波相比，擴展卡爾曼濾波后的數據噪聲得到了有效的抑制，濾波后計算的兩點間距離更加精確。

3.2 自主導航定位精度仿真模擬試驗

用MATLAB進行仿真分析來驗證多傳感器融合方式對導航定位精度的濾波效果，試驗數據獲得如下：試驗地點選在塔里木大學實訓基地。規劃路徑預先確定，來模擬農田的條件。

由圖7可知，采用KF算法融合后，定位數據偏離較大，逼近參考數據較慢。與KF算法相比，采取EKF算法融合，定位數據更快、更穩定、更接近參考數據。

從圖8、圖9可以看出，融合后的估計誤差較融合前均有所減小，位置偏差在±0.1 m以內，且EKF濾波算法定位數據更接近真實值。利用EKF濾波融合北斗系統和機器視覺導航信息的方式，與獨自使用北斗系統導航對比，定位精度和定位速度大大提升；且EKF濾波算法比KF濾波算法的濾波效果更好，更靠近真實值，穩定性較單個導航系統也有所提升，總之，融合傳感器的性能比使用其中單個傳感器要好。通過對系統總體測試成果的分析，該系統能夠實現較高的定位精度和導航速度，更好地滿足農田里導航系統的要求。

4 結論

1）? 針對農田環境，選擇BDS、視覺CCD為外部傳感器，實現了一種基于擴展卡爾曼濾波器（EKF）的數據融合算法，該算法融合了BDS和視覺傳感器數據，實時定位農機智能體的位置。

2）? 設計了信息融合算法仿真試驗，通過直線和折線試驗得出EKF濾波后的位置誤差較觀測值變小很多；濾波運動軌跡較測量運動軌跡更加平滑，所得的坐標數據更加穩定。且與KF算法相比，EKF算法所得的數據更平滑，濾波效果更好，更接近真值。通過長度試驗看出最大誤差從濾波前的0.184 m降到濾波后的0.108 m，最小誤差由0.055 m降到0.032 m，且平均誤差由0.119 5 m降到0.070 m。

3）? 利用MATLAB對兩種算法融合的定位數據進行仿真分析。分別采用KF濾波法和EKF濾波法對北斗系統定位成果和視覺CCD信息進行融合和比較，并進行了試驗測試。得出未經EKF返回的數據具有大而不均勻的路徑抖動和大的處理噪聲，且數據擬合曲線與實際路徑的偏差也較大。濾波后的路線更平滑，抖動偏差更小，坐標數據更穩定，位置偏差在±0.1 m以內，組合導航的定位精度優于單一導航系統，數據與KF相比更平滑。試驗結果表明，該算法能有效降低過程噪聲，為農業機械自動導航的研究提供一定理論支持。

參 考 文 獻

［1］ 劉美辰. 農機作業障礙物檢測與避障方法研究［D］. 楊凌： 西北農林科技大學， 2018.

Liu Meichen. Research on inspection and obstacle avoidance method of agro-machinery obstruction ［D］. Yangling： Northwest A & F University，? 2018.

［2］ 王葵， 翟榮剛， 王道斌， 等. 基于激光測距雷達和車載GPS的動態障礙物檢測［J］. 工業儀表與自動化裝置， 2013（2）： 10-13.

Wang Kui， Zhai Ronggang， Wang Daobin， et al. Dynamic obstacle detection based on laser ranging radar and GPS in-car ［J］. Industrial Instrumentation & Automation， 2013（2）： 10-13.

［3］ 杜芳， 任明武. 基于激光雷達的道路環境障礙物檢測方法［J］. 計算機與數字工程， 2019， 47（5）： 1136-1139.

Du Fang， Ren Mingwu. Method of obstacle detection on road environment using LIDAR ［J］. Computer & Digital Engineering， 2019， 47（5）： 1136-1139.

［4］ Shih Y J， Hsu C C， Wang W Y， et al. Feature extracted algorithm for simultaneous localization and mapping （SLAM） ［C］. IEEE International Conference on Consumer Electronics. IEEE， 2015.

［5］ 劉洋. 基于多傳感器信息融合的機器人避障系統的研究與實現［D］. 武漢： 武漢理工大學， 2019.

Liu Yang. Research and realization of robot obstacle avoidance system based on multi-sensor information fusion ［D］. Wuhan： Wuhan University of Technology， 2019.

［6］ Dobelis D， Zvirgzds J. Network RTK performance analysis： A case study in Latvia ［J］. Geodesy and Cartography， 2016， 42（3）： 69-74.

［7］ 黎永鍵， 趙祚喜， 黃培奎， 等. 基于DGPS與雙閉環控制的拖拉機自動導航系統［J］. 農業機械學報， 2017， 48（2）： 11-19.

Li Yongjian， Zhao Zuoxi， Huang Peikui， et al. Automatic navigation system of tractor based on DGPS and double closed-loop steering control ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2017， 48（2）： 11-19.

［8］ 羅錫文， 張智剛， 趙祚喜， 等. 東方紅X-804拖拉機的DGPS自動導航控制系統［J］. 農業工程學報， 2009， 25（11）： 139-145.

Luo Xiwen， Zhang Zhigang， Zhao Zuoxi， et al. Design of navigation control system of Dongfanghong X-804 tractor ［J］. Transactions of the Chinese Society of Agricultural Engineering， 2009， 25（11）： 139-145.

［9］ 周建軍， 鄭文剛， 李素， 等. 基于ISO 11783的拖拉機導航控制系統設計與試驗［J］. 農業機械學報， 2010， 41（4）： 190-194.

Zhou Jianjun， Zheng Wengang， Li Su， et al. Automatic navigation control system for tractor based on ISO 11783 ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2010， 41（4）： 190-194.

［10］ 紀朝鳳， 劉剛， 周建軍， 等. 基于CAN總線的農業車輛自動導航控制系統［J］. 農業機械學報， 2009， 40（S1）： 28-32.

Ji Chaofeng， Liu Gang， Zhou Jianjun， et al. Automatic guidance system of agricultural vehicle based on CAN Bus ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2009， 40（S1）： 28-32.

［11］ 張曉寒， 趙景波， 董振振. 農機BDS/INS組合導航算法研究［J］. 中國農機化學報， 2021， 42（2）： 171-177.

Zhang Xiaohan， Zhao Jingbo， Dong Zhenzhen. Research on BDS/INS integrated navigation algorithm for agricultural machinery ［J］. Journal of Chinese Agricultural Mechanization， 2021， 42（2）： 171-177.

［12］ 王中立， 牛穎. 基于多傳感器信息融合的機器人障礙物檢測［J］. 中國測試， 2017， 43（8）： 80-85.

Wang Zhongli， Niu Ying. Obstacle detection of robot based on multi-sensor information fusion ［J］. China Measurement & Test， 2017， 43（8）： 80-85.

［13］ Inal C， Bulbul S， Bilgen B. Statistical analysis of accuracy and precision of GNSS receivers used in network RTK ［J］. Arabian Journal of Geosciences， 2018， 11（10）： 81-86.

［14］ 程嘉煜. 基于機器視覺的農業機器人運動障礙檢測及避障策略研究［D］. 南京： 南京農業大學， 2011.

Cheng Jiayu. Research on moving obstacle detection and avoidance strategy for agricultural robot based on machine vision ［D］. Nanjing： Nanjing Agricultural University， 2011.

［15］ 薛金林， 董淑嫻， 范博文. 基于信息融合的農業自主車輛障礙物檢測方法［J］， 農業機械學報， 2018， 49（S1）： 29-34.

Xue Jinlin， Dong Shuxian， Fan Bowen. Detection of obstacle method based on information fusion for autonomous agricultural vehicles ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2018， 49（S1）： 29-34.

［16］ Lacy M C D. Real-time cycle slip detection in triple-frequency GNSS ［J］. GPS Solutions， 2012， 16（3）： 353-362.

［17］ 趙康寧， 趙勇. 面向信息融合的北斗衛星導航系統及應用發展［J］. 中國鐵路， 2013， 21（4）： 1-3.

［18］ 施闖， 魏娜， 李敏， 等. 利用北斗系統建立和維持國家大地坐標參考框架的方法研究［J］. 武漢大學學報（信息科學版）， 2017， 23（11）： 1635-1643.

Shi Chuang， Wei Na， Li Min， et al. Approaches to realize and maintain national terrestrial reference frame based on BDS data ［J］. Geomatics and Information Science of Wuhan University， 2017， 23（11）： 1635-1643.

［19］ Heidari F， Fotouhi R. Point-to-point and path following navigation of mobile robot in farm settings ［C］. ASME 2014 International Design Engineering Technical Conferences and Computers and Information in Engineering Conference， 2014.

［20］ 任泓潤， 劉瑞華. 低成本北斗/INS組合導航系統的研究［C］. 第七屆全國信號和智能信息處理與應用學術會議， 2013.

［21］ 陳艷麗. 基于北斗定位的農機車載組合導航系統研究［D］. 鎮江： 江蘇大學， 2016.

Chen Yanli. Research on farm machinery integrated navigation system based on beidou positioning ［D］. Zhenjiang： Jiangsu University， 2016.