一种水田拖拉机位姿检测和偏航角提取方法及装置与流程

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

背景技术：

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

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

技术实现要素：

(一)解决的技术问题

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

(二)技术方案

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

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

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

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

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

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

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

优选的，所述初始摄像模块的输出端与第一运行轨迹特征采集模块的输入端连接，所述第一运行轨迹特征采集模块的输出端与标准运行轨迹确定模块的输入端连接，并且标准运行轨迹确定模块的输出端与中央处理模块的输入端连接。

优选的，所述运行摄像模块的输出端与第二运行轨迹特征采集模块的输入端连接，所述第二运行轨迹特征采集模块的输出端与实时运行轨迹确定模块的输入端连接，并且实时运行轨迹确定模块的输出端与中央处理模块的输入端连接。

优选的，所述中央处理模块的输出端与偏航角提取系统的输入端连接，所述偏航角提取系统的输出端分别与数据接收模块和驱动轮控制系统的输入端连接，并且驱动轮控制系统的输出端与驱动轮的输入端连接。

优选的，所述偏航角提取系统包括轨迹对比模块和偏航角计算模块，所述轨迹对比模块的输出端与偏航角计算模块的输入端连接，并且偏航角计算模块的输出端与驱动轮控制系统的输入端连接，所述偏航角计算模块的输入端与数据接收模块的输出端连接。

优选的，所述驱动轮控制系统包括驱动轮转向分析模块和驱动轮转向控制模块，所述驱动轮转向分析模块的输出端与驱动轮转向控制模块的输入端连接，并且驱动轮转向控制模块的输出端与驱动轮的输入端连接，所述驱动轮转向分析模块的输入端与偏航角计算模块的输出端连接。

本发明还公开了一种水田拖拉机位姿检测和偏航角提取装置，包括拖拉机车架，所述拖拉机车架两侧的顶部和底部均转向连接有驱动轮，所述拖拉机车架表面的下方固定连接有驱动轮转向控制模块，所述拖拉机车架的表面且位于驱动轮转向控制模块的正上方固定连接有陀螺仪，所述拖拉机车架的表面且位于陀螺仪的上方通过连接架固定连接有显示屏，所述拖拉机车架顶部的两侧分别固定连接有初始摄像模块和运行摄像模块。

优选的，所述s2-s3中的第二运行轨迹特征采集模块(6)根据摄像对水田拖拉机的运行轨迹特征进行采集和偏航角提取系统(8)中的偏航角计算模块(802)会对偏航角度进行计算得出最终偏航角具体步骤如下所述：

首先，将所述初始摄像模块(1)对水田拖拉机的运行轨迹进行摄像得到的摄像视频中的每一帧都作为一张图像进行保存，形成图像数据库，则所述图像数据库中有p张图像，并保存所述图像所对应的轨迹特征；

然后将所述运行摄像模块(5)实时拍摄的图像进行图像与所述图像数据库中的图像进行匹配，其具体的匹配方式如下：

将所述实时拍摄的图像进行像素点的提取，得到像素点的值的矩阵a，矩阵a包括l行m列，同时因为像素是包含rgb三个值，则像素点的矩阵a中的每个元素中间都是包含有3个值组成的集合，则利用公式(1)将像素矩阵a中的每个元素都处理为只有一个值的灰度化后的像素矩阵b

其中，bit为像素矩阵a第i行t列灰度化后的值，arit为像素矩阵a第i行t列像素点的r值，agit为像素矩阵a第i行t列像素点的g值，abit为像素矩阵a第i行t列像素点的b值，i＝1、2、3……l,t＝1、2、3……m,将所有元素都灰度化形成矩阵b后，将矩阵b利用公式(2)计算出相应的奇异特征值，

|b*b^t-λe|＝0

其中b为灰度化后的矩阵b，b^t为矩阵b的转置，e为l阶的单位矩阵，λ中间求解出来的值，且所求解出来的特征值为l个值，σ为最终求解的奇异特征值，且σ也是一个包含l个值的向量，将这l个值按从大到小进行排序，形成向量c；

然后分别获取所述图像数据库中的图像的像素点的值，形成相应的像素矩阵di，d为l行m列的矩阵，然后利用公式(1)将所述像素矩阵di进行灰度化，得到相应的灰度化后的矩阵ddi，对于每个灰度矩阵都利用公式(2)计算出相应的奇异特征值并从大到小排序，形成相应的向量ei,最后将所有的ei组成一个矩阵f，其中矩阵f有p行l列的矩阵，每一行代表一张图像数据库中的图像的奇异特征值；

然后利用公式(3)计算向量c与矩阵f中的每一行的奇异特征值距离

其中，ρj为所述实时拍摄的图像与所述图像数据库中第j张图像的奇异特征值距离，e为自然常熟，c为向量c，ci为向量c的第i个值，fji为矩阵

f的第j行第i列的值，fj^t为矩阵f的第j行的转置，l为矩阵f中每行的值的个数，i＝1、2、3……l,j＝1、2、3……p，提取求解的所有的ρj中的最小值ρs,s小于等于j，所述最小值所对应的所述图像数据库中的第s张图像则为与实时拍摄距离最接近的图像，则所述第s张图像对应的运行轨迹特征则为实时拍摄的图像所对应的运行轨迹特征；

将所述确定的运行轨迹特征传入待实时运行轨迹确定模块(7),判断所述第s-2、s-1、s张图所确定的运行轨迹特征的顺序是否为待标准运行轨迹确定模块(3)所确定的运行轨迹是否偏航，若没有，则继续运行，若偏航利用所述偏航角提取系统(8)中的偏航角计算模块(802)会对偏航角度进行计算，计算步骤为以s-2张图像所确定运行轨迹特征为原点，正东方向为x轴，正北方向为y轴，得到对应的第s-1、s正两张图片的运行轨迹特征的坐标，利用公式(4)计算偏航角度：

其中，tt为中间参数，[s-1]x为第s-1张图像所确定运行轨迹特征的横坐标，[s-1]y为第s-1张图像所确定运行轨迹特征的纵坐标，[s]x为第s张图像所确定运行轨迹特征的横坐标，[s]y为第s-1张图像所确定运行轨迹特征的纵坐标，arcsec(tt)为tt的反余弦值，θ为求解的偏航角度。

(三)有益效果

本发明提供了一种水田拖拉机位姿检测和偏航角提取方法及装置。与现有技术相比具备以下有益效果：

(1)、该水田拖拉机位姿检测和偏航角提取方法及装置，通过s1、在水田拖拉机初始运行时，初始摄像模块会对水田拖拉机的运行轨迹进行摄像，s2、在水田拖拉机正常运行时，运行摄像模块会对水田拖拉机的实时运行轨迹进行摄像，s3、待水田拖拉机的标准轨迹和实时轨迹均确定完毕后，中央处理模块会将标准轨迹和实时轨迹均发送至偏航角提取系统，陀螺仪的输出端与中央处理模块的输入端连接，中央处理模块的输出端与数据接收模块的输入端连接，并且数据接收模块的输出端与显示屏的输入端连接，通过陀螺仪对水田拖拉机的位姿进行了很好的检测，通过将标准轨迹与实时轨迹进行对比，得出水田拖拉机的偏航角，使用者可以通过显示屏对水田拖拉机的位姿和偏航角进行及时查看，从而使用者可以根据位姿和偏航角对拖拉机进行及时调整，使得水田拖拉机能够高效而准确的进行作业。

(2)、该水田拖拉机位姿检测和偏航角提取方法及装置，通过s4、待偏航角计算模块计算出偏航角后，会将偏航数据分别发送至数据接收模块和驱动轮控制系统中的驱动轮转向分析模块，驱动轮控制系统包括驱动轮转向分析模块和驱动轮转向控制模块，驱动轮转向分析模块的输出端与驱动轮转向控制模块的输入端连接，并且驱动轮转向控制模块的输出端与驱动轮的输入端连接，驱动轮转向分析模块的输入端与偏航角计算模块的输出端连接，在水田拖拉机偏航后，能自动对其进行校正，使其能快速的回到正常轨道，方便了操作人员的使用。

(3)、该水田拖拉机位姿检测和偏航角提取方法及装置，通过拖拉机车架两侧的顶部和底部均转向连接有驱动轮，拖拉机车架表面的下方固定连接有驱动轮转向控制模块，拖拉机车架的表面且位于驱动轮转向控制模块的正上方固定连接有陀螺仪，拖拉机车架的表面且位于陀螺仪的上方通过连接架固定连接有显示屏，拖拉机车架顶部的两侧分别固定连接有初始摄像模块和运行摄像模块，将部件集中安装在车架上，方便了后期的维护。

附图说明

图1为本发明系统的结构原理框图；

图2为本发明偏航角提取系统的结构原理框图；

