一种SINS任意失准角无奇异快速传递对准方法与流程

本发明属于惯性导航技术领域，具体涉及一种sins任意失准角无奇异快速传递对准方法。

背景技术

由于现代战争对战术武器装备精确打击能力和快速反应能力的要求日益提高，许多舰载和机载武器装备都安装了捷联惯性导航系统，简称惯导系统。而由于惯导系统为战术级，传递对准成为其初始对准的主要方案，研究战术武器系统的快速高精度传递对准具有很高的军事应用价值。

传统大失准角传递对准方案主要包括两个方面：一是建立大失准角条件下的非线性误差模型；二是设计相应的非线性滤波估计算法。根据姿态描述方式不同，可以得到不同的非线性误差模型。如基于四元数非线性误差模型、基于旋转矢量非线性误差模型、基于rodrigues参数非线性误差模型，其中，四元数非线性误差模型无奇异点，使用最为广泛，但在设计滤波算法时需要考虑其模值约束的影响。欧拉角姿态描述法存在奇异点，因此不适用于任意姿态对准，且基于欧拉角的非线性误差模型中含有状态量的正余弦函数，使得误差模型非线性增大。基于rodrigues参数的误差模型也存在奇异点，模型非线性度也较大。

另一方面，在非线性滤波算法的选择上，常规ekf滤波算法需要求导计算jacobian矩阵，且在处理严重非线性问题时，可能出现滤波误差增大甚至发散的现象。因此，一类基于sigma点的非线性滤波算法成为研究的热点，如ukf滤波、改进强跟踪ukf滤波、粒子滤波、gauss-hermite滤波，以及容积卡尔曼滤波(ckf)等。基于sigma点的非线性滤波算法，用确定性或随机采样策略逼近非线性函数的概率分布，无需对非线性模型求导，且可以通过优化采样策略，减小计算量并提高非线性函数概率分布的近似精度。总体来讲，基于sigma点的非线性滤波算法可以做到计算量与ekf滤波相当，但是存在计算量较大，且在大失准角下估计精度不高等问题。

技术实现要素：

为了克服上述现有技术存在的不足，本发明提供了一种sins任意失准角无奇异快速传递对准方法。

为了实现上述目的，本发明提供如下技术方案：

一种sins任意失准角无奇异快速传递对准方法，包括以下步骤：

步骤1、利用主、子惯导系统陀螺仪输出分别跟踪主、子惯导相对惯性空间的姿态变化结合主惯导的姿态矩阵和主、子惯导之间的安装矩阵建立子惯导系统姿态矩阵

子惯导系统姿态矩阵的链式分解为：

其中，n系为实时导航坐标系，即载体时变位置东北天地理坐标系；m为主惯导载体坐标系；im为主惯导载体惯性系，与传递对准开始时刻的坐标系m重合；s为子惯导载体坐标系；is为子惯导载体惯性系，与传递对准开始时刻的坐标系s重合；

式(1)中分别为主、子惯导系统相对惯性空间的姿态变化，可分别利用惯导陀螺仪输出进行跟踪，是由主惯导提供的实时姿态矩阵，所以，惯性系动基座传递对准过程实际上是间接对常值姿态阵的估计；

步骤2、主、子惯导惯性系下的姿态更新

由步骤1中对惯性坐标系im和is的定义可知，对准过程中姿态矩阵可分别利用主、子惯导的陀螺仪输出进行跟踪，忽略主惯导误差时，即有

考虑子惯导陀螺随机常值误差的影响，姿态跟踪结果记为则有

式中分别为主、子惯导陀螺仪的实时输出；

的跟踪误差仅由子惯导陀螺仪器件误差引起，短时间对准过程中，该误差可以视为小量，记为则

式中，ε^s为子惯导陀螺仪随机常值零偏；为子惯导陀螺仪测量噪声；

步骤3、构造量测量

已知，主、子惯导系统敏感的比力和角速度在地心惯性系内的投影关系分别为：

