CN114162127B - 一种基于机具位姿估计的水田无人农机路径跟踪控制方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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)，计算公式：

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

获得农机机具悬挂点^bP_g(x_gb,y_gb,z_gb)在{t}系下的坐标^tP_g(X_gt,Y_gt,Z_gt)；

由安装于车体上的姿态传感器测得{b}系下横滚角

俯仰角θ和航向角ψ，以表达车体任意点的位姿；

为描述农机在导航坐标系{t}的空间位姿，采用横滚-俯仰-偏航角的顺序进行坐标旋转描述悬挂点^bP_g的方位，旋转矩阵R_b为：

GNSS主定位天线在{b}系下的位姿为^bP_A(x_Ab,y_Ab,z_Ab)，记为P_A|_b，在{t}系下的位姿为^tP_A(x_At,y_At,z_At)，记为P_A|_t，主天线P_A|_b和导航控制点悬挂点^bP_g(x_gb,y_gb,z_gb)，记为P_g|_b，存在固定的位置关系，

P_g|_b＝[l_a,-l_b,-l_h]^tR_b+P_A|_b (3)

其中，l_a，l_b，l_h分别表示主定位天线与车体中心的距离、与机具悬挂点的距离和主定位天线与机具悬挂点的高程差。

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

获得农机机具中心投影至地面点P_J在{t}系下的坐标P_jNt(x_jNt,y_jNt,z_jNt)；

建立机具坐标系{j}，将机具中心点的空间位姿变换至导航坐标系{t}；

农机机具在导航坐标系{t}的空间位姿，可由机具悬挂点位姿P_g|_t进行坐标转换获得；

机具的横滚角、俯仰角和航向角分别为

θ′_j和ψ'_j，由安装于农机机具上的姿态传感器测得，将{j}系P_J变换至{b}系下，需左乘旋转矩阵^bR_j，

P_jN|_b＝[l_ja,-l_jb,-l_jh]·^bR_j+P_g|_b (5)

其中，l_ja,l_jb,l_jh分别表示农机机具中心与插秧机机具悬挂点的x、y、z轴方向的距离；

其中，l_ja＝l_a，P_jN|_b表示在载体系{b}下的位置^bP_jN(x_jNb,y_jNb,z_jNb)，^bR_j表示{j}系到{b}系的旋转矩阵，P_jN|_j表示P_J点在{j}系中的描述；

P_J点在导航坐标系{t}下的位置P_jNt(x_jNt,y_jNt,z_jNt)，记为P_jN|_t，则P_J在导航坐标系{t}的描述为：

P_jN|_t＝R_b·P_jN|_b (6)。

进一步的，步骤S2中，建立农机车体和机具的运动学模型具体为：

令(X_f,Y_f)、(X_r,Y_r)和(X_m,Y_m)分别为农机车体的前轮轴、后轮轴中心坐标和机具中心坐标，根据农机和机具运动学约束条件：

得：

由车辆运动学模型可知：

令ω_r为机头的横摆角速度，

由公式(8)至公式(11)车体和机具之间的运动学关系，得到农机机头的运动学模型为：

农机机具的运动学模型为：

改写为：

其中，δ_f为车体的前轮转向角度，v_r为车体的后轮轴中心速度，v_m是机具中心的速度，

和

分别是车体和机具的横摆角，L、l_g和l_j分别为车体的轴距、后轮轴到悬挂点的距离及悬挂点到机具中心的距离，θ为机头与机具的夹角，

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

S31、建立农机机具线性误差模型；

S32、设计农机机具预测模型路径跟踪控制器；

S33、农机机具预测模型路径跟踪控制器约束与优化。

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

农机无人驾驶系统速度和转向轮角作为系统输入u(v_r,δ_f)，以

为系统状态量的控制系统，其控制的一般形式为：

在任意点(X_r,u_r)代表参考量，一般形式为：

其中，

u_r＝[v_rr δ_fr]

在(X_r,u_r)处采用泰勒级数展开，保留一阶忽略高阶项：

获得线性化的农机机具误差模型：

其中，

采用欧拉法对模型进行离散化：

其中，A_k,t和B_k,t分别为离散化处理后的矩阵，T为离散时间步长，I为单位矩阵；

由上述公式得：

即为离散化后的农机机具线性误差模型。

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

设计目标函数：

