[发明专利]基于九轴MEMS传感器的农业机械全姿态角更新方法

摘要

本发明公开了一种基于九轴MEMS传感器的农业机械全姿态角更新方法，所述基于九轴MEMS传感器的农业机械全姿态角更新方法包括如下步骤：建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型，并设定相应车体运动姿态的参数向量；实用获取车体运动的加速度、角速度与地球磁场强度数据；通过建立陀螺仪误差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位置信息、航向角度；通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进行数据融合处理，对车体的运动姿态角进行实时更新，上述方法步骤，误差小、精度高、稳定可靠。

主权项

1.一种基于九轴MEMS传感器的农业机械全姿态角更新方法，其特征在于，所述基于九轴MEMS传感器的农业机械全姿态角更新方法包括如下步骤：建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型，并设定相应车体运动姿态的参数向量；陀螺仪误差模型通过陀螺仪误差计算公式对陀螺仪角速度进行计算，其中，陀螺仪误差计算公式：ω＝ωib+bωr+bωg，其中ω为陀螺仪输出角速度，ωib为陀螺仪真实角速度，bωr为陀螺仪零漂，bωg为陀螺仪输出白噪声；通过电子罗盘校准椭圆模型消除磁场干扰；其中，电子罗盘校准椭圆模型：mx、my为地磁场强度，Xoffset和Yoffset为硬磁干扰，Xsf和Ysf为软磁干扰；通过七维EKF滤波模型对车体姿态进行更新处理，其中，七维EKF滤波模型为七维状态向量的扩展卡尔曼滤波，EKF包括状态方程与观测方程：y＝h(x)+v1状态矩阵为x＝[q bωr]，q为四元数向量[q0 q1 q2 q3]T，其中ω为陀螺仪输出角速度，w1为过程噪声矩阵，v1为观测噪声矩阵，y为观测量，y＝[a ψmag]T，其中a为三轴加速度值，ψmag为电子罗盘计算的航向角；通过九轴MEMS传感器实时获取车体运动的加速度、角速度与地球磁场强度数据；根据获取的车体运动的加速度、角速度与地球磁场强度数据通过建立的陀螺仪误差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位置信息、航向角度；通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进行数据融合处理，对车体的运动姿态角进行实时更新；其中，所述九轴MEMS传感器由三轴陀螺仪、三轴加速度计和三轴地磁传感器组成。