CN111964689A - 深海惯导方位快速校正的方法 - Google Patents

深海惯导方位快速校正的方法 Download PDF

Info

Publication number
CN111964689A
CN111964689A CN202010652167.1A CN202010652167A CN111964689A CN 111964689 A CN111964689 A CN 111964689A CN 202010652167 A CN202010652167 A CN 202010652167A CN 111964689 A CN111964689 A CN 111964689A
Authority
CN
China
Prior art keywords
fine alignment
resolver
inertial navigation
azimuth
misalignment angle
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.)
Pending
Application number
CN202010652167.1A
Other languages
English (en)
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.)
Hebei Hanguang Heavy Industry Ltd
Original Assignee
Hebei Hanguang Heavy Industry Ltd
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 Hebei Hanguang Heavy Industry Ltd filed Critical Hebei Hanguang Heavy Industry Ltd
Priority to CN202010652167.1A priority Critical patent/CN111964689A/zh
Publication of CN111964689A publication Critical patent/CN111964689A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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
    • G01C21/203—Specially adapted for sailing ships

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)
  • Gyroscopes (AREA)

Abstract

本发明公开了深海惯导方位快速校正的方法，属于惯性导航技术领域，能够针对深海惯导进行方位校正，且能够大幅缩短校正时间，同时较大提升校正精度。包括如下步骤：S1、载体上搭载捷联惯导系统和多普勒计程仪DVL；捷联惯导系统，包含并行的两个捷联积分解算器，分别为第一解算器I和第二解算器II。S2、构建水平精对准滤波器，以第一解算器I和多普勒计程仪DVL的输出，估计水平失准角；S3、将水平失准角反馈至第二解算器II，并控制载体开始做转弯机动；S4、构建方位精对准滤波器，以第二解算器II和多普勒计程仪DVL的输出，估计方位失准角；S5、将方位失准角反馈至第二解算器II，完成载体姿态校正，并且将校正后的载体姿态反馈给第一解算器I。

Description

深海惯导方位快速校正的方法
技术领域
本发明涉及惯性导航技术领域，具体涉及深海惯导方位快速校正的方法。
背景技术
惯性导航是以牛顿经典力学理论为基础，利用陀螺仪和加速度计测量载体角速度和线加速度来计算其姿态、速度和位置的一种推算方法。惯性导航系统不需借助外界信息，只依靠自身惯性传感器的测量值便可以完成导航定位任务，具有其它导航手段无法比拟的自主性和隐蔽性。作为一种不可或缺的导航手段，惯性导航在许多领域都得到了广泛应用，尤其是在军事领域。
由于是一种航位推算方法，惯性导航系统的精度会随着时间的增长而迅速下降，甚至失去导航能力。因此在一些需要长时间导航定位的场合，比如自主水下潜器，就需要定期对惯导系统进行校正。水下潜器一般只需要惯导系统提供姿态信息作为主控系统的输入信号，因此只需要保证惯导系统的姿态精度即可。
由于捷联惯导系统的方位可观性较弱，Kalman滤波校正过程中方位失准角收敛速度较慢。因此如何加速方位失准角收敛，以提高深海惯导系统的方位校正效率与校正精度是目前亟待解决的问题。
发明内容
有鉴于此，本发明提供了深海惯导方位快速校正的方法，能够针对深海惯导进行方位校正，且能够大幅缩短校正时间，同时较大提升校正精度。
为达到上述目的，本发明的技术方案为：深海惯导方位快速校正的方法，包括如下步骤：
S1、载体上搭载捷联惯导系统和多普勒计程仪DVL；捷联惯导系统，包含并行的两个捷联积分解算器，分别为第一解算器I和第二解算器II。
S2、构建水平精对准滤波器，以第一解算器I和多普勒计程仪DVL的输出，估计水平失准角。
S3、将水平失准角反馈至第二解算器II，并控制载体开始做转弯机动。
S4、构建方位精对准滤波器，以第二解算器II和多普勒计程仪DVL的输出，估计方位失准角。
S5、将方位失准角反馈至第二解算器II，完成载体姿态校正，并且将校正后的载体姿态反馈给第一解算器I。
进一步地，捷联积分解算器为捷联惯导系统的解算程序。
进一步地，构建水平精对准滤波器，以第一解算器I和多普勒计程仪DVL的输出，估计水平失准角，具体为：
S201、构建水平精对准滤波器，包括如下步骤：
S2011、选取姿态误差、速度误差和陀螺常值零偏作为水平精对准滤波器的状态量，即状态量为X＝[φE φN φU δVE δVN εx εy εz]。
其中φE为东向平台失准角，φN为北向平台失准角，φU为天向平台失准角，δVE为东向速度误差，δVN为北向速度误差，εx为捷联惯导坐标系x向的陀螺零偏，εy捷联惯导坐标系y向的陀螺零偏，εz捷联惯导坐标系z向的陀螺零偏；其中x、y、z分别为捷联惯导坐标系的x、y、z轴。
S2012，构建水平精对准滤波器的离散化状态方程为
Figure BDA0002575368460000021
其中
Figure BDA0002575368460000022
为水平精对准滤波器的k时刻状态量预测值；
Figure BDA0002575368460000023
为水平精对准滤波器的k-1时刻的状态量；A为离散化状态一步转移矩阵；Wk为系统噪声。
Figure BDA0002575368460000031
其中第一中间矩阵A1用于指代
Figure BDA0002575368460000032
即
Figure BDA0002575368460000033
Figure BDA0002575368460000034
为捷联惯导的姿态矩阵；ΔT为时间更新周期。
第二中间矩阵A2为
Figure BDA0002575368460000035
ΔVE、ΔVN和ΔVU分别是时间更新周期ΔT内捷联惯导的加速度计的输出即比力在东向、北向、天向三个方向上的积分。
I3×3和I2×2分别为3×3和2×2的单位阵；02×3、03×3、03×2分别为2×3、3×3以及3×2的零矩阵。
S2013、水平精对准滤波器的量测值为
Figure BDA0002575368460000036
量测矩阵为H＝[02×3 I2×2 03×3]。
其中Z1为水平精对准滤波器的量测值；H为量测矩阵；
Figure BDA0002575368460000037
和
Figure BDA0002575368460000038
分别为第一解算器Ι出的载体东向速度和北向速度，
Figure BDA0002575368460000039
和
Figure BDA00025753684600000310
分别为多普勒计程仪DVL输出的载体东向速度和北向速度。
S202、以第一解算器I和多普勒计程仪DVL的输出作为水平精对准滤波器的输入，水平精对准滤波器输出状态量的预测值，包含估计的水平失准角，水平失准角包括东向平台失准角φE和北向平台失准角φN。
进一步地，将水平失准角反馈至第二解算器II，具体为：
将估计的水平失准角，包括东向平台失准角φE和北向平台失准角φN，反馈给第二解算器II，得到第一次校正后的捷联惯导的姿态矩阵
Figure BDA00025753684600000311
Figure BDA00025753684600000312
其中
Figure BDA00025753684600000313
为第一次校正前的捷联惯导的姿态矩阵，即
Figure BDA00025753684600000314
进一步地，构建方位精对准滤波器，以第二解算II和多普勒计程仪DVL的输出，估计方位失准角；具体为：
S401、构建方位精对准滤波器的离散化线性系统方程：
Figure BDA0002575368460000041
其中
Figure BDA0002575368460000042
为方位精对准滤波器的k时刻状态量预测值；
Figure BDA0002575368460000043
为方位精对准滤波器的k-1时刻的状态量；
Figure BDA0002575368460000044
为方位精对准滤波器的k刻的状态量；Wk为系统噪声；
Figure BDA0002575368460000045
为方位精对准滤波器的量测方程中k时刻的量测值，H为量测矩阵；Vk为量测噪声。
S402、方位精对准滤波器的系统量测残差为
Figure BDA0002575368460000046
其中
Figure BDA0002575368460000047
为方位精对准滤波器的k时刻的状态量的估计值。
方位精对准滤波器的系统量测残差的方差为Sk/k-1＝E[ek(ek)T]＝HPk/k-1(H)T+Rk
上式中Rk为方位精对准滤波器的系统量测噪声协方差矩阵；Pk/k-1为方位精对准滤波器的预测误差协方差矩阵。
S403、对Pk/k-1进行分解：
Figure BDA0002575368460000048
其中
Figure BDA0002575368460000049
为分解得到的第一误差协方差；
Figure BDA00025753684600000410
为分解得到的第二误差协方差矩阵；α为滤波自适应矩阵。
根据分解得到的
Figure BDA00025753684600000411
和
Figure BDA00025753684600000412
将系统量测残差的方差Sk/k-1分解为：
Figure BDA00025753684600000413
其中
Figure BDA00025753684600000414
为对应
Figure BDA00025753684600000415
的方差分解项：
Figure BDA00025753684600000416
Figure BDA00025753684600000417
为对应
Figure BDA00025753684600000418
的方差分解项：
Figure BDA00025753684600000419
S404、取
Figure BDA00025753684600000420
式中diag(*)表示取矩阵*的对角元素；
Figure BDA00025753684600000421
为系统量测残差协方差的均值。
并根据α的取值计算优化后的Pk/k-1。
S405、利用优化后的Pk/k-1成形Kalman滤波算法，得到方位精对准滤波器；
S406、以第二解算器II和多普勒计程仪DVL的输出之差作为量测值输入至方位精对准滤波器，在载体做转弯机动时，方位精对准滤波器输出估计的方位失准角，即天向平台失准角φU。
进一步地，将方位失准角反馈至第二解算器II，完成载体姿态校正，并且将校正后的载体姿态反馈给第一解算器I，具体为：
将方位失准角反馈至第二解算器II，对载体的姿态矩阵进行校正，其中载体的姿态矩阵即为捷联惯导的姿态矩阵，校正前载体的姿态矩阵为第一次校正后的捷联惯导的姿态矩阵
Figure BDA0002575368460000051
校正后载体的姿态矩阵为
Figure BDA0002575368460000052
Figure BDA0002575368460000053
将校正后载体的姿态矩阵
Figure BDA0002575368460000054
反馈给第一解算器I即可。
有益效果：
本发明提供了深海惯导方位快速校正的方法，利用两个并行的捷联积分解算器，将惯导系统的水平回路和方位回路解耦，分别设计水平精对准滤波器和方位精对准滤波器，利用两个滤波器同时滤波。首先利用水平精对准滤波器，以解算器Ι和DVL的输出，快速估计水平失准角，待水平失准角收敛后，载体转弯，这时方位失准角会快速收敛、设计方位精对准滤波器，以解算器Π和DVL的输出，快速估计方位失准角。这样可以通过载体本身的机动来激励方位失准角以加速其收敛，从而达到了提高深海惯导系统的方位校正效率与校正精度的目的。
2、为了抑制转弯过程中水平失准角的估计值发生波动，本发明采用一种方差成形自适应Kalman滤波算法进行方位精对准滤波器的设计，结果表明，与传统Kalman滤波方法相比，该方法大幅缩短校正时间，同时较大提升校正精度。
附图说明
图1为本发明实施例提供的深海惯导方位快速校正的方法原理图；
图2为本发明实施例的俯仰角校正曲线仿真结果图；
图3为本发明实施例的横滚角校正曲线仿真结果图；
图4为本发明实施例的方位角校正曲线仿真结果图。
具体实施方式
下面结合附图并举实施例，对本发明进行详细描述。
本发明提供了深海惯导方位快速校正的方法，其原理如图1所示，包括如下步骤：
S1、载体上搭载捷联惯导系统和多普勒计程仪DVL；捷联惯导系统，包含并行的两个捷联积分解算器，分别为第一解算器I和第二解算器II。捷联积分解算器为捷联惯导系统的解算程序。
S2、构建水平精对准滤波器，以第一解算器I和多普勒计程仪DVL的输出，估计水平失准角。
本发明实施例中，S2包括如下具体步骤：
S201、构建水平精对准滤波器，包括如下步骤：
S2011、选取姿态误差、速度误差和陀螺常值零偏作为水平精对准滤波器的状态量，即状态量为X＝[φE φN φU δVE δVN εx εy εz]。
其中φE为东向平台失准角，φN为北向平台失准角，φU为天向平台失准角，δVE为东向速度误差，δVN为北向速度误差，εx为捷联惯导坐标系x向的陀螺零偏，εy捷联惯导坐标系y向的陀螺零偏，εz捷联惯导坐标系z向的陀螺零偏；其中x、y、z分别为捷联惯导坐标系的x、y、z轴。
S2012，构建水平精对准滤波器的离散化状态方程为
Figure BDA0002575368460000071
其中
Figure BDA0002575368460000072
为水平精对准滤波器的k时刻状态量预测值(经过一个滤波更新周期的k时刻状态量预测值)；
Figure BDA0002575368460000073
为水平精对准滤波器的k-1时刻的状态量；A为离散化状态一步转移矩阵；Wk为系统噪声；。
Figure BDA0002575368460000074
其中矩阵A1和A2均为设置的中间矩阵，其中第一中间矩阵A1用于指代
Figure BDA0002575368460000075
即
Figure BDA0002575368460000076
Figure BDA0002575368460000077
为捷联惯导的姿态矩阵(其中b指代惯导本身坐标系，n指代地理系)；ΔT为时间的滤波更新周期。
第二中间矩阵A2为
Figure BDA0002575368460000078
ΔVE、ΔVN和ΔVU分别是时间更新周期ΔT内捷联惯导的加速度计的输出即比力在东向、北向、天向三个方向上的积分。
I3×3和I2×2分别为3×3和2×2的单位阵；02×3、03×3、03×2分别为2×3、3×3以及3×2的零矩阵。
S2013、水平精对准滤波器的量测值为
Figure BDA0002575368460000079
量测矩阵为H＝[02×3 I2×2 03×3]；
其中Z1为水平精对准滤波器的量测值；H为量测矩阵；
Figure BDA00025753684600000710
和
Figure BDA00025753684600000711
分别为第一解算器Ι出的载体东向速度和北向速度，
Figure BDA00025753684600000712
和
Figure BDA00025753684600000713
分别为多普勒计程仪DVL输出的载体东向速度和北向速度；
S202、以第一解算器I和多普勒计程仪DVL的输出作为水平精对准滤波器的输入，水平精对准滤波器输出状态量的预测值，包含估计的水平失准角，水平失准角包括东向平台失准角φE和北向平台失准角φN。
S3、将水平失准角反馈至第二解算器II，并控制载体开始做转弯机动。具体为：
将估计的水平失准角，包括东向平台失准角φE和北向平台失准角φN，反馈给第二解算器II，得到第一次校正后的捷联惯导的姿态矩阵
Figure BDA0002575368460000081
Figure BDA0002575368460000082
其中
Figure BDA0002575368460000083
为第一次校正前的捷联惯导的姿态矩阵，即
Figure BDA0002575368460000084
载体的转弯机动过程中，方位失准角会快速收敛。因此此时载体做转弯机动是为了后续步骤能够更快更精确地获取方位失准角。
S4、构建方位精对准滤波器，以第二解算器II和多普勒计程仪DVL的输出，估计方位失准角。
本发明实施例中，方位精对准滤波器系统方程、量测方程与步骤二中设计的水平精对准滤波器相同，但是利用了载体的转弯机动来提高方位失准角的可观测性，并且在校正方位之前首先校正了载体的水平姿态。在利用载体不同阶段的动态特性估计方位失准角时，为了实现滤波器的优化与稳定，采用协方差成形方法动态调节各通道间的滤波增益。
结合上述原理，具体地S4包括如下步骤：
S401、构建方位精对准滤波器的离散化线性系统方程：
Figure BDA0002575368460000085
其中
Figure BDA0002575368460000086
为方位精对准滤波器的k时刻状态量预测值(经过一个滤波更新周期的k时刻状态量预测值)；
Figure BDA0002575368460000087
为方位精对准滤波器的k-1时刻的状态量；
Figure BDA0002575368460000088
为方位精对准滤波器的k刻的状态量；Wk为系统噪声；
Figure BDA0002575368460000089
为方位精对准滤波器的量测方程中k时刻的量测值，H为量测矩阵(水平精对准滤波器和方位精对准滤波器的量测矩阵相同)；Vk为量测噪声。A为离散化状态一步转移矩阵(水平精对准滤波器和方位精对准滤波器的离散化状态一步转移矩阵相同)。
S402、方位精对准滤波器的系统量测残差为
Figure BDA0002575368460000091
其中
Figure BDA0002575368460000092
为方位精对准滤波器的k时刻的状态量的估计值。
方位精对准滤波器的系统量测残差的方差为Sk/k-1＝E[ek(ek)T]＝HPk/k-1(H)T+Rk
上式中Rk为方位精对准滤波器的系统量测噪声协方差矩阵；Pk/k-1为方位精对准滤波器的预测误差协方差矩阵。
S403、对Pk/k-1进行分解：
Figure BDA0002575368460000093
其中
Figure BDA0002575368460000094
为分解得到的第一误差协方差；
Figure BDA0002575368460000095
为分解得到的第二误差协方差矩阵；α为滤波自适应矩阵。
根据分解得到的
Figure BDA0002575368460000096
和
Figure BDA0002575368460000097
将系统量测残差的方差Sk/k-1分解为：
Figure BDA0002575368460000098
其中
Figure BDA0002575368460000099
为对应
Figure BDA00025753684600000910
的方差分解项：
Figure BDA00025753684600000911
Figure BDA00025753684600000912
为对应
Figure BDA00025753684600000913
的方差分解项：
Figure BDA00025753684600000914
S404、计算方位精对准滤波器的系统量测残差协方差的均值
Figure BDA00025753684600000915
Figure BDA00025753684600000916
其中N为实际测得的量测值的样本总数；i为量测值编号；Z(k+1-N+i)为第i个量测值。
构建目标函数J(α)，并使得目标函数J(α)取最小值。
Figure BDA00025753684600000917
其中||*||2表示矩阵*的2范数。
若要使目标函数J(α)最小，则应满足以下关系
Figure BDA00025753684600000918
整理得到
取
Figure BDA00025753684600000919
式中diag(*)表示取矩阵*的对角元素；
Figure BDA00025753684600000920
为系统量测残差协方差的均值。
并根据α的取值计算优化后的Pk/k-1。
S405、利用优化后的Pk/k-1成形Kalman滤波算法，得到方位精对准滤波器。
S406、以第二解算器II和多普勒计程仪DVL的输出之差作为量测值输入至方位精对准滤波器，在载体做转弯机动时，方位精对准滤波器输出估计的方位失准角，即天向失准角φU。
S5、将方位失准角反馈至第二解算器II，完成载体姿态校正，并且将校正后的载体姿态反馈给第一解算器I。
本发明实施例中，将方位失准角反馈至第二解算器II，对载体的姿态矩阵进行校正，其中载体的姿态矩阵即为捷联惯导的姿态矩阵，校正前载体的姿态矩阵为第一次校正后的捷联惯导的姿态矩阵
Figure BDA0002575368460000101
校正后载体的姿态矩阵为
Figure BDA0002575368460000102
Figure BDA0002575368460000103
将校正后载体的姿态矩阵
Figure BDA0002575368460000104
反馈给第一解算器I即可。
本发明设计了“水平+方位”两个滤波器，并且在校正方位之前首先校正了载体的水平姿态，本质上是一个双滤波器的并行方案。
本发明实施例给出了如下仿真实例：
表格1捷联惯组和多普勒计程仪性能指标
Figure BDA0002575368460000105
Figure BDA0002575368460000111
0～95s潜器直行，95s时进行转弯机动。由于在仿真开始之前深海惯导已经导航了一段时间，姿态信息有误差，因此在0s时刻给载体设置一定的姿态误差，仿真结果如附图2～4所示：图2为本发明实施例的俯仰角校正曲线仿真结果图；图3为本发明实施例的横滚角校正曲线仿真结果图；图4为本发明实施例的方位角校正曲线仿真结果图。根据图2～图4所示，可以看出本发明实施例提供的方差成形自适应Kalman滤波算法进行方位精对准滤波器的设计实例，相比于标准卡尔曼滤波方法具有较高的精确度，且方位精对准滤波器收敛较快，因此能够大幅缩短校正时间。
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.深海惯导方位快速校正的方法，其特征在于，包括如下步骤：
S1、载体上搭载捷联惯导系统和多普勒计程仪DVL；所述捷联惯导系统，包含并行的两个捷联积分解算器，分别为第一解算器I和第二解算器II；
S2、构建水平精对准滤波器，以第一解算器I和多普勒计程仪DVL的输出，估计水平失准角；
S3、将所述水平失准角反馈至第二解算器II，并控制载体开始做转弯机动；
S4、构建方位精对准滤波器，以第二解算器II和多普勒计程仪DVL的输出，估计方位失准角；
S5、将方位失准角反馈至第二解算器II，完成载体姿态校正，并且将校正后的载体姿态反馈给第一解算器I。
2.如权利要求1所述的方法，其特征在于，所述捷联积分解算器为捷联惯导系统的解算程序。
3.如权利要求1或2所述的方法，其特征在于，所述构建水平精对准滤波器，以第一解算器I和多普勒计程仪DVL的输出，估计水平失准角，具体为：
S201、构建水平精对准滤波器，包括如下步骤：
S2011、选取姿态误差、速度误差和陀螺常值零偏作为所述水平精对准滤波器的状态量，即状态量为X＝[φE φN φU δVE δVN εx εy εz]；
其中φE为东向平台失准角，φN为北向平台失准角，φU为天向平台失准角，δVE为东向速度误差，δVN为北向速度误差，εx为捷联惯导坐标系x向的陀螺零偏，εy捷联惯导坐标系y向的陀螺零偏，εz捷联惯导坐标系z向的陀螺零偏；其中x、y、z分别为捷联惯导坐标系的x、y、z轴；
S2012，构建所述水平精对准滤波器的离散化状态方程为
Figure FDA0002575368450000011
其中
Figure FDA0002575368450000021
为所述水平精对准滤波器的k时刻状态量预测值；
Figure FDA0002575368450000022
为所述水平精对准滤波器的k-1时刻的状态量；A为离散化状态一步转移矩阵；Wk为系统噪声；
Figure FDA0002575368450000023
其中第一中间矩阵A1用于指代
Figure FDA0002575368450000024
即
Figure FDA0002575368450000025
Figure FDA0002575368450000026
为捷联惯导的姿态矩阵；ΔT为时间的滤波更新周期，
第二中间矩阵A2为
Figure FDA0002575368450000027
ΔVE、ΔVN和ΔVU分别是时间更新周期ΔT内捷联惯导的加速度计的输出即比力在东向、北向、天向三个方向上的积分；
I3×3和I2×2分别为3×3和2×2的单位阵；02×3、03×3、03×2分别为2×3、3×3以及3×2的零矩阵；
S2013、所述水平精对准滤波器的量测值为
Figure FDA0002575368450000028
量测矩阵为H＝[02×3 I2×2 03×3]；
其中Z1为所述水平精对准滤波器的量测值；H为量测矩阵；
Figure FDA0002575368450000029
和
Figure FDA00025753684500000210
分别为所述第一解算器Ι出的载体东向速度和北向速度，
Figure FDA00025753684500000211
和
Figure FDA00025753684500000212
分别为多普勒计程仪DVL输出的载体东向速度和北向速度；
S202、以第一解算器I和多普勒计程仪DVL的输出作为所述水平精对准滤波器的输入，所述水平精对准滤波器输出状态量的预测值，包含估计的水平失准角，所述水平失准角包括东向平台失准角φE和北向平台失准角φN。
4.如权利要求3所述的方法，其特征在于，所述将所述水平失准角反馈至第二解算器II，具体为：
将估计的水平失准角，包括东向平台失准角φE和北向平台失准角φN，反馈给第二解算器II，得到第一次校正后的捷联惯导的姿态矩阵
Figure FDA0002575368450000031
Figure FDA0002575368450000032
其中
Figure FDA0002575368450000033
为第一次校正前的捷联惯导的姿态矩阵，即
Figure FDA0002575368450000034
5.如权利要求4所述的方法，其特征在于，所述构建方位精对准滤波器，以第二解算器II和多普勒计程仪DVL的输出，估计方位失准角；具体为：
S401、构建方位精对准滤波器的离散化线性系统方程：
Figure FDA0002575368450000035
其中
Figure FDA0002575368450000036
为所述方位精对准滤波器的k时刻状态量预测值；
Figure FDA0002575368450000037
为所述方位精对准滤波器的k-1时刻的状态量；
Figure FDA0002575368450000038
为所述方位精对准滤波器的k刻的状态量；Wk为系统噪声；
Figure FDA0002575368450000039
为方位精对准滤波器的量测方程中k时刻的量测值，H为量测矩阵；Vk为量测噪声；
S402、方位精对准滤波器的系统量测残差为
Figure FDA00025753684500000310
其中
Figure FDA00025753684500000311
为方位精对准滤波器的k时刻的状态量的估计值；
方位精对准滤波器的系统量测残差的方差为Sk/k-1＝E[ek(ek)T]＝HPk/k-1(H)T+Rk
上式中Rk为方位精对准滤波器的系统量测噪声协方差矩阵；Pk/k-1为方位精对准滤波器的预测误差协方差矩阵；
S403、对Pk/k-1进行分解：
Figure FDA00025753684500000312
其中
Figure FDA00025753684500000313
为分解得到的第一误差协方差；
Figure FDA00025753684500000314
为分解得到的第二误差协方差矩阵；α为滤波自适应矩阵；
根据分解得到的
Figure FDA00025753684500000315
和
Figure FDA00025753684500000316
将系统量测残差的方差Sk/k-1分解为：
Figure FDA00025753684500000317
其中
Figure FDA00025753684500000318
为对应
Figure FDA00025753684500000319
的方差分解项：
Figure FDA00025753684500000320
Figure FDA00025753684500000321
为对应
Figure FDA00025753684500000322
的方差分解项：
Figure FDA0002575368450000041
S404、取
Figure FDA0002575368450000042
式中diag(*)表示取矩阵*的对角元素；
Figure FDA0002575368450000043
为系统量测残差协方差的均值；
并根据α的取值计算优化后的Pk/k-1；
S405、利用优化后的Pk/k-1成形Kalman滤波算法，得到方位精对准滤波器；
S406、以第二解算器II和多普勒计程仪DVL的输出之差作为量测值输入至所述方位精对准滤波器，在载体做转弯机动时，所述方位精对准滤波器输出估计的方位失准角，即天向平台失准角φU。
6.如权利要求1～5任一所述的方法，其特征在于，所述将方位失准角反馈至第二解算器II，完成载体姿态校正，并且将校正后的载体姿态反馈给第一解算器I，具体为：
将方位失准角反馈至第二解算器II，对载体的姿态矩阵进行校正，其中载体的姿态矩阵即为所述捷联惯导的姿态矩阵，校正前载体的姿态矩阵为第一次校正后的捷联惯导的姿态矩阵
Figure FDA0002575368450000044
校正后载体的姿态矩阵为
Figure FDA0002575368450000045
Figure FDA0002575368450000046
将校正后载体的姿态矩阵
Figure FDA0002575368450000047
反馈给第一解算器I即可。
CN202010652167.1A 2020-07-08 2020-07-08 深海惯导方位快速校正的方法 Pending CN111964689A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010652167.1A CN111964689A (zh) 2020-07-08 2020-07-08 深海惯导方位快速校正的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010652167.1A CN111964689A (zh) 2020-07-08 2020-07-08 深海惯导方位快速校正的方法

Publications (1)

Publication Number Publication Date
CN111964689A true CN111964689A (zh) 2020-11-20

Family

ID=73361316

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010652167.1A Pending CN111964689A (zh) 2020-07-08 2020-07-08 深海惯导方位快速校正的方法

Country Status (1)

Country Link
CN (1) CN111964689A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113503891A (zh) * 2021-04-22 2021-10-15 中国人民解放军海军工程大学 一种sinsdvl对准校正方法、系统、介质及设备

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101566477A (zh) * 2009-06-03 2009-10-28 哈尔滨工程大学 舰船局部捷联惯导系统初始姿态快速测量方法
CN103245357A (zh) * 2013-04-03 2013-08-14 哈尔滨工程大学 一种船用捷联惯导系统二次快速对准方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101566477A (zh) * 2009-06-03 2009-10-28 哈尔滨工程大学 舰船局部捷联惯导系统初始姿态快速测量方法
CN103245357A (zh) * 2013-04-03 2013-08-14 哈尔滨工程大学 一种船用捷联惯导系统二次快速对准方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
刘斌: "GNSS辅助捷联惯导行进间对准自适应滤波方法", 中国惯性技术学报, vol. 24, no. 5, pages 577 - 582 *
曹洁: "AUV中SINS/DVL组合导航技术研究", 中国航海, no. 02, pages 55 - 59 *
王博: "SINS/DVL组合导航技术综述", 导航定位学报, pages 1 - 4 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113503891A (zh) * 2021-04-22 2021-10-15 中国人民解放军海军工程大学 一种sinsdvl对准校正方法、系统、介质及设备

Similar Documents

Publication Publication Date Title
CN108827310B (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN109945859B (zh) 一种自适应h∞滤波的运动学约束捷联惯性导航方法
US5050087A (en) System and method for providing accurate attitude measurements at remote locations within an aircraft
RU2348903C1 (ru) Способ определения навигационных параметров бесплатформенной инерциальной навигационной системой
CN112630813A (zh) 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN112146655B (zh) 一种BeiDou/SINS紧组合导航系统弹性模型设计方法
CN114858189B (zh) 捷联惯导系统陀螺漂移等效补偿方法
CN110285834B (zh) 基于一点位置信息的双惯导系统快速自主重调方法
CN110044385B (zh) 一种大失准角情况下的快速传递对准方法
CN107576977B (zh) 基于多源信息自适应融合的无人机导航系统及方法
CN116147624B (zh) 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法
CN116007620A (zh) 一种组合导航滤波方法、系统、电子设备及存储介质
CN114526731A (zh) 一种基于助力车的惯性组合导航方向定位方法
CN110388942B (zh) 一种基于角度和速度增量的车载姿态精对准系统
CN116222551A (zh) 一种融合多种数据的水下导航方法及装置
CN114608583A (zh) 一种高超声速飞行器动态杆臂效应智能补偿方法
Guan et al. Sensor fusion of gyroscope and accelerometer for low-cost attitude determination system
CN111964689A (zh) 深海惯导方位快速校正的方法
CN111707292B (zh) 一种自适应滤波的快速传递对准方法
CN111854741B (zh) 一种gnss/ins紧组合滤波器及导航方法
CN111141285B (zh) 一种航空重力测量装置
CN110567490B (zh) 一种大失准角下sins初始对准方法
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN114993296B (zh) 一种制导炮弹高动态组合导航方法
CN113447018B (zh) 一种水下惯性导航系统的姿态实时估计方法

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