参考撰写范本3.doc

1、1说 明 书 摘 要一 种 基 于 Euler-q（Euler- quaternion）算 法 和 DD2（Divided Difference 2）滤 波 的 航 天 器 姿 态 确 定 方 法 ，本 发 明 涉 及 一 种 基 于 矢 量 观 测 的 状 态 估 计姿 态 确 定 方 法 。其 特 征 在 于 根 据 参 考 星 历 和 由 星 敏 感 器 观 测 得 到 的 参 考 矢 量5 和 观 测 矢 量 ，通 过 Euler-q 算 法 获 得 航 天 器 姿 态 四 元 数 ，经 过 DD2 滤 波 ，一方 面 反 馈 校 正 陀 螺 解 算 获 得 的 姿 态 阵 ，另 一

2、方 面 ，修 正 三 轴 陀 螺 角 速 度 输 出 ，对 陀 螺 仪 的 常 值 漂 移 进 行 补 偿 。这 样 既 可 以 得 到 高 精 度 实 时 的 航 天 器 姿 态 信息 ，又 可 以 在 线 标 定 陀 螺 仪 的 常 值 漂 移 。本 发 明 是 一 种 自 主 式 姿 态 确 定 方 法 ，具 有 精 度 高 ，实 时 性 好 ，简 便 易 行 的 特 点 ，可 应 用 于 各 种 航 天 器 的 姿 态 确 定10 系 统 。1摘 要 附 图陀螺角速度输出陀螺补偿姿态阵D D 2 滤波星敏感器观测矢量 参考星历E u l e r - q 算法输出姿态反馈校正观测量陀螺常

3、值漂移姿态四元数Comment U1: 修 改 过 公 式 ， 上次 修 改 说 明 中 做 过 解 释 。1权 利 要 求 书1、一 种 基 于 Euler-q算 法 和 DD2滤 波 的 航 天 器 姿 态 确 定 方 法 ，其 特 征在 于 包 括 以 下 步 骤 ：步 骤 a： 由 陀 螺 仪 敏 感 航 天 器 姿 态 ，输 出 三 轴 角 速 率 ，获 得 航 天 器 在 地 心5 惯 性 坐 标 系 下 的 实 时 姿 态 ；步 骤 b：利 用 星 敏 感 器 敏 感 星 光 ，经 过 星 图 预 处 理 得 到 观 测 星 光 矢 量 在s航 天 器 本 体 坐 标 系 下 的

4、 坐 标 ；并 经 过 星 图 匹 配 识 别 之 后 得 到 与 之 对 应 的 参 考星 光 矢 量 在 地 心 惯 性 坐 标 系 下 的 坐 标 ；v步 骤 c：使 用 Euler-q算 法 ，利 用 观 测 星 光 矢 量 和 与 之 对 应 的 参 考 星 光 矢10 量 ，获 得 航 天 器 姿 态 四 元 数 ；步 骤 d：以 星 光 观 测 得 到 的 航 天 器 姿 态 四 元 数 与 陀 螺 仪 解 算 的 航 天 器 姿态 之 差 ，作 为 观 测 量 ，对 航 天 器 姿 态 误 差 和 陀 螺 漂 移 进 行 DD2滤 波 ；步 骤 e：由 DD2滤 波 得 到 的

5、 航 天 器 姿 态 误 差 和 陀 螺 仪 常 值 漂 移 ，反 馈 校正 陀 螺 仪 输 出 姿 态 和 陀 螺 仪 输 出 角 速 率 ，得 到 航 天 器 本 体 坐 标 系 相 对 于 地 心15 惯 性 坐 标 系 的 姿 态 和 陀 螺 输 出 ；步 骤 f：反 馈 校 正 后 ，重 复 步 骤 a步 骤 e。2、根 据 权 利 要 求 1所 述 的 基 于 Euler-q算 法 和 DD2滤 波 的 航 天 器 姿 态确 定 方 法 ，其 特 征 在 于 ：所 述 的 步 骤 d中 ，对 航 天 器 姿 态 误 差 和 陀 螺 漂 移 的估 计 是 通 过 DD2滤 波 器 实