式中，为主惯导敏感的比力经杆臂效应补偿后的物理量；为主子惯导相对角速度；

对主惯导系统在m系内的比力量测值进行杆臂补偿，记为则

式中为杆臂效应残差项；

然后，利用式(5)实时跟踪的姿态将补偿后的比力量测值投影到im系，即

对于子惯导同理可得

式中为子惯导加速度计常值零偏；为子惯导加速度计测量噪声；

将式(12)展开整理，略去高阶小量，可得

由式(8)(10)(12)可得

对主、子惯导系统测量的角速度信息进行类似处理，可得

式中

式(14)、(15)等号两端包含了相关误差，若对其进行积分操作，则能够有效平滑相关噪声、杆臂残差以及弹性变形项，提高信噪比；

对等式(14)左端取积分，并记

对等式(14)右端括号内取积分，并记

由式(14)、(17)、(18)可得

对式(15)进行类似操作，同理可得

其中

式中，可视为由弹性变形引起的量测噪声；

值得注意的是，在推导式(19)和式(20)的过程中，并未对主、子惯导系统之间的相对姿态提出任何假设；也即，无论主、子惯导系统之间相对姿态为小姿态角还是大姿态角，惯性系内得到的匹配方程具有一致性，基于此，可以在惯性系内，设计出无需区分相对姿态大小的统一传递对准模型，此外由于模型基于姿态矩阵表示，也避免了奇异点问题。下面将以比力积分加角速度积分匹配为基础，来设计基于姿态矩阵任意失准角下的无奇异快速传递对准算法；

忽略弹性变形，对准期间为常值矩阵，即满足如下微分方程：

考虑陀螺漂移和加速度计零偏为随机常值，即

其中，wgk，wak均为零均值白噪声序列。

若惯性系动基座传递对准选取如下状态

则由式(7)、(17)、(21)、(23)、(24)可构成如下矩阵离散型状态方程

其中系统转移矩阵为

其中，e^ij为8×8矩阵，且第ij个元素为1，其余为0；

由式(19)、(20)可构成量测方程，即

其中，vk+1为量测噪声矩阵，且

步骤4、矩阵卡尔曼滤波估计算法

步骤4.1、滤波时间更新算法

式(26)、(28)描述的系统方程和量测方程均为线性，而状态为矩阵形式，因此要利用标准卡尔曼滤波基本方程，首先要将式(26)、(28)矩阵方程向量化，为此定义如下向量化算子：

x＝vec(x)(30)

其中，

若定义为kronecker积，则

假设，在tk时刻，状态xk和估计均方误差阵pk已知，对式(24)进行向量化操作，并利用式(29)特性，可得

即等价于标准卡尔曼滤波离散状态方程

xk+1＝ψkxk+wk(33)

则状态一步预测

利用算子vec的线性特性和式(31)，则上式可等价于

一步预测均方误差

其中，qk＝cov{wk}；

式(33)、式(34)即为矩阵滤波的时间更新；

步骤4.2、滤波量测更新算法

根据(28)描述的量测方程，推导量测更新算法；

记量测一步预测

残差

对式(38)进行向量化操作，即

则由标准卡尔曼滤波可得

sk+1＝γk+1pk+1/k(γk+1)^t+rk+1(40)

kk+1＝pk+1/k(γk+1)^t(sk+1)^-1(41)

其中，

将式(42)恢复到矩阵形式

其中由式(41)得到，即

式(43)则为矩阵状态在时刻tk+1的更新方程；

由标准卡尔曼滤波理论，矩阵估计均方误差为

式(38)、(40)、(41)、(43)和(45)即为不考虑约束条件下，所设计的矩阵滤波量测更新；

步骤4中基于矩阵滤波实时估计，选取状态

则以姿态矩阵为姿态描述的线性状态方程为：

式中为对准初始时刻主子惯导之间固定的安装矩阵；为子惯导姿态更新误差；为子惯导惯性系内比力积分误差；为子惯导惯性系内角速度积分误差；ε^s为子惯导陀螺仪随机常值零偏；为子惯导加速度计常值零偏；为子惯导陀螺仪测量噪声；为子惯导加速度计测量噪声；

