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

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

Info

Publication number
WO2024012602A1
WO2024012602A1 PCT/CN2023/114183 CN2023114183W WO2024012602A1 WO 2024012602 A1 WO2024012602 A1 WO 2024012602A1 CN 2023114183 W CN2023114183 W CN 2023114183W WO 2024012602 A1 WO2024012602 A1 WO 2024012602A1
Authority
WO
WIPO (PCT)
Prior art keywords
quaternion
time
unscented
formula
attitude
Prior art date
Application number
PCT/CN2023/114183
Other languages
English (en)
French (fr)
Inventor
邱真兵
李艳君
Original Assignee
浙大城市学院
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 浙大城市学院 filed Critical 浙大城市学院
Publication of WO2024012602A1 publication Critical patent/WO2024012602A1/zh

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/24—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for cosmonautical navigation

Definitions

  • the present invention relates to the technical field of attitude estimation. More specifically, it relates to an unscented quaternion attitude estimation method with weight and reference quaternion correction.
  • the most important representation parameters describing the attitude of the spacecraft include Euler angles, direction cosines, generalized Rodrigue parameters and quaternions.
  • quaternion has received the most attention due to its lack of singularity and simple calculation, and is widely used in attitude estimation systems.
  • the spacecraft attitude estimation model based on quaternions has high nonlinear characteristics, especially the measurement model is established by nonlinear functions, so attitude estimation is essentially a nonlinear filtering problem.
  • quaternions have a normalization constraint problem.
  • EKF Extended Kalman Filter
  • multiplicative extended Kalman filter Multiplicative EKF, MEKF
  • backwards-smoothing extended Kalman filter Backwards-Smoothing EKF, BSEKF
  • norm-constrained extended Kalman filter Normal EKF, NEKF
  • the unscented quaternion estimator uses the method of switching between generalized Rodrigue parameters and quaternions to avoid the quaternion normalization constraint problem, thereby effectively solving the predicted mean and corresponding covariance.
  • the selection of reference quaternions and the setting of parameters in this type of algorithm need further modification.
  • the methods for determining the mean quaternion include the singular value decomposition method and the method based on eigenvectors.
  • the Lagrangian function method is a relatively simple method for solving the weighted mean of multiplicative quaternions.
  • attitude estimation problem is different from most types of problems such as single-variable models and target tracking, and is relatively more complex.
  • the setting of parameters has a negative impact on the algorithm.
  • the effect has a very obvious impact. Therefore, based on the above discussion, studying an unscented quaternion attitude estimation method with weight and reference quaternion correction has important theoretical and practical significance for improving the estimation accuracy and stability of the attitude estimation system.
  • an unscented quaternion attitude estimation method (Weight and reference) modified by weights and reference quaternions is provided.
  • quaternion correction unscented quaternion estimator,CUSQUE including:
  • Step 1 Obtain measurement data through gyroscope and star sensor as quantity measurement
  • Step 2 Establish a nonlinear state space model of the discrete spacecraft based on quaternions
  • Step 3 use the unscented quaternion estimator based on parameter and reference quaternion correction to estimate the error quaternion, gyro drift and corresponding error covariance at time k;
  • step 2 includes:
  • Step 2.1 Establish the discrete quaternion state motion equation of the spacecraft:
  • ⁇ [ ⁇ ] and ⁇ k-1 represent the function sign at k-1 time
  • I 3 ⁇ 3 is a 3 ⁇ 3 unit matrix
  • ⁇ t represents the gyro sampling interval
  • represents the norm of the vector
  • [ ⁇ k-1 ⁇ ] represents the antisymmetric matrix of ⁇ k-1 ;
  • ⁇ k represents the true value of the gyro at time k
  • ⁇ k represents the gyro drift at time k
  • N v and N u are Gaussian white noise with zero mean
  • the variance is a three-dimensional unit matrix
  • Step 2.3 Establish the star sensor observation model:
  • z k represents the quantity measurement
  • r i represents the i-th reference vector
  • i 1,...,L
  • L is the number of stars observed by the star sensor
  • v i zero-mean Gaussian white noise, and its covariance is The covariance of all v i constitutes the measurement variance R k
  • A(q) represents the attitude matrix, which is defined as follows
  • [ ⁇ ⁇ ] represents the antisymmetric matrix of ⁇ .
  • step 3 includes:
  • Step 3.1.1 Calculate the sigma point and corresponding weight at time k-1:
  • Step 3.1.2 Calculate the quaternions that need to be propagated:
  • ⁇ ( ⁇ ) represents the error
  • f -1 represents the inverse of f
  • a and f are generalized Rodrigue parameters
  • Step 3.2 Use the discrete quaternion equation of motion to update the quaternion:
  • ⁇ p represents the generalized Rodrigue parameter:
  • Step 3.3 Calculate the reference quaternion according to the following equation:
  • ⁇ is the Lagrangian multiplicative factor
  • the eigenvector corresponding to the smallest eigenvalue in the solution of the equation is set to the reference quaternion.
  • Step 3.5 one-step prediction state and corresponding error covariance estimation are:
  • Step 3.6 measurement update, including:
  • Step 3.6.1 Recalculate the weights corresponding to the sigma points in the measurement update part:
  • Step 3.6.3 one-step measurement prediction
  • k-1 Calculation of prediction variance P zz,k
  • Step 3.7 Calculate the filter gain, state vector and corresponding error covariance at time k:
  • step 3.1.2 According to step 3.1.2 and ⁇ i,k-1 calculation formula to solve the error quaternion According to the quaternion product, and using the eigenvector corresponding to the minimum eigenvalue calculated in the calculation formula of step 3.3 as the reference quaternion, the quaternion solution at time k is as follows:
  • Step 3.9 reset is a zero vector, preparing for the next filtering cycle.
  • the present invention uses the unscented transformation theory to approximate the mean and variance of the nonlinear function, which can avoid calculating the Jacobian matrix, which is beneficial to improving the accuracy of attitude estimation and improving the stability of the algorithm.
  • the present invention uses the Lagrangian function method to solve the weighted mean of quaternions and uses it as a reference quaternion to solve the updated quaternions, which is beneficial to improving the accuracy of attitude estimation and improving the stability of the algorithm.
  • Figure 1 is the result of the number of available stars observed by the star sensor in the simulation
  • Figure 2 is a comparison chart of the attitude angle root mean square error results of all filter estimates under the initial condition error in case 1;
  • Figure 3 is a comparison chart of the attitude angle root mean square error results of all filter estimates under the initial condition error in case 2;
  • Figure 4 is a comparison chart of the attitude angle root mean square error results of all filter estimates under the initial condition error in case 3;
  • Figure 5 is a comparison chart of the attitude angle root mean square error results of all filter estimates under the initial condition error in case 4;
  • Figure 6 is a comparison chart of the attitude angle error results of USQUE and CUSQUE in three directions under the initial condition error in scenario 4;
  • Figure 7 is a flow chart of an unscented quaternion attitude estimation method modified by weights and reference quaternions.
  • this application provides an unscented quaternion attitude estimation method with weight and reference quaternion correction, including:
  • Step 1 Obtain measurement data through gyroscope and star sensor as quantity measurement
  • Step 2 Establish a nonlinear state space model of the discrete spacecraft based on quaternions
  • Step 3 use the unscented quaternion estimator based on parameter and reference quaternion correction to estimate the error quaternion, gyro drift and corresponding error covariance at time k;
  • Step 2 includes:
  • Step 2.1 Establish the discrete quaternion state motion equation of the spacecraft:
  • I 3 ⁇ 3 is a 3 ⁇ 3 unit matrix
  • ⁇ t represents the gyro sampling interval
  • represents the norm of the vector
  • [ ⁇ k-1 ⁇ ] represents the antisymmetric matrix of ⁇ k-1 ; for example, the mathematical symbol [ ⁇ ] represents:
  • ⁇ k represents the true value of the gyro at time k
  • ⁇ k represents the gyro drift at time k
  • N v and N u are Gaussian white noise with zero mean
  • the variance is a three-dimensional unit matrix
  • Step 2.3 Establish the star sensor observation model:
  • z k represents the quantity measurement
  • r i represents the i-th reference vector
  • i 1,...,L
  • L is the number of stars observed by the star sensor
  • v i zero-mean Gaussian white noise, and its covariance is The covariance of all v i constitutes the measurement variance R k
  • A(q) represents the attitude matrix, which is defined as follows
  • [ ⁇ ⁇ ] represents the antisymmetric matrix of ⁇ .
  • Step 3 includes:
  • Step 3.1.1 Calculate the sigma point and corresponding weight at time k-1:
  • Step 3.1.2 Calculate the quaternions that need to be propagated:
  • ⁇ ( ⁇ ) represents the error
  • f -1 represents the inverse of f
  • Step 3.2 Use the discrete quaternion equation of motion to update the quaternion:
  • ⁇ p represents the generalized Rodrigue parameter:
  • Step 3.3 Calculate the reference quaternion according to the following equation:
  • ⁇ is the Lagrangian multiplicative factor
  • the eigenvector corresponding to the smallest eigenvalue in the solution of the equation is set as the reference quaternion.
  • Step 3.5 one-step prediction state and corresponding error covariance estimation are:
  • Step 3.6 measurement update, including:
  • Step 3.6.1 Recalculate the weights corresponding to the sigma points in the measurement update part:
  • Step 3.6.3 one-step measurement prediction
  • k-1 are calculated as follows:
  • Step 3.7 Calculate the filter gain, state vector and corresponding error covariance at time k:
  • step 3.1.2 According to step 3.1.2 and ⁇ i,k-1 calculation formula to solve the error quaternion According to the quaternion product, and using the eigenvector corresponding to the minimum eigenvalue calculated in the calculation formula of step 3.3 as the reference quaternion, the quaternion solution at time k is as follows:
  • Step 3.9 reset is a zero vector, preparing for the next filtering cycle.
  • the initial conditions have the following four situations: Situation 1, The initial attitude error is [1° 1° 1°] T , and the initial attitude covariance is (0.5°I 3 ⁇ 3 ) 2 ; in case 2, the initial attitude error is [30° 30° 30°] T , and the initial attitude covariance is is (30°) 2 I 3 ⁇ 3 ; in case 3, the initial attitude error is [-50° 50° 160°] T , and the initial attitude covariance is (50°) 2 I 3 ⁇ 3 .
  • Case 1 to case 3 the initial The gyro drift is [0.1° 0.1° 0.1°] T , and the initial gyro drift covariance is Except for the initial attitude error and its covariance, Case 4 is the same as Case 3.
  • the initial gyro drift and initial gyro drift covariance are [10° 20° 10°] T and
  • the performance of the method (CUSQUE) provided in this application is compared with existing nonlinear filtering algorithms including MEKF, USQUE and Cubature Kalman Filter (CKF) algorithms.
  • the simulation hardware environment is Inter(R)Core(TM) i5-2400 CPU @3.10GHz, 2.00GB RAM, Windows 7 operating system.
  • the dotted line (:) represents the root mean square error of the attitude angle estimated by MEKF
  • the dotted line (--) represents the root mean square error of the attitude angle estimated by USQUE
  • the dotted line (-.) represents the root mean square error of the attitude angle estimated by CKF.
  • the solid line (-) represents the attitude angle root mean square error estimated by CUSQUE.

