[发明专利]一种基于星敏感器和陀螺的高精度卫星姿态确定方法

权利要求书

1.一种基于星光和陀螺的高精度卫星姿态确定方法，其特征在于，采用陀螺和星敏感器作为姿态敏感器，利用预测滤波算法实时估计出卫星姿态确定系统的模型误差并实时进行补偿，再利用二阶插值滤波进行状态估计，具体包括以下几个步骤：

步骤一：建立卫星姿态确定系统的状态方程；

卫星的姿态动力学方程为：

Jω·+ω×Jω=Ggb+Ggb+Ggb+Gsb---(1)]]>

式中，ω＝[ω_x，ω_y，ω_z]^T为卫星本体坐标系对惯性坐标系的转动角速度，ω_x、ω_y、ω_z为卫星本体坐标系对惯性坐标系在卫星本体坐标系下沿x，y，z三个轴的分量；J为卫星的惯性张量矩阵；G_mb为地磁力矩；G_ab为气动力矩；G_sb为太阳光压力矩；G_gb为重力梯度力矩；

G_gb具体为：

Ggb=3μr5Rb×JRb---(2)]]>

式中，μ为地心引力常数；r为卫星的地心距；R_b为卫星在卫星本体坐标系下的位置矢量，具体为：

Rb=CibRi---(3)]]>

式中，R_i为卫星在惯性坐标系下的位置矢量，为从惯性坐标系到卫星本体坐标系的转换矩阵，具体为：

地心距真近点角偏近点角平近点角M＝n(t-t₀)；n为卫星的平运动速度；t₀为卫星到达近地点的时间；t为时间；为地心轨道坐标系到惯性坐标系的转换矩阵，具体为：

Coi=cosΩocoswo-sinwocosiosinΩo-cosΩosinwo-coswocosiosinΩosinΩosiniosinΩocoswo+sinwocosiocosΩo-sinΩosinwo+coswocosiocosΩo-cosΩosiniosinwosiniosinwocosiocosio]]>

其中，Ω_o为卫星轨道升交点赤经；i_o为卫星轨道倾角；w_o为卫星轨道近地点幅角；

将G_mb、G_ab和G_sb的合力矩表示为d_G，作为未建模的模型误差，则式(1)的卫星姿态动力学方程写为：

ω·=J-1(-ω×Jω+Ggb)+J-1dG---(4)]]>

采用四元数来描述的卫星姿态运动学方程如下：

q·=12Ω(ω)q=12Ξ(q)ω---(5)]]>

式中，为卫星本体坐标系相对于惯性坐标系的姿态四元数；q₁₃＝[q₁q₂q₃]^T，I_3×3为单位阵；[ω×]、[q₁₃×]分别表示由向量ω、q₁₃的分量构成的反对称矩阵，

由式(4)和式(5)构成卫星姿态确定系统的状态方程为：

x·=J-1(-ω×Jω+Ggb)12Ξ(q)ω+G·dG+W---(6)]]>

式中：x为状态向量，x＝[ω^T，q^T]^T；G为误差扰动矩阵，系统噪声w(k)～(0，Q(k))，即w(k)服从均值为0、方差为Q(k)的高斯分布；

将式(6)简记为：

x·=f(x,dG,w)---(7)]]>

步骤二：建立卫星姿态确定系统的测量方程；

采用三轴陀螺和两个星敏感器作为姿态敏感器；

1)陀螺；

陀螺的测量方程为：

g1(x(k))=ωxωyωz+vxvyvz=gω(x(k))+vω(k)---(8)]]>

式中，g₁为陀螺的测量输出，记ωi为卫星本体坐标系对惯性坐标系的转动角速度，v_i为陀螺测量的高斯白噪声，i＝x，y，z；

2)星敏感器；

在惯性坐标系中两个星敏感的主光轴的单位方向矢量分别是l_i1和l_i2，则星敏感器的测量方程为：

g2=CibIi1+vs1=gs1(x(k))+vs1g3=CibIi2+vs2=gs2(x(k))+vs2---(9)]]>

式中，v_s1、v_s2是星敏感器测量的高斯白噪声；g₁、g₂分别是两个星敏感器的测量输出；记是由惯性坐标系到卫星本体坐标系的转换矩阵，用姿态四元数表示，具体为：

Cib=q12-q22-q32+q422(q1q2+q3q4)2(q1q3-q2q4)2(q1q2-q3q4)-q12+q22-q32+q422(q2q3+q1q4)2(q1q3+q2q4)2(q2q3-q1q4)-q12-q22+q32+q42---(10)]]>

