CN102901514B - 一种基于多惯组信息约束的协同初始对准方法 - Google Patents

Abstract

本发明公开了一种基于多惯组信息约束的协同初始对准方法，步骤一、建立单惯组对准数学模型，惯组A和惯组B利用各自的惯性测量单元测得载体的加速度和角速度信息，并进行捷联惯导导航解算，得到惯组A和惯组B导航参数信息；步骤二、建立多惯组协同对准的状态方程；步骤三、建立多惯组系统的约束关系；步骤四：建立基于惯组信息约束的多惯组协同对准的量测方程；步骤五、建立离散型卡尔曼滤波器的递推方程，进行Kalman滤波解算，得到子惯导导航参数修正信息；步骤六、将导航参数修正信息反馈到导航解算中，进行惯组A和惯组B导航参数的修正和对准。本发明提高多惯组的对准精度，能同时提升多惯组器件误差估计的精度和速度。

Description

一种基于多惯组信息约束的协同初始对准方法

技术领域

本发明涉及一种基于多惯组信息约束的协同初始对准方法，属于捷联惯性导航技术领域。

背景技术

捷联惯导：由于不需要任何外界的参考信息，也不向外界发射任何信息，因而捷联惯导是一种完全自主的导航方式，它具有不依赖外界信息，隐蔽性强，机动灵活等优点，且具备多功能参数输出；与平台惯导相比，捷联惯导系统不需要精密的稳定平台，减少了硬件结构，因而成本大大降低。捷联惯导系统存在误差随时间迅速积累的问题，导航精度随时间而发散，在精度要求较高的情况下，捷联惯导系统不能单独长时间工作，必须不断以其他信息加以修正。

初始对准：对于捷联式惯导系统来说，初始对准就是确定初始时刻的姿态矩阵，使数学平台坐标系与导航坐标系重合。惯导系统的初始对准按对准的阶段分可分为粗对准和精对准，按对准轴系分可分为水平对准和方位对准，按基座的运动状态分可分为静基座对准和动基座对准，按对外部信息的依赖程度分可分为主动式对准和非主动式对准。初始对准的精确性和快速性是惯导系统的两项重要技术指标。

传递对准：当系统中存在多个惯组时，如果惯组之间的精度存在较大的差别，可以使用精度较高的惯组去校正精度较低的惯组，这种技术即传递对准技术。传递对准实际上是一种动基座对准，它利用精度较高的主惯导系统(MINS)来校准未对准的子惯导系统(SINS)。传递对准技术的提出最初是为了应用于机载导弹的快速对准，后来又引入到舰载设备初始对准领域。现在已广泛应用于机载、舰载及陆地战车的导弹初始对准，舰载机起飞前的对准等等。

协同概念：如果惯组之间存在直接相对观测或者间接相对观测，那么通过一定的信息交换，就可以实现相互之间导航资源的共享，从而获得比独自导航更优的性能，这种导航方式称为协同导航。将协同导航的概念引入多惯组系统中，当惯组之间存在相互测量信息或者多惯组的测量信息有一定的联系的时候，可以利用相互导航资源的共享来提高单独导航性能。Kalman滤波技术：在组合导航系统中，Kalman滤波器根据外部参考信息，对惯导系统的误差进行校正。Kalman滤波是从与被提取信号有关的观测量中，通过算法估计出所需信号的一种滤波算法，是一种递推线性最小方差估计。其中被估信号是由白噪声激励所引起的随机响应，估计过程中利用了系统的状态方程、量测方程、白噪声激励的统计特性和量测误差的统计特性。

张立川等在《基于双水听器的多自主水下航行器协同导航方法》中提到，提出了一种基于双水听器信号的MAUVS协同定位方法在主从式结构中，主AUV内部装备高精度导航设备，从AUV内部装备低精度导航设备，外部均装备水声装置测量相对位置关系，从AUV通过水声测量确定出相对距离和相对方位角，再辅以主AUV精确位置，得到从AUV精确位置。（参考文献：[张立川，徐德民，刘明雍.基于双水听器的多自主水下航行器协同导航方法[J].西安：西北工业大学航海学院.2011]），但是存在的缺点是：1、要求主惯组比从惯组的精度高；2、同等精度惯组不适用该方案进行协同导航。

