CN114413895B - 光纤陀螺旋转惯导联合定位方法、装置、设备及介质 - Google Patents

光纤陀螺旋转惯导联合定位方法、装置、设备及介质 Download PDF

Info

Publication number
CN114413895B
CN114413895B CN202210176753.2A CN202210176753A CN114413895B CN 114413895 B CN114413895 B CN 114413895B CN 202210176753 A CN202210176753 A CN 202210176753A CN 114413895 B CN114413895 B CN 114413895B
Authority
CN
China
Prior art keywords
inertial navigation
navigation system
fiber
error
optic gyroscope
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210176753.2A
Other languages
English (en)
Other versions
CN114413895A (zh
Inventor
吴文启
崔加瑞
马铁锋
王茂松
冯国虎
潘献飞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202210176753.2A priority Critical patent/CN114413895B/zh
Publication of CN114413895A publication Critical patent/CN114413895A/zh
Application granted granted Critical
Publication of CN114413895B publication Critical patent/CN114413895B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

一种光纤陀螺旋转惯导联合定位方法、装置、设备及介质，将两套光纤陀螺旋转调制惯导系统的联合误差、陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差作为状态变换卡尔曼滤波器的状态量，构建状态变换卡尔曼滤波器的状态方程，将两套光纤陀螺旋转调制惯导系统的导航信息间的差值作为观测量，建立构建状态变换卡尔曼滤波器的观测方程，完成对两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计；利用所述估计结果来分别预测和校正两套光纤陀螺旋转调制惯导系统的定位误差，最后输出校正后的光纤陀螺旋转调制惯导系统的定位结果。本发明能够提高冗余配置光纤陀螺旋转惯导联合定位系统的联合定位精度。

Description

光纤陀螺旋转惯导联合定位方法、装置、设备及介质
技术领域
本发明属于多惯导信息融合技术领域，特别涉及一种光纤陀螺旋转惯导联合定位方法、装置、设备及介质。
背景技术
目前常采用冗余配置惯性导航系统的方式来提高导航系统的可靠性和定位精度。较为常见的配置方式为双惯导系统冗余配置，即主惯导系统和从惯导系统(备份惯导系统)，从惯导系统一般作为主惯导系统的热备份系统，当主惯导发生故障时切换到从惯导进行导航信息输出。现有的技术多利用冗余惯导信息进行故障诊断和隔离，利用冗余惯导系统信息提高定位精度的方法研究还不充分。
现有的公开号为CN107167134A的中国专利公开了冗余配置激光陀螺航海惯导协同定位方法，该方法针对的对象是冗余配置的单轴和双轴激光陀螺旋转调制惯导系统。其特点在于能够在线估计器件零偏并实现误差补偿，但是此专利仅针对激光陀螺。光纤陀螺比激光陀螺和成本低，可靠性好，是更具有潜力的光学陀螺，但是其标度因数误差稳定性较差。为此，本发明设计了基于冗余配置光纤陀螺旋转陀螺惯导的联合定位系统与算法，目的是实现对光纤陀螺标度因数误差的在线估计，提高冗余配置的光纤陀螺旋转调制系统的定位精度。
发明内容
针对冗余配置光纤陀螺旋转惯导的联合定位问题，本发明提出了光纤陀螺旋转惯导联合定位方法、装置、设备及介质。本发明旨在通过两套或多套惯导系统之间的姿态、速度、位置之差作为观测量，利用状态变换卡尔曼滤波器(或其他最优估计算法)进行信息融合进而估计出两套或多套光纤惯导系统的器件零偏和陀螺标度因数误差，进而进行输出校正以提高冗余配置惯导的导航系统的联合定位精度。
为实现上述技术目的，本发明提出的技术方案为：
一方面，本发明提供一种光纤陀螺旋转惯导联合定位方法、装置、设备及介质，包括：
接收冗余配置的两套光纤陀螺旋转调制惯导系统在初始对准结束后输出的导航信息；
将两套光纤陀螺旋转调制惯导系统的联合误差、陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差作为状态变换卡尔曼滤波器的状态量，构建状态变换卡尔曼滤波器的状态方程；
将两套光纤陀螺旋转调制惯导系统的导航信息间的差值作为观测量，构建状态变换卡尔曼滤波器的观测方程；
由状态变换卡尔曼滤波器完成对两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计；
利用两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计结果来分别预测两套光纤陀螺旋转调制惯导系统的定位误差，输出校正后的光纤陀螺旋转调制惯导系统的定位结果。
另一方面，本发明提供一种光纤陀螺旋转惯导联合定位装置，包括：
输入模块，用于接收冗余配置的两套光纤陀螺旋转调制惯导系统在初始对准结束后输出的导航信息，导航信息包括三维速度信息，位置信息和姿态信息；
状态变换卡尔曼滤波器构建模块，用于将两套光纤陀螺旋转调制惯导系统的联合误差、陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差作为状态变换卡尔曼滤波器的状态量，构建状态变换卡尔曼滤波器的状态方程；将两套光纤陀螺旋转调制惯导系统的导航信息间的差值作为观测量，构建状态变换卡尔曼滤波器的观测方程；
状态变化卡尔曼滤波模块，基于构建的状态变换卡尔曼滤波器完成对两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计；
定位误差预测与输出校正模块，用于利用两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计结果来分别预测和补偿两套光纤陀螺旋转调制惯导系统的定位误差，输出校正后的光纤陀螺旋转调制惯导系统的定位结果。
另一方面，本发明提供一种冗余配置光纤陀螺旋转惯导联合定位设备，包括两套或两套以上的光纤陀螺旋转调制惯导系统，任意两套光纤陀螺旋转调制惯导系统之间通过所述光纤陀螺旋转惯导联合定位方法实现联合定位。
另一方面，本发明提供一种计算机设备，包括存储器和处理器，存储器存储有计算机程序，处理器执行计算机程序时实现以下步骤：
接收冗余配置的两套光纤陀螺旋转调制惯导系统在初始对准结束后输出的导航信息；
将两套光纤陀螺旋转调制惯导系统的联合误差、陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差作为状态变换卡尔曼滤波器的状态量，构建状态变换卡尔曼滤波器的状态方程；
将两套光纤陀螺旋转调制惯导系统的导航信息间的差值作为观测量，构建状态变换卡尔曼滤波器的观测方程；
由状态变换卡尔曼滤波器完成对两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计；
利用两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计结果来分别预测两套光纤陀螺旋转调制惯导系统的定位误差，输出校正后的光纤陀螺旋转调制惯导系统的定位结果。
再一方面，本发明还提供一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：
接收冗余配置的两套光纤陀螺旋转调制惯导系统在初始对准结束后输出的导航信息；
将两套光纤陀螺旋转调制惯导系统的联合误差、陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差作为状态变换卡尔曼滤波器的状态量，构建状态变换卡尔曼滤波器的状态方程；
将两套光纤陀螺旋转调制惯导系统的导航信息间的差值作为观测量，构建状态变换卡尔曼滤波器的观测方程；
由状态变换卡尔曼滤波器完成对两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计；
利用两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计结果来分别预测两套光纤陀螺旋转调制惯导系统的定位误差，输出校正后的光纤陀螺旋转调制惯导系统的定位结果。
与现有技术相比，本发明的优点在于：
1)本发明有效利用冗余配置的光纤陀螺旋转惯性导航系统的联合误差作为状态量对两套惯导系统确定性误差状态进行估计，能够提高导航系统的定位精度。
2)本发明使用的校正方式为输出校正，无需对现有惯导系统的内部导航系统和算法架构进行改动即可实现对定位误差的预测，改装成本较小。
3)本发明中应用的状态变换卡尔曼滤波器使得导航系统模型中的比力项被引力相关项替代，克服了比力变化对状态估计的不利影响，同时减小了计算量。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
图1是本发明一实施例的流程图；
图2为本发明一实施例中第一惯导系统的陀螺常值零偏估计误差图；
图3为本发明一实施例中第一惯导系统的加速度计常值零偏估计误差图；
图4为本发明一实施例中第一惯导系统的陀螺仪标度因数误差图；
图5为本发明一实施例中第一惯导系统在其确定性误差补偿前后的定位精度对比图，其中(a)为北向误差补偿前后的定位精度对比图，(b)为东向误差补偿前后的定位精度对比图，(c)为整体定位误差补偿前后的定位精度对比图；
图6为本发明一实施例的结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚明白，下面将以附图及详细叙述来清楚说明本发明所揭示内容的精神，任何所属技术领域技术人员在了解本发明内容的实施例后，当可由本发明内容所教示的技术，加以改变及修饰，其并不脱离本发明内容的精神与范围。本发明的示意性实施例及其说明用于解释本发明，但并不作为对本发明的限定。
在一实施例中，参照图1，提供一种光纤陀螺旋转惯导联合定位方法，包括：
(S1)接收冗余配置的两套光纤陀螺旋转调制惯导系统在初始对准结束后输出的导航信息，导航信息包括三维速度信息，位置信息和姿态信息；
(S2)将两套光纤陀螺旋转调制惯导系统的联合误差、陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差作为状态变换卡尔曼滤波器的状态量，构建状态变换卡尔曼滤波器的状态方程；
(S3)将两套光纤陀螺旋转调制惯导系统的导航信息间的差值作为观测量，构建状态变换卡尔曼滤波器的观测方程；
(S4)由状态变换卡尔曼滤波器完成对两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计；
(S5)由利用两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计结果来分别预测两套光纤陀螺旋转调制惯导系统的定位误差，输出校正后的光纤陀螺旋转调制惯导系统的定位结果。
在步骤(S1)中，所述导航信息包括：横滚角、俯仰角、航向角、经度、纬度、高度以及当地导航系下的三维速度信息。
在步骤(S2)中，将两套光纤陀螺旋转调制惯导系统分别定义为第一惯导系统和第二惯导系统。姿态联合误差为第一惯导系统姿态误差/>与第二惯导系统姿态误差/>的差值。状态变换速度联合误差/>为第一惯导系统状态变换速度误差/>与第二惯导系统状态变换速度误差/>的差值。位置联合误差包括第一惯导系统纬度误差δL1与第二惯导系统纬度误差δL2的差值δL12、第一惯导系统经度误差δλ1与第二惯导系统经度误差δλ2的差值δλ12以及第一惯导系统海拔高度误差δh1与第二惯导系统海拔高度误差δh2的差值δh12。
在一实施例中，当地导航系下，定义的姿态联合误差、状态变换速度联合误差、位置联合误差的微分方程分别表示为：
其中：
δL12＝δL1-δL2,δλ12＝δλ1-δλ2,δh12＝δh1-δh2
两套光纤陀螺旋转调制惯导系统陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差的微分方程分别为：
分别表示第一惯导系统与第二惯导系统的方向余弦矩阵；
gn表示当地地理重力加速度矢量；
分别表示地球自转角速度矢量和转移角速度矢量；
分别表示第一惯导系统与第二惯导系统的联合速度误差北向，东向和地向分量；
L、h分别表示当地地理纬度和高度；
vN,vE,vD分别表示第一惯导系统或第二惯导系统输出的当地导航系下的东向速度信息，通常使用精度较高的惯导的速度信息即可；
分别表示第一惯导系统与第二惯导系统在导航系下的速度误差；分别表示第一惯导系统与第二惯导系统的纬度误差差值δL1,δL2引起的地球自转角速度误差；/>表示第一惯导系统与第二惯导系统的纬度误差差值δL12引起的地球自转角速度误差；
分别表示第一惯导系统与第二惯导系统的纬度误差差值δL1,δL2及状态变换速度误差差值/>的相关的转移角速度误差；/>表示第一惯导系统与第二惯导系统的纬度误差差值δL12及速度误差差值/>相关的转移角速度误差；
表示第一惯导系统的三轴陀螺输出；/>表示第一惯导系统的陀螺输出误差，建模为第一惯导系统的标度因数误差/>陀螺常值零偏/>和白噪声/>
表示第二惯导系统的三轴陀螺输出；/>表示第二惯导系统的陀螺输出误差，建模为第二惯导系统的标度因数误差/>陀螺常值零偏/>和白噪声/>
表示第一惯导系统的三轴加速度计输出；/>表示第一惯导系统的加速度计输出误差，建模为第一惯导系统的加速度计常值零偏/>和白噪声
表示第二惯导系统的三轴加速度计输出；/>表示第二惯导系统的加速度计输出误差，建模为第二惯导系统的加速度计常值零偏/>和白噪声/>
表示第一惯导系统的陀螺常值零偏；
表示第二惯导系统的陀螺常值零偏；
表示第一惯导系统的加速度计常值零偏；
表示第二惯导系统的加速度计常值零偏；
表示第一惯导系统的陀螺仪标度因数误差；
表示第二惯导系统的陀螺仪标度因数误差；
表示第一惯导系统或第二惯导系统输出的当地导航系下的速度信息，通常使用精度较高的惯导的速度信息即可；
RN表示地球子午圈主曲率半径；
RE表示地球卯酉圈主曲率半径；
Ω表示地球自转角速率模值。
×代表矢量叉乘运算，同时也可以表示将矢量转化为斜对称矩阵。
在一些实施例中，光纤陀螺旋转调制惯导系统器件零偏也可建模成一阶马尔科夫过程等随机过程，本领域技术人员可以根据情况自由构建。
本发明中构建状态变换卡尔曼滤波器，其中状态变换卡尔曼滤波器包括状态方程和观测方程。
在一实施例中，步骤(S2)中将两套光纤陀螺旋转调制惯导系统的联合误差、陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差作为状态变换卡尔曼滤波器的状态量，表示为：
构建状态变换卡尔曼滤波器的状态方程，如下：
其中，
在一实施例的步骤(S3)中，将两套光纤陀螺旋转调制惯导系统的导航信息间的差值作为观测量，建立构建状态变换卡尔曼滤波器的观测方程，构建状态变换卡尔曼滤波器的观测方程，表示如下：
z(t)＝H(t)x(t)+v(t)
其中：
z(t)＝[zφ zv zr]T
分别为第一惯导系统和第一惯导系统输出的航向角，/>分别是第一惯导系统和第一惯导系统输出的纬度；/>分别是第一惯导系统和第一惯导系统输出的经度；/>分别表示第一惯导系统与第二惯导系统输出的高度；/>分别是第一惯导系统和第一惯导系统输出的东向速度；/>分别是第一惯导系统和第一惯导系统输出的北向速度，/>分别是第一惯导系统和第一惯导系统输出的地向速度。
步骤(S4)，基于所构建的状态变换卡尔曼滤波器，完成对两套光纤陀螺旋转调制惯导系统陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计。
在一实施例中，步骤(S5)各光纤陀螺旋转调制惯导系统的定位误差预测方法包括：将光纤陀螺旋转调制惯导系统自身的姿态误差、速度误差和位置误差作为误差状态量，构建关于光纤陀螺旋转调制惯导系统陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差的误差状态动态模型，对误差状态动态模型离散化，得到光纤陀螺旋转调制惯导系统定位误差预测模型，将状态变换卡尔曼滤波器估计得到的光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差的估计结果代入光纤陀螺旋转调制惯导系统定位误差预测模型，得到光纤陀螺旋转调制惯导系统的定位误差预测值。
在一实施例的步骤(S5)中，以第一惯导系统为例，说明其定位误差预测方法:
第一惯导系统的误差状态可以表示为：
其中δr1分别表示第一惯导系统的姿态误差，联合速度误差和位置误差相应的，第一惯导系统其误差状态动态模型可以表示为：
其中，
离散化第一惯导系统其误差状态动态模型，得到第一惯导系统定位误差预测模型如下：
其中，
表示tk时刻的第一惯导系统定位误差预测值，其初值为/> 为状态变换卡尔曼滤波器估计得到的光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差的估计结果；Δt表示离散化步长。
采用与第一惯导系统其定位误差预测方法相同的方法，可以完成第二惯导系统其定位误差预测。
在一实施例的步骤(S5)中，通过输出校正补偿第一惯导系统和第二惯导系统的定位误差。具体地，将定位误差预测值从光纤陀螺旋转调制惯导系统输出的导航信息中扣除，即得到校正后的光纤陀螺旋转调制惯导系统的定位结果。本发明补偿光纤陀螺旋转调制惯导系统定位误差的方式为输出校正，校正过程表示如下：
其中上标“-”和“^”分别表示校正后的值和误差预测值。
通过以上方法，实现了冗余配置光纤陀螺旋转调制惯导系统的联合误差状态估计，能够估计出各光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差，通过输出校正补偿了定位误差后的第一惯导系统和第二惯导系统均能够实现更高的定位精度。
此方法有效利用了冗余配置的光纤陀螺旋转调制惯导系统之间的联合误差作为状态量，应用卡尔曼滤波器对各光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差进行了估计，本例中说明的冗余光纤陀螺旋转调制惯导为两套三轴旋转调制系统配置，除此之外还适用于包含一套光纤陀螺三轴旋转调制惯导与其他调制类型(包括捷联式、单轴旋转调制、双轴旋转调制)的光纤陀螺惯导的配置。
在一实施例中，提供一种光纤陀螺旋转惯导联合定位装置，其特征在于，包括：
输入模块，用于接收冗余配置的两套光纤陀螺旋转调制惯导系统在初始对准结束后输出的导航信息，导航信息包括三维速度信息，位置信息和姿态信息；
状态变换卡尔曼滤波器构建模块，用于将两套光纤陀螺旋转调制惯导系统的联合误差、陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差作为状态变换卡尔曼滤波器的状态量，构建状态变换卡尔曼滤波器的状态方程；将两套光纤陀螺旋转调制惯导系统的导航信息间的差值作为观测量，构建状态变换卡尔曼滤波器的观测方程；
状态变化卡尔曼滤波模块，基于构建的状态变换卡尔曼滤波器完成对两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计；
定位误差预测与校正输出模块，用于利用两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计结果来分别预测两套光纤陀螺旋转调制惯导系统的定位误差，输出校正后的光纤陀螺旋转调制惯导系统的定位结果。
上述各模块功能的实现方法，可以采用前述各实施例中相同的方法实现，在此不再赘述。
在一实施例中，提供一种冗余配置光纤陀螺旋转惯导联合定位设备，包括两套或两套以上的光纤陀螺旋转调制惯导系统，任意两套光纤陀螺旋转调制惯导系统之间通过上述任一实施例中所述的光纤陀螺旋转惯导联合定位方法实现联合定位。
在一典型应用实施例中，所述冗余配置光纤陀螺旋转惯导联合定位设备搭载在水下潜航器上。目前大型水下潜航器常采用冗余配置惯性导航系统的方式来提高水下自主导航系统的可靠性和定位精度。较为常见的配置方式为双惯导系统冗余配置，即主惯导系统和从惯导系统(备份惯导系统)，从惯导系统一般作为主惯导系统的热备份系统，当主惯导发生故障时切换到从惯导进行导航信息输出。现有的技术多利用冗余惯导信息进行故障诊断和隔离，利用冗余惯导系统信息提高定位精度的方法研究还不充分。将本发明提供的所述光纤陀螺旋转惯导联合定位方法应用于水下潜航器，能够解决冗余配置光纤陀螺旋转惯导的水下潜航器联合定位问题。
在本发明一实施例中，水下潜航器冗余配置的两套光纤陀螺三轴旋转调制惯导系统分别独立输出导航信息，包括三维速度信息，位置信息和姿态信息。构建状态变换状态卡尔曼滤波器，由状态变换卡尔曼滤波器完成对两套光纤陀螺三轴旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计。构建惯导系统定位误差预测模型，惯导系统定位误差预测模型使用估计出的确定性误差进而分别预测两套光纤陀螺三轴旋转调制惯导系统的定位误差并使用输出校正方式对各光纤陀螺三轴旋转调制惯导系统的导航信息输出进行校正，最后输出校正后的导航信息。其中，状态变换卡尔曼滤波器的构建方法、惯导系统定位误差预测模型的构建方法、输出校正方法等在前述实施例中有详尽的描述，在此不再赘述。
采用本发明提供的光纤陀螺旋转惯导联合定位方法，上述实施例的具体执行流程图，包括如下步骤：
步骤一，冗余配置光纤旋转惯导的初始对准。
水下潜航器处于系泊状态或位于舰艇船舱时进行初始对准或传递对准，装订包括初始姿态角，初始速度和初始位置信息，使得水下潜航器冗余配置的两套光纤陀螺三轴旋转调制惯导系统能够在初始对准结束后输出正确的导航信息。
步骤二，利用双惯导导航信息构建状态变换卡尔曼滤波器的状态方程。
采集两套光纤陀螺三轴旋转调制惯导系统的导航信息输出，包括两套光纤陀螺三轴旋转调制惯导系统的姿态信息，三维速度信息和三维位置信息。
将两套光纤陀螺旋转调制惯导系统的联合误差、陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差作为状态变换卡尔曼滤波器的状态量，构建状态变换卡尔曼滤波器的状态方程。所述联合误差包括姿态联合误差、速度联合误差和位置联合误差。
步骤三，将两套光纤陀螺旋转调制惯导系统的导航信息间的差值作为观测量，建立构建状态变换卡尔曼滤波器的观测方程。
步骤四，由状态变换卡尔曼滤波器的状态方程和观测方程，完成对两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计。其中，状态变换卡尔曼滤波更新周期选为1秒。
状态变换卡尔曼滤波器的滤波过程与标准卡尔曼滤波一致，属于本领域的公知常识，在此不再赘述。
步骤五，根据状态变换卡尔曼滤波器的估计结果预测光纤陀螺惯导定位误差，输出校正后的光纤陀螺旋转调制惯导系统的定位结果。
当状态变换卡尔曼滤波器稳定后，一般可以选择4-6小时，根据两套惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计结果来分别预测两套光纤陀螺旋转调制惯导系统的定位误差。
当状态变换卡尔曼滤波器稳定后，使用校正后的两套光纤陀螺三轴旋转调制惯导系统的定位结果作为导航输出系统输出。
为验证本发明所提供方法的有效性，以数值仿真条件下的光纤陀螺三轴旋转调制惯导系统数据来验证联合误差状态估计方法对于系统定位精度提高的性能。两套光纤陀螺三轴旋转调制惯导系统的导航信息频率为1Hz，分别定义为第一惯导系统和第二惯导系统。仅以第一惯导系统的结果为例，陀螺常值零偏估计误差如图2所示，加速度计常值零偏估计误差如图3所示，陀螺仪标度因数误差如图4所示。经过确定性误差补偿前后的第一惯导系统的结果如图5所示，图5中(a)为北向误差补偿前后的定位精度对比图，(b)为东向误差补偿前后的定位精度对比图，(c)为整体定位误差补偿前后的定位精度对比图，可以看出经过补偿后的定位误差从1下降到0.377(归一化后)，定位精度提高了62.3％。
本发明能够在水下潜航器无外界信息情况下，有效利用冗余配置的光纤陀螺旋转惯性导航系统的联合误差作为观测量对两套系统确定性误差状态进行估计，能够提高水下潜航器导航系统的定位精度。
在本实施例中，提供了一种计算机设备，该计算机设备可以是服务器，其内部结构图可以如图6所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口和数据库。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据库。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的数据库用于存储样本数据。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现上述实施例中光纤陀螺旋转惯导联合定位方法的步骤。
本领域技术人员可以理解，图6中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，提供了一种计算机设备，包括存储器和处理器，该存储器存储有计算机程序，该处理器执行计算机程序时实现上述实施例中光纤陀螺旋转惯导联合定位方法的步骤。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述实施例中光纤陀螺旋转惯导联合定位方法的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.光纤陀螺旋转惯导联合定位方法，其特征在于，包括：
接收冗余配置的两套光纤陀螺旋转调制惯导系统在初始对准结束后输出的导航信息；
将两套光纤陀螺旋转调制惯导系统的联合误差、陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差作为状态变换卡尔曼滤波器的状态量，构建状态变换卡尔曼滤波器的状态方程；
将两套光纤陀螺旋转调制惯导系统的导航信息间的差值作为观测量，构建状态变换卡尔曼滤波器的观测方程；
由状态变换卡尔曼滤波器完成对两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计；
利用两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计结果来分别预测两套光纤陀螺旋转调制惯导系统的定位误差，输出校正后的光纤陀螺旋转调制惯导系统的定位结果。
2.根据权利要求1所述的光纤陀螺旋转惯导联合定位方法，其特征在于，光纤陀螺旋转调制惯导系统的导航信息包括：横滚角、俯仰角、航向角、经度、纬度、海拔高度以及当地导航系下的三维速度信息。
3.根据权利要求1所述的光纤陀螺旋转惯导联合定位方法，其特征在于，两套光纤陀螺旋转调制惯导系统分别定义为第一惯导系统和第二惯导系统，姿态联合误差为第一惯导系统姿态误差/>与第二惯导系统姿态误差/>的差值；速度联合误差/>为第一惯导系统状态变换速度误差/>与第二惯导系统状态变换速度误差/>的差值；位置联合误差δr12包括第一惯导系统纬度误差δL1与第二惯导系统纬度误差δL2的差值δL12、第一惯导系统经度误差δλ1与第二惯导系统经度误差δλ2的差值δλ12以及第一惯导系统海拔高度误差δh1与第二惯导系统海拔高度误差δh2的差值δh12。
4.根据权利要求3所述的光纤陀螺旋转惯导联合定位方法，其特征在于，状态变换卡尔曼滤波器的状态量为：
其中：表示第一惯导系统的陀螺常值零偏；
表示第二惯导系统的陀螺常值零偏；
表示第一惯导系统的加速度计常值零偏；
表示第二惯导系统的加速度计常值零偏；
表示第一惯导系统的陀螺仪标度因数误差；
表示第二惯导系统的陀螺仪标度因数误差；构建状态变换卡尔曼滤波器的状态方程，如下：
其中，
其中：表示第一惯导系统的陀螺白噪声，/>表示第一惯导系统的加速度计白噪声，表示第二惯导系统的陀螺白噪声，/>表示第二惯导系统的加速度计白噪声； 分别表示地球自转角速度矢量和转移角速度矢量；/>表示第一惯导系统或第二惯导系统输出的当地导航系下的速度信息；gn表示当地地理重力加速度矢量；/>分别表示第一惯导系统与第二惯导系统的方向余弦矩阵；RN表示地球子午圈主曲率半径；RE表示地球卯酉圈主曲率半径；Ω表示地球自转角速率模值；L、h分别表示当地地理纬度和高度；vN,vE,vD分别表示第一惯导系统或第二惯导系统输出的当地导航系下的东向速度信息；表示第一惯导系统的陀螺输出；/>表示第二惯导系统的陀螺输出；/>表示第一惯导系统的加速度计输出；/>表示第二惯导系统的加速度计输出。
5.根据权利要求4所述的光纤陀螺旋转惯导联合定位方法，其特征在于，构建状态变换卡尔曼滤波器的观测方程，如下：
z(t)＝H(t)x(t)+v(t)
其中：
z(t)＝[zφ zv zr]T
分别为第一惯导系统和第一惯导系统输出的航向角，/>分别是第一惯导系统和第一惯导系统输出的纬度；/>分别是第一惯导系统和第一惯导系统输出的经度；分别表示第一惯导系统与第二惯导系统输出的高度；/>分别是第一惯导系统和第一惯导系统输出的东向速度；/>分别是第一惯导系统和第一惯导系统输出的北向速度，/>分别是第一惯导系统和第一惯导系统输出的地向速度。
6.根据权利要求3或4或5所述的光纤陀螺旋转惯导联合定位方法，其特征在于：将光纤陀螺旋转调制惯导系统自身的姿态误差、速度误差和位置误差作为误差状态量，构建关于光纤陀螺旋转调制惯导系统陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差的误差状态动态模型，对误差状态动态模型离散化，得到光纤陀螺旋转调制惯导系统定位误差预测模型，将状态变换卡尔曼滤波器估计得到的光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差的估计结果代入光纤陀螺旋转调制惯导系统定位误差预测模型，得到光纤陀螺旋转调制惯导系统的定位误差预测值。
7.根据权利要求6所述的光纤陀螺旋转惯导联合定位方法，其特征在于，将定位误差预测值从光纤陀螺旋转调制惯导系统输出的导航信息中扣除，即得到校正后的光纤陀螺旋转调制惯导系统的定位结果。
8.光纤陀螺旋转惯导联合定位装置，其特征在于，包括：
输入模块，用于接收冗余配置的两套光纤陀螺旋转调制惯导系统在初始对准结束后输出的导航信息，导航信息包括三维速度信息，位置信息和姿态信息；
状态变换卡尔曼滤波器构建模块，用于将两套光纤陀螺旋转调制惯导系统的联合误差、陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差作为状态变换卡尔曼滤波器的状态量，构建状态变换卡尔曼滤波器的状态方程；将两套光纤陀螺旋转调制惯导系统的导航信息间的差值作为观测量，构建状态变换卡尔曼滤波器的观测方程；
状态变化卡尔曼滤波模块，基于构建的状态变换卡尔曼滤波器完成对两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计；
定位误差预测与校正输出模块，用于利用两套光纤陀螺旋转调制惯导系统的陀螺常值零偏、加速度计常值零偏和陀螺仪标度因数误差估计结果来分别预测两套光纤陀螺旋转调制惯导系统的定位误差，输出校正后的光纤陀螺旋转调制惯导系统的定位结果。
9.冗余配置光纤陀螺旋转惯导联合定位设备，其特征在于，包括两套或两套以上的光纤陀螺旋转调制惯导系统，任意两套光纤陀螺旋转调制惯导系统之间通过如权利要求1所述的光纤陀螺旋转惯导联合定位方法实现联合定位。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于：处理器执行计算机程序时实现如权利要求1所述光纤陀螺旋转惯导联合定位方法的步骤。
CN202210176753.2A 2022-02-24 2022-02-24 光纤陀螺旋转惯导联合定位方法、装置、设备及介质 Active CN114413895B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210176753.2A CN114413895B (zh) 2022-02-24 2022-02-24 光纤陀螺旋转惯导联合定位方法、装置、设备及介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210176753.2A CN114413895B (zh) 2022-02-24 2022-02-24 光纤陀螺旋转惯导联合定位方法、装置、设备及介质