Abstract

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

Description

一种权值及参考四元数修正的无迹四元数姿态估计方法 技术领域
本发明涉及姿态估计技术领域，更确切地说，它涉及一种权值及参考四元数修正的无迹四元数姿态估计方法。
背景技术
描述航天器姿态的最为重要的表示参数包括欧拉角、方向余弦、广义罗德里格参数和四元数。其中四元数由于没有奇异性、计算简单而最受关注，且广泛地应用于姿态估计系统中。基于四元数的航天器姿态估计模型具有较高的非线性特性，尤其是量测模型由非线性函数建立，因此姿态估计本质上是一个非线性滤波问题。另外，四元数存在归一化约束问题。对于解决以上问题，扩展卡尔曼滤波(Extended Kalman Filter,EKF)已成为针对姿态估计系统的最为广泛使用的非线性滤波算法。例如，先后提出的乘性扩展卡尔曼滤波(Multiplicative EKF,MEKF)、向后平滑扩展卡尔曼滤波(Backwards-Smoothing EKF,BSEKF)以及范数约束扩展卡尔曼滤波(Norm-constrained EKF,NEKF)等等。
然而，扩展卡尔曼滤波截断了泰勒展开的高阶项，估计精度有限，这类算法一般只在较小的初始误差条件下具有满意的滤波结果。因此有必要去研究新的非线性滤波算法，如之后提出的无迹四元数估计器(Unscented Quaternion Estimator,USQUE)、稀疏高斯厄密特正交滤波(Sparse Gauss-Hermite Quadrature Filter,SGHQF)及粒子滤波(Particle Filter,PF)算法。而SGHQF与PF算法计算量较大。其中，无迹四元数估计器采用广义罗德里格参数与四元数切换的方法避免四元数归一化约束问题，从而有效地求解预测均值和相应的协方差。然而，此类算法中参考四元数的选择及参数的设定有待进一步修正。针对无迹卡尔曼滤波，其中，计算更新四元数的参考四元数应该是一步预测四元数的均值。而确定均值四元数的方法包括奇异值分解方法和基于特征矢量的方法，在此基础上，拉格朗日函数方法是一种相对简单的求解乘性四元数加权均值的方法。另外，无迹卡尔曼滤波器中存在多个参数需要因实际环境进行设定，而姿态估计问题不同于单变量模型及目标跟踪等大多数类问题，相对更为复杂，参数的设定对算法的效果有着极为明显的影响。因此，基于以上讨论，研究一种权值及参考四元数修正的无迹四元数姿态估计方法对提高姿态估计系统的估计精度和稳定性具有重要的理论及实际意义。
发明内容
本发明的目的是克服现有技术中的不足，为了提高姿态估计系统的估计精度和稳定性，提供了一种权值及参考四元数修正的无迹四元数姿态估计方法(Weight and reference quaternion correction unscented quaternion estimator,CUSQUE)，包括：
步骤1、过陀螺和星敏感器获取量测数据作为量测量；
步骤2、建立基于四元数的离散型航天器非线性状态空间模型；
步骤3、在k-1时刻利用基于参数及参考四元数修正的无迹四元数估计器估计k时刻误差四元数、陀螺漂移和相应的误差协方差；
步骤4、设置滤波时间为Ntime，若k<Ntime则重复步骤三，若k＝Ntime，则滤波结束，输出姿态四元数、陀螺漂移和相应的误差协方差。
作为优选，步骤2包括：
步骤2.1、建立航天器的离散型四元数状态运动方程：


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

