CN110044385A - 一种大失准角情况下的快速传递对准方法 - Google Patents

Abstract

本发明公开了一种大失准角情况下的快速传递对准方法，首先用主子惯导所获得的速度信息、姿态信息作为传递对准算法的输入项，经过滤波算法处理后，得到子惯导作为失准角的速度误差与姿态误差，对速度误差与姿态误差进行实时补偿，从而实现传递对准；本发明所设计的传递对准方法，根据传递对准不同阶段的不同特征，采用不同的滤波输入与滤波模型实现传递对准，在大失准角阶段采用非线性模型保证传递对准的准确性；在小失准角阶段采用线性模型提升传递对准计算效率；有效解决了传统传递对准方法只能使用小失准角情况下的问题。

Description

一种大失准角情况下的快速传递对准方法

技术领域

本发明涉及惯性导航领域，具体涉及一种大失准角情况下的快速传递对准方法。

背景技术

随着现代战争对武器装备精确打击能力和快速反应能力的要求日益提高，当前绝大部分车载、机载、舰载武器系统均以捷联惯性导航系统(Strapdown InertialNavigation System，SINS)为核心导航设备，SINS的导航精度很大程度上决定了武器系统的最终打击精度。而对于武器系统而言，考虑到系统安全性，无法采用卫星导航等有源导航手段来对惯性导航进行辅助，此时，惯性导航精度主要取决于自身导航精度。而由于惯性导航采用积分推算原理，高精度初始位姿是获取高精度惯性导航定位数据的关键。目前一般采用传递对准技术对武器系统进行初始对准，即利用舰船、飞机、车辆上已对准好、精度高的惯导系统(主惯导系统)来对准武器系统上的惯导系统(子惯导系统)。传递对准过程可看作将主子惯导两套系统进行数据比对和匹配的过程，传递对准结果可通过对准精度和时间两个至关重要的因素来评判。子惯导系统在完成传递对准这一初始对准过程以后才可以进入正常导航状态。目前主流的传递对准均是基于小角度假设(即子惯导系统的失准角是小角度)，从而可以利用线性滤波方法进行解决，然而，在实际使用中，很多情况下是无法满足小角度假设的，即失准角并非小量误差。针对大失准角对准时，目前采用的方法主要有两种，一是传递对准姿态装订时，对子惯导进行安装角补偿，以期满足小角度要求，然后进行传统线性模型传递对准；二是采用非线性滤波模型和滤波算法，很多学者建立了以姿态角、姿态四元数或旋转矢量为姿态描述的非线性误差模型，可适用于大失准角下的传递对准。然而第一种方法的算法稳定性较差，容易出现算法发散的问题，较难应用于工程实际；而第二种方法存在计算量较大、收敛时间较长、在大失准角下估计度不高等问题。

发明内容

本发明拟提出一种大失准角情况下的快速传递对准方法，针对大失准角情况下的传递对准，采用“速度+姿态”匹配的传递对准滤波方法；在此基础上，根据传递对准算法运行过程中失准角不同的收敛情况，进一步将传递对准流程分割为两个阶段，依据不同阶段传递对准的不同情况与不同特征，采用不同的滤波模型和算法架构予以应对，实现了在大失准角情况下，快速且准确的传递对准。

为了实现上述目的，本发明的技术方案是：

一种大失准角情况下的快速传递对准方法，首先用主子惯导所获得的速度信息、姿态信息作为传递对准算法的输入项，经过滤波算法处理后，得到子惯导作为失准角的速度误差与姿态误差，对速度误差与姿态误差进行实时补偿，从而实现传递对准，其中，所述对速度误差与姿态误差进行补偿是：在传递对准开始时，首先，将子惯导速度误差与姿态误差之间的非线性耦合关系作为滤波模型，将子惯导速度观测误差作为滤波算法的输入量展开滤波计算，得到子惯导的速度误差与姿态误差后进行实时补偿；随着传递对准的运行，当失准角位于小失准角范畴时，将子惯导速度观测误差和姿态观测误差作为滤波算法的输入量，将子惯导速度误差与姿态误差之间的线性化后的耦合关系作为线性滤波模型，展开滤波计算，得到子惯导的速度误差与姿态误差后进行实时补偿。

方案进一步是：所述作为失准角的速度误差与姿态误差是通过将主惯导导航输出的速度和姿态变量转换为子惯导的外参考输出获得的，获得的过程是：

