[发明专利]一种农具轨迹控制方法、装置及农业机械

权利要求书

1.一种农具轨迹控制方法，其特征在于，应用于农业机械(10)，所述农业机械(10)包括农具及车身，所述车身拖拽所述农具，所述车身安装有可电动控制的方向盘，所述农具轨迹控制方法包括：

获取上一时刻的所述农具的融合运动状态；

依据上一时刻的所述融合运动状态计算所述农具在当前时刻的理论运动状态；

获取所述农具在当前时刻的测量运动状态；

依据所述农具在当前时刻的测量运动状态及所述农具在当前时刻的理论运动状态计算所述农具在当前时刻的融合运动状态；

依据所述农具在当前时刻的融合运动状态及预设轨迹计算所述方向盘的控制量；

根据所述控制量控制所述方向盘转动，使所述农具在下一时刻运动至所述预设轨迹。

2.根据权利要求1所述的农具轨迹控制方法，其特征在于，所述依据所述农具在当前时刻的融合运动状态及预设轨迹计算所述方向盘的控制量的步骤包括：

依据所述农具在当前时刻的融合运动状态计算所述农具在下一时刻的融合运动状态；

依据所述农具在下一时刻的所述融合运动状态及所述预设轨迹计算所述控制量。

3.根据权利要求2所述的农具轨迹控制方法，其特征在于，所述融合运动状态包括融合角度值及融合坐标值，所述依据所述农具在当前时刻的融合运动状态计算所述农具在下一时刻的融合运动状态的步骤包括：

获取所述农具的运行速度；

依据所述农具的所述运行速度、所述农具在当前时刻的融合坐标值及所述农具当前时刻的融合角度值计算所述农具在下一时刻的融合坐标值。

4.根据权利要求2所述的农具轨迹控制方法，其特征在于，所述融合运动状态包括融合角度值及融合坐标值，所述依据所述农具在下一时刻的所述融合运动状态及所述预设轨迹计算所述方向盘的控制量的步骤包括：

依据所述农具在下一时刻的融合坐标值及所述预设轨迹计算航向差值；

依据所述航向差值及下一时刻的所述融合角度值计算所述控制量。

5.根据权利要求4所述的农具轨迹控制方法，其特征在于，所述依据所述农具在下一时刻的融合坐标值及所述预设轨迹计算所述航向差值的步骤包括：

在所述预设轨迹上设置第一位置点及第二位置点；

计算所述第一位置点与所述第二位置点的位置向量；

计算所述第一位置点与所述农具在下一时刻的融合坐标值的差值向量；

依据所述差值向量及所述位置向量计算所述航向差值。

6.根据权利要求1所述的农具轨迹控制方法，其特征在于，所述理论运动状态包括理论角度值以及理论坐标值，所述融合运动状态包括融合角度值以及融合坐标值，所述依据上一时刻的融合运动状态计算所述农具在当前时刻的理论运动状态的步骤包括：

获取所述农具及所述车身在上一时刻的转动角度差值；

获取所述农具与所述车身的间隔距离；

获取所述农具的运行速度；

依据所述上一时刻的融合角度值、上一时刻的所述转动角度差值、所述运行速度以及所述间隔距离计算当前时刻的所述理论角度值。

7.根据权利要求6所述的农具轨迹控制方法，其特征在于，所述测量运动状态包括测量角度值以及测量坐标值，所述依据上一时刻的融合运动状态计算所述农具在当前时刻的理论运动状态的步骤包括：

依据当前时刻的理论角度值及当前时刻的测量角度值融合所述农具在当前时刻的融合角度值；

获取所述车身在当前时刻的车身坐标值；

依据所述车身在当前时刻的车身坐标值及所述农具在当前时刻的融合角度值计算所述农具在当前时刻的理论坐标值。

8.根据权利要求6所述的农具轨迹控制方法，其特征在于，所述获取所述农具及所述车身在上一时刻的转动角度差值的步骤包括：

获取所述农具在上一时刻的理论角度值；

获取所述车身在上一时刻的实际转角值；

计算所述农具在上一时刻的理论角度值及所述车身在上一时刻的实际转角值的差值得到所述转动角度差值。