一种带双轴转位机构捷联惯导的三位置自对准方法与流程

本发明属于捷联惯性导航领域，提供一种带双轴转位机构捷联惯导的三位置自对准方法。

背景技术：

现代战争对战略战术导弹提出了越来越高的要求，为提高导弹武器系统的反应速度和命中精度，对惯性导航系统提出了更高的要求。提高惯性传感器精度成本高、周期长、难度大，受国内基础工业薄弱和国外对华武器禁运的影响，短期内难以取得突破性进展。而通过对模型的改进和算法的合理设计可以有效的提高惯导系统的精度。

与平台式惯性导航系统相比，捷联惯导系统具有反应快、成本低、信息全、可靠性高的优点，但同时捷联惯导系统的精度，为提高捷联惯导系统的精度，避免反复将捷联惯导系统从载体上拆下进行参数标定，进行系统分解维护，节约使用成本。捷联惯导增加双轴转位机构，使捷联惯导系统的免拆弹自检、标定和维护，有利于提高导弹武器装备的可操作性和机动性，对我国武器装备发展具有重要的使用价值和意义。

捷联惯导系统自对准，由惯性敏感元件的输出信息，经过导航计算机按照对准程序实时运算，不断地将数学平台变换到能精确描述理想载体坐标系到地理坐标系的方向余弦阵。传统的捷联式惯导系统由于无转位机构，自对准为单位置自对准，其本质是将重力加速度的方向做为水平对准的基准，其对准精度主要取决于两个加速度计的精度。用于方位对准本质是将地球角速率的北向分量方向做为方位对准的基准，其对准精度主要取决于东向陀螺漂移。带有单轴转位机构的捷联惯导系统能够在姿态接近水平的条件下，对等效东向陀螺的漂移进行辨识，进而提高自对准的精度。对双轴转位机构的捷联惯导系统，由于存在转位机构，可以通过旋转转位机构进行多位置对准，通过对自对准误差模型进行改进，设计三位置的快速自对准方法，可有效提高带双轴转位机构的捷联惯导系统的自对准精度。

技术实现要素：

本发明的目的在于克服现有单位置自对准和双位置自对准技术的不足，提供一种双轴转位机构捷联惯导的三位置自对准方法，该方法通过改进自对准误差模型，设置双轴转位机构的旋转次序和位置，以及设计三位置自对准算法，有效提高了自对准精度。

为达到上述目的，本发明采用以下技术方案予以实现：

一种带双轴转位机构捷联惯导的三位置自对准方法，包括以下步骤：

1)对以速度误差为量测量的卡尔曼滤波模型进行可观测性分析，对滤波模型进行改进，变换出新的可观测状态组合，并对其可观测性和收敛速度进行分析；

2)在初始位置按照转位机构先绕内框架转-90°，再绕转位机构的外框架转动-90°，作为捷联惯导系统自对准的第一位置，采集惯导系统的陀螺和加速度计数据。根据记录的到时刻陀螺和加速度计的输出，进行惯性系粗对准和改进模型的精对准，完成等效北向陀螺漂移和天向加速度零位的测漂；

3)绕外框架转90°得到第二位置，在第二位置静止采集陀螺和加速度计的输出。根据记录的到时刻陀螺和加速度计的输出，进行惯性系粗对准和改进模型的精对准，完成等效北向陀螺漂移和天向加速度零位的测漂。

4)绕内框架转90°得到第三位置，在第三位置静止采集陀螺和加速度计的输出。根据记录的到时刻陀螺和加速度计的输出，进行惯性系粗对准和改进模型的精对准，完成等效北向陀螺漂移和天向加速度零位的测漂。

5)依据三个位置姿态矩阵和和测漂结果进行反解得到ε_x、ε_y和ε_z。利用第三位置的姿态矩阵和ε_x、ε_y和ε_z计算依据三个位置姿态矩阵和和测漂结果进行反解得到▽_x、▽_y和▽_z。利用第三位置的姿态矩阵和▽_x、▽_y和▽_z计算和

6)依据改进模型计算对准结束时刻的失准角φ_E、φ_N和φ_U。进行一次修正后得到最终的精对准结果。

与现有技术相比，本发明具有以下有益效果：

