基于EKF的農(nóng)機智能體自主導(dǎo)航算法研究

孟福軍 岳勝如

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

關(guān)鍵詞：農(nóng)機智能體；障礙物檢測；EKF；BDS

中圖分類號：S126： TP273

文獻(xiàn)標(biāo)識碼：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 引言

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

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

1.1 組合導(dǎo)航模型

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

1.2 建立坐標(biāo)系及坐標(biāo)轉(zhuǎn)換模型

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

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

3.1.2 長度試驗

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

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

3.2 自主導(dǎo)航定位精度仿真模擬試驗

用MATLAB進行仿真分析來驗證多傳感器融合方式對導(dǎo)航定位精度的濾波效果，試驗數(shù)據(jù)獲得如下：試驗地點選在塔里木大學(xué)實訓(xùn)基地。規(guī)劃路徑預(yù)先確定，來模擬農(nóng)田的條件。

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

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

4 結(jié)論

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

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

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

參 考 文 獻(xiàn)

［1］ 劉美辰. 農(nóng)機作業(yè)障礙物檢測與避障方法研究［D］. 楊凌： 西北農(nóng)林科技大學(xué)， 2018.

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

［2］ 王葵， 翟榮剛， 王道斌， 等. 基于激光測距雷達(dá)和車載GPS的動態(tài)障礙物檢測［J］. 工業(yè)儀表與自動化裝置， 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］ 杜芳， 任明武. 基于激光雷達(dá)的道路環(huán)境障礙物檢測方法［J］. 計算機與數(shù)字工程， 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］ 劉洋. 基于多傳感器信息融合的機器人避障系統(tǒng)的研究與實現(xiàn)［D］. 武漢： 武漢理工大學(xué)， 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與雙閉環(huán)控制的拖拉機自動導(dǎo)航系統(tǒng)［J］. 農(nóng)業(yè)機械學(xué)報， 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自動導(dǎo)航控制系統(tǒng)［J］. 農(nóng)業(yè)工程學(xué)報， 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的拖拉機導(dǎo)航控制系統(tǒng)設(shè)計與試驗［J］. 農(nóng)業(yè)機械學(xué)報， 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］ 紀(jì)朝鳳， 劉剛， 周建軍， 等. 基于CAN總線的農(nóng)業(yè)車輛自動導(dǎo)航控制系統(tǒng)［J］. 農(nóng)業(yè)機械學(xué)報， 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］ 張曉寒， 趙景波， 董振振. 農(nóng)機BDS/INS組合導(dǎo)航算法研究［J］. 中國農(nóng)機化學(xué)報， 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］ 程嘉煜. 基于機器視覺的農(nóng)業(yè)機器人運動障礙檢測及避障策略研究［D］. 南京： 南京農(nóng)業(yè)大學(xué)， 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］ 薛金林， 董淑嫻， 范博文. 基于信息融合的農(nóng)業(yè)自主車輛障礙物檢測方法［J］， 農(nóng)業(yè)機械學(xué)報， 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］ 趙康寧， 趙勇. 面向信息融合的北斗衛(wèi)星導(dǎo)航系統(tǒng)及應(yīng)用發(fā)展［J］. 中國鐵路， 2013， 21（4）： 1-3.

［18］ 施闖， 魏娜， 李敏， 等. 利用北斗系統(tǒng)建立和維持國家大地坐標(biāo)參考框架的方法研究［J］. 武漢大學(xué)學(xué)報（信息科學(xué)版）， 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組合導(dǎo)航系統(tǒng)的研究［C］. 第七屆全國信號和智能信息處理與應(yīng)用學(xué)術(shù)會議， 2013.

［21］ 陳艷麗. 基于北斗定位的農(nóng)機車載組合導(dǎo)航系統(tǒng)研究［D］. 鎮(zhèn)江： 江蘇大學(xué)， 2016.

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