[发明专利]一种两轴姿态倾角测量方法

说明书

本发明公开了一种两轴姿态倾角测量方法，包括下述步骤：S1、基于三轴加速度计空间几何模型模型，准静态情况下对横滚角与俯仰角初始值进行准确估算，并作为卡尔曼滤波器的状态输入量；S2、将两轴倾角零偏bias_roll、bias_pitch也作为卡尔曼滤波的状态输入量；S3、将X轴陀螺仪与Y轴陀螺仪测量值Gyro_x、Gyro_y作为卡尔曼滤波的输入向量；S4、建立包含4个状态向量与2个观测向量的系统卡尔曼滤波状态方程和测量方程，精准测量两轴姿态倾角。本发明方法考虑两轴倾角零偏，利用卡尔曼滤波融合方法自动修正，保证两轴倾角的高精度测量。

技术领域

本发明属于惯性导航技术领域，特别涉及一种两轴姿态倾角测量方法。

背景技术

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

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

目前姿态倾角检测多采用惯性传感器、图像处理等方法获取，还有一些采用多种传感器融合技术进行姿态监测。其中惯性传感器一般采用加速度计和陀螺仪、倾角传感器、超声波传感器等对姿态进行监测。传感器融合技术主要是将多个传感器结合起来对姿态倾角进行监测。利用MEMS传感器实对姿态倾角测量是目前应用最广泛的方法，其经济性、稳定性和适应性指标相比其他测量方法都具有突出优势。

目前广泛应用于军工、航天航空、船舶、汽车、机器人等各个领域的高精度姿态传感器价格昂贵，适用性有限。低成本的姿态倾角传感器一般采用简单的互补滤波等简单算法进行融合，测量精度有限。且大多缺乏考虑姿态倾角零偏问题，产生许多实际应用问题。

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

发明内容

本发明的主要目的在于克服两轴姿态倾角测量的需求以及惯性传感器测量技术存在的问题，提供一种两轴姿态倾角测量方法，通过传感器模型与算法模型的构建，提供一种两轴姿态倾角精准测量方法，并考虑其应用的实际问题。

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

本发明一种两轴姿态倾角测量方法，包括下述步骤：

S1、基于三轴加速度计空间几何模型模型，准静态情况下对横滚角与俯仰角初始值进行准确估算，并作为卡尔曼滤波器的状态输入量；

S2、将两轴倾角零偏bias_roll、bias_pitch也作为卡尔曼滤波的状态输入量；