βk＝βk-1+σuΔt1/2Nu
式中，表示k时刻陀螺的输出值，ωk表示k时刻陀螺的真实值，βk表示k时刻陀螺漂移，和为陀螺测量噪声和漂移噪声的均方差，Nv和Nu是均值为零的高斯白噪声，方差为3维的单位阵；
步骤2.3、建立星敏感器观测模型：
式中，zk表示量测量，ri表示第i个参考矢量，i＝1,…,L，L为星敏感器所观测到恒星数目；vi为零均值高斯白噪声，其协方差为而所有vi的协方差构成量测方差Rk；A(q)表示姿态矩阵，其定义如下
式中，ρ＝[q1 q2 q3]T表示四元数向量部分，[ρ×]表示ρ的反对称矩阵。
作为优选，步骤3包括：
步骤3.1.1、计算k-1时刻西格玛点及相应权值：





式中，表示的第i列，及均表示权值，n表示状态的维数，κ为无迹卡尔曼滤波调节参数，Qk为过程噪声，定义如下：
将西格玛点划分为姿态误差部分和陀螺漂移部分：
步骤3.1.2、计算需要传播的四元数：
式中，表示四元数乘积，误差四元数和通过下式计算：

δρ＝f-1[a+δq4]δp
式中，δ(·)表示误差，f-1表示f的逆，a与f为广义罗德里格参数；
步骤3.2、使用离散的四元数运动方程更新四元数：
则，误差四元数通过四元数乘积可求得：
姿态误差部分西格玛点通过下式求解，δp表示广义罗德里格参数：
步骤3.3、根据以下方程计算参考四元数：
式中， λ为拉格朗日乘性因子，方程的解中最小特征值对应的特征向量设为参考四元数
步骤3.4、传播陀螺漂移：
步骤3.5、一步预测状态及相应的误差协方差估计为：

