基于地磁场自适应修正的航姿系统姿态解算方法.pdf

《基于地磁场自适应修正的航姿系统姿态解算方法.pdf》由会员分享，可在线阅读，更多相关《基于地磁场自适应修正的航姿系统姿态解算方法.pdf（14页完成版）》请在专利查询网上搜索。

1、(19)中华人民共和国国家知识产权局 (12)发明专利申请 (10)申请公布号 (43)申请公布日 (21)申请号 201910364315.7 (22)申请日 2019.04.30 (71)申请人 西北工业大学 地址 710072 陕西省西安市友谊西路127号 (72)发明人 刘明雍宋保维郭娇娇牛云 刘禄王梦凡雷罡李赛楠 杨扬向举苗王录 (74)专利代理机构 西北工业大学专利中心 61204 代理人 陈星 (51)Int.Cl. G01C 25/00(2006.01) G01C 21/20(2006.01) G01C 21/18(2006.01) G01C 21/16(2006.01) (5。

2、4)发明名称 一种基于地磁场自适应修正的航姿系统姿 态解算方法 (57)摘要 本发明提出一种基于地磁场自适应修正的 航姿系统姿态解算方法， 通过分别计算状态转移 矩阵、 量测矩阵、 系统噪声协方差矩阵、 量测噪声 协方差矩阵和自适应矩阵， 实现采用自适应扩展 卡尔曼滤波算法进行航姿系统姿态解算； 本发明 有效解决了载体姿态的累积误差较大的问题， 同 时提高了姿态解算精度。 权利要求书2页 说明书9页 附图2页 CN 110174121 A 2019.08.27 CN 110174121 A 1.一种基于地磁场自适应修正的航姿系统姿态解算方法， 其特征在于： 采用自适应扩 展卡尔曼滤波算法进行航。

3、姿系统姿态解算， 其中 状态方程为： Xkk/k-1 Xk-1+Wk-1 量测方程为： ZkHkXk+Vk 其中Xk为tk时刻的状态向量， Zk为tk时刻的量测向量， k/k-1为tk-1时刻至tk时刻的状态 转移矩阵， Hk为量测矩阵， Vk为量测噪声， Wk-1为系统噪声， 系统噪声协方差矩阵为Qk-1， 量测 噪声协方差矩阵为Rk； 为自适应矩阵； 采用以下步骤计算状态转移矩阵、 量测矩阵、 系统噪声协方差矩阵、 量测噪声协方差矩 阵和自适应矩阵： 状态转移矩阵采用公式 计算得到， 其中I为单位矩阵， 根据公式确定， 根据公式 得到， 其中为航姿系统中传感器输出的载体坐标系B下的载体角速。

4、 度信息； 量测矩阵采用公式 计算得到， 其中 a为在载体坐标系B中测量的重力加速度矢量， g为在全局坐标系G中测量的重力加速度 矢量， 并采用零标量的四元数方式表示矢量g与a； 系统噪声协方差矩阵采用公式 权利要求书 1/2 页 2 CN 110174121 A 2 计算得到， 其中t为相邻时刻的时间间隔， gyro为陀螺仪噪声协方差矩阵q(tk-1)为 tk-1时刻全局坐标系G到载体坐标系B的旋转四元数， 对于某一四元数q， (q)为 gyro为航姿系统中陀螺仪噪声协方差矩阵； 量测噪声协方差矩阵采用公式 计算得到， 其中acc为航姿系统中加速度计噪声协方差矩阵， q(tk)为tk时刻全局。

5、坐标 系G到载体坐标系B的旋转四元数； 自适应矩阵采用公式 计算得到， 其中为根据公式 计算得到的四元数； lx2+ly2， 矢量l(lx ly lz)T表示由载体坐标系下测量到的地 磁场矢量mmx my mzT进行坐标变换后得到的地磁场矢量， 其中为计算 得到的姿态四元数。 2.根据权利要求1所述一种基于地磁场自适应修正的航姿系统姿态解算方法， 其特征 在于： 根据公式 RkRk+ I 0 对量测噪声协方差矩阵进行改进， 其中Rk为改进后的量测噪声协方差矩阵， 为大于0 的设定常数。 权利要求书 2/2 页 3 CN 110174121 A 3 一种基于地磁场自适应修正的航姿系统姿态解算方法。

