捷联惯导系统航姿算法的比较及仿真分析

ｌ ｇ０２＋ｑ 一ｑ；一ｑ； ２（ｑ１ｇ２一ｇｏｇ３） ２（ｇＩｑ２＋ｑｏｑ３）Ｉ ｃ ＝Ｉ ２（ｇ。ｑ２＋ｑｏｑ３） ｇ 一ｑ ＋ｇ；一ｇ； ２（ｇ１ｑ２一ｑｏｑ３）ｌ
ｌ ２（ｇｌｇ３一ｇｏｇ２） ２（ｇ２ｇ３＋ｇ０ｇ１） ｇ —ｇ 一ｇ；＋ｇ Ｉ
（２）
利 用式 （２）便 可 以从 由载 体 坐标 系 转 移 至导 航 地 理 坐 标 系 的姿 态 转 移 矩 阵 中提 取 实 时姿 态 角 供导 航 系统使 用 。 １．２ 四阶龙 格一 库塔 法 求解 四 元数微 分 方程
Ｑ（ｔ＋ ）＝Ｑ（ｆ）＋（ ＋ｋ２＋ｋ３十 ）／６ ｋｌ＝０．５ｈ［ｃｏ（ｔ）］Ｑ（ｔ、 ｋ２＝０．５ｈ［ａ￣（ｔ＋ｈ／２）］［Ｑ（ｆ）＋ｋｉ／２］ （４） ｋ３＝０．５ｈ［ｃｏ（ｔ＋ｈ／２）］［Ｑ（ｆ）＋ｋ２／２］ ｋ４＝０．５ｈ［ｃｏ（ｔ＋ ）儿Ｑ（ ）＋ｋ３］
收稿 日期：２０１６．０１．０５。王小峰 （１９８３一 ）：陕西人，工程 师 ，工 学硕 士，研究方向：惯性及卫星导航 。
矢 量 三子样 法 进行 仿真 分析 比较 ，在典 型 圆锥运 动
下 ，四元 数 算法利 用 四阶 龙格 一库 塔法 对 四元数 微
分 方程 进 行姿 态求 解 ，而 旋转 矢量 三子 样算 法主 要
周 期一 般 都很短 ， 很 小 ， 的高 次项可 略 去不计 ， 得 工程 上 常用 的近 似方 程 ：
＝∞＋二２ ×∞＋ ×（ ×∞） （６）
四元数 微分 方程 为 ：
１
．
ａ＝去Ｑ
（３）
其 中， 表示 由机体 坐 标 系到 导 航坐 标 系 在机 体 坐 标 系 中 的投 影 。式 （３） 的求 解 可 以通 过 用 四阶 龙 格一 库塔 法来 求 取如 下 的 四元 数 递推 公式 ：
·１０４·
现 代 导 航
２０１６年
１．１ 四元数 与姿 态矩 阵 间的 关系
四元数 Ｑ＝ｃ。ｓ ＋ｕｓｉｎ旦描 述 了刚 体 绕 固 定 ２
点 的转动 形态 ，可看成 刚 体绕 瞬轴 Ｕ （单 位 向量 ） 转过 角一 次 完成 的旋 转 。四元 数与 姿态 矩 阵的关 系可 以表 示 如下 ：
Ｋｅｙ ｗｏｒｄｓ： Ａｔｉｔｕｄｅ Ａｌｇｏｒｉｔｈｍｓ；Ｒｏｔａｔｉｏｎ Ｖｅｃｔｏｒ；Ｑｕａｔｅｍｉｏｎ；Ｃｏｎｉｎｇ Ｍｏｖｅｍｅｎｔ；Ｓｉｍｕｌａｔｉｏｎ
０ 引言
在 常 规 的捷联 惯 性导航 系 统 中 ，陀螺 仪和 加速 度 计 是 固定连 接在 运 动载体 上 的 ，用 于直 接感 知测 量 运 动载体 的角运 动和 线运 动 。通 过姿 态 转移 矩 阵 把 惯 性 元件 加 速 度 计 输 出 的 比力 信 息 转 换 到 导 航 地 理坐 标系 中，对 其进 行 两次 积分 运算 便 可得 到地 理 坐 标 系 下 的位 置 速度 等 导 航 信 息 ，实 现 实 时 导 航 。因此 姿 态转 移矩 阵 的计算 就 显得 尤为 重要 。本 文 主 要 对 目前 常 用 航 姿 算法 中 的 四元 数 法 和 旋 转
仿真 了上述算法。仿真结果表明，四元数算法在精度上不如旋转矢量三予样 法，但在 陀螺角速率
输入模 式下比三子样法 占有优势且计算量小，旋转矢量三子样法可以抑制不可 交换误差，适于高
机动 高精度的导航要求，但也存在计算量大以及在 陀螺角速率输入模 式下存在明显的噪声放大效
应 。
关键词 ：航姿 算法；旋转矢量；四元数；圆锥运动；仿真
中图分类号 ：Ｖ２４９
文献标识码 ：Ａ
文章编号 ：１６７４．７９７６．（２０１６）０４—１０３—０４
Ｃｏｍ ｐａｒｉｓｏｎ ａｎｄ Ｓｉｍｕｌａｔｉｏｎ Ａｎａｌｙｓｉｓ ｆｏｒ Ａｔｔｉｔｕｄｅ Ａ ｌｇｏｒｉｔｈｍ ｓ ｏｆ ＳＩＮ Ｓ
Ａｂｓｔｒａｃｔ： Ｃｏｍｂｉｎｉｎｇ ｗｉｔｈ ｅｎｇｉｎｅｅｒｉｎｇ ａｐｐｌｉｃａｔｉｏｎ，ｔｈｅ ｑｕａｔｅｒｎｉｏｎ ａｎｄ ｒｏｔａｔｉｏｎ ｖｅｃｔｏｒ ａｔｔｉｔｕｄｅ ａｌｇｏｒｉｔｈｍｓ ｗｈｉｃｈ ａｒｅ ｍ ａｉｎｓｔｒｅａｍ ａｌｇｏｒｉｔｈｍｓ ａｒｅ ｉｎｔｒｏｄｕｃｅｄ，ａｎ ｄ ａ ｎｅｗ ｍｅｔｈｏｄ ｏｆ ｆｉ ｎｄｉ ｎｇ ａ ｓｏｌｕｔｉｏｎ ｔｏ ｑｕａｔｅｍ ｉｏｎ ａｎｄ ｔｈｒｅｅ ｓｕｂｓａｍｐｌｅ ｒｏｔａｔｉｏｎ ｖｅｃｔｏｒ ｉｎ ｔｙｐｉｃａｌ ｃｏｎｉｎｇ ｍ ｏｖｅｍｅｎｔ ｉｓ ｐｒｏｐｏｓｅｄ ａｎ ｄ ａｎ ａｌｙｚｅｄ ｔｈｒｏｕｇｈ ｔ ｈｅ ｓｉｍｕｌａｔｉｏｎ．Ｔｈｅ ｒｅｓｕｌｔｓ ｓｈｏｗ ｔｈａｔ ｑｕａｔｅｒｎｉｏｎ ｈａｓ ｓｍａｌｌｅｒ ａ ｍ ｏｕｎｔ ｏｆ ｃｏｍｐｕｍｔｉｏｎｓ ｉｎ ｔｈｅ ｍｏｄｅ ｏｆ ｇｙｒｏ ｖｅｌｏｃｉｔｙ ｉｎｐｕｔ，ｂｕｔ ｉｔｓ ｐｒｅｃｉｓｉｏｎ ｉｓ ｎｏｔ ｂｅｔｅｒ ｔ ｈａ ｎ ｒｏｔａｔｉｏｎ ｖｅｃｔｏｒ ａｔｉｔ ｕ ｄｅ ａｌｇｏｒｉｔｈｍｓ．Ｍ ｅａ ｎ ｗｈｉｌｅ， ｒｏｔａｔｉｏｎ ｖｅｃｔｏｒ ａｔｉｔｕｄｅ ａｌｇｏｒｉｔｈｍｓ ｍ ｅｅｔ ｒｅｑｕｉｒｅｍｅｎｔｓ ｏｆ ｈｉｇｈ－ｄｙｎａｍｉｃ ａｎｄ ｈｉｇｈ－ｐｒｅｃｉｓｉｏｎ，ｂｕｔ ｉｔｓ ｎｏｉｓｅ ａｍｐｌｉｆ ｉｃａｔｉｏｎ ｉｓ ｍｏｒｅ ｏｂｖｉｏｕｓ．
是 通 过 对 有 限制 转 动 的 不 可 交 换 性 误 差 进 行 补 偿
来 进行 姿态 更 新和解 算 。
’
１四元数 算法
四元数 是是 由四个元 素 构成 的数 ： Ｑ（ｑ０，ｑ１，ｑ２，ｑ３）＝ｑ０＋ｑｌｉ＋ｇ２ｊ＋ｑ３ｋ （１）
其中，ｑｏ，ｑ。，ｑ：，ｑ，是实数，ｉ，ｊ，ｋ是互相正交的单位 向量 ，用 来表 示 四元 数 的乘法 运 算关 系 。
２０１６年 ４月第导系统航姿算 法的比较及仿真分析
王小峰 ，董正芳
（中国 电子 科技集团公司第二十研究所 ，西安 ７１００６８）
摘 要 ：结合 工程 实际应用，介绍了当前主流航姿算法中的四元数法和旋转 矢量三子样 算法，
提 出了一种在典型圆锥运动下求解四元数和旋转矢量三子样 的新方法，并且详 细地分析 、比较和