一种基于加速度的卡尔曼滤波姿态估计方法及系统的制作方法
【技术领域】
[0001] 本发明涉及姿态测量技术领域，尤其涉及一种卡尔曼滤波姿态估计方法及卡尔曼 滤波姿态估计系统。
【背景技术】
[0002] 在基于MEMS (Micro-Electro-Mechanical System，微机电系统）惯性传感器的姿 态测量系统中，对扰动加速度的感知和处理是决定系统性能的关键。目前常用的方法有两 种：
[0003] 1)通过卡尔曼滤波新息进行判据；
[0004] Vk= Z k-HkXk/k 1
[0005] 所谓卡尔曼滤波新息是指量测值与状态预测值的偏差，当卡尔曼滤波器工作正常 且量测新息正确反应重力场矢量时，新息的模值为一小量，因而可设定适当的门限值对载 体的运动加速度情况进行判定。
[0006] 2)基于三轴加速度计输出的判别机制；当载体无运动加速度时，加速度计的输出 满足
[0007] f = (2 ω ie+ ω en) X v~g
[0008] 其中，（2 ω ie+ ω J X v在低速应用中的基本可以忽略，因而基于加速度计的判据 可表示为：
,则认为此时无运动加速度； 则认为此时存在运动加速度；
[0011] 其中，f"iZ为三轴加速度计的输出，δ为根据加速度计噪声特性确定的一个很小 的值。这样，通过以上加速度判别机制，进而调整扰动加速度存在时滤波器的量测噪声矩 阵，亦或断开卡尔曼滤波的量测修正回路。
[0012] 第一种方法的判别机制是较为严格的，但它不能区分新息超阈是由于扰动加速度 的影响还是陀螺漂移误差造成的。这种情况在低成本MEMS陀螺中尤为严重。实际应用中由 于陀螺漂移误差，在载体机动时间较长、温度变化较大或者初始零偏扣除不准确的情况下， 新息将会迅速超出阈值并失效。因而基于此方法的卡尔曼滤波算法鲁棒性较差，系统对温 度、机动时间等因素都较为敏感。
[0013] 第二种方法中需要根据加速度计噪声特性设定合适的门限值，虽然不像第一种方 法那样受诸多因素影响，但其仅为载体无运动加速度的必要条件。在实际应用过程中，存在 诸多漏判的情况；以二维水平情况举例，当扰动运动加速度矢量a和重力矢量g的合成矢量 模值和重力加速度g相等时即为漏判的情况；而当发生漏判时，系统的姿态将会被错误的 量测信息迅速修坏，造成较大的姿态误差。
[0014] 以上两个方法当扰动加速度进入卡尔曼滤波新息时，只能增大量测矩阵亦或屏蔽 量测修正回路，而增大量测矩阵并不能抵消长时间机动对姿态造成的错误修正。
【发明内容】
[0015] 针对上述问题，本发明旨在提供基于加速度的卡尔曼滤波姿态估计方法及卡尔曼 滤波姿态估计系统，其将载体运动加速度引入卡尔曼姿态估计和修正中，提高了卡尔曼滤 波姿态估计的精确度。
[0016] 本发明提供的技术方案如下：
[0017] -种基于加速度的卡尔曼滤波姿态估计方法，所述卡尔曼滤波姿态估计方法具体 包括以下步骤：
[0018] Sl 建立MEMS(Micr〇-Electro_Mechanical System，微机电系统）陀螺输出又以模 型：
[0019] yG t= ω t+bt+vG_t
[0020] 其中，ω t表示t时刻载体的真实转动角速率；b t表示t时刻的偏置项；v s,t表示t 时刻的第一高斯白噪声；G表示MEMS陀螺输出模型；
[0021 ] S2建立MEMS加速度计输出yA, t模块：
[0022] yA,t= bat-VvAit
[0023] 其中，bat表示载体坐标系下t时刻的运动加速度； bg表示载体坐标系下重力场向 量；vAit表示t时刻的第二高斯白噪声；A表示MEMS加速度计输出模型；
[0024] S3使用AR过程建立地理坐标系下的运动加速度nat模型：
[0025] nBt= c a · nat !+vAjt
[0026] 其中，Ca表示衰减系数，且取值范围为0~I ; nat i表示t_l时刻的运动加速度； vAit表不t时刻的第二尚斯白噪声；
[0027] S4搭建卡尔曼滤波姿态估计系统进行数据融合，得到卡尔曼滤波姿态估计系统的 状态方程义:
[0029] 其中，系统误差状态向量
i表不系统 误差状态向量X(t)的导数，表示东向欧拉失准角，仍V表示北向欧拉失准角，表示天 向欧拉失准角，bx表示X轴陀螺零偏值，b y表示y轴陀螺零偏值，b z表示z轴陀螺零偏值； 系统状态转移矩阵
表示载体坐标系到地理坐标系的变换坐标矩 阵；系统零均值白噪声W(t) = [wgx Wgy Wgz 0 0 0]τ，且W~N(0，Q)，Q为系统噪声矩阵，wgx、 Wgy、wgz分别为陀螺中X、y、z三个轴上的噪声；
[0030] S5测量卡尔曼滤波姿态估计系统得到量测方程Z (t);
[0031] Z(t) = H(t)X(t)+V(t)
[0032] 其中，测量向量
δ gE表示地理坐标系中东向重力场矢量与测量东 向重力场矢量的差值，S gN表示地理坐标系中北向重力场矢量与测量北向重力场矢量的差 值；测量矩阵
测量零均值白噪声V (t)~N(0, R)，R为测量噪 声矩阵；
[0033] S6离散化所述状态方程X (t)和所述量测方程Z (t);
[0036] 其中，k-l、k分别表示t = k-Ι时刻和t = k时刻；Fk/k t = k时刻到t = k-1 时刻状态一步转移矩阵;Wk i表示t = k-1时刻零均值白噪声；Ht = k时刻的测量矩阵； Vk表示t = k时刻的零均值白噪声；
[0037] S7分别对状态一步转移矩阵Fk/k JP系统噪声矩阵Q(t k)进行二阶近似：
[0039] 其中，Fk/k i表示t = k时刻到t = k-Ι时刻状态一步转移矩阵；I表示单位矩阵； 滤波周期T = tk_tk 1;F(t k J表示t = k-Ι时刻的系统状态转移矩阵；
[0041] 其中，Q(tk i)表示t = k-ι时刻的系统噪声矩阵，F(tk i)表示t = k-ι时刻的系 统状态转移矩阵；
[0042] S8循环迭代入卡尔曼滤波公式实现载体姿态的修正。
[0043] 在本技术方案中，偏置项bt即为MEMS陀螺零偏噪声。
[0044] 优选地，在步骤Sl中，所述偏置项1^使用一个受白噪声驱动的一阶马尔科夫过程 表不：
[0045] bt=btl+wtt
[0046] 其中，1^表不t时刻的偏置项，b t i表不t_l时刻的偏置项，w tit表不t时刻的第三 尚斯白噪声。
[0047] 优选地，在步骤S2中，还包括以下步骤：
[0048] 对MEMS加速度计输出yA,t采用滑动平均的方式进行低通滤波。
[0049] 优选地，在步骤S5中，得到系统测量方程Z(t)具体包括以下步骤：
[0050] 在地理坐标系下重力场矢量ng为：
[0051] ng = [0 0 -1]τ
[0052] 由步骤S2得到，载体坐标系下重力场矢量bg为：
[0053] bg = bat-yA_ t
[0054] 得到地理坐标系下测量重力场矢量ng为：
[0056] 其中，CT为陀螺积分姿态环节解算出的方向余弦矩阵，具体为：
[0059] 其中，三轴欧拉失准角的反对称矩阵
[0060] 故，通过地理坐标系下重力场矢量ng与地理坐标系下测量重力场矢量ng之间的 差值得到系统的测量方程：
[0062] 其中，δ gE表示地理坐标系中东向重力场矢量与测量东向重力场矢量的差值，δ gN 表示地理坐标系中北向重力场矢量与测量北