6、 技术领域 0001 本发明涉及一种基于地球重力场与地磁场自适应修正的姿态解算方法， 属于定位 导航技术领域。 背景技术 0002 MEMS传感器(陀螺仪， 加速度计， 磁力计)在移动设备领域使用越来越普及， 具有成 本低、 集成度高以及体积小等优点， 但是在进行姿态解算时， 却存在输出噪声过大、 零点漂 移无法完全消除、 角速率输出易受干扰等问题。 因此， 通常情况下， 需要采用多个传感器通 过信息融合算法实现高精度的姿态解算。 航姿系统(AHRS)传感器在姿态解算过程中， 一般 采用基于四元数的互补滤波算法和扩展卡尔曼滤波算法来解决上述问题， 但目前在基于四 元数的互补滤波算法和扩展卡尔曼。

7、滤波算法中， 对于环境中的软磁干扰问题都是直接利用 世界地磁模型进行修正， 忽略了实际中可能会出现的磁异常对姿态解算的影响。 发明内容 0003 针对现有技术存在的： (1)当仅仅使用陀螺仪与加速度数据时， 解算的姿态角存在 累积误差； (2)引入磁力计数据时， 由于环境中的磁干扰会对磁测量产生影响， 出现航向姿 态输出噪声过大、 姿态解算精度降低等问题。 本发明提出一种基于地磁场自适应修正的航 姿系统姿态解算方法， 在引入地磁场信息后， 修正载体航向姿态的累计误差， 保证解算姿态 的精度。 0004 本发明的技术方案为： 0005 所述一种基于地磁场自适应修正的航姿系统姿态解算方法， 其特征。

8、在于： 采用自 适应扩展卡尔曼滤波算法进行航姿系统姿态解算， 其中 0006 状态方程为： 0007 Xkk/k-1 Xk-1+Wk-1 0008 量测方程为： 0009 ZkHkXk+Vk 0010 其中Xk为tk时刻的状态向量， Zk为tk时刻的量测向量， k/k-1为tk-1时刻至tk时刻的 状态转移矩阵， Hk为量测矩阵， Vk为量测噪声， Wk-1为系统噪声， 系统噪声协方差矩阵为 Qk-1， 量测噪声协方差矩阵为Rk； 为自适应矩阵； 0011 采用以下步骤计算状态转移矩阵、 量测矩阵、 系统噪声协方差矩阵、 量测噪声协方 差矩阵和自适应矩阵： 0012 状态转移矩阵采用公式 00。

9、13 0014计算得到， 其中I为单位矩阵， 根据公式确定， 根据公 式 说明书 1/9 页 4 CN 110174121 A 4 0015 0016得到， 其中为航姿系统中传感器输出的载体坐标系B下的载体 角速度信息； 0017 量测矩阵采用公式 0018 0019 计算得到， 其中 0020 0021 0022 a为在载体坐标系B中测量的重力加速度矢量， g为在全局坐标系G中测量的重力加 速度矢量， 并采用零标量的四元数方式表示矢量g与a； 0023 系统噪声协方差矩阵采用公式 0024 0025 计算得到， 其中t为相邻时刻的时间间隔， gyro为陀螺仪噪声协方差矩阵 q (tk-1)为。

10、tk-1时刻全局坐标系G到载体坐标系B的旋转四元数， 对于某一四元数q， (q) 为 0026 0027 gyro为航姿系统中陀螺仪噪声协方差矩阵； 0028 量测噪声协方差矩阵采用公式 0029 0030 计算得到， 其中acc为航姿系统中加速度计噪声协方差矩阵， q(tk)为tk时刻全局 坐标系G到载体坐标系B的旋转四元数； 0031 自适应矩阵采用公式 0032 说明书 2/9 页 5 CN 110174121 A 5 0033计算得到， 其中为根据公式 0034 0035 计算得到的四元数； lx2+ly2， 矢量l(lx ly lz)T表示由载体坐标系下测量到 的地磁场矢量mmx m。

