CN111024128B - 一种机载光电吊舱光轴稳定状态传递对准方法 - Google Patents

Abstract

本发明属于组合导航技术领域，具体涉及一种机载光电吊舱光轴稳定状态传递对准方法。本发明利用主惯导的姿态信息和光电吊舱两轴稳定平台的编码器融合得到光轴稳定状态吊舱可用的姿态信息，加入量测信息对SINS系统进行实时校正，并针对数据丢包现象利用一步预测值代替丢包现象发生时的量测量，用自适应滤波器估计量测量的误差。本发明提出的姿态加速度匹配方法在任意失准角情况下收敛精度更高，更稳定，在有数据丢包现象发生时有较好的收敛精度，提高了系统的鲁棒性。

Description

一种机载光电吊舱光轴稳定状态传递对准方法

技术领域

本发明属于组合导航技术领域，具体涉及一种机载光电吊舱光轴稳定状态传递对准方法。

背景技术

机载光电吊舱在开始工作之前要先进行初始对准，目的是使得吊舱内惯导系统(INS，Inertial Navigation System)能够建立起合适的导航坐标系。初始导航信息的准确与否决定了接下来的导航是否可靠，因此精确地初始对准是组合导航系统保证精度的关键。初始对准的指标要求有两个，一个是对准精度，二是对准时间，通常要求对准时间尽量短，对准精度尽量高，这是两个互相矛盾的条件，在不同的场合侧重点不同。对于高精度惯导系统，系统可以在静基座环境下通过感应地球自转和重力加速度确定初始姿态，但是高精度陀螺通常具有体积大，重量大等特点。对于低精度陀螺，系统噪声大于地球自转角速率，无法进行静基座对准，需要外部传感器辅助在运动状态下完成初始对准。由于飞机对外挂设备的体积和重量限制，吊舱内部只能安装中低等精度陀螺，需要通过动态对准来完成初始对准。

现阶段，适用于机载设备的动基座对准的解决办法大致分为两类：一种是利用全球导航卫星系统(GNSS，Global Navigation Satellite System)辅助惯导系统进行动基座初始对准；另一种是利用飞机主惯导系统的导航定位信息辅助低精度惯导系统进行传递对准。主惯导在可靠性、信息连续性和抗干扰方面具有显著优势，所以利用主惯导系统的导航定位信息辅助子惯导系统进行传递对准是一种有效的方法。

为了稳定平台隔离载机振动对内部光电传感器的影响，高精度吊舱在对准阶段借助陀螺稳定平台来保持光轴稳定(光轴相对于惯性系稳定)。吊舱的光轴稳定状态导致传递对准过程中主惯导系统(MINS，Master Inertial Navigation System)姿态和子惯导系统(SINS，Slave Inertial Navigation System)姿态不匹配，传统传递对准算法中只能利用主惯导的速度信息辅助吊舱进行对准。光轴稳定吊舱现有传递对准方法存在以下问题：第一，只利用速度匹配的传递对准算法姿态误差可观测性较差，导致对准结果姿态精度差，收敛速度慢，不满足吊舱对姿态精度要求以及工作状态准备时间要求；第二，由于光电吊舱使用时不能准确将MINS系统和SINS系统标校到同一坐标系，如果直接利用MINS系统姿态信息对SINS系统姿态进行装订，会出现任意初始失准角，当初始失准角较大时，会导致对准精度较差甚至对准失败。第三，由于主子惯导系统拥有独立的时钟源，主子惯导时钟不同步以及主惯导数据传递给子惯导时会有一定的误码率，为了保证数据的完整性，当子惯导系统检测到数据含有误码时会舍弃掉整包数据，发生数据丢包现象。数据丢包现象发生时，子惯导系统利用之前未丢包末时刻的数据当做量测量进行最优估计，会严重影响子惯导系统的性能。

发明内容

本发明的目的在于提供适用于任意初始失准角，精度更高，鲁棒性更强的光轴稳定状态吊舱传递对准的一种机载光电吊舱光轴稳定状态传递对准方法。

本发明的目的通过如下技术方案来实现：包括以下步骤：

步骤1：生成光轴稳定状态下光电吊舱与MINS仿真轨迹；

步骤2：SINS系统传递对准参数初始化；

步骤3：使用SINS传感器数据外推SINS姿态、速度和位置变化；

步骤4：将编码器值和MINS导航数据融合为SINS系统可用的姿态信息；

步骤5：选取状态变量，构建传递对准系统的状态方程和量测方程；

选择系统状态变量X:

其中，

为载体系姿态矩阵对应的失准角；δv＝[δv_E δv_N]为东向，北向速度误差；ε＝[ε_x ε_y ε_z]为陀螺常值漂移；

为加表常值漂移；μ＝[μ_x μ_y μ_z]为固定安装偏差角；

传递对准系统的状态方程为：

其中，

为状态变量

的导数；

为状态变量δv的导数；

为状态变量ε的导数；

为状态变量

的导数；

为状态变量

的导数；

为当地n系相对于i系的旋转角速率，

R_M和R_N分别为子午圈曲率半径和卯酉圈曲率半径，h为当地高度，L为纬度，fⁿ为子惯导在n系下的比力；

(3)量测方程为：

其中，qq2phi(·)表示将方向余弦矩阵转换为姿态角，

表示子惯导方向余弦矩阵，

表示主惯导方向余弦矩阵，

即由子惯导坐标系到编码器零位坐标系的方向余弦矩阵，μ为主惯导载体系与编码器零位之间安装偏差角；

步骤6：利用量测数据更新时间间隔检测数据丢包现象是否发生；

步骤7：使用改进的自适应滤波算法估计失准角、速度误差、陀螺漂移、加表漂移和安装偏差角等误差；

当主惯导系统所传递的量测信息丢失时，运用一步预测值来代替当前量测值，前一时刻均方误差阵代替当前时刻均方误差阵，直到接收到新的量测值后进行标准卡尔曼滤波。

若k时刻数据丢包发生，具体的改进的自适应滤波算法方案步骤如下。

步骤7.1：k时刻对下一步状态

的预测：

其中，X_k为状态向量，Φ_k/k-1为一步状态转移矩阵；

步骤7.2：计算一步预测均方误差矩阵P_k/k-1：

其中，Γ_k-1为系统状态噪声驱动矩阵；

步骤7.3：计算量测新息：

其中，H_k为量测矩阵；Z_k为系统的量测向量；

步骤7.4：Sage-Husa自适应估计量测噪声协方差矩阵：

其中，初值β₀＝1，b为渐消因子，b＝0.9～0.999；

步骤7.5：滤波增益更新：

其中，K_k是系统的滤波增益矩阵，P_k是系统的状态均方差矩阵；

步骤7.6：状态估计：

步骤7.7：状态估计均方差：

P_k＝(I-K_kH_k)P_k/k-1

步骤8：将估计的失准角和速度误差对SINS导航信息输出校正；

步骤9：实验采集吊舱内部SINS传感器数据、MINS系统导航数据和吊舱编码器数据；循环步骤2到步骤8，完成光电吊舱实测数据传递对准。

本发明的有益效果在于：

本发明设计了一种机载光电吊舱光轴稳定状态传递对准方法，利用主惯导的姿态信息和光电吊舱两轴稳定平台的编码器融合得到光轴稳定状态吊舱可用的姿态信息，加入量测信息对SINS系统进行实时校正，并针对数据丢包现象利用一步预测值代替丢包现象发生时的量测量，用自适应滤波器估计量测量的误差。本发明提出的姿态加速度匹配方法在任意失准角情况下收敛精度更高，更稳定，在有数据丢包现象发生时有较好的收敛精度，提高了系统的鲁棒性。

附图说明

图1为本发明的吊舱光轴稳定状态仿真轨迹生成原理图.

图2为吊舱光轴稳定状态仿真轨迹中MINS、SINS航姿与编码器值变化图。

图3为定点瞄目标示意图。

图4为本发明实施例1中仿真轨迹图。

图5为本发明实施例1中仿真轨迹载机航姿变化图。

图6为本发明实施例1中仿真轨迹光电吊舱航姿变化图。

图7为本发明实施例1中仿真轨迹编码器值变化。；

图8为本发明实施例1中姿态加速度匹配传递对准算法与速度匹配算法小初始失准角条件下失准角对比图。