还有一种基于数据链无线电测距功能实现智能导弹协同定位的方法。考虑智能导弹编队由领弹和攻击弹组成，给出了智能导弹编队协同作战模式和基于数据链的无线电测距方法。这种方法将领弹与攻击弹之间的相对距离信息和攻击弹自身的惯导信息相结合，从而实现了攻击弹的精确定位。（参考文献：[王小刚，郭继峰，崔乃刚.基于数据链的智能导弹协同定位方法[J].哈尔滨:哈尔滨工业大学航天学院，2009]）但是存在的缺点是：1、要求领弹惯组具有较高的精度，领弹的惯组精度直接影响定位的精度；2、领弹和从弹惯组有相同精度的时候，该方案不适用；3、既定数目领弹执行协同搜索任务时，在考虑最大搜索范围的同时，也要考虑到搜索编队构型对协同定位精度的影响；4、该方法也受领弹数目和领弹与攻击弹之间队形几何条件约束。

传递对准技术通常要求母惯组的精度一般比子惯组的精度高几个数量级，当两个同等精度的惯组同时进行对准时，传统意义上的传递对准方案并不适用。

发明内容

本发明的目的是为了解决上述问题，提出一种基于多惯组信息约束的协同初始对准方法，本发明将协同概念引入初始对准中，建立多惯组间不同方面的约束关系，得到了适用于同等精度条件下多惯组协同初始对准方案。

本发明的一种基于多惯组信息约束的协同初始对准方法，包括以下几个步骤：

步骤一、建立单惯组对准数学模型，惯组A和惯组B利用各自的惯性测量单元测得载体的加速度和角速度信息，并进行捷联惯导导航解算，得到惯组A和惯组B导航参数信息；

步骤二、基于单惯组对准数学模型，建立多惯组协同对准的状态方程；

步骤三、建立多惯组系统的约束关系；

步骤四：建立基于惯组信息约束的多惯组协同对准的量测方程；

步骤五、建立离散型卡尔曼滤波器的递推方程，进行Kalman滤波解算，得到子惯导导航参数修正信息；

步骤六、将惯组A和惯组B的导航参数修正信息反馈到导航解算中，进行惯组A和惯组B导航参数的修正和对准。

本发明的优点在于：

本发明的仿真结果表明，利用各惯组漂移误差间的约束关系建立的协同对准方案可以有效地提高多惯组的对准精度，能同时提升多惯组器件误差估计的精度和速度；在引入姿态角约束信息后，对准性能及器件误差估计的改善更为显著。

附图说明

图1是本发明的方法流程图；

图2是协同对准方案1与无协同对准，对准误差仿真结果的比较；

图3是协同对准方案1与协对准方案2，对准误差仿真结果比较；

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明是一种基于多惯组信息约束的协同初始对准方法，针对由两个惯组构成的系统，两个惯组分别记为惯组A和惯组B，方法流程如图1所示，包括以下几个步骤：

步骤一、建立单惯组对准数学模型，惯组A和惯组B利用各自的惯性测量单元测得载体的加速度和角速度信息，并进行捷联惯导导航的解算，得到惯组A和惯组B导航参数信息。

建立单惯组对准数学模型，包括状态方程和量测方程，具体如下：

1、根据对INS（惯性导航系统）误差模型的分析，建立对准滤波器状态方程：

$\stackrel{.}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

其中，X(t)∈R¹⁵为状态变量，F(t)∈R^15×15为系统误差矩阵，根据误差模型得到，W(t)为系统噪声向量，G(t)为系统噪声矩阵。

状态变量为：

$\mathrm{\δ}{\mathrm{\θ}}_x,\mathrm{\δ}{\mathrm{\θ}}_y,\mathrm{\δh},\mathrm{\δ}{v}_x,\mathrm{\δ}{v}_y,\mathrm{\δ}{v}_z,{\mathrm{\ψ}}_x,{\mathrm{\ψ}}_y,{\mathrm{\ψ}}_z,{\mathrm{\ϵ}}_x,{\mathrm{\ϵ}}_y,{\mathrm{\ϵ}}_z,{\▿}_x,{\▿}_y,{\▿}_z$