步骤3.6、量测更新，包括：
步骤3.6.1、重新计算量测更新部分西格玛点对应的权值：

式中，为无迹卡尔曼滤波调节参数；
步骤3.6.2、计算西格玛点：
步骤3.6.3、一步量测预测一步量测预测方差Pzz,k|k-1以及互协方差Pxz,k|k-1计算 如下：


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


步骤3.8、更新四元数：
根据步骤3.1.2中和δρi,k-1的计算式求解出误差四元数根据四元数乘积，并以步骤3.3的计算式中求出的最小特征值对应的特征矢量为参考四元数，则k时刻的四元数求解如下：
步骤3.9、重置为零矢量，为下一次滤波循环做准备。
作为优选，步骤3中，权值参数为κ＝-3,广义罗德里格参数为a＝1,f＝4。
作为优选，步骤4中，滤波时间Ntime＝90min。
本发明的有益效果是：
(1)本发明采用无迹变换理论近似非线性函数的均值和方差，可避免计算雅克比矩阵，有利于提高姿态估计的精度和改进算法的稳定性。
(2)本发明在时间更新和量测更新中，对权值的不同设定，不仅提高了姿态估计的精度而且有利于降低算法的计算量。
(3)本发明采用拉格朗日函数方法求解四元数加权均值，并作为求解更新四元数的参考四元数，有利于提高姿态估计的精度和改善算法的稳定性。
附图说明
图1是仿真中星敏感器观测到的可用恒星的数目结果图；
图2是情形1初始条件误差情况下所有滤波估计的姿态角均方根误差结果对比图；
图3是情形2初始条件误差情况下所有滤波估计的姿态角均方根误差结果对比图；
图4是情形3初始条件误差情况下所有滤波估计的姿态角均方根误差结果对比图；
图5是情形4初始条件误差情况下所有滤波估计的姿态角均方根误差结果对比图；
图6是情形4初始条件误差情况下USQUE和CUSQUE三个方向的姿态角误差结果对比图；
图7是一种权值及参考四元数修正的无迹四元数姿态估计方法的流程图。
具体实施方式
下面结合实施例对本发明做进一步描述。下述实施例的说明只是用于帮助理解本发明。应当指出，对于本技术领域的普通人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干修饰，这些改进和修饰也落入本发明权利要求的保护范围内。
作为一种实施例，本申请提供了一种权值及参考四元数修正的无迹四元数姿态估计方法，包括：
步骤1、过陀螺和星敏感器获取量测数据作为量测量；
步骤2、建立基于四元数的离散型航天器非线性状态空间模型；
步骤3、在k-1时刻利用基于参数及参考四元数修正的无迹四元数估计器估计k时刻误差四元数、陀螺漂移和相应的误差协方差；
步骤4、设置滤波时间为Ntime，若k<Ntime则重复步骤三，若k＝Ntime，则滤波结束，输出姿态四元数、陀螺漂移和相应的误差协方差。
步骤2包括：
步骤2.1、建立航天器的离散型四元数状态运动方程：


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

