CN110109470A - 基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统 - Google Patents

Abstract

本发明属于卫星姿态确定技术领域，公开了一种基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统；基于UKF的星敏感器和陀螺联合定姿算法，构建微分形式非线性状态方程，并采用四阶龙格‑库塔积分法求解状态变量的时间更新，避免了非线性微分状态方程的离散化过程；利用UKF算法对星敏感器四元数和陀螺角速度进行滤波修正。本发明通过四阶龙格‑库塔积分法进行时间更新，利用四元数乘性误差计算加权均值与协方差，采用无迹卡尔曼滤波算法，引入星敏感器的观测值进行滤波更新，最后利用估计误差四元数对姿态数据进行修正。在姿态敏感器测量误差较大时具有良好的定姿性能，较基于扩展卡尔曼滤波的定姿算法有更高的估计精度。

Description

基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统

技术领域

本发明属于卫星姿态确定技术领域，尤其涉及一种基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统。

背景技术

目前，最接近的现有技术：姿态确定技术是通过某种算法，对姿态敏感器带误差测量值进行处理，减小误差，修正测量值的过程，广泛应用于卫星导航定位等领域。对测绘卫星来说，姿态确定的精度是高精度定位的重要前提。当前卫星上主要使用星敏感器和陀螺联合的形式，利用动态估计法进行滤波处理。动态估计法是利用一系列连续时刻的测量信息，采用姿态运动学模型等理论，融合多种姿态设备的测量信息来计算姿态信息。由于卫星导航系统以及卫星姿态确定系统一般都是非线性系统，近年来，卫星上广泛利用扩展卡尔曼滤波算法进行姿态确定。扩展卡尔曼滤波的关键步骤是对卫星的非线性状态方程和量测方程做泰勒级数展开并保留线性项，获得线性模型，之后再利用传统卡尔曼滤波进行处理。但由于EKF近似保留线性项，因此其仅适用于弱非线性系统，当系统为强非线性时，EKF的线性化处理会导致滤波精度降低，甚至滤波发散。为了克服上述缺点，提出无迹卡尔曼滤波算法，无迹卡尔曼滤波在估计点附近确定若干采样点，利用这些采样点所表示的高斯概率密度对非线性函数的概率密度分布进行近似，以替代EKF中的状态方程和量测方程近似过程。但UKF算法在解决状态方程为连续非线性常微分方程组的非线性定姿问题时，通常会对状态方程做离散化处理，一方面离散化过程推导复杂，另一方面会产生离散化误差。另外，传统UKF在解算状态变量的均值和协方差时，将四元数等同于一般向量求解，无法满足四元数归一化条件。

综上所述，现有技术存在的问题是：现有的算法对状态方程和量测方程进行线性化和离散化处理，不可避免地产生线性化和离散化误差，忽略四元数运算特性与物理意义，导致滤波性能下降。

解决上述技术问题的难度：

上述技术问题的难度在于：1)如何避免线性化和离散化处理，有效提升姿态估计的精度；2)如何将四元数的运算特性运用到均值和协方差矩阵的求解过程。

解决上述技术问题的意义：

通过本发明提供的方法，可以解决在系统测量误差较大时传统算法滤波性能下降的问题，对比传统滤波算法有更高的估计精度。

发明内容

针对现有技术存在的问题，本发明提供了一种基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统。

本发明是这样实现的，一种基于无迹卡尔曼滤波的联合定姿方法，所述基于无迹卡尔曼滤波的联合定姿方法采用误差四元数和陀螺常值漂移的非线性微分状态方程的UKF的星敏感器和陀螺联合定姿算法，构建微分形式非线性状态方程，并采用四阶龙格-库塔积分法求解状态变量的时间更新；利用四元数乘性误差计算加权均值与协方差，采用无迹卡尔曼滤波算法，引入星敏感器的观测值进行滤波更新，最后利用估计误差四元数对姿态数据进行修正。

进一步，所述基于无迹卡尔曼滤波的联合定姿方法的系统状态方程和量测方程构建：

(1)陀螺的量测模型表示为式：

ω_g＝ω+b+d+n_g；

卫星三轴稳定，其中ω_g、ω分别为陀螺输出的本体坐标系相对于惯性坐标系的三轴角速度测量值和理论真实值；b为陀螺仪的常值漂移噪声，满足η_b为白噪声；d为陀螺的随机漂移噪声，满足D_τ与时间常数τ有关，η_d为白噪声；η_g为陀螺的测量噪声，一般认为是高斯白噪声；