第一步：确定主惯导与子惯导的速度转换关系式，

式中，

为子惯导在导航北东地坐标系中表示的外参考三轴速度，

为主惯导输出在导航坐标系中表示的三轴速度，

为杆臂速度输出在导航坐标系中表示的补偿项，其中，的计算如下：

其中：

代表主惯导由载体坐标系转换至导航坐标系输出的姿态转换矩阵；

代表主惯导中的三轴陀螺仪所输出的载体坐标系相对于惯性坐标系的旋转角速度在主惯导载体坐标系中的表示；

r^bm代表主惯导质心和子惯导质心的杆臂距离在在主惯导载体坐标系中的表示；

×代表向量叉乘运算；

第二步：确定主惯导与子惯导的姿态转换关系式：

式中：

C_ref为子惯导的外参考姿态；

为主惯导由载体坐标系转换至导航坐标系输出的姿态转换矩阵；

为子惯导载体坐标系到主惯导坐标系的转换矩阵；

第三步：在得到和C_ref的基础上，将转换得到的子惯导的外参考输出与子惯导真实输出做差，得到的差值即为子惯导三轴速度误差和子惯导三轴姿态误差；

所述子惯导三轴速度误差Z_V通过关系式获取，

其中，为子惯导输出的三轴速度在导航坐标系中的表示；

所述子惯导三轴姿态误差Z_θ通过关系式获取，

Z_DCM的计算关系式为

其中，为子惯导输出由载体坐标系转换至导航坐标系的姿态转换矩阵。

方案进一步是：所述将子惯导速度误差与姿态误差之间的非线性耦合关系作为滤波模型，展开滤波计算，得到子惯导的速度误差与姿态误差的步骤包括：

一，取子惯导的姿态误差角、速度误差和子惯导的三轴加速度计零偏作为状态变量，为：

式中：

为在导航坐标系下的子惯导姿态误差，依次代表在导航坐标系X，Y，Z轴上的误差角度；

为在导航坐标系下的子惯导速度误差；

代表子惯导的三轴加速度计零偏；

二，确定连续情形下的状态转移部分非线性滤波模型，为：

其中：

I代表3×3维数的单位矩阵；

为由载体坐标系转换至导航坐标系的子惯导输出的姿态转换矩阵；

代表子惯导所敏感到的比力信息在子惯导载体坐标系下的投影；

代表子惯导所敏感到的角速度信息在子惯导载体坐标系下的投影；

与W₁ ^v分别对应于对应的系统过程噪声；

三，确定连续情形下的观测向量部分非线性滤波模型，为：

其中：

Z¹代表量测值；

V¹对应于系统量测噪声；

四，对应于二和三的连续情况下非线性滤波模型进行离散化，同时采用相应的非线性滤波算法得到在k时刻的失准角的估计结果：速度误差的估计结果：加速度计零偏的估计结果：

利用误差估计结果与对子惯导的计算结果进行修正，同时在修正完成后，将估计得到的姿态误差与速度误差与进行置零处理。

方案进一步是：所述失准角是否位于小失准角范畴的检测方法是：定义利用所述非线性模型滤波算法得到的k时刻加速度计零偏估计结果的变化值D(k)为：

其中，||代表求取对应的绝对值；

对加速度计零偏的估计结果进行监控，如果连续20个时刻中，加速度计零偏估计结果的变化值均小于预设阈值，则认为进入平稳状态，即：

对于整数i从1递增至20，下式始终成立：

D(k-i+1)<5×10^-3

则认为进入平稳状态。

方案进一步是：所述将子惯导速度误差与姿态误差之间的线性化后的耦合关系作为线性滤波模型，展开滤波计算，得到子惯导的速度误差的步骤包括：

一，取子惯导的姿态误差角、速度误差、陀螺零偏、加速度计零偏、加速度计刻度因数误差、加速度计不正交的安装误差和安装误差角作为线性滤波模型的状态量，为，

式中：

是作为失准角的子惯导姿态误差；

为子惯导速度误差；

为子惯导陀螺三轴常值漂移；

为子惯导加速度计三轴常值漂移；

为子惯导加速度计三轴常值刻度因数误差；

为子惯导加速度计三轴不正交误差；

μ^bh为实际弹体相对于名义安装位置的安装误差角；

二，确定线性滤波模型对应的状态线性方程，为：

Z＝HX+V