本发明通过改进模型可以使捷联惯导系统的等效北向陀螺漂移和等效天向加速度计的零偏快速收敛；提出的带双轴转位机构捷联惯导三位置自对准方法，适用于捷联惯导系统在初始任意姿态角条件下的快速自对准；本发明可以有效缩短对准时间，提高带双轴转位机构的捷联惯导系统的自对准精度。

【附图说明】

图1为零位锁定时坐标系与两框架轴编排示意图；

图2为初始位置及转位过程示意图；

图3为第一位置等效北向陀螺漂移估计曲线；

图4为第二位置等效北向陀螺漂移估计曲线；

图5为第三位置等效北向陀螺漂移估计曲线；

图6为两种方法俯仰角误差对比曲线；

图7为两种方法滚转角误差对比曲线；

图8为两种方法航向角误差对比曲线。

【具体实施方式】

下面结合附图对本发明做进一步详细描述：

参见图1-图8，本发明带双轴转位机构捷联惯导的三位置自对准方法，包括以下步骤：

步骤1，建立改进的卡尔曼滤波模型

以东北天坐标系作为导航坐标系，其中导航坐标系用n表示、机体坐标系用b表示、惯性坐标系用i表示。ω_ie中的下标表示地球坐标系相对于惯性坐标系的旋转角速度，ε^b表示等效陀螺漂移ε在b系内的投影，▽^b表示等效加速度计的零偏▽在b系内的投影。速度误差向量δVⁿ、姿态误差向量φ、导航坐标系下的陀螺常值漂移εⁿ和加速度计零偏▽ⁿ，其形式如下所示：

在自对准过程中，载体无移动，捷联惯导系统的速度误差方程为：

其中，F_vφ＝(fⁿ×)，“×”表示叉乘运算。

捷联惯导系统的姿态误差方程为：

其中，R_M、R_N分别为经纬圈和卯酉圈半径，L、h分别为纬度和高度。

将等效陀螺的漂移ε^b和等效加速度计零偏▽^b视为随机常值误差，选取状态变量为：X＝[δVⁿ φ ▽ⁿ εⁿ]^Τ，同时以速度误差δVⁿ作为外界量测，则系统模型可表示如下：

其中H＝[I 0 0 0]，I为3阶单位矩阵，0为3阶零矩阵。V为由晃动等干扰引起的量测噪声，可以近似看作白噪声序列，假设其方差为R。

步骤2，可观测性分析

依据连续时间线性定常系统可观测性分析相关理论，在单一位置对式(3)所表示的系统进行可观测性分析，构造可观测性矩阵Q＝[H^Τ (HF)^Τ … (HF¹¹)^Τ]^Τ，令Q₁＝[H^Τ(HF)^Τ … (HF⁴)^Τ]^Τ。

其中

令其中N₂₁＝-F_vv，N₃₁＝-F_φv，

可以证明，当L≠0°或90°时A₂可逆，当L＝0°或90°时用类似的方法也能得到式(6)同样的结果。

对式(4)左乘矩阵N进行行变换，得：

由式(5)中第5行N₅₂HF+N₅₄HF³+HF⁴＝0可得，HF⁴可由HF和HF³线性表示，因此HF^k(k＝4,5…11)均可由HF和HF³线性表示。因此rank(Q)＝rank(Q₁)＝rank(NQ₁)。

由式(5)可得，rank(NQ₁)＝9，因此可以得出结论：在任一固定位置，式(3)表示的系统可观测矩阵的秩为9，不满足完全可观测的条件。但是存在可观测组合，现加以分析。

记则Y＝Q₁X，令其中

其中

则式(3)表示的系统的可观测组合状态如下：

至此可观测性分析完毕，从分析结果可见，除速度误差状态和等效天向加速度计零偏直接可观测以外，其余的状态均为各种形式的组合，其中水平姿态误差角和等效东向及等效北向加速度计零偏为一可观测状态组合，航向误差角和等效东向陀螺漂移以及等效东向加速度计零偏为一可观测状态组合，等效北向陀螺漂移和等效北向加速度计零偏为一可观测状态组合，等效天向陀螺漂移和等效北向加速度计零偏为一可观测状态组合。可以看出，采用单位置对准方法时，φ_E的对准误差为φ_N的对准误差为φ_U的对准误差为由此可见，除等效天向加速度计的零偏以外其余的状态估计精度均受制于相应的惯性器件的精度。