图3为本发明驱动轮控制系统的结构原理框图；

图4为本发明拖拉机车架、陀螺仪和显示屏结构的俯视图。

图中，1、初始摄像模块；2、第一运行轨迹特征采集模块；3、标准运行轨迹确定模块；4、中央处理模块；5、运行摄像模块；6、第二运行轨迹特征采集模块；7、实时运行轨迹确定模块；8、偏航角提取系统；9、数据接收模块；10、驱动轮控制系统；11、驱动轮；12、陀螺仪；13、显示屏；14、拖拉机车架；101、驱动轮转向分析模块；102、驱动轮转向控制模块；801、轨迹对比模块；802、偏航角计算模块。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1-4，本发明实施例提供一种技术方案：一种水田拖拉机位姿检测和偏航角提取方法，包括如下步骤：

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上进行显示。

本发明中，陀螺仪12的输出端与中央处理模块4的输入端连接，中央处理模块4的输出端与数据接收模块9的输入端连接，并且数据接收模块9的输出端与显示屏13的输入端连接。

本发明中，初始摄像模块1的输出端与第一运行轨迹特征采集模块2的输入端连接，第一运行轨迹特征采集模块2的输出端与标准运行轨迹确定模块3的输入端连接，并且标准运行轨迹确定模块3的输出端与中央处理模块4的输入端连接。

本发明中，运行摄像模块5的输出端与第二运行轨迹特征采集模块6的输入端连接，第二运行轨迹特征采集模块6的输出端与实时运行轨迹确定模块7的输入端连接，并且实时运行轨迹确定模块7的输出端与中央处理模块4的输入端连接。

本发明中，中央处理模块4的输出端与偏航角提取系统8的输入端连接，偏航角提取系统8的输出端分别与数据接收模块9和驱动轮控制系统10的输入端连接，并且驱动轮控制系统10的输出端与驱动轮11的输入端连接。

本发明中，偏航角提取系统8包括轨迹对比模块801和偏航角计算模块802，轨迹对比模块801的输出端与偏航角计算模块802的输入端连接，并且偏航角计算模块802的输出端与驱动轮控制系统10的输入端连接，偏航角计算模块802的输入端与数据接收模块9的输出端连接。

本发明中，驱动轮控制系统10包括驱动轮转向分析模块101和驱动轮转向控制模块102，驱动轮转向分析模块101的输出端与驱动轮转向控制模块102的输入端连接，并且驱动轮转向控制模块102的输出端与驱动轮11的输入端连接，驱动轮转向分析模块101的输入端与偏航角计算模块802的输出端连接。

本发明还公开了一种水田拖拉机位姿检测和偏航角提取装置，包括拖拉机车架14，拖拉机车架14两侧的顶部和底部均转向连接有驱动轮11，拖拉机车架14表面的下方固定连接有驱动轮转向控制模块102，拖拉机车架14的表面且位于驱动轮转向控制模块102的正上方固定连接有陀螺仪12，陀螺仪12是用高速回转体的动量矩敏感壳体相对惯性空间绕正交于自转轴的一个或二个轴的角运行检测装置，拖拉机车架14的表面且位于陀螺仪12的上方通过连接架固定连接有显示屏13，拖拉机车架14顶部的两侧分别固定连接有初始摄像模块1和运行摄像模块5。

本发明中，所述s2-s3中的第二运行轨迹特征采集模块(6)根据摄像对水田拖拉机的运行轨迹特征进行采集和偏航角提取系统(8)中的偏航角计算模块(802)会对偏航角度进行计算得出最终偏航角具体步骤如下所述：

首先，将所述初始摄像模块(1)对水田拖拉机的运行轨迹进行摄像得到的摄像视频中的每一帧都作为一张图像进行保存，形成图像数据库，则所述图像数据库中有p张图像，并保存所述图像所对应的轨迹特征；

然后将所述运行摄像模块(5)实时拍摄的图像进行图像与所述图像数据库中的图像进行匹配，其具体的匹配方式如下：

将所述实时拍摄的图像进行像素点的提取，得到像素点的值的矩阵a，矩阵a包括l行m列，同时因为像素是包含rgb三个值，则像素点的矩阵a中的每个元素中间都是包含有3个值组成的集合，则利用公式(1)将像素矩阵a中的每个元素都处理为只有一个值的灰度化后的像素矩阵b

