CN101852605B - 基于简化自适应滤波的磁测微小卫星姿态确定方法 - Google Patents

Abstract

本发明公布了一种基于简化自适应滤波的磁测微小卫星姿态确定方法，包括如下步骤：第一步：建立卫星姿态运动模型，第二步：改进磁强计测量模型，第三步：量测噪声模型的自适应修正，第四步：滤波解算。本发明对磁强计量测模型做出了分析和改进，在此基础上简化了滤波增益阵的计算。在不明显影响精度的情况下，使得计算量得到了显著降低，数学仿真表明算法是有效的，三轴磁强计定姿方案可以满足中等姿态精度要求。具有良好的工程意义和应用前景。

Description

基于简化自适应滤波的磁测微小卫星姿态确定方法

技术领域

本发明涉及一种磁测微小卫星姿态确定方法，尤其涉及一种基于简化自适应滤波的磁测微小卫星姿态确定方法。

背景技术

微小卫星技术是当前国际空间技术研究的热点。微小卫星发射灵活，成本低、功能密度高、研制周期短，具有独特的优势，发达国家在该技术领域走在了前列，民用与军用领域都从中受益。地磁场是中、低轨道环境中的特殊自然资源，三轴磁强计具有体积小、重量轻、成本低、性能可靠、没有视场限制的优点，并能够提供全天侯、实时连续的自主导航信息，与地磁相互作用实现星体控制的磁力矩器也具有成熟可靠、成本低功耗小等特点。磁强计和磁力矩器是目前国内外发射的低轨小卫星上的最基本配置。

目前，地磁场已有相当好的磁场模型，采用高斯球谐函数来描述地磁场模型，这样磁场的强度和方向是位置的函数。当确定了卫星轨道参数，就可求得地磁场矢量在地理坐标系的投影，利用地理坐标系与轨道坐标系的转换关系，得到地磁矢量在轨道坐标系的分量，进一步借助姿态方向余弦矩阵给出地磁矢量在星体坐标系的分量，将其与三轴磁强计敏感到的地磁矢量的分量进行比较，就建立起三轴磁强计与卫星姿态动力学的数学关系，采用卡尔曼滤波器递推可以得到卫星姿态角。考虑到小卫星的性价比需求，要求尽可能使用较低的成本来实现小卫星自主导航系统设计，因此，利用丰富的地磁场资源，并结合磁强计测量信息进行低轨小卫星自主轨道确定，将能有效降低微小卫星自主导航系统研制成本，满足小卫星自主导航系统的基本需求。

但是，鉴于地磁场的可变性及磁强计本身精度的限制，以及星载计算机的性能限制，使用单磁强计进行卫星自主导航的定轨精度普遍较低，耗时较长。因此需要开展磁定姿技术研究，研究出复杂度适中，适合星上计算的磁导航数学模型与滤波处理方法，以节约星上计算资源。

发明内容

本发明目的是针对现有技术存在的缺陷提供一种基于简化自适应滤波的磁测微小卫星姿态确定方法。

本发明为实现上述目的，采用如下技术方案：

本发明基于简化自适应滤波的磁测微小卫星姿态确定方法，包括如下步骤：

第一步：建立卫星姿态运动模型

四元素运动学微分方程为：

$\stackrel{\·}{q}=\frac{1}{2}[q\×]\mathrm{\ω}---\left(1\right)$

q＝[q₁ q₂ q₃ q₄]^T为四元素，上标T表示转置，ω为星体坐标系相对轨道坐标系的角速率，

表示q的微分；

对式(1)进行求导，并忽略二阶小量可得：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{q}}_{13}=-[\widehat{\mathrm{\ω}}\×]\mathrm{\δ}{q}_{13}+\frac{1}{2}\mathrm{\Δ\ω}\\ \mathrm{\δ}{\stackrel{\·}{q}}_4=0\end{array}\right.---\left(2\right)$

为ω估计值，Δω＝[Δω₁ Δω₂ Δω₃]^T为ω误差，δq＝[δq₁ δq₂ δq₃ δq₄]^T为四元素误差，δq₁₃＝[δq₁ δq₂ δq₃]^T，上标^·表示微分；

卫星在外力矩的作用下，发生姿态的改变，外力矩一般包括卫星的控制力矩与空间扰动力矩；

$\stackrel{\·}{\mathrm{\ω}}=J^{-1}\{N_r-\mathrm{\ω}\×(\mathrm{J\ω}+h)+N_{\mathrm{\τ}}\}---\left(3\right)$

其中

