CN106595669A - 一种旋转体姿态解算方法 - Google Patents
一种旋转体姿态解算方法 Download PDFInfo
- Publication number
- CN106595669A CN106595669A CN201611228008.9A CN201611228008A CN106595669A CN 106595669 A CN106595669 A CN 106595669A CN 201611228008 A CN201611228008 A CN 201611228008A CN 106595669 A CN106595669 A CN 106595669A
- Authority
- CN
- China
- Prior art keywords
- angle
- theta
- calculation method
- value
- sin
- 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
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种旋转体姿态解算方法,包括以下步骤:步骤1、利用三轴陀螺角速率信息采用旋转矢量法优化算法解算姿态信息;步骤2、利用地磁信息计算,通过积分比值法求解横滚角信息;步骤3、将地磁信号解算的滚转角和陀螺信号解算的俯仰角和偏航角作为下一时刻解算四元数;步骤4、重复步骤1至步骤3即实现姿态更新。本发明利用陀螺当前角速率、角增量,以及上一时刻角增量信息,计算出姿态信息,并利用两轴地磁信号解算出的滚转角修正以获得更高的精度。
Description
技术领域
本发明属于姿态测量领域,涉及一种旋转体姿态解算方法。
背景技术
高动态环境下捷联惯导***的姿态解算是提高***精度的关键技术。姿态解算是指利用载体传感器的输出计算分析得到的姿态角,包括航向角、俯仰角、滚转角。对作高动态运动处在高动态环境的导弹炮弹等载体来说,姿态测量精度是决定其捷联惯导***能否正常工作的关键性因素。旋转弹体绕自身纵轴高速旋转,高精度陀螺仪的量程有限,难以应用在高速高旋的弹载环境。地磁场探测具有响应速度快、体积小、抗高过载能力强、无积累误差等优点,适合用于高速高旋的常规弹药弹体姿态测量。
磁传感器测量弹体姿态的方法有多种方式,非正交两轴传感器、三轴正交传感器、四轴传感器等。非正交磁传感器组合测量法解算载体姿态角主要有极值比值法等。
然而传统的姿态解算算法存在一些不足之处。在实际采样中,极值比值法在每个旋转周期仅取一个数据,这就容易产生数据偶然性误差。在噪声较大的高动态环境下,随着误差的不断积累,传统的姿态解算将不再适用。
因此,针对上述问题提出一种新的旋转体姿态解算方法。
发明内容
本发明的目的就在于为了解决上述问题而提供一种稳定性和抗干扰性较高的旋转体姿态解算方法。
本发明通过以下技术方案来实现上述目的,一种旋转体姿态解算方法,利用单轴磁传感器S1和单轴磁传感器S2对旋转体进行测量,在旋转体上设置弹体坐标系Ox1y1z1,原点O为旋转体的质点,Ox1轴与旋转体纵轴重合,Oy1轴和Oz1轴相互垂直并与Ox1轴垂直,单轴磁传感器S1和单轴磁传感器S2均设置在Ox1z1平面内,单轴磁传感器S1沿Oz1轴安装,单轴磁传感器S2与Ox1轴成λ角安装,单轴磁传感器S1的测量值为HS1,单轴磁传感器S2的测量值为HS2,包括以下步骤:
步骤1、计算积分数学模型f(θm)的值;
步骤2、根据得到f(θm)的函数求解俯仰角θm;计算测量值HS1或HS2为零时的横滚角γ;
步骤3、以步骤2的俯仰角θm和横滚角γ作为初值,以俯仰角、俯仰角速度和横滚角为状态集,以测量值HS1或HS2为观测集,用容积卡尔曼算法进行滤波。
更进一步的,步骤1中积分数学模型f(θm)由下式表示:
式中,ψ表示航向角,θm表示包含磁倾角的俯仰角,γ表示横滚角。
更进一步的,步骤2中俯仰角θm通过下式计算得到:
式中, 和不同时为零。
更进一步的,步骤2中磁传感器S1的测量值HS1=0时,横滚角γ通过下式计算得到:
γ=arctan2((-1)j+1sinψcosθm,(-1)jsinθm)
式中,j=1,2,(-1)j+1sinψcosθm和(-1)jsinθm两个参数不同时为零,ψ表示航向角,θm表示包含磁倾角的俯仰角。
更进一步的,步骤2中磁传感器S2的测量值HS2=0时,横滚角γ通过下式计算得到:
式中,ψ表示航向角,θm表示包含磁倾角的俯仰角。
更进一步的,步骤3中用容积卡尔曼算法进行滤波,包括时间更新和量测更新,其中,时间更新包括以下步骤:
a、计算容积点:
其中,Sk-1为协方差矩阵Pk-1的cholesky分解,即Sk-1=chol{Pk-1};
b、计算通过状态方程传播的容积点:
c、状态预测和协方差预测:
量测更新包括以下步骤:
a1、计算容积点
其中Sk|k-1为协方差矩阵Pk|k-1的cholesky分解,即Sk|k-1=chol{Pk|k-1};
b1、计算通过测量方程传播的容积点
Zj,k|k-1=h(Xj,k|k-1)
c1、量测更新
d1、估计新息协方差和互协方差
e1、计算CKF增益
f1、更新状态和协方差更新
有益效果:本发明的旋转体姿态解算方法利用旋转体旋转一圈范围内磁传感器的所求数据作为CKF滤波初值,有利于提高初值估计的准确度,降低初值不准确对后续姿态实时解算影响。而CKF的引入,对高动态环境下弹丸非线性磁场采样数据的平滑处理和减小平滑误差有着较好的性能,作为一种非线性滤波算法,CKF通过Spherical-Radial准则进行非线性处理,相比较于零点交叉法和极值比值法有着更高的精度,同时,现有的成熟CKF硬件实现技术使本发明算法的工程应用成为可能。
附图说明
图1是本发明实施例的两轴磁传感器安装示意图;
图2是本发明实施例积分比值法的算法流程示意图;
图3是本发俯仰角与磁传感器输出值积分的函数示意图;
图4是传统积分比值法滤波值与真实值之差示意图;
图5是扩展卡尔曼(EKF)滤波值与真实值之差示意图;
图6是本发明(容积卡尔曼CKF)滤波值与真实值之差示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
下面结合附图和具体实施例对本发明的方法作进一步说明:
三轴磁传感器安装在弹体上,并使得磁传感器三轴对准载体坐标系的三轴,则磁传感器的三轴测量值就是向量Hb在弹体坐标系Ox1y1z1中的坐标Hbx、Hby、Hbz,可表示为:
其中,Hb表示磁场强度矢量H在弹体坐标系Ox1y1z1中的表达式,ψ表示航向角,θm表示包含磁倾角的俯仰角,γ表示横滚角,h为地磁场矢量H的标量大小。
本发明的两轴磁传感器安装图如图1所示,两个非正交的单轴磁传感器S1、S2分别安装在弹体坐标系的Ox1y1z1的原点上,Ox1轴与弹体纵轴重合,两个敏感轴都在Ox1z1平面内,S1沿Oz1轴安装,S2与Ox1轴成λ角安装。
磁传感器S1的测量值为Hbz、S2的测量值为Hbx和Hbz的组合,因此,可得磁传感器S1和S2的测量值HS1和HS2与弹体姿态角θm、γ、h的表达式
HS1=h(cosγsinψcosθm+sinγsinθm)
HS2=h(cosθmcosψcosλ+cosγsinψcosθmsinλ+sinγsinθmsinλ)
积分数学模型:
其中,N表示弹丸旋转一圈的总采样次数,n表示采样点;和表示关于横滚角γ的积分运算;和表示弹丸旋转一圈两个传感器采样值HS1和HS2的离散采样点的平方和运算;
弹丸飞行一圈时,假设夹角λ和航向角ψ不变,通过两个传感器采样值平方和的积分运算,可以得到一个f(θm)的值。求出f(θm)的解,即可得到俯仰角θm的值。HS1或HS2为零时,消去未知数磁场强度标量h,可获得横滚角γ的估计值。
图2给出了积分比值法的算法流程:
1.计算积分模型f(θm)
令:HS1=h(d2cosγ+e2sinγ)
HS2=h(a2+b2cosγ+c2sinγ)
其中,
根据滤波器输出计算HS1关于θm的积分表达式
得到
图3表示俯仰角θm旋转一圈时,磁传感器的输出值根据模型进行积分运算后的输出值与俯仰角θm的关系。H1、H2分别表示磁传感器S1、S2输出值积分所得,f(θm)表示两个积分所得值的比值。
俯仰角θm的周期是2π,模型f(θm)关于俯仰角θm的函数周期是π。横滚角γ旋转一圈时,积分运算得到一个f(θm)数值,并对应得到四个对应的俯仰角θm的值。
2.计算f(θm)的解俯仰角θm
将sin2θm=1-cos2θm将代入f(θm)表达式,可得关于cos2θm的表达式:
cos2θm(2cos2ψcos2λ+sin2ψsin2λ-sin2λ-f(θm)sin2ψ+f(θm))=f(θm)-sin2λ
假设分母不等于零,可解得cosθm和sinθm,为得到俯仰角θm更通用的表达式,同时避免引入不等于零的假设,需做如下的变换:
整理式可得:
uvsin2θm=u(u-v)cos2θm
最后,可以得到俯仰角θm的表达式:
式中,函数两个参数不能同时为零。
3.计算横滚角γ
弹体旋转的一周内,可以近似为匀速转动,而且航向角ψ和俯仰角θ几乎不变,可获得弹体某一特定时刻的横滚角γ角度值,即可计算出弹体旋转一周内所有时刻的横滚角γ。假设特定时刻为HS1或HS2的零点,测量值HS1或HS2为零时,可以消去未知数磁场强度标量h,减少周围干扰磁场环境对计算结果的影响。
(1)磁传感器S1的测量值HS1=0时:
cosγsinψcosθm+sinγsinθm=0
整理得γ=arctan2((-1)j+1sinψcosθm,(-1)jsinθm)
式中,j=1,2,函数的两个参数不同时为零。
(2)磁传感器S2的测量值HS2=0时:
cosθmcosψcosλ+cosγsinψcosθmsinλ+sinγsinθmsinλ=0
式中,
当|sinλ|>|cosθmcosψ|时,磁传感器S2的测量值HS2有两个零点,此时横滚角γ有两个解,应根据横滚角γ的角速率进行取舍;
当|sinλ|=|cosθmcosψ|时,磁传感器S2的测量值HS2有一个零点;
当|sinλ|<|cosθmcosψ|时,磁传感器S2的测量值HS2不一定有零点。
4.结合容积卡尔曼滤波算法
以旋转一周计算出的俯仰角θm和横滚角γ为初值,状态集X=[θm,ω,γ],观测集Z=[HS1,HS2],采用CKF算法,具体流程如下:
(1)时间更新
a.计算容积点
其中Sk-1为协方差矩阵Pk-1的cholesky分解,即Sk-1=chol{Pk-1}。
b.计算通过状态方程传播的容积点
c.状态预测和协方差预测
(2)量测更新
a.计算容积点
其中Sk|k-1为协方差矩阵Pk|k-1的cholesky分解,即Sk|k-1=chol{Pk|k-1}。
b.计算通过测量方程传播的容积点
Zj,k|k-1=h(Xj,k|k-1)
c.量测更新
d.估计新息协方差和互协方差
e.计算CKF增益
f.状态更新和协方差更新
容积卡尔曼滤波(CKF)通过采用Spherical-Radial准则,以高斯假设的贝叶斯估计为基础,将非线性滤波转化为非线性函数与高斯概率密度函数乘积的积分求解问题,并利用容积数值积分原则来逼近状态后验分布。
本发明用第一圈计算的姿态角为初值,并结合CKF滤波算法的姿态角解算方法)精度好于传统的积分比值法和极值比值法,从图4~6中可以看出,积分比值法的精度远低于扩展卡尔曼(EKF)和容积卡尔曼(CKF)滤波算法,而后两者中又以CKF的姿态角解算精度更高。
Claims (6)
1.一种旋转体姿态解算方法,其特征在于,利用单轴磁传感器S1和单轴磁传感器S2对旋转体进行测量,在旋转体上设置弹体坐标系Ox1y1z1,原点O为旋转体的质点,Ox1轴与旋转体纵轴重合,Oy1轴和Oz1轴相互垂直并与Ox1轴垂直,单轴磁传感器S1和单轴磁传感器S2均设置在Ox1z1平面内,单轴磁传感器S1沿Oz1轴安装,单轴磁传感器S2与Ox1轴成λ角安装,单轴磁传感器S1的测量值为HS1,单轴磁传感器S2的测量值为HS2,包括以下步骤:
步骤1、根据计算积分数学模型f(θm)的值:
θm表示包含磁倾角的俯仰角,γ表示横滚角;
步骤2、根据得到f(θm)的函数求解俯仰角θm;计算测量值HS1或HS2为零时的横滚角γ;
步骤3、以步骤2的俯仰角θm和横滚角γ作为初值,以俯仰角、俯仰角速度和横滚角为状态集,以测量值HS1或HS2为观测集,用容积卡尔曼算法进行滤波。
2.根据权利要求1所述的旋转体姿态解算方法,其特征在于:步骤1中积分数学模型f(θm)由下式表示:
式中,ψ表示航向角,θm表示包含磁倾角的俯仰角,γ表示横滚角。
3.根据权利要求1所述的旋转体姿态解算方法,其特征在于:步骤2中俯仰角θm通过下式计算得到:
式中,
和不同时为零。
4.根据权利要求1所述的旋转体姿态解算方法,其特征在于:步骤2中磁传感器S1的测量值HS1=0时,横滚角γ通过下式计算得到:
γ=arctan2((-1)j+1sinψcosθm,(-1)jsinθm)
式中,j=1,2,(-1)j+1sinψcosθm和(-1)jsinθm两个参数不同时为零,ψ表示航向角,θm表示包含磁倾角的俯仰角。
5.根据权利要求1所述的旋转体姿态解算方法,其特征在于:步骤2中磁传感器S2的测量值HS2=0时,横滚角γ通过下式计算得到:
式中,ψ表示航向角,θm表示包含磁倾角的俯仰角。
6.根据权利要求1所述的旋转体姿态解算方法,其特征在于:步骤3中用容积卡尔曼算法进行滤波,包括时间更新和量测更新,其中,
时间更新包括以下步骤:
a、计算容积点:
其中,Sk-1为协方差矩阵Pk-1的cholesky分解,即Sk-1=chol{Pk-1};
b、计算通过状态方程传播的容积点:
c、状态预测和协方差预测:
量测更新包括以下步骤:
a1、计算容积点
其中Sk|k-1为协方差矩阵Pk|k-1的cholesky分解,即Sk|k-1=chol{Pk|k-1};
b1、计算通过测量方程传播的容积点
Zj,k|k-1=h(Xj,k|k-1)
c1、量测更新
d1、估计新息协方差和互协方差
e1、计算CKF增益
f1、更新状态和协方差更新
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201611228008.9A CN106595669B (zh) | 2016-12-27 | 2016-12-27 | 一种旋转体姿态解算方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201611228008.9A CN106595669B (zh) | 2016-12-27 | 2016-12-27 | 一种旋转体姿态解算方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106595669A true CN106595669A (zh) | 2017-04-26 |
CN106595669B CN106595669B (zh) | 2023-04-11 |
Family
ID=58604655
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201611228008.9A Active CN106595669B (zh) | 2016-12-27 | 2016-12-27 | 一种旋转体姿态解算方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106595669B (zh) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107314718A (zh) * | 2017-05-31 | 2017-11-03 | 中北大学 | 基于磁测滚转角速率信息的高速旋转弹姿态估计方法 |
CN110044321A (zh) * | 2019-03-22 | 2019-07-23 | 北京理工大学 | 利用地磁信息和角速率陀螺解算飞行器姿态的方法 |
CN110672078A (zh) * | 2019-10-12 | 2020-01-10 | 南京理工大学 | 一种基于地磁信息的高旋弹丸姿态估计方法 |
CN110967007A (zh) * | 2019-11-21 | 2020-04-07 | 中国船舶重工集团公司第七0五研究所 | 一种适用于稳态航行可节省两轴捷联陀螺的惯导解算方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103278837A (zh) * | 2013-05-17 | 2013-09-04 | 南京理工大学 | 基于自适应滤波的sins/gnss多级容错组合导航方法 |
CN103900574A (zh) * | 2014-04-04 | 2014-07-02 | 哈尔滨工程大学 | 一种基于迭代容积卡尔曼滤波姿态估计方法 |
CN104121907A (zh) * | 2014-07-30 | 2014-10-29 | 杭州电子科技大学 | 一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法 |
CN104165640A (zh) * | 2014-08-11 | 2014-11-26 | 东南大学 | 基于星敏感器的近空间弹载捷联惯导***传递对准方法 |
CN104567871A (zh) * | 2015-01-12 | 2015-04-29 | 哈尔滨工程大学 | 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法 |
CN105937911A (zh) * | 2016-07-01 | 2016-09-14 | 南京理工大学 | 一种磁传感器姿态解算方法 |
-
2016
- 2016-12-27 CN CN201611228008.9A patent/CN106595669B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103278837A (zh) * | 2013-05-17 | 2013-09-04 | 南京理工大学 | 基于自适应滤波的sins/gnss多级容错组合导航方法 |
CN103900574A (zh) * | 2014-04-04 | 2014-07-02 | 哈尔滨工程大学 | 一种基于迭代容积卡尔曼滤波姿态估计方法 |
CN104121907A (zh) * | 2014-07-30 | 2014-10-29 | 杭州电子科技大学 | 一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法 |
CN104165640A (zh) * | 2014-08-11 | 2014-11-26 | 东南大学 | 基于星敏感器的近空间弹载捷联惯导***传递对准方法 |
CN104567871A (zh) * | 2015-01-12 | 2015-04-29 | 哈尔滨工程大学 | 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法 |
CN105937911A (zh) * | 2016-07-01 | 2016-09-14 | 南京理工大学 | 一种磁传感器姿态解算方法 |
Non-Patent Citations (1)
Title |
---|
韩萍等: "基于容积卡尔曼滤波的飞机姿态估计方法", 《交通运输工程学报》 * |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107314718A (zh) * | 2017-05-31 | 2017-11-03 | 中北大学 | 基于磁测滚转角速率信息的高速旋转弹姿态估计方法 |
CN107314718B (zh) * | 2017-05-31 | 2018-11-13 | 中北大学 | 基于磁测滚转角速率信息的高速旋转弹姿态估计方法 |
CN110044321A (zh) * | 2019-03-22 | 2019-07-23 | 北京理工大学 | 利用地磁信息和角速率陀螺解算飞行器姿态的方法 |
CN110044321B (zh) * | 2019-03-22 | 2021-01-29 | 北京理工大学 | 利用地磁信息和角速率陀螺解算飞行器姿态的方法 |
CN110672078A (zh) * | 2019-10-12 | 2020-01-10 | 南京理工大学 | 一种基于地磁信息的高旋弹丸姿态估计方法 |
CN110967007A (zh) * | 2019-11-21 | 2020-04-07 | 中国船舶重工集团公司第七0五研究所 | 一种适用于稳态航行可节省两轴捷联陀螺的惯导解算方法 |
CN110967007B (zh) * | 2019-11-21 | 2023-02-21 | 中国船舶重工集团公司第七0五研究所 | 一种适用于稳态航行可节省两轴捷联陀螺的惯导解算方法 |
Also Published As
Publication number | Publication date |
---|---|
CN106595669B (zh) | 2023-04-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106595669A (zh) | 一种旋转体姿态解算方法 | |
CN106482734A (zh) | 一种用于imu多传感器数据融合的滤波方法 | |
CN105607106B (zh) | 一种低成本高精度bd/mems融合姿态测量方法 | |
CN103630137B (zh) | 一种用于导航***的姿态及航向角的校正方法 | |
CN105737823B (zh) | 一种基于五阶ckf的gps/sins/cns组合导航方法 | |
CN103954286B (zh) | 微小卫星磁传感器多误差模型在轨迭代标定方法 | |
CN107063254A (zh) | 一种陀螺地磁组合的姿态解算方法 | |
CN104698485B (zh) | 基于bd、gps及mems的组合导航***及导航方法 | |
CN107247157B (zh) | 一种面向大姿态机动的四元数全角域转换欧拉角的获取方法 | |
CN103712598B (zh) | 一种小型无人机姿态确定方法 | |
CN101246012B (zh) | 一种基于鲁棒耗散滤波的组合导航方法 | |
CN105136145A (zh) | 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法 | |
CN108318038A (zh) | 一种四元数高斯粒子滤波移动机器人姿态解算方法 | |
CN105157705A (zh) | 一种半捷联雷达导引头视线角速度提取方法 | |
CN105937911A (zh) | 一种磁传感器姿态解算方法 | |
CN101706287A (zh) | 一种基于数字高通滤波的旋转捷联***现场标定方法 | |
CN105509737A (zh) | 一种不受地磁变化影响的航空运动平台磁干扰补偿方法 | |
CN106767925A (zh) | 带双轴转位机构的惯导***三位置参数辨识对准方法 | |
CN110737194B (zh) | 一种多mems传感器组合测姿方法 | |
CN107860382A (zh) | 一种在地磁异常情况下应用ahrs测量姿态的方法 | |
CN112066993A (zh) | 一种基于误差四元数三维矢量分布的高斯粒子滤波数据处理方法 | |
CN107270940A (zh) | 三轴磁强计的输出偏差估计方法 | |
CN113218391A (zh) | 一种基于ewt算法的姿态解算方法 | |
Gao et al. | An integrated land vehicle navigation system based on context awareness | |
CN108426584A (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 |