其中：导航坐标系x、y、z分别表示东向、北向、天向，ψ_x，ψ_y，ψ_z为惯导系统东向、北向、天向的平台误差角，δv_x,δv_y,δv_z为惯导系统东向、北向、天向的速度误差，δθ_x,δθ_y,δh为惯导系统向东、北向、天向的位置误差，ε_x,ε_y,ε_z为陀螺仪东向、北向、天向的随机漂移，为东向、北向、天向的加速度计偏置。初始对准的系统误差矩阵为：

$F=\left[\begin{array}{cc}A& \begin{array}{cc}{0}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ -C_b^n& 0_{3\×3}\end{array}\\ 0_{6\×6}& 0_{6\×6}\end{array}\right]$

其中，A具体为：

$A=\left[\begin{array}{ccccccccc}-\frac{v_z}{R}& 0& \frac{v_y}{R^2}& 0& -\frac{1}{R}& 0& 0& 0& 0\\ 0& -\frac{v_z}{R}& -\frac{v_x}{R^2}& \frac{1}{R}& 0& 0& 0& 0& 0\\ -v_y& v_x& 0& 0& 0& 1& 0& 0& 0\\ 0& -g& 0& 0& 2{\mathrm{\Ω}}_z& -{(\mathrm{\ρ}+2\mathrm{\Ω})}_y& 0& -f_z& f_y\\ g& 0& 0& -2{\mathrm{\Ω}}_z& 0& {(\mathrm{\ρ}+2\mathrm{\Ω})}_x& f_z& 0& -f_x\\ 0& 0& 2\frac{g}{R}& {(\mathrm{\ω}+\mathrm{\Ω})}_y& -{(\mathrm{\ω}+\mathrm{\Ω})}_x& 0& -f_y& f_x& 0\\ 0& 0& 0& 0& 0& 0& 0& {\mathrm{\Ω}}_z& -{\mathrm{\ω}}_y\\ 0& 0& 0& 0& 0& 0& -{\mathrm{\Ω}}_z& 0& {\mathrm{\ω}}_x\\ 0& 0& 0& 0& 0& 0& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]$

其中，v_x、v_y、v_z分别为载体东向、北向、天向的运动速度矢量；ρ_x、ρ_y、ρ_y分别为载体东向、北向、天向的运动角速率矢量；Ω为地球自转角速率矢量；Ω_z表示天向的地球自转角速率矢量；ω＝ρ+Ω；g为地球重力加速度；R为地球半径；f_x、f_y、f_z为东向、北向、天向载体感受的比力矢量；δf为加速度计输出误差；ε为陀螺输出误差。(ρ+2Ω)_x、(ρ+2Ω)_y分别表示ρ+2Ω在东向、北向的分量；(ω+Ω)_x、(ω+Ω)_y分别表示ω+Ω在东向、北向的分量；

2、单惯组初始对准量测方程具体为：

采用零速修正，以速度作为观测量，建立量测方程为：

Z₁(t)＝H₁X(t)+η₁(t)＝[0_3×3|I_3×3|0_3×9]X(t)+η₁(t)

其中，Z₁(t)表示系统的量测矢量，H₁表示量测矩阵，X(t)表示系统状态矢量，η₁(t)为量测噪声矢量。

$Z_V\left(t\right)=\mathrm{\δV}=\left(\begin{array}{c}{v}_x-0\\ v_y-0\\ v_z-0\end{array}\right)$

其中，Z_V(t)载体速度为有δV速度误差时刻的系统量测矢量。v_x、v_y、v_z为惯导解算的东向、北向、天向的速度。

惯组A、惯组B根据各自的惯性测量单元，分别测量得到载体加速度数据和角速度数据，通过状态方程和量测方程，分别得到惯组A和惯组B的水平速度误差、姿态误差，并分别对惯组A和惯组B的速度误差和姿态误差进行积分运算，得到惯组A和惯组B的位置、速度和姿态值。

步骤二、基于单惯组对准数学模型，建立多惯组协同对准的状态方程。

多惯组状态方程为：

$\stackrel{.}{X}{\left(t\right)}^{\′}=F{\left(t\right)}^{\′}X{\left(t\right)}^{\′}+G{\left(t\right)}^{\′}W{\left(t\right)}^{\′}$

其中：为多惯组系统状态变量，F(t)′为多惯组系统误差矩阵，W(t)′为多惯组系统噪声向量，G(t)′为多惯组系统噪声矩阵。惯组A和惯组B的状态变量取为：

$\mathrm{\δ}{\mathrm{\θ}}_{x\_1},\mathrm{\δ}{\mathrm{\θ}}_{y\_1},\mathrm{\δ}{h}_{\_1},\mathrm{\δ}{v}_{x\_1},\mathrm{\δ}{v}_{y\_1},\mathrm{\δ}{v}_{z\_1},{\mathrm{\ψ}}_{x\_1},{\mathrm{\ψ}}_{y\_1},{\mathrm{\ψ}}_{z\_1},{\mathrm{\ϵ}}_{x\_1},{\mathrm{\ϵ}}_{y\_1},{\mathrm{\ϵ}}_{z\_1},{\▿}_{x\_1},{\▿}_{y\_1},{\▿}_{z\_1}$

$\mathrm{\δ}{\mathrm{\θ}}_{x\_2},\mathrm{\δ}{\mathrm{\θ}}_{y\_2},\mathrm{\δ}{h}_{\_2},\mathrm{\δ}{v}_{x\_2},\mathrm{\δ}{v}_{y\_2},\mathrm{\δ}{v}_{z\_2},{\mathrm{\ψ}}_{x\_2},{\mathrm{\ψ}}_{y\_2},{\mathrm{\ψ}}_{z\_2},{\mathrm{\ϵ}}_{x\_2},{\mathrm{\ϵ}}_{y\_2},{\mathrm{\ϵ}}_{z\_2},{\▿}_{x\_2},{\▿}_{y\_2},{\▿}_{z\_2}$

其中，下标_1表示惯组A的状态变量，下标_2表示惯组B的状态变量，多惯组误差矩阵F(t)′为：

$F{\left(t\right)}^{\′}=\left[\begin{array}{cc}{F}_1& 0_{15\×15}\\ 0_{15\×15}& F_2\end{array}\right]$

其中，F₁、F₂为惯组A和惯组B分别进行初始对准的误差矩阵。

步骤三、建立多惯组系统的约束关系。

假设两个惯组的导航信息输出是同步的，不存在时间差，考虑子惯组之间存在以下的约束关系：

（1）由陀螺以及加速度计的误差特性，可以认为同批次的陀螺与加速度计在相同环境下工作时，其漂移近似呈零均值高斯分布，可以以多惯组系统中存在的所有器件漂移误差零均值为协同对准的补充约束条件，设计协同对准方案。即基于惯组信息约束的协同初始对准方案。

（2）在地面对准过程中，可以考虑事前测量惯组之间的相对姿态或者以确定的相对关系进行安装，即假设惯组之间的相对姿态已知，在这种假设条件下对惯组对准过程进行协同，设计协同方案，称之为基于姿态角信息约束的协同初始对准方案。

步骤四：建立基于惯组信息约束的多惯组协同对准的量测方程；

判断多惯组系统符合的约束条件；

（1）如果符合第一个约束条件，利用第一个约束条件，即基于惯组信息约束的协同传递对准方案，针对惯组的所有陀螺与加速度计有：

$\left.\begin{array}{c}{\mathrm{\ϵ}}_{x\_1}+{\mathrm{\ϵ}}_{y\_1}+{\mathrm{\ϵ}}_{z\_1}+{\mathrm{\ϵ}}_{x\_2}+{\mathrm{\ϵ}}_{y\_2}+{\mathrm{\ϵ}}_{z\_2}\≈0\\ {\▿}_{x\_1}+{\▿}_{y\_1}+{\▿}_{z\_1}+{\▿}_{x\_2}+{\▿}_{y\_2}+{\▿}_{z\_2}\≈0\end{array}\right\}$

将其与速度信息综合得到新的量测方程为：

Z₂=H₂X+η₂

其中，观测量z₂=[v_{x_1}-0v_{y_1}-0v_{z_1}-0 v_{x_2}-0 v_{y_2}-0 v_{z_2}-000]^T，0表示0矩阵，其中，H₂为基于惯组信息约束条件的量测矩阵，具体为：

H₂＝[H₂₁ H₂₂ H₂₃ H₂₄]^T

H₂₁＝[0_3×3 I_3×3 0_3×24]

H₂₂＝[0_3×18 I_3×3 0_3×9]

H₂₃＝[0_1×9 1 1 1 0_1×12 1 1 1 0_1×3]

H₂₄＝[0_1×12 1 1 1 0_1×12 1 1 1]

（2）如果符合第二个约束条件，当惯组A和惯组B之间符合第二个约束条件时，即可基于姿态角约束建立量测方程：

Z₃=H₃X+η₃

其中，观测量Z₃=[θ_x θ_y θ_z]^T，θ_x、θ_y、θ_z为解算得到的惯组A相对于惯组B相对姿态角和已知相对姿态角差值。H₃=[0_3×6 F₁(φ) 0_3×12 F₂(φ) 0_3×6]，F₁(φ),F₂(φ)分别表示惯组A、惯组B惯组姿态角误差状态变量相关的量测矩阵系数。

步骤五、建立离散型卡尔曼滤波器的递推方程，进行Kalman滤波解算，得到子惯导导航参数修正信息。

卡尔曼滤波器为：多惯组系统（惯组A和惯组B）的状态方程和量测方程可离散化成如下形式：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.$

其中：X_k表示k时刻多惯组系统的的状态矢量，Φ_k，k-1表示k-1到k时刻的多惯组系统一步转移矩阵，Γ_k-1表示多惯组系统噪声矩阵，W_k-1表示k-1时刻的多惯组系统噪声，H_k表示多惯组系统量测矩阵，V_k表示多惯组系统k时刻量测噪声，其中{W_k-1}、{V_k}为互不相关零均值白噪声序列。

步骤六、将惯组A和惯组B的导航参数修正信息反馈到导航解算中，进行惯组A和惯组B导航参数的修正和对准。

导航参数包括平台误差角、速度误差、位置误差、陀螺随机漂移、加速度计偏置。

为了验证协同初始对准方法相对于传统方法的优势，采用数学仿真进行详细分析。

以弹用惯组典型的起竖两位置对准过程为例：取0~300s为第一位置对准时段；300s~430s为起竖第二位置对准阶段。两套惯组误差源分配如表1，则协同对准滤波器的误差模型初始协方差阵为：

P(0)＝diag{(10″)²,(10″)²,(30m)²,(0.01m/s)²,(0.01m/s)²,

(0.01m/s)²,(3600″)²,(3600″)²,(7200″)²,(0.2°/h)²,

(0.2°/h)²,(0.2°/h)²,(500μg)²,(500μg)²,(500μg)²,

(10″)²,(10″)²,(30m)²,(0.01m/s)²,(0.01m/s)²,(0.01m/s)²,

(3600″)²,(3600″)²,(7200″)²,(0.2°/h)²,(0.2°/h)²,

(0.2°/h)²,(500μg)²,(500μg)²,(500μg)²}

表1惯组误差源分配表

表格2第一位置对准各误差状态变量协方差均方根稳态收敛值

表格3第二位置对准各器件误差状态变量协方差均方根稳态收敛值

仿真结果曲线如上图2与图3所示。其中方案1表示基于惯组信息约束的协同初始对准方案，方案2表示同时加入姿态角信息约束的协同初始对准方案。第一位置对准各误差状态变量协方差均方根稳态收敛值如表2，第二位置对准各器件误差状态变量协方差均方根稳态收敛值如表3。

由仿真结果可以看出，协同对准方法中对准误差、器件误差在滤波器滤波初始阶段即有阶跃式收敛过程；在0~300s的第一位置对准时段，基于惯组信息约束的方案1相比无协同方案，除天向加表误差完全可观，变化不明显，三个方向的陀螺误差状态变量的协方差均方根分别由0.0965°/h、0.1676°/h、0.1999°/h降至0.08667°/h、0.1445°/h、0.1779°/h，水平方向加表误差协方均方根由499μg、499μg降至432μg、431μg；姿态角误差协方差均方根由104″、104″、3417″降至100″、79″、3008″，各项主要误差减少10%以上。在300s~420s的第二位置对准时段，方案1可进一步提升X向加表误差的收敛速度，其协方差均方根稳态值由498μg收敛至350μg；相应地，俯仰角姿态误差由96″降至68″，横滚角姿态误差由41″降至31″，各项主要误差减少约30%。

同时加入姿态角信息约束后，相对于方案1，可进一步提升对准精度。在0s~300s的第一位置对准时段，进一步提高了姿态角及器件误差收敛精度，其中横滚角误差由79″收敛至29″，航向角误差由2608″收敛至1702″，水平加表误差均由432μg降至252μg；在300s~430s的第二位置对准时段，X向加表误差收敛较为明显，由350μg降至50μg，三方向姿态角误差均降至原值的50%以下。

综上，协同对准方法可以同时提升多惯组对准及器件误差估计的精度和速度，其中引入姿态角信息约束后的协同对准方案对天向陀螺和水平加表误差的估计效果改善最为明显。

Claims (2)

1.一种基于多惯组信息约束的协同初始对准方法，针对由两个惯组构成的系统，两个惯组分别记为惯组A和惯组B，其特征在于，包括以下几个步骤：

步骤一、建立单惯组对准数学模型，惯组A和惯组B利用各自的惯性测量单元测得载体的加速度和角速度信息，并进行捷联惯导导航解算，得到惯组A和惯组B导航参数信息；

建立单惯组对准数学模型，包括状态方程和量测方程，具体如下：

1、建立对准滤波器状态方程：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

其中，X(t)∈R¹⁵为状态变量，F(t)∈R^15×15为系统误差矩阵，根据误差模型得到，W(t)为系统噪声向量，G(t)为系统噪声矩阵；

状态变量为：

δθ_x,δθ_y,δh,δv_x,δv_y,δv_z,ψ_x,ψ_y,ψ_z,ε_x,ε_y,ε_z,▽_x,▽_y,▽_z

其中：导航坐标系x、y、z分别表示东向、北向、天向，ψ_x,ψ_y,ψ_z为惯导系统东向、北向、天向的平台误差角，δv_x,δv_y,δv_z为惯导系统东向、北向、天向的速度误差，δθ_x,δθ_y,δh为惯导系统向东、北向、天向的位置误差，ε_x,ε_y,ε_z为陀螺仪东向、北向、天向的随机漂移，▽_x,▽_y,▽_z为东向、北向、天向的加速度计偏置；初始对准的系统误差矩阵为：

$F=\left[\begin{array}{cc}A& \begin{array}{cc}{0}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ -C_b^n& 0_{3\×3}\end{array}\\ 0_{6\×6}& 0_{6\×6}\end{array}\right]$

其中，A具体为：

$A=\left[\begin{array}{ccccccccc}-\frac{v_z}{R}& 0& \frac{v_y}{R^2}& 0& -\frac{1}{R}& 0& 0& 0& 0\\ 0& -\frac{v_z}{R}& -\frac{v_x}{R^2}& \frac{1}{R}& 0& 0& 0& 0& 0\\ -v_y& v_x& 0& 0& 0& 1& 0& 0& 0\\ 0& -g& 0& 0& 2{\mathrm{\Ω}}_z& -{(\mathrm{\ρ}+2\mathrm{\Ω})}_y& 0& -f_z& f_y\\ g& 0& 0& -2{\mathrm{\Ω}}_z& 0& {(\mathrm{\ρ}+2\mathrm{\Ω})}_x& f_z& 0& -f_x\\ 0& 0& 2\frac{g}{R}& {(\mathrm{\ω}+\mathrm{\Ω})}_y& -{(\mathrm{\ω}+\mathrm{\Ω})}_x& 0& -f_y& f_x& 0\\ 0& 0& 0& 0& 0& 0& 0& {\mathrm{\Ω}}_z& -{\mathrm{\ω}}_y\\ 0& 0& 0& 0& 0& 0& -{\mathrm{\Ω}}_z& 0& {\mathrm{\ω}}_x\\ 0& 0& 0& 0& 0& 0& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]$

其中，v_x、v_y、v_z分别为载体东向、北向、天向的运动速度矢量；ρ_x、ρ_y、ρ_z分别为载体东向、北向、天向的运动角速率矢量；Ω为地球自转角速率矢量；Ω_z表示天向的地球自转角速率矢量；ω＝ρ+Ω；g为地球重力加速度；R为地球半径；f_x、f_y、f_z为东向、北向、天向载体感受的比力矢量；δf为加速度计输出误差；ε为陀螺输出误差；(ρ+2Ω)_x、(ρ+2Ω)_y分别表示ρ+2Ω在东向、北向的分量；(ω+Ω)_x、(ω+Ω)_y分别表示ω+Ω在东向、北向的分量；

2、单惯组初始对准量测方程具体为：

采用零速修正，以速度作为观测量，建立量测方程为：

Z₁(t)＝H₁X(t)+η₁(t)＝[0_3×3|I_3×3|0_3×9]X(t)+η₁(t)

其中，Z₁(t)表示系统的量测矢量，H₁表示量测矩阵，X(t)表示系统状态矢量，η₁(t)为量测噪声矢量；

$Z_V\left(t\right)=\mathrm{\δV}=\left(\begin{array}{c}{v}_x-0\\ v_y-0\\ v_z-0\end{array}\right)$

其中，Z_V(t)载体速度为有δV速度误差时刻的系统量测矢量；v_x、v_y、v_z为惯导解算的东向、北向、天向的速度；

惯组A、惯组B根据各自的惯性测量单元，分别测量得到载体加速度数据和角速度数据，通过状态方程和量测方程，分别得到惯组A和惯组B的水平速度误差、姿态误差，并分别对惯组A和惯组B的速度误差和姿态误差进行积分运算，得到惯组A和惯组B的位置、速度和姿态值；

步骤二、基于单惯组对准数学模型，建立多惯组协同对准的状态方程；

多惯组状态方程为：

$\stackrel{\·}{X}{\left(t\right)}^{\′}=F{\left(t\right)}^{\′}X{\left(t\right)}^{\′}+G{\left(t\right)}^{\′}W{\left(t\right)}^{\′}$

其中：为多惯组系统状态变量，F(t)′为多惯组系统误差矩阵，W(t)′为多惯组系统噪声向量，G(t)′为多惯组系统噪声矩阵，惯组A和惯组B的状态变量取为：

δθ_{x_1},δθ_{y_1},δh_{_1},δv_{x_1},δv_{y_1},δv_{z_1},ψ_{x_1},ψ_{y_1},ψ_{z_1},ε_{x_1},ε_{y_1},ε_{z_1},▽_{x_1},▽_{y_1},▽_{z_1}

δθ_{x_2},δθ_{y_2},δh_{_2},δv_{x_2},δv_{y_2},δv_{z_2},ψ_{x_2},ψ_{y_2},ψ_{z_2},ε_{x_2},ε_{y_2},ε_{z_2},▽_{x_2},▽_{y_2},▽_{z_2}

其中，下标_1表示惯组A的状态变量，下标_2表示惯组B的状态变量，多惯组误差矩阵F(t)′为：

$F{\left(t\right)}^{\′}=\left[\begin{array}{cc}{F}_1& 0_{15\×15}\\ 0_{15\×15}& F_2\end{array}\right]$

