CN102829781A - 一种旋转式捷联光纤罗经实现的方法 - Google Patents

Abstract

本发明提供的是一种旋转式捷联光纤罗经的实现方法。步骤包括：定义坐标系，根据系统采集的光纤陀螺和石英加速度计数据等信息，完成旋转式捷联光纤罗经系统的初始对准，确定初始姿态矩阵；按照设定的单轴旋转方案进行间断型往返转动，利用四元数微分方程完成姿态更新；同时根据外界提供的信息完成航向修正。在方位仪状态，通过旋转抑制陀螺常值漂移造成的导航误差，在高纬度地区更好地跟踪载体航向。

Description

一种旋转式捷联光纤罗经实现的方法

技术领域

本发明涉及一种利用惯性传感器（光纤陀螺仪和加速度计）实现对载体航向和姿态的测量技术，并能够在高纬度地区给载体提供航向，属于导航、制导技术领域。

背景技术

旋转技术早期应用于静电陀螺系统，对保持静电陀螺长时间工作的精度十分有效。自从光学陀螺出现以后，利用旋转调制消除陀螺常值漂移对导航计算结果的影响，大大提高了系统长时间导航精度。目前，国内外主要将旋转式机构应用到激光惯导系统中去。Sperry公司的WSN-7B系统为目前应用最广泛的旋转式惯导系统。

随着光纤陀螺技术的迅速发展，捷联光纤罗经已经成为国内外的研究热点。与传统陀螺罗经相比，捷联光纤罗经具有全固态、体积小、启动快、可靠性高等优点。LITEF公司的LFK-95型光纤捷联罗经的对准时间30min，航向精度为0.7°secL，水平精度为0.5°，法国IXSEA公司的OCTANS光纤捷联罗经能够在5min内完成对准，航向精度达到0.1°secL，水平精度优于0.01°(RMS)。

因此，将旋转技术应用到捷联光纤罗经系统中，为船舶提供高精度的航向和姿态信息，具有十分重要的意义。

目前也有部分与本发明有关的研究报告，1、例如专利申请号为200910044759.9，名称为“基于激光陀螺的高精度单轴旋转姿态测量系统”。2、旋转式光学陀螺捷联惯导系统的旋转方案设计，中国惯性技术学报，2009,17(1)。

发明内容

本发明针对光纤陀螺的常值漂移会随着时间发生改变，提出一种采用旋转来抑制陀螺常值漂移对导航解算精度影响的旋转式捷联光纤罗经实现的方法，该方法投入成本较低，但可以大幅提高光纤捷联罗经系统的导航精度。本发明的目的是这样实现的：

在低纬度使用的修正状态算法包括以下步骤：

步骤1定义坐标系：导航坐标系n系以载体质心为原点，x_n、y_n、z_n分别指向所在地的东、北、天，地球坐标系e系以地心为原点，x_e轴穿越本初子午线与赤道的交点，y_e轴穿越东经90°子午线与赤道的交点，z_e轴穿越地球北极点，载体坐标系b系以载体中心为原点，x_b轴沿横轴指向右，y_b轴沿纵轴指向前，z_b轴垂直载体指向上，旋转坐标系p系以旋转台面的中心为原点，z_p轴沿转轴指向上，x_p轴和y_p轴位于旋转台面内，并和台面一起旋转，三个坐标轴构成右手坐标系，惯性坐标系i系以地心为原点，x_i轴指向春分点，z_i轴沿地球自转轴，y_i轴与x_i、z_i轴构成右手坐标系，游离坐标系T_e系，水平轴

和

相对于导航坐标系的东向轴和北向轴存在游离方位角α_f，经线地球坐标系e₀系以地球中心为原点，并与地球同步旋转，

轴在地球赤道平面内，

轴指向载体所在点经线，

轴指向地球自转轴方向，经线地心惯性坐标系i₀系定义为在粗对准起始时刻将经线地球坐标系惯性凝固成的右手坐标系，载体惯性坐标系i_b0系定义为在粗对准起始时刻将载体坐标系惯性凝固后的坐标系，计算导航坐标系c系定义为计算机输出结果确定的导航坐标系，

步骤2根据三只光纤陀螺仪的输出数据三只石英加速度计的输出数据f^p，以及地球自转角速率ω_ie、重力加速度g、载体所在地的纬度L，应用基于惯性系重力矢量的解析对准算法计算导航坐标系n系与载体坐标系b系之间的转移矩阵完成光纤捷联罗经系统初始对准，所述应用基于惯性系重力矢量的解析对准算法完成光纤捷联罗经系统初始对准的过程如下：

步骤2.1计算导航坐标系n系与经线地球坐标系e₀系之间的转移矩阵

$C_n^{e_0}=\left[\begin{array}{ccc}0& -\sin L& \cos L\\ 1& 0& 0\\ 0& \cos L& \sin L\end{array}\right]$

步骤2.2计算经线地球坐标系e₀系与经线地心惯性坐标系i₀系之间的转移矩阵

$C_{e_0}^{i_0}\left(t\right)=\left[\begin{array}{ccc}\cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& -\sin \left({\mathrm{\ω}}_{\mathrm{ie}}\right)t& 0\\ \sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& \cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& 0\\ 0& 0& 1\end{array}\right]$

t表示对准时间，ω_ie为地球自转角速率，

步骤2.3计算载体惯性坐标系与载体坐标系之间的转移矩阵

在起始时刻，载体惯性坐标系与载体坐标系重合，即

的初值为单位矩阵，根据陀螺仪输出的旋转坐标系p系相对惯性坐标系i系在旋转坐标系p系下的角速度

并通过四元数方法求解

步骤2.4计算经线地心惯性坐标系i₀系与载体惯性坐标系i_b0系之间的转移矩阵

$C_{i_0}^{i_{b0}}={\left[\begin{array}{c}{\left[V^{i_{b0}}\left(t_1\right)\right]}^T\\ {\left[V^{i_{b0}}\left(t_2\right)\right]}^T\\ {[V^{i_{b0}}\left(t_1\right)\×V^{i_{b0}}\left(t_2\right)]}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left[V^{i_0}\left(t_1\right)\right]}^T\\ {\left[V^{i_0}\left(t_2\right)\right]}^T\\ {[V^{i_0}\left(t_1\right)\×V^{i_0}\left(t_2\right)]}^T\end{array}\right]$

式中， $V^{i_0}\left(t\right)=\left[\begin{array}{c}\frac{g\cos L\sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ \frac{g\cos L[1-\cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ g\sin L\·t\end{array}\right]$

$V^{i_{b0}}\left(t\right)={\∫}_0^t{f}^{i_{b0}}\left(\mathrm{\τ}\right)\mathrm{d\τ}={\∫}_0^t\left[C_b^{i_{b0}}\left(\mathrm{\τ}\right)f^p\left(\mathrm{\τ}\right)\right]\mathrm{d\τ}={\∫}_0^t\left[{\left(C_{i_{b0}}^b\left(\mathrm{\τ}\right)\right)}^T{f}^p\left(\mathrm{\τ}\right)\right]\mathrm{d\τ}$

t₁和t₂表示对准过程中选取的两个时间点，τ表示时间参数，t₁取值1分钟，t₂取值6分钟。

步骤2.5根据式 $C_n^b\left(t\right)=C_{i_{\mathrm{bo}}}^b\left(t\right)\·C_{i_0}^{i_{\mathrm{bo}}}\·C_{e_0}^{i_0}\left(t\right)\·C_n^{e_0},$ 求出

完成初始对准，

步骤3控制电机，使与惯性测量单元IMU固连的转台旋转，首先从0°正转到180°停止，然后从180°反转到0°停止；然后从0°反转到180°停止，最后从180°正转到0°停止，这样周而复始的转动，旋转角速率为8°/s，每个位置停止时间为5分钟，每个时刻k获取的转动角度值为θ(k)，

步骤4根据k时刻三只光纤陀螺仪的输出数据和三只石英加速度计在k时刻输出数据f^p(k)，求出k时刻旋转坐标系p系相对于计算导航坐标系c系的姿态变换矩阵

再利用转动角度值θ(k)求出旋转坐标系p系相对于载体坐标系b系的姿态变换矩阵

最后通过所述的两个姿态变换矩阵，求出载体坐标系b系相对于计算导航坐标系c系的姿态变换矩阵

步骤5利用载体上辅助导航系统提供的比力信息，对姿态进行修正，并提取出载体的方位角H、纵摇角P和横摇角R；

所述的高纬度使用的方位仪状态算法的步骤如下：

步骤6根据修正状态切换成方位仪状态时刻载体所在位置的经度λ、纬度L和载体在导航坐标系n系中的水平速度V_e、V_n分别初始化方向余弦矩阵和游离坐标系中的水平速度V_x、V_y；初始游离方位角α_f设置为0，

$C_e^{T_e}=\left[\begin{array}{ccc}-\cos a_f\sin \mathrm{\λ}-\sin a_f\sin L\cos \mathrm{\λ}& \cos a_f\cos \mathrm{\λ}-\sin a_f\sin L\sin \mathrm{\λ}& \sin a_f\cos L\\ \sin a_f\sin \mathrm{\λ}-\cos a_f\sin L\cos \mathrm{\λ}& -\sin a_f\cos \mathrm{\λ}-\cos a_f\sin L\sin \mathrm{\λ}& \cos a_f\cos L\\ \cos L\cos \mathrm{\λ}& \cos L\sin \mathrm{\λ}& \sin L\end{array}\right]$

V_x＝V_e V_y＝V_n

步骤7控制电机，使与惯性测量单元IMU固连的转台旋转，首先从0°正转到180°停止，然后从180°反转到0°停止；然后从0°反转到180°停止，最后从180°正转到0°停止。这样周而复始的转动，旋转角速率为8°/s，每个位置停止时间为5分钟，每个时刻r获取的转动角度值为θ(r)，

步骤8根据r时刻三只光纤陀螺仪的输出数据

以及游离方位角α_f，求取旋转坐标系p系相对于游离坐标系T_e系的姿态变换矩阵再利用r时刻转动角度值θ(r)求出旋转坐标系p系相对于载体坐标系b系的姿态变换矩阵

最后通过所述的两个姿态变换矩阵，求出载体坐标系b系相对于游离坐标系T_e系的姿态矩阵

以及根据r时刻三只加速度计输出的数据f^p(r)，计算出游离坐标系T_e系下载体的水平速度V_x、V_y，最后提取出载体的航机角Ψ_Tb，

步骤9利用求得游离坐标系T_e系下载体的水平速度V_x、V_y，确定出游离坐标系T_e系下的载体位置速率

为游离坐标系T_e系相对地球坐标系e系在游离坐标系T_e系下的角速度，然后根据微分方程

求出方向余弦矩阵

的值，并提取出游离方位角α_f，

步骤10利用提取出的游离方位角α_f，得到载体的航向角H，H＝Ψ_Tb-α_f。

1、根据权利要求1所述一种旋转式捷联光纤罗经的实现方法，其特征在于：所述步骤4中载体坐标系b系相对于计算导航坐标系c系的姿态变换矩阵

具体解算过程包括：

步骤4.1根据k时刻获取的三只光纤陀螺仪的输出数据

和三个加速度计的输出数据f^p，用四元数法对旋转坐标系p系相对于计算导航坐标系c系的姿态变换矩阵

进行更新：

$\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{cpx}}^p\\ {\mathrm{\ω}}_{\mathrm{cpy}}^p\\ {\mathrm{\ω}}_{\mathrm{cpz}}^p\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ipx}}^p\\ {\mathrm{\ω}}_{\mathrm{ipy}}^p\\ {\mathrm{\ω}}_{\mathrm{ipz}}^p\end{array}\right]-{\left(C_p^c\left(k\right)\right)}^T\left[\begin{array}{c}-\frac{V_N\left(k\right)}{R_M}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\left(k\right)+\frac{V_E\left(k\right)}{R_N}\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\left(k\right)+\frac{V_E\left(k\right)}{R_N}\tan L\left(k\right)\end{array}\right]$