βk＝βk-1+σuΔt1/2Nu
式中，表示k时刻陀螺的输出值，ωk表示k时刻陀螺的真实值，βk表示k时刻陀螺漂移，和为陀螺测量噪声和漂移噪声的均方差，Nv和Nu是均值为零的高斯白噪声，方差为3维的单位阵；
步骤2.3、建立星敏感器观测模型：
式中，zk表示量测量，ri表示第i个参考矢量，i＝1,…,L，L为星敏感器所观测到恒星数目；vi为零均值高斯白噪声，其协方差为而所有vi的协方差构成量测方差Rk；A(q)表示姿态矩阵，其定义如下
式中，ρ＝[q1 q2 q3]T表示四元数向量部分，[ρ×]表示ρ的反对称矩阵。
步骤3包括：
步骤3.1.1、计算k-1时刻西格玛点及相应权值：





式中，表示的第i列，及均表示权值，n表示状态的维数，Qk为过程噪声，定义如下：
将西格玛点划分为姿态误差部分和陀螺漂移部分：
步骤3.1.2、计算需要传播的四元数：
式中，表示四元数乘积，误差四元数和δρi,k-1通过下式计算：

δρ＝f-1[a+δq4]δp
式中，δ(·)表示误差，f-1表示f的逆；
步骤3.2、使用离散的四元数运动方程更新四元数：
则，误差四元数通过四元数乘积可求得：
姿态误差部分西格玛点通过下式求解，δp表示广义罗德里格参数：
步骤3.3、根据以下方程计算参考四元数：
式中， λ为拉格朗日乘性因子，方程的解中最小特征值对应的特征向量设为参考四元数
步骤3.4、传播陀螺漂移：
步骤3.5、一步预测状态及相应的误差协方差估计为：

