WO2020062791A1 - 一种深海潜航器的sins/dvl水下抗晃动对准方法 - Google Patents

一种深海潜航器的sins/dvl水下抗晃动对准方法 Download PDF

Info

Publication number
WO2020062791A1
WO2020062791A1 PCT/CN2019/077888 CN2019077888W WO2020062791A1 WO 2020062791 A1 WO2020062791 A1 WO 2020062791A1 CN 2019077888 W CN2019077888 W CN 2019077888W WO 2020062791 A1 WO2020062791 A1 WO 2020062791A1
Authority
WO
WIPO (PCT)
Prior art keywords
error
sins
navigation system
dvl
underwater
Prior art date
Application number
PCT/CN2019/077888
Other languages
English (en)
French (fr)
Inventor
陈熙源
王俊玮
杨萍
邵鑫
方琳
Original Assignee
东南大学
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 东南大学 filed Critical 东南大学
Publication of WO2020062791A1 publication Critical patent/WO2020062791A1/zh

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
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • 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

Definitions

  • the invention relates to a SINS / DVL underwater anti-shake alignment method for a deep-sea submarine, and belongs to the field of navigation technology.
  • the ocean is rich in resources and energy.
  • deep-sea submarines play an important role in many aspects, such as monitoring the marine environment and identifying underwater targets.
  • the unique driving and motion performance of deep-sea submersibles makes it a great performance advantage compared to other unmanned underwater vehicles.
  • large-depth, long-distance, long-term underwater navigation requires high accuracy for initial alignment.
  • the complicated underwater currents and the possibility of impacts from fish schools can cause shaking of the base.
  • the initial alignment speed and accuracy of the deep-sea submersibles are reduced, which makes the submersibles inertial navigation
  • the cumulative error of the positioning solution is further increased.
  • the inertial linearity error model cannot guarantee the accuracy of the filtering. Therefore, a more accurate non-linear error model and a non-linear filter are considered to complete the precise alignment process of the underwater DVL assisted inertial guidance. .
  • the purpose of the present invention is to provide a SINS (Stripdown Inertial Navigation System) / DVL (Doppler Velocimeter) underwater anti-shake alignment method for deep-sea submersibles.
  • Alignment and SINS / DVL SVD (Singular Value) Decomposition-based Fuzzy Adaptive Robust CKF (Volume Kalman Filter) Filter Alignment (Fine Alignment)
  • a SINS / DVL underwater anti-shake alignment method for a deep-sea submarine includes the following steps:
  • the specific methods of the angular non-linear error model and the fine alignment filter equation are:
  • the speed and attitude angle errors are defined as:
  • ⁇ n is the projection of the true speed of the submarine in the n system
  • ⁇ b is the constant error of the gyroscope in the carrier system, Is the constant error of the accelerometer under the carrier system;
  • R E is the radius of the ⁇ circle, and R N is the radius of the meridian circle;
  • the scale factor error ⁇ C is described by a random constant, and the ground speed error error ⁇ d and the drift angle error ⁇ are described by a first-order Markov process.
  • ⁇ d and ⁇ ⁇ are the time related to the speed deviation error and the drift angle error, respectively.
  • W d and w ⁇ are zero-mean Gaussian white noises of the speed deviation error and the deviation angle error, respectively;
  • the Euler angle eastward, northward, and skyward platform error angles of the submersible are:
  • the east, north and sky constant errors of the gyro sensor are
  • the east and north constant errors of the accelerometer sensor are ⁇ d is the ground speed error of the underwater DVL navigation system, ⁇ is the drift angle error of the underwater DVL navigation system, and ⁇ C is the scale factor error of the underwater DVL navigation system, which becomes a 15-dimensional state variable:
  • the difference between the SINS solution speed and the DVL measurement speed is selected as the nonlinear filtering observation variable of the SINS / DVL navigation system:
  • ⁇ SINSe and ⁇ SINSn are the navigation solution speeds of the strapdown inertial navigation system ⁇ SINS projections in the east and north directions of the navigation system, and ⁇ e and ⁇ n are the navigation solution speed errors of the strapdown inertial navigation system ⁇ respectively.
  • ⁇ de and ⁇ dn are the projections of the navigation solution speed ⁇ d of the four-beam underwater Doppler navigation system in the east and north directions of the navigation system.
  • ⁇ de and ⁇ dn are respectively The projection of the navigation solution speed error ⁇ d of the four-beam underwater Doppler navigation system in the east and north directions of the navigation system.
  • the underwater anti-shake alignment method for a deep-sea submersible includes the following steps:
  • the components of the integral of the gravity vector in the time period of 0-t 1 and 0-t 2 under the navigation system can be calculated by the above formula.
  • the underwater anti-sloshing alignment method for a deep-sea submersible vehicle includes the following steps:
  • k is the filtering time
  • U j, k-1 is the unitary matrix decomposed by SVD at k-1
  • Q k-1 is the process noise matrix of the navigation system at k-1;
  • K k P xz, k / P zz, k
  • ⁇ is the threshold value of the H ⁇ suboptimal solution, which is related to the robust performance of the filter.
  • the sufficient and necessary conditions for the existence of a solution of the H ⁇ suboptimal problem can be given by Riccatiinequality:
  • the threshold ⁇ fuzzy adaptive algorithm is as follows:
  • the fuzzy adaptive factor ⁇ update formula of the threshold ⁇ is constructed as:
  • Trace (P xz, k ) is a trace operation, that is, the sum of the diagonal elements of the matrix.
  • the invention can overcome the problem that the speed and accuracy of the fixed position caused by the conventional inertial navigation alignment algorithm during the initial alignment of the deep sea submersible under the condition of shaking the base and the large misalignment angle are such that the submersible cannot continue to work normally. Achieve long-hour operation of deep sea submersibles.
  • FIG. 1 is a schematic diagram of a deep-sea submersible navigation system according to an embodiment of the present invention.
  • FIG. 2 is a specific underwater initial alignment flowchart according to an embodiment of the present invention.
  • FIGS. 1 and 2 are a scheme diagram of a deep-sea submersible navigation system and a specific underwater initial alignment flowchart disclosed in the present invention.
  • n system select the navigation coordinate system calculated by the SINS / DVL navigation system as the navigation calculation coordinate system as the n ′ system, and select the upper right front coordinate system of the submarine cabin as the carrier coordinate Line (b line).
  • the speed and attitude angle errors are defined as:
  • ⁇ n is the projection of the true speed of the submarine in the n system
  • Projection of the strapdown inertial navigation system's navigation solution speed in the n system Is the component of the speed error ⁇ n in the northeast sky direction
  • For the projection of the true attitude angle of the submarine in the n series Solving the projection of the attitude angle of the strapdown inertial navigation system under n system Attitude angle error The weight in the northeast direction.
  • the large-angle non-linear error model of the combined system is divided into: SINS nonlinear error model and DVL linear error model.
  • Attitude matrix between n and n ′ systems under shaking base Cannot be ignored.
  • Is the earth's rotation angular velocity Is the rotational angular velocity of the navigation system relative to the Earth system
  • Is the rotational angular velocity of the navigation system relative to the inertial system Is the corresponding amount of error.
  • ⁇ b is the constant error of the gyroscope in the carrier system
  • Is the constant error of the accelerometer under the carrier system R N is the radius of the ⁇ circle
  • R E is the radius of the meridian circle.
  • They are the attitude matrix of the navigation system and the computing system, and the inverse matrix of the Euler angle differential coefficient matrix.
  • the specific matrix forms are as follows:
  • FIG. 3 it is a schematic diagram of a ship with a four-beam DVL Janus configuration.
  • the four-beam Jenner configuration refers to transmitting an acoustic beam obliquely toward the bow, bow, and starboard sides.Due to the symmetry of the four beams, when the cabin of a deep-sea submersible has up and down, left and right fluctuations (rolling) , Pitch) can improve the accuracy of lateral and vertical speed measurement.
  • the four-beam DVL speed measurement expression is:
  • c is the speed at which ultrasonic waves propagate in seawater, which is regarded as a constant value.
  • f 0 is the ultrasonic frequency
  • f d13 and f d24 are the Doppler frequency shifts of the longitudinal x and lateral y.
  • ⁇ is the tilt angle of the ultrasonic beam.
  • the cabin deflection angle can be calculated:
  • ⁇ d is the actual ground speed of the underwater DVL navigation system
  • ⁇ d is the ground speed error of the underwater DVL navigation system
  • K d is the track direction of the uncompensated drift angle ⁇ of the underwater DVL navigation system
  • ⁇ is Deviation angle error of underwater DVL navigation system
  • ⁇ C is the scale factor error of underwater DVL navigation system
  • Unaligned misalignment angle of the underwater DVL navigation system Unaligned misalignment angle of the underwater DVL navigation system.
  • the scale factor ⁇ C is described by a random constant
  • the errors ⁇ d and ⁇ are described by a first-order Markov process
  • ⁇ d and ⁇ ⁇ are the correlation times of the velocity offset error and the drift angle error, respectively.
  • w d and w ⁇ are zero-mean Gaussian white noises of speed deviation error and bias angle error. error.
  • the nonlinear filtering state equation of the SINS / DVL navigation system can be abbreviated as:
  • the difference between the SINS solution speed and the DVL measurement speed is selected as the nonlinear filtering observation variable of the SINS / DVL navigation system:
  • ⁇ SINSe and ⁇ SINSn are the navigation solution speeds of the strapdown inertial navigation system ⁇ SINS projections in the east and north directions of the navigation system, and ⁇ e and ⁇ n are the navigation solution speed errors of the strapdown inertial navigation system.
  • East and North projections, ⁇ de and ⁇ dn are projections of the navigation solution speed of the four-beam underwater Doppler navigation system ⁇ d in the east and north directions of the navigation system, and ⁇ de and ⁇ dn are four-beam underwater The projection speed error ⁇ d of the navigation solution of the Doppler navigation system in the east and north directions of the navigation system
  • the non-linear filtering measurement equation of SINS / DVL navigation system can be abbreviated as:
  • the DSP uses the received fiber-optic inertial guidance three-axis gyroscope and accelerometer signals to perform SINS anti-shake dual vector fixed-position self-alignment (coarse alignment).
  • the interference angular velocity caused by the large-scale shaking under the shaking base is large, and the signal-to-noise of the fiber-optic gyro output is relatively small.
  • the interference acceleration in the gyro output and the earth's rotation angular velocity ⁇ ie cannot be separated, so that the posture cannot be completed by traditional analytical coarse alignment matrix A rough estimate.
  • the earth's rotation angular velocity ⁇ ie is a fixed value.
  • Is the component of the gravity vector in the navigation system Is the component of the gravity vector in the navigation system.
  • the component of the integral of the gravity vector in the time period of 0-t 1 and 0-t 2 in the navigation system can be calculated by the above formula.
  • DSP uses the received optical fiber inertial navigation three-axis gyroscope, accelerometer signals, and four-beam underwater Doppler velocimeter speed signals to perform SINS / DVL fuzzy adaptive robust CKF filter alignment based on SVD decomposition (Fine alignment).
  • DVL speed signals should be used to assist inertial navigation, and then based on the nonlinear error model and fine alignment filter equation established in step 1), SVD decomposition of CKF filtering to complete the fine alignment process and further improve the attitude matrix The accuracy.
  • the process of fuzzy adaptive robust CKF filter based on SVD decomposition is as follows:
  • K k P xz, k / P zz, k
  • ⁇ is the threshold of H ⁇ suboptimal solution, which is related to the robust performance of the filter. If the threshold ⁇ can be adaptive to different water environments, the attitude matrix can be made on the basis of ensuring robustness More accurate, to achieve a quasi-determined attitude of deep-sea submersibles.
  • the threshold ⁇ fuzzy adaptive algorithm is as follows:
  • ⁇ represents the spectral radius of the matrix.
  • ⁇ is the fuzzy adaptive factor.
  • the influence of system uncertainty will cause abnormal observations, which will cause the filter to malfunction. Such aberrations cause changes in the statistical characteristics of the innovation sequence.
  • Trace (P xz, k ) is a trace operation, that is, the sum of the diagonal elements of the matrix.