其中，

分别为三只光纤陀螺仪在ox_p、oy_p、oz_p轴上采集到的数据，即为旋转坐标系p系相对于惯性坐标系i系的角速率在旋转坐标系p系下三个轴向上的分量，

为旋转坐标系p系相对于计算导航坐标系c系的角速率在旋转坐标系p系下三个轴向上的分量，

步骤4.2更新四元数和姿态矩阵：

设旋转坐标系p系相对于计算导航坐标系c系的转动四元数为：

Q＝q₀+q₁w_p+q₂j_p+q₃h_p

其中：q₀、q₁、q₂、q₃是实数，w_p、j_p、h_p分别表示旋转坐标系p系ox_p轴、oy_p轴、oz_p轴上的单位方向向量；

四元数初始化为：

四元数的初始值Q(0)由初始对准确定：设初始对准得到的姿态矩阵为

其中 $C_b^c={\left(C_n^b\right)}^T,$

$C_b^c=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

则四元数q₀、q₁、q₂、q₃的表达式如下：

$\left\{\begin{array}{c}\left|q_0\right|=\frac{1}{2}\sqrt{1+C_{11}+C_{22}+C_{33}}\\ \left|q_1\right|=\frac{1}{2}\sqrt{1+C_{11}-C_{22}-C_{33}}\\ \left|q_2\right|=\frac{1}{2}\sqrt{1-C_{11}+C_{22}-C_{33}}\\ \left|q_3\right|=\frac{1}{2}\sqrt{1-C_{11}-C_{22}+C_{33}}\end{array}\right.$

q₀、q₁、q₂、q₃的符号可按下式确定：

$\left\{\begin{array}{c}\mathrm{sign}\left(q_1\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(C_{32}-C_{23})\right]\\ \mathrm{sign}\left(q_2\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(C_{13}-C_{31})\right]\\ \mathrm{sign}\left(q_3\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(C_{21}-C_{12})\right]\end{array}\right.$

其中，sign(q₀)可以任选，

利用四元数微分方程

修正四元数q₀、q₁、q₂、q₃：

$\left[\begin{array}{c}{q}_0(k+1)\\ q_1(k+1)\\ q_2(k+1)\\ q_3(k+1)\end{array}\right]=\left[\begin{array}{c}{q}_0\left(k\right)\\ q_1\left(k\right)\\ q_2\left(k\right)\\ q_3\left(k\right)\end{array}\right]+\frac{T_s}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{cpx}}^0& -{\mathrm{\ω}}_{\mathrm{cpy}}^p& -{\mathrm{\ω}}_{\mathrm{cpz}}^p\\ {\mathrm{\ω}}_{\mathrm{cpx}}^p& 0& {\mathrm{\ω}}_{\mathrm{cpz}}^p& -{\mathrm{\ω}}_{\mathrm{cpy}}^p\\ {\mathrm{\ω}}_{\mathrm{cpy}}^p& -{\mathrm{\ω}}_{\mathrm{cpz}}^p& 0& {\mathrm{\ω}}_{\mathrm{cpx}}^p\\ {\mathrm{\ω}}_{\mathrm{cpz}}^p& {\mathrm{\ω}}_{\mathrm{cpy}}^p& -{\mathrm{\ω}}_{\mathrm{cpx}}^p& 0\end{array}\right]\left[\begin{array}{c}{q}_0\left(k\right)\\ q_1\left(k\right)\\ q_2\left(k\right)\\ q_3\left(k\right)\end{array}\right],$

Ts是采样周期，取值为10ms，则k+1时刻姿态矩阵

更新过程如下：

$C_p^c(k+1)=\left[\begin{array}{ccc}{q}_0^2(k+1)+q_1^2(k+1)-q_2^2(k+1)-q_3^2(k+1)& 2(q_0(k+1)q_2(k+1)-q_0(k+1)q_3(k+1))& 2(q_1(k+1)q_3(k+1)+q_0(k+1)q_2(k+1))\\ 2(q_0(k+1)q_2(k+1)+q_0(k+1)q_3(k+1))& q_0^2(k+1)-q_1^2(k+1)+q_2^2(k+1)-q_3^2(k+1)& 2(q_2(k+1)q_3(k+1)-q_0(k+1)q_1(k+1))\\ 2(q_1(k+1)q_3(k+1)-q_0(k+1)q_2(k+1))& 2(q_2(k+1)q_3(k+1)+q_0(k+1)q_1(k+1))& q_0^2(k+1)-q_1^2(k+1)-q_2^2(k+1)+q_3^2(k+1)\end{array}\right]$

步骤4.3再根据k+1时刻角度值θ(k+1)求出旋转坐标系p系相对于载体坐标系b系的姿态变换矩阵

最后通过所述的两个姿态变换矩阵，求出k+1时刻，载体坐标系b系相对于计算导航坐标系c系的姿态矩阵

则和

分别为：

$c_p^b(k+1)=\left[\begin{array}{ccc}\cos \mathrm{\θ}(k+1)& -\sin \mathrm{\θ}(k+1)& 0\\ \sin \mathrm{\θ}(k+1)& \cos \mathrm{\θ}(k+1)& 0\\ 0& 0& 1\end{array}\right]$

$C_b^c(k+1)=C_p^c(k+1){\left(C_p^b(k+1)\right)}^T$

2、根据权利要求1所述一种旋转式捷联光纤罗经的实现方法，其特征在于：所述步骤5中姿态校正的计算过程包括：

建立以东向失准角φ_e、北向失准角φ_n、天向失准角φ_u为状态，以东向比力信息f_e和北向比力信息f_n为量测的旋转式捷联光纤罗经系统卡尔曼滤波模型，

系统状态向量为X＝[φ_e,φ_n,φ_u]^T，系统矩阵F为：

$F=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ -{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{isnL}& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\end{array}\right]$

系泊情况下，由于忽略了晃动引起的干扰加速度，导航坐标系下的东向比力和北向比力为零，则加速度计的输出在导航坐标系下投影的水平分量即为与失准角耦合的信息，系统量测Y为：

$Y=\left[\begin{array}{c}\frac{\underset{1}{\overset{N}{\mathrm{\Σ}}}{f}_e}{N}\\ \frac{\underset{1}{\overset{N}{\mathrm{\Σ}}}{f}_n}{N}\end{array}\right]$

其中，f_e和f_n为由加速度计测得的比力信息在导航坐标系下的投影，N为滤波周期的采样次数，N=500，

量测矩阵H为：

$H=\left[\begin{array}{ccc}0& g& 0\\ -g& 0& 0\end{array}\right]$

式中，g为当地的重力加速度值，g＝9.8m/s²，

利用卡尔曼滤波模型估计得到由东向失准角φ_e、北向失准角φ_n、天向失准角φ_u构成的闭环修正姿态矩阵然后通过修正后的

提取方位角H、纵摇角P和横摇角R，

$C_b^n=C_c^n\·C_b^c=\left[\begin{array}{ccc}1& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 1& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 1\end{array}\right]\·C_b^c$

设

其中c₁₁,c₁₂,c₁₃,c₂₁,c₂₂,c₂₃,c₃₁,c₃₂,c₃₃为姿态矩阵的值，则载体的方位角H、纵摇角P和横摇角R，提取公式如下：

$H=\arctan \frac{c_{12}}{c_{11}}$

$R=\arctan \frac{-c_{13}}{\sqrt{{c_{11}}^2+{c_{12}}^2}}$

$P=\arctan \frac{c_{23}}{c_{33}}.$

3、根据权利要求1所述的旋转式捷联光纤罗经的实现方法，其特征在于，所述步骤8中载体的航机角Ψ_Tb的提取过程如下：

步骤8.1根据r时刻获取的三只光纤陀螺仪的输出数据

以及游离方位角α_f，通过四元数法对旋转坐标系p系相对于游离坐标系T_e系的姿态变换矩阵进行更新：

$\left[\begin{array}{c}{\mathrm{\ω}}_{T_e\mathrm{px}}^p\\ {\mathrm{\ω}}_{T_e\mathrm{py}}^p\\ {\mathrm{\ω}}_{T_e\mathrm{pz}}^p\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ipx}}^p\\ {\mathrm{\ω}}_{\mathrm{ipy}}^p\\ {\mathrm{\ω}}_{\mathrm{ipz}}^p\end{array}\right]-{\left(C_p^{T_e}\left(r\right)\right)}^T\left[\begin{array}{c}-\frac{V_N\left(r\right)}{R_M}\cos {\mathrm{\α}}_f\left(r\right)+({\mathrm{\ω}}_{\mathrm{ie}}\cos L\left(r\right)+\frac{V_E\left(r\right)}{R_N})\sin {\mathrm{\α}}_f\left(r\right)\\ \frac{V_N\left(r\right)}{R_M}\sin {\mathrm{\α}}_f\left(r\right)+({\mathrm{\ω}}_{\mathrm{ie}}\cos L\left(r\right)+\frac{V_E\left(r\right)}{R_N})\cos {\mathrm{\α}}_f\left(r\right)\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\left(r\right)\end{array}\right]$

其中：分别为三只光纤陀螺仪在ox_p、oy_p、oz_p轴上采集到的数据，即为旋转坐标系p系相对于惯性坐标系i系的角速率在旋转坐标系p系下三个轴向上的分量，

为旋转坐标系p系相对于游离坐标系T_e系的角速率在旋转坐标系p系下三个轴向上的分量，

步骤8.2更新四元数和姿态矩阵：

设旋转坐标系p系相对于游离坐标系T_e系的转动四元数为：

Q＝q₄+q₅w_p+q₆j_p+q₇h_p

其中：q₄、q₅、q₆、q₇是实数，w_p、j_p、h_p分别表示旋转坐标系p系ox_p轴、oy_p轴、oz_p轴上的单位方向向量；

四元数修正通过解四元数微分方程

来实现：

$\left[\begin{array}{c}{q}_4(r+1)\\ q_5(r+1)\\ q_6(r+1)\\ q_7(r+1)\end{array}\right]=\left[\begin{array}{c}{q}_4\left(r\right)\\ q_5\left(r\right)\\ q_6\left(r\right)\\ q_7\left(r\right)\end{array}\right]+\frac{T_s}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{T_e\mathrm{px}}^p& -{\mathrm{\ω}}_{T_e\mathrm{py}}^p& -{\mathrm{\ω}}_{T_e\mathrm{pz}}^p\\ {\mathrm{\ω}}_{T_e\mathrm{px}}^p& 0& {\mathrm{\ω}}_{T_e\mathrm{pz}}^p& -{\mathrm{\ω}}_{T_e\mathrm{py}}^p\\ {\mathrm{\ω}}_{T_e\mathrm{py}}^p& -{\mathrm{\ω}}_{T_e\mathrm{pz}}^p& 0& {\mathrm{\ω}}_{T_e\mathrm{px}}^p\\ {\mathrm{\ω}}_{T_e\mathrm{pz}}^p& {\mathrm{\ω}}_{T_e\mathrm{py}}^p& -{\mathrm{\ω}}_{T_e\mathrm{px}}^p& 0\end{array}\right]\left[\begin{array}{c}{q}_4\left(r\right)\\ q_5\left(r\right)\\ q_6\left(r\right)\\ q_7\left(r\right)\end{array}\right]$

Ts是采样周期，取值为10ms，，r+1时刻的姿态矩阵

更新过程如下：

$C_p^T(r+1)=\left[\begin{array}{ccc}{q}_4^2(r+1)+q_5^2(r+1)-q_6^2(r+1)-q_7^2(r+1)& 2(q_4(r+1)q_6(r+1)-q_4(r+1)q_7(r+1))& 2(q_5(r+1)q_7(r+1)+q_4(r+1)q_6(r+1))\\ 2(q_4(r+1)q_6(r+1)+q_4(r+1)q_7(r+1))& q_4^2(r+1)-q_5^2(r+1)+q_6^2(r+1)-q_7^2(r+1)& 2(q_5(r+1)q_7(r+1)-q_4(r+1)q_5(r+1))\\ 2(q_5(r+1)q_7(r+1)-q_4(r+1)q_6(r+1))& 2(q_6(r+1)q_7(r+1)+q_4(r+1)q_5(r+1))& q_4^2(r+1)-q_5^2(r+1)-q_6^2(r+1)+q_7^2(r+1)\end{array}\right]$

步骤8.3再利用r+1时刻角度值θ(r+1)求出旋转坐标系p系相对于载体坐标系b系的姿态变换矩阵

最后通过所述的两个姿态变换矩阵，求出载体坐标系b系相对于游离坐标系T_e系的姿态矩阵

则

和

分别为：

$C_p^b(r+1)=\left[\begin{array}{ccc}\cos \mathrm{\θ}(r+1)& -\sin \mathrm{\θ}(r+1)& 0\\ \sin \mathrm{\θ}(r+1)& \cos \mathrm{\θ}(r+1)& 0\\ 0& 0& 1\end{array}\right]$

$C_b^{T_e}(r+1)=C_p^{T_e}(r+1){\left(C_p^b(r+1)\right)}^T$

设 $C_b^{T_e}(r+1)=\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],$ 其中t₁₁,t₁₂,t₁₃,t₂₁,t₂₂,t₂₃,t₃₁,t₃₂,t₃₃为姿态矩阵的值，载体的航机角Ψ_Tb提取公式为：

${\mathrm{\Ψ}}_{\mathrm{Tb}}=\arctan \frac{t_{12}}{t_{11}}$

由r+1时刻获取的加速度计输出的数据f^p对游离系下的速度V_x(r)、V_y(r)更新为：

$V_x(r+1)=V_x\left(r\right)+(C_p^T{f}^{\mathrm{px}}+(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x\left(r\right)}{R_N}\tan L)V_y\left(r\right))\·T_s$

$V_y(r+1)=V_y\left(r\right)+(C_p^T{f}^{\mathrm{py}}-(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x\left(r\right)}{R_N}\tan L)V_x\left(r\right))\·T_s$

其中：ω_ie为地球自转角速率；R_N为沿卯酉圈曲率半径。

4、根据权利要求1所述的旋转式捷联光纤罗经的实现方法，其特征在于，所述步骤9中方向余弦计算过程包括：

设 $C_e^{T_e}=\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],$ 其中T₁₁,T₁₂,T₁₃,T₂₁,T₂₂,T₂₃,T₃₁,T₃₂,T₃₃为姿态矩阵的值，根据r时刻计算得到的V_x(r)和V_y(r)，来更新

在x轴向分量

和在y轴向分量

${\mathrm{\ω}}_{{\mathrm{eT}}_{e}x}^{T_e}\left(r\right)=-\frac{2\×1/298.257}{R_e}{T}_{13}{T}_{23}{V}_x\left(r\right)-\frac{1}{R_e}(1-1/298.257\×T_{33}^2+2\×1/298.257\×T_{23}^2)V_y\left(r\right)$

${\mathrm{\ω}}_{{\mathrm{eT}}_{e}y}^{T_e}\left(r\right)=\frac{2\×1/298.257}{R_e}{T}_{13}{T}_{23}{V}_y\left(r\right)+\frac{1}{R_e}(1-1/298.257\×T_{33}^2+2\×1/298.257\×T_{13}^2)V_x\left(r\right)$

其中：R_e为地球半径；

然后求解微分方程得出方向余弦矩阵

的值，则游离方位角α_f为： ${\mathrm{\α}}_f=\arctan \frac{C_{13}}{C_{23}}.$

对本发明有益的效果说明如下：

在VC仿真条件下，对该方法进行仿真实验：

载体做三轴摇摆运动，其数学模型为：

$\left\{\begin{array}{c}H=H_0+H_m\sin (2\mathrm{\π}/T_H+{\mathrm{\φ}}_H)\\ P=P_0+P_m\sin (2\mathrm{\π}/T_P+{\mathrm{\φ}}_P)\\ R=R_0+R_m\sin (2\mathrm{\π}/T_R+{\mathrm{\φ}}_R)\end{array}\right.$

其中：H、P、R分别表示载体的航向角、纵摇角和横摇角；H_m、P_m、R_m分别表示相应的摇摆幅值；T_H、T_P、T_R分别表示相应的摇摆周期；φ_H、φ_P、φ_R分别表示相应的初始相位；H₀、P₀、R₀分别为初始角度值；仿真时取：H_m=5°，P_m=10°，R_m=20°，T_H=6s，T_P=10s，T_R=8s，H₀=45°，P₀＝R₀=0°。

载体初始位置：北纬39.1°，东经117.2°，方位仪仿真时纬度为70°；

陀螺常值漂移和随机漂移均为：0.01°/h；

加速度计零偏和随机偏置均为：0.1mg；

利用发明所述方法得到载体的姿态误差曲线、及方位仪状态下(航向角摇摆幅值为0)的航向曲线，分别如图6、图7所示。结果表明在海况比较恶劣的条件下(如5级海况条件下)，采用本发明方法可以获得较高的航向精度，航向角H的误差能控制在最大0.2°范围内，纵摇角P的误差能控制在最大0.06°范围内，横摇角R的误差能控制在最大0.06°范围内。

附图说明

图1为旋转式捷联光纤罗经系统示意图；

图2为旋转式捷联光纤罗经系统惯性测量单元结构示意图；

图3为本发明的流程框图；

图4为本发明的转位方案示意图；

图5为本发明姿态校正算法流程图；

图6为本发明的罗经状态下姿态解算误差曲线；

图7为本发明的方位仪状态下航向跟踪曲线。

具体实施方式

下面举例对本发明做详尽描述：

在低纬度使用的修正状态算法包括以下步骤：

步骤1定义坐标系：导航坐标系n系以载体质心为原点，x_n、y_n、z_n分别指向所在地的东、北、天，地球坐标系e系以地心为原点，x_e轴穿越本初子午线与赤道的交点，y_e轴穿越东经90°子午线与赤道的交点，z_e轴穿越地球北极点，载体坐标系b系以载体中心为原点，x_b轴沿横轴指向右，y_b轴沿纵轴指向前，z_b轴垂直载体指向上，旋转坐标系p系以旋转台面的中心为原点，z_p轴沿转轴指向上，x_p轴和y_p轴位于旋转台面内，并和台面一起旋转，三个坐标轴构成右手坐标系，惯性坐标系i系以地心为原点，x_i轴指向春分点，z_i轴沿地球自转轴，y_i轴与x_i、z_i轴构成右手坐标系，游离坐标系T_e系，水平轴

和

相对于导航坐标系的东向轴和北向轴存在游离方位角α_f，经线地球坐标系e₀系以地球中心为原点，并与地球同步旋转，

轴在地球赤道平面内，

轴指向载体所在点经线，

轴指向地球自转轴方向，经线地心惯性坐标系i₀系定义为在粗对准起始时刻将经线地球坐标系惯性凝固成的右手坐标系，载体惯性坐标系i_b0系定义为在粗对准起始时刻将载体坐标系惯性凝固后的坐标系，计算导航坐标系c系定义为计算机输出结果确定的导航坐标系，

步骤2根据三只光纤陀螺仪的输出数据

三只石英加速度计的输出数据f^p，以及地球自转角速率ω_ie、重力加速度g、载体所在地的纬度L，应用基于惯性系重力矢量的解析对准算法计算导航坐标系n系与载体坐标系b系之间的转移矩阵

完成光纤捷联罗经系统初始对准，所述应用基于惯性系重力矢量的解析对准算法完成光纤捷联罗经系统初始对准的过程如下：

步骤2.1计算导航坐标系n系与经线地球坐标系e₀系之间的转移矩阵

$C_n^{e_0}=\left[\begin{array}{ccc}0& -\sin L& \cos L\\ 1& 0& 0\\ 0& \cos L& \sin L\end{array}\right]$

步骤2.2计算经线地球坐标系e₀系与经线地心惯性坐标系i₀系之间的转移矩阵

$C_{e_0}^{i_0}\left(t\right)=\left[\begin{array}{ccc}\cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& -\sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& 0\\ \sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& \cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& 0\\ 0& 0& 1\end{array}\right]$

t表示对准时间，ω_ie为地球自转角速率，

步骤2.3计算载体惯性坐标系与载体坐标系之间的转移矩阵

在起始时刻，载体惯性坐标系与载体坐标系重合，即

的初值为单位矩阵，根据陀螺仪输出的旋转坐标系p系相对惯性坐标系i系在旋转坐标系p系下的角速度

并通过四元数方法求解

步骤2.4计算经线地心惯性坐标系i₀系与载体惯性坐标系i_b0系之间的转移矩阵

$C_{i_0}^{i_{b0}}={\left[\begin{array}{c}{\left[V^{i_{b0}}\left(t_1\right)\right]}^T\\ {\left[V^{i_{b0}}\left(t_2\right)\right]}^T\\ {[V^{i_{b0}}\left(t_1\right)\×V^{i_{b0}}\left(t_2\right)]}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left[V^{i_0}\left(t_1\right)\right]}^T\\ {\left[V^{i_0}\left(t_2\right)\right]}^T\\ {[V^{i_0}\left(t_1\right)\×V^{i_0}\left(t_2\right)]}^T\end{array}\right]$

式中， $V^{i_0}\left(t\right)=\left[\begin{array}{c}\frac{g\cos L\sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ \frac{g\cos L[1-\cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ g\sin L\·t\end{array}\right]$

$V^{i_{b0}}\left(t\right)={\∫}_0^t{f}^{i_{b0}}\left(\mathrm{\τ}\right)\mathrm{d\τ}={\∫}_0^t\left[C_b^{i_{b0}}\left(\mathrm{\τ}\right)f^p\left(\mathrm{\τ}\right)\right]\mathrm{d\τ}={\∫}_0^t\left[{\left(C_{i_{b0}}^b\left(\mathrm{\τ}\right)\right)}^T{f}^p\left(\mathrm{\τ}\right)\right]\mathrm{d\τ}$

t₁和t₂表示对准过程中选取的两个时间点，τ表示时间参数，t₁取值1分钟，t₂取值6分钟。

步骤2.5根据式 $C_n^b\left(t\right)=C_{i_{\mathrm{bo}}}^b\left(t\right)\·C_{i_0}^{i_{\mathrm{bo}}}\·C_{e_0}^{i_0}\left(t\right)\·C_n^{e_0},$ 求出

完成初始对准，

步骤3控制电机，使与惯性测量单元IMU固连的转台旋转，首先从0°正转到180°停止，然后从180°反转到0°停止；然后从0°反转到180°停止，最后从180°正转到0°停止，这样周而复始的转动，旋转角速率为8°/s，每个位置停止时间为5分钟，每个时刻k获取的转动角度值为θ(k)，

步骤4根据k时刻三只光纤陀螺仪的输出数据

和三只石英加速度计在k时刻输出数据f^p(k)，求出k时刻旋转坐标系p系相对于计算导航坐标系c系的姿态变换矩阵

再利用转动角度值θ(k)求出旋转坐标系p系相对于载体坐标系b系的姿态变换矩阵

最后通过所述的两个姿态变换矩阵，求出载体坐标系b系相对于计算导航坐标系c系的姿态变换矩阵

步骤5利用载体上辅助导航系统提供的比力信息，对姿态进行修正，并提取出载体的方位角H、纵摇角P和横摇角R；

所述的高纬度使用的方位仪状态算法的步骤如下：

步骤6根据修正状态切换成方位仪状态时刻载体所在位置的经度λ、纬度L和载体在导航坐标系n系中的水平速度V_e、V_n分别初始化方向余弦矩阵和游离坐标系中的水平速度V_x、V_y；初始游离方位角α_f设置为0，

$C_e^{T_e}=\left[\begin{array}{ccc}-\cos a_f\sin \mathrm{\λ}-\sin a_f\sin L\cos \mathrm{\λ}& \cos a_f\cos \mathrm{\λ}-\sin a_f\sin L\sin \mathrm{\λ}& \sin a_f\cos L\\ \sin a_f\sin \mathrm{\λ}-\cos a_f\sin L\cos \mathrm{\λ}& -\sin a_f\cos \mathrm{\λ}-\cos a_f\sin L\sin \mathrm{\λ}& \cos a_f\cos L\\ \cos L\cos \mathrm{\λ}& \cos L\sin \mathrm{\λ}& \sin L\end{array}\right]$

V_x＝V_e V_y＝V_n

步骤7控制电机，使与惯性测量单元IMU固连的转台旋转，首先从0°正转到180°停止，然后从180°反转到0°停止；然后从0°反转到180°停止，最后从180°正转到0°停止。这样周而复始的转动，旋转角速率为8°/s，每个位置停止时间为5分钟，每个时刻r获取的转动角度值为θ(r)，

步骤8根据r时刻三只光纤陀螺仪的输出数据

以及游离方位角α_f，求取旋转坐标系p系相对于游离坐标系T_e系的姿态变换矩阵

再利用r时刻转动角度值θ(r)求出旋转坐标系p系相对于载体坐标系b系的姿态变换矩阵

最后通过所述的两个姿态变换矩阵，求出载体坐标系b系相对于游离坐标系T_e系的姿态矩阵以及根据r时刻三只加速度计输出的数据f^p(r)，计算出游离坐标系T_e系下载体的水平速度V_x、V_y，最后提取出载体的航机角Ψ_Tb，

步骤9利用求得游离坐标系T_e系下载体的水平速度V_x、V_y，确定出游离坐标系T_e系下的载体位置速率

为游离坐标系T_e系相对地球坐标系e系在游离坐标系T_e系下的角速度，然后根据微分方程求出方向余弦矩阵

的值，并提取出游离方位角α_f，

步骤10利用提取出的游离方位角α_f，得到载体的航向角H，H＝Ψ_Tb-α_f。

1、根据权利要求1所述一种旋转式捷联光纤罗经的实现方法，其特征在于：所述步骤4中载体坐标系b系相对于计算导航坐标系c系的姿态变换矩阵

具体解算过程包括：

步骤4.1根据k时刻获取的三只光纤陀螺仪的输出数据

和三个加速度计的输出数据f^p，用四元数法对旋转坐标系p系相对于计算导航坐标系c系的姿态变换矩阵

进行更新：

$\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{cpx}}^p\\ {\mathrm{\ω}}_{\mathrm{cpy}}^p\\ {\mathrm{\ω}}_{\mathrm{cpz}}^p\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ipx}}^p\\ {\mathrm{\ω}}_{\mathrm{ipy}}^p\\ {\mathrm{\ω}}_{\mathrm{ipz}}^p\end{array}\right]-{\left(C_p^c\left(k\right)\right)}^T\left[\begin{array}{c}-\frac{V_N\left(k\right)}{R_M}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\left(k\right)+\frac{V_E\left(k\right)}{R_N}\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\left(k\right)+\frac{V_E\left(k\right)}{R_N}\tan L\left(k\right)\end{array}\right]$

其中，分别为三只光纤陀螺仪在ox_p、oy_p、oz_p轴上采集到的数据，即为旋转坐标系p系相对于惯性坐标系i系的角速率在旋转坐标系p系下三个轴向上的分量，

为旋转坐标系p系相对于计算导航坐标系c系的角速率在旋转坐标系p系下三个轴向上的分量，

步骤4.2更新四元数和姿态矩阵：

设旋转坐标系p系相对于计算导航坐标系c系的转动四元数为：

Q＝q₀+q₁w_p+q₂j_p+q₃h_p

其中：q₀、q₁、q₂、q₃是实数，w_p、j_p、h_p分别表示旋转坐标系p系ox_p轴、oy_p轴、oz_p轴上的单位方向向量；

四元数初始化为：

四元数的初始值Q(0)由初始对准确定：设初始对准得到的姿态矩阵为

其中 $C_b^c={\left(C_n^b\right)}^T,$

$C_b^c=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

则四元数q₀、q₁、q₂、q₃的表达式如下：

$\left\{\begin{array}{c}\left|q_0\right|=\frac{1}{2}\sqrt{1+C_{11}+C_{22}+C_{33}}\\ \left|q_1\right|=\frac{1}{2}\sqrt{1+C_{11}-C_{22}-C_{33}}\\ \left|q_2\right|=\frac{1}{2}\sqrt{1-C_{11}+C_{22}-C_{33}}\\ \left|q_3\right|=\frac{1}{2}\sqrt{1-C_{11}-C_{22}+C_{33}}\end{array}\right.$

q₀、q₁、q₂、q₃的符号可按下式确定：

$\left\{\begin{array}{c}\mathrm{sign}\left(q_1\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(C_{32}-C_{23})\right]\\ \mathrm{sign}\left(q_2\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(C_{13}-C_{31})\right]\\ \mathrm{sign}\left(q_3\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(C_{21}-C_{12})\right]\end{array}\right.$

其中，sign(q₀)可以任选，

利用四元数微分方程

修正四元数q₀、q₁、q₂、q₃：

$\left[\begin{array}{c}{q}_0(k+1)\\ q_1(k+1)\\ q_2(k+1)\\ q_3(k+1)\end{array}\right]=\left[\begin{array}{c}{q}_0\left(k\right)\\ q_1\left(k\right)\\ q_2\left(k\right)\\ q_3\left(k\right)\end{array}\right]+\frac{T_s}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{cpx}}^0& -{\mathrm{\ω}}_{\mathrm{cpy}}^p& -{\mathrm{\ω}}_{\mathrm{cpz}}^p\\ {\mathrm{\ω}}_{\mathrm{cpx}}^p& 0& {\mathrm{\ω}}_{\mathrm{cpz}}^p& -{\mathrm{\ω}}_{\mathrm{cpy}}^p\\ {\mathrm{\ω}}_{\mathrm{cpy}}^p& -{\mathrm{\ω}}_{\mathrm{cpz}}^p& 0& {\mathrm{\ω}}_{\mathrm{cpx}}^p\\ {\mathrm{\ω}}_{\mathrm{cpz}}^p& {\mathrm{\ω}}_{\mathrm{cpy}}^p& -{\mathrm{\ω}}_{\mathrm{cpx}}^p& 0\end{array}\right]\left[\begin{array}{c}{q}_0\left(k\right)\\ q_1\left(k\right)\\ q_2\left(k\right)\\ q_3\left(k\right)\end{array}\right],$

Ts是采样周期，取值为10ms，则k+1时刻姿态矩阵

更新过程如下：

$C_p^c(k+1)=\left[\begin{array}{ccc}{q}_0^2(k+1)+q_1^2(k+1)-q_2^2(k+1)-q_3^2(k+1)& 2(q_0(k+1)q_2(k+1)-q_0(k+1)q_3(k+1))& 2(q_1(k+1)q_3(k+1)+q_0(k+1)q_2(k+1))\\ 2(q_0(k+1)q_2(k+1)+q_0(k+1)q_3(k+1))& q_0^2(k+1)-q_1^2(k+1)+q_2^2(k+1)-q_3^2(k+1)& 2(q_2(k+1)q_3(k+1)-q_0(k+1)q_1(k+1))\\ 2(q_1(k+1)q_3(k+1)-q_0(k+1)q_2(k+1))& 2(q_2(k+1)q_3(k+1)+q_0(k+1)q_1(k+1))& q_0^2(k+1)-q_1^2(k+1)-q_2^2(k+1)+q_3^2(k+1)\end{array}\right]$

步骤4.3再根据k+1时刻角度值θ(k+1)求出旋转坐标系p系相对于载体坐标系b系的姿态变换矩阵

最后通过所述的两个姿态变换矩阵，求出k+1时刻，载体坐标系b系相对于计算导航坐标系c系的姿态矩阵则

和

分别为：

$c_p^b(k+1)=\left[\begin{array}{ccc}\cos \mathrm{\θ}(k+1)& -\sin \mathrm{\θ}(k+1)& 0\\ \sin \mathrm{\θ}(k+1)& \cos \mathrm{\θ}(k+1)& 0\\ 0& 0& 1\end{array}\right]$

$C_b^c(k+1)=C_p^c(k+1){\left(C_p^b(k+1)\right)}^T$

2、根据权利要求1所述一种旋转式捷联光纤罗经的实现方法，其特征在于：所述步骤5中姿态校正的计算过程包括：

建立以东向失准角φ_e、北向失准角φ_n、天向失准角φ_u为状态，以东向比力信息f_e和北向比力信息f_n为量测的旋转式捷联光纤罗经系统卡尔曼滤波模型，

系统状态向量为X＝[φ_e,φ_n,φ_u]^T，系统矩阵F为：

$F=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ -{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{isnL}& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\end{array}\right]$

系泊情况下，由于忽略了晃动引起的干扰加速度，导航坐标系下的东向比力和北向比力为零，则加速度计的输出在导航坐标系下投影的水平分量即为与失准角耦合的信息，系统量测Y为：

$Y=\left[\begin{array}{c}\frac{\underset{1}{\overset{N}{\mathrm{\Σ}}}{f}_e}{N}\\ \frac{\underset{1}{\overset{N}{\mathrm{\Σ}}}{f}_n}{N}\end{array}\right]$

其中，f_e和f_n为由加速度计测得的比力信息在导航坐标系下的投影，N为滤波周期的采样次数，N=500，

量测矩阵H为：

$H=\left[\begin{array}{ccc}0& g& 0\\ -g& 0& 0\end{array}\right]$

式中，g为当地的重力加速度值，g＝9.8m/s²，

利用卡尔曼滤波模型估计得到由东向失准角φ_e、北向失准角φ_n、天向失准角φ_u构成的闭环修正姿态矩阵

然后通过修正后的

提取方位角H、纵摇角P和横摇角R，

$C_b^n=C_c^n\·C_b^c=\left[\begin{array}{ccc}1& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 1& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 1\end{array}\right]\·C_b^c$

设 $C_b^n=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right],$ 其中c₁₁,c₁₂,c₁₃,c₂₁,c₂₂,c₂₃,c₃₁,c₃₂,c₃₃为姿态矩阵的值，

则载体的方位角H、纵摇角P和横摇角R，提取公式如下：

$H=\arctan \frac{c_{12}}{c_{11}}$

$R=\arctan \frac{-c_{13}}{\sqrt{{c_{11}}^2+{c_{12}}^2}}$

$P=\arctan \frac{c_{23}}{c_{33}}.$

3、根据权利要求1所述的旋转式捷联光纤罗经的实现方法，其特征在于，所述步骤8中载体的航机角Ψ_Tb的提取过程如下：

步骤8.1根据r时刻获取的三只光纤陀螺仪的输出数据

以及游离方位角α_f，通过四元数法对旋转坐标系p系相对于游离坐标系T_e系的姿态变换矩阵

进行更新：

$\left[\begin{array}{c}{\mathrm{\ω}}_{T_e\mathrm{px}}^p\\ {\mathrm{\ω}}_{T_e\mathrm{py}}^p\\ {\mathrm{\ω}}_{T_e\mathrm{pz}}^p\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ipx}}^p\\ {\mathrm{\ω}}_{\mathrm{ipy}}^p\\ {\mathrm{\ω}}_{\mathrm{ipz}}^p\end{array}\right]-{\left(C_p^{T_e}\left(r\right)\right)}^T\left[\begin{array}{c}-\frac{V_N\left(r\right)}{R_M}\cos {\mathrm{\α}}_f\left(r\right)+({\mathrm{\ω}}_{\mathrm{ie}}\cos L\left(r\right)+\frac{V_E\left(r\right)}{R_N})\sin {\mathrm{\α}}_f\left(r\right)\\ \frac{V_N\left(r\right)}{R_M}\sin {\mathrm{\α}}_f\left(r\right)+({\mathrm{\ω}}_{\mathrm{ie}}\cos L\left(r\right)+\frac{V_E\left(r\right)}{R_N})\cos {\mathrm{\α}}_f\left(r\right)\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\left(r\right)\end{array}\right]$

其中：

分别为三只光纤陀螺仪在ox_p、oy_p、oz_p轴上采集到的数据，即为旋转坐标系p系相对于惯性坐标系i系的角速率在旋转坐标系p系下三个轴向上的分量，

为旋转坐标系p系相对于游离坐标系T_e系的角速率在旋转坐标系p系下三个轴向上的分量，

步骤8.2更新四元数和姿态矩阵：

设旋转坐标系p系相对于游离坐标系T_e系的转动四元数为：

Q＝q₄+q₅w_p+q₆j_p+q₇h_p

其中：q₄、q₅、q₆、q₇是实数，w_p、j_p、h_p分别表示旋转坐标系p系ox_p轴、oy_p轴、oz_p轴上的单位方向向量；

四元数修正通过解四元数微分方程

来实现：

$\left[\begin{array}{c}{q}_4(r+1)\\ q_5(r+1)\\ q_6(r+1)\\ q_7(r+1)\end{array}\right]=\left[\begin{array}{c}{q}_4\left(r\right)\\ q_5\left(r\right)\\ q_6\left(r\right)\\ q_7\left(r\right)\end{array}\right]+\frac{T_s}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{T_e\mathrm{px}}^p& -{\mathrm{\ω}}_{T_e\mathrm{py}}^p& -{\mathrm{\ω}}_{T_e\mathrm{pz}}^p\\ {\mathrm{\ω}}_{T_e\mathrm{px}}^p& 0& {\mathrm{\ω}}_{T_e\mathrm{pz}}^p& -{\mathrm{\ω}}_{T_e\mathrm{py}}^p\\ {\mathrm{\ω}}_{T_e\mathrm{py}}^p& -{\mathrm{\ω}}_{T_e\mathrm{pz}}^p& 0& {\mathrm{\ω}}_{T_e\mathrm{px}}^p\\ {\mathrm{\ω}}_{T_e\mathrm{pz}}^p& {\mathrm{\ω}}_{T_e\mathrm{py}}^p& -{\mathrm{\ω}}_{T_e\mathrm{px}}^p& 0\end{array}\right]\left[\begin{array}{c}{q}_4\left(r\right)\\ q_5\left(r\right)\\ q_6\left(r\right)\\ q_7\left(r\right)\end{array}\right]$

Ts是采样周期，取值为10ms，，r+1时刻的姿态矩阵

更新过程如下：

$C_p^T(r+1)=\left[\begin{array}{ccc}{q}_4^2(r+1)+q_5^2(r+1)-q_6^2(r+1)-q_7^2(r+1)& 2(q_4(r+1)q_6(r+1)-q_4(r+1)q_7(r+1))& 2(q_5(r+1)q_7(r+1)+q_4(r+1)q_6(r+1))\\ 2(q_4(r+1)q_6(r+1)+q_4(r+1)q_7(r+1))& q_4^2(r+1)-q_5^2(r+1)+q_6^2(r+1)-q_7^2(r+1)& 2(q_5(r+1)q_7(r+1)-q_4(r+1)q_5(r+1))\\ 2(q_5(r+1)q_7(r+1)-q_4(r+1)q_6(r+1))& 2(q_6(r+1)q_7(r+1)+q_4(r+1)q_5(r+1))& q_4^2(r+1)-q_5^2(r+1)-q_6^2(r+1)+q_7^2(r+1)\end{array}\right]$

步骤8.3再利用r+1时刻角度值θ(r+1)求出旋转坐标系p系相对于载体坐标系b系的姿态变换矩阵

最后通过所述的两个姿态变换矩阵，求出载体坐标系b系相对于游离坐标系T_e系的姿态矩阵

则

和

分别为：

$C_p^b(r+1)=\left[\begin{array}{ccc}\cos \mathrm{\θ}(r+1)& -\sin \mathrm{\θ}(r+1)& 0\\ \sin \mathrm{\θ}(r+1)& \cos \mathrm{\θ}(r+1)& 0\\ 0& 0& 1\end{array}\right]$

$C_b^{T_e}(r+1)=C_p^{T_e}(r+1){\left(C_p^b(r+1)\right)}^T$

设 $C_b^{T_e}(r+1)=\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],$ 其中t₁₁,t₁₂,t₁₃,t₂₁,t₂₂,t₂₃,t₃₁,t₃₂,t₃₃为姿态矩阵的值，载体的航机角Ψ_Tb提取公式为：

${\mathrm{\Ψ}}_{\mathrm{Tb}}=\arctan \frac{t_{12}}{t_{11}}$

由r+1时刻获取的加速度计输出的数据f^p对游离系下的速度V_x(r)、V_y(r)更新为：

$V_x(r+1)=V_x\left(r\right)+(C_p^T{f}^{\mathrm{px}}+(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x\left(r\right)}{R_N}\tan L)V_y\left(r\right))\·T_s$

$V_y(r+1)=V_y\left(r\right)+(C_p^T{f}^{\mathrm{py}}-(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x\left(r\right)}{R_N}\tan L)V_x\left(r\right))\·T_s$

其中：ω_ie为地球自转角速率；R_N为沿卯酉圈曲率半径。

4、根据权利要求1所述的旋转式捷联光纤罗经的实现方法，其特征在于，所述步骤9中方向余弦计算过程包括：

设 $C_e^{T_e}=\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],$ 其中T₁₁,T₁₂,T₁₃,T₂₁,T₂₂,T₂₃,T₃₁,T₃₂,T₃₃为姿态矩阵的值，根据r时刻计算得到的V_x(r)和V_y(r)，来更新

在x轴向分量

和在y轴向分量

${\mathrm{\ω}}_{{\mathrm{eT}}_{e}x}^{T_e}\left(r\right)=-\frac{2\×1/298.257}{R_e}{T}_{13}{T}_{23}{V}_x\left(r\right)-\frac{1}{R_e}(1-1/298.257\×T_{33}^2+2\×1/298.257\×T_{23}^2)V_y\left(r\right)$

${\mathrm{\ω}}_{{\mathrm{eT}}_{e}y}^{T_e}\left(r\right)=\frac{2\×1/298.257}{R_e}{T}_{13}{T}_{23}{V}_y\left(r\right)+\frac{1}{R_e}(1-1/298.257\×T_{33}^2+2\×1/298.257\×T_{13}^2)V_x\left(r\right)$

其中：R_e为地球半径；

然后求解微分方程得出方向余弦矩阵

的值，则游离方位角α_f为： ${\mathrm{\α}}_f=\arctan \frac{C_{13}}{C_{23}}.$

Claims (5)

1.一种旋转式捷联光纤罗经的实现方法，包括在低纬度使用的修正状态算法和高纬度使用的方位仪状态算法，所述的旋转式捷联光纤罗经由包含3只光纤陀螺和3只石英挠性加速度计的惯性测量装置A和单轴机械转台B两大部分组成，采用标准紧固螺钉将惯性测量装置A固定在单轴机械转台B上，其特征在于，

在低纬度使用的修正状态算法包括以下步骤：

步骤1定义坐标系：导航坐标系n系以载体质心为原点，x_n、y_n、z_n分别指向所在地的东、北、天，地球坐标系e系以地心为原点，x_e轴穿越本初子午线与赤道的交点，y_e轴穿越东经90°子午线与赤道的交点，z_e轴穿越地球北极点，载体坐标系b系以载体中心为原点，x_b轴沿横轴指向右，y_b轴沿纵轴指向前，z_b轴垂直载体指向上，旋转坐标系p系以旋转台面的中心为原点，z_p轴沿转轴指向上，x_p轴和y_p轴位于旋转台面内，并和台面一起旋转，三个坐标轴构成右手坐标系，惯性坐标系i系以地心为原点，x_i轴指向春分点，z_i轴沿地球自转轴，y_i轴与x_i、z_i轴构成右手坐标系，游离坐标系T_e系，水平轴

和

相对于导航坐标系的东向轴和北向轴存在游离方位角α_f，经线地球坐标系e₀系以地球中心为原点，并与地球同步旋转，

轴在地球赤道平面内，

轴指向载体所在点经线，

轴指向地球自转轴方向，经线地心惯性坐标系i₀系定义为在粗对准起始时刻将经线地球坐标系惯性凝固成的右手坐标系，载体惯性坐标系i_b0系定义为在粗对准起始时刻将载体坐标系惯性凝固后的坐标系，计算导航坐标系c系定义为计算机输出结果确定的导航坐标系，

步骤2根据三只光纤陀螺仪的输出数据

三只石英加速度计的输出数据f^p，以及地球自转角速率ω_ie、重力加速度g、载体所在地的纬度L，应用基于惯性系重力矢量的解析对准算法计算导航坐标系n系与载体坐标系b系之间的转移矩阵

完成光纤捷联罗经系统初始对准，所述应用基于惯性系重力矢量的解析对准算法完成光纤捷联罗经系统初始对准的过程如下：

步骤2.1计算导航坐标系n系与经线地球坐标系e₀系之间的转移矩阵

$C_n^{e_0}=\left[\begin{array}{ccc}0& -\sin L& \cos L\\ 1& 0& 0\\ 0& \cos L& \sin L\end{array}\right]$

步骤2.2计算经线地球坐标系e₀系与经线地心惯性坐标系i₀系之间的转移矩阵

$C_{e_0}^{i_0}\left(t\right)=\left[\begin{array}{ccc}\cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& -\sin \left({\mathrm{\ω}}_{\mathrm{ie}}\right)t& 0\\ \sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& \cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& 0\\ 0& 0& 1\end{array}\right]$

t表示对准时间，ω_ie为地球自转角速率，

步骤2.3计算载体惯性坐标系与载体坐标系之间的转移矩阵

在起始时刻，载体惯性坐标系与载体坐标系重合，即

的初值为单位矩阵，根据陀螺仪输出的旋转坐标系p系相对惯性坐标系i系在旋转坐标系p系下的角速度

并通过四元数方法求解

步骤2.4计算经线地心惯性坐标系i₀系与载体惯性坐标系i_b0系之间的转移矩阵

$C_{i_0}^{i_{b0}}={\left[\begin{array}{c}{\left[V^{i_{b0}}\left(t_1\right)\right]}^T\\ {\left[V^{i_{b0}}\left(t_2\right)\right]}^T\\ {[V^{i_{b0}}\left(t_1\right)\×V^{i_{b0}}\left(t_2\right)]}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left[V^{i_0}\left(t_1\right)\right]}^T\\ {\left[V^{i_0}\left(t_2\right)\right]}^T\\ {[V^{i_0}\left(t_1\right)\×V^{i_0}\left(t_2\right)]}^T\end{array}\right]$

式中， $V^{i_0}\left(t\right)=\left[\begin{array}{c}\frac{g\cos L\sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ \frac{g\cos L[1-\cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ g\sin L\·t\end{array}\right]$

$V^{i_{b0}}\left(t\right)={\∫}_0^t{f}^{i_{b0}}\left(\mathrm{\τ}\right)\mathrm{d\τ}={\∫}_0^t\left[C_b^{i_{b0}}\left(\mathrm{\τ}\right)f^p\left(\mathrm{\τ}\right)\right]\mathrm{d\τ}={\∫}_0^t\left[{\left(C_{i_{b0}}^b\left(\mathrm{\τ}\right)\right)}^T{f}^p\left(\mathrm{\τ}\right)\right]\mathrm{d\τ}$

t₁和t₂表示对准过程中选取的两个时间点，τ表示时间参数，t₁取值1分钟，t₂取值6分钟,

步骤2.5根据式 $C_n^b\left(t\right)=C_{i_{\mathrm{bo}}}^b\left(t\right)\·C_{i_0}^{i_{\mathrm{bo}}}\·C_{e_0}^{i_0}\left(t\right)\·C_n^{e_0},$ 求出

完成初始对准，

步骤3控制电机，使与惯性测量单元IMU固连的转台旋转，首先从0°正转到180°停止，然后从180°反转到0°停止；然后从0°反转到180°停止，最后从180°正转到0°停止，这样周而复始的转动，旋转角速率为8°/s，每个位置停止时间为5分钟，每个时刻k获取的转动角度值为θ(k)，

步骤4根据k时刻三只光纤陀螺仪的输出数据和三只石英加速度计在k时刻输出数据f^p(k)，求出k时刻旋转坐标系p系相对于计算导航坐标系c系的姿态变换矩阵再利用k时刻的转动角度值θ(k)求出旋转坐标系p系相对于载体坐标系b系的姿态变换矩阵最后通过所述的两个姿态变换矩阵，求出载体坐标系b系相对于计算导航坐标系c系的姿态变换矩阵

步骤5利用载体上辅助导航系统提供的比力信息，对姿态进行修正，并提取出载体的方位角H、纵摇角P和横摇角R；

所述的高纬度使用的方位仪状态算法的步骤如下：

步骤6根据修正状态切换成方位仪状态时刻载体所在位置的经度λ、纬度L和载体在导航坐标系n系中的水平速度V_e、V_n分别初始化方向余弦矩阵和游离坐标系中的水平速度V_x、V_y；初始游离方位角α_f设置为0，

$C_e^{T_e}=\left[\begin{array}{ccc}-\cos a_f\sin \mathrm{\λ}-\sin a_f\sin L\cos \mathrm{\λ}& \cos a_f\cos \mathrm{\λ}-\sin a_f\sin L\sin \mathrm{\λ}& \sin a_f\cos L\\ \sin a_f\sin \mathrm{\λ}-\cos a_f\sin L\cos \mathrm{\λ}& -\sin a_f\cos \mathrm{\λ}-\cos a_f\sin L\sin \mathrm{\λ}& \cos a_f\cos L\\ \cos L\cos \mathrm{\λ}& \cos L\sin \mathrm{\λ}& \sin L\end{array}\right]$

V_x＝V_e V_y＝V_n

步骤7控制电机，使与惯性测量单元IMU固连的转台旋转，首先从0°正转到180°停止，然后从180°反转到0°停止；然后从0°反转到180°停止，最后从180°正转到0°停止。这样周而复始的转动，旋转角速率为8°/s，每个位置停止时间为5分钟，每个时刻r获取的转动角度值为θ(r)，

步骤8根据r时刻三只光纤陀螺仪的输出数据

以及游离方位角α_f，求取旋转坐标系p系相对于游离坐标系T_e系的姿态变换矩阵

再利用r时刻转动角度值θ(r)求出旋转坐标系p系相对于载体坐标系b系的姿态变换矩阵

最后通过所述的两个姿态变换矩阵，求出载体坐标系b系相对于游离坐标系T_e系的姿态矩阵

以及根据r时刻三只加速度计输出的数据f^p(r)，计算出游离坐标系T_e系下载体的水平速度V_x、V_y，最后提取出载体的航机角Ψ_Tb，

步骤9利用求得游离坐标系T_e系下载体的水平速度V_x、V_y，确定出游离坐标系T_e系下的载体位置速率

为游离坐标系T_e系相对地球坐标系e系在游离坐标系T_e系下的角速度，然后根据微分方程

求出方向余弦矩阵的值，并提取出游离方位角α_f，

步骤10利用提取出的游离方位角α_f，得到载体的航向角H，H＝Ψ_Tb-α_f。

2.根据权利要求1所述一种旋转式捷联光纤罗经的实现方法，其特征在于：

所述步骤4中载体坐标系b系相对于计算导航坐标系c系的姿态变换矩阵

具体解算过程包括：

步骤4.1根据k时刻获取的三只光纤陀螺仪的输出数据

和三个加速度计的输出数据f^p，用四元数法对旋转坐标系p系相对于计算导航坐标系c系的姿态变换矩阵

进行更新：

$\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{cpx}}^p\\ {\mathrm{\ω}}_{\mathrm{cpy}}^p\\ {\mathrm{\ω}}_{\mathrm{cpz}}^p\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ipx}}^p\\ {\mathrm{\ω}}_{\mathrm{ipy}}^p\\ {\mathrm{\ω}}_{\mathrm{ipz}}^p\end{array}\right]-{\left(C_p^c\left(k\right)\right)}^T\left[\begin{array}{c}-\frac{V_N\left(k\right)}{R_M}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\left(k\right)+\frac{V_E\left(k\right)}{R_N}\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\left(k\right)+\frac{V_E\left(k\right)}{R_N}\tan L\left(k\right)\end{array}\right]$

其中，

分别为三只光纤陀螺仪在ox_p、oy_p、oz_p轴上采集到的数据，即为旋转坐标系p系相对于惯性坐标系i系的角速率在旋转坐标系p系下三个轴向上的分量，

为旋转坐标系p系相对于计算导航坐标系c系的角速率在旋转坐标系p系下三个轴向上的分量，

步骤4.2更新四元数和姿态矩阵：

设旋转坐标系p系相对于计算导航坐标系c系的转动四元数为：

Q＝q₀+q₁w_p+q₂j_p+q₃h_p

其中：q₀、q₁、q₂、q₃是实数，w_p、j_p、h_p分别表示旋转坐标系p系ox_p轴、oy_p轴、oz_p轴上的单位方向向量；

四元数初始化为：

四元数的初始值Q(0)由初始对准确定：设初始对准得到的姿态矩阵为

其中 $C_b^c={\left(C_n^b\right)}^T,$

$C_b^c=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

则四元数q₀、q₁、q₂、q₃的表达式如下：

$\left\{\begin{array}{c}\left|q_0\right|=\frac{1}{2}\sqrt{1+C_{11}+C_{22}+C_{33}}\\ \left|q_1\right|=\frac{1}{2}\sqrt{1+C_{11}-C_{22}-C_{33}}\\ \left|q_2\right|=\frac{1}{2}\sqrt{1-C_{11}+C_{22}-C_{33}}\\ \left|q_3\right|=\frac{1}{2}\sqrt{1-C_{11}-C_{22}+C_{33}}\end{array}\right.$

q₀、q₁、q₂、q₃的符号可按下式确定：

$\left\{\begin{array}{c}\mathrm{sign}\left(q_1\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(C_{32}-C_{23})\right]\\ \mathrm{sign}\left(q_2\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(C_{13}-C_{31})\right]\\ \mathrm{sign}\left(q_3\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(C_{21}-C_{12})\right]\end{array}\right.$

其中，sign(q₀)可以任选，

利用四元数微分方程

修正四元数q₀、q₁、q₂、q₃：

$\left[\begin{array}{c}{q}_0(k+1)\\ q_1(k+1)\\ q_2(k+1)\\ q_3(k+1)\end{array}\right]=\left[\begin{array}{c}{q}_0\left(k\right)\\ q_1\left(k\right)\\ q_2\left(k\right)\\ q_3\left(k\right)\end{array}\right]+\frac{T_s}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{cpx}}^0& -{\mathrm{\ω}}_{\mathrm{cpy}}^p& -{\mathrm{\ω}}_{\mathrm{cpz}}^p\\ {\mathrm{\ω}}_{\mathrm{cpx}}^p& 0& {\mathrm{\ω}}_{\mathrm{cpz}}^p& -{\mathrm{\ω}}_{\mathrm{cpy}}^p\\ {\mathrm{\ω}}_{\mathrm{cpy}}^p& -{\mathrm{\ω}}_{\mathrm{cpz}}^p& 0& {\mathrm{\ω}}_{\mathrm{cpx}}^p\\ {\mathrm{\ω}}_{\mathrm{cpz}}^p& {\mathrm{\ω}}_{\mathrm{cpy}}^p& -{\mathrm{\ω}}_{\mathrm{cpx}}^p& 0\end{array}\right]\left[\begin{array}{c}{q}_0\left(k\right)\\ q_1\left(k\right)\\ q_2\left(k\right)\\ q_3\left(k\right)\end{array}\right],$

Ts是采样周期，取值为10ms，则k+1时刻姿态矩阵

更新过程如下：

$C_p^c(k+1)=\left[\begin{array}{ccc}{q}_0^2(k+1)+q_1^2(k+1)-q_2^2(k+1)-q_3^2(k+1)& 2(q_0(k+1)q_2(k+1)-q_0(k+1)q_3(k+1))& 2(q_1(k+1)q_3(k+1)+q_0(k+1)q_2(k+1))\\ 2(q_0(k+1)q_2(k+1)+q_0(k+1)q_3(k+1))& q_0^2(k+1)-q_1^2(k+1)+q_2^2(k+1)-q_3^2(k+1)& 2(q_2(k+1)q_3(k+1)-q_0(k+1)q_1(k+1))\\ 2(q_1(k+1)q_3(k+1)-q_0(k+1)q_2(k+1))& 2(q_2(k+1)q_3(k+1)+q_0(k+1)q_1(k+1))& q_0^2(k+1)-q_1^2(k+1)-q_2^2(k+1)+q_3^2(k+1)\end{array}\right]$

步骤4.3再根据k+1时刻角度值θ(k+1)求出旋转坐标系p系相对于载体坐标系b系的姿态变换矩阵

最后通过所述的两个姿态变换矩阵，求出k+1时刻，载体坐标系b系相对于计算导航坐标系c系的姿态矩阵

则

和

分别为：

$c_p^b(k+1)=\left[\begin{array}{ccc}\cos \mathrm{\θ}(k+1)& -\sin \mathrm{\θ}(k+1)& 0\\ \sin \mathrm{\θ}(k+1)& \cos \mathrm{\θ}(k+1)& 0\\ 0& 0& 1\end{array}\right]$

$C_b^c(k+1)=C_p^c(k+1){\left(C_p^b(k+1)\right)}^T$

3.根据权利要求1所述一种旋转式捷联光纤罗经的实现方法，其特征在于：所述步骤5中姿态校正的计算过程包括：

建立以东向失准角φ_e、北向失准角φ_n、天向失准角φ_u为状态，以东向比力信息f_e和北向比力信息f_n为量测的旋转式捷联光纤罗经系统卡尔曼滤波模型，

系统状态向量为X＝[φ_e,φ_n,φ_u]^T，系统矩阵F为：

$F=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ -{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{isnL}& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\end{array}\right]$

系泊情况下，忽略晃动引起的干扰加速度，导航坐标系下的东向比力和北向比力为零，则加速度计的输出在导航坐标系下投影的水平分量即为与失准角耦合的信息，系统量测Y为：

$Y=\left[\begin{array}{c}\frac{\underset{1}{\overset{N}{\mathrm{\Σ}}}{f}_e}{N}\\ \frac{\underset{1}{\overset{N}{\mathrm{\Σ}}}{f}_n}{N}\end{array}\right]$

其中，f_e和f_n为由加速度计测得的比力信息在导航坐标系下的投影，N为滤波周期的采样次数，N=500，

量测矩阵H为：

$H=\left[\begin{array}{ccc}0& g& 0\\ -g& 0& 0\end{array}\right]$

式中，g为当地的重力加速度值，g＝9.8m/s²，

利用卡尔曼滤波模型估计得到由东向失准角φ_e、北向失准角φ_n、天向失准角φ_u构成的闭环修正姿态矩阵

然后通过修正后的

提取方位角H、纵摇角P和横摇角R，

$C_b^n=C_c^n\·C_b^c=\left[\begin{array}{ccc}1& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 1& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 1\end{array}\right]\·C_b^c$

设 $C_b^n=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right],$ 其中c₁₁,c₁₂,c₁₃,c₂₁,c₂₂,c₂₃,c₃₁,c₃₂,c₃₃为姿态矩阵的值，则载体的方位角H、纵摇角P和横摇角R，提取公式如下：

$H=\arctan \frac{c_{12}}{c_{11}}$

$R=\arctan \frac{-c_{13}}{\sqrt{{c_{11}}^2+{c_{12}}^2}}$

$P=\arctan \frac{c_{23}}{c_{33}}.$

4.根据权利要求1所述的旋转式捷联光纤罗经的实现方法，其特征在于，所述步骤8中载体的航机角Ψ_Tb的提取过程如下：

步骤8.1根据r时刻获取的三只光纤陀螺仪的输出数据

以及游离方位角α_f，通过四元数法对旋转坐标系p系相对于游离坐标系T_e系的姿态变换矩阵

进行更新：

$\left[\begin{array}{c}{\mathrm{\ω}}_{T_e\mathrm{px}}^p\\ {\mathrm{\ω}}_{T_e\mathrm{py}}^p\\ {\mathrm{\ω}}_{T_e\mathrm{pz}}^p\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ipx}}^p\\ {\mathrm{\ω}}_{\mathrm{ipy}}^p\\ {\mathrm{\ω}}_{\mathrm{ipz}}^p\end{array}\right]-{\left(C_p^{T_e}\left(r\right)\right)}^T\left[\begin{array}{c}-\frac{V_N\left(r\right)}{R_M}\cos {\mathrm{\α}}_f\left(r\right)+({\mathrm{\ω}}_{\mathrm{ie}}\cos L\left(r\right)+\frac{V_E\left(r\right)}{R_N})\sin {\mathrm{\α}}_f\left(r\right)\\ \frac{V_N\left(r\right)}{R_M}\sin {\mathrm{\α}}_f\left(r\right)+({\mathrm{\ω}}_{\mathrm{ie}}\cos L\left(r\right)+\frac{V_E\left(r\right)}{R_N})\cos {\mathrm{\α}}_f\left(r\right)\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\left(r\right)\end{array}\right]$

其中：

分别为三只光纤陀螺仪在ox_p、oy_p、oz_p轴上采集到的数据，即为旋转坐标系p系相对于惯性坐标系i系的角速率在旋转坐标系p系下三个轴向上的分量，

为旋转坐标系p系相对于游离坐标系T_e系的角速率在旋转坐标系p系下三个轴向上的分量，

步骤8.2更新四元数和姿态矩阵：

设旋转坐标系p系相对于游离坐标系T_e系的转动四元数为：

Q＝q₄+q₅w_p+q₆j_p+q₇h_p

其中：q₄、q₅、q₆、q₇是实数，w_p、j_p、h_p分别表示旋转坐标系p系ox_p轴、oy_p轴、oz_p轴上的单位方向向量；

四元数修正通过解四元数微分方程

来实现：

$\left[\begin{array}{c}{q}_4(r+1)\\ q_5(r+1)\\ q_6(r+1)\\ q_7(r+1)\end{array}\right]=\left[\begin{array}{c}{q}_4\left(r\right)\\ q_5\left(r\right)\\ q_6\left(r\right)\\ q_7\left(r\right)\end{array}\right]+\frac{T_s}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{T_e\mathrm{px}}^p& -{\mathrm{\ω}}_{T_e\mathrm{py}}^p& -{\mathrm{\ω}}_{T_e\mathrm{pz}}^p\\ {\mathrm{\ω}}_{T_e\mathrm{px}}^p& 0& {\mathrm{\ω}}_{T_e\mathrm{pz}}^p& -{\mathrm{\ω}}_{T_e\mathrm{py}}^p\\ {\mathrm{\ω}}_{T_e\mathrm{py}}^p& -{\mathrm{\ω}}_{T_e\mathrm{pz}}^p& 0& {\mathrm{\ω}}_{T_e\mathrm{px}}^p\\ {\mathrm{\ω}}_{T_e\mathrm{pz}}^p& {\mathrm{\ω}}_{T_e\mathrm{py}}^p& -{\mathrm{\ω}}_{T_e\mathrm{px}}^p& 0\end{array}\right]\left[\begin{array}{c}{q}_4\left(r\right)\\ q_5\left(r\right)\\ q_6\left(r\right)\\ q_7\left(r\right)\end{array}\right]$

Ts是采样周期，取值为10ms，，r+1时刻的姿态矩阵更新过程如下：

$C_p^T(r+1)=\left[\begin{array}{ccc}{q}_4^2(r+1)+q_5^2(r+1)-q_6^2(r+1)-q_7^2(r+1)& 2(q_4(r+1)q_6(r+1)-q_4(r+1)q_7(r+1))& 2(q_5(r+1)q_7(r+1)+q_4(r+1)q_6(r+1))\\ 2(q_4(r+1)q_6(r+1)+q_4(r+1)q_7(r+1))& q_4^2(r+1)-q_5^2(r+1)+q_6^2(r+1)-q_7^2(r+1)& 2(q_5(r+1)q_7(r+1)-q_4(r+1)q_5(r+1))\\ 2(q_5(r+1)q_7(r+1)-q_4(r+1)q_6(r+1))& 2(q_6(r+1)q_7(r+1)+q_4(r+1)q_5(r+1))& q_4^2(r+1)-q_5^2(r+1)-q_6^2(r+1)+q_7^2(r+1)\end{array}\right]$

步骤8.3再利用r+1时刻角度值θ(r+1)求出旋转坐标系p系相对于载体坐标系b系的姿态变换矩阵

最后通过所述的两个姿态变换矩阵，求出载体坐标系b系相对于游离坐标系T_e系的姿态矩阵

则

和

分别为：

$C_p^b(r+1)=\left[\begin{array}{ccc}\cos \mathrm{\θ}(r+1)& -\sin \mathrm{\θ}(r+1)& 0\\ \sin \mathrm{\θ}(r+1)& \cos \mathrm{\θ}(r+1)& 0\\ 0& 0& 1\end{array}\right]$

$C_b^{T_e}(r+1)=C_p^{T_e}(r+1){\left(C_p^b(r+1)\right)}^T$

设 $C_b^{T_e}(r+1)=\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],$ 其中t₁₁,t₁₂,t₁₃,t₂₁,t₂₂,t₂₃,t₃₁,t₃₂,t₃₃为姿态矩阵的值，载体的航机角Ψ_Tb提取公式为：

${\mathrm{\Ψ}}_{\mathrm{Tb}}=\arctan \frac{t_{12}}{t_{11}}$

由r+1时刻获取的加速度计输出的数据f^p对游离系下的速度V_x(r)、V_y(r)更新为：

$V_x(r+1)=V_x\left(r\right)+(C_p^T{f}^{\mathrm{px}}+(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x\left(r\right)}{R_N}\tan L)V_y\left(r\right))\·T_s$

$V_y(r+1)=V_y\left(r\right)+(C_p^T{f}^{\mathrm{py}}-(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x\left(r\right)}{R_N}\tan L)V_x\left(r\right))\·T_s$

其中：ω_ie为地球自转角速率；R_N为沿卯酉圈曲率半径。

5.根据权利要求1所述的旋转式捷联光纤罗经的实现方法，其特征在于，所述步骤9中方向余弦计算过程包括：

设 $C_e^{T_e}=\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],$ 其中T₁₁,T₁₂,T₁₃,T₂₁,T₂₂,T₂₃,T₃₁,T₃₂,T₃₃为姿态矩阵的值，根据r时刻计算得到的V_x(r)和V_y(r)，来更新

在x轴向分量

和在y轴向分量

${\mathrm{\ω}}_{{\mathrm{eT}}_{e}x}^{T_e}\left(r\right)=-\frac{2\×1/298.257}{R_e}{T}_{13}{T}_{23}{V}_x\left(r\right)-\frac{1}{R_e}(1-1/298.257\×T_{33}^2+2\×1/298.257\×T_{23}^2)V_y\left(r\right)$

${\mathrm{\ω}}_{{\mathrm{eT}}_{e}y}^{T_e}\left(r\right)=\frac{2\×1/298.257}{R_e}{T}_{13}{T}_{23}{V}_y\left(r\right)+\frac{1}{R_e}(1-1/298.257\×T_{33}^2+2\×1/298.257\×T_{13}^2)V_x\left(r\right)$

其中：R_e为地球半径；

然后求解微分方程

得出方向余弦矩阵

的值，则游离方位角α_f为： ${\mathrm{\α}}_f=\arctan \frac{C_{13}}{C_{23}}.$

Images