(2)利用星敏感器误差四元数以及陀螺的常值漂移误差定义系统状态变量：

其中Δq₄为标量，Δq＝[Δq₁ Δq₂ Δq₃]^T为矢量，且满足Δq^TΔq+Δq₄ ²＝1；

(3)利用姿态运动学方程和陀螺的量测模型建立非线性微分系统状态方程。基于四元数的姿态运动学方程式：

其中，

实际四元数估计四元数误差四元数满足：

联合建立非线性微分系统状态方程：

其中系统噪声服从N(0，Q)， 为估计角速度，满足

(4)定义星敏感器的量测残差为星敏感器的估计值与测量值之差，即：

根据建立非线性离散量测方程：

其中z_k是误差四元数k时刻的量测值，H＝[I_4×4 0_4×3]为观测矩阵，量测噪声υ_k服从N(0，R)。

进一步，所述基于无迹卡尔曼滤波的联合定姿方法具体包括：

输入：星敏感器量测值、陀螺的量测值；

输出：估计姿态四元数；

1)设置初始状态变量、初始状态协方差矩阵、系统噪声协方差矩阵、量测噪声协方差矩阵；

2)k+1时刻的姿态预测，利用k时刻的姿态估计值及k时刻的角速度估计值预测k+1时刻的姿态；计算k时刻的角度增量；将Δω_k转换成四元数增量最后预测k+1时刻的姿态

若无星敏感器测量值，则k+1时刻的姿态估计值等于k+1时刻的姿态预测值，转2)；若有星敏感器测量值，转3)进入UKF滤波；

3)对状态变量进行增广，并生成sigma点集；

4)进行时间更新；计算状态变量的统计特性；

5)量测更新，计算得到k+1时刻的误差四元数和陀螺常值漂移估计值；

6)姿态与角速度修正，利用下式对姿态四元数与陀螺角速度进行修正；之后将状态变量置零，转2)；

进一步，首先计算k时刻的角度增量其次利用将Δω_k转换成四元数增量最后利用预测k+1时刻的姿态

进一步，利用式对状态变量进行增广，并根据式生成sigma点集。

进一步，利用式进行时间更新；利用式计算陀螺常值漂移的统计特性。构造四阶矩阵解算M的最大特征值对应的特征向量，该特征向量即为四元数均值利用式计算星敏感器误差四元数的协方差矩阵。

进一步，利用公式计算得到k+1时刻的误差四元数和陀螺常值漂移估计值。

本发明的另一目的在于提供一种应用所述基于无迹卡尔曼滤波的联合定姿方法的航天器姿态控制系统。

本发明的另一目的在于提供一种应用所述基于无迹卡尔曼滤波的联合定姿方法的飞行器。

本发明的另一目的在于提供一种应用所述基于无迹卡尔曼滤波的联合定姿方法的卫星。

综上所述，本发明的优点及积极效果为：星敏感器和陀螺联合定姿是当前获取卫星高精度姿态的主要手段。针对扩展卡尔曼滤波算法存在线性化近似、容易发散的弊端，实现了一种基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统算法。本发明推导了基于误差四元数和陀螺常值漂移的非线性微分状态方程，通过四阶龙格-库塔积分法进行时间更新，利用四元数乘性误差计算加权均值与协方差，采用无迹卡尔曼滤波算法，引入星敏感器的观测值进行滤波更新，最后利用估计误差四元数对姿态数据进行修正。仿真实验结果表明，该算法在姿态敏感器测量误差较大时具有良好的定姿性能，较基于扩展卡尔曼滤波的定姿算法有更高的估计精度。

表1 EKF和UKF定姿中误差对比

附图说明

图1是本发明实施例提供的基于无迹卡尔曼滤波的联合定姿方法流程图。

图2是本发明实施例提供的基于无迹卡尔曼滤波的联合定姿方法实现流程图。

图3是本发明实施例提供的EKF和UKF定姿误差对比曲线(陀螺测量误差：2″/s)示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明基于UKF的星敏感器和陀螺联合定姿算法，构建微分形式非线性状态方程，并采用四阶龙格-库塔积分法求解状态变量的时间更新；避免了非线性微分状态方程的离散化过程，利用UKF算法对星敏感器四元数和陀螺角速度进行滤波修正；另外引入四元数乘性误差以保证四元数的归一化约束性质及几何意义，实验结果验证了算法的有效性。

下面结合附图对本发明的应用原理作详细的描述。

如图1所示，本发明实施例提供的基于无迹卡尔曼滤波的联合定姿方法包括以下步骤：

