基于MTI的组合导航算法研究
- 格式：pdf
- 大小：251.75 KB
- 文档页数：4
第３Ｏ卷第４期 ２０１０年８月 弹箭与制导学报
Ｊｏｕｒｎａｌ ｏｆ Ｐｒｏｊｅｃｔｉｌｅｓ．Ｒｏｃｋｅｔｓ，Ｍｉｓｓｉｌｅｓ ａｎｄ Ｇｕｉｄａｎｃｅ ＶｏＩ＿３Ｏ ＮＯ．４
Ａｕｇ ２０１０
基于ＭＴＩ的组合导航算法研究 姬振龙，钱峰 （上海交通大学仪器科学与工程系，上海 ２００２４０） 摘要：推导了磁阻／ＤＲ的组合方法，利用加速度计与磁强计测量的载体姿态角适时的对陀螺所测的载体姿 态角进行修正，抑制了陀螺所测量载体姿态角误差的迅速扩大。探讨了ＧＰＳ／ＳＩＮＳ组合导航系统中抗差自校 正卡尔曼滤波的应用，并对此进行了仿真。最后的仿真结果表明：抗差自校正卡尔曼滤波效果优于标准卡尔 曼滤波效果。 关键词：磁阻／ＤＲ；ＧＰＳ／ＳＩＮＳ；卡尔曼滤波；抗差自校正 中图分类号：Ｖ２４１．６２２ 文献标志码：Ａ
Ａｌｇｏｒｉｔｈｍ ｆｏｒ ＭＴＩ—ｂａｓｅｄ Ｉｎｔｅｇｒａｔｅｄ Ｎａｖｉｇａｔｉｏｎ ＪＩ Ｚｈｅｎｌｏｎｇ，ＱＩＡＮ Ｆｅｎｇ （Ｄｅｐａｒｔｍｅｎｔ ｏｆ Ｉｎｓｔｒｕｍｅｎｔ Ｓｃｉｅｎｃｅ Ｅｎｇｉｎｅｅｒｉｎｇ，Ｓｈａｎｇｈａｉ Ｊｉａｏｔｏｎｇ Ｕｎｉｖｅｒｓｉｔｙ，Ｓｈａｎｇｈａｉ ２００２４０，Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ａ ｋｉｎｄ ｏｆ ＭＲ／ＤＲ ｎａｖｉｇａｔｉｏｎ ｍｏｄｅ ｗａｓ ｄｅｄｕｃｅｄ．Ａｎ ａｃｃｅｌｅｒｏｍｅｔｅｒ ａｎｄ ａ ｍａｇｎｅｔｏｍｅｔｅｒ ｗｅｒｅ ｕｓｅｄ ｆｏｒ ｃａｌｃｕｌａｔｉｎｇ ｔｈｅ ｃａｒ— ｒｉｅｒ’Ｓ ａｔｔｉｔｕｄｅ ａｎｇｌｅ ｆｏｒ ｃｏｒｒｅｃｔｉｎｇ ｔｈｅ ｃａｒｒｉｅｒ’Ｓ ａｔｔｉｔｕｄｅ ａｎｇｌｅ ｃａｌｃｕｌａｔｅｄ ｂｙ ｔｈｅ ｇｙｒｏ．Ｔｈｅ ａｐｐｌｉｃａｔｉｏｎ ｏｆ ｒｏｂｕｓｔ ｓｅｌｆ－ｔｕｎｉｎｇ Ｋａｌｍａｎ ｆｉｌｔｅｒ ｉｎ ＧＰＳ／ＳＩＮＳ ｓｙｓｔｅｍ ｗａｓ ｄｉｓｃｕｓｓｅｄ．Ｔｈｅ ｓｉｍｕｌａｔｉｏｎ ｒｅｓｕｌｔｓ ｓｈｏｗ ｔｈａｔ ｔｈｅ ｒｏｂｕｓｔ ｓｅｌｆ－ｔｕｎｉｎｇ Ｋａｌｍａｎ ｆｉｌｔｅｒ’Ｓ ｆｉｌｔｅｒｉｎｇ ｅｆｆｅｃｔ ｉｓ ｓｕｐｅｒｉｏｒ ｔＯ ｔｈａｔ ｏｆ ｔｈｅ ｓｔａｎｄａｒｄ Ｋａｌｍａｎ ｆｉｌｔｅｒ． Ｋｅｙｗｏｒｄｓ：ＭＲ／ＤＲ；ＧＰＳ／ＳＩＮＳ；Ｋａｌｍａｎ ｆｉｌｔｅｒ；ｒｏｂｕｓｔ ｓｅｌｆ－ｔｕｎｉｎｇ
０ 引言 常规的航位推算（ＤＲ）系统用陀螺来测量载体的 姿态角，其具有稳定和短期精度高的优点，但陀螺所 测量的载体姿态角误差会随时间快速累积。加速度 计与磁强计也可以用来测量载体的姿态，且其测量误 差不随时间累积，但当载体加速度较大时，这种方法 则无法使用。如何将二者组合在一起来提高ＤＲ精 度是文中探讨的一个重点。同时为了弥补标准卡尔 曼滤波在ＧＰＳ／ＳＩＮＳ组合导航系统中应用的不足，文 中分析并推导了抗差自校正卡尔曼滤波并以其与标 准卡尔曼滤波进行了对比。 文中是基于ＭＴＩ进行研究的，ＭＴＩ是一个集成 有ＧＰＳ芯片、陀螺、加速度计等相关元器件的微航姿 参考系统。 １磁阻／ＤＲ组合导航算法 １．１载体姿态的陀螺测量方法 载体姿态角包括俯仰角 、横滚角ｙ和航向角 ， 载体姿态的计算就是确定这三个角的过程。用陀螺对 载体姿态进行更新较为常用的方法是四元数法。 假设ｑ—ｑ０＋ｑ。ｉ＋ｑｚＪ＋ｑ。ｋ为ｎ系至ｂ系的旋 转四元数，ｒｎ和ｒｂ为同一矢量ｒ分别在ｎ系和ｂ系下 的表示，则，ｒＩ和ｒｂ之间的变换关系可采用四元数乘法 来表示： ｒｎ—ｑ ｏ ｒｂ 口 （１） 式中： 为四元数乘法，口 为ｑ的共轭四元数。根据 四元数乘法法则，姿态矩阵可以用四元数表示为：
厂ｑ：＋ｑ｝一ｇ；一ｑ； ２（ｑ１ｑ２一ｑｏｑ。） —ｌ ２（ｑ１ｑ２＋ｑ。ｑ ） ｑ３一ｑ｝＋ｑ；一ｑ； ｌ ２（ｑ１吼一ｑｏｑ２） ２（ｑ２吼＋ｑ。ｑ１）
＊收稿日期：２００９—０９—０２ 作者简介：姬振龙（１９８５一）。男，河南开封人，硕士研究生，研究方向：组合导航系统。
（２） ・ １２ ・ 弹箭与制导学报 第３Ｏ卷 一 １ ｑ ∞ ｂ （３）
其中∞ｂｂ为载体相对于导航坐标系的转动角速度在载 下： 体坐标系下的投影。
运用ＩＮ阶龙格一库塔法来求解上述微分方程，其 中ｈ为更新步长： Ｋｌ一 ｈ ｙ』Ｌ∞ ｂ
ｂ（￡）］・口（ ）
Ｋ２一 ｈ ｃｃ， ｂ（￡＋ ）］・Ｅｑ（ ）＋等］
一 Ｍ［ ｂ （￡十 ）］・Ｅｑ（￡）＋Ｋ ２］
Ｋ４一 Ｍ［ ｂｂ（￡＋ ）］・Ｅｑ（ｔ）＋Ｋ３－１ ｑ（ｔ＋ ）一ｇ（￡）＋百ｈ（Ｋ１＋２Ｋ２＋２Ｋ３＋Ｋ４）
ＭＴＩ采用的为北西天坐标系，由导航系至载体系 的坐标轴旋转顺序为：Ｚ—ｙ—Ｘ，其姿态角可通过姿态 矩阵来求取：
ｆ ｙ—ａｒｃｔａｎ（ ）［一１８０￣，１８０￣］ ｊ ＝一ａｒｃｓｉｎ（ａ（３，１）） ［一９０。，９０。］ （５） ａｒｅｔａｎ（暮 ）［＿１８ ｌ８０。］
１．２ 载体姿态的加速度计／磁阻测量方法 基于大地磁场和重力磁场，利用加速度计和磁强 计便可以测量载体的姿态角。ＭＴＩ中加速度计的输出 是加速度在载体坐标系下的投影。当载体处于非加速 状态时，在导航坐标系下，加速度计的输出应该为户 一［Ｏ ０ ｇ］ ，即： ，“一 ｊ－ （６） 因为航向角的大小对加速度计的输出没有影响， 所以可以令 一０，则：
㈩
一ａｒｃｓ ｙ—ａｒｃｓｉｎ ｃｇ ＣＯＳ ｇ
在知道俯仰角和横滚角的情况下，利用磁强计的 输出便可以计算出航向角。ＭＴＩ中磁强计的输出是地 球磁感应强度在载体坐标系下的投影。记磁强计的输 出为： Ｍｂ一［ Ｍ Ｍ ］ 利用姿态矩阵将磁强计的输出转至导航坐标系 Ｍｎ— Ｍｂ （８） 则可得地磁场在水平面的分量： ｒ 一Ｍ￣ｃｏｓ ０＋Ｍ￣ｙｓｉｎ ０ｓｉｎ），＋Ｍ￣ｓｉｎ ０ｃｏｓ ｙ １ 一Ｍｙｂｃｏｓ ｙ一／Ｗ２ｓｉｎ ｙ （９） 根据磁强计原理，则航向角为： 一ａｒｃｔａｎ（ ） （１０） 但由于上式算出的只是载体纵轴相对于磁北的 夹角，因此真正的航向角还要由上式再加上一个磁偏 角。不同地方的磁偏角有所不同，其具体值可以通过 查表得到。 １．３ 磁阻／ＤＲ的组合导航方法 由上述可知，用陀螺测量姿态角其误差会随时间 迅速累积。而用加速度计和磁强计测量载体姿态角， 其误差虽不会随时间累积，但当载体有较大加速度 时，测量误差较大。针对两种测量方法的不同特点，文 中初步探讨了将二者组合在一起进行ＤＲ推算的方 法。 假设ｚ （￡）一Ｅ０ （ ） ），ｇ（￡） （￡）］为陀螺所测 量的一组姿态角，ｚａ（￡）一［ （￡） ｙ （￡） （ ）］为 加速度计和磁阻所测量的一组姿态角。则任一时刻 ｔ 、载体的姿态角Ｚ（ｔ ）可按以下方案决定： １）首先根据陀螺测姿短期精度高的优点，在初始 的一段时间内载体的姿态可完全由陀螺的计算值决 定，即：当０≤ｔ ≤Ｔ时，Ｚ（ｔ ）一Ｚｇ（ ），其中丁值应 根据所使用陀螺的精度来确定。 ２）当ｔ ＞丁时，若加速度计的输出满足下面的条 件，即：Ｉ￣／尼＋危＋ ：一ｇ Ｉ＜ｅ时，令Ｚ（ｔｔ）一 ｚａ（ ），同时用加速度计和磁阻所测量的姿态值对陀 螺所测的姿态值进行更新，即令 （￡ ）一Ｚｏ（ ），使 陀螺以此值为依据进行下面的推算。若上述条件不满 足时，则载体姿态仍以陀螺所测姿态为准。其中ｅ为一 小值，其值应根据加速度计的精度来确定。 行驶在公路上的载体大部分时间内加速度都比 较小，此时用加速度计和磁阻所测量的姿态值对陀螺 所测的姿态值进行校正可有效抑制陀螺测姿误差的 发散。 第４期 姬振龙等：基于ＭＴＩ的组合导航算法研究 ・ １３ ・ ２ ＧＰＳ／ＳＩＮＳ组合导航算法 ２．１ 卡尔曼滤波模型 卡尔曼滤波是ＧＰＳ／ＳＩＮＳ组合导航系统中一种 比较常用并且有效的组合导航算法。 由于计算机的计算是在离散时间点上进行的，所 以卡尔曼滤波多采用离散的形式，以下便是离散化的 标准卡尔曼滤波［２ 方程： Ｉ —ｌ一 。卜１ ＝ ｌ卜。＋Ｋ，Ｅｚ，一 Ｉ卜。］
Ｋｋ＝Ｐｋｌ 一ｌ ［Ｈ女Ｐ川卜１Ｈ２’＋Ｒ ］ （１１） Ｐ Ｉ卜ｌ一 ｔ，卜１ Ｐ卜ｌ 王卜１＋Ｑ卜１
Ｐ ＝［Ｉ一 Ｈ１］ ｌ卜１ 由于卡尔曼滤波器是高斯过程最优滤波的一种 有效算法，当对象模型足够准确且系统状态和参数不 发生突变时，性能才比较好。当模型存在误差时，这种 增长记忆滤波使过老的测量数据对现时的状态估计 产生不良影响，甚至可能发散，从而影响滤波器动态 性能。为解决这一问题，可以引人调节因子 卜 ，式 （１１）中一步预测协方差的求解可变为： ＰｋｌＩ—ｌ＝ 妒 ，卜１ Ｐ 一１ 卜１＋Ｑ卜１ （１２）
其中Ｓ 为渐消因子，且满足Ｓ ≥１，其可由下式求解： Ｓ女一ｍａｘ｛ｌ，ｔｒ［Ｎｋ］／ｔｒ［Ｍｋ］） （１３） 式中，ｔｒＥ・］是矩阵迹的符号。 Ｍｋ—Ｈ ，卜 Ｐ｝－ 卜 Ｈ （１４） Ｉ Ｎ —Ｃｏ 一 Ｒ Ｈ ２＿一Ｑ
ｆ 盟，ｋ＞１ ｃｏＣｏ —ｊ卜 — 一 （１５）
Ｉ寺ｙ０Ｖｏ￣， ｋ＝ Ｌ厶 式中Ｖｔ—Ｚｋ—Ｈ 女Ｉ卜１。
使用 后，滤波器的性能得到了明显的改善，但 是其动态性能仍不理想，当状态发生突变时， 尽管 大于１，相对状态变化的实际幅度仍然太小，不能有效 的调节状态突变引起的偏差。在此基础上可以做一些 适当的改进，当状态预报值与滤波值相差较大时，可 以做以下改动： Ｐ々『卜ｌ一口Ｊ）女ｌ卜１ （１６） ｆ １ ＡＸｔ≤Ｃ ｛／ｘＸｋ＞Ｃ ７ ｌ ，、 ／ 【
式中Ｃ为一常数，取１．０～３．５。
ＡＸ 一ｌＪ 一 。卜 ｌＩ／￣／石 （１８） 从上式可以看出，当滤波预报值与定位结果相差 较大时，就增加当前观测值在滤波过程中的作用，从 而修正了预报值的影响。有效改善了系统的性能。 ２．２ ＧＰＳ／ＳＩＮＳ系统滤波方程建立 取系统状态变量： Ｘ一［ 劬Ｅ跏Ｎ曲ｕ乩姒８ｈ ｅｈ Ｅ ｂ ￡ ｅ ￡ ｅ Ｖ Ｖ：］ ，其元素依次为３个姿态 角误差分量、３个速度误差分量、３个位置误差分量、３ 个陀螺漂移误差分量、３个陀螺一阶马尔可夫误差分 量和３个加速度误差分量。 噪声变量： Ｗ：Ｅｗｇｚ Ｗｇ Ｗｇ：Ｗｋ Ｗｂ Ｗｂ：Ｗ 叫ａ ７．０ ］ 其元素依次为３个陀螺漂移白噪声误差分量、３个陀 螺一阶马尔可夫白噪声误差分量和３个加速度计白 噪声误差分量。 则系统的状态方程： （ ）一Ｆ（ ）Ｘ（ ）＋Ｇ（￡）Ｗ（ ） （１９） 其中： Ｅ｛ｗ（￡））一０，Ｅ｛Ｗ（￡）（Ｗ（ｒ）） ）一Ｑ（ｔ）８（ｔ—ｒ） 设ＧＰＳ与ＳＩＮＳ输出的位置信息为： Ｉ＇ＬＧ＝Ｌ＋轧Ｇ ｒＬＩ—Ｌ＋ｄＬｌ Ｇ＝ ＋积Ｇ 与 ｌ— ＋毅ｌ ｌ ｈＧ＝ｈ＋ｄｈＧ Ｉ ｈＴ—ｈ＋ ｌ 其中：Ｌ、 、ｈ为真实的经纬度与高度，乩 、积ｃ、虢 与 乩 、积，、 分别为ＧＰＳ、ＳＩＮＳ的经纬度与高度误差。 以二者的差值作观测量，可列观测方程： Ｇ一乩１］ ｚ—ｆ淑Ｇ一毅Ｉ ｆ＝ＨＸ＋Ｖ （２０） ｌ矾Ｇ一虢１ Ｉ 其中，ｙ为ＧＰＳ的位置观测噪声且其满足： ＥＷ（ｔ））：＝＝０，Ｅ｛ｙ（ ）（Ｖ（ｒ）） ）＝Ｒ（￡） （ｔ—ｆ）
