CN110440830B - 动基座下车载捷联惯导系统自对准方法 - Google Patents

Abstract

本发明公开了一种动基座下车载捷联惯导系统自对准方法，包括以下步骤：步骤1，获得捷联惯导系统所在地理位置信息，其中地理位置信息包括经度λ、纬度

重力加速度g；步骤2，利用经度、纬度、重力加速度信息及惯性器件输出信息，采用凝固坐标系，利用多矢量定姿方法进行粗对准，求得初始姿态矩阵；步骤3，根据捷联惯性系统误差方程及惯性器件误差方程构建kalman滤波状态方程和量测方程；步骤4，设计混合校准kalman滤波方法估计系统误差量和惯性器件误差量并修正，实现精对准过程，完成初始对准。本发明具有较强的抗干扰能力，保证了动基座条件下的自对准精度。

Description

动基座下车载捷联惯导系统自对准方法

技术领域

本发明特别涉及一种动基座下车载捷联惯导系统自对准方法，涉及动基座下基于凝固坐标系粗对准、精对准kalman滤波状态空间模型的建立及基于混合校准方法的kalman滤波方法。本发明适合应用于车体晃动、人员上下、自然干扰、车辆发动机启动等车载环境下及拥有相同工作状态情况的其他载体的自对准过程。

背景技术

对于捷联惯导系统(strapdown inertial navigation system)而言，初始对准是其开展导航任务的重要前提，其目的是建立起精确的初始坐标基准，即载体系相对于导航系的姿态矩阵。对准精度和对准速度是初始对准的两项重要技术指标。初始对准的精度对惯导系统的导航精度有着重要影响，初始对准的速度在很大程度上决定了武器系统的快速反应能力，因此要求对准精度高、对准时间短，既精又快。

初始对准按不同的划分标准可以把惯性导航系统初始对准方式作如下分类：按对外部信息的依赖程度，可分为主动式对准和非主动式对准:按是否需要更高精度主惯导提供匹配参数，可分为传递对准和自对准；按对准的阶段，一般可分为粗对准和精对准；按基座的运动状态，可分为静基座对准和动基座对准。

常用的自对准方案是在忽略扰动的条件下利用加速度计和陀螺仪测量两个在空间不共线的矢量实现粗对准，求取粗略的姿态矩阵，然后利用基于小失准角线性误差模型的卡尔曼滤波算法完成初始姿态失准角的精确估计。但当载体处于晃动干扰条件下，惯性测量组件输出中包含干扰加速度信息和干扰角速度信息，鉴于陀螺输出信号的信噪比较低，而干扰信号频带较宽，此时难以获得准确的地球自转角速度，进而导致粗对准精度不高甚至得不到有效的对准结果，对精对准结果影响较大。

在精对准过程中，常用kalman滤波进行系统状态变量估计，解算精确姿态转换矩阵，实现精对准过程。然而在实际系统中，由于测量过程中动态干扰条件的影响，导致系统模型的线性近似度变弱使得滤波效果变差，甚至可能直接导致滤波器发散。针对上述滤波环境，有必要提出新的滤波方法，能够修正系统内部的状态误差，能保证长时间滤波的稳定性，实现高精度的精对准过程。

发明内容

本发明的目的在于，针对动基座条件下系统噪声变大的情况，提出一种动基座下车载捷联惯导系统自对准方法，首先采用凝固法粗对准，在时间可以精确已知的基础上，计算重力加速度矢量在惯性空间中旋转的角度，提取重力加速度矢量在惯性空间方向改变中所包含的真北信息，实现粗对准；然后在完成粗对准基础上，利用惯导系统误差方程及惯性器件误差方程构建kalman滤波状态方程和量测方程；最后再利用混合校准kalman滤波方法估计惯导系统速度误差、位置误差、姿态误差及惯性器件误差等并对其进行修正，实现高精度自对准。本发明具有较强的抗干扰能力，保证了动基座条件下的自对准精度。

为解决上述技术问题，本发明所采用的技术方案是：

一种动基座下车载捷联惯导系统自对准方法，其特点是包括以下步骤：

步骤1，获得捷联惯导系统所在地理位置信息，其中地理位置信息包括经度λ、纬度

重力加速度g；

步骤2，利用经度、纬度、重力加速度信息及惯性器件输出信息，采用凝固坐标系，利用多矢量定姿方法进行粗对准，求得初始姿态矩阵；

步骤3，根据捷联惯性系统误差方程及惯性器件误差方程构建kalman滤波状态方程和量测方程；

