[发明专利]基于联邦UKF算法的低轨卫星多传感器容错自主导航方法

权利要求书

1.一种基于联邦UKF算法的低轨卫星多传感器容错自主导航方法，其特征在于，包括下列步骤：

(一)系统初始化；

读取轨道数据初值，状态初始条件为：

X^0=E(X0)---(1)]]>

P0=E[(X0-X^0)(X0-X^0)T]---(2)]]>

状态矢量X₀＝[x₀，y₀，z₀，v_x0，v_y0，v_z0]^T，x₀，y₀，z₀，v_x0，v_y0，v_z0，分别为卫星在X，Y，Z三个方向的初始位置和速度；并设系统过程噪声w(k)和量测噪声v(k)为加性噪声，Q_k和R_k分别为系统和量测噪声协方差阵；

(二)建立基于直角坐标系的地球卫星轨道运动学方程；

选取历元(J2000.0)地球赤道惯性坐标系，建立基于直角坐标系的地球卫星轨道运动学方程，即状态方程，通过四阶龙格-库塔方法解微分方程计算出地球卫星的位置(x，y，z)和速度(v_x，v_y，v_z)信息，简化的系统状态方程为：

dxdt=vxdydt=vydzdt=vzdvxdt=-μexr3[1-J2(Rer)2(7.5z2r2-1.5)]+ΔFxdvydt=-μeyr3[1-J2(Rer)2(7.5z2r2-1.5)]+ΔFydyzdt=-μezr3[1-J2(Rer)2(7.5z2r2-4.5)]+ΔFz---(3)]]>

式中，R_e为地球参考赤道半径，状态矢量X＝[x，y，z，v_x，v_y，v_z]^T，x，y，z，v_x，v_y，v_z分别为卫星在X，Y，Z三个方向的位置和速度，r=x2+y2+z2]]>为地心距，μ是地球引力常数，J₂为地球引力二阶带谐项系数，ΔF_x，ΔF_y，ΔF_z为地球非球形摄动的高阶摄动项和日、月摄动以及太阳光压摄动和大气摄动等摄动力的影响，在简化模型中这些摄动力的影响用系统过程噪声w(k)表示；

对于系统过程噪声为零均值白噪声有：

E{w(k)}＝0，E{w(k)w^T(j)}＝Q_kδ_kj，k，j＝1，2，3…；

式中，δ_kj为克朗涅克笛耳塔(Kronecker Delta)函数；

δkj=1j=k0j≠k;]]>

(三)  建立以星敏感器和红外地球敏感器的输出值为量测量的子系统量测方程；

以星敏感器和红外地球敏感器的子系统的量测量为星光角距，星光角距是指从卫星上观测到的导航恒星星光的矢量方向与地心矢量方向之间的夹角，星光角距a的量测方程为：

Za=α+va=arccos(-r→·s→r)+va---(4)]]>

式中为卫星在地心惯性坐标系中的位置矢量，由红外地球敏感器获得；为地球惯性坐标系下的导航星星光方向的单位矢量，由星敏感器识别；r为地心距；v_a为测量误差；

设量测噪声为零均值白噪声：

E{v_a(k)}＝0，E{va(k)vaT(j)}=Rk1δkj,]]>k，j＝1，2，3…；

(四)建立以磁强计和雷达高度计的输出值为量测量的子系统量测方程；三轴磁强计的测量值为卫星所在位置的地磁场矢量在卫星本体系中的三个分量，为简化量测模型，取地磁场强度矢量的模作为磁测自主导航观测量，比较此值与国际地磁场模型(IGRF)之间的差值来提供导航信息，量测方程为：

ZB=Bx2+By2+Bz2=(1r∂V∂θ)2+(-1rsinθ∂V∂φ)2+(∂V∂r→)2+vB---(5)]]>

其中，B_x、B_y、B_z为地磁场矢量在卫星本体系中的三个分量；V为地磁场势函数，r为地心距，φ为地理经度，θ为地心余纬，为地理坐标系下的卫星位置矢量，v_B为量测噪声；

设量测噪声为零均值白噪声：

E{v_B(k)}＝0，E{vB(k)vBT(j)}=Rk2δkj,]]>k，j＝1，2，3…；

星载雷达高度计的测量值为卫星到星下点实际海平面的距离信息，取测量模型为：

其中，R_e为地球参考赤道半径，为地心纬度，f为地球参考椭球的扁率；v_H(t)为量测噪声；

设量测噪声为零均值白噪声：

E{v_H(k)}＝0，E{vH(k)vHT(j)}=Rk3δkj,]]>k，j＝1，2，3…；

从式5和式6看出，需建立地心惯性坐标系和地球固联坐标系之间的转换矩阵，从地心赤道惯性坐标系S_i到地球固连坐标系S_e的坐标变换矩阵为：

Lei=Lz(aG)=cosaGsinaG0-sinaGcosaG0001---(7)]]>

式中：a_G为Greenwich赤经；

(五)建立以紫外敏感器的输出值为量测量的子系统量测方程；

紫外敏感器的量测值为卫星的位置矢量，紫外敏感器工作在紫外波段，同时观测多个天体目标，提供三轴的航天器姿态信息，定姿精度可达0.05°；另外，通过对地球圆盘的图像处理，提取星体坐标系中的地心方向和地心距离信息；结合定姿过程得到的姿态信息，计算出卫星在惯性系中的位置矢量，子系统的量测方程为：

Zu=r→r‾r+vu---(8)]]>

其中，为卫星在地心惯性坐标系中的位置矢量，r=x2+y2+z2]]>为地心距；v_u为测量误差；

设量测噪声为零均值白噪声：

E{v_u(k)}＝0，E{vu(k)vuT(j)}=Rk4δkj,]]>k，j＝1，2，3…；

(六)  依照最小偏度采样策略选择Sigma采样点；

①选择0≤W₀＜1，Sigma权值为：

Wi=1-W02ni=1,22i-2W1i=3,···,n+1---(9)]]>

Wim=Wic=Wi]]>

式中，W为权值，n为状态方程维数，W_i^m为均值加权值，W_i^c为协方差加权值；

②对应状态为1维情况，迭代初始向量：

χ01=[0],]]>χ11=[-12W1],]]>χ21=[12W1]---(10)]]>

③对于输入维数j＝2，…n时，迭代公式为：

χij+1=χ0j-10i=0χij-1-12Wj+1i=1,···j0j-112Wj+1i=j+1---(11)]]>

(七)根据最小方差估计原则建立离散型UKF算法的预测方程和更新方程，各子系统分别独立进行Sigma采样点计算以及预测更新和量测更新；

①预测方程：

对所生成的Sigma点加入状态矢量X的均值和协方差信息：

χi=X‾+P(k)χij,]]>i＝0，1，2，…，n+1                          (12)

χ_i(k+1|k)＝f[χ_i(k|k)]                    (13)

X^(k+1|k)=Σi=0L-1Wimχi(k+1|k)---(14)]]>

其中L为采样点个数，为t_k时刻对t_k+1的预测估值，P(k)为误差协方差阵初值，f(·)为系统状态方程；

P(k+1|k)=Σi=0L-1Wic(χi(k+1|k)-X^(k+1|k))(χi(k+1|k)-X^(k+1|k))T+Qk---(15)]]>

Z_i(k+1|k)＝h[χ_i(k+1|k)]                      (16)

Z^(k+1|k)=Σi=0L-1WimZi(k+1|k)---(17)]]>

式中，P(k+1|k)为预测估值误差协方差阵，h(·)为量测方程，为对t_k+1观测值的预测值；

Pyy(k+1|k)=Σi=0L-1Wic(Zi(k+1|k)-Z^(k+1|k))(Zi(k+1|k)-Z^(k+1|k))T+Rk---(18)]]>

Pxy(k+1|k)=Σi=0L-1Wic(χi(k+1|k)-X^(k+1|k))(Zi(k+1|k)-Z^(k+1|k))T---(19)]]>

②更新方程：

W(k+1)=Pxy(k+1|k)Pyy-1(k+1|k)---(20)]]>

X^(k+1|k+1)=X^(k+1|k)+W(k+1)(Z(k+1)-Z^(k+1|k))---(21)]]>

P(k+1|k+1)＝P(k+1|k)-W(k+1)P_yy(k+1|k)W^T(k+1)    (22)

式中，W(k+1)为增益矩阵，为滤波值，P(k+1|k+1)为滤波值误差协方差阵；

(八)利用残差检验法，根据预测滤波残差判断各子滤波器输出值是否有效，如出现故障则将其隔离，否则将滤波结果输入主滤波器进行信息融合；残差检验法的故障函数为：

其中，χm2(k)=ΔvkT(k)Pyy-1(k|k-1)vk(k)~χm2,]]>即χ_m²(k)为具有m个自由度的χ²变量，m为量测矢量Z的维数；v_k为残差，vk=z(k)-z^(k|k-1);]]>

对于正常滤波，取误警率P{χm2(k)>TD}=∫χa2+∞km(χ2)dχ2=a;]]>其中k_m(·)为χ_m²的概率密度函数，a为显著性水平，T_D为由误警率确定的门限值；

(九)建立基于UKF算法的无复位联邦UKF滤波方程；

联邦滤波器的信息融合方式采用无复位模式，仅对状态量进行融合和反馈，各子系统的协方差阵独立进行时间更新和预测更新，不参加主滤波器的融合：

Pk+1|k+1g=[Σi=1l(Pk+1|k+1i)-1]-1]]>l＝1，2，3    (24)

x^k+1|k+1g=Pk+1|k+1g[Σi=1l(Pk+1|k+1i)-1x^k+1|k+1i]]]>l＝1，2，3    (25)

(十)按照上述步骤(一)-(九)，输出为地球卫星状态矢量估计值及其方差矩阵P^g，其中状态估计值包括地球卫星位置和速度矢量[x，y，z，v_x，v_y，v_z]^T，状态估计方差矩阵P^g包括地球卫星位置和速度估计方差[P_x^g，P_y^g，P_z^g，P_vx^g，P_vy^g，P_vz^g]^T。