3)建立卫星姿态确定系统的观测方程为：

y(k)=gω(x(k))gs1(x(k))gs2(x(k))+vω(k)vs1(k)vs2(k)---(11)]]>

将式(11)简记为：

y(k)＝g(x(k)，v(k))    (12)

其中，即v(k)服从均值为0、方差为R(k)的高斯分布，并且w(k)，v(k)相互独立；

步骤三：利用预测滤波在线实时估计模型误差；

模型误差的估计式为：

d^G(k)=-({Λ(T)U[x^(k)]}TR-1{Λ(T)U[x^(k)]}+W)-1{Λ(T)U[x^(k)]}TR-1]]>(13)

×{y^(k)+z[x^(k),T]-y(k+1)}]]>

式中：为陀螺和两个星敏感器的输出估计，T为滤波周期，U为灵敏度矩阵，Λ(T)为对角矩阵，为一个列向量；W为加权矩阵，预先设定数值；

其中，具体为：

y^(k)=g[x^(k),v‾(k)]---(14)]]>

式中：为k时刻的状态量的估计值；表示v(k)的均值；

灵敏度矩阵U为：

U=LG[L0f(gw)]LG[L1f(gs1)]LG[L1f(gs2)],---(15)]]>

相关的李导数的具体为：

LG[L0f(gw)]=J-1]]>

LG[L1f(gs1)]=∂L1f(gs1)∂x^·G=-ΞT(q^)Γ(li1)Ξ(q^)J-1]]>

LG[L1f(gs2)]=∂L1f(gs2)∂x^·G=-ΞT(q^)Γ(li2)Ξ(q^)J-1]]>

对角矩阵Λ(T)为：

Λ(T)=Λw000Λs1000Λs2---(16)]]>

其中：Λ_w＝T·I_3×3，Λs1=T22·I3×3,]]>Λs2=T22·I3×3,;]]>列向量为：

z[x^(k),T]=zωzs1zs2=T·L1f(gω)T·L1f(gs1)+T22·L2f(gs1)T·L1f(gs2)+T22·L2f(gs2)---(17)]]>

上式中各阶李导数的为：

L1f(gω)=J-1[-ω^×Jω^+Ggb]]]>

L1f(gs1)=-ΞT(q^)Γ(li1)Ξ(q^)ω^]]>

L2f(gs1)=[ω^×]·ΞT(q^)Γ(li1)Ξ(q^)ω^-ΞT(q^)Γ(li1)Ξ(q^)×J-1[-ω^×Jω^+Ggb]]]>

L1f(gs2)=-ΞT(q^)Γ(li2)Ξ(q^)ω^]]>

L2f(gs2)=[ω^×]·ΞT(q^)Γ(li2)Ξ(q^)ω^-ΞT(q^)Γ(li2)Ξ(q^)×J-1[-ω^×Jω^+Ggb]]]>

其中：Γ(a)=-[a×]-aaT0;]]>

最后，根据式(13)得到模型误差的估计值

步骤四：对补偿后的模型利用二阶插值滤波进行状态估计，得到卫星的姿态；

具体为：

将步骤三中得到的模型误差代入状态方程(7)进行补偿，将补偿后的卫星姿态确定系统模型的状态方程和测量方程写成离散的非线性形式，为：

x_k+1＝f(x_k，d_G，w_k)    (18)

y_k＝g(x_k，v_k)

设滤波值的误差方差阵的平方根为即是的Cholesky分解，模型误差方差阵Q的平方根为S_w，测量噪声的方差阵R的平方根为S_v，状态预测误差的方差阵的平方根为即：

Q=SwSwT,]]>R=SvSvT,]]>(19)

P‾=S‾xS‾xT,]]>P^=S^xS^xT.]]>

状态及其误差方差阵的一步预测：

x‾k+1=h2-nx-nwh2f(x^k,d^G,w‾k)]]>

+12h2Σp=1nx(f(x^k+hs^x,p,d^G,w‾k)+f(x^k-hs^x,p,d^G,w‾k))---(20)]]>

+12h2Σp=1nv(f(x^k,d^G,w‾k+hs^w,p)+f(x^k,d^G,w‾k-hs^w,p))]]>

其中，表示w(k)的均值，n_x表示状态向量的维数，n_w表示系统噪声的维数，插值步长h²＝3；

