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

说明书

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

技术领域

本发明涉及测量技术领域，尤其涉及一种基于九轴MEMS传感器的农业机械全姿态角更新方法。

背景技术

随着MEMS(Micro-Electro-Mechanical-System)传感器、导航和控制技术的发展以及国家对农业扶持力度的进一步加大，精准农业正在快速变成一种趋势，而在农业机械辅助驾驶控制过程中，车体的姿态，包括俯仰角、翻滚角和航向角，这些信息能够为高精度的组合导航和控制算法提供重要的数据输入。

目前，惯性导航系统分为PINS(Platform Inertial Navigation System)和SINS(Strapdown Inertial Navigation System)，SINS相比于PINS是采用IMU(InertialMeasuring Unit)传感器通过计算建立一个“数学平台”来代替PINS。SINS多使用在飞行器导航控制系统中，而针对农业机械控制领域的研究和应用则属于起步阶段，而且二者的应用对象和环境条件有较大的差别，飞行器控制系统中捷联惯导实现的方法在农业机械控制中不尽适用。

发明内容

鉴于目前在农业机械上应用惯性导航存在的上述不足，本发明提供一种基于九轴MEMS传感器的农业机械全全姿态角更新方法，误差小、精度高、稳定可靠。

为达到上述目的，本发明的实施例采用如下技术方案：

一种基于九轴MEMS传感器的农业机械全姿态角更新方法，所述基于九轴MEMS传感器的农业机械全姿态角更新方法包括如下步骤：

建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型，并设定相应车体运动姿态的参数向量；

通过九轴MEMS传感器实时获取车体运动的加速度、角速度与地球磁场强度数据；

根据获取的车体运动的加速度、角速度与地球磁场强度数据通过建立的陀螺仪误差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位置信息、航向角度；

通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进行数据融合处理，对车体的运动姿态角进行实时更新；

其中，所述九轴MEMS传感器由三轴陀螺仪、三轴加速度计和三轴地磁传感器组成。

依照本发明的一个方面，所述建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型，并设定相应车体运动姿态的参数向量步骤具体为：

陀螺仪误差模型通过陀螺仪误差计算公式对陀螺仪角速度进行计算，其中，陀螺仪误差计算公式：ω＝ω_ib+b_ωr+b_ωg，其中ω为陀螺仪输出角速度，ω_ib为陀螺仪真实角速度，b_ωr为陀螺仪零漂，b_ωg为陀螺仪输出白噪声；

通过电子罗盘校准椭圆模型消除磁场干扰；其中，电子罗盘校准椭圆模型：mx，my为地磁场强度，Xoffset和Yoffset为硬磁干扰，Xsf和Ysf为软磁干扰；

通过七维EKF滤波模型对车体姿态进行更新处理，其中，七维EKF滤波模型为七维状态向量的扩展卡尔曼滤波，EKF包括状态方程与观测方程：