为ω的微分，N_τ为空间扰动力矩，N_r为控制力矩，ω为星体坐标系相对轨道坐标系的角速率，J为星体惯量矩阵，h星体偏置动量；

相应的误差小量方程为：

$\mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}=J^{-1}\left\{\right[(J\widehat{\mathrm{\ω}}+h)\×]-(\widehat{\mathrm{\ω}}\×)J\}\·\mathrm{\Δ\ω}+J^{-1}\·N_{\mathrm{\τ}}---\left(4\right)$

由式(2)、(4)联合构成状态方程，系统状态量为：

X＝[δq₁ δq₂ δq₃ Δω₁ Δω₂ Δω₃]^T；

第二步：改进磁强计量测模型

量测模型为：

e_m＝(B_m×B_b)+(B_m-B_b)                      (5)

其中：e_m为磁场误差，B_m为磁强计测量值，B_b＝[B₁ B₂ B₃]^T为本体磁矢量估计值，同时v_m为磁强计量测噪声；

磁强计测量残差为：

B_m＝B_b+v_m                      (6)

则：

e_m＝((2(B_b×)δq₁₃+B_b+v_m)×B_b)+((2(B_b×)δq₁₃+B_b+v_m)-B_b)

＝-2[(B_b×)(B_b×)]δq₁₃+v_m×B_b+2(B_b×)δq₁₃+v_m

＝H′δq₁₃+v′_m

$H^{\′}=-2\left[\begin{array}{ccc}-(B_2^2+B_3^2)& 0& 0\\ 0& -(B_1^2+B_3^2)& 0\\ 0& 0& -(B_1^2+B_2^2)\end{array}\right]$

量测噪声为：v′_m＝v_m×B_b+v_m

当状态量为X时，量测阵为：H＝[H′0_3*3]_3*6；

第三步：量测噪声模型的自适应修正

v′_m＝v_m×B_b+v_m

磁强计测量噪声v_m为均值Y(nT)白噪声，则：

观测噪声方差阵

Diag表示对角矩阵，

分别表示量测噪声方差阵对角线元素；

[r₁ r₂ r₃]^T＝-(B_b×)[Y Y Y]^T+[Y Y Y]^T，Y为v_m均值，nT表示噪声单位纳特；

第四步：滤波解算

采用卡尔曼滤波算法进行滤波解算确定卫星姿态。

本发明利用三轴磁强计量测的地磁场矢量在仪表坐标系中的分量，通过卡尔曼滤波器可以确定低轨道卫星的姿态。本文提出了一种基于简化滤波算法的磁控微小卫星姿态确定算法。对磁强计量测模型做出了分析和改进，在此基础上简化了滤波增益阵的计算。在不明显影响精度的情况下，使得计算量得到了显著降低，数学仿真表明算法是有效的，三轴磁强计定姿方案可以满足中等姿态精度要求。具有良好的工程意义和应用前景。

附图说明

图1：滚动角曲线；

图2：俯仰角曲线；

图3：偏航角曲线。

具体实施方式

下面结合附图对发明的技术方案进行详细说明：

本发明中，磁强计作为基本姿态器件，提供长期的低精度姿态信息；当卫星上能源紧张或处在某种不安全状态下，磁强计提供基本信息，维持卫星的生命。

1.磁测微小卫星轨道计算

微小卫星采用GPS作为轨道信息的唯一来源，全天时提供高精度轨道信息。GPS的输出频率为1Hz，由于姿态输出频率高达10Hz，或者GPS由于可见星不够而不可用时，需要对轨道信息进行预测，预测考虑J2摄动项，以保证足够的精度；由于GPS输出位置和速度信息，为运算方便，轨道参数选择位置速度参数。

2.磁强计姿态确定

2.1卫星姿态运动模型

四元素运动学微分方程为：

$\stackrel{\·}{q}=\frac{1}{2}[q\×]\mathrm{\ω}---\left(1\right)$

q＝[q₁ q₂ q₃ q₄]^T为四元素，上标T表示转置，ω为星体坐标系相对轨道坐标系的角速率，

表示q的微分。

对式(1)进行求导，并忽略二阶小量可得

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{q}}_{13}=-[\widehat{\mathrm{\ω}}\×]\mathrm{\δ}{q}_{13}+\frac{1}{2}\mathrm{\Δ\ω}\\ \mathrm{\δ}{\stackrel{\·}{q}}_4=0\end{array}\right.---\left(2\right)$

为ω估计值，Δω＝[Δω₁ Δω₂ Δω₃]^T为ω误差，δq＝[δq₁ δq₂ δq₃ δq₄]^T为四元素误差，δq₁₃＝[δq₁ δq₂ δq₃]^T，上标^·表示微分。

