[发明专利]一种基于多惯组信息约束的协同初始对准方法

摘要

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

主权项

1.一种基于多惯组信息约束的协同初始对准方法，针对由两个惯组构成的系统，两个惯组分别记为惯组A和惯组B，其特征在于，包括以下几个步骤： 步骤一、建立单惯组对准数学模型，惯组A和惯组B利用各自的惯性测量单元测得载体的加速度和角速度信息，并进行捷联惯导导航解算，得到惯组A和惯组B导航参数信息； 建立单惯组对准数学模型，包括状态方程和量测方程，具体如下： <1>建立对准滤波器状态方程： 其中，X(t)∈R¹⁵为状态变量，F(t)∈R^15×15为系统误差矩阵，根据误差模型得到，W(t)为系统噪声向量，G(t)为系统噪声矩阵； 状态变量为： 其中：导航坐标系x、y、z分别表示东向、北向、天向，ψ_x，ψ_y，ψ_z为惯导系统东向、北向、天向的平台误差角，δv_x,δv_y,δv_z为惯导系统东向、北向、天向的速度误差，δθ_x,δθ_y,δh为惯导系统向东、北向、天向的位置误差，ε_x,ε_y,ε_z为陀螺仪东向、北向、天向的随机漂移，为东向、北向、天向的加速度计偏置；初始对准的系统误差矩阵为：其中，A具体为： 其中，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(t)载体速度为有δV速度误差时刻的系统量测矢量；v_x、v_y、v_z为惯导解算的东向、北向、天向的速度； 惯组A、惯组B根据各自的惯性测量单元，分别测量得到载体加速度数据和角速度数据，通过状态方程和量测方程，分别得到惯组A和惯组B的水平速度误差、姿态误差，并分别对惯组A和惯组B的速度误差和姿态误差进行积分运算，得到惯组A和惯组B的位置、速度 和姿态值； 步骤二、基于单惯组对准数学模型，建立多惯组协同对准的状态方程； 多惯组状态方程为： 其中：为多惯组系统状态变量，F(t)′为多惯组系统误差矩阵，W(t)′为多惯组系统噪声向量，G(t)′为多惯组系统噪声矩阵，惯组A和惯组B的状态变量取为：其中，下标_1表示惯组A的状态变量，下标_2表示惯组B的状态变量，多惯组误差矩阵F(t)′为： 其中，F₁、F₂为惯组A和惯组B分别进行初始对准的误差矩阵； 步骤三、建立多惯组系统的约束关系； 假设两个惯组的导航信息输出是同步的，不存在时间差，考虑子惯组之间存在以下的约束关系： （1）假设同批次的陀螺与加速度计在相同环境下工作时，其漂移为零均值高斯分布，以多惯组系统中存在的所有器件漂移误差零均值为协同对准的补充约束条件，设计协同对准方案，即基于惯组信息约束的协同初始对准方案； （2）在地面对准过程中，假设惯组之间的相对姿态已知，对惯组对准过程进行协同，设计协同方案，即基于姿态角信息约束的协同初始对准方案； 步骤四：建立基于惯组信息约束的多惯组协同对准的量测方程； 判断多惯组系统符合的约束条件； （1）如果符合第一个约束条件，利用第一个约束条件，即基于惯组信息约束的协同传递对准方案，针对惯组的所有陀螺与加速度计有： 将其与速度信息综合得到新的量测方程为： 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}-000]^T，0表示0矩阵，其中，H₂为基于惯组信息约束条件的量测矩阵，具体为： H₂＝[H₂₁ H₂₂ H₂₃ H₂₄]^TH₂₁＝[0_3×3 I_3×3 0_3×24] H₂₂＝[0_3×18 I_3×3 0_3×9] H₂₃＝[0_1×9 1110_1×12 1110_1×3] H₂₄＝[0_1×12 1110_1×12 111] （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）的状态方程和量测方程可离散化成如下形式： 其中：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导航参数的修正和对准。