CN104697521A - 一种采用陀螺冗余斜交配置方式测量高速旋转体姿态和角速度的方法 - Google Patents

Abstract

本发明属于惯性导航的领域，具体涉及采用陀螺冗余斜交配置方式测量高速旋转体姿态和角速度的方法。本发明包括：利用GPS确定载体的初始位置参数；采集光纤陀螺仪和石英加速度计输出的数据：根据GPS确定的经纬度值计算出对准点的重力扰动值，对加速度计的输出进行补偿；采用解析法来完成系统的粗对准，初步确定载体的姿态信息；粗对准结束后建立捷联惯性导航系统初始对准非线性状态误差方程；利用UKF滤波方法进行滤波；利用估计出来的平台失准角修正系统的捷联初始矩阵，得到精确的初始捷联矩阵，完成高精度的初始对准。本发明采用斜置陀螺不加入其它控制方式，从而不会引入新误差，解算简单，价格低廉。

Description

一种采用陀螺冗余斜交配置方式测量高速旋转体姿态和角速度的方法

技术领域

本发明属于惯性导航的领域，具体涉及采用陀螺冗余斜交配置方式测量高速旋转体姿态和角速度的方法。

背景技术

高速旋转体的姿态和角速度测量一直是一个难点，如旋转弹的滚转角速率可高达20圈/秒(约7200度/秒)，而普通的陀螺，其角速度测量范围一般仅为几百度每秒，存在较大的差距。尽管目前有些特殊的激光陀螺、光纤陀螺或微机械陀螺具有大量程，但大量程通常导致陀螺的分辨率降低、线性度和可靠性较差，重量或体积增加。

用加速度计代替陀螺的无陀螺测量方式需要较大的杆臂长度，安装难度较大，特别在被测物体体积较小时更明显。

根据公开的文献资料显示，某些高速度旋转的火箭弹，在惯性测量组件(包含陀螺和加速度计等)上加装伺服电机，运行过程中与旋转方向相反，达到减少旋转(即减旋)的影响。这种主动减旋方式直接有效，但在实际使用过程中，缺点也很明显：(1)增加了一个伺服电机环节，降低了整个系统的可靠性；(2)由于旋转速度较大，控制难度大，如果控制不当，会起到反作用；(3)导航系统的体积、重量相应地增大，影响弹的射程。

传统的其它导航方式，如地磁导航、天文导航、GPS导航等方式存在实时性差、抗干扰能力弱等问题，无法实现对旋转载体实时、高速、精确的角速度和姿态测量，这使得后续的制导过程变得十分困难，因此研制能准确、有效测量高速旋转体的角速度和姿态的方法迫在眉睫。

本发明采用陀螺冗余斜交配置的方式，解决高速旋转体的角速度和姿态测量难题。使用斜置陀螺测量高速旋转角的角速度，积分得到姿态角。通常，陀螺按三轴正交方式安装，构成一个完整的右手坐标系，冗余斜交配置是指在正交三轴陀螺之外，斜置一个或多个陀螺测量旋转角速度。这种冗余方式使用多个器件对同一个角速度冗余测量，从而提高测量精度和系统可靠性。

发明内容

本发明的目的在于提供一种提高了导航系统在自旋方向上的测量范围和可靠性的采用陀螺冗余斜交配置方式测量高速旋转体姿态和角速度的方法。

本发明的目的是这样实现的：

(1)利用GPS确定载体的初始位置参数；

(2)采集光纤陀螺仪和石英加速度计输出的数据：

(3)根据GPS确定的经纬度值计算出对准点的重力扰动值，对加速度计的输出进行补偿；

(4)采用解析法来完成系统的粗对准，初步确定载体的姿态信息

(5)粗对准结束后建立捷联惯性导航系统初始对准非线性状态误差方程；

(6)利用UKF滤波方法进行滤波；

(7)利用估计出来的平台失准角修正系统的捷联初始矩阵得到精确的初始捷联矩阵即完成高精度的初始对准。

本发明的有益效果在于：

本发明解决了高速旋转体滚转角测量中陀螺量程的问题。只需要在斜置方向上测量一个比较小的角速度，就能推算出较大的旋转角速度。同时，采用斜置陀螺不加入其它控制方式，从而不会引入新误差，解算简单，价格低廉。

附图说明

图1为采用陀螺冗余邪教方式测量高速旋转体角速度及姿态流程图。

图2为滚转角测量陀螺冗余配置图。

图3为高速旋转体滚转角冗余测量原理图。

图4为系统在高速旋转体上的轴向配置示意图。

具体实施方式

下面结合附图对本发明做进一步描述。

针对高速旋转载体的飞行角速度和姿态测量，采用普通的陀螺，设计一种斜交冗余配置方式，扩充陀螺的测量范围，。

技术方案

本发明的技术方案分系统布置，旋转体载体系角速度解算，旋转体姿态角解算三个步骤，流程图如图1。

系统布置：

如图2所示，用4枚放置于xoy平面和xoz平面内且与x轴成θ角的斜置陀螺完成滚转角的测量。斜置陀螺与x轴的夹角θ由x轴最大角速度ω_xmax和y轴所需陀螺量程ω_ymax以及所用陀螺的量程FS_g决定：

FS_g≥ω_xmaxcosθ+ω_ymaxsinθ

确定θ角后，可进一步确定测量矩阵H

$H=\left[\begin{array}{ccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}& 0\\ \cos \mathrm{\θ}& -\sin \mathrm{\θ}& 0\\ \cos \mathrm{\θ}& 0& \sin \mathrm{\θ}\\ \cos \mathrm{\θ}& 0& -\sin \mathrm{\θ}\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

设正交载体坐标系的测量向量为ω＝[ω_x ω_y ω_z]^T，n个惯性器件的测量输出为m＝[m₁ m₂ ... m_n]^T，则m＝Hω.

旋转体载体系角速度解算

在按计算出的θ角布置系统并标定后，使用时获取六个陀螺的测量输出m＝[m₁ m₂ ... m_n]^T。

由m＝Hω得：

$\begin{array}{c}\hat{X}\left(\mathrm{\ω}\right)={\left(H^{T}H\right)}^{-1}{H}^{T}m=\\ \left[\begin{array}{cccccc}\frac{1}{4\cos \mathrm{\θ}}& \frac{1}{4\cos \mathrm{\θ}}& \frac{1}{4\cos \mathrm{\θ}}& \frac{1}{4\cos \mathrm{\θ}}& 0& 0\\ \frac{\sin \mathrm{\θ}}{1+2{\sin }^2\mathrm{\θ}}& \frac{-\sin \mathrm{\θ}}{1+2{\sin }^2\mathrm{\θ}}& 0& 0& \frac{1}{1+2{\sin }^2\mathrm{\θ}}& 0\\ 0& 0& \frac{\sin \mathrm{\θ}}{1+2{\sin }^2\mathrm{\θ}}& \frac{-\sin \mathrm{\θ}}{1+2{\sin }^2\mathrm{\θ}}& 0& \frac{1}{1+2{\sin }^2\mathrm{\θ}}\end{array}\right]m\end{array}$

将H阵和陀螺仪测量输出m代入即可得到旋转体载体系上的角速度

旋转体姿态角解算

初始对准获得初始对准信息，即得到其初始的姿态角偏航角(ψ)，俯仰角(θ)，滚转角(φ)及初始的姿态矩阵

则姿态四元数q为：

$q=\left[\begin{array}{c}w\\ x\\ y\\ z\end{array}\right]=\left[\begin{array}{c}\cos (\mathrm{\φ}/2)\cos (\mathrm{\θ}/2)\cos (\mathrm{\ψ}/2)+\sin (\mathrm{\φ}/2)\sin (\mathrm{\θ}/2)\sin (\mathrm{\ψ}/2)\\ \sin (\mathrm{\φ}/2)\cos (\mathrm{\θ}/2)\cos (\mathrm{\ψ}/2)-\cos (\mathrm{\φ}/2)\sin (\mathrm{\θ}/2)\sin (\mathrm{\ψ}/2)\\ \cos (\mathrm{\φ}/2)\sin (\mathrm{\θ}/2)\cos (\mathrm{\ψ}/2)+\sin (\mathrm{\φ}/2)\cos (\mathrm{\θ}/2)\sin (\mathrm{\ψ}/2)\\ \cos (\mathrm{\φ}/2)\cos (\mathrm{\θ}/2)\sin (\mathrm{\ψ}/2)-\sin (\mathrm{\φ}/2)\sin (\mathrm{\θ}/2)\cos (\mathrm{\ψ}/2)\end{array}\right]$

将初始对准得到的初始偏航角、俯仰角、滚转角代入上式，即可完成四元数的初始化。

使用二阶龙格库塔法更新四元数，令其中i为x、y、z，

${\mathrm{\Ω}}_b\left(t\right)=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x\left(t\right)& {-\mathrm{\ω}}_y\left(t\right)& {-\mathrm{\ω}}_z\left(t\right)\\ {\mathrm{\ω}}_x\left(t\right)& 0& {\mathrm{\ω}}_z\left(t\right)& {-\mathrm{\ω}}_y\left(t\right)\\ {\mathrm{\ω}}_y\left(t\right)& {-\mathrm{\ω}}_z\left(t\right)& 0& {\mathrm{\ω}}_x\left(t\right)\\ {\mathrm{\ω}}_z\left(t\right)& {\mathrm{\ω}}_y\left(t\right)& {-\mathrm{\ω}}_x\left(t\right)& 0\end{array}\right]$

则 $\left\{\begin{array}{c}{K}_1={\mathrm{\Ω}}_b\left(t\right)q\left(t\right)\\ Y=q\left(t\right)+\mathrm{\Δ}{\mathrm{TK}}_1\\ K_2={\mathrm{\Ω}}_b(t+\mathrm{\ΔT})Y\\ q(t+\mathrm{\ΔT})=q\left(t\right)+(\mathrm{\ΔT}/2)*(K_1+K_2)\end{array}\right.$

计算姿态矩阵和姿态角

已知q＝[w x y z]^T，则

$C_E^b=\left[\begin{array}{ccc}{w}^2+x^2-y^2-z^2& 2(\mathrm{xy}-\mathrm{wz})& 2(\mathrm{xz}+\mathrm{wy})\\ 2(\mathrm{xy}+\mathrm{wz})& w^2+y^2-x^2-z^2& 2(\mathrm{yz}-\mathrm{wx})\\ 2(\mathrm{xz}-\mathrm{wy})& 2(\mathrm{yz}+\mathrm{wx})& w^2+z^2-x^2-y^2\end{array}\right]=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

则滚转角(φ)，俯仰角(θ)，偏航角(ψ)分别为：

$\left\{\begin{array}{c}\mathrm{\φ}=\arctan \left(\frac{T_{32}}{T_{33}}\right)\\ \mathrm{\θ}=\arcsin (-T_{31})\\ \mathrm{\ψ}=\arctan \left(\frac{T_{21}}{T_{11}}\right)\end{array}\right.$

系统布置

元件选择：

可选择单轴MEMS陀螺完成配置，也可以选择多轴MEMS陀螺中的某一轴来代替单轴陀螺完成配置。

元件布置：

以前述情况为例，如图4，x、y、z是载体系的三个正交轴，布置时令x轴与滚转轴平行，z轴和y轴分别与偏航轴和俯仰轴平行。y、z二轴分别布置陀螺y、陀螺z，另有1、2、3、4四个陀螺分别布置在yox、zox、-yox、-zox平面上，与x轴夹角θ。

与x轴的夹角θ由x轴最大角速度ω_xmax，y轴所需陀螺量程ω_ymax，和所使用陀螺的量程FS_g决定：

FS_g≥ω_xmaxcosθ+ω_ymaxsinθ

系统标定

冗余系统的标定与传统系统不同，这里只介绍系统中陀螺部分的标定。其中，六只陀螺的测量输出为g＝[g₁ g₂ g₃ g₄ g_x g_y]^T。

利用转台为陀螺提供一定角速率并采集数据进行标定。沿正交载体坐标系的三轴分别输入角速率ω₀和-ω₀，如下表：

表1陀螺标定输入在正交载体坐标系中的表示

表示第i个陀螺在第j个位置的测量误差值。则第i个陀螺的常值误差各陀螺的标度因数误差为

$\left\{\begin{array}{c}{k}_{\mathrm{si}}=\frac{{-\mathrm{\Δg}}_i^1-{\mathrm{\Δg}}_i^2+{2\mathrm{\Δg}}_i^3-{\mathrm{\Δg}}_i^4-{\mathrm{\Δg}}_i^5-{4\mathrm{\Δg}}_i^6}{{6\mathrm{\ω}}_0\sin {\mathrm{El}}_0^i}+\frac{({\mathrm{\Δg}}_i^3-{\mathrm{\Δg}}_i^6)\sin {\mathrm{El}}_0^i}{{2\mathrm{\ω}}_0}\\ \frac{({\mathrm{\Δg}}_i^1-{\mathrm{\Δg}}_i^4)\cos {\mathrm{Az}}_0^i\cos {\mathrm{El}}_0^i}{{2\mathrm{\ω}}_0}+\frac{({\mathrm{\Δg}}_i^2-{\mathrm{\Δg}}_i^5)\sin {\mathrm{Az}}_0^i\cos {\mathrm{El}}_0^i}{{2\mathrm{\ω}}_0}(i=\mathrm{1,2,3,4})\\ k_{\mathrm{sy}}=\frac{\sqrt{{({\mathrm{\Δg}}_y^1-{\mathrm{\Δg}}_y^4)}^2+{({\mathrm{\Δg}}_y^2-{\mathrm{\Δg}}_y^5)}^2+{({\mathrm{\Δg}}_y^3-{\mathrm{\Δg}}_y^6)}^2}}{{4\mathrm{\ω}}_0}-1\\ k_{\mathrm{sy}}=\frac{\sqrt{{({\mathrm{\Δg}}_z^4-{\mathrm{\Δg}}_z^1)}^2+{({\mathrm{\Δg}}_z^2-{\mathrm{\Δg}}_z^5)}^2+{({\mathrm{\Δg}}_z^3-{\mathrm{\Δg}}_z^6)}^2}}{{2\mathrm{\ω}}_0}-1\end{array}\right.$

表2各陀螺理论安装角(°)

各陀螺的安装误差(单位：°)为：

${\mathrm{\&delta;}}_{\mathrm{Ei}}^i=\left\{\begin{array}{ccc}\mathrm{\&theta;}-\arctan \frac{\sqrt{{({\mathrm{\&Delta;g}}_i^2-{\mathrm{\&Delta;g}}_i^5)}^2+{({\mathrm{\&Delta;g}}_i^1-{\mathrm{\&Delta;g}}_i^4)}^2}}{{\mathrm{\&Delta;g}}_i^3-{\mathrm{\&Delta;g}}_i^6}& ({\mathrm{\&Delta;g}}_i^1-{\mathrm{\&Delta;g}}_i^4>0)& \\ \mathrm{\&theta;}+\arctan \frac{\sqrt{{({\mathrm{\&Delta;g}}_i^2-{\mathrm{\&Delta;g}}_i^5)}^2+{({\mathrm{\&Delta;g}}_i^1-{\mathrm{\&Delta;g}}_i^4)}^2}}{{\mathrm{\&Delta;g}}_i^3-{\mathrm{\&Delta;g}}_i^6}& ({\mathrm{\&Delta;g}}_i^1-{\mathrm{\&Delta;g}}_i^4<0)& (i=\mathrm{1,2,3,4})\end{array}\right.$

${\mathrm{\&delta;}}_{\mathrm{Az}}^i=\arctan \frac{{\mathrm{\&Delta;g}}_i^2-{\mathrm{\&Delta;g}}_i^5}{{\mathrm{\&Delta;g}}_i^1-{\mathrm{\&Delta;g}}_i^4}-{\mathrm{Az}}_0^i$

${\mathrm{\&delta;}}_{\mathrm{El}}^y=\left\{\begin{array}{cc}\arctan \frac{{\mathrm{\&Delta;g}}_y^3-{\mathrm{\&Delta;g}}_y^6}{\sqrt{{({\mathrm{\&Delta;g}}_y^2-{\mathrm{\&Delta;g}}_y^5)}^2+{({\mathrm{\&Delta;g}}_y^1-{\mathrm{\&Delta;g}}_y^4)}^2}}& ({\mathrm{\&Delta;g}}_y^3-{\mathrm{\&Delta;g}}_y^6>0)\\ -\arctan \frac{{\mathrm{\&Delta;g}}_y^3-{\mathrm{\&Delta;g}}_y^6}{\sqrt{{({\mathrm{\&Delta;g}}_y^2-{\mathrm{\&Delta;g}}_y^5)}^2+{({\mathrm{\&Delta;g}}_y^1-{\mathrm{\&Delta;g}}_y^4)}^2}}& ({\mathrm{\&Delta;g}}_y^3-{\mathrm{\&Delta;g}}_y^6<0)\end{array}\right.$

${\mathrm{\&delta;}}_{\mathrm{Az}}^y=\arctan \frac{{\mathrm{\&Delta;g}}_y^2-{\mathrm{\&Delta;g}}_y^5}{{\mathrm{\&Delta;g}}_y^1-{\mathrm{\&Delta;g}}_y^4}-90$

${\mathrm{\&delta;}}_{\mathrm{El}}^z=\left\{\begin{array}{cc}90-\arctan \frac{{\mathrm{\&Delta;g}}_y^3-{\mathrm{\&Delta;g}}_y^6}{\sqrt{{({\mathrm{\&Delta;g}}_y^2-{\mathrm{\&Delta;g}}_y^5)}^2+{({\mathrm{\&Delta;g}}_y^1-{\mathrm{\&Delta;g}}_y^4)}^2}}& ({\mathrm{\&Delta;g}}_y^3-{\mathrm{\&Delta;g}}_y^6>0)\\ 90+\arctan \frac{{\mathrm{\&Delta;g}}_y^3-{\mathrm{\&Delta;g}}_y^6}{\sqrt{{({\mathrm{\&Delta;g}}_y^2-{\mathrm{\&Delta;g}}_y^5)}^2+{({\mathrm{\&Delta;g}}_y^1-{\mathrm{\&Delta;g}}_y^4)}^2}}& ({\mathrm{\&Delta;g}}_y^3-{\mathrm{\&Delta;g}}_y^6<0)\end{array}\right.$

${\mathrm{\&delta;}}_{\mathrm{Az}}^z=\arctan \frac{{\mathrm{\&Delta;g}}_y^2-{\mathrm{\&Delta;g}}_y^5}{{\mathrm{\&Delta;g}}_y^1-{\mathrm{\&Delta;g}}_y^4}$

则将求出的常值误差、标度因数误差代入

g'＝g-Δg＝g-(B+K_sg+ΔH(H^TH)^-1H^Tg)

可使常值误差、标度因数误差和安装误差得到补偿，g'即为陀螺经标定后的测量值。

旋转体载体系角速度解算

设陀螺标定后的测量值为g'＝[g'₁ g'₂ g'₃ g'₄ g'_x g'_y]^T，载体系坐标轴上的旋转角分量分别为ω_x、ω_y、ω_z，ω＝[ω_x ω_y ω_z]^T，则

$\begin{array}{c}{g}^{\&prime;}=\mathrm{H\&omega;}\\ \left[\begin{array}{c}{g}_1\\ g_2\\ g_3\\ g_4\\ g_x\\ g_y\end{array}\right]\end{array}=\left[\begin{array}{ccc}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}& 0\\ \cos \mathrm{\&theta;}& -\sin \mathrm{\&theta;}& 0\\ \cos \mathrm{\&theta;}& 0& \sin \mathrm{\&theta;}\\ \cos \mathrm{\&theta;}& 0& -\sin \mathrm{\&theta;}\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_z\end{array}\right]$

用最小二乘估计进行数据处理，经过计算可得到：

$\begin{array}{c}\hat{X}\left(\mathrm{\&omega;}\right)={\left(H^{T}H\right)}^{-1}{H}^T{g}^{\&prime;}=\\ \left[\begin{array}{cccccc}\frac{1}{4\cos \mathrm{\&theta;}}& \frac{1}{4\cos \mathrm{\&theta;}}& \frac{1}{4\cos \mathrm{\&theta;}}& \frac{1}{4\cos \mathrm{\&theta;}}& 0& 0\\ \frac{\sin \mathrm{\&theta;}}{1+2{\sin }^2\mathrm{\&theta;}}& \frac{-\sin \mathrm{\&theta;}}{1+2{\sin }^2\mathrm{\&theta;}}& 0& 0& \frac{1}{1+2{\sin }^2\mathrm{\&theta;}}& 0\\ 0& 0& \frac{\sin \mathrm{\&theta;}}{1+2{\sin }^2\mathrm{\&theta;}}& \frac{-\sin \mathrm{\&theta;}}{1+2{\sin }^2\mathrm{\&theta;}}& 0& \frac{1}{1+2{\sin }^2\mathrm{\&theta;}}\end{array}\right]g^{\&prime;}\end{array}$

使用中，将陀螺测量值Z代入上式，就得到载体坐标系上的角速率

旋转体姿态角解算

初始对准

系统对姿态和角速度的测量需要进行初始对准。可使用静基座对准得到其初始的姿态角偏航角(ψ)，俯仰角(θ)，滚转角(φ)及初始的姿态矩阵

四元数初始化

被测物体姿态可以由偏航角(ψ)，俯仰角(θ)，滚转角(φ)来表示，每种姿态对应一个四元数q，则

$q=\left[\begin{array}{c}w\\ x\\ y\\ z\end{array}\right]=\left[\begin{array}{c}\cos (\mathrm{\&phi;}/2)\cos (\mathrm{\&theta;}/2)\cos (\mathrm{\&psi;}/2)+\sin (\mathrm{\&phi;}/2)\sin (\mathrm{\&theta;}/2)\sin (\mathrm{\&psi;}/2)\\ \sin (\mathrm{\&phi;}/2)\cos (\mathrm{\&theta;}/2)\cos (\mathrm{\&psi;}/2)-\cos (\mathrm{\&phi;}/2)\sin (\mathrm{\&theta;}/2)\sin (\mathrm{\&psi;}/2)\\ \cos (\mathrm{\&phi;}/2)\sin (\mathrm{\&theta;}/2)\cos (\mathrm{\&psi;}/2)+\sin (\mathrm{\&phi;}/2)\cos (\mathrm{\&theta;}/2)\sin (\mathrm{\&psi;}/2)\\ \cos (\mathrm{\&phi;}/2)\cos (\mathrm{\&theta;}/2)\sin (\mathrm{\&psi;}/2)-\sin (\mathrm{\&phi;}/2)\sin (\mathrm{\&theta;}/2)\cos (\mathrm{\&psi;}/2)\end{array}\right]$

将初始对准得到的初始偏航角、俯仰角、滚转角代入上式，即可完成四元数的初始化。

四元数更新

使用二阶龙格库塔法更新四元数：

令其中i为x、y、z，

${\mathrm{\&Omega;}}_b\left(t\right)=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_x\left(t\right)& {-\mathrm{\&omega;}}_y\left(t\right)& {-\mathrm{\&omega;}}_z\left(t\right)\\ {\mathrm{\&omega;}}_x\left(t\right)& 0& {\mathrm{\&omega;}}_z\left(t\right)& {-\mathrm{\&omega;}}_y\left(t\right)\\ {\mathrm{\&omega;}}_y\left(t\right)& {-\mathrm{\&omega;}}_z\left(t\right)& 0& {\mathrm{\&omega;}}_x\left(t\right)\\ {\mathrm{\&omega;}}_z\left(t\right)& {\mathrm{\&omega;}}_y\left(t\right)& {-\mathrm{\&omega;}}_x\left(t\right)& 0\end{array}\right]$

则 $\left\{\begin{array}{c}{K}_1={\mathrm{\&Omega;}}_b\left(t\right)q\left(t\right)\\ Y=q\left(t\right)+\mathrm{\&Delta;}{\mathrm{TK}}_1\\ K_2={\mathrm{\&Omega;}}_b(t+\mathrm{\&Delta;T})Y\\ q(t+\mathrm{\&Delta;T})=q\left(t\right)+(\mathrm{\&Delta;T}/2)*(K_1+K_2)\end{array}\right.$

计算姿态矩阵

已知q＝[w x y z]^T，则

$C_E^b=\left[\begin{array}{ccc}{w}^2+x^2-y^2-z^2& 2(\mathrm{xy}-\mathrm{wz})& 2(\mathrm{xz}+\mathrm{wy})\\ 2(\mathrm{xy}+\mathrm{wz})& w^2+y^2-x^2-z^2& 2(\mathrm{yz}-\mathrm{wx})\\ 2(\mathrm{xz}-\mathrm{wy})& 2(\mathrm{yz}+\mathrm{wx})& w^2+z^2-x^2-y^2\end{array}\right]=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

计算姿态角

已知姿态矩阵则滚转角(φ)，俯仰角(θ)，偏航角(ψ)分别为：

$\left\{\begin{array}{c}\mathrm{\&phi;}=\arctan \left(\frac{T_{32}}{T_{33}}\right)\\ \mathrm{\&theta;}=\arcsin (-T_{31})\\ \mathrm{\&psi;}=\arctan \left(\frac{T_{21}}{T_{11}}\right)\end{array}\right.$

既得载体的姿态角。

本发明采用惯性导航方式测量高速旋转体的姿态和角速度测量时，常受到陀螺量程的限制。使用陀螺冗余斜交配制方法，可以在单轴高速旋转体的测量中，用量程较小的陀螺测量较大的角速度，提高导航系统在旋转方向上的测量范围和可靠性，同时不引入其它误差。

Claims (1)

1.一种采用陀螺冗余斜交配置方式测量高速旋转体姿态和角速度的方法，其特征在于：

(1)利用GPS确定载体的初始位置参数；

(2)采集光纤陀螺仪和石英加速度计输出的数据：

(3)根据GPS确定的经纬度值计算出对准点的重力扰动值，对加速度计的输出进行补偿；

(4)采用解析法来完成系统的粗对准，初步确定载体的姿态信息

(5)粗对准结束后建立捷联惯性导航系统初始对准非线性状态误差方程；

(6)利用UKF滤波方法进行滤波；

(7)利用估计出来的平台失准角修正系统的捷联初始矩阵得到精确的初始捷联矩阵即完成高精度的初始对准。