式中W代表对应维数的系统过程噪声，V代表对应维数的系统量测噪声

其中：

为子惯导计算得到的bs系至n系的转换矩阵；

f_bs为子惯导敏感到的三轴比力在子惯导载体坐标系下的投影，

f_bs(1),f_bs(2),f_bs(3)分别为f_bs的第1，2，3个元素；

()×代表求取对应向量的反对称矩阵；

I_3×3代表3行3列的单位矩阵；

0_3×3代表3行3列的零矩阵；

三，对上述线性滤波模型进行离散化，得到k时刻离散线性模型公式：

X(k)＝φ(k/k-1)X(k-1)+W(k-1)

Z(k)＝H(k)X(k)+V(k)

用Kalman滤波器对k时刻离散线性模型公式进行传递对准滤波计算得到滤波结果；然后对子惯导姿态误差与速度误差进行修正即，在修正完成后，将上述kalman滤波器中的估计得到的姿态误差与速度误差进行置零，即将上述线性滤波模型中的子惯导姿态误差与子惯导速度误差进行置零处理。

本发明的有益效果是：

(1)本发明所设计的一种大失准角情况下的快速传递对准方法，可以在大失准角情况下实现传递对准，有效解决了传统传递对准方法只能使用小失准角情况下的问题。

(2)本发明所设计的传递对准方法，根据传递对准不同阶段的不同特征，采用不同的滤波输入与滤波模型实现传递对准，在大失准角阶段采用非线性模型保证传递对准的准确性；在小失准角阶段采用线性模型提升传递对准计算效率。

(3)本发明所设计的传递对准方法，在充分分析惯导误差传播机理的基础上，采用了两种滤波模型：非线性滤波模型与全维线性滤波模型，这两套滤波模型均保证原有误差传播机理不变，其中，非线性滤波模型可以在大失准角情况下准确的完成误差估计与补偿，但是算法计算量较大，同时考虑到非线性滤波模型的使用阶段对滤波精度的要求较低，为了减小系统计算开销，非线性滤波模型在建立的过程中牺牲了一部分模型精度(对一些小量误差予以忽略)，从而实现了模型简化；而线性滤波模型根据其使用阶段传递对准的特点，对系统做了线性化近似，从而缩减算法计算量，提升计算效率；通过两套滤波模型切换使用的模式，在保证传递对准精度的同时，有效的提升了传递对准算法的计算效率。

下面结合附图和实施例对本发明进行详细描述。

附图说明

图1为方法整体流程图；

图2为方法整体算法时序图；

图3为子惯导系统在传递对准模式下的速度误差；

图4为子惯导系统在传递对准模式下的姿态误差；

图5为子惯导系统在传递对准模式下的加速度计零偏估计结果。

具体实施方式

一种大失准角情况下的快速传递对准方法，所述大失准是子惯导器件误差值处于不稳定，不是在一个稳定值附近反复震荡的情况，首先用主子惯导所获得的速度信息、姿态信息作为传递对准算法的输入项，经过滤波算法处理后，得到子惯导作为失准角的速度误差与姿态误差，对速度误差与姿态误差进行实时补偿，从而实现传递对准，其中，所述对速度误差与姿态误差进行补偿是：在传递对准开始时，首先，将子惯导速度误差与姿态误差之间的非线性耦合关系作为滤波模型，将子惯导速度观测误差作为滤波算法的输入量展开滤波计算，得到子惯导的速度误差与姿态误差后进行实时补偿；随着传递对准的运行，当失准角位于小失准角范畴时，将子惯导速度观测误差和姿态观测误差作为滤波算法的输入量，将子惯导速度误差与姿态误差之间的线性化后的耦合关系作为线性滤波模型，展开滤波计算，得到子惯导的速度误差与姿态误差后进行实时补偿。

所述方法整体流程图如图1所示。方法整体算法时序图如图2所示。此外，定义本实施例中所使用的坐标系定义如下：导航坐标系为：东北天，载体坐标系为：右前上.

实施例针对大失准角情况下的传递对准，采用“速度+姿态”匹配的传递对准滤波方法；在此基础上，根据传递对准算法运行过程中失准角不同的收敛情况，进一步将传递对准流程分割为两个阶段，依据不同阶段传递对准的不同情况与不同特征，采用不同的滤波模型和算法架构予以应对，实现了在大失准角情况下，快速且准确的传递对准。其核心思想是：首先采用“速度+姿态”的匹配方式，即利用主子惯导所获得的速度信息、姿态信息作为传递对准算法的输入项，经过滤波算法处理后，得到子惯导的速度误差与姿态误差(失准角)，进而对速度误差与姿态误差进行补偿，从而达到提升子惯导初始导航精度的目的。在此基础上，设计了两阶段传递对准方案：

阶段1：传递对准开始时，处于大失准角状态，将“速度”作为滤波算法的输入量，将子惯导速度误差与姿态误差之间的非线性耦合关系作为滤波模型，展开滤波计算，得到子惯导的速度误差与姿态误差后进行实时补偿，使得子惯导误差迅速收敛；

阶段2：传递对准运行一段时间后，子惯导姿态误差逐渐减小，失准角可以认为已经收敛至小失准角范畴，体现为：由传递对准计算得到的子惯导器件误差值已经趋近于稳定，出现了在稳定值附近反复震荡的情况，此时可以认为失准角基本上已经收敛，但是收敛精度不足。则此时将“速度+姿态”作为滤波算法的输入量，将子惯导速度误差与姿态误差之间的线性化后的耦合关系(此时认为失准角满足小角度近似)作为滤波模型，展开滤波计算，得到子惯导的速度误差与姿态误差后进行实时补偿，使得子惯导误差进一步收敛，以满足预设的滤波精度。所设计的两阶段的传递对准方案阶段1，此时处于大失准角状态，必须采用非线性滤波算法，此时仅采用“速度”的匹配模式，利用速度信息实现失准角的快速收敛，此外，由于阶段1的最终目的是使得系统由大失准角状态快速进入小失准角状态，对于滤波精度的要求相对较低，因此为了减小计算开销，在所采用的非线性滤波模型中，对一些小量误差予以忽略处理；阶段2，此时已经脱离大失准角状态，可以采用线性化模型进行计算，如果继续采用大失准角情况下的滤波模型进行计算会面临计算量大，收敛时间长等问题，为了节约系统计算开销，此时则采用全维数的线性化滤波模型进行传递对准计算，以满足传递对准预设估计精度的要求。

对计算传递对准而言，首先需要实现的是对主惯导的输出进行转换/补偿，将其转换为子惯导的外参考信息，进而利用子惯导输出与子惯导的外参考信息做差值，得到子惯导输出误差，将该输出误差作为kalman滤波器的输入信息，利用kalman滤波器完成对子惯导姿态误差、速度误差、惯性器件误差的在线估计，进而实现误差的在线补偿。在设计的两个传递对准阶段中，由主惯导信息转换为子惯导外参考信息的转换方式均为一样，区别仅在于每个阶段会选择不同的外参考信息、不同的滤波模型与不同的滤波方法进行滤波处理。

其中：所述作为失准角的速度误差与姿态误差是通过将主惯导导航输出的速度和姿态变量转换为子惯导的外参考输出获得的，主惯导的导航输出包含速度和姿态两部分，需要将其转换为子惯导的外参考速度与外参考姿态，具体操作如下：

第一步：确定主惯导与子惯导的速度转换关系式，

式中，

为子惯导在导航北东地坐标系中表示的外参考三轴速度，

为主惯导输出在导航坐标系中表示的三轴速度，

为杆臂速度输出在导航坐标系中表示的补偿项，其中，的计算如下：

其中：

代表主惯导由载体坐标系转换至导航坐标系输出的姿态转换矩阵；

代表主惯导中的三轴陀螺仪所输出的载体坐标系相对于惯性坐标系的旋转角速度在主惯导载体坐标系中的表示；

r^bm代表主惯导质心和子惯导质心的杆臂距离在在主惯导载体坐标系中的表示；

×代表向量叉乘运算；

第二步：确定主惯导与子惯导的姿态转换关系式：

式中：

C_ref为子惯导的外参考姿态；

为主惯导由载体坐标系转换至导航坐标系输出的姿态转换矩阵；

为子惯导载体坐标系到主惯导坐标系的转换矩阵；

第三步：在得到和C_ref的基础上，将转换得到的子惯导的外参考输出与子惯导真实输出做差，得到的差值即为子惯导三轴速度误差和子惯导三轴姿态误差；

所述子惯导三轴速度误差Z_V通过关系式获取，

其中，为子惯导输出的三轴速度在导航坐标系中的表示；

