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

说明书

技术领域

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

背景技术

惯性导航技术是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；