[发明专利]一种基于机具位姿估计的水田无人农机路径跟踪控制方法

说明书

本发明公开了一种基于机具位姿估计的水田无人农机路径跟踪控制方法，包括以下步骤：S1、在农机机身安装全球卫星定位测量装置GNSS，获取农机车体中心坐标，获取农机机具位姿估计；S2、建立农机车体和机具的运动学模型；S3、建立农机运动学模型预测路径跟踪控制器，以水田农机行驶速度和转向轮角作为系统输入控制变量，以农机具位姿为系统状态变量，建立农机机具的状态空间模型，采用欧拉法对模型进行离散化，获得农机具线性误差模型；设计目标函数和约束条件，求解农机机具的最优控制输入增量。本发明提高了水田无人农机路径跟踪精度和抗干扰性能。

技术领域

本发明属于智能农业机械技术领域，具体涉及一种基于机具位姿估计的水田无人农机路径跟踪控制方法。

背景技术

农业机械自动化和智能化已成为现代农业生产的必然趋势和发展高效节本农业的有效途径，越来越多的智能农机装备将投入应用，以满足精准生产的需求。无人驾驶农机的推广应用，能有效的减少劳动力投入，节约人工成本，提高农机作业质量，避免作业时重行、漏行等问题造成减产。

目前，无人农机在我国新疆、黑龙江等旱作物作业中已经得到了较好广应用，而水田无人农机作业效果仍需进一步研究提高。现有的水田无人农机大都以农机机头为测控对象，而农机与机具挂接时都存在“间隙”，作业机具运动姿态相对机头变化较大，造成机头行驶轨迹和机具行驶轨迹不一致，进而导致作业直线度和作业质量下降。

在水田中，水田无人农机作业时受复杂水田环境影响，农机频繁起伏波动、横滚和俯仰角度大，干扰因素多，农机在水田作业时路径跟踪精准控制难度更大。

发明内容

本发明的主要目的在于克服水田农机因环境干扰大，且农机与机具相对运动姿态导致作业直线度和作业质量下降的问题，提出一种基于机具位姿估计的水田无人农机路径跟踪控制方法，以水田农机机具为导航位姿的量测对象，农机机头为控制对象，估计农机作业机具位姿；提高水田农机无人驾驶路径跟踪精度和抗干扰性能。

为了达到上述目的，本发明采用以下技术方案：

一种基于机具位姿估计的水田无人农机路径跟踪控制方法，包括以下步骤：

S1、获取农机机具位姿估计，在农机机身安装全球卫星定位测量装置GNSS，获取农机车体中心坐标，设计基于农机车体位姿的农机机具位姿的估计方法，获得农机机具位姿估计；

S2、建立农机车体和机具的运动学模型，以机具位姿为观测量，根据车体的位姿和农机参数以及车体和机具的相对位姿变化关系，获取机具的运动状态方程；

S3、基于农机机具运动学模型，建立农机机具预测模型路径跟踪控制器，以农机行驶速度和转向轮角作为输入控制变量，以农机机具位姿为状态变量，建立农机机具的状态空间模型，采用欧拉法对模型进行离散化，获得农机机具线性误差模型；设计目标函数和约束条件，求解农机机具的最优控制输入增量。

进一步的，步骤S1具体包括：

S11、建立坐标系，以ENU坐标系为基础的导航坐标系统，包括大地坐标系{e}、导航坐标系{t}以及载体坐标系{b}，定义坐标原点位于农机主GNSS定位天线中心，并与之固联；

S12、获取农机车体中心坐标；

S13、获取农机机具悬挂点在导航坐标系下的坐标；

S14、获得农机机具中心投影至地面点在导航坐标系下的坐标。

进一步的，步骤S12具体为：

获取农机车体中心坐标，测量农机车体GNSS主定位天线的大地坐标^eP_A(B,L,H)转换到导航坐标系下^tP_A(x_At,y_At,z_At)，计算公式：