6、 现 的 ，且 姿 态 确 定 模 型 如 下 ： 20 状 态 量 为 ： ，其 中 是 姿 态 误 差 四 元 数13123qb123q的 矢 量 部 分 ， 分 别 是 陀 螺 三 轴 输 出 的 常 值 漂 移 ；2b状 态 方 程 ，观 测 方 程 分 别 为 ：（1）314432qwqIbqIb 2（2）30yIXW其 中 ， 为 观 测 量 ， ， 为 高 斯 白 噪 声 ， 为 陀 螺 输 出 角 速 率 ， 为 量 测y12wW噪 声 ， 为 单 位 矢 量 。3I3、根 据 权 利 要 求 1 所 述 的 基 于 Euler-q 算 法 和 DD2 滤 波 的 航 天 器

7、姿 态5 确 定 方 法 ，其 特 征 在 于 ：所 述 的 步 骤 e 中 ，对 航 天 器 姿 态 误 差 和 陀 螺 漂 移 的反 馈 校 正 是 通 过 以 下 步 骤 实 现 的 ：由 滤 波 得 出 的 状 态 中 ，获 得 误 差 四 元 数 ：123q，计 算 出 姿 态 误 差 阵 ，修 正 由132123()qsrtq cgC陀 螺 输 出 的 姿 态 阵 ，得 到 反 馈 校 正 后 的 姿 态 阵 ：C10 （3）cendgC由 滤 波 得 出 状 态 中 的 = ，修 正 陀 螺 输 出 ：b123（4）endoutwb式 中 ， 为 修 正 前 的 陀 螺 输 出

8、。t151说 明 书一 种 基 于 Euler-q 算 法 和 DD2 滤 波 的 航 天 器 姿 态 确 定 方 法5 技 术 领 域本 发 明 涉 及 一 种 航 天 器 的 姿 态 确 定 方 法 ，特 别 是 一 种 基 于 Euler-q（Euler- quaternion）算 法 和 DD2（Divided Difference 2）滤 波 的 航 天 器 姿 态确 定 方 法 ，用 于 各 种 中 高 精 度 的 惯 性 /天 文 组 合 导 航 系 统 的 姿 态 确 定 。10 背 景 技 术航 天 器 的 姿 态 确 定 任 务 是 利 用 航 天 器 上 的 姿 态 敏

9、感 器 测 量 所 得 到 的 信息 ，经 过 适 当 的 处 理 ，求 得 固 连 于 航 天 器 本 体 坐 标 系 相 对 空 间 某 一 参 考 坐 标系 的 信 息 ，航 天 器 由 于 各 种 任 务 的 要 求 ，需 要 高 精 度 姿 态 信 息 ，这 是 航 天 器发 展 和 广 泛 应 用 的 关 键 技 术 之 一 。15 惯 性 /天 文 ，即 INS（Inertial Navigation System）/CNS（Celestial Navigation System）姿 态 确 定 系 统 是 一 种 完 全 自 主 的 姿 态 确 定 导 航 系 统 ，它利 用

10、陀 螺 测 量 的 载 体 角 速 度 信 息 和 星 敏 感 器 测 量 视 场 内 的 星 光 信 息 ，在 初 始信 息 的 基 础 上 进 行 姿 态 解 算 和 滤 波 ，可 以 连 续 、实 时 地 提 供 姿 态 ，具 有 自 主性 强 、隐 蔽 性 好 、不 受 气 候 条 件 限 制 等 优 点 ，因 而 广 泛 应 用 于 航 天 等 领 域 。20 目 前 使 用 的 航 天 器 姿 态 确 定 模 型 ，大 多 以 姿 态 四 元 数 为 状 态 ，由 于 三 轴 姿 态的 自 由 度 为 3，因 此 姿 态 四 元 数 存 在 冗 余 ，而 如 果 单 纯 取 姿 态

11、 四 元 数 的 矢 量为 估 计 的 状 态 ，则 必 须 对 估 计 模 型 线 性 化 简 化 ，造 成 模 型 的 精 度 降 低 。应 用于 航 天 器 姿 态 确 定 方 法 中 ，单 纯 使 用 确 定 性 矢 量 观 测 ，例 如 Davenport q 方法 由 于 需 要 计 算 特 征 值 和 特 征 矩 阵 ，在 线 计 算 困 难 ，因 而 难 以 得 到 应 用 。而25 在 Davenport q 方 法 上 改 进 的 QUEST(Quaternion Estimator)方 法 ，虽 然 在算 法 上 得 到 了 极 大 的 改 进 ，但 是 仍2然 存 在