卫星在外力矩的作用下，发生姿态的改变。外力矩一般包括卫星的控制力矩与空间扰动力矩。

$\stackrel{\·}{\mathrm{\ω}}=J^{-1}\{N_r-\mathrm{\ω}\×(\mathrm{J\ω}+h)+N_{\mathrm{\τ}}\}---\left(3\right)$

其中

为ω的微分，N_τ为空间扰动力矩；N_r为控制力矩，ω为星体坐标系相对轨道坐标系的角速率，J为星体惯量矩阵，h星体偏置动量。

相应的误差小量方程为：

$\mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}=J^{-1}\left\{\right[(J\widehat{\mathrm{\ω}}+h)\×]-(\widehat{\mathrm{\ω}}\×)J\}\·\mathrm{\Δ\ω}+J^{-1}\·N_{\mathrm{\τ}}---\left(4\right)$

(2)、(4)联合构成状态方程的话，系统状态量为：

X＝[δq₁ δq₂ δq₃ Δω₁ Δω₂ Δω₃]^T

2.2改进磁强计测量模型

定义量测模型为：

e_m＝(B_m×B_b)+(B_m-B_b)                   (5)

其中：e_m为磁场误差，B_m为磁强计测量值，B_b＝[B₁ B₂ B₃]^T为本体磁矢量估计值，同时定义v_m为磁强计量测噪声。

磁强计测量残差为：

B_m＝B_b+v_m                              (6)

则：

e_m＝((2(B_b×)δq₁₃+B_b+v_m)×B_b)+((2(B_b×)δq₁₃+B_b+v_m)-B_b)

＝-2[(B_b×)(B_b×)]δq₁₃+v_m×B_b+2(B_b×)δq₁₃+v_m

＝H′δq₁₃+v′_m

$H^{\′}=-2\left[\begin{array}{ccc}-(B_2^2+B_3^2)& 0& 0\\ 0& -(B_1^2+B_3^2)& 0\\ 0& 0& -(B_1^2+B_2^2)\end{array}\right]$

量测噪声为：v′_m＝v_m×B_b+v_m

当状态量为X时，量测阵为：H＝[H′0_3*3]_3*6。

2.3观测噪声模型的自适应修正

v′_m＝v_m×B_b+v_m

假设磁强计测量噪声v_m为均值Y(nT)白噪声。则：

量测噪声方差阵Diag表示对角矩阵，

分别表示量测噪声方差阵对角线元素；

[r₁ r₂ r₃]^T＝-(B_b×)[Y Y Y]^T+[Y Y Y]^T，Y为v_m均值，nT表示噪声单位纳特；

2.4滤波解算

卡尔曼滤波算法在建立系统方程、量测方程以后，就是按部就班的常规算法。本申请中，系统量测噪声方差阵为自适应计算，系统状态噪声方差阵则根据系统实际情况取常值就可以，并没有特别算法。

采用扩展卡尔曼滤波器进行滤波，建立离散的扩展卡尔曼滤波方程为