其中，N_p为预测时域，N_c为控制时域，且N_p≥N_c≥1；ρ为权重系数，ε为松弛因子；Q和R为权重矩阵，根据控制需求的不同而更改权重大小；η(k+i|t)和η_r(k+i|t)分别表示系统的输出姿态和参考输出状态；

表示系统的控制输入增量；

给定状态空间表达式：

其中，

m为控制量维度，n为状态量维度；

对于农机机具运动学模型，m＝2、n＝3，在预测时域N_p内假定：

预测输出表达式为：

其中：

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

考虑控制量极限、控制增量等约束、农机运动轨迹受田边界以及路边界限制，对输出量进行约束；

在时刻t及预测周期N_p内，控制量极限、控制增量及输出量约束表达如下：

其中，u_min和u_max分别表示控制时域内控制量最小值和最大值集合，Δu_min和Δu_max分别表示控制时域内控制增量最小值和最大值集合，y_min和y_max表示输出最小和最大约束集合；

控制量增量和输出变量表示为：

其中，

为N_c行列向量，I_m为m维单位矩阵，

为克罗内克积，u(k-1)表示上一时刻实际控制量，y(k-1)表示上一时刻实际输出量；

将目标函数转换为标准二次型形式，结合约束条件，解决优化问题：

其中，

G_t＝[2e_t ^TQΘ_t 0]，e_t为预测时域内的跟踪误差，U_min和U_max分别为控制时域内的控制量最小值和最大值集合；

在每个控制周期内求解后，得到控制时域内控制输入增量：

控制序列的第一个元素即为控制输入增量作用于控制系统，

u(t)＝u(t-1)+Δu_t ^* (31)

循环迭代实现对无人农机的路径跟踪控制。

本发明与现有技术相比，具有如下优点和有益效果：

1、本发明提供了一种基于机具位姿估计的水田无人农机路径跟踪控制方法，以农机机具位姿为量测对象，以机头为控制对象设计水田农机机具姿态定位校正方法，估计机具中心位姿；以水田农机速度和转向轮角作为输入，以农机具位姿为状态量，建立水田农机非线性运动学模型和线性误差模型；采用欧拉法对线性误差模型离散化，建立水田农机机具线性路径跟踪误差预测模型；根据水田无人驾驶农机的转向和速度变化受机械系统、环境因素和控制系统约束，构建了模型预测控制器的优化目标函数和约束条件，实现水田无人农机的路径跟踪控制。

2、本发明以农机作业机具作为主要测控对象，针对农机具的作业路径进行无人驾驶控制，能有效避免因农机行驶轨迹和机具行驶轨迹不一致进而导致作业直线度和作业质量下降等问题。

3、本发明基于机具位姿估计设计的控制方法，能有效抑制水田农机侧滑、滑移等突变性干扰，可进一步提高机具作业轨迹跟踪精度。

附图说明

图1是本发明方法的流程图；

图2a是大地坐标系与导航坐标系的关系示意图；

图2b是载体坐标系与导航坐标系的关系示意图；

图3是本发明机具中心位置示意图；

图4是本发明机具中心坐标系示意图；

图5是本发明农机机具运动学模型示意图；

图6是本发明水田无人农机路径跟踪控制方法原理图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例

如图1、图6所示，本发明，一种基于机具位姿估计的水田无人农机路径跟踪控制方法，包括以下步骤：

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

S11、建立坐标系，以ENU坐标系为基础的导航坐标系统，包括大地坐标系{e}、导航坐标系{t}以及载体坐标系{b}，定义坐标原点

位于农机主GNSS定位天线中心，并与之固联；各坐标系的关联关系如图2a和图2b所示。

S12、获取农机车体中心坐标，具体为：

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

S13、获取农机机具悬挂点在导航坐标系下的坐标，具体为：

获得农机机具悬挂点^bP_g(x_gb,y_gb,z_gb)在{t}系下的坐标^tP_g(X_gt,Y_gt,Z_gt)；

由安装于车体上的姿态传感器测得{b}系下横滚角

俯仰角θ和航向角ψ，以表达车体任意点的位姿；

为描述农机在导航坐标系{t}的空间位姿，采用横滚-俯仰-偏航角的顺序进行坐标旋转描述悬挂点^bP_g的方位，旋转矩阵R_b为：