步骤3，可观测组合的估计收敛速度分析

由式(7)可以对式(8)表示的可观测组合的收敛速度进行分析。根据式(7)可知，MNY的前9阶对应可观测组合的量测，因此，仅需对MNY矩阵的前9行进行分析，过程如下：

[MN]_1:9Y＝[MNQ₁]_1:9X (9)

式中，下标1:9表示矩阵的前9行。

由式(10)可知，式(9)的前3行对应状态量可由Z直接得到，第4～6行对应的可观测状态组合可由Z和Z一阶导得到，第7～9行对应的可观测状态组合分析如下

NY＝NQ₁X (11)

由式(12)可得

即方程两边同时左乘矩阵可得取其前两行记为

取式(15)的前两行，记作

第7～8行对应的可观测状态组合可由Z、和得到，第9行对应的可观测状态组合可由Z、和Z⁽³⁾得到。需要量测量阶次信息的阶次越高对应状态所需估计的时间就越长。因此可以得出如下结论：除速度误差外状态组合外，(PX)_4:6的收敛速度最快，(PX)_7:8次之，而(PX)₉最慢，至此收敛速度分析完毕。

步骤4，三位置自对准方法

由式(8)可得，若ε_E、▽_E和▽_N能够快速准确辨识出，即可求解初始失准角φ_E,φ_N,φ_U。因此，自对准的问题转化为如何快速准确辨识ε_E、▽_E和▽_N。在实际对准过程中，与▽_E，▽_N相比，ε_E的影响较大。为减小ε_E的的影响，双位置中提出绕方位轴正向转90°再次进行对准。在该位置东向等效陀螺漂移的计算值其中为第二个位置处等效的北向陀螺漂移。但存在两个问题，第一：若初始对准时载体不在水平面，即存在一定的俯仰角或者横滚角，如导弹发射车处于斜坡或者一侧轮胎处于台阶上，此时第二位置的等效北向陀螺漂移不等于第一位置的等效东向陀螺漂移，此时，第一位置的等效东向陀螺漂移不可测；第二，两位置测漂法中，无法对与▽_E和▽_N进行辨识，水平对准的精度无法进一步提高。基于以上两个问题，本专利在改进对准误差模型的基础上，提出了适用于带双轴转位机构捷联惯导系统的三位置对准法。

如图1所示，双轴捷联惯导系统内框架可以沿惯导系统的Z轴转动，外框架沿惯导系统的Y轴转动。在初始对准时，首先控制内框架绕IMU的Z轴转动-90°，然后控制外框架绕IMU的X轴转动-90°，作为初始对准的第一个位置。按照式(9)在此位置进行改进模型的卡尔曼滤波精对准。

其中θ，γ和ψ分别为载体的俯仰，横滚和航向角。ε_x，ε_y和ε_z分别为x，y，z方向的等效陀螺漂移，▽_x，▽_y和▽_z分别为x，y，z方向的等效加速度计零偏。

控制外框架绕IMU的X轴转动90°，作为初始对准的第二个位置。按照式(9)在此位置进行改进模型的卡尔曼滤波精对准。

控制外框架绕IMU的Z轴转动90°，作为初始对准的第三个位置。按照式(9)在此位置进行改进模型的卡尔曼滤波精对准。

在三个位置处均可对等效北向陀螺漂移ε_N和等效天向加速度计零偏▽_U进行辨识，即和▽_x、▽_y、和▽_z可以得到。根据式(24)，可得：

其中

若矩阵A满秩，此时ε_x、ε_y和ε_z可由式(28)唯一确定。

此时，可由式(29)得到。

其中C＝[cosγcosψ-sinγsinθsinψ -cosθsinψ sinγcosψ+cosγsinθsinψ]。

若矩阵B满秩，此时▽_x、▽_y和▽_z可由式(30)唯一确定。

此时，和可由式(31)得到。

其中

在第三位置进行对准，按照估计出来的和对估计结果进行补偿，即可计算出对准结束时刻的失准角φ_E、φ_N和φ_U。

步骤5，殊情况的讨论

ε_x、ε_y、ε_z、▽_x、▽_y、和▽_z的可观测性问题等价于式(26)和式(27)的求解问题，为此，先引入两个定理。

定理1：齐次线性方程组的系数矩阵秩rank(A)＝n，方程组有唯一解。齐次线性方程组的系数矩阵秩rank(A)＜n,方程组有无数多解。