图9为本发明实施例1中姿态加速度匹配传递对准算法与速度匹配算法小初始失准角条件下速度误差对比图。

图10为本发明实施例1中姿态加速度匹配传递对准算法与速度匹配算法大初始失准角条件下失准角对比图。

图11为本发明实施例1中姿态加速度匹配传递对准算法与速度匹配算法大初始失准角条件下速度误差对比。

图12为本发明实施例2中跑车实验车过程中运动轨迹图。

图13为本发明实施例2中运载体参考航向变化图。

图14为本发明实施例2中吊舱编码器值变化图。

图15为本发明实施例2中姿态加速度匹配传递对准算法与速度匹配算法小初始失准角条件下航向角对比图。

图16为本发明实施例2中姿态加速度匹配传递对准算法与速度匹配算法大初始失准角条件下姿态误差对比图。

图17为本发明实施例2中姿态加速度匹配传递对准算法与速度匹配算法大初始失准角条件下速度误差对比图。

图18(a)为本发明实施例2中实测数据有数据丢包与未数据丢包的速度对比图。

图18(b)为本发明实施例2中实测数据有数据丢包与未数据丢包的速度对比图的局部放大图。

图19为本发明实施例2中实测数据对数据丢包现象进行滤波算法改进与未改进的姿态误差角对比图。

图20为本发明实施例2中实测数据对数据丢包现象进行滤波算法改进与未改进的速度误差对比图。

具体实施方式

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

本发明提供了一种主惯导系统(MINS，Master Inertial Navigation System)辅助的机载光电吊舱子惯导系统(SINS，Slave Inertial Navigation System)在光轴稳定状态下的姿态加速度匹配传递对准方法。主要解决光轴稳定状态下机载光电吊舱初始对准问题。光轴稳定状态下主子惯导姿态不匹配，只能用速度匹配进行传递对准，导致SINS系统姿态可观测性较差，姿态对准精度较差；同时速度匹配方法会导致SINS系统在对准初始时刻会有任意失准角，当失准角为大失准角时有可能会导致对准失败；由于主子惯导系统时钟不同步以及数据传输较快，传递至子惯导的MINS导航信息会有一定的误码，产生数据丢包现象会导致传递对准精度较差。本发明建立光轴稳定状态下MINS、SINS与编码器之间运动关系方程，并提出利用MINS姿态信息融合光电吊舱编码器信息，建立光轴稳定状态下姿态加速度匹配传递对准方法；针对数据丢包现象对滤波算法做了改进。通过与传统速度匹配传递对准方法对比，本发明所提出方法能够在任意初始失准角条件下有效提高传递对准精度，在数据丢包现象发生时，本发明提出的算法也有较好的收敛精度，提高了系统鲁棒性。

通过对机载光电吊舱工作状态以及传递对准算法研究，本发明设计了一种新型的姿态加速度匹配传递对准方法，利用主惯导的姿态信息和光电吊舱两轴稳定平台的编码器融合得到光轴稳定状态吊舱可用的姿态信息，加入量测信息对SINS系统进行实时校正，并针对数据丢包现象利用一步预测值代替丢包现象发生时的量测量，用自适应滤波器估计量测量的误差。光电吊舱伺服编码器零位相对载机姿态固定，所以只需要将编码器处于零位时的子惯导与飞机主惯导标校到同一坐标系即可。为了验证本发明有效性，用仿真数据和实测数据对分别在小初始失准角和大初始失准角情况下进行了两种匹配方法的仿真对比验证，并对针对数据丢包现象改进的滤波算法进行了实测数据验证。通过验证，本发明提出的姿态加速度匹配方法在任意失准角情况下收敛精度更高，更稳定，在有数据丢包现象发生时有较好的收敛精度，提高了系统的鲁棒性。

本发明设计了一种适用于任意初始失准角，精度更高，鲁棒性更强的光轴稳定状态吊舱传递对准方法。通过分析光轴稳定状态下MINS姿态、SINS姿态和编码器值之间关系，融合MINS姿态与编码器数据为吊舱可用姿态，引入量测方程，替代传统速度匹配传递对准方法，并针对实验环境数据丢包现象对滤波算法做了改进。

本发明解决的技术问题所采用的技术方案包括以下步骤：