所述子惯导三轴姿态误差Z_θ通过关系式获取，

Z_DCM的计算关系式为

其中，为子惯导输出由载体坐标系转换至导航坐标系的姿态转换矩阵。

在得到Z_V和Z_θ的基础上，执行后续流程的计算。

步骤1：传递对准算法开始运行，首先执行阶段1算法，将“速度”作为滤波算法的输入量，将子惯导速度误差与姿态误差之间的非线性耦合关系作为滤波模型，展开滤波计算，得到速度误差与姿态误差后进行实时反馈补偿。

在本步骤中，主要利用“速度”信息(即子惯导速度观测误差(Z_V))作为量测输入，以子惯导速度误差与姿态误差之间的非线性耦合关系(非线性滤波模型)作为滤波模型展开计算，即：

所述将子惯导速度误差与姿态误差之间的非线性耦合关系作为滤波模型，展开滤波计算，得到子惯导的速度误差与姿态误差的步骤包括：

一，取子惯导的姿态误差角、速度误差和子惯导的三轴加速度计零偏作为状态变量，为：

式中：

为在导航坐标系下的子惯导姿态误差，依次代表在导航坐标系X，Y，Z轴上的误差角度；

为在导航坐标系下的子惯导速度误差；

代表子惯导的三轴加速度计零偏；

二，确定连续情形下的状态转移部分非线性滤波模型，为：

其中：

I代表3×3维数的单位矩阵；

为由载体坐标系转换至导航坐标系的子惯导输出的姿态转换矩阵；

代表子惯导所敏感到的比力信息在子惯导载体坐标系下的投影；

代表子惯导所敏感到的角速度信息在子惯导载体坐标系下的投影；

与W₁ ^v分别对应于对应的系统过程噪声；

三，确定连续情形下的观测向量部分非线性滤波模型，为：

其中：

Z¹代表量测值；

V¹对应于系统量测噪声；

四，对应于二和三的连续情况下非线性滤波模型进行离散化，同时采用相应的非线性滤波算法得到在k时刻的失准角的估计结果：速度误差的估计结果：加速度计零偏的估计结果：

利用误差估计结果与对子惯导的计算结果进行修正，同时在修正完成后，将估计得到的姿态误差与速度误差与进行置零处理。

步骤2：对步骤1中滤波得到的惯性器件误差的进行实时监测，当器件误差估计值变化较为平稳时，进入阶段2的传递对准运算，即：

所述失准角是否位于小失准角范畴的检测方法是：定义利用所述非线性模型滤波算法得到的k时刻加速度计零偏估计结果的变化值D(k)为：

其中，||代表求取对应的绝对值；

对加速度计零偏的估计结果进行监控，如果连续20个时刻中，加速度计零偏估计结果的变化值均小于预设阈值，则认为进入平稳状态，即：

对于整数i从1递增至20，下式始终成立：

D(k-i+1)<5×10^-3

则认为进入平稳状态。

步骤3：执行阶段2的传递对准运算，将“速度+姿态”作为滤波算法的输入量，将子惯导速度误差与姿态误差之间的线性化后的耦合关系作为滤波模型，展开滤波计算，得到子惯导的速度误差与姿态误差后进行实时反馈补偿。

在本步骤中，主要利用“速度+姿态”信息(即子惯导速度观测误差与姿态观测误差(Z_V和Z_θ))作为量测输入，即：

所述将子惯导速度误差与姿态误差之间的线性化后的耦合关系作为线性滤波模型，展开滤波计算，得到子惯导的速度误差的步骤包括：

一，取子惯导的姿态误差角、速度误差、陀螺零偏、加速度计零偏、加速度计刻度因数误差、加速度计不正交的安装误差和安装误差角作为线性滤波模型的状态量，为，

式中：

是作为失准角的子惯导姿态误差；

为子惯导速度误差；

为子惯导陀螺三轴常值漂移；

为子惯导加速度计三轴常值漂移；

为子惯导加速度计三轴常值刻度因数误差；

为子惯导加速度计三轴不正交误差；

μ^bh为实际弹体相对于名义安装位置的安装误差角；

二，确定线性滤波模型对应的状态线性方程，为：

Z＝HX+V

式中W代表对应维数的系统过程噪声，V代表对应维数的系统量测噪声

其中：

为子惯导计算得到的bs系至n系的转换矩阵；