定理2：A是n×n矩阵，则

a)rank(A)＝n的充要条件为|A|≠0，称A为满秩矩阵；

b)rank(A)＜n的充要条件为|A|＝0。

式(26)能够求解陀螺漂移的前提是矩阵A满秩，因此，求解使得|A|＝0时的初始姿态角θ₀、γ₀和ψ₀，出当初始姿态角为θ₀、γ₀和ψ₀时，ε_x、ε_y和ε_z不完全可观测。同理可以求出▽_x、▽_y、和▽_z不可观测的姿态角初值。

在导弹武器系统的实际的应用过程中，导弹发射车的初始姿态角受到实际应用环境的限制，俯仰角、横滚角在一定范围内变化，根据实际使用情况，取-45°≤θ≤45°，-45°≤γ≤45°，-180°＜ψ≤180°。求解|A|＝0得：当θ₀＝0°，ψ₀＝0°(或ψ₀＝180°)时，|A|＝0，rank(A)＜3。此时ε_x、ε_y和ε_z不完全可观测。当θ₀＝0°，ψ₀＝0°时，初始位置时Y向指北，第一位置和第二位置时X向指南，第三位置时Y向指北，此时，X向和Y向的陀螺漂移ε_x和ε_y均可通过等效北向陀螺测漂得到，Z向陀螺的漂移ε_z不可观测。当θ₀＝0°，ψ₀＝180°时，初始位置时Y向指南，第一位置和第二位置时X向指北，第三位置时Y向指南，此时，X向和Y向的陀螺漂移ε_x和ε_y均可通过等效北向陀螺测漂得到，Z向陀螺的漂移ε_z不可观测。此时，由分析可知，陀螺漂移ε_x和ε_y可以观测，ε_z不可观测，只能设置为ε_z＝0。

当θ₀≠0°或ψ₀≠0°和180°时，|A|≠0，rank(A)＝3。此时ε_x、ε_y和ε_z可由式(28)唯一确定。

求解|B|＝0得：当θ₀＝0°和γ₀＝0°同时成立时，|B|＝0，rank(B)＜3。此时▽_x、▽_y和▽_z不完全可观测。当θ₀＝0°，γ₀＝0°时，初始位置时Z向指天，第一位置Y向指地，第二位置和第三位置时Z向指天，此时，Y向和Z向的加速度计零偏▽_y、和▽_z均可通过等效天向加速度计测漂得到，X向加速度计的零偏▽_x不可观测。此时，由分析可知，加速度计零偏▽_y和▽_z可以观测，▽_x不可观测，只能设置为▽_x＝0。

当θ₀≠0°或γ₀≠0°时，|B|≠0|，rank(B)＝3。此时▽_x、▽_y和▽_z可由式(30)唯一确定。

【验证】

为验证分析的正确性，进行了仿真验证，陀螺仪的等效漂移取为[0.03 0.03 0.03]°/h，加速度计等效零偏取为[50 50 50]μg，采样频率为200Hz，捷联惯导系统的初始姿态为att_0＝[15° 10° 30°]。开始对准时捷联惯导系统的台体先绕内框Z轴转动-90°，再绕外框X轴转动-90°作为对准的第一位置，转动过程10s，在第一位置采集数据90s。然后绕外框X轴转动90°度作为对准的第二位置，转动过程10s，在第二位置采集数据90s。再绕内框Z轴转动90°作为对准的第三位置，转动过程10s，在第三位置采集数据90s，对准时间共计300s。分别采用单位置卡尔曼滤波对准法、双位置卡尔曼滤波对准法和本专利提出的三位置自对准方法进行对准，图3～图5为三个位置上的等效北向陀螺漂移和等效加速度计天向零偏的估计曲线。表1为利用不同方法进行对准的试验结果，图6～图8为不同方法进行姿态估计的曲线。由图中可以看出，本专利提出的三位置自对准方法航向角收敛速度和精度明显优于另两种方法。

表1 不同对准方法的结果

通过由仿真结果可以看出，在相同的时间内，本专利提出的可以有效的提高捷联惯导系统的初始对准精度，是双位置对准方法误差的10％，是单位置对准方法误差的5％。使用本专利提出的可以使捷联惯导系统的航向对准误差有效降低。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。