CN102620748B - 捷联惯导***晃动基座条件下杆臂效应的估计和补偿方法 - Google Patents

Abstract

捷联惯导***晃动基座条件下杆臂效应的估计和补偿方法，估计杆臂效应对***的影响并按一定策略进行补偿，以提高精对准和导航解算的精度。首先在粗对准基础上，将精对准过程分成两个阶段。第一阶段：运行杆臂速度实时补偿和扩展变量的参数辨识法精对准算法，估计***中除杆臂速度外的残余干扰速度。第二阶段：对残余干扰速度进行一次补偿；继续运行杆臂速度实时补偿和扩展变量的参数辨识法精对准算法，估计失准角信息；利用估计的失准角对粗对准结果进行一步修正，确定初始的姿态矩阵，完成精对准。导航解算阶段，对杆臂速度进行实时补偿的同时，运行捷联解算程序，提供导航结果。

Description

捷联惯导***晃动基座条件下杆臂效应的估计和补偿方法

技术领域

本发明属于捷联惯导技术领域，涉及惯性导航***，是一种捷联惯导***晃动基座条件下，在精对准和导航阶段由杆臂效应引起的速度误差的估计和补偿方法。

背景技术

惯性技术是世界各工业强国发展的技术领域之一，惯性导航***(INS，简称惯导)是一种利用惯性技术实现运载体自主导航的***，捷联惯导没有实体平台，运载体的晃动干扰直接加给陀螺仪和加速度计。初始对准是捷联惯导***关键技术之一，它为导航***提供解算的初始值，对导航***的精度有很大的影响，近年来成为国内外学者研究的热点。外界干扰会使传感器输出信号引入噪声，即影响传感器信号的可靠性，进而影响对准和导航精度。

杆臂效应是由于在惯性测量组件安装位置与载体摇摆中心不重合的情况下，载体受到外界干扰或者载体运动使捷联惯导***基座处于摇摆或晃动状态，致使加速度计输出信息受到干扰，进而影响速度信息。杆臂效应引起的干扰会引起导航***初始对准和导航解算很大的误差，必须进行估计和补偿。

由于杆臂长度计算不准确、载体运动的角速率和角加速率估计噪声大、初始的杆臂速度误差难估计等因素，传统的杆臂速度动力学补偿方法不能完全补偿杆臂误差，存有残余。通过低通滤波法滤除杆臂效应引起高频干扰，会导致有用信息丢失、传感器信息不同步、对准精度下降等问题。设计一种晃动基座条件下新的杆臂效应估计和补偿的方法具有重要的工程实用价值。

发明内容

本发明要解决的问题是：杆臂效应对捷联惯导***的初始对准和导航解算有很大的影响，现有的技术手段不能对杆臂效应有效补偿。本发明主要解决晃动基座条件下，在对准和导航阶段如何估计杆臂效应以及如何补偿其影响的问题。

本发明的技术方案为：一种捷联惯导***晃动基座条件下杆臂效应的估计和补偿方法，估计杆臂效应对捷联惯导***的影响并进行补偿，包括以下步骤：

1)捷联惯导***开机预热，采集惯性测量组件的输出数据；

2)进行粗对准，得到粗略的初始姿态矩阵

3)在粗对准基础上分两个阶段完成精对准过程；

31)建立扩展变量的参数辨识法精对准数学模型，构造***方程和观测方程；扩展变量的参数辨识法精对准数学模型：

$\left\{\begin{array}{c}\mathrm{\Δ}{V}_e=({\▿}_e-g.{\mathrm{\φ}}_{n0})t-\frac{t^2}{2}g{u}_n+\frac{t^3}{6}g{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\Δ}{V}_n=({\▿}_n+g.{\mathrm{\φ}}_{e0})t+\frac{t^2}{2}g{u}_e+\frac{t^3}{6}g{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)+V_{\mathrm{dn}}++V_{\mathrm{sn}}\end{array}\right.$

其中 $\left\{\begin{array}{c}{u}_e={\mathrm{\φ}}_{n0}{\mathrm{\ω}}_{\mathrm{ie}}\sin L-{\mathrm{\φ}}_{u0}{\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\ϵ}}_e\\ u_n=-{\mathrm{\φ}}_{e0}{\mathrm{\ω}}_{\mathrm{ie}}\sin L-{\mathrm{\ϵ}}_n\\ u_u={\mathrm{\φ}}_{e0}{\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\ϵ}}_u\end{array}\right.$

式中：分别表示加速度计等效东向和北向的常值偏置；φ_e0、φ_n0、φ_u0表示初始失准角；ω_e、ω_n、ω_u表示等效东、北、天向陀螺常值漂移；ω_ie表示地球自转角速率；L表示当地纬度；g表示地球重力加速度；V_se、V_sn分别表示东向和北向随机干扰速度，V_de、V_dn表示东向和北向残余干扰速度，为常值；ΔV_e、ΔV_n分别表示东向和北向速度误差，是捷联惯导***解算的速度剔除杆臂速度后的值与外部基准提供的速度值之差，在晃动基座条件下，外部基准提供的速度为0m/s，从捷联惯导***解算的速度中剔除杆臂速度的过程即为杆臂速度的实时补偿，由杆臂效应引起的杆臂速度根据杆臂速度模型计算得到：

$\mathrm{\δ}{v}_g={\mathrm{\ω}}_{\mathrm{ib}}^b\×r=\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_z-{\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_y\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_x-{\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_z\\ {\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_y-{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_x\end{array}\right)$

式中：δv_g表示杆臂速度；r＝(r_x、r_y、r_z)表示杆臂长度矢量，该值根据运载体的设计结构和导航***安装位置预先计算并装定入***，实际应用中，杆臂长度矢量会因为运载体挠曲变形、载荷分布变化等因素偏离该值；表示三个方向上的陀螺仪输出角速率；

所述残余干扰速度包括：***存在的随机干扰速度；因杆臂长度测量不准确、载体存在挠曲变形、陀螺仪测量的角速率存在干扰等因素导致的杆臂速度计算误差；在晃动基座条件下，捷联解算初始时刻的速度中存在杆臂速度，运行捷联解算算法时，初始速度按0m/s处理，造成的残余速度；粗对准后，初始的姿态矩阵存在误差，运行捷联解算算法时，带来的速度误差；

根据扩展变量的参数辨识法数学模型，构造***方程和观测方程为：

以捷联惯导***中速度误差为观测量，把扩展变量的参数辨识法数学模型改写成如下形式：

$\left\{\begin{array}{c}\mathrm{\Δ}{V}_e=a_{1e}\left(\mathrm{KT}\right)+a_{2e}{\left(\mathrm{KT}\right)}^2+a_{3e}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\Δ}{V}_n=a_{1n}\left(\mathrm{KT}\right)+a_{2n}{\left(\mathrm{KT}\right)}^2+a_{3n}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{dn}}+V_{\mathrm{sn}}\end{array}\right.,K=\mathrm{0,1,2},\·\·\·$

式中：T为捷联惯导***的传感器数据采样周期；

$\left\{\begin{array}{c}{a}_{1e}=({\▿}_e-g.{\mathrm{\φ}}_{n0}),a_{2e}=-\frac{1}{2}g{u}_n,a_{3e}=\frac{1}{6}g{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L\\ a_{1n}=({\▿}_n+g.{\mathrm{\φ}}_{e0}),a_{2n}=\frac{1}{2}g{u}_e,a_{3n}=\frac{1}{6}g{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\end{array}\right.$

把东向和北向速度误差ΔV_e、ΔV_e作为观测量，a_1e、a_2e、a_3e、V_de、a_1n、a_2n、a_3n、V_dn作为待辨识参数，构造***方程和观测方程：

定义待辨识参数，即***状态变量为：

$X_e=\left[\begin{array}{c}{a}_{1e}\\ a_{2e}\\ a_{3e}\\ V_{\mathrm{de}}\end{array}\right],$ $X_n=\left[\begin{array}{c}{a}_{1n}\\ a_{2n}\\ a_{3n}\\ V_{\mathrm{dn}}\end{array}\right]$

列出***方程和观测方程：

$\left\{\begin{array}{c}{X}_e(k+1)=X_e\left(k\right)\\ \mathrm{\Δ}{V}_e\left(k\right)=H\left(k\right)X_e\left(k\right)+V_{\mathrm{ge}}\left(k\right)\end{array}\right.,$ $\left\{\begin{array}{c}{X}_n(k+1)=X_n\left(k\right)\\ \mathrm{\Δ}{V}_n\left(k\right)=H\left(k\right)X_n\left(k\right)+V_{\mathrm{gn}}\left(k\right)\end{array}\right.$

式中：观测矩阵H(k)＝[kT，(kT)²，(kT)³，1]；V_ge(k)、V_gn(k)表示东向和北向的速度观测噪声，其方差强度为R_e、R_n；

32)精对准第一阶段，根据粗对准结果，运行捷联解算程序，进行杆臂速度的实时补偿，同时根据步骤31)建立的***方程和观测方程估计***状态变量，状态变量的第四个分量即为残余干扰速度；

33)精对准第二阶段：对残余干扰速度进行一次补偿；而后，继续运行精对准第一阶段的杆臂速度实时补偿以及对***状态变量的估计算法，并进行初始失准角估计，待估计的初始失准角收敛后，将估计值φ_e0、φ_n0、φ_u0代入失准角变化规律模型中计算当前时刻的失准角φ_e、φ_n、φ_u；利用当前时刻的失准角信息对粗对准结果进行一步修正，获得当前时刻的姿态矩阵通过姿态矩阵提取方位角H、纵摇角P和横摇角R，精对准完成；

其中，根据步骤32)估计得到的状态变量计算u_e、u_n、u_u、φ_e0、φ_n0和φ_u0：

$u_e=\frac{2a_{2n}}{g},$ $u_n=-\frac{2a_{2e}}{g},$ $u_u=-\frac{6a_{3n}}{g{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{2a_{2e}}{g}\tan L$

${\mathrm{\φ}}_{e0}=\frac{a_{1n}}{g},$ ${\mathrm{\φ}}_{n0}=-\frac{a_{1e}}{g},$ ${\mathrm{\φ}}_{u0}={\mathrm{\φ}}_{n0}.\tan L-\frac{u_e}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}$

根据初始失准角和失准角变化规律模型计算当前时刻失准角：

$\left\{\begin{array}{c}{\mathrm{\φ}}_e={\mathrm{\φ}}_{e0}+u_e.t+\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\\ {\mathrm{\φ}}_n={\mathrm{\φ}}_{n0}+u_n.t-\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L\\ {\mathrm{\φ}}_u={\mathrm{\φ}}_{u0}+u_u.t+\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\cos L\end{array}\right.$

对粗对准结果的一步修正为：

$C_b^n=C_{n^{\′}}^n{C}_b^{n^{\′}}=[I+(\mathrm{\φ}\×)]C_b^{n^{\′}},$ 其中 $C_{n^{\′}}^n=I+(\mathrm{\φ}\×)=\left(\begin{array}{ccc}0& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 0& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 0\end{array}\right)$

4)导航解算阶段，进行杆臂速度的实时补偿，根据步骤33)得到的姿态矩阵运行捷联解算算法，提供导航结果。

步骤32)中，采用改良kalman滤波法估计***状态变量：

$\left\{\begin{array}{c}{X}_i(k+1)=X_i\left(k\right)+K_i\left(k\right)e_i\left(k\right)\\ K_i\left(k\right)=P_i\left(k\right)H^T\left(k\right){\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}}^{-1}\\ P_i(k+1)=P_i\left(k\right)-K_i\left(k\right)\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}{K}_i^T\left(k\right),i=e,n;k=\mathrm{0,1,2}\·\·\·\\ R_i(k+1)=R_i\left(k\right)+(e_i^2\left(k\right)-R_i\left(k\right))/(k+1)\\ e_i\left(k\right)=\mathrm{\Δ}{V}_i\left(k\right)-H\left(k\right)X_i\left(k\right)\end{array}\right.$

i对应表示***状态变量X_e，X_n中的下标e，n，初始状态变量X_i(0)、初始状态估计误差方差阵P_i(0)和初始观测噪声方差强度R_i(0)的值均可任选，

对上式用递推算法估计出***状态变量X_e，X_n。

步骤32)、33)和4)中，杆臂速度的实时补偿为：进行捷联解算时，每次速度更新后，把当前时刻的杆臂速度从更新后的速度信息中剔除，由杆臂效应引起的杆臂速度根据杆臂速度模型计算得到：

$\mathrm{\δ}{v}_g={\mathrm{\ω}}_{\mathrm{ib}}^b\×r=\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_z-{\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_y\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_x-{\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_z\\ {\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_y-{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_x\end{array}\right).$

步骤33)中，对残余干扰速度进行一次补偿方法为：精对准第一阶段结束后，估计出的残余干扰速度是常值，精对准第二阶段一开始，将残余的干扰速度从捷联解算出的速度信息中剔除，方法为：

$\left\{\begin{array}{c}{V^{\′}}_e=V_e-V_{\mathrm{de}}\\ {V^{\′}}_n=V_n-V_{\mathrm{dn}}\end{array}\right.$

式中：V′_e、V′_n表示剔除残余干扰速度后的东向和北向速度；V_e、V_n表示捷联解算的东向和北向速度；V_de、V_dn表示东向和北向残余干扰速度。

目前也有部分与本发明有关的研究报告，其中本发明参数辨识法精对准参考了以下两篇文章：1、捷联惯导***改进参数辨识初始对准方法，中国惯性技术学报，2010，18(5)；2、SINS在摇摆基座上的快速精确对准方法，北京航空航天大学学报，2009，35(1)；杆臂速度模型参考了以下专利：3、专利申请号为CN201010270972.4，名称为“消除水下运载体捷联惯导***杆臂效应误差的对准方法”。本发明区别于现有技术的特征在于：1、将精对准过程分成两个阶段，第一阶段估计残余干扰速度，第二阶段对残余干扰速度一次补偿后，估计失准角，对粗对准结果进行修正。2、在精对准阶段，对杆臂效应的实时补偿和对残余干扰速度的一次补偿相互结合，能提高杆臂效应的补偿效率。3、详细分析残余干扰速度的误差源，针对性的设计了扩展变量的参数辨识法精对准算法，能相对准确的估计出***中的残余干扰速度。4、在整个对准过程中，保持对杆臂速度的实时补偿。

本发明提出一种捷联惯导***晃动基座条件下精对准和导航阶段由杆臂效应引起的速度误差的估计和补偿方法，目的是估计杆臂效应对***的影响并按一定策略进行补偿，以提高精对准和导航解算的精度。本发明的优势在于：采用扩展变量的参数辨识法精对准算法，计算量小，精度高；能够估计出动力学补偿杆臂速度后的***残余干扰速度；将精对准算法分成两个阶段，第一阶段结束后对残余干扰速度进行一次补偿，能提高第二阶段精对准精度；在导航解算阶段对杆臂速度进行实时补偿，能提高导航解算精度。

附图说明

图1为本发明的流程图。

图2为杆臂效应原理图。

图3为本发明的杆臂效应估计和补偿算法流程图。

图4为本发明的第一组扩展变量的参数辨识法精对准算法估计的失准角曲线图。

图5为本发明的第一组扩展变量的参数辨识法精对准算法估计的残余干扰速度曲线图。

图6为本发明的第二组扩展变量的参数辨识法精对准算法估计的失准角曲线图。

图7为本发明的第二组扩展变量的参数辨识法精对准算法估计的残余干扰速度曲线图。

具体实施方式

下面对本发明的实施过程做详尽描述，流程如图1所示。

首先定义坐标系。定义‘东北天’坐标系为导航坐标系，记为n系；载体坐标系b系以载体重心为原点，X轴沿横轴指向右，Y轴沿纵轴指向前，Z轴垂直载体指向上；地球坐标系e系，坐标系原点在地心点，X轴在赤道平面，指向本初子午线，Y轴与X轴垂直，也在赤道平面内，Z轴通过右手准则确定，指向地球自转轴方向。地心惯性坐标系i系，该坐标系在初始时刻和地球坐标系重合。

步骤1)捷联惯导***开机预热后采集惯性测量组件的输出数据；

步骤2)进行粗对准，得到粗略的初始姿态矩阵因为快速性是惯导***对准的一个性能指标，运行3-5分钟粗对准算法，可以得到粗略的姿态矩阵；

步骤3)在粗对准基础上分两个阶段完成精对准过程，其流程如图3所示；

步骤31)建立扩展变量的参数辨识法精对准数学模型，构造***方程和观测方程；

首先建立扩展变量的参数辨识法数学模型；

在晃动基座上，粗对准确定导航系和理论的导航系之间的失准角变化规律如下：

$\left\{\begin{array}{c}{\mathrm{\φ}}_e={\mathrm{\φ}}_{e0}+u_e.t+\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\\ {\mathrm{\φ}}_n={\mathrm{\φ}}_{n0}+u_n.t-\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L\\ {\mathrm{\φ}}_u={\mathrm{\φ}}_{u0}+u_u.t+\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\cos L\end{array}\right.$ 其中 $\left\{\begin{array}{c}{u}_e={\mathrm{\φ}}_{n0}{\mathrm{\ω}}_{\mathrm{ie}}\sin L-{\mathrm{\φ}}_{u0}{\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\ϵ}}_e\\ u_n=-{\mathrm{\φ}}_{e0}{\mathrm{\ω}}_{\mathrm{ie}}\sin L-{\mathrm{\ϵ}}_n\\ u_u={\mathrm{\φ}}_{e0}{\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\ϵ}}_u\end{array}\right.$

等效水平方向上的比力方程为：

$\left\{\begin{array}{c}{f_e}^{n^{\′}}=-g.{\mathrm{\φ}}_n+f_{\mathrm{de}}+{\▿}_e\\ {f_n}^{n^{\′}}=g.{\mathrm{\φ}}_e+f_{\mathrm{dn}}+{\▿}_n\end{array}\right.$

式中：f_de、f_dn分别为等效东向和等效北向的干扰加速度。从上式可以看出，由于存在失准角φ_e、φ_n、φ_u，导致水平方向的比力信息中耦合了地球重力加速度信息g。对水平方向上的比力在[0，t]时间段内求积分，得到该时间段内累积的速度。因为载体在晃动基座上，无线运动，所以该速度值也即速度误差值，其表达式为：

$\left\{\begin{array}{c}\mathrm{\Δ}{V}_e=({\▿}_e-g.{\mathrm{\φ}}_{n0})t-\frac{t^2}{2}g{u}_n+\frac{t^3}{6}g{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\Δ}{V}_n=({\▿}_n+g.{\mathrm{\φ}}_{e0})t+\frac{t^2}{2}g{u}_e+\frac{t^3}{6}g{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)+V_{\mathrm{dn}}++V_{\mathrm{sn}}\end{array}\right.$

当捷联惯导***安装位置不在摇摆中心时，杆臂效应会对东向和北向速度造成影响，其原理如图2所示。上面的公式中，分别表示加速度计等效东向和北向的常值偏置；φ_e0、φ_n0、φ_u0表示初始失准角；ε_e、ε_n、ε_u表示等效东、北、天向陀螺常值漂移；ω_ie表示地球自转角速率；L表示当地纬度；g表示地球重力加速度；V_se、V_sn分别表示东向和北向随机干扰速度，V_de、V_dn表示东向和北向残余干扰速度，为常值；ΔV_e、ΔV_n分别表示东向和北向速度误差，是捷联惯导***解算的速度剔除杆臂速度后的值与外部基准提供的速度值之差，在晃动基座条件下，外部基准提供的速度为0m/s，从捷联惯导***解算的速度中剔除杆臂速度的过程即为杆臂速度的实时补偿。设杆臂长度矢量为r＝(r_x、r_y、r_z)，由杆臂效应引起的杆臂速度模型为：

$\mathrm{\δ}{v}_g={\mathrm{\ω}}_{\mathrm{ib}}^b\×r=\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_z-{\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_y\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_x-{\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_z\\ {\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_y-{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_x\end{array}\right)$

式中：δv_g表示杆臂速度；r＝(r_x、r_y、r_z)表示杆臂长度矢量，该值根据运载体的设计结构和导航***安装位置预先计算并装定入***，实际应用中，杆臂长度矢量会因为运载体挠曲变形、载荷分布变化等因素偏离该值，进而产生误差；表示三个方向上的陀螺仪输出角速率；因此本发明详细分析了残余干扰速度的误差源，所述残余干扰速度包括：***存在的随机干扰速度；因杆臂长度测量不准确、载体存在挠曲变形、陀螺仪测量的角速率存在干扰等因素导致的杆臂速度计算误差；在晃动基座条件下，捷联解算初始时刻的速度中存在杆臂速度，运行捷联解算算法时，初始速度按0m/s处理，造成的残余速度；粗对准后，初始的姿态矩阵存在误差，运行捷联解算算法时，带来的速度误差。

根据扩展变量的参数辨识法数学模型构造***方程和观测方程：

以捷联惯导***中速度误差作为观测量，把扩展变量的参数辨识法数学模型改写成如下形式：

$\left\{\begin{array}{c}\mathrm{\Δ}{V}_e=a_{1e}\left(\mathrm{KT}\right)+a_{2e}{\left(\mathrm{KT}\right)}^2+a_{3e}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\Δ}{V}_n=a_{1n}\left(\mathrm{KT}\right)+a_{2n}{\left(\mathrm{KT}\right)}^2+a_{3n}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{dn}}+V_{\mathrm{sn}}\end{array}\right.$

式中：T为捷联惯导***的传感器数据采样周期；

$\left\{\begin{array}{c}{a}_{1e}=({\▿}_e-g.{\mathrm{\φ}}_{n0}),a_{2e}=-\frac{1}{2}g{u}_n,a_{3e}=\frac{1}{6}g{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L\\ a_{1n}=({\▿}_n+g.{\mathrm{\φ}}_{e0}),a_{2n}=\frac{1}{2}g{u}_e,a_{3n}=\frac{1}{6}g{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\end{array}\right.$

把东向和北向速度误差ΔV_e、ΔV_n作为观测量，a_1e、a_2e、a_3e、V_de、a_1n、a_2n、a_3n、V_dn看作待辨识参数，构造***方程和观测方程：

定义待辨识参数，即***状态变量为：

$X_e=\left[\begin{array}{c}{a}_{1e}\\ a_{2e}\\ a_{3e}\\ V_{\mathrm{de}}\end{array}\right],$ $X_n=\left[\begin{array}{c}{a}_{1n}\\ a_{2n}\\ a_{3n}\\ V_{\mathrm{dn}}\end{array}\right]$

根据前面的分析，列出***方程和观测方程：

$\left\{\begin{array}{c}{X}_e(k+1)=X_e\left(k\right)\\ \mathrm{\Δ}{V}_e\left(k\right)=H\left(k\right)X_e\left(k\right)+V_{\mathrm{ge}}\left(k\right)\end{array}\right.,$ $\left\{\begin{array}{c}{X}_n(k+1)=X_n\left(k\right)\\ \mathrm{\Δ}{V}_n\left(k\right)=H\left(k\right)X_n\left(k\right)+V_{\mathrm{gn}}\left(k\right)\end{array}\right.$

式中：观测矩阵H(k)＝[kT，(kT)²，(kT)³，1]；V_ge(k)、V_gn(k)表示东向和北向的速度观测噪声，其方差强度为R_e、R_n。

步骤32)精对准第一阶段，根据粗对准结果，运行捷联解算程序，进行杆臂速度的实时补偿，同时根据步骤31)建立的***方程和观测方程估计***状态变量，状态变量的第四个分量即为残余干扰速度。估计状态变量的方法很多，可以通过最小二乘法递推或kalman滤波法，本发明采用改良kalman滤波法估计状态变量。基于快速性和精度的考虑，该过程一般设为3-5分钟。

本发明中所述杆臂速度的实时补偿的方法为：进行捷联解算时，每次速度更新后，把当前时刻的杆臂速度从更新后的速度信息中剔除，由杆臂效应引起的杆臂速度根据杆臂速度模型计算得到：

$\mathrm{\δ}{v}_g={\mathrm{\ω}}_{\mathrm{ib}}^b\×r=\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_z-{\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_y\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_x-{\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_z\\ {\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_y-{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_x\end{array}\right)$

状态变量X_e，X_n的值可以通过最小二乘法作递推计算得到。在递推过程中，参数逐渐向真值收敛，这个过程是从观测信息中提炼估计值的浓缩过程，随着递推越深入，浓缩过程越有效，估计值也越接近真实值。

本发明对状态变量的估计采用了改良kalman滤波法进行估计，估计算法如下：

$\left\{\begin{array}{c}{X}_i(k+1)=X_i\left(k\right)+K_i\left(k\right)e_i\left(k\right)\\ K_i\left(k\right)=P_i\left(k\right)H^T\left(k\right){\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}}^{-1}\\ P_i(k+1)=P_i\left(k\right)-K_i\left(k\right)\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}{K}_i^T\left(k\right),i=e,n;k=\mathrm{0,1,2}\·\·\·\\ R_i(k+1)=R_i\left(k\right)+(e_i^2\left(k\right)-R_i\left(k\right))/(k+1)\\ e_i\left(k\right)=\mathrm{\Δ}{V}_i\left(k\right)-H\left(k\right)X_i\left(k\right)\end{array}\right.$

i对应***状态变量X_e，X_n中的下标e，n。初始状态变量X_i(0)、初始状态估计误差的方差阵P_i(0)和初始观测噪声的方差强度R_i(0)的值均可任选。不妨取X_i(0)＝0；P_i(0)＝Iα，α为任意的正标量；R_i(0)＝0.2。算法中，计算滤波增益阵K_i(k)时利用了新息e_i(k)，这样作的好处是：即使***中的观测噪声统计特性未知，也可以根据新息值自适应的计算出当前时刻的观测噪声，并根据观测噪声来更新滤波增益阵，有利于算法的快速收敛。

对上式用递推算法可以估计出***状态变量X_e，X_n。

步骤33)精对准第二阶段：对残余干扰速度进行一次补偿；而后，继续运行精对准第一阶段的杆臂速度实时补偿以及对***状态变量的估计算法，进行初始失准角估计，待估计的初始失准角收敛后，将估计值φ_e0、φ_n0、φ_u0代入失准角变化规律模型中计算当前时刻的失准角φ_e、φ_n、φ_u；利用当前时刻的失准角信息对粗对准结果进行一步修正，获得当前时刻的姿态矩阵通过姿态矩阵提取方位角H、纵摇角P和横摇角R，精对准完成。基于快速性和精度的考虑，该过程一般设为5分钟。具体如下：

***状态变量估计出来后，算出u_e、u_n、u_u、φ_e0、φ_n0和φ_u0：

$u_e=\frac{2a_{2n}}{g},$ $u_n=-\frac{2a_{2e}}{g},$ $u_u=-\frac{6a_{3n}}{g{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{2a_{2e}}{g}\tan L$

${\mathrm{\φ}}_{e0}=\frac{a_{1n}}{g},$ ${\mathrm{\φ}}_{n0}=-\frac{a_{1e}}{g},$ ${\mathrm{\φ}}_{u0}={\mathrm{\φ}}_{n0}.\tan L-\frac{u_e+{\mathrm{\ϵ}}_e}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}$

因为等效东向陀螺漂移ε_e未知，天向失准角的初值用下式近似计算：

${\mathrm{\φ}}_{u0}\≈{\mathrm{\φ}}_{n0}.\tan L-\frac{u_e}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}$

由于在计算中忽略了传感器的常值漂移，对失准角的初值估计存在误差，误差值的表达式为：

$\left\{\begin{array}{c}\mathrm{\δ}{\mathrm{\φ}}_{e0}=-\frac{{\▿}_n}{g}\\ \mathrm{\δ}{\mathrm{\φ}}_{n0}=\frac{{\▿}_e}{g}\\ \mathrm{\δ}{\mathrm{\φ}}_{u0}=-\frac{{\mathrm{\ϵ}}_e}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}\end{array}\right.$

粗对准结束后，由导航计算机建立的导航系n′与理想的导航系n存在失准角φ_e、φ_n、φ_u，它们都是小角，矢量φ＝(φ_eφ_nφ_u)可以看作是n系到n′系的等效旋转矢量，n系到n′系的姿态转移矩阵可以由下式表示：

$C_{n^{\′}}^n=I+(\mathrm{\φ}\×)=\left(\begin{array}{ccc}0& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 0& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 0\end{array}\right)$

对粗对准结果的一步修正为：

$C_b^n=C_{n^{\′}}^n{C}_b^{n^{\′}}=[I+(\mathrm{\φ}\×)]C_b^{n^{\′}},$ 其中 $C_{n^{\′}}^n=I+(\mathrm{\φ}\×)=\left(\begin{array}{ccc}0& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 0& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 0\end{array}\right)$

对残余干扰速度进行一次补偿方法为：精对准第一阶段结束后，估计出的残余干扰速度是常值，精对准第二阶段一开始，将残余的干扰速度从捷联解算出的速度信息中剔除，方法为：

$\left\{\begin{array}{c}{V^{\′}}_e=V_e-V_{\mathrm{de}}\\ {V^{\′}}_n=V_n-V_{\mathrm{dn}}\end{array}\right.$

式中：V′_e、V′_n表示剔除残余干扰速度后的东向和北向速度；V_e、V_n表示捷联解算的东向和北向速度；V_de、V_dn表示东向和北向残余干扰速度。

步骤4)导航解算阶段，进行杆臂速度的实时补偿，根据步骤33)得到的姿态矩阵运行捷联解算算法，提供导航结果。

对本发明的方法进行仿真实验：

仿真条件：运载体处于水平晃动基座条件下，晃动幅度和频率分别为：0.17°/1Hz；***中考虑杆臂影响，杆臂长度分别为：0.456m、-0.456m、22.546m；三个方向的陀螺常值漂移均为0.15°/h，加表常值偏置为0.11mg，纬度为31183°，载体姿态为：俯仰角2°，横滚角3°。粗对准结束后，在航向为90°的位置进行扩展变量的参数辨识法精对准，为了验证对***残余干扰速度的估计精度，进行了两组实验。第一组实验执行精对准第一阶段算法，运行5分钟；第二组实验执行精对准第二阶段算法，运行5分钟。

第一组实验：设***存在初始的速度误差，即存在残余干扰速度，运行扩展变量的参数辨识法精对准程序后，估计出初始干扰速度，对准结果如图4、图5所示：

第一组实验结果：粗对准结果为：航向误差δH＝-38.76′，δP＝0.61′，δR＝-0.21′，对准后的姿态矩阵和理论的姿态矩阵的距离dis(C)＝0.016。参数辨识法精准结果为：航向误差δH＝-42′，δP＝0.17′，δR＝-20′，对准后的姿态矩阵和理论的姿态矩阵的距离dis(C)＝0.017。因为第一组实验时，***存在初始的干扰速度，即存在残余干扰速度，致使精对准过程存在干扰影响。该方法估计的东向干扰速度为0.4138m/s，北向干扰速度大约为-0.4245m/s。

第二组实验，在前一组实验的基础上把估计出来的干扰速度在精对准第二阶段进行一次补偿，对准效果如图6、图7所示：

第二组实验参数辨识法精准结果为：航向误差δH＝-38.94′，δP＝-0.15′，δR＝-0.20′，对准后的姿态矩阵和理论的姿态矩阵的距离dis(C)＝0.0016，相比粗对准和第一阶段精对准结果，精度有所提高。因为第二组仿真实验对第一组实验估计的初始干扰速度进行了补偿，失准角估计误差很快收敛到比较小的值，提高了对准精度。根据实验结果，说明第一组实验估计的初始杆臂干扰速度有效。在第二组实验中，因为已经对残余干扰速度进行了估计和补偿，***中理论的残余的干扰速度为：东向0m/s、北向0m/s。在这组试验中，对***中残存的干扰速度进行估计，估计结果为：东向干扰速度为0.00045m/s，北向干扰速度大约为-0.0006m/s，和理论的0m/s、0m/s很接近，估计精度超过95％。

Claims (1)

1.一种捷联惯导***晃动基座条件下杆臂效应的估计和补偿方法，其特征是在晃动基座条件下，估计杆臂效应对捷联惯导***的影响并进行补偿，包括以下步骤：

1）捷联惯导***开机预热，采集惯性测量组件的输出数据；

2）进行粗对准，得到粗略的初始姿态矩阵

3）在粗对准基础上分两个阶段完成精对准过程；

31）建立扩展变量的参数辨识法精对准数学模型，构造***方程和观测方程；扩展变量的参数辨识法精对准数学模型：

$\left\{\begin{array}{c}\mathrm{\Δ}{V}_e=({\▿}_e-g\·{\mathrm{\φ}}_{n0})t-\frac{t^2}{2}{\mathrm{gu}}_n+\frac{t^3}{6}g{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\Δ}{V}_n=({\▿}_n+g\·{\mathrm{\φ}}_{e0})t+\frac{t^2}{2}{\mathrm{gu}}_e+\frac{t^3}{6}g{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)+V_{\mathrm{dn}}+V_{\mathrm{sn}}\end{array}\right.$

其中 $\left\{\begin{array}{c}{u}_e={\mathrm{\φ}}_{n0}{\mathrm{\ω}}_{\mathrm{ie}}\sin L-{\mathrm{\φ}}_{u0}{\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\ϵ}}_e\\ u_n=-{\mathrm{\φ}}_{e0}{\mathrm{\ω}}_{\mathrm{ie}}\sin L-{\mathrm{\ϵ}}_n\\ u_u={\mathrm{\φ}}_{e0}{\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\ϵ}}_u\end{array}\right.$

式中：分别表示加速度计等效东向和北向的常值偏置；φ_e0、φ_n0、φ_u0表示初始失准角；ε_e、ε_n、ε_u表示等效东、北、天向陀螺常值漂移；ω_ie表示地球自转角速率；L表示当地纬度；g表示地球重力加速度；V_se、V_sn分别表示东向和北向随机干扰速度，V_de、V_dn表示东向和北向残余干扰速度，为常值；ΔV_e、ΔV_n分别表示东向和北向速度误差，是捷联惯导***解算的速度剔除杆臂速度后的值与外部基准提供的速度值之差，在晃动基座条件下，外部基准提供的速度为0m/s，从捷联惯导***解算的速度中剔除杆臂速度的过程即为杆臂速度的实时补偿，由杆臂效应引起的杆臂速度根据杆臂速度模型计算得到：

${\mathrm{\δv}}_g={\mathrm{\ω}}_{\mathrm{ib}}^b\×r=\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_z-{\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_y\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_x-{\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_z\\ {\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_y-{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_x\end{array}\right)$

式中：δv_g表示杆臂速度；r=(r_x、r_y、r_z)表示杆臂长度矢量，该值根据运载体的设计结构和导航***安装位置预先计算并装定入***，实际应用中，杆臂长度矢量会因为运载体挠曲变形、载荷分布变化等因素偏离该值；表示三个方向上的陀螺仪输出角速率；

所述残余干扰速度包括：***存在的随机干扰速度；因杆臂长度测量不准确、载体存在挠曲变形、陀螺仪测量的角速率存在干扰等因素导致的杆臂速度计算误差；在晃动基座条件下，捷联解算初始时刻的速度中存在杆臂速度，运行捷联解算算法时，初始速度按0m/s处理，造成的残余速度；粗对准后，初始的姿态矩阵存在误差，运行捷联解算算法时，带来的速度误差；

根据扩展变量的参数辨识法数学模型，构造***方程和观测方程为：

以捷联惯导***中速度误差为观测量，把扩展变量的参数辨识法数学模型改写成如下形式：

$\left\{\begin{array}{c}\mathrm{\Δ}{V}_e=a_{1e}\left(\mathrm{KT}\right)+a_{2e}{\left(\mathrm{KT}\right)}^2+a_{3e}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\Δ}{V}_n=a_{1n}\left(\mathrm{KT}\right)+a_{2n}{\left(\mathrm{KT}\right)}^2+a_{3n}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{dn}}+V_{\mathrm{sn}}\end{array}\right.,K=\mathrm{0,1,2},...$

式中：T为捷联惯导***的传感器数据采样周期；

$\left\{\begin{array}{c}{a}_{1e}=({\▿}_e-g\·{\mathrm{\φ}}_{n0}),a_{2e}=-\frac{1}{2}{\mathrm{gu}}_n,a_{3e}=\frac{1}{6}g{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L\\ a_{1n}=({\▿}_n+g\·{\mathrm{\φ}}_{e0}),a_{2n}=\frac{1}{2}{\mathrm{gu}}_e,a_{3n}=\frac{1}{6}g{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\end{array}\right.$

把东向和北向速度误差△V_e、△V_n作为观测量，a_1e、a_2e、a_3e、V_de、a_1n、a_2n、a_3n、V_dn作为待辨识参数，构造***方程和观测方程：

定义待辨识参数，即***状态变量为：

$X_e=\left[\begin{array}{c}{a}_{1e}\\ a_{2e}\\ a_{3e}\\ a_{\mathrm{de}}\end{array}\right],X_n=\left[\begin{array}{c}{a}_{1n}\\ a_{2n}\\ a_{3n}\\ V_{\mathrm{dn}}\end{array}\right]$

列出***方程和观测方程：

$\left\{\begin{array}{c}{X}_e(k+1)=X_e\left(k\right)\\ \mathrm{\Δ}{V}_e\left(k\right)=H\left(k\right)X_e\left(k\right)+V_{\mathrm{ge}}\left(k\right)\end{array}\right.,\left\{\begin{array}{c}{X}_n(k+1)=X_n\left(k\right)\\ \mathrm{\Δ}{V}_n\left(k\right)=H\left(k\right)X_n\left(k\right)+V_{\mathrm{gn}}\left(k\right)\end{array}\right.$

式中：观测矩阵H(k)=[kT,(kT)²,(kT)³,1]；V_ge(k)、V_gn(k)表示东向和北向的速度观测噪声，其方差强度为R_e、R_n；

32）精对准第一阶段，根据粗对准结果，运行捷联解算程序，进行杆臂速度的实时补偿，同时根据步骤31）建立的***方程和观测方程估计***状态变量，状态变量的第四个分量即为残余干扰速度；其中，采用改良kalman滤波法估计***状态变量：

$\left\{\begin{array}{c}{X}_i(k+1)=X_i\left(k\right)+K_i\left(k\right)e_i\left(k\right)\\ K_i\left(k\right)=P_i\left(k\right)H^T\left(k\right){\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}}^{-1}\\ P_i(k+1)=P_i\left(k\right)-K_i\left(k\right)\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}{K}_i^T\left(k\right)\\ R_i(k+1)=R_i\left(k\right)+(e_i^2\left(k\right)-R_i\left(k\right))/(k+1)\\ e_i\left(k\right)=\mathrm{\Δ}{V}_i\left(k\right)-H\left(k\right)X_i\left(k\right)\end{array}\right.,i=e,n;k=\mathrm{0,1,2}...$

i对应表示***状态变量X_e，X_n中的下标e，n，初始状态变量X_i(0)、初始状态估计误差方差阵P_i(0)和初始观测噪声方差强度R_i(0)的值均可任选，

对上式用递推算法估计出***状态变量X_e，X_n；

33）精对准第二阶段：对残余干扰速度进行一次补偿，方法为：精对准第一阶段结束后，估计出的残余干扰速度是常值，精对准第二阶段一开始，将残余的干扰速度从捷联解算出的速度信息中剔除，方法为：

$\left\{\begin{array}{c}{V^{\′}}_e=V_e-V_{\mathrm{de}}\\ {V^{\′}}_n=V_n-V_{\mathrm{dn}}\end{array}\right.$

式中：V'_e、V'_n表示剔除残余干扰速度后的东向和北向速度；V_e、V_n表示捷联解算的东向和北向速度；V_de、V_dn表示东向和北向残余干扰速度；

而后，继续运行精对准第一阶段的杆臂速度实时补偿以及对***状态变量的估计算法，并进行初始失准角估计，待估计的初始失准角收敛后，将估计值φ_e0、φ_n0、φ_u0代入失准角变化规律模型中计算当前时刻的失准角φ_e、φ_n、φ_u；利用当前时刻的失准角信息对粗对准结果进行一步修正，获得当前时刻的姿态矩阵通过姿态矩阵提取方位角H、纵摇角P和横摇角R，精对准完成；

其中，根据步骤32）估计得到的状态变量计算u_e、u_n、u_u、φ_e0、φ_n0和φ_u0：

$u_e=\frac{2a_{2n}}{g},u_n=-\frac{2a_{2e}}{g},u_u=-\frac{6a_{3n}}{g{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{2a_{2e}}{g}\tan L$

${\mathrm{\φ}}_{e0}=\frac{a_{1n}}{g},{\mathrm{\φ}}_{n0}=-\frac{a_{1e}}{g},{\mathrm{\φ}}_{u0}={\mathrm{\φ}}_{n0}.\tan L-\frac{u_e}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}$

根据初始失准角和失准角变化规律模型计算当前时刻失准角：

$\left\{\begin{array}{c}{\mathrm{\φ}}_e={\mathrm{\φ}}_{e0}+u_e.t+\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\\ {\mathrm{\φ}}_n={\mathrm{\φ}}_{n0}+u_n.t-\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L\\ {\mathrm{\φ}}_u={\mathrm{\φ}}_{u0}+u_u.t+\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\cos L\end{array}\right.$

对粗对准结果的一步修正为：

$C_b^n=C_{n^{\′}}^n{C}_b^{n^{\′}}=[I+(\mathrm{\φ}\×)]C_b^{n^{\′}},$ 其中 $C_{n^{\′}}^n=I+(\mathrm{\φ}\×)=\left(\begin{array}{ccc}0& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 0& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 0\end{array}\right)$

4）导航解算阶段，进行杆臂速度的实时补偿，根据步骤33）得到的姿态矩阵运行捷联解算算法，提供导航结果；

步骤32）、33）和4）中，杆臂速度的实时补偿为：进行捷联解算时，每次速度更新后，把当前时刻的杆臂速度从更新后的速度信息中剔除，由杆臂效应引起的杆臂速度根据杆臂速度模型计算得到：

${\mathrm{\δv}}_g={\mathrm{\ω}}_{\mathrm{ib}}^b\×r=\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_z-{\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_y\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_x-{\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_z\\ {\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_y-{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_x\end{array}\right).$