Publications (2)

Publication Number Publication Date
CN114413895A CN114413895A (zh) 2022-04-29
CN114413895B true CN114413895B (zh) 2023-10-13

Family

ID=81262145

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210176753.2A Active CN114413895B (zh) 2022-02-24 2022-02-24 光纤陀螺旋转惯导联合定位方法、装置、设备及介质

Country Status (1)

Country Link
CN (1) CN114413895B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115585826B (zh) * 2022-08-12 2023-03-07 中国人民解放军国防科技大学 多惯导旋转调制光纤陀螺标度因数误差自校正方法与装置
CN116067400B (zh) * 2023-03-07 2023-06-16 中国船舶集团有限公司第七〇七研究所 基于旋转调制测试的光纤陀螺性能分析方法及系统
CN116242397B (zh) * 2023-03-11 2024-01-30 中国人民解放军国防科技大学 一种速度误差修正模型下的双惯导协同标定方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105352527A (zh) * 2015-05-25 2016-02-24 哈尔滨工程大学 一种基于双轴转位机构光纤陀螺标定方法
CN107167134A (zh) * 2017-06-22 2017-09-15 中国人民解放军国防科学技术大学 冗余配置激光陀螺航海惯导协同定位方法
CN107830873A (zh) * 2017-11-01 2018-03-23 北京航空航天大学 一种基于单轴水平旋转惯导与里程计组合的高精度车辆定位定向方法
WO2020220729A1 (zh) * 2019-04-29 2020-11-05 南京航空航天大学 基于角加速度计/陀螺/加速度计的惯性导航解算方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105352527A (zh) * 2015-05-25 2016-02-24 哈尔滨工程大学 一种基于双轴转位机构光纤陀螺标定方法
CN107167134A (zh) * 2017-06-22 2017-09-15 中国人民解放军国防科学技术大学 冗余配置激光陀螺航海惯导协同定位方法
CN107830873A (zh) * 2017-11-01 2018-03-23 北京航空航天大学 一种基于单轴水平旋转惯导与里程计组合的高精度车辆定位定向方法
WO2020220729A1 (zh) * 2019-04-29 2020-11-05 南京航空航天大学 基于角加速度计/陀螺/加速度计的惯性导航解算方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Wang L et al..Navigation information fusion in a redundant marine rotational inertial navigation system configuration.The Journal of Navigation.2018,全文. *
袁保伦 ; 饶谷音 ; 廖丹 ; .光学陀螺旋转式惯导系统的安装误差效应分析.系统工程与电子技术.2010,(11),全文. *

Also Published As

Publication number Publication date
CN114413895A (zh) 2022-04-29

Similar Documents

Publication Publication Date Title
CN114413895B (zh) 光纤陀螺旋转惯导联合定位方法、装置、设备及介质
CN112629538B (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN110017837B (zh) 一种姿态抗磁干扰的组合导航方法
CN115585826B (zh) 多惯导旋转调制光纤陀螺标度因数误差自校正方法与装置
CN104181574A (zh) 一种捷联惯导系统/全球导航卫星系统组合导航滤波系统及方法
CN111982106A (zh) 导航方法、装置、存储介质及电子装置
CN113503894B (zh) 基于陀螺基准坐标系的惯导系统误差标定方法
CN109489661B (zh) 一种卫星初始入轨时陀螺组合常值漂移估计方法
CN116222551A (zh) 一种融合多种数据的水下导航方法及装置
CN114812545A (zh) 基于双激光多普勒测速仪和惯导系统组合导航方法及装置
CN111722295A (zh) 一种水下捷联式重力测量数据处理方法
CN112985391B (zh) 一种基于惯性和双目视觉的多无人机协同导航方法和装置
CN107764268B (zh) 一种机载分布式pos传递对准的方法和装置
Maliňák et al. Pure-inertial AHRS with adaptive elimination of non-gravitational vehicle acceleration
CN115574817B (zh) 一种基于三轴旋转式惯导系统的导航方法及导航系统
CN110375740B (zh) 车辆导航方法、装置、设备和存储介质
Al-Jlailaty et al. Efficient attitude estimators: A tutorial and survey
CN115856922A (zh) 一种松耦合陆地组合导航方法、装置、计算机设备和介质
CN111207734A (zh) 一种基于ekf的无人机组合导航方法
CN115900705A (zh) 一种紧耦合陆地组合导航方法、装置、计算机设备和介质
CN113932803B (zh) 适用于高动态飞行器的惯性/地磁/卫星组合导航系统
CN115950419A (zh) 超小型无人机用组合导航方法、装置及系统
Libero et al. A Unified Filter for Fusion of Multiple Inertial Measurement Units
CN115031731A (zh) 一种基于相对安装位置关系的多惯导协同导航方法及装置
Gul et al. GPS/SINS navigation data fusion using quaternion model and unscented Kalman filter

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant