CN106595669A - 一种旋转体姿态解算方法 - Google Patents

Abstract

本发明公开了一种旋转体姿态解算方法，包括以下步骤：步骤1、利用三轴陀螺角速率信息采用旋转矢量法优化算法解算姿态信息；步骤2、利用地磁信息计算，通过积分比值法求解横滚角信息；步骤3、将地磁信号解算的滚转角和陀螺信号解算的俯仰角和偏航角作为下一时刻解算四元数；步骤4、重复步骤1至步骤3即实现姿态更新。本发明利用陀螺当前角速率、角增量，以及上一时刻角增量信息，计算出姿态信息，并利用两轴地磁信号解算出的滚转角修正以获得更高的精度。

Description

一种旋转体姿态解算方法

技术领域

本发明属于姿态测量领域，涉及一种旋转体姿态解算方法。

背景技术

高动态环境下捷联惯导***的姿态解算是提高***精度的关键技术。姿态解算是指利用载体传感器的输出计算分析得到的姿态角，包括航向角、俯仰角、滚转角。对作高动态运动处在高动态环境的导弹炮弹等载体来说，姿态测量精度是决定其捷联惯导***能否正常工作的关键性因素。旋转弹体绕自身纵轴高速旋转，高精度陀螺仪的量程有限，难以应用在高速高旋的弹载环境。地磁场探测具有响应速度快、体积小、抗高过载能力强、无积累误差等优点，适合用于高速高旋的常规弹药弹体姿态测量。

磁传感器测量弹体姿态的方法有多种方式，非正交两轴传感器、三轴正交传感器、四轴传感器等。非正交磁传感器组合测量法解算载体姿态角主要有极值比值法等。

然而传统的姿态解算算法存在一些不足之处。在实际采样中，极值比值法在每个旋转周期仅取一个数据，这就容易产生数据偶然性误差。在噪声较大的高动态环境下，随着误差的不断积累，传统的姿态解算将不再适用。

因此，针对上述问题提出一种新的旋转体姿态解算方法。

发明内容

本发明的目的就在于为了解决上述问题而提供一种稳定性和抗干扰性较高的旋转体姿态解算方法。

本发明通过以下技术方案来实现上述目的，一种旋转体姿态解算方法，利用单轴磁传感器S1和单轴磁传感器S2对旋转体进行测量，在旋转体上设置弹体坐标系Ox₁y₁z₁，原点O为旋转体的质点，Ox₁轴与旋转体纵轴重合，Oy₁轴和Oz₁轴相互垂直并与Ox₁轴垂直，单轴磁传感器S1和单轴磁传感器S2均设置在Ox₁z₁平面内，单轴磁传感器S1沿Oz₁轴安装，单轴磁传感器S2与Ox₁轴成λ角安装，单轴磁传感器S1的测量值为H_S1，单轴磁传感器S2的测量值为H_S2，包括以下步骤：

步骤1、计算积分数学模型f(θ_m)的值；

步骤2、根据得到f(θ_m)的函数求解俯仰角θ_m；计算测量值H_S1或H_S2为零时的横滚角γ；

步骤3、以步骤2的俯仰角θ_m和横滚角γ作为初值，以俯仰角、俯仰角速度和横滚角为状态集，以测量值H_S1或H_S2为观测集，用容积卡尔曼算法进行滤波。

更进一步的，步骤1中积分数学模型f(θ_m)由下式表示：

式中，ψ表示航向角，θ_m表示包含磁倾角的俯仰角，γ表示横滚角。

更进一步的，步骤2中俯仰角θ_m通过下式计算得到：

式中， 和不同时为零。

更进一步的，步骤2中磁传感器S1的测量值H_S1＝0时，横滚角γ通过下式计算得到：

γ＝arctan2((-1)^j+1sinψcosθ_m,(-1)^jsinθ_m)

式中，j＝1,2，(-1)^j+1sinψcosθ_m和(-1)^jsinθ_m两个参数不同时为零，ψ表示航向角，θ_m表示包含磁倾角的俯仰角。

更进一步的，步骤2中磁传感器S2的测量值H_S2＝0时，横滚角γ通过下式计算得到：

式中，ψ表示航向角，θ_m表示包含磁倾角的俯仰角。

更进一步的，步骤3中用容积卡尔曼算法进行滤波，包括时间更新和量测更新，其中，时间更新包括以下步骤：

a、计算容积点：

其中，S_k-1为协方差矩阵P_k-1的cholesky分解，即S_k-1＝chol{P_k-1}；

b、计算通过状态方程传播的容积点：

c、状态预测和协方差预测：

量测更新包括以下步骤：

a1、计算容积点

其中S_k|k-1为协方差矩阵P_k|k-1的cholesky分解，即S_k|k-1＝chol{P_k|k-1}；

b1、计算通过测量方程传播的容积点

Z_j,k|k-1＝h(X_j,k|k-1)

c1、量测更新

d1、估计新息协方差和互协方差

e1、计算CKF增益

f1、更新状态和协方差更新

有益效果：本发明的旋转体姿态解算方法利用旋转体旋转一圈范围内磁传感器的所求数据作为CKF滤波初值，有利于提高初值估计的准确度，降低初值不准确对后续姿态实时解算影响。而CKF的引入，对高动态环境下弹丸非线性磁场采样数据的平滑处理和减小平滑误差有着较好的性能，作为一种非线性滤波算法，CKF通过Spherical-Radial准则进行非线性处理，相比较于零点交叉法和极值比值法有着更高的精度，同时，现有的成熟CKF硬件实现技术使本发明算法的工程应用成为可能。

附图说明

图1是本发明实施例的两轴磁传感器安装示意图；

图2是本发明实施例积分比值法的算法流程示意图；

图3是本发俯仰角与磁传感器输出值积分的函数示意图；

图4是传统积分比值法滤波值与真实值之差示意图；

图5是扩展卡尔曼(EKF)滤波值与真实值之差示意图；

图6是本发明(容积卡尔曼CKF)滤波值与真实值之差示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

下面结合附图和具体实施例对本发明的方法作进一步说明:

三轴磁传感器安装在弹体上，并使得磁传感器三轴对准载体坐标系的三轴，则磁传感器的三轴测量值就是向量H_b在弹体坐标系Ox₁y₁z₁中的坐标H_bx、H_by、H_bz，可表示为：

其中，H_b表示磁场强度矢量H在弹体坐标系Ox₁y₁z₁中的表达式，ψ表示航向角，θ_m表示包含磁倾角的俯仰角，γ表示横滚角，h为地磁场矢量H的标量大小。

本发明的两轴磁传感器安装图如图1所示，两个非正交的单轴磁传感器S1、S2分别安装在弹体坐标系的Ox₁y₁z₁的原点上，Ox₁轴与弹体纵轴重合，两个敏感轴都在Ox₁z₁平面内，S1沿Oz₁轴安装，S2与Ox₁轴成λ角安装。

磁传感器S1的测量值为H_bz、S2的测量值为H_bx和H_bz的组合，因此，可得磁传感器S1和S2的测量值H_S1和H_S2与弹体姿态角θ_m、γ、h的表达式

H_S1＝h(cosγsinψcosθ_m+sinγsinθ_m)

H_S2＝h(cosθ_mcosψcosλ+cosγsinψcosθ_msinλ+sinγsinθ_msinλ)

积分数学模型：

其中，N表示弹丸旋转一圈的总采样次数，n表示采样点；和表示关于横滚角γ的积分运算；和表示弹丸旋转一圈两个传感器采样值H_S1和H_S2的离散采样点的平方和运算；

弹丸飞行一圈时，假设夹角λ和航向角ψ不变，通过两个传感器采样值平方和的积分运算，可以得到一个f(θ_m)的值。求出f(θ_m)的解，即可得到俯仰角θ_m的值。H_S1或H_S2为零时，消去未知数磁场强度标量h，可获得横滚角γ的估计值。

图2给出了积分比值法的算法流程：

1.计算积分模型f(θ_m)

令：H_S1＝h(d₂cosγ+e₂sinγ)

H_S2＝h(a₂+b₂cosγ+c₂sinγ)

其中，

根据滤波器输出计算H_S1关于θ_m的积分表达式

得到

图3表示俯仰角θ_m旋转一圈时，磁传感器的输出值根据模型进行积分运算后的输出值与俯仰角θ_m的关系。H₁、H₂分别表示磁传感器S1、S2输出值积分所得，f(θ_m)表示两个积分所得值的比值。

俯仰角θ_m的周期是2π，模型f(θ_m)关于俯仰角θ_m的函数周期是π。横滚角γ旋转一圈时，积分运算得到一个f(θ_m)数值，并对应得到四个对应的俯仰角θ_m的值。

2.计算f(θ_m)的解俯仰角θ_m

将sin²θ_m＝1-cos²θ_m将代入f(θ_m)表达式，可得关于cos²θ_m的表达式：

cos²θ_m(2cos²ψcos²λ+sin²ψsin²λ-sin²λ-f(θ_m)sin²ψ+f(θ_m))＝f(θ_m)-sin²λ

假设分母不等于零，可解得cosθ_m和sinθ_m，为得到俯仰角θ_m更通用的表达式，同时避免引入不等于零的假设，需做如下的变换：

整理式可得：

uvsin²θ_m＝u(u-v)cos²θ_m

最后，可以得到俯仰角θ_m的表达式：

式中，函数两个参数不能同时为零。

3.计算横滚角γ

弹体旋转的一周内，可以近似为匀速转动，而且航向角ψ和俯仰角θ几乎不变，可获得弹体某一特定时刻的横滚角γ角度值，即可计算出弹体旋转一周内所有时刻的横滚角γ。假设特定时刻为H_S1或H_S2的零点，测量值H_S1或H_S2为零时，可以消去未知数磁场强度标量h，减少周围干扰磁场环境对计算结果的影响。

(1)磁传感器S1的测量值H_S1＝0时：

cosγsinψcosθ_m+sinγsinθ_m＝0

整理得γ＝arctan2((-1)^j+1sinψcosθ_m,(-1)^jsinθ_m)

式中，j＝1,2，函数的两个参数不同时为零。

(2)磁传感器S2的测量值H_S2＝0时：

cosθ_mcosψcosλ+cosγsinψcosθ_msinλ+sinγsinθ_msinλ＝0

式中，

当|sinλ|＞|cosθ_mcosψ|时，磁传感器S2的测量值H_S2有两个零点，此时横滚角γ有两个解，应根据横滚角γ的角速率进行取舍；

当|sinλ|＝|cosθ_mcosψ|时，磁传感器S2的测量值H_S2有一个零点；

当|sinλ|＜|cosθ_mcosψ|时，磁传感器S2的测量值H_S2不一定有零点。

4.结合容积卡尔曼滤波算法

以旋转一周计算出的俯仰角θ_m和横滚角γ为初值，状态集X＝[θ_m,ω,γ]，观测集Z＝[H_S1,H_S2]，采用CKF算法，具体流程如下：

(1)时间更新

a.计算容积点

其中S_k-1为协方差矩阵P_k-1的cholesky分解，即S_k-1＝chol{P_k-1}。

b.计算通过状态方程传播的容积点

c.状态预测和协方差预测

(2)量测更新

a.计算容积点

其中S_k|k-1为协方差矩阵P_k|k-1的cholesky分解，即S_k|k-1＝chol{P_k|k-1}。

b.计算通过测量方程传播的容积点

Z_j,k|k-1＝h(X_j,k|k-1)

c.量测更新

d.估计新息协方差和互协方差

e.计算CKF增益

f.状态更新和协方差更新

容积卡尔曼滤波(CKF)通过采用Spherical-Radial准则，以高斯假设的贝叶斯估计为基础，将非线性滤波转化为非线性函数与高斯概率密度函数乘积的积分求解问题，并利用容积数值积分原则来逼近状态后验分布。

本发明用第一圈计算的姿态角为初值，并结合CKF滤波算法的姿态角解算方法)精度好于传统的积分比值法和极值比值法，从图4～6中可以看出，积分比值法的精度远低于扩展卡尔曼(EKF)和容积卡尔曼(CKF)滤波算法，而后两者中又以CKF的姿态角解算精度更高。

Claims (6)

1.一种旋转体姿态解算方法，其特征在于，利用单轴磁传感器S1和单轴磁传感器S2对旋转体进行测量，在旋转体上设置弹体坐标系Ox₁y₁z₁，原点O为旋转体的质点，Ox₁轴与旋转体纵轴重合，Oy₁轴和Oz₁轴相互垂直并与Ox₁轴垂直，单轴磁传感器S1和单轴磁传感器S2均设置在Ox₁z₁平面内，单轴磁传感器S1沿Oz₁轴安装，单轴磁传感器S2与Ox₁轴成λ角安装，单轴磁传感器S1的测量值为H_S1，单轴磁传感器S2的测量值为H_S2，包括以下步骤：

步骤1、根据计算积分数学模型f(θ_m)的值：

θ_m表示包含磁倾角的俯仰角，γ表示横滚角；

步骤2、根据得到f(θ_m)的函数求解俯仰角θ_m；计算测量值H_S1或H_S2为零时的横滚角γ；

步骤3、以步骤2的俯仰角θ_m和横滚角γ作为初值，以俯仰角、俯仰角速度和横滚角为状态集，以测量值H_S1或H_S2为观测集，用容积卡尔曼算法进行滤波。

2.根据权利要求1所述的旋转体姿态解算方法，其特征在于：步骤1中积分数学模型f(θ_m)由下式表示：

$f\left({\mathrm{\θ}}_m\right)=\frac{2{\cos }^2{\mathrm{\θ}}_m{\cos }^2{\mathrm{\ψcos}}^2\mathrm{\λ}+{\sin }^2{\mathrm{\ψcos}}^2{\mathrm{\θ}}_m{\sin }^2\mathrm{\λ}+{\sin }^2{\mathrm{\θ}}_m{\sin }^2\mathrm{\λ}}{{\sin }^2{\mathrm{\ψcos}}^2{\mathrm{\θ}}_m+{\sin }^2{\mathrm{\θ}}_m}.$

式中，ψ表示航向角，θ_m表示包含磁倾角的俯仰角，γ表示横滚角。

3.根据权利要求1所述的旋转体姿态解算方法，其特征在于：步骤2中俯仰角θ_m通过下式计算得到：

${\mathrm{\θ}}_m=arctan2(\±\sqrt{\left|u(u-v)\right|},\±\sqrt{\left|uv\right|})$

式中，

和不同时为零。

4.根据权利要求1所述的旋转体姿态解算方法，其特征在于：步骤2中磁传感器S1的测量值H_S1＝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的测量值H_S2＝0时，横滚角γ通过下式计算得到：

$cos\mathrm{\γ}=\frac{-a_1{b}_1\±\sqrt{{c_1}^2({b_1}^2+{c_1}^2-{a_1}^2)}}{{b_1}^2+{c_1}^2}$

式中，ψ表示航向角，θ_m表示包含磁倾角的俯仰角。

6.根据权利要求1所述的旋转体姿态解算方法，其特征在于：步骤3中用容积卡尔曼算法进行滤波，包括时间更新和量测更新，其中，

时间更新包括以下步骤：

a、计算容积点：

$X_{j,k-1}=S_{k-1}{\mathrm{\ξ}}_j+{\hat{X}}_{k-1}$

其中，S_k-1为协方差矩阵P_k-1的cholesky分解，即S_k-1＝chol{P_k-1}；

b、计算通过状态方程传播的容积点：

$X_{j,k|k-1}^{*}={\mathrm{\Φ}}_{k|k-1}{X}_{j,k-1};$

c、状态预测和协方差预测：

${\hat{X}}_{k|k-1}=\underset{j=1}{\overset{m}{\Σ}}{\mathrm{\ω}}_j{X}_{j,k|k-1}^{*}$

$P_{k|k-1}=\underset{j=1}{\overset{m}{\Σ}}{\mathrm{\ω}}_j{X}_{j,k|k-1}^{*}{X}_{j,k|k-1}^{*T}-{\hat{X}}_{k|k-1}{\hat{X}}_{k|k-1}^T+Q_{k-1};$

量测更新包括以下步骤：

a1、计算容积点

$X_{j,k|k-1}=S_{k|k-1}{\mathrm{\ξ}}_j+{\hat{X}}_{k|k-1}$

其中S_k|k-1为协方差矩阵P_k|k-1的cholesky分解，即S_k|k-1＝chol{P_k|k-1}；

b1、计算通过测量方程传播的容积点

Z_j,k|k-1＝h(X_j,k|k-1)

c1、量测更新

${\hat{Z}}_{k|k-1}=\underset{j=1}{\overset{m}{\Σ}}{\mathrm{\ω}}_j{Z}_{j,k|k-1}$

d1、估计新息协方差和互协方差

$P_{zz,k|k-1}=\underset{j=1}{\overset{m}{\Σ}}{\mathrm{\ω}}_j{Z}_{j,k|k-1}{Z}_{j,k|k-1}^T-{\hat{Z}}_{k|k-1}{\hat{Z}}_{k|k-1}^T+R_k$

$P_{xz,k|k-1}=\underset{j=1}{\overset{m}{\Σ}}{\mathrm{\ω}}_j{X}_{j,k|k-1}{Z}_{j,k|k-1}^T-{\hat{X}}_{k|k-1}{\hat{Z}}_{k|k-1}^T$

e1、计算CKF增益

$K_k=P_{xz,k|k-1}{P}_{zz,k|k-1}^{-1}$

f1、更新状态和协方差更新

${\hat{X}}_k={\hat{X}}_{k|k-1}+K_k(Z_k-{\hat{Z}}_{k|k-1})$

$P_k=P_{k|k-1}-K_k{P}_{zz,k|k-1}{K}_k^T.$