基于最优REQUEST_CKF组合的航天器姿态确定

P xz， k =
T w j X j， ∑ k Z j， k j =1
T －x 珔 珋 kz k
⑤计算增益、 状态和协方差估计
－1 K k = P xz， k P zz， k
x ^k = x 珔 珋 k + Kk （ zk － z k） 珔 Pk = P k － K k P zz， kK
T k
（ 14 ）
m
则可得量测方程为： 部分 e 作为量测量， I3 × 3 e = HX + v = ［ 1． 2 滤波方法分析 1 ） CKF 滤波 考虑如下加性噪声离散非线性动态系统 ： x k = f（ x k －1 ） + w k －1 zk = h（ x k ） + v k （ 5） 0 3 ×12］ X +v （ 4）
弹箭与制导学报
第 35 卷
g sf 、 g ma 为状态 的矢量部分 e 和陀螺漂移估计误差 b、
1
1． 1
问题的描述和分析
航天器姿态确定的四元数描述 由姿态四元数描述的航天器姿态， 其运动学方程
e 变量： X = ［
b
g sf
T g m源自文库］ 。
由四元数运动学方程有： q = 1 e 0 e －ω z 2 = 1 2 e 3 ωy q － ωx 4 1 q ω 2 ωz 0 － ωx － ωy － ωy ωx 0 － ωz ω x e1 ω y e2 ω z e3 0 q4
［8 ］ 元数估计 （ ＲEQUEST ） ， 不仅可以顺序处理观测的
星光矢量 ， 且只有单星的情况下也能进行姿态确定 。
*
1105 收稿日期： 2014基金项目： 航空科学基金资助 作者简介： 肖支才（ 1977 － ） ， 男， 湖北武汉人， 副教授， 博士， 研究方向： 导航与制导。
· 2·
T vT ， 1］ ； v 是高斯白噪声向量。 取 q m 的矢量 为 qv = ［
进行全排列和改变元素符号产生的点集 ， 称为完整全 ［ 1］ 1］ 对称点集， 的第 j 个点。 若 k － 1 j 表示点集中［ 时刻的 后 验 概 率 为 p （ x k － 1 z1 ： k － 1 ） ～ （ x k － 1 ； x ^k －1，
T
槡
m 1 ［ 1］ j ， wj = 2 m
（ 6）
z 珋 k =
w j Z j， ∑ k j =1
第6 期
m
肖支才等： 基于最优 ＲEQUEST / CKF 组合的航天器姿态确定
· 3·
P zz， k =
T w j Z j， ∑ k Z j， k j =1 m
T －z 珋 珋 kz k + Ｒk
基于最优 ＲEQUEST / CKF 组合的航天器姿态确定
肖支才，李海君，王
摘
*
朕
（ 海军航空工程学院，山东烟台 264001 ） 要： 针对航天器姿态确定的高精度要求， 提出 一 种 基 于 最 优 递 推 四 元 数 估 计 / 容 积 卡 尔 曼 滤 波 （ ＲEQUEST / CKF） 的姿态估计器。该方法将最优递归四元数估计（ ＲEQUEST ） 方法和容积卡尔曼滤波（ CKF ） 结合 起来， 用最优 ＲEQUEST 方法确定的姿态四元数直接作为 CKF 滤波器的观测量， 由 CKF 滤波估计陀螺漂移来 从而提高了姿态确定的精度和收敛速度 。仿真结果说明了方法的有效性 。 补偿系统模型误差， 关键词： 航天器姿态确定； 惯性 / 天文组合； 最优递归四元数估计； 容积卡尔曼滤波 中图分类号： V249. 3 文献标志码： A
（ 7） （ 8）
x 珔=
m
wj X* ∑ j， k j =1
T －x x 珔 珔 k + Q k －1
式中： x k 为系统状态向量； z k 为量测值。 假定过程噪 Qk － 1 ） ， vk 声 w k － 1 和量测噪声 v k 相互独立， 且 wk ～ （ 0 ， ～ （ 0， Ｒk ） 。 CKF 方法首先计算加权函数为标准正态分布密 度的积分的基本容积点和对应的权值 ： ξj =
Spacecraft Attitude Determination of ＲEQUEST / CKF Integration
XIAO Zhicai，LI Haijun，WANG Zhen （ Naval Aeronautical and Astronautical University Shandong Yantai 264001 ，China） Abstract： According to highprecision requirement for spacecraft attitude determination，an integrated ＲEQUEST / CKF attitude determination method was proposed． This method combines different characteristics of vector observation attitude determination algorithm and CKF algorithm to achieve highprecision attitude determination． The algorithm combines ＲEQUEST and CKF． The quaternion determined by and CKF filter estimates gyro drift so that it can compensate for model error． SimulaＲEQUEST is directly used as CKF filter observations， tion results show effectiveness of the method． Keywords： spacecraft attitude determination； INS / CNS integration； optimalＲEQUEST； cubature Kalman filter
（ 1）
则可得系统状态方程为：
（ ω gx － b － g sfx － g max ） q4 － （ ω gy － b y － g sfy － g may ） e3 + （ ω gz － b － g sfz － g maz ） e2 （ω －b －g －g ）e －（ω －b －g －g ）q +（ω －b －g －g ）e gx sfx max 3 gy y sfy may 4 gz sfz maz 1 － （ ω gx － b － g sfx － g max ） e2 － （ ω gy － b y － g sfy － g may ） e1 + （ ω gz － b － g sfz － g maz ） q4 1 = f（ x， X t） + n（ t） = 2 nx ny nz （ 2） 2 ） 量测方程 星敏感器是根据星光矢量信息确定航天器的体 坐标系相对于惯性坐标系的姿态四元数 q m ； 将 q m 作 为 CKF 滤波器的观测量， 其量测方程为： qm = q qv （ 3） 式中： q v 为星敏感器精度的误差四元数， 一般可近似
2
2． 1
最优 ＲEQUEST 与 CKF 的组合定姿
的线性形式能够得到解析解。 文中采用四元数进行 姿态描述， 建立的状态方程和量测方程如下 ： 1 ） 状态方程 姿态四元数满足正交约束条件， 如果选择其 4 个 分量作为状态变量， 则所构成方差阵必是奇异的； 随 着方差的递推， 这种奇异性很难得到保证。 因此， 在 基于 CKF 滤波的航天器姿态确定中， 直接取四元数
第 35 卷 第 6 期 2015 年 12 月
弹 箭 与 制 导 学 报 Journal of Projectiles， Ｒockets， Missiles and Guidance
Vol． 35 No． 6 Dec 2015
DOI： 10． 15892 / j． cnki． djzdxb． 2015． 06． 001
珔= P
* T wj X* ∑ j， k X j， k j =1
（ 9）
2 ） 量测更新 ①分解因式 珔 珔 S k = chol（ P k） ②计算容积点 珔 X j， 珔 k = Sk ξj + x k ③计算通过非线性量测方程传播的容积点 Z j， k = h （ X j， k） ④计算量测预测、 新息方差和协方差估计
Pk － 1 ） ， chol ｛ ｝ 表示矩阵的乔列 且 S k － 1 = chol ｛ P k － 1 ｝ ， 斯基分解。容积卡尔曼滤波算法如下： 1 ） 时间更新 ①计算容积点 X j， ^ k －1 k －1 = S k －1 ξ j + x ②计算通过非线性状态方程传播的容积点 X* j， k = f （ X j， k －1 ） ③计算状态和方差预测
m
（ 10 ） （ 11 ） （ 12 ）
m 表示容积点总数。 使用三阶容积原则， 式中， 容积 n x 为系统的状 即 m = 2nx ， 点总数是状态维数的 2 倍， 态维数。基本容积点按照下列方式产生， 记 n x 维单 1， 0， …， 0］ ， 1］ 位向量为 e = ［ 使用［ 表示对 e 的元素
（ 13 ）
2 ） 在一步预测前， 对陀螺漂移进行补偿， 即从测 量的角速度中减去陀螺漂移； 然后计算容积点和相应 的权值。 3 ） 根据计算的容积点和相应的权值进行时间更 新， 求解状态和方差预测。 4 ） 如 果 有 星 光 矢 量 信 息 输 入， 则 用 最 优 ＲEQUEST 算法递推 K 矩阵； 并从 K 矩阵中分离出四元 数， 取矢量部分作为 CKF 的观测量， 进行 CKF 的量测 更新， 回到步骤 2 ） ， 继续下一个循环。 组合定姿工作原理如图 1 所示。
910］ 近年来， 文献［ 提出了容积卡尔曼滤波 （ cubature
0
引言
基于星敏感器和陀螺仪组合的航天器定姿方法
［1 ］
kalman filter， CKF） ， 为非线性估计问题提供了一种新 CKF 算法具 的实现方式。与其他的非线性方法相比， 备更优的非线性逼近性能、 数值精度以及滤波稳定 11］将 性， 且 CKF 实现简单和滤波精度较高。 文献［ CKF 用于卫星的姿态估计， 给出了容积四元数估计 器， 并与信息滤波结合提出容积信息四元数估计器， 用以解决初始估计误差大的姿态估计问题。 但该方 法没有考虑矢量测量四元数的精度问题 ， 使得矢量测 量四元数的误差影响了整个滤波的精度 。 文中结合 ＲEQUEST 和 CKF 各自的优点提出一 以满足航天器高精度 种惯性 / 天文组合的定姿方法 ， 的定姿要求 。 该方法将最优 ＲEQUEST 方法嵌入到 CKF 滤波器 中 ， 将 最 优 ＲEQUEST 方 法 确 定 的 四 元 数直接作为 CKF 滤波器的观测量 ， 由 CKF 滤波器估 计陀螺 漂 移 来 补 偿 最 优 ＲEQUEST 的 系 统 模 型 误 差 。 仿真 结 果 表 明 这 种 方 法 可 达 到 较 高 的 滤 波 精度 。 。 此定姿方案常用的
［6 ］ ［4 ］ ［5 ］ 、 高斯滤波算法 、 多模
7］将基于矢量观测的 等。 文献［
姿态确定算法与 EKF 方法相结合提出了一种分段信 息融合的姿态估计器来实现高精度定姿 ， 取得了较好 的滤波效果， 但 EKF 固有的缺点使得滤波精度受到 很大限制。 以四元数为姿态参数确定航天器姿态的递推四
是较为有效的定姿方案之一 滤波算法是 EKF 算法中 UKF
［3 ］ ［2 ］
， 但该算法对于较强的非线性系统
常存在精度低的问题。在航天器姿态确定的各种 时， 得到广泛的使用， 但 UKF 计算量较大， 并且算法使用中参数的选择难度较大。 随着非线性 滤波方法的发展， 航天器姿态确定领域也出现了一些 新的算法， 如粒子滤波算法 自适应估计算法