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

说明书

本发明公开了一种水田拖拉机位姿检测和偏航角提取的方法及装置，该方法包括：通过三轴加速度传感器测量水田拖拉机三个坐标轴上的加速度数据，从而得到水田拖拉机各个坐标轴与重力之间的夹角；通过三轴陀螺仪传感器测量水田拖拉机绕坐标轴转动的角速度数据，从而得到水田拖拉机转动的角度；通过三轴地磁传感器测量水田拖拉机各个方向的磁场数据，从而得到水田拖拉机的偏航角；采用互补滤波算法将三轴加速度传感器测量的角度数据和三轴陀螺仪传感器测量的角度数据进行融合；采用姿态数据融合算法将三轴陀螺仪传感器测量的角度数据和三轴地磁传感器测量的偏航角数据进行融合。本发明可以保证水田拖拉机在作业过程中获得精确的位姿信息和偏航角信息。

技术领域

本发明涉及一种水田拖拉机位姿检测和偏航角提取的方法及装置，属于水田拖拉机导航技术领域。

背景技术

随着农业机械化的快速发展，对高精度导航技术的需求也越来越高，为了保证水田拖拉机能够准确、高效地完成田间工作，提取精确的水田拖拉机位姿信息和偏航角信息对水田拖拉机的导航和智能化工作至关重要。

发明内容

本发明的目的是为了解决上述现有技术的缺陷，提供了一种水田拖拉机位姿检测和偏航角提取的方法，该方法可以保证水田拖拉机在作业过程中获得精确的位姿信息和偏航角信息。

本发明的另一目的在于提供一种一种水田拖拉机位姿检测和偏航角提取的装置。

本发明的目的可以通过采取如下技术方案达到：

一种水田拖拉机位姿检测和偏航角提取的方法，所述方法包括：

通过三轴加速度传感器测量水田拖拉机三个坐标轴上的加速度数据，从而得到水田拖拉机各个坐标轴与重力之间的夹角；

通过三轴陀螺仪传感器测量水田拖拉机绕坐标轴转动的角速度数据，从而得到水田拖拉机转动的角度；

通过三轴地磁传感器测量水田拖拉机各个方向的磁场数据，从而得到水田拖拉机的偏航角；

采用互补滤波算法将三轴加速度传感器测量的角度数据和三轴陀螺仪传感器测量的角度数据进行融合；

采用姿态数据融合算法将三轴陀螺仪传感器测量的角度数据和三轴地磁传感器测量的偏航角数据进行融合。

进一步的，所述方法还包括：

根据陀螺仪传感器的温度零点偏移对陀螺仪传感器的温度进行补偿。

进一步的，所述温度零点偏移曲线为三次多项式BX＝At₃+Bt₂+Ct₁+D，通过最小二乘法拟合之后得到各项系数。

进一步的，所述方法还包括：

通过三轴加速度传感器输入输出加速度数据，通过三轴陀螺仪传感器输入输出角速度数据，以及通过三轴地磁传感器输入输出磁场数据，利用卡尔曼滤波算法对各传感器状态进行最优估计，并通过最优估计去除各传感器中噪声和干扰对测量数据的影响。

进一步的，所述方法还包括：

通过卡尔曼滤波融合算法采集到的加速度数据、角速度数据和磁场数据进行误差补偿和滤波，将三轴陀螺仪传感器测量的角度数据作为卡尔曼滤波器的预测值，并估计过程噪声协方差；将三轴加速度传感器测量的角度数据和三轴地磁传感器测量的偏航角作为测量值，并结合三轴陀螺仪传感器误差估计测量噪声协方差矩阵，实现三轴加速度传感器、三轴陀螺仪传感器和三轴地磁传感器的融合。

进一步的，所述方法还包括：

利用三轴陀螺仪传感器测量的角速度数据作为四元数的更新，以三轴加速度传感器测量的重力加速度数据作为四元数的观测，实时解算姿态角。