GNSS主定位天线在{b}系下的位姿为^bP_A(x_Ab,y_Ab,z_Ab)，记为P_A|_b，在{t}系下的位姿为^tP_A(x_At,y_At,z_At)，记为P_A|_t，主天线P_A|_b和导航控制点悬挂点^bP_g(x_gb,y_gb,z_gb)，记为P_g|_b，存在固定的位置关系，

P_g|_b＝[l_a,-l_b,-l_h]^tR_b+P_A|_b (3)

其中，l_a，l_b，l_h分别表示主定位天线与车体中心的距离、与机具悬挂点的距离和主定位天线与机具悬挂点的高程差，如图3所示。

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

获得农机机具中心投影至地面点P_J在{t}系下的坐标P_jNt(x_jNt,y_jNt,z_jNt)；

如图4所示，建立机具坐标系{j}，将机具中心点的空间位姿变换至导航坐标系{t}；

农机机具在导航坐标系{t}的空间位姿，可由机具悬挂点位姿P_g|_t进行坐标转换获得；

机具的横滚角、俯仰角和航向角分别为

θ′_j和ψ'_j，由安装于农机机具上的姿态传感器测得，将{j}系P_J变换至{b}系下，需左乘旋转矩阵^bR_j，

P_jN|_b＝[l_ja,-l_jb,-l_jh]·^bR_j+P_g|_b (5)

其中，l_ja,l_jb,l_jh分别表示农机机具中心与插秧机机具悬挂点的x、y、z轴方向的距离，如图3所示，

其中，l_ja＝l_a，P_jN|_b表示在载体系{b}下的位置^bP_jN(x_jNb,y_jNb,z_jNb)，^bR_j表示{j}系到{b}系的旋转矩阵，P_jN|_j表示P_J点在{j}系中的描述；

P_J点在导航坐标系{t}下的位置P_jNt(x_jNt,y_jNt,z_jNt)，记为P_jN|_t，则P_J在导航坐标系{t}的描述为：

P_jN|_t＝R_b·P_jN|_b (6)

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

令(X_f,Y_f)、(X_r,Y_r)和(X_m,Y_m)分别为车体的前轮轴、后轮轴中心坐标和机具中心坐标，根据农机和机具运动学约束条件：

得：

由车辆运动学模型可知：

令ω_r为机头的横摆角速度，

由公式(8)至公式(11)车体和机具之间的运动学关系，得到农机机头的运动学模型为：

农机机具的运动学模型为：

改写为：

其中，δ_f为车体的前轮转向角度，v_r为车体的后轮轴中心速度，v_m是机具中心的速度，

和

分别是车体和机具的横摆角，L、l_g和l_j分别为车体的轴距、后轮轴到悬挂点的距离及悬挂点到机具中心的距离，θ为机头与机具的夹角，

如图5所示，为农机机具运动学模型示意图。

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

S31、建立农机机具线性误差模型，具体为：

农机无人驾驶系统速度和转向轮角作为系统输入u(v_r,δ_f)，以

为系统状态量的控制系统，其控制的一般形式为：

在任意点(X_r,u_r)代表参考量，一般形式为：

其中，

u_r＝[v_rr δ_fr]

在(X_r,u_r)处采用泰勒级数展开，保留一阶忽略高阶项：

获得线性化的农机机具误差模型：

其中，

采用欧拉法对模型进行离散化：

其中，A_k,t和B_k,t分别为离散化处理后的系统矩阵，T为离散时间步长，I为单位矩阵；

由上述公式得：

即为离散化后的农机机具线性误差模型。

S32、设计农机机具预测模型路径跟踪控制器，具体为：

设计目标函数：

其中，N_p为预测时域，N_c为控制时域，且N_p≥N_c≥1；ρ为权重系数，ε为松弛因子；Q和R为权重矩阵，可根据控制需求的不同而更改权重大小；η(k+i|t)和η_r(k+i|t)分别表示系统的输出姿态和参考输出状态；

表示系统的控制输入增量；

第一项反映的是系统的路径跟随能力，第二项反映的是对控制量增量变化的约束，第三项为控制量缩放松弛项，对控制量增量直接限制，防止执行过程中出现没有可行解的情况。

给定状态空间表达式：

其中，

m为控制量维度，n为状态量维度；

对于农机机具运动学模型，m＝2、n＝3，在预测时域N_p内假定：

预测输出表达式为：

其中：

S33、农机机具预测模型控制器约束与优化，具体为：

