一种权值及参考四元数修正的无迹四元数姿态估计方法

摘要

本发明涉及一种权值及参考四元数修正的无迹四元数姿态估计方法，包括：过陀螺和星敏感器获取量测数据作为量测量；建立基于四元数的离散型航天器非线性状态空间模型；在k‑1时刻利用基于参数及参考四元数修正的无迹四元数估计器估计k时刻误差四元数、陀螺漂移和相应的误差协方差；针对上述姿态估计非线性滤波时间为Ntime，若k＜Ntime则重复步骤三，若k＝Ntime，则滤波结束，输出姿态四元数、陀螺漂移和相应的误差协方差。本发明的有益效果是：本发明有利于提高姿态估计的精度和改善姿态估计算法的稳定性。

权利要求

1.一种权值及参考四元数修正的无迹四元数姿态估计方法，其特征在于，包括：步骤1、过陀螺和星敏感器获取量测数据作为量测量；

步骤2、建立基于四元数的离散型航天器非线性状态空间模型；

步骤3、在k‑1时刻利用基于参数及参考四元数修正的无迹四元数估计器估计k时刻误差四元数、陀螺漂移和相应的误差协方差；

步骤4、设置滤波时间为Ntime，若k＜Ntime则重复步骤三，若k＝Ntime，则滤波结束，输出姿态四元数、陀螺漂移和相应的误差协方差；

其中，步骤2包括：

步骤2.1、建立航天器的离散型四元数状态运动方程：T

式中，q＝[q1 q2 q3 q4]表示四元数矢量，Ω[·]与ψk‑1表示k‑1时刻的函数符号，ω＝T[ω1  ω2ω3]表示陀螺三轴角速度输出矢量， 表示k‑1时刻的角速度估计值， 表示姿态四元数q在k‑1时刻的估计值，I3×3为3×3的单位阵，Δt表示陀螺采样区间，||·||表示向量的范数， 表示ψk‑1的转置，[ψk‑1×]表示ψk‑1的反对称矩阵；

步骤2.2、建立离散型的角速度测量模型：

1/2

βk＝βk‑1+σuΔt Nu

式中， 表示k时刻陀螺的输出值，ωk表示k时刻陀螺的真实值，βk表示k时刻陀螺漂移， 和 为陀螺测量噪声和漂移噪声的均方差，Nv和Nu是均值为零的高斯白噪声，方差为3维的单位阵；

步骤2.3、建立星敏感器观测模型：式中，zk表示量测量，ri表示第i个参考矢量，i＝1，…，L，L为星敏感器所观测到恒星数目；vi为零均值高斯白噪声，其协方差为 而所有vi的协方差构成量测方差Rk；A(q)表示姿态矩阵，其定义如下：T

式中，ρ＝[q1 q2 q3]表示四元数向量部分，[ρ×]表示ρ的反对称矩阵；

其中，步骤3包括：

步骤3.1、计算k‑1时刻西格玛点及相应权值，并计算需要传播的四元数：步骤3.2、使用离散的四元数运动方程更新四元数；

步骤3.3、根据以下方程计算参考四元数：式中，

λ为拉格朗日乘性因子，方程的解中最小特征值对应的特征向量设为参考四元数

步骤3.4、传播陀螺漂移：

步骤3.5、一步预测状态及相应的误差协方差估计为：其中， 和 均表示权值；

步骤3.6、量测更新；

步骤3.7、计算k时刻的滤波增益、状态矢量及相应的误差协方差：其中，一步量测预测 一步量测预测方差Pzz,k|k‑1以及互协方差Pxz,k|k‑1；

步骤3.8、更新四元数：

步骤3.9、重置 为零矢量，为下一次滤波循环做准备。

2.根据权利要求1所述的权值及参考四元数修正的无迹四元数姿态估计方法，其特征在于，步骤3.1包括：步骤3.1.1、计算k‑1时刻西格玛点及相应权值：式中， 表示 的第i列，n表示状态的维数，κ为无迹卡尔曼滤波调节参数，Qk为过程噪声，定义如下：将西格玛点划分为姿态误差部分和陀螺漂移部分：步骤3.1.2、计算需要传播的四元数：式中， 表示四元数乘积，误差四元数 和δρi，k‑1通过下式计算：

‑1

δρ＝f [a+δq4]δp

‑1

式中，δ(·)表示误差，f 表示f的逆，a与f为广义罗德里格参数。

3.根据权利要求2所述的权值及参考四元数修正的无迹四元数姿态估计方法，其特征在于，步骤3.2中，所述离散的四元数运动方程表示为：则，误差四元数通过四元数乘积可求得：姿态误差部分西格玛点 通过下式求解，δp表示广义罗德里格参数：

4.根据权利要求3所述的权值及参考四元数修正的无迹四元数姿态估计方法，其特征在于，步骤3.6包括：步骤3.6.1、重新计算量测更新部分西格玛点对应的权值：式中，为无迹卡尔曼滤波调节参数；

步骤3.6.2、计算西格玛点：

步骤3.6.3、一步量测预测 一步量测预测方差Pzz,k|k‑1以及互协方差Pxz,k|k‑1计算如下：

5.根据权利要求4所述的权值及参考四元数修正的无迹四元数姿态估计方法，其特征在于，步骤3.8中，根据步骤3.1.2中 和δρi,k‑1的计算式求解出误差四元数根据四元数乘积，并以步骤3.3的计算式中求出的最小特征值对应的特征矢量为参考四元数，则k时刻的四元数求解如下：

说明书

一种权值及参考四元数修正的无迹四元数姿态估计方法

技术领域

[0001] 本发明涉及姿态估计技术领域，更确切地说，它涉及一种权值及参考四元数修正的无迹四元数姿态估计方法。

背景技术

[0002] 描述航天器姿态的最为重要的表示参数包括欧拉角、方向余弦、广义罗德里格参数和四元数。其中四元数由于没有奇异性、计算简单而最受关注，且广泛地应用于姿态估计系统中。基于四元数的航天器姿态估计模型具有较高的非线性特性，尤其是量测模型由非线性函数建立，因此姿态估计本质上是一个非线性滤波问题。另外，四元数存在归一化约束问题。对于解决以上问题，扩展卡尔曼滤波(Extended Kalman Filter,EKF)已成为针对姿态估计系统的最为广泛使用的非线性滤波算法。例如，先后提出的乘性扩展卡尔曼滤波(Multiplicative EKF,MEKF)、向后平滑扩展卡尔曼滤波(Backwards‑Smoothing EKF,BSEKF)以及范数约束扩展卡尔曼滤波(Norm‑constrained EKF,NEKF)等等。

[0003] 然而，扩展卡尔曼滤波截断了泰勒展开的高阶项，估计精度有限，这类算法一般只在较小的初始误差条件下具有满意的滤波结果。因此有必要去研究新的非线性滤波算法，如之后提出的无迹四元数估计器(Unscented Quaternion Estimator,USQUE)、稀疏高斯厄密特正交滤波(Sparse Gauss‑Hermite Quadrature Filter,SGHQF)及粒子滤波(Particle Filter,PF)算法。而SGHQF与PF算法计算量较大。其中，无迹四元数估计器采用广义罗德里格参数与四元数切换的方法避免四元数归一化约束问题，从而有效地求解预测均值和相应的协方差。然而，此类算法中参考四元数的选择及参数的设定有待进一步修正。针对无迹卡尔曼滤波，其中，计算更新四元数的参考四元数应该是一步预测四元数的均值。而确定均值四元数的方法包括奇异值分解方法和基于特征矢量的方法，在此基础上，拉格朗日函数方法是一种相对简单的求解乘性四元数加权均值的方法。另外，无迹卡尔曼滤波器中存在多个参数需要因实际环境进行设定，而姿态估计问题不同于单变量模型及目标跟踪等大多数类问题，相对更为复杂，参数的设定对算法的效果有着极为明显的影响。因此，基于以上讨论，研究一种权值及参考四元数修正的无迹四元数姿态估计方法对提高姿态估计系统的估计精度和稳定性具有重要的理论及实际意义。

发明内容

[0004] 本发明的目的是克服现有技术中的不足，为了提高姿态估计系统的估计精度和稳定性，提供了一种权值及参考四元数修正的无迹四元数姿态估计方法(Weight and reference quaternion correction unscented quaternion estimator,CUSQUE)，包括：

[0005] 步骤1、过陀螺和星敏感器获取量测数据作为量测量；

[0006] 步骤2、建立基于四元数的离散型航天器非线性状态空间模型；

[0007] 步骤3、在k‑1时刻利用基于参数及参考四元数修正的无迹四元数估计器估计k时刻误差四元数、陀螺漂移和相应的误差协方差；

[0008] 步骤4、设置滤波时间为Ntime，若k

[0009] 作为优选，步骤2包括：

[0010] 步骤2.1、建立航天器的离散型四元数状态运动方程：

[0011]

[0012]

[0013]

[0014] 式中，q＝[q1 q2 q3 q4]T表示四元数矢量，Ω[·]与ψk‑1表示k‑1时刻的函数符号，Tω＝[ω1  ω2  ω3] 表示陀螺三轴角速度输出矢量， 表示k‑1时刻的角速度估计值，表示姿态四元数q在k‑1时刻的估计值，I3×3为3×3的单位阵，Δt表示陀螺采样区间，‖·‖表示向量的范数， 表示ψk‑1的转置，[ψk‑1×]表示ψk‑1的反对称矩阵；

[0015] 步骤2.2、建立离散型的角速度测量模型：

[0016]

[0017] βk＝βk‑1+σuΔt1/2Nu

[0018] 式中， 表示k时刻陀螺的输出值，ωk表示k时刻陀螺的真实值，βk表示k时刻陀螺漂移， 和 为陀螺测量噪声和漂移噪声的均方差，Nv和Nu是均值为零的高斯白噪声，方差为3维的单位阵；

[0019] 步骤2.3、建立星敏感器观测模型：

[0020]

[0021] 式中，zk表示量测量，ri表示第i个参考矢量，i＝1,…,L，L为星敏感器所观测到恒星数目；vi为零均值高斯白噪声，其协方差为 而所有vi的协方差构成量测方差Rk；A(q)表示姿态矩阵，其定义如下

[0022]

[0023] 式中，ρ＝[q1 q2 q3]T表示四元数向量部分，[ρ×]表示ρ的反对称矩阵。

[0024] 作为优选，步骤3包括：

[0025] 步骤3.1.1、计算k‑1时刻西格玛点及相应权值：

[0026]

[0027]

[0028]

[0029]

[0030]

[0031]

[0032] 式中， 表示 的第i列， 及 均表示权值，n表示状态的维数，κ为无迹卡尔曼滤波调节参数，Qk为过程噪声，定义如下：

[0033]

[0034] 将西格玛点划分为姿态误差部分和陀螺漂移部分：

[0035]

[0036] 步骤3.1.2、计算需要传播的四元数：

[0037]

[0038] 式中， 表示四元数乘积，误差四元数 和δρi,k‑1通过下式计算：

[0039]

[0040] δρ＝f‑1[a+δq4]δp

[0041] 式中，δ(·)表示误差，f‑1表示f的逆，a与f为广义罗德里格参数；

[0042] 步骤3.2、使用离散的四元数运动方程更新四元数：

[0043]

[0044] 则，误差四元数通过四元数乘积可求得：

[0045]

[0046] 姿态误差部分西格玛点 通过下式求解，δp表示广义罗德里格参数：

[0047]

[0048] 步骤3.3、根据以下方程计算参考四元数：

[0049]

[0050] 式中，λ为拉格朗日乘性因子，方程的解中最小特征值对应
的特征向量设为参考四元数

[0051] 步骤3.4、传播陀螺漂移：

[0052]

[0053] 步骤3.5、一步预测状态及相应的误差协方差估计为：

[0054]

[0055]

[0056] 步骤3.6、量测更新，包括：

[0057] 步骤3.6.1、重新计算量测更新部分西格玛点对应的权值：

[0058]

[0059]

[0060] 式中，为无迹卡尔曼滤波调节参数；

[0061] 步骤3.6.2、计算西格玛点：

[0062]

[0063] 步骤3.6.3、一步量测预测 一步量测预测方差Pzz,k|k‑1以及互协方差Pxz,k|k‑1计算如下：

[0064]

[0065]

[0066]

[0067] 步骤3.7、计算k时刻的滤波增益、状态矢量及相应的误差协方差：

[0068]

[0069]

[0070]

[0071] 步骤3.8、更新四元数：

[0072]

[0073] 根据步骤3.1.2中 和δρi,k‑1的计算式求解出误差四元数根据四元数乘积，并以步骤3.3的计算式中求出的最小特征值对应的特征矢量为参考四元数，则k时刻的四元数求解如下：

[0074]

[0075] 步骤3.9、重置 为零矢量，为下一次滤波循环做准备。

[0076] 作为优选，步骤3中，权值参数为κ＝‑3, 广义罗德里格参数为a＝1,f＝4。

[0077] 作为优选，步骤4中，滤波时间Ntime＝90min。

[0078] 本发明的有益效果是：

[0079] (1)本发明采用无迹变换理论近似非线性函数的均值和方差，可避免计算雅克比矩阵，有利于提高姿态估计的精度和改进算法的稳定性。

[0080] (2)本发明在时间更新和量测更新中，对权值的不同设定，不仅提高了姿态估计的精度而且有利于降低算法的计算量。

[0081] (3)本发明采用拉格朗日函数方法求解四元数加权均值，并作为求解更新四元数的参考四元数，有利于提高姿态估计的精度和改善算法的稳定性。

附图说明

[0082] 图1是仿真中星敏感器观测到的可用恒星的数目结果图；

[0083] 图2是情形1初始条件误差情况下所有滤波估计的姿态角均方根误差结果对比图；

[0084] 图3是情形2初始条件误差情况下所有滤波估计的姿态角均方根误差结果对比图；

[0085] 图4是情形3初始条件误差情况下所有滤波估计的姿态角均方根误差结果对比图；

[0086] 图5是情形4初始条件误差情况下所有滤波估计的姿态角均方根误差结果对比图；

[0087] 图6是情形4初始条件误差情况下USQUE和CUSQUE三个方向的姿态角误差结果对比图；

[0088] 图7是一种权值及参考四元数修正的无迹四元数姿态估计方法的流程图。

具体实施方式

[0089] 下面结合实施例对本发明做进一步描述。下述实施例的说明只是用于帮助理解本发明。应当指出，对于本技术领域的普通人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

[0090] 作为一种实施例，本申请提供了一种权值及参考四元数修正的无迹四元数姿态估计方法，包括：

[0091] 步骤1、过陀螺和星敏感器获取量测数据作为量测量；

[0092] 步骤2、建立基于四元数的离散型航天器非线性状态空间模型；

[0093] 步骤3、在k‑1时刻利用基于参数及参考四元数修正的无迹四元数估计器估计k时刻误差四元数、陀螺漂移和相应的误差协方差；

[0094] 步骤4、设置滤波时间为Ntime，若k

[0095] 步骤2包括：

[0096] 步骤2.1、建立航天器的离散型四元数状态运动方程：

[0097]

[0098]

[0099]

[0100] 式中，q＝[q1 q2 q3 q4]T表示四元数矢量，ω＝[ω1  ω2  ω3]T表示陀螺三轴角速度输出矢量， 表示姿态四元数q在k‑1时刻的估计值，I3×3为3×3的单位阵，Δt表示陀螺采样区间，‖·‖表示向量的范数， 表示ψk‑1的转置，[ψk‑1×]表示ψk‑1的反对称矩阵；例如数学符号[ω×]表示：

[0101]

[0102] 步骤2.2、建立离散型的角速度测量模型：

[0103]

[0104] βk＝βk‑1+σuΔt1/2Nu

[0105] 式中， 表示k时刻陀螺的输出值，ωk表示k时刻陀螺的真实值，βk表示k时刻陀螺漂移， 和 为陀螺测量噪声和漂移噪声的均方差，Nv和Nu是均值为零的高斯白噪声，方差为3维的单位阵；

[0106] 步骤2.3、建立星敏感器观测模型：

[0107]

[0108] 式中，zk表示量测量，ri表示第i个参考矢量，i＝1,…,L，L为星敏感器所观测到恒星数目；vi为零均值高斯白噪声，其协方差为 而所有vi的协方差构成量测方差Rk；A(q)表示姿态矩阵，其定义如下

[0109]

[0110] 式中，ρ＝[q1 q2 q3]T表示四元数向量部分，[ρ×]表示ρ的反对称矩阵。

[0111] 步骤3包括：

[0112] 步骤3.1.1、计算k‑1时刻西格玛点及相应权值：

[0113]

[0114]

[0115]

[0116]

[0117]

[0118]

[0119] 式中， 表示 的第i列， 及 均表示权值，n表示状态的维数，Qk为过程噪声，定义如下：

[0120]

[0121] 将西格玛点划分为姿态误差部分和陀螺漂移部分：

[0122]

[0123] 步骤3.1.2、计算需要传播的四元数：