步骤4，设计混合校准kalman滤波方法估计系统误差量和惯性器件误差量并修正，实现精对准过程，完成初始对准。

作为一种优选方式，所述步骤2中，初始姿态矩阵

分解为以下公式(1)中的形式：

其中，

为地理坐标系e系与导航坐标系n系之间的变换矩阵，表示载体与地球的相对位置；

为地心惯性坐标系i系与地理坐标系e系之间的变换矩阵，反映了载体的自转，

由时间间隔Δt＝t-t₀确定；

记录了载体坐标系b系与载体惯性凝固坐标系i_b0系之间的姿态变化信息；

为载体惯性凝固坐标系i_b0系与地心惯性坐标系i系之间的变换矩阵。

作为一种优选方式，

其中，

为初始单位阵，

为陀螺仪输出，

表示载体坐标系b系相对于地心惯性坐标系i系的载体运动角速度在载体坐标系b系内的投影，

为向量

构造的叉乘反对称矩阵；

计算过程包括：

选择m个时刻的速度作为参考矢量，构造公式(5)中的目标函数：

其中，

公式(7)中，f^b为加速度计输出；

整理变形公式(5)得：

L(C)＝L'-tr(CB^T) (8)

其中，

且与B无关；

是使L取最小值的正交矩阵C；

对B进行奇异值分解，得：

B＝USV^T (9)

式(9)中，U和V为正交矩阵；S＝diag(s₁,s₂,s₃)，s₁≥s₂≥s₃；

求得：

作为一种优选方式，所述步骤3中，捷联惯性系统误差方程及惯性器件误差方程如下：

其中，δv_e、δv_n、δv_u分别为惯导系统东向、北向、天向速度误差；φ_e、φ_n、φ_u分别为俯仰、横滚、方位角误差；

分别为X、Y、Z三轴向加速度计的零偏；τ_a为加速度计零偏相关时间；ε为陀螺的零漂；τ_g为陀螺零漂相关时间；ω_ie为地球自转角速度；R_M为地球纬度圈半径，R_N为地球经度圈半径；w_a和w_g分别为加速度计和陀螺仪零均值的白噪声；

根据捷联惯导系统误差方程，建立系统状态方程为：

其中，A(t)为状态转移矩阵，由式(11)得到；略去惯性器件的刻度系数误差和安装误差，又因为惯性器件偏置量的重复性误差对系统精度的影响最大，因此将陀螺漂移和加速度零偏的随机常数部分列入状态，则滤波状态矢量及系统噪声分别为：

假设载体位置不变，且已经精确已知，选取位置误差作为观测量，则量测方程：

其中，

分别为捷联惯导系统解算的经纬度值；V为量测白噪声，H为量测矩阵，H＝[I_2×2 0_2×12]。

将式(12)和式(15)进行离散化，可得到系统离散化的kalman滤波状态方程和量测方程。

作为一种优选方式，所述步骤4包括步骤41和步骤42，其中：

步骤41，设计混合校准kalman滤波方法：

输出校准采用公式(16)中的kalman滤波方程：

其中，

表示一步预测估计值；

表示X_k的状态估计值；K_k为k时刻的增益矩阵；P_k,k-1表示一步预测估计方差阵；P_k-1表示状态估计方差阵；R_k表示量测噪声方差阵；Q_k-1表示系统噪声方差阵；具有反馈控制的系统状态方程和量测方程为：

对应的滤波方程为：

将控制矢量U_k-1选取为：

反馈校准的滤波方程为：

步骤42，精对准过程误差姿态误差修正：

姿态捷联矩阵修正如下：

其中，

kalman滤波估计出失准角φ_e、φ_n、φ_u后，通过混合校准方法对姿态矩阵

进行校准，即可精确确定出载体姿态，完成自对准过程。

本发明具有以下有益效果：

1、引入惯性凝固思想：在时间可以精确已知的基础上，地球自转角速度可以精确计算。通过选取多个中间时刻计算重力加速度矢量在惯性空间中旋转的角度，提取重力加速度矢量在惯性空间方向改变中所包含的真北信息，实现粗对准。该方法只利用了地球自转角速度和重力加速度矢量在惯性空间的运动，避免了载体处于动基座条件下惯性器件测量受到的干扰信息的影响，有效提高了粗对准精度。

2、本发明提供混合校准kalman滤波方法，即在滤波初始阶段先进行输出校准，滤波稳定后进行反馈校准。进行反馈校准时将估计误差量反馈到系统中，可以防止滤波结果发散，使滤波收敛速度更快，自对准精度更高。

3、反馈校准在滤波稳定后进行，更新后的状态协方差矩阵Pk反应了滤波估计的稳定状态，以前八个对角元素作为判断滤波稳定的参考量，通过与设定值比较，若小于设定值则认为此时滤波处于稳定状态可对系统误差及惯性器件误差进行修正，修正后的误差参数直接进行下一周期计算，重复此过程可将误差逐步削减直至消失。

附图说明

图1为动基座下捷联惯导系统自对准过程流程图。

图2为混合校准kalman滤波方法计算流程图。

具体实施方式

下面结合图1和图2对本发明的具体实施步骤进行详细的描述：

在下文实施步骤的的详细描述中坐标系定义如下：

地球坐标系e系：原点选取地球中心，X轴位于赤道平面内，从地心指向载体所在点经线，Z轴沿地球自转轴方向，随地球自转而转动，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动。

地心惯性坐标系i系：惯性坐标系是指在空间静止或匀速直线运动的参考坐标系。地心惯性坐标系是原点取在地球地心O_e时的右手直角惯性坐标系。z_i轴沿地轴指向北极，y_i轴，z_i轴在地球赤道平面内，并指向空间的两颗恒星。

导航坐标系n系：是在导航时根据导航系统工作的需要而选取的作为导航基准的坐标系。本发明选取东北天坐标系作为导航坐标系，即Ox指向水平东向(e)，Oy轴指向水平北向(n)，Oz沿垂线方向指向天(u)。

载体坐标系b系：坐标原点O位于载体的重心处，纵轴Oy_b沿其首尾线方向并指向前，横轴Ox_b指向其右，Oz_b垂直于Ox_by_b，沿载体竖轴指向上。

惯性凝固坐标系i_b0系：在t₀时刻将载体坐标系b经惯性凝固后得到，其中t₀为粗对准的起始时刻。

动基座下车载捷联惯导系统自对准方法包括以下步骤：

步骤1，获得捷联惯导系统所在地理位置信息，其中地理位置信息包括经度λ、纬度

重力加速度g。

步骤2，利用经度、纬度、重力加速度信息及惯性器件输出信息，采用凝固坐标系，利用多矢量定姿方法进行粗对准，求得初始姿态矩阵。

初始姿态矩阵

分解为以下公式(1)中的形式：

其中，

为地理坐标系e系与导航坐标系n系之间的变换矩阵，表示载体与地球的相对位置；

为地心惯性坐标系i系与地理坐标系e系之间的变换矩阵，反映了载体的自转，

由时间间隔Δt＝t-t₀确定；

记录了载体坐标系b系与载体惯性凝固坐标系i_b0系之间的姿态变化信息；根据坐标系的相关定义，在载体纬度精确及地球自转角速度精确已知时，

和

可以精确求得；由定义可知，在t₀时刻，

可利用陀螺仪输出的b系相对于i_b0系的角运动信息通过姿态跟踪算法实时求解，计算公式如下：

其中，

为初始单位阵，

为陀螺仪输出，

表示载体坐标系b系相对于地心惯性坐标系i系的载体运动角速度在载体坐标系b系内的投影，

为向量

构造的叉乘反对称矩阵。

为载体惯性凝固坐标系i_b0系与地心惯性坐标系i系之间的变换矩阵。由定义可知该矩阵不随时间变化且与载体的运动状态无关，为一常值矩阵。选择其中m个时刻的速度作为参考矢量，构造公式(5)中的目标函数：

其中，

公式(7)中，f^b为加速度计输出；

整理变形公式(5)得：

L(C)＝L'-tr(CB^T) (8)

其中，

且与B无关；

是使L取最小值的正交矩阵C(行列式为1)。

对B进行奇异值分解，得：

B＝USV^T (9)

式(9)中，U和V为正交矩阵；S＝diag(s₁,s₂,s₃)，s₁≥s₂≥s₃；

求得：

由式(6)计算t时刻的vⁱ(t)，由式(7)分别计算t时刻的

再由式(10)即可计算

通过式(2)～式(4)分别计算

代入式(1)即可得到粗略的初始姿态矩阵，完成粗对准。

步骤3，根据捷联惯性系统误差方程及惯性器件误差方程构建kalman滤波状态方程和量测方程。其中，捷联惯性系统误差方程及惯性器件误差方程如下：

其中，δv_e、δv_n、δv_u分别为惯导系统东向、北向、天向速度误差；φ_e、φ_n、φ_u分别为俯仰、横滚、方位角误差；

