CN107014376B - 一种适用于农业机械精准作业的姿态倾角估计方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于惯性导航技术领域，特别涉及一种适用于农业机械精准作业的姿态倾角估计方法。

背景技术

惯性导航技术是20世纪中期发展起来的完全自主式导航技术。通过惯性测量组件测量载体相对惯性空间的角速率与加速度信息，利用牛顿运动定律自动推算载体瞬时速度与位置信息，具有不依赖外界信息、不向外界辐射能量、不受干扰、隐蔽性好的特点。能连续地提供载体的全部导航、制导参数(位置、线速度、角速度、姿态角)，广泛运用于航天、航空、航海领域，特别是军事领域。而姿态倾角的准确估量是实现运动物体控制的关键参数。

姿态倾角的准确估量对农业机械精准作业也尤为重要。譬如农业机械导航作业时，由于田面的不平整，单纯依赖GNSS定位并不能保证拖拉机直线作业，只有借助车身实时姿态信息进行投影校正，方能保证拖拉机按照规划路径准确行走作业，具体说明书附图1与图2所示。又如平地机与喷雾机作业时，作业质量的保证取决于平地铲与喷杠的水平，这要求农机具姿态检测信息的实时反馈，以保证水平作业。准确的姿态倾角检测更是农业机械与农机具动力学建模、作业安全预警的研究的基础。

农业机械作业姿态倾角检测多采用惯性传感器、卫星导航系统和图像处理等方法获取，还有一些采用多种传感器融合技术进行姿态监测。其中惯性传感器一般采用加速度计和陀螺仪、倾角传感器、超声波传感器等对姿态进行监测。卫星导航系统在农业机械中的姿态倾角获取方法主要是通过GPS或北斗等卫星导航系统结合其他惯性传感器进行融合测量。机器视觉的方法针对不同田间作物的特点，采取的处理算法也不同。常见的用于姿态倾角获取的田间特征有作物行线、垄线、田间道路等。传感器融合技术主要是将多个传感器结合起来对姿态倾角进行监测。利用MEMS传感器实对农业机械作业姿态倾角测量是目前应用最广泛的方法，其经济性、稳定性和适应性指标相比其他测量方法都具有突出优势。

然而目前农业机械姿态倾角检测倾向于直接将广泛应用于军工、航天航空、船舶、汽车、机器人等各个领域的高精度姿态传感器运用过来。这既增加了农业机械作业成本，同时由于缺乏对农业机械本身特殊结构以及农田复杂作业环境的考虑，效果并不理想，产生许多实际应用问题。

姿态倾角更新解算是惯性导航系统的关键算法，传统的姿态倾角更新算法有欧拉角法、方向余弦和四元素法。四元素法计算时需要进行泰勒展开，通常采用忽略其高阶项将非线性转化成线性进行姿态估算，存在误差。方向余弦总共有9个参数，计算量大，不适宜工程应用。欧拉角法是直接迭代欧拉角微分方程，计算最为简便直接，适用于地面作业农业机械(不存在俯仰角奇点情况)。MEMS陀螺仪存在固有零偏，长时间积分运算存在累积误差，不适合单独使用。加速度计可以解算准静态峡物体欧拉姿态角，无积分运算不存在累积误差问题，但其解算的姿态角动态响应差，无法在动态条件下应用。目前国内外已经开发多种将两类器件相结合的融合算法，例如互补滤波、卡尔曼滤波、梯度算法、模糊算法等，其中数卡尔曼滤波算法应用最为广泛。

但目前实际运用时缺乏考虑传感器本身特性，如陀螺仪与加速度计固有零偏、陀螺仪的随机扰动等。同时还缺乏对不同工况下过程误差协方差矩阵Q与测量误差协方差矩阵R的研究，适应性有限，特别是对于作业工况与环境复杂的农业机械。

发明内容

针对农业机械作业姿态倾角测量的需求以及惯性传感器测量技术存在的问题，本发明提供了一种适用于农业机械精准作业的姿态倾角估计方法。其目的在与通过传感器模型与算法模型的构建，提供一种适用于农业机械精准作业需求的姿态倾角估计方法，并充分考虑其在农业机械作业应用的实际问题。

为了达到上述目的，本发明采用以下技术方案：

本发明的一种适用于农业机械精准作业的姿态倾角估计方法，包括下述步骤：

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，精准估算姿态倾角信息。

作为优选的技术方案，步骤S1之前，还包括硬件平台的搭建步骤，所述硬件平台是基于ADIS16445惯性传感器与STM32F446核心处理器搭建而成，具有稳定可靠3轴加速度计、3轴陀螺仪与温度信息输出，输出频率为100Hz。

