[发明专利]一种农业机械的导航控制方法

说明书

技术领域

本发明涉及农业机械领域，特别涉及一种农业机械的导航控制方法。

背景技术

农业机械导航控制的主要任务是根据导航定位结果，确定农业机械与预定路线的位置关系，进而结合农业机械的运动状态决策出合适的转向轮操纵角，以修正路径跟踪误差。国外常用的导航控制方法有三种，即线性模型控制方法、最优控制方法和模糊控制方法。东京大学利用机器视觉技术进行KUBOTA SPJ45A型久保田插秧机的导航控制技术研究，他将目标方向角度与车辆纵向方向角度进行对比，用Abe的线性转向控制模型计算得出转向轮转向角度。Qiu提出了一种基于预瞄的导航控制算法，该导航控制器包括一个前馈比例控制器和反馈PD控制器；前进速度决定前视距离，位置误差决定前馈增益；前馈控制器的输入是路径曲率，反馈控制器的输入是航向偏差，两控制器的输出合成为期望转向轮操纵角。东京大学采用模糊控制技术来进行导航控制研究，模糊控制器根据方向偏差和位置偏差确定转向角度和左转执行时间及右转执行时间。韩国的Lee等人利用机器视觉、DGPS、超声波传感器和模糊控制技术进行果树喷雾机的导航控制研究。O’Connor等人采用方向偏差、方向偏差变化率、转向角度、转向角度变化率、跟踪误差等五个状态变量建立车辆运动学方程，以转向角度变化率最小为优化指标，利用车辆线性运动学方程组建立了基于LQR的最优导航控制器。Kise等人开发了最优操纵控制器用于农用车辆导航，在曲线路径跟踪方面取得了一定的效果。周俊(2003)提出了视觉导航的图象处理技术，同时还利用线性状态反馈控制方法、二值控制方法建立了导航控制的内外环结构；在车辆纵向速度较高的状态下，提出了把横向偏差、航向偏差以及横向加速度作为输入量的三维横向模糊控制算法，以保证车辆横向运动的平稳性。毛恩荣等人(2005)研究了在铁牛654拖拉机上搭建DGPS自动驾驶系统的硬件组成及关键技术，根据最优控制理论，研究了横向偏差和航向角输出反馈控制系数的离线优化调节方法，为试验凑试最优控制参数提供依据。本发明人在2005年将GPS技术、计算机技术、传感器技术进行集成，研制了一种以蓄电池为电源、电动机为动力的农用智能移动作业平台，该样机采用前轮驱动、差速转向方式；并以DGPS、电子罗盘为主要导航传感器，在样机上建立了DGPS导航控制系统；还设计了基于预瞄跟随的导航控制算法，其核心是由航向偏差线性决定驱动轮速度差。由于该智能移动作业平台采用差速转向方式，路径跟踪的控制难度较大，导航精度难以提高。

常规线性模型控制方法，包括PID控制方法，可以获得高精度的路径跟踪效果，但是抗干扰能力弱。模糊控制方法具有良好的稳定性效果，但路径跟踪的稳态精度难以保证。最优控制器的建立主要依据农业机械运动学和动力学模型，其模型精确性对控制器性能有重要影响。但是，田间作业条件下农业机械运动学和动力学模型的建立是比较困难的。田间作业时，随着农业机械、作业机具与地面相互作用的变化，田间土壤特性的变化，农业机械运动特性必然是时变的，还有作业工况差引起的传感器测试干扰等，这些因素对常规线性模型控制方法和最优控制方法的影响都较大。

发明内容

本发明的目的在于克服现有技术中存在的缺点，提供一种导航跟踪精度高、稳定性好的农业机械的导航控制方法。该方法基于预瞄跟随理论，采用PID和模糊逻辑相结合的方法来设计路径跟踪的控制算法，充分利用PID路径跟踪的高精度和模糊控制的稳定性特点，建立稳定性好、精度高的路径跟踪控制算法。

本发明一种农业机械的导航控制方法，包括下述步骤：

(1)通过导航传感器测量得到农业机械当前时刻的位姿和运动状态数据；

(2)采用基于预瞄控制的动态目标点搜索算法，将所述位姿和运动状态数据与预定义路线数据进行对比分析，推算出两个导航状态参数即横向跟踪误差和纵向航向偏差；

(3)再通过Fuzzy-PID控制算法推算出操纵控制量即前轮转向偏角；

(4)最后根据所述操纵控制量对农业机械进行实时控制，实现路径跟踪。

所述的预定义路线P[N]由点数组表示，包含元素为：

{P₀，P₁，...，P_k-1，P_k，P_k+1，...，P_N}

其数学表达式为：