$\left\{\begin{array}{c}{\hat{X}}_{k+1|k}={\hat{X}}_{k|k}+f\left({\hat{X}}_{k|k}\right)T\\ P_{k+1|k}={\mathrm{\Φ}}_{k+1|k}{P_{k|k}\mathrm{\Φ}}_{k+1|k}^T+Q_k\\ K_{k+1}=P_{k+1|k}{H}_{k+1}^T{(H_{k+1}{P}_{k+1|k}{H}_{k+1}^T+R_{k+1})}^{-1}\\ {\hat{X}}_{k+1|k+1}={\hat{X}}_{k+1|k}+K_{k+1}(Z_{k+1}-h\left({\hat{X}}_{k+1|k}\right))\\ P_{k+1|k+1}=(I-K_{k+1}{H}_{k+1})P_{k+1|k}{(I-K_{k+1}{H}_{k+1})}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\end{array}\right.$

式中：

表示t_k时刻的状态对t_k+1时刻的状态的最优预测估计值；P_k+1|k表示最优预测估值误差协方差阵；

表示t_k+1时刻的状态的最优实时估计值；P_k+1|k+1表示最优滤波误差协方差阵；Φ_k+1|k表示状态矢量X从t_k时间转移到t_k+1时刻的状态转移矩阵；T表示采样周期；H_k+1表示t_k+1时刻观测矢量Z_k+1与t_k+1时刻的状态矢量X_k+1间的量测系数矩阵；Q_k表示系统状态噪声方差阵；R_k+1表示量测噪声方差阵。

解算后令只保留对角线元素，K阵简化为对角阵，滤波无矩阵求逆运算。

仿真结果

假设微小卫星为太阳同步轨道卫星，轨道高度约500km，姿态控制系统由星载计算机、三轴磁强计、偏置动量轮及三轴磁力矩器等构成。轨道测量位置误差优于10km、速度误差优于2ms。

卫星的剩磁矩为0.3Am²，在各个轴上的分量是diag[0.173 0.173 0.173]Am²。其余干扰力矩设为白噪声，量级为5e-7Nm，滤波时噪声参数调整为5e-6Nm。卫星的三轴转动惯量为diag[1.03 1.05 1.01]kgm²。磁强计的测量精度为300nT(1σ)，输出频率0.5Hz。

由图1～图3与相关数据分析可得，三轴指向精度滚动角优于2.0°，俯仰角优于2.0°，偏航角优于2.0°。

Claims (1)

1.一种基于简化自适应滤波的磁测微小卫星姿态确定方法，其特征在于包括如下步骤：

第一步：建立卫星姿态运动模型

四元素运动学微分方程为：

$\stackrel{\·}{q}=\frac{1}{2}[q\×]\mathrm{\ω}---\left(1\right)$

q＝[q₁q₂q₃q₄]^T为四元素，上标T表示转置，ω为星体坐标系相对轨道坐标系的角速率，上标表示微分，下同；

对式(1)进行求导，并忽略二阶小量可得：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{q}}_{13}=-[\widehat{\mathrm{\ω}}\×]\mathrm{\δ}{q}_{13}+\frac{1}{2}\mathrm{\Δ\ω}\\ \mathrm{\δ}{\stackrel{\·}{q}}_4=0\end{array}\right.---\left(2\right)$

为ω估计值，Δω＝[Δω₁Δω₂Δω₃]^T为ω误差，δq＝[δq₁δq₂δq₃δq₄]^T为四元素误差，δq₁₃＝[δq₁δq₂δq₃]^T；

卫星在外力矩的作用下，发生姿态的改变，外力矩包括卫星的控制力矩N_r与空间扰动力矩N_τ；

$\stackrel{\·}{\mathrm{\ω}}=J^{-1}\{N_r-\mathrm{\ω}\×(\mathrm{J\ω}+h)+N_{\mathrm{\τ}}\}---\left(3\right)$

其中J为星体惯量矩阵，h为星体偏置动量；

星体坐标系相对轨道坐标系的角速率相应的误差小量方程为：

$\mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}=J^{-1}\left\{\right[(J\widehat{\mathrm{\ω}}+h)\×]-(\widehat{\mathrm{\ω}}\×)J\}\·\mathrm{\Δ\ω}+J^{-1}\·N_{\mathrm{\τ}}---\left(4\right)$

由式(2)、(4)联合构成状态方程，系统状态量为：X＝[δq₁  δq₂  δq₃  Δω₁  Δω₂  Δω₃]^T；

第二步：改进磁强计量测模型

量测模型为：

e_m＝(B_m×B_b)+(B_m-B_b)    (5)

其中：e_m为磁场误差，B_m为磁强计测量值，B_b＝[B₁ B₂ B₃]^T为本体磁矢量估计值，同时v_m为磁强计量测噪声；

磁强计测量残差为：

B_m＝B_b+v_m    (6)

则：

e_m＝((2(B_b×)δq₁₃+B_b+v_m)×B_b)+((2(B_b×)δq₁₃+B_b+v_m)-B_b)

＝-2[(B_b×)(B_b×)]δq₁₃+v_m×B_b+2(B_b×)δq₁₃+v_m

＝H′δq₁₃+v′_m

$H^{\′}=-2\left[\begin{array}{ccc}-(B_2^2+B_3^2)& 0& 0\\ 0& -(B_1^2+B_3^2)& 0\\ 0& 0& -(B_1^2+B_2^2)\end{array}\right]$

量测噪声为：v′_m＝v_m×B_b+v_m

当状态量为X时，量测阵为：H＝[H′0_3*3]_3*6；

第三步：量测噪声模型的自适应修正

v′_m＝v_m×B_b+v_m

磁强计测量噪声v_m为均值Y(nT)白噪声，则：

量测噪声方差阵

Diag表示对角矩阵，

分别表示量测噪声方差阵对角线元素；

[r₁r₂r₃]^T＝-(B_b×)[YYY]^T+[YYY]^T，Y为v_m均值，nT表示噪声单位纳特；

第四步：滤波解算

采用卡尔曼滤波算法进行滤波解算确定卫星姿态。

Images