考虑控制量极限、控制增量等约束、农机运动轨迹受田边界、路边界等限制，对输出量进行约束。在时刻t及预测周期N_p内，控制量极限、控制增量及输出量约束表达如下：

其中，u_min和u_max分别表示控制时域内控制量最小值和最大值集合，Δu_min和Δu_max分别表示控制时域内控制增量最小值和最大值集合，y_min和y_max表示输出最小和最大约束集合；

控制量增量和输出变量表示为：

其中，

为N_c行列向量，I_m为m维单位矩阵，

为克罗内克积，u(k-1)表示上一时刻实际控制量，y(k-1)表示上一时刻实际输出量；

将目标函数转换为标准二次型形式，结合约束条件，解决优化问题：

其中，

G_t＝[2e_t ^TQΘ_t 0]，e_t为预测时域内的跟踪误差，U_min和U_max分别为控制时域内的控制量最小值和最大值集合；

在每个控制周期内求解后，可得控制时域内控制输入增量：

控制序列的第一个元素即为控制输入增量作用于控制系统，

u(t)＝u(t-1)+Δu_t ^* (31)

循环迭代实现对无人农机的路径跟踪控制。

本实施例中，无人农机路径跟踪过程中，获取当前时刻水田无人驾驶农机机头和机具的运动状态，再结合水田农机机体和机具运动学预测模型、前一时刻控制输入、未知输入序列计算未来预测输出轨迹序列；然后将预测输出轨迹序列、参考轨迹和未知输入序列代入目标函数，在考虑系统约束情况下求解最优输入序列使目标函数值最优；最后将最优输入序列的第一个输入量作用到无人农机并保持一个控制周期。

本发明采用基于机具位姿估计的水田农机无人驾驶路径跟踪控制方法，以水田农机机具为测控对象，能在颠簸起伏水田环境中抑制水田农机的突变性干扰，提高路径跟踪控制精度，该方法鲁棒性较强，有效提高水田农机机具作业轨迹直线度。

还需要说明的是，在本说明书中，诸如术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其他实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (6)

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

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

S11、建立坐标系，以ENU坐标系为基础的导航坐标系统，包括大地坐标系{e}、导航坐标系{t}以及载体坐标系{b}，定义坐标原点

位于农机主GNSS定位天线中心，并与之固联；

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

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

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

获得农机机具悬挂点^bP_g(x_gb,y_gb,z_gb)在{t}系下的坐标^tP_g(X_gt,Y_gt,Z_gt)；

由安装于车体上的姿态传感器测得{b}系下横滚角

俯仰角θ和航向角ψ，以表达车体任意点的位姿；

为描述农机在导航坐标系{t}的空间位姿，采用横滚-俯仰-偏航角的顺序进行坐标旋转描述悬挂点^bP_g的方位，旋转矩阵R_b为：

GNSS主定位天线在{b}系下的位姿为^bP_A(x_Ab,y_Ab,z_Ab)，记为P_A|_b，在{t}系下的位姿为^tP_A(x_At,y_At,z_At)，记为P_A|_t，主天线P_A|_b和导航控制点悬挂点^bP_g(x_gb,y_gb,z_gb)，记为P_g|_b，存在固定的位置关系，

P_g|_b＝[l_a,-l_b,-l_h]^tR_b+P_A|_b (3)

其中，l_a，l_b，l_h分别表示主定位天线与车体中心的距离、与机具悬挂点的距离和主定位天线与机具悬挂点的高程差；

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

获得农机机具中心投影至地面点P_J在{t}系下的坐标P_jNt(x_jNt,y_jNt,z_jNt)；

建立机具坐标系{j}，将机具中心点的空间位姿变换至{t}系；

农机机具在{t}系的空间位姿，可由机具悬挂点位姿P_g|_t进行坐标转换获得；

机具的横滚角、俯仰角和航向角分别为

θ′_j和Ψ′_j，由安装于农机机具上的姿态传感器测得，将{j}系P_J变换至{b}系下，需左乘旋转矩阵^bR_j，

P_jN|_b＝[l_ja,-l_jb,-l_jh]·^bR_j+P_g|_b (5)

其中，l_ja,l_jb,l_jh分别表示农机机具中心与插秧机机具悬挂点的x、y、z轴方向的距离；

其中，l_ja＝l_a，P_jN|_b表示在{b}系下的位置^bP_jN(x_jNb,y_jNb,z_jNb)，^bR_j表示{j}系到{b}系的旋转矩阵，P_jN|_j表示P_J点在{j}系中的描述；

