CN116858286A - 一种基于特殊欧式群和地球坐标系的线性传递对准方法 - Google Patents

Abstract

本发明公开了一种基于特殊欧式群和地球坐标系的线性传递对准方法，包括：将主惯性导航系统的导航信息传递给子惯性导航系统作为初始导航信息，其中所述初始导航信息包括初始姿态、初始速度和初始位置；子惯性导航系统通过初始导航信息进行导航解算，并引入李群理论，构建误差向量，获取传递对准误差和传递对准误差状态模型；基于主惯性导航系统的导航信息构建误差观测模型，采用卡尔曼滤波器对所述传递对准误差进行误差估计，并根据误差定义对所述子惯性导航系统进行反馈校正，获得所述子惯性导航系统准确的姿态和速度，完成传递对准。本发明具有计算量小、无数学奇异点、具备全球导航能力等优点。

Description

一种基于特殊欧式群和地球坐标系的线性传递对准方法

技术领域

本发明涉及惯性导航技术领域，尤其涉及一种基于特殊欧式群和地球坐标系的线性传递对准方法。

背景技术

惯性导航系统(INS)是机载和舰载制导武器的核心导航设备，具有较强的隐蔽性和抗干扰能力等优点。INS的性能在很大程度上取决于初始对准的准确性，初始对准指INS获取初始姿态的过程。受成本和体积空间的限制，与机载或舰载高精度INS相比，制导武器装备的INS器件误差较大。现有高精度INS所采用的自初始对准方法不适用于低精度INS，否则会出现收敛速度慢和对准误差大等问题。为了实现低精度INS快速准确的初始对准，借助机载或舰载高精度主INS信息对低精度子INS进行校准的传递对准方法被提出。现有的线性传递对准算法只适用于主子惯导间安装偏差角为小角度的情况。而对于机载光电定位系统及沿舰尾布置的武器这类大安装偏差角场景，现有的线性传递对准算法无法被正常使用，四元数优化算法对随机噪声敏感，而非线性算法又会存在计算量大和数值计算不稳定等问题。

因此，本发明将研究适用于大安装偏差角环境的线性传递对准方法。考虑到以地球坐标系作为导航系的惯导更新算法具有计算量小、无数学奇异点、具备全球导航能力等优点，新的传递对准方法将以地球系作为导航坐标系。

发明内容

为解决上述技术问题，本发明提出了一种基于特殊欧式群和地球坐标系的线性传递对准方法，避免了优化算法可扩展性差、对噪声敏感以及非线性滤波算法计算量大的问题。

为实现上述目的，本发明提供了一种基于特殊欧式群和地球坐标系的线性传递对准方法，包括：

将主惯性导航系统的导航信息传递给子惯性导航系统作为初始导航信息，其中所述初始导航信息包括初始姿态、初始速度和初始位置；

所述子惯性导航系统通过所述初始导航信息进行导航解算，并引入李群理论，构建误差向量，获取传递对准误差和传递对准误差状态模型；

基于所述主惯性导航系统的导航信息构建误差观测模型，采用卡尔曼滤波器对所述传递对准误差进行误差估计，并根据误差定义对所述子惯性导航系统进行反馈校正，获得所述子惯性导航系统准确的姿态和速度，完成传递对准。

优选地，将所述主惯性导航系统的导航信息传递给子惯性导航系统作为初始导航信息，包括：

其中，为主INS姿态、/>为主INS速度、/>为主INS位置，/>为子INS姿态、/>为子INS速度、/>为子INS位置。

优选地，所述子惯性导航系统通过所述初始导航信息进行导航解算，包括：

在地球坐标系下，结合微分方程对所述子惯性导航系统的初始导航信息进行导航解算，所述子惯性导航系统的初始导航信息微分方程为：

其中，为子INS姿态变换矩阵的导数，/>为子INS姿态，/>为子INS三轴陀螺仪输出的角速度矢量，/>为地球自转角速度矢量，/>为子INS相对于惯性空间的速度矢量的导数，f^s为子INS三轴加速度计输出的加速度矢量，/>为子INS相对于惯性空间的速度矢量，为子INS的重力加速度矢量，/>为子INS在地球坐标系下的位置矢量的导数，/>为子INS在地球坐标系下的位置矢量。

优选地，获取所述传递对准误差状态模型，包括：

将子惯性导航系统的初始姿态和初始速度构建为特殊欧式群，根据群误差定义，以所述主惯性导航系统群状态作为参考群，得到群误差；

采用所述李群理论和李代数之间的对数映射关系，获得对数线性化后的量测失准角向量和速度误差，分别对所述量测失准角向量和所述速度误差求微积分，获取量测失准角微分方程和速度误差微分方程；

将所述速度误差、量测失准角、安装偏差角、陀螺仪常值零偏和加速度计常值漂移构建为状态向量，获得所述传递对准误差状态模型。

优选地，所述群误差为：

其中，η表示群误差，表示由子INS解算的导航参数所构成的特殊欧式群χ^m表示由主INS解算的导航参数所构成的特殊欧式群/> 表示子INS解算的姿态变换阵的转置，/>表示子INS解算的相对于惯性空间的速度矢量，/>表示主INS解算的姿态变换阵的转置，/>表示主INS解算的相对于惯性空间的速度矢量。

优选地，所述对数线性化后的量测失准角向量和速度误差分别为：

其中，I_3×3为3×3的单位矩阵，φ_m为量测失准角向量，表示子INS所解算姿态与主INS姿态之间的偏差，为主INS的速度与子INS解算的速度间的偏差。

优选地，所述传递对准误差状态模型为：

其中，X_SE表示子INS导航误差状态量，dv表示速度误差，φ_m表示子INS与主INS间的量测失准角，φ_a表示子INS与主INS间的安装偏差角，ε^s表示陀螺仪常值漂移，表示加速度常值零偏，/>表示误差状态的导数，F_SE表示状态转移矩阵，G_SE表示噪声分配矩阵，W表示过程噪声矢量。

优选地，构建所述误差观测模型，包括：

基于所述主惯性导航系统和所述子惯性导航系统的速度差构建观测向量，基于所述主惯性导航系统和所述子惯性导航系统的姿态，构建姿态误差向量，结合所述观测向量和所述姿态误差向量构建所述误差观测模型。

优选地，所述误差观测模型为：

其中，Z_SE表示观测矢量，z_SE,v表示主INS与子INS间的速度误差观测，z_SE,φ表示主INS与子INS间的姿态误差观测，H_SE表示观测矩阵，V为观测噪声，X_SE表示子INS导航误差状态量。

优选地，获得所述子惯性导航系统准确的姿态和速度，包括：

采用卡尔曼滤波器对所述传递对准误差进行误差估计，并根据误差定义获得姿态和速度的反馈校正模型，基于所述反馈校正模型对所述子惯性导航系统进行反馈校正，获得所述子惯性导航系统准确的姿态和速度；

其中，所述姿态和速度的反馈校正模型为：

其中，为子INS校正后的速度矢量，/>为子INS纯惯性解算的速度矢量，/>为子INS校正后的姿态阵，/>为子INS纯惯性解算的姿态阵，I_3×3为3×3的单位矩阵。

与现有技术相比，本发明具有如下优点和技术效果：

在高精度主INS的辅助下，相比于子INS的自初始对准，传递对准方法具有更快的对准速度和更高的对准精度。能够让子INS快速而准确获得初始姿态、速度和位置。

本发明所提出得基于特殊欧式群的传递对准方法，通过将姿态和速度定义在同一特殊欧式群空间，使得速度误差dv的定义考虑了姿态误差的影响，解决了传统误差模型中速度误差建模粗糙未考虑空间失准信息的问题，进而得到了完全线性的误差状态空间模型，状态空间模型中状态转移矩阵和观测矩阵不存在姿态、速度、位置等与导航参数有关的物理量，较大的初始导航误差不会对误差模型的准确性产生影响。因此相比于传统的线性传递对准方法，所提出的方法适合于在初始导航误差很大的条件下工作，对初始导航误差的容忍程度更强。

附图说明

构成本申请的一部分的附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1为本发明实施例的主子惯导关系示意图；

图2为本发明实施例的传递对准原理框图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

本发明提出了一种基于特殊欧式群和地球坐标系的线性传递对准方法，如图2，具体包括：

步骤1：将主INS的导航信息传递给子INS作为初始姿态、速度和位置；

为了方便接下来的介绍，相关坐标系的定义如下：

惯性坐标系被定义为i系，它相对于惯性空间保持静止。地球坐标系被定义为e系，它随着地球的旋转而旋转。“东-北-天”地理坐标系被定义为g系。主载体坐标系被定义为m系，子载体坐标系被定义为s系。如图1所示。

定义主INS陀螺仪输出为主INS加速度计输出为其中m表示主INS载体坐标系，i表示惯性坐标系。和/>分别表示X轴、Y轴和Z轴的陀螺仪角速度输出；/>和/>分别表示X轴、Y轴和Z轴的加速度计比力输出。同理可以得到子INS陀螺仪输出为子INS加速度计输出为/>主INS姿态为速度为/>位置为/>则子INS姿态为/>速度为/>位置为/>

初始时刻，将主INS的姿态、速度和位置传递给子INS。

步骤2：根据主INS提供的初始导航信息，子INS执行导航解算；

地球坐标系下，子INS的姿态、速度和位置微分方程如下：

其中，为子INS姿态变换矩阵的导数，/>为子INS姿态，/>为子INS三轴陀螺仪输出的角速度矢量，/>为地球自转角速度矢量，/>为子INS相对于惯性空间的速度矢量的导数，f^s为子INS三轴加速度计输出的加速度矢量，/>为子INS相对于惯性空间的速度矢量，为子INS的重力加速度矢量，/>为子INS在地球坐标系下的位置矢量的导数，/>为子INS在地球坐标系下的位置矢量。

根据步骤1得到的初始导航信息，结合微分方程，就能够对后续的载体姿态、速度以及位置进行连续积分解算。此处可采用常见的数值积分方法，例如四阶龙格-库塔法，故不再赘述。

步骤3：引入李群理论构建误差向量和状态方程；

首先，将姿态和速度构建为特殊欧式群：

根据群误差的定义，以主INS群状态作为参考群得到群误差如下：

η表示群误差，表示由子INS解算的导航参数所构成的特殊欧式群/>χ^m表示由主INS解算的导航参数所构成的特殊欧式群/> 表示子INS解算的姿态变换阵的转置，/>表示子INS解算的相对于惯性空间的速度矢量，/>表示主INS解算的姿态变换阵的转置，/>表示主INS解算的相对于惯性空间的速度矢量。由于主INS比子INS的精度高出多个数量级，因此认为主INS无误差，所有主INS解算的导航参数不带“～”符号。

采用李群和李代数之间的对数映射关系，得到对数线性化后的量测失准角向量和速度误差为：

I_3×3为3×3的单位矩阵，φ_m为量测失准角向量，表示子INS所解算姿态与主INS姿态之间的偏差，为主INS的速度与子INS解算的速度间的偏差。

针对公式(9)两边同时求微分得到：

其中，表示子INS陀螺仪误差。

对公式(10)两边同时求微分得到：

其中，表示子INS加速度计误差。

安装偏差角矩阵近似为：

将一阶近似后的式(9)和式(13)代入式(11)和式(12)，忽略高阶小量得到量测失准角微分方程和速度误差微分方程：

由于主子惯导间安装偏差角为固定的常值，因此

将陀螺仪误差和加速度计误差都建模为常值误差加随机噪声的形式：

将速度误差、量测失准角、安装偏差角、陀螺仪常值零偏和加速度计常值漂移构建为状态向量，得到传递对准误差状态方程：

X_SE表示子INS导航误差状态量，dv表示速度误差，φ_m表示子INS与主INS间的量测失准角，φ_a表示子INS与主INS间的安装偏差角，ε^s表示陀螺仪常值漂移，表示加速度常值零偏。/>表示误差状态的导数，F_SE表示状态转移矩阵，G_SE表示噪声分配矩阵，W表示过程噪声矢量。

步骤4：引入主INS信息构建误差观测方程；

首先，主INS和子INS的速度差被用作构建观测向量：

其次，主INS和子INS的姿态被用于构建姿态误差向量：

将(22)和(24)结合可以构造误差观测方程

Z_SE表示观测矢量，z_SE,v表示主INS与子INS间的速度误差观测，z_SE,φ表示主INS与子INS间的姿态误差观测，H_SE表示观测矩阵，V为观测噪声。

步骤5：采用卡尔曼滤波器对传递对准误差进行估计，并对子INS进行反馈校正；

利用标准线性卡尔曼滤波器和式(19)、式(25)构成的传递对准误差模型进行误差估计，得到

根据误差定义，可以得到姿态和速度的反馈校正公式如下：

为子INS校正后的速度矢量，/>为子INS纯惯性解算的速度矢量(未进行误差校正)，/>为子INS校正后的姿态阵，/>为子INS纯惯性解算的姿态阵(未进行误差校正)。

除传递对准开始时刻执行完成的步骤1到步骤5，其他时刻只需执行步骤2到步骤5即可获得子INS准确的姿态和速度，完成传递对准。

从式(20)和式(25)可以看出，状态转移矩阵和观测矩阵中没有与姿态、速度有关的导航量。因此当安装偏差较大，使得初始时刻不准确的导航量不会对误差模型的准确性造成影响时，误差模型仍然能精准的刻画误差传播特性。因此本发明所提出的线性传递对准算法适合于主INS和子INS间存在大安装偏差角条件。

在高精度主INS的辅助下，相比于子INS的自初始对准，传递对准方法具有更快的对准速度和更高的对准精度。能够让子INS快速而准确获得初始姿态、速度和位置。

本发明所提出得基于特殊欧式群的传递对准方法，通过将姿态和速度定义在同一特殊欧式群空间，使得速度误差dv的定义考虑了姿态误差的影响，解决了传统误差模型中速度误差建模粗糙未考虑空间失准信息的问题，进而得到了完全线性的误差状态空间模型，状态空间模型中状态转移矩阵和观测矩阵不存在姿态、速度、位置等与导航参数有关的物理量，较大的初始导航误差不会对误差模型的准确性产生影响。因此相比于传统的线性传递对准方法，所提出的方法适合于在初始导航误差很大的条件下工作，对初始导航误差的容忍程度更强。

以上，仅为本申请较佳的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应该以权利要求的保护范围为准。

Claims (10)

1.一种基于特殊欧式群和地球坐标系的线性传递对准方法，其特征在于，包括：

将主惯性导航系统的导航信息传递给子惯性导航系统作为初始导航信息，其中所述初始导航信息包括初始姿态、初始速度和初始位置；

所述子惯性导航系统通过所述初始导航信息进行导航解算，并引入李群理论，构建误差向量，获取传递对准误差和传递对准误差状态模型；

基于所述主惯性导航系统的导航信息构建误差观测模型，采用卡尔曼滤波器对所述传递对准误差进行误差估计，并根据误差定义对所述子惯性导航系统进行反馈校正，获得所述子惯性导航系统准确的姿态和速度，完成传递对准。

2.根据权利要求1所述的基于特殊欧式群和地球坐标系的线性传递对准方法，其特征在于，将所述主惯性导航系统的导航信息传递给子惯性导航系统作为初始导航信息，包括：

其中，为主INS姿态、/>为主INS速度、/>为主INS位置，/>为子INS姿态、/>为子INS速度、/>为子INS位置。

3.根据权利要求1所述的基于特殊欧式群和地球坐标系的线性传递对准方法，其特征在于，所述子惯性导航系统通过所述初始导航信息进行导航解算，包括：

在地球坐标系下，结合微分方程对所述子惯性导航系统的初始导航信息进行导航解算，所述子惯性导航系统的初始导航信息微分方程为：

其中，为子INS姿态变换矩阵的导数，/>为子INS姿态，/>为子INS三轴陀螺仪输出的角速度矢量，/>为地球自转角速度矢量，/>为子INS相对于惯性空间的速度矢量的导数，f^s为子INS三轴加速度计输出的加速度矢量，/>为子INS相对于惯性空间的速度矢量，/>为子INS的重力加速度矢量，/>为子INS在地球坐标系下的位置矢量的导数，/>为子INS在地球坐标系下的位置矢量。

4.根据权利要求3所述的基于特殊欧式群和地球坐标系的线性传递对准方法，其特征在于，获取所述传递对准误差状态模型，包括：

将子惯性导航系统的初始姿态和初始速度构建为特殊欧式群，根据群误差定义，以所述主惯性导航系统群状态作为参考群，得到群误差；

采用所述李群理论和李代数之间的对数映射关系，获得对数线性化后的量测失准角向量和速度误差，分别对所述量测失准角向量和所述速度误差求微积分，获取量测失准角微分方程和速度误差微分方程；

将所述速度误差、量测失准角、安装偏差角、陀螺仪常值零偏和加速度计常值漂移构建为状态向量，获得所述传递对准误差状态模型。

5.根据权利要求4所述的基于特殊欧式群和地球坐标系的线性传递对准方法，其特征在于，所述群误差为：

其中，η表示群误差，表示由子INS解算的导航参数所构成的特殊欧式群/>χ^m表示由主INS解算的导航参数所构成的特殊欧式群/> 表示子INS解算的姿态变换阵的转置，/>表示子INS解算的相对于惯性空间的速度矢量，/>表示主INS解算的姿态变换阵的转置，/>表示主INS解算的相对于惯性空间的速度矢量。

6.根据权利要求5所述的基于特殊欧式群和地球坐标系的线性传递对准方法，其特征在于，所述对数线性化后的量测失准角向量和速度误差分别为：

其中，I_3×3为3×3的单位矩阵，φ_m为量测失准角向量，表示子INS所解算姿态与主INS姿态之间的偏差，为主INS的速度与子INS解算的速度间的偏差。

7.根据权利要求6所述的基于特殊欧式群和地球坐标系的线性传递对准方法，其特征在于，所述传递对准误差状态模型为：

其中，X_SE表示子INS导航误差状态量，dv表示速度误差，φ_m表示子INS与主INS间的量测失准角，φ_a表示子INS与主INS间的安装偏差角，ε^s表示陀螺仪常值漂移，表示加速度常值零偏，/>表示误差状态的导数，F_SE表示状态转移矩阵，G_SE表示噪声分配矩阵，W表示过程噪声矢量。

8.根据权利要求1所述的基于特殊欧式群和地球坐标系的线性传递对准方法，其特征在于，构建所述误差观测模型，包括：

基于所述主惯性导航系统和所述子惯性导航系统的速度差构建观测向量，基于所述主惯性导航系统和所述子惯性导航系统的姿态，构建姿态误差向量，结合所述观测向量和所述姿态误差向量构建所述误差观测模型。

9.根据权利要求8所述的基于特殊欧式群和地球坐标系的线性传递对准方法，其特征在于，所述误差观测模型为：

其中，Z_SE表示观测矢量，z_SE,v表示主INS与子INS间的速度误差观测，z_SE,φ表示主INS与子INS间的姿态误差观测，H_SE表示观测矩阵，V为观测噪声，X_SE表示子INS导航误差状态量。

10.根据权利要求1所述的基于特殊欧式群和地球坐标系的线性传递对准方法，其特征在于，获得所述子惯性导航系统准确的姿态和速度，包括：

采用卡尔曼滤波器对所述传递对准误差进行误差估计，并根据误差定义获得姿态和速度的反馈校正模型，基于所述反馈校正模型对所述子惯性导航系统进行反馈校正，获得所述子惯性导航系统准确的姿态和速度；

其中，所述姿态和速度的反馈校正模型为：

其中，为子INS校正后的速度矢量，/>为子INS纯惯性解算的速度矢量，/>为子INS校正后的姿态阵，/>为子INS纯惯性解算的姿态阵，I_3×3为3×3的单位矩阵。