(1)构建光电吊舱MINS、SINS与编码器之间运动关系方程；

(2)根据MINS运动轨迹、吊舱光轴稳定条件以及步骤1运动关系方程推导SINS运动轨迹和吊舱编码器值，生成轨迹验证上述推导的正确性；

(3)融合编码器值与MINS姿态为吊舱SINS可用姿态，选取状态变量，将SINS可用姿态引入量测方程，重新构建量测方程。

(4)针对数据丢包现象对滤波算法做鲁棒处理。

本发明机载半稳态光电吊舱的传递对准方法包括：

1.构建光电吊舱MINS、SINS与编码器之间运动关系方程

首先定义编码器零位坐标系(c0系)—O_c0x_c0y_c0z_c0

该坐标系的原点O_c0位于光电吊舱两转动轴的交点，x_c0轴、y_c0轴和z_c0轴分别指向在编码器零位时光电吊舱的右方，前方和上方。

SINS姿态矩阵分解：

其中，n表示导航系，s表示SINS载体系，m表示MINS载体系，c0表示编码器零位系，

为s系到n系的姿态变化矩阵，

为m系到n系的姿态变化矩阵，

为c0系到m系的姿态变化矩阵，

为s系到c0系的姿态变化矩阵。

将上式移项得：

式中

光电吊舱是双轴稳定平台，

由方位码盘值α_c，俯仰编码盘值β_c构成。其中

分别为m系与c₀系之间的航向，俯仰及横滚安装偏差角，

分别为SINS航向，俯仰及横滚角，

分别为MINS航向，俯仰及横滚角。

2.根据MINS运动轨迹、吊舱光轴稳定条件以及步骤1运动关系方程推导SINS运动轨迹和吊舱编码器值，生成轨迹验证上述推导的正确性

光轴稳定状态下的吊舱光轴相对于惯性系稳定，但是由于平台是两轴稳定平台，并不能做到三轴全部相对于惯性系稳定，只能保证光轴(即子惯导系统Y轴)相对于惯性系稳定。

首先定义光轴坐标系(2系)—O₂x₂y₂z₂

该坐标系的原点在光电吊舱(即SINS)中心，在传递对准初始零时刻，x₂轴、y₂轴和z₂轴指向与SINS载体坐标系重合，在传递对准过程中t时刻光轴坐标系相对于惯性坐标系稳定。

由于在传递对准过程中t时刻光轴坐标系相对于惯性坐标系稳定，即：

根据姿态矩阵的链乘法则，将

拆分得：

式中

其中，i表示惯性系，t代表从吊舱内部导航系统开始工作到当前时刻的时间差，e(t)表示t时刻地球坐标系，n(t)表示t时刻导航坐标系，2(t)表示吊舱只绕俯仰轴和方位轴转动后的光轴坐标系，

为t时刻e系到i系的姿态变化矩阵，

为t时刻n系到e系的姿态变化矩阵，

为t时刻2系到n系的姿态变化矩阵，ω_ie表示地球自转角速度，λ(t)和L(t)分别代表t时刻的经度和纬度,

和β_s(t)为t时刻SINS系统的航向角和俯仰角。

由上式可得在轨迹发生器中光轴稳定状态下吊舱理论方位角和俯仰角信息

根据

β_s代入

可以求得光轴稳定状态下横滚角以及两轴编码器的角度

为了书写方便，式中将sin(·)、cos(·)简写为s(·)、c(·)，L(t)、λ(t)简写为L_t、λ_t，L(0)、λ(0)简写为L₀、λ₀，ω_ie简写为ω，

简写为

简写为

简写

光电吊舱轨迹生成原理图如图1所示。假设传递对准初始时刻，光轴坐标系与子惯导系统载体坐标系重合，由于光轴稳定平台是两轴稳定平台，当运载体没有横滚轴的转动时，

为了验证上面光电吊舱光轴稳定下的轨迹推导正确。假设在光电吊舱光轴稳定状态下，运载体没有线性运动(即速度为零)，只有角度转动。SINS以及编码器值仿真结果图2所示。由仿真曲线可以看出，在转动载体和俯仰角时吊舱编码器会等值反方向转动，转动横滚角时，SINS会跟随MINS同时转动，证明仿真平台能够保证光电吊舱光轴稳定，验证推导公式的正确性。