以比力积分+角速度积分构成的线性量测方程为：

其中

式中，分别为第k时刻子惯导比力积分和角速度积分；分别为第k时刻主惯导比力积分和角速度积分；为量测矩阵；vk为量测噪声矩阵；

步骤5、子惯导初值姿态矩阵求解

在传递对准结束时刻，利用步骤2中主、子惯导惯性系下的姿态更新姿态矩阵以及步骤3、4估计得到的常值矩阵根据步骤1中的式(1)求解子惯导姿态矩阵即实现动基座下的传递对准。

本发明提供的sins任意失准角无奇异快速传递对准方法对于战术级捷联惯导系统传递对准问题，给出了一种基于姿态矩阵模型和矩阵形式卡尔曼滤波的对准算法，通过姿态矩阵对姿态的描述，系统方程和量测方程均为线性方程，避免了传统大失准角下的非线性模型和非线性滤波问题，大大降低了计算量，提高了工程实用性。仿真分析结果表明，该方法在摇摆条件下，能够快速收敛，对准精度高，实现了任意失准角下的无奇异快速传递对准。该方法利用姿态矩阵描述姿态关系，不存在奇异点和模型非线性问题，无需对失准角做任何假设约束，采用线性矩阵卡尔曼滤波，在保证对准精度和对准时间的同时，避免了繁琐非线性滤波，降低了计算量，提高了工程实用性。

附图说明

图1为本发明实施例1的sins任意失准角无奇异快速传递对准方法的流程图；

图2为本发明实施例1提出的mkf方法与传统方法的估计误差均方根(小角度)对比图；

图3为本发明实施例1提出的mkf方法与传统方法的10s内估计均值(小角度)对比图；

图4为本发明实施例1提出的mkf方法与传统方法的估计误差均方根(大角度)对比图；

图5为本发明实施例1提出的mkf方法与传统方法的10s内估计均值(大角度)对比图。

具体实施方式

下面结合附图，对本发明的具体实施方式作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

实施例1

本发明提供了一种sins任意失准角无奇异快速传递对准方法，具体如图1所示，包括以下步骤：

步骤1、利用主、子惯导系统陀螺仪输出分别跟踪主、子惯导相对惯性空间的姿态变化结合主惯导的姿态矩阵和主、子惯导之间的安装矩阵建立子惯导系统姿态矩阵

子惯导系统姿态矩阵的链式分解为：

其中，n系为实时导航坐标系，即载体时变位置东北天地理坐标系；m为主惯导载体坐标系；im为主惯导载体惯性系，与传递对准开始时刻的坐标系m重合；s为子惯导载体坐标系；is为子惯导载体惯性系，与传递对准开始时刻的坐标系s重合；

式(1)中分别为主、子惯导系统相对惯性空间的姿态变化，可分别利用惯导陀螺仪输出进行跟踪，是由主惯导提供的实时姿态矩阵，所以，惯性系动基座传递对准过程实际上是间接对常值姿态阵的估计；

步骤2、主、子惯导惯性系下的姿态更新

由步骤1中对惯性坐标系im和is的定义可知，对准过程中姿态矩阵可分别利用主、子惯导的陀螺仪输出进行跟踪，忽略主惯导误差时，即有

考虑子惯导陀螺随机常值误差的影响，姿态跟踪结果记为则有

式中分别为主、子惯导陀螺仪的实时输出；

的跟踪误差仅由子惯导陀螺仪器件误差引起，短时间对准过程中，该误差可以视为小量，记为则

式中，ε^s为子惯导陀螺仪随机常值零偏；为子惯导陀螺仪测量噪声；

步骤3、构造量测量

已知，主、子惯导系统敏感的比力和角速度在地心惯性系内的投影关系分别为：

式中，为主惯导敏感的比力经杆臂效应补偿后的物理量；为主子惯导相对角速度；

对主惯导系统在m系内的比力量测值进行杆臂补偿，记为则

式中为杆臂效应残差项；

然后，利用式(5)实时跟踪的姿态将补偿后的比力量测值投影到im系，即

对于子惯导同理可得

式中为子惯导加速度计常值零偏；为子惯导加速度计测量噪声；

将式(12)展开整理，略去高阶小量，可得

由式(8)(10)(12)可得

对主、子惯导系统测量的角速度信息进行类似处理，可得

式中

式(14)、(15)等号两端包含了相关误差，若对其进行积分操作，则能够有效平滑相关噪声、杆臂残差以及弹性变形项，提高信噪比；

对等式(14)左端取积分，并记

对等式(14)右端括号内取积分，并记

由式(14)、(17)、(18)可得

对式(15)进行类似操作，同理可得

其中

式中，可视为由弹性变形引起的量测噪声；

值得注意的是，在推导式(19)和式(20)的过程中，并未对主、子惯导系统之间的相对姿态提出任何假设；也即，无论主、子惯导系统之间相对姿态为小姿态角还是大姿态角，惯性系内得到的匹配方程具有一致性，基于此，可以在惯性系内，设计出无需区分相对姿态大小的统一传递对准模型，此外由于模型基于姿态矩阵表示，也避免了奇异点问题。下面将以比力积分加角速度积分匹配为基础，来设计基于姿态矩阵任意失准角下的无奇异快速传递对准算法；

忽略弹性变形，对准期间为常值矩阵，即满足如下微分方程：

考虑陀螺漂移和加速度计零偏为随机常值，即

其中，wgk，wak均为零均值白噪声序列。

若惯性系动基座传递对准选取如下状态

则由式(7)、(17)、(21)、(23)、(24)可构成如下矩阵离散型状态方程

其中系统转移矩阵为

其中，e^ij为8×8矩阵，且第ij个元素为1，其余为0；

由式(19)、(20)可构成量测方程，即

其中，vk+1为量测噪声矩阵，且

步骤4、矩阵卡尔曼滤波估计算法

步骤4.1、滤波时间更新算法

式(26)、(28)描述的系统方程和量测方程均为线性，而状态为矩阵形式，因此要利用标准卡尔曼滤波基本方程，首先要将式(26)、(28)矩阵方程向量化，为此定义如下向量化算子：

x＝vec(x)(30)

其中，

若定义为kronecker积，则

假设，在tk时刻，状态xk和估计均方误差阵pk已知，对式(24)进行向量化操作，并利用式(29)特性，可得

即等价于标准卡尔曼滤波离散状态方程

xk+1＝ψkxk+wk(33)

则状态一步预测

利用算子vec的线性特性和式(31)，则上式可等价于

一步预测均方误差

其中，qk＝cov{wk}；

式(33)、式(34)即为矩阵滤波的时间更新；

步骤4.2、滤波量测更新算法

根据(28)描述的量测方程，推导量测更新算法；

记量测一步预测

残差

对式(38)进行向量化操作，即

则由标准卡尔曼滤波可得

sk+1＝γk+1pk+1/k(γk+1)^t+rk+1(40)

kk+1＝pk+1/k(γk+1)^t(sk+1)^-1(41)

其中，

将式(42)恢复到矩阵形式

其中由式(41)得到，即

式(43)则为矩阵状态在时刻tk+1的更新方程；

由标准卡尔曼滤波理论，矩阵估计均方误差为

式(38)、(40)、(41)、(43)和(45)即为不考虑约束条件下，所设计的矩阵滤波量测更新；

步骤4中基于矩阵滤波实时估计，选取状态

则以姿态矩阵为姿态描述的线性状态方程为：

式中为对准初始时刻主子惯导之间固定的安装矩阵；为子惯导姿态更新误差；为子惯导惯性系内比力积分误差；为子惯导惯性系内角速度积分误差；ε^s为子惯导陀螺仪随机常值零偏；为子惯导加速度计常值零偏；为子惯导陀螺仪测量噪声；为子惯导加速度计测量噪声；