f_bs为子惯导敏感到的三轴比力在子惯导载体坐标系下的投影，

f_bs(1),f_bs(2),f_bs(3)分别为f_bs的第1，2，3个元素；

()×代表求取对应向量的反对称矩阵；

I_3×3代表3行3列的单位矩阵；

0_3×3代表3行3列的零矩阵；

三，对上述线性滤波模型进行离散化，得到k时刻离散线性模型公式：

X(k)＝φ(k/k-1)X(k-1)+W(k-1)

Z(k)＝H(k)X(k)+V(k)

用Kalman滤波器对k时刻离散线性模型公式进行传递对准滤波计算得到滤波结果；然后对子惯导姿态误差与速度误差进行修正即，在修正完成后，将上述kalman滤波器中的估计得到的姿态误差与速度误差进行置零，即将上述线性滤波模型中的子惯导姿态误差与子惯导速度误差进行置零处理。

步骤4：结束传递对准计算，输出最终计算结果。

采用本实施例所示大失准角情况下的快速传递对准方法的进行仿真验证，传递对准结果如图3～图5所示。其中图3为子惯导系统在传递对准模式下的速度误差，图4为子惯导系统在传递对准模式下的姿态误差，图5为子惯导系统在传递对准模式下的加速度计零偏估计结果。从图3中可以看出，在传递对准模式下三轴速度误差均被有效的抑制在一个小的范围震荡，且震荡范围的量级较小，处于可接受范围内。从图4中可以看出，在实施例中所设计的传递对准模式下，三轴姿态误差开始位于大失准角情况下，利用阶段1中的方法可以较快收敛至0附近，而当检测到加速度计零偏估计结果出现较为稳定情况时(参见图5中的方框部分)，将切换至阶段2，利用阶段2中的方法进行传递对准，随着传递对准的进行，三轴姿态误差估计逐渐收敛至0且不再发散震荡。通过图3～图5可知，本实施例所设计的大失准角情况下的快速传递对准方法可以实现在大失准角情况下对子惯导导航误差的准确实时补偿，从而快速准确的实现传递对准。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种大失准角情况下的快速传递对准方法，首先用主子惯导所获得的速度信息、姿态信息作为传递对准算法的输入项，经过滤波算法处理后，得到子惯导作为失准角的速度误差与姿态误差，对速度误差与姿态误差进行实时补偿，从而实现传递对准，其特征在于，所述对速度误差与姿态误差进行补偿是：在传递对准开始时，首先，将子惯导速度误差与姿态误差之间的非线性耦合关系作为滤波模型，将子惯导速度观测误差作为滤波算法的输入量展开滤波计算，得到子惯导的速度误差与姿态误差后进行实时补偿；随着传递对准的运行，当失准角位于小失准角范畴时，将子惯导速度观测误差和姿态观测误差作为滤波算法的输入量，将子惯导速度误差与姿态误差之间的线性化后的耦合关系作为线性滤波模型，展开滤波计算，得到子惯导的速度误差与姿态误差后进行实时补偿。

2.根据权利要求1所述的方法，其特征在于，所述作为失准角的速度误差与姿态误差是通过将主惯导导航输出的速度和姿态变量转换为子惯导的外参考输出获得的，获得的过程是：

第一步：确定主惯导与子惯导的速度转换关系式，

式中，

为子惯导在导航北东地坐标系中表示的外参考三轴速度，

为主惯导输出在导航坐标系中表示的三轴速度，

为杆臂速度输出在导航坐标系中表示的补偿项，其中，的计算如下：

其中：

代表主惯导由载体坐标系转换至导航坐标系输出的姿态转换矩阵；

代表主惯导中的三轴陀螺仪所输出的载体坐标系相对于惯性坐标系的旋转角速度在主惯导载体坐标系中的表示；

r^bm代表主惯导质心和子惯导质心的杆臂距离在在主惯导载体坐标系中的表示；

×代表向量叉乘运算；

第二步：确定主惯导与子惯导的姿态转换关系式：

式中：

C_ref为子惯导的外参考姿态；

为主惯导由载体坐标系转换至导航坐标系输出的姿态转换矩阵；

为子惯导载体坐标系到主惯导坐标系的转换矩阵；

第三步：在得到和C_ref的基础上，将转换得到的子惯导的外参考输出与子惯导真实输出做差，得到的差值即为子惯导三轴速度误差和子惯导三轴姿态误差；

所述子惯导三轴速度误差Z_V通过关系式获取，

其中，为子惯导输出的三轴速度在导航坐标系中的表示；

所述子惯导三轴姿态误差Z_θ通过关系式获取，

Z_DCM的计算关系式为

其中，为子惯导输出由载体坐标系转换至导航坐标系的姿态转换矩阵。

3.根据权利要求1所述的方法，其特征在于，所述将子惯导速度误差与姿态误差之间的非线性耦合关系作为滤波模型，展开滤波计算，得到子惯导的速度误差与姿态误差的步骤包括：

一，取子惯导的姿态误差角、速度误差和子惯导的三轴加速度计零偏作为状态变量，为：

式中：

为在导航坐标系下的子惯导姿态误差，依次代表在导航坐标系X，Y，Z轴上的误差角度；

为在导航坐标系下的子惯导速度误差；

代表子惯导的三轴加速度计零偏；

二，确定连续情形下的状态转移部分非线性滤波模型，为：

其中：

，I代表3×3维数的单位矩阵；

为由载体坐标系转换至导航坐标系的子惯导输出的姿态转换矩阵；

代表子惯导所敏感到的比力信息在子惯导载体坐标系下的投影；

代表子惯导所敏感到的角速度信息在子惯导载体坐标系下的投影；

与W₁ ^v分别对应于对应的系统过程噪声；

三，确定连续情形下的观测向量部分非线性滤波模型，为：

其中：

Z¹代表量测值；

V¹对应于系统量测噪声；

四，对应于二和三的连续情况下非线性滤波模型进行离散化，同时采用相应的非线性滤波算法得到在k时刻的失准角的估计结果：速度误差的估计结果：加速度计零偏的估计结果：

利用误差估计结果与对子惯导的计算结果进行修正，同时在修正完成后，将估计得到的姿态误差与速度误差与进行置零处理。

4.根据权利要求3所述的方法，其特征在于，所述失准角是否位于小失准角范畴的检测方法是：定义利用所述非线性模型滤波算法得到的k时刻加速度计零偏估计结果的变化值D(k)为：

其中，||代表求取对应的绝对值；

对加速度计零偏的估计结果进行监控，如果连续20个时刻中，加速度计零偏估计结果的变化值均小于预设阈值，则认为进入平稳状态，即：

对于整数i从1递增至20，下式始终成立：

D(k-i+1)<5×10^-3

则认为进入平稳状态。

5.根据权利要求1所述的方法，其特征在于，所述将子惯导速度误差与姿态误差之间的线性化后的耦合关系作为线性滤波模型，展开滤波计算，得到子惯导的速度误差的步骤包括：

一，取子惯导的姿态误差角、速度误差、陀螺零偏、加速度计零偏、加速度计刻度因数误差、加速度计不正交的安装误差和安装误差角作为线性滤波模型的状态量，为，

式中：

是作为失准角的子惯导姿态误差；

为子惯导速度误差；

为子惯导陀螺三轴常值漂移；

为子惯导加速度计三轴常值漂移；

为子惯导加速度计三轴常值刻度因数误差；

为子惯导加速度计三轴不正交误差；

μ^bh为实际弹体相对于名义安装位置的安装误差角；

二，确定线性滤波模型对应的状态线性方程，为：

Z＝HX+V

式中W代表对应维数的系统过程噪声，V代表对应维数的系统量测噪声

其中：

为子惯导计算得到的bs系至n系的转换矩阵；

f_bs为子惯导敏感到的三轴比力在子惯导载体坐标系下的投影，

f_bs(1),f_bs(2),f_bs(3)分别为f_bs的第1，2，3个元素；

()×代表求取对应向量的反对称矩阵；

I_3×3代表3行3列的单位矩阵；

0_3×3代表3行3列的零矩阵；

三，对上述线性滤波模型进行离散化，得到k时刻离散线性模型公式：

X(k)＝φ(k/k-1)X(k-1)+W(k-1)

Z(k)＝H(k)X(k)+V(k)

用Kalman滤波器对k时刻离散线性模型公式进行传递对准滤波计算得到滤波结果；然后对子惯导姿态误差与速度误差进行修正即，在修正完成后，将上述kalman滤波器中的估计得到的姿态误差与速度误差进行置零，即将上述线性滤波模型中的子惯导姿态误差与子惯导速度误差进行置零处理。