12、过 于 依 赖 星 敏 感 器 精 度 且 不 能 修 正 陀 螺 漂 移 并 输 出 实 时 姿 态 的 缺 点 。SVD （the Singular Value Decomposition）鲁 棒 性 很 强 但 却 需 要 奇 异 值 分 解 ，由 于 奇 异 值 分 解 是 一 项 运 算 量 很 大 的 复 杂 工 作 ，因 此 不 是 很 有 效 。FOAM(A Fast Optimal Matrix Algurithm)算 法 也 不 能 估 计 陀 螺 漂 移 使 陀 螺 输 出 获 得 在5 线 标 定 ；而 使 用 其 它 状 态 估 计 法 ，例 如 ，扩 展 卡 尔 曼

13、 滤 波 EKF（Extend Kalman Filter），由 于 使 用 泰 勒 展 开 的 一 阶 非 线 性 处 理 ，虽 然 可 以 估 计 非 线性 模 型 ，但 是 对 非 线 性 模 型 估 计 精 度 不 高 ，而 UKF(Unsented Kalman Filter)则利 用 一 系 列 近 似 高 斯 分 布 的 采 样 点 ，通 过 Unscented 变 换 来 进 行 状 态 与 误差 协 方 差 的 递 推 和 更 新 ，虽 然 精 度 有 所 提 高 ，但 是 计 算 繁 琐 ，实 时 性 不 高 ，10 因 而 很 难 满 足 姿 态 确 定 实 时 性 的

14、要 求 。发 明 内 容本 发 明 的 技 术 解 决 问 题 是 ：克 服 现 有 技 术 的 不 足 ，提 供 一 种 基 于 Euler-q算 法 和 DD2 滤 波 的 航 天 器 姿 态 确 定 方 法 ，该 方 法 由 Euler-q 算 法 计 算 矢 量15 观 测 得 到 的 航 天 器 姿 态 ，同 时 由 DD2 滤 波 估 计 姿 态 误 差 和 陀 螺 的 常 值 漂 移 ，不 仅 提 供 了 高 精 度 的 姿 态 信 息 ，而 且 校 正 了 陀 螺 角 速 率 输 出 。本 发 明 的 技 术 解 决 方 案 为 ：一 种 基 于 Euler-q 算 法 和 D

15、D2 滤 波 的 航 天 器姿 态 确 定 方 法 ，其 特 征 在 于 使 用 Euler-q 算 法 和 DD2 滤 波 估 计 模 型 ，利 用 星敏 感 器 和 陀 螺 仪 ，既 可 得 到 高 精 度 实 时 的 航 天 器 姿 态 ，又 可 以 估 计 出 陀 螺 仪20 的 常 值 漂 移 ，其 具 体 步 骤 如 下 ：（1）由 陀 螺 仪 敏 感 航 天 器 姿 态 ，输 出 三 轴 角 速 率 ，并 使 用 角 增 量 算 法 更新 姿 态 阵 ，获 得 航 天 器 在 地 心 惯 性 坐 标 系 下 的 实 时 姿 态 ：航 向 角 ，俯 仰 角 ，横 滚 角 ；（2）利

16、 用 星 敏 感 器 敏 感 星 光 ，经 过 星 图 预 处 理 得 到 观 测 星 光 矢 量 在 航s25 天 器 本 体 坐 标 系 下 的 坐 标 ；并 经 过 星 图 匹 配 识 别 之 后 得 到 与 之 对 应 的 参 考 星光 矢 量 在 地 心 惯 性 坐 标 系 下 的 坐v 标 ；3（3）使 用 Euler-q 算 法 ，利 用 观 测 星 光 矢 量 和 与 之 对 应 的 参 考 星 光 矢 量 ，获 得 航 天 器 姿 态 四 元 数 ；（4）以 星 光 观 测 得 到 的 航 天 器 姿 态 四 元 数 与 步 骤 （1）中 陀 螺 仪 解 算 的 航天 器 姿

