CN107228674B - 一种针对星敏感器和陀螺联合滤波的改进方法 - Google Patents
一种针对星敏感器和陀螺联合滤波的改进方法 Download PDFInfo
- Publication number
- CN107228674B CN107228674B CN201710419373.6A CN201710419373A CN107228674B CN 107228674 B CN107228674 B CN 107228674B CN 201710419373 A CN201710419373 A CN 201710419373A CN 107228674 B CN107228674 B CN 107228674B
- Authority
- CN
- China
- Prior art keywords
- quaternion
- value
- extended kalman
- star sensor
- reference value
- 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.)
- Active
Links
- 238000001914 filtration Methods 0.000 title claims abstract description 50
- 238000000034 method Methods 0.000 title claims abstract description 32
- 238000005259 measurement Methods 0.000 claims abstract description 22
- 230000009191 jumping Effects 0.000 claims abstract description 9
- 230000003044 adaptive effect Effects 0.000 claims abstract 2
- 238000004364 calculation method Methods 0.000 claims description 10
- 239000011159 matrix material Substances 0.000 claims description 8
- 239000004576 sand Substances 0.000 claims description 5
- 125000004432 carbon atom Chemical group C* 0.000 description 3
- 230000008859 change Effects 0.000 description 3
- 230000007704 transition Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
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/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)
- Astronomy & Astrophysics (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Gyroscopes (AREA)
Abstract
Description
技术领域
本发明涉及一种针对星敏感器与陀螺的扩展卡尔曼滤波的改进方法,具体地说是在滤波算法的四元数测量值发生跳变时重置四元数参考值的方法,属于卫星姿态确定技术领域。
背景技术
星敏感器和陀螺均为卫星姿控***最为主要的敏感器单机。星敏感器的测量值为星敏感器测量坐标系相对于惯性坐标系的四元数,由星敏感器安装矩阵可得到卫星本体坐标系相对于惯性坐标系的四元数;陀螺的测量值为陀螺测量坐标系相对于惯性坐标系的角速度在陀螺测量坐标系内的投影,由陀螺安装矩阵可得到卫星本体坐标系相对于惯性坐标系的角速度在卫星本体坐标系内的投影。由于星敏感器存在高频的测量白噪声,陀螺存在常值漂移等误差,因此卫星姿控***中通常会将星敏感器与陀螺进行联合滤波,算法为扩展卡尔曼滤波。滤波得到的四元数估计值将显著的抑制星敏感器的高频噪声。目前大多数星敏感器在单机内部将测量四元数的标部处理为始终为正,即当星敏感器测量四元数标部过零时,四元数的四个数全部取反。此时若扩展卡尔曼滤波算法的测量值参考坐标系为惯性坐标系,则该滤波算法的四元数测量值也会发生跳变,因此滤波算法给出的四元数估计值逐渐发散,较长时间后再重新收敛。卫星在轨时,星敏感器测量四元数以轨道周期为变化周期,即每轨会经过一次零位,因此滤波得到四元数估计值也会每轨出现一次发散-重新收敛的过程,这对于卫星在轨稳态控制是不可接受的。
根据国内外已发表的相关文献可知,对于三轴对地卫星,卫星控制的参考坐标系为轨道坐标系,在该坐标系下滤波算法的四元数测量值的参考坐标系也为轨道坐标系,因此该滤波算法的四元数测量值不存在跳变的现象,相应地的四元数估计值可以始终稳定地收敛;对于惯性定向卫星或目标姿态的参考坐标系为惯性坐标系,则常见的做法是令星敏感器单机给出的测量四元数连续变化,即滤波算法的四元数测量值标部有正有负,为完整的正弦或者余弦曲线。而针对四元数测量值跳变时的算法处理未见有效方法。
发明内容
本发明的目的在于提供一种针对星敏感器和陀螺联合滤波的改进方法,解决现有扩展卡尔曼滤波算法的四元数测量值发生跳变时四元数估计值需重新收敛的问题。
为了达到上述发明目的,本发明提供了一种针对星敏感器和陀螺联合滤波的改进方法,该方法包含如下步骤:
步骤1,构造扩展卡尔曼滤波器,进行扩展卡尔曼滤波算法;
步骤2,确定出扩展卡尔曼滤波算法的输入量四元数测量值qs的跳变时刻,该跳变时刻的判断需同时满足:四元数测量值的标部小于阈值,且,四元数测量值矢量部分中绝对值最大的数的当前拍与前一拍异号;
上述的针对星敏感器和陀螺联合滤波的改进方法,其中,步骤1中,所述的扩展卡尔曼滤波器的状态量为偏差四元数的矢量部分以及陀螺常值漂移的估计误差,取状态变量 为陀螺常值漂移值b与其估计值之间的差值,则可得扩展卡尔曼滤波器的一步预测x(k/k-1)为:
其中,
上述的针对星敏感器和陀螺联合滤波的改进方法,其中,扩展卡尔曼滤波器的状态更新公式为:
上述的针对星敏感器和陀螺联合滤波的改进方法,其中,ωs的计算公式如下:
ωs=ω+b+vg
其中,b、vg分别为陀螺的常值漂移与白噪声,ω为真实的三轴角速度。
上述的针对星敏感器和陀螺联合滤波的改进方法,其中,步骤3中,重置四元数参考值的方法为将四元数参考值的四个数全部取反。
本发明的针对星敏感器和陀螺联合滤波的改进方法,与现有技术相比,其优点和有益效果是:
1)滤波算法的四元数测量值发生跳变后,滤波算法的四元数估计值仍然连续且平稳,为姿态控制***提供了稳定的高精度姿态;
2)跳变时刻的判断对阈值选取的要求低,具备工程可操作性;
3)跳变时刻的判断方法严谨可靠,误判及漏判的概率极低,具备工程实用性;
4)对扩展卡尔曼滤波算法的改动较小,原理清晰,算法简单,星载软件容易实现。
具体实施方式
以下结合实施例对本发明的技术方案做进一步的说明。
1.构造扩展卡尔曼滤波器
根据陀螺测量值解算出的卫星本体坐标系相对于惯性坐标系的三轴角速度在卫星本体坐标系内的投影ωs可表示如下:
ωs=ω+b+vg
上式中,b、vg分别为陀螺的常值漂移与白噪声,ω为真实的三轴角速度。
根据星敏感器测量值解算出惯性坐标系相对于卫星本体坐标系的测量四元数qs,该数值即为扩展卡尔曼滤波算法的四元数测量值。该qs与ωs为扩展卡尔曼滤波器的两个输入量。
上式中F(k)为
状态更新:
上式中,ΔQ为偏差四元数量测值Δq的矢量部分,Kk为卡尔曼滤波器的增益系数。
2.确定四元数测量值的跳变时刻
设四元数测量值qs为
上式中,qs0为标部,qs1、qs2、qs3为矢量部分。
当qs0小于阈值ε(阈值通常是根据经验选择,本发明所涉及的阈值无特别限定,一般选择在0-1之间趋近于0的值,如取0.1或0.01等)时,令qm为qs1、qs2、qs3三个数中绝对值最大的数,若qm的当前拍qm(k)与前一拍qm(k-1)异号,即
qm(k)·qm(k-1)<0
则认为当前拍四元数测量值发生跳变。
扩展卡尔曼滤波算法给出的四元数估计值在四元数测量值跳变时刻需要重新收敛的原因是此时四元数测量值发生了反号,从而导致偏差四元数量测值Δq发生了跳变。
处理的方法是将当前拍四元数参考值的计算结果取反,即
根据取反后的四元数参考值再去进行后续计算(即,继续扩展卡尔曼滤波算法),则可使四元数估计值连续平稳。
综上所述,本发明通过构造扩展卡尔曼滤波,重置跳变时刻四元数参考值的方法解决了四元数测量值跳变导致四元数估计值需重新收敛的问题,为姿态确定***提供了稳定地连续地高精度姿态信息;该方法算法简单,星载软件容易实现,可靠性高。
尽管本发明的内容已经通过上述优选实施例作了详细介绍,但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后,对于本发明的多种修改和替代都将是显而易见的。因此,本发明的保护范围应由所附的权利要求来限定。
Claims (5)
1.一种针对星敏感器和陀螺联合滤波的改进方法,其特征在于,该方法包含如下步骤:
步骤1,构造扩展卡尔曼滤波器,进行扩展卡尔曼滤波算法;所述的扩展卡尔曼滤波器的状态量为偏差四元数的矢量部分以及陀螺常值漂移的估计误差,取状态变量 为陀螺常值漂移值b与其估计值之间的差值,则可得扩展卡尔曼滤波器的一步预测x(k/k-1)为:
其中,F(k)为:
扩展卡尔曼滤波器的状态更新公式为:
步骤2,确定出扩展卡尔曼滤波算法的输入量四元数测量值qs的跳变时刻,该跳变时刻的判断需同时满足:四元数测量值的标部小于阈值,且四元数测量值矢量部分中绝对值最大的数的当前拍与前一拍异号;
3.如权利要求2所述的针对星敏感器和陀螺联合滤波的改进方法,其特征在于,ωs的计算公式如下:
ωs=ω+b+vg
其中,b、vg分别为陀螺的常值漂移与白噪声,ω为真实的三轴角速度。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710419373.6A CN107228674B (zh) | 2017-06-06 | 2017-06-06 | 一种针对星敏感器和陀螺联合滤波的改进方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710419373.6A CN107228674B (zh) | 2017-06-06 | 2017-06-06 | 一种针对星敏感器和陀螺联合滤波的改进方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107228674A CN107228674A (zh) | 2017-10-03 |
CN107228674B true CN107228674B (zh) | 2021-01-29 |
Family
ID=59935425
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710419373.6A Active CN107228674B (zh) | 2017-06-06 | 2017-06-06 | 一种针对星敏感器和陀螺联合滤波的改进方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107228674B (zh) |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109489661B (zh) * | 2018-11-02 | 2020-06-09 | 上海航天控制技术研究所 | 一种卫星初始入轨时陀螺组合常值漂移估计方法 |
CN109649691B (zh) * | 2018-12-27 | 2021-07-13 | 上海航天控制技术研究所 | 一种偏置动量卫星单飞轮与磁联合控制方法和*** |
CN110109470B (zh) * | 2019-04-09 | 2021-10-29 | 西安电子科技大学 | 基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制*** |
CN110260869B (zh) * | 2019-05-10 | 2023-04-25 | 哈尔滨工业大学 | 一种降低星敏感器和陀螺联合滤波计算量的改进方法 |
CN111207776B (zh) * | 2020-02-25 | 2022-04-12 | 上海航天控制技术研究所 | 一种适用于火星探测的星敏感器与陀螺联合标定方法 |
CN113074753A (zh) * | 2021-03-19 | 2021-07-06 | 南京天巡遥感技术研究院有限公司 | 一种星敏感器陀螺联合定姿方法、联合定姿***及应用 |
CN113686334B (zh) * | 2021-07-07 | 2023-08-04 | 上海航天控制技术研究所 | 一种提高星敏感器和陀螺在轨联合滤波精度的方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6317662B1 (en) * | 2000-07-24 | 2001-11-13 | Hughes Electronics Corporation | Stable and verifiable state estimation methods and systems with spacecraft applications |
CN105300384A (zh) * | 2015-04-03 | 2016-02-03 | 东南大学 | 一种用于卫星姿态确定的交互式滤波方法 |
CN106500695A (zh) * | 2017-01-05 | 2017-03-15 | 大连理工大学 | 一种基于自适应扩展卡尔曼滤波的人体姿态识别方法 |
-
2017
- 2017-06-06 CN CN201710419373.6A patent/CN107228674B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6317662B1 (en) * | 2000-07-24 | 2001-11-13 | Hughes Electronics Corporation | Stable and verifiable state estimation methods and systems with spacecraft applications |
CN105300384A (zh) * | 2015-04-03 | 2016-02-03 | 东南大学 | 一种用于卫星姿态确定的交互式滤波方法 |
CN106500695A (zh) * | 2017-01-05 | 2017-03-15 | 大连理工大学 | 一种基于自适应扩展卡尔曼滤波的人体姿态识别方法 |
Non-Patent Citations (2)
Title |
---|
基于陀螺和四元数的EKF卫星姿态确定算法;朱庆华等;《上海航天》;20051231(第4期);第0-3节 * |
陀螺飞轮在微小航天器姿态控制中的应用;张晗;《中国优秀硕士学位论文全文数据库 工程科技II辑》;20140315(第3期);第50页 * |
Also Published As
Publication number | Publication date |
---|---|
CN107228674A (zh) | 2017-10-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107228674B (zh) | 一种针对星敏感器和陀螺联合滤波的改进方法 | |
CN111721289B (zh) | 自动驾驶中车辆定位方法、装置、设备、存储介质及车辆 | |
JP6094026B2 (ja) | 姿勢判定方法、位置算出方法及び姿勢判定装置 | |
CN106500693B (zh) | 一种基于自适应扩展卡尔曼滤波的ahrs算法 | |
CN107618678B (zh) | 卫星姿态角度偏差下的姿控信息联合估计方法 | |
JP2009098125A (ja) | 動的に較正されるセンサデータと、ナビゲーションシステム内の繰り返し拡張カルマンフィルタとを使用する、ジャイロコンパスの整合用のシステム及び方法 | |
CN107270891B (zh) | 基于抗差估计的惯性地磁匹配定位方法 | |
CN114485641B (zh) | 一种基于惯导卫导方位融合的姿态解算方法及装置 | |
CN103776449B (zh) | 一种提高鲁棒性的动基座初始对准方法 | |
JP2015148450A (ja) | センサ誤差補正装置、imu較正システム、imu較正方法、および、imu較正プログラム | |
Cao et al. | Anti-disturbance fault tolerant initial alignment for inertial navigation system subjected to multiple disturbances | |
CN111189442A (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
CN113739806B (zh) | 利用参数识别的惯导***阻尼方法 | |
CN114459466A (zh) | 一种基于模糊控制的mems多传感器数据融合处理方法 | |
CN116608853B (zh) | 载体动态姿态估计方法、设备、存储介质 | |
JP2019082328A (ja) | 位置推定装置 | |
CN110160530B (zh) | 一种基于四元数的航天器姿态滤波方法 | |
CN116608859A (zh) | 一种基于阈值处理的自适应无迹卡尔曼滤波的导航方法、存储介质和设备 | |
CN109459769A (zh) | 一种自主定位方法与*** | |
CN106092141B (zh) | 一种改善相对位置传感器性能的方法及装置 | |
CN114812546A (zh) | 遮蔽空间单兵导航位姿修正方法及装置 | |
JP2004045385A (ja) | 移動体の姿勢検出装置 | |
CN109489689B (zh) | 一种基于α-β滤波的星矢量测量误差在轨估计方法 | |
JP2015102330A (ja) | 移動情報算出装置、移動情報算出方法、移動情報算出プログラム、および移動体 | |
Alexiev et al. | An algorithm for error reducing in IMU |
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 |