基于扩展卡尔曼滤波的农机路径控制算法研究

doi:10.13733/j.jcam.issn.2095-5553.2024.07.032

摘要/Abstract

摘要： 智能农机的路径跟踪精度受田地地形、农机结构等多因素的影响，会导致压坏滴灌带等严重后果。以棉花播种机为研究对象，提出一种由Stanley跟踪算法和线性二次最优控制(LQR)算法组成的切换式跟踪算法，引导农机快速入线和保持直线精度；另外为消除静态横向误差，加入针对航向角误差的扩展卡尔曼滤波器(EKF)。仿真结果显示，切换式跟踪算法在进入目标直线后可有效消除静态横向误差。实车试验结果表明，当给定初始横向误差为0.5 m，速度为3.6 km/h时，入线时间为6.88 s，超调量为0.041 m；当给定初始横向误差为0 m，速度为3.6 km/h时，直线跟踪精度控制在±0.025 m范围内，满足实际农机高精度的直线作业要求，表明该算法具有良好的跟踪精度和抗干扰能力，有利于提升农业生产效率。

关键词: 智能农机, 定位技术, 扩展卡尔曼滤波, 路径跟踪, 自动驾驶

Abstract: The path tracking accuracy of intelligent agricultural machinery is affected by many factors such as field terrain and machinery structure, which may lead to serious consequences such as crushing the drip irrigation belt. In this paper, taking the cotton planter as the research object, a switching tracking algorithm consisting of Stanley tracking algorithm and linear quadratic optimal control (LQR) algorithm is proposed to guide the agricultural machine to enter the line quickly and maintain the straight line accuracy. In addition, in order to eliminate the static lateral error, an extended Kalman filter (EKF) for heading angle error is added. The simulation results show that the switching tracking algorithm can eliminate the static lateral error after entering the straight target line. The real vehicle test results show that when the given initial lateral error is 0.5 m and the speed is 3.6 km/h, the entry time is 6.88 s, and the overshoot is 0.041 m. When the given initial lateral error is 0 m, and the speed is 3.6 km/h, the straightline tracking accuracy is controlled within ±0.025 m, which meets the requirements of highprecision straightline operation of actual agricultural machinery. It shows that the algorithm studied has good tracking accuracy and antiinterference ability and it is conducive to improving agricultural production efficiency.

Key words: intelligent agricultural machinery, positioning technology, extended Kalman filter, path tracking, automatic driving

中图分类号:

S220

钱俊楠, 冯桑, 利航, 张泳. 基于扩展卡尔曼滤波的农机路径控制算法研究[J]. 中国农机化学报, 2024, 45(7): 215-221.

Qian Junnan, Feng Sang, Li Hang, Zhang Yong. Research on path tracking control algorithm of agricultural machinery based on extended Kalman filter[J]. Journal of Chinese Agricultural Mechanization, 2024, 45(7): 215-221.

参考文献

［1］　张漫, 季宇寒, 李世超, 等. 农业机械导航技术研究进展［J］. 农业机械学报, 2020, 51(4): 1-18.
Zhang Man, Ji Yuhan, Li Shichao, et al. Research progress of agricultural machinery navigation technology ［J］. Transactions of the Chinese Society for Agricultural Machinery, 2020, 51(4): 1-18.
［2］　王辉, 王桂民, 罗锡文, 等. 基于预瞄追踪模型的农机导航路径跟踪控制方法［J］. 农业工程学报, 2019, 35(4): 11-19.
Wang Hui, Wang Guimin, Luo Xiwen, et al. Path tracking control method of agricultural machine navigation based on aiming pursuit model ［J］. Transactions of the Chinese Society of Agricultural Engineering, 2019, 35(4): 11-19.
［3］　徐广飞, 陈美舟, 金诚谦, 等. 拖拉机自动驾驶关键技术综述［J］. 中国农机化学报, 2022, 43(6): 126-134.
Xu Guangfei, Chen Meizhou, Jin Chengqian, et al. A review of key technology of tractor automatic driving ［J］. Journal of Chinese Agricultural Mechanization, 2022, 43(6): 126-134.
［4］　严国军, 贲能军, 顾建华, 等. 基于MPC的无人驾驶拖拉机轨迹跟踪控制［J］. 重庆交通大学学报(自然科学版), 2019, 38(9): 1-6.
Yan Guojun, Ben Nengjun, Gu Jianhua, et al. Trajectory tracking control of intelligent tractor based on MPC algorithm ［J］. Journal of Chongqing Jiaotong University of Technology (Natural Science), 2019, 38(9): 1-6.
［5］　Alipour K, Robat A B,Tarvirdizadeh B. Dynamics modeling and sliding mode control of tractortrailer wheeled mobile robots subject to wheels slip ［J］. Mechanism and Machine Theory, 2019, 138: 16-37.
［6］　Khalaji A K. Pidbased target tracking control of a tractortrailer mobile robot ［J］. Proceedings of the Institution of Mechanical Engineers, Part C. Journal of Mechanical Engineering Science, 2019, 233(13): 4776-4787.
［7］　Han X, Kim H J, Jeon C W, et al. Application of a 3D tractordriving simulator for slip estimationbased pathtracking control of autoguided tillage operation ［J］. Biosystems Engineering, 2019, 178: 70-85.
［8］　Murillo M, Sanchez G, Deniz N, et al. Improving pathtracking performance of an articulated tractortrailer system using a nonlinear kinematic model ［J］. Computers and Electronics in Agriculture, 2022, 196: 106826.
［9］　Nehme H, Aubry C, Solatges T, et al. Lidarbased structure tracking for agricultural robots: Application to autonomous navigation in vineyards ［J］. Journal of Intelligent & Robotic Systems, 2021, 103(4): 61.
［10］　Xu J, Lai J, Guo R, et al. Efficiencyoriented MPC algorithm for path tracking in autonomous agricultural machinery ［J］. Agronomy, 2022, 12(7): 1662.
［11］　Zhou S, Zhao H, Chen W, et al. Robust path following of the tractortrailers system in GPSdenied environments ［J］. IEEE Robotics and Automation Letters, 2019, 5(2): 500-507.
［12］　武涛, 李彦明, 林洪振, 等. 基于干扰观测器的直播机路径跟踪快速终端滑模控制［J］. 农业机械学报, 2021, 52(12): 24-31.
Wu Tao, Li Yanming, Lin Hongzhen, et al. Fast terminal sliding mode control for autonomous rice seeding machine based on disturbance observer ［J］. Transactions of the Chinese Society for Agricultural Machinery, 2021, 52(12): 24-31.
［13］　张智刚, 王明昌, 毛振强, 等. 基于星基增强精密单点定位的农机自动导航系统开发与测试［J］. 华南农业大学学报, 2021, 42(6): 109-116.
Zhang Zhigang, Wang Mingchang, Mao Zhenqiang, et al. Development and test of autonavigation system for agricultural machinery based on satellite based precision singlepoint positioning ［J］. Journal of South China Agricultural University, 2021, 42(6): 109-116.
［14］　张培培, 杨自栋, 赵相君. 拖拉机—牵引式农机具的运动建模与跟踪控制［J］. 中国农机化学报, 2021, 42(4): 128-133.
Zhang Peipei, Yang Zidong, Zhao Xiangjun. Kinematic model and tracking control for a trailed implement ［J］. Journal of Chinese Agricultural Mechanization, 2021, 42(4): 128-133.
［15］　狄桓宇, 张亚辉, 王博, 等. 自动驾驶横向控制模型及方法研究综述［J］. 重庆理工大学学报(自然科学), 2021, 35(7): 71-81.
Di Huanyu, Zhang Yahui, Wang Bo, et al. Review on the method and model of intelligent vehicle lateral control ［J］. Journal of Chongqing University of Technology (Natural Science), 2021, 35(7): 71-81.
［16］　左帅, 刘成昊, 张笑, 等. 基于LQR的工程车辆自动驾驶路径跟踪技术研究［J］. 工程机械, 2022, 53(3): 54-58, 10.
Zuo Shuai, Liu Chenghao, Zhang Xiao, et al. Research on automatic driving path tracking technology of engineering vehicles based on LQR ［J］. Construction Machinery and Equipment, 2022, 53 (03): 54-58, 10.
［17］　王鑫, 凌铭, 饶启鹏, 等. 基于改进Stanley算法的无人车路径跟踪融合算法研究［J］. 汽车技术, 2022(7): 25-31.
Wang Xin, Ling Ming, Rao Qipeng, et al. Research on fusion algorithm of unmanned vehicle path tracking based on improved Stanley algorithm ［J］. Automotive Technology, 2022(7): 25-31.
［18］　熊璐, 杨兴, 卓桂荣, 等. 无人驾驶车辆的运动控制发展现状综述［J］. 机械工程学报, 2020, 56(10): 127-143.
Xiong Lu, Yang Xing, Zhuo Guirong, et al. Review on motion control of autonomous vehicles ［J］. Journal of Mechanical Engineering, 2020, 56(10): 127-143.
［19］　杨菊花, 李文元, 陈光武, 等. 引入滑模观测器的GPS/INS组合导航滤波方法［J］. 仪器仪表学报, 2019, 40(9):78-86.
Yang Juhua, Li Wenyuan, Chen Guangwu, et al. GPS/INS integrated navigation filtering method based on sliding mode observer ［J］ Chinese Journal of Science Instrument, 2019, 40(9): 78-86.
［20］　孟秀云, 王语嫣. 一种SINS/GPS紧组合导航系统的改进自适应扩展卡尔曼滤波算法［J］. 北京理工大学学报, 2018, 38(6): 625-630, 636.
Meng Xiuyun, Wang Yuyan. An improved adaptive extended Kalman filter algorithm of SINS/GPS tightlycoupled integrated navigation system ［J］. Transactions of Beijing Institute of Technology, 2018, 38(6): 625-630, 636.

[1]	郑拓, 袁丽峰, 戚得众, 闫行行, 杨文广, . 丘陵山地果园行间运输车自动跟随系统研究[J]. 中国农机化学报, 2024, 45(9): 178-183.
[2]	杨帆, 滕利, 孙宜田, 孙永佳, 蒋振晗, 侯加林. 农业机械底盘智能化发展现状与展望[J]. 中国农机化学报, 2024, 45(8): 138-147.
[3]	杨涛, 孙付春, 黄波, 吴柏强, 冉光泽. 果园作业平台关键技术研究进展[J]. 中国农机化学报, 2024, 45(1): 152-159.
[4]	张俊峰, 张唐娟, 肖进, 王琢, 田满洲, 何雨霜. 池塘养殖船自动导航系统设计与试验[J]. 中国农机化学报, 2023, 44(3): 49-54.
[5]	吴正开, , 王家忠, , 郉雅周, , 弋景刚, , 李珊珊, , 赵春明. 基于无人驾驶电动拖拉机的自适应跟踪算法研究[J]. 中国农机化学报, 2023, 44(11): 176-183.
[6]	何义川, , 杨毅, , 弋晓康, , 王维鹏, . 基于变量施肥系统的施肥准确性影响因素仿真与试验[J]. 中国农机化学报, 2023, 44(10): 51-57.
[7]	王玉亮, 李汉卿, 陈兆英, 石绍军, 陈国防, 王金星, . 基于MPC的插秧机路径跟踪控制算法研究[J]. 中国农机化学报, 2022, 43(7): 173-178.
[8]	徐广飞, 陈美舟, 金诚谦, 苗河泉, 逄焕晓, 刁培松. 拖拉机自动驾驶关键技术综述[J]. 中国农机化学报, 2022, 43(6): 126-134.
[9]	姚竟发;刘静;滕桂法;张璠;范晓飞;. 机群协同作业路径动态优化[J]. 中国农机化学报, 2021, 42(6): 103-110.
[10]	石志刚, 梅松, 邵毅帆, 万如, 宋志禹, 谢铭露, 李燕. 基于人工势场法的移动机器人路径规划研究现状与展望[J]. 中国农机化学报, 2021, 42(12): 182-188.
[11]	柴善鹏, 姚立健, 徐丽君, 陈钦汉, 徐涛涛, 杨岩坤. 基于动态前视距离纯追踪模型的温室农机路径跟踪研究^*[J]. 中国农机化学报, 2021, 42(11): 58-64.
[12]	杨涛, 李晓晓. 农机自动驾驶系统研究进展与行业竞争环境分析^*[J]. 中国农机化学报, 2021, 42(11): 222-231.
[13]	尹奇辉;李捷;梁志鹏;杨磊;. 基于模糊神经网络PID的机器人路径跟踪控制[J]. 中国农机化学报, 2020, 41(5): 182-187.
[14]	高菊玲;钟兴;张居同;欧阳凯琪;. 自动驾驶拖拉机轨迹跟踪研究[J]. 中国农机化学报, 2020, 41(10): 104-110.