机动加速度辅助的扩展卡尔曼滤波航姿系统姿态估计方法技术方案

本发明专利技术提出了一种机动加速度辅助的扩展卡尔曼滤波航姿系统姿态估计方法，其扩展卡尔曼滤波的状态量为三个姿态角误差、三轴陀螺的零偏误差和载体系的三轴载体机动加速度误差，观测量为三轴加速度误差和三轴地磁场误差。将捷联姿态算法和该九态扩展卡尔曼滤波算法进行数据融合，得到航姿系统姿态估计。通过单轴转台试验、车载动态实验和飞行实验，验证了这种机动加速度辅助的九态扩展卡尔曼滤波数据融合算法在不同机动情形下姿态角精度稳定，其姿态角均方差可限制在２°以内。

【技术实现步骤摘要】

本专利技术涉及一种捷联惯性航姿系统航姿估计技术，尤其涉及一种利用载体机动加速度辅助的九态扩展卡尔曼滤波航姿系统姿态估计方法。
技术介绍
由于开环式IFOG航姿系统的陀螺精度有限，初始化后仅依靠传统的捷联算法，姿态将很快发散，需要利用加速度计和磁罗盘的信息来进行修正。现有方法在载体机动小时比较有效，而当应用于无人机、直升机等载体时，由于其机动时间较长，且要求姿态仍能保持较高的精度，那么载体本身的机动加速度的影响无法消除，这样根据加速度计信息不能得到较准确的俯仰角和横滚角估计，同时也不能根据磁罗盘信息对航向角信息进行准确的估计。所以当载体长时间处于大机动状态时(如飞机长时间协调转弯)，由于向心加速度的存在，导致一段时间后加速度计和磁罗盘不能有效的对姿态误差进行修正，相应陀螺的零偏也越来越大，这是开环式IFOG航姿系统在机动载体应用时存在的关键性障碍。
技术实现思路
现有的开环式IFOG航姿系统三轴加速度计和三轴磁传感器信息不仅包含了对横滚角、俯仰角和航向角的观测，同时也能对载体自身机动加速度进行观测。为解决载体长时机动情形下的姿态估计问题，本专利技术利用加速度和磁场观测冗余信息，将载体机动加速度描述为一种非零均值时间相关模型，提出了一种九状态扩展卡尔曼滤波方法，即取三个姿态角误差、三轴陀螺的零偏误差和载体系的三轴载体机动加速度误差为状态量，观测量为三轴加速度误差和三轴地磁场误差的九状态扩展卡尔曼滤波，并与捷联姿态算法进行数据融合，能够将不同机动情形下姿态角均方差控制在2°以内。 本专利技术提供一种能够有效解决载体长时机动情形下的姿态精度保持的捷联惯性航姿系统航姿估计方法，主要包括下列步骤 步骤一、惯性传感器标定，包括光纤陀螺仪、加速度计和磁传感器的标定，分别取标定后载体坐标系下的三轴角速度、三轴加速度及三轴地磁场。 步骤二、罗差校正，利用陀螺和地磁场的转动信息对三轴软铁和硬铁效应进行校正。 步骤三、捷联姿态解算采用四元数表示方法，进行捷联姿态解算。 步骤四、载体机动加速度模型 当载体进行直线加减速或长时间转弯机动时，将机动加速度描述为一种非零均值时间相关模型，即当载体以某一加速度机动时，下一瞬时的加速度只能在当前加速度的邻域内，如下式 其中a为机动加速度， 为下一时刻的机动加速度，a为机动加速度均值，取前一时刻的机动加速度，且在每一采样周期内为常数，δa为零均值有色加速度噪声，即为相邻时刻的机动加速度误差， 为下一相邻时刻的机动加速度误差，α为机动频率，wa为白噪声，方差σa2为机动加速度方差，当机动行为不太剧烈时可取为常值。 步骤五、建立机动加速度辅助的九态扩展卡尔曼滤波模型，状态量取为三个姿态角误差、三轴陀螺的零偏误差和载体系的三轴载体机动加速度误差，观测量为三轴加速度误差和三轴地磁场误差。 i.状态方程 九态扩展卡尔曼滤波器的误差状态量选取如下 其中δρ＝T，是姿态角误差向量；δb＝T，是陀螺零偏误差向量；δa＝T，是载体系中的机动加速度误差向量，模型如式(2)所示。姿态角误差方程忽略地球表观运动角速度，以式(3)为状态量的状态方程为 其中 δb建模为一个随机游走过程，αx，αy，αz分别为载体系内三轴机动加速度的机动频率； ii.量测方程 量测方程中用到了两种量测信息，分别是 ①加速度误差量测方程 取向量gn＝T，为重力向量在导航坐标系的投影。载体系中加速度计测得加速度为 为载体系中重力向量gb、载体加速度a、量测噪声vf之和，其中加速度的常值零偏可归结到载体加速度a中。 为加速度估计值， 为导航坐标系到载体坐标系变换矩阵的估计阵，取加速度计在载体系测得的加速度 与估计加速度 之差δfb为观测量 ②地磁场误差量测方程 向量mn为地磁场向量在导航坐标系的投影，mb为地磁场向量在载体坐标系分量，航姿系统工作前在现场要首先进行软硬铁补偿，然后根据静态初始化时的初始姿态矩阵和三轴磁传感器测量值可得到当地的mn。取磁传感器在载体系内测得的三轴地磁场 与估计地磁场 之差δmb为观测量 其中vm为载体系中磁场的量测噪声，是由向量mb构成的反对称阵 综合式(5)定义的载体系中加速度误差和式(6)中定义的载体系中地磁场误差，得到量测方程 Z(t)＝H(t)δx+v(t) (7) 其中 步骤六、将机动加速度辅助的九态扩展卡尔曼滤波和捷联姿态算法进行数据融合，得到航姿系统姿态估计。 采用四阶龙格库塔法进行捷联姿态解算，解算得到的四元数 实时地由误差四元数 进行修正得到滤波修正后的输出四元数 其中误差四元数 正是由九态扩展卡尔曼滤波估计的姿态角误差向量 经转换后得到的。与此同时，由九态扩展卡尔曼滤波估计的陀螺零偏误差向量δb+和机动加速度误差向量 分别用来实时修正陀螺零偏向量 和机动加速度向量 九态扩展卡尔曼滤波观测量取为δfb和δmb，状态量为姿态角误差向量δρ、陀螺零偏误差向量δb和载体系中的机动加速度误差向量δa。 步骤七、将三轴角速度、加速度、横滚、俯仰、航向角等数据打包输出，即得到航姿估计值。本专利技术具有以下优点 a、能够有效解决载体长时间机动情形下的姿态精度保持问题； b、实时修正机动加速度估计值和陀螺零偏； c、不改变硬件结构和安装方式。 附图说明 图1九态扩展卡尔曼滤波姿态数据融合算法流程图。 图2(a)9态EKF三轴载体机动加速度曲线。 图2(b)三轴陀螺零偏估计曲线。 图2(c)横滚、俯仰和航向角估计曲线。 图3(a)9态EKF三轴载体机动加速度曲线。 图3(b)6态EKF三轴陀螺零偏曲线。 图3(c)9态EKF三轴陀螺零偏曲线。 图3(d)捷联算法横滚、俯仰和航向角曲线。 图3(e)6态EKF横滚、俯仰和航向角曲线。 图3(f)9态EKF横滚、俯仰和航向角曲线。 图4(a)9态EKF三轴载体机动加速度曲线。 图4(b)6态EKF三轴陀螺零偏曲线。 图4(c)9态EKF三轴陀螺零偏曲线。 图4(d)捷联算法横滚、俯仰和航向角曲线。 图4(e)6态EKF横滚、俯仰和航向角曲线。 图4(f)9态EKF横滚、俯仰和航向角曲线。 图5(a)9态EKF三轴载体机动加速度曲线。 图5(b)6态EKF三轴陀螺零偏曲线。 图5(c)9态EKF三轴陀螺零偏曲线。 图5(d)6态EKF横滚、俯仰和航向角曲线。 图5(e)9态EKF横滚、俯仰和航向角曲线。 图69态EKF横滚角和俯仰角曲线。 图7POS与AHRS横滚、俯仰和航向角对比曲线。 具体实施例方式 下面结合附图和实施例对本专利技术进行详细说明。 本专利技术提供的，通过如下步骤实现 步骤一、惯性传感器标定，包括光纤陀螺仪、加速度计和磁传感器的标定，分别取标定后载体坐标系下的三轴角速度、三轴加速度及三轴地磁场，如图1。 步骤二、罗差校正，利用陀螺和地磁场的转动信息对三轴软铁和硬铁效应进行校正本文档来自技高网...

