CN105021212B - 一种初始方位信息辅助下潜航器快速传递对准方法 - Google Patents

一种初始方位信息辅助下潜航器快速传递对准方法 Download PDF

Info

Publication number
CN105021212B
CN105021212B CN201510390727.XA CN201510390727A CN105021212B CN 105021212 B CN105021212 B CN 105021212B CN 201510390727 A CN201510390727 A CN 201510390727A CN 105021212 B CN105021212 B CN 105021212B
Authority
CN
China
Prior art keywords
msub
mrow
mtd
msubsup
mtr
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
CN201510390727.XA
Other languages
English (en)
Other versions
CN105021212A (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 CN201510390727.XA priority Critical patent/CN105021212B/zh
Publication of CN105021212A publication Critical patent/CN105021212A/zh
Application granted granted Critical
Publication of CN105021212B publication Critical patent/CN105021212B/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
    • 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

Abstract

本发明针对现有传递对准方法在保证潜航器惯导对准精度的条件下，不能实现移动基准与潜航器快速分离和一次性多潜航器布放的问题，公开了一种初始方位信息辅助下潜航器快速传递对准方法。本发明包括以下四步：1、初始信息装订，信息装订完毕后潜航器与移动基准即分离；2、潜航器姿态更新方程、速度更新方程、位置更新方程确定；3、卡尔曼滤波状态方程与观测方程确定；4、卡尔曼滤波并对潜航器姿态、速度误差修正，完成传递对准。本发明实现了移动基准与潜航器的快速分离，同时能够保证潜航器惯导对准精度，且能实现一次性多潜航器布放。

Description

一种初始方位信息辅助下潜航器快速传递对准方法
技术领域
本发明涉及一种传递对准方法，特别是一种初始方位信息辅助下潜航器快速传递对准方法。
背景技术
潜航器遂行作战任务之前，由舰船将其带入指定地点，然后将其投放；之后，潜航器利用其自身惯性导航设备在多普勒测速仪辅助下进行自主导航。潜航器布放前其惯性导航系统的初始对准，可以选择采用组合导航初始对准的方式进行自对准，也可以以舰船惯导系统或移动基准惯导系统为主惯导系统进行传递对准。
采用组合导航自对准方式时不需要舰船惯导系统提供数据，但动基座环境下自对准通常需要较长时间才能完成初始对准过程，难以达到快速对准的要求。为了满足快速对准的需求，目前国内外多采取了利用主惯导或移动基准惯导系统(MINS)为信息源辅助子惯导系统(SINS)进行传递对准，这就需要主惯导系统实时与子惯导系统进行一段时间的测量参数匹配或者导航参数匹配，这种传递对准方法存在的问题是：不能实现主惯导或移动基准惯导与潜航器的快速分离，影响潜航器的快速布放，战场环境下，不仅影响潜航器作战效能的发挥，还会对携带潜航器进入战场环境的移动基准载体的生存能力构成威胁，并且按传统传递对准方式逐一进行传递对准，难以实现短时间同时布放多个潜航器的任务。因此，寻找一种能实现移动基准惯导与潜航器惯导快速分离，同时能够保证潜航器惯导对准精度，且能实现一次性多潜航器布放的传递对准方式具有重要意义。
发明内容
本发明针对现有传递对准方法在保证潜航器惯导对准精度的条件下，不能实现移动基准惯导与潜航器惯导的快速分离，且不能实现一次性多潜航器布放的问题，提出了一种初始方位信息辅助下潜航器快速传递对准方法。
该方法只需移动基准惯导在初始对准开始时刻提供潜航器惯导系统方位信息及概略的水平姿态信息后，潜航器随即布放入水，其惯导系统在多普勒测速仪的速度信息辅助下估计惯性系下的初始水平姿态误差和当前的速度误差，进行组合导航自对准。由于水平姿态误差和速度误差可通过卡尔曼滤波快速收敛，可实现移动基准惯导与潜航器惯导快速分离，且能满足短时间投放较多数量潜航器的快速对准要求。
为实现本发明所采取的技术解决方案是：
一种初始方位信息辅助下潜航器快速传递对准方法，包含以下步骤：
步骤一：移动基准惯导(MINS)对每一个潜航器惯导(SINS)装订初始位置(包括初始经度λ0、初始纬度L0)、初始速度初始姿态信息(包括准确的方位角存在误差的俯仰角与横滚角)，装订完成后，潜航器与移动基准分离；
其中，t0表示对准初始时刻，表示潜航器相对于地球的速度在地理坐标系n系下的投影，地理坐标系n系定义为北向-东向-地向(N-E-D)，地球坐标系e系的原点位于地心，ez轴沿地球自转轴方向，ex轴在赤道平面内指向格林威治子午线，ey轴与另外两轴构成右手正交坐标系；
表示惯性坐标系系到惯性坐标系系的方向余弦矩阵，系由对准初始时刻载体坐标系b系与惯性空间固联得到(相对惯性空间固定不动)，系由对准初始时刻n系与惯性空间固联得到(相对惯性空间固定不动)，载体坐标系b系轴向沿载体横滚轴-俯仰轴-偏航轴(前-右-下)；
步骤二：潜航器惯导(SINS)建立姿态更新方程、速度更新方程、位置更新方程分别如下：
姿态更新方程：
其中，
n0系定义为对准初始时刻的地理坐标系，相对地球表面固定不动，不随导航系统在地球表面运动而运动；
为对准初始时刻导航系n0系到当前时刻导航系n系的方向余弦矩阵；
为惯性坐标系系到对准初始时刻导航系n0系的方向余弦矩阵；
为惯性坐标系系到惯性坐标系系的方向余弦矩阵；
为载体坐标系b系到惯性坐标系系的方向余弦矩阵；
速度更新方程：
其中，
tk-1、tk分别表示第k-1、k个潜航器速度更新时刻，为对应的速度，fb为加表比力输出，为tk-1时刻相应的方向余弦矩阵，Δt＝tk-tk-1为惯导更新时间间隔，为地理系下的地球自转角速度，为地理系下的转移角速度，为地理系下的重力；
位置更新方程：
潜航器经纬度通过航迹推算进行更新，其中，
Lk-1、Lk分别表示k-1、k时刻潜航器的纬度，λk-1、λk分别表示k-1、k时刻潜航器的经度，vn、ve表示潜航器的北向、东向速度，RN表示子午面曲率半径，h表示高度；
步骤三：构建卡尔曼滤波状态方程与观测方程如下：
选取横滚角误差δθ0、俯仰角误差δγ0、北向速度误差δvn、东向速度误差δve为系统状态，即
x＝[δθ0 δγ0 δvn δve]T (4)
构建系统状态方程如下，
其中，
上标T表示矩阵或向量的转置，x表示系统状态，表示状态微分，F表示状态转移矩阵，G表示系统噪声矩阵； 为陀螺输出噪声，δfb为加表输出噪声；
取1s更新一次的多普勒速度为观测量，构建观测方程如下，
z＝Hx+v (6)
其中，
多普勒坐标系d系轴向沿多普勒测速仪的横滚轴-俯仰轴-偏航轴(前-右-下)；
z为观测向量，H为观测矩阵，v为测量噪声；
步骤四：根据卡尔曼滤波状态方程和观测方程进行滤波，对横滚角误差、俯仰角误差、北向速度误差、东向速度误差进行估计，对潜航器的姿态、速度进行修正，完成传递对准；
其中，在步骤二中所述的姿态更新方程按如下方法更新：
其中，I3×3表示三维单位矩阵，ωie为地球自转角速度，t为初始时刻到当前时刻的解算时间，λ0、L0为潜航器装订的初始经度、纬度，λ、L为当前时刻经度、纬度；
可由潜航器初始经纬度和t确定，将按如下分解，
其中，
系由对准初始时刻e系与惯性空间固联得到(相对惯性空间固定不动)；
为地球坐标系e系到对准初始时刻导航系n0系的方向余弦矩阵；
为惯性坐标系系到地球坐标系e系的方向余弦矩阵；
为惯性坐标系系到惯性坐标系系的方向余弦矩阵；
利用陀螺输出数据求解，传递对准完成时间内，忽略陀螺的零偏、随机游走等误差，按如下微分方程和初值进行姿态更新求解，即
其中，为载体角速度，为的初值；
在步骤三中所述的卡尔曼滤波状态方程按如下方法构建：
1)水平欧拉角误差微分方程构建如下
水平欧拉角误差为常值，其微分为零，即
2)速度误差微分方程构建如下
①构建估算的姿态矩阵失准角与水平欧拉角误差δθ0、δγ0间的关系
其中，φ为估算的姿态矩阵失准角，
进一步，将(13)代入的表达式(12)并化简可得，
②构建包含水平欧拉角误差δθ0、δγ0的速度误差微分方程
潜航器相对于地球真实速度在n系下的速度微分方程为，
估算速度微分方程为，
(16)式与(15)式相减得速度误差微分方程，
其中，为有误差的比力输出，为估算速度，为速度误差，为估算的转移角速度；
将(14)式代入(17)式，得到包含水平欧拉角误差δθ0、δγ0的北向速度误差δvn、东向速度误差δve微分方程如下，
其中，δvn为北向速度误差、δve为东向速度误差，为估算的北向速度，为估算的东向速度，vn为真实北向速度，ve为真实东向速度；
M4＝[I2×2 02×1]M1M,M5＝[I2×2 02×1]M2[I2×2 02×1]T,M6＝[I2×2 02×1]M3；
3)由1)、2)步中水平欧拉角误差微分方程、速度误差微分方程得到卡尔曼滤波状态方程中状态转移矩阵及系统噪声矩阵相应为
在步骤三中所述的卡尔曼滤波观测方程按如下方法构建：
1)构建地理系n系下含有误差的多普勒测速输出与姿态矩阵失准角φ的关系
其中，为多普勒测速仪测得的潜航器速度在n系下投影，为多普勒测速仪测得的潜航器速度在d系下的投影，多普勒测速仪测速误差在d系下的投影；
2)构建速度误差观测量z
潜航器惯导解算速度将潜航器惯导解算速度的北向速度东向速度与多普勒测速仪输出中的北向速度东向速度作差得到速度误差观测量，
其中，
3)确定观测方程z＝Hx+v中观测矩阵为
H＝[M7 I2×2]。
与现有技术相比，本发明的有益效果是：
1)移动基准惯导(MINS)对每一个潜航器惯导(SINS)装订完成初始位置、初始速度、初始姿态信息后，潜航器与移动基准即可分离，不需要主/子惯导继续传送信息，传送信息少，改变了目前传递对准需要主/子惯导实时传送一段时间的信息才能完成对准的现状，实现了分离的快速性。
2)移动基准惯导(MINS)可同时对多个潜航器惯导(SINS)进行信息装订，同时布放多个潜航器，改变了目前传递对准只能实现主惯导与每一个子惯导一一进行传递对准的现状。
3)该传递对准方法对惯性坐标系下的常值水平欧拉角误差进行估计，估计精度高，缩短了对准时间。
附图说明
图1为本发明方法的流程示意图；
图2为无初始方位角信息辅助下传统对准过程中方位角误差曲线；
图3为初始方位角信息辅助下传递对准过程中方位角误差曲线。
具体实施方式
下面结合附图对本发明中的方法作进一步详细说明。
步骤一：移动基准惯导(MINS)对每一个潜航器惯导(SINS)装订初始位置(包括初始经度λ0、初始纬度L0)、初始速度初始姿态信息(包括准确的方位角存在误差的俯仰角与横滚角)，装订完成后，潜航器与移动基准分离；
其中，t0表示对准初始时刻，表示潜航器相对于地球的速度在地理坐标系n系下的投影，地理坐标系n系定义为北向-东向-地向(N-E-D)，地球坐标系e系的原点位于地心，ez轴沿地球自转轴方向，ex轴在赤道平面内指向格林威治子午线，ey轴与另外两轴构成右手正交坐标系；
表示惯性坐标系系到惯性坐标系系的方向余弦矩阵，系由对准初始时刻载体坐标系b系与惯性空间固联得到(相对惯性空间固定不动)，系由对准初始时刻n系与惯性空间固联得到(相对惯性空间固定不动)，载体坐标系b系轴向沿载体横滚轴-俯仰轴-偏航轴(前-右-下)；
步骤二：潜航器惯导(SINS)建立姿态更新方程、速度更新方程、位置更新方程分别如下：
姿态更新方程：
其中，
n0系定义为对准初始时刻的地理坐标系，相对地球表面固定不动，不随导航系统在地球表面运动而运动；
为对准初始时刻导航系n0系到当前时刻导航系n系的方向余弦矩阵；
为惯性坐标系系到对准初始时刻导航系n0系的方向余弦矩阵；
为惯性坐标系系到惯性坐标系系的方向余弦矩阵；
为载体坐标系b系到惯性坐标系系的方向余弦矩阵；
速度更新方程：
其中，
tk-1、tk分别表示第k-1、k个潜航器速度更新时刻，为对应的速度，fb为加表比力输出，为tk-1时刻相应的方向余弦矩阵，Δt＝tk-tk-1为惯导更新时间间隔，为地理系下的地球自转角速度，为地理系下的转移角速度，为地理系下的重力；
位置更新方程：
潜航器经纬度通过航迹推算进行更新，其中，
Lk-1、Lk分别表示k-1、k时刻潜航器的纬度，λk-1、λk分别表示k-1、k时刻潜航器的经度，vn、ve表示潜航器的北向、东向速度，RN表示子午面曲率半径，h表示高度；
步骤三：构建卡尔曼滤波状态方程与观测方程如下：
选取横滚角误差δθ0、俯仰角误差δγ0、北向速度误差δvn、东向速度误差δve为系统状态，即
x＝[δθ0 δγ0 δvn δve]T (24)
构建系统状态方程如下，
其中，上标T表示矩阵或向量的转置，x表示系统状态，表示状态微分，F表示状态转移矩阵，G表示系统噪声矩阵； 为陀螺输出噪声，δfb为加表输出噪声；
系统状态方程包括水平欧拉角误差微分方程和速度误差微分方程，分别构建如下：
1)水平欧拉角误差微分方程
水平欧拉角误差为常值，其微分为零，即
2)速度误差微分方程
①构建估算的姿态矩阵失准角与水平欧拉角误差δθ0、δγ0间的关系
其中，φ为估算的姿态矩阵失准角，
进一步，将(28)代入的表达式(27)并化简可得，
②构建包含水平欧拉角误差δθ0、δγ0的速度误差微分方程
潜航器相对于地球真实速度在n系下的速度微分方程为，
估算速度微分方程为，
(31)式与(30)式相减得速度误差微分方程，
其中，为有误差的比力输出，为估算速度，为速度误差，为估算的转移角速度；
将(29)式代入(32)式，得到包含水平欧拉角误差δθ0、δγ0的北向速度误差δvn、东向速度误差δve微分方程如下，
其中，δvn为北向速度误差、δve为东向速度误差，为估算的北向速度，为估算的东向速度，vn为真实北向速度，ve为真实东向速度；
M4＝[I2×2 02×1]M1M,M5＝[I2×2 02×1]M2[I2×2 02×1]T,M6＝[I2×2 02×1]M3；
3)由1)、2)步中水平欧拉角误差微分方程、速度误差微分方程得到卡尔曼滤波状态方程中状态转移矩阵及系统噪声矩阵相应为
取1s更新一次的多普勒速度为观测量，构建观测方程如下，
z＝Hx+v (34)
其中，
多普勒坐标系d系轴向沿多普勒测速仪的横滚轴-俯仰轴-偏航轴(前-右-下)；
z为观测向量，H为观测矩阵，v为测量噪声；
观测方程包括观测量与观测矩阵的确定，分别确定如下：
1)构建地理系n系下含有误差的多普勒测速输出与姿态矩阵失准角φ的关系
其中，为多普勒测速仪测得的潜航器速度在n系下投影，为多普勒测速仪测得的潜航器速度在d系下的投影，多普勒测速仪测速误差在d系下的投影；
2)构建速度误差观测量z
潜航器惯导解算速度将潜航器惯导解算速度的北向速度东向速度与多普勒测速仪输出中的北向速度东向速度作差得到速度误差观测量，
其中，
3)确定观测方程z＝Hx+v中观测矩阵为
H＝[M7 I2×2]；
步骤四：根据卡尔曼滤波状态方程和观测方程进行滤波，对横滚角误差、俯仰角误差、北向速度误差、东向速度误差进行估计，对潜航器的姿态、速度进行修正，完成传递对准。
下面对本发明方法的效果进行检验。
使用水下潜航器数据对本发明中的方法进行验证，实验中主惯导系统传递给子惯导的姿态信息为初始基准值为θ0＝-3.15°，γ0＝1.51°，潜航器姿态基准值为利用原始数据进行10min的INS/GPS组合导航解算得到。潜航器惯导系统为激光陀螺捷联惯导系统，陀螺精度为0.01°/h，加表精度为5×10-5g。实验截取潜航器运行前10min数据进行方法验证。
对比图2对应的无初始方位角信息辅助下传统对准过程中方位角误差曲线(以多普勒测速仪速度输出值为观测量，潜航器在无初始方位信息下自对准)、图3对应的初始方位角信息辅助下传递对准过程中方位角误差曲线，可以发现图2中方位角误差在600s时才收敛，图3中采用初始方位角信息辅助下的传递对准方案，方位角误差到50s时就收敛到1密位，对准时间与精度都得到明显改善。
本方法利用移动基准惯导的方位角信息确定子惯导系统的方位角信息，缩短了与子惯导系统信息传输时间，使得子惯导系统可以实现与主惯导系统分离的快速性，在此基础上，可以实现短时间投放大量潜航器的实际需求，同时可以保证子惯导系统在短时间内实现对准。
以上仅是本发明的优选实施方式，本发明的保护范围并不仅限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应当视为落入本发明的保护范围。

Claims (2)

1.一种初始方位信息辅助下潜航器快速传递对准方法，其特征在于包含以下步骤：
步骤一：移动基准惯导(MINS)对每一个潜航器惯导(SINS)装订初始位置，初始位置包括初始经度λ0、初始纬度L0，初始速度初始姿态信息初始姿态信息包括准确的方位角存在误差的俯仰角与横滚角装订完成后，潜航器与移动基准分离；
其中，t0表示对准初始时刻，表示潜航器相对于地球的速度在地理坐标系n系下的投影，地理坐标系n系定义为北向-东向-地向(N-E-D)，地球坐标系e系的原点位于地心，ez轴沿地球自转轴方向，ex轴在赤道平面内指向格林威治子午线，ey轴与另外两轴构成右手正交坐标系；
表示惯性坐标系系到惯性坐标系系的方向余弦矩阵，系由对准初始时刻载体坐标系b系与惯性空间固联得到，系相对惯性空间固定不动，系由对准初始时刻n系与惯性空间固联得到，系相对惯性空间固定不动，载体坐标系b系轴向沿载体横滚轴-俯仰轴-偏航轴(前-右-下)；
步骤二：潜航器惯导(SINS)建立姿态更新方程、速度更新方程、位置更新方程分别如下：
姿态更新方程：
其中，
n0系定义为对准初始时刻的地理坐标系，相对地球表面固定不动，不随导航系统在地球表面运动而运动；
为对准初始时刻导航系n0系到当前时刻导航系n系的方向余弦矩阵；
为惯性坐标系系到对准初始时刻导航系n0系的方向余弦矩阵；
为惯性坐标系系到惯性坐标系系的方向余弦矩阵；
为载体坐标系b系到惯性坐标系系的方向余弦矩阵；
速度更新方程：
其中，
<mrow> <msub> <mi>&amp;Delta;v</mi> <mi>a</mi> </msub> <mo>=</mo> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msub> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msub> <msub> <mi>i</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msubsup> <mo>&amp;Integral;</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>t</mi> <mi>k</mi> </msub> </msubsup> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>i</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msub> </msubsup> <msup> <mi>f</mi> <mi>b</mi> </msup> <mi>d</mi> <mi>t</mi> </mrow>
<mrow> <msub> <mi>&amp;Delta;v</mi> <mi>b</mi> </msub> <mo>=</mo> <mi>&amp;Delta;</mi> <mi>t</mi> <mrow> <mo>(</mo> <mo>(</mo> <mrow> <mn>2</mn> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> <mi>n</mi> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>e</mi> <mi>n</mi> </mrow> <mi>n</mi> </msubsup> </mrow> <mo>)</mo> <mo>&amp;times;</mo> <msubsup> <mi>v</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>&amp;Delta;v</mi> <mi>c</mi> </msub> <mo>=</mo> <msubsup> <mi>&amp;Delta;tg</mi> <mi>l</mi> <mi>n</mi> </msubsup> </mrow>
tk-1、tk分别表示第k-1、k个潜航器速度更新时刻，为对应的速度，fb为加表比力输出，为tk-1时刻相应的方向余弦矩阵，Δt＝tk-tk-1为惯导更新时间间隔，为地理系下的地球自转角速度，为地理系下的转移角速度，为地理系下的重力；
位置更新方程：
潜航器经纬度通过航迹推算进行更新，其中，
Lk-1、Lk分别表示k-1、k时刻潜航器的纬度，λk-1、λk分别表示k-1、k时刻潜航器的经度，vn、ve表示潜航器的北向、东向速度，RN表示子午面曲率半径，h表示高度；
步骤三：构建卡尔曼滤波状态方程与观测方程如下：
选取横滚角误差δθ0、俯仰角误差δγ0、北向速度误差δvn、东向速度误差δve为系统状态，即
x＝[δθ0 δγ0 δvn δve ]T (4)
构建系统状态方程如下，
<mrow> <mover> <mi>x</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mi>F</mi> <mi>x</mi> <mo>+</mo> <mi>G</mi> <mi>w</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>
其中，
上标T表示矩阵或向量的转置，x表示系统状态，表示状态微分，F表示状态转移矩阵，G表示系统噪声矩阵； 为陀螺输出噪声，δfb为加表输出噪声；
卡尔曼滤波状态方程按如下方法构建：
1)水平欧拉角误差微分方程构建如下
水平欧拉角误差为常值，其微分为零，即
<mrow> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>&amp;delta;</mi> <msub> <mover> <mi>&amp;theta;</mi> <mo>&amp;CenterDot;</mo> </mover> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mi>&amp;delta;</mi> <msub> <mover> <mi>&amp;gamma;</mi> <mo>&amp;CenterDot;</mo> </mover> <mn>0</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>=</mo> <msub> <mn>0</mn> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>
2)速度误差微分方程构建如下
<mrow> <mtable> <mtr> <mtd> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>&amp;delta;</mi> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>n</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mi>&amp;delta;</mi> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>e</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mover> <mover> <mi>v</mi> <mo>~</mo> </mover> <mo>&amp;CenterDot;</mo> </mover> <mi>n</mi> </msub> </mtd> <mtd> <msub> <mover> <mover> <mi>v</mi> <mo>~</mo> </mover> <mo>&amp;CenterDot;</mo> </mover> <mi>e</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>-</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>n</mi> </msub> </mtd> <mtd> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>e</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mo>&amp;lsqb;</mo> <mtable> <mtr> <mtd> <msub> <mi>I</mi> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>2</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> </mtable> <mo>&amp;rsqb;</mo> <msub> <mi>M</mi> <mn>1</mn> </msub> <mi>M</mi> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;&amp;theta;</mi> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;delta;&amp;gamma;</mi> <mn>0</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>+</mo> <mo>&amp;lsqb;</mo> <mtable> <mtr> <mtd> <msub> <mi>I</mi> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>2</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> </mtable> <mo>&amp;rsqb;</mo> <msub> <mi>M</mi> <mn>2</mn> </msub> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>I</mi> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>2</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;v</mi> <mi>n</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;delta;v</mi> <mi>e</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>+</mo> <mo>&amp;lsqb;</mo> <mtable> <mtr> <mtd> <msub> <mi>I</mi> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>2</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> </mtable> <mo>&amp;rsqb;</mo> <msub> <mi>M</mi> <mn>3</mn> </msub> <msup> <mi>&amp;delta;f</mi> <mi>b</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <msub> <mi>M</mi> <mn>4</mn> </msub> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;&amp;theta;</mi> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;delta;&amp;gamma;</mi> <mn>0</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>M</mi> <mn>5</mn> </msub> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;v</mi> <mi>n</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;delta;v</mi> <mi>e</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>M</mi> <mn>6</mn> </msub> <msup> <mi>&amp;delta;f</mi> <mi>b</mi> </msup> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow>
其中，为估算的北向速度，为估算的东向速度，vn为真实北向速度，ve为真实东向速度，
3)由1)、2)步中水平欧拉角误差微分方程、速度误差微分方程得到卡尔曼滤波状态方程中状态转移矩阵及系统噪声矩阵相应为
<mrow> <mi>F</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>2</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>2</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>M</mi> <mn>4</mn> </msub> </mtd> <mtd> <msub> <mi>M</mi> <mn>5</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <mi>G</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>M</mi> <mn>6</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>
取1s更新一次的多普勒速度为观测量，构建观测方程如下，
z＝Hx+v (8)
其中，
多普勒坐标系d系轴向沿多普勒测速仪的横滚轴-俯仰轴-偏航轴(前-右-下)；
z为观测向量，H为观测矩阵，v为测量噪声；
卡尔曼滤波观测方程按如下方法构建：
1)构建地理系n系下含有误差的多普勒测速输出与姿态矩阵失准角φ的关系
<mrow> <msubsup> <mover> <mi>v</mi> <mo>~</mo> </mover> <mrow> <mi>e</mi> <mi>d</mi> </mrow> <mi>n</mi> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> <mi>n</mi> </msubsup> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msub> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mo>&amp;lsqb;</mo> <mi>I</mi> <mo>-</mo> <mi>&amp;phi;</mi> <mo>&amp;times;</mo> <mo>&amp;rsqb;</mo> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msub> <msub> <mi>i</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msub> </msubsup> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>i</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msub> </msubsup> <msubsup> <mi>C</mi> <mi>d</mi> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <msubsup> <mi>v</mi> <mrow> <mi>e</mi> <mi>d</mi> </mrow> <mi>d</mi> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;delta;v</mi> <mrow> <mi>e</mi> <mi>d</mi> </mrow> <mi>d</mi> </msubsup> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>
其中，为多普勒测速仪测得的潜航器速度在n系下投影，为多普勒测速仪测得的潜航器速度在d系下的投影，多普勒测速仪测速误差在d系下的投影；
2)构建速度误差观测量z
潜航器惯导解算速度为将潜航器惯导解算速度的北向速度东向速度与多普勒测速仪输出中的北向速度东向速度作差得到速度误差观测量，
<mrow> <mtable> <mtr> <mtd> <mrow> <mi>z</mi> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mover> <mi>v</mi> <mo>~</mo> </mover> <mi>n</mi> </msub> </mtd> <mtd> <msub> <mover> <mi>v</mi> <mo>~</mo> </mover> <mi>e</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>-</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mover> <mi>v</mi> <mo>~</mo> </mover> <mrow> <mi>n</mi> <mi>D</mi> <mi>V</mi> <mi>L</mi> </mrow> </msub> </mtd> <mtd> <msub> <mover> <mi>v</mi> <mo>~</mo> </mover> <mrow> <mi>e</mi> <mi>D</mi> <mi>V</mi> <mi>L</mi> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mo>&amp;lsqb;</mo> <mtable> <mtr> <mtd> <msub> <mi>I</mi> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>2</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> </mtable> <mo>&amp;rsqb;</mo> <mrow> <mo>(</mo> <msubsup> <mover> <mi>v</mi> <mo>~</mo> </mover> <mi>e</mi> <mi>n</mi> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>v</mi> <mo>~</mo> </mover> <mrow> <mi>e</mi> <mi>d</mi> </mrow> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mo>&amp;lsqb;</mo> <mtable> <mtr> <mtd> <msub> <mi>I</mi> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>2</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> </mtable> <mo>&amp;rsqb;</mo> <mrow> <mo>(</mo> <msubsup> <mi>v</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;delta;v</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>-</mo> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> <mi>n</mi> </msubsup> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msub> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mo>&amp;lsqb;</mo> <mi>I</mi> <mo>-</mo> <mi>&amp;phi;</mi> <mo>&amp;times;</mo> <mo>&amp;rsqb;</mo> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msub> <msub> <mi>i</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msub> </msubsup> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>i</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msub> </msubsup> <msubsup> <mi>C</mi> <mi>d</mi> <mi>b</mi> </msubsup> <mo>(</mo> <mrow> <msubsup> <mi>v</mi> <mrow> <mi>e</mi> <mi>d</mi> </mrow> <mi>d</mi> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;delta;v</mi> <mrow> <mi>e</mi> <mi>d</mi> </mrow> <mi>d</mi> </msubsup> </mrow> <mo>)</mo> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mo>&amp;lsqb;</mo> <mtable> <mtr> <mtd> <msub> <mi>I</mi> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>2</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>2</mn> <mo>&amp;times;</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> </mtable> <mo>&amp;rsqb;</mo> <mrow> <mo>(</mo> <msubsup> <mi>&amp;delta;v</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>+</mo> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> <mi>n</mi> </msubsup> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msub> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mo>&amp;lsqb;</mo> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>i</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msub> </msubsup> <msubsup> <mi>C</mi> <mi>d</mi> <mi>b</mi> </msubsup> <msubsup> <mi>v</mi> <mrow> <mi>e</mi> <mi>d</mi> </mrow> <mi>d</mi> </msubsup> <mo>&amp;times;</mo> <mo>&amp;rsqb;</mo> <mi>&amp;phi;</mi> <mo>-</mo> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> <mi>n</mi> </msubsup> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msub> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msub> <msub> <mi>i</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msub> </msubsup> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>i</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msub> </msubsup> <msubsup> <mi>C</mi> <mi>d</mi> <mi>b</mi> </msubsup> <msubsup> <mi>&amp;delta;v</mi> <mrow> <mi>e</mi> <mi>d</mi> </mrow> <mi>d</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;v</mi> <mi>n</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;delta;v</mi> <mi>e</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>M</mi> <mn>7</mn> </msub> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;&amp;theta;</mi> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;delta;&amp;gamma;</mi> <mn>0</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>+</mo> <mi>v</mi> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>
其中，
3)确定观测方程z＝Hx+v中观测矩阵为
H＝[M7 I2×2]；
步骤四：根据卡尔曼滤波状态方程和观测方程进行滤波，对横滚角误差、俯仰角误差、北向速度误差、东向速度误差进行估计，对潜航器的姿态、速度进行修正，完成传递对准。
2.根据权利要求1所述的一种初始方位信息辅助下潜航器快速传递对准方法，其特征在于：
在步骤二中所述的姿态更新方程按如下方法更新：
<mrow> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> <mi>n</mi> </msubsup> <mo>&amp;ap;</mo> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> <mo>+</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mrow> <mo>(</mo> <mi>&amp;lambda;</mi> <mo>-</mo> <msub> <mi>&amp;lambda;</mi> <mn>0</mn> </msub> <mo>+</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>t</mi> <mo>)</mo> </mrow> <mi>sin</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> <mtd> <mrow> <msub> <mi>L</mi> <mn>0</mn> </msub> <mo>-</mo> <mi>L</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>(</mo> <mi>&amp;lambda;</mi> <mo>-</mo> <msub> <mi>&amp;lambda;</mi> <mn>0</mn> </msub> <mo>+</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>t</mi> <mo>)</mo> <mi>sin</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mrow> <mo>(</mo> <mi>&amp;lambda;</mi> <mo>-</mo> <msub> <mi>&amp;lambda;</mi> <mn>0</mn> </msub> <mo>+</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>t</mi> <mo>)</mo> </mrow> <mi>cos</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>L</mi> <mo>-</mo> <msub> <mi>L</mi> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <mi>&amp;lambda;</mi> <mo>-</mo> <msub> <mi>&amp;lambda;</mi> <mn>0</mn> </msub> <mo>+</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>t</mi> <mo>)</mo> <mi>cos</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>
其中，I3×3表示三维单位矩阵，ωie为地球自转角速度，t为初始时刻到当前时刻的解算时间，λ0、L0为潜航器装订的初始经度、纬度，λ、L为当前时刻经度、纬度；
可由潜航器初始经纬度和t确定，将按如下分解，
<mrow> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msub> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <mi>e</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <msub> <mi>e</mi> <mn>0</mn> </msub> </msub> <mi>e</mi> </msubsup> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msub> <msub> <mi>i</mi> <msub> <mi>e</mi> <mn>0</mn> </msub> </msub> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>
其中，
<mrow> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>C</mi> <mn>0</mn> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi> </mi> <msub> <mi>L</mi> <mn>0</mn> </msub> <msub> <mi>cos&amp;lambda;</mi> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi> </mi> <msub> <mi>L</mi> <mn>0</mn> </msub> <msub> <mi>sin&amp;lambda;</mi> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>cosL</mi> <mn>0</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>cos</mi> <mi> </mi> <msub> <mi>L</mi> <mn>0</mn> </msub> <msub> <mi>cos&amp;lambda;</mi> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi> </mi> <msub> <mi>L</mi> <mn>0</mn> </msub> <msub> <mi>sin&amp;lambda;</mi> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>sin&amp;lambda;</mi> <mn>0</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mi>sin&amp;lambda;</mi> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>cos&amp;lambda;</mi> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <msub> <mi>e</mi> <mn>0</mn> </msub> </msub> <mi>e</mi> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <msubsup> <mi>C</mi> <msub> <mi>i</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msub> <msub> <mi>i</mi> <msub> <mi>e</mi> <mn>0</mn> </msub> </msub> </msubsup> <mo>=</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>C</mi> <mi>e</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow>
系由对准初始时刻e系与惯性空间固联得到，系相对惯性空间固定不动；
为地球坐标系e系到对准初始时刻导航系n0系的方向余弦矩阵；
为惯性坐标系系到地球坐标系e系的方向余弦矩阵；
为惯性坐标系系到惯性坐标系系的方向余弦矩阵；
利用陀螺输出数据求解，传递对准完成时间内，忽略陀螺的零偏、随机游走误差，按如下微分方程和初值进行姿态更新求解，即
<mrow> <msubsup> <mover> <mi>C</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>b</mi> <msub> <mi>i</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msub> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>i</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msub> </msubsup> <mo>&amp;lsqb;</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>b</mi> </mrow> <mi>b</mi> </msubsup> <mo>&amp;times;</mo> <mo>&amp;rsqb;</mo> <mo>,</mo> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>i</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msub> </msubsup> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> </mrow>
其中，为载体角速度，为的初值。
CN201510390727.XA 2015-07-06 2015-07-06 一种初始方位信息辅助下潜航器快速传递对准方法 Active CN105021212B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510390727.XA CN105021212B (zh) 2015-07-06 2015-07-06 一种初始方位信息辅助下潜航器快速传递对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510390727.XA CN105021212B (zh) 2015-07-06 2015-07-06 一种初始方位信息辅助下潜航器快速传递对准方法

Publications (2)

Publication Number Publication Date
CN105021212A CN105021212A (zh) 2015-11-04
CN105021212B true CN105021212B (zh) 2017-09-26

Family

ID=54411359

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510390727.XA Active CN105021212B (zh) 2015-07-06 2015-07-06 一种初始方位信息辅助下潜航器快速传递对准方法

Country Status (1)

Country Link
CN (1) CN105021212B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
FR3043469B1 (fr) * 2015-11-10 2019-10-18 Safran Electronics & Defense Procede de detection de mouvements parasites lors d'un alignement statique d'une centrale inertielle, et dispositif de detection associe
CN109460075B (zh) * 2018-11-01 2021-10-01 湖北航天技术研究院总体设计所 一种快速方位角对准的方法及系统
CN109764752B (zh) * 2018-12-20 2021-07-13 北京恒星箭翔科技有限公司 一种单兵制导火箭降低加工精度要求的快速初始对准方法
CN111912405A (zh) * 2019-05-10 2020-11-10 中国人民解放军火箭军工程大学 一种基于车载惯组与多普勒雷达的组合导航方法及系统
CN116295511B (zh) * 2022-12-16 2024-04-02 南京安透可智能系统有限公司 一种用于管道潜航机器人的鲁棒初始对准方法及系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101696883A (zh) * 2009-10-29 2010-04-21 哈尔滨工程大学 光纤陀螺捷联惯性导航系统阻尼方法
CN103047999A (zh) * 2012-12-18 2013-04-17 东南大学 一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法
WO2013059989A1 (zh) * 2011-10-25 2013-05-02 国防科学技术大学 一种惯性导航系统的运动对准方法
CN103245360A (zh) * 2013-04-24 2013-08-14 北京工业大学 晃动基座下的舰载机旋转式捷联惯导系统自对准方法
CN103644913A (zh) * 2013-12-25 2014-03-19 东南大学 基于直接导航模型的无迹卡尔曼非线性初始对准方法
CN104655131A (zh) * 2015-02-06 2015-05-27 东南大学 基于istssrckf的惯性导航初始对准方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101696883A (zh) * 2009-10-29 2010-04-21 哈尔滨工程大学 光纤陀螺捷联惯性导航系统阻尼方法
WO2013059989A1 (zh) * 2011-10-25 2013-05-02 国防科学技术大学 一种惯性导航系统的运动对准方法
CN103047999A (zh) * 2012-12-18 2013-04-17 东南大学 一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法
CN103245360A (zh) * 2013-04-24 2013-08-14 北京工业大学 晃动基座下的舰载机旋转式捷联惯导系统自对准方法
CN103644913A (zh) * 2013-12-25 2014-03-19 东南大学 基于直接导航模型的无迹卡尔曼非线性初始对准方法
CN104655131A (zh) * 2015-02-06 2015-05-27 东南大学 基于istssrckf的惯性导航初始对准方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
A Fast SINS Initial Alignment Scheme for Underwate;Wanli Li等;《The Journal of navigation》;20131231;181-198 *
Rapid transfer alignment for SINS of carrier craft;Jingshou Xu等;《Journal of Systems Engineering and Electronics》;20130430;第24卷(第2期);303-308 *
一种改进的舰载机快速传递对准方法;王勇军等;《中国惯性技术学报》;20141031;第22卷(第5期);597-600 *
基于惯性参考系的动基座初始对准与定位导航;严恭敏等;《系统工程与电子技术》;20110331;第33卷(第03期);618-621 *
水下潜器捷联惯导系统初始对准技术研究;高伟等;《中国航海》;20030930(第03期);5-8 *

Also Published As

Publication number Publication date
CN105021212A (zh) 2015-11-04

Similar Documents

Publication Publication Date Title
CN105021212B (zh) 一种初始方位信息辅助下潜航器快速传递对准方法
CN104457754B (zh) 一种基于sins/lbl紧组合的auv水下导航定位方法
CN103245360B (zh) 晃动基座下的舰载机旋转式捷联惯导系统自对准方法
CN103090867B (zh) 相对地心惯性系旋转的光纤陀螺捷联惯性导航系统误差抑制方法
CN106767793A (zh) 一种基于sins/usbl紧组合的auv水下导航定位方法
CN103900565B (zh) 一种基于差分gps的惯导系统姿态获取方法
CN106507913B (zh) 用于管道测绘的组合定位方法
CN103900611B (zh) 一种惯导天文高精度复合两位置对准及误差标定方法
CN105823481A (zh) 一种基于单天线的gnss-ins车辆定姿方法
CN102519485B (zh) 一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法
CN104236586B (zh) 基于量测失准角的动基座传递对准方法
CN105157701B (zh) 一种水下位置基准布设的方法
CN103542816B (zh) 基于时间延迟补偿的船体变形测量方法
CN107806874B (zh) 一种视觉辅助的捷联惯导极区初始对准方法
CN104215242A (zh) 一种基于横向游移坐标系的极区惯性导航方法
CN105783943A (zh) 一种基于无迹卡尔曼滤波的极区环境下舰船大方位失准角传递对准方法
CN103335654B (zh) 一种行星动力下降段的自主导航方法
CN103792561B (zh) 一种基于gnss通道差分的紧组合降维滤波方法
CN103047999A (zh) 一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法
CN104776847B (zh) 一种适用于水下导航系统单点估计陀螺漂移的方法
CN105953795A (zh) 一种用于航天器表面巡视的导航装置及方法
CN103471614A (zh) 一种基于逆坐标系的极区传递对准方法
CN108426575A (zh) 用地球椭球模型改进的捷联惯导极地横向导航方法
CN102707080B (zh) 一种星敏感器模拟捷联惯导陀螺的方法
CN106017460B (zh) 一种地形辅助惯导紧组合的水下潜器导航定位方法

Legal Events

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