3.融合编码器值与MINS姿态为吊舱SINS可用姿态，选取状态变量，将SINS可用姿态引入量测方程，重新构建量测方程

(1)选择系统状态变量X:

其中，

为载体系姿态矩阵对应的失准角，δv＝[δv_E δv_N]为东向，北向速度误差，ε＝[ε_x ε_y ε_z]为陀螺常值漂移，

为加表常值漂移，μ＝[μ_x μ_y μ_z]为固定安装偏差角。

(2)状态方程为：

其中，

为状态变量

的导数，

为状态变量δv的导数，

为状态变量ε的导数，

为状态变量

的导数，

为状态变量

的导数，

为当地n系相对于i系的旋转角速率，

R_M和R_N分别为子午圈曲率半径和卯酉圈曲率半径，h为当地高度，L为纬度，fⁿ为子惯导在n系下的比力。

(3)量测方程为：

式中，

qq2phi(·)表示将方向余弦矩阵转换为姿态角，

表示子惯导方向余弦矩阵，

表示主惯导方向余弦矩阵，

即由子惯导坐标系到编码器零位坐标系的方向余弦矩阵，μ为主惯导载体系与编码器零位之间安装偏差角。

4.针对数据丢包现象对滤波算法做鲁棒处理

当主惯导系统所传递的量测信息丢失时，运用一步预测值来代替当前量测值，前一时刻均方误差阵代替当前时刻均方误差阵，直到接收到新的量测值后进行标准卡尔曼滤波。

若k时刻数据丢包发生，具体的改进的自适应滤波算法方案步骤如下。

Stept1.k时刻对下一步状态

的预测：

Stept2.计算一步预测均方误差矩阵P_k/k-1：

Stept3.计算量测新息：

Stept4.Sage-Husa自适应估计量测噪声协方差矩阵：

Stept5.滤波增益更新：

Stept6.状态估计：

Stept7.状态估计均方差：

P_k＝(I-K_kH_k)P_k/k-1

其中，X_k为状态向量，Φ_k/k-1为一步状态转移矩阵，Γ_k-1为系统状态噪声驱动矩阵，H_k为量测矩阵。Z_k为系统的量测向量，K_k是系统的滤波增益矩阵，P_k是系统的状态均方差矩阵。初值β₀＝1，b为渐消因子，b＝0.9～0.999。

本发明为机载光电吊舱传递对准方法研究，图1是本发明流程图，具体步骤如下：

(1)生成光轴稳定状态下光电吊舱与MINS仿真轨迹；

(2)SINS系统传递对准参数初始化；

(3)使用SINS传感器数据外推SINS姿态、速度和位置变化；

(4)将编码器值和MINS导航数据融合为SINS系统可用的姿态信息；

(5)选取状态变量，构建传递对准系统的状态方程和量测方程；

(6)利用量测数据更新时间间隔检测数据丢包现象是否发生；

(7)使用改进的自适应滤波算法估计失准角、速度误差、陀螺漂移、加表漂移和安装偏差角等误差；

(8)将估计的失准角和速度误差对SINS导航信息输出校正；

(9)实验采集吊舱内部SINS传感器数据、MINS系统导航数据和吊舱编码器数据；

(10)循环步骤2到步骤8，完成光电吊舱实测数据传递对准。

本发明是针对现有光轴稳定机载光电吊舱传统速度匹配方法对准精度差，在初始大失准角状态下滤波器容易发散以及数据丢包导致对准精度差等问题，通过分析光轴稳定状态下主惯导与子惯导运动关系，提出融合编码器与飞机主惯导系统的姿态加速度匹配方法，并针对数据丢包现象对滤波算法做了改进。本发明是利用MATLAB软件仿真和在实际设备实验中，将本发明与传统速度匹配算法进行了比较。

实施例1：用MATLAB仿真对本发明进行验证：