分别为X、Y、Z三轴向加速度计的零偏；τ_a为加速度计零偏相关时间；ε为陀螺的零漂；τ_g为陀螺零漂相关时间；ω_ie为地球自转角速度；R_M为地球纬度圈半径，R_N为地球经度圈半径；w_a和w_g分别为加速度计和陀螺仪零均值的白噪声。

根据捷联惯导系统误差方程，建立系统状态方程为：

其中，A(t)为状态转移矩阵，由式(11)得到；略去惯性器件的刻度系数误差和安装误差，又因为惯性器件偏置量的重复性误差对系统精度的影响最大，因此将陀螺漂移和加速度零偏的随机常数部分列入状态，则滤波状态矢量及系统噪声分别为：

假设载体位置不变，且已经精确已知，选取位置误差作为观测量，则量测方程：

其中，

分别为捷联惯导系统解算的经纬度值；V为量测白噪声，H为量测矩阵，H＝[I_2×2 0_2×12]。

将式(12)和式(15)进行离散化，可得到系统离散化的kalman滤波状态方程和量测方程。

步骤4，设计混合校准kalman滤波方法估计系统误差量和惯性器件误差量并修正，实现精对准过程，完成初始对准。具体实现方法如下：

(1)设计混合校准kalman滤波方法

目前，应用kalman滤波方法对系统进行校准常用的两种方法为：输出校准和反馈校准。进行输出校准时，产品进行滤波估计但是估计值不对系统状态量进行修正；当应用反馈校准时，它将导航系统输出导航参数的误差值的滤波估计反馈到内部计算中，直接参与每次的导航解算，在理想情况下，这些误差量会在不断的迭代过程中被渐进消除。但kalman滤波器从开始到稳定需要一定的时间，且在滤波刚开始的时候，估计误差很大，并且有研究显示滤波估计的精度会受到载体运动状态的影响，因而若是在此时采用反馈校准将估计误差较大的导航误差补偿到系统内部计算中会更容易导致导航系统精度的降低，因此一般情况下反馈校正不能在滤波不稳定时实时进行。基于对二者适用性的分析，本发明在滤波开始阶段使用输出校准方式进行滤波估计，滤波稳定后进行反馈校准即用滤波估计值对系统速度误差、姿态误差、位置误差及惯性器件误差进行修正，修正后的系统误差直接参与下一周期的计算，重复此过程直至自对准结束。

上述滤波稳定状态通过取状态估计方差阵P_k中对应姿态、速度、位置对角线元素作为度量滤波估值程度的参考量，这八个对角元素反映了相应估值的稳定程度，将其与设定值(根据实际系统及实验条件进行设定)相比较，若小于设定值则认为滤波稳定，此时将滤波估计值对状态变量相应系统误差及惯性器件误差进行修正，修正后误差参数重新参与下一周期的计算，否则只进行输出校准。

输出校准采用公式(16)中的一般kalman滤波方程即可实现：

其中，

表示一步预测估计值；

表示X_k的状态估计值；K_k为k时刻的增益矩阵；P_k,k-1表示一步预测估计方差阵；P_k-1表示状态估计方差阵；R_k表示量测噪声方差阵；Q_k-1表示系统噪声方差阵。

具有反馈控制的系统状态方程和量测方程为：

与基本滤波方程相比，反馈校准仅在状态方程中增加控制项B_k-1U_k-1，其中B_k-1是控制矢量U_k-1的系数矩阵。其对应的滤波方程为：

根据反馈校准的原理可知，校准所要达到的效果就要将被估算出的误差量消减直至没有估计误差，因此可将控制矢量U_k-1选取为：

反馈校准的滤波方程为：

(2)，精对准过程误差姿态误差修正

粗对准确定的姿态矩阵是精对准过程姿态更新的初值。由于粗对准确定的姿态矩阵误差较大，实际建立的导航坐标系n'与要求建立的理想导航坐标系n系间存在偏差角，该偏差角即为对准失准角。设该失准角为φ_e、φ_n、φ_u，粗对准后该偏差角都为小角，所以φ＝[φ_e φ_n φ_u]^T可看做是n系至n'系的等效旋转矢量。则姿态捷联矩阵修正如下：

其中，

由式(23)可知，kalman滤波估计出失准角φ_e、φ_n、φ_u后，通过混合校准方法对姿态矩阵

进行校准，即可精确确定出载体姿态，完成自对准过程。

上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是局限性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护范围之内。