其中，F₁、F₂为惯组A和惯组B分别进行初始对准的误差矩阵；

步骤三、建立多惯组系统的约束关系；

假设两个惯组的导航信息输出是同步的，不存在时间差，考虑子惯组之间存在以下的约束关系：

(1)假设同批次的陀螺与加速度计在相同环境下工作时，其漂移为零均值高斯分布，以多惯组系统中存在的所有器件漂移误差零均值为协同对准的补充约束条件，设计协同对准方案，即基于惯组信息约束的协同初始对准方案；

(2)在地面对准过程中，假设惯组之间的相对姿态已知，对惯组对准过程进行协同，设计协同方案，即基于姿态角信息约束的协同初始对准方案；

步骤四：建立基于惯组信息约束的多惯组协同对准的量测方程；

判断多惯组系统符合的约束条件；

(1)如果符合第一个约束条件，利用第一个约束条件，即基于惯组信息约束的协同传递对准方案，针对惯组的所有陀螺与加速度计有：

$\left.\begin{array}{c}{\mathrm{\ϵ}}_{x\_1}+{\mathrm{\ϵ}}_{y\_1}+{\mathrm{\ϵ}}_{z\_1}+{\mathrm{\ϵ}}_{x\_2}+{\mathrm{\ϵ}}_{y\_2}+{\mathrm{\ϵ}}_{z\_2}\≈0\\ {\▿}_{x\_1}+{\▿}_{y\_1}+{\▿}_{z\_1}+{\▿}_{x\_2}+{\▿}_{y\_2}+{\▿}_{z\_2}\≈0\end{array}\right\}$

将其与速度信息综合得到新的量测方程为：

Z₂＝H₂X+η₂

其中，观测量Z₂＝[v_{x_1}-0 v_{y_1}-0 v_{z_1}-0 v_{x_2}-0 v_{y_2}-0 v_{z_2}-0 0 0]^T，0表示0矩阵，

其中，H₂为基于惯组信息约束条件的量测矩阵，具体为：

H₂＝[H₂₁ H₂₂ H₂₃ H₂₄]^T

H₂₁＝[0_3×3 I_3×3 0_3×24]

H₂₂＝[0_3×18 I_3×3 0_3×9]

H₂₃＝[0_1×9 1 1 1 0_1×12 1 1 1 0_1×3]

H₂₄＝[0_1×12 1 1 1 0_1×12 1 1 1]

(2)如果符合第二个约束条件，当惯组A和惯组B之间符合第二个约束条件时，即可基于姿态角约束建立量测方程：

Z₃＝H₃X+η₃

其中，观测量Z₃＝[θ_x θ_y θ_z]^T，θ_x、θ_y、θ_z为解算得到的惯组A相对于惯组B相对姿态角和已知相对姿态角差值；H₃＝[0_3×6 F₁(φ) 0_3×12 F₂(φ) 0_3×6]，F₁(φ),F₂(φ)分别表示惯组A、惯组B惯组姿态角误差状态变量相关的量测矩阵系数；

步骤五、建立离散型卡尔曼滤波器的递推方程，进行Kalman滤波解算，得到子惯导导航参数修正信息；

卡尔曼滤波器为：多惯组系统(惯组A和惯组B)的状态方程和量测方程可离散化成如下形式：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.$

其中：X_k表示k时刻多惯组系统的的状态矢量，Φ_k,k-1表示k-1到k时刻的多惯组系统一步转移矩阵，Γ_k-1表示多惯组系统噪声矩阵，W_k-1表示k-1时刻的多惯组系统噪声，H_k表示多惯组系统量测矩阵，V_k表示多惯组系统k时刻量测噪声，其中{W_k-1}、{V_k}为互不相关零均值白噪声序列；

步骤六、将惯组A和惯组B的导航参数修正信息反馈到导航解算中，进行惯组A和惯组B导航参数的修正和对准。

2.根据权利要求1所述的一种基于多惯组信息约束的协同初始对准方法，其特征在于，步骤六中所述的导航参数包括平台误差角、速度误差、位置误差、陀螺随机漂移、加速度计偏置。