仿真参数设置为：SINS陀螺仪的常值漂移和随机噪声分别为1°/h和0.1°/h，加速度计的常值漂移和随机噪声分别为1000ug和100ug，陀螺仪和加速度计的输出更新频率均为100Hz；MINS导航参数不设误差，导航信息频率为100Hz；编码器不设误差，输出频率为100Hz。载机的初始位置为北纬45°和东经126°，初始姿态航向角0°、俯仰角0°和横滚角0°，初始速度为0m/s；小失准角状态下SINS系统航向安装偏差角为1°，俯仰和横滚安装偏差角为0°；大失准角状态下SINS系统航向安装偏差角为20°，俯仰和横滚安装偏差角为0°。载机运动轨迹为：载体匀加速(10m/s²)直线运动5s→匀速直线运动20s→正向转动横滚角(3°/s)10s→反向转动横滚角(3°/s)20s→正向转动横滚角(3°/s)10s→匀减速(-8m/s²)直线运动5s→左转弯(9°/s)90°→匀加速(8m/s²)直线运动5s→匀速直线运动80s→匀减速(-8m/s²)直线运动5s→左转弯(9°/s)90°→匀加速(8m/s²)直线运动5s→匀速直线运动80s→匀减速(-8m/s²)直线运动5s→左转弯(9°/s)90°→匀加速(8m/s²)直线运动5s→匀速直线运动80s→匀减速(-10m/s²)直线运动5s→静止10s，总的运动时长为400s。图4为载体运动轨迹变化图，图5为载机航姿变化图，图6为吊舱航姿变化值图，图7为吊舱编码器值变化图。

图8为小失准角条件下传统速度匹配和提出的融合编码器值与主惯导姿态的姿态加速度匹配失准角对比图，图9为两种匹配方式的速度误差对比图。根据图8可以看出，就收敛速度来看，两种方法效果差异不大，10s以内失准角均会稳定收敛值附近，就收敛精度来看，姿态加速度方法失准角可以保持在1′以内，传统速度匹配方法的东向和北向失准角收敛至5′以内，天向失准角收敛在10′以内，明显姿态加速度匹配方法收敛精度更高。根据图9可以看出，两种方法对速度误差的估计效果类似，提出的方法在转弯时要优于速度匹配方法。图中两种算法均能够收敛，提出的方法收敛精度更高，更稳定。

图10为大失准角条件下传统速度匹配和提出方法的失准角对比图，图11为两种匹配方式的速度误差对比图。和小失准角情况下对比效果类似，就收敛速度来看，两种方法效果差异不大，10s以内失准角均会稳定收敛值附近，就收敛精度来看，姿态加速度方法失准角可以保持在2′以内，传统速度匹配方法的东向和北向失准角收敛至10′以内，天向失准角收敛在20′以内，姿态加速度匹配方法收敛精度更高。两种方法在失准角的收敛速度上类似，提出的方法失准角收敛精度更高，转弯时的速度误差更小。

实施例2：对本发明进行跑车实验验证：

为验证本发明的传递对准性能，跑车实验使用高精度惯导系统F110作为MINS辅助子惯导系统KVH1775对准，图12为跑车实验中汽车运动轨迹，图13为运动过程中MINS姿态变化，图14为运动过程中两轴编码器值变化，整个实验过程大约4200s。

图15为小失准角条件下两种方法解算航向对比图，由于实验过程中光电吊舱处于光轴稳定状态，本实验采用定点瞄目标(水塔)并记录航向来验证航向稳定精度，目标为10公里外的一处水塔，示意图如图3所示。实验中共采七次实验数据，为了使数据看起来更加直观，将相邻组数据用虚线连接。传统速度匹配方法的所测得航向在154.1度到154.4度之间，前三组数据看起来稳定一些，后四组数据稳定误差有些大，数据的峰峰值为0.3度。提出方法所测得航向大概稳定在154.4度，只有第六组数据相对其他组数据偏差大一些，七组数据的峰峰值差为0.1度。实测的结果与仿真的结果相似，提出方法的收敛精度更高，更稳定。

为了验证大失准角状态下本发明的有效性，实际实验验证时在实测数据初始航向上加了160°航向角偏差，以小失准角时解算导航信息作为参考信息。图16为实测大失准角条件下吊舱姿态误差，图17为实测大失准角条件下速度误差，算法在20s之后开始运行，光轴稳定状态下航向加入大失准角后传统速度匹配方法仿真结果航向发散，水平姿态和速度误差很大，导致对准失败；融合编码器值后的姿态加速度匹配方法能够航姿和速度均能够较好的收敛，航向在经过140s后收敛到2度以内，大约经过160s后收敛到1度以内。验证了在大失准角条件下提出方法的有效性，无论是收敛速度还是收敛精度都明显优于传统方法。

为了验证本发明提出的对数据丢包现象发生时的改进的滤波算法的有效性，对图12跑车实验数据的子惯导量测信息做了每隔30s丢1s数据的处理，图18左图为子惯导接受到有数据丢包和未数据丢包的速度对比图，右图为左图的局部放大图。图19为在图18数据丢包现象发生的条件下，利用改进的自适应滤波算法与传统的滤波算法仿真姿态误差角对比图，图20为速度误差角对比图。根据图19和图20可以看出，有数据丢包现象发生时，传统滤波算法在数据丢包现象发生时姿态误差与速度误差会有尖峰，尖峰大小与载体运动的加速度、角速度有关，改进的自适应滤波算法对相对于传统算法姿态误差和速度误差的更稳定。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种机载光电吊舱光轴稳定状态传递对准方法，其特征在于，包括以下步骤：

步骤1：生成光轴稳定状态下光电吊舱与MINS仿真轨迹；

步骤2：SINS系统传递对准参数初始化；

步骤3：使用SINS传感器数据外推SINS姿态、速度和位置变化；

步骤4：将编码器值和MINS导航数据融合为SINS系统可用的姿态信息；

步骤5：选取状态变量，构建传递对准系统的状态方程和量测方程；

选择系统状态变量X：

其中，

为载体系姿态矩阵对应的失准角；δv＝[δv_E δv_N]为东向，北向速度误差；ε＝[ε_x ε_y ε_z]为陀螺常值漂移；

为加表常值漂移；μ＝[μ_x μ_y μ_z]为主惯导载体系与编码器零位之间安装偏差角；

传递对准系统的状态方程为：

其中，

为状态变量

的导数；

为状态变量δv的导数；

为状态变量ε的导数；

为状态变量

的导数；

为当地n系相对于i系的旋转角速率，

R_M和R_N分别为子午圈曲率半径和卯酉圈曲率半径，h为当地高度，L为纬度，fⁿ为子惯导在n系下的比力；

量测方程为：

其中，qq2phi(·)表示将方向余弦矩阵转换为姿态角，

表示子惯导方向余弦矩阵，

表示主惯导方向余弦矩阵，

表示由子惯导坐标系到编码器零位坐标系的方向余弦矩阵；

步骤6：利用量测数据更新时间间隔检测数据丢包现象是否发生；

步骤7：使用改进的自适应滤波算法估计失准角、速度误差、陀螺漂移、加表漂移和安装偏差角误差；

当主惯导系统所传递的量测信息丢失时，运用一步预测值来代替当前量测值，前一时刻均方误差阵代替当前时刻均方误差阵，直到接收到新的量测值后进行标准卡尔曼滤波；

若k时刻数据丢包发生，具体的改进的自适应滤波算法方案步骤如下：

步骤7.1：k时刻对下一步状态

的预测：

其中，X_k为状态向量，Φ_k/k-1为一步状态转移矩阵；

步骤7.2：计算一步预测均方误差矩阵P_k/k-1：

其中，Γ_k-1为系统状态噪声驱动矩阵；

步骤7.3：计算量测新息：

其中，H_k为量测矩阵；Z_k为系统的量测向量；

步骤7.4：Sage-Husa自适应估计量测噪声协方差矩阵：

其中，初值β₀＝1，b为渐消因子，b＝0.9～0.999；

步骤7.5：滤波增益更新：

其中，K_k是系统的滤波增益矩阵，P_k是系统的状态均方差矩阵；

步骤7.6：状态估计：

步骤7.7：状态估计均方差：

P_k＝(I-K_kH_k)P_k/k-1

步骤8：将估计的失准角和速度误差对SINS导航信息输出校正；

步骤9：实验采集吊舱内部SINS传感器数据、MINS系统导航数据和吊舱编码器数据；循环步骤2到步骤8，完成光电吊舱实测数据传递对准。

Images