基于九轴mems传感器的农业机械全姿态角更新方法
【技术领域】
[0001] 本发明涉及测量技术领域，尤其涉及一种基于九轴MEMS传感器的农业机械全姿 态角更新方法。
【背景技术】
[0002] 随着MEMS(Micro-Electro-Mechanical-System)传感器、导航和控制技术的发展 以及国家对农业扶持力度的进一步加大，精准农业正在快速变成一种趋势，而在农业机械 辅助驾驶控制过程中，车体的姿态，包括俯仰角、翻滚角和航向角，这些信息能够为高精度 的组合导航和控制算法提供重要的数据输入。
[0003]目前，惯性导航系统分为PINS(PlatformInertialNavigationSystem)和 SINS(StrapdownInertialNavigationSystem)，SINS相比于PINS是米用IMU(Inertial MeasuringUnit)传感器通过计算建立一个"数学平台"来代替PINS。SINS多使用在飞行 器导航控制系统中，而针对农业机械控制领域的研究和应用则属于起步阶段，而且二者的 应用对象和环境条件有较大的差别，飞行器控制系统中捷联惯导实现的方法在农业机械控 制中不尽适用。
【发明内容】
[0004] 鉴于目前在农业机械上应用惯性导航存在的上述不足，本发明提供一种基于九轴 MEMS传感器的农业机械全全姿态角更新方法，误差小、精度高、稳定可靠。
[0005] 为达到上述目的，本发明的实施例采用如下技术方案：
[0006] -种基于九轴MEMS传感器的农业机械全姿态角更新方法，所述基于九轴MEMS传 感器的农业机械全姿态角更新方法包括如下步骤：
[0007] 建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型，并设定相应 车体运动姿态的参数向量；
[0008] 通过九轴MEMS传感器实时获取车体运动的加速度、角速度与地球磁场强度数据；
[0009] 根据获取的车体运动的加速度、角速度与地球磁场强度数据通过建立的陀螺仪误 差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位置信息、航向角度；
[0010] 通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进行数据融合处 理，对车体的运动姿态角进行实时更新；
[0011] 其中，所述九轴MEMS传感器由三轴陀螺仪、三轴加速度计和三轴地磁传感器组 成。
[0012] 依照本发明的一个方面，所述建立陀螺仪误差模型、电子罗盘校准椭圆模型与七 维EKF滤波模型，并设定相应车体运动姿态的参数向量步骤具体为：
[0013] 陀螺仪误差模型通过陀螺仪误差计算公式对陀螺仪角速度进行计算，其中，陀螺 仪误差计算公式：《 = ?lb+b_"+bug，其中《为陀螺仪输出角速度，C0lb为陀螺仪真实角速 度，为陀螺仪零漂，bug为陀螺仪输出白噪声；
[0014] 通过电子罗盘校准椭圆模型消除磁场干扰；其中，电子罗盘校准椭圆模型：
ix，my为地磁场强度，Xoffset和Yoffset为硬磁干扰， Xsf?和Ysf为软磁干扰；
[0015] 通过七维EKF滤波模型对车体姿态进行更新处理，其中，七维EKF滤波模型为七维 状态向量的扩展卡尔曼滤波，EKF包括状态方程与观测方程：
[0016] .v= /'(.V,of)+u'l
[0017] y=h(x)+vl
[0018] 状态矩阵为x=[qbuJ，q为四元数向量qO，ql，q2，q3，b("为XYZ三轴陀螺仪零 漂；其中《为陀螺仪输出角速度，wl为过程噪声矩阵，vl为观测噪声矩阵，y为观测量，y =[a$nag]T，其中a为三轴加速度值，$nag为电子罗盘计算的航向角，
[0019]
[0020]
[0021] 依照本发明的一个方面，所述通过九轴MEMS传感器实时获取车体运动的加速度、 角速度与地球磁场强度数据步骤具体为：
[0022] 通过陀螺仪获取车体的角速度，对陀螺仪零点漂移进行补偿；
[0023] 通过加速度传感器采集车体的加速度数据；
[0024] 通过地磁传感器采集车体的地磁场强度数据。
[0025] 依照本发明的一个方面，所述根据获取的车体运动的加速度、角速度与地球磁场 强度数据通过建立的陀螺仪误差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位 置信息、航向角度步骤具体为：
[0026] 通过陀螺仪误差模型对角速度进行积分计算获得角度数据；
[0027] 通过对加速度数据的积分计算出速度，再次积分计算出位置信息；
[0028] 地磁场强度数据经椭圆模型计算出来的校准参数补偿和倾角修正后计算车体航 向角。
[0029] 依照本发明的一个方面，所述通过七维EKF滤波模型对车体的角度、速度、位置信 息、航向角度进行数据融合处理，对车体的运动姿态角进行实时更新步骤具体为：
[0030] 七维EKF滤波模型通过四元数姿态更新算法对车体姿态数据进行计算，其中，EKF 算法计算过程：
[0038] k为采样时刻，为系统状态估计量，（_)为前一时刻，（+)为后一时刻，①k 为状态转移矩阵，Pk为最小均方误差矩阵，Q为状态向量对应的协方差矩阵，Kk为误 差增益，yk为观测向量，Hk为观测方程转移矩阵，Rk为观测向量对应的协方差矩阵。
[0039] Q为四元数向量，q0、ql、q2、q3为组成四元数向量的标量，i、j、k为三维坐标系单 位向量，更新后的姿态矩阵为：
[0040]
[0041] G：为载体坐标系到导航坐标系的旋转矩阵。
LUWJ」 共TY、0、w刀、力俄依;?、1卿1屮用仲肌丨口」用〇
[0044] 依照本发明的一个方面，所述通过七维EKF滤波模型对车体的角度、速度、位置信 息、航向角度进行数据融合处理，对车体的运动姿态角进行实时更新步骤执行后执行以下 步骤：从车体姿态更新数据中提取车体全姿态角数据，确定姿态角数据值，车体全姿态角包 括俯仰角、翻滚角和航向角，其中，
[0045]
[0046] 航向角：
[0047]
[0048] 俯仰角：
[0049] 0 = 0主
[0050] 横滚角：
[0051]
[0052] 本发明实施的优点：通过MEMS传感器实时获取物体的运动的加速度和角速度，通 过陀螺仪输出的角加速度积分可以得到角度，通过对加速度的积分可以计算出速度和再次 积分可以计算出位置信息，通过地磁传感器获取地球磁场，通过补偿算法和与陀螺仪融合 计算出航向角，然后将姿态换算成转换矩阵，从而实现载体坐标系和导航坐标系的转换，此 转换矩阵起着是"数学平台"的作用，将SINS(StrapdownInertialNavigationSystem) 算法应用到农业机械中，转换矩阵尤为重要，由于农业机械时刻在运动，其姿态也在不停的 变化，即转换矩阵也要不停地进行重新计算和更新。常用的姿态更新算法有欧拉角、方向余 弦和四元数，四元数与欧拉角算法相比没有奇异点，与方向余弦相比计算量小非常适合在 嵌入式产品中使用，通过针对农用机械平面内建立地磁场和陀螺仪误差模型陀螺仪误差模 型，建立7维EKF(扩展卡尔曼滤波）更新姿态矩阵，对四元数和陀螺仪零偏进行估计，然后 通过加速度和磁场强度计算的航向角进行观测，从而可以得到较高精度的三维姿态角，采 用的误差补偿和修正算法，大大减小了SINS算法误差干扰，采用的MEMS传感器和SINS算 法使得本发明具有较高的性能参数，经拖拉机测试输出航向角误差小于〇. 1°，俯仰和横滚 角度误差小于0.01°，将四元数作为卡尔曼滤波状态向量可以进一步提高目标参数的计算 精度。
【附图说明】
[0053] 为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的 附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领 域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附 图。
[0054] 图1为本发明所述的一种基于九轴MEMS传感器的农业机械全姿态角更新方法的 实施例1的方法流程图；
[0055] 图2为本发明所述的一种基于九轴MEMS传感器的农业机械全姿态角更新方法的 实施例2的方法流程图。
【具体实施方式】
[0056] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完 整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于 本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所