S101：基于误差四元数和陀螺常值漂移的非线性微分状态方程，通过四阶龙格-库塔积分法进行时间更新；

S102：利用四元数乘性误差计算加权均值与协方差，采用无迹卡尔曼滤波算法，引入星敏感器的观测值进行滤波更新；

S103：利用估计误差四元数对姿态数据进行修正。

下面结合附图对本发明的应用原理作进一步的描述。

1基本原理

1.1无迹变换基本原理

无迹变换(Unscented Transform,UT)在估计点附近确定若干采样点，利用采样点所表示的高斯概率密度对非线性函数的概率密度分布进行近似，以替代EKF滤波中的状态方程和量测方程近似过程。其基本原理如下：假设有一非线性变换y＝f(x)，x是n维状态变量，均值为协方差为P_x，可以通过无迹变换来计算y的均值和协方差矩阵P_y。首先构建2n+1个对称分布的采样点(Sigma点)：

其中，为矩阵的LL^T分解；为矩阵的第i列(或行)。λ为缩放比例系数。

之后将Sigma点集通过非线性函数y＝f(x)，得到点集{yⁱ}，计算{yⁱ}的统计特征如(2)：

权值为：

其中α决定了sigma点的散布程度，10^-4≤α≤1；β描述了状态变量的分布，对高斯分布取2达到最优；λ＝α²(n+k)-n是缩放比例参数，k保证矩阵(N+λ)P为半正定矩阵，通常取0。

1.2四阶龙格-库塔积分基本原理

龙格-库塔积分法是一种常用的常微分方程的数值解法，是高精度的单步法。利用龙格-库塔积分代替UKF算法中对连续微分方程离散化的过程，避免繁琐的计算，同时四阶龙格-库塔积分能够得到近似四阶泰勒级数的精度。其基本原理如下：

设有m个一阶方程组成的常微分方程组，其矩阵形式为(4)：

四阶龙格-库塔公式为(5)：

其中：

1.3无迹卡尔曼滤波算法原理

UKF是将UT变换和kalman滤波框架结合的一种算法。将UT变换应用于Kalman滤波中状态和量测的均值及协方差的递推中，即能够得到UKF滤波器。与EKF算法相同，UKF算法沿袭了Kalman滤波框架。但UKF不必对系统做近似线性化处理，避免了雅克比矩阵的求解；EKF算法是对非线性对象的一阶或二阶近似，而UT变换能达到泰勒级数三阶精度；UKF能适用于强非线性系统下的估计，不会滤波发散，且估计精度更高、稳定性更强。

非线性状态方程和量测方程的一般形式分别如(6)、(7)：

z_k＝h(x_k，v_k) (7)

其中x(t)是t时刻系统状态变量；z_k是系统量测向量；w(t)是t时刻系统噪声，服从N(0，Q)；v_k是量测噪声，服从N(0，R)。

1)初始化。对于非可加性噪声，需要对状态变量进行扩展，将系统噪声和量测噪声向量添加进状态变量中一同进行估计，形成增广状态向量和增广协方差矩阵分为为(8)、(9)：

其中x₀是n维初始状态变量；P₀是对应的初始状态协方差矩阵；是增广状态向量；P_a，0是增广状态协方差矩阵；N是增广后的维度。

2)利用无迹变换原理处理均值和协方差的非线性传递问题。根据公式(1)生成k时刻的Sigma点集其中，任意都能够分解为

3)时间更新

时间更新时需要计算k+1时刻的状态变量值，通常做法是对式(6)进行离散化处理，但离散化需要对方程进行泰勒展开并忽略二阶以上项，这不可避免地会产生离散化误差，也会带来大量繁琐的计算。因此，本发明利用四阶龙格-库塔法对式(6)在(t，t+Δt)区间内积分，得到式(10)：

计算：

4)量测更新

2算法设计与实现

2.1系统状态方程和量测方程构建

陀螺的量测模型可以表示为式(13)：

ω_g＝ω+b+d+n_g (13)

假设卫星三轴稳定，其中ω_g、ω分别为陀螺输出的本体坐标系相对于惯性坐标系的三轴角速度测量值和理论真实值；b为陀螺仪的常值漂移噪声，满足η_b为白噪声；d为陀螺的随机漂移噪声，满足D_τ与时间常数τ有关，η_d为白噪声；η_g为陀螺的测量噪声，一般认为是高斯白噪声。

星敏感器获得的是星敏坐标系相对于惯性坐标系的四元数数据(可以通过乘安装矩阵转换为本体坐标系相对于惯性坐标系的数据)，由于星敏感器精度较高，一般认为测量噪声为高斯白噪声。

