[发明专利]一种基于加速度的卡尔曼滤波姿态估计方法及系统

说明书

技术领域

本发明涉及姿态测量技术领域，尤其涉及一种卡尔曼滤波姿态估计方法及卡尔曼滤波姿态估计系统。

背景技术

在基于MEMS(Micro-Electro-MechanicalSystem，微机电系统)惯性传感器的姿态测量系统中，对扰动加速度的感知和处理是决定系统性能的关键。目前常用的方法有两种：

1)通过卡尔曼滤波新息进行判据；

v_k＝Z_k-H_kX_k/k-1

所谓卡尔曼滤波新息是指量测值与状态预测值的偏差，当卡尔曼滤波器工作正常且量测新息正确反应重力场矢量时，新息的模值为一小量，因而可设定适当的门限值对载体的运动加速度情况进行判定。

2)基于三轴加速度计输出的判别机制；当载体无运动加速度时，加速度计的输出满足

f＝(2ω_ie+ω_en)×v-g

其中，(2ω_ie+ω_en)×v在低速应用中的基本可以忽略，因而基于加速度计的判据可表示为：

当时，则认为此时无运动加速度；

当时，则认为此时存在运动加速度；

其中，f_x,y,z为三轴加速度计的输出，δ为根据加速度计噪声特性确定的一个很小的值。这样，通过以上加速度判别机制，进而调整扰动加速度存在时滤波器的量测噪声矩阵，亦或断开卡尔曼滤波的量测修正回路。

第一种方法的判别机制是较为严格的，但它不能区分新息超阈是由于扰动加速度的影响还是陀螺漂移误差造成的。这种情况在低成本MEMS陀螺中尤为严重。实际应用中由于陀螺漂移误差，在载体机动时间较长、温度变化较大或者初始零偏扣除不准确的情况下，新息将会迅速超出阈值并失效。因而基于此方法的卡尔曼滤波算法鲁棒性较差，系统对温度、机动时间等因素都较为敏感。

第二种方法中需要根据加速度计噪声特性设定合适的门限值，虽然不像第一种方法那样受诸多因素影响，但其仅为载体无运动加速度的必要条件。在实际应用过程中，存在诸多漏判的情况；以二维水平情况举例，当扰动运动加速度矢量a和重力矢量g的合成矢量模值和重力加速度g相等时即为漏判的情况；而当发生漏判时，系统的姿态将会被错误的量测信息迅速修坏，造成较大的姿态误差。

以上两个方法当扰动加速度进入卡尔曼滤波新息时，只能增大量测矩阵亦或屏蔽量测修正回路，而增大量测矩阵并不能抵消长时间机动对姿态造成的错误修正。

发明内容

针对上述问题，本发明旨在提供基于加速度的卡尔曼滤波姿态估计方法及卡尔曼滤波姿态估计系统，其将载体运动加速度引入卡尔曼姿态估计和修正中，提高了卡尔曼滤波姿态估计的精确度。

本发明提供的技术方案如下：

一种基于加速度的卡尔曼滤波姿态估计方法，所述卡尔曼滤波姿态估计方法具体包括以下步骤：

S1建立MEMS(Micro-Electro-MechanicalSystem，微机电系统)陀螺输出y_G,t模型：

y_G,t＝ω_t+b_t+v_G,t

其中，ω_t表示t时刻载体的真实转动角速率；b_t表示t时刻的偏置项；v_G,t表示t时刻的第一高斯白噪声；G表示MEMS陀螺输出模型；

S2建立MEMS加速度计输出y_A,t模块：

y_A,t＝^ba_t-^bg+v_A,t

其中，^ba_t表示载体坐标系下t时刻的运动加速度；^bg表示载体坐标系下重力场向量；v_A,t表示t时刻的第二高斯白噪声；A表示MEMS加速度计输出模型；

S3使用AR过程建立地理坐标系下的运动加速度ⁿa_t模型：

ⁿa_t＝c_a·ⁿa_t-1+v_A,t