专利名称：机动加速度辅助的扩展卡尔曼滤波航姿系统姿态估计方法
技术领域：
本发明涉及一种捷联惯性航姿系统航姿估计技术，尤其涉及一种利用载体机动加速度辅助的九态扩展卡尔曼滤波航姿系统姿态估计方法。
背景技术：
由于开环式IFOG航姿系统的陀螺精度有限，初始化后仅依靠传统的捷联算法，姿态将很快发散，需要利用加速度计和磁罗盘的信息来进行修正。现有方法在载体机动小时比较有效，而当应用于无人机、直升机等载体时，由于其机动时间较长，且要求姿态仍能保持较高的精度，那么载体本身的机动加速度的影响无法消除，这样根据加速度计信息不能得到较准确的俯仰角和横滚角估计，同时也不能根据磁罗盘信息对航向角信息进行准确的估计。所以当载体长时间处于大机动状态时(如飞机长时间协调转弯)，由于向心加速度的存在，导致一段时间后加速度计和磁罗盘不能有效的对姿态误差进行修正，相应陀螺的零偏也越来越大，这是开环式IFOG航姿系统在机动载体应用时存在的关键性障碍。
发明内容
现有的开环式IFOG航姿系统三轴加速度计和三轴磁传感器信息不仅包含了对横滚角、俯仰角和航向角的观测，同时也能对载体自身机动加速度进行观测。为解决载体长时机动情形下的姿态估计问题，本发明利用加速度和磁场观测冗余信息，将载体机动加速度描述为一种非零均值时间相关模型，提出了一种九状态扩展卡尔曼滤波方法，即取三个姿态角误差、三轴陀螺的零偏误差和载体系的三轴载体机动加速度误差为状态量，观测量为三轴加速度误差和三轴地磁场误差的九状态扩展卡尔曼滤波，并与捷联姿态算法进行数据融合，能够将不同机动情形下姿态角均方差控制在2°以内。
本发明提供一种能够有效解决载体长时机动情形下的姿态精度保持的捷联惯性航姿系统航姿估计方法，主要包括下列步骤 步骤一、惯性传感器标定，包括光纤陀螺仪、加速度计和磁传感器的标定，分别取标定后载体坐标系下的三轴角速度、三轴加速度及三轴地磁场。
步骤二、罗差校正，利用陀螺和地磁场的转动信息对三轴软铁和硬铁效应进行校正。
步骤三、捷联姿态解算采用四元数表示方法，进行捷联姿态解算。
步骤四、载体机动加速度模型 当载体进行直线加减速或长时间转弯机动时，将机动加速度描述为一种非零均值时间相关模型，即当载体以某一加速度机动时，下一瞬时的加速度只能在当前加速度的邻域内，如下式 其中a为机动加速度，
为下一时刻的机动加速度，a为机动加速度均值，取前一时刻的机动加速度，且在每一采样周期内为常数，δa为零均值有色加速度噪声，即为相邻时刻的机动加速度误差，
为下一相邻时刻的机动加速度误差，α为机动频率，wa为白噪声，方差σa2为机动加速度方差，当机动行为不太剧烈时可取为常值。
步骤五、建立机动加速度辅助的九态扩展卡尔曼滤波模型，状态量取为三个姿态角误差、三轴陀螺的零偏误差和载体系的三轴载体机动加速度误差，观测量为三轴加速度误差和三轴地磁场误差。
i.状态方程 九态扩展卡尔曼滤波器的误差状态量选取如下 其中δρ＝[∈x，∈y，∈z]T，是姿态角误差向量；δb＝[bx，by，bz]T，是陀螺零偏误差向量；δa＝[δax，δay，δaz]T，是载体系中的机动加速度误差向量，模型如式(2)所示。姿态角误差方程忽略地球表观运动角速度，以式(3)为状态量的状态方程为 其中 δb建模为一个随机游走过程，αx，αy，αz分别为载体系内三轴机动加速度的机动频率； ii.量测方程 量测方程中用到了两种量测信息，分别是 ①加速度误差量测方程 取向量gn＝
T，为重力向量在导航坐标系的投影。载体系中加速度计测得加速度为
为载体系中重力向量gb、载体加速度a、量测噪声vf之和，其中加速度的常值零偏可归结到载体加速度a中。
为加速度估计值，
为导航坐标系到载体坐标系变换矩阵的估计阵，取加速度计在载体系测得的加速度
与估计加速度
之差δfb为观测量 ②地磁场误差量测方程 向量mn为地磁场向量在导航坐标系的投影，mb为地磁场向量在载体坐标系分量，航姿系统工作前在现场要首先进行软硬铁补偿，然后根据静态初始化时的初始姿态矩阵和三轴磁传感器测量值可得到当地的mn。取磁传感器在载体系内测得的三轴地磁场
与估计地磁场
之差δmb为观测量 其中vm为载体系中磁场的量测噪声，[mb×]是由向量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。
步骤二、罗差校正，利用陀螺和地磁场的转动信息对三轴软铁和硬铁效应进行校正。
步骤三、捷联姿态解算采用四元数表示方法，进行捷联姿态解算。
步骤四、载体机动加速度模型建立 当载体进行直线加减速或长时间转弯机动时，将机动加速度描述为一种非零均值时间相关模型，即当载体以某一加速度机动时，下一瞬时的加速度只能在当前加速度的邻域内，如下式 其中a为机动加速度，
为下一时刻的机动加速度，a为机动加速度均值，取前一时刻的机动加速度，且在每一采样周期内为常数，δa为零均值有色加速度噪声，即为相邻时刻的机动加速度误差，
为下一相邻时刻的机动加速度误差，α为机动频率，wa为白噪声，方差σa2为机动加速度方差，当机动行为不太剧烈时可取为常值。
步骤五、机动加速度辅助的九态扩展卡尔曼滤波算法 i.状态方程 九态扩展卡尔曼滤波器的误差状态量选取如下 其中δρ＝[∈x，∈y，∈z]T，是姿态角误差向量；δb＝[bx，by，bz]T，是陀螺零偏误差向量，δa＝[δax，δay，δaz]T，是载体系中的机动加速度误差向量，模型如式(2)所示。姿态角误差方程忽略地球表观运动角速度，以式(3)为状态量的状态方程为 其中 δb建模为一个随机游走过程，αx，αy，αz分别为载体系内三轴机动加速度的机动频率。
ii.量测方程 量测方程中用到了两种量测信息，分别是 ①加速度误差量测方程 取向量gn＝
T，为重力向量在导航坐标系的投影。载体系中加速度计测得加速度为
为载体系中重力向量gb、载体加速度a、量测噪声vf之和，其中加速度的常值零偏可归结到载体加速度a中。
为加速度估计值，
为导航坐标系到载体坐标系变换矩阵的估计阵，取加速度计在载体系测得的加速度
与估计加速度
之差
为观测量 ②地磁场误差量测方程 向量mn为地磁场向量在导航坐标系的投影，mb为地磁场向量在载体坐标系分量，航姿系统工作前在现场要首先进行软硬铁补偿，然后根据静态初始化时的初始姿态矩阵和三轴磁传感器测量值可得到当地的mn。取磁传感器在载体系内测得的三轴地磁场
与估计地磁场
之差δmb为观测量 其中vm为载体系中磁场的量测噪声，[mb×]是由向量mb构成的反对称阵 综合式(5)定义的载体系中加速度误差和式(6)中定义的载体系中地磁场误差，得到量测方程 Z(t)＝H(t)δx+v(t)(7) 其中 iii.离散化和递推方程 卡尔曼滤波器在离散时间系统中的实现需要一个离散时间状态转移矩阵Ф，和一个离散时间过程噪声协方差阵Qd。
AHRS在一个滤波周期ΔT内能得到10个采样值，一步状态转移阵和等效离散噪声方差阵可简化计算如下 每个采样周期内，根据采用四元数对姿态进行更新；而每个滤波周期内，误差状态及其协方差的时间更新如下 当获得有效的测量值时，滤波器增益计算如下 误差状态协方差阵和误差状态观测更新的估计如下 通过反馈δxk+1+可对姿态角和陀螺零偏进行校正，而在下个滤波周期误差状态预测值δxk+1-应该被设为零。
通过分析加速度计和磁传感器测量噪声的频谱密度，可以确定R。通过分析陀螺的过程噪声及与其相关的漂移，可以确定Qd。当系统处于静态模式时，利用已确定的R和Qd驱使卡尔曼滤波器产生最佳的增益，从而得到最好的状态估计。
步骤六、将机动加速度辅助的九态扩展卡尔曼滤波和捷联姿态算法进行数据融合，得到航姿系统姿态估计。
结合捷联姿态算法和机动加速度辅助的九态扩展卡尔曼滤波算法，得到的航姿系统姿态算法流程图如图1所示，其中b0是航姿系统静态初始上电一段时间得到的零偏，
是捷联算法得到的四元数，
是根据卡尔曼滤波估计的姿态角误差向量δρ+转换后得到的误差四元数，
是滤波修正后四元数，加速度测量值增加了机动加速度a，而在测量方程中也引入了机动加速度误差，其中的三个虚线部分表示卡尔曼滤波估计后实时修正了机动加速度、捷联矩阵和陀螺零偏。
步骤七、将三轴角速度、加速度、横滚、俯仰、航向角等数据打包输出，即得到航姿估计值。
实施例 本实施例中以纯捷联算法、六态扩展卡尔曼滤波姿态算法(状态量为三个姿态角误差、三轴陀螺的零偏误差，加速度误差量测方程未考虑载体机动加速度，或者说将载体机动加速度设为量测白噪声)、九态扩展卡尔曼滤波姿态算法及高精度组合导航系统POS(Positionand Orientation System)进行试验，得出有益的结论。
(一)单轴转台试验 (1)静态试验 将航姿系统缓慢转动两周，进行软铁和硬铁补偿，再将航姿系统倾斜大概-60°，在其静止时采集一段时间数据；滤波参数设置三轴机动频率均选为1，机动加速度均方差为0.001g，姿态角误差噪声均方差为0.01°，陀螺零偏误差噪声均方差为0.001°/s，加速度计测量噪声均方差为0.01g，磁传感器测量噪声均方差为0.006gauss，结果见图2a、2b、2c。
(2)转台向心加速度试验 将航姿系统转动两圈进行罗差补偿，然后将长铝棒垂直固定在单轴速率转台上方，通过在铝棒顶端固定并引出水平木棒，将航姿系统接近水平固定在水平木棒远端，这样航姿系统距离转台中心水平距离约为0.8米。航姿系统从静止开始工作一段时间，接着绕垂直轴以-150°/s匀速转动70秒，然后停止转动，静止一段时间。滤波参数设置三轴机动频率选为1，由于转台突起突停变化非常快，机动加速度均方差放大，选为0.004g，由于150°/s不在一体化标定的测试转速内，非线性补偿效果相对较差，因此姿态角误差噪声均方差放大，选为0.1°/s，陀螺零偏误差噪声均方差为0.001°/s，加速度计测量噪声均方差为0.01g，磁传感器测量噪声均方差为0.006gauss，结果见图3a～3f。
(二)车载动态试验 (1)快速机动试验 实验设计测试车在丰台科技园总部基地沿一矩形区域行驶四圈，直线段不时做突然加减速运动，每个拐弯处尽量快速转弯，最后慢慢停下来。在这个测试中，没有准确的地面实况，不能准确判定系航姿统真正的性能。然而，地面实况的倾角变化不应超过3°，而且每圈的数据可看做具有重复性。滤波参数选择测试车加减速均较快，三轴机动频率选为1。安装在测试车顶部的AHRS受周围软铁和硬铁影响较大，在进行软硬铁补偿时，由于找不到倾角变化较大的场地做14参数罗差补偿，因此补偿后的磁场精度较差，这里降低磁场观测精度，提高状态方程中姿态角精度；由于机动频繁且不同机动间切换很快，考虑降低加速度观测精度。机动加速度均方差为0.002g，姿态角误差噪声均方差为0.01°/s，陀螺零偏误差噪声均方差为0.0003°/s，加速度计测量噪声均方差为0.02g，磁传感器测量噪声均方差为0.02gauss，结果见图4a～4f。
(2)原地连续转弯试验 实验设计测试车原地沿半径大约3米的圆周以大约40°/s的速度原地连续旋转将近5分钟，实际路况有明显坑凹处(如下水道井盖等)，但仍不会有3°以上的倾角变化。滤波参数选择提高机动加速度均方差为0.003g，姿态角误差噪声均方差为0.01°/s，陀螺零偏误差噪声均方差为0.0003°/s，加速度计测量噪声均方差为0.01g，降低磁传感器测量噪声均方差为0.03gauss，结果见图5a～5e。
(3)综合跑车试验 实验设计测试车进行超过半个小时的综合跑车实验，包括加减速上下坡、原地转动、S形机动、上台阶等，本次实验中设计了各种机动情况和车载应用可能遇到的状况，结果见图6。
(三)飞行试验 本航姿系统在某机型航拍系统进行了飞行摸底，并同航拍高精度组合导航系统进行了共计4000多秒的对比试验。航拍高精度组合导航系统采用加拿大Applanix公司的POS(Position and Orientation System)，航姿系统和POS系统数据采集用PDA。试验过程飞机在地面滑行一段时间后起飞，爬升至大约800米高空，在作业区域来回飞了3段折线，其中有3处明显的长时间协调转弯，其他都处于平飞状态。对比结果见图7。
权利要求
1.机动加速度辅助的扩展卡尔曼滤波航姿系统姿态估计方法，其特征在于包括以下步骤
(1)惯性传感器标定，分别取标定后载体坐标系下的三轴角速度、三轴加速度及三轴地磁场，该惯性传感器包括光纤陀螺仪、加速度计和磁传感器；
(2)罗差校正，利用陀螺和地磁场的转动信息对三轴软铁和硬铁效应进行校正；
(3)捷联姿态解算采用四元数表示方法，进行捷联姿态解算；
(4)将载体机动加速度描述为一种非零均值时间相关模型，即当载体以某一加速度机动时，下一瞬时的加速度只能在当前加速度的邻域内，描述为
其中a为机动加速度，
为下一时刻的机动加速度，a为机动加速度均值，取前一时刻的机动加速度，且在每一采样周期内为常数，δa为零均值有色加速度噪声，即为相邻时刻的机动加速度误差，
为下一相邻时刻的机动加速度误差，α为机动频率；wa为白噪声；方差σa2为机动加速度方差，取为常值；
(5)建立机动加速度辅助的九态扩展卡尔曼滤波算法，状态量取为三个姿态角误差、三轴陀螺的零偏误差和载体系的三轴载体机动加速度误差，观测量为三轴加速度误差和三轴地磁场误差；
(6)将机动加速度辅助的九态扩展卡尔曼滤波和捷联姿态算法进行数据融合，得到航姿系统姿态估计。
2.如权利要求1所述的机动加速度辅助的扩展卡尔曼滤波航姿系统姿态估计方法，其主要特征在于所述步骤(5)包括
(a)状态方程
九态扩展卡尔曼滤波器的误差状态量选取如下
其中δρ＝[∈x，∈y，∈z]T，是姿态角误差向量；δb＝[bx，by，bz]T，是陀螺零偏误差向量；δa＝[δax，δay，δaz]T，是载体系中的机动加速度误差向量，模型如式(2)所示；姿态角误差方程忽略地球表观运动角速度，以式(3)为状态量的状态方程为
其中
δb建模为一个随机游走过程，αx，αy，αz分别为载体系内三轴机动加速度的机动频率；
(b)量测方程
量测方程中用到了两种量测信息，分别是
①加速度误差量测方程
取加速度计在载体系测得的加速度与估计加速度之差为观测量；
②地磁场误差量测方程
向量mn为地磁场向量在导航坐标系的投影，mb为地磁场向量在载体坐标系分量，航姿系统工作前在现场要首先进行软硬铁补偿，然后根据静态初始化时的初始姿态矩阵和三轴磁传感器测量值可得到当地的mn；取磁传感器在载体系内测得的三轴地磁场
与估计地磁场
之差δmb为观测量
其中vm为载体系中磁场的量测噪声，[mb×]是由向量mb构成的反对称阵
综合加速度误差量测方程中定义的载体系中加速度误差和式(5)中定义的载体系中地磁场误差，得到量测方程
Z(t)＝H(t)δx+v(t)(6)
其中
3.如权利要求2所述的机动加速度辅助的扩展卡尔曼滤波航姿系统姿态估计方法，其特征在于所述步骤(b)中的①具体为
取向量gn＝
T，为重力向量在导航坐标系的投影，载体系中加速度计测得加速度为
为载体系中重力向量gb、载体加速度a、量测噪声vf之和，其中加速度的常值零偏可归结到载体加速度a中，
为加速度估计值，
为导航坐标系到载体坐标系变换矩阵的估计阵，取加速度计在载体系测得的加速度
与估计加速度
之差δfb为观测量
全文摘要
本发明提出了一种机动加速度辅助的扩展卡尔曼滤波航姿系统姿态估计方法，其扩展卡尔曼滤波的状态量为三个姿态角误差、三轴陀螺的零偏误差和载体系的三轴载体机动加速度误差，观测量为三轴加速度误差和三轴地磁场误差。将捷联姿态算法和该九态扩展卡尔曼滤波算法进行数据融合，得到航姿系统姿态估计。通过单轴转台试验、车载动态实验和飞行实验，验证了这种机动加速度辅助的九态扩展卡尔曼滤波数据融合算法在不同机动情形下姿态角精度稳定，其姿态角均方差可限制在2°以内。
文档编号G01C21/18GK101782391SQ20091008731
公开日2010年7月21日 申请日期2009年6月22日 优先权日2009年6月22日
发明者任章, 郭鹏飞 申请人:北京航空航天大学