利用星敏感器误差四元数以及陀螺的常值漂移误差定义系统状态变量：

其中Δq₄为标量，Δq＝[Δq₁ Δq₂ Δq₃]^T为矢量，且满足Δq^TΔq+Δq₄ ²＝1

利用姿态运动学方程和陀螺的量测模型建立非线性微分系统状态方程。基于四元数的姿态运动学方程如式(14)：

其中，

实际四元数估计四元数误差四元数满足(15)：

对(15)左右求导，将(14)代入并化简，联合建立非线性微分系统状态方程如(16)：

其中系统噪声服从N(0，Q)， 为估计角速度，满足

定义星敏感器的量测残差为星敏感器的估计值与测量值之差，即：

根据式(17)建立非线性离散量测方程：

其中z_k是误差四元数k时刻的量测值(即测量残差)，H＝[I_4×4 O_4×3]为观测矩阵，量测噪声v_k服从N(0，Q)。

2.2算法具体实现

针对基于无迹卡尔曼滤波的星敏感器和陀螺组合定姿算法做出算法流程图2。

输入：星敏感器量测值(四元数)、陀螺的量测值(角速度)

输出：估计姿态四元数

1)参数初始化。设置初始状态变量、初始状态协方差矩阵、系统噪声协方差矩阵、量测噪声协方差矩阵等；

2)k+1时刻的姿态预测。利用k时刻的姿态估计值及k时刻的角速度估计值预测k+1时刻的姿态。首先计算k时刻的角度增量其次利用式(19)将Δω_k转换成四元数增量最后利用式(15)预测k+1时刻的姿态

若无星敏感器测量值，则k+1时刻的姿态估计值等于k+1时刻的姿态预测值，转2)；若有星敏感器测量值，转3)进入UKF滤波。

3)生成sigma点集。利用式(8)、(9)对状态变量进行增广，并根据式(1)生成sigma点集。

4)时间更新。利用式(10)、(11)进行时间更新。利用式(3)计算陀螺常值漂移的统计特性。注意四元数在计算均值和协方差矩阵时与普通向量有所区别，应该利用四元数乘性误差进行计算，以保证四元数的归一性及几何意义的完整：

均值：构造四阶矩阵解算M的最大特征值对应的特征向量，该特征向量即为四元数均值

协方差矩阵：

5)量测更新。利用公式(12)计算得到k+1时刻的误差四元数和陀螺常值漂移估计值。

6)姿态与角速度修正。利用式(20)对姿态四元数与陀螺角速度进行修正。之后将状态变量置零，转2)。

下面结合试验对本发明的应用效果作详细的描述。

1、利用仿真数据对UKF定姿算法进行实验。实验数据的参数为：星敏感器采样频率为5Hz，陀螺采样频率为10Hz，星敏感器测量精度为5″，陀螺测量精度为0.1～2.5″/s。同时仿真了无误差的星敏感器姿态四元数，作为对比使用。UKF定姿的参数设置为：状态变量初值0_7×1，陀螺常值漂移初值0_3×1，常值漂移噪声均方根值为10^-6I_3×3。

定姿误差指标为三轴(俯仰、横滚、偏航)姿态中误差(″)，首先计算每一时刻的姿态误差四元数，接着将每一时刻的误差四元数转换为三轴姿态角，最后统计每一个轴向的姿态中误差。

给出上述实验条件下的姿态估计结果，如图3、表1所示。

表1 EKF和UKF定姿中误差对比

由图3可知，本发明提出的定姿算法与传统EKF定姿算法相比，更加平稳；由表1可知，在其他条件相同，陀螺精度需≤0.8″/s，EKF定姿精度才能优于1″，而使用本发明提出的UKF算法只需陀螺精度≤1.0″/s；在姿态精度较好时，EKF和UKF差距不大，但在姿态精度较差时，例如陀螺测量精度为2.2″/s时，UKF三轴精度较EKF提升14％、12％、13％，UKF定姿算法优于传统EKF定姿方法，有良好的定姿性能。