[0124]

[0125] 式中， 表示四元数乘积，误差四元数 和δρi,k‑1通过下式计算：

[0126]

[0127] δρ＝f‑1[a+δq4]δp

[0128] 式中，δ(·)表示误差，f‑1表示f的逆；

[0129] 步骤3.2、使用离散的四元数运动方程更新四元数：

[0130]

[0131] 则，误差四元数通过四元数乘积可求得：

[0132]

[0133] 姿态误差部分西格玛点 通过下式求解，δp表示广义罗德里格参数：

[0134]

[0135] 步骤3.3、根据以下方程计算参考四元数：

[0136]

[0137] 式中，λ为拉格朗日乘性因子，方程的解中最小特征值对应
的特征向量设为参考四元数

[0138] 步骤3.4、传播陀螺漂移：

[0139]

[0140] 步骤3.5、一步预测状态及相应的误差协方差估计为：

[0141]

[0142]

[0143] 步骤3.6、量测更新，包括：

[0144] 步骤3.6.1、重新计算量测更新部分西格玛点对应的权值：

[0145]

[0146]

[0147] 步骤3.6.2、计算西格玛点：

[0148]

[0149] 步骤3.6.3、一步量测预测 一步量测预测方差Pzz,k|k‑1以及互协方差Pxz,k|k‑1计算如下：

[0150]

[0151]

[0152]

[0153] 步骤3.7、计算k时刻的滤波增益、状态矢量及相应的误差协方差：

[0154]

[0155]

[0156]

[0157] 步骤3.8、更新四元数：

[0158]

[0159] 根据步骤3.1.2中 和δρi,k‑1的计算式求解出误差四元数根据四元数乘积，并以步骤3.3的计算式中求出的最小特征值对应的特征矢量为参考四元数，则k时刻的四元数求解如下：

[0160]

[0161] 步骤3.9、重置 为零矢量，为下一次滤波循环做准备。

[0162] 以下通过Matlab仿真软件，在以下仿真条件对本申请所提供的方法进行仿真实验：

[0163] 步骤2中角速度ω＝[‑1/(90/(2π)×60) 0 0]T，星敏感器采样频率为1Hz，星敏感器测量噪声标准差为σs＝0.005deg，陀螺采样频率Δt＝1s，陀螺测量噪声标准差为陀螺漂移噪声标准差为 初始条件有以下四种T 2
情形：情形1，初始姿态误差为[1° 1° 1°] ，初始姿态协方差为(0.5°I3×3) ；情形2，初始姿T 2
态误差为[30° 30° 30°]，初始姿态协方差为(30°) I3×3；情形3，初始姿态误差为[‑50° T 2
50° 160°] ，初始姿态协方差为(50°) I3×3，情形1至情形3初始陀螺漂移为[0.1° 0.1° T
0.1°]，初始陀螺漂移协方差为 情形4除了初始姿态误差及其协方差与情形3相T
同外，初始陀螺漂移和初始陀螺漂移协方差分别为[10° 20° 10°]和

[0164] 步骤3中，权值参数为κ＝‑3, 广义罗德里格参数为a＝1,f＝4。

[0165] 步骤4中，滤波时间Ntime＝90min。

[0166] 将本申请所提供的方法(CUSQUE)与现有的非线性滤波算法包括MEKF、USQUE以及容积卡尔曼滤波(Cubature Kalman Filter,CKF)算法进行性能对比。仿真硬件环境为Inter(R)Core(TM)i5‑2400 CPU@3.10GHz,2.00GB RAM,Windows 7操作系统。图2至图5中，点线(:)代表MEKF估计的姿态角均方根误差，虚线(‑‑)代表USQUE估计的姿态角均方根误差，点划线(‑.)代表CKF估计的姿态角均方根误差，实线(‑)代表CUSQUE估计的姿态角均方根误差，图6中分别用点线和实线代表USQUE和CUSQUE估计的三个方向的姿态角误差。从图2至图5中可以很明显发现除了在较小初始条件误差情形下，CUSQUE的估计精度与其它非线性滤波算法相近外，在其它情形下，CUSQUE的估计精度都高于其它算法，且具有更快的收敛速度，通过图6的对比，在最具戏剧性情形下CUSQUE三个方向的姿态角误差均收敛于3倍的均方协方差边界内。这说明本发明所提出的方法的有效性。