基于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.