CN114061592B - Adaptive robust AUV navigation method based on multiple models - Google Patents

Adaptive robust AUV navigation method based on multiple models Download PDF

Info

Publication number
CN114061592B
CN114061592B CN202111430297.1A CN202111430297A CN114061592B CN 114061592 B CN114061592 B CN 114061592B CN 202111430297 A CN202111430297 A CN 202111430297A CN 114061592 B CN114061592 B CN 114061592B
Authority
CN
China
Prior art keywords
submodel
value
time
model
covariance
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
CN202111430297.1A
Other languages
Chinese (zh)
Other versions
CN114061592A (en
Inventor
张昕
翟宁
张迪
何波
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Qingdao Pengpai Ocean Exploration Technology Co ltd
Original Assignee
Qingdao Pengpai Ocean Exploration Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Qingdao Pengpai Ocean Exploration Technology Co ltd filed Critical Qingdao Pengpai Ocean Exploration Technology Co ltd
Priority to CN202111430297.1A priority Critical patent/CN114061592B/en
Publication of CN114061592A publication Critical patent/CN114061592A/en
Application granted granted Critical
Publication of CN114061592B publication Critical patent/CN114061592B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/20—Instruments for performing navigational calculations
    • 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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass

Landscapes

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

Abstract

The invention relates to the field of autonomous underwater vehicle navigation and positioning, in particular to a multi-model-based adaptive robust AUV navigation method. The method comprises the following steps: s1, multi-model input interaction; s2, model filtering: s3, updating the model probability; and S4, multi-model estimation fusion. The sensor observation noise covariance estimation method based on the adaptive filtering comprises the following steps that two submodels with different parameters based on the adaptive filtering can estimate the covariance of observation noise of a sensor in real time, can better describe the statistical characteristics of the observation noise, are suitable for complex and changeable marine environments, and improve the filtering positioning precision; the abnormal value detector submodel can detect and isolate the abnormal value observed by the sensor in real time, and utilizes a machine learning method to perform online regression on the pseudo displacement position when the abnormal value is observed, so that the abnormal value detector submodel acts on each submodel, can isolate the observation abnormal, and maintain the high navigation precision and robustness of the AUV when the abnormal value is observed.

Description

Self-adaptive robust AUV navigation method based on multiple models
Technical Field
The invention relates to the field of autonomous underwater vehicle navigation and positioning, in particular to a multi-model-based adaptive robust AUV navigation method.
Background
The existing AUV navigation methods, such as Extended Kalman Filter (EKF for short), unscented Kalman Filter (UKF for short), etc., consider that the statistical properties of the sensor observation noise are known a priori. However, the marine environment in which the AUV performs tasks is complex and variable, and navigation related sensors carried by the AUV are easily interfered by the external environment. Therefore, the statistical property of the observation noise of the navigation sensor carried by the AUV is changed in real time, and the statistical property of the observation noise of the sensor, which is not changed a priori, may affect the filtering precision, even may cause filtering divergence.
Meanwhile, the navigation sensor carried by the AUV is susceptible to interference of an abnormal value during observation, and particularly, a Doppler Velocimeter (DVL) is very susceptible to influences of marine organisms, seabed gully and the like in a marine environment due to the principle characteristics of the sensor, so that the abnormal value is observed. The existing AUV navigation filtering method has no robustness on an abnormal value in observation, and the observation of the abnormal value can cause great deviation of a filtering estimation track, so that the AUV positioning precision is influenced.
Disclosure of Invention
The invention aims to overcome the defects in the prior art, and provides a multi-model-based adaptive robust AUV navigation method which can improve AUV navigation positioning precision and maintain high navigation precision and strong robustness of AUV when abnormal values occur.
The technical scheme of the invention is as follows: a multi-model based adaptive robust AUV navigation method, wherein the method comprises the following steps,
s1, multi-model input interaction:
filtering estimation result of each submodel at k-1 time
Figure BDA0003379906680000011
And the mixing probability mu ij(k-1) Combining and calculating to obtain the mixed state of the sub-model j at the time k
Figure BDA0003379906680000012
Sum mixture covariance P 0j(k-1) As an initial filtering input for each submodel;
s2, model filtering:
s2.1, constructing an abnormal value detector submodel:
s2.1.1 time updating process: obtaining the one-step predicted value of the state of the AUV navigation system of the abnormal value detector submodel from the time k-1 to the time k
Figure BDA0003379906680000013
And one-step prediction covariance value P j(k|k-1) ；
S2.1.2 measurement update process:
calculating to obtain the observed predicted value of the abnormal value detector submodel system state
Figure BDA0003379906680000021
The observed value and the observed predicted value which are acquired in real time by using the current sensor
Figure BDA0003379906680000022
Calculating to obtain residual error v between the two j(k) And constructing a fault detection function theta k ；
If theta k ≤T D If the current submodel is in the k-time state, the sensor observes no abnormal value at the moment, and the state estimation of the current submodel at the k-time is obtained according to the standard UKF algorithm
Figure BDA0003379906680000025
Sum covariance estimation P j(k) ：
Figure BDA0003379906680000026
Figure BDA0003379906680000027
If theta is k ＞T D If the abnormal value detector submodel detects that the abnormal value appears at the moment k when the sensor is observed, the AUV displacement pseudo value delta in the current time step is obtained by online regression through a machine learning method x* ,δ y* And superposing the displacement pseudo value to the state estimation position at the time k-1 to obtain the final position state quantities of the north direction and the east direction of the AUV estimated by the abnormal value detector submodel at the time k, wherein the estimation of the rest state quantities adopts the system state one-step prediction value of the abnormal value detector submodel:
Figure BDA0003379906680000029
system state one step prediction at time kMeasured value covariance estimation P j(k) Comprises the following steps:
Figure BDA00033799066800000210
s2.2, two adaptive sub-models with different parameters are constructed:
when the abnormal value detector submodel does not detect abnormal observation, the two adaptive submodels adopt VB adaptive UKF algorithms with different parameters to execute model filtering, and the method specifically comprises the following steps:
s2.2.1 time updating process: obtaining AUV navigation system state one-step predicted value of two self-adaptive sub-models from k-1 time to k time
Figure BDA00033799066800000211
One-step prediction covariance value P j(k|k-1) One-step predicted value gamma of the degree of freedom parameter j(k|k-1) One-step prediction value V of scale matrix j(k|k-1) ；
S2.2.2 measurement updating process: iterative computation of the observed noise covariance R by means of a loop k To obtain the final state estimate at time k for the two adaptive sub-models
Figure BDA00033799066800000212
Covariance estimation P j(k) Sum scale matrix estimate V j(k) ；
When the abnormal value detector submodel detects an abnormal observation value:
firstly, synchronizing step S2.2.1 to obtain a system state one-step predicted value of two adaptive sub-models from the time k-1 to the time k
Figure BDA0003379906680000031
And one-step prediction covariance value P j(k|k-1) (ii) a Then, synchronizing step S2.2.2, measuring and updating the normal running of the process, at this time, superposing the displacement pseudo values obtained in step S2.1.2 to the state estimation of the system at the time k-1 to obtain the state quantities of the north and east final positions of the AUV estimated by the two self-adaptive submodels at the time k, and estimating the rest state quantitiesThe system state one-step prediction value is counted as two self-adaptive sub-models:
Figure BDA0003379906680000032
p obtained by covariance estimation synchronization step S2.2.2 of two adaptive sub-models at time k j(k) ；
S3, updating the model probability;
and S4, multi-model estimation fusion to obtain system state estimation and system state covariance estimation at the moment k.
In the invention, before the step S1, an AUV navigation system state model and an observation model are established;
let the system state vector at time k be:
Figure BDA0003379906680000033
wherein x and y respectively represent the north and east position information of AUV at the time k in the UTM coordinate system,
Figure BDA0003379906680000034
course angle, v, representing AUV at time k x And v y Respectively represents the forward and the right speed of the AUV at the time k under the front and the right lower coordinate systems of the carrier, a x And a y Respectively representing acceleration information, w, corresponding to the velocity z Indicating angular velocity information corresponding to a heading angle;
the AUV navigation system is expressed by adopting a discrete time state space model, t represents a unit sampling time interval, and the state equation of the navigation system is set as follows:
X k ＝f(X k-1 ,m k-1 )
Figure BDA0003379906680000041
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003379906680000042
representing Gaussian white noise with the mean value of 0 and covariance of Q, wherein Q is a preset value;
let the system observation vector at time k be:
Figure BDA0003379906680000043
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003379906680000044
the course angle measured by an attitude sensor carried by an AUV at the moment k, a xm And a ym Forward and right accelerations w measured by attitude sensor at time k in AUV carrier coordinate system zm Is angular velocity data, v, corresponding to the course angle measured by the attitude sensor at time k xm And v ym Distributing and representing the forward and right speeds of the AUV carrier coordinate system measured by the DVL at the k moment;
the observation equation of the AUV navigation system is as follows:
Z k ＝H k X k +r k
wherein, H = [0 = 6×2 I 6×6 ]To observe the matrix, r k The noise is Gaussian white noise with the average value of 0 and the covariance of R, and R is a preset value.
The step S1 includes the following steps: the switching of the submodels is based on a Markov process, and the switching of each submodel is determined by a Markov probability transition matrix A:
Figure BDA0003379906680000045
wherein, a ij Represents the transition probability from submodel i to submodel j, the model probability of the submodel i is mu i R represents the number of submodels employed;
the filtering estimation result of each submodel at the moment of k-1 in the multi-model input interaction stage
Figure BDA0003379906680000051
And the mixing probability mu ij(k-1) Combining to obtain the initial input of each submodel at the time k, wherein the mixed probability from the submodel i to the submodel j at the time k-1 is as follows:
Figure BDA0003379906680000052
wherein
Figure BDA0003379906680000053
Predicted probability for submodel j:
Figure BDA0003379906680000054
the mixing state of the submodel j is:
Figure BDA0003379906680000055
the mixed covariance of model j is:
Figure BDA0003379906680000056
mixed state
Figure BDA0003379906680000057
Sum-mixture covariance P 0j(k-1) For the initial filtering input of each submodel.
The step S2.1.1 comprises the following specific steps:
s2.1.1 samples 2d +1 sigma points based on unscented transformation, and if the state vector of any sigma point is ξ, then
Figure BDA0003379906680000058
Figure BDA0003379906680000059
Figure BDA00033799066800000510
Wherein, b is the serial number of the sigma point; j is the sub-model number; d is the dimension of the system state; lambda is a scale parameter and is a set value;
s2.1.1.2, calculating the weight w of the sigma point:
Figure BDA00033799066800000511
Figure BDA00033799066800000512
Figure BDA00033799066800000513
wherein the content of the first and second substances,
Figure BDA00033799066800000514
in order to be the weight of the mean value,
Figure BDA00033799066800000515
the covariance weight, and alpha and beta are preset parameters;
s2.1.1.3, calculating to obtain a system state one-step predicted value of the abnormal value detector sub-model
Figure BDA0003379906680000061
And one-step prediction covariance value P j(k|k-1) ：
Figure BDA0003379906680000062
Figure BDA0003379906680000063
Wherein, f (xi) j(b,k-1) ) For AUV navigation system equation of state, Q j(k) Is the process noise covariance, is the set point.
In the above step S2.1.2, a fault detection function theta is constructed k The method comprises the following specific steps:
s2.1.2.1 one-step prediction value of system state of abnormal value detector submodel
Figure BDA0003379906680000065
UT conversion is carried out to obtain a state one-step predicted value epsilon of 2d +1 sigma points j(b,k|k-1) And obtaining the observation predicted value z of each sigma point according to the following formula j(b,k|k-1) ：
z j(b,k|k-1) ＝H k ε j(b,k|k-1) ,b＝0～2d；
Wherein H k For the observation matrix at the time k, the observation predicted value of the abnormal value detector sub-model system state is obtained by using the following formula
Figure BDA0003379906680000066
Figure BDA0003379906680000067
S2.1.2.2 weighting calculation to obtain the observation covariance of the system
Figure BDA0003379906680000068
And covariance between observation and prediction
Figure BDA0003379906680000069
Figure BDA00033799066800000610
Figure BDA00033799066800000611
Wherein R is k The covariance of the observed noise is a preset value;
s2.1.2.3 based on observed covariance
Figure BDA00033799066800000612
And covariance between observation and prediction
Figure BDA00033799066800000613
Calculating to obtain a Kalman gain value K j(k) ：
Figure BDA00033799066800000614
S2.1.2.4 obtaining residual error v between the observation value collected by the current sensor in real time and the observation predicted value obtained by calculation j(k) And constructing a fault detection function theta k ：
Figure BDA0003379906680000071
Figure BDA0003379906680000072
In the step s2.1.2, the AUV displacement false value in the current time step is obtained through the following specific steps:
step A, constructing a regression training data set:
let the e-th training data vector be
Figure BDA0003379906680000073
Respectively a course angle, a pitch angle and a roll angle acquired by an attitude sensor, forward speed, right speed and downward speed measured by a speed sensor under an AUV carrier coordinate system, and forward acceleration, right acceleration and downward acceleration measured by the attitude sensor under the AUV carrier coordinate systemThe angle and the angular speed information which is measured by the attitude sensor and corresponds to the course angle, the pitch angle and the roll angle;
let the e-th training data label be y e ＝[δ xe ,δ ye ]Wherein δ xe And delta ye Respectively representing north and east-west displacement amounts estimated by the AUV navigation system between time f and time f-1:
Figure BDA0003379906680000074
Figure BDA0003379906680000075
at time k, performing online regression on the training data pairs of the previous M time steps as training data for regression, wherein the training data set at time k is as follows:
D＝{(u k-M ,y k-M ),(u k-M+1 ,y k-M+1 ),…,(u k-1 ,y k-1 )}
b, obtaining displacement pseudo values delta in the north direction and the east direction x* ,δ y* ；
Let u be the test data at time k k Training data set D and test data u at time k k Inputting the output y into GPR to perform online regression calculation to obtain the output y at the k moment k* ：
y k* ＝[δ x* ,δ y* ]。
The step S2.2.1 comprises the following specific steps:
s2.2.1.1 samples 2d +1 sigma points based on UT transformation, and if the state vector of any sigma point is xi, then
Figure BDA0003379906680000076
Figure BDA0003379906680000077
Figure BDA0003379906680000078
Wherein j is the serial number of the sub-model, and d is the dimension of the system state; lambda is a scale parameter and is a set value;
s2.2.1.2, calculating the weight w of the sigma point:
Figure BDA0003379906680000081
Figure BDA0003379906680000082
Figure BDA0003379906680000083
wherein the content of the first and second substances,
Figure BDA0003379906680000084
in order to be the weight of the mean value,
Figure BDA0003379906680000085
is a covariance weight;
s2.2.1.3, calculating one-step predicted value xi of each sigma point j(b,k|k-1) Weighting each sigma point by using the weight, and calculating to obtain a system state one-step predicted value of the submodel II or the submodel III
Figure BDA0003379906680000086
And one-step prediction covariance value P j(k|k-1) ：
Figure BDA0003379906680000087
Figure BDA0003379906680000088
S2.2.1.4 calculating one-step predicted value gamma of degree of freedom parameter of submodel inverse Weishate distribution j(k|k-1) One-step predictor V of sum-scale matrix j(k|k-1) ：
γ j(k|k-1) ＝ρ(γ j(k-1) -d-1)+d+1
Figure BDA0003379906680000089
Wherein, gamma is k Is a degree of freedom parameter; v k Is a scale matrix; ρ is a real number and 0<ρ≤1；
Figure BDA00033799066800000810
Where I is the identity matrix.
The step S2.2.2 comprises the following specific steps:
s2.2.2.1, calculating a measurement update value of the sub-model inverse Weishate distribution freedom degree parameter:
γ j(k) ＝γ j(k|k-1) +1
s2.2.2.2 System State one-step prediction value for two adaptive sub-models
Figure BDA00033799066800000811
UT conversion is carried out again to obtain 2d +1 sigma point state one-step predicted values
Figure BDA00033799066800000812
Obtaining the observation predicted value of each sigma point according to the following formula
Figure BDA00033799066800000813
Figure BDA00033799066800000814
Then, the observation predicted value of the current self-adaptive sub-model system state is obtained according to the following formula
Figure BDA0003379906680000091
Figure BDA0003379906680000092
S2.2.2.3 calculating the observed noise covariance R of the current system k ：
Figure BDA0003379906680000093
Wherein the content of the first and second substances,
Figure BDA0003379906680000094
s2.2.2.4 weighting calculation to obtain the observation covariance of the system
Figure BDA0003379906680000095
And covariance between observation and prediction
Figure BDA0003379906680000096
Figure BDA0003379906680000097
Figure BDA0003379906680000098
S2.2.2.5 calculating to obtain a Kalman gain value according to the observation covariance and the covariance between observation and prediction
Figure BDA0003379906680000099
Figure BDA00033799066800000910
S2.2.2.6 State of computing SystemNew
Figure BDA00033799066800000911
Covariance update
Figure BDA00033799066800000912
Sum-scale matrix update
Figure BDA00033799066800000913
Figure BDA00033799066800000914
Figure BDA00033799066800000915
Figure BDA00033799066800000916
S2.2.2.7l is iteration times, the initial value of l is set to be 0, the step S2.2.2 to the step S2.2.6 are iteratively calculated in a circulating mode, l is accumulated according to the iteration times, when l = N, the circulation is ended, and the final state estimation of the current sub-model at the time k is obtained
Figure BDA00033799066800000917
Covariance estimation P j(k) Sum scale matrix estimate V j(k) N is a set value:
Figure BDA00033799066800000918
Figure BDA00033799066800000919
Figure BDA00033799066800000920
the step S3 includes the following steps:
after each submodel obtains respective state estimation through model filtering, the model probability is updated, and the likelihood function of the submodel j is as follows:
Figure BDA0003379906680000101
wherein v is j(k) As residual terms of submodel j, S j(k) Is the observed covariance of the submodel j; m is the dimension of the system observation vector; model probability mu of submodel j at time k j(k) Comprises the following steps:
Figure BDA0003379906680000102
wherein c is a normalization constant:
Figure BDA0003379906680000103
the step S4 includes the following steps:
model probability mu based on k time obtained in step S3 j(k) Obtaining the final state estimation of the AUV navigation system at the k moment by weighting and fusing the estimation results of each submodel
Figure BDA0003379906680000104
Figure BDA0003379906680000105
Final system state covariance estimate P k Comprises the following steps:
Figure BDA0003379906680000106
the beneficial effects of the invention are:
(1) Two submodels with different parameters based on adaptive filtering can estimate the covariance of the observed noise of the sensor in real time, can better describe the statistical characteristics of the observed noise, are suitable for complex and changeable marine environments, and improve the filtering positioning precision;
(2) The abnormal value detector submodel can detect and isolate abnormal values observed by the sensor in real time, and can perform online regression on the pseudo displacement position by using a machine learning method when the abnormal values are observed to act on each submodel, so that the abnormal values can be isolated, and the high navigation accuracy and robustness of the AUV when the abnormal values are observed can be maintained.
Drawings
FIG. 1 is a schematic flow diagram of the present invention;
FIG. 2 is a schematic diagram comparing a trajectory obtained by the method proposed by the invention and a trajectory obtained by the prior art;
FIG. 3 (a) is a diagram of the forward velocity of a DVL in an AUV carrier coordinate system;
fig. 3 (b) is a schematic diagram comparing a trajectory obtained by the method proposed by the present invention after insertion of an anomalous observation with a trajectory obtained by the prior art.
Detailed Description
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in detail below.
In the following description, specific details are set forth in order to provide a thorough understanding of the present invention. The invention can be embodied in many different forms than those described herein and those skilled in the art will appreciate that the invention is susceptible to similar forms of embodiment without departing from the spirit and scope of the invention. Therefore, the present invention is not limited to the specific embodiments disclosed below.
The multi-model-based adaptive robust AUV navigation method comprises the following steps.
Firstly, an AUV navigation system state model and an observation model are constructed.
The AUV executes tasks in an underwater environment, and the navigation system provides accurate position information for the AUV. Firstly, an AUV navigation system is constructed and divided into a system state model and a measurement model.
The state of the AUV navigation system can reflect the information of the current position, attitude and the like of the AUV estimated by the navigation system. Let the system state vector at time k be:
Figure BDA0003379906680000111
wherein x and y respectively represent the north and east position information of AUV at the time k in the Universal Transverse Mercator (UTM) coordinate system,
Figure BDA0003379906680000112
indicating the course angle, v, of the AUV at time k x And v y Respectively represents the forward and the right speed of the AUV at the time k under the front and the right lower coordinate systems of the carrier, a x And a y Respectively representing acceleration information, w, corresponding to the velocity z Indicating angular velocity information corresponding to a heading angle.
The AUV navigation system is expressed by adopting a discrete time state space model, t represents a unit sampling time interval, and the state equation of the navigation system is set as follows:
X k ＝f(X k-1 ,m k-1 )
Figure BDA0003379906680000113
wherein the content of the first and second substances,
Figure BDA0003379906680000121
the mean is 0, and the covariance is Q, which is a predetermined value.
The AUV navigation system observation can reflect the currently and actually detected information such as AUV attitude, speed and the like. Let the system observation vector at time k be:
Figure BDA0003379906680000122
wherein the content of the first and second substances,
Figure BDA0003379906680000123
indicating the course angle, a, measured by an attitude sensor mounted on the AUV at time k xm And a ym Forward and right accelerations w measured by attitude sensor at time k in AUV carrier coordinate system zm Is angular velocity data v corresponding to course angle measured by attitude sensor at time k xm And v ym The distribution represents the forward and right velocities measured at time k DVL in the AUV carrier coordinate system. The observation equation of the AUV navigation system is as follows:
Z k ＝H k X k +r k
wherein, H = [0 = 6×2 I 6×6 ]To observe the matrix, r k The noise is Gaussian white noise with the mean value of 0 and the covariance of R, and R is a preset value.
And secondly, constructing multi-model AUV navigation.
The adaptive robust AUV navigation method provided by the invention is based on an Interactive Multiple Model (IMM) method and comprises four stages of input interaction, model filtering, model probability updating and estimation fusion. The submodel switching of the multi-model AUV navigation method is based on a Markov process, and the switching of each submodel is determined by a Markov probability transfer matrix A:
Figure BDA0003379906680000124
wherein, a ij Representing the transition probability from submodel i to submodel j, the model probability of submodel i being mu i And r represents the number of adopted submodels, and the method adopts three submodels to run in parallel, namely r =3 in the formula, so as to obtain more accurate AUV positioning estimation. This step includes the following detailed steps.
First, multi-model input interaction.
The filtering estimation result of each submodel at the moment of k-1 in the multi-model input interaction stage
Figure BDA0003379906680000125
And the mixing probability mu ij(k-1) And combining, and calculating to obtain the initial input of each sub-model at the time k. The mixing probability from submodel i to submodel j at time k-1 is:
Figure BDA0003379906680000131
wherein
Figure BDA0003379906680000137
Predicted probability for submodel j:
Figure BDA0003379906680000132
the mixing state of the submodel j is:
Figure BDA0003379906680000133
the mixed covariance of model j is:
Figure BDA0003379906680000134
wherein, in a mixed state
Figure BDA0003379906680000135
Sum-mixture covariance P 0j(k-1) I.e. the initial filter input for each submodel j.
Second, model filtering.
And after the mixed state and the mixed covariance of each submodel are obtained in the multi-model input interaction stage, the mixed state and the mixed covariance are respectively used as initial inputs of the three submodels. And three submodels run in parallel, wherein the submodel I is an abnormal value detector submodel, and the submodel II and the submodel III are adaptive submodels with different parameters. In the step, each submodel is respectively constructed for filtering.
And (I) constructing a submodel I.
The submodel I can also be called as an abnormal value detector submodel, the abnormal value detector submodel carries out model filtering based on the existing UKF algorithm, and residual errors x are introduced in the filtering process 2 And detecting an abnormal value in the sensor observation, wherein once the abnormal value is detected, gaussian Process Regression (GPR for short) is used for online Regression to obtain an AUV displacement pseudo value in the current time step so as to maintain the AUV navigation precision and robustness when the abnormal value occurs. The method specifically comprises the following steps.
1. And (4) a time updating process.
In the time updating process, the submodel I calculates and obtains the predicted value of the state of the AUV navigation system from the time k-1 to the time k by using the state model of the AUV navigation system constructed in the first step. The method specifically comprises the following steps.
(1) Sampling 2d +1 sigma points based on Unscented Transformation (UT), and setting the state vector of any sigma point as xi, then
Figure BDA0003379906680000136
Figure BDA0003379906680000141
Figure BDA0003379906680000142
Wherein, b represents the serial number of the sigma point; j represents the serial number of the sub-model, and j =1 in the embodiment; d is the dimension of the system state, and d =8 in the embodiment; and lambda is a scale parameter and is a set value.
(2) Calculating the weight w of the sigma point:
Figure BDA0003379906680000143
Figure BDA0003379906680000144
Figure BDA0003379906680000145
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003379906680000146
the weight of the mean value is represented by,
Figure BDA0003379906680000147
representing covariance weights, and α and β are preset parameters.
(3) Based on the AUV navigation system state model constructed in the first step, calculating to obtain one-step predicted value xi of each sigma point j(b,k|k-1) And then weighting each sigma point by using the weight, and calculating to obtain a system state one-step predicted value and a one-step predicted covariance value of the sub-model I:
Figure BDA0003379906680000148
Figure BDA0003379906680000149
wherein Q is j(k) Is the process noise covariance, is the set point.
2. A measurement update procedure.
And the AUV carries navigation related sensors, acquires information such as the current attitude, speed and the like of the AUV in real time, is used for the measurement updating process of the submodel I, corrects the prediction deviation of the time updating process, and obtains the state estimation value of the submodel I at the final k moment. The method specifically comprises the following steps.
(1) Carrying out UT conversion again on the system state one-step predicted value of the sub model I to obtain state one-step predicted value epsilon of 2d +1 sigma points j(b,k|k-1) (ii) a Then observing by using the navigation system constructed in the first stepThe model obtains the observation predicted value z of each sigma point j(b,k|k-1) (ii) a Then, the observation predicted value of the current submodel I system state is obtained by weight weighting calculation
Figure BDA00033799066800001410
z j(b,k|k-1) ＝H k ε j(b,k|k-1) ,b＝0～2d；
Figure BDA0003379906680000151
Wherein H k Representing the observation matrix at time k.
(2) Obtaining the system observation covariance by weighted calculation
Figure BDA0003379906680000152
And covariance between observation and prediction
Figure BDA0003379906680000153
Figure BDA0003379906680000154
Figure BDA0003379906680000155
Wherein R is k The covariance of the noise is observed and is a preset value.
(3) Calculating to obtain a Kalman gain value K according to the observation covariance and the covariance between observation and prediction j(k)
Figure BDA0003379906680000156
(4) Calculating to obtain a residual error v between an observed value acquired by the current sensor in real time and a calculated observed predicted value j(k) And constructing a fault detection function theta k ：
Figure BDA0003379906680000158
Figure BDA0003379906680000159
For fault detection function and T D The size of the T is respectively treated by the following method, wherein T D Is a preset threshold value.
(1) If theta k ≤T D And then, considering that the sensor observes no abnormal value at the moment, and calculating the state estimation and covariance estimation of the current submodel at the moment k according to a standard UKF algorithm:
Figure BDA00033799066800001511
Figure BDA00033799066800001512
(2) if theta is k ＞T D If the sub-model I detects that the abnormal value appears at the moment k when the sensor is observed, obtaining the AUV displacement pseudo value in the current time step by utilizing GPR regression:
a. constructing a regression training data set:
let the e-th training data vector be
Figure BDA0003379906680000161
Respectively obtaining a course angle, a pitch angle and a roll angle which are obtained by an attitude sensor, forward speed, right speed and downward speed under an AUV carrier coordinate system measured by a speed sensor, forward acceleration, right acceleration and downward acceleration under the AUV carrier coordinate system measured by the attitude sensor, and angular speed information corresponding to the course angle, the pitch angle and the roll angle measured by the attitude sensor;
let the e-th training data label be y e ＝[δ xe ,δ ye ]Wherein δ xe And delta ye Respectively representing north and east-west displacement amounts estimated by the AUV navigation system between time f and time f-1:
Figure BDA0003379906680000162
Figure BDA0003379906680000163
b. obtaining displacement false values delta in the north direction and the east direction x* ,δ y* ；
At time k, performing online regression on the training data pairs of the previous M time steps as training data for regression, namely the training data set at time k is as follows:
D＝{(u k-M ,y k-M ),(u k-M+1 ,y k-M+1 ),…,(u k-1 ,y k-1 )}
let the test data at time k be u k A training data set D and test data u at the time k k The output at the time k can be obtained by inputting the data into GPR to perform online regression calculation:
y k* ＝[δ x* ,δ y* ]
and superposing the output obtained by GPR regression on the state estimation position at the time k-1 to obtain the final position state quantities of the north direction and the east direction of the AUV estimated by the sub-model I at the time k, and simultaneously, in order to isolate the influence of the observation abnormal value, adopting one-step prediction values of the sub-model to obtain the state estimation of other postures, speeds and the like:
Figure BDA0003379906680000164
the covariance estimation of the submodel I at the time k is as follows:
Figure BDA0003379906680000165
and (II) constructing a submodel II and a submodel III.
The submodel II and the submodel III are adaptive submodels. When the submodel I does not detect abnormal observation, performing model filtering on the submodel II and the submodel III by adopting a Variational Bayesian (VB) based adaptive UKF filtering method; and once the sub-model I detects an abnormal value, filtering and estimating the displacement obtained by filtering the sub-model II and the sub-model III by using the pseudo displacement obtained by GPR online regression in the sub-model I. The processing procedure for the above two cases will be described in detail below.
A. When no abnormal observation is detected by submodel i:
at the moment, the sub-model II and the sub-model III adopt VB self-adaptive UKF algorithms with different parameters to execute model filtering, and the method specifically comprises the following steps.
1. A time update procedure.
And calculating to obtain the AUV navigation system state predicted value of the submodel II or the submodel III from the time k-1 to the time k by using the constructed AUV navigation system model in the first step.
(1) Sampling 2d +1 sigma points based on UT transformation, and if the state vector of any sigma point is ξ, then
Figure BDA0003379906680000171
Figure BDA0003379906680000172
Figure BDA0003379906680000173
Wherein j represents a sub-model number, and j =2 or 3 in this embodiment; d is the dimension of the system state, d =8 in this embodiment; and lambda is a scale parameter and is a set value.
(2) Calculating the weight w of the sigma point:
Figure BDA0003379906680000174
Figure BDA0003379906680000175
Figure BDA0003379906680000176
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003379906680000177
the weight of the mean value is represented by,
Figure BDA0003379906680000178
representing the covariance weights.
(3) Calculating one-step predicted value xi of each sigma point based on the AUV navigation system state model constructed in the first step j(b,k|k-1) And then weighting each sigma point by using the weight, and calculating to obtain a one-step predicted value and a one-step predicted covariance value of the system state of the sub-model II or the sub-model III:
Figure BDA0003379906680000181
Figure BDA0003379906680000182
(4) Calculating one-step predicted value of degree of freedom parameter and scale matrix of sub-model inverse Weishate distribution
γ j(k|k-1) ＝ρ(γ j(k-1) -d-1)+d+1
Figure BDA0003379906680000183
Wherein, γ k Is a degree of freedom parameter; v k Is a scale matrix; ρ is a real number and 0<ρ≤1；
Figure BDA0003379906680000184
Where I is the identity matrix.
2. A measurement update procedure.
And acquiring information such as the current attitude, the current speed and the like of the AUV by using the navigation related sensor carried by the AUV, and using the information for the measurement updating process of the submodel II or the submodel III to correct the prediction deviation of the time updating process to obtain the state estimation value of the final k moment of the submodel II or the submodel III. The method specifically comprises the following steps.
(1) Calculating the measurement update value of the sub-model inverse Weisset distribution freedom degree parameter:
γ j(k) ＝γ j(k|k-1) +1
(2) Performing UT conversion on the one-step predicted value of the system state of the sub-model II or the sub-model III again to obtain the state one-step predicted value of 2d +1 sigma points
Figure BDA0003379906680000185
Then, the observation predicted value of each sigma point is calculated and obtained by utilizing the navigation system observation model constructed in the first step
Figure BDA0003379906680000186
Then, the observation predicted value of the current sub-model system state is obtained by weight weighting calculation
Figure BDA0003379906680000187
Figure BDA0003379906680000188
Figure BDA0003379906680000189
And l is iteration number, an initial value of l is set to be 0, steps (2) to (6) are iteratively calculated in a loop mode, l is accumulated according to the iteration number until l = N, and the loop is exited, wherein N is a set value.
(3) Method for calculating observation noise covariance R of current system by VB principle k ：
Figure BDA0003379906680000191
Wherein the content of the first and second substances,
Figure BDA0003379906680000192
(4) Weighted calculation to obtain the system observation covariance
Figure BDA0003379906680000193
And covariance between observation and prediction
Figure BDA0003379906680000194
Figure BDA0003379906680000195
Figure BDA0003379906680000196
(5) Calculating to obtain a Kalman gain value according to the observation covariance and the covariance between the observation and the prediction
Figure BDA0003379906680000197
Figure BDA0003379906680000198
(6) Status update for computing systems
Figure BDA0003379906680000199
Covariance update
Figure BDA00033799066800001910
Sum-scale matrix update values
Figure BDA00033799066800001911
Figure BDA00033799066800001912
Figure BDA00033799066800001913
Figure BDA00033799066800001914
(7) When l = N, ending the cycle, and obtaining the final state estimation, the covariance estimation and the scale matrix estimation of the current submodel j at the moment k:
Figure BDA00033799066800001915
Figure BDA00033799066800001916
Figure BDA00033799066800001917
B. when the sub-model I detects abnormal observed values:
according to the fact that the self-adaptive filtering process based on VB normally operates when the submodel II or the submodel III does not detect abnormal observation, the system state one-step predicted value and the system state covariance estimated value of the submodel II or the submodel III at the moment of k-1 are obtained, the displacement pseudo value obtained by GPR online regression in the submodel I is superposed into the state estimation of the system at the moment of k-1, the final position state quantities of the AUV estimated by the submodel II or the submodel III at the moment of k in the north direction and the east direction are obtained, and the state estimation of other postures, speeds and the like is the system state one-step predicted value of the submodel II or the submodel III:
Figure BDA0003379906680000201
covariance estimation of the submodel j, namely the submodel II or the submodel III at the moment k, and one-step prediction covariance value P obtained in the adaptive filtering process of the submodel II or the submodel III based on VB j(k) 。
Third, the model probabilities are updated.
And after each sub-model obtains respective state estimation through model filtering, updating the model probability. The likelihood function for submodel j is:
Figure BDA0003379906680000202
wherein v is j(k) As residual terms of submodels j, S j(k) Is the observed covariance of the submodel j; m is the dimension of the system observation vector, and m =6 in the embodiment; the model probability of the submodel j at the time k can be calculated to obtain:
Figure BDA0003379906680000203
wherein c is a normalization constant:
Figure BDA0003379906680000204
fourth, multi-model estimation fusion.
Based on the model probability at the time k, the final state estimation of the AUV navigation system at the time k is obtained by weighting and fusing the estimation results of the submodels:
Figure BDA0003379906680000205
the final system state covariance estimate is:
Figure BDA0003379906680000206
the AUV state estimation at each moment is calculated in an iterative mode through the four stages of input interaction, model filtering, model probability updating and estimation fusion, so that the AUV underwater navigation positioning with high precision and high robustness is realized.
As shown in fig. 2, the dot line represents the trajectory obtained by the AUV navigation method provided by the present invention, the regular triangle line is the trajectory obtained by the existing EKF, the inverted triangle line is the trajectory obtained by the existing UKF, the diamond line is the trajectory obtained by the existing IMM-UKF, and the square line is the GPS trajectory, which are compared as true values.
Fig. 3 (a) shows the forward velocity of the DVL in the AUV carrier coordinate system, and the velocity of 15m/s is artificially inserted in 51 and 52 seconds as abnormal observation, and on the premise of abnormal observation, the trajectory obtained by the method of the present application and the existing method is compared, as shown in fig. 3 (b), wherein the dot line represents the trajectory obtained by the AUV navigation method provided by the present invention, the regular triangle line is the trajectory obtained by the existing EKF, the inverted triangle line is the trajectory obtained by the existing UKF, the diamond line is the trajectory obtained by the existing IMM-UKF, and the square line is the GPS trajectory, and the comparison is performed as a true value.
The multi-model-based adaptive robust AUV navigation method provided by the invention is described in detail above. The principles and embodiments of the present invention are explained herein using specific examples, which are presented only to assist in understanding the method and its core concepts. It should be noted that, for those skilled in the art, it is possible to make various improvements and modifications to the present invention without departing from the principle of the present invention, and those improvements and modifications also fall within the scope of the claims of the present invention. The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims (10)

1. A multi-model-based adaptive robust AUV navigation method is characterized by comprising the following steps,
s1, multi-model input interaction:
filtering estimation result of each submodel at k-1 time
Figure FDA0003861012470000011
And the mixing probability mu ij(k-1) Combining and calculating to obtain the mixed state of the sub-model j at the moment k
Figure FDA0003861012470000012
Sum-mixture covariance P 0j(k-1) As an initial filtering input for each submodel;
s2, model filtering:
s2.1, constructing an abnormal value detector submodel:
s2.1.1 time updating process: AUV navigation system state one-step predicted value of abnormal value detector submodel from k-1 time to k time
Figure FDA0003861012470000013
And one-step prediction covariance value P j(k|k-1) ；
S2.1.2 measurement update process:
calculating to obtain the observed predicted value of the abnormal value detector submodel system state
Figure FDA0003861012470000014
The observed value and the observed predicted value which are acquired in real time by using the current sensor
Figure FDA0003861012470000015
Calculating to obtain residual error v between the two j(k) And constructing a fault detection function theta k ；
If theta k ≤T D If the current submodel is in the k-time state, the sensor observes no abnormal value at the moment, and the state estimation of the current submodel at the k-time is obtained according to the standard UKF algorithm
Figure FDA0003861012470000016
Sum covariance estimation P j(k) ：
Figure FDA0003861012470000017
Figure FDA0003861012470000018
If theta k >T D If the abnormal value detector submodel detects that the abnormal value appears at the moment k when the sensor is observed, the AUV displacement pseudo value in the current time step is obtained by online regression through a machine learning method
Figure FDA00038610124700000111
And superposing the displacement pseudo values to the state estimation position at the time k-1 to obtain the final position state quantities of the north direction and the east direction of the AUV estimated by the abnormal value detector submodel at the time k, wherein the estimation of the rest state quantities adopts the system state one-step prediction value of the abnormal value detector submodel:
Figure FDA0003861012470000019
covariance estimation P of one-step predicted value of system state at moment k j(k) Comprises the following steps:
Figure FDA00038610124700000110
s2.2, two adaptive sub-models with different parameters are constructed:
when the abnormal value detector submodel does not detect abnormal observation, the two adaptive submodels adopt VB adaptive UKF algorithms with different parameters to execute model filtering, and the method specifically comprises the following steps:
s2.2.1 time updating process: obtaining AUV navigation system state one-step predicted value of two self-adaptive submodels from k-1 moment to k moment
Figure FDA0003861012470000021
One-step prediction covariance value P j(k|k-1) One-step predicted value gamma of the degree of freedom parameter j(k|k-1) One-step prediction value V of scale matrix j(k|k-1) ；
S2.2.2 measurement updating process: iterative computation of the observed noise covariance R by means of a loop k To obtain the final state estimate at time k for the two adaptive sub-models
Figure FDA0003861012470000022
Covariance estimation P j(k) Sum scale matrix estimate V j(k) ；
When the outlier detector sub-model detects an outlier observation:
firstly, synchronizing step S2.2.1 to obtain a system state one-step predicted value of two adaptive sub-models from the time k-1 to the time k
Figure FDA0003861012470000023
And one-step prediction covariance value P j(k|k-1) (ii) a However, the device is not suitable for use in a kitchenAnd then, synchronizing a step S2.2.2, measuring and updating the normal running of the process, at the moment, superposing the displacement pseudo values obtained in the step S2.1.2 to the state estimation of the system at the time k-1 to obtain the final position state quantities of the north direction and the east direction of the AUV estimated by the two self-adaptive submodels at the time k, and estimating the rest state quantities to be the system state one-step predicted values of the two self-adaptive submodels:
Figure FDA0003861012470000024
p obtained by covariance estimation synchronization step S2.2.2 of two adaptive sub-models at time k j(k) ；
S3, updating the model probability;
and S4, carrying out multi-model estimation fusion to obtain system state estimation and system state covariance estimation at the moment k.
2. The multi-model-based adaptive robust AUV navigation method according to claim 1, wherein before S1, an AUV navigation system state model and an observation model are constructed;
let the system state vector at time k be:
Figure FDA0003861012470000025
wherein x and y respectively represent the north and east position information of AUV at the time k in the UTM coordinate system,
Figure FDA0003861012470000031
course angle, v, representing AUV at time k x And v y Respectively represents the forward and the right speed of the AUV at the time k under the front-right lower coordinate system of the carrier, a x And a y Respectively representing acceleration information, w, corresponding to the velocity z Indicating angular velocity information corresponding to a heading angle;
the AUV navigation system is expressed by adopting a discrete time state space model, t represents a unit sampling time interval, and the state equation of the navigation system is set as follows:
X k ＝f(X k-1 ,m k-1 )
Figure FDA0003861012470000032
wherein the content of the first and second substances,
Figure FDA0003861012470000033
expressing Gaussian white noise with the mean value of 0 and the covariance of Q, wherein Q is a preset value;
let the system observation vector at time k be:
Figure FDA0003861012470000034
wherein the content of the first and second substances,
Figure FDA0003861012470000035
the course angle measured by an attitude sensor carried by an AUV at the moment k, a xm And a ym Forward and right accelerations w measured by attitude sensor at time k in AUV carrier coordinate system zm Is angular velocity data, v, corresponding to the course angle measured by the attitude sensor at time k xm And v ym Distributing and representing the forward and right speeds of the AUV carrier coordinate system measured by the DVL at the k moment;
the observation equation of the AUV navigation system is as follows:
Z k ＝H k X k +r k
wherein H = [0 = 6×2 I 6×6 ]To observe the matrix, r k The noise is Gaussian white noise with the average value of 0 and the covariance of R, and R is a preset value.
3. The multi-model-based adaptive robust AUV navigation method according to claim 1, wherein the step S1 comprises the following specific steps:
the switching of the submodels is based on a Markov process, and the switching of each submodel is determined by a Markov probability transfer matrix A:
Figure FDA0003861012470000041
wherein, a ij Representing the transition probability from submodel i to submodel j, the model probability of submodel i being mu i And r represents the number of submodels employed;
the filtering estimation result of each submodel at the moment of k-1 in the multi-model input interaction stage
Figure FDA0003861012470000042
And the mixing probability mu ij(k-1) Combining to obtain the initial input of each submodel at the time k, wherein the mixed probability from the submodel i to the submodel j at the time k-1 is as follows:
Figure FDA0003861012470000043
wherein
Figure FDA0003861012470000044
Predicted probability for submodel j:
Figure FDA0003861012470000045
the mixing state of the submodel j is:
Figure FDA0003861012470000046
the mixed covariance of model j is:
Figure FDA0003861012470000047
mixed state
Figure FDA0003861012470000048
Sum-mixture covariance P 0j(k-1) For the initial filtering input of each submodel.
4. The multi-model-based adaptive robust AUV navigation method according to claim 1, wherein the step S2.1.1 comprises the following specific steps:
s2.1.1 samples 2d +1 sigma points based on unscented transformation, and if the state vector of any sigma point is xi, then
Figure FDA0003861012470000049
Figure FDA00038610124700000410
When the value range of b is 1 to d;
Figure FDA00038610124700000411
when the value range of b is d +1 to 2d; wherein b is the serial number of the sigma point; j is the sub-model number; d is the dimension of the system state; lambda is a scale parameter and is a set value;
s2.1.1.2, calculating the weight w of the sigma point:
Figure FDA0003861012470000051
Figure FDA0003861012470000052
Figure FDA0003861012470000053
b ranges from 1 to 2d;
wherein the content of the first and second substances,
Figure FDA0003861012470000054
in order to be the weight of the mean value,
Figure FDA0003861012470000055
the covariance weight, and alpha and beta are preset parameters;
s2.1.1.3, calculating to obtain a system state one-step predicted value of the abnormal value detector sub-model
Figure FDA0003861012470000056
And one-step prediction covariance value P j(k|k-1) ：
Figure FDA0003861012470000057
Figure FDA0003861012470000058
Wherein, f (ξ) j(b,k-1) ) For AUV navigation system equation of state, Q j(k) Is the process noise covariance, is the set point.
5. The multi-model-based adaptive robust AUV navigation method according to claim 1, wherein in step S2.1.2, a fault detection function θ is constructed k The method comprises the following specific steps:
s2.1.2.1 one-step prediction value of system state of abnormal value detector submodel
Figure FDA0003861012470000059
UT conversion is carried out to obtain the state one-step predicted value epsilon of 2d +1 sigma points j(b,k|k-1) D is the dimension of the system state, and the observation predicted value z of each sigma point is obtained according to the following formula j(b,k|k-1) ：
z j(b,k|k-1) ＝H k ε j(b,k|k-1) B ranges from 0 to 2d
Wherein H k For the observation matrix at the time k, the observation predicted value of the abnormal value detector sub-model system state is obtained by using the following formula
Figure FDA00038610124700000510
Figure FDA00038610124700000511
S2.1.2.2 weighting calculation to obtain the observation covariance of the system
Figure FDA0003861012470000061
And covariance between observation and prediction
Figure FDA0003861012470000062
Figure FDA0003861012470000063
Figure FDA0003861012470000064
Wherein R is k The covariance of the noise is observed and is a preset value;
s2.1.2.3 covariance based on observations
Figure FDA0003861012470000065
And covariance between observation and prediction
Figure FDA0003861012470000066
Calculating to obtain a Kalman gain value K j(k) ：
Figure FDA0003861012470000067
S2.1.2.4 obtaining residual error v between the observation value collected by the current sensor in real time and the observation predicted value obtained by calculation j(k) And constructing a fault detection function theta k ：
Figure FDA0003861012470000068
Figure FDA0003861012470000069
6. The multi-model-based adaptive robust AUV navigation method according to claim 1, wherein in step S2.1.2, the AUV displacement false value in the current time step is obtained through the following specific steps:
step A, constructing a regression training data set:
let the e-th training data vector be
Figure FDA00038610124700000610
The elements are respectively a course angle, a pitch angle and a roll angle which are acquired by an attitude sensor, forward speed, right speed and downward speed which are measured by a speed sensor under an AUV carrier coordinate system, forward acceleration, right acceleration and downward acceleration which are measured by the attitude sensor under the AUV carrier coordinate system, and angular speed information which is measured by the attitude sensor and corresponds to the course angle, the pitch angle and the roll angle;
let the e-th training data label be y e ＝[δ xe ,δ ye ]Wherein δ xe And delta ye Respectively representing north and east-west displacement amounts estimated by the AUV navigation system between time f and time f-1:
Figure FDA00038610124700000611
Figure FDA00038610124700000612
at time k, performing online regression on the training data pairs of the previous M time steps as training data for regression, wherein the training data set at time k is as follows:
D＝{(u k-M ,y k-M ),(u k-M+1 ,y k-M+1 ),…,(u k-1 ,y k-1 )}
b, obtaining displacement pseudo values of the north direction and the east direction
Figure FDA00038610124700000711
Let the test data at time k be u k Training data set D and test data u at time k k Inputting the data into GPR for online regression calculation to obtain the output at k moment
Figure FDA00038610124700000712
Figure FDA00038610124700000713
7. The multi-model-based adaptive robust AUV navigation method according to claim 1, wherein step S2.2.1 comprises the following specific steps:
s2.2.1.1 samples 2d +1 sigma points based on UT transformation, and if the state vector of any sigma point is ξ, then
Figure FDA0003861012470000071
Figure FDA0003861012470000072
b ranges from 1 to d;
Figure FDA0003861012470000073
when the value range of b is d +1 to 2d;
wherein j is the serial number of the sub-model, and d is the dimension of the system state; lambda is a scale parameter and is a set value;
s2.2.1.2, calculating the weight w of the sigma point:
Figure FDA0003861012470000074
Figure FDA0003861012470000075
Figure FDA0003861012470000076
b ranges from 1 to 2d;
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003861012470000077
in order to be the weight of the mean value,
Figure FDA0003861012470000078
the covariance weight, and alpha and beta are preset parameters;
s2.2.1.3, calculating one-step predicted value xi of each sigma point j(b,k|k-1) Weighting each sigma point by using the weight, and calculating to obtain a system state one-step predicted value of the submodel II or the submodel III
Figure FDA0003861012470000079
And one-step prediction covariance value P j(k|k-1) ：
Figure FDA00038610124700000710
Figure FDA0003861012470000081
S2.2.1.4 calculating one-step predicted value gamma of degree of freedom parameter of submodel inverse Weishate distribution j(k|k-1) One-step predictor V of sum-scale matrix j(k|k-1) ：
γ j(k|k-1) ＝ρ(γ j(k-1) -d-1)+d+1
Figure FDA0003861012470000082
Wherein, γ k Is a degree of freedom parameter; v k Is a scale matrix; ρ is a real number and 0<ρ≤1；
Figure FDA0003861012470000083
Where I is the identity matrix.
8. The multi-model-based adaptive robust AUV navigation method according to claim 1, wherein step S2.2.2 comprises the following specific steps:
s2.2.2.1, calculating a measurement update value of the sub-model inverse Weishate distribution freedom degree parameter:
γ j(k) ＝γ j(k|k-1) +1
s2.2.2.2 one-step prediction value of system state of two self-adaptive sub-models
Figure FDA0003861012470000084
UT conversion is carried out again to obtain 2d +1 sigma point state one-step predicted values
Figure FDA0003861012470000085
d is the dimension of the system state according to the following disclosureThe formula obtains the observation predicted value of each sigma point
Figure FDA0003861012470000086
Figure FDA0003861012470000087
The value range of b is 0 to 2d;
then obtaining the observation predicted value of the current self-adaptive sub-model system state according to the following formula
Figure FDA0003861012470000088
Figure FDA0003861012470000089
S2.2.2.3 calculating the observed noise covariance R of the current System k ：
Figure FDA00038610124700000810
Wherein, the first and the second end of the pipe are connected with each other,
Figure FDA00038610124700000811
s2.2.2.4 weighting calculation to obtain the observation covariance of the system
Figure FDA00038610124700000812
And covariance between observation and prediction
Figure FDA00038610124700000813
Figure FDA0003861012470000091
Figure FDA0003861012470000092
S2.2.2.5 calculating to obtain a Kalman gain value according to the observation covariance and the covariance between observation and prediction
Figure FDA0003861012470000093
Figure FDA0003861012470000094
State update for S2.2.2.6 computing systems
Figure FDA0003861012470000095
Covariance update
Figure FDA0003861012470000096
Sum-scale matrix update values
Figure FDA0003861012470000097
Figure FDA0003861012470000098
Figure FDA0003861012470000099
Figure FDA00038610124700000910
S2.2.2.7l is iteration number, the initial value of l is set to 0, the step S2.2.2 to the step S2.2.2.6 are iterated and calculated in a circulation mode, l is accumulated according to the iteration number, when l = N, the circulation is ended, and the final state estimation of the current sub-model at the time k is obtained
Figure FDA00038610124700000911
Covariance estimation P j(k) Sum scale matrix estimate V j(k) N is a set value:
Figure FDA00038610124700000912
Figure FDA00038610124700000913
Figure FDA00038610124700000914
9. the multi-model-based adaptive robust AUV navigation method according to claim 1, wherein the step S3 comprises the following specific steps:
after each submodel obtains respective state estimation through model filtering, model probability is updated, and the likelihood function of the submodel j is as follows:
Figure FDA00038610124700000915
wherein v is j(k) As residual terms of submodel j, S j(k) Is the observed covariance of the submodel j; m is the dimension of the system observation vector; model probability mu of submodel j at time k j(k) Comprises the following steps:
Figure FDA00038610124700000916
wherein c is a normalization constant:
Figure FDA00038610124700000917
r denotes the number of sub-models employed.
10. The multi-model-based adaptive robust AUV navigation method according to claim 9, wherein the step S4 comprises the following specific steps:
model probability mu based on k time obtained in step S3 j(k) Obtaining the final state estimation of the AUV navigation system at the time k by weighting and fusing the estimation results of each submodel
Figure FDA0003861012470000101
Figure FDA0003861012470000102
Final system state covariance estimate P k Comprises the following steps:
Figure FDA0003861012470000103
where r represents the number of submodels employed.
CN202111430297.1A 2021-11-29 2021-11-29 Adaptive robust AUV navigation method based on multiple models Active CN114061592B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111430297.1A CN114061592B (en) 2021-11-29 2021-11-29 Adaptive robust AUV navigation method based on multiple models

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111430297.1A CN114061592B (en) 2021-11-29 2021-11-29 Adaptive robust AUV navigation method based on multiple models

Publications (2)

Publication Number Publication Date
CN114061592A CN114061592A (en) 2022-02-18
CN114061592B true CN114061592B (en) 2022-11-29

Family

ID=80277163

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111430297.1A Active CN114061592B (en) 2021-11-29 2021-11-29 Adaptive robust AUV navigation method based on multiple models

Country Status (1)

Country Link
CN (1) CN114061592B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116182949B (en) * 2023-02-23 2024-03-19 中国人民解放军91977部队 Marine environment water quality monitoring system and method

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105337985A (en) * 2015-11-19 2016-02-17 北京师范大学 Attack detection method and system
CN107765347A (en) * 2017-06-29 2018-03-06 河海大学 A kind of Gaussian process returns and the short-term wind speed forecasting method of particle filter
CN110906933A (en) * 2019-11-06 2020-03-24 中国海洋大学 AUV (autonomous Underwater vehicle) auxiliary navigation method based on deep neural network
CN112818819A (en) * 2021-01-28 2021-05-18 青岛澎湃海洋探索技术有限公司 AUV state monitoring method based on dynamic model and complex network theory
CN113654559A (en) * 2021-08-20 2021-11-16 青岛澎湃海洋探索技术有限公司 AUV navigation method based on multi-model observation correction

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20140032167A1 (en) * 2011-04-01 2014-01-30 Physical Sciences, Inc. Multisensor Management and Data Fusion via Parallelized Multivariate Filters

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105337985A (en) * 2015-11-19 2016-02-17 北京师范大学 Attack detection method and system
CN107765347A (en) * 2017-06-29 2018-03-06 河海大学 A kind of Gaussian process returns and the short-term wind speed forecasting method of particle filter
CN110906933A (en) * 2019-11-06 2020-03-24 中国海洋大学 AUV (autonomous Underwater vehicle) auxiliary navigation method based on deep neural network
CN112818819A (en) * 2021-01-28 2021-05-18 青岛澎湃海洋探索技术有限公司 AUV state monitoring method based on dynamic model and complex network theory
CN113654559A (en) * 2021-08-20 2021-11-16 青岛澎湃海洋探索技术有限公司 AUV navigation method based on multi-model observation correction

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
传感器自适应跟踪算法研究及仿真系统实现;贺贺等;《智能仪器与传感技术》;20201231;第28卷(第11期);第260-264页 *
基于交互式模型的多ＡＵＶ协同导航鲁棒性滤波算法;徐博等;《系统工程与电子技术》;20170930;第39卷(第9期);第2087-2093页 *

Also Published As

Publication number Publication date
CN114061592A (en) 2022-02-18

Similar Documents

Publication Publication Date Title
CN111156987B (en) Inertia/astronomy combined navigation method based on residual compensation multi-rate CKF
CN103776453A (en) Combination navigation filtering method of multi-model underwater vehicle
CN109724599A (en) A kind of Robust Kalman Filter SINS/DVL Combinated navigation method of anti-outlier
CN110954132B (en) GRNN-assisted self-adaptive Kalman filtering navigation fault identification method
CN107229060B (en) A kind of GPS measurement data processing method based on adaptive-filtering
Zhang et al. Multiple model AUV navigation methodology with adaptivity and robustness
CN107727097B (en) Information fusion method and device based on airborne distributed position and attitude measurement system
CN112285676A (en) Laser radar and IMU external reference calibration method and device
CN111007557B (en) Adaptive kinematics model assisted GNSS carrier phase and Doppler fusion speed measurement method
CN114061592B (en) Adaptive robust AUV navigation method based on multiple models
Bai et al. A Robust Generalized $ t $ Distribution-Based Kalman Filter
Cohen et al. A-KIT: Adaptive Kalman-informed transformer
JP2019082328A (en) Position estimation device
CN113156473A (en) Self-adaptive discrimination method for satellite signal environment of information fusion positioning system
CN116772867A (en) Multi-AUV self-adaptive co-location method and system based on node optimization of factor graph
CN116734860A (en) Multi-AUV self-adaptive cooperative positioning method and system based on factor graph
CN114741659B (en) Adaptive model on-line reconstruction robust filtering method, device and system
Zhang et al. Feature-based ukf-slam using imaging sonar in underwater structured environment
CN114018250B (en) Inertial navigation method, electronic device, storage medium and computer program product
Wang et al. EDI: ESKF-based Disjoint Initialization for Visual-Inertial SLAM Systems
CN113654559B (en) AUV navigation method based on multi-model observation correction
CN115828533A (en) Interactive multi-model robust filtering method based on Student&#39;s t distribution
Hua et al. Relative pose estimation from bearing measurements of three unknown source points
CN114001759A (en) Array type MEMS sensor control method and system
CN113932815A (en) Robustness optimized Kalman filtering method and device, electronic equipment and storage medium

Legal Events

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