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

权利要求书

1.一种水田拖拉机位姿检测和偏航角提取的方法，其特征在于，所述方法包括：

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

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

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

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

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

所述采用互补滤波算法将三轴加速度传感器测量的角度数据和三轴陀螺仪传感器测量的角度数据进行融合，具体为：

通过三轴加速度传感器测量的角度数据和上一次融合后的角度数据对三轴陀螺仪传感器的角速度数据进行修改，然后再进行积分；当水田拖拉机处于静止状态时，增大三轴加速度传感器测量的角度数据权重，当水田拖拉机处于运动状态时，增大三轴陀螺仪传感器测量的角度数据的权重；

所述采用姿态数据融合算法将三轴陀螺仪传感器测量的角度数据和三轴地磁传感器测量的偏航角数据进行融合，具体为：

将三轴陀螺仪传感器测量的角度数据和三轴地磁传感器测量的偏航角数据进行融合；当水田拖拉机处于静止状态时，增大三轴地磁传感器测量的偏航角数据的权重，当水田拖拉机处于运动状态时，增大三轴陀螺仪传感器测量的角度数据的权重。

2.根据权利要求1所述的水田拖拉机位姿检测和偏航角提取的方法，其特征在于，所述方法还包括：

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

3.根据权利要求1所述的水田拖拉机位姿检测和偏航角提取的方法，其特征在于，所述方法还包括：

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

4.根据权利要求1所述的水田拖拉机位姿检测和偏航角提取的方法，其特征在于，所述方法还包括：

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

5.根据权利要求1所述的水田拖拉机位姿检测和偏航角提取的方法，其特征在于，所述方法还包括：

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