[发明专利]一种适用于农业机械精准作业的姿态倾角估计方法

摘要

本发明公开了一种适用于农业机械精准作业的姿态倾角估计方法，包括对MEMS陀螺仪与加速度计固有零偏估算和结合陀螺仪二阶AR(Auto Regression)误差模型的自适应卡尔曼滤波姿态融合算法。采用AR模型估计X轴陀螺仪与Y轴陀螺仪测量误差φ_err1、φ_err2、θ_err1、θ_err2作为卡尔曼滤波的状态输入量并自适应估算测量误差协方差矩阵R。建立包含6个状态向量与2个观测向量的系统外部拓展卡尔曼滤波状态方程和测量方程，借助三轴加速度计空间几何模型自适应调整过程误差协方差矩阵Q。本发明方法考虑陀螺仪与加速度计的固有零偏，可避免姿态估算时引入系统误差；二阶AR误差模型可避免随机陀螺仪随机误差对姿态估计造成影响；自适应卡尔曼滤波系统方程可以保证在不同状态下的姿态估计精度。

主权项

1.一种适用于农业机械精准作业的姿态倾角估计方法，其特征在于，包括下述步骤：S1、对陀螺仪与加速度计固有零偏估算，准静态情况下在MEMS传感器预热完成后进行统计平均估算，将传感器原始输出减去得到的固有零偏b_ax、b_ay、b_az、b_gx、b_gy、b_gz作为后续计算的输入；S2、采用二阶AR模型估计X轴陀螺仪与Y轴陀螺仪测量误差φ_err1、φ_err2、θ_err1、θ_err2并作为卡尔曼滤波的状态输入量，并自适应估算测量误差协方差矩阵R；S3、利用三轴加速度计的三维空间几何模型估计姿态横滚角φ、俯仰角θ的初始值，并作为卡尔曼滤波器的状态输入；S4、建立包含6个状态向量与2个观测向量的系统卡尔曼滤波状态方程和测量方程，并借助三轴加速度计空间几何模型自适应调整过程误差协方差矩阵Q，精准估算姿态倾角信息。