CN115655285A - 一种权值及参考四元数修正的无迹四元数姿态估计方法 - Google Patents
一种权值及参考四元数修正的无迹四元数姿态估计方法 Download PDFInfo
- Publication number
- CN115655285A CN115655285A CN202211423293.5A CN202211423293A CN115655285A CN 115655285 A CN115655285 A CN 115655285A CN 202211423293 A CN202211423293 A CN 202211423293A CN 115655285 A CN115655285 A CN 115655285A
- Authority
- CN
- China
- Prior art keywords
- quaternion
- attitude
- time
- unscented
- error
- Prior art date
- Legal status (The legal status 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 status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 30
- 238000001914 filtration Methods 0.000 claims abstract description 28
- 238000005259 measurement Methods 0.000 claims abstract description 23
- 150000001875 compounds Chemical class 0.000 claims description 14
- 239000011159 matrix material Substances 0.000 claims description 14
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000005070 sampling Methods 0.000 claims description 3
- 230000000644 propagated effect Effects 0.000 claims 1
- 238000004422 calculation algorithm Methods 0.000 abstract description 16
- 230000009286 beneficial effect Effects 0.000 abstract description 6
- 238000012986 modification Methods 0.000 description 4
- 230000004048 modification Effects 0.000 description 4
- 238000004088 simulation Methods 0.000 description 4
- 238000012937 correction Methods 0.000 description 3
- 238000010606 normalization Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
Images
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Astronomy & Astrophysics (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
本发明涉及一种权值及参考四元数修正的无迹四元数姿态估计方法,包括:过陀螺和星敏感器获取量测数据作为量测量;建立基于四元数的离散型航天器非线性状态空间模型;在k‑1时刻利用基于参数及参考四元数修正的无迹四元数估计器估计k时刻误差四元数、陀螺漂移和相应的误差协方差;针对上述姿态估计非线性滤波时间为Ntime,若k<Ntime则重复步骤三,若k=Ntime,则滤波结束,输出姿态四元数、陀螺漂移和相应的误差协方差。本发明的有益效果是:本发明有利于提高姿态估计的精度和改善姿态估计算法的稳定性。
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)及粒子滤波(ParticleFilter,PF)算法。而SGHQF与PF算法计算量较大。其中,无迹四元数估计器采用广义罗德里格参数与四元数切换的方法避免四元数归一化约束问题,从而有效地求解预测均值和相应的协方差。然而,此类算法中参考四元数的选择及参数的设定有待进一步修正。针对无迹卡尔曼滤波,其中,计算更新四元数的参考四元数应该是一步预测四元数的均值。而确定均值四元数的方法包括奇异值分解方法和基于特征矢量的方法,在此基础上,拉格朗日函数方法是一种相对简单的求解乘性四元数加权均值的方法。另外,无迹卡尔曼滤波器中存在多个参数需要因实际环境进行设定,而姿态估计问题不同于单变量模型及目标跟踪等大多数类问题,相对更为复杂,参数的设定对算法的效果有着极为明显的影响。因此,基于以上讨论,研究一种权值及参考四元数修正的无迹四元数姿态估计方法对提高姿态估计系统的估计精度和稳定性具有重要的理论及实际意义。
发明内容
本发明的目的是克服现有技术中的不足,为了提高姿态估计系统的估计精度和稳定性,提供了一种权值及参考四元数修正的无迹四元数姿态估计方法(Weight andreference 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
步骤2.3、建立星敏感器观测模型:
式中,ρ=[q1 q2 q3]T表示四元数向量部分,[ρ×]表示ρ的反对称矩阵。
作为优选,步骤3包括:
步骤3.1.1、计算k-1时刻西格玛点及相应权值:
将西格玛点划分为姿态误差部分和陀螺漂移部分:
步骤3.1.2、计算需要传播的四元数:
δρ=f-1[a+δq4]δp
式中,δ(·)表示误差,f-1表示f的逆,a与f为广义罗德里格参数;
步骤3.2、使用离散的四元数运动方程更新四元数:
则,误差四元数通过四元数乘积可求得:
步骤3.3、根据以下方程计算参考四元数:
步骤3.4、传播陀螺漂移:
步骤3.5、一步预测状态及相应的误差协方差估计为:
步骤3.6、量测更新,包括:
步骤3.6.1、重新计算量测更新部分西格玛点对应的权值:
步骤3.6.2、计算西格玛点:
步骤3.7、计算k时刻的滤波增益、状态矢量及相应的误差协方差:
步骤3.8、更新四元数:
作为优选,步骤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
步骤2.3、建立星敏感器观测模型:
式中,ρ=[q1 q2 q3]T表示四元数向量部分,[ρ×]表示ρ的反对称矩阵。
步骤3包括:
步骤3.1.1、计算k-1时刻西格玛点及相应权值:
将西格玛点划分为姿态误差部分和陀螺漂移部分:
步骤3.1.2、计算需要传播的四元数:
δρ=f-1[a+δq4]δp
式中,δ(·)表示误差,f-1表示f的逆;
步骤3.2、使用离散的四元数运动方程更新四元数:
则,误差四元数通过四元数乘积可求得:
步骤3.3、根据以下方程计算参考四元数:
步骤3.4、传播陀螺漂移:
步骤3.5、一步预测状态及相应的误差协方差估计为:
步骤3.6、量测更新,包括:
步骤3.6.1、重新计算量测更新部分西格玛点对应的权值:
步骤3.6.2、计算西格玛点:
步骤3.7、计算k时刻的滤波增益、状态矢量及相应的误差协方差:
步骤3.8、更新四元数:
以下通过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和
步骤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
步骤2.3、建立星敏感器观测模型:
式中,ρ=[q1 q2 q3]T表示四元数向量部分,[ρ×]表示ρ的反对称矩阵。
Priority Applications (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211423293.5A CN115655285B (zh) | 2022-11-15 | 2022-11-15 | 一种权值及参考四元数修正的无迹四元数姿态估计方法 |
PCT/CN2023/114183 WO2024012602A1 (zh) | 2022-11-15 | 2023-08-22 | 一种权值及参考四元数修正的无迹四元数姿态估计方法 |
US18/426,210 US20240175685A1 (en) | 2022-11-15 | 2024-01-29 | Weight and reference quaternoin correction unscented quaternoin estimation method |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211423293.5A CN115655285B (zh) | 2022-11-15 | 2022-11-15 | 一种权值及参考四元数修正的无迹四元数姿态估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115655285A true CN115655285A (zh) | 2023-01-31 |
CN115655285B CN115655285B (zh) | 2023-12-01 |
Family
ID=85022108
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211423293.5A Active CN115655285B (zh) | 2022-11-15 | 2022-11-15 | 一种权值及参考四元数修正的无迹四元数姿态估计方法 |
Country Status (3)
Country | Link |
---|---|
US (1) | US20240175685A1 (zh) |
CN (1) | CN115655285B (zh) |
WO (1) | WO2024012602A1 (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2024012602A1 (zh) * | 2022-11-15 | 2024-01-18 | 浙大城市学院 | 一种权值及参考四元数修正的无迹四元数姿态估计方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004224171A (ja) * | 2003-01-22 | 2004-08-12 | Mitsubishi Electric Corp | 人工衛星の姿勢決定装置 |
CN106767837A (zh) * | 2017-02-23 | 2017-05-31 | 哈尔滨工业大学 | 基于容积四元数估计的航天器姿态估计方法 |
CN108225337A (zh) * | 2017-12-28 | 2018-06-29 | 西安电子科技大学 | 基于sr-ukf滤波的星敏感器和陀螺组合定姿方法 |
CN110109470A (zh) * | 2019-04-09 | 2019-08-09 | 西安电子科技大学 | 基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统 |
CN113074753A (zh) * | 2021-03-19 | 2021-07-06 | 南京天巡遥感技术研究院有限公司 | 一种星敏感器陀螺联合定姿方法、联合定姿系统及应用 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105300384B (zh) * | 2015-04-03 | 2017-12-22 | 东南大学 | 一种用于卫星姿态确定的交互式滤波方法 |
CN115655285B (zh) * | 2022-11-15 | 2023-12-01 | 浙大城市学院 | 一种权值及参考四元数修正的无迹四元数姿态估计方法 |
-
2022
- 2022-11-15 CN CN202211423293.5A patent/CN115655285B/zh active Active
-
2023
- 2023-08-22 WO PCT/CN2023/114183 patent/WO2024012602A1/zh unknown
-
2024
- 2024-01-29 US US18/426,210 patent/US20240175685A1/en active Pending
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004224171A (ja) * | 2003-01-22 | 2004-08-12 | Mitsubishi Electric Corp | 人工衛星の姿勢決定装置 |
CN106767837A (zh) * | 2017-02-23 | 2017-05-31 | 哈尔滨工业大学 | 基于容积四元数估计的航天器姿态估计方法 |
CN108225337A (zh) * | 2017-12-28 | 2018-06-29 | 西安电子科技大学 | 基于sr-ukf滤波的星敏感器和陀螺组合定姿方法 |
CN110109470A (zh) * | 2019-04-09 | 2019-08-09 | 西安电子科技大学 | 基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统 |
CN113074753A (zh) * | 2021-03-19 | 2021-07-06 | 南京天巡遥感技术研究院有限公司 | 一种星敏感器陀螺联合定姿方法、联合定姿系统及应用 |
Non-Patent Citations (1)
Title |
---|
邱真兵: "基于非线性滤波算法的航天器姿态估计方法研究", 工程科技Ⅱ辑, no. 03, pages 031 - 24 * |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2024012602A1 (zh) * | 2022-11-15 | 2024-01-18 | 浙大城市学院 | 一种权值及参考四元数修正的无迹四元数姿态估计方法 |
Also Published As
Publication number | Publication date |
---|---|
WO2024012602A1 (zh) | 2024-01-18 |
US20240175685A1 (en) | 2024-05-30 |
CN115655285B (zh) | 2023-12-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110109470B (zh) | 基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统 | |
CN112013836B (zh) | 一种基于改进自适应卡尔曼滤波的航姿参考系统算法 | |
CN108827299B (zh) | 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法 | |
CN105300384B (zh) | 一种用于卫星姿态确定的交互式滤波方法 | |
CN108225337A (zh) | 基于sr-ukf滤波的星敏感器和陀螺组合定姿方法 | |
CN103940433B (zh) | 一种基于改进的自适应平方根ukf算法的卫星姿态确定方法 | |
CN112945225A (zh) | 基于扩展卡尔曼滤波的姿态解算系统及解算方法 | |
CN105043348A (zh) | 基于卡尔曼滤波的加速度计陀螺仪水平角度测量方法 | |
CN108344409B (zh) | 提高卫星姿态确定精度的方法 | |
CN115655285A (zh) | 一种权值及参考四元数修正的无迹四元数姿态估计方法 | |
CN111044082B (zh) | 一种基于星敏感器辅助的陀螺误差参数在轨快速标定方法 | |
CN113008272B (zh) | 一种用于微小卫星的mems陀螺在轨常值漂移标定方法和系统 | |
CN114139109A (zh) | 一种目标跟踪方法、系统、设备、介质及数据处理终端 | |
CN110132287A (zh) | 一种基于极限学习机网络补偿的卫星高精度联合定姿方法 | |
CN113432612A (zh) | 飞行体的导航方法、装置及系统 | |
CN110912535B (zh) | 一种新型无先导卡尔曼滤波方法 | |
CN113074753A (zh) | 一种星敏感器陀螺联合定姿方法、联合定姿系统及应用 | |
CN112632454A (zh) | 一种基于自适应卡尔曼滤波算法的mems陀螺滤波方法 | |
CN110186483B (zh) | 提高惯性制导航天器落点精度的方法 | |
CN111578931A (zh) | 基于在线滚动时域估计的高动态飞行器自主姿态估计方法 | |
CN109188422B (zh) | 一种基于lu分解的卡尔曼滤波目标跟踪方法 | |
CN111310110A (zh) | 一种高维耦合不确定系统混合状态估计方法 | |
CN114323007A (zh) | 一种载体运动状态估计方法及装置 | |
CN110260869B (zh) | 一种降低星敏感器和陀螺联合滤波计算量的改进方法 | |
CN112595319B (zh) | 一种模型自适应补偿的返回弹道估计方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |