[发明专利]一种农业机械车辆姿态测量方法及测量模块

权利要求书

1.一种农业机械车辆姿态测量模块，其特征在于，包括集成在同一 块电路板上的陀螺仪、磁力计和加速度计:

所述加速度计用于测量农业机械车辆加速度的大小，一处理器根 据加速度计测得的加速度的大小来确定农业机械车辆的俯仰角和 横滚角；

所述磁力计用于测量地球磁场强度的大小，处理器根据磁力计测 得的磁场强度大小和根据计算得到的俯仰角、航向角来确定农业 机械车辆的航向角；

所述陀螺仪用于测量农业机械车辆的角速度，处理器根据陀螺仪 测得的角速度对时间的积分来更新农业机械的航向角、俯仰角和 横滚角。

2.一种应用于农业机械车辆姿态测量模块的测量方法，其特征在于，

S1:一处理器获取加速度计测量到的加速度信息，根据所述加速度 信息计算得到农业机械车辆当前的俯仰角和横滚角；

S2：处理器获取磁力计测量到的地球磁场强度的大小，结合俯仰 角和横滚角计算得到的航向角；

S3：在S1和S2中得到的俯仰角、横滚角和航向角记为姿态量的 初始值，处理器获取陀螺仪测量到的角速度信息，并通过四元数 姿态解算方法对姿态量的初始值进行更新，得到姿态量更新值；

S4：处理器获取当前的姿态量更新值并分解出加速度和磁场强度， 将分解出的加速度和磁场强度与加速度计和磁力计的测量得到的 加速度信息和磁场强度大小的差值通过卡尔曼滤波算法估计出姿 态量更新值的误差和仪器参数的误差；

S5：利用通过卡尔曼滤波算法校正过的仪器参数对陀螺仪测得的 角速度信息进行补偿。