【技术保护点】
机动加速度辅助的扩展卡尔曼滤波航姿系统姿态估计方法，其特征在于包括以下步骤：（１）惯性传感器标定，分别取标定后载体坐标系下的三轴角速度、三轴加速度及三轴地磁场，该惯性传感器包括光纤陀螺仪、加速度计和磁传感器；（２）罗差校正，利用陀螺和地磁场的转动信息对三轴软铁和硬铁效应进行校正；（３）捷联姿态解算：采用四元数表示方法，进行捷联姿态解算；（４）将载体机动加速度描述为一种非零均值时间相关模型，即当载体以某一加速度机动时，下一瞬时的加速度只能在当前加速度的邻域内，描述为：＊＝＊＋δａ（１）δ＊＝－αδａ＋ｗ↓［ａ］（２）其中ａ为机动加速度，＊为下一时刻的机动加速度，＊为机动加速度均值，取前一时刻的机动加速度，且在每一采样周期内为常数，δａ为零均值有色加速度噪声，即为相邻时刻的机动加速度误差，δ＊为下一相邻时刻的机动加速度误差，α为机动频率；ｗ↓［ａ］为白噪声；方差σ↓［ｗ↓［ａ］］↑［２］＝２ασ↓［ａ］↑［２］，σ↓［ａ］↑［２］为机动加速度方差，取为常值；（５）建立机动加速度辅助的九态扩展卡尔曼滤波算法，状态量取为三个姿态角误差、三轴陀螺的零偏误差和载体系的三轴载体机动加速度误差，观测量为三轴加速度误差和三轴地磁场误差；（６）将机动加速度辅助的九态扩展卡尔曼滤波和捷联姿态算法进行数据融合，得到航姿系统姿态估计。...

【技术特征摘要】

【专利技术属性】
技术研发人员：任章，郭鹏飞，
申请(专利权)人：北京航空航天大学，
类型：发明
国别省市：11[中国|北京]