[发明专利]一种水田拖拉机位姿检测和偏航角提取方法及装置

说明书

本发明公开了一种水田拖拉机位姿检测和偏航角提取方法及装置，S1、在水田拖拉机初始运行时，初始摄像模块会对水田拖拉机的运行轨迹进行摄像，此时第一运行轨迹特征采集模块会对根据摄像对水田拖拉机的运行轨迹特征进行采集，从而标准运行轨迹确定模块对其运行轨迹进行确定，本发明涉及农用设备技术领域。该水田拖拉机位姿检测和偏航角提取方法及装置，通过陀螺仪对水田拖拉机的位姿进行了很好的检测，通过将标准轨迹与实时轨迹进行对比，得出水田拖拉机的偏航角，使用者可以通过显示屏对水田拖拉机的位姿和偏航角进行及时查看，在水田拖拉机偏航后，能自动对其进行校正，使其能快速的回到正常轨道，方便了操作人员的使用。

技术领域

本发明涉及农用设备技术领域，具体为一种水田拖拉机位姿检测和偏航角提取方法及装置。

背景技术

拖拉机用于牵引和驱动作业机械完成各项移动式作业的自走式动力机，也可做固定作业动力，由发动机、传动、行走、转向、液压悬挂、动力输出、电器仪表、驾驶操纵及牵引等系统或装置组成，发动机动力由传动系统传给驱动轮，使拖拉机行驶，现实生活中，常见的都是以橡胶皮带作为动力传送的媒介，按功能和用途分农业、工业和特殊用途等拖拉机，按结构类型分轮式、履带式、船形拖拉机和自走底盘等。

随着农业机械化的快速发展，对高精度导航技术的需求也越来越高，在水田拖拉机运行时，由于是通过人工控制拖拉机，拖拉机经常会出现偏离正常轨道的情况，从而田间工作不能够准确、高效的完成，因此提取水田拖拉机的位姿和偏航角是非常有必要的。

发明内容

(一)解决的技术问题

针对现有技术的不足，本发明提供了一种水田拖拉机位姿检测和偏航角提取方法及装置，解决了水田拖拉机在工作时没有对其位姿和偏航角进行实时提取，导致其不能够精准、高效的完成田间工作的问题。

(二)技术方案

为实现以上目的，本发明提出一种水田拖拉机位姿检测和偏航角提取方法，包括如下步骤：

S1、在水田拖拉机初始运行时，初始摄像模块会对水田拖拉机的运行轨迹进行摄像，此时第一运行轨迹特征采集模块会根据摄像对水田拖拉机的运行轨迹特征进行采集，从而标准运行轨迹确定模块对运行轨迹进行确定，作为水田拖拉机后期运行的标准轨迹，待标准运行轨迹确定模块确定完毕后会将其发送至中央处理模块；

S2、在水田拖拉机正常运行时，运行摄像模块会对水田拖拉机的实时运行轨迹进行摄像，此时第二运行轨迹特征采集模块会根据摄像对水田拖拉机的运行轨迹特征进行采集，从而实时运行轨迹确定模块对运行轨迹进行确定，作为水田拖拉机运行的实时轨迹，待实时运行轨迹确定模块确定完毕后会将其发送至中央处理模块；

S3、待水田拖拉机的标准轨迹和实时轨迹均确定完毕后，中央处理模块会将标准轨迹和实时轨迹均发送至偏航角提取系统，此时偏航角提取系统中的轨迹对比模块会对标准轨迹和实时轨迹进行对比，若标准轨迹和实时轨迹不吻合，此时偏航角提取系统中的偏航角计算模块会对偏航角度进行计算，从而得出最终偏航角；

S4、待偏航角计算模块计算出偏航角后，会将偏航角数据分别发送至数据接收模块和驱动轮控制系统中的驱动轮转向分析模块，此时驱动轮转向分析模块会分析驱动轮回到标准轨迹所要转向的角度，从而驱动轮转向控制模块会根据分析结果来控制驱动轮转向的角度；

S5、在水田拖拉机运行时，陀螺仪会对其位姿进行监测，陀螺仪会将实时监测数据通过中央处理模块发送至数据接收模块，从而数据接收模块会将接收到的位姿信息和偏航角数据在显示屏上进行显示。

优选的，所述陀螺仪的输出端与中央处理模块的输入端连接，所述中央处理模块的输出端与数据接收模块的输入端连接，并且数据接收模块的输出端与显示屏的输入端连接。