17、 态 之 差 作 为 观 测 量 ，对 航 天 器 姿 态 误 差 和 陀 螺 漂 移 进 行 DD2 滤 波 ，5 状 态 量 为 ： ；123123qb（5）由 DD2 滤 波 得 到 的 航 天 器 姿 态 误 差 和 陀 螺 仪 常 值 漂 移 ，反 馈 校 正 陀螺 仪 输 出 姿 态 和 陀 螺 仪 输 出 角 速 率 ，得 到 高 精 度 的 航 天 器 姿 态 以 及 高 精 度 的陀 螺 输 出 ；（6）反 馈 校 正 后 ，重 复 （1）（5）步 骤 。10 本 发 明 的 原 理 是 ：使 用 陀 螺 仪 输 出 角 速 率 解 算 姿 态 作 为 短 期 参 考 ，提

18、供实 时 的 姿 态 ，短 时 间 内 有 较 高 的 精 度 ，但 是 姿 态 信 息 不 断 发 散 ，由 星 敏 感 器获 得 的 星 光 矢 量 通 过 Euler-q 算 法 以 一 定 的 频 率 提 供 高 精 度 的 长 期 参 考 航天 器 姿 态 ，用 这 个 长 期 参 考 姿 态 作 为 观 测 量 ，通 过 DD2 滤 波 器 ，修 正 陀 螺 输出 姿 态 ，纠 正 发 散 信 息 ，同 时 估 计 陀 螺 常 值 漂 移 ，反 馈 校 正 陀 螺 输 出 ，对 陀15 螺 进 行 在 线 标 定 ，保 持 高 精 度 的 姿 态 实 时 输 出 。本 发 明 与

19、现 有 技 术 相 比 的 优 点 在 于 ：本 发 明 提 供 由 Euler 轴 和 Euler 角的 旋 转 特 性 来 计 算 姿 态 阵 的 Euler q 方 法 ，计 算 速 度 更 快 ，同 时 采 用 以 姿 态四 元 数 矢 量 和 陀 螺 误 差 为 状 态 的 模 型 ，提 高 了 模 型 的 精 度 ，避 免 了 计 算 中 的奇 异 性 ；并 且 在 组 合 定 姿 滤 波 过 程 中 采 用 DD2 滤 波 估 计 四 元 数 矢 量 误 差 和20 陀 螺 漂 移 ，以 多 项 式 逼 近 的 新 的 滤 波 器 考 虑 了 新 模 型 状 态 估 计 的 不

20、确 定 性 ，更 容 易 实 现 且 不 需 要 求 导 ，精 度 优 于 EKF 且 不 低 于 UKF，在 保 证 定 姿 精 度的 同 时 大 大 提 高 的 计 算 速 度 ，同 时 估 计 出 陀 螺 的 常 值 漂 移 ，大 大 方 便 了 工 程实 际 应 用 。25 附 图 说 明图 1 为 本 发 明 的 使 用 Euler-q 算 法4和 DD2 滤 波 的 航 天 器 姿 态 确 定 方 法 流 程 图 ；图 2 为 航 天 器 本 体 坐 标 系 与 地 心 惯 性 坐 标 系 之 间 的 关 系 。具 体 实 施 方 式5 如 图 1、2 所 示 ，本 发 明 的 具

21、 体 实 施 方 法 如 下 ：1、由 陀 螺 仪 敏 感 航 天 器 姿 态 ，输 出 三 轴 角 速 率 ，并 使 用 角 增 量 算 法 更 新姿 态 阵 ，获 得 航 天 器 在 地 心 惯 性 坐 标 系 ，即 地 球 地 心 为 原 点 ，指 向 天 顶 为 ，iz指 向 春 分 点 为 ， 与 、 成 右 手 螺 旋 下 的 实 时 姿 态 ：航 向 角 ，俯 仰 角 ，ixiyiiz 横 滚 角 。陀 螺 输 出 姿 态 频 率 由 实 际 陀 螺 采 样 速 率 决 定 ，本 例 中 采 用 输 出 频10 率 为 100HZ；由 初 始 姿 态 计 算 得 出 初 始 姿

22、态 四 元 数 q 阵 ，下 标 0 为 初 始1234q值 ：（5）1000000023400qcos(/2)(/)cos(/2)+in(/)si(/in/)inco2=/i/-i/i/s/s()00000i(s)()()()更 新 矩 阵 为 ：15 （6）00sin21coqnIqn为 陀 螺 输 出 角 增 量 。 为 的 斜 对 称 阵 ：123 21300计 算 姿 态 余 弦 阵 为 ：（7）224131243134212132442132143qqqqCC 20 由 方 向 余 弦 阵 计 算 得 出 地 心 惯 性 坐 标 系 下 的 实 时 姿 态 角 。5姿 态 角 解

23、算 公 式 如 下 ：俯 仰 角 为 ：（8）123sinC偏 航 角 如 下 表 ：值 判 断21 值 判 断2C偏 航 角=0 0 00 12tanC0 0 =0 30 0 12t5 横 滚 角 值 如 下 表 ：值 判 断13C值 判 断3C横 滚 角=0 0 0 =0 2任 意 值 0 13t0 =0 0 0 13tanC2、利 用 星 敏 感 器 敏 感 星 光 ，经 过 星 图 预 处 理 得 到 观 测 星 光 矢 量 在 航 天s器 本 体 坐 标 系 下 的 坐 标 ，即 航 天 器 质 心 为 原 点 ，与 地 心 连 线 反 向 为 轴 ，与bz天 纬 相 切 指 向 东

24、 为 ， 与 、 成 右 手 螺 旋 ；并 经 过 星 图 匹 配 识 别 之 后 得 到bxybzx与 之 对 应 的 参 考 星 光 矢 量 在 地 心 惯 性 坐 标 系 下 的 坐 标 ；v10 3、使 用 Euler-q 算 法 ，利 用 观 测 星 光 矢 量 和 与 之 对 应 的 参 考 星 光 矢 量 ，6获 得 航 天 器 姿 态 四 元 数 ；星 敏 感 器 相 关 精 度 ： （9）1inik计 算 B 矩 阵 ： （10）213113nTibsv计 算 向 量 Z： （11）23112Tbb5 计 算 相 关 权 重 ： （12）1inik其 中 下 标 为 第 个

25、星 光 ， 为 星 敏 感 器 观 测 第 个 星 光 时 的 精 度 ，n 为 星iii i光 个 数 ，以 星 图 识 别 星 光 的 个 数 为 准 。引 入 单 位 向 量 ： （13）iivsd计 算 对 称 矩 阵 ： （14）121313nTihH10 Euler 轴 即 为 阵 的 最 小 特 征 值 的 特 征 向 量 。 阵 的 最 小 特 征 值 的 特 征H向 量 的 计 算 公 式 如 下 ：（15）minoptopteH为 最 小 特 征 值 ， 为 最 优 特 征 矢 量 。minopt可 以 直 接 从 阵 3 阶 特 征 等 式 中 直 接 解 算 出 来 ：

26、15 （16）320abc这 里 a,b,c 系 数 可 以 根 据 阵 的 元 素 来 表 示 ：H（17）1232212313123 1dettrhajhhc设 定 参 数 为 ：7（18）2213()()3cospabqacwp三 个 实 根 解 为 ：（19）123sinco3wap由 于 ，得 出 满 足 条 件 ：0wi5 （20）1230即 为 最 小 值 。一 下 计 算 的 特 征 向 量 ，即 Euler q 轴 ：minmin（21）123 0axyoptoptoptbzoptycMeeHI 给 出 三 个 可 选 择 的 解 ：（22）2123 21 232bczyzx

27、cxzybyxcayaxzybxzabxemmM10 所 有 的 （k=1，2，3）都 是 平 行 的 ，计 算 各 个 矢 量 的 模 为 ：e（23）21223bczaybxpm选 择 最 接 近 单 位 值 的 （k=1，2，3），然 后 由 得 出 最 优 的p/optkeEuler-q 轴 。计 算 得 出 相 关 Euler-q 角 ：opt15 （24）sin1co()TptoptTt toptDZrB其 中 ： 2 2()Topt ttee8航 天 器 姿 态 矩 阵 A 可 以 由 Euler-q 轴 和 Euler-q 角 来 表 示 ：（25）cos1cossinTpt

28、ptooptAIe是 的 叉 乘 斜 对 称 矩 阵 。由 此 解 算 航 天 器 姿 态 。et4、以 星 光 观 测 得 到 的 航 天 器 姿 态 四 元 数 与 陀 螺 仪 解 算 的 航 天 器 姿 态 之5 差 ，作 为 观 测 量 ，对 航 天 器 姿 态 误 差 和 陀 螺 漂 移 进 行 DD2 滤 波 ，滤 波 周 期 由星 图 识 别 间 隔 时 间 决 定 ，本 例 中 频 率 为 1HZ，状 态 量 为 ：123123qb状 态 方 程 ：（26）0303121wqIqbIqb 10 观 测 方 程 ：（27）30yIX离 散 化 后 可 得 ：（28）1,kkxfv

29、（29）ygw15 以 下 各 式 中 ，上 标 为 量 的 均 值 ，上 标 为 量 的 估 值 ，上 标 为 量 的 误 差 。引 入 四 个 cholesky 因 子 分 解 ：（30）TTvwxxQSRSP其 中 ， ，Q 为 状 态 噪1TkkkEYTkTkkkyPExxYPK20 声 方 差 阵 ，R 为 量 测 噪 声 方 差 阵 ， ，为 过 去 的 量 测 矩 阵 。1011kYy使 用 表 示 矩 阵 的 j 列 元 素 。,xjSx可 以 得 到 以 下 各 量 ：(1) , ,() , ,(1) , ,() , ,22xikxjkikxjkvivjijyxikxjkik

30、xjkwiwjiwjfhfhSvSSgghhS（31）9（32）2(2) , ,2(2) , ,2(2) , ,12, ,1 2,xikxjkikxjkikxvikvjikxjkikyxikxjkikxjkikyhSkfhSfhSvfxvfffhSkghSwghSwgx 2(2) , , ,wikjikjik 假 设 估 计 误 差 是 高 斯 且 无 偏 的 ，设 间 隔 长 度 为 。23h一 步 更 新 ：（33）21 , ,21, ,21 ,xv nxvk kkxpkkxpkpnkvpkvphxffhSfSvhfSf 5 （34）, ,221, ,21,xv nxwkkkxpkkxpk

31、pnkpkwpphyfghSghSwhgS从 复 合 矩 阵 中 得 到 ：（35）(1)(1)(2)(2) xxxvxxvSkSkSk （36）()()()() yyywyyw（37）TxxxPkk10 增 益 矩 阵 为 ：（38）1TkxyyKSk（39）kk协 方 差 阵 ：（40）(1)(1)(1)()(2)()(2)() TTTTxyxyxywyxywPSKSKSKSS15 （41）()()()()yxwkkk 5、由 DD2 滤 波 得 到 的 航 天 器10姿 态 误 差 ，反 馈 校 正 陀 螺 仪 输 出 姿 态 ，得 到 高 精 度 的 航 天 器 姿 态 ；由 滤 波

32、得到 的 陀 螺 仪 常 值 漂 移 ，反 馈 校 正 陀 螺 仪 输 出 角 速 率 ，得 到 校 正 后 的 陀 螺 输 出 ：由 滤 波 得 出 的 状 态 中 ，获 得 误 差 四 元 数 ：123q5 ，计 算 出 姿 态 误 差 阵 ，修 正 由132123()qsrtq cgC陀 螺 输 出 的 姿 态 阵 ： （42）cendgC由 此 计 算 得 出 反 馈 校 正 后 的 姿 态 。由 滤 波 得 出 状 态 中 的 = ，修 正 陀 螺 输 出 ：b123（43）endoutwb10 式 中 ， 为 修 正 前 的 陀 螺 输 出 。t6、反 馈 校 正 后 ，重 复 15 步 骤 。1说 明 书 附 图陀螺角速度输出陀螺补偿姿态阵D D 2 滤波星敏感器观测矢量 参考星历E u l e r - q 算法输出姿态反馈校正观测量陀螺常值漂移姿态四元数图 1 bxbyzix iyiz地球天球天顶春分点航天器图 2