本发明为了解决EKF算法存在线性化近似、容易发散的问题，对基于UKF的星敏感器和陀螺联合定姿算法进行研究。经过仿真实验表明，与传统的EKF算法相比，本发明的算法即使在传感器测量精度不高时，也能获得较好的姿态估计结果，而且更加稳定。该算法为进一步提高卫星姿态确定的精度提供了参考，具有重要的意义。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于无迹卡尔曼滤波的联合定姿方法，其特征在于，所述基于无迹卡尔曼滤波的联合定姿方法采用误差四元数和陀螺常值漂移的非线性微分状态方程的UKF的星敏感器和陀螺联合定姿算法，构建微分形式非线性状态方程，并采用四阶龙格-库塔积分法求解状态变量的时间更新；利用四元数乘性误差计算加权均值与协方差，采用无迹卡尔曼滤波算法，引入星敏感器的观测值进行滤波更新，最后利用估计误差四元数对姿态数据进行修正。

2.如权利要求1所述的基于无迹卡尔曼滤波的联合定姿方法，其特征在于，所述基于无迹卡尔曼滤波的联合定姿方法的系统状态方程和量测方程构建：

(1)陀螺的量测模型表示为式：

ω_g＝ω+b+d+n_g；

卫星三轴稳定，其中ω_g、ω分别为陀螺输出的本体坐标系相对于惯性坐标系的三轴角速度测量值和理论真实值；b为陀螺仪的常值漂移噪声，满足η_b为白噪声；d为陀螺的随机漂移噪声，满足D_τ与时间常数τ有关，η_d为白噪声；η_g为陀螺的测量噪声；

(2)利用星敏感器误差四元数以及陀螺的常值漂移误差定义系统状态变量：

其中Δq₄为标量，Δq＝[Δq₁ Δq₂ Δq₃]^T为矢量，且满足Δq^TΔq+Δq₄ ²＝1；

(3)利用姿态运动学方程和陀螺的量测模型建立非线性微分系统状态方程；基于四元数的姿态运动学方程式：

其中，

实际四元数估计四元数误差四元数满足：

联合建立非线性微分系统状态方程：

其中系统噪声服从N(0，Q)， 为估计角速度，满足

(4)定义星敏感器的量测残差为星敏感器的估计值与测量值之差：

建立非线性离散量测方程：

其中z_k是误差四元数k时刻的量测残差，H＝[I_4×4 0_4×3]为观测矩阵，量测噪声v_k服从N(0，R)。

3.如权利要求1所述的基于无迹卡尔曼滤波的联合定姿方法，其特征在于，所述基于无迹卡尔曼滤波的联合定姿方法具体包括：

输入：星敏感器量测值、陀螺的量测值；

输出：估计姿态四元数；

1)设置初始状态变量、初始状态协方差矩阵、系统噪声协方差矩阵、量测噪声协方差矩阵；

2)k+1时刻的姿态预测；

若无星敏感器测量值，则k+1时刻的姿态估计值等于k+1时刻的姿态预测值，转2)；若有星敏感器测量值，转3)进入UKF滤波；

3)对状态变量进行增广，并生成sigma点集；

4)进行时间更新，计算状态变量的统计特性；

5)量测更新，计算得到k+1时刻的误差四元数和陀螺常值漂移估计值；

6)姿态与角速度修正，利用下式对姿态四元数与陀螺角速度进行修正；之后将状态变量置零，转2)；

4.如权利要求3所述的基于无迹卡尔曼滤波的联合定姿方法，其特征在于，所述方法中步骤2)具体包括：

首先计算k时刻的角度增量

其次利用将Δw_k转换成四元数增量

最后利用预测k+1时刻的姿态

5.如权利要求3所述的基于无迹卡尔曼滤波的联合定姿方法，其特征在于，所述方法中步骤3)具体包括：

利用式对状态变量进行增广，并根据式生成sigma点集。

6.如权利要求3所述的基于无迹卡尔曼滤波的联合定姿方法，其特征在于，所述方法中步骤4)中具体包括：

步骤4a)时间更新

利用式进行时间更新；

步骤4b)计算陀螺常值漂移的统计特性

利用式计算陀螺常值漂移的统计特性；

步骤4c)计算星敏感器误差四元数的统计特性

构造四阶矩阵解算M的最大特征值对应的特征向量，该特征向量即为误差四元数均值

利用式计算误差四元数的协方差矩阵。

7.如权利要求3所述的基于无迹卡尔曼滤波的联合定姿方法，其特征在于，所述方法中步骤5)具体包括：

利用公式计算得到k+1时刻的误差四元数和陀螺常值漂移估计值。

8.一种应用权利要求1～7任意一项所述基于无迹卡尔曼滤波的联合定姿方法的航天器姿态控制系统。

9.一种应用权利要求1～7任意一项所述基于无迹卡尔曼滤波的联合定姿方法的飞行器。

10.一种应用权利要求1～7任意一项所述基于无迹卡尔曼滤波的联合定姿方法的卫星。