第34卷第2期2013年2月
宇航学报
JournalofAstronautics
V01.34February
No.22013
基于容积卡尔曼滤波的卫星姿态估计
魏喜庆,宋申民
(哈尔滨工业大学控制理论与制导技术研究中心,哈尔滨150001)
摘要:为了获得更好的估计精度和滤波稳定性,提出了一种基于容积卡尔曼滤波(CubatureKalmanFilter,
CKF)的容积四元数估计器(CubatureQuaternionEstimator,CQE)估计卫星姿态。靳方法利用四元数进行姿态更新,同时采用广义罗德里格参数表示误差角,有效地避免了滤波过程中的奇异。为克服多传感器融合时运算效率低的问题,通过容积四元数估计器与信息滤波相结合,提出了一种容积信息四元数估计器(Cubature
关键词:姿态估计;容积卡尔曼滤波;容积四元数估计器;容积信息四元数估计器中图分类号:V448.2
文献标识码:A
文章编号:1000—1328(2013)02-0193-08
Information
QuateruionEstimator,CIQE)。仿真表明角度和陀螺漂移初始估计误差较大时,新方法仍能取得良好的估计性能。
DOI:10.3873/j.issn.1000—1328.2013.02.007
CubatureKalmanFilter.BasedSatelliteAttitudeEstimation
WEIXi—qing,SONGShen—min
(CenterofContmlTheoryandGuidanceTechnology,HarbinInstituteofTechnology,Harbin150001,China)
Abstract:Anewsatelliteattitudeestimationmethodnamedcubaturequaternion
Kalmanfilter(CKF)isproposedforbetterestimationperformancein
estimator(CQE)based
on
cubature
termsofnumericalaccuracyandfilteringstability,
AttitudeispropagatedbyusingquaternionwhilegeneralizedRodriguesparametersareemployedtoexpressangularerrorsforavoidingsingularityofthenewmethod.Thecubatureinformationquaternion
estimator(CIQE)is
ofmultiple
proposedbycombining
fusion.Simulations
CQE
and
information
filteringparadigmtoovercomelow
efficiency
onaccountsensor
demonstratethatthenewmethodachievesdesired
performanceundertheconditionoflargeinitial
quaternion
errors.
Keywords:Attitudeestimation;CubatureKalmaninformationquaternion
filter(CKF);Cubatureestimator(CQE);Cubature
estimator(CIQE)
0引言
降和滤波器收敛速度缓慢的问题¨1。为了进一步
改善卫星姿态估计性能,UKF被用于卫星姿态估计,通过经uT变换后的采样点集来逼近非线性函
数概率分布,姿态估计结果较EKF具有更好的鲁棒性[8。9J。粒子滤波用于卫星姿态估计,具有更好的
无论是执行对地观测、在轨服务还是编队飞行任务,精确已知姿态信息是卫星完成任务的前
提[1。]。矢量观测敏感器和陀螺构成的卫星姿态估
计系统,是实现高精度卫星姿态确定的标准方案[3。]。事实上,无论是利用欧拉角、罗德里格参数还是四元数来描述航天器的姿态运动学,都具有非线性特性,因此经常采用EKF进行状态估计∞J。但是由于EKF经过一阶线性化近似后,忽略了部分非线性特性,在初始误差较大时,存在估计效果急剧下
收稿日期:2011-12-27;
修回13期:2012-08-06
收敛速度和估计精度¨¨¨j。由于粒子滤波是一种
基于蒙特卡罗仿真的滤波方法,理论上只要粒子数目足够多,其对非线性、非高斯系统可以达到任意的
估计精度。但与其他滤波方法相比,粒子滤波随着
粒子数目增多而导致计算量增加,因此粒子滤波难以满足卫星姿态估计工程应用中对实时性的要求。
基金项目:国家自然科学基金(61174037)
万方数据
194
宇航学报第34卷
为了更好地满足卫星姿态估计要求,有必要研
究精度更高、工程实现更简单的方法。容积卡尔曼滤波就是近年来提出的一种非线性高斯滤波方法‘1卜13J。CKF具有严格的数学证明,通过三阶容
积法则数值积分近似加权高斯积分,充分利用了其在多维函数积分数值运算中具有的高效率特点。容积卡尔曼滤波具有等权值的2n个容积点(/Z为系统状态维数),经证明其逼近非线性变换后的概率分
布精度优于UKF【12]。
基于四元数描述的姿态运动学方程具有线性形
式且易于推导解析解,在卫星姿态更新中较其他姿
态描述方法具有更大的优势¨4。15I。因此在进行卫
星姿态估计时,经常采用四元数进行姿态更新。然而由于四元数的单位模限制,在进行协方差阵更新时会造成协方差阵奇异。利用罗德里格参数表示角
度误差,可以有效避免四元数协方差阵更新时出现的奇异现象【l6|。文中采用的广义罗德里格参数是
一种包含罗德里格参数和修正罗德里格参数的更广
义的三维角度描述形式¨7。。另外,卫星通过多传感
器融合进行姿态估计时,普通的滤波器存在计算效率不高的问题。将UKF与信息滤波框架结合,有效地提高非线性系统的多传感器融合的效率,但是其估计精度还有进一步提高的空间¨8l。
在上述研究基础上,本文提出了容积四元数估计器,在进一步提高卫星姿态估计精度的同时,通过四元数与广义罗德里格参数结合的方法,避免了由于状态冗余导致的协方差阵奇异。进一步,通过容积四元数估计器与信息滤波的结合,提出了容积信息四元数估计器。虽然其与容积四元数估计器本质上是等价的,但在多矢量观测情况下能有效提高卫星姿态估计的计算效率。最后将两种方法应用于三轴稳定卫星的仿真研究,验证了所提出方法及时在角度和陀螺漂移初始估计误差较大的情况,仍然具
有良好的估计精度和收敛速度。
1
CKF滤波
考虑具有加性噪声的非线性系统状态方程和量
测方程:
Xt=八X¨,口¨)+’.,¨(1)z^=h(X‘,H^)+l,I
(2)
万方数据
其中x。∈R“是系统在k时刻的状态,U㈧∈R~是系统的输入,z。∈R“是系统的量测值。w¨和l,。为不相关零均值高斯白噪声,其协方差阵分别为Q^一。
和冠k。
非线性高斯滤波的核心问题是求解多变量非线性函数与高斯密度函数乘积的积分。
Arasaratnam【12o等人通过三阶容积积分法则,利用
2n个容积点加权求和近似计算加权高斯积分,对于函数f(x)的加权高斯积分:
“(,)=If(x)N(z;肛,P)dx
J
R“
。
2n
一2t-n2f(P+庐孝。)…I=l
(3)
其中N(x;肛,P)表示X服从均值,正和协方差阵P的
正态分布,乒为协方差阵尸的平方根,满足痧渺)7
=P,具有2凡个元素的容积点集旧}具有如下
形式:
一1
00
0
:
:
●
●
0一1
其第i个容积点煮是n维列向量。
从而在贝叶斯估计框架下,利用容积积分法则,提出了CKF算法。假设在第k时刻已知前一时刻后验密度函数p(x¨J
z㈨一。)=N(x¨;王乏,,哎。),
CKF算法流程如下:
(1)时间预测
①计算容积点:
墨扣。=√PI。孝。+J0。,i=1,…,2n(4)
②容积点传播:
x。:^=f(Xm小U㈧),i=1,…,2n(5)
③估计预测均值和协方差阵:
.
2n
王f
2五1萎置▲
(6)
,
2n
Pi=去∑Xi:。(xi:。)’一;i(王i)’+Q。一。(7)
(2)量测更新
①计算容积点
置.^=√Pi亭i+工i,i=1,…,2n(8)
②容积点传播:
Zm=h(X“,U‘),i=l,…,2n
(9)
第2期
魏喜庆等:基于容积卡尔曼滤波的卫星姿态估计
195
③计算量测预测值、新息方差和协方差矩阵ii
2刍荟z“
(10)
P甜2刍荟Zi,kZijt一“i)’+Rt(11)匕广刍荟x泓Z“T一王眩)7
(12)
④计算量测更新:
羹=五+K(z。一乏),其中甄=巴.。巧:(13)
P:=P;一KkP矗?℃砭(14)
2卫星姿态运动学
2.1姿态运动学模型
单位四元数g=[P7
q。IT的乘法具有inT定
义㈨:
口”卸Qg
2
f…q'4q卯,-(叫P'∥)Vp抽】(15)
符号0表示四元数乘法运算符,式中
其中P;,P:和P;是p’的三个分量。
定义误差四元数两=[(印)1却。]7具有如
下形式:
田=qo蚕一
(17)
其中毒一为四元数毒的求逆运算,对于单位四元数
逆即共轭。
四元数的姿态运动学方程如下:
i
2丁1【‘oD]◇9
(18)
其中tO是体系相对于惯性系的角速度在体系下的
表示。
陀螺角速度输出采用经典模型:
∞=葫一卢一田。卢=叼。
(19)
其中西为陀螺输出,卢为陀螺漂移,叼。和71。为不相关零均值高斯白噪声,其协方差阵分别为矿l。1,。,和
0"213×3。
万方数据
it=[二]{。+y*=[二:二;二]}。+yt
e2。,
A(g)=(q:一IIt,||2)厶。3—2q。[P×]+2pt,7
栅嘶趟估懈器
翻美怫&洲提拥拙韵算
匠积
磁断拾黼估古
(CQE)。公式(18)和(19)构成了系统的状态方程,
:一
虢拼
啉耱嚣公式(20)是系统的量测方程,系统的状态向量为[q1
p1]1。
为了避免四元数状态冗余造成协方差阵出现奇
异的情况,在CQE的设计中将状态向量选为X=[(印)1卢’]7,其中印为式(17)中误差四元数却
对应的误差广义罗德里格参数:
印=b羔
(23)
参数a从0到l取值,b为尺度参数。当a=0,b=1
式(23)退化为罗德里格参数,a=1,b=1时式(23)退化为修正罗德里格参数,广义罗德里格参数的奇异点在180。到360。间。当选取b=2(a+1)
时,l|印lI与欧拉定理定义的旋转角在小角度时近似
相等。由却到却的对应关系由文献[17]可知
6q4=出业菩赫蔓必
0
十IlAn旷
3.1时间更新
196
宇航学报
第34卷
劫=b。1(a+曲。)印
(24)
已知滤波初值R,q“。,风,由第k一1时刻状态估计值和协方差阵生成容积点:
‰・2戮1],…,…’12(25)
其中却“一,和p“一,分别表示容积点的姿态误差角和陀螺漂移。由于在每一个滤波周期开始时刻,均采用状态后验估计作为初始值,所以在预测开始时,式
(25)中的姿态误差部分却m一。恒等于0。为了避免
姿态更新时产生奇异,先将误差修正罗德里格参数
转换为容积点误差四元数,利用公式(24)和(17):
地m,————瓦币i而—一。+
一a}lSp洙一,J12+b√62+(卜a2)IJ印泓~,j12
却i¨=b。1[o+isq4+i“,]却“小江1,…,12(26)
由容积点误差四元数得到容积点四元数点集:
云㈦=却i川。证。,
i=1,2,…,12
(27)
对卫星姿态运动方程进行离散化处理,当采样时间内转过的角度满足lIAteoll<<1,过程噪声方差阵具有如下形式【l引:
Q:『-‘z△。:-÷矿:(△D。厶×3l一(寻仃:(&)2)13。,(0-2,At)13。,一(÷盯x△幻2)Jr3妇]j
容积点四元数蚕i。由姿态运动学方程计算,
式(18)可以利用四阶龙格库塔法求解,本文采用文
献常用的解析形式阳圳,其对应容积点的估计角速度为:
O—Ji,t=矾一l一#¨,i=1,2,…,12(28)
通过姿态运动学方程得到蚕f。,进一步
锄ft=虻女o(qk+-。)~,
j=1,2,…,12
(29)
将容积点误差四元数转换为误差修正罗德里格参数
矗at,I:b墼,i:1,2,…,12(30)
a十94“
预测均值;f和方差阵矸由公式(6),(7)得到。
3.2量测更新
通过公式(10)计算量测预测值可得
“万∑一’n厶i,
i,k
(31)
\。1/
其中量测估计值容积点万方数据
A
^口
^
n
Z
=
e
(32)
A^拿々o
协方差矩阵计算采用公式(11)和(12),进一步求取滤波增益矩阵K,从而误差修正罗德里格参数和陀螺漂移的量测更新:
xk=zk+Ⅸ%L;:=;i+K(三。一ii)(3333)
zt—z^,J其中露=[(印÷)’(西:)7]To接下来计算更新四
元数和陀螺漂移。陀螺漂移即为王:后三项卢:,更新误差四元数却:为
妇-2————丁可丽了『—一
。+
一all(印:)7|J2+b√62+(1一a2)|I(印;)1112
却:=b‘1(口+硼0)(印:)1
(34)
进一步:
蚕:=曲:o蚕i(35)
假设已知P。,玩,JB。,将CQE用于卫星姿态估
计算法流程归纳为如下步骤:
CQE算法(1)时间预测
①利用式(25)生成容积点集鼍如。;
②由式(26)~(28)和姿态运动学方程(18)生
成预测四元数蚕f。;
③通过式(29)和(30)得到误差修正罗德里格参数;
④由式(6)和(7)计算预测均值蠢和方差阵矸;
(2)量测更新
①利用式(31)生成量测预测值三i;②采用式(33)计算量测更新;:;
③由式(34)和(35)得到更新后的四元数靠;
④将量测更新曼:的前三个元素重置为零。
虽然CQE具有良好的估计精度,但如果量测值过多,需要进行多传感器融合时,计算量过高导致实时性下降。为了提高计算效率,本文受无迹信息滤波器启发¨8l,提出了一种容积信息四元数估计器
(CIQE)并将其运用于卫星姿态确定。
对于多传感器数据融合过程的量测更新,信息
4容积信息四元数估计器
第2期魏喜庆等:基于容积卡尔曼滤波的卫星姿态估计197
滤波具有线性的形式Ⅲ】:
y:=K+∑t,;
(36)
y—l=Y‘k+∑l’s,k=
+乞
(37)Lj,)
其中K和玎为信息矩阵ji和露为信息状态,且有:
K=(竹)“
(38)1
2n
多i=麦K善Xi扣-
(39)
t.。和f“分别为信息矩阵增量和信息状态增量:
t,t=HmxR鼬-1日“
(40)i班=H¨T
Rm三“一蠢+日k;i]
(41)
定义伪量测矩阵巩具有如下形式:
磁=K吒,。I。。
(42)
将其带人到公式(41)和(40)得到:t.t=YkP。,^I。。露i1(P。,^I。。)7(K)T
(43)f小=K咒几戤‰一乏+(巴A)T(Yk)TXk]
(44)
假设已知P。,;。,风,容积信息四元数估计器
流程如下步骤:
CIQE算法(1)时间预测
①生成容积点与容积四元数估计器类似,同
式(25);
②由式(26)一(28)和姿态运动学方程(18)生
成预测四元数;i。;
③通过式(29)和(30)得到误差修正罗德里格
参数;
④由式(6),(7)计算预测均值乏和方差阵ei;
(2)量测更新
①信息矩阵K和信息状态歹f分别由式(38)
和(39)计算;
②由式(36)和(37)计算信息矩阵聪和信息
状态多:,进一步由公式;j=(矸)‘1多:得到量测更
新后的状态向量;
③由式(34)和(35)得到更新后的四元数毒:;
④将量测更新;:的前三个元素重置为零。由于信息滤波本质上是将卡尔曼滤波用协方差矩阵逆的形式表示,在进行多传感器融合时计算效万方数据
率更具有优势。CIQE将CQE与信息滤波框架结
合,因此两者具有等价性。同时利用m个矢量观测值进行卫星姿态确定,如果采用容积四元数估计器,
在滤波更新时需要对巴.。求逆,算法的实时性难以
实现。容积信息四元数估计器相比容积四元数估计器,利用公式(37)进行量测更新,信息状态是对m个量测值所对应信息增量的累加,避免了协方差矩
阵巴.。的求逆计算。采用此方法避免了对高维矩阵
的求逆,同时量测更新公式更加简洁,因此相比容积四元数估计器更加适用于多传感器融合。
5仿真验证
对CQE和CIQE进行卫星姿态估计仿真对比,对象为运行在轨道高度700km圆轨道的三轴稳定卫星,轨道角速度为0.06560/s。设卫星体系和轨道
坐标系完全重合,则卫星体系相对于惯性系角速度
与轨道坐标系相对于惯性系角速度相等。角速度和角度矢量测量装置分别利用三轴陀螺和星敏感器。公式(19)中陀螺测量输出包含的随机漂移标准差盯。=1.812×10。5。/s¨2,漂移速率斜坡标准差矿。=
1.812
X
10’8。/sⅣ2,姿态矢量测量含有标准差为
0.005。的高斯白噪声。陀螺采样周期为1S,星敏感器的采样周期为lOs,仿真时间设为5个轨道周期。
为了验证滤波器在不同情况下的估计精度,分三种情况进行仿真:情况一,初始角度估计误差小,初始陀螺漂移估计误差小;情况二,初始角度估计误差大,初始陀螺漂移估计误差小;情况三,初始角度估计误差大,初始陀螺漂移估计误差大。由于CQE和CIQE在理论上具有等价性,而且仿真时姿态估计精
度非常接近,为了显示清晰,未在图中画出,在后面
表中以及分析时详细对比了两者估计结果。
情况一:首先假设滤波器的初值Xo=
[(印。)1赋]1。都为零,初始方差阵Po中关于姿态角
部分方差阵,么=(0.2。)2厶m陀螺漂移方差阵JP盘=
(0.10/h)2厶肿广义罗德里格参数中选取o=1,b=4,
即选取为修正罗德里格参数。图1中角度误差范数定义为三轴角度估计误差向量的2一范数,图2中三条曲线分别为三轴角度估计误差。从图l和图2可以看出当角度和陀螺漂移初值估计误差较小时,三轴的角度误差曲线收敛速度较快,滤波精度能达到0.002。以下。
198
宇航学报
第34卷
一。一\藩靼巡援
图1
情况一角度误差范数曲线
Fig.1
NormofattitudeestimationenDrs
of
CQE
for
case
1
未z
X10’3
搬0
霪-2
O
l2345678
^
≮2
×10-3
搬0
蓬-2
0
12
3
4
5
67
8
p
×10-3
≮2
援0
饕一:O
l2
45678
f/h
图2情况一CQE角度估计误差
Fig.2
Attitudeestimation
en’ors
of
CQE
for
case
1
情况二:三轴角度初值有较大估计误差分别为
一50。,500,160。,陀螺漂移初值估计误差为零。只关于姿态角部分方差阵P胡=(40。)213小陀螺漂移方差
阵依然取为%=(0.1。/h)213。,。由图3和图4可知
两个小时滤波器才能达到比较高的估计精度。
fV
\
藏耀趟避
图3情况二角度误差范数曲线
Fig.3
Norm
ofattitudeestimation
et,Tors
of
CQE
for
ease
2
情况三:角度初值有较大估计误差分别为
万方数据
乏:
×10-3
援0
霪。:
0t2345678
o×l伊3
2
援0星.2害0l
2345678
<2
×10。’
藿.0:
O
12
3
4
5
67
8
t,h
图4情况二CQE角度估计误差
Fig.4
Attitudeestimation
en‘ors
of
CQE
for
ease
2
—50。,50。,160。,三轴陀螺漂移初值估计误差分别为
lO。/h
loo/h]T只关于姿态角部分
一o
\
氟辗麒援
图5情况三角度误差范数曲线
Fig.5
Normofattitudeestimation
el'l'ors
of
CQE
for
ease
3
乏:
×10-3
援0
萋-2
012
345678
f
≮2
×100
艘0
羹.z0
l234567
8
图6情况三CQE角度估计误差
Fig.6
AttitudeestimationerrOrsof
CQE
for
ease
3
容积信息四元数估计器在多传感器融合时,具Bo=[10。/h
方差阵P砷=(40。)2j,小陀螺漂移方差阵取为P舯=
(15。/h)213肿由图5和图6可知CQE的收敛速度虽
然进一步降低,但在3小时内仍达到稳定状态。
CQE相对于在情况一的快速收敛,在情况二时需要约有实现简单和计算量小的优点,在三种情况下仿真
第2期
魏喜庆等:基于容积卡尔曼滤波的卫星姿态估计199
结果与容积四元数估计器估计精度类似。下面采用
平均均方根误差(Root
Time—AverageMean
Square
Error,RTAMSE)来比较CQE和CIQE的估计精度。
RTAMSE的定义如下旧1I:
RTAMSE
2√高荟荟ttx:一王:II(46)
厂■—r1『———一
其中丁为总的仿真步长,M为蒙特卡罗仿真次数,X和i分别是三轴角度真值及其估计值。
CQE和CIQE滤波器在三种情况下进行了20次蒙特卡罗仿真,RTAMSE如表1所示,其单位为度。从表中可以看出两种滤波器的估计精度相似,随着初始估计误差增大,RTAMSE也随之增大,但仍能保持良好的估计精度。仿真结果也验证了前文关于CQE和CIQE一致性的描述。
表1两种滤波器RTAMSE表
Table1
AverageRTAMSEoftwofilters
高斯一若当法数值求解矩阵逆的计算量约为3n3/2次乘除法,计算量随着矩阵维数的增加而呈指数级增长。图7仅绘制一个轨道周期内观测恒星个数,从中可以看出在大部分时间能观测到两颗或三颗恒星,最多时能观测到四颗恒星(仅持续几个量测周期)。每爪J叵星提供三维星光矢量,因此量测协方差矩
阵P¨;最多能达到12维。CIQE量测更新过程,只对状态协方差矩阵求逆运算,运算量只和状态维数相
关,而不会随量测数据的增加而导致计算量急剧增
加,即使在维数更高的情况也能保持实时眭。
图7轨道周期观测恒星个数
Fig.7
Numberofavailablestarsineachperiod
万方数据
6结论
本文提出了一种容积四元数估计器用于卫星姿态确定。对于较小的初值估计误差,新的滤波器具有良好的估计精度,而当估计误差较大时CQE也能保持较好的估计精度和较快的收敛速度。进一步,本文提出了一种容积信息四元数估计器算法用于多传感器融合时的卫星姿态估计,新算法与容积四元数估计器具有相似的估计精度,并且滤波形式简单、
更易于实现。
参考文献
rL●1J
StoU
E,ktschrIikJ,WalterU,eta1.On—orbitservicing[J].IEEE
RoboticsandAutomation
Magazine,2009.16(4):29-33,
2
1J
MeLoughlinTH,CampbellM.Distributedestimatefusionfilterforlargespacecraft
fornmtions[R].Honolulu,HI,United
States:
AmericanInstituteofAeronauticsandAstronauticsInc.,2008.
rL3
MurrellJ
W.Precisionattitudedeterminationformuhimission
spacecraft[C].The
AIAA
Guidance,Navigation,andControl
Conference,Palo,CA,UnitedStates,1978.
rL4AndrewsSF,BilanowS.RecentflJightresultsoftheTRMMKalman
filter【C].AlAAGmdance,Navigation,and
Control
Conferenceand
Exhibit,Monterey,CA,Unitedstates,2002.
rL51J
AbdelrahmanM,ParkSY.Sigma—point
kalmanfilteringforspacecraft
attitude
and
rate
estimationusing
magnetometer
measurements[J].IEEE
Transactions
on
Aerospace
and
Electronic
Systems,2011,47(2):1401—1415.
rL6
1J
LeffertsEJ,MarkleyFL,Shuster
MD.Kalmanfilteringfor
spacecraftattitudeestimation[J].Journal
ofGuidance,Control,
and
Dynamics,1982,5(5):417—429.
r●L
71j范春石,张高飞,孟子阳,等.一种基于线性伪量测方程的无陀螺姿态确定方法[J].宇航学报,2008,29(4):1290—1296.[Fan
Chun-shi。ZhangGao-fei,MengZi—yang,eta1.
NovelquaternionKalmanfilterforgyroless
attitudedetermination
[j],]oumal
of
Astronautics,2008,29(4):1290—1296.]
rL81J
CrassidisJL.Markley
FL.Unscentedfilteringforspacecraft
attitude
estimation[J].Journalof
Guidance,Control,and
Dynamics,2003,26(4):536—542.
rL9
1J
陈记争,袁建平,方群.基于修正Rodrigues参数和UKF的姿态估计算法[J].宇航学报,2008,29(5):1622—1626.[ChenJi-zheng,YuanJian-ping,FangQun.Attitude
estimation
usingmodified
rodriguesparameters
and
UKF[J].Journal
of
Astronautics,2008,29(5):1622—1626.]
rLK1J
Carmi
A,OshmanY.Fast
particle
filteringforattitude
andangular—rateestimationfrom
vector
observations[J].Journal
of
200
宇航学报
第34卷
Guidance,Control,and
Dynamics,2009,32(1):70一78.
张惟,林宝军,张泽明,等.矢量观测确定卫星姿态的预测粒子滤波算法[J].宇航学报,201l,32(5):1077—1085.[ZhangWei,LinBao-jun,Zhang
Ze—ming,eta1.Particlefiltering
fusing
predictive
filter
for
attitude
determination
from
vector
observations[J].JournalofAstronautics,2011,32(5):1077—1085.]
[12]
ArasaratnamI,HaykinS.CubatureKalmanfilters[J].IEEETransactions
on
Automatic
Control,2009,54(6):1254—1269.
[13]ArasaratnamI.HaykinS.HuMTR.CubatureKalmanfilteringforcontinuous—discretesystems:Theoryand
simulations[J].
IEEETransactionson
Signal
Processing,2010,58(10):4977
—4993.
[14]
ShusterM.A
survey
ofattitude
representations[J].Journal
of
theAstronautical
Sciences,1993,41(4):439~517.
[15]KimSG,CrassidisJL。ChengY.eta1.Kalman
filteringfor
relativespacecraftattitudeandpositionestimation[J].Journal
of
Guidance,Control,and
Dynamics,2007,30(1):133—143.[16]郭庆,魏瑞轩,许洁,等.基于MRPs/NPUPF的地磁/加速度计测量的姿态估计新方法[J].宇航学报,2011,32(2):336—342.[GuoQing,Wei
Rui-xuan,XuJie,eta1.A
new
method
forattitudeestimationinmagnetometerandaccelerometerattitudesystembased
on
MRPsand
NPUPF[J].JournalofAstronautics,
万方数据
2011,32(2):336—342.]
[17]
SchaubH.JunkinsJL.Stereographieorientationparametersfor
attitudedynamics:ageneralizationoftheRedriguesparameters[J].
JournaloftheAstronautical
Sciences,1996,44(1):l—19.
[18]
LeeD
J.Nonlinearestimationandmultiplesensorfusionusing
unscented
information
filtering[J].IEEE
SignalProcessing
Letters,2008,15:861—864.
[19]
Crassidis
J.MarkleyF.AttitudeestimationusingmodifiedRodrigues
parameters[C].The
American
Astronautical
Society
F.Landis
MarkleyAstronauticsSymposium,Greenbelt,MD,United
States,
1996.
[20]
Crassidis
J.JunkinsJ.Optimalestimationofdynamicsystems
[M].Boca
Raton:Chapman&Hall,2004:419-433.
[21]
Arulampalam
MS,RisticB,GordonN,eta1.Bearings—only
trackingofmanoeuvringtargetsusingparticlefilters[J].Eurasip
Journal
on
AppliedSignalProcessing,2004(15):2351—2365.
作者简介:
魏喜庆(1982一),男,博士生,研究方向为非线性滤波和卫星相对导航。
通信地址:啥工大科学园3032信箱(150001)电话:(0451)86402204—8212
E—mail:weixiqing@gmail.corn
(编辑:曹亚君)
基于容积卡尔曼滤波的卫星姿态估计
作者:作者单位:刊名:英文刊名:年,卷(期):
魏喜庆, 宋申民, WEI Xi-qing, SONG Shen-min
哈尔滨工业大学控制理论与制导技术研究中心,哈尔滨,150001宇航学报
Journal of Astronautics2013,34(2)
参考文献(21条)
1.Stoll E;Letschnik J;Walter U On-orbit servicing[外文期刊] 2009(04)
2.McLoughlin T H;Campbell M Distributed estimate fusion filter for large spacecraft formations 20083.Murrell J W Precision attitude determination for multimission spacecraft 19784.Andrews S F;Bilanow S Recent flight results of the TRMM Kalman filter 2002
5.Abdelrahman M;Park S Y Sigma-point kalman filtering for spacecraft attitude and rate estimation using magnetometermeasurements 2011(02)
6.Lefferts E J;Markley F L;Shuster M D Kalman filtering for spacecraft attitude estimation 1982(05)7.范春石;张高飞;孟子阳 一种基于线性伪量测方程的无陀螺姿态确定方法[期刊论文]-宇航学报 2008(04)
8.Crassidis J L;Markley F L Unscented filtering for spacecraft attitude estimation[外文期刊] 2003(04)9.陈记争;袁建平;方群 基于修正Rodrigues参数和UKF的姿态估计算法[期刊论文]-宇航学报 2008(05)
10.Carmi A;Oshman Y Fast particle filtering for attitude and angular-rate estimation from vector observations[外文期刊]2009(01)
11.张惟;林宝军;张泽明 矢量观测确定卫星姿态的预测粒子滤波算法[期刊论文]-宇航学报 2011(05)12.Arasaratnam I;Haykin S Cubature Kalman filters[外文期刊] 2009(06)
13.Arasaratnam I;Haykin S;Hurd T R Cubature Kalman filtering for continuous-discrete systems:Theory and simulations[外文期刊] 2010(10)
14.Shuster M A survey of attitude representations 1993(04)
15.Kim S G;Crassidis J L;Cheng Y Kalman filtering for relative spacecraft attitude and position estimation[外文期刊]2007(01)
16.郭庆;魏瑞轩;许洁 基于MRPs/NPUPF的地磁/加速度计测量的姿态估计新方法[期刊论文]-宇航学报 2011(02)
17.Schaub H;Junkins J L Stereographic orientation parameters for attitude dynamics:a generalization of the Rodriguesparameters 1996(01)
18.Lee D J Nonlinear estimation and multiple sensor fusion using unscented information filtering[外文期刊] 200819.Crassidis J;Markley F Attitude estimation using modified Rodrigues parameters 199620.Crassidis J;Junkins J Optimal estimation of dynamic systems 2004
21.Arulampalam M S;Ristic B;Gordon N Bearings-only tracking of manoeuvring targets using particle filters 2004(15)
本文链接:http://d.wanfangdata.com.cn/Periodical_yhxb201302007.aspx
第34卷第2期2013年2月
宇航学报
JournalofAstronautics
V01.34February
No.22013
基于容积卡尔曼滤波的卫星姿态估计
魏喜庆,宋申民
(哈尔滨工业大学控制理论与制导技术研究中心,哈尔滨150001)
摘要:为了获得更好的估计精度和滤波稳定性,提出了一种基于容积卡尔曼滤波(CubatureKalmanFilter,
CKF)的容积四元数估计器(CubatureQuaternionEstimator,CQE)估计卫星姿态。靳方法利用四元数进行姿态更新,同时采用广义罗德里格参数表示误差角,有效地避免了滤波过程中的奇异。为克服多传感器融合时运算效率低的问题,通过容积四元数估计器与信息滤波相结合,提出了一种容积信息四元数估计器(Cubature
关键词:姿态估计;容积卡尔曼滤波;容积四元数估计器;容积信息四元数估计器中图分类号:V448.2
文献标识码:A
文章编号:1000—1328(2013)02-0193-08
Information
QuateruionEstimator,CIQE)。仿真表明角度和陀螺漂移初始估计误差较大时,新方法仍能取得良好的估计性能。
DOI:10.3873/j.issn.1000—1328.2013.02.007
CubatureKalmanFilter.BasedSatelliteAttitudeEstimation
WEIXi—qing,SONGShen—min
(CenterofContmlTheoryandGuidanceTechnology,HarbinInstituteofTechnology,Harbin150001,China)
Abstract:Anewsatelliteattitudeestimationmethodnamedcubaturequaternion
Kalmanfilter(CKF)isproposedforbetterestimationperformancein
estimator(CQE)based
on
cubature
termsofnumericalaccuracyandfilteringstability,
AttitudeispropagatedbyusingquaternionwhilegeneralizedRodriguesparametersareemployedtoexpressangularerrorsforavoidingsingularityofthenewmethod.Thecubatureinformationquaternion
estimator(CIQE)is
ofmultiple
proposedbycombining
fusion.Simulations
CQE
and
information
filteringparadigmtoovercomelow
efficiency
onaccountsensor
demonstratethatthenewmethodachievesdesired
performanceundertheconditionoflargeinitial
quaternion
errors.
Keywords:Attitudeestimation;CubatureKalmaninformationquaternion
filter(CKF);Cubatureestimator(CQE);Cubature
estimator(CIQE)
0引言
降和滤波器收敛速度缓慢的问题¨1。为了进一步
改善卫星姿态估计性能,UKF被用于卫星姿态估计,通过经uT变换后的采样点集来逼近非线性函
数概率分布,姿态估计结果较EKF具有更好的鲁棒性[8。9J。粒子滤波用于卫星姿态估计,具有更好的
无论是执行对地观测、在轨服务还是编队飞行任务,精确已知姿态信息是卫星完成任务的前
提[1。]。矢量观测敏感器和陀螺构成的卫星姿态估
计系统,是实现高精度卫星姿态确定的标准方案[3。]。事实上,无论是利用欧拉角、罗德里格参数还是四元数来描述航天器的姿态运动学,都具有非线性特性,因此经常采用EKF进行状态估计∞J。但是由于EKF经过一阶线性化近似后,忽略了部分非线性特性,在初始误差较大时,存在估计效果急剧下
收稿日期:2011-12-27;
修回13期:2012-08-06
收敛速度和估计精度¨¨¨j。由于粒子滤波是一种
基于蒙特卡罗仿真的滤波方法,理论上只要粒子数目足够多,其对非线性、非高斯系统可以达到任意的
估计精度。但与其他滤波方法相比,粒子滤波随着
粒子数目增多而导致计算量增加,因此粒子滤波难以满足卫星姿态估计工程应用中对实时性的要求。
基金项目:国家自然科学基金(61174037)
万方数据
194
宇航学报第34卷
为了更好地满足卫星姿态估计要求,有必要研
究精度更高、工程实现更简单的方法。容积卡尔曼滤波就是近年来提出的一种非线性高斯滤波方法‘1卜13J。CKF具有严格的数学证明,通过三阶容
积法则数值积分近似加权高斯积分,充分利用了其在多维函数积分数值运算中具有的高效率特点。容积卡尔曼滤波具有等权值的2n个容积点(/Z为系统状态维数),经证明其逼近非线性变换后的概率分
布精度优于UKF【12]。
基于四元数描述的姿态运动学方程具有线性形
式且易于推导解析解,在卫星姿态更新中较其他姿
态描述方法具有更大的优势¨4。15I。因此在进行卫
星姿态估计时,经常采用四元数进行姿态更新。然而由于四元数的单位模限制,在进行协方差阵更新时会造成协方差阵奇异。利用罗德里格参数表示角
度误差,可以有效避免四元数协方差阵更新时出现的奇异现象【l6|。文中采用的广义罗德里格参数是
一种包含罗德里格参数和修正罗德里格参数的更广
义的三维角度描述形式¨7。。另外,卫星通过多传感
器融合进行姿态估计时,普通的滤波器存在计算效率不高的问题。将UKF与信息滤波框架结合,有效地提高非线性系统的多传感器融合的效率,但是其估计精度还有进一步提高的空间¨8l。
在上述研究基础上,本文提出了容积四元数估计器,在进一步提高卫星姿态估计精度的同时,通过四元数与广义罗德里格参数结合的方法,避免了由于状态冗余导致的协方差阵奇异。进一步,通过容积四元数估计器与信息滤波的结合,提出了容积信息四元数估计器。虽然其与容积四元数估计器本质上是等价的,但在多矢量观测情况下能有效提高卫星姿态估计的计算效率。最后将两种方法应用于三轴稳定卫星的仿真研究,验证了所提出方法及时在角度和陀螺漂移初始估计误差较大的情况,仍然具
有良好的估计精度和收敛速度。
1
CKF滤波
考虑具有加性噪声的非线性系统状态方程和量
测方程:
Xt=八X¨,口¨)+’.,¨(1)z^=h(X‘,H^)+l,I
(2)
万方数据
其中x。∈R“是系统在k时刻的状态,U㈧∈R~是系统的输入,z。∈R“是系统的量测值。w¨和l,。为不相关零均值高斯白噪声,其协方差阵分别为Q^一。
和冠k。
非线性高斯滤波的核心问题是求解多变量非线性函数与高斯密度函数乘积的积分。
Arasaratnam【12o等人通过三阶容积积分法则,利用
2n个容积点加权求和近似计算加权高斯积分,对于函数f(x)的加权高斯积分:
“(,)=If(x)N(z;肛,P)dx
J
R“
。
2n
一2t-n2f(P+庐孝。)…I=l
(3)
其中N(x;肛,P)表示X服从均值,正和协方差阵P的
正态分布,乒为协方差阵尸的平方根,满足痧渺)7
=P,具有2凡个元素的容积点集旧}具有如下
形式:
一1
00
0
:
:
●
●
0一1
其第i个容积点煮是n维列向量。
从而在贝叶斯估计框架下,利用容积积分法则,提出了CKF算法。假设在第k时刻已知前一时刻后验密度函数p(x¨J
z㈨一。)=N(x¨;王乏,,哎。),
CKF算法流程如下:
(1)时间预测
①计算容积点:
墨扣。=√PI。孝。+J0。,i=1,…,2n(4)
②容积点传播:
x。:^=f(Xm小U㈧),i=1,…,2n(5)
③估计预测均值和协方差阵:
.
2n
王f
2五1萎置▲
(6)
,
2n
Pi=去∑Xi:。(xi:。)’一;i(王i)’+Q。一。(7)
(2)量测更新
①计算容积点
置.^=√Pi亭i+工i,i=1,…,2n(8)
②容积点传播:
Zm=h(X“,U‘),i=l,…,2n
(9)
第2期
魏喜庆等:基于容积卡尔曼滤波的卫星姿态估计
195
③计算量测预测值、新息方差和协方差矩阵ii
2刍荟z“
(10)
P甜2刍荟Zi,kZijt一“i)’+Rt(11)匕广刍荟x泓Z“T一王眩)7
(12)
④计算量测更新:
羹=五+K(z。一乏),其中甄=巴.。巧:(13)
P:=P;一KkP矗?℃砭(14)
2卫星姿态运动学
2.1姿态运动学模型
单位四元数g=[P7
q。IT的乘法具有inT定
义㈨:
口”卸Qg
2
f…q'4q卯,-(叫P'∥)Vp抽】(15)
符号0表示四元数乘法运算符,式中
其中P;,P:和P;是p’的三个分量。
定义误差四元数两=[(印)1却。]7具有如
下形式:
田=qo蚕一
(17)
其中毒一为四元数毒的求逆运算,对于单位四元数
逆即共轭。
四元数的姿态运动学方程如下:
i
2丁1【‘oD]◇9
(18)
其中tO是体系相对于惯性系的角速度在体系下的
表示。
陀螺角速度输出采用经典模型:
∞=葫一卢一田。卢=叼。
(19)
其中西为陀螺输出,卢为陀螺漂移,叼。和71。为不相关零均值高斯白噪声,其协方差阵分别为矿l。1,。,和
0"213×3。
万方数据
it=[二]{。+y*=[二:二;二]}。+yt
e2。,
A(g)=(q:一IIt,||2)厶。3—2q。[P×]+2pt,7
栅嘶趟估懈器
翻美怫&洲提拥拙韵算
匠积
磁断拾黼估古
(CQE)。公式(18)和(19)构成了系统的状态方程,
:一
虢拼
啉耱嚣公式(20)是系统的量测方程,系统的状态向量为[q1
p1]1。
为了避免四元数状态冗余造成协方差阵出现奇
异的情况,在CQE的设计中将状态向量选为X=[(印)1卢’]7,其中印为式(17)中误差四元数却
对应的误差广义罗德里格参数:
印=b羔
(23)
参数a从0到l取值,b为尺度参数。当a=0,b=1
式(23)退化为罗德里格参数,a=1,b=1时式(23)退化为修正罗德里格参数,广义罗德里格参数的奇异点在180。到360。间。当选取b=2(a+1)
时,l|印lI与欧拉定理定义的旋转角在小角度时近似
相等。由却到却的对应关系由文献[17]可知
6q4=出业菩赫蔓必
0
十IlAn旷
3.1时间更新
196
宇航学报
第34卷
劫=b。1(a+曲。)印
(24)
已知滤波初值R,q“。,风,由第k一1时刻状态估计值和协方差阵生成容积点:
‰・2戮1],…,…’12(25)
其中却“一,和p“一,分别表示容积点的姿态误差角和陀螺漂移。由于在每一个滤波周期开始时刻,均采用状态后验估计作为初始值,所以在预测开始时,式
(25)中的姿态误差部分却m一。恒等于0。为了避免
姿态更新时产生奇异,先将误差修正罗德里格参数
转换为容积点误差四元数,利用公式(24)和(17):
地m,————瓦币i而—一。+
一a}lSp洙一,J12+b√62+(卜a2)IJ印泓~,j12
却i¨=b。1[o+isq4+i“,]却“小江1,…,12(26)
由容积点误差四元数得到容积点四元数点集:
云㈦=却i川。证。,
i=1,2,…,12
(27)
对卫星姿态运动方程进行离散化处理,当采样时间内转过的角度满足lIAteoll<<1,过程噪声方差阵具有如下形式【l引:
Q:『-‘z△。:-÷矿:(△D。厶×3l一(寻仃:(&)2)13。,(0-2,At)13。,一(÷盯x△幻2)Jr3妇]j
容积点四元数蚕i。由姿态运动学方程计算,
式(18)可以利用四阶龙格库塔法求解,本文采用文
献常用的解析形式阳圳,其对应容积点的估计角速度为:
O—Ji,t=矾一l一#¨,i=1,2,…,12(28)
通过姿态运动学方程得到蚕f。,进一步
锄ft=虻女o(qk+-。)~,
j=1,2,…,12
(29)
将容积点误差四元数转换为误差修正罗德里格参数
矗at,I:b墼,i:1,2,…,12(30)
a十94“
预测均值;f和方差阵矸由公式(6),(7)得到。
3.2量测更新
通过公式(10)计算量测预测值可得
“万∑一’n厶i,
i,k
(31)
\。1/
其中量测估计值容积点万方数据
A
^口
^
n
Z
=
e
(32)
A^拿々o
协方差矩阵计算采用公式(11)和(12),进一步求取滤波增益矩阵K,从而误差修正罗德里格参数和陀螺漂移的量测更新:
xk=zk+Ⅸ%L;:=;i+K(三。一ii)(3333)
zt—z^,J其中露=[(印÷)’(西:)7]To接下来计算更新四
元数和陀螺漂移。陀螺漂移即为王:后三项卢:,更新误差四元数却:为
妇-2————丁可丽了『—一
。+
一all(印:)7|J2+b√62+(1一a2)|I(印;)1112
却:=b‘1(口+硼0)(印:)1
(34)
进一步:
蚕:=曲:o蚕i(35)
假设已知P。,玩,JB。,将CQE用于卫星姿态估
计算法流程归纳为如下步骤:
CQE算法(1)时间预测
①利用式(25)生成容积点集鼍如。;
②由式(26)~(28)和姿态运动学方程(18)生
成预测四元数蚕f。;
③通过式(29)和(30)得到误差修正罗德里格参数;
④由式(6)和(7)计算预测均值蠢和方差阵矸;
(2)量测更新
①利用式(31)生成量测预测值三i;②采用式(33)计算量测更新;:;
③由式(34)和(35)得到更新后的四元数靠;
④将量测更新曼:的前三个元素重置为零。
虽然CQE具有良好的估计精度,但如果量测值过多,需要进行多传感器融合时,计算量过高导致实时性下降。为了提高计算效率,本文受无迹信息滤波器启发¨8l,提出了一种容积信息四元数估计器
(CIQE)并将其运用于卫星姿态确定。
对于多传感器数据融合过程的量测更新,信息
4容积信息四元数估计器
第2期魏喜庆等:基于容积卡尔曼滤波的卫星姿态估计197
滤波具有线性的形式Ⅲ】:
y:=K+∑t,;
(36)
y—l=Y‘k+∑l’s,k=
+乞
(37)Lj,)
其中K和玎为信息矩阵ji和露为信息状态,且有:
K=(竹)“
(38)1
2n
多i=麦K善Xi扣-
(39)
t.。和f“分别为信息矩阵增量和信息状态增量:
t,t=HmxR鼬-1日“
(40)i班=H¨T
Rm三“一蠢+日k;i]
(41)
定义伪量测矩阵巩具有如下形式:
磁=K吒,。I。。
(42)
将其带人到公式(41)和(40)得到:t.t=YkP。,^I。。露i1(P。,^I。。)7(K)T
(43)f小=K咒几戤‰一乏+(巴A)T(Yk)TXk]
(44)
假设已知P。,;。,风,容积信息四元数估计器
流程如下步骤:
CIQE算法(1)时间预测
①生成容积点与容积四元数估计器类似,同
式(25);
②由式(26)一(28)和姿态运动学方程(18)生
成预测四元数;i。;
③通过式(29)和(30)得到误差修正罗德里格
参数;
④由式(6),(7)计算预测均值乏和方差阵ei;
(2)量测更新
①信息矩阵K和信息状态歹f分别由式(38)
和(39)计算;
②由式(36)和(37)计算信息矩阵聪和信息
状态多:,进一步由公式;j=(矸)‘1多:得到量测更
新后的状态向量;
③由式(34)和(35)得到更新后的四元数毒:;
④将量测更新;:的前三个元素重置为零。由于信息滤波本质上是将卡尔曼滤波用协方差矩阵逆的形式表示,在进行多传感器融合时计算效万方数据
率更具有优势。CIQE将CQE与信息滤波框架结
合,因此两者具有等价性。同时利用m个矢量观测值进行卫星姿态确定,如果采用容积四元数估计器,
在滤波更新时需要对巴.。求逆,算法的实时性难以
实现。容积信息四元数估计器相比容积四元数估计器,利用公式(37)进行量测更新,信息状态是对m个量测值所对应信息增量的累加,避免了协方差矩
阵巴.。的求逆计算。采用此方法避免了对高维矩阵
的求逆,同时量测更新公式更加简洁,因此相比容积四元数估计器更加适用于多传感器融合。
5仿真验证
对CQE和CIQE进行卫星姿态估计仿真对比,对象为运行在轨道高度700km圆轨道的三轴稳定卫星,轨道角速度为0.06560/s。设卫星体系和轨道
坐标系完全重合,则卫星体系相对于惯性系角速度
与轨道坐标系相对于惯性系角速度相等。角速度和角度矢量测量装置分别利用三轴陀螺和星敏感器。公式(19)中陀螺测量输出包含的随机漂移标准差盯。=1.812×10。5。/s¨2,漂移速率斜坡标准差矿。=
1.812
X
10’8。/sⅣ2,姿态矢量测量含有标准差为
0.005。的高斯白噪声。陀螺采样周期为1S,星敏感器的采样周期为lOs,仿真时间设为5个轨道周期。
为了验证滤波器在不同情况下的估计精度,分三种情况进行仿真:情况一,初始角度估计误差小,初始陀螺漂移估计误差小;情况二,初始角度估计误差大,初始陀螺漂移估计误差小;情况三,初始角度估计误差大,初始陀螺漂移估计误差大。由于CQE和CIQE在理论上具有等价性,而且仿真时姿态估计精
度非常接近,为了显示清晰,未在图中画出,在后面
表中以及分析时详细对比了两者估计结果。
情况一:首先假设滤波器的初值Xo=
[(印。)1赋]1。都为零,初始方差阵Po中关于姿态角
部分方差阵,么=(0.2。)2厶m陀螺漂移方差阵JP盘=
(0.10/h)2厶肿广义罗德里格参数中选取o=1,b=4,
即选取为修正罗德里格参数。图1中角度误差范数定义为三轴角度估计误差向量的2一范数,图2中三条曲线分别为三轴角度估计误差。从图l和图2可以看出当角度和陀螺漂移初值估计误差较小时,三轴的角度误差曲线收敛速度较快,滤波精度能达到0.002。以下。
198
宇航学报
第34卷
一。一\藩靼巡援
图1
情况一角度误差范数曲线
Fig.1
NormofattitudeestimationenDrs
of
CQE
for
case
1
未z
X10’3
搬0
霪-2
O
l2345678
^
≮2
×10-3
搬0
蓬-2
0
12
3
4
5
67
8
p
×10-3
≮2
援0
饕一:O
l2
45678
f/h
图2情况一CQE角度估计误差
Fig.2
Attitudeestimation
en’ors
of
CQE
for
case
1
情况二:三轴角度初值有较大估计误差分别为
一50。,500,160。,陀螺漂移初值估计误差为零。只关于姿态角部分方差阵P胡=(40。)213小陀螺漂移方差
阵依然取为%=(0.1。/h)213。,。由图3和图4可知
两个小时滤波器才能达到比较高的估计精度。
fV
\
藏耀趟避
图3情况二角度误差范数曲线
Fig.3
Norm
ofattitudeestimation
et,Tors
of
CQE
for
ease
2
情况三:角度初值有较大估计误差分别为
万方数据
乏:
×10-3
援0
霪。:
0t2345678
o×l伊3
2
援0星.2害0l
2345678
<2
×10。’
藿.0:
O
12
3
4
5
67
8
t,h
图4情况二CQE角度估计误差
Fig.4
Attitudeestimation
en‘ors
of
CQE
for
ease
2
—50。,50。,160。,三轴陀螺漂移初值估计误差分别为
lO。/h
loo/h]T只关于姿态角部分
一o
\
氟辗麒援
图5情况三角度误差范数曲线
Fig.5
Normofattitudeestimation
el'l'ors
of
CQE
for
ease
3
乏:
×10-3
援0
萋-2
012
345678
f
≮2
×100
艘0
羹.z0
l234567
8
图6情况三CQE角度估计误差
Fig.6
AttitudeestimationerrOrsof
CQE
for
ease
3
容积信息四元数估计器在多传感器融合时,具Bo=[10。/h
方差阵P砷=(40。)2j,小陀螺漂移方差阵取为P舯=
(15。/h)213肿由图5和图6可知CQE的收敛速度虽
然进一步降低,但在3小时内仍达到稳定状态。
CQE相对于在情况一的快速收敛,在情况二时需要约有实现简单和计算量小的优点,在三种情况下仿真
第2期
魏喜庆等:基于容积卡尔曼滤波的卫星姿态估计199
结果与容积四元数估计器估计精度类似。下面采用
平均均方根误差(Root
Time—AverageMean
Square
Error,RTAMSE)来比较CQE和CIQE的估计精度。
RTAMSE的定义如下旧1I:
RTAMSE
2√高荟荟ttx:一王:II(46)
厂■—r1『———一
其中丁为总的仿真步长,M为蒙特卡罗仿真次数,X和i分别是三轴角度真值及其估计值。
CQE和CIQE滤波器在三种情况下进行了20次蒙特卡罗仿真,RTAMSE如表1所示,其单位为度。从表中可以看出两种滤波器的估计精度相似,随着初始估计误差增大,RTAMSE也随之增大,但仍能保持良好的估计精度。仿真结果也验证了前文关于CQE和CIQE一致性的描述。
表1两种滤波器RTAMSE表
Table1
AverageRTAMSEoftwofilters
高斯一若当法数值求解矩阵逆的计算量约为3n3/2次乘除法,计算量随着矩阵维数的增加而呈指数级增长。图7仅绘制一个轨道周期内观测恒星个数,从中可以看出在大部分时间能观测到两颗或三颗恒星,最多时能观测到四颗恒星(仅持续几个量测周期)。每爪J叵星提供三维星光矢量,因此量测协方差矩
阵P¨;最多能达到12维。CIQE量测更新过程,只对状态协方差矩阵求逆运算,运算量只和状态维数相
关,而不会随量测数据的增加而导致计算量急剧增
加,即使在维数更高的情况也能保持实时眭。
图7轨道周期观测恒星个数
Fig.7
Numberofavailablestarsineachperiod
万方数据
6结论
本文提出了一种容积四元数估计器用于卫星姿态确定。对于较小的初值估计误差,新的滤波器具有良好的估计精度,而当估计误差较大时CQE也能保持较好的估计精度和较快的收敛速度。进一步,本文提出了一种容积信息四元数估计器算法用于多传感器融合时的卫星姿态估计,新算法与容积四元数估计器具有相似的估计精度,并且滤波形式简单、
更易于实现。
参考文献
rL●1J
StoU
E,ktschrIikJ,WalterU,eta1.On—orbitservicing[J].IEEE
RoboticsandAutomation
Magazine,2009.16(4):29-33,
2
1J
MeLoughlinTH,CampbellM.Distributedestimatefusionfilterforlargespacecraft
fornmtions[R].Honolulu,HI,United
States:
AmericanInstituteofAeronauticsandAstronauticsInc.,2008.
rL3
MurrellJ
W.Precisionattitudedeterminationformuhimission
spacecraft[C].The
AIAA
Guidance,Navigation,andControl
Conference,Palo,CA,UnitedStates,1978.
rL4AndrewsSF,BilanowS.RecentflJightresultsoftheTRMMKalman
filter【C].AlAAGmdance,Navigation,and
Control
Conferenceand
Exhibit,Monterey,CA,Unitedstates,2002.
rL51J
AbdelrahmanM,ParkSY.Sigma—point
kalmanfilteringforspacecraft
attitude
and
rate
estimationusing
magnetometer
measurements[J].IEEE
Transactions
on
Aerospace
and
Electronic
Systems,2011,47(2):1401—1415.
rL6
1J
LeffertsEJ,MarkleyFL,Shuster
MD.Kalmanfilteringfor
spacecraftattitudeestimation[J].Journal
ofGuidance,Control,
and
Dynamics,1982,5(5):417—429.
r●L
71j范春石,张高飞,孟子阳,等.一种基于线性伪量测方程的无陀螺姿态确定方法[J].宇航学报,2008,29(4):1290—1296.[Fan
Chun-shi。ZhangGao-fei,MengZi—yang,eta1.
NovelquaternionKalmanfilterforgyroless
attitudedetermination
[j],]oumal
of
Astronautics,2008,29(4):1290—1296.]
rL81J
CrassidisJL.Markley
FL.Unscentedfilteringforspacecraft
attitude
estimation[J].Journalof
Guidance,Control,and
Dynamics,2003,26(4):536—542.
rL9
1J
陈记争,袁建平,方群.基于修正Rodrigues参数和UKF的姿态估计算法[J].宇航学报,2008,29(5):1622—1626.[ChenJi-zheng,YuanJian-ping,FangQun.Attitude
estimation
usingmodified
rodriguesparameters
and
UKF[J].Journal
of
Astronautics,2008,29(5):1622—1626.]
rLK1J
Carmi
A,OshmanY.Fast
particle
filteringforattitude
andangular—rateestimationfrom
vector
observations[J].Journal
of
200
宇航学报
第34卷
Guidance,Control,and
Dynamics,2009,32(1):70一78.
张惟,林宝军,张泽明,等.矢量观测确定卫星姿态的预测粒子滤波算法[J].宇航学报,201l,32(5):1077—1085.[ZhangWei,LinBao-jun,Zhang
Ze—ming,eta1.Particlefiltering
fusing
predictive
filter
for
attitude
determination
from
vector
observations[J].JournalofAstronautics,2011,32(5):1077—1085.]
[12]
ArasaratnamI,HaykinS.CubatureKalmanfilters[J].IEEETransactions
on
Automatic
Control,2009,54(6):1254—1269.
[13]ArasaratnamI.HaykinS.HuMTR.CubatureKalmanfilteringforcontinuous—discretesystems:Theoryand
simulations[J].
IEEETransactionson
Signal
Processing,2010,58(10):4977
—4993.
[14]
ShusterM.A
survey
ofattitude
representations[J].Journal
of
theAstronautical
Sciences,1993,41(4):439~517.
[15]KimSG,CrassidisJL。ChengY.eta1.Kalman
filteringfor
relativespacecraftattitudeandpositionestimation[J].Journal
of
Guidance,Control,and
Dynamics,2007,30(1):133—143.[16]郭庆,魏瑞轩,许洁,等.基于MRPs/NPUPF的地磁/加速度计测量的姿态估计新方法[J].宇航学报,2011,32(2):336—342.[GuoQing,Wei
Rui-xuan,XuJie,eta1.A
new
method
forattitudeestimationinmagnetometerandaccelerometerattitudesystembased
on
MRPsand
NPUPF[J].JournalofAstronautics,
万方数据
2011,32(2):336—342.]
[17]
SchaubH.JunkinsJL.Stereographieorientationparametersfor
attitudedynamics:ageneralizationoftheRedriguesparameters[J].
JournaloftheAstronautical
Sciences,1996,44(1):l—19.
[18]
LeeD
J.Nonlinearestimationandmultiplesensorfusionusing
unscented
information
filtering[J].IEEE
SignalProcessing
Letters,2008,15:861—864.
[19]
Crassidis
J.MarkleyF.AttitudeestimationusingmodifiedRodrigues
parameters[C].The
American
Astronautical
Society
F.Landis
MarkleyAstronauticsSymposium,Greenbelt,MD,United
States,
1996.
[20]
Crassidis
J.JunkinsJ.Optimalestimationofdynamicsystems
[M].Boca
Raton:Chapman&Hall,2004:419-433.
[21]
Arulampalam
MS,RisticB,GordonN,eta1.Bearings—only
trackingofmanoeuvringtargetsusingparticlefilters[J].Eurasip
Journal
on
AppliedSignalProcessing,2004(15):2351—2365.
作者简介:
魏喜庆(1982一),男,博士生,研究方向为非线性滤波和卫星相对导航。
通信地址:啥工大科学园3032信箱(150001)电话:(0451)86402204—8212
E—mail:weixiqing@gmail.corn
(编辑:曹亚君)
基于容积卡尔曼滤波的卫星姿态估计
作者:作者单位:刊名:英文刊名:年,卷(期):
魏喜庆, 宋申民, WEI Xi-qing, SONG Shen-min
哈尔滨工业大学控制理论与制导技术研究中心,哈尔滨,150001宇航学报
Journal of Astronautics2013,34(2)
参考文献(21条)
1.Stoll E;Letschnik J;Walter U On-orbit servicing[外文期刊] 2009(04)
2.McLoughlin T H;Campbell M Distributed estimate fusion filter for large spacecraft formations 20083.Murrell J W Precision attitude determination for multimission spacecraft 19784.Andrews S F;Bilanow S Recent flight results of the TRMM Kalman filter 2002
5.Abdelrahman M;Park S Y Sigma-point kalman filtering for spacecraft attitude and rate estimation using magnetometermeasurements 2011(02)
6.Lefferts E J;Markley F L;Shuster M D Kalman filtering for spacecraft attitude estimation 1982(05)7.范春石;张高飞;孟子阳 一种基于线性伪量测方程的无陀螺姿态确定方法[期刊论文]-宇航学报 2008(04)
8.Crassidis J L;Markley F L Unscented filtering for spacecraft attitude estimation[外文期刊] 2003(04)9.陈记争;袁建平;方群 基于修正Rodrigues参数和UKF的姿态估计算法[期刊论文]-宇航学报 2008(05)
10.Carmi A;Oshman Y Fast particle filtering for attitude and angular-rate estimation from vector observations[外文期刊]2009(01)
11.张惟;林宝军;张泽明 矢量观测确定卫星姿态的预测粒子滤波算法[期刊论文]-宇航学报 2011(05)12.Arasaratnam I;Haykin S Cubature Kalman filters[外文期刊] 2009(06)
13.Arasaratnam I;Haykin S;Hurd T R Cubature Kalman filtering for continuous-discrete systems:Theory and simulations[外文期刊] 2010(10)
14.Shuster M A survey of attitude representations 1993(04)
15.Kim S G;Crassidis J L;Cheng Y Kalman filtering for relative spacecraft attitude and position estimation[外文期刊]2007(01)
16.郭庆;魏瑞轩;许洁 基于MRPs/NPUPF的地磁/加速度计测量的姿态估计新方法[期刊论文]-宇航学报 2011(02)
17.Schaub H;Junkins J L Stereographic orientation parameters for attitude dynamics:a generalization of the Rodriguesparameters 1996(01)
18.Lee D J Nonlinear estimation and multiple sensor fusion using unscented information filtering[外文期刊] 200819.Crassidis J;Markley F Attitude estimation using modified Rodrigues parameters 199620.Crassidis J;Junkins J Optimal estimation of dynamic systems 2004
21.Arulampalam M S;Ristic B;Gordon N Bearings-only tracking of manoeuvring targets using particle filters 2004(15)
本文链接:http://d.wanfangdata.com.cn/Periodical_yhxb201302007.aspx