步骤3.6、量测更新，包括：
步骤3.6.1、重新计算量测更新部分西格玛点对应的权值：

步骤3.6.2、计算西格玛点：
步骤3.6.3、一步量测预测一步量测预测方差Pzz,k|k-1以及互协方差Pxz,k|k-1计算如下：


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


步骤3.8、更新四元数：
根据步骤3.1.2中和δρi,k-1的计算式求解出误差四元数根据四元数乘积，并以步骤3.3的计算式中求出的最小特征值对应的特征矢量为参考四元数，则k时刻的四元数求解如下：
步骤3.9、重置为零矢量，为下一次滤波循环做准备。
以下通过Matlab仿真软件，在以下仿真条件对本申请所提供的方法进行仿真实验：
步骤2中角速度ω＝[-1/(90/(2π)×60) 0 0]T，星敏感器采样频率为1Hz，星敏感器测量噪声标准差为σs＝0.005deg，陀螺采样频率Δt＝1s，陀螺测量噪声标准差为 陀螺漂移噪声标准差为初始条件有以下四种情形：情形1， 初始姿态误差为[1° 1° 1°]T，初始姿态协方差为(0.5°I3×3)2；情形2，初始姿态误差为[30° 30° 30°]T，初始姿态协方差为(30°)2I3×3；情形3，初始姿态误差为[-50° 50° 160°]T，初始姿态协方差为(50°)2I3×3，情形1至情形3初始陀螺漂移为[0.1° 0.1° 0.1°]T，初始陀螺漂移协方差为情形4除了初始姿态误差及其协方差与情形3相同外，初始陀螺漂移和初始陀螺漂移协方差分别为[10° 20° 10°]T和
步骤3中，权值参数为κ＝-3,广义罗德里格参数为a＝1,f＝4。
步骤4中，滤波时间Ntime＝90min。
将本申请所提供的方法(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倍的均方协方差边界内。这说明本发明所提出的方法的有效性。

Claims (7)

  1. 一种权值及参考四元数修正的无迹四元数姿态估计方法，其特征在于，包括：
    步骤1、过陀螺和星敏感器获取量测数据作为量测量；
    步骤2、建立基于四元数的离散型航天器非线性状态空间模型；
    步骤3、在k-1时刻利用基于参数及参考四元数修正的无迹四元数估计器估计k时刻误差四元数、陀螺漂移和相应的误差协方差；
    步骤4、设置滤波时间为Ntime，若k<Ntime则重复步骤三，若k＝Ntime，则滤波结束，输出姿态四元数、陀螺漂移和相应的误差协方差。
  2. 根据权利要求1所述的权值及参考四元数修正的无迹四元数姿态估计方法，其特征在于，步骤2包括：
    步骤2.1、建立航天器的离散型四元数状态运动方程：


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

    βk＝βk-1+σuΔt1/2Nu
    式中，表示k时刻陀螺的输出值，ωk表示k时刻陀螺的真实值，βk表示k时刻陀螺漂移，和为陀螺测量噪声和漂移噪声的均方差，Nv和Nu是均值为零的高斯白噪声，方差为3维的单位阵；
    步骤2.3、建立星敏感器观测模型：
    式中，zk表示量测量，ri表示第i个参考矢量，i＝1,…,L，L为星敏感器所观测到恒星数 目；vi为零均值高斯白噪声，其协方差为而所有vi的协方差构成量测方差Rk；A(q)表示姿态矩阵，其定义如下
    式中，ρ＝[q1 q2 q3]T表示四元数向量部分，[ρ×]表示ρ的反对称矩阵。
  3. 根据权利要求2所述的权值及参考四元数修正的无迹四元数姿态估计方法，其特征在于，步骤3包括：
    步骤3.1、计算k-1时刻西格玛点及相应权值，并计算需要传播的四元数：
    步骤3.2、使用离散的四元数运动方程更新四元数；
    步骤3.3、根据以下方程计算参考四元数：
    式中， λ为拉格朗日乘性因子，方程的解中最小特征值对应的特征向量设为参考四元数
    步骤3.4、传播陀螺漂移：
    步骤3.5、一步预测状态及相应的误差协方差估计为：

    步骤3.6、量测更新；
    步骤3.7、计算k时刻的滤波增益、状态矢量及相应的误差协方差：


    步骤3.8、更新四元数：
    步骤3.9、重置为零矢量，为下一次滤波循环做准备。
  4. 根据权利要求3所述的权值及参考四元数修正的无迹四元数姿态估计方法，其特征在 于，步骤3.1包括：
    步骤3.1.1、计算k-1时刻西格玛点及相应权值：





    式中，表示的第i列，及均表示权值，n表示状态的维数，κ为无迹卡尔曼滤波调节参数，Qk为过程噪声，定义如下：
    将西格玛点划分为姿态误差部分和陀螺漂移部分：
    步骤3.1.2、计算需要传播的四元数：
    式中，表示四元数乘积，误差四元数和δρi,k-1通过下式计算：

    δρ＝f-1[a+δq4]δp
    式中，δ(·)表示误差，f-1表示f的逆，a与f为广义罗德里格参数；
  5. 根据权利要求4所述的权值及参考四元数修正的无迹四元数姿态估计方法，其特征在于，步骤3.2中，所述离散的四元数运动方程表示为：
    则，误差四元数通过四元数乘积可求得：
    姿态误差部分西格玛点通过下式求解，δp表示广义罗德里格参数：
  6. 根据权利要求4所述的权值及参考四元数修正的无迹四元数姿态估计方法，其特征在于，步骤3.6包括：
    步骤3.6.1、重新计算量测更新部分西格玛点对应的权值：

    式中，为无迹卡尔曼滤波调节参数；
    步骤3.6.2、计算西格玛点：
    步骤3.6.3、一步量测预测一步量测预测方差Pzz,k|k-1以及互协方差Pxz,k|k-1计算如下：


  7. 根据权利要求5所述的权值及参考四元数修正的无迹四元数姿态估计方法，其特征在于，步骤3.8中，根据步骤3.1.2中和δρi,k-1的计算式求解出误差四元数根据四元数乘积，并以步骤3.3的计算式中求出的最小特征值对应的特征矢量为参考四元数，则k时刻的四元数求解如下：
PCT/CN2023/114183 2022-11-15 2023-08-22 一种权值及参考四元数修正的无迹四元数姿态估计方法 WO2024012602A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202211423293.5 2022-11-15
CN202211423293.5A CN115655285B (zh) 2022-11-15 2022-11-15 一种权值及参考四元数修正的无迹四元数姿态估计方法

Publications (1)

Publication Number Publication Date
WO2024012602A1 true WO2024012602A1 (zh) 2024-01-18

Family

ID=85022108

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2023/114183 WO2024012602A1 (zh) 2022-11-15 2023-08-22 一种权值及参考四元数修正的无迹四元数姿态估计方法

Country Status (2)

Country Link
CN (1) CN115655285B (zh)
WO (1) WO2024012602A1 (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115655285B (zh) * 2022-11-15 2023-12-01 浙大城市学院 一种权值及参考四元数修正的无迹四元数姿态估计方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105300384A (zh) * 2015-04-03 2016-02-03 东南大学 一种用于卫星姿态确定的交互式滤波方法
CN106767837A (zh) * 2017-02-23 2017-05-31 哈尔滨工业大学 基于容积四元数估计的航天器姿态估计方法
CN110109470A (zh) * 2019-04-09 2019-08-09 西安电子科技大学 基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统
CN113074753A (zh) * 2021-03-19 2021-07-06 南京天巡遥感技术研究院有限公司 一种星敏感器陀螺联合定姿方法、联合定姿系统及应用
CN115655285A (zh) * 2022-11-15 2023-01-31 浙大城市学院 一种权值及参考四元数修正的无迹四元数姿态估计方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3795865B2 (ja) * 2003-01-22 2006-07-12 三菱電機株式会社 人工衛星の姿勢決定装置
CN108225337B (zh) * 2017-12-28 2020-12-15 西安空间无线电技术研究所 基于sr-ukf滤波的星敏感器和陀螺组合定姿方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105300384A (zh) * 2015-04-03 2016-02-03 东南大学 一种用于卫星姿态确定的交互式滤波方法
CN106767837A (zh) * 2017-02-23 2017-05-31 哈尔滨工业大学 基于容积四元数估计的航天器姿态估计方法
CN110109470A (zh) * 2019-04-09 2019-08-09 西安电子科技大学 基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统
CN113074753A (zh) * 2021-03-19 2021-07-06 南京天巡遥感技术研究院有限公司 一种星敏感器陀螺联合定姿方法、联合定姿系统及应用
CN115655285A (zh) * 2022-11-15 2023-01-31 浙大城市学院 一种权值及参考四元数修正的无迹四元数姿态估计方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
QIAN HUAMING; QIU ZHENBING: "Iterated Unscented Kalman Filter for Spacecraft Attitude Estimation", 2018 37TH CHINESE CONTROL CONFERENCE (CCC), TECHNICAL COMMITTEE ON CONTROL THEORY, CHINESE ASSOCIATION OF AUTOMATION, 25 July 2018 (2018-07-25), pages 4557 - 4562, XP033414965, DOI: 10.23919/ChiCC.2018.8483808 *

Also Published As

Publication number Publication date
CN115655285B (zh) 2023-12-01
CN115655285A (zh) 2023-01-31

Similar Documents

Publication Publication Date Title
CN104567871B (zh) 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法
WO2018014602A1 (zh) 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
CN110109470B (zh) 基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统
CN112013836B (zh) 一种基于改进自适应卡尔曼滤波的航姿参考系统算法
CN105300384B (zh) 一种用于卫星姿态确定的交互式滤波方法
WO2024012602A1 (zh) 一种权值及参考四元数修正的无迹四元数姿态估计方法
Rhudy et al. Evaluation of matrix square root operations for UKF within a UAV GPS/INS sensor fusion application
CN104121907B (zh) 一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法
CN106767837B (zh) 基于容积四元数估计的航天器姿态估计方法
CN109343550B (zh) 一种基于滚动时域估计的航天器角速度的估计方法
CN105136145A (zh) 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法
WO2018214227A1 (zh) 一种无人车实时姿态测量方法
Stovner et al. Attitude estimation by multiplicative exogenous Kalman filter
CN110567455B (zh) 一种求积更新容积卡尔曼滤波的紧组合导航方法
CN103900574A (zh) 一种基于迭代容积卡尔曼滤波姿态估计方法
CN112066993B (zh) 一种基于误差四元数三维矢量分布的高斯粒子滤波数据处理方法
Gao et al. Adaptively random weighted cubature Kalman filter for nonlinear systems
CN111578931B (zh) 基于在线滚动时域估计的高动态飞行器自主姿态估计方法
CN113091754B (zh) 一种非合作航天器位姿一体化估计和惯性参数确定方法
CN114139109A (zh) 一种目标跟踪方法、系统、设备、介质及数据处理终端
Zhe et al. Adaptive complementary filtering algorithm for imu based on mems
CN110610513A (zh) 自主移动机器人视觉slam的不变性中心差分滤波器方法
CN112649022B (zh) 一种考虑挠曲变形和杆臂效应的大失准角传递对准方法
CN108225373B (zh) 一种基于改进的5阶容积卡尔曼的大失准角对准方法
CN113587926A (zh) 一种航天器空间自主交会对接相对导航方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 23839087

Country of ref document: EP

Kind code of ref document: A1