Abstract

一种深海潜航器的SINS(捷联惯导系统)/DVL(多普勒测速仪)水下抗晃动对准方法。根据水下复杂环境和捷联惯导、四波束水下多普勒导航系统特点所建立的大失准角非线性误差模型以及精对准滤波方程，将深海潜航器的水下对准过程分为：SINS抗晃动双矢量定姿自对准和SINS/DVL的基于SVD(奇异值)分解的模糊自适应鲁棒CKF(容积卡尔曼滤波)滤波器对准；在保证鲁棒性的基础上，进一步使姿态失准角减小、使得姿态矩阵更为精确，实现深海潜航器的准确定姿。

Description

一种深海潜航器的SINS/DVL水下抗晃动对准方法 技术领域：
本发明涉及一种深海潜航器的SINS/DVL水下抗晃动对准方法，属于导航技术领域。
背景技术：
海洋有着丰富的资源和能源。在开发过程中，深海潜航器在诸多方面起着重要作用，如海洋环境的监测、水下目标的识别。而深海潜航器所具有的独特驱动和运动性能，使得其与其他无人水下航行器相比具有较大的性能优势。但大深度、远距离、长时间的水下航行对初始对准的精度要求高。然而水下复杂的洋流涌动环境以及鱼群冲击的可能性，会造成晃动基座，大失准角情况下，深海潜航器的初始对准速度、精度降低,从而使得潜航器惯导水下定位解算的累积误差进一步增大。
因此如何保证深海潜航器的快速精确定姿，成为制约深海潜航器长航时工作的重要因素。
同时由于晃动基座的特殊性，惯导线性误差模型无法保证滤波的精度满足要求，因此考虑采用更为精准的非线性误差模型和非线性滤波器完成水下DVL辅助惯导的精对准过程。
发明内容
本发明的目的是提供一种深海潜航器的SINS(捷联惯性导航系统)/DVL(多普勒测速仪)水下抗晃动对准方法，通过SINS抗晃动双矢量定姿自对准(粗对准)和SINS/DVL的基于SVD(奇异值)分解的模糊自适应鲁棒CKF(容积卡尔曼滤波)滤波器对准(精对准)来保证深海潜航器准确定姿的鲁棒性和快速性，以克服晃动基座，大失准角条件下的深海潜航器在初始对准时，传统惯导对准算法所导致的定姿速度、精度降低以至于潜航器无法继续正常工作的问题，最终实现深海潜航器的长航时工作。
上述的目的通过以下技术方案实现：
一种深海潜航器的SINS/DVL水下抗晃动对准方法，该方法包括如下步骤：
(1)根据水下复杂环境和捷联惯导、四波束水下多普勒导航系统特点所建立的大失准角非线性误差模型以及精对准滤波方程，将深海潜航器的水下对准过程分为：SINS抗晃动双矢量定姿自对准和SINS/DVL的基于SVD分解的模糊自适应鲁棒CKF滤波器对准；
(2)SINS抗晃动双矢量定姿自对准在选定重力矢量为主参考矢量的前提下，预先对参与姿态解算的矢量作单位正交化处理；
(3)SINS/DVL的基于SVD分解的模糊自适应鲁棒CKF滤波器对准则在保证鲁棒性的基础上，进一步使姿态失准角减小、使得姿态矩阵
Figure PCTCN2019077888-appb-000001
更为精确，实现深海潜航器的准确定姿。
所述的深海潜航器的水下抗晃动对准方法，步骤(1)中所述的根据水下复杂环境和捷联惯导、四波束水下多普勒导航系统特点所建立的大失准角非线性误差模型以及精对准滤波方程的具体方法是：
选取东北天地理坐标系作为导航坐标系，记为n系，选取SINS/DVL导航系统解算的导航坐标系作为导航计算坐标系为n′系，选取潜航器舱体的右前上坐标系作为载体坐标系，记为b系，
定义速度和姿态角误差为：
Figure PCTCN2019077888-appb-000002
Figure PCTCN2019077888-appb-000003
其中，
ν n为潜航器的真实速度在n系下的投影，
Figure PCTCN2019077888-appb-000004
为捷联惯导系统的导航解算速度在n系下的投影，
Figure PCTCN2019077888-appb-000005
为速度误差δν n在东北天方向上的分量，
Figure PCTCN2019077888-appb-000006
为潜航器的真实姿态角在n系下的投影，
Figure PCTCN2019077888-appb-000007
为捷联惯导系统的导航解算姿态角在n系下的投影，
Figure PCTCN2019077888-appb-000008
为姿态角误差
Figure PCTCN2019077888-appb-000009
在东北天方向上的分量，
晃动基座条件下，n系与n′系间的姿态矩阵
Figure PCTCN2019077888-appb-000010
不可忽略，组合系统大失准角非线性误差模型分为：SINS非线性误差模型和DVL线性误差模型：
1)建立SINS非线性误差模型：
SINS速度误差方程：
Figure PCTCN2019077888-appb-000011
SINS姿态误差方程：
Figure PCTCN2019077888-appb-000012
SINS位置误差方程：
Figure PCTCN2019077888-appb-000013
其中，
Figure PCTCN2019077888-appb-000014
为实际的加速度计输出；
Figure PCTCN2019077888-appb-000015
为地球自转角速度，
Figure PCTCN2019077888-appb-000016
为地球自转角速度的误差量，
Figure PCTCN2019077888-appb-000017
为导航系相对地球系的旋转角速度，
Figure PCTCN2019077888-appb-000018
为导航系相对地球系的旋转角速度的误差量；
Figure PCTCN2019077888-appb-000019
为导航系相对惯性系的旋转角速度，
Figure PCTCN2019077888-appb-000020
为导航系相对惯性系的旋转角速度的误差量；
ε b为陀螺仪在载体系下的常值误差，
Figure PCTCN2019077888-appb-000021
为加速度计在载体系下的常值误差；
R E为卯酉圈半径，R N为子午圈半径；
Figure PCTCN2019077888-appb-000022
分别为导航系与计算系的姿态矩阵以及欧拉角微分系数矩阵的逆矩阵，具体矩阵形式如下：
Figure PCTCN2019077888-appb-000023
Figure PCTCN2019077888-appb-000024
2)建立DVL线性误差模型：
Figure PCTCN2019077888-appb-000025
其中，刻度因数误差δC用随机常数描述，对地速度误差误差δν d、偏流角误差δΔ用一阶马尔可夫过程描述，τ d、τ Δ分别为速度偏移误差和偏流角误差的相关时间，w d、w Δ分别为速度偏移误差和偏流角误差的零均值高斯白噪声；
3)建立精对准滤波方程：由于SINS/DVL组合系统的天向通道发散，因此忽略天向通道状态量，从而选取潜航器的纬度位置误差δL、经度位置误差δλ，潜航器的东向速度误差δν e、北向速度误差δν n，潜航器的欧拉角东向、北向、天向平台误差角分别为
Figure PCTCN2019077888-appb-000026
陀螺仪传感器的东向、北向、天向常值误差分别为
Figure PCTCN2019077888-appb-000027
加速度计传感器的东向、北向常值误差分别为
Figure PCTCN2019077888-appb-000028
δν d为水下DVL导航系统的对地速度误差，δΔ为水下DVL导航系统的偏流角误差，δC为水下DVL导航系统的刻度因数误差，成为15维状态变量：
Figure PCTCN2019077888-appb-000029
SINS/DVL非线性滤波状态方程为：
Figure PCTCN2019077888-appb-000030
其中，
Figure PCTCN2019077888-appb-000031
为从载体系到计算系的姿态转换矩阵，
Figure PCTCN2019077888-appb-000032
为从计算系到导航系的姿态转换矩阵；
SINS/DVL导航系统的非线性滤波状态方程简记为：
Figure PCTCN2019077888-appb-000033
同时，选取SINS解算速度和DVL测量速度之差作为SINS/DVL导航系统的非线性滤波观测变量：
Figure PCTCN2019077888-appb-000034
其中，ν SINSe、ν SINSn分别为捷联惯导系统的导航解算速度ν SINS在导航系东向和北向的投影，δν e、δν n分别为捷联惯导系统的导航解算速度误差δν在导航系东向和北向的投影，ν de、ν dn分别为四波束水下多 普勒导航系统的导航解算速度ν d在导航系东向和北向的投影，δν de、δν dn分别为四波束水下多普勒导航系统的导航解算速度误差δν d在导航系东向和北向的投影。
SINS/DVL导航系统的非线性滤波量测方程简记为：
Z＝h(X,t)+v(t)。
所述的深海潜航器的水下抗晃动对准方法，步骤(1)中所述的SINS抗晃动双矢量定姿自对准，包括以下步骤：
为抑制舱体横摇、纵摇产生的干扰线振动加速度
Figure PCTCN2019077888-appb-000035
杆臂加速度
Figure PCTCN2019077888-appb-000036
以及补偿零位误差
Figure PCTCN2019077888-appb-000037
选取0-t 1、0-t 2时间段的重力矢量的积分v t1、v t2作为参考双矢量，
0-t时间段内重力在导航系(n系)积分为：
Figure PCTCN2019077888-appb-000038
其中，
Figure PCTCN2019077888-appb-000039
为0-τ时间段导航系的变化矩阵，
Figure PCTCN2019077888-appb-000040
为τ时刻重力矢量在导航系下的分量。
Figure PCTCN2019077888-appb-000041
为0-t 1、0-t 2时间段的重力矢量的积分在导航系下的分量，可由上式计算得到，
0-t时间段内重力在载体系(b系)下的积分为：
Figure PCTCN2019077888-appb-000042
其中，
Figure PCTCN2019077888-appb-000043
为载体系变化矩阵，
Figure PCTCN2019077888-appb-000044
为重力矢量在载体系下的分量；
粗对准过程中，加速度计输出比力
Figure PCTCN2019077888-appb-000045
由重力矢量g b、线振动干扰加速度
Figure PCTCN2019077888-appb-000046
杆臂加速度
Figure PCTCN2019077888-appb-000047
以及零位误差
Figure PCTCN2019077888-appb-000048
构成：
Figure PCTCN2019077888-appb-000049
经过简化
Figure PCTCN2019077888-appb-000050
为：
Figure PCTCN2019077888-appb-000051
Figure PCTCN2019077888-appb-000052
根据双矢量定姿原理，由
Figure PCTCN2019077888-appb-000053
不难得到晃动基座下t＝0时刻姿态矩阵
Figure PCTCN2019077888-appb-000054
Figure PCTCN2019077888-appb-000055
所述的深海潜航器的水下抗晃动对准方法，步骤(1)中所述的SINS/DVL的基于SVD分解的模糊自适应鲁棒CKF滤波器对准，包括以下步骤：
1)计算基本容积点和相应权值：
Figure PCTCN2019077888-appb-000056
式中，m表示容积点总数(m＝2num)，num为CKF滤波器的状态维数，[1]表示对num维单位向量e＝[1,0,..,0] T的全排列和改变元素符号产生的点集；
2)时间更新：
①基于SVD分解计算容积点X j,k-1
Figure PCTCN2019077888-appb-000057
其中，k为滤波时刻，U j,k-1为k-1时刻SVD分解出的酉阵，s j(j＝1,2,..,num)为k-1时刻滤波器输出的最优滤波估计协方差P k-1|k-1的特征值的平方根，
Figure PCTCN2019077888-appb-000058
为k-1时刻滤波器输出的最优状态估计；
②根据权利2所述的非线性状态方程计算传播容积点
Figure PCTCN2019077888-appb-000059
Figure PCTCN2019077888-appb-000060
③计算状态量
Figure PCTCN2019077888-appb-000061
和状态预测P k|k-1
Figure PCTCN2019077888-appb-000062
其中Q k-1为k-1时刻的导航系统过程噪声矩阵；
3)量测更新：
①基于SVD分解计算容积点X j,k|k-1
Figure PCTCN2019077888-appb-000063
Figure PCTCN2019077888-appb-000064
②通过根据权利2所述的非线性量测方程计算传播容积点Z j,k
Z j,k＝h(X j,k|k-1,t)
③计算量测量
Figure PCTCN2019077888-appb-000065
量测协方差P zz,k以及新息协方差估计P xz,k
Figure PCTCN2019077888-appb-000066
④计算增益阵K k、更新最优状态估计
Figure PCTCN2019077888-appb-000067
以及最优估计协方差：
K k＝P xz,k/P zz,k
Figure PCTCN2019077888-appb-000068
为在晃动基座下，使精对准过程具有一定鲁棒性，基于H∞滤波器的相关原理，对传统CKF的最优估计协方差进行改写：
Figure PCTCN2019077888-appb-000069
其中，γ为H∞次优解的阈值，与滤波器的鲁棒性能有关，H∞次优问题存在解的充分必要条件可由黎卡提不等式(Riccati inequality)给出:
Figure PCTCN2019077888-appb-000070
阈值γ模糊自适应算法如下：
γ＝η·γ a
其中，
Figure PCTCN2019077888-appb-000071
且ρ()表示矩阵的谱半径，η为模糊自适应因子，
从新息序列的统计特性的变化入手，构造出了阈值γ的模糊自适应因子η更新式为：
Figure PCTCN2019077888-appb-000072
其中，
Figure PCTCN2019077888-appb-000073
为新息序列的平方和。Trace(P xz,k)为求迹运算，即求矩阵对角线元素之和。
有益效果：
本发明可以克服晃动基座，大失准角条件下的深海潜航器在初始对准时，传统惯导对准算法所导致的定姿速度、精度降低以至于潜航器无法继续正常工作的问题，最终实现深海潜航器的长航时工作。
针对水下复杂的洋流涌动环境以及鱼群冲击潜航器的可能性，会造成潜航器晃动基座，大失准角情况 下，深海潜航器的初始对准速度、精度降低,从而使得潜航器惯导水下定位解算的累积误差进一步增大。根据水下复杂环境和捷联惯导、四波束水下多普勒导航系统特点所建立了大失准角非线性误差模型以及精对准滤波方程，通过SINS抗晃动双矢量定姿自对准和SINS/DVL的基于SVD分解的模糊自适应鲁棒CKF滤波器对准。在保证水下潜航器导航系统鲁棒性的基础上，进一步使姿态失准角减小、使得姿态矩阵
Figure PCTCN2019077888-appb-000074
更为精确，实现深海潜航器的准确定姿。
附图说明
图1是本发明实施例的深海潜航器导航系统方案图
图2是本发明实施例的具体水下初始对准流程图。
具体实施方式
下面结合具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
如图1、2所示，是本发明公开的深海潜航器导航系统方案图和具体水下初始对准流程图。
具体实施步骤如下：
1)在对准之前根据水下复杂环境和SINS、四波束DVL导航系统特点，分析SINS/DVL导航定位原理，再建立大失准角非线性误差模型以及精对准滤波方程。
选取东北天地理坐标系作为导航坐标系(n系)，选取SINS/DVL导航系统解算的导航坐标系作为导航计算坐标系为n′系，选取潜航器舱体的右前上坐标系作为载体坐标系(b系)。
定义速度和姿态角误差为：
Figure PCTCN2019077888-appb-000075
Figure PCTCN2019077888-appb-000076
其中，ν n为潜航器的真实速度在n系下的投影、
Figure PCTCN2019077888-appb-000077
为捷联惯导系统的导航解算速度在n系下的投影，
Figure PCTCN2019077888-appb-000078
为速度误差δν n在东北天方向上的分量，
Figure PCTCN2019077888-appb-000079
为潜航器的真实姿态角在n系下的投影、
Figure PCTCN2019077888-appb-000080
为捷联惯导系统的导航解算姿态角在n系下的投影
Figure PCTCN2019077888-appb-000081
为姿态角误差
Figure PCTCN2019077888-appb-000082
在东北天方向上的分量。
组合系统大失准角非线性误差模型分为：SINS非线性误差模型和DVL线性误差模型。晃动基座条件下，n系与n′系间的姿态矩阵
Figure PCTCN2019077888-appb-000083
不可忽略。
①建立SINS非线性误差模型：
速度误差方程：
Figure PCTCN2019077888-appb-000084
姿态误差方程：
Figure PCTCN2019077888-appb-000085
位置误差方程：
Figure PCTCN2019077888-appb-000086
其中，
Figure PCTCN2019077888-appb-000087
为实际的加速度计输出。
Figure PCTCN2019077888-appb-000088
为地球自转角速度，
Figure PCTCN2019077888-appb-000089
为导航系相对地球系的旋转角速度，
Figure PCTCN2019077888-appb-000090
为导航系相对惯性系的旋转角速度，
Figure PCTCN2019077888-appb-000091
为相应的误差量。ε b为陀螺仪在载体系下的常值误差，
Figure PCTCN2019077888-appb-000092
为加速度计在载体系下的常值误差，R N为卯酉圈半径，R E为子午圈半径。
Figure PCTCN2019077888-appb-000093
分别为导航系与计算系的姿态矩阵以及欧拉角微分系数矩阵的逆矩阵，具体矩阵形式如下：
Figure PCTCN2019077888-appb-000094
Figure PCTCN2019077888-appb-000095
如图3所示，是四波束DVL詹纳斯(Janus)配置的船舶示意图。其中四波束詹纳斯配置指的是向船艏、船艉以及左右舷方向分别斜着发射一个声波束，由于四波束的对称性，当深海潜航器的舱体有上下、左右波动(横摇、纵摇)时，能够提高横向、纵向的速度测量精度。
四波束DVL速度测量表达式为：
Figure PCTCN2019077888-appb-000096
其中，c为超声波在海水中传播的速度，视为常值。f 0为超声波频率，f d13、f d24为纵向x和横向y的多普勒频移。α为超声波波束倾角。
根据多普勒频移f d13、f d24的可计算出舱体偏流角：
Figure PCTCN2019077888-appb-000097
将实际误差项考虑其中后，DVL测速方程为:
Figure PCTCN2019077888-appb-000098
其中，ν d为水下DVL导航系统的实际对地速度，δν d为水下DVL导航系统的对地速度误差，K d为水下DVL导航系统未补偿偏流角Δ的航迹向，δΔ为水下DVL导航系统的偏流角误差，δC为水下DVL导航系统的刻度因数误差，
Figure PCTCN2019077888-appb-000099
为水下DVL导航系统的未方位失准角。
②建立DVL线性误差模型：
Figure PCTCN2019077888-appb-000100
其中，刻度因数δC用随机常数描述，误差δν d、δΔ用一阶马尔可夫过程描述，τ d、τ Δ分别为速度偏移误差和偏流角误差的相关时间。w d、w Δ为速度偏移误差和偏流角误差的零均值高斯白噪声。误差。
③建立精对准滤波方程。由于SINS/DVL组合系统的天向通道发散，因此忽略天向通道状态量，从而选取潜航器的位置误差δL、δλ，潜航器的速度误差δν e、δν n，潜航器的欧拉角平台误差角
Figure PCTCN2019077888-appb-000101
Figure PCTCN2019077888-appb-000102
陀螺仪传感器的常值误差
Figure PCTCN2019077888-appb-000103
加速度计传感器的常值误差
Figure PCTCN2019077888-appb-000104
δν d为水下DVL导航系统的对地速度误差，δΔ为水下DVL导航系统的偏流角误差，δC为水下DVL导航系统的刻度因数误差，成为15维状态变量：
Figure PCTCN2019077888-appb-000105
SINS/DVL非线性滤波状态方程为：
Figure PCTCN2019077888-appb-000106
其中，
Figure PCTCN2019077888-appb-000107
为从载体系到计算系的姿态转换矩阵，
Figure PCTCN2019077888-appb-000108
为从计算系到导航系的姿态转换矩阵；
SINS/DVL导航系统的非线性滤波状态方程可简记为：
Figure PCTCN2019077888-appb-000109
同时，选取SINS解算速度和DVL测量速度之差作为SINS/DVL导航系统的非线性滤波观测变量：
Figure PCTCN2019077888-appb-000110
其中，ν SINSe、ν SINSn为捷联惯导系统的导航解算速度ν SINS在导航系东向和北向的投影，δν e、δν n为捷联惯导系统的导航解算速度误差δν在导航系东向和北向的投影，ν de、ν dn为四波束水下多普勒导航系统的导航解算速度ν d在导航系东向和北向的投影，δν de、δν dn为四波束水下多普勒导航系统的导航解算速度误差δν d在导航系东向和北向的投影。
SINS/DVL导航系统的非线性滤波量测方程可简记为：
Z＝h(X,t)+v(t)
2)DSP利用接收到的光纤惯导三轴陀螺仪、加速度计信号，进行SINS抗晃动双矢量定姿自对准(粗对准)。
晃动基座下大幅度晃动产生的干扰角速度较大，且光纤陀螺输出的信噪比较小，无法分离陀螺输出中的干扰加速度和地球自转角速度ω ie，从而无法通过传统解析粗对准完成姿态矩阵
Figure PCTCN2019077888-appb-000111
的粗略估计。但是地球自转角速度ω ie是一固定值，当舱体姿态在晃动基座下随时间变化时，使得惯性空间内重力矢量g方向的改变能够包含地球北向信息。因此选用不同时刻t 1、t 2(0＜t 1＜t 2)的两个重力相关矢量(不相互平行)作为参考双矢量，即可以实现晃动基座下t＝0时刻姿态矩阵
Figure PCTCN2019077888-appb-000112
的粗略估计。
为抑制舱体横摇、纵摇产生的干扰线振动加速度
Figure PCTCN2019077888-appb-000113
杆臂加速度
Figure PCTCN2019077888-appb-000114
以及补偿零位误差
Figure PCTCN2019077888-appb-000115
选取0-t 1、0-t 2时间段的重力矢量的积分v t1、v t2作为参考双矢量。
0-t时间段内重力在导航系(n系)积分为：
Figure PCTCN2019077888-appb-000116
其中，
Figure PCTCN2019077888-appb-000117
为导航系变化矩阵，
Figure PCTCN2019077888-appb-000118
为重力矢量在导航系下的分量。
Figure PCTCN2019077888-appb-000119
为0-t 1、0-t 2时间段的重力矢量的积分在导航系下的分量，可由上式计算得到。
0-t时间段内重力在载体系(b系)下的积分为：
Figure PCTCN2019077888-appb-000120
其中，
Figure PCTCN2019077888-appb-000121
为载体系变化矩阵，
Figure PCTCN2019077888-appb-000122
为重力矢量在载体系下的分量。
粗对准过程中，加速度计输出比力
Figure PCTCN2019077888-appb-000123
由重力矢量g b、线振动干扰加速度
Figure PCTCN2019077888-appb-000124
杆臂加速度
Figure PCTCN2019077888-appb-000125
以及零位误差
Figure PCTCN2019077888-appb-000126
构成：
Figure PCTCN2019077888-appb-000127
将上式代入重力积分式，得:
Figure PCTCN2019077888-appb-000128
Figure PCTCN2019077888-appb-000129
其中，
Figure PCTCN2019077888-appb-000130
分别为线振动干扰速度和零位误差速度，在粗对准过程中可以近似忽略。
Figure PCTCN2019077888-appb-000131
为杆臂速度，若已知杆臂长度r b，则：
Figure PCTCN2019077888-appb-000132
因此
Figure PCTCN2019077888-appb-000133
可以简化为：
Figure PCTCN2019077888-appb-000134
Figure PCTCN2019077888-appb-000135
根据双矢量定姿原理，由
Figure PCTCN2019077888-appb-000136
不难得到晃动基座下t＝0时刻姿态矩阵
Figure PCTCN2019077888-appb-000137
Figure PCTCN2019077888-appb-000138
3)DSP利用接收到的光纤惯导三轴陀螺仪、加速度计信号，四波束水下多普勒测速仪速度信号，进行SINS/DVL的基于SVD分解的模糊自适应鲁棒CKF滤波器对准(精对准)。
大深度、远距离、长时间的水下航行对初始对准的精度要求非常高，需要利用DVL速度信号辅助惯导，再根据步骤1)建立的非线性误差模型和精对准滤波方程进行基于SVD分解的CKF滤波，完成精对准过程，进一步提高姿态矩阵
Figure PCTCN2019077888-appb-000139
的精度。基于SVD分解的模糊自适应鲁棒CKF滤波器过程如下：
①计算基本容积点和相应权值：
Figure PCTCN2019077888-appb-000140
式中，m表示容积点总数(m＝2num)，num为CKF滤波器的状态维数。[1]表示对num维单位向量e＝[1,0,..,0] T的全排列和改变元素符号产生的点集。
②时间更新：
a)基于SVD分解计算容积点X j,k-1
Figure PCTCN2019077888-appb-000141
其中，S＝diag(s 1,s 2,...,s num)为对角阵。由于最优滤波估计协方差P k-1|k-1通常为对称阵，因此其特征值为
Figure PCTCN2019077888-appb-000142
且U＝V。最优滤波估计协方差P k-1|k-1可改写为：
Figure PCTCN2019077888-appb-000143
Figure PCTCN2019077888-appb-000144
b)通过步骤1)建立的非线性状态方程计算传播容积点
Figure PCTCN2019077888-appb-000145
Figure PCTCN2019077888-appb-000146
c)计算状态量
Figure PCTCN2019077888-appb-000147
和状态预测P k|k-1
Figure PCTCN2019077888-appb-000148
③量测更新
a)基于SVD分解计算容积点X j,k|k-1
Figure PCTCN2019077888-appb-000149
Figure PCTCN2019077888-appb-000150
b)通过步骤1)建立的非线性量测方程计算传播容积点Z j,k
Z j,k＝h(X j,k|k-1,t)
c)计算量测量
Figure PCTCN2019077888-appb-000151
量测协方差P zz,k以及新息协方差估计P xz,k
Figure PCTCN2019077888-appb-000152
d)计算增益阵K k、更新最优状态估计
Figure PCTCN2019077888-appb-000153
以及最优估计协方差
K k＝P xz,k/P zz,k
Figure PCTCN2019077888-appb-000154
为在晃动基座下，使精对准过程具有一定鲁棒性。基于H∞滤波器的相关原理，对传统CKF的最优估计协方差进行改写：
Figure PCTCN2019077888-appb-000155
其中，γ为H∞次优解的阈值，与滤波器的鲁棒性能有关。如果能够使得阈值γ自适应于不同水域环境，则能够在保证鲁棒性的基础上，使得姿态矩阵
Figure PCTCN2019077888-appb-000156
更为精确，实现深海潜航器的准确定姿。
H∞次优问题存在解的充分必要条件可由黎卡提不等式(Riccati inequality)给出:
Figure PCTCN2019077888-appb-000157
阈值γ模糊自适应算法如下：
γ＝η·γ a
其中，
Figure PCTCN2019077888-appb-000158
且ρ()表示矩阵的谱半径。η为模糊自适应因子。
在实际系统中，系统不确定性影响会导致观测量的异常，从而引起滤波器的失常。这样的失常会引起新息序列的统计特性的变化。理想情况下CKF的新息协方差阵为V k＝N(0,P xz,k)，因此模糊自适应因子η更新式为：
Figure PCTCN2019077888-appb-000159
其中，
Figure PCTCN2019077888-appb-000160
为新息序列的平方和。Trace(P xz,k)为求迹运算，即求矩阵对角线元素之和。

