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

权利要求书

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

所述陀螺仪(12)的输出端与中央处理模块(4)的输入端连接，所述中央处理模块(4)的输出端与数据接收模块(9)的输入端连接，并且数据接收模块(9)的输出端与显示屏(13)的输入端连接；

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

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

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

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

其中，B_it为像素矩阵A第i行t列灰度化后的值，AR_it为像素矩阵A第i行t列像素点的R值，AG_it为像素矩阵A第i行t列像素点的G值，AB_it为像素矩阵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个值按从大到小进行排序，形成向量C；

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

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

其中，ρ_j为所述实时拍摄的图像与所述图像数据库中第j张图像的奇异特征值距离，e为自然常数，C为向量C，C_i为向量C的第i个值，F_ji为矩阵F的第j行第i列的值，F_j^T为矩阵F的第j行的转置，L为矩阵F中每行的值的个数，i＝1、2、3……L,j＝1、2、3……P，提取求解的所有的ρ_j中的最小值ρ_S,S小于等于j，所述最小值所对应的所述图像数据库中的第S张图像则为与实时拍摄的图像最接近的图像，所述第S张图像对应的运行轨迹特征则为实时拍摄的图像所对应的运行轨迹特征；

将第S-2、S-1、S张图像所确定的运行轨迹特征传入实时运行轨迹确定模块(7),利用轨迹对比模块(801)对实时运行轨迹确定模块(7)所确定的实时轨迹与标准运行轨迹确定模块(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张图像所确定运行轨迹特征的纵坐标，arcsec(tt)为tt的反余弦值，θ为求解的偏航角度。