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

摘要

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

主权项

1.一种水田拖拉机位姿检测和偏航角提取方法，其特征在于：具体包括以下步骤：S1、在水田拖拉机初始运行时，初始摄像模块(1)会对水田拖拉机的运行轨迹进行摄像，此时第一运行轨迹特征采集模块(2)会对根据摄像对水田拖拉机的运行轨迹特征进行采集，从而标准运行轨迹确定模块(3)对其运行轨迹进行确定，作为水田拖拉机后期运行的标准轨迹，待标准运行轨迹确定模块(3)确定完毕后会将其发送至中央处理模块(4)；S2、在水田拖拉机正常运行时，运行摄像模块(5)会对水田拖拉机的实时运行轨迹进行摄像，此时第二运行轨迹特征采集模块(6)会对根据摄像对水田拖拉机的运行轨迹特征进行采集，从而实时运行轨迹确定模块(7)对运行轨迹进行确定，为水田拖拉机运行的实时轨迹，待实时运行轨迹确定模块(7)确定完毕后会将其发送至中央处理模块(4)；S3、待水田拖拉机的标准轨迹和实时轨迹均确定完毕后，中央处理模块(4)会将标准轨迹和实时轨迹均发送至偏航角提取系统(8)，此时偏航角提取系统(8)中的轨迹对比模块(801)会对标准轨迹和实时轨迹进行对比，若标准轨迹和实时轨迹不吻合，此时偏航角提取系统(8)中的偏航角计算模块(802)会对偏航角度进行计算，从而得出最终偏航角；S4、待偏航角计算模块(802)计算出偏航角后，会将偏航数据分别发送至数据接收模块(9)和驱动轮控制系统(10)中的驱动轮转向分析模块(101)，此时驱动轮转向分析模块(101)会分析驱动轮(11)回到标准轨道所要转向的角度，从而驱动轮转向控制模块(102)会根据分析结果来控制驱动轮(11)转向的角度；S5、在水田拖拉机运行时，陀螺仪(12)会对其位姿进行监测，陀螺仪(12)会将实时监测数据通过中央处理模块(4)发送至数据接收模块(9)，从而数据接收模块(9)会将接收到的位姿信息和偏航角数据在显示屏(13)上进行显示。