CN114061592A - 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
CN114061592A
CN114061592A CN202111430297.1A CN202111430297A CN114061592A CN 114061592 A CN114061592 A CN 114061592A CN 202111430297 A CN202111430297 A CN 202111430297A CN 114061592 A CN114061592 A CN 114061592A
Authority
CN
China
Prior art keywords
model
value
time
submodel
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.)
Granted
Application number
CN202111430297.1A
Other languages
Chinese (zh)
Other versions
CN114061592B (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

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; filtering by an S2 model: s3 updating model probability; 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

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 influence the precision of filtering, even cause the filtering divergence.
Meanwhile, the navigation sensor carried by the AUV is susceptible to interference of an abnormal value in observation, and particularly, a Doppler Velocity Log (DVL) is easily affected by marine organisms, seabed gullies 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 the 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 muij(k-1)Combining and calculating to obtain the mixed state of the sub-model j at the moment k
Figure BDA0003379906680000012
Sum-mixture covariance P0j(k-1)As an initial filtering input for each submodel;
filtering by an S2 model:
s2.1, constructing an abnormal value detector submodel:
s2.1.1 time update procedure: AUV navigation system state one-step predicted value of abnormal value detector submodel from k-1 time to k time
Figure BDA0003379906680000013
And one-step prediction covariance value Pj(k|k-1)；
S2.1.2 measurement update procedure:
calculating to obtain an observed predicted value of the abnormal value detector sub-model system state
Figure BDA0003379906680000021
Observed value and observed predicted value acquired in real time by using current sensor
Figure BDA0003379906680000022
Calculating to obtain residual error v between the twoj(k)And constructing a fault detection function thetak；
If thetak≤TDIf 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 Pj(k)：
Figure BDA0003379906680000026
Figure BDA0003379906680000027
If thetak＞TDIf the abnormal value detector submodel detects that the abnormal value appears at the moment k when the sensor is observed, the abnormal value is obtained by online regression by using a machine learning methodAUV displacement pseudo value delta to current time stepx*,δ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
covariance estimation P of one-step predicted value of system state at moment kj(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 update procedure: 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 Pj(k|k-1)One-step predicted value gamma of the degree of freedom parameterj(k|k-1)One-step prediction value V of scale matrixj(k|k-1)；
S2.2.2 measurement update procedure: iterative computation of the observed noise covariance R by means of a loopkTo obtain the final state estimate at time k for the two adaptive sub-models
Figure BDA00033799066800000212
Covariance estimation Pj(k)Sum scale matrix estimate Vj(k)；
When the outlier detector sub-model detects an outlier observation:
first, in step S2.2.1, a system state one-step predicted value of two adaptive sub-models from time k-1 to time k is obtained
Figure BDA0003379906680000031
And one-step prediction covariance value Pj(k|k-1)(ii) a Then, in step S2.2.2, the measurement and update process is normally performed, and at this time, the displacement pseudo value obtained in step S2.1.2 is superimposed on the state estimation of the system at the time k-1, so as to obtain the state quantities of the final positions in the north direction and the east direction of the AUV estimated by the two adaptive sub-models at the time k, and the estimation of the remaining state quantities is the one-step predicted values of the system states of the two adaptive sub-models:
Figure BDA0003379906680000032
p obtained in the covariance estimation synchronization step S2.2.2 of the two adaptive sub-models at time kj(k)；
S3 updating model probability;
and S4, fusing multi-model estimation 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 constructed;
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
indicating the course angle, v, of the AUV at time kxAnd vyRespectively 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, axAnd ayRespectively representing acceleration information, w, corresponding to the velocityzIndicating angular velocity information corresponding to course angleInformation;
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:
Xk＝f(Xk-1,mk-1)
Figure BDA0003379906680000041
wherein the content of the first and second substances,
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 content of the first and second substances,
Figure BDA0003379906680000044
indicating the course angle, a, measured by an attitude sensor mounted on the AUV at time kxmAnd aymForward and right accelerations w measured by attitude sensor at time k in AUV carrier coordinate systemzmIs angular velocity data, v, corresponding to the course angle measured by the attitude sensor at time kxmAnd vymDistributing 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:
Zk＝HkXk+rk
wherein H is [0 ]6×2 I6×6]To observe the matrix, rkThe 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 transfer matrix A:
Figure BDA0003379906680000045
wherein, aijRepresenting the transition probability from submodel i to submodel j, the model probability of submodel i being muiAnd 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 muij(k-1)And combining to obtain the initial input of each submodel at the moment k, wherein the mixed probability from the submodel i to the submodel j at the moment 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 P0j(k-1)For the initial filtering input of each submodel.
The step S2.1.1 includes the following steps:
s2.1.1.1 sampling 2d +1 sigma points based on unscented transformation, and setting the state vector of any sigma point as xi
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 one-step predicted value of system state of abnormal value detector submodel
Figure BDA0003379906680000061
And one-step prediction covariance value Pj(k|k-1)：
Figure BDA0003379906680000062
Figure BDA0003379906680000063
Wherein, f (ξ)j(b,k-1)) For AUV navigation system equation of state, Qj(k)Is the process noise covariance, is the set point.
In the above step S2.1.2, the fault detection function θ is constructedkThe method comprises the following specific steps:
s2.1.2.1 predict value of abnormal value detector submodel system state by one step
Figure BDA0003379906680000065
Performing UT conversion to obtain the state one-step predicted value epsilon of 2d +1 sigma pointsj(b,k|k-1)And obtaining the observation predicted value z of each sigma point according to the following formulaj(b,k|k-1)：
zj(b,k|k-1)＝Hkεj(b,k|k-1),b＝0～2d；
Wherein HkFor 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 weighted calculation to obtain the observed covariance of the system
Figure BDA0003379906680000068
And covariance between observation and prediction
Figure BDA0003379906680000069
Figure BDA00033799066800000610
Figure BDA00033799066800000611
Wherein R iskThe 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 Kj(k)：
Figure BDA00033799066800000614
S2.1.2.4 obtaining residual v between the current sensor and the observation predicted value by using the observation value collected by the current sensor in real time and the calculated observation predicted valuej(k)And constructing a fault detection function thetak：
Figure BDA0003379906680000071
Figure BDA0003379906680000072
In the step S2.1.2, the AUV displacement pseudo 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 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 which are measured by a speed sensor, forward acceleration, right acceleration and downward acceleration under the AUV carrier coordinate system which are measured by the attitude sensor, 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 ye＝[δxe,δye]Wherein δxeAnd deltayeRespectively 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＝{(uk-M,yk-M),(uk-M+1,yk-M+1),…,(uk-1,yk-1)}
b, obtaining displacement pseudo values delta in the north direction and the east directionx*,δy*；
Let the test data at time k be ukTraining data set D and test data u at time kkInputting the output y into GPR to perform online regression calculation to obtain the output y at the k momentk*：
yk*＝[δx*,δy*]。
The step S2.2.1 includes the following steps:
s2.2.1.1 sampling 2d +1 sigma points based on UT transform, and setting the state vector of any sigma point as 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 pointj(b,k|k-1)Weighting each sigma point by using the weight, and calculating to obtain a sub-modelOne-step prediction value of system state of model II or submodel III
Figure BDA0003379906680000086
And one-step prediction covariance value Pj(k|k-1)：
Figure BDA0003379906680000087
Figure BDA0003379906680000088
S2.2.1.4 calculating one-step predicted value gamma of degree of freedom parameter of sub-model inverse Weishate distributionj(k|k-1)One-step predictor V of sum-scale matrixj(k|k-1)：
γj(k|k-1)＝ρ(γj(k-1)-d-1)+d+1
Figure BDA0003379906680000089
Wherein, γkIs a degree of freedom parameter; vkIs a scale matrix; ρ is a real number and 0<ρ≤1；
Figure BDA00033799066800000810
Where I is the identity matrix.
The step S2.2.2 includes the following steps:
s2.2.2.1, calculating the measurement update value of the sub-model inverse Weissett distribution freedom degree parameter:
γj(k)＝γj(k|k-1)+1
s2.2.2.2 one-step prediction value of system state of two adaptive sub-models
Figure BDA00033799066800000811
Performing UT conversion 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 systemk：
Figure BDA0003379906680000093
Wherein the content of the first and second substances,
Figure BDA0003379906680000094
s2.2.2.4 weighted calculation to obtain the observed covariance of the system
Figure BDA0003379906680000095
And covariance between observation and prediction
Figure BDA0003379906680000096
Figure BDA0003379906680000097
Figure BDA0003379906680000098
S2.2.2.5 Kalman gain value is calculated based on the observed covariance and the covariance between the observation and the prediction
Figure BDA0003379906680000099
Figure BDA00033799066800000910
S2.2.2.6 status update of computing system
Figure BDA00033799066800000911
Covariance update
Figure BDA00033799066800000912
Sum-scale matrix update values
Figure BDA00033799066800000913
Figure BDA00033799066800000914
Figure BDA00033799066800000915
Figure BDA00033799066800000916
S2.2.2.7l is iteration number, l initial value is set to 0, steps S2.2.2.2 to S2.2.2.6 are iterated and calculated in a loop mode, l is accumulated according to the iteration number, when l is N, the loop is ended, and the final state estimation of the current submodel at the time k is obtained
Figure BDA00033799066800000917
Covariance estimation Pj(k)Sum scale matrix estimate Vj(k)N is setThe 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 isj(k)As residual terms of submodel j, Sj(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 kj(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 time k obtained in step S3j(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 BDA0003379906680000104
Figure BDA0003379906680000105
Final system state covariance estimate PkComprises the following steps:
Figure BDA0003379906680000106
the invention has the beneficial effects that:
(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 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.
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 present invention with 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 implemented in a number of ways different from those described herein and similar generalizations can be made by those skilled in the art without departing from the spirit 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 such as the current position and attitude 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 kxAnd vyRespectively 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, axAnd ayRespectively representing acceleration information, w, corresponding to the velocityzIndicating 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:
Xk＝f(Xk-1,mk-1)
Figure BDA0003379906680000113
wherein the content of the first and second substances,
Figure BDA0003379906680000121
representing white gaussian noise with mean 0 and covariance Q, Q being a preset value.
The AUV navigation system observation can reflect the currently and actually detected AUV attitude, speed and other information. 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 kxmAnd aymForward and right accelerations w measured by attitude sensor at time k in AUV carrier coordinate systemzmIs angular velocity data, v, corresponding to the course angle measured by the attitude sensor at time kxmAnd vymThe 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:
Zk＝HkXk+rk
wherein H is [0 ]6×2 I6×6]To observe the matrix, rkThe noise is Gaussian white noise with the average 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, aijRepresenting the transition probability from submodel i to submodel j, the model probability of submodel i being muiAnd r represents the number of adopted submodels, and the method adopts three submodels to run in parallel, namely r is 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 muij(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 P0j(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 process2And detecting an abnormal value in sensor observation, and once the abnormal value is detected, performing Gaussian Process Regression (GPR for short) on-line 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. A time update procedure.
In the time updating process, the sub-model 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) 2d +1 sigma points are sampled based on Unscented Transformation (UT), and if the state vector of any sigma point is set to be xi, then
Figure BDA0003379906680000136
Figure BDA0003379906680000141
Figure BDA0003379906680000142
Wherein b represents the serial number of the sigma point; j represents a sub-model number, and j is 1 in the embodiment; d is the dimension of the system state, in this embodiment, d is 8; 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 content of the first and second substances,
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 pointj(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 isj(k)Is the process noise covariance, is the set value。
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) And performing UT conversion on the system state one-step predicted value of the sub-model I again to obtain the state one-step predicted value epsilon of 2d +1 sigma pointsj(b,k|k-1)(ii) a Then, the observation predicted value z of each sigma point is obtained by utilizing the navigation system observation model constructed in the first stepj(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
zj(b,k|k-1)＝Hkεj(b,k|k-1),b＝0～2d；
Figure BDA0003379906680000151
Wherein HkRepresenting the observation matrix at time k.
(2) Weighted calculation to obtain the system observation covariance
Figure BDA0003379906680000152
And covariance between observation and prediction
Figure BDA0003379906680000153
Figure BDA0003379906680000154
Figure BDA0003379906680000155
Wherein R iskThe 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 the observation and the predictionj(k)
Figure BDA0003379906680000156
(4) Calculating to obtain a residual difference v between an observed value acquired by a current sensor in real time and a calculated observed predicted valuej(k)And constructing a fault detection function thetak：
Figure BDA0003379906680000158
Figure BDA0003379906680000159
For fault detection function and TDThe size of the T is respectively treated by the following method, wherein TDIs a preset threshold value.
If thetak≤TDAnd 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
if thetak＞TDIf 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 which are measured by a speed sensor, forward acceleration, right acceleration and downward acceleration under the AUV carrier coordinate system which are measured by the attitude sensor, 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 ye＝[δxe,δye]Wherein δxeAnd deltayeRespectively 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 directionx*,δy*；
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＝{(uk-M,yk-M),(uk-M+1,yk-M+1),…,(uk-1,yk-1)}
let the test data at time k be ukTraining data set D and test data u at time kkThe output at the time k can be obtained by inputting the data into GPR to perform online regression calculation:
yk*＝[δ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 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, carrying out filtering estimation on the displacement obtained by filtering the pseudo displacement substitution sub-model II and the sub-model III, wherein the pseudo displacement is 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) 2d +1 sigma points are sampled based on UT conversion, and if the state vector of any sigma point is set to be xi, then
Figure BDA0003379906680000171
Figure BDA0003379906680000172
Figure BDA0003379906680000173
Wherein j represents a sub-model serial number, and j is 2 or 3 in the embodiment; d is the dimension of the system state, in this embodiment, d is 8; 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 content of the first and second substances,
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 stepj(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, γkIs a degree of freedom parameter; vkIs 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 in the measurement updating process of the submodel II or the submodel III to correct the prediction deviation in 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) and performing UT conversion on the one-step predicted value of the system state of the submodel II or the submodel 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 of the current sub-model system state is obtained by weight weighting calculationPrediction value
Figure BDA0003379906680000187
Figure BDA0003379906680000188
Figure BDA0003379906680000189
And l is the iteration number, the initial value of l is set to be 0, the steps (2) to (6) are iteratively calculated in a loop mode, l is accumulated according to the iteration number until l is equal to N, and the loop is exited, wherein N is a set value.
(3) Method for calculating observation noise covariance R of current system by using VB principlek：
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) Based on observation covariance and observation and predictionThe covariance between measurements is calculated to obtain the Kalman gain value
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) And when l is equal to 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 VBj(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 isj(k)As residual terms of submodel j, Sj(k)Is the observed covariance of the submodel j; m is the dimension of the system observation vector, and m is 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) is the forward velocity of DVL in AUV carrier coordinate system, 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, the square line is the GPS trajectory, as true value comparison, it can be seen that the existing filtering method is affected by abnormal observation, resulting in larger positioning deviation, and the AUV navigation method provided by the present invention has stronger robustness, is not affected by abnormal observation, and has high navigation accuracy, the estimated trajectory is very close to the real trajectory.
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 FDA0003379906670000011
And the mixing probability muij(k-1)Combining and calculating to obtain the mixed state of the sub-model j at the moment k
Figure FDA0003379906670000012
Sum and mixture covarianceP0j(k-1)As an initial filtering input for each submodel;
filtering by an S2 model:
s2.1, constructing an abnormal value detector submodel:
s2.1.1 time update procedure: AUV navigation system state one-step predicted value of abnormal value detector submodel from k-1 time to k time
Figure FDA0003379906670000013
And one-step prediction covariance value Pj(k|k-1)；
S2.1.2 measurement update procedure:
calculating to obtain an observed predicted value of the abnormal value detector sub-model system state
Figure FDA0003379906670000014
Observed value and observed predicted value acquired in real time by using current sensor
Figure FDA0003379906670000015
Calculating to obtain residual error v between the twoj(k)And constructing a fault detection function
Figure FDA0003379906670000016
If it is
Figure FDA0003379906670000017
Then the sensor observes no abnormal value at the moment, and the state estimation of the current sub-model at the moment k is obtained according to the standard UKF algorithm
Figure FDA0003379906670000018
Sum covariance estimation Pj(k)：
Figure FDA0003379906670000019
Figure FDA00033799066700000110
If it is
Figure FDA00033799066700000111
Then the abnormal value detector submodel detects that the abnormal value appears at the moment k when the sensor is observed, and the AUV displacement pseudo value delta in the current time step is obtained by online regression through a machine learning methodx*,δ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 FDA00033799066700000112
covariance estimation P of one-step predicted value of system state at moment kj(k)Comprises the following steps:
Figure FDA00033799066700000113
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 update procedure: obtaining AUV navigation system state one-step predicted value of two self-adaptive sub-models from k-1 time to k time
Figure FDA0003379906670000021
One-step prediction covariance value Pj(k|k-1)One-step predicted value gamma of the degree of freedom parameterj(k|k-1)One-step prediction value V of scale matrixj(k|k-1)；
S2.2.2 measurement update procedure: iterative computation of the observed noise covariance R by means of a loopkTo obtain the final state estimate at time k for the two adaptive sub-models
Figure FDA0003379906670000022
Covariance estimation Pj(k)Sum scale matrix estimate Vj(k)；
When the outlier detector sub-model detects an outlier observation:
first, in step S2.2.1, a system state one-step predicted value of two adaptive sub-models from time k-1 to time k is obtained
Figure FDA0003379906670000023
And one-step prediction covariance value Pj(k|k-1)(ii) a Then, in step S2.2.2, the measurement and update process is normally performed, and at this time, the displacement pseudo value obtained in step S2.1.2 is superimposed on the state estimation of the system at the time k-1, so as to obtain the state quantities of the final positions in the north direction and the east direction of the AUV estimated by the two adaptive sub-models at the time k, and the estimation of the remaining state quantities is the one-step predicted values of the system states of the two adaptive sub-models:
Figure FDA0003379906670000024
p obtained in the covariance estimation synchronization step S2.2.2 of the two adaptive sub-models at time kj(k)；
S3 updating model probability;
and S4, fusing multi-model estimation 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 step S1, an AUV navigation system state model and an observation model are constructed;
let the system state vector at time k be:
Figure FDA0003379906670000025
wherein x and y respectively represent the north and east position information of AUV at the time k in the UTM coordinate system,
Figure FDA0003379906670000031
indicating the course angle, v, of the AUV at time kxAnd vyRespectively 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, axAnd ayRespectively representing acceleration information, w, corresponding to the velocityzIndicating 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:
Xk＝f(Xk-1,mk-1)
Figure FDA0003379906670000032
wherein the content of the first and second substances,
Figure FDA0003379906670000033
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 FDA0003379906670000034
wherein the content of the first and second substances,
Figure FDA0003379906670000035
indicating the course angle, a, measured by an attitude sensor mounted on the AUV at time kxmAnd aymForward and right accelerations w measured by attitude sensor at time k in AUV carrier coordinate systemzmIs k atAngular velocity data v corresponding to the course angle measured by the attitude sensorxmAnd vymDistributing 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:
Zk＝HkXk+rk
wherein H is [0 ]6×2I6×6]To observe the matrix, rkThe 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 FDA0003379906670000041
wherein, aijRepresenting the transition probability from submodel i to submodel j, the model probability of submodel i being muiAnd 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 FDA0003379906670000042
And the mixing probability muij(k-1)And combining to obtain the initial input of each submodel at the moment k, wherein the mixed probability from the submodel i to the submodel j at the moment k-1 is as follows:
Figure FDA0003379906670000043
wherein
Figure FDA0003379906670000044
Predicted probability for submodel j:
Figure FDA0003379906670000045
the mixing state of the submodel j is:
Figure FDA0003379906670000046
the mixed covariance of model j is:
Figure FDA0003379906670000047
mixed state
Figure FDA0003379906670000048
Sum-mixture covariance P0j(k-1)For the initial filtering input of each submodel.
4. The robust adaptive AUV navigation method based on multiple models according to claim 1, wherein the step S2.1.1 comprises the following specific steps:
s2.1.1.1 sampling 2d +1 sigma points based on unscented transformation, and setting the state vector of any sigma point as xi
Figure FDA0003379906670000049
Figure FDA00033799066700000410
Figure FDA00033799066700000411
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 FDA0003379906670000051
Figure FDA0003379906670000052
Figure FDA0003379906670000053
wherein the content of the first and second substances,
Figure FDA0003379906670000054
in order to be the weight of the mean value,
Figure FDA0003379906670000055
the covariance weight, and alpha and beta are preset parameters;
s2.1.1.3 calculating one-step predicted value of system state of abnormal value detector submodel
Figure FDA0003379906670000056
And one-step prediction covariance value Pj(k|k-1)：
Figure FDA0003379906670000057
Figure FDA0003379906670000058
Wherein, f (ξ)j(b,k-1)) For AUV navigation system equation of state, Qj(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
Figure FDA0003379906670000059
The method comprises the following specific steps:
s2.1.2.1 predict value of abnormal value detector submodel system state by one step
Figure FDA00033799066700000510
Performing UT conversion to obtain the state one-step predicted value epsilon of 2d +1 sigma pointsj(b,k|k-1)And obtaining the observation predicted value z of each sigma point according to the following formulaj(b,k|k-1)：
zj(b,k|k-1)＝Hkεj(b,k|k-1),b＝0～2d；
Wherein HkFor 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 FDA00033799066700000511
Figure FDA00033799066700000512
S2.1.2.2 weighted calculation to obtain the observed covariance of the system
Figure FDA00033799066700000513
And covariance between observation and prediction
Figure FDA00033799066700000514
Figure FDA0003379906670000061
Figure FDA0003379906670000062
Wherein R iskThe covariance of the observed noise is a preset value;
s2.1.2.3 based on observed covariance
Figure FDA0003379906670000063
And covariance between observation and prediction
Figure FDA0003379906670000064
Calculating to obtain a Kalman gain value Kj(k)：
Figure FDA0003379906670000065
S2.1.2.4 obtaining residual v between the current sensor and the observation predicted value by using the observation value collected by the current sensor in real time and the calculated observation predicted valuej(k)And constructing a fault detection function
Figure FDA0003379906670000066
Figure FDA0003379906670000067
Figure FDA0003379906670000068
6. The multi-model-based adaptive robust AUV navigation method according to claim 1, wherein in step S2.1.2, the AUV displacement pseudo 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 FDA0003379906670000069
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 which are measured by a speed sensor, forward acceleration, right acceleration and downward acceleration under the AUV carrier coordinate system which are measured by the attitude sensor, 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 ye＝[δxe,δye]Wherein δxeAnd deltayeRespectively representing north and east-west displacement amounts estimated by the AUV navigation system between time f and time f-1:
Figure FDA00033799066700000610
Figure FDA00033799066700000611
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＝{(uk-M,yk-M),(uk-M+1,yk-M+1),…,(uk-1,yk-1)}
b, obtaining displacement pseudo values delta in the north direction and the east directionx*,δy*；
Let the test data at time k be ukTraining data set D and test data u at time kkInputting the output y into GPR to perform online regression calculation to obtain the output y at the k momentk*：
yk*＝[δx*,δy*]。
7. The multi-model-based adaptive robust AUV navigation method according to claim 1, wherein the step S2.2.1 comprises the following specific steps:
s2.2.1.1 sampling 2d +1 sigma points based on UT transform, and setting the state vector of any sigma point as xi, then
Figure FDA0003379906670000071
Figure FDA0003379906670000072
Figure FDA0003379906670000073
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 FDA0003379906670000074
Figure FDA0003379906670000075
Figure FDA0003379906670000076
wherein the content of the first and second substances,
Figure FDA0003379906670000077
in order to be the weight of the mean value,
Figure FDA0003379906670000078
is a covariance weight;
s2.2.1.3 calculating one-step predicted value xi of each sigma pointj(b,k|k-1)Weighting each sigma point by using the weight, and calculating to obtain a one-step predicted value of the system state of the submodel II or the submodel III
Figure FDA0003379906670000079
And one-step prediction covariance value Pj(k|k-1)：
Figure FDA00033799066700000710
Figure FDA00033799066700000711
S2.2.1.4 calculating one-step predicted value gamma of degree of freedom parameter of sub-model inverse Weishate distributionj(k|k-1)One-step predictor V of sum-scale matrixj(k|k-1)：
γj(k|k-1)＝ρ(γj(k-1)-d-1)+d+1
Figure FDA0003379906670000081
Wherein, γkIs a degree of freedom parameter; vkIs a scale matrix; ρ is a real number and 0<ρ≤1；
Figure FDA0003379906670000082
Where I is the identity matrix.
8. The multi-model-based adaptive robust AUV navigation method according to claim 1, wherein the step S2.2.2 comprises the following specific steps:
s2.2.2.1, calculating the measurement update value of the sub-model inverse Weissett distribution freedom degree parameter:
γj(k)＝γj(k|k-1)+1
s2.2.2.2 one-step prediction value of system state of two adaptive sub-models
Figure FDA0003379906670000083
Performing UT conversion again to obtain 2d +1 sigma point state one-step predicted values
Figure FDA0003379906670000084
Obtaining the observation predicted value of each sigma point according to the following formula
Figure FDA0003379906670000085
Figure FDA0003379906670000086
Then, the observation predicted value of the current self-adaptive sub-model system state is obtained according to the following formula
Figure FDA0003379906670000087
Figure FDA0003379906670000088
S2.2.2.3 calculating the observed noise covariance R of the current systemk：
Figure FDA0003379906670000089
Wherein the content of the first and second substances,
Figure FDA00033799066700000810
s2.2.2.4 weighted calculation to obtain the observed covariance of the system
Figure FDA00033799066700000811
And covariance between observation and prediction
Figure FDA00033799066700000812
Figure FDA00033799066700000813
Figure FDA00033799066700000814
S2.2.2.5 Kalman gain value is calculated based on the observed covariance and the covariance between the observation and the prediction
Figure FDA00033799066700000815
Figure FDA0003379906670000091
S2.2.2.6 status update of computing system
Figure FDA0003379906670000092
Covariance update
Figure FDA0003379906670000093
Sum-scale matrix update values
Figure FDA0003379906670000094
Figure FDA0003379906670000095
Figure FDA0003379906670000096
Figure FDA0003379906670000097
S2.2.2.7l is iteration number, l initial value is set to 0, steps S2.2.2.2 to S2.2.2.6 are iterated and calculated in a circulating mode, l is accumulated according to iteration number, when l is equal to N, the circulation is ended, and the final state estimation of the current sub-model at the time k is obtained
Figure FDA0003379906670000098
Covariance estimation Pj(k)Sum scale matrix estimate Vj(k)N is a set value:
Figure FDA0003379906670000099
Figure FDA00033799066700000910
Figure FDA00033799066700000911
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, the model probability is updated, and the likelihood function of the submodel j is as follows:
Figure FDA00033799066700000912
wherein v isj(k)As a sub-modelResidual term of j, Sj(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 kj(k)Comprises the following steps:
Figure FDA00033799066700000913
wherein c is a normalization constant:
Figure FDA00033799066700000914
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 time k obtained in step S3j(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 FDA0003379906670000101
Figure FDA0003379906670000102
Final system state covariance estimate PkComprises the following steps:
Figure FDA0003379906670000103
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 true CN114061592A (en) 2022-02-18
CN114061592B 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)

Cited By (1)

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

Citations (6)

* 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
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

Patent Citations (6)

* 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
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
徐博等: "基于交互式模型的多ＡＵＶ协同导航鲁棒性滤波算法", 《系统工程与电子技术》 *
贺贺等: "传感器自适应跟踪算法研究及仿真系统实现", 《智能仪器与传感技术》 *

Cited By (2)

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

Also Published As

Publication number Publication date
CN114061592B (en) 2022-11-29

Similar Documents

Publication Publication Date Title
CN109211276B (en) SINS initial alignment method based on GPR and improved SRCKF
CN111156987B (en) Inertia/astronomy combined navigation method based on residual compensation multi-rate CKF
CN109724599A (en) A kind of Robust Kalman Filter SINS/DVL Combinated navigation method of anti-outlier
CN107229060B (en) A kind of GPS measurement data processing method based on adaptive-filtering
CN103776453A (en) Combination navigation filtering method of multi-model underwater vehicle
CN110954132B (en) GRNN-assisted self-adaptive Kalman filtering navigation fault identification method
Li et al. An information theoretic approach to interacting multiple model estimation
Zhang et al. Multiple model AUV navigation methodology with adaptivity and robustness
CN103940433A (en) Satellite attitude determining method based on improved self-adaptive square root UKF (Unscented Kalman Filter) algorithm
CN107727097B (en) Information fusion method and device based on airborne distributed position and attitude measurement system
Vivone et al. Multiple sensor measurement updates for the extended target tracking random matrix model
Huang et al. Variational Bayesian-based filter for inaccurate input in underwater navigation
CN112285676A (en) Laser radar and IMU external reference calibration method and device
Seow et al. Detecting and solving the kidnapped robot problem using laser range finder and wifi signal
CN114061592B (en) Adaptive robust AUV navigation method based on multiple models
Bai et al. A Robust Generalized $ t $ Distribution-Based Kalman Filter
CN114741659B (en) Adaptive model on-line reconstruction robust filtering method, device and system
CN116608859A (en) Navigation method, storage medium and device of self-adaptive unscented Kalman filtering based on threshold processing
CN115828533A (en) Interactive multi-model robust filtering method based on Student&#39;s t distribution
CN113654559B (en) AUV navigation method based on multi-model observation correction
Hua et al. Relative pose estimation from bearing measurements of three unknown source points
Wang et al. EDI: ESKF-based Disjoint Initialization for Visual-Inertial SLAM Systems
Li et al. Exploring the Potential of Deep Learning Aided Kalman Filter for GNSS/INS Integration: A Study on 2D Simulation Datasets
Huang et al. Bearing-only target tracking using a bank of MAP estimators
CN114018250B (en) Inertial navigation method, electronic device, storage medium and computer program product

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