Claims (4)

1.一种动基座下车载捷联惯导系统自对准方法，其特征在于，包括以下步骤：

步骤1，获得捷联惯导系统所在地理位置信息，其中地理位置信息包括经度λ、纬度

重力加速度g；

步骤2，利用经度、纬度、重力加速度信息及惯性器件输出信息，采用凝固坐标系，利用多矢量定姿方法进行粗对准，求得初始姿态矩阵；

步骤3，根据捷联惯性系统误差方程及惯性器件误差方程构建kalman滤波状态方程和量测方程；

步骤4，设计混合校准kalman滤波方法估计系统误差量和惯性器件误差量并修正，实现精对准过程，完成初始对准；所述步骤4包括步骤41和步骤42，其中：

步骤41，设计混合校准kalman滤波方法：

输出校准采用公式(16)中的kalman滤波方程：

其中，

表示一步预测估计值；

表示X_k的状态估计值；K_k为k时刻的增益矩阵；P_k,k-1表示一步预测估计方差阵；P_k-1表示状态估计方差阵；R_k表示量测噪声方差阵；Q_k-1表示系统噪声方差阵；

具有反馈控制的系统状态方程和量测方程为：

对应的滤波方程为：

将控制矢量U_k-1选取为：

反馈校准的滤波方程为：

步骤42，精对准过程误差姿态误差修正：

姿态捷联矩阵修正如下：

其中，

kalman滤波估计出失准角φ_e、φ_n、φ_u后，通过混合校准方法对姿态矩阵

进行校准，精确确定出载体姿态，完成自对准过程。

2.如权利要求1所述的动基座下车载捷联惯导系统自对准方法，其特征在于，所述步骤2中，初始姿态矩阵

分解为以下公式(1)中的形式：

其中，

为地理坐标系e系与导航坐标系n系之间的变换矩阵，表示载体与地球的相对位置；

为地心惯性坐标系i系与地理坐标系e系之间的变换矩阵，反映了载体的自转，

由时间间隔Δt＝t-t₀确定；

记录了载体坐标系b系与载体惯性凝固坐标系i_b0系之间的姿态变化信息；

为载体惯性凝固坐标系i_b0系与地心惯性坐标系i系之间的变换矩阵。

3.如权利要求2所述的动基座下车载捷联惯导系统自对准方法，其特征在于，

其中，

记录了载体坐标系b系与载体惯性凝固坐标系i_b0系之间的姿态变化信息，

表示载体坐标系b系相对于地心惯性坐标系i系的载体运动角速度在载体坐标系b系内的投影，

为向量

构造的叉乘反对称矩阵；

计算过程包括：

选择m个时刻的速度作为参考矢量，构造公式(5)中的目标函数：

其中，

公式(7)中，f^b为加速度计输出；

整理变形公式(5)得：

L(C)＝L'-tr(CB^T) (8)

其中，

且与B无关；

是使L取最小值的正交矩阵C；

对B进行奇异值分解，得：

B＝USV^T (9)

式(9)中，U和V为正交矩阵；S＝diag(s₁,s₂,s₃)，s₁≥s₂≥s₃；

求得：

4.如权利要求1或2所述的动基座下车载捷联惯导系统自对准方法，其特征在于，所述步骤3中，捷联惯性系统误差方程及惯性器件误差方程如下：

其中，δv_e、δv_n、δv_u分别为惯导系统东向、北向、天向速度误差；φ_e、φ_n、φ_u分别为俯仰、横滚、方位角误差；τ_a为加速度计零偏相关时间；ε为陀螺的零漂；τ_g为陀螺零漂相关时间；ω_ie为地球自转角速度；R_M为地球纬度圈半径，R_N为地球经度圈半径；w_a和w_g分别为加速度计和陀螺仪零均值的白噪声；

根据捷联惯导系统误差方程，建立系统状态方程为：

其中，A(t)为状态转移矩阵，由式(11)得到；略去惯性器件的刻度系数误差和安装误差，又因为惯性器件偏置量的重复性误差对系统精度的影响最大，因此将陀螺漂移和加速度零偏的随机常数部分列入状态，则滤波状态矢量及系统噪声分别为：

假设载体位置不变，且已经精确已知，选取位置误差作为观测量，则量测方程：

其中，

分别为捷联惯导系统解算的经纬度值；V为量测白噪声，H为量测矩阵，H＝[I_2×2 0_2×12]；

将式(12)和式(15)进行离散化，得到系统离散化的kalman滤波状态方程和量测方程。

Images