Claims (3)

  1. 一种深海潜航器的SINS/DVL水下抗晃动对准方法，其特征在于：该方法包括如下步骤：
    (1)根据水下复杂环境和捷联惯导、四波束水下多普勒导航系统特点所建立的大失准角非线性误差模型以及精对准滤波方程，将深海潜航器的水下对准过程分为：SINS抗晃动双矢量定姿自对准和SINS/DVL的基于SVD分解的模糊自适应鲁棒CKF滤波器对准；
    (2)SINS抗晃动双矢量定姿自对准在选定重力矢量为主参考矢量的前提下，预先对参与姿态解算的矢量作单位正交化处理；
    (3)SINS/DVL的基于SVD分解的模糊自适应鲁棒CKF滤波器对准则在保证鲁棒性的基础上，进一步使姿态失准角减小、使得姿态矩阵
    Figure PCTCN2019077888-appb-100001
    更为精确，实现深海潜航器的准确定姿。
  2. 根据权利要求1所述的深海潜航器的水下抗晃动对准方法，其特征在于：步骤(1)中所述的根据水下复杂环境和捷联惯导、四波束水下多普勒导航系统特点所建立的大失准角非线性误差模型以及精对准滤波方程的具体方法是：
    选取东北天地理坐标系作为导航坐标系，记为n系，选取SINS/DVL导航系统解算的导航坐标系作为计算坐标系为n′系，选取潜航器舱体的右前上坐标系作为载体坐标系，记为b系，
    定义速度和姿态角误差为：
    Figure PCTCN2019077888-appb-100002
    Figure PCTCN2019077888-appb-100003
    其中，
    ν n为潜航器的真实速度在n系下的投影，
    Figure PCTCN2019077888-appb-100004
    为捷联惯导系统的导航解算速度在n系下的投影，
    Figure PCTCN2019077888-appb-100005
    为速度误差δν n在东北天方向上的分量，
    Figure PCTCN2019077888-appb-100006
    为潜航器的真实姿态角在n系下的投影，
    Figure PCTCN2019077888-appb-100007
    为捷联惯导系统的导航解算姿态角在n系下的投影，
    Figure PCTCN2019077888-appb-100008
    为姿态角误差
    Figure PCTCN2019077888-appb-100009
    在东北天方向上的分量，
    晃动基座条件下，n系与n′系间的姿态矩阵
    Figure PCTCN2019077888-appb-100010
    不可忽略，组合系统大失准角非线性误差模型分为：SINS非线性误差模型和DVL线性误差模型：
    1)建立SINS非线性误差模型：
    SINS速度误差方程：
    Figure PCTCN2019077888-appb-100011
    SINS姿态误差方程：
    Figure PCTCN2019077888-appb-100012
    SINS位置误差方程：
    Figure PCTCN2019077888-appb-100013
    其中，
    Figure PCTCN2019077888-appb-100014
    为实际的加速度计输出；
    Figure PCTCN2019077888-appb-100015
    为地球自转角速度，
    Figure PCTCN2019077888-appb-100016
    为地球自转角速度的误差量，
    Figure PCTCN2019077888-appb-100017
    为导航系相对地球系的旋转角速度，
    Figure PCTCN2019077888-appb-100018
    为导航系相对地球系的旋转角速度的误差量；
    Figure PCTCN2019077888-appb-100019
    为导航系相对惯性系的旋转角速度，
    Figure PCTCN2019077888-appb-100020
    为导航系相对惯性系的旋转角速度的误差量；
    ε b为陀螺仪在载体系下的常值误差，
    Figure PCTCN2019077888-appb-100021
    为加速度计在载体系下的常值误差；
    R E为卯酉圈半径，R N为子午圈半径；
    Figure PCTCN2019077888-appb-100022
    分别为导航系与计算系的姿态矩阵以及欧拉角微分系数矩阵的逆矩阵，具体矩阵形式如下：
    Figure PCTCN2019077888-appb-100023
    Figure PCTCN2019077888-appb-100024
    2)建立DVL线性误差模型：
    Figure PCTCN2019077888-appb-100025
    其中，刻度因数误差δC用随机常数描述，对地速度误差误差δν d、偏流角误差δΔ用一阶马尔可夫过程描述，τ d、τ Δ分别为速度偏移误差和偏流角误差的相关时间，w d、w Δ分别为速度偏移误差和偏流角误差的零均值高斯白噪声；
    3)建立精对准滤波方程：由于SINS/DVL组合系统的天向通道发散，因此忽略天向通道状态量，从而选取潜航器的纬度位置误差δL、经度位置误差δλ，潜航器的东向速度误差δν e、北向速度误差δν n，潜航器的欧拉角东向、北向、天向平台误差角分别为
    Figure PCTCN2019077888-appb-100026
    陀螺仪传感器的东向、北向、天向常值误差分别为
    Figure PCTCN2019077888-appb-100027
    Figure PCTCN2019077888-appb-100028
    加速度计传感器的东向、北向常值误差分别为
    Figure PCTCN2019077888-appb-100029
    δν d为水下DVL导航系统的对地速度误差，δΔ为水下DVL导航系统的偏流角误差，δC为水下DVL导航系统的刻度因数误差，成为15维状态变量：
    Figure PCTCN2019077888-appb-100030
    SINS/DVL非线性滤波状态方程为：
    Figure PCTCN2019077888-appb-100031
    其中，
    Figure PCTCN2019077888-appb-100032
    为从载体系到计算系的姿态转换矩阵，
    Figure PCTCN2019077888-appb-100033
    为从计算系到导航系的姿态转换矩阵；
    SINS/DVL导航系统的非线性滤波状态方程简记为：
    Figure PCTCN2019077888-appb-100034
    同时，选取SINS解算速度和DVL测量速度之差作为SINS/DVL导航系统的非线性滤波观测变量：
    Figure PCTCN2019077888-appb-100035
    其中，ν SINSe、ν SINSn分别为捷联惯导系统的导航解算速度ν SINS在导航系东向和北向的投影，δν e、δν n分别为捷联惯导系统的导航解算速度误差δν在导航系东向和北向的投影，ν de、ν dn分别为四波束水下多普勒导航系统的导航解算速度ν d在导航系东向和北向的投影，δν de、δν dn分别为四波束水下多普勒导航系统的导航解算速度误差δν d在导航系东向和北向的投影。
    SINS/DVL导航系统的非线性滤波量测方程简记为：
    Z＝h(X,t)+v(t)。
    所述的深海潜航器的水下抗晃动对准方法，步骤(1)中所述的SINS抗晃动双矢量定姿自对准，包括以下步骤：
    为抑制舱体横摇、纵摇产生的干扰线振动加速度
    Figure PCTCN2019077888-appb-100036
    杆臂加速度
    Figure PCTCN2019077888-appb-100037
    以及补偿零位误差
    Figure PCTCN2019077888-appb-100038
    选取0-t 1、0-t 2时间段的重力矢量的积分v t1、v t2作为参考双矢量，
    0-t时间段内重力在导航系(n系)积分为：
    Figure PCTCN2019077888-appb-100039
    其中，
    Figure PCTCN2019077888-appb-100040
    为0-τ时间段导航系的变化矩阵，
    Figure PCTCN2019077888-appb-100041
    为τ时刻重力矢量在导航系下的分量。
    Figure PCTCN2019077888-appb-100042
    为0-t 1、0-t 2时间段的重力矢量的积分在导航系下的分量，可由上式计算得到，
    0-t时间段内重力在载体系(b系)下的积分为：
    Figure PCTCN2019077888-appb-100043
    其中，
    Figure PCTCN2019077888-appb-100044
    为载体系变化矩阵，
    Figure PCTCN2019077888-appb-100045
    为重力矢量在载体系下的分量；
    粗对准过程中，加速度计输出比力
    Figure PCTCN2019077888-appb-100046
    由重力矢量g b、线振动干扰加速度
    Figure PCTCN2019077888-appb-100047
    杆臂加速度
    Figure PCTCN2019077888-appb-100048
    以及零位误差
    Figure PCTCN2019077888-appb-100049
    构成：
    Figure PCTCN2019077888-appb-100050
    经过简化
    Figure PCTCN2019077888-appb-100051
    为：
    Figure PCTCN2019077888-appb-100052
    Figure PCTCN2019077888-appb-100053
    根据双矢量定姿原理，由
    Figure PCTCN2019077888-appb-100054
    不难得到晃动基座下t＝0时刻姿态矩阵
    Figure PCTCN2019077888-appb-100055
    Figure PCTCN2019077888-appb-100056
  3. 根据权利要求1所述的深海潜航器的水下抗晃动对准方法，其特征在于：所述的深海潜航器的水下抗晃动对准方法，步骤(1)中所述的SINS/DVL的基于SVD分解的模糊自适应鲁棒CKF滤波器对准，包括以下步骤：
    1)计算基本容积点和相应权值：
    Figure PCTCN2019077888-appb-100057
    式中，m表示容积点总数(m＝2num)，num为CKF滤波器的状态维数，[1]表示对num维单位向量e＝[1,0,..,0] T的全排列和改变元素符号产生的点集；
    2)时间更新：
    ①基于SVD分解计算容积点X j,k-1
    Figure PCTCN2019077888-appb-100058
    其中，k为滤波时刻，U j,k-1为k-1时刻SVD分解出的酉阵，s j(j＝1,2,..,num)为k-1时刻滤波器输出的最优滤波估计协方差P k-1|k-1的特征值的平方根，
    Figure PCTCN2019077888-appb-100059
    为k-1时刻滤波器输出的最优状态估计；
    ②根据权利2所述的非线性状态方程计算传播容积点
    Figure PCTCN2019077888-appb-100060
    Figure PCTCN2019077888-appb-100061
    ③计算状态量
    Figure PCTCN2019077888-appb-100062
    和状态预测P k|k-1
    Figure PCTCN2019077888-appb-100063
    其中Q k-1为k-1时刻的导航系统过程噪声矩阵；
    3)量测更新：
    ①基于SVD分解计算容积点X j,k|k-1
    Figure PCTCN2019077888-appb-100064
    Figure PCTCN2019077888-appb-100065
    ②通过根据权利2所述的非线性量测方程计算传播容积点Z j,k
    Z j,k＝h(X j,k|k-1,t)
    ③计算量测量
    Figure PCTCN2019077888-appb-100066
    量测协方差P zz,k以及新息协方差估计P xz,k
    Figure PCTCN2019077888-appb-100067
    ④计算增益阵K k、更新最优状态估计
    Figure PCTCN2019077888-appb-100068
    以及最优估计协方差：
    K k＝P xz,k/P zz,k
    Figure PCTCN2019077888-appb-100069
    为在晃动基座下，使精对准过程具有一定鲁棒性，基于H∞滤波器的相关原理，对传统CKF的最优估计协方差进行改写：
    Figure PCTCN2019077888-appb-100070
    其中，γ为H∞次优解的阈值，与滤波器的鲁棒性能有关，H∞次优问题存在解的充分必要条件可由黎卡提不等式(Riccati inequality)给出:
    Figure PCTCN2019077888-appb-100071
    阈值γ模糊自适应算法如下：
    γ＝η·γ a
    其中，
    Figure PCTCN2019077888-appb-100072
    且ρ()表示矩阵的谱半径，η为模糊自适应因子，
    从新息序列的统计特性的变化入手，构造出了阈值γ的模糊自适应因子η更新式为：
    Figure PCTCN2019077888-appb-100073
    其中，
    Figure PCTCN2019077888-appb-100074
    为新息序列的平方和。Trace(P xz,k)为求迹运算，即求矩阵对角线元素之和。