作为优选的技术方案，ADIS16445惯性传感器在上电后至农业机械开始作业前的准静态过程中，监控传感器输出温度，预热完成后便开始存储传感器原始数据，利用统计平均方法估算出三轴陀螺仪和三轴加速度计的固有偏零。

作为优选的技术方案，步骤S2中还包括下述步骤：

搭建X轴与Y轴陀螺仪的时间序列自回归滑动平均误差模型:

其中n与m为陀螺仪误差模型阶数，A_i与B_j为陀螺仪误差模型系数，ε为符合正态分布的随机白噪声残差，k为本次计算的时序；

陀螺仪误差模型根据AIC、BIC法则定型后可解算出φ_err1、φ_err2、θ_err1、θ_err24个状态向量。

作为优选的技术方案，在步骤S3中，

初始姿态传感器为准静态，利用三轴加速度计的空间几何模型估算姿态倾角原始量：

横滚角初始值

俯仰角初始值

其中a_x、a_y、a_z分别为三轴加速度计的测量值，由此可估算2个初始状态向量。

作为优选的技术方案，步骤S4具体为：

建立姿态参考系统的外部拓展卡尔曼滤波状态方程与测量方程：

其中x_k和z_k分别表示姿态参考系统的状态向量与观测向量，u_k-1为系统输入，ω_k-1和υ_k分别为状态方程的过程误差与测量方程的测量噪声，k表示本次计算的时序其中：

系统的状态向量为

系统方程为

系统状态矩阵为

系统输入矩阵为

系统观测向量为

系统输出矩阵

作为优选的技术方案，状态方程的过程误差ω_k-1与观测方程的测量误差υ_k均假设为零均值且符合正态分布的高斯白噪声，对应着系统的过程误差协方差矩阵Q_k和测量误差协方差矩阵R_k，其中：

ω_k～N(0,Q_k)，υ_k～N(0,R_k)且cov[ω_k,υ_k]＝0。

作为优选的技术方案，测量误差协方差矩阵R_k由X轴陀螺仪与Y轴陀螺仪的2阶AR误差模型决定，过程误差协方差矩阵Q_k由三轴加速度计的空间几何模型决定，其中：

E表示陀螺仪测量误差残差计算；

表示系统过程误差协方差计算。

作为优选的技术方案，过程误差协方差由三轴加速度计的空间几何模型决定，其中：

与为系统过程误差协方差的最大值与最小值，对应着不同的运用工况；T为置信系数，取值范围为0到1；α为系统过程误差协方差系数，有三轴加速度计空间几何模型决定；

过程误差协方差系数α为：

g为当地重力加速度；

当处于准静态时，有

本发明与现有技术相比，具有如下优点和有益效果：

1、本发明采用MEMS惯性传感器，集成度高，性能稳定，成本低；

2、本发明考虑了陀螺仪与加速度计固有零偏的影响，采用统计平均方法进行估算，减小原始传感器输出误差；

3、本发明考虑了陀螺仪随机误差的影响，建立陀螺仪2阶AR误差模型，提高陀螺仪测量的准确性；

4、本发明考虑了实际不同工况下过程误差与测量误差的变化，基于加速度计与陀螺仪模型自适应协方差矩阵Q与R，提高系统适用性；

5、本发明搭建系统的具有6个状态向量和2个观测向量的外部拓展卡尔曼滤波模型进行姿态角融合，提高姿态解算精度；

综上所述，本发明提供一种适用于农业机械精准作业的姿态倾角估计方法，采用MEMS惯性传感器；考虑陀螺仪与加速度计固有零偏的影响，准静态时利用统计平均算法进行估算滤除；建立陀螺仪的2阶AR误差模型估算陀螺仪的随机零偏，并结合三轴加速度计空间几何模型自适应调整系统协方差矩阵Q和R；搭建具有6个状态向量与2个观测向量的外部拓展卡尔曼滤波模型进行姿态倾角融合，保证了农业机械精准作业的姿态倾角测量精度与稳定性。

附图说明

图1为地形起伏时横滚倾角对农业机械田间精准作业影响的示意图；

图2为地形起伏时俯仰倾角对农业机械田间精准作业影响的示意图；

图3为本发明采用的技术路线图；

图4姿态倾角测量系统坐标系示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实例，对本发明进行进一步详细说明。此处所描述的具体实例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以互相结合。

如图3所示，本实施例适用于农业机械精准作业的姿态倾角估计方法：包括对陀螺仪与加速度计固有零偏估算和结合陀螺仪AR误差模型的自适应卡尔曼滤波姿态融合算法。其中陀螺仪与加速度计固有零偏估计算法是准静态情况下在MEMS(Micro-electromechanical Systems)传感器预热完成后进行统计平均估算，将传感器原始输出减去得到的固有零偏b_ax、b_ay、b_az、b_gx、b_gy、b_gz作为后续计算的输入。采用二阶AR(AutoRegression)模型估计X轴陀螺仪与Y轴陀螺仪测量误差φ_err1、φ_err2、θ_err1、θ_err2并作为卡尔曼滤波的状态输入量并自适应估算测量误差协方差矩阵R。利用三轴加速度计的三维空间几何模型估计姿态横滚角φ、俯仰角θ的初始值，并作为卡尔曼滤波器的状态输入。建立包含6个状态向量与2个观测向量的系统卡尔曼滤波状态方程和测量方程，并借助三轴加速度计空间几何模型自适应调整过程误差协方差矩阵Q。本发明方法考虑陀螺仪与加速度计的固有零偏，可避免姿态倾角估算时引入系统误差；建立陀螺仪的二阶AR误差模型可避免随机陀螺仪随机误差对姿态估计造成影响；而建立基于Q、R协方差矩阵的自适应卡尔曼滤波系统方程可以保证在不同状态下的姿态估计精度。

首先是硬件平台的搭建，本发明的硬件平台是基于ADIS16445惯性传感器与STM32F446核心处理器搭建而成，具有稳定可靠3轴加速度计、3轴陀螺仪与温度信息输出，输出频率为100Hz。由于本发明重点在于姿态倾角融合算法介绍，故详细的硬件平台信息不在此展开介绍。

建立姿态倾角测量系统坐标系，如图4所示：绕坐标系X轴转动为横滚角φ，绕坐标系Y轴转动为俯仰角θ。φ和θ均属于欧拉转动角，基于欧拉角的物理特性，再平台式惯性导航系统中，欧拉角可以通过坐标系的3个框架的角传感器测量。

姿态倾角测量对应的框架角速率分别为和由欧拉角随时间的传递关系可建立欧拉角形式的陀螺仪运动方程：

并由此推导出姿态测量模型

陀螺仪与加速度计固有零偏估计：硬件系统一上电程序便开始运行，ADIS16445以100Hz输出传感器原始数据，从预热到农业机械开始作业前姿态倾角测量系统处于准静态。

程序初始化后开始监控ADIS16445输出的温度信息，ADIS16445的温度测量精度为0.07386摄氏度。程序设立长度为500的堆栈存储温度信息，实时更新比较堆栈中温度最大值与最小值的差值。若差值小于0.3，由经验可判断姿态倾角测量模块已达到热平衡(即工作温度)。

此时程序开始打开陀螺仪与加速度计存储堆栈，长度500。由统计平均算法可估算出，三轴加速度计于三轴陀螺仪在准静态的固有零偏b_ax、b_ay、b_az、b_gx、b_gy、b_gz。

搭建X轴与Y轴陀螺仪的时间序列自回归滑动平均误差模型:其中n与m为陀螺仪误差模型阶数，A_i与B_j为陀螺仪误差模型系数，ε为符合正态分布的随机白噪声残差，k为本次计算的时序。根据记录的准静态时X轴与Y轴陀螺仪数据,离线多次基于MATLAB进行陀螺仪的时间序列自回归滑动平均误差模型判定,运用最通用的AIC(Akaike Information Criterion)与BIC(BayesianInformation Criterion)准则判定，结果为2阶AR模型最为适宜。故实际工作时可在程序初始化后实时解算得到φ_err1、φ_err2、θ_err1、θ_err24个状态向量，其模型为：

准静态情况下，利用三轴加速度计的空间几何模型估算姿态倾角原始量：其中横滚角初始值俯仰角初始值a_x、a_y、a_z分别为三轴加速度计的测量值。实际工作时可在陀螺仪与加速度计固有零偏估算完成后实时解算得到φ、θ2个初始状态向量；

建立姿态倾角参考系统的外部拓展卡尔曼滤波状态方程与测量方程：其中x_k和z_k分别表示姿态倾角参考系统的状态向量与观测向量，u_k-1为系统输入，ω_k-1和υ_k分别为状态方程的过程误差与测量方程的测量噪声，k表示本次计算的时序，A、B、H分别为系统状态矩阵、输入矩阵与输出矩阵，其中：

系统的状态向量为

系统方程为

系统状态矩阵为

系统输入矩阵为

系统观测向量为

系统输出矩阵

系统状态方程的过程误差ω_k-1与观测方程的测量误差υ_k均假设为零均值且符合正态分布的高斯白噪声，对应着系统的过程误差协方差矩阵Q_k和测量误差协方差矩阵R_k，其中：

ω_k～N(0,Q_k)，υ_k～N(0,R_k)且cov[ω_k,υ_k]＝0；