P_J点在{t}系下的位置P_jNt(x_jNt,y_jNt,z_jNt)，记为P_jN|_t，则P_J在{t}系的描述为：

P_jN|_t＝R_b·P_jN|_b (6)。

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

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

2.根据权利要求1所述的一种基于机具位姿估计的水田无人农机路径跟踪控制方法，其特征在于，步骤S2中，建立农机车体和机具的运动学模型具体为：

令(X_f,Y_f)、(X_r,Y_r)和(X_m,Y_m)分别为农机车体的前轮轴、后轮轴中心坐标和机具中心坐标，根据农机和机具运动学约束条件：

得：

由车辆运动学模型可知：

令ω_r为机头的横摆角速度，

由公式(8)至公式(11)车体和机具之间的运动学关系，得到农机机头的运动学模型为：

农机机具的运动学模型为：

改写为：

其中，δ_f为车体的前轮转向角度，v_r为车体的后轮轴中心速度，v_m是机具中心的速度，

和

分别是车体和机具的横摆角，L、l_g和l_j分别为车体的轴距、后轮轴到悬挂点的距离及悬挂点到机具中心的距离，θ为机头与机具的夹角，

3.根据权利要求1所述的一种基于机具位姿估计的水田无人农机路径跟踪控制方法，其特征在于，步骤S3具体包括：

S31、建立农机机具线性误差模型；

S32、设计农机机具预测模型路径跟踪控制器；

S33、农机机具预测模型路径跟踪控制器约束与优化。

4.根据权利要求3所述的一种基于机具位姿估计的水田无人农机路径跟踪控制方法，其特征在于，步骤S31具体为：

农机无人驾驶系统速度和转向轮角作为系统输入u(v_r,δ_f)，以

为系统状态量的控制系统，其控制的一般形式为：

在任意点(X_r,u_r)代表参考量，一般形式为：

其中，

u_r＝[v_rr δ_fr]

在(X_r,u_r)处采用泰勒级数展开，保留一阶忽略高阶项：

获得线性化的农机机具误差模型：

其中，

采用欧拉法对模型进行离散化：

其中，A_k,t和B_k,t分别为离散化处理后的矩阵，T为离散时间步长，I为单位矩阵；

由上述公式得：

即为离散化后的农机机具线性误差模型。

5.根据权利要求4所述的一种基于机具位姿估计的水田无人农机路径跟踪控制方法，其特征在于，步骤S32具体为：

设计目标函数：

其中，N_p为预测时域，N_c为控制时域，且N_p≥N_c≥1；ρ为权重系数，ε为松弛因子；Q和R为权重矩阵，根据控制需求的不同而更改权重大小；η(k+i|t)和η_r(k+i|t)分别表示系统的输出姿态和参考输出状态；

表示系统的控制输入增量；

给定状态空间表达式：

其中，

m为控制量维度，n为状态量维度；

对于农机机具运动学模型，m＝2、n＝3，在预测时域N_p内假定：

预测输出表达式为：

其中：

6.根据权利要求5所述的一种基于机具位姿估计的水田无人农机路径跟踪控制方法，其特征在于，步骤S33具体为：

考虑控制量极限、控制增量约束、农机运动轨迹受田边界以及路边界限制，对输出量进行约束；

在时刻t及预测周期N_p内，控制量极限、控制增量及输出量约束表达如下：

其中，u_min和u_max分别表示控制时域内控制量最小值和最大值集合，Δu_min和Δu_max分别表示控制时域内控制增量最小值和最大值集合，y_min和y_max表示输出最小和最大约束集合；

控制量增量和输出变量表示为：

其中，

为N_c行列向量，I_m为m维单位矩阵，

为克罗内克积，u(k-1)表示上一时刻实际控制量，y(k-1)表示上一时刻实际输出量；

将目标函数转换为标准二次型形式，结合约束条件，解决优化问题：

其中，

G_t＝[2e_t ^TQΘ_t 0]，e_t为预测时域内的跟踪误差，U_min和U_max分别为控制时域内的控制量最小值和最大值集合；

在每个控制周期内求解后，得到控制时域内控制输入增量：

控制序列的第一个元素即为控制输入增量作用于控制系统，

u(t)＝u(t-1)+Δu_t ^* (31)

循环迭代实现对无人农机的路径跟踪控制。

Images