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

权利要求书

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