过程误差协方差矩阵Q_k和测量误差协方差矩阵R_k自适应：测量误差协方差矩阵R_k由X轴陀螺仪与Y轴陀螺仪的2阶AR误差模型决定，过程误差协方差矩阵Q_k由三轴加速度计的空间几何模型决定，其中：

E表示陀螺仪测残差计算，为不同工况下自适应实时更新；

其中表示系统过程误差协方差计算；

系统过程误差协方差由三轴加速度计的空间几何模型决定，其中：

与为系统过程误差协方差的最大值与最小值，本发明中的经验值为0.00038与0.000112；T为置信系数，取值范围为0到1，本发明中的经验值为0.9；α为系统过程误差协方差系数，有三轴加速度计空间几何模型决定；

本发明中系统过程误差协方差系数α为：g为当地重力加速度；

当处于准静态时有

不同工况下Q自适应实时更新。

可以看出本发明充分考虑了MEMS惯性陀螺仪与加速计特性，首先对准静态下固有零偏进行估算剔除，又建立了陀螺仪的2阶AR模型排除了随机扰动的影响。建立自适应外部卡尔曼滤波模型，提高了不同工况下的解算精度与系统测量精度。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (9)

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，精准估算姿态倾角信息。

2.根据权利要求1所述适用于农业机械精准作业的姿态倾角估计方法，其特征在于，步骤S1之前，还包括硬件平台的搭建步骤，所述硬件平台是基于ADIS16445惯性传感器与STM32F446核心处理器搭建而成，具有稳定可靠三轴加速度计、三轴陀螺仪与温度信息输出，输出频率为100Hz。

3.根据权利要求2所述适用于农业机械精准作业的姿态倾角估计方法，其特征在于，ADIS16445惯性传感器在上电后至农业机械开始作业前的准静态过程中，监控传感器输出温度，预热完成后便开始存储传感器原始数据，利用统计平均方法估算出三轴陀螺仪和三轴加速度计的固有偏零。

4.根据权利要求1所述适用于农业机械精准作业的姿态倾角估计方法，其特征在于，步骤S2中还包括下述步骤：

搭建X轴与Y轴陀螺仪的时间序列自回归滑动平均误差模型:

其中n与m为陀螺仪误差模型阶数，A_i与B_j为陀螺仪误差模型系数，ε为符合正态分布的随机白噪声残差，k为本次计算的时序；

陀螺仪误差模型根据AIC(Akaike Information Criterion)与BIC(BayesianInformation Criterion)法则定型后可解算出φ_err1、φ_err2、θ_err1、θ_err24个状态向量。

5.根据权利要求1所述适用于农业机械精准作业的姿态倾角估计方法，其特征在于，在步骤S3中，

初始姿态传感器为准静态，利用三轴加速度计的空间几何模型估算姿态原始量：

横滚角初始值

俯仰角初始值

其中a_x、a_y、a_z分别为三轴加速度计的测量值，由此可估算2个初始状态向量。

6.根据权利要求1所述适用于农业机械精准作业的姿态倾角估计方法，其特征在于，步骤S4具体为：

建立姿态参考系统的外部拓展卡尔曼滤波状态方程与测量方程：

其中x_k和z_k分别表示姿态参考系统的状态向量与观测向量，u_k-1为系统输入，ω_k-1和υ_k分别为状态方程的过程误差与测量方程的测量误差，k表示本次计算的时序，H为输出矩阵，其中：

系统的状态向量为

系统方程为

系统状态矩阵为

系统输入矩阵为

系统观测向量为

系统输出矩阵

7.根据权利要求6所述适用于农业机械精准作业的姿态倾角估计方法，其特征在于，状态方程的过程误差ω_k-1与测量方程的测量误差υ_k均假设为零均值且符合正态分布的高斯白噪声，对应着系统的过程误差协方差矩阵Q_k和测量误差协方差矩阵R_k，其中：

ω_k～N(0,Q_k)，υ_k～N(0,R_k)且cov[ω_k,υ_k]＝0。

8.根据权利要求6所述适用于农业机械精准作业的姿态倾角估计方法，其特征在于，测量误差协方差矩阵R_k由X轴陀螺仪与Y轴陀螺仪的2阶AR误差模型决定，过程误差协方差矩阵Q_k由三轴加速度计的空间几何模型决定，其中：

E表示陀螺仪测量误差残差计算；

表示系统过程误差协方差。

9.根据权利要求8所述适用于农业机械精准作业的姿态倾角估计方法，其特征在于，过程误差协方差由三轴加速度计的空间几何模型决定，其中：

与为系统过程误差协方差的最大值与最小值，对应着不同的运用工况；T为置信系数，取值范围为0到1；α为系统过程误差协方差系数，有三轴加速度计空间几何模型决定；

过程误差协方差系数α为：

g为当地重力加速度；

当处于准静态时，有