第 4 卷 3第 2 2013期年 2 月
宇航
学报
ournJal o fAsrontatuci
Vso． l43 o．N 2F beruay 2r13
0
基容于卡尔曼积滤的波星姿态估计卫
喜魏，宋申民
庆 （尔哈工滨大业学制理控与论制技导研究术中，心哈尔 滨150001
）
摘要
为：了获得更的估计好度和精滤稳波定性，提出了一 种基容积于卡尔滤波曼（Cuba tureK laam nilFtre
，CF） 的容K四积数估计器元（ Cuabtru euQaetnirn Eostmaitro C，E） Q估计星卫态姿新方。法利用元四数进行态更新，姿同时采用 义广罗里格德参数表误差角，示 有效地避免了波滤过程中的奇 异为克。多服传感融器合时运算效率低的 题，问 通容 积 四过元 数 估 计器 与 息 滤信 相 波 结， 合 提出 了 一 种 积 容 信息四 元 估 数 计 器（ ubaturCeIn ofmrtiao nQautenron isEtimtoa， rICEQ） 仿真表。明度角陀螺和漂初始估移计误较大时差 ，方新法仍能得取良的好计性估 。 能键关： 词姿估态；计 积容卡曼滤波； 尔容四元积估数计器 ；积信息四容数估元计 器中图类分号：V4 48．2 文 标献识码：A 1 38 2（2013 ） 2009130 8文章编号： 0100OD： I1． 3073 / j．8is ns ．1001302． 8023． 10． 207
Cub0tarue almKna ilFtreaBsd eaSetllie ttAttuiedE tsiatmon
iWI XEqiingS，ON ShGemin
n（ Centreo f ontCrolTh oey ard nGiduace Tnchenolgy，oarbiH Innsittteu fo ecThnlogyoH，abin 1r05001， hCnai
Abs）rtcat ：Anew sat elile atttiutd eetsiamitonm ehto ndmed caubtauer qatureion nsteimaort（ CQ ）Eb sead nocu btuareKa manlf iltr（ eKF ） Csipr pooed sfr boetet estrmaitoniper ormancf in teresm f nuoerimcla cacuarcya n dfliertig stnaibltyi Att．itduei srpoapgatde b uysnig uqaetrnon iwhle ginerealzed Ridroiues garpmetear sareemp olydet oxpeerssa nuglarerro rsfo raovding siigunlariyt o fhtenew m etodh．T he cuaturb ienofmarion qtuterainonesti mator （C QE ） iIs poposre dy cbmboinng CiEQa dn nfiroamtoin filtriegn pardaimgt oovreocm loe efwfciency oin ccauno tf muolitleps ensrofu iosn S．miualitos nemonsdtart tehatth ene wm teod ahcheves deisied rpreofmarnecund erthe ocdintoi on larfe igitnal eirorrs． eK wyrosd ：Atttuied etsimation；Cubat reu Klaamn filtr （eCK ）F ；Cuatbru quaterneion stemaitr o C（E） Q ；Cuatbre informautinoqu tareino enstmiatr o（ ICEQ）
0
引
言
降
和滤器收敛波度缓慢速问题
的［7
］
。
为 进了步一
UK 被 F用于 星卫 姿态估 改卫善 姿星 态 估计 性 ， 计能 ，过通经 UT变换后的 采样点集逼近非线来性函数概率 分布， 态姿计结果估较EKF 具更有好的鲁 性棒
［ 8 －9
无］论执是行地观对、 测轨服务在是编队飞行还 确精知已态姿息信卫是完星成务任前的 任，务 提
［案 1－2
］
矢。量测敏感器和观陀构螺成的卫星姿态 估。实上事， 论是无用利拉角欧、罗 德里参格数［
6
］。
粒子波滤于用卫星姿态估计，具 更有好的
1［0－ 1 ］1
是
现高实度精星姿卫确态的标定准 计方 系统 ，
［3 5－
］
收速敛和度计估度精
。 由于粒
子波滤是一种
理论只要上子数粒基 蒙于特卡仿罗真的波滤方，法 目够多足 其对，线性、 非高非斯统可系达以任到意的 计精度。但估与他滤其波法方相比， 子滤粒波着随 粒子数增多而导致目计算量加， 增此因子粒滤波难 以足卫满星态估计姿工应程中用实时性的要对 。求
还
四是元数描来述航天的姿器运态动学，都 有具 线非特性性，因此 常经采用E F 进K状态估行 。计但 由于是E KF 经一过线性阶化近后， 忽似了略部非分线 特性， 性初在始差误较大时，存 估计在效果剧急
122下 7 收稿；日： 2期1008106修回 期日：2 01-
2基
项金：目国家 自科学基然金 （1167407 3
）
194
宇航学
报
第
34卷 n n
为了更好满地足星卫态姿计估求要 ，必要研有 究精度高、更工程 现更实简的单方。 容积卡尔曼法滤 波就是 近 来年提 出 一的 种 非线性 高 滤斯 波方
法1［2 13－ ］
u
－k ∈1 Ru 是其 x中k ∈ R 是 统在 k系 刻的时态状 z k ， R∈是系统 量测的值。 k w1－和 k v为 系统输的入 不相关零，均高斯值白噪声 其协，方阵差分为别Q k－ 和 1Rk 。 非线性高斯滤波的核问心题是解多求变非线量 函 性数 高 与 密 斯度 函数 乘 积 的 积 。 分Aararstnam［1a］等人通 2 过三阶 容 积 积 分 则法，利用 2 n 个容点积加求和近权似计算加权斯积高分， 于对 函数 （ xf）的 权高斯加分积：I N（ f =）
≈
。CmKF具有严 的格学数明证，通过 三容
阶充利用了分其 法积数值则分近似加权积斯积分，高在 多维数函分积数运算中具值有的高效率点 。特容 积 尔曼滤波具卡有权值等 的2 n容积点个 n （系为统 状态数）维， 证经明逼其近非线变换性的后率概 分精布优度于U F
K1［ ］
2
。
于基四元描数述的姿态运动方程学具线性形有 且式易于导推析解， 解在星卫态更新姿中较其他 姿描态述法具方有更的大势优［
41 － 15 ］
∫
R
n
（fx ）N（ ；xμ ， P dx
2）
n 。此在因进卫行
星态估姿时计，经 常采四用数元行进姿更态。新 然由而四元于的单位模数制，限在 行协方差阵进新更时会 成造方协差阵异奇。用罗德里格参利数表角 示以有可避效免元数协四方阵差新更时出现 度误差，的 异奇象
［1现 6
］ 1f（μ + P 槡ξ i） 2n∑ i=1
（ 3）
P）
表示x 服从均 μ 值协和方差 阵 的P其中 （ N； xμ， P为协方差阵 P的 平方，根 P 槡（ ） PT正 态分布 满足槡 槡 ， =，P具 有 2n 个元 素 的 容积点 集 ｛i ξ｝具 有 如 形下式 ：
1
0 n 槡 0 0 0 1 － 1 0 0 0 0 － 1
文。采用的广中义罗德里格数是
参1［ ］
7一
包含种德里罗格数和修正罗德参格参里的数更 义广三的维度描角述形式 。另外 ，卫星通多传感 器融过合进姿行估计态， 普通时的波器存在滤算效计率 高的不题。将 U问FK 信与滤波息框架合， 有结 但是效其地 高提线性非系统多传的感器合的效融 ， 估计率精还度进一有提高的空间步
18 ］［
…
…
其第
i 个容积 点 i ξ是n 维 向列量 。从而贝在斯叶计框架下估， 利用容积分法积则，提 了出CKF 算法。 设在第假k 时刻已 知一时刻后 前 k+^1 －，P k－+ ） 1，验 度密函 数p（ x －1k z|1：k 1－） = N（x k －1 ； xKCF算 流程法下： 如（1 ）时间测预 ①计算 容点积 X：i k －，1= ② 容积 传点：播P k 1－ 槡
。
+在
上述究研础上基，本文提 出了积四元容估 数器， 计在一进提步高卫星姿态计精度估的时同 ， 通 过四数与广元罗义里德参数格结合的方法 ，避了免由 于态状余冗导的致方差协奇异阵 进。一步， 通容 过四元数积计估与信器息滤波的结，合提出了 容信积息四元 数估器计。虽然与其容积元四估数器计质 但本多在矢量观情况测下有能提高卫 上是等价的， 效姿态星估计计算的率效最后。将种两方应用于法三轴稳定 卫的星仿研究，真验 了所证出方法提及时在角 和度陀漂螺初始移估计误差较大的情况 ， 然仍 具有好良的估精度和计敛速收。度1 KC 滤F 考波具虑加有噪性的非线性系统状声方态和程量 方程： 测xk f= x（ k－1 ， ku－1 ） +w k－ 1 z = k（h x k u， k ）+v k（ ） 1（2）
^k+ －1， i =1 ， ，… n2ξ i +x
（
4 （ 5）
）X u*k －1 ） i，= 1 ，，… 2 n， ki= f（ X ，i k －1 ③， 估计预均测值协方和阵差 ^ ：k－ = ∑1X * x k2 ni =1 i， P －k= 1 X**i， k X （i， ）k 2n i∑ =1
n2T n2
（ 6
^ k）－ （ ^x k ） －x
－
T+Q k1－ 7（）
2（ ） 测更量 新① 计容积算点 iX k = ②， 积容点传播： Zi， u k） ， i=1 ，… 2， kn= h（ X ，ik， （ 9）Pk
－槡
^
k－ i，= 1 ， … 2， nξi+ x
（ 8
）
2第
期魏庆喜：等基于 容卡尔积曼滤波的卫星态估计姿
1
5
③ 计算9量测测值、 预息方新差和方差协阵矩1 ^ iZ， zk－ =k 2n ∑i=1 珓 zP珓z ， k P xz=，k 1 T－ ^－^ Z i， Zki k ，－z k（ z k） 2 n i =1∑2n
2nT n2
． 22
矢量观测
模型姿 的态矢量观模测型为 ：b1 珓 kz = bm （Aq ） 1r
+ vk = （ qA） r m + vk （ 02）
（ 10k） + kR （1 ）1（ 12 ）
k
1 T^ － ^－= X ，ik Z i， k －xk（ k z ） n2∑i 1=
T
其
中 bm 和 r 分m为别第m 个参 矢考在量体和系惯 系下的分性， 量量噪测 v k声 为方 差 kR 零的均值斯白 高（ Aq） 为转矩旋［19］ 阵（ 文所有 中 均表示·噪声 ， 量 向2 － 范） 数
： 2 T（ q A） =（ q 2ρ × ］+ 2 ρρ ρ I）3 3×－ 2 q［4 4 －
④
算计测更新： －1 量^ +k= ^ x x－ +kK k（ k z－ ^ kz－） ， 其中 K k=P xz， k 珓 z珓P ，z k（ 13 ）
TP + = P kk－－ kKP 珓 z珓 z ，kKk
（14 ）
2
2．1
卫姿星运动学态 态运姿动学模型 ρ单四位数 q =元［
T
2（1） 值得注意是，的如 将果公（式 1） 中的5q 定 为初
″义q' 定义增为四元量数，始 元四数， 四元数则 q 含义是
q的］ 的4乘具法有下定如
' '
T
在 q 基的础增加了上 q ，示表姿态矩阵的为形式A（ q = A（ ） q ）q= A （q） A（q ）
″ ''
'
'［
0 2
］
：义
［
14 ］
：
q″ = q ' q =
（2 ）
[
q2 ρ+ q 4 －ρ［ ρ ×］ ρ
q4'q4 －（ ρ ' ） T ρ
' 4
]
1（5）
从可以而看， 出在A （） q础上基乘左 （A q） 得 到关 于q的 态姿矩， 阵我们与常使用通姿的态阵连矩续进 坐标转行的换序顺相同。
″
号表符四示数元法运乘算符 式中， 0 ''［ ρ ×］ = ρ 3 ' － ρ
2 ' '1－ ρ
3 ρ0
'
1ρ
2 ' －ρ 1 0
'
（ 163
）容
积元四估数计 本节利器用积容尔曼卡波滤和四数元结合计估卫
星 态姿，提出的新算法 称为容积四数元估计器（ CE Q ）。式（公18 ） （ 19 ） 构和成了统系状的方程，
态
T
' ''其中 ， ρ2 和ρ 3 ρ ρ 是三个分量。
的
（ρδ 定义误差）四元数δq = ［ 下形式 ： －1 ^其中q
T
q4］ δ具有
如式公（ 02） 是系统量的方程测，统的状态系向为量［ qT
T T ］。
β^
－1 （ 1 ） δ7q= qq ^ 为四 元数q 的求逆算运 ，于单对四元数
位3 1．
间更新时 为避免四了数状元态余造冗成方差协阵出奇现
逆即共轭
四。元数的姿运动态方程如下：学 = 1ω q 2 q0表示 。陀 螺角度速出采用经输模型：典珟 － －β v ω η=ω β = uη 零关均值高斯白声噪 ，其方协阵差分别为σ I σ I 2
u 33×
异·情的况，在 CQE 的计设将状态中向选为 x量 =［ （δ p ）（ 81） T
T 其中T p 为式δ（ 7 1 ）误中差四元数δq ］，β
[ ]
对
的应差误广义罗德格里数：参δ p = b δρa + δq 4 （2 ）
3
中 其ω是 体相对于系惯系性的速角在体系度的下
b
为尺 参数度。b = 参1数 a 从 0到1 取值， 当 = 0，a a 1=， b= 1 时式 （ 式2 3 退） 为化 罗 德 格里参 数 ，（ 23 退）为化正修
罗德里参数，格 义罗德广里格参数的奇异 在 1点80° 到 3 0 ° 间6。当选取 b = 2 a +（ 1 ）
（
19 ）2
3v 3×
， 时δp与 拉定欧定理义的转旋在小角角时度近似 7］ 相1。 由 等δp 到q δ的对关应系文由献［可知 δ4q= － δap2
珟
陀螺输为出， β为螺漂移，陀 中 ω其 vη η和 u不为相 和
。
+b 槡b2+ （ － 1a2） p bδ2+ δ p 2
2
196－
1 ρδ b=（ a + δ 4 ）qδ p
宇学航报
第
34 卷
2（4 ） Zi，
k
^ ^， 0β ， q已滤知初波值 0P， 0 第由 k － 1时 刻态
估计状和协值方阵生差容成点：积 X ， ik－ 1
= i－ ^A（ ，qk ） r1 = ^ i－ A（ q ） r ，km
（ 32
） k
[
δβ ]，
p i k ，－
1，ik －1
i
1=，…， 12
（
5 2
）
方差协矩计阵算采用公 式（ 11） 和 （ 12 ） ，进 步一取滤求波益矩增阵 kK， 从而差误修正德里罗参 格数和陀漂螺的量测更移新 ^ ：k+= x k^ － K+ （k珓 x k －z ^z －k） ^k+ ［= 其 x中（ δp + k
T
）
其 中δ p i， －k 和1 i，βk － 1 别表分容示点积的姿态误差角 于在每一由滤个周期波始开时刻 ，均采和陀 漂移。螺 状用后验态估作为初计始值， 所在以预开测时，始式 （ 2 ）5 的中姿误差态分部 δ p，i为 了免避k － 1恒等于 。 姿态更0时新产生奇异， 将误差修正先罗德里格数参 换转为容积误差四元数，点利用 式（公24 ） 和（ 1 7） ： +
= δq i 4 k， 1 －2
（
33
）
T^ ）+ ］ 。 T接下来计更算新四（ βk
^+ ，^ k 后+项三β 元数陀螺和移漂 陀。漂螺即为移x k 更
新差误四元数 qδ 为
k
－+ aδ pi， k －
1
2+
bb2
+ δ p i ， －k
b1槡
+ 1（－ a2 ） δpi，k －1 2
+ 2δq 4 =
－k （aδpk+ T
）2
+b
b2 +
（ δpk+ ）T
Tb
槡
2
+ （ 1 a－2 ） （ δk+p ）T
2
2
+
1－ a+ δ 4qi， ］+i = 1， …，12 δρ ，iδ pi， k － 1 =b［ k －1 k， －
1 2（6
+ －） + 1+δρ k =b（ a +δ q4 k） （ p k δ
）
（34 （ 3） ）
5由容点误积差四元得数到容点积元数四点集 ： +^^+ qi+ 2 ，…， 2 （1 7 ） 2， k1 =－δq i ，k －1 k －1q ， i=1， 对 星姿态运卫方程进行离散动处理化， 当采样时间内 转过的度角足 Δ满tω 1，过程噪 声方 差阵具如下有式形
2 ［v16 ］
进
一： ^ k步+= δq k+ q ^ k－q
^
，将 CQE 用于 星卫姿态 ^ 0估 ，β q设已假知 P0 ，0
算法流计归程纳如下为骤步：C Q E算法（ 1 ）间时测 预 利①用（ 式2 ）5生 容积成集点X i， k －1 ②；由式（ 6 ） ～ 2 2（ 8） 和姿运态学动程 （方 8 1） ^ 生－ i预测四元成 q ，数 k ；③通过 式（ 2 ） 和9（ 0 ）3 到误得修差正罗里格德参数； xk 和方－阵 差P－k； ④ 式由（ ）6和（ 7） 算计预
测均 值 （ ^2 ） 量更测新 kz ； ① 利式用（3 ）1生成量 测测预 值 ^^ k+； ② 用采（ 3式 3 ）计量测更新算x －
：
1
（ Δtσ + 1 σ2（ t） 3Δ） 3I×3 － （2 σ（Δ ） 2t） I3 × 33 2uu = Q 1 2 2（ 2σ u Δ） I3 t× 3－（ σ2 （u Δ） ）tI 3× 3 ^ i－ 积点四容元数 q， k由 姿 态运 动学 方 计 程算，式 1（ ） 8以利用可四龙格阶塔法库求解，本 文采用文 常用的献析解式形度 为：β
^ +i珟 ，2 …， 12（ 28 ） ω，k = ωk－ 1－ xi， k －1 ，i = ，1－ ^ i，通过 姿态运动方学程到得 q进一步 k， － ^－k （q^ k －1+ ） δ iq， k= qi ， 1－［ 2 0
，其］应容对点的估计积速
角， i= 1，2， ， …2 （ 192
）将容
点误积四元差转数为误差修正换德罗里参格
p数 xδi ， = b
kqk + ； ③式（ 由43） 和 3（5 得）更到新后的四数元 ^^ k+ 的三个元素重前为置零。④ 将测更新 x 量4 容信息四元积数计估 虽然 CQ器 具有良好的E估计度精， 但如果量值测过 ， 多要需进行多感器融合传，时 算量过高计导致 实性下时。为降提了计高效率， 算本受无迹信文息 滤器启波
［1发8］
δρ
i k ， ， = i1， 2 ，， …1 2 + qa4－i ，k
－
（
30）
k^－ 方差阵 和P － k公由（ 式 6 ， （） 7 ） 得到。 预测均值x ．32 量更测新 通过式（公 10 计算）量测预测可得 1值 ^ kz－ =Z i ， k 2∑ ni 1=其 中测估计量容积点值
2n
（
1 ）
3
，出了提一种积容息信四数元估计器
（IQEC 并）将其用运卫星于态姿确定 对。于传多器感据数合过融的量程更新测 信，
息
第
期
2魏
喜庆： 基等于容积卡尔滤曼波的卫姿星估计态［18
］
19
7
波滤具线有性的形
：式m
率
更具 有 势。 优IQE 将C CQE 信与 息 滤波 框 架结 I， ∑ k s =1sm
Yk+ = Y k +－^ + k = y^k－ + y
（ 3
） （ 36 ）
7
合，因此 两者有具价等性同。时利用 m矢量观测 个如果用容积采四数估元计器， 值行进卫星态确姿，定在滤 更波时需要新 P对珓 法算的实性时难以z 珓 ， zk 求 ， 容积逆息四元信数计估器比相容积元数估四 计现。 实，器利 用公 式（37 ）进 行测更新量 ，信息态状是 对m个 测值所对应信量增量息累加的， 避了协免差矩 阵 P珓 采方用此方避法了对高免矩阵 维z 珓z， k 的求计算 。 逆求逆， 同时量测的新公式更更加洁简 ，因此相容比 四积元数计器更估加用于多传感适融器合。 5 仿 验真 证 对QEC和 CI E 进行卫星Q态姿计估仿真比，
对i
s ， k∑ =1
－1 sn2
－
^+yk 和－^ ky +信息状态为，其 中Yk和 Yk 为 信息矩，阵 有：
且Y
－
k
=
（P
－）
（k38 ） 3（ 9）
k^ = －1 Yk－ ∑ i，Xy 2n i 1 = －k
1
I ， s RHs ， =kH k^ +k H ^ x］ 珓i s， R z［ ， sk= Hk z
T－ s k －1 ，，s － kk－ kT s k，－ s，1 k
I ， k s和 i， ks 别为分信息矩增量和信阵息态状量增 ：（ 04 ） （14） （4 2）
T
定
伪义测量阵矩 Hk 有具下如形式
－： TH = k Yk P zx，k tk
象为运行在轨对高道度7 00k 圆轨m道三轴的稳定 星卫，轨道角 度为 速．006 6°5/ s设。卫星体系轨和道 坐系完标全合， 则卫重星系体对于惯性相角系度 速与轨坐道标相系于惯对系角速度相等性。 角度速 和度矢角测量装量置别利用分轴陀螺三和敏星感器。 公 式 1（ 9 中）螺陀测输出包含量的随机移漂准标差－5 1
/2 σv =1 ． 812 ×01° / ，s 漂移速率斜坡准标 差σu =
其将入带公到（ 式14 ）和（4 0 ）得到
－：I s， k Y = P xz，kk － si，k = Yk xz，Pk t
k
R k1 － P （x， zk
k
t T ） Y（ － ）k
（4 3）） T Yk－ ）（T ^xk－］ t （ 44k ）
^－
珓 R－1zs， ［ kk －zk +（ Pxz t， ks， k
^
， 积信息容四元数估计 器 0 ^β，q 假设 知 P0已 0
流，如程步骤：下 CIQ E算法 （1） 时间 测 ① 预生容积点与容积成元四数 估 器 类计 似 同 式（ ，25） ； ② 由 式 26（ ） ～（ 82 ）和姿运动学态方程 （18 ）生 ^ － i预测四成元数 q ， ； k③ 通式过（ 29 ）和 （3 0 得到误差修）罗德正格 参里；数x 和方差阵P ； （ 7 ） 算计测预均 值 ④^由式 6（），
－ － kk
1 ．
812 × 01－8 ° / s 3/ 2 ，姿态 量 测矢 量含 有 标 准差 为 0 ．005° 高的斯白声噪。 陀螺采样周期为1s， 星敏感 器的采 周样期为 0s， 仿真1时设间 为5个 道轨期周。为 了验证滤波器不在同情下的况计估精度 分三种 情况，进行仿：真 情况一，初 始度角估误差计小， 初 初始始角度计估差 陀螺误移估漂计误小差 情况二， ；， 大始初陀漂移螺估误差小计；情况 三，初 始角估 计误度差，大 初陀始螺移估计误差漂。大由 于QEC 和CQEI 在理论具上有价等， 而性且仿真时姿估态精计度非常接近 ，为 了显示晰， 未在图清画出，中 后面在 中表及以析时分细对详比两者估了计果 。结情 况 一： 首 先 设 滤假波 器 的 初值 0x = （ pδ 0 ）T［ T
T β ］ 都为0零， 初方差阵始P0 关中姿态角 于 部分2差阵方P a0 =（ 0． °） 2I ×33 陀螺，漂移方阵 差P0β =
（2 量）测新更－
k^ －别由分式（ 38 ） ① 信矩息 阵Y k和信 息态 状y
和
（ 3 ） 计算；9+
②由式 3（6 ）和 （3 7） 算信计矩阵 Y k息 和息 信^ + ，k^ k + = Y（k+ ） － y 1 ^k 得+到测量更 态状y 进 步一由公式
（ x． 10°/ h ）2 3I 3× 。 b 4，= 广义德罗格参里数选取 a中 1=，即选 取修正罗德为格里数参 图。1 中
角
度误差范数定义为 三角度轴计估误向量差的2 －数范， 图 2中 条曲 三线分为别轴角三度估误计。从图差 和1 图 可2以看 当出角和陀螺漂度移初值估误差计小时较 ，轴的三角 度滤精波度能达 0到． 00°2下以。 差曲误收线敛速较快，
度新
后的态状量； 向^ ；③ 由式 （ 4 3） （和 3 5） 得到更后新四的元 q ^ k+数的 前个三素重元置零为。④ 将测更新 量x
+k
由
信于息波滤质本上将是卡尔曼滤波协方用 差在行多进感传器合融计时效 算矩阵逆的形表式示，
918
宇学航报
第 34
卷图
1Fig． 1
情况一角 误度差数范线 F曲g． i
4图4
情况二
QEC角 估计度误差
Nom rfoatt tuide etsmationie rrorsof CQE fro csae1
Attitude
esitmtaonierror s of CQEf r oase 2
－c 05°，50 °， 60°， 三轴1螺漂陀移值初估误差分别为计
T 01°/ h 1 ° / 0h 1°0/ h ］ P0，关 姿态角部分于 0β= 2［陀螺漂 方移阵取为差P β0 =方差 P阵a0 = （ 4°0 ）3I×3 ，
（1 5° / ） h I2 ×3 3 由。 5图 和 6 可图 知CQ 的E收速敛虽 度但在3 时小仍内达稳定到状态。 然进一降步，
低图
2F gi．2
情况 一C EQ角 估计误差
度A
titute destmatiione rros or fCQE fo racs e1
况二情 ：三角度初值有较轴大计估差误别为分 － 5°0 50°， ，61°， P00 关陀 漂螺移初值计误估为零。
差 于2态姿部分角方阵差 Pa0 = （40 °） I 3×3， 陀螺 漂方差 移 阵2依取为然 Pβ0 = （ 0 ．° 1/ ）h I3× 。 由图 3 和3图 4可知
5 F图ig 5
情．况三角误差范度曲数线fo rcaes3
CQE
相对在于情况一快的速收， 敛在情二况需时要约 两个时滤波小器才达到能比高较估计精度的。
Norm
of atttudiee tsiamiot enrrosr o fCE
Q图
3Fi ．g 3
情况
角二度误差数范曲 Fi线． g 6of cares
2图6
情况
三 QCE角度估计误差
Nor
mof atitudetes timtian erororso fC EQ
Att
tidu estimetion areors rof QEC of rasc 3e
情 况
三： 角 初 度 值 较有大 估 误 计差分 别 为
容积
信四元息估数计在器多传感融合时器 具， 实有简单和现计算小量优点的， 在三种情况下仿真
第
期
2喜庆魏：等 基容于积尔曼卡滤波卫的星态估计姿
19
9果与结积四元数估容器计估精度类似 计。下面用采 Averae geMna quSra e均平均 方根 误差 （ oRotT meErroi，RrAMSE ） T来比 较CEQ C和QIE 估计的度。 精RAMTS 的定义E如下 RTMASE
=［21
］
6
结论
本提文出一种容了积四元估计数器用卫于星姿 态定。对于较确的初值估小计误差 ，新的滤器具 有良好的波估精度计 ，而当估计误差大较 CQE 也时 保持能好的较计估精和度快较收的敛速。 进一度，步 本文提了出种容积一信四息数估计器元算用法多于 算法与容新四元积传 感融器合时的卫姿态星计， 估估数器具计有相的估似计度
精
， 并滤且波式形单、 更简易于实现 参 。 考 文献
：T
MM 为蒙
卡罗仿特次数，真 其x T中为 总仿真步长的 ，^分别是三 轴角度值真及估其计值 。和 x QEC 和C IE Q波器在三滤种况下情行了进 0 2TAMRSE 表 1如所示， 蒙特次罗仿卡， 其真单为位度 从表中。可看以出种两波滤的估器精计相度 ，似 RTMSA 也E之随大， 随着增初始计误差估增， 大仍但能 持良保好的估精度。仿计结真果也验证前了关 于文CQE 和 CIQ 一致E的描述性
。1表Ta lbe1 两 滤波种器RTAM E S 表veraAg eRAMTSE f owto fitlers
况情 4．二 13 ×10 －1 4．11 1×0 －1 情况三 ． 70 0． 809
槡
1 ^
i k ∑ ik －xx MT∑ k 1= i =1
（ 46
）
［
1］ S oltlE L，tecsnikhJ， WaterlU ，et a． Onorblt iervsciing［J ． IEEE ］Rbootcsi ndaA tomatuoi Mngaaizne ，2009 16， （） 4 ：9 2－33． 2 ［］M cLogulihn T ， CHmapelblM ．D itsiburtd eseimtaetfusion fi letr of r．H nooull，uHI， U ntedi tateS： lasge rspacecarf ftomrtaois ［n］R Aermcina nIsttuti of Aereonatuics adnA srotnutiascI cn．， 2008． 3 ［ ］uMrerll JW．Pr eicsoi ntaitutd deetreimntain foo mrultimssionis pacercat［C］． Tfeh IAAA uiGdnace，Naivatgon，aid Cnotrnlo1 978 ．Cofnreecn，Peaol，ACU，ntide Staes， ［4 ］tA nderswS F ，ilaBnw So ．Recen flight restults o thf eRTMMKal anm filte［rC ． A］AA IuidGnac， Neavgaiiot， nnda onCrtloC onfernecean dE xhbit，iMo teneyr，CA U，itndes ttea，s 0220 ［． 5 Abd］leahmar nM， arkPS Y． S giampiont alkamnf ilterni forg pasccreatfat ttiud ane drta eestmiaiot unings mangetoetme reasuremenmt ［sJ ． IEEE T］rnasactiosnon eArospaceand
情况一 QECCI EQ5 ．0 9× 0 1 －45． 0 9 ×10 4
高－斯 － 若法当值数求解阵矩的计逆量约算 3n3 为/2 乘除法，次计算 随着量矩维阵的增加数呈而指图 7仅 制一绘轨个道周内期观测恒星 数个增长级 。从可以看出中大部在分间能时观到测颗或三颗 两数 ，最时能观测多四颗到恒星（ 仅续持个几测量周 星，恒每个 星提供恒维三星矢量，光因 此量测协方矩差 ）期。 IQCE 量测更新程， 阵过 珓P只对 z 珓z，k 最 能达多 到2 1。维 状态方差协阵矩求逆算运 运，算只量状态和数维 关， 而不会相随量测数的增加据导而计致量急剧算 加，增 使在即维数高更的情况能也保实持时性。
E
letrcoin Scstyesm 20，11， 4 （ 72 ： ）1041 1－41． 5［ ］ L6ffeetr sE ，JarkMely F ，ShLsuet rMD． K amanlfilt eirgn of rpacsecarf attttiude etimsatio［ Jn］．Jo ruan oflGuidance，C ntoorl，a d nDnymaci， s1892 5 ， 5（ ）： 41 － 724． ［97 ］范 石春，张高飞，子阳，孟等． 种基一于线性量测方程伪的无 螺姿态陀确方法定［］J 宇．航报，2学08 02，9 4（ ） ：1902－ 216． ［9FnaC uhnsih ，ZhngaG oafi eMe，n Zigang y，
et a． lNvolequ aterion nalKma nilfetr for gryoles astittdeu deermitationn［ J］． Jo rnula of sAtroautncis， 008 ， 229（ 4 ： ）2109－ 1296．］ ［ ］ Cr8ssidias J L，MakrlyeF L． Usncneet fdlteiinrg ofrs acpcreatf atttude eitismtaoi ［J］n． Jounal orfG idanue，c oCtnol， and Dynarmcis， 0032， 26 （ 4 ） 5：6 －3 524． ［9 ］记陈争，袁平建方群．，基 于修正 Rordigus e参和数 UF 的 K 宇航．学，2报080 ，29 5 （）： 1 62 2 1626－ ．态估姿算计法J［ ］［C hne Jihengz，uYa Jniapnnig，aFn gun．QAtti uted stiemaion utings mdoiiefd odrrguie psraaetemsra nd KU［J］． FournJa ofl
图7
F i． g7
道轨周观测期恒个星数
A
tsroaunits，c 2080 ， 29 （ ） 5： 1262－ 162． 6 ］［ 1］ Car0im ， OsAham Y．nFas tp aticrel fitlring efroa tittdu eadn angulraraet etsmaitionf romv ctor ebosevatiron［s］J ．Jurona lf
oumbNe rf ovaaiallbes ats in ercahpe iodr
02
Gu0idanc，eCotnolra，ndDy nmaisc 2009， ，32 （ 1） ： 07 －78．
宇航学报
2
11 ， 02 3 2 （）： 336 － 34．2］
第34
［ 11］ 卷张惟林，军宝，泽明张，等．矢量 测观定确卫姿星态预测 的． 航学报宇2，011，3 2 （ 5） ： 017 7 －0158 粒．子滤波法算［J］ ［Zh nagW i， LineB ojau，n hZng aZmien， egtal． P artcilefiltering fusnig perdictie fvilterf roa ttitue dedtemirntaon ifor vmetcroob servtiaon［Js．］J orualnof A stroauticns，2011 ， 3 2（ ）5 ：107 －7 015．8 ］［ 12 ］rAasraatam I，naHkyin S ．ubCautr ealKamn ilfets［Jr． ］EEE I002 9 ，4 5 6（） ： 2145 － 1296 ．rTasantioncs on Autoamit cCnorolt， ［ 1］ A3rsaartnamaI，Hayk i n，SHurd TR C．ubaure taKmla fnitleinrg ． for cotnnuouisdsicrte syestem： Tsehry and osmulitiaos n［J ］EEI TEarnactsoinson Sig al Procenssni，g0120，58 （10） ： 4977 － 499． ［3 4］1S huste Mr． sAurvy efo atitutedrepres neationt［J］ s．Jou rnl oaf1 99 ，341 （ 4 ： ）43 － 5971 ．het sAtronuticaal cieSnces ［ 1，］5 Kim SG C，rassdii J sLC，enhgY， et la ．aKlmna iflteirngf o r］ ．JJ uonarl ofr latieve pasecraft atcittue anddp siotoi esnimattoin［ 2007 ， 30（ 1） ：1 33－ 14． G3idaucenC，noroltan， dDnaymic， ［ s6］ 1郭， 魏庆瑞轩， 洁许， ．等 于基MRPs NP/UP 的F地磁/ 加速 J度． ］宇航报，学 2101， 32 2 （ ） ：363计 测的量态估计新方姿［法－ 342．［ Guo Q ni，g eW Riuiuax，n Xu Je，iet a l ． Aew nmthoedf ro atttiue desitmtiaon ni mganteoemert adna cclereomtee atrttuid ．e ournaJl f ostroAnuaict，s ystsme asbe dn MoRs aPd nPUNP［JF
］［
17 ］chSaubH，J nkunsi J L St．ereograhpcior intatieno aprmetears of J］r． a titutded yamnisc：a egenalizatironof t he Rdriguoes paaremerts ［9916 ，4（41 ） ：1 － 1． 9oJruna olfthe strAnoaticul Scaienesc，［ 18 L］e e JD Nonlinear．esti amiont na mdlutilpes neosrf usino suin gunc
senedt informatoi niftelrign J［． ］EIEE iSganl Pocrsesign 0082， 15 ：8 16－ 8 4．6L tetes，r ［ 9］ C1rsasiids J ，arMkley F Att．iutd eesimttioa usnig nmodfiid Reordguei ．sT he Ameriacn AtsonarticulaS cieot Fy． Ladnisp raamteersC］［M ralek yAtsonautrisc Smypoium，Gserebnlt，eMD，Unitd Staees，t19 96 ．［20 ］ rassiCid J，Jsnukns Ji Op．tiam lestmation io fydnmic systemas［ ］．MBo caRa otn ：hapCma ＆nH all ，204 0：4 19 － 433 ［．2 1 ］rAluapamaml M S，RsticiB，G rdoon Ne， tla．B erangsinlo tracyikgn o mafoenuvinrgt raetgsus ignpartic l eilftrs［e］． JErauipsJ uorna onl Apliep dSingl arPoescisn，g 020 4（ 15） ： 2 351 －362．5
作简者介 魏：喜庆 （9182 ）－， 男， 士生博 ，研究向为方线性非滤和卫星波 对导相航。 通信地址： 工大科学园 303哈 2信箱 （510010） 电 话 ： （0415 8）462204 － 02182E mia：l weixqiin@ gmgail c．m
o（
辑编 曹亚君：）
第 4 卷 3第 2 2013期年 2 月
宇航
学报
ournJal o fAsrontatuci
Vso． l43 o．N 2F beruay 2r13
0
基容于卡尔曼积滤的波星姿态估计卫
喜魏，宋申民
庆 （尔哈工滨大业学制理控与论制技导研究术中，心哈尔 滨150001
）
摘要
为：了获得更的估计好度和精滤稳波定性，提出了一 种基容积于卡尔滤波曼（Cuba tureK laam nilFtre
，CF） 的容K四积数估计器元（ Cuabtru euQaetnirn Eostmaitro C，E） Q估计星卫态姿新方。法利用元四数进行态更新，姿同时采用 义广罗里格德参数表误差角，示 有效地避免了波滤过程中的奇 异为克。多服传感融器合时运算效率低的 题，问 通容 积 四过元 数 估 计器 与 息 滤信 相 波 结， 合 提出 了 一 种 积 容 信息四 元 估 数 计 器（ ubaturCeIn ofmrtiao nQautenron isEtimtoa， rICEQ） 仿真表。明度角陀螺和漂初始估移计误较大时差 ，方新法仍能得取良的好计性估 。 能键关： 词姿估态；计 积容卡曼滤波； 尔容四元积估数计器 ；积信息四容数估元计 器中图类分号：V4 48．2 文 标献识码：A 1 38 2（2013 ） 2009130 8文章编号： 0100OD： I1． 3073 / j．8is ns ．1001302． 8023． 10． 207
Cub0tarue almKna ilFtreaBsd eaSetllie ttAttuiedE tsiatmon
iWI XEqiingS，ON ShGemin
n（ Centreo f ontCrolTh oey ard nGiduace Tnchenolgy，oarbiH Innsittteu fo ecThnlogyoH，abin 1r05001， hCnai
Abs）rtcat ：Anew sat elile atttiutd eetsiamitonm ehto ndmed caubtauer qatureion nsteimaort（ CQ ）Eb sead nocu btuareKa manlf iltr（ eKF ） Csipr pooed sfr boetet estrmaitoniper ormancf in teresm f nuoerimcla cacuarcya n dfliertig stnaibltyi Att．itduei srpoapgatde b uysnig uqaetrnon iwhle ginerealzed Ridroiues garpmetear sareemp olydet oxpeerssa nuglarerro rsfo raovding siigunlariyt o fhtenew m etodh．T he cuaturb ienofmarion qtuterainonesti mator （C QE ） iIs poposre dy cbmboinng CiEQa dn nfiroamtoin filtriegn pardaimgt oovreocm loe efwfciency oin ccauno tf muolitleps ensrofu iosn S．miualitos nemonsdtart tehatth ene wm teod ahcheves deisied rpreofmarnecund erthe ocdintoi on larfe igitnal eirorrs． eK wyrosd ：Atttuied etsimation；Cubat reu Klaamn filtr （eCK ）F ；Cuatbru quaterneion stemaitr o C（E） Q ；Cuatbre informautinoqu tareino enstmiatr o（ ICEQ）
0
引
言
降
和滤器收敛波度缓慢速问题
的［7
］
。
为 进了步一
UK 被 F用于 星卫 姿态估 改卫善 姿星 态 估计 性 ， 计能 ，过通经 UT变换后的 采样点集逼近非线来性函数概率 分布， 态姿计结果估较EKF 具更有好的鲁 性棒
［ 8 －9
无］论执是行地观对、 测轨服务在是编队飞行还 确精知已态姿息信卫是完星成务任前的 任，务 提
［案 1－2
］
矢。量测敏感器和观陀构螺成的卫星姿态 估。实上事， 论是无用利拉角欧、罗 德里参格数［
6
］。
粒子波滤于用卫星姿态估计，具 更有好的
1［0－ 1 ］1
是
现高实度精星姿卫确态的标定准 计方 系统 ，
［3 5－
］
收速敛和度计估度精
。 由于粒
子波滤是一种
理论只要上子数粒基 蒙于特卡仿罗真的波滤方，法 目够多足 其对，线性、 非高非斯统可系达以任到意的 计精度。但估与他滤其波法方相比， 子滤粒波着随 粒子数增多而导致目计算量加， 增此因子粒滤波难 以足卫满星态估计姿工应程中用实时性的要对 。求
还
四是元数描来述航天的姿器运态动学，都 有具 线非特性性，因此 常经采用E F 进K状态估行 。计但 由于是E KF 经一过线性阶化近后， 忽似了略部非分线 特性， 性初在始差误较大时，存 估计在效果剧急
122下 7 收稿；日： 2期1008106修回 期日：2 01-
2基
项金：目国家 自科学基然金 （1167407 3
）
194
宇航学
报
第
34卷 n n
为了更好满地足星卫态姿计估求要 ，必要研有 究精度高、更工程 现更实简的单方。 容积卡尔曼法滤 波就是 近 来年提 出 一的 种 非线性 高 滤斯 波方
法1［2 13－ ］
u
－k ∈1 Ru 是其 x中k ∈ R 是 统在 k系 刻的时态状 z k ， R∈是系统 量测的值。 k w1－和 k v为 系统输的入 不相关零，均高斯值白噪声 其协，方阵差分为别Q k－ 和 1Rk 。 非线性高斯滤波的核问心题是解多求变非线量 函 性数 高 与 密 斯度 函数 乘 积 的 积 。 分Aararstnam［1a］等人通 2 过三阶 容 积 积 分 则法，利用 2 n 个容点积加求和近权似计算加权斯积高分， 于对 函数 （ xf）的 权高斯加分积：I N（ f =）
≈
。CmKF具有严 的格学数明证，通过 三容
阶充利用了分其 法积数值则分近似加权积斯积分，高在 多维数函分积数运算中具值有的高效率点 。特容 积 尔曼滤波具卡有权值等 的2 n容积点个 n （系为统 状态数）维， 证经明逼其近非线变换性的后率概 分精布优度于U F
K1［ ］
2
。
于基四元描数述的姿态运动方程学具线性形有 且式易于导推析解， 解在星卫态更新姿中较其他 姿描态述法具方有更的大势优［
41 － 15 ］
∫
R
n
（fx ）N（ ；xμ ， P dx
2）
n 。此在因进卫行
星态估姿时计，经 常采四用数元行进姿更态。新 然由而四元于的单位模数制，限在 行协方差阵进新更时会 成造方协差阵异奇。用罗德里格参利数表角 示以有可避效免元数协四方阵差新更时出现 度误差，的 异奇象
［1现 6
］ 1f（μ + P 槡ξ i） 2n∑ i=1
（ 3）
P）
表示x 服从均 μ 值协和方差 阵 的P其中 （ N； xμ， P为协方差阵 P的 平方，根 P 槡（ ） PT正 态分布 满足槡 槡 ， =，P具 有 2n 个元 素 的 容积点 集 ｛i ξ｝具 有 如 形下式 ：
1
0 n 槡 0 0 0 1 － 1 0 0 0 0 － 1
文。采用的广中义罗德里格数是
参1［ ］
7一
包含种德里罗格数和修正罗德参格参里的数更 义广三的维度描角述形式 。另外 ，卫星通多传感 器融过合进姿行估计态， 普通时的波器存在滤算效计率 高的不题。将 U问FK 信与滤波息框架合， 有结 但是效其地 高提线性非系统多传的感器合的效融 ， 估计率精还度进一有提高的空间步
18 ］［
…
…
其第
i 个容积 点 i ξ是n 维 向列量 。从而贝在斯叶计框架下估， 利用容积分法积则，提 了出CKF 算法。 设在第假k 时刻已 知一时刻后 前 k+^1 －，P k－+ ） 1，验 度密函 数p（ x －1k z|1：k 1－） = N（x k －1 ； xKCF算 流程法下： 如（1 ）时间测预 ①计算 容点积 X：i k －，1= ② 容积 传点：播P k 1－ 槡
。
+在
上述究研础上基，本文提 出了积四元容估 数器， 计在一进提步高卫星姿态计精度估的时同 ， 通 过四数与广元罗义里德参数格结合的方法 ，避了免由 于态状余冗导的致方差协奇异阵 进。一步， 通容 过四元数积计估与信器息滤波的结，合提出了 容信积息四元 数估器计。虽然与其容积元四估数器计质 但本多在矢量观情况测下有能提高卫 上是等价的， 效姿态星估计计算的率效最后。将种两方应用于法三轴稳定 卫的星仿研究，真验 了所证出方法提及时在角 和度陀漂螺初始移估计误差较大的情况 ， 然仍 具有好良的估精度和计敛速收。度1 KC 滤F 考波具虑加有噪性的非线性系统状声方态和程量 方程： 测xk f= x（ k－1 ， ku－1 ） +w k－ 1 z = k（h x k u， k ）+v k（ ） 1（2）
^k+ －1， i =1 ， ，… n2ξ i +x
（
4 （ 5）
）X u*k －1 ） i，= 1 ，，… 2 n， ki= f（ X ，i k －1 ③， 估计预均测值协方和阵差 ^ ：k－ = ∑1X * x k2 ni =1 i， P －k= 1 X**i， k X （i， ）k 2n i∑ =1
n2T n2
（ 6
^ k）－ （ ^x k ） －x
－
T+Q k1－ 7（）
2（ ） 测更量 新① 计容积算点 iX k = ②， 积容点传播： Zi， u k） ， i=1 ，… 2， kn= h（ X ，ik， （ 9）Pk
－槡
^
k－ i，= 1 ， … 2， nξi+ x
（ 8
）
2第
期魏庆喜：等基于 容卡尔积曼滤波的卫星态估计姿
1
5
③ 计算9量测测值、 预息方新差和方差协阵矩1 ^ iZ， zk－ =k 2n ∑i=1 珓 zP珓z ， k P xz=，k 1 T－ ^－^ Z i， Zki k ，－z k（ z k） 2 n i =1∑2n
2nT n2
． 22
矢量观测
模型姿 的态矢量观模测型为 ：b1 珓 kz = bm （Aq ） 1r
+ vk = （ qA） r m + vk （ 02）
（ 10k） + kR （1 ）1（ 12 ）
k
1 T^ － ^－= X ，ik Z i， k －xk（ k z ） n2∑i 1=
T
其
中 bm 和 r 分m为别第m 个参 矢考在量体和系惯 系下的分性， 量量噪测 v k声 为方 差 kR 零的均值斯白 高（ Aq） 为转矩旋［19］ 阵（ 文所有 中 均表示·噪声 ， 量 向2 － 范） 数
： 2 T（ q A） =（ q 2ρ × ］+ 2 ρρ ρ I）3 3×－ 2 q［4 4 －
④
算计测更新： －1 量^ +k= ^ x x－ +kK k（ k z－ ^ kz－） ， 其中 K k=P xz， k 珓 z珓P ，z k（ 13 ）
TP + = P kk－－ kKP 珓 z珓 z ，kKk
（14 ）
2
2．1
卫姿星运动学态 态运姿动学模型 ρ单四位数 q =元［
T
2（1） 值得注意是，的如 将果公（式 1） 中的5q 定 为初
″义q' 定义增为四元量数，始 元四数， 四元数则 q 含义是
q的］ 的4乘具法有下定如
' '
T
在 q 基的础增加了上 q ，示表姿态矩阵的为形式A（ q = A（ ） q ）q= A （q） A（q ）
″ ''
'
'［
0 2
］
：义
［
14 ］
：
q″ = q ' q =
（2 ）
[
q2 ρ+ q 4 －ρ［ ρ ×］ ρ
q4'q4 －（ ρ ' ） T ρ
' 4
]
1（5）
从可以而看， 出在A （） q础上基乘左 （A q） 得 到关 于q的 态姿矩， 阵我们与常使用通姿的态阵连矩续进 坐标转行的换序顺相同。
″
号表符四示数元法运乘算符 式中， 0 ''［ ρ ×］ = ρ 3 ' － ρ
2 ' '1－ ρ
3 ρ0
'
1ρ
2 ' －ρ 1 0
'
（ 163
）容
积元四估数计 本节利器用积容尔曼卡波滤和四数元结合计估卫
星 态姿，提出的新算法 称为容积四数元估计器（ CE Q ）。式（公18 ） （ 19 ） 构和成了统系状的方程，
态
T
' ''其中 ， ρ2 和ρ 3 ρ ρ 是三个分量。
的
（ρδ 定义误差）四元数δq = ［ 下形式 ： －1 ^其中q
T
q4］ δ具有
如式公（ 02） 是系统量的方程测，统的状态系向为量［ qT
T T ］。
β^
－1 （ 1 ） δ7q= qq ^ 为四 元数q 的求逆算运 ，于单对四元数
位3 1．
间更新时 为避免四了数状元态余造冗成方差协阵出奇现
逆即共轭
四。元数的姿运动态方程如下：学 = 1ω q 2 q0表示 。陀 螺角度速出采用经输模型：典珟 － －β v ω η=ω β = uη 零关均值高斯白声噪 ，其方协阵差分别为σ I σ I 2
u 33×
异·情的况，在 CQE 的计设将状态中向选为 x量 =［ （δ p ）（ 81） T
T 其中T p 为式δ（ 7 1 ）误中差四元数δq ］，β
[ ]
对
的应差误广义罗德格里数：参δ p = b δρa + δq 4 （2 ）
3
中 其ω是 体相对于系惯系性的速角在体系度的下
b
为尺 参数度。b = 参1数 a 从 0到1 取值， 当 = 0，a a 1=， b= 1 时式 （ 式2 3 退） 为化 罗 德 格里参 数 ，（ 23 退）为化正修
罗德里参数，格 义罗德广里格参数的奇异 在 1点80° 到 3 0 ° 间6。当选取 b = 2 a +（ 1 ）
（
19 ）2
3v 3×
， 时δp与 拉定欧定理义的转旋在小角角时度近似 7］ 相1。 由 等δp 到q δ的对关应系文由献［可知 δ4q= － δap2
珟
陀螺输为出， β为螺漂移，陀 中 ω其 vη η和 u不为相 和
。
+b 槡b2+ （ － 1a2） p bδ2+ δ p 2
2
196－
1 ρδ b=（ a + δ 4 ）qδ p
宇学航报
第
34 卷
2（4 ） Zi，
k
^ ^， 0β ， q已滤知初波值 0P， 0 第由 k － 1时 刻态
估计状和协值方阵生差容成点：积 X ， ik－ 1
= i－ ^A（ ，qk ） r1 = ^ i－ A（ q ） r ，km
（ 32
） k
[
δβ ]，
p i k ，－
1，ik －1
i
1=，…， 12
（
5 2
）
方差协矩计阵算采用公 式（ 11） 和 （ 12 ） ，进 步一取滤求波益矩增阵 kK， 从而差误修正德里罗参 格数和陀漂螺的量测更移新 ^ ：k+= x k^ － K+ （k珓 x k －z ^z －k） ^k+ ［= 其 x中（ δp + k
T
）
其 中δ p i， －k 和1 i，βk － 1 别表分容示点积的姿态误差角 于在每一由滤个周期波始开时刻 ，均采和陀 漂移。螺 状用后验态估作为初计始值， 所在以预开测时，始式 （ 2 ）5 的中姿误差态分部 δ p，i为 了免避k － 1恒等于 。 姿态更0时新产生奇异， 将误差修正先罗德里格数参 换转为容积误差四元数，点利用 式（公24 ） 和（ 1 7） ： +
= δq i 4 k， 1 －2
（
33
）
T^ ）+ ］ 。 T接下来计更算新四（ βk
^+ ，^ k 后+项三β 元数陀螺和移漂 陀。漂螺即为移x k 更
新差误四元数 qδ 为
k
－+ aδ pi， k －
1
2+
bb2
+ δ p i ， －k
b1槡
+ 1（－ a2 ） δpi，k －1 2
+ 2δq 4 =
－k （aδpk+ T
）2
+b
b2 +
（ δpk+ ）T
Tb
槡
2
+ （ 1 a－2 ） （ δk+p ）T
2
2
+
1－ a+ δ 4qi， ］+i = 1， …，12 δρ ，iδ pi， k － 1 =b［ k －1 k， －
1 2（6
+ －） + 1+δρ k =b（ a +δ q4 k） （ p k δ
）
（34 （ 3） ）
5由容点误积差四元得数到容点积元数四点集 ： +^^+ qi+ 2 ，…， 2 （1 7 ） 2， k1 =－δq i ，k －1 k －1q ， i=1， 对 星姿态运卫方程进行离散动处理化， 当采样时间内 转过的度角足 Δ满tω 1，过程噪 声方 差阵具如下有式形
2 ［v16 ］
进
一： ^ k步+= δq k+ q ^ k－q
^
，将 CQE 用于 星卫姿态 ^ 0估 ，β q设已假知 P0 ，0
算法流计归程纳如下为骤步：C Q E算法（ 1 ）间时测 预 利①用（ 式2 ）5生 容积成集点X i， k －1 ②；由式（ 6 ） ～ 2 2（ 8） 和姿运态学动程 （方 8 1） ^ 生－ i预测四元成 q ，数 k ；③通过 式（ 2 ） 和9（ 0 ）3 到误得修差正罗里格德参数； xk 和方－阵 差P－k； ④ 式由（ ）6和（ 7） 算计预
测均 值 （ ^2 ） 量更测新 kz ； ① 利式用（3 ）1生成量 测测预 值 ^^ k+； ② 用采（ 3式 3 ）计量测更新算x －
：
1
（ Δtσ + 1 σ2（ t） 3Δ） 3I×3 － （2 σ（Δ ） 2t） I3 × 33 2uu = Q 1 2 2（ 2σ u Δ） I3 t× 3－（ σ2 （u Δ） ）tI 3× 3 ^ i－ 积点四容元数 q， k由 姿 态运 动学 方 计 程算，式 1（ ） 8以利用可四龙格阶塔法库求解，本 文采用文 常用的献析解式形度 为：β
^ +i珟 ，2 …， 12（ 28 ） ω，k = ωk－ 1－ xi， k －1 ，i = ，1－ ^ i，通过 姿态运动方学程到得 q进一步 k， － ^－k （q^ k －1+ ） δ iq， k= qi ， 1－［ 2 0
，其］应容对点的估计积速
角， i= 1，2， ， …2 （ 192
）将容
点误积四元差转数为误差修正换德罗里参格
p数 xδi ， = b
kqk + ； ③式（ 由43） 和 3（5 得）更到新后的四数元 ^^ k+ 的三个元素重前为置零。④ 将测更新 x 量4 容信息四元积数计估 虽然 CQ器 具有良好的E估计度精， 但如果量值测过 ， 多要需进行多感器融合传，时 算量过高计导致 实性下时。为降提了计高效率， 算本受无迹信文息 滤器启波
［1发8］
δρ
i k ， ， = i1， 2 ，， …1 2 + qa4－i ，k
－
（
30）
k^－ 方差阵 和P － k公由（ 式 6 ， （） 7 ） 得到。 预测均值x ．32 量更测新 通过式（公 10 计算）量测预测可得 1值 ^ kz－ =Z i ， k 2∑ ni 1=其 中测估计量容积点值
2n
（
1 ）
3
，出了提一种积容息信四数元估计器
（IQEC 并）将其用运卫星于态姿确定 对。于传多器感据数合过融的量程更新测 信，
息
第
期
2魏
喜庆： 基等于容积卡尔滤曼波的卫姿星估计态［18
］
19
7
波滤具线有性的形
：式m
率
更具 有 势。 优IQE 将C CQE 信与 息 滤波 框 架结 I， ∑ k s =1sm
Yk+ = Y k +－^ + k = y^k－ + y
（ 3
） （ 36 ）
7
合，因此 两者有具价等性同。时利用 m矢量观测 个如果用容积采四数估元计器， 值行进卫星态确姿，定在滤 更波时需要新 P对珓 法算的实性时难以z 珓 ， zk 求 ， 容积逆息四元信数计估器比相容积元数估四 计现。 实，器利 用公 式（37 ）进 行测更新量 ，信息态状是 对m个 测值所对应信量增量息累加的， 避了协免差矩 阵 P珓 采方用此方避法了对高免矩阵 维z 珓z， k 的求计算 。 逆求逆， 同时量测的新公式更更加洁简 ，因此相容比 四积元数计器更估加用于多传感适融器合。 5 仿 验真 证 对QEC和 CI E 进行卫星Q态姿计估仿真比，
对i
s ， k∑ =1
－1 sn2
－
^+yk 和－^ ky +信息状态为，其 中Yk和 Yk 为 信息矩，阵 有：
且Y
－
k
=
（P
－）
（k38 ） 3（ 9）
k^ = －1 Yk－ ∑ i，Xy 2n i 1 = －k
1
I ， s RHs ， =kH k^ +k H ^ x］ 珓i s， R z［ ， sk= Hk z
T－ s k －1 ，，s － kk－ kT s k，－ s，1 k
I ， k s和 i， ks 别为分信息矩增量和信阵息态状量增 ：（ 04 ） （14） （4 2）
T
定
伪义测量阵矩 Hk 有具下如形式
－： TH = k Yk P zx，k tk
象为运行在轨对高道度7 00k 圆轨m道三轴的稳定 星卫，轨道角 度为 速．006 6°5/ s设。卫星体系轨和道 坐系完标全合， 则卫重星系体对于惯性相角系度 速与轨坐道标相系于惯对系角速度相等性。 角度速 和度矢角测量装量置别利用分轴陀螺三和敏星感器。 公 式 1（ 9 中）螺陀测输出包含量的随机移漂准标差－5 1
/2 σv =1 ． 812 ×01° / ，s 漂移速率斜坡准标 差σu =
其将入带公到（ 式14 ）和（4 0 ）得到
－：I s， k Y = P xz，kk － si，k = Yk xz，Pk t
k
R k1 － P （x， zk
k
t T ） Y（ － ）k
（4 3）） T Yk－ ）（T ^xk－］ t （ 44k ）
^－
珓 R－1zs， ［ kk －zk +（ Pxz t， ks， k
^
， 积信息容四元数估计 器 0 ^β，q 假设 知 P0已 0
流，如程步骤：下 CIQ E算法 （1） 时间 测 ① 预生容积点与容积成元四数 估 器 类计 似 同 式（ ，25） ； ② 由 式 26（ ） ～（ 82 ）和姿运动学态方程 （18 ）生 ^ － i预测四成元数 q ， ； k③ 通式过（ 29 ）和 （3 0 得到误差修）罗德正格 参里；数x 和方差阵P ； （ 7 ） 算计测预均 值 ④^由式 6（），
－ － kk
1 ．
812 × 01－8 ° / s 3/ 2 ，姿态 量 测矢 量含 有 标 准差 为 0 ．005° 高的斯白声噪。 陀螺采样周期为1s， 星敏感 器的采 周样期为 0s， 仿真1时设间 为5个 道轨期周。为 了验证滤波器不在同情下的况计估精度 分三种 情况，进行仿：真 情况一，初 始度角估误差计小， 初 初始始角度计估差 陀螺误移估漂计误小差 情况二， ；， 大始初陀漂移螺估误差小计；情况 三，初 始角估 计误度差，大 初陀始螺移估计误差漂。大由 于QEC 和CQEI 在理论具上有价等， 而性且仿真时姿估态精计度非常接近 ，为 了显示晰， 未在图清画出，中 后面在 中表及以析时分细对详比两者估了计果 。结情 况 一： 首 先 设 滤假波 器 的 初值 0x = （ pδ 0 ）T［ T
T β ］ 都为0零， 初方差阵始P0 关中姿态角 于 部分2差阵方P a0 =（ 0． °） 2I ×33 陀螺，漂移方阵 差P0β =
（2 量）测新更－
k^ －别由分式（ 38 ） ① 信矩息 阵Y k和信 息态 状y
和
（ 3 ） 计算；9+
②由式 3（6 ）和 （3 7） 算信计矩阵 Y k息 和息 信^ + ，k^ k + = Y（k+ ） － y 1 ^k 得+到测量更 态状y 进 步一由公式
（ x． 10°/ h ）2 3I 3× 。 b 4，= 广义德罗格参里数选取 a中 1=，即选 取修正罗德为格里数参 图。1 中
角
度误差范数定义为 三角度轴计估误向量差的2 －数范， 图 2中 条曲 三线分为别轴角三度估误计。从图差 和1 图 可2以看 当出角和陀螺漂度移初值估误差计小时较 ，轴的三角 度滤精波度能达 0到． 00°2下以。 差曲误收线敛速较快，
度新
后的态状量； 向^ ；③ 由式 （ 4 3） （和 3 5） 得到更后新四的元 q ^ k+数的 前个三素重元置零为。④ 将测更新 量x
+k
由
信于息波滤质本上将是卡尔曼滤波协方用 差在行多进感传器合融计时效 算矩阵逆的形表式示，
918
宇学航报
第 34
卷图
1Fig． 1
情况一角 误度差数范线 F曲g． i
4图4
情况二
QEC角 估计度误差
Nom rfoatt tuide etsmationie rrorsof CQE fro csae1
Attitude
esitmtaonierror s of CQEf r oase 2
－c 05°，50 °， 60°， 三轴1螺漂陀移值初估误差分别为计
T 01°/ h 1 ° / 0h 1°0/ h ］ P0，关 姿态角部分于 0β= 2［陀螺漂 方移阵取为差P β0 =方差 P阵a0 = （ 4°0 ）3I×3 ，
（1 5° / ） h I2 ×3 3 由。 5图 和 6 可图 知CQ 的E收速敛虽 度但在3 时小仍内达稳定到状态。 然进一降步，
低图
2F gi．2
情况 一C EQ角 估计误差
度A
titute destmatiione rros or fCQE fo racs e1
况二情 ：三角度初值有较轴大计估差误别为分 － 5°0 50°， ，61°， P00 关陀 漂螺移初值计误估为零。
差 于2态姿部分角方阵差 Pa0 = （40 °） I 3×3， 陀螺 漂方差 移 阵2依取为然 Pβ0 = （ 0 ．° 1/ ）h I3× 。 由图 3 和3图 4可知
5 F图ig 5
情．况三角误差范度曲数线fo rcaes3
CQE
相对在于情况一快的速收， 敛在情二况需时要约 两个时滤波小器才达到能比高较估计精度的。
Norm
of atttudiee tsiamiot enrrosr o fCE
Q图
3Fi ．g 3
情况
角二度误差数范曲 Fi线． g 6of cares
2图6
情况
三 QCE角度估计误差
Nor
mof atitudetes timtian erororso fC EQ
Att
tidu estimetion areors rof QEC of rasc 3e
情 况
三： 角 初 度 值 较有大 估 误 计差分 别 为
容积
信四元息估数计在器多传感融合时器 具， 实有简单和现计算小量优点的， 在三种情况下仿真
第
期
2喜庆魏：等 基容于积尔曼卡滤波卫的星态估计姿
19
9果与结积四元数估容器计估精度类似 计。下面用采 Averae geMna quSra e均平均 方根 误差 （ oRotT meErroi，RrAMSE ） T来比 较CEQ C和QIE 估计的度。 精RAMTS 的定义E如下 RTMASE
=［21
］
6
结论
本提文出一种容了积四元估计数器用卫于星姿 态定。对于较确的初值估小计误差 ，新的滤器具 有良好的波估精度计 ，而当估计误差大较 CQE 也时 保持能好的较计估精和度快较收的敛速。 进一度，步 本文提了出种容积一信四息数估计器元算用法多于 算法与容新四元积传 感融器合时的卫姿态星计， 估估数器具计有相的估似计度
精
， 并滤且波式形单、 更简易于实现 参 。 考 文献
：T
MM 为蒙
卡罗仿特次数，真 其x T中为 总仿真步长的 ，^分别是三 轴角度值真及估其计值 。和 x QEC 和C IE Q波器在三滤种况下情行了进 0 2TAMRSE 表 1如所示， 蒙特次罗仿卡， 其真单为位度 从表中。可看以出种两波滤的估器精计相度 ，似 RTMSA 也E之随大， 随着增初始计误差估增， 大仍但能 持良保好的估精度。仿计结真果也验证前了关 于文CQE 和 CIQ 一致E的描述性
。1表Ta lbe1 两 滤波种器RTAM E S 表veraAg eRAMTSE f owto fitlers
况情 4．二 13 ×10 －1 4．11 1×0 －1 情况三 ． 70 0． 809
槡
1 ^
i k ∑ ik －xx MT∑ k 1= i =1
（ 46
）
［
1］ S oltlE L，tecsnikhJ， WaterlU ，et a． Onorblt iervsciing［J ． IEEE ］Rbootcsi ndaA tomatuoi Mngaaizne ，2009 16， （） 4 ：9 2－33． 2 ［］M cLogulihn T ， CHmapelblM ．D itsiburtd eseimtaetfusion fi letr of r．H nooull，uHI， U ntedi tateS： lasge rspacecarf ftomrtaois ［n］R Aermcina nIsttuti of Aereonatuics adnA srotnutiascI cn．， 2008． 3 ［ ］uMrerll JW．Pr eicsoi ntaitutd deetreimntain foo mrultimssionis pacercat［C］． Tfeh IAAA uiGdnace，Naivatgon，aid Cnotrnlo1 978 ．Cofnreecn，Peaol，ACU，ntide Staes， ［4 ］tA nderswS F ，ilaBnw So ．Recen flight restults o thf eRTMMKal anm filte［rC ． A］AA IuidGnac， Neavgaiiot， nnda onCrtloC onfernecean dE xhbit，iMo teneyr，CA U，itndes ttea，s 0220 ［． 5 Abd］leahmar nM， arkPS Y． S giampiont alkamnf ilterni forg pasccreatfat ttiud ane drta eestmiaiot unings mangetoetme reasuremenmt ［sJ ． IEEE T］rnasactiosnon eArospaceand
情况一 QECCI EQ5 ．0 9× 0 1 －45． 0 9 ×10 4
高－斯 － 若法当值数求解阵矩的计逆量约算 3n3 为/2 乘除法，次计算 随着量矩维阵的增加数呈而指图 7仅 制一绘轨个道周内期观测恒星 数个增长级 。从可以看出中大部在分间能时观到测颗或三颗 两数 ，最时能观测多四颗到恒星（ 仅续持个几测量周 星，恒每个 星提供恒维三星矢量，光因 此量测协方矩差 ）期。 IQCE 量测更新程， 阵过 珓P只对 z 珓z，k 最 能达多 到2 1。维 状态方差协阵矩求逆算运 运，算只量状态和数维 关， 而不会相随量测数的增加据导而计致量急剧算 加，增 使在即维数高更的情况能也保实持时性。
E
letrcoin Scstyesm 20，11， 4 （ 72 ： ）1041 1－41． 5［ ］ L6ffeetr sE ，JarkMely F ，ShLsuet rMD． K amanlfilt eirgn of rpacsecarf attttiude etimsatio［ Jn］．Jo ruan oflGuidance，C ntoorl，a d nDnymaci， s1892 5 ， 5（ ）： 41 － 724． ［97 ］范 石春，张高飞，子阳，孟等． 种基一于线性量测方程伪的无 螺姿态陀确方法定［］J 宇．航报，2学08 02，9 4（ ） ：1902－ 216． ［9FnaC uhnsih ，ZhngaG oafi eMe，n Zigang y，
et a． lNvolequ aterion nalKma nilfetr for gryoles astittdeu deermitationn［ J］． Jo rnula of sAtroautncis， 008 ， 229（ 4 ： ）2109－ 1296．］ ［ ］ Cr8ssidias J L，MakrlyeF L． Usncneet fdlteiinrg ofrs acpcreatf atttude eitismtaoi ［J］n． Jounal orfG idanue，c oCtnol， and Dynarmcis， 0032， 26 （ 4 ） 5：6 －3 524． ［9 ］记陈争，袁平建方群．，基 于修正 Rordigus e参和数 UF 的 K 宇航．学，2报080 ，29 5 （）： 1 62 2 1626－ ．态估姿算计法J［ ］［C hne Jihengz，uYa Jniapnnig，aFn gun．QAtti uted stiemaion utings mdoiiefd odrrguie psraaetemsra nd KU［J］． FournJa ofl
图7
F i． g7
道轨周观测期恒个星数
A
tsroaunits，c 2080 ， 29 （ ） 5： 1262－ 162． 6 ］［ 1］ Car0im ， OsAham Y．nFas tp aticrel fitlring efroa tittdu eadn angulraraet etsmaitionf romv ctor ebosevatiron［s］J ．Jurona lf
oumbNe rf ovaaiallbes ats in ercahpe iodr
02
Gu0idanc，eCotnolra，ndDy nmaisc 2009， ，32 （ 1） ： 07 －78．
宇航学报
2
11 ， 02 3 2 （）： 336 － 34．2］
第34
［ 11］ 卷张惟林，军宝，泽明张，等．矢量 测观定确卫姿星态预测 的． 航学报宇2，011，3 2 （ 5） ： 017 7 －0158 粒．子滤波法算［J］ ［Zh nagW i， LineB ojau，n hZng aZmien， egtal． P artcilefiltering fusnig perdictie fvilterf roa ttitue dedtemirntaon ifor vmetcroob servtiaon［Js．］J orualnof A stroauticns，2011 ， 3 2（ ）5 ：107 －7 015．8 ］［ 12 ］rAasraatam I，naHkyin S ．ubCautr ealKamn ilfets［Jr． ］EEE I002 9 ，4 5 6（） ： 2145 － 1296 ．rTasantioncs on Autoamit cCnorolt， ［ 1］ A3rsaartnamaI，Hayk i n，SHurd TR C．ubaure taKmla fnitleinrg ． for cotnnuouisdsicrte syestem： Tsehry and osmulitiaos n［J ］EEI TEarnactsoinson Sig al Procenssni，g0120，58 （10） ： 4977 － 499． ［3 4］1S huste Mr． sAurvy efo atitutedrepres neationt［J］ s．Jou rnl oaf1 99 ，341 （ 4 ： ）43 － 5971 ．het sAtronuticaal cieSnces ［ 1，］5 Kim SG C，rassdii J sLC，enhgY， et la ．aKlmna iflteirngf o r］ ．JJ uonarl ofr latieve pasecraft atcittue anddp siotoi esnimattoin［ 2007 ， 30（ 1） ：1 33－ 14． G3idaucenC，noroltan， dDnaymic， ［ s6］ 1郭， 魏庆瑞轩， 洁许， ．等 于基MRPs NP/UP 的F地磁/ 加速 J度． ］宇航报，学 2101， 32 2 （ ） ：363计 测的量态估计新方姿［法－ 342．［ Guo Q ni，g eW Riuiuax，n Xu Je，iet a l ． Aew nmthoedf ro atttiue desitmtiaon ni mganteoemert adna cclereomtee atrttuid ．e ournaJl f ostroAnuaict，s ystsme asbe dn MoRs aPd nPUNP［JF
］［
17 ］chSaubH，J nkunsi J L St．ereograhpcior intatieno aprmetears of J］r． a titutded yamnisc：a egenalizatironof t he Rdriguoes paaremerts ［9916 ，4（41 ） ：1 － 1． 9oJruna olfthe strAnoaticul Scaienesc，［ 18 L］e e JD Nonlinear．esti amiont na mdlutilpes neosrf usino suin gunc
senedt informatoi niftelrign J［． ］EIEE iSganl Pocrsesign 0082， 15 ：8 16－ 8 4．6L tetes，r ［ 9］ C1rsasiids J ，arMkley F Att．iutd eesimttioa usnig nmodfiid Reordguei ．sT he Ameriacn AtsonarticulaS cieot Fy． Ladnisp raamteersC］［M ralek yAtsonautrisc Smypoium，Gserebnlt，eMD，Unitd Staees，t19 96 ．［20 ］ rassiCid J，Jsnukns Ji Op．tiam lestmation io fydnmic systemas［ ］．MBo caRa otn ：hapCma ＆nH all ，204 0：4 19 － 433 ［．2 1 ］rAluapamaml M S，RsticiB，G rdoon Ne， tla．B erangsinlo tracyikgn o mafoenuvinrgt raetgsus ignpartic l eilftrs［e］． JErauipsJ uorna onl Apliep dSingl arPoescisn，g 020 4（ 15） ： 2 351 －362．5
作简者介 魏：喜庆 （9182 ）－， 男， 士生博 ，研究向为方线性非滤和卫星波 对导相航。 通信地址： 工大科学园 303哈 2信箱 （510010） 电 话 ： （0415 8）462204 － 02182E mia：l weixqiin@ gmgail c．m
o（
辑编 曹亚君：）