其中，bit为像素矩阵a第i行t列灰度化后的值，arit为像素矩阵a第i行t列像素点的r值，agit为像素矩阵a第i行t列像素点的g值，abit为像素矩阵a第i行t列像素点的b值，i＝1、2、3……l,t＝1、2、3……m,将所有元素都灰度化形成矩阵b后。

利用公式(1)可以将所述图片的像素矩阵中每个元素的三个值转变为一个值，避免了计算量过大的情况，同时根据rgb在像素中所占有的地位不同，在灰度化时分别给予了不同的权重，使灰度化后的像素相比于原始像素矩阵损失的信息量最小。

矩阵b利用公式(2)计算出相应的奇异特征值，

|b*b^t-λe|＝0

其中b为灰度化后的矩阵b，b^t为矩阵b的转置，e为l阶的单位矩阵，λ中间求解出来的值，且所求解出来的特征值为l个值，σ为最终求解的奇异特征值，且σ也是一个包含l个值的向量，将这l个值按从大到小进行排序，形成向量c；

利用公式(2)则能将原来研究时，本来需要研究一个较为复杂的矩阵转变为仅仅研究一个向量，将后面的计算量大幅度的减小，而且利用公式(2)可以克服在在求取特征值时的矩阵不是方阵而对应的不能求解特征值的情况，使任意矩阵都可以通过公式(2)求解出相应的奇异特征值，而且将奇异特征值从大到小排序，使向量中的前面向量的值比后面向量的值所包含的信息量更大，在公式(3)中计算时，所可能舍去的信息量更少，关联度的计算更准确。

然后分别获取所述图像数据库中的图像的像素点的值，形成相应的像素矩阵di，d为l行m列的矩阵，然后利用公式(1)将所述像素矩阵di进行灰度化，得到相应的灰度化后的矩阵ddi，对于每个灰度矩阵都利用公式(2)计算出相应的奇异特征值并从大到小排序，形成相应的向量ei,最后将所有的ei组成一个矩阵f，其中矩阵f有p行l列的矩阵，每一行代表一张图像数据库中的图像的奇异特征值；

然后利用公式(3)计算向量c与矩阵f中的每一行的奇异特征值距离

其中，ρj为所述实时拍摄的图像与所述图像数据库中第j张图像的奇异特征值距离，e为自然常熟，c为向量c，ci为向量c的第i个值，fji为矩阵f的第j行第i列的值，fj^t为矩阵f的第j行的转置，l为矩阵f中每行的值的个数，i＝1、2、3……l,j＝1、2、3……p，提取求解的所有的ρj中的最小值ρs,s小于等于j，所述最小值所对应的所述图像数据库中的第s张图像则为与实时拍摄距离最接近的图像，则所述第s张图像对应的运行轨迹特征则为实时拍摄的图像所对应的运行轨迹特征。

利用公式(3)求解奇异特征值距离时，根据同一矩阵中，奇异特征值特征值的大小不同，所包含的信息量不同这一特性，在求解奇异特征值距离时根据特征值的排名不同给予了不同的权重，使得包含信息量更多的权重对距离的影响更大。

将所述确定的运行轨迹特征传入待实时运行轨迹确定模块(7),判断所述第s-2、s-1、s张图所确定的运行轨迹特征的顺序是否为待标准运行轨迹确定模块(3)所确定的运行轨迹是否偏航，若没有，则继续运行，若偏航利用所述偏航角提取系统(8)中的偏航角计算模块(802)会对偏航角度进行计算，计算步骤为以s-2张图像所确定运行轨迹特征为原点，正东方向为x轴，正北方向为y轴，得到对应的第s-1、s正两张图片的运行轨迹特征的坐标，利用公式(4)计算偏航角度：

其中，tt为中间参数，[s-1]x为第s-1张图像所确定运行轨迹特征的横坐标，[s-1]y为第s-1张图像所确定运行轨迹特征的纵坐标，[s]x为第s张图像所确定运行轨迹特征的横坐标，[s]y为第s-1张图像所确定运行轨迹特征的纵坐标，arcsec(tt)为tt的反余弦值，θ为求解的偏航角度。

利用上述技术确定运行轨迹特征时，可以在不借助外界数据库的情况下，也不需要人为干预操作的情况下，仅仅根据初始摄像模块(1)所拍摄的视频和运行摄像模块所拍摄的图片就能通过一些简单的运算准确的确定出运行轨迹特征，从而使得实时运行轨迹确定模块(7)能对运行轨迹进行确定，同时通过对轨迹的确定判断是否出现偏航，若出现偏航则自动计算调整偏航角度。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。