一种适用于农业机械精准作业的姿态倾角估计方法技术

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

A method of attitude dip estimation for precision operation of agricultural machinery

The present invention discloses a kind of attitude angle for agricultural machinery precision estimation method, including MEMS gyroscope and accelerometer inherent bias estimation and combination of the two order AR gyroscope (Auto Regression) adaptive Calman filtering attitude error model fusion algorithm. The AR model is used to estimate the measurement errors err1, phi, err2, theta, err1, theta err2 of X axis gyroscope and Y axis gyroscope, as the state input of Calman filter, and adaptively estimate the measurement error covariance matrix R. The establishment of extended Calman filter state equation and measurement equation of external system contains 6 state vector and 2 observation vectors, with the aid of three axis accelerometer geometry model adaptive adjustment process error covariance matrix Q. The method considering the inherent gyroscope and accelerometer bias, can avoid the introduction of system error attitude estimation; two order AR error model can avoid the random error random impact on gyro attitude estimation; adaptive Calman filtering system can guarantee the precision estimation equation in different state attitude.

【技术实现步骤摘要】
一种适用于农业机械精准作业的姿态倾角估计方法
本专利技术属于惯性导航
，特别涉及一种适用于农业机械精准作业的姿态倾角估计方法。
技术介绍
惯性导航技术是20世纪中期发展起来的完全自主式导航技术。通过惯性测量组件测量载体相对惯性空间的角速率与加速度信息，利用牛顿运动定律自动推算载体瞬时速度与位置信息，具有不依赖外界信息、不向外界辐射能量、不受干扰、隐蔽性好的特点。能连续地提供载体的全部导航、制导参数(位置、线速度、角速度、姿态角)，广泛运用于航天、航空、航海领域，特别是军事领域。而姿态倾角的准确估量是实现运动物体控制的关键参数。姿态倾角的准确估量对农业机械精准作业也尤为重要。譬如农业机械导航作业时，由于田面的不平整，单纯依赖GNSS定位并不能保证拖拉机直线作业，只有借助车身实时姿态信息进行投影校正，方能保证拖拉机按照规划路径准确行走作业，具体说明书附图1与图2所示。又如平地机与喷雾机作业时，作业质量的保证取决于平地铲与喷杠的水平，这要求农机具姿态检测信息的实时反馈，以保证水平作业。准确的姿态倾角检测更是农业机械与农机具动力学建模、作业安全预警的研究的基础。农业机械作业姿态倾角检测本文档来自技高网...

【技术保护点】
一种适用于农业机械精准作业的姿态倾角估计方法，其特征在于，包括下述步骤：S1、对陀螺仪与加速度计固有零偏估算，准静态情况下在MEMS传感器预热完成后进行统计平均估算，将传感器原始输出减去得到的固有零偏bax、bay、baz、bgx、bgy、bgz作为后续计算的输入；S2、采用二阶AR模型估计X轴陀螺仪与Y轴陀螺仪测量误差φerr1、φerr2、θerr1、θerr2并作为卡尔曼滤波的状态输入量，并自适应估算测量误差协方差矩阵R；S3、利用三轴加速度计的三维空间几何模型估计姿态横滚角φ、俯仰角θ的初始值，并作为卡尔曼滤波器的状态输入；S4、建立包含6个状态向量与2个观测向量的系统卡尔曼滤波状态方...

【技术特征摘要】
1.一种适用于农业机械精准作业的姿态倾角估计方法，其特征在于，包括下述步骤：S1、对陀螺仪与加速度计固有零偏估算，准静态情况下在MEMS传感器预热完成后进行统计平均估算，将传感器原始输出减去得到的固有零偏bax、bay、baz、bgx、bgy、bgz作为后续计算的输入；S2、采用二阶AR模型估计X轴陀螺仪与Y轴陀螺仪测量误差φerr1、φerr2、θerr1、θerr2并作为卡尔曼滤波的状态输入量，并自适应估算测量误差协方差矩阵R；S3、利用三轴加速度计的三维空间几何模型估计姿态横滚角φ、俯仰角θ的初始值，并作为卡尔曼滤波器的状态输入；S4、建立包含6个状态向量与2个观测向量的系统卡尔曼滤波状态方程和测量方程，并借助三轴加速度计空间几何模型自适应调整过程误差协方差矩阵Q，精准估算姿态倾角信息。2.根据权利要求1所述适用于农业机械精准作业的姿态倾角估计方法，其特征在于，步骤S1之前，还包括硬件平台的搭建步骤，所述硬件平台是基于ADIS16445惯性传感器与STM32F446核心处理器搭建而成，具有稳定可靠3轴加速度计、3轴陀螺仪与温度信息输出，输出频率为100Hz。3.根据权利要求1所述适用于农业机械精准作业的姿态倾角估计方法，其特征在于，ADIS16445惯性传感器在上电后至农业机械开始作业前的准静态过程中，监控传感器输出温度，预热完成后便开始存储传感器原始数据，利用统计平均方法估算出三轴陀螺仪和三轴加速度计的固有偏零。4.根据权利要求1所述适用于农业机械精准作业的姿态倾角估计方法，其特征在于，步骤S2中还包括下述步骤：搭建X轴与Y轴陀螺仪的时间序列自回归滑动平均误差模型:其中n与m为陀螺仪误差模型阶数，Ai与Bj为陀螺仪误差模型系数，ε为符合正态分布的随机白噪声残差，k为本次计算的时序；陀螺仪误差模型根据AIC(AkaikeInformationCriterion)与BIC(BayesianInformationCrite...

【专利技术属性】
技术研发人员：张智刚，黄培奎，罗锡文，刘兆鹏，王辉，张健，高维炜，林志健，
申请(专利权)人：华南农业大学，
类型：发明
国别省市：广东,44