状态预测误差方差阵的Cholesky分解是经Householder变换后的矩阵：

S_(k+1)=[Sxx^(1)(k)Sxw(1)(k)Sxx^(2)(k)Sxw(2)(k)]---(21)]]>

其中，

Sxx^(1)(k)={Sxx^(1)(k)(i,j)}={(fi(x^k+hs^x,j,d^G,w‾k)-fi(x^k-hs^x,j,d^G,w‾k))/2h},]]>

Sxw(1)(k)={Sxw(1)(k)(i,j)}={(fi(x^k,d^G,w‾k+hs^w,j)-fi(x^k,d^G,w‾k-hs^w,j))/2h},]]>

Sxx^(2)(k)={Sxx^(2)(k)(i,j)}={h2-12h2(fi(x^k+hs^x,j,d^G,w‾k)]]>

+fi(x^k-hs^x,j,d^G,w‾k)-2fi(x^k,d^G,w‾k))},]]>

Sxw(2)(k)={Sxw(2)(k)(i,j)}={h2-12h2(fi(x^k,d^G,w‾k+hs^w,j)]]>

+fi(x^k,d^G,w‾k-hs^w,j)-2fi(x^k,d^G,w‾k))},]]>

其中，表示的第j列，表示的第j列，表示S_w的第j列；观测值的一步预测为：

y‾k+1=h2-nx-nvh2g(x‾k+1,v‾k+1)]]>

+12h2Σp=1nx(g(x‾k+1+hs‾x,p,v‾k+1)+g(x‾k+1-hs‾x,p,v‾k+1))---(22)]]>

+12h2Σp=1nv(g(x‾k+1,v‾k+1+hs‾v,p)+g(x‾k+1,v‾k+1-hs‾v,p))]]>

其中，n_v是测量噪声维数；的误差方差阵的Cholesky分解是S_y(k+1)经Householder变换后的矩阵：

Sy(k+1)=[Syx‾(1)(k+1)Syv(1)(k+1)Syx‾(2)(k+1)Syv(2)(k+1)]---(23)]]>

其中，

Syx‾(1)(k+1)={Syx‾(1)(k+1)(i,j)}={(gi(x‾k+1+hs‾x,j,v‾k+1)-gi(x‾k+1-hs‾x,j,v‾k+1))/2h},]]>

Syv(1)(k+1)={Syv(1)(k+1)(i,j)}={(gi(x‾k+1,v‾k+1+hs‾v,j)-gi(x‾k+1,v‾k+1-hs‾v,j))/2h},]]>

Syx‾(2)(k+1)={Syx‾(2)(k+1)(i,j)}={h2-12h2(gi(x‾k+1+hs‾x,j,v‾k+1)]]>

+gi(x‾k+1-hs‾x,j,v‾k+1)-2gi(x‾k+1,v‾k+1))},]]>

Syv(2)(k+1)={Syv(2)(k+1)(i,j)}={h2-12h2(gi(x‾k+1,v‾k+1+hs‾v,j)]]>

+gi(x‾k+1,v‾k+1-hs‾v,j)-2gi(x‾k+1,v‾k+1))},]]>

式中：表示S_v的第j列；

状态和测量的互协方差阵为：

Pxy(k+1)=S‾x(k+1)Syx‾(k+1)T---(24)]]>

由式(24)得到增益矩阵K_k+1为：

K_k+1＝P_xy(k+1)[S_y(k+1)S_y(k+1)^T]^-1    (25)

基于测量的状态及估计误差方差阵更新为：

x^k+1=x-k+1+Kk+1(yk+1-y‾k+1)---(26)]]>

P^(k+1)=[S‾x(k+1)-Kk+1Syx‾(1)(k+1)][S‾x(k+1)-Kk+1Syx(1)(k+1)]T+Kk+1Syv(1)(k+1)[Kk+1Syv(1)(k+1)]T]]>

+Kk+1Syx(2)(k+1)[Kk+1Syx(2)(k+1)]T+Kk+1Syv(2)(k+1)[Kk+1Syv(2)(k+1)]T---(27)]]>

由此得到状态估计值其中为卫星的姿态四元数，每次估计之后对姿态四元数进行归一化处理，并且按照式(28)(29)(30)解算得到姿态角；

俯仰角：θ＝-arcsin(2_q1q3-2_q2q4)    (28)

偏航角：Ψ=arctan(2q2q3-2q1q4q32+q42-q12-q22)---(29)]]>

滚转角：