一种基于加速度的卡尔曼滤波姿态估计方法及系统技术方案

本发明专利技术提供了一种基于加速度的卡尔曼滤波姿态估计方法及系统，其中，该卡尔曼滤波姿态估计方法中包括以下步骤：S1建立MEMS陀螺输出yG,t模型；S2建立MEMS加速度计输出yA,t模块；S3使用AR过程建立地理坐标系下的运动加速度nat模型；S4搭建卡尔曼滤波姿态估计系统进行数据融合，得到卡尔曼滤波姿态估计系统的状态方程S5测量卡尔曼滤波姿态估计系统得到量测方程Z(t)；S6离散化所述状态方程X(t)和所述量测方程Z(t)；S7分别对状态一步转移矩阵Fk/k-1和系统噪声矩阵Q(tk)进行二阶近似；统状态转移矩阵；S8循环迭代入卡尔曼滤波公式实现载体姿态的修正。其将载体运动加速度引入卡尔曼姿态估计和修正中，大大提高了本发明专利技术中卡尔曼姿态估计方法的精确度。

【技术实现步骤摘要】

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

【技术保护点】
一种基于加速度的卡尔曼滤波姿态估计方法，其特征在于，所述卡尔曼滤波姿态估计方法具体包括以下步骤：S1 建立MEMS陀螺输出yG,t模型：yG,t＝ωt+bt+vG,t其中，ωt表示t时刻载体的真实转动角速率；bt表示t时刻的偏置项；vG,t表示t时刻的第一高斯白噪声；G表示MEMS陀螺输出模型；S2 建立MEMS加速度计输出yA,t模块：yA,t＝bat‑bg+vA,t其中，bat表示载体坐标系下t时刻的运动加速度；bg表示载体坐标系下重力场向量；vA,t表示t时刻的第二高斯白噪声；A表示MEMS加速度计输出模型；S3 使用AR过程建立地理坐标系下的运动加速度nat模型：nat＝ca·nat‑1+vA,t其中，cα表示衰减系数，且取值范围为0～1；nat‑1表示t‑1时刻的运动加速度；vA,t表示t时刻的第二高斯白噪声；S4 搭建卡尔曼滤波姿态估计系统进行数据融合，得到卡尔曼滤波姿态估计系统的状态方程其中，系统误差状态向量表示系统误差状态向量X(t)的导数，表示东向欧拉失准角，表示北向欧拉失准角，表示天向欧拉失准角，bx表示x轴陀螺零偏值，by表示y轴陀螺零偏值，bz表示z轴陀螺零偏值；系统状态转移矩阵表示载体坐标系到地理坐标系的变换坐标矩阵；系统零均值白噪声W(t)＝[wgx wgy wgz 0 0 0]T，且W～N(0,Q)，Q为系统噪声矩阵，wgx、wgy、wgz分别为陀螺中x、y、z三个轴上的噪声；S5 测量卡尔曼滤波姿态估计系统得到量测方程Z(t)；Z(t)＝H(t)X(t)+V(t)其中，测量向量δgE表示地理坐标系中东向重力场矢量与测量 东向重力场矢量的差值，δgN表示地理坐标系中北向重力场矢量与测量北向重力场矢量的差值；测量矩阵测量零均值白噪声V(t)～N(0，R)，R为测量噪声矩阵；S6 离散化所述状态方程X(t)和所述量测方程Z(t)；Xk＝Fk/k‑1Xk‑1+Wk‑1Zk＝HkXk+Vk其中，k‑1、k分别表示t＝k‑1时刻和t＝k时刻；Fk/k‑1为t＝k时刻到t＝k‑1时刻状态一步转移矩阵；Wk‑1表示t＝k‑1时刻零均值白噪声；Hk为t＝k时刻的测量矩阵；Vk表示t＝k时刻的零均值白噪声；S7 分别对状态一步转移矩阵Fk/k‑1和系统噪声矩阵Q(tk)进行二阶近似：其中，Fk/k‑1表示t＝k时刻到t＝k‑1时刻状态一步转移矩阵；I表示单位矩阵；滤波周期T＝tk‑tk‑1；F(tk‑1)表示t＝k‑1时刻的系统状态转移矩阵；其中，Q(tk‑1)表示t＝k‑1时刻的系统噪声矩阵，F(tk‑1)表示t＝k‑1时刻的系统状态转移矩阵；S8 循环迭代入卡尔曼滤波公式实现载体姿态的修正。...

【技术特征摘要】

【专利技术属性】
技术研发人员：吕印新，张家奇，陈养彬，
申请(专利权)人：上海新纪元机器人有限公司，
类型：发明
国别省市：上海;31