基于矢量观测确定飞行器姿态的算法综述_林玉荣.pdf

第 35 卷 第 1期 2 0 0 3 年1 月 哈 尔 滨 工 业 大 学 学 报 JOURNAL OF HARBIN INSTITUTE OF TECHNOLOGY Vol 35No 1 Jan 2003 基于矢量观测确定飞行器姿态的算法综述 林玉荣 邓正隆 哈尔滨工业大学 航天学院 黑龙江 哈尔滨 150001 摘 要 随着无陀螺飞行器的出现 关于矢量定姿算法的研究已引起重视 这方面的理论基础比较薄弱 有 待解决的问题还很多 为给今后的研究工作提供有用的参考 按照确定性算法与状态估计法的分类原则 介 绍近几十年来国外研究者在矢量定姿算法研究方面所取得的主要研究成果 阐述典型算法的基本思想及其 发展状况 并进行比较分析 结合航天科技发展现状及趋势 特别指出了无陀螺矢量定姿法研究的重要现实 意义 不仅为促进小型 无陀螺 飞行器的发展奠定了理论基础 而且为延长载有陀螺飞行器的寿命提供一个 新的解决思路 关键词 矢量观测 姿态确定 状态估计法 飞行器 中图分类号 TP273文献标识码 A文章编号 0367 6234 2003 01 0038 08 Summary of algorithms for determination of spacecraft attitude from vector observations LIN Yu rong DENG Zheng long School of Astronautics Harbin Institute of Technology Harbin 150001 China Abstract With appearance of gyro less spacecrafts study of attitude determination from vector observations drew much attention The determination of attitude from vector observations is incomplete and there are many problems to be solved Achievements have been made by foreign researchers in recent thirty years In order to provide useful reference for our future researchwork the primary research results of those achievements about determination of atti tude from vector observations is introduced by dividing them into deterministic methods and sate estimation algo rithms Both basic ideas and development status of typical algorithms are illustrated and analyzed through compari son Considering the current development and future trend of the aerospace science and technology the important actual significance of making research into algorithms for determination of gyro less attitude from vector observations is indicated not only the academic base has been established for advancing development of small gyro less space crafts but also a novel thought has been suggested for lengthening the life span of spacecraft loaded with gyros Key words vector observation attitude determination state estimation algorithm spacecraft 收稿日期 2001 12 30 基金项目 航天科技集团创新基金资助项目 作者简介 林玉荣 1972 女 助研 博士 邓正隆 1939 男 教授 博士生导师 矢量姿态敏感器 如太阳敏感器 星敏感器 是飞行器上广泛应用的姿态测量部件 它通过测 量选定参考天体相对飞行器的方向得到包含飞行 器姿态状况的矢量观测信息 利用矢量观测信息 能够确定飞行器的姿态 20 世纪 60 年代中期 Wahba 提出了利用星敏 感器的矢量观测确定卫星姿态的问题 1 以后这 方面的研究一直引起人们的兴趣 产生了各种基 于矢量观测求解三轴姿态的算法 算法基本上可 以分为两类 一类是确定性算法 如 TRIAD 法和 基于求解Wahba 问题产生的QUEST SVD 等算法 这类算法可以利用两个或多个矢量观测直接确定 飞行器姿态 另一类算法称为状估计法 其中最常 用的技术就是扩展卡尔曼滤波 EKF 状态估计 法采用动力学模型 或运动学模型 对姿态参数及 其误差参数进行实时估计 关于矢量定姿算法方 面的研究 国内相关文献上还很少见到 而国外却 发展得比较快 本文将介绍国外研究者在近几十 年内所取得的主要研究成果 阐述算法的发展状 况及其基本思想 1 确定性算法 矢量姿态敏感器的测量原理可以表述为 bi Ari bi i 1 n 1 式中 ri是由惯性坐标系中的某个已知方向 如 相对于太阳 地球或选定的恒星天体 定义的单 位参考矢量 bi是在机体坐标系中测量该方向得 到的单位观测 测量 矢量 A 是由惯性系到机体 系的姿态矩阵 方向余弦阵 bi是姿态敏感器 的测量噪声 式 1 中的 ri与 bi构成一个矢量观 测对 其中包含了飞行器的姿态信息 Wahba 将利用星敏感器确定卫星姿态的问 题描述为在最小二乘意义下 求解最优正交姿态 矩阵的问题 1 即满足式 1 的姿态矩阵是使 Wahba 损失函数 L A 1 2 N i 1 ai bi Ari 2 2 达到最小的最优正交矩阵 Aopt 式中 ai为加权系数 最早解决 Wahba 问题的方 法是 根据式 2 直接求解姿态矩阵 考虑由正交 性带来的约束条件 得到批处理结果 2 A B BTB 1 2 1 3 式中 B N i 1a ibirTi 4 根据已有的研究成果 Shuster 和 Oh 给出两种姿 态求解算法 TRIAD 法与 QUEST 法 1 1 TRIAD 法 3 TRIAD法是一种简单的代数方法 已知在参 考坐标系中有两个互不平行的单位参考矢量 r1 r2 它们在机体坐标系中被测得的单位观测矢量 分别是 b1 b2 分别在参考坐标系 机体坐标系中 建立新的正交坐标系 L S L S 坐标系中各坐标 轴的单位矢量为 l1 r1 l2 r1 r2 r1 r2 l3 l1 l2 s1 b1 s2 b1 b2 b1 b2 s3 s1 s2 则存在唯一的正交姿态矩阵 A 满足 A 3 i 1 silTi 上述即为TRIAD 法的实现思想 TRIAD 法出现至 少已有 30 年 由于算法简单 已被应用于 SAS Seasat AEM Magsat 和 DE 等的 实际飞 行任务 中 3 TRIAD法在建立正交基的过程中第一个矢量 被作为 基准 anchor 而第二个矢量中的部分信 息被丢失了 姿态矩阵解对于下标为 1 2 的两个 矢量具有不对称性 Bar itzhack 证明由 TRIAD 法 得到的姿态矩阵解不是最优的 并提出了一种最 优TRIAD 算法 4 该算法包含两次 TRIAD 运算 每次分别以下标为 1 2 的矢量作为 基准 得到 姿态矩阵 A1 A2 然后利用测量器件的统计特性 对两个姿态矩阵进行加权平均处理 得到一个比 A1 A2更准确的姿态矩阵估值 A 1 2 QUEST 法 3 QUaternion ESTimator QUEST 法是 Wahba 问题的解 给出最小二乘 意义下的最优四元数估计值 不失一般性 对式 2 中的加权系数进行归一化处理 可得到 g A 1 L A tr ABT 5 则姿态确定问题转化成为增益函数 g A 取最大 值问题 增益函数 g A 可被化成关于四元数 q 的 二次型函数 g q qTKq 式中 K 为 4 4 维矩阵 即 K B BT trB I3 3 Z ZT trB Z B23 B32B31 B13B1 2 B2 1 T 6 则确定最优姿态的问题转化为求解使二次型函数 g q 达到最大的四元数 考虑到四元数的正交性 约束 利用拉格朗日乘子法可以证明 使 g q 达 到最大的四元数恰好是矩阵 K 的最大特征值所 对应的特征向量 即最优四元数 qopt满足 Kqopt maxqopt 这就是 QUEST 算法的思想 QUEST 算法被结合用 于 5 Magsat 飞行的地面支持软件中 在飞行过程 中 QUEST 算法的执行次数超过了 8 108次 QUEST 算 法还被 用于 5 ERBS 和空 间望 远镜 Space Telescope 的地面支持系统中 文献 3 中 对QUEST 和TRIAD算法进行协方差分析时 利用 了模型 E bi 0 E bi bTi 2 i I3 3 Ari Ari T 7 式中 2 i为敏感器的测量噪声方差 上述模型虽 然是用近似法求得的 但是却准确地给出了姿态 敏感器误差的均值与协方差 这个模型具有重要 意义 在很多算法中得到了应用 6 10 39 第 1 期林玉荣 等 基于矢量观测确定飞行器姿态的算法综述 基于QUEST 法 Shuster 作了进一步的研究工 作 证明 6 如果采用由式 1 和式 7 构成的 QUEST 测量模型 且加权系数选为 ai 1 2 i 则 Wahba 问题等价于最大似然姿态估计问题 而且 由式 4 定义的姿态分布矩阵 B 确切地描述了最 大似然姿态 B 阵可按照下式进行递推计算 Bi Bi 1 1 2 ib irTi 8 QUEST 算法是一种单点批处理算法 即它只利用 在某一时刻所获得的矢量观测来确定此时刻的姿 态 这样在过去时刻的测量量中所包含的信息就 丢失了 为解决此问题 基于 QUEST 算法并利用 式 8 Shuster 8 导出了求解 Wahba 问题的Kalman 滤波解 所得到 的算法称为滤波 QUEST filter QUEST 法 由于能够利用所有过去时刻的测量信 息 即使在某个时刻只能获得一个测量量 滤波 QUEST 法仍能通过 B 阵的传播进行姿态修正 Bar itzhack 11 也对 QUEST 法进行了扩展 提 出了能递推处理矢量观测信息的 RQUEST 法 Re cursive QUEST 该算法通过式 6 中定义的 K 阵 的传播来实现递推功能 其传播式为 Kk j mk mk jKk 1 mk j Kk j mk k i 1 ai 式中 下标 k 为已经处理过的矢量观测对数目 j 为新增的矢量观测对数目 1 3 SVD 法 12 与 FOAM 法 13 基于3 3矩阵 B 的奇异值分解 Markley 提出 了求解 Wahba 问题的新方法 即 SVD 法 Singular Value Decomposition SVD 法利用 B 阵分解得到的 正交矩阵 U V 按照下式计算最优姿态矩阵 Aopt U diag 1 1 detU det V VT SVD 法是一种鲁棒性很强的姿态求解算法 但却不是很有效的 因为进行奇异值分解是一项 运算量很大的复杂工作 鉴于此 Markley 提出了 间接利用奇异值分解思想的 FOAM 法 Fast Opti mal Attitude Matrix FOAM 法中的最优姿态估值计 算如下 13 Aopt B 2 B adjBT BBTB 式中 的计算不需要直接进行奇异值分 解 而是采用迭代算法 与前述几种算法相比 FOAM 法的运算速度最快 而且鲁棒性更好 1 4 Euler q 法 14 描述飞行器姿态最直观的参数是欧拉轴和欧 拉角 利用欧拉轴转动特性 Mortari 定义了一个新 的损失函数 LM e n i 1 ieTdidTiedi bi ri bi ri 9 式中 i为相对加权 e 为欧拉轴 转动轴矢量 基于使 LM e 达到最小的最优准则 Mortari 提出 了计算最优姿态参数的 Euler q 法 利用欧拉轴 e 和欧拉角 由式 5 定义的增益矩阵可表示为 g e cos tr B 1 cos eTBe sin ZTe 10 Euler q 法的思路是 首先求出使 l LM e 达到最小的最优欧拉轴 eopt 然后将 eopt代入式 10 求解使 g e 取极值的最优欧拉角 opt eopt opt即为所求的满足式 1 的最优姿态参数 与直接基于 Wahba 最优准则产生的算法相 比 Euler q 法精度要稍差一些 但从计算速度上 比较 Euler q法要优于QUEST SVD 等方法 2 状态估计法 前述的 TRIAD QUEST 与 SVD 等确定性算 法 其结果有明确的物理或几何上的意义 但这类 方法要求参考矢量的参数足够精确 15 这类方法 原则上很难克服参考矢量的不确定性 如姿态敏 感器的偏置误差及安装误差等 难以建立包括这 些不确定性误差在内的定姿模型及加权处理不同 精度的测量值 与这类方法相反 在状态估计法 中 被估计的量不限于姿态参数 矢量观测中的一 些不确定性参数也可作为被估计的量 状态估计 法提供被估计量的统计最优解 在一定程度上免 除某些不确定性因素的影响 提高姿态确定的 精度 状态估计法的特点是 在系统模型设计中采 用状态空间表达式 状态矢量由需确定的姿态参 数组成 必要时包含精确定姿所需要的其他变量 描述状态量变化的状态方程可根据姿态动力学 或 运动学等动态特性建立 其一般模式为 x f x t g x t n t 11 式中 n t 为状态过程的噪声 姿态动力学方程 表示为 J N h J h 12 式中 为姿态角速率 J 为惯性矩阵 N 为总的 外部力矩 h 为飞轮 或反作用轮 的角动量 由姿 态矩阵表征的运动学方程为 A A 13 式中 0 z y z0 x y x0 40 哈 尔 滨 工 业 大 学 学 报 第 35卷 状态估计法中的观测矢量由姿态敏感器测 量值组成 其元素可以是姿态敏感器的直接测量 值 也可以是由测量值根据某种模式建立的导出 量 考虑观测噪声 观测方程的一般形式为 y h x t v 14 姿态估计需要求解的是由式 11 14 构成 的非线性系统 处理非线性估计问题最常用的方 法就是扩展卡尔曼滤波 EKF 卡尔曼滤波技术 应用于姿态估计中已有很长的历史了 Kau 等人 把非线性滤波理论应用于卫星的姿态估计 16 White 等人采用 EKF 处理星光与已知陆标的测量 信息 17 用以估计飞行器姿态 轨道位置与捷联 陀螺的漂移误差 基于卡尔曼滤波与平滑技术 Schmidtbauer 提出了估计自旋稳定探空火箭三轴 姿态的方案 18 几十年来 研究者们提出了各种 利用矢量观测估计飞行器姿态的 EKF 实现方案 相应的算法不断发展 成熟 20 世纪 90 年代末 Crassidis和 Markley 提出了新的姿态估计算法 最 小模型误差 MME Minimum Model Error 算法与预 测滤波算法 算法是基于最小模型误差准则产生 的 这类算法能够克服卡尔曼滤波算法的缺点 在 动态方程中存在着显著模型误差的情况下 能够 获得准确的状态估值 借鉴状态估计法的优点 Markley Shuster 与 Psiaki 等人分别对 QUEST 法的 功能进行了扩展 提出了具有状态估计特点的改 进算法 2 1 扩展卡尔曼滤波算法 在 EKF 定姿算法中 被估计的姿态参数可以 是四元数 方向余弦阵 欧拉角或 Rodrigues 参数 下面将根据所选姿态参数的不同 阐述各种 EKF 算法的基本思想 2 1 1 估计四元数 四元数是姿态估计算法中最常用的姿态参 数 因为它具有以下几个优点 19 1 动态方程是 线性的 2 不存在奇异性 3 姿态矩阵可表示为 四元数元素的代数形式 回顾 总结已有的成果 以载有矢量姿态敏感 器和三轴陀螺的飞行器为背景 Lefferts 和 Markley 对EKF 在四元数估计中的应用进行了综合讨 论 19 滤波器的状态变量由姿态四元数和陀螺漂 移速率误差构成 状态方程利用四元数微分方程 式 13 的四元数表达形式 和描述陀螺漂移速率 误差的动态方程建立 观测量是星敏感器提供的矢 量测量值 把四元数作为状态参数应用于滤波方程 中存在着困难 因为四元数模为 1的约束条件会造 成四元数协方差阵的奇异性 为解决奇异性问题 Lefferts 和Markley 给出了三种 EKF 的实现方法 三 种方法中 状态矢量的传播方程是相同的 不同的 是协方差阵的传播方程 状态矢量与协方差阵的修 正方程 第一种方法 利用状态误差矢量的转移矩 阵得到了一个降阶的 即 6 6 维的误差协方差阵 第二种方法 通过从状态误差矢量中删除一个四元 数元素 非矢量元素 获得降阶的协方差阵 第三 种方法 通过定义增量误差四元数 q q q 1 表示四元数相乘 15 使得有用的姿态信息全都包含在误差四元数 q 的矢量部分 q13中 从而获得一个非冗余 6 1 的状态误差矢量 结果证明 第一种方法与第三种 方法所得到的协方差阵是相同的 实际应用中第 三种方法最常用 文献 19 关注的是奇异性问题 并没有考虑 动态模型与观测模型中所固有的非线性 而非线 性会在以下几个地方对姿态确定系统产生影响 1 误差协方差阵的传播 2 四元数的预测估计 3 误差协方差阵的修正 4 四元数与漂移速率 误差的修正估计 基于 Lefferts 和 Markley 所提出 的第一种方法 Vathsal 给出了估计姿态四元数的 二阶非线性滤波算法 20 与 EKF 相比 采用二阶 滤波器时姿态估计精度高 而且陀螺漂移速率误 差的估值收敛速度快 不过由于增加了修正项 二 阶算法的计算量也加大了 Bar itzhack 和Oshman 所提出的四元数估计 法 21 是基于 EKF 理论导出的一种变形算法 算 法中 被估计的量是四元数真实值与估计值的差 q q q q 观测量是通过对方向余弦阵与 四元数间的非线性关系进行线性化处理 得到的 一个与 q 具有线性关系的矢量e 四元数估计法 的基本思想是 根据新时刻的观测量 ei 1 利用 与 EKF 相似的方法获得新的状态估计值 qi 1 然后将 qi 1与估计值 qi 1 i相加 得到此刻的四 元数估计值 qi 1 在利用新估计值 qi 1进行状态 传播前 将 qi 1置为零 四元数估计法的实现与 EKF 基本相似 不过 在进行协方差修正时多了一 步 即利用当前时刻的四元数估计值 qi 1重新计 算协方差修正方程中的测量灵敏度矩阵和测量噪 声协方差阵 而这一步是常规 EKF 中所没有的 利用四元数的归一化性质 Bar itzhack 等人又进 一步提出了标准化四元数估计法 21 这种算法不 但提高了姿态估计精度 而且加快了姿态估值的 收敛速度 四元数估计法及其标准化方法只涉及 到姿态四元数的估计 不过要估计诸如陀螺漂移 误差等变量 这两种方法也很容易扩展 41 第 1 期林玉荣 等 基于矢量观测确定飞行器姿态的算法综述 上述几种算法 需要由陀螺提供角速率信息 用以传播姿态 以往 受灵敏度和带宽所限 矢量 姿态敏感器不能被用作在轨运行姿态速率确定的 主要敏感器 不过随着高精度 高带宽 低噪声矢 量姿态敏感器的出现 基于矢量观测确定飞行器 姿态的方法被扩展到姿态速率的估计 产生了无 陀螺姿态估计算法 22 25 而且这方面的研究在 近几年来越来越受到研究者们的关注 这是一个 必然的发展趋势 因为无陀螺姿态估计算法的研 究具有重要意义 首先 陀螺存在着漂移误差特 性 随着时间的推移和器件的老化 陀螺的测量精 度逐渐降低以至于失效 造成的结果是影响姿控 系统的正常工作 如果将无陀螺姿态估计算法作 为姿态确定系统的备份方案 当陀螺组件失效时 则可启动备份算法 维持姿控系统的工作 为延长 飞行器的寿命提供一个新的解决思路 另外 陀螺 的价格很昂贵 如果不装载陀螺必然会在很大程 度上降低生产成本 这一点对于小型飞行器的发 展来说尤为重要 而无陀螺姿态估计算法是无陀 螺飞行器运行的软件基础 使得矢量姿态敏感器 能够用于无陀螺飞行器上 Gai 等人提出了一种实用的卫星姿态 姿态 角速率估计算法 22 该算法仅利用高带宽的星敏 感器输出来驱动一个误差状态 EKF 误差状态矢 量由 q 式 15 定义 的矢量部分和扰动角速率 误差 d构成 d定 义为扰 动角速 率真值 d 由闭环控制系统中的干扰力矩所引起 与其 估计值 d的差 状态方程利用式 15 的定义根 据四元数微分方程和 d的动态特性方程 一阶 马尔柯夫过程 建立 算法的思想与四元数估计法 的思想相似 不过该算法的实现方法与 EKF 相 同 为了提供姿态控制系统所需的速率反馈信息 一般要求姿态速率估计值 nom d nom 是姿态角速率的标称值 的修正频率要高于姿态 修正频率 因此 Gai 分别建立了姿态观测方程和 角速率观测方程 角速率观测量的产生不需要进 行星识别 从而减少星历表的尺寸 提高运算效 率 与文献 22 中的思想不同 Psiaki 等人所提出 的姿态估计法 23 是利用如式 12 所示的动力学 方程来获得角速率信息的估计值 EKF 中的状态 矢量是由角速率 姿态四元数和动力学模型中的 未知干扰力矩构成的 10 维列向量 为避免奇异性 问题 算法中采用由式 15 所定义的增量误差四 元数的矢量部分进行协方差阵的传播 2 1 2 估计方向余弦阵 基于矢量观测直接估计姿态矩阵 有两类方 法 一类是由 Bar itzhack 提出的直接辨识法 DC MI 26 Direction Cosine Matrix Identification 法 另一 类是由 Oshman 和 Markley 在 1998 1999 年所提 出的最小参数化法 9 10 DCMI 法的思想很简单 直接利用方向余弦阵 的微分方程 如式 13 建立 9 维状态方程 滤波 算法采用的是带有 EKF 特征的卡尔曼滤波 如果 利用方向余弦阵的正交特性 则可提高 DCMI 的 滤波性能 加快方向余弦阵正交估值的收敛速度 最小参数化法 9 将 9 元素姿态矩阵的估计问 题转化为 3 维非线性滤波问题 转化过程通过由 角速率 t 定义的 IRP Integrated Rate Parame ters 向量 t x y z T 来实现 t 的 构成元素定义为 i t t t0 i d i x y z 姿态矩阵 A t 的解可利用 t 求得 引入跟踪 理论中的方法 利用指数自相关的随机过程来描 述角加速度的动态特性 基于最小参数化法 Osh man和 Markley 又提出了能同时估计姿态和姿态 速率的序列非线性算法 10 上述两种基于最小 参数化产生的姿态矩阵估计算法 运算效率都 很高 2 1 3 估计欧拉角 Rodrigues参数 欧拉角在姿态估计算法中很少被采用 因为 它存在奇异性问题 不过 采用欧拉角参数也有优 点 飞行器的操纵命令 steering commands 通常是 欧拉角的显函数 文献 27 中给出了基于矢量观 测估计欧拉角参数的 EKF 算法 要利用的欧拉角 微分方程与观测方程都是非线性的 所以算法通 过对非线性方程进行线性化处理导出 该算法能 够解决奇异性问题 其方法是 当发生奇异时 把 奇异项 1 cos 和 1 cos 冻结 freezing 在处理器 所允许的最大数值 这种解决奇异问题的方法会 使非线性的和线性化后的动态方程发生轻微变 化 不过欧拉角估计精度的降低很微小 而且精度 降低持续的时间很短 很快又会达到所要求的精 度 为提高姿态确定精度 可将陀螺的漂移误差角 速率作为扩充状态变量加以估计 Rodrigues 参数是一个三元素姿态描述参数 它与四元数有着很直接的关系 因此它与姿态矩 阵间的关系及其他的运动微分方程很容易得到 Idan 给出了利用观测矢量估计 Rodrigues 参数的 EKF 算法 28 选用 Rodrigues 参数作为姿态参数 姿态估计算法在运算效率上的提高很显著 与四 42 哈 尔 滨 工 业 大 学 学 报 第 35卷 元数估计法 21 相比 运算量减少 35 45 而 姿态估计精度与收敛速度都没有降低 2 2 基于最小模型误差准则产生的姿态估计算 法 在没有陀螺提供角速率信息的情况下 基于 矢量观测信息估计飞行器姿态 通常要利用动力 学方程作为姿态速率的传播方程 23 由于不确定 性干扰力矩及其他摄动因素的的影响 表征飞行 器动态特性的动力学方程中必定存在着力矩模型 误差 对于动态方程中的模型误差 Kalman 滤波 是将其作为过程噪声来处理 并假定可用均值为 零协方差已知的高斯过程来描述 这种假设往往 缺少理论根据 不能正确反映模型误差的真实特 性 从而导致状态估计精度的严重降低 以至于使 滤波器失效 为解决上述问题 Crassidis 和 Markley 将 Mook 等人提出的最小模型误差 MME Minimum Model Error 算法引入姿态估计中 24 与 Kalman 滤波相 比 MME 算法具有以下几个优点 1 无需知道模 型误差的统计特性 2 模型误差作为两点边界值 问题解的一部分被确定 3 状态估计值没有跳变 不连续性 在MME 姿态估计器中 24 被估计的状态矢 量由姿态四元数和角动量构成 状态方程由四元 数微分方程和欧拉动力学方程建立 观测方程直 接基于矢量观测原理建立 将动力学方程中的力 矩模型误差向量记为 d t 则估计器的非线性 系统估计模型为 x t f x t t G t d t 16 y t h x t t 17 MME 算法中 最优状态估计是基于 y tk h x tk tk T y tk h x tk tk Rk 这个 协方差约束 前提来确定的 MME 算法确定 状态估值的最优准则是寻找以 协方差约束 为条 件的模型误差的最小估计值 即使损失函 J 1 2 m k 1 y t k h x tk tk T R 1 y tk h x tk tk 1 2 tf t0d T Wd d 达到最小的模型误差解 上式中 W 为加权矩阵 使 J 关于d t 达到最小的必要条件将导致一个 两点边界值问题 TPBVP Two Point Boundary Value Problem 求解TPBVP 则状态估值与动态模 型误差作为解的一部分被确定 不过MME 姿态估计器是一个批处理 离线 估计器 必须利用验后测量数据 根据最小模型误 差准则 基于预测跟踪方法 Lu 提出 Crassidis 和 Markley 又提出了一种实时的无陀螺姿态估计算 法 称为预测滤波算法 25 预测滤波器的系统方 程如式 16 17 所示 选取泛函 J 1 2 y t t y t t TR 1 y t t y t t dT t Wd t 18 作为损失函数 预测滤波器的实现思想是 对式 17 中的输出估计向量 y t t t tk 1 tk 进行泰勒级数展开 得到包含模型误差 d t 的近似等式 将近似等式代入式 18 求解使 J 达 到最小的模型误差解 然后把得到的 d t 解代入 状态方程式 16 中 实现状态估值的传播 预测滤 波姿态估计算法兼具了MME 估计与Kalman 滤波 的优点 能够对含有噪声的测量量进行滤波 同时 能在线辨识姿态与姿态速率曲线 而且在估计过 程中 四元数始终保持标准化形式 由于不涉及协 方差的传播 预测滤波算法的运算效率很高 而且 鲁棒性能好 2 3 基于 QUEST法产生的扩展算法 与 EKF 相比 QUEST 算法有两个重要优点 一是不需要初始值 通过求解特征值直接解决 Wahba 问题 二是显式地保持了四元数的标准化 形式 不过 QUEST 算法也有其不足之处 它只能 估计姿态四元数 而不能像 EKF 一样对敏感器不 对准误差 陀螺漂移速率误差等典型的滤波器状 态进行估计 Markley 对 QUEST 算法进行了扩展 使其能够估计其他参数 算法的实现是基于扩展 的Wahba 二次型问题 29 L q x 1 2 n i 1a i bi x A q ri x 2 1 2 x x0 T W0 x x0 19 即在四元数的归一化约束条件下 求解使 L q x 达到最小的 q与x x 为附加的参数向量 x0为 x 的先验值 W0为半正定的对称加权阵 求解过 程分为两步 首先 使损失函数 L q x 关于 q 达 到最小 利用QUEST 算法得到含有参数向量 x 的 最优姿态估值qopt t x 然后 将 qopt t x 代入 式 19 得到关于 x的新损失函数 利用给定的 x0 作为初值 采用迭代法求解使新损失函数达到最 小的最优修正值 xopt 修正值加上初始 x 值获得 新的参数向量估值 当修正值足够小时 结束迭代 过程 得到最优估值 xopt 继而得到qopt 上述算法 43 第 1 期林玉荣 等 基于矢量观测确定飞行器姿态的算法综述 的有效性通过仿真测试得到了验证 30 测试数据 由捷联陀螺和星跟踪器提供 陀螺漂移速率误差 作为向量 x 的一部分被估计 不过算法的运算量 比较大 由于没有考虑动态过程中的噪声 所以当 这种噪声显著时 算法的有效性很难保证 QUEST 测量模型是描述矢量姿态敏感器的 最简单测量模型 QUEST 测量协方差阵真实地反 映了姿态敏感器误差 文献 7 对如何将 QUEST 测量模型应用于估计姿态的卡尔曼滤波器 19 中 进行了讨论 实现算法的总体思想是对 QUEST 测 量模型进行线性化处理 此项工作影响的只是卡 尔曼滤波器的修正阶段 将 QUEST 测量模型应用 于卡尔曼滤波器中的好处是 QUEST 测量协方差 阵的简单性使卡尔曼滤波器得到了简化 另外在 滤波器中重要的只是被观测矢量的几何特性而不 是敏感器的结构 不过由于线性化处理 算法的优 越性在很大程度上被抵消 与QUEST 法相比 EKF 也有其不足之处 即 EKF 对初始状态估值很敏感 在有些情况下 这种 敏感性会导致 EKF 发散 而QUEST 法却不需要初 始姿态估值 由此 Psiaki 对 QUEST 法进行扩展使 其具有 EKF 结构 目的是使所得到滤波算法将保 持QUEST 法的优点 在初始姿态估值很差或存在 非线性时具有良好的收敛性 文献 31 给出了 Psiaki 所提出的实现方法 扩展 QUEST 滤波器算 法 扩展滤波器中的状态矢量由姿态四元数和陀 螺漂移速率误差等参数构成 滤波算法的实现包 括两个阶段 第一阶段 利用 k 1 时刻的状态估 计值进行动态传播 传播方法同 EKF 第二阶段是 测量修正过程 求解二次型约束的二次规划问题 采用基于QUEST 特征值 特征向量解产生的扩展 方法来解 决 仿真 研究的 结果 表明 31 扩展 QUEST 滤波器实现了预期目的 具有良好的抗初 始误差性能 即使在存在 180 初始姿态误差和 2 400 h的陀螺漂移初始误差的情况下 滤波器仍 能收敛 这样的性能对于要求高度自主的飞行器 任务来说很重要 参考文献 1 WAHBA G A least squares estimate of spacecraft attitude J Problem 65 1 SIAM Review 1965 7 3 409 2 FARRELL J L STUELPNAGEL JC WESSNER R H et al A least squares estimate of spacecraft attitude J SIAM Re view 1966 8 3 384 386 3 SHUSTER M D OH S D Three axis attitude determination from vector observations J J of Guidance and Control 1981 4 1 70 77 4 BAR ITZHACK I Y HARMAN R R Optimized TRIAD al gorithm for attitude determination J J of Guidance Con trol and Dynamics 1997 20 1 208 211 5 SHUSTERM D A comment on fast three axis attitude deter mination using vector observations and inverse iteration J J of the Astronautical Sciences 1983 31 4 579 584 6 SHUSTERM D Maximum likelihood estimation of spacecraft attitude J J of the Astronautical Sciences 1989 37 1 79 88 7 SHUSTER M D Kalman filtering of spacecraft attitude and the QUEST model J J of the Astronautical Sciences 1990 38 3 377 393 8 SHUSTER M D A simple Kalman filter and smoother for spacecraft attitude J J of the Astronautical Sciences 1989 37 1 89 106 9 OSHMAN Y MARKLEY F L Minimal parameter attitude matrix estimation from vector observations J J of Guid ance Control and Dynamics 1998 21 4 595 602 10 OSHMAN Y MARKLEY F L Sequential attitude and atti tude rate estimation using integrated rate parameters J J of Guidance Control and Dynamics 1999 22 3 385 394 11 BAR ITZHACK I Y RQUEST a recursive QUEST algo rithm for sequential attitude determination J J of Guid ance Control and Dynamics 1996 19 5 1034 1038 12 MARKLEY F L Attitude determination using vector obser vations and the singular value decomposition J J of the Astronautical Sciences 1988 36 3 245 258 13 MARKLEY F L Attitude determination using vector obser vations a fast optimal matrix algorithm J J of the Astro nautical Sciences 1993 41 2 261 280 14 MORTARI D Euler q algorithm for attitude determination from vector observations J J of Guidance Control and Dynamics 1998 21 2 328 334 15 章仁为 卫星轨道姿态动力学与控制 M 北京 北京 航空航天大学出版社 1998 16 KAU S KUMAR K S P GRANLEY G B Attitude determi nation via nonlinear filtering J IEEE Transactions on Aerospace and Electronic Systems 1969 5 6 906 911 17 WHITE R L ADAMS M B GEISLER EG et al Attitude and orbit estimation using stars and landmarks J IEEE Transactions on Aerospace and Electronic Systems 1975 11 2 195 202 18 SCHMIDTBAUER B High accuracy sounding rocket atti tude estimation using star sensor data J IEEE Transac tions on Aerospace and Electronic Systems 1978 14 5 891 897 19 LEFFERTS E J MARKLEY F L SHUSTER M D Kalman filtering for spacecraft attitude estimation J J of Guid ance Control and Dynamics 1982 5 5 417 429 20 VATHSAL S Spacecraft attitude determination using a sec 44 哈 尔 滨 工 业 大 学 学 报 第 35卷 ond order nonlinear filter J J of Guidance Control and Dynamics 1987 10 6 559 566 21 BAR ITZHACK I Y OSHMAN J Attitude determination from vector observations quaternion estimation J IEEE Transactions on Aerospace and Electronic Systems 1985 21 1 128 135 22 GAI E G DALY K C HARRISON J Y et al Star sensor based satellite attitude attitude rate estimator J J of Guidance Control and Dynamics 1985 8 5 560 565 23 PSIAKI M L MARTEL F PAL P K Three axis attitude de termination via Kalman filtering of magnetometer data J J of Guidance Control and Dynamics 1990 13 3 506 514 24 CR ASSIDIS J L MARKLEY F L Minimum model error ap proachfor attitude estimation J J of Guidance Control and Dynamics 1997 20 6 1241 1247 25 CRASSIDIS J L MARKLEY F L Predictive filtering for at titude estimation without rate sensors J