11、y mzT进行坐标变换后得到的地磁场矢量， 其中为 计算得到的姿态四元数。 0036 进一步的优选方案， 所述一种基于地磁场自适应修正的航姿系统姿态解算方法， 其特征在于： 根据公式 0037 RkRk+ I 0 0038 对量测噪声协方差矩阵进行改进， 其中Rk为改进后的量测噪声协方差矩阵， 为 大于 0的设定常数。 0039 有益效果 0040 本发明提出的自适应卡尔曼滤波算法有效解决了载体姿态的累积误差较大的问 题， 同时提高了姿态解算精度。 0041 本发明的附加方面和优点将在下面的描述中部分给出， 部分将从下面的描述中变 得明显， 或通过本发明的实践了解到。 附图说明 0042 本发。

12、明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得 明显和容易理解， 其中： 0043 附图1是基于地球重力场与地磁场自适应修正的姿态解算算法流程框图； 0044 附图2是ROS平台实验下， 利用本发明输出的载体姿态四元数波形图； 0045 附图3是动态姿态测试条件下， 输入抖动脉冲的姿态欧拉角变化曲线； (a)载体俯 仰角变换曲线， (b)载体横滚角变化曲线， (c)载体航向角变化曲线； 0046 附图4是本专利提出的算法与互补滤波器、 扩展卡尔曼滤波器姿态欧拉角输出波 形对比图； (a)载体俯仰角变换曲线， (b)载体横滚角变化曲线， (c)载体航向角变化曲线。 具体实施方。

13、式 0047 下面详细描述本发明的实施例， 所述实施例是示例性的， 旨在用于解释本发明， 而 不能理解为对本发明的限制。 0048 本实施例提出的一种基于地磁场自适应修正的航姿系统姿态解算方法， 采用自适 应扩展卡尔曼滤波算法进行航姿系统姿态解算， 其中 0049 状态方程为： 0050 Xkk/k-1 Xk-1+Wk-1 0051 量测方程为： 0052 ZkHkXk+Vk 0053 其中Xk为tk时刻的状态向量， Zk为tk时刻的量测向量， k/k-1为tk-1时刻至tk时刻的 说明书 3/9 页 6 CN 110174121 A 6 状态转移矩阵， Hk为量测矩阵， Vk为量测噪声， W。

14、k-1为系统噪声， 系统噪声协方差矩阵为 Qk-1， 量测噪声协方差矩阵为Rk； 为自适应矩阵。 0054 下面计算状态转移矩阵、 量测矩阵、 系统噪声协方差矩阵、 量测噪声协方差矩阵和 自适应矩阵： 0055 步骤1中给出根据AHRS中MEMS陀螺仪采集的角速率计算系统模型的状态转移矩阵 与量测矩阵的具体理论分析过程。 0056 (1.1)建立陀螺仪动力学模型： 0057 对于航姿系统(AHRS)， 其动力学模型如下： 0058 0059 即 0060 0061其中， q表示全局坐标系G到载体坐标系B的旋转四元数，表示在载体坐标系中 测量的MEMS陀螺角速度信息， 且 0062 0063 利。

15、用四元数微分方程的比卡求解法， 将式(1)离散化可得： 0064 0065 其中， q(tk-1)为tk-1时刻全局坐标系G到载体坐标系B的旋转四元数， q(tk)为tk时刻 全局坐标系G到载体坐标系B的旋转四元数， I为单位矩阵 0066 0067且 0068(1.2)获得载体坐标系B下的载体角速度信息 0069(1.3)根据所述载体角速度信息计算系统状态转移矩阵 0070 (1.4)计算量测矩阵Hk。 0071 AHRS系统加速度计观测模型中， 将三维矢量g与a看作零标量的四元数， 即 |g| |a|1， g为在全局坐标系G中测量的重力加速度， a为在载体坐标系B中测量的重力加 速度， 将。

16、单位矢量g与a看作零标量的四元数， 则g与a间的变换关系可用四元数乘法表示为： 说明书 4/9 页 7 CN 110174121 A 7 0072 0073 其中， 四元数q*(tk)表示四元数q(tk)的逆。 0074 经过变换可得 0075 M (a)q(tk)-M(g)q(tk)0 (4) 0076 其中， M (a)与M(g)表示由零标量的四元数g与a构成的矩阵， 如下 0077 0078 0079 令矩阵 0080 0081 向量 0082 Zk0 (6) 0083 则式(4)可改写为 0084 ZkHkq(tk) (1) 0085则系统量测矩阵表示为： 0086 步骤2中进行系统噪。

17、声协方差矩阵的计算： 0087 由于陀螺仪在时间间隔t内， 其真实的角增量 是无法测量的。 因此假设在第k 个时间间隔内， 陀螺仪的实际输出与真实角增量的误差为 k， 则有 0088 0089 实际状态转移矩阵与真实状态转移矩阵的误差为k/k-1， 即有 0090 0091 将上式代入式(2)的陀螺仪离散化的动力学模型中， 可得 0092 0093 将 k/k-1q(tk-1)变形为： 说明书 5/9 页 8 CN 110174121 A 8 0094 k/k-1q(tk-1) (q(tk-1) k/k-1 (11) 0095 其中 0096 0097 设陀螺仪的输出噪声为ngyro,则有 0。

18、098 0099 则系统噪声协方差矩阵为： 0100 0101 其中gyro为陀螺仪噪声协方差矩阵。 0102 步骤3中进行量测噪声协方差矩阵的计算： 0103 (3.1)加速度计输出噪声的计算； 0104 量测噪声Vk主要由加速度计产生。 设加速度计的输出噪声为nacc， 则有 0105 nacca-a (14) 0106 其中a 表示载体坐标系下加速度计的真实值。 0107 (3.2)量测噪声协方差矩阵的计算； 0108 实际量测矩阵与理想量测矩阵误差为 0109 0110 把式(15)代入式(7)可得 0111 0112类似式(12)，变形为 0113 0114 则量测噪声协方差矩阵为 。

19、0115 0116 其中acc为加速度计噪声协方差矩阵。 0117 (3.3)量测噪声协方差矩阵的改进 0118 步骤(3.2)中得到的量测噪声协方差矩阵Rk在应用的过程中很有可能是奇异的， 为了避免矩阵的奇异， 需要对该量测噪声协方差矩阵Rk进行改进， 即 0119 RkRk+ I 0 (19) 0120 步骤4中进行自适应因子的计算： 0121 (4.1)建立磁力计观测模型： 0122 把三维矢量h与m看作零标量的四元数， 则h与m间的变换关系可用四元数乘法表示 说明书 6/9 页 9 CN 110174121 A 9 为 0123 0124 其中， h为在全局坐标系G中测量的地磁场矢量，。

20、 m为在载体坐标系B中测量的地磁 场矢量， 且满足|g|a|1。 0125 上式经过变换可得 0126 M (m)qk-M(h)qk0 (21) 0127 其中， M (m)与M(h)表示由零标量的四元数h与m构成的矩阵， 如下 0128 0129 0130 (4.2)获得载体坐标系B下的载体地磁矢量信息mmx my mzT； 0131 (4.3)计算先验地磁矢量场h； 0132经过最小方差准则计算得到的姿态四元数为且假设载体坐标系下测量到的 地磁场矢量为m,则由坐标变换可得 0133 0134 其中， 矢量l表示测量得到的在包含航向误差的四元数坐标变换下的地磁场矢量。 假设真实的先验地磁场矢。

21、量为 0135 0136 则有 0137 0138其中，表示在h与l之间的转换四元数,lx2+ly2。 0139 (4.4)计算地磁场矢量量测噪声协方差矩阵Rmk 0140 设磁力计的输出噪声为nmag， 则有 0141 nmagm-m (25) 0142 其中， m 表示载体坐标系下加速度计的真实值。 0143 实际量测矩阵与理想量测矩阵误差为 说明书 7/9 页 10 CN 110174121 A 10 0144 0145其中，表示理想量测矩阵。 0146 把上式代入步骤(3.1)中的磁力计观测模型， 可得 0147 0HmkXk-HmkXk (27) 0148 -HmkXk变形为 014。

22、9 0150 其中， 0151 0152令则量测噪声协方差矩阵为 0153 0154 其中， mag为磁力计噪声协方差矩阵。 0155 上式得到的量测噪声协方差矩阵在应用的过程中很有可能是奇异的， 为了避免矩 阵的奇异， 需要对该量测噪声协方差矩阵进行改进， 即 0156 RmkRmk+ mI m0 (30) 0157 (4.4)计算自适应矩阵 。 0158矢量h与l在全局坐标系下， 在水平面内存在微小的夹角， 因此将四元数表示 为： 0159 0160 将式(31)代入式(24)可得 0161 0162则加入修正四元数的姿态四元数为 0163 0164 由此可知， 自适应矩阵因此为 0165。

23、 0166 通过上述研究过程说明， 我们能够实现基于地磁场自适应修正的航姿系统姿态解 算方法， 下面给出一个具体实施例， 使用AH-100B AHRS传感器输出的三轴陀螺、 三轴加速 说明书 8/9 页 11 CN 110174121 A 11 度、 三轴磁场计数据， 分别为、 a和m； 对载体初始姿态四元数初始化， 初始姿态四元数均方 误差初始化； 读取陀螺仪数据， 得到系统噪声协方差矩阵； 读取加速度计数据， 得到量测噪 声协方差矩阵， 并对载体姿态四元数进行一次修正； 读取磁力计数据， 对载体姿态四元数进 行二次修正， 得到自适应矩阵因子。 0167 载体姿态四元数与均方误差矩阵初始化为。

24、单位矩阵， 陀螺仪噪声协方差矩阵设置 为加速度计噪声协方差矩阵设置为磁 力计噪声协方差矩阵设置为 0168 利用该姿态解算算法解算出的载体姿态四元数输出波形如图2所示， 姿态四元数 的分量w在0.9930.994之间变化， 分量x在-0.004-0.002之间波动， 分量y在 0.005 0.007之间波动， 分量z在-0.114-0.110之间波动。 通过上述分析可知， 虽然姿态四元数的 分量z所包含的噪声比其他三个分量大， 但是基本保持在同一水平线上， 因此有效消除了陀 螺仪姿态解算过程中的累积误差。 0169 图3为分别向PITCH轴、 ROLL轴及HEADING轴输入正负抖动脉冲， 载。

25、体姿态欧拉角的 变化曲线。 该图中， 当系统输入正抖动脉冲时， 载体横滚角、 俯仰角及航向角都可以收敛到 输入抖动脉冲之前的姿态； 当系统输入负抖动脉冲时， 三个姿态角同样可以收敛到输入抖 动脉冲之前的姿态。 因此该自适应扩展卡尔曼滤波算法具有良好的动态性能和抗干扰能 力。 0170 本发明将目前已有的算法与自适应扩展卡尔曼滤波算法进行了比较， 如图4所示。 该图(a)、 (b)中， 自适应扩展卡尔曼滤波算法、 互补滤波算法与扩展卡尔曼滤波算法的俯仰 角及横滚角输出波形图变化幅度较小， 且比较平直。 由于自适应扩展卡尔曼滤波与扩展卡 尔曼滤波都是通重力加速度信息对系统俯仰角及横滚角进行修正， 。

26、因此两者的俯仰角及横 滚角变化曲线重合。 相较于基于四元数的互补滤波算法， 自适应扩展卡尔曼滤波算法与扩 展卡尔曼滤波算法中包含的噪声较小。 图(c)中， 互补滤波器的航向角变化曲线在5 10 范围内变化， 精度大约为4 。 扩展卡尔曼滤波算法的航向角变化曲线随着时间的增加逐渐 偏离初始航向角， 具有较大的累积误差。 相较于上述两种算法， 自适应扩展卡尔曼滤波算法 的航向角变约为0.5 。 综上所述， 本发明提出的自适应卡尔曼滤波算法有效解决了载体姿 态的累积误差较大的问题， 同时提高了姿态解算精度。 0171 尽管上面已经示出和描述了本发明的实施例， 可以理解的是， 上述实施例是示例 性的， 不能理解为对本发明的限制， 本领域的普通技术人员在不脱离本发明的原理和宗旨 的情况下在本发明的范围内可以对上述实施例进行变化、 修改、 替换和变型。 说明书 9/9 页 12 CN 110174121 A 12 图1 图2 说明书附图 1/2 页 13 CN 110174121 A 13 图3 图4 说明书附图 2/2 页 14 CN 110174121 A 14 。