捷联系统四元数姿态解算的精细积分法
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
明相 同条件下 它是进一步 减小姿 态解算误 差的有效方 法 ， 为捷联式惯性导航技术 的工程实践提供参考 。
载体 的姿态 和航 向体现 了载 体坐 标 系与导航 坐 标系
之 间的方位关系 ， 确定 ２个 坐标 系之间 的方位关 系需要借
１ 四元数微分方程及其龙格库塔解法
１ １ 四 元数 姿态 微 分 方 程 ．
得到 了广泛 的研 究 。而且 四元数 法 的正确 性和有 效 性 已
假 定矢 量 ｒ 绕通过定点 Ｏ某一轴转动了一个角度 ０如 ，
转 动后 的四元数用 ｒ 表示 ， 用 四元 数表示 ｒ ｒ 的关 则 间 和
系 如 下 ｒ＝９ ｒ Ｑ
Leabharlann Baidu
被相关专家证明 ， 四元 数法 由于其计 算无 奇点 ， 计算 量
１４ ０
四 川 兵 工 学 报
用解析公式 ， 因此它 可 以大大 提高计 算精 度 ， 数值 结果 其
可 以比拟于精 确解 的数值 结果 。该 方法 自提 出 以来 在力
其中： ＝ ｏ ； Ｃ 下 ｉ 。 Ｑ ｑ＋ Ｏ ０＋ ｓ Ｓ ０
四元数 的实 时值 可通过 求解 四元数 姿态 微分方 程而
Ｑ ＝ｑ ０＋ｑ：＋ｇ ＋ｑ ｌ ３
弦虽然 消除 了欧拉方程 的奇异性 ，但为了减小正交误差 不 得不 以计算 时间 为代价 。随着 飞行 器导航 控制 系 统 的迅 速发展和数字计算机在运 动控制 中的应用 ， 控制 系统要 求 导航计算环节更加合理地描述 载体 的空间运 动 ， 四元 数法
肇 收稿 日期 ：０ ０— ２—１ ２１ ０ ０ 基金项 目： 国家 自然科学基金资助项 目（０ ３ ０５ ６ ８５ ０ ） 作者简介 ： 项凤涛 （９ ６ ） 男 ， １８ 一 ， 硕士研究生 ， 主要从 事组合导航研究 ； 王正志 （９ ５ ） 男 ， １４ 一 ， 教授 ， 博士生导师 ， 主要从事 自动控制 ， 模式识别 与图像处理和生物信息学研究 。
抗干扰能力 强 ， 目前 已广 泛应 用 于航 空 航 天 、 海 、 器 航 机
人、 智能交通 、 无人 飞机 和精 确制 导炸 弹等 飞行器 的 导航 控制当 中。实时高 精度 的姿 态解算 是捷 联式 惯性 导航 系 统的关键技术 ， 它在很大程度上 影响着捷 联惯性 导航 系统 的导航 精 度 ， 姿态 解算 主 要指 的就 是 姿态变 换 矩 阵 的求 解， 通过姿态变换矩阵可以得到 载体 的姿 态和导航 参数计
四元数姿态微 分 方程 的一 种有效 手段 ， 精度 较高 ， 运 其 但
捷联惯性 导航控 制 系统 （ Ｉ Ｓ 是 目前 自主导航 控制 ＳＮ ）
领域研究热点之一 ， 该方法 将陀螺仪 和加 速度计直 接安装
在载体上 ， 掉机 电式 导航平 台 ， 省 利用 导航 计算 机软 件建 立一个 “ 数学 平 台” 来代 替机 电平 台实体 。由于其结 构简 单， 成本低廉 ， 易于实现惯 性测量单 元 （ＭＵ） Ｉ 的小 型化 ， 且
助矩阵法和力 学 中的刚 体定 点旋 转 的理 论。通 过矩 阵法 推导方 向余弦表 ， 刚体 定点 运动 旋转 的理 论表 明 ， 而 定点 运动刚体的任何有 限位 移都 可 以用绕 过定点 的某 一轴 的
一
四元数的数学概念是 １４ ８ ３年由哈密 顿首先提出 的 ， 它 是把三维空间和 １ 四维空 间联 系起来 ， 四维 空间 的性 个 用 质和运算规则来研究三维空间中的刚体定点转动问题 。 设坐标系 ， 为空 间中的一个 惯性参 考 坐标 系 ， 机体 坐
次转动来 实现。 目前 ， 主要 的研究 方法有 ： 欧拉 法 、 向 方
余弦法与四元数 法。已有 大量科 研文献 和工程 实践表 明 ， 四元数 法相对 于其他 两种 方法 的优越性 ， 比如 在大姿态 角 情况下 ， 欧拉方程存 在奇异点 ， 又称 作平衡环 锁定 ； 向余 方
标 系 曰为空 间中的一个运动坐标 系 ， 假设机体 坐标系 曰到 惯性 坐标 系 ， 的姿态变换 四元数为 ‘
算需要 的数据 ， 是捷联式惯导算法中的重要工作 。
算量很大 ， 并且对数据光 滑性要求 较高 。旋转 矢量算 法是 基于 １７ ９ １年 Ｂ ｒ 提 出的旋转矢量理论及旋转矢量微分方 ｏｚ ｔ
程， 该算法能够对 圆锥误差进行 有效补偿 。本 文 中把精
细积分法应用 于四元数微 分方程 的求解 ， 数值仿 真结果表
到有效抑制 ， 以为捷联式惯性导航技术的工程实践提供参考 。 可 关键词 ： 捷联惯导系统 ； 四元数 姿态微分方程 ； 精细积分 ； 龙格库塔法
中图分类号 ：Ｐ ０ Ｔ３ １
文献标识码 ｉ Ａ
文章编号 ：０６— ７ ７ ２ １ ）５一 １３一 ４ １０ ００ （０ ０ ０ ｏ Ｏ ｏ 小， 所以常用于飞行器实时解算 。 四元数 的另外一个 优点 是便于采用卡尔曼滤波实现 四元数最 优估计 ， 也正是 基于 这个原 因，四元 数广 泛应用 于航 天器 的姿态 控制系统 。近 年来 ， 出现了很 多有 效求 解 四元数 微分 方程 的方 法 ， 例如 龙格库塔法 、 旋转矢量算 法等 。四阶龙 格库塔算 法是计 算
第３ 卷 １
第 ５期
四 川 兵 工 学 报
２１ ０ ０年 ５月
【 其他研究】
捷 联 系统 四元数 姿态 解 算 的精 细 积 分 法
项凤 涛 ， 王正志 ， 吴第曼 ， 岳
（ 国防科学技术大学 机 电工程与 自动化学院 ， 长沙
达
４０ ７ ） １０３
摘要 ： 了基 于四元数 的相关理论 ， 研究 把精细积分方法应用于求解四元数姿态微分方 程当 中， 以典 型圆锥 运动 并 作为输入条件对该算法进行 了仿真分析。相同仿 真步长条件下的仿 真结果表 明， 该方法在 高动态角运动 环境下 的解算 精度要优 于四阶龙格库塔算 法 ， 由圆锥运动引起 的俯仰角算法漂移误差相对 于于 四阶龙 格库塔方法也 得
载体 的姿态 和航 向体现 了载 体坐 标 系与导航 坐 标系
之 间的方位关系 ， 确定 ２个 坐标 系之间 的方位关 系需要借
１ 四元数微分方程及其龙格库塔解法
１ １ 四 元数 姿态 微 分 方 程 ．
得到 了广泛 的研 究 。而且 四元数 法 的正确 性和有 效 性 已
假 定矢 量 ｒ 绕通过定点 Ｏ某一轴转动了一个角度 ０如 ，
转 动后 的四元数用 ｒ 表示 ， 用 四元 数表示 ｒ ｒ 的关 则 间 和
系 如 下 ｒ＝９ ｒ Ｑ
Leabharlann Baidu
被相关专家证明 ， 四元 数法 由于其计 算无 奇点 ， 计算 量
１４ ０
四 川 兵 工 学 报
用解析公式 ， 因此它 可 以大大 提高计 算精 度 ， 数值 结果 其
可 以比拟于精 确解 的数值 结果 。该 方法 自提 出 以来 在力
其中： ＝ ｏ ； Ｃ 下 ｉ 。 Ｑ ｑ＋ Ｏ ０＋ ｓ Ｓ ０
四元数 的实 时值 可通过 求解 四元数 姿态 微分方 程而
Ｑ ＝ｑ ０＋ｑ：＋ｇ ＋ｑ ｌ ３
弦虽然 消除 了欧拉方程 的奇异性 ，但为了减小正交误差 不 得不 以计算 时间 为代价 。随着 飞行 器导航 控制 系 统 的迅 速发展和数字计算机在运 动控制 中的应用 ， 控制 系统要 求 导航计算环节更加合理地描述 载体 的空间运 动 ， 四元 数法
肇 收稿 日期 ：０ ０— ２—１ ２１ ０ ０ 基金项 目： 国家 自然科学基金资助项 目（０ ３ ０５ ６ ８５ ０ ） 作者简介 ： 项凤涛 （９ ６ ） 男 ， １８ 一 ， 硕士研究生 ， 主要从 事组合导航研究 ； 王正志 （９ ５ ） 男 ， １４ 一 ， 教授 ， 博士生导师 ， 主要从事 自动控制 ， 模式识别 与图像处理和生物信息学研究 。
抗干扰能力 强 ， 目前 已广 泛应 用 于航 空 航 天 、 海 、 器 航 机
人、 智能交通 、 无人 飞机 和精 确制 导炸 弹等 飞行器 的 导航 控制当 中。实时高 精度 的姿 态解算 是捷 联式 惯性 导航 系 统的关键技术 ， 它在很大程度上 影响着捷 联惯性 导航 系统 的导航 精 度 ， 姿态 解算 主 要指 的就 是 姿态变 换 矩 阵 的求 解， 通过姿态变换矩阵可以得到 载体 的姿 态和导航 参数计
四元数姿态微 分 方程 的一 种有效 手段 ， 精度 较高 ， 运 其 但
捷联惯性 导航控 制 系统 （ Ｉ Ｓ 是 目前 自主导航 控制 ＳＮ ）
领域研究热点之一 ， 该方法 将陀螺仪 和加 速度计直 接安装
在载体上 ， 掉机 电式 导航平 台 ， 省 利用 导航 计算 机软 件建 立一个 “ 数学 平 台” 来代 替机 电平 台实体 。由于其结 构简 单， 成本低廉 ， 易于实现惯 性测量单 元 （ＭＵ） Ｉ 的小 型化 ， 且
助矩阵法和力 学 中的刚 体定 点旋 转 的理 论。通 过矩 阵法 推导方 向余弦表 ， 刚体 定点 运动 旋转 的理 论表 明 ， 而 定点 运动刚体的任何有 限位 移都 可 以用绕 过定点 的某 一轴 的
一
四元数的数学概念是 １４ ８ ３年由哈密 顿首先提出 的 ， 它 是把三维空间和 １ 四维空 间联 系起来 ， 四维 空间 的性 个 用 质和运算规则来研究三维空间中的刚体定点转动问题 。 设坐标系 ， 为空 间中的一个 惯性参 考 坐标 系 ， 机体 坐
次转动来 实现。 目前 ， 主要 的研究 方法有 ： 欧拉 法 、 向 方
余弦法与四元数 法。已有 大量科 研文献 和工程 实践表 明 ， 四元数 法相对 于其他 两种 方法 的优越性 ， 比如 在大姿态 角 情况下 ， 欧拉方程存 在奇异点 ， 又称 作平衡环 锁定 ； 向余 方
标 系 曰为空 间中的一个运动坐标 系 ， 假设机体 坐标系 曰到 惯性 坐标 系 ， 的姿态变换 四元数为 ‘
算需要 的数据 ， 是捷联式惯导算法中的重要工作 。
算量很大 ， 并且对数据光 滑性要求 较高 。旋转 矢量算 法是 基于 １７ ９ １年 Ｂ ｒ 提 出的旋转矢量理论及旋转矢量微分方 ｏｚ ｔ
程， 该算法能够对 圆锥误差进行 有效补偿 。本 文 中把精
细积分法应用 于四元数微 分方程 的求解 ， 数值仿 真结果表
到有效抑制 ， 以为捷联式惯性导航技术的工程实践提供参考 。 可 关键词 ： 捷联惯导系统 ； 四元数 姿态微分方程 ； 精细积分 ； 龙格库塔法
中图分类号 ：Ｐ ０ Ｔ３ １
文献标识码 ｉ Ａ
文章编号 ：０６— ７ ７ ２ １ ）５一 １３一 ４ １０ ００ （０ ０ ０ ｏ Ｏ ｏ 小， 所以常用于飞行器实时解算 。 四元数 的另外一个 优点 是便于采用卡尔曼滤波实现 四元数最 优估计 ， 也正是 基于 这个原 因，四元 数广 泛应用 于航 天器 的姿态 控制系统 。近 年来 ， 出现了很 多有 效求 解 四元数 微分 方程 的方 法 ， 例如 龙格库塔法 、 旋转矢量算 法等 。四阶龙 格库塔算 法是计 算
第３ 卷 １
第 ５期
四 川 兵 工 学 报
２１ ０ ０年 ５月
【 其他研究】
捷 联 系统 四元数 姿态 解 算 的精 细 积 分 法
项凤 涛 ， 王正志 ， 吴第曼 ， 岳
（ 国防科学技术大学 机 电工程与 自动化学院 ， 长沙
达
４０ ７ ） １０３
摘要 ： 了基 于四元数 的相关理论 ， 研究 把精细积分方法应用于求解四元数姿态微分方 程当 中， 以典 型圆锥 运动 并 作为输入条件对该算法进行 了仿真分析。相同仿 真步长条件下的仿 真结果表 明， 该方法在 高动态角运动 环境下 的解算 精度要优 于四阶龙格库塔算 法 ， 由圆锥运动引起 的俯仰角算法漂移误差相对 于于 四阶龙 格库塔方法也 得