以比力积分+角速度积分构成的线性量测方程为：

其中

式中，分别为第k时刻子惯导比力积分和角速度积分；分别为第k时刻主惯导比力积分和角速度积分；为量测矩阵；vk为量测噪声矩阵；

步骤5、子惯导初值姿态矩阵求解

在传递对准结束时刻，利用步骤2中主、子惯导惯性系下的姿态更新姿态矩阵以及步骤3、4估计得到的常值矩阵根据步骤1中的式(1)求解子惯导姿态矩阵即实现动基座下的传递对准。

本实施例提供了一种基于姿态矩阵和矩阵卡尔曼滤波的传递对准算法，利用姿态阵分解，将子惯导姿态的估计问题转化为一个常值姿态的估计，借助经典姿态矩阵描述姿态，建立了在惯性坐标系矩阵形式的线性系统方程，量测方程也为姿态矩阵的线性模型。对于这种系统方程、量测方程均为线性模型的估计，借鉴传统线性向量卡尔曼滤波更新的思路，推导了一种基于线性矩阵卡尔曼滤波更新算法。可以认为新算法用线性滤波方式解决了大失准角非线性滤波对准问题，在计算量上远小于导航系内基于sigma点的各类非线性滤波对准算法。同时避免了经典rodrigues参数和姿态角描述姿态的奇异性。根据上述方案，下面通过仿真试验对本发明的具体实施方式和实施效果做进一步说明。

具体描述如下：

仿真条件：本发明实施例1中，以舰船在水中的摇摆运动为例，运动轨迹可看作由一系列幅值和频率相近的正弦波来描述为：

其中，θ、γ、分别为绕俯仰轴、横滚轴和方位轴的摇摆角度；θm、γm、分别为摇摆角度幅值；ωθ、ωγ、为摇摆的角频率；θ0、γ0、为摇摆的初始相位，各参数设置如表1所示。

表1摇摆参数设置

子惯导imu中陀螺漂移误差为5°/h、随机游走噪声为加速度计零位为1mg、随机噪声为惯导系统对准时间共计10s，采样周期为5ms。

失准角设置：小失准角[1°1°1°]和大失准角[60°60°150°]。对这两种姿态初值条件下的初始对准算法各进行50次的蒙特卡洛仿真，仿真结果如图2～图5所示。

图2～图5描述的是在两种失准角条件下，50次仿真得到的估计误差均方根和估计均值曲线，图中实线表示本实施例的方法得到的数据曲线，虚线表示传统方法得到的数据曲线。从图2～图5中可以得出两点结论：第一，提出的传递对准方法收敛速度很快，基本上在0.2s左右；第二，提出的传递对准方法对准精度很高，水平精度在0.02°以内，方位精度在0.03°以内。

图2给出了小失准角条件下50次仿真得到的估计误差均方根曲线。可以看出，在小失准角条件下，本实施例提出的对准方法和传统方法的估计误差均方根分别为[0.016°0.013°0.026°]^t和[0.031°0.027°0.037°]^t。图3给出了小失准角条件下50次仿真得到的均方曲线，在对准结束时刻，本实施例提出的对准方法和传统方法的均值分别为[1.001°0.996°0.994°]^t和[1.031°0.981°0.987°]^t。图4给出了大失准角条件下50次仿真得到的估计误差均方根曲线。可以看出，在大失准角条件下，本实施例提出的对准方法和传统方法的估计误差均方根分别为[0.0142°0.0184°0.0282°]^t和[0.0597°0.0349°0.0472°]^t。图5给出了大失准角条件下50次仿真得到的均方曲线，在对准结束时刻，本实施例提出的对准方法和传统方法的均值分别为[59.997°59.984°149.987°]^t和[60.057°59.966°149.954°]^t。

以上所述实施例仅为本发明较佳的具体实施方式，本发明的保护范围不限于此，任何熟悉本领域的技术人员在本发明披露的技术范围内，可显而易见地得到的技术方案的简单变化或等效替换，均属于本发明的保护范围。