PCT/CN2019/077888 2018-09-28 2019-03-12 一种深海潜航器的sins/dvl水下抗晃动对准方法 WO2020062791A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201811144284.6 2018-09-28
CN201811144284.6A CN109443379B (zh) 2018-09-28 2018-09-28 一种深海潜航器的sins/dvl水下抗晃动对准方法

Publications (1)

Publication Number Publication Date
WO2020062791A1 true WO2020062791A1 (zh) 2020-04-02

Family

ID=65544787

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2019/077888 WO2020062791A1 (zh) 2018-09-28 2019-03-12 一种深海潜航器的sins/dvl水下抗晃动对准方法

Country Status (2)

Country Link
CN (1) CN109443379B (zh)
WO (1) WO2020062791A1 (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111504324A (zh) * 2020-04-27 2020-08-07 西北工业大学 一种噪声自适应滤波的水下组合导航方法
CN112747770A (zh) * 2020-12-16 2021-05-04 中国船舶重工集团有限公司第七一0研究所 一种基于速度量测的载体机动中初始对准方法
CN112985368A (zh) * 2021-02-09 2021-06-18 西北工业大学 水下航行器在移动运载平台发射前的快速罗经对准方法
CN114777812A (zh) * 2022-04-17 2022-07-22 中国人民解放军国防科技大学 一种水下组合导航系统行进间对准与姿态估计方法

Families Citing this family (29)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109443379B (zh) * 2018-09-28 2020-07-21 东南大学 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN109974695B (zh) * 2019-04-09 2022-08-26 东南大学 基于Krein空间的水面舰艇导航系统的鲁棒自适应滤波方法
CN110057383B (zh) * 2019-05-05 2023-01-03 哈尔滨工程大学 一种auv推位导航系统杆臂误差标校方法
CN111912427B (zh) * 2019-05-10 2022-03-01 中国人民解放军火箭军工程大学 一种多普勒雷达辅助捷联惯导运动基座对准方法及系统
CN110146076B (zh) * 2019-06-06 2023-04-18 哈尔滨工业大学（威海） 一种无逆矩阵自适应滤波的sins/dvl组合定位方法
CN110567490B (zh) * 2019-08-29 2022-02-18 桂林电子科技大学 一种大失准角下sins初始对准方法
CN110514203B (zh) * 2019-08-30 2022-06-28 东南大学 一种基于isr-ukf的水下组合导航方法
CN110940340A (zh) * 2019-12-23 2020-03-31 中科探海(苏州)海洋科技有限责任公司 基于小型uuv平台的多传感器信息融合方法
CN111141281A (zh) * 2020-01-03 2020-05-12 中国船舶重工集团公司第七0七研究所 一种sins/dvl组合导航数据后处理误差估计方法
CN111397603B (zh) * 2020-04-24 2022-07-12 东南大学 载体姿态动态情况下的惯性/多普勒动基座粗对准方法
CN111750865B (zh) * 2020-07-04 2023-05-16 东南大学 一种用于双功能深海无人潜器导航系统的自适应滤波导航方法
CN112254718B (zh) * 2020-08-04 2024-04-09 东南大学 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法
CN111854747B (zh) * 2020-08-25 2022-08-12 东南大学 一种载体大机动情况下的dvl辅助sins粗对准方法
CN112284384B (zh) * 2020-10-26 2023-11-17 东南大学 考虑量测异常的集群式多深海潜航器的协同定位方法
CN112507281B (zh) * 2020-11-19 2024-01-02 东南大学 一种基于双状态多因子抗差估计sins/dvl紧组合系统的方法
CN112525218B (zh) * 2020-11-23 2023-01-03 哈尔滨工程大学 一种ins/dvl组合导航系统鲁棒智能协同校准方法
CN112684207B (zh) * 2020-12-17 2022-03-11 东南大学 一种深潜载人潜水器adcp速度估计与修正算法
CN112683271B (zh) * 2020-12-17 2023-10-27 东南大学 一种考虑可观测性的水域观测平台的组合定位方法
CN112798016A (zh) * 2020-12-22 2021-05-14 中国航天空气动力技术研究院 一种基于sins与dvl组合的auv行进间快速初始对准方法
CN112747748A (zh) * 2020-12-22 2021-05-04 中国航天空气动力技术研究院 一种基于逆向解算的领航auv导航数据后处理方法
CN112729291B (zh) * 2020-12-29 2022-03-04 东南大学 一种深潜长航潜水器sins/dvl洋流速度估计方法
CN114485723B (zh) * 2021-02-08 2024-02-27 北京理工大学 一种自适应鲁棒矩阵卡尔曼滤波的高旋体空中对准方法
CN113503892B (zh) * 2021-04-25 2024-03-01 中船航海科技有限责任公司 一种基于里程计和回溯导航的惯导系统动基座初始对准方法
CN113218421B (zh) * 2021-05-11 2023-07-04 中国人民解放军63921部队 北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法
CN114459476B (zh) * 2022-03-09 2024-03-01 东南大学 基于虚拟速度量测的水下无人潜航器测流dvl/sins组合导航方法
CN115031724A (zh) * 2022-03-21 2022-09-09 哈尔滨工程大学 Sins/dvl紧组合系统dvl波束故障处理方法
CN115031727B (zh) * 2022-03-31 2023-06-20 哈尔滨工程大学 一种基于状态变换的多普勒辅助捷联惯导系统初始对准方法
CN115060274B (zh) * 2022-08-17 2022-11-18 南开大学 一种水下一体式自主导航装置及其初始对准方法
CN116295511B (zh) * 2022-12-16 2024-04-02 南京安透可智能系统有限公司 一种用于管道潜航机器人的鲁棒初始对准方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103245360A (zh) * 2013-04-24 2013-08-14 北京工业大学 晃动基座下的舰载机旋转式捷联惯导系统自对准方法
CN103471616A (zh) * 2013-09-04 2013-12-25 哈尔滨工程大学 一种动基座sins大方位失准角条件下初始对准方法
CN105806363A (zh) * 2015-11-16 2016-07-27 东南大学 基于srqkf的sins/dvl水下大失准角对准方法
CN107990910A (zh) * 2017-11-06 2018-05-04 哈尔滨工业大学 一种基于容积卡尔曼滤波的舰船大方位失准角传递对准方法
CN109443379A (zh) * 2018-09-28 2019-03-08 东南大学 一种深海潜航器的sins/dvl水下抗晃动对准方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103245360A (zh) * 2013-04-24 2013-08-14 北京工业大学 晃动基座下的舰载机旋转式捷联惯导系统自对准方法
CN103471616A (zh) * 2013-09-04 2013-12-25 哈尔滨工程大学 一种动基座sins大方位失准角条件下初始对准方法
CN105806363A (zh) * 2015-11-16 2016-07-27 东南大学 基于srqkf的sins/dvl水下大失准角对准方法
CN107990910A (zh) * 2017-11-06 2018-05-04 哈尔滨工业大学 一种基于容积卡尔曼滤波的舰船大方位失准角传递对准方法
CN109443379A (zh) * 2018-09-28 2019-03-08 东南大学 一种深海潜航器的sins/dvl水下抗晃动对准方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
LIANG, XINYU ET AL.: "H? Robust Adaptive CKF Algorithm Used in GNSS/INS Integrated Navigation", COMPUTER ENGINEERING AND APPLICATIONS, vol. 54, no. 8, 14 September 2017 (2017-09-14), ISSN: 1002-8331 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111504324A (zh) * 2020-04-27 2020-08-07 西北工业大学 一种噪声自适应滤波的水下组合导航方法
CN111504324B (zh) * 2020-04-27 2022-07-26 西北工业大学 一种噪声自适应滤波的水下组合导航方法
CN112747770A (zh) * 2020-12-16 2021-05-04 中国船舶重工集团有限公司第七一0研究所 一种基于速度量测的载体机动中初始对准方法
CN112985368A (zh) * 2021-02-09 2021-06-18 西北工业大学 水下航行器在移动运载平台发射前的快速罗经对准方法
CN112985368B (zh) * 2021-02-09 2022-10-14 西北工业大学 水下航行器在移动运载平台发射前的快速罗经对准方法
CN114777812A (zh) * 2022-04-17 2022-07-22 中国人民解放军国防科技大学 一种水下组合导航系统行进间对准与姿态估计方法
CN114777812B (zh) * 2022-04-17 2024-04-05 中国人民解放军国防科技大学 一种水下组合导航系统行进间对准与姿态估计方法

Also Published As

Publication number Publication date
CN109443379B (zh) 2020-07-21
CN109443379A (zh) 2019-03-08

Similar Documents

Publication Publication Date Title
WO2020062791A1 (zh) 一种深海潜航器的sins/dvl水下抗晃动对准方法
US20220404152A1 (en) Motion constraint-aided underwater integrated navigation method employing improved sage-husa adaptive filtering
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN112097763B (zh) 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
Li et al. A fast SINS initial alignment scheme for underwater vehicle applications
Huang et al. A new fast in-motion coarse alignment method for GPS-aided low-cost SINS
WO2020062807A1 (zh) 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法
CN106643709B (zh) 一种海上运载体的组合导航方法及装置
CN104316045A (zh) 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法
CN111829512B (zh) 一种基于多传感器数据融合的auv导航定位方法及系统
CN105486313A (zh) 一种基于usbl辅助低成本sins系统的定位方法
CN106895853B (zh) 一种电磁计程仪辅助船用陀螺罗经行进间对准方法
CN103454662B (zh) 一种基于ckf的sins/北斗/dvl组合对准方法
Xue et al. In-motion alignment algorithm for vehicle carried SINS based on odometer aiding
CN112556697A (zh) 一种基于联邦结构的浅耦合数据融合导航方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
CN110133695A (zh) 一种双天线gnss位置延迟时间动态估计系统及方法
CN104061930A (zh) 基于捷联惯性制导和多普勒计程仪的导航方法
Pei et al. Initial self-alignment for marine rotary SINS using novel adaptive Kalman filter
Dukan et al. Integration filter for APS, DVL, IMU and pressure gauge for underwater vehicles
CN113108781B (zh) 一种应用于无人船行进间的改进粗对准方法
Allotta et al. Localization algorithm for a fleet of three AUVs by INS, DVL and range measurements
CN110873813B (zh) 一种水流速度估算方法、组合导航方法及装置
Liu et al. Velocity-aided in-motion alignment for SINS based on pseudo-Earth frame
CN103616026A (zh) 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 19867006

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 19867006

Country of ref document: EP

Kind code of ref document: A1

122 Ep: pct application non-entry in european phase

Ref document number: 19867006

Country of ref document: EP

Kind code of ref document: A1

32PN Ep: public notification in the ep bulletin as address of the adressee cannot be established

Free format text: NOTING OF LOSS OF RIGHTS PURSUANT TO RULE 112(1) EPC (EPO FORM 1205 DATED 10-05-2023)

122 Ep: pct application non-entry in european phase

Ref document number: 19867006

Country of ref document: EP

Kind code of ref document: A1