WO2022088797A1 - 考虑量测异常的集群式多深海潜航器的协同定位方法 - Google Patents
考虑量测异常的集群式多深海潜航器的协同定位方法 Download PDFInfo
- Publication number
- WO2022088797A1 WO2022088797A1 PCT/CN2021/108864 CN2021108864W WO2022088797A1 WO 2022088797 A1 WO2022088797 A1 WO 2022088797A1 CN 2021108864 W CN2021108864 W CN 2021108864W WO 2022088797 A1 WO2022088797 A1 WO 2022088797A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- matrix
- submersible
- uncertainty
- measurement
- deep
- Prior art date
Links
- 238000005259 measurement Methods 0.000 title claims abstract description 44
- 238000000034 method Methods 0.000 title claims abstract description 27
- 230000004807 localization Effects 0.000 title abstract 6
- 238000001914 filtration Methods 0.000 claims abstract description 11
- 230000005856 abnormality Effects 0.000 claims abstract description 10
- 238000013178 mathematical model Methods 0.000 claims abstract description 9
- 230000003044 adaptive effect Effects 0.000 claims abstract description 8
- 239000011159 matrix material Substances 0.000 claims description 60
- 230000006978 adaptation Effects 0.000 claims description 21
- 238000013461 design Methods 0.000 claims description 5
- 230000005540 biological transmission Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 239000013535 sea water Substances 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—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 combined with non-inertial navigation instruments
-
- 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
-
- 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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
- G01S5/22—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/30—Assessment of water resources
Definitions
- the invention belongs to the technical field of navigation, and relates to a navigation and positioning method of a deep-sea submersible, in particular to a cooperative positioning method of a clustered multi-submarine submersible that considers measurement anomalies.
- the WEINIG cluster multi-submersible cooperative working system has the characteristics of wide detection range, strong fault tolerance and high work efficiency, and can realize diverse and complex underwater tasks that are difficult for single submersibles to complete.
- the co-location technology of multiple deep-sea submersibles has certain particularities, and there are inherent difficulties such as complex environment interference, sensor limitations, and communication technology. Therefore, the information acquisition and communication between multiple deep-sea submersibles can only be accomplished by ultrasonic waves.
- the relative distance information between the master and slave deep-sea submersibles often contains multiplicative noise.
- the underwater acoustic wave speed is affected by changes in seawater temperature and salinity, and its uncertainty will cause the master-slave deep sea.
- the present invention aims at the problem that the measurement abnormality is prone to occur in the collaborative positioning process of the multi-deep sea submersibles of the power cluster, which causes the accuracy and reliability of the collaborative navigation system to decrease, thereby providing a power cluster that considers the measurement abnormality.
- the present invention effectively solves the difficulty that the traditional H ⁇ filter is difficult to model when there are multiplicative noise and parameter uncertainty in the measurement by designing a new form of krein space filter, and reduces the time and space complexity of the filter.
- an adaptive algorithm is designed to estimate and compensate the parameter uncertainty of filter modeling online, so as to improve the matching degree between the physical process of co-location and the established mathematical model.
- S1 Establish the state error equation of the slave submersible in the local geographic coordinate system and the measurement equation considering the abnormality of the relative distance measurement information of the master and slave submersible;
- S3 Design an adaptive algorithm to estimate and compensate the parameter uncertainty of filter modeling online, and improve the matching degree between the physical process of co-location and the established mathematical model.
- the further improvement of the present invention is:
- the step S1 specifically includes the following steps:
- ⁇ , ⁇ , ⁇ represent the attitude angle error from the submersible
- ⁇ v E , ⁇ v N , ⁇ v U represent the northeast sky speed error from the submersible
- ⁇ L, ⁇ , ⁇ h represent From the latitude, longitude and altitude errors of the submersible, represents the constant offset from the inertial totalizer in the submersible, represents the constant offset from the inertial gyroscope in the submersible;
- the discrete time error equation of the inertial navigation system from the submarine in the local geographic coordinate system is:
- Z k ⁇ R m is the amount of external information
- L m , ⁇ m , h m are the positions of the main submersibles
- L s , ⁇ s , h s are the positions of the submerged submersibles
- H k [ ⁇ 3x6 I 3x3 ⁇ 3x6 ] is the measurement matrix
- ⁇ H k is the uncertain parameter matrix
- ⁇ is the multiplicative noise adaptation matrix
- v 1,k , v 2,k are the uncorrelated measurement white Gaussian noise respectively.
- ⁇ R ⁇ , ⁇ R L , and ⁇ R h are mainly the relative eastward distance, northward relative distance and vertical relative distance from the deep-sea submersible;
- ⁇ V represents the uncertainty of the underwater acoustic wave velocity V
- ⁇ L , ⁇ ⁇ , and ⁇ h represent the underwater acoustic wave transmission time.
- step S2 is specifically as follows:
- ⁇ k is the system noise, which is Gaussian white noise obeying zero mean
- Y k is the estimation matrix
- L k is the linear combination of the system state variables, usually the identity matrix I
- ⁇ is the multiplicative noise adaptation matrix
- ⁇ H k is an uncertain parameter matrix, and it satisfies:
- ⁇ k is the unknown uncertainty matrix
- Matrices A and E k are adaptation matrices of known dimensions, which describe the process of ⁇ k uncertainty matrix affecting ⁇ H k uncertain parameter matrix
- Sk and ⁇ k are perturbation parameters, and have:
- X 0 is the initial state quantity of the system
- P 0 is the initial covariance
- Q k , R k are the covariance matrix of the system state noise ⁇ k and the measurement noise v 2,k respectively
- ⁇ is the H ⁇ filter in the Design threshold
- ⁇ is a finite constant
- R E is the adaptive noise variance
- the ⁇ -level robust H ⁇ posterior filtering equation based on the new form of krein space system is:
- step S3 specifically includes the following steps:
- the adaptation matrix is estimated online according to the difference between the actual innovation covariance and the theoretical innovation covariance
- the uncertain parameter array can improve the matching degree between the physical process of co-location and the established mathematical model, suppress the influence of the uncertainty of the underwater acoustic wave speed on the filter, and finally realize the overall high precision, High reliability collaborative navigation and positioning.
- the present invention estimates and compensates the uncertainty of modeling parameters caused by the influence of changes in seawater temperature and salinity on the underwater acoustic wave speed by designing a new form of krein space filter, thereby improving the multi-deep sea vehicle. Accuracy of co-location. Secondly, it effectively solves the difficulty of modeling the traditional H ⁇ filter when there is multiplicative noise in the measurement, and reduces the time and space complexity of the filter while ensuring the high reliability of the multi-submarine co-location system.
- Fig. 1 is the power cluster multi-deep sea submersible system in the embodiment of the present invention
- FIG. 2 is a block diagram of the co-location of the WEINIG cluster type multi-deep-sea submersible system in the embodiment of the present invention.
- this embodiment provides a method for co-locating clustered multi-submersible submersibles that considers measurement anomalies, including the following steps:
- S1 Establish the state error equation of the slave submersible in the local geographic coordinate system and the measurement equation considering the abnormality of the relative distance measurement information of the master and slave submersibles, which are as follows:
- ⁇ , ⁇ , ⁇ represent the attitude angle error from the submersible
- ⁇ v E , ⁇ v N , ⁇ v U represent the northeast sky speed error from the submersible
- ⁇ L, ⁇ , ⁇ h represent From the latitude, longitude and altitude errors of the submersible, represents the constant offset from the inertial totalizer in the submersible, represents the constant offset from the inertial gyroscope in the submersible;
- the discrete time error equation of the inertial navigation system from the submarine in the local geographic coordinate system is:
- Z k ⁇ R m is the amount of external information
- L m , ⁇ m , h m are the positions of the main submersibles
- L s , ⁇ s , h s are the positions of the submerged submersibles
- H k [ ⁇ 3x6 I 3x3 ⁇ 3x6 ] is the measurement matrix
- ⁇ H k is the uncertain parameter matrix
- ⁇ is the multiplicative noise adaptation matrix
- v 1,k , v 2,k are the uncorrelated measurement white Gaussian noise respectively.
- ⁇ R ⁇ , ⁇ R L , and ⁇ R h are mainly the relative eastward distance, northward relative distance and vertical relative distance from the deep-sea submersible.
- ⁇ V represents the uncertainty of the underwater acoustic wave velocity V
- ⁇ L , ⁇ ⁇ , and ⁇ h represent the underwater acoustic wave transmission time.
- ⁇ k is the system noise, which is Gaussian white noise obeying zero mean
- Y k is the estimation matrix
- L k is the linear combination of the system state variables, usually the identity matrix I
- ⁇ is the multiplicative noise adaptation matrix
- ⁇ H k is an uncertain parameter matrix, and it satisfies:
- ⁇ k is the unknown uncertainty matrix
- Matrices A and E k are adaptation matrices of known dimensions, which describe the process of ⁇ k uncertainty matrix affecting ⁇ H k uncertain parameter matrix
- Sk and ⁇ k are perturbation parameters, and have:
- X 0 is the initial state quantity of the system
- P 0 is the initial covariance
- Q k , R k are the covariance matrix of the system state noise ⁇ k and the measurement noise v 2,k respectively
- ⁇ is the H ⁇ filter in the Design threshold
- ⁇ is a finite constant.
- the energy-constrained SQC inequality can only give an elliptical set, which is not suitable for describing the results of uncertain systems, the energy-constrained SQC inequality is transformed into an equivalent objective quadratic form:
- R E is the adaptive noise variance
- the adaptation matrix is estimated online according to the difference between the actual innovation covariance and the theoretical innovation covariance.
- the uncertain parameter array can improve the matching degree between the physical process of co-location and the established mathematical model, suppress the influence of the uncertainty of the underwater acoustic wave speed on the filter, and finally realize the overall high precision, High reliability collaborative navigation and positioning.
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Manufacturing & Machinery (AREA)
- Navigation (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
一种考虑量测异常的威力集群式多深海潜航器协同定位方法，在当地地理坐标系下建立了状态误差方程和考虑主从深海潜航器的相对距离测量信息异常的量测方程，为完成多深海潜航器协同定位提供数学模型。其次，根据多深海潜航器协同定位的特点以及Krein空间线性估计理论，引入乘性噪声和参数不确定性，在考虑量测异常的情况下设计了新形式Krein空间的协同导航的鲁棒后验滤波方程。同时，设计自适应算法对滤波器建模的参数不确定性进行在线估计和补偿，提高协同定位物理过程与所建数学模型的匹配度。最终实现威力集群多深海潜航器整体的高精度、高可靠性协同导航定位。
Description
本发明属于导航技术领域，涉及了深海潜航器的导航定位方法，具体是一种考虑量测异常的集群式多深海潜航器的协同定位方法。
威力集群多深海潜航器协同工作***具有探测范围广、容错能力强、工作效率高的特点，能够实现单体深海潜航器难以完成的多样化、复杂化水下任务。
多深海潜航器的协同定位技术与空中多无人机、陆地多机器人协同工作***的协同定位技术相比有着一定的特殊性，存在着复杂环境干扰、传感器限制、通信技术等固有难点。因此多深海潜航器间的信息获取和交流只能靠超声波来完成。
但由于超声波距离传感器的信号相关特性，主从深海潜航器间的相对距离信息中往往含有乘性噪声，同时水下声波波速受海水温度、盐度变化影响，其不确定性会造成主从深海潜航器的相对距离建模中的参数存在不确定性。这两种情况统称为量测异常，会造成主从潜航器协同导航***的定位精度降低，甚至于失效。
因此在海洋复杂的环境中，考虑量测异常情况下实现主从式深海潜航器的精准协同定位是目前多深海潜航器研究的一个热门方向。
发明内容
为解决上述问题，本发明针对威力集群多深海潜航器在协同定位过程中容易出现量测异常情况，造成协同导航***的精度和可靠性下降的问题，从而提供一种考虑量测异常的威力集群式多深海潜航器协同定位方法。
本发明通过设计新形式krein空间滤波器来有效解决量测中存在乘性噪声和参数不确定性时传统H∞滤波器难以建模的困难，降低滤波器的时空复杂度。同时，设计自适应算法对滤波器建模的参数不确定性进行在线估计和补偿，提高协同定位物理过程与所建数学模型的匹配度。最终实现威力集群多深海潜航器整体的高精度、高可靠性协同导航定位。
技术方案：为实现上述发明目的，本发明采用的技术方案，包括如下步骤：
S1：建立从深海潜航器在当地地理坐标系下的状态误差方程和考虑主从深海潜航器的相对距离测量信息异常的量测方程；
S2：根据多深海潜航器协同定位的特点以及Krein空间线性估计理论，引入乘性噪声和参数不确定性，在考虑量测异常的情况下设计新形式Krein空间的协同导航的鲁棒后验滤波方程；
S3：设计自适应算法对滤波器建模的参数不确定性进行在线估计和补偿，提高协同定位物理过程与所建数学模型的匹配度。
本发明进一步改进在于：
所述步骤S1具体包括如下步骤：
S1-1:考虑一个15维***状态量的主从潜航器协同定位离散系 统：
其中，△θ,△φ,△ω表示从潜航器的姿态角误差，△v
E,△v
N,△v
U代表从潜航器的东北天向速度误差，△L,△λ,△h表示从潜航器的纬度、经度、高度误差，
代表从潜航器中惯性加计的常值偏移，
代表从潜航器中惯性陀螺仪的常值偏移；
从潜航器惯导***在当地地理坐标系下的离散时间误差方程为：
其中，
为状态转移矩阵，X
k∈R
n是为状态量，k是滤波时刻，τ
s是离散时间间隔，
是从载体坐标系到导航坐标系的转换矩阵。且
为捷联惯导***连续时间误差方程中相应的***矩阵；
S1-2:考虑主从深海潜航器的相对距离测量信息异常的量测方程为：
其中，Z
k∈R
m是为外信息量，L
m、λ
m、h
m为主深海潜航器的位置，L
s、λ
s、h
s为从深海潜航器的位置，H
k＝[Ο
3x6 I
3x3 Ο
3x6]为量测矩阵，△H
k为不确定参数阵，Γ为乘性噪声适配矩阵，v
1,k、v
2,k分别为互不相关的量测高斯白噪声。△R
λ、△R
L、△R
h为主从深海潜航器的东向相对距离、北向相对距离、垂向相对距离；
由于水下声波波速V的不确定性问题，会导致主从深海潜航器间 相对距离测量的不确定：
其中，△V表示水下声波波速V的不确定，τ
L、τ
λ、τ
h表示水下声波传输时间。
本发明进一步改进在于：所述步骤S2其具体如下：
S2-1：建立目标二次型
考虑乘性噪声和参数不确定性的情况下一个线性离散***：
其中，μ
k为***噪声，是服从零均值的高斯白噪声；Y
k为估计矩阵；L
k表示对***状态变量的线性组合，通常为单位阵I；Γ为乘性噪声适配矩阵；△H
k为不确定参数阵，并且使其满足：
△HX
k＝A△
kE
kX
k＝A△
kS
k＝Aξ
k
其中，△
k是未知的不确定性矩阵，且
矩阵A、E
k为已知维度的适配矩阵，它描述了△
k不确定性矩阵影响△H
k不确定参数阵的过程；S
k、ξ
k为摄动参数，且有：
||ξ
k||
2≤||S
k||
2
其中，||·||表示向量H
2范数；
令v
1,kX
k＝η
k，将
以及
引入传统能量约束SQC不等式，建立新型的鲁棒H∞***的不确定性可用能量约束SQC不等式：
其中，X
0为***初始状态量；P
0为初始协方差；Q
k、R
k分别为***状态噪声μ
k和量测噪声v
2,k的协方差阵；γ为H∞滤波器中的设计的阈值；
表示状态量线性组合估计量Y
k的误差；ε为一限定常数；
将能量约束SQC不等式转换为等价目标二次型：
S2-2：建立新形式krein空间***模型和滤波方程
考虑乘性噪声和参数不确定性的情况下建立新形式krein空间***：
其中，
且具有形式噪声方差阵：
其中，
R
E为适配噪声方差；
基于新形式krein空间***的γ水平鲁棒H∞后验滤波方程为：
其中，
本发明进一步改进在于：所述步骤S3具体包括如下步骤：
理想情况中新形式krein空间***的γ水平鲁棒H∞后验滤波器的新息
为：
则理论情况中
而实际情况滤波器中估计的状态量
受参数不确定性干扰成为有偏估计，其新息期望和协方差分别有
且有：
参数不确定性会反映在滤波器新息中，不确定性干扰越大，实际新息协方差与理论新息协方差越大。因此可以新息协方差入手，在线估计
适配矩阵：
即：
当且仅当适配矩阵
被正确估计时，上式取等号；于是在滤波器迭代过程，根据实际新息协方差与理论新息协方差的差异，在线估计适配矩阵
将正确估计的适配矩阵
带入△H
k+1不确定参数阵，得到：
正确估计的
不确定参数阵能够提高协同定位物理过程与所建数学模型的匹配度，抑制水下声波波速的不确定性所给滤波器带来的影响，最终实现威力集群多深海潜航器整体的高精度、高可靠性协同导航定位。
本发明与现有方法相比，通过设计新形式krein空间滤波器来估计和补偿了水下声波波速受海水温度、盐度变化影响而产生的建模参数不确定性，提高了多深海潜航器协同定位的精度。其次，有效解决了量测中存在乘性噪声时传统H∞滤波器难以建模的困难，在保证多深海潜航器协同定位***高可靠性的同时降低了滤波器的时空复杂度。
图1、是本发明实施例中威力集群多深海潜航器***；
图2是本发明实施例中考威力集群式多深海潜航器***协同定位框图。
下面结合附图和具体实施方式，进一步阐明本发明，应理解下述 具体实施方式仅用于说明本发明而不用于限制本发明的范围。需要说明的是，下面描述中使用的词语“前”、“后”、“左”、“右”、“上”和“下”指的是附图中的方向，词语“内”和“外”分别指的是朝向或远离特定部件几何中心的方向。
如图1、2所示，本实施例提供一种考虑量测异常的集群式多深海潜航器的协同定位方法，包括如下步骤：
S1：建立从深海潜航器在当地地理坐标系下的状态误差方程和考虑主从深海潜航器的相对距离测量信息异常的量测方程，其具体如下：
考虑一个15维***状态量的主从潜航器协同定位离散***：
其中，△θ,△φ,△ω表示从潜航器的姿态角误差，△v
E,△v
N,△v
U代表从潜航器的东北天向速度误差，△L,△λ,△h表示从潜航器的纬度、经度、高度误差，
代表从潜航器中惯性加计的常值偏移，
代表从潜航器中惯性陀螺仪的常值偏移；
从潜航器惯导***在当地地理坐标系下的离散时间误差方程为：
其中，
为状态转移矩阵，X
k∈R
n是为状态量，k是滤波时刻，τ
s是离散时间间隔，
是从载体坐标系到导航坐标系的转换矩阵。且
为 捷联惯导***连续时间误差方程中相应的***矩阵。
考虑主从深海潜航器的相对距离测量信息异常的量测方程为：
其中，Z
k∈R
m是为外信息量，L
m、λ
m、h
m为主深海潜航器的位置，L
s、λ
s、h
s为从深海潜航器的位置，H
k＝[Ο
3x6 I
3x3 Ο
3x6]为量测矩阵，△H
k为不确定参数阵，Γ为乘性噪声适配矩阵，v
1,k、v
2,k分别为互不相关的量测高斯白噪声。△R
λ、△R
L、△R
h为主从深海潜航器的东向相对距离、北向相对距离、垂向相对距离。
由于水下声波波速V的不确定性问题，会导致主从深海潜航器间相对距离测量的不确定：
其中，△V表示水下声波波速V的不确定，τ
L、τ
λ、τ
h表示水下声波传输时间。
S2：根据多深海潜航器协同定位的特点以及Krein空间线性估计理论，引入乘性噪声和参数不确定性，在考虑量测异常的情况下设计新形式Krein空间的协同导航的鲁棒滤波方程，其具体如下：
S2-1：建立目标二次型
考虑乘性噪声和参数不确定性的情况下一个线性离散***：
其中，μ
k为***噪声，是服从零均值的高斯白噪声；Y
k为估计矩阵；L
k表示对***状态变量的线性组合，通常为单位阵I；Γ为乘性 噪声适配矩阵；△H
k为不确定参数阵，并且使其满足：
△HX
k＝A△
kE
kX
k＝A△
kS
k＝Aξ
k
其中，△
k是未知的不确定性矩阵，且
矩阵A、E
k为已知维度的适配矩阵，它描述了△
k不确定性矩阵影响△H
k不确定参数阵的过程；S
k、ξ
k为摄动参数，且有：
||ξ
k||
2≤||S
k||
2
其中，||·||表示向量H
2范数。
令v
1,kX
k＝η
k，将
以及
引入传统能量约束SQC不等式，建立新型的鲁棒H∞***的不确定性可用能量约束SQC不等式：
其中，X
0为***初始状态量；P
0为初始协方差；Q
k、R
k分别为***状态噪声μ
k和量测噪声v
2,k的协方差阵；γ为H∞滤波器中的设计的阈值；
表示状态量线性组合估计量Y
k的误差；ε为一限定常数。
由于能量约束SQC不等式只能给出椭圆集合，不适合描述不确定***的结果，因此将能量约束SQC不等式转换为等价目标二次型：
S2-2：建立新形式krein空间***模型和滤波方程
考虑乘性噪声和参数不确定性的情况下建立新形式krein空间***：
其中，
且具有形式噪声方差阵：
其中，
R
E为适配噪声方差；
基于新形式krein空间***的γ水平鲁棒H∞后验滤波方程为：
其中，
值得注意的是，当且仅当
成立时才能使用上述滤波方程。
S3：由于水下声波波速的不确定性会造成主从深海潜航器的相对距离建模中的参数存在不确定性，最优滤波器估计的状态量
无法满足精确的协同定位。设计自适应算法对滤波器建模的参数不确定性进行在线估计和补偿，提高协同定位物理过程与所建数学模型的匹配度，其具体如下：
理想情况中新形式krein空间***的γ水平鲁棒H∞后验滤波器的新息
为：
则理论情况中
而实际情况滤波器中估计的状态量
受参数不确定性干扰成为有偏估计，其新息期望和协方差分别有
且有：
参数不确定性会反映在滤波器新息中，不确定性干扰越大，实际新息协方差与理论新息协方差越大。因此可以新息协方差入手，在线估计
适配矩阵：
即：
当且仅当适配矩阵
被正确估计时，上式取等号。于是在滤波器迭代过程，根据实际新息协方差与理论新息协方差的差异，在线估计适配矩阵
将正确估计的适配矩阵
带入△H
k+1不确定参数阵，得到：
正确估计的
不确定参数阵能够提高协同定位物理过程与所建数学模型的匹配度，抑制水下声波波速的不确定性所给滤波器带来的影响，最终实现威力集群多深海潜航器整体的高精度、高可靠性协同导航定位。
本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。
Claims (4)
- 一种考虑量测异常的威力集群式多深海潜航器协同定位方法，其特征在于：包括以下步骤：S1：建立从深海潜航器在当地地理坐标系下的状态误差方程和考虑主从深海潜航器的相对距离测量信息异常的量测方程；S2：根据多深海潜航器协同定位的特点以及Krein空间线性估计理论，引入乘性噪声和参数不确定性，在考虑量测异常的情况下设计新形式Krein空间的协同导航的鲁棒后验滤波方程；S3：设计自适应算法对滤波器建模的参数不确定性进行在线估计和补偿，提高协同定位物理过程与所建数学模型的匹配度。
- 如权利要求1所述的一种考虑量测异常的威力集群式多深海潜航器协同定位方法，其特征在于：所述步骤S1具体包括如下步骤：S1-1:考虑一个15维***状态量的主从潜航器协同定位离散***：
其中，△θ,△φ,△ω表示从潜航器的姿态角误差，△v E,△v N,△v U代表从潜航器的东北天向速度误差，△L,△λ,△h表示从潜航器的纬度、经度、高度误差，代表从潜航器中惯性加计的常值偏移，
代表从潜航器中惯性陀螺仪的常值偏移；
从潜航器惯导***在当地地理坐标系下的离散时间误差方程为：
其中，为状态转移矩阵，X k∈R n是为状态量，k是滤波时刻，τ s是离散时间间隔，
是从载体坐标系到导航坐标系的转换矩阵。且
为捷联惯导***连续时间误差方程中相应的***矩阵；
S1-2:考虑主从深海潜航器的相对距离测量信息异常的量测方程为：
其中，Z k∈R m是为外信息量，L m、λ m、h m为主深海潜航器的位置，L s、λ s、h s为从深海潜航器的位置，H k＝[Ο 3x6 I 3x3 Ο 3x6]为量测矩阵，△H k为不确定参数阵，Γ为乘性噪声适配矩阵，v 1,k、v 2,k分别为互不相关的量测高斯白噪声。△R λ、△R L、△R h为主从深海潜航器的东向相对距离、北向相对距离、垂向相对距离；由于水下声波波速V的不确定性问题，会导致主从深海潜航器间相对距离测量的不确定：
其中，△V表示水下声波波速V的不确定，τ L、τ λ、τ h表示水下声波传输时间。 - 根据权利要求1所述的一种考虑量测异常的威力集群式多深海潜航器协同定位方法，其特征在于：所述步骤S2其具体如下：S2-1：建立目标二次型考虑乘性噪声和参数不确定性的情况下一个线性离散***：
其中，μ k为***噪声，是服从零均值的高斯白噪声；Y k为估计矩阵；L k表示对***状态变量的线性组合，通常为单位阵I；Γ为乘性噪声适配矩阵；△H k为不确定参数阵，并且使其满足：△HX k＝A△ kE kX k＝A△ kS k＝Aξ k其中，△ k是未知的不确定性矩阵，且矩阵A、E k为已知维度的适配矩阵，它描述了△ k不确定性矩阵影响△H k不确定参数阵的过程；S k、ξ k为摄动参数，且有：
||ξ k|| 2≤||S k|| 2其中，||·||表示向量H 2范数；令v 1,kX k＝η k，将以及
引入传统能量约束SQC不等式，建立新型的鲁棒H∞***的不确定性可用能量约束SQC不等式：
其中，X 0为***初始状态量；P 0为初始协方差；Q k、R k分别为***状态噪声μ k和量测噪声v 2,k的协方差阵；γ为H∞滤波器中的设计的阈值；表示状态量线性组合估计量Y k的误差；ε为一限定常数；
将能量约束SQC不等式转换为等价目标二次型：
S2-2：建立新形式krein空间***模型和滤波方程考虑乘性噪声和参数不确定性的情况下建立新形式krein空间***：
其中，
且具有形式噪声方差阵：
其中，R E为适配噪声方差；
基于新形式krein空间***的γ水平鲁棒H∞后验滤波方程为：
其中，
- 根据权利要求1所述的一种考虑量测异常的威力集群式多深海潜航器协同定位方法，其特征在于：所述步骤S3具体包括如下步骤：理想情况中新形式krein空间***的γ水平鲁棒H∞后验滤波器的新息为：
则理论情况中而实际情况滤波器中估计的状态量
受参数不确定性干扰成为有偏估计，其新息期望和协方差分别有
且有：
参数不确定性会反映在滤波器新息中，不确定性干扰越大，实际新息协方差与理论新息协方差越大。因此可以新息协方差入手，在线估计适配矩阵：
即：
当且仅当适配矩阵被正确估计时，上式取等号；于是在滤波器迭代过程，根据实际新息协方差与理论新息协方差的差异，在线估计适配矩阵
将正确估计的适配矩阵带入△H k+1不确定参数阵，得到：
正确估计的不确定参数阵能够提高协同定位物理过程与所建数 学模型的匹配度，抑制水下声波波速的不确定性所给滤波器带来的影响，最终实现威力集群多深海潜航器整体的高精度、高可靠性协同导航定位。
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011156769.4A CN112284384B (zh) | 2020-10-26 | 2020-10-26 | 考虑量测异常的集群式多深海潜航器的协同定位方法 |
| CN202011156769.4 | 2020-10-26 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| WO2022088797A1 true WO2022088797A1 (zh) | 2022-05-05 |
Family
ID=74373757
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| PCT/CN2021/108864 WO2022088797A1 (zh) | 2020-10-26 | 2021-07-28 | 考虑量测异常的集群式多深海潜航器的协同定位方法 |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN112284384B (zh) |
| WO (1) | WO2022088797A1 (zh) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116222582A (zh) * | 2023-05-10 | 2023-06-06 | 北京航空航天大学 | 一种基于变分贝叶斯推断多物理场自适应组合导航方法 |
| CN116242350A (zh) * | 2023-05-12 | 2023-06-09 | 北京航空航天大学 | 一种空间分布式偏振/惯导协同定位方法 |
| CN116358564A (zh) * | 2023-06-01 | 2023-06-30 | 中国人民解放军战略支援部队航天工程大学 | 无人机蜂群质心运动状态跟踪方法、***、设备及介质 |
| CN116667390A (zh) * | 2023-07-27 | 2023-08-29 | 华北电力大学（保定） | 一种基于动态面一致算法的负荷频率控制方法 |
| CN116680500A (zh) * | 2023-06-12 | 2023-09-01 | 哈尔滨工程大学 | 水下航行器在非高斯噪声干扰下的位置估计方法及*** |
| CN117250970A (zh) * | 2023-11-13 | 2023-12-19 | 青岛澎湃海洋探索技术有限公司 | 基于模型嵌入生成对抗网络实现auv故障检测的方法 |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112284384B (zh) * | 2020-10-26 | 2023-11-17 | 东南大学 | 考虑量测异常的集群式多深海潜航器的协同定位方法 |
| CN114018250B (zh) * | 2021-10-18 | 2024-05-03 | 杭州鸿泉物联网技术股份有限公司 | 惯性导航方法、电子设备、存储介质和计算机程序产品 |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106568442A (zh) * | 2016-10-18 | 2017-04-19 | 中冶华天南京电气工程技术有限公司 | 一种具有鲁棒特性的协同导航滤波方法 |
| CN108827305A (zh) * | 2018-05-25 | 2018-11-16 | 哈尔滨工程大学 | 一种基于鲁棒信息滤波的auv协同导航方法 |
| CN109443379A (zh) * | 2018-09-28 | 2019-03-08 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
| CN109829938A (zh) * | 2019-01-28 | 2019-05-31 | 杭州电子科技大学 | 一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法 |
| CN109974695A (zh) * | 2019-04-09 | 2019-07-05 | 东南大学 | 基于Krein空间的水面舰艇导航***的鲁棒自适应滤波方法 |
| CN110579740A (zh) * | 2019-09-17 | 2019-12-17 | 大连海事大学 | 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法 |
| CN111504324A (zh) * | 2020-04-27 | 2020-08-07 | 西北工业大学 | 一种噪声自适应滤波的水下组合导航方法 |
| CN112284384A (zh) * | 2020-10-26 | 2021-01-29 | 东南大学 | 考虑量测异常的集群式多深海潜航器的协同定位方法 |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103744098B (zh) * | 2014-01-23 | 2017-03-15 | 东南大学 | 基于sins/dvl/gps的auv组合导航*** |
| CN105808911A (zh) * | 2014-12-31 | 2016-07-27 | 中国科学院沈阳自动化研究所 | 一种深海潜水器导航方法 |
| CN106767837B (zh) * | 2017-02-23 | 2019-10-22 | 哈尔滨工业大学 | 基于容积四元数估计的航天器姿态估计方法 |
| CN109470266A (zh) * | 2018-11-02 | 2019-03-15 | 佛山科学技术学院 | 一种处理乘性噪声的星敏感器陀螺组合定姿方法 |
| CN111444474B (zh) * | 2020-03-24 | 2024-02-27 | 宁波飞拓电器有限公司 | 一种基于乘性噪声相关自适应ckf的目标跟踪方法 |
-
2020
- 2020-10-26 CN CN202011156769.4A patent/CN112284384B/zh active Active
-
2021
- 2021-07-28 WO PCT/CN2021/108864 patent/WO2022088797A1/zh active Application Filing
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106568442A (zh) * | 2016-10-18 | 2017-04-19 | 中冶华天南京电气工程技术有限公司 | 一种具有鲁棒特性的协同导航滤波方法 |
| CN108827305A (zh) * | 2018-05-25 | 2018-11-16 | 哈尔滨工程大学 | 一种基于鲁棒信息滤波的auv协同导航方法 |
| CN109443379A (zh) * | 2018-09-28 | 2019-03-08 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
| CN109829938A (zh) * | 2019-01-28 | 2019-05-31 | 杭州电子科技大学 | 一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法 |
| CN109974695A (zh) * | 2019-04-09 | 2019-07-05 | 东南大学 | 基于Krein空间的水面舰艇导航***的鲁棒自适应滤波方法 |
| CN110579740A (zh) * | 2019-09-17 | 2019-12-17 | 大连海事大学 | 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法 |
| CN111504324A (zh) * | 2020-04-27 | 2020-08-07 | 西北工业大学 | 一种噪声自适应滤波的水下组合导航方法 |
| CN112284384A (zh) * | 2020-10-26 | 2021-01-29 | 东南大学 | 考虑量测异常的集群式多深海潜航器的协同定位方法 |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116222582A (zh) * | 2023-05-10 | 2023-06-06 | 北京航空航天大学 | 一种基于变分贝叶斯推断多物理场自适应组合导航方法 |
| CN116242350A (zh) * | 2023-05-12 | 2023-06-09 | 北京航空航天大学 | 一种空间分布式偏振/惯导协同定位方法 |
| CN116358564A (zh) * | 2023-06-01 | 2023-06-30 | 中国人民解放军战略支援部队航天工程大学 | 无人机蜂群质心运动状态跟踪方法、***、设备及介质 |
| CN116358564B (zh) * | 2023-06-01 | 2023-07-28 | 中国人民解放军战略支援部队航天工程大学 | 无人机蜂群质心运动状态跟踪方法、***、设备及介质 |
| CN116680500A (zh) * | 2023-06-12 | 2023-09-01 | 哈尔滨工程大学 | 水下航行器在非高斯噪声干扰下的位置估计方法及*** |
| CN116680500B (zh) * | 2023-06-12 | 2024-03-22 | 哈尔滨工程大学 | 水下航行器在非高斯噪声干扰下的位置估计方法及*** |
| CN116667390A (zh) * | 2023-07-27 | 2023-08-29 | 华北电力大学（保定） | 一种基于动态面一致算法的负荷频率控制方法 |
| CN116667390B (zh) * | 2023-07-27 | 2023-09-29 | 华北电力大学（保定） | 一种基于动态面一致算法的负荷频率控制方法 |
| CN117250970A (zh) * | 2023-11-13 | 2023-12-19 | 青岛澎湃海洋探索技术有限公司 | 基于模型嵌入生成对抗网络实现auv故障检测的方法 |
| CN117250970B (zh) * | 2023-11-13 | 2024-02-02 | 青岛澎湃海洋探索技术有限公司 | 基于模型嵌入生成对抗网络实现auv故障检测的方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112284384A (zh) | 2021-01-29 |
| CN112284384B (zh) | 2023-11-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2022088797A1 (zh) | 考虑量测异常的集群式多深海潜航器的协同定位方法 | |
| WO2020062791A1 (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
| CN113819906B (zh) | 一种基于统计相似度量的组合导航鲁棒滤波方法 | |
| CN110514203B (zh) | 一种基于isr-ukf的水下组合导航方法 | |
| CN110132308B (zh) | 一种基于姿态确定的usbl安装误差角标定方法 | |
| Bo et al. | Cooperative localisation of AUVs based on Huber-based robust algorithm and adaptive noise estimation | |
| CN112015086B (zh) | 一种欠驱动水面船有限时间路径跟踪输出反馈控制方法 | |
| CN112747748A (zh) | 一种基于逆向解算的领航auv导航数据后处理方法 | |
| CN116295386A (zh) | 一种基于多传感器融合的水下航行器导航控制***及方法 | |
| Peng et al. | Marginalized Point Mass Filter with Estimating Tidal Depth Bias for Underwater Terrain‐Aided Navigation | |
| CN115855049A (zh) | 基于粒子群优化鲁棒滤波的sins/dvl导航方法 | |
| CN115096302A (zh) | 捷联惯性基导航***信息滤波鲁棒对准方法、***及终端 | |
| Xu et al. | Accurate two-step filtering for AUV navigation in large deep-sea environment | |
| Wang et al. | A novel adaptive sliding observation-based cooperative positioning algorithm under factor graph framework for multiple UUVs | |
| CN113218421B (zh) | 北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法 | |
| CN114459476A (zh) | 基于虚拟速度量测的水下无人潜航器测流dvl/sins组合导航方法 | |
| CN116608864A (zh) | 一种通信时延影响下基于因子图的auv协同定位方法 | |
| CN111829511A (zh) | 一种基于m估计的auv组合导航方法及*** | |
| CN116380067A (zh) | 一种适应于挑战环境下的无人艇旋转调制惯导***及方法 | |
| CN116358554A (zh) | 具有异常观测处理能力的水下航行器因子图融合导航方法 | |
| CN113670303B (zh) | 一种基于rbf神经网络的sins/dvl组合导航流速补偿方法 | |
| CN113156368B (zh) | 一种基于因子图的误差参数辨识协同定位方法 | |
| CN114577211A (zh) | 考虑洋流影响的基于因子图的主从式auv协同定位方法 | |
| CN112697145A (zh) | 一种基于ckf的主从式多水下无人潜器协同定位方法 | |
| Hao et al. | Research on optimized m-estimate arithmetic in ins/usbl integrated navigation system |
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: 21884532 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: 21884532 Country of ref document: EP Kind code of ref document: A1 |