一种农业机械车辆姿态测量方法及测量模块技术领域
本发明属于姿态测量领域，具体涉及一种农业机械车辆姿态测
量方法及测量模块。
背景技术
目前的车辆姿态检测主要分两种应用场景，一种是异常报警，比
如翻车检测；一种高精度的导航辅助，即本发明涉及的领域。目前实
现高精度姿态测量的方法是在车辆上并排安装上两副卫星天线，通过
卫星接收机，实现静止和运动时的航向角测量，同时运用陀螺仪和加
速计，计算车辆横滚角和俯仰角，两者结合，实现车辆的姿态测量。
以上方法出自专利《AttitudemeasurementusingasingleGPS
receiverwithtwoclosely-spacedantennas》。
使用双天线结合陀螺仪和加速计的方案，固然可以取得高精度
的姿态测量结果。但该方案要求提供两副高精度卫星天线，成本不菲；
其次，该方案要求卫星接收机具备双天线定向测量功能。卫星接收机
最主要的功能是定位，而在定向上，相比目前的其他技术，并没有太
大的优势。
发明内容
本发明的目的之一是提供了一种农业机械车辆姿态测量模块，其
要解决的是当前测量设备成本高，测量精度不高和依赖卫星信号的问
题。
为了达到上述的目的，所采用的技术方案如下：
一种农业机械车辆姿态测量模块，包括集成在同一块电路板上的
陀螺仪、磁力计和加速度计:
所述加速度计用于测量农业机械车辆加速度的大小，一处理器根
据加速度计测得的加速度的大小来确定农业机械车辆的俯仰角和
横滚角；
所述磁力计用于测量地球磁场强度的大小，处理器根据磁力计测
得的磁场强度的大小结合俯仰角、横滚角来确定农业机械车辆的
航向角；
所述陀螺仪用于测量农业机械车辆的角速度，处理器根据陀螺仪
测得的角速度对时间的积分来对测得的航向角、俯仰角和横滚角
进行更新。
本发明另一个目的在于提供了一种农业机械车辆姿态测量方法，
其要解决的是当前农业机械车辆姿态测量不准确的问题。
为了达到上述的目的，所采用的技术方案如下：
一种应用于农业机械车辆姿态测量模块的测量方法：
S1:一处理器获取加速度计测量到的加速度信息，根据所述加速
度信息计算得到农业机械车辆当前的俯仰角和横滚角；
S2：处理器获取磁力计测量到的地球磁场强度的大小结合俯仰角、
横滚角，计算得到的航向角；
S3：在S1和S2中得到的俯仰角、横滚角和航向角为姿态量的初
始值，处理器获取陀螺仪测量到的角速度信息，并通过四元数姿
态解算方法对姿态量的初始值进行更新，得到姿态量更新值；
S4：处理器获取当前的姿态量更新值并分解出加速度和磁场强度，
将分解出的加速度和磁场强度与加速度计和磁力计的测量得到的
加速度信息和磁场强度大小的差值通过卡尔曼滤波算法估计出姿
态量更新值的误差和仪器参数的误差；
S5：利用通过卡尔曼滤波算法校正过的仪器参数对陀螺仪测得的
角速度信息进行补偿。
与现有技术相比，本发明的有益效果有：
本发明在一块电路板上同时集成磁力计、加速计和陀螺仪，结合
三者的测量数据进行融合解算，开发出一个小型化、低成本、高精度
的姿态测量模块。使用基于磁力计的定向方案，使得姿态测量模块的
得以排除掉体积庞大的卫星天线，并且具备完整姿态测量功能的模块
很方便集成到更大型的项目当中，方便二次开发。
附图说明
图1为本发明较佳实施例的一种农业机械车辆姿态测量模块的原理
示意图；
图2为本发明农业机械车辆姿态量的示意图；
图3为本发明测量农业机械车辆的俯仰角的示意图；
图4为本发明农业机械车辆姿态测量方法的流程示意图。
具体实施方式
下面，结合附图以及具体实施方式，对本发明做进一步描述：
如图1所示，农业机械车辆姿态测量模块的原理图，俯仰角为农
业机械车辆坐标系的X轴与水平面的夹角，横滚角为农业机械车辆坐
标系的Z轴与通过车辆坐标系的X轴的铅垂面间的夹角，航向角为车
辆坐标系X轴在水平面上投影与地面坐标系X轴之间的夹角。也即图
2所示，图2中的坐标为水平面的坐标系，以X轴为旋转轴转动的角
度为横滚角，以Y轴为旋转轴转动的角度为俯仰角，以Z轴为旋转轴
旋转的角度为航向角。一种农业机械车辆姿态测量模块，包括集成在
同一块电路板上的陀螺仪、磁力计和加速度计:
所述加速度计用于测量农业机械车辆加速度的大小，一处理器根
据加速度计测得的加速度的大小来确定农业机械车辆的俯仰角和横
滚角；
所述磁力计用于测量地球磁场强度的大小，处理器根据磁力计测
得的磁场强度的大小来确定农业机械车辆的航向角；
所述陀螺仪用于测量各个方向上的角加速度，处理器根据陀螺仪
测得的角速度对时间的积分来对测得的航向角、俯仰角和横滚角进行
更新。
本发明还提供了一种能够精确测量农农业机械车辆的姿态，为其
卫星定位解算提供姿态修正值的农业机械车辆姿态测量方法。
S1:处理器获取加速度计测量到的加速度信息，根据所述加速度信
息计算得到农业机械车辆当前的俯仰角和横滚角；
S2：处理器获取磁力计测量到的地球磁场强度的的大小结合俯仰
角和横滚角，计算得到的航向角；
S3：在S1和S2中得到的俯仰角、横滚角和航向角为姿态量的初
始值，处理器获取陀螺仪测量到的角速度信息，并通过四元数姿态解
算方法对姿态量的初始值进行更新，得到姿态量更新值；
S4：处理器获取当前的姿态量更新值处理器获取当前的姿态量更
新值并分解出加速度和磁场强度，将分解出的加速度和磁场强度与加
速度计和磁力计的测量得到的加速度信息和磁场强度大小的差值通
过卡尔曼滤波算法估计出姿态量更新值的误差和仪器参数的误差；
S5：利用通过卡尔曼滤波算法校正过的仪器参数对陀螺仪测得的
角速度信息进行补偿。
本发明方法的工作原理：当农业机械车辆在水平面上由静止启动
的时候，此时通过姿态测量模块获得一个最初的姿态量更新值。
如果农业机械车辆进入到斜坡时，加速度计获取三轴的加速度信
息。如图3所示，用一个轴从二维角度来说明如何测量角度，正常情
况下，X轴与重力呈垂直状态，此时X轴的测量值是0，Z轴的值等
于g。当X轴仰起，此时X轴测量到的是非零值，此时Z轴测量到图
中虚线箭头的矢量的值，通过三角函数可以算出X轴的仰起角度θ。
此时仰起角度θ即为俯仰角，同理，对于沿Y轴方向得到的角度为横
滚角。
磁力计可以测量到地球磁场强度的大小，由于地球存在磁偏角，
所以磁力计测得的并不是正北的方向，并且处于斜坡之上，通过俯仰
角和横滚角对此时磁力计测量值进行倾斜补偿校正，来得到当前的磁
航向角，通过磁偏校正来得到航向角。
获得陀螺仪测量的角度是角速度乘时间间隔得到的量即是姿态的
增量，由加速度计和磁力计得到的姿态量的初值加上姿态的增量即可
实现姿态的更新。在更新的过程中，如果直接对俯仰角、横滚角和航
向角更新存在局限，所以需要用四元数来表示姿态，四元数主要是一
种数学的处理方法，通过四元数乘法进行姿态更新，本质上是一样的。
测量中由于仪器的测量会存在一些误差，为了得到更好更精准的
测量效果，将得到的姿态量更新值与上一时刻的姿态量更新值进行平
滑计算，然后通过卡尔曼滤波算法来输出姿态四元数的误差和仪器参
数的误差，并更新姿态量更新值和仪器参数。仪器参数主要是姿态测
量模块中产生的零偏误差，这个校正的过程是在计算过程中不断进行
的，可以实时获得仪器参数的最新状态，将得到的仪器参数对所述的
姿态测量模块进行补偿，这样，能更加准确的测量姿态量更新值。
对本领域的技术人员来说，可根据以上描述的技术方案以及构思，
做出其它各种相应的改变以及形变，而所有的这些改变以及形变都应
该属于本发明权利要求的保护范围之内。