CN114459477B - SINS/DVL (strapdown inertial navigation system/dynamic virtual local area network) tightly-combined navigation method based on improved PSO-ANFIS (PSO-ANFIS) assistance - Google Patents

Abstract

The invention provides an SINS/DVL tightly combined navigation technology based on improved PSO-ANFIS assistance, which comprises the following steps: establishing a state equation and a measurement equation of the SINS/DVL tightly combined navigation system in the underwater diving process; collecting sample data on the water surface by means of GNSS and through a variable decibel She Sika Kalman filtering algorithm, acquiring innovation, mahalanobis distance and measurement noise covariance matrix containing various abnormal measurement types as input information of ANFIS, and acquiring DVL absolute error as expected output; optimizing ANFIS model parameters through a particle swarm optimization algorithm, and training to obtain a better ANFIS model; when the ship is sailing under water, adopting an ANFIS model obtained through training to conduct online prediction on the four-beam absolute error of the DVL; further, based on the ANFIS prediction result, the characteristic change of the error is monitored through an anomaly discrimination mechanism, and the DVL measurement value is selectively compensated for the measurement updating process of the integrated navigation system. The SINS/DVL integrated navigation system positioning method and the SINS/DVL integrated navigation system positioning system can improve positioning accuracy and robustness of the SINS/DVL integrated navigation system in a complex underwater environment.

Description

SINS/DVL (strapdown inertial navigation system/dynamic virtual local area network) tightly-combined navigation method based on improved PSO-ANFIS (PSO-ANFIS) assistance

Technical Field

The invention belongs to the field of integrated navigation, relates to an SINS/DVL tightly combined navigation method based on improved PSO-ANFIS assistance.

Background

More and more countries take exploration and development of the propulsion ocean as strategic targets, and the breadth and depth of exploration are always the global common pursuit. Accurate and stable underwater navigation systems are very important for people to explore the ocean. Common underwater navigation systems and sensors mainly comprise SINS, DVL, underwater sound positioning technology, geophysical navigation systems, depth gauges, magnetometers and the like. Among them, SINS is the most central navigation system in underwater navigation because it is autonomous and blind, but the problem of error accumulation over time limits its independent use. Unlike on the ground, in the air, the global navigation satellite system (Global Navigation Satellite System, GNSS) has no way to use under water, due to the severe attenuation of electromagnetic waves under water. Geophysical navigation techniques require the establishment of a matching database of task areas in advance, and underwater sound localization also requires the placement of sound heads in advance. Therefore, the combined navigation of the SINS and the DVL becomes a main stream navigation mode of the underwater vehicle (Autonomous Underwater Vehicle, AUV) so as to realize long-endurance and high-precision underwater navigation.

For SINS/DVL integrated systems, there are many factors that affect the navigation accuracy, such as installation angle errors, lever arm errors, and speed measurement errors. After SINS and DVL are fixed, the installation angle and lever arm error are relatively stable, and calibration can be performed before navigation. DVL is active sonar, which requires the receipt of reflected sound waves from the outside, and the received acoustic signals are highly dependent on the surrounding acoustic environment. The actual speed measurement accuracy of the DVL is very affected by the complex environment, which is the portion that has the greatest influence on the integrated navigation accuracy. When the AUV sails with marine organisms blocking or rapid acceleration causes surrounding bubbles, the DVL measurement data may be misaligned, possibly producing outliers and large interference noise. In order to solve the problem of outliers, a series of robust estimation methods are proposed. The most easily realized is classical chi-square detection, and whether the field value is judged by setting a certain threshold value. The scholars propose a Kalman filter based on Huber-M estimation, solve the Huber kernel function through innovation, further solve the weight matrix, and update the filtering by using the reconstruction observance, but the parameter selection of the kernel function needs a certain experience. A kalman filter based on the maximum correlation entropy criterion is also a similar principle. The scholars propose a Kalman filter based on Student's-t distribution, which considers that the measurement wild value in a complex underwater environment can cause the measurement noise to present thick tail characteristics, models the measurement noise as Student's-t distribution, and carries out iterative calculation on state estimation through variable decibel leaf study. The method is characterized in that a learner solves the problem of unknown interference noise through an adaptive filter, and a model which is more fit with the actual situation is obtained through adjusting model parameters in an iteration process, wherein the model comprises a Sage-Husa adaptive filter, a variational Bayesian adaptive filter and the like, but the accuracy of the methods is very dependent on the initial values of a system process noise variance matrix and a system measurement noise covariance matrix, and the method has no universality. In fact, when deep trenches or seabed sludge are encountered in the AUV navigation process, part of the DVL beams cannot obtain effective reflected sound waves, which may cause irregular data update and even short-time failure of part of the DVL beams. These errors are relatively small at the initial stage of occurrence, and all result in a decline in the accuracy of the integrated navigation. Current research on these factors is still in the beginning and there is not a sufficiently effective solution. The above errors are all required to be compensated for in a complex marine environment, which is also the task of the present invention.

In addition, the correlation relationship can be obtained by an artificial intelligence method in consideration of the fact that errors can cause the change of some correlation variables. In recent years, artificial intelligence is rapidly developed in various fields, wherein ANFIS fuses a learning mechanism of a neural network and a language reasoning capability of a fuzzy system, has convenient and efficient learning capability, and is widely applied to various fields: medical symptom discrimination, bridge deformation estimation, power system parameter estimation, and the like.

Disclosure of Invention

In order to solve the problems, the invention discloses a method for tightly combining navigation based on an improved particle swarm optimization (Particle Swarm Optimization, PSO) -Adaptive Neuro-Fuzzy Inference System (ANFIS) -assisted strapdown inertial navigation system (Strapdown Inertial Navigation System, SINS)/Doppler log (Doppler Velocity Log, DVL); the SINS/DVL tightly combined navigation system is taken as a research object, an improved PSO-ANFIS algorithm is adopted to solve the speed errors of four beams of DVL, and the actual DVL measured value is compensated. And then, carrying out integrated navigation on the compensated measurement data and corresponding data calculated by inertial navigation, thereby obtaining high-precision navigation information.

In order to achieve the above purpose, the present invention provides the following technical solutions:

an SINS/DVL tightly combined navigation method based on improved PSO-ANFIS assistance specifically comprises the following steps:

step 1: establishing a state equation of the SINS/DVL integrated navigation system according to the system error equation, and establishing a measurement equation of the SINS/DVL integrated navigation system by taking the difference between pseudo measurement information calculated by navigation information calculated by the SINS, four-beam speed information measured by the DVL and depth information measured by the depth gauge as measurement;

step 2: controlling the diving depth of the AUV to reach the vicinity of the water surface, and after the initial alignment of the navigation positioning system is completed, assisting in collecting sample data containing various abnormal measurement types by means of global navigation satellite system (Global Navigation Satellite System, GNSS) information and a variational Bayesian Kalman filtering algorithm (Variational Bayesian Kalman Filter, VBKF) for training an ANFIS;

step 3: normalizing the sample data collected in the step 2, and processing the sample data through a particle swarm optimization algorithm to realize the optimization of the parameters of the ANFIS model, so as to complete the training process of the ANFIS model;

step 4: controlling the underwater of the AUV, and predicting the four-beam absolute error of the DVL by adopting an improved PSO-ANFIS algorithm, wherein the characteristic information is required to be acquired on line;

Step 5: based on an ANFIS prediction result, monitoring the characteristic change of the error, compensating the actual measurement of the DVL through an abnormality discrimination mechanism, and carrying out Kalman filtering on the compensated DVL quantity measurement and the pseudo observed quantity calculated by the SINS.

Further, in the step 1, a state equation and a measurement equation of the SINS/DVL tightly combined navigation system are established, and the specific process is as follows:

step 1.1, defining a coordinate system which needs to be used:

e-earth coordinate system: is fixedly connected with the earth, the origin is positioned at the earth center, x _e The axis passes through the intersection point of the primary meridian and the equator, z _e The axis pointing north, y _e Axis x _e 、z _e Forming a right-hand coordinate system;

n-a navigational coordinate system coincident with the east-north-day geographic coordinate system;

b—carrier coordinate system: the origin being at the centre of the carrier, z _b Axis-vertical carrier up, x _b Directed forward of the carrier, y _b And x _b 、z _b Forming a right-hand coordinate system;

d-an orthogonal coordinate system aligned with the beam center of the DVL, here denoted the beam system;

step 1.2, a state equation of the SINS/DVL tightly combined navigation system is established, and the specific steps are as follows:

taking the attitude error angle phi= [ phi ] _x φ _y φ _z ]Speed error δv= [ δv ] _E δV _N δV _U ]Position error δp= [ δ ] _L δ _λ δ _h ]Gyro constant drift epsilon and accelerometer random constant error As a state quantity of the SINS system, it is noted that:

wherein phi is _x Is the east misalignment angle phi _y Is the north misalignment angle phi _z Is the angle of the misalignment in the sky; δV (delta V) _E Is the east speed error, δV _N Is the north speed error, δV _U Is the error of the tangential velocity; δl is latitude error, δλ is longitude error, δh is altitude error; epsilon _x Is drift of the x-direction gyro, epsilon _y Is drift of the Y-direction gyro, epsilon _z Is z-direction gyro drift;is the random constant error of the x-direction accelerometer,is the random constant error of the y-direction accelerometer, < >>Is the random constant error of the z-direction accelerometer;

noise of SINS system:

W _SINS ＝[ω _gx ω _gy ω _gz ω _ax ω _ay ω _az ] ^T

wherein omega _g Is the process noise vector, omega of the gyro _a Is the process noise vector of the accelerometer.

Taking DVL four-beam velocity zero offset delta b= [ delta b ] ₁ δb ₂ δb ₃ δb ₄ ]The scale factor error δk is taken as a DVL system state variable and is noted as:

X _DVL ＝[δb ₁ δb ₂ δb ₃ δb ₄ δk] ^T

wherein δb ₁ Is the beam1 velocity zero offset, delta b ₂ Is the beam2 velocity zero offset, delta b ₃ Is the beam3 velocity zero offset, delta b ₄ Is beam4 velocity zero offset;

the noise of DVL system is omega _d ；

Taking the offset delta b of the depth gauge _ps As state variables of the depth gauge:

X _PS ＝δb _ps

the noise of the depth gauge system is omega _ps ；

The state quantity of the integrated navigation system can be expressed as:

X＝[X _SINS X _DVL X _PS ] ^T

from the error model of the navigation system, the state equation can be derived:

wherein F is a state transition matrix, and W is system noise; Specific F _SINS The derivation is not developed in detail here, there are many literature on the derivation process, the +.>

Step 1.3, a measurement equation of the SINS/DVL tightly combined navigation system is established, and the specific steps are as follows:

in the case of neglecting sensor errors, the speed is defined as follows:

wherein,is the speed of SINS under n line, +.>Is the speed of SINS under b line, +.>SINS speed under beam line, < +.>Is the velocity of the DVL measurement. There is a relationship between them as follows:

wherein,represents the transformation matrix from n-series to b-series, < >>Representing a conversion matrix from b-series to beam-series, which specifically represents the following formula:

wherein b _i Is based on the geometrical relationship between DVL beam and AUV, slave V ^b To V ^d Can be expressed as:

where α is the beam tilt angle of the DVL, a characteristic of DVL fixation.Can be expressed as +.>Wherein->DVL for "+" configuration, and +.>DVL for an "x" configuration;

the amount formed by SINS, DVL and depth gauge:

wherein,depth information calculated by SINS, +.>Is depth information measured by the depth gauge. Defining a measurement error model of the depth gauge as follows:

wherein H is _PS Is a depth truth value. Based on the above analysis, SINS solutionThe calculation formula is as follows:

defining a measurement error model of DVL as follows:

Based on the above analysis, the SINS solution speed is converted into the beam systemThe calculation formula is as follows:

wherein, [. Times ] represents a cross multiplication operation. From this, the measurement equation of the integrated navigation system can be obtained:

Z＝HX+V

wherein,

V＝[ω _d ω _ps ] ^T

further, in the step 2, sample data including various abnormal measurement types is collected with assistance of GNSS information and VBKF algorithm for training ANFIS, which specifically includes the following steps:

in order to predict data by the ANFIS model, the ANFIS parameters need to be trained beforehand by sample data. To obtain an accurate ANFIS model, navigation data needs to be collected by VBKF to identify the variables most relevant to anomalies occurring in the DVL measurements and use these variables as inputs. At the same time, we want to find variables that can directly represent measured anomalies as outputs;

step 2.1, collecting the variable most relevant to the occurrence of the abnormality of the DVL measurement information, and taking the variable as characteristic information, wherein the specific steps are as follows:

the innovation refers to the difference between the model predicted value and the measured value. Innovation v in Kalman filtering in SINS/DVL integrated navigation system _k The difference between the estimated value of the system model and the actual measured value of DVL can be mappedDifferent from each other. The computational expression is as follows:

Wherein,the actual measurement information of DVL at time k.Is a one-step predictor of time k. We consider the system model to be accurately modeled, so that when the innovation suddenly gets large, the DVL measurement is not accurate enough and the measurement error increases. Thus, the innovation is taken as one of the variables reflecting the DVL measurement error, as input to the ANFIS system;

the mahalanobis distance describes the standard deviation distance from a sampling point to a distribution, and can effectively calculate the similarity of two groups of random variables. The invention constructs a second input based on mahalanobis distance:

wherein lambda is _k Is measurement anomaly characteristic information defined according to a mahalanobis distance.P _k/k-1 Is a one-step prediction variance matrix. For convenience, λ will be herein _k Known as mahalanobis distance;

when the DVL measurement information is abnormal, the measurement noise may change. A specific method by which the measurement noise covariance matrix can be made to show this variation will be described in step 2.3. Measuring noise covariance matrixIs characteristic information that may represent measurement anomalies, and thus serves as a third input;

step 2.2, collecting absolute error of DVL measurement information as output of training data, specifically comprising the following steps:

the learning process of the model requires an accurate ideal output value. During testing and verification, the accurate measurement error is selected as the output of the training data, taking into account the sensors loaded on the AUV. Such accurate measurement errors can be obtained from the PHINS loaded by the AUV:

In the method, in the process of the invention,the absolute error of the DVL measurement at time k. PHINS is an optical fiber inertial navigation sensor, which can integrate GPS information and provide the most accurate attitude, speed and position information. Z is Z _GPS,k For accurate pseudo-measurement information obtained at the kth time according to the posture and speed information provided by PHINS, and +.>The solving mode is the same;

to sum up, in order to make the training of the target model more accurate, we have chosen the three pieces of characteristic information that react best to the measured outliers. They are innovation v _k Lambda based on mahalanobis distance construction _k And VBKF-based acquisitionIn addition, the absolute error of the DVL measurement information is selected as output, so that the DVL data quality can be intuitively displayed, and the subsequent compensation and utilization are facilitated.

Step 2.3, expanding a noise uncertainty processing method based on VBKF, which comprises the following specific steps:

the constant filter parameters do not describe the statistical characteristics of the observed quantity variations. Classical kalman filtering algorithms treat all observations as the same feature, and when the system observations are abnormal or noise changes, the algorithm cannot adapt to the system changes. In order to accurately respond to possible anomalies, the system must respond specifically to unknown anomalies. The phase needs to be obtained in consideration of the abnormality of the measurement information The corresponding mahalanobis distance and R _k VBKF was introduced to characterize its variation. Unknown can be estimated based on VBKFTo improve the validity of ANFIS input information;

when the system noise is known and the measurement noise is unknown, the optimal bayesian filtering containing the measurement noise can be summarized as prediction and update:

p(x _k ,R _k |z _1:k-1 )

＝∫p(x _k |x _k-1 )p(R _k |R _k-1 )p(x _k-1 ,R _k-1 |z _1:k-1 )dx _k-1 dR _k-1

p(x _k ,R _k |z _1:k )∝p(z _k |x _i ,z _k )p(x _k ,R _k |z _1:k-1 )

since the Bayesian filtering is difficult to solve, a unified density q (x) _k ,R _k ) To approximate the true posterior probability distribution function p (x _k ,R _k |z _1:k ) Namely, a variational Bayesian algorithm:

p(x _k ,R _k |z _1:k )≈q(x _k )q(R _k )

where q (·) is an approximate posterior probability density function of p (·). The optimal solution of the expression can be determined by minimizing the true posterior probability density function (Probability Density Function, PDF) p (x _k ,R _k |z _1:k ) And approximate posterior PDFq (x _k )q(R _k ) The Kullback-Leibler divergence therebetween. q (x) _k )q(R _k ) The updates are gaussian distribution and inverse Wishart distribution:

the above method needs fixed-point iterative solution, q ⁽ⁱ⁺¹⁾ (R _k ) Can be updated as:

in the degree of freedom factorAnd inverse scale matrix->Can be expressed as:

wherein,can be expressed as:

it is desirable toExpressed as:

q ⁽ⁱ⁺¹⁾ (x _k ) The updating is as follows:

wherein the mean value is obtained by standard Kalman filteringAnd covariance matrix->

The method is simple derivation of VBKF, and more remarkable characteristic information is obtained through the VBKF;

Step 2.4 shows an application case when part of the DVL beam is missing, and specifically comprises the following steps:

based on the assumption that DVL beams have the following characteristics:

at this time, it is required to satisfy that the AUV has no vertical velocity, and when the AUV fluctuates excessively with the ocean current, the formula is not established. The invention has the following limitation conditions for the application when part of DVL wave beams are absent:

1) Three beams are active: at this time, the complete pseudo beam information can be obtained through the formula, and error prediction compensation can be carried out through the invention;

2) Two orthogonal beams are active: at this time, the complete pseudo beam information can be obtained through the formula, and error prediction compensation can be carried out through the invention;

in other three cases, that is, that two parallel beams are effective or only one beam is effective and all the beams are invalid, DVL information has many defects, and the invalid DVL beam can be predicted by selecting different characteristic information (such as selecting information related to SINS solution);

further, in the step 3, the sample data collected in the step 2 is standardized, and the sample data is processed through a PSO algorithm to realize the optimization of the parameters of the ANFIS model, so as to complete the training process of the ANFIS model, and the specific steps are as follows:

Step 3.1 normalize sample data:

z-score normalization of the innovation;

performing min-max standardization on the Markov distance and the measurement noise covariance matrix to enable the result to fall into a [0,1] interval;

step 3.2 the specific ANFIS algorithm procedure is as follows:

for a simple TSK fuzzy system model: the manner in which functions combine or interact is called a rule, which includes a pre-parameter and a post-parameter. In order to realize the learning process of the TSK fuzzy model, the TSK fuzzy model is generally converted into a self-adaptive neural network, and the membership function of the neural network, namely the ANFIS, is obtained by training sample data. Given a pre-parameter, the output of an ANFIS may be represented as a linear combination of post-parameters;

the ANFIS has five layers in total,

step 3.2.1 first layer, membership function layer of input variables:

each node i has an output function:

where in is an input, including x, y, z; m is M _i Is a fuzzy set comprising A _i ,B _i ,C _i ；Is a membership function of the fuzzy set M, and represents the degree to which a given input in meets M;

membership functions are numerous and include bell-shaped, gaussian-shaped, triangular, etc. In general we choose μ _M Is a generalized bell-shaped membership function:

wherein a is _i ,b _i ,c _i Is a set of parameters whose values change the shape of the bell-shaped function, resulting in different membership functions. These parameters are called front-piece parameters, which are adaptively adjusted during the learning process of the algorithm;

Step 3.2.2 second layer, regular intensity release layer:

each node i is responsible for multiplying the input signals:

wherein omega _i Is the output of each node, representing the trustworthiness of the rule;

step 3.2.3 normalization process of all rule intensities:

the i-th node calculates the ratio of the release strength of rule i to the sum of all rule release strengths:

step 3.2.4, calculating the output of the fuzzy rule in the fourth layer:

each node i of this layer is an adaptive node whose output is:

wherein,is the output of the third layer, according to the backward parameter { m } _i ,p _i ,q _i ,r _i Calculating the output of the fuzzy rule by the aid of the } and the membership function;

step 3.2.5 fifth layer, calculating the total output of the input signals:

as previously described, the output of an ANFIS may be represented as a linear combination of backward parameters, the front piece parameters having been given:

the training process of the ANFIS first extracts an initial blur model from the acquired sample data and then optimizes model parameters from Layer 1 to Layer 5. Wherein the node parameters of the first and fourth layers are adaptive and the node parameters of the second and third layers are fixed.

The invention adopts PSO algorithm to optimize the front part and the back part parameters of the ANFIS model. From the first layer to the fourth layer, the back-piece parameters are calculated by least squares estimation, the error between the iteration value and the expected value of the training data is calculated, and in the reverse transmission process, the error signal is transmitted from the output layer back to the input layer, and the back-piece parameters are adjusted by PSO. In the process of changing the parameters, the shape of the membership function is continuously modified so as to achieve the aim of minimizing the output error in the set period.

Step 3.3 the specific PSO parameter optimizing algorithm process is as follows:

PSO is a random optimization algorithm, the solution of the problem is called particles, and the optimization result can be checked by simulating the cooperation and competition modes of individuals in the particle swarm;

for a population of N particles, there is an N-dimensional search space. In PSO, each particle is assigned a position vector x _i And a velocity vector v _i The corresponding objective function allows the particle to obtain fitness and to determine the fitness from the previous and current position (x _i ) Is selected to be the best position (p _best ) The method comprises the steps of carrying out a first treatment on the surface of the In addition, in one group, all particles have their global optimal position (g _best )；

The speed vector and position vector update of the ith particle are expressed as:

in the above-mentioned formula(s),a velocity vector representing the particle i at the d-dimensional kth time; />A position vector representing the particle i at the k-th moment of the d-dimension; omega represents an inertia weight; r is (r) ₁ And r ₂ Representing a random number from 0 to 1; c ₁ And c ₂ For acceleration coefficient, i.e. cognition coefficient (c ₁ ) And social coefficient (c) ₂ )；

The velocity vector equation mainly consists of three parts, namely cognition, society and inertia. Wherein the inertial component is a memory of the previous direction of motion that caused the particle to fly over its path at time k; the cognitive component is a velocity component generated by moving the particles to a previous optimal position; the social component is to evaluate the performance of a particle with respect to its neighbors and the whole population of particles. These three components define the trajectory of the particle throughout the search space;

Optimizing parameters of the ANFIS model by adopting a PSO algorithm according to the training data comprising the input data and the target output acquired in the step 2 to obtain a trained ANFIS model;

further, in the step 4, the AUV is controlled to submerge under water, and the improved PSO-ANFIS algorithm is adopted to predict the four-beam absolute error of the DVL, and the specific steps are as follows:

step 4.1, inertial navigation calculation is carried out through the initial information and the IMU information;

step 4.2, calculating pseudo measurement information corresponding to the velocity of the DVL four beams according to the DVL update frequency through inertial navigation calculation information;

step 4.3 filtering the difference between the calculated pseudo measurement information and the DVL four-beam measurement value as the observed quantity through VBKF, thereby obtaining three kinds of characteristic information v consistent with the step 2 _k ,λ _k ,As input to the ANFIS model;

step 4.4, carrying out a standardization process consistent with the step 3 on the collected characteristic information, and carrying out error prediction on DVL measurement information through an ANFIS model of PSO auxiliary training in the step 3;

further, in the step 5, based on the ANFIS prediction result, the characteristic change of the error is monitored, the actual measurement of the DVL is compensated by the anomaly discrimination mechanism, and the compensated DVL quantity measurement and the pseudo observed quantity calculated by the SINS are subjected to kalman filtering, which specifically comprises the following steps:

Step 5.1, analyzing training samples through Root-Mean-Square Error (RMSE) epsilon of predicted data, selecting 3 epsilon as a measurement abnormal threshold T of model prediction according to experience, and taking the value as a state discrimination standard of model prediction output;

step 5.2 modeling the predicted values according to the Bernoulli distribution:

step 5.3, compensating the DVL measured value according to the state discrimination standard:

wherein, gamma _k =0 means that DVL measurement is normal; gamma ray _k =1 represents abnormal DVL measurement, predicted by ANFISCompensating;

step 5.4 selecting the compensated DVL measurement Z _DVL,k And the difference of the corresponding pseudo observed information calculated by the SINS is used as an observed value, and the SINS/DVL tight combination navigation under the complex environment is realized by VBKF.

Compared with the prior art, the invention has the following advantages and beneficial effects:

1. high quality ANFIS system input information is collected using a variable decibel-based adaptive filtering method. The new information, the mahalanobis distance and the measured noise covariance matrix are originally closely related to the measured value, and the adaptive filtering method can enable the mahalanobis distance and the measured noise covariance matrix in the ANFIS input information to be more remarkable when encountering abnormal values, so that the training quality of the ANFIS is improved.

2. And introducing a particle swarm optimization algorithm to perform parameter optimization on the ANFIS system. In order to obtain a more accurate and stable ANFIS model, the PSO is used to assist in training the ANFIS model, considering that the number of samples is not necessarily sufficient and the ANFIS initial parameter settings are random.

3. Instead of eliminating error points, hierarchical processing of ANFIS prediction data is selected. Therefore, when the DVL measured value encounters continuous errors such as irregular updating of measurement information, short-time failure of partial wave beams and the like, the information of the DVL can be utilized to the maximum extent, and the navigation accuracy is prevented from being reduced due to continuous elimination of data points.

Drawings

FIG. 1 is a schematic diagram of a SINS/DVL tightly integrated navigation system provided by the invention;

FIG. 2 is a diagram of an ANFIS model structure with 3-inputs and 3-rules provided by the present invention;

FIG. 3 is a schematic diagram of an SINS/DVL integrated navigation system in a complex environment according to the present invention.

Detailed Description

The present invention is further illustrated in the following drawings and detailed description, which are to be understood as being merely illustrative of the invention and not limiting the scope of the invention. It should be noted that the words "front", "rear", "left", "right", "upper" and "lower" used in the following description refer to directions in the drawings, and the words "inner" and "outer" refer to directions toward or away from, respectively, the geometric center of a particular component.

The invention discloses an application method of an SINS/DVL tightly combined navigation method based on improved PSO-ANFIS assistance in AUV navigation, which has a flow shown in a figure 3 and comprises the following steps:

step 1: establishing a state equation of the SINS/DVL integrated navigation system according to the system error equation, and establishing a measurement equation of the SINS/DVL integrated navigation system according to the difference between pseudo measurement information calculated by navigation information calculated by the SINS, four-beam speed information measured by the DVL and depth information measured by the depth gauge as measurement, wherein the measurement equation is shown in figure 1;

step 1.1, defining a coordinate system which needs to be used:

e-earth coordinate system: is fixedly connected with the earth, the origin is positioned at the earth center, x _e The axis passes through the intersection point of the primary meridian and the equator, z _e The axis pointing north, y _e Axis x _e 、z _e Forming a right-hand coordinate system;

n-a navigational coordinate system coincident with the east-north-day geographic coordinate system;

b—carrier coordinate system: the origin being at the centre of the carrier, z _b Axis-vertical carrier up, x _b Directed forward of the carrier, y _b And x _b 、z _b Forming a right-hand coordinate system;

d-an orthogonal coordinate system aligned with the beam center of the DVL, here denoted the beam system;

step 1.2, a state equation of the SINS/DVL tightly combined navigation system is established, and the specific steps are as follows:

Taking the attitude error angle phi= [ phi ] _x φ _y φ _z ]Speed error δv= [ δv ] _E δV _N δV _U ]Position error δp= [ δ ] _L δ _λ δ _h ]Gyro constant drift epsilon and accelerometer random constant errorAs a state quantity of the SINS system, it is noted that:

wherein phi is _x Is the east misalignment angle phi _y Is the north misalignment angle phi _z Is the angle of the misalignment in the sky; δV (delta V) _E Is the east speed error, δV _N Is the north speed error, δV _U Is the error of the tangential velocity;δl is latitude error, δλ is longitude error, δh is altitude error; epsilon _x Is drift of the x-direction gyro, epsilon _y Is drift of the Y-direction gyro, epsilon _z Is z-direction gyro drift;is the random constant error of the x-direction accelerometer,is the random constant error of the y-direction accelerometer, < >>Is the random constant error of the z-direction accelerometer;

noise of SINS system:

W _SINS ＝[ω _gx ω _gy ω _gz ω _ax ω _ay ω _az ] ^T

wherein omega _g Is the process noise vector, omega of the gyro _a Is the process noise vector of the accelerometer.

Taking DVL four-beam velocity zero offset delta b= [ delta b ] ₁ δb ₂ δb ₃ δb ₄ ]The scale factor error δk is taken as a DVL system state variable and is noted as:

X _DVL ＝[δb ₁ δb ₂ δb ₃ δb ₄ δk] ^T

wherein δb ₁ Is the beam1 velocity zero offset, delta b ₂ Is the beam2 velocity zero offset, delta b ₃ Is the beam3 velocity zero offset, delta b ₄ Is beam4 velocity zero offset;

the noise of DVL system is omega _d ；

Taking the offset delta b of the depth gauge _ps As state variables of the depth gauge:

X _PS ＝δb _ps

the noise of the depth gauge system is omega _ps ；

The state quantity of the integrated navigation system can be expressed as:

X＝[X _SINS X _DVL X _PS ] ^T

from the error model of the navigation system, the state equation can be derived:

wherein F is a state transition matrix, and W is system noise;specific F _SINS The derivation is not developed in detail here, there are many literature on the derivation process, the +.>

Step 1.3, a measurement equation of the SINS/DVL tightly combined navigation system is established, and the specific steps are as follows:

in the case of neglecting sensor errors, the speed is defined as follows:

wherein,is the speed of SINS under n line, +.>Is the speed of SINS under b line, +.>SINS speed under beam line, < +.>Is the velocity of the DVL measurement. There is a relationship between them as follows:

wherein,represents the transformation matrix from n-series to b-series, < >>Representing a conversion matrix from b-series to beam-series, which specifically represents the following formula:

wherein b _i Is based on the geometrical relationship between DVL beam and AUV, slave V ^b To V ^d Can be expressed as:

where α is the beam tilt angle of the DVL, a characteristic of DVL fixation.Can be expressed as +.>Wherein->DVL for "+" configuration, and +.>DVL for an "x" configuration;

the amount formed by SINS, DVL and depth gauge:

wherein,depth information calculated by SINS, +.>Is depth information measured by the depth gauge. Defining the measurement of a depth gauge The error model is:

wherein H is _PS Is a depth truth value. Based on the above analysis, SINS solutionThe calculation formula is as follows: />

Defining a measurement error model of DVL as follows:

based on the above analysis, the SINS solution speed is converted into the beam systemThe calculation formula is as follows:

wherein, [. Times ] represents a cross multiplication operation. From this, the measurement equation of the integrated navigation system can be obtained:

Z＝HX+V

wherein,

V＝[ω _d ω _ps ] ^T

step 2: controlling the diving depth of the AUV to reach the vicinity of the water surface, and after the initial alignment of the navigation positioning system is completed, assisting in collecting sample data containing various abnormal measurement types by means of GNSS information and a variational Bayesian Kalman filtering algorithm for training the ANFIS;

step 2.1, collecting the variable most relevant to the occurrence of the abnormality of the DVL measurement information, and taking the variable as characteristic information, wherein the specific steps are as follows:

the innovation refers to the difference between the model predicted value and the measured value. Innovation v in Kalman filtering in SINS/DVL integrated navigation system _k The difference between the estimated value of the system model and the actual measured value of DVL may be mapped. The computational expression is as follows:

wherein,the actual measurement information of DVL at time k.Is a one-step predictor of time k. We consider the system model to be accurately modeled, so that when the innovation suddenly gets large, the DVL measurement is not accurate enough and the measurement error increases. Thus, the innovation is taken as one of the variables reflecting the DVL measurement error, as input to the ANFIS system;

The mahalanobis distance describes the standard deviation distance from a sampling point to a distribution, and can effectively calculate the similarity of two groups of random variables. The invention constructs a second input based on mahalanobis distance:

wherein lambda is _k Is measurement anomaly characteristic information defined according to a mahalanobis distance.P _k/k-1 Is a one-step prediction variance matrix. For convenience, λ will be herein _k Known as mahalanobis distance;

when the DVL measurement information is abnormal, the measurement noise may change. Specific methods that can cause the measurement noise covariance matrix to show this variation will be in stepThe description is given in step 2.3. Measuring noise covariance matrixIs characteristic information that may represent measurement anomalies, and thus serves as a third input;

step 2.2, collecting absolute error of DVL measurement information as output of training data, specifically comprising the following steps:

the learning process of the model requires an accurate ideal output value. During testing and verification, the accurate measurement error is selected as the output of the training data, taking into account the sensors loaded on the AUV. Such accurate measurement errors can be obtained from the PHINS loaded by the AUV:

in the method, in the process of the invention,the absolute error of the DVL measurement at time k. PHINS is an optical fiber inertial navigation sensor, which can integrate GPS information and provide the most accurate attitude, speed and position information. Z is Z _GPS,k For accurate pseudo-measurement information obtained at the kth time according to the posture and speed information provided by PHINS, and +.>The solving mode is the same;

to sum up, in order to make the training of the target model more accurate, we have chosen the three pieces of characteristic information that react best to the measured outliers. They are innovation v _k Lambda based on mahalanobis distance construction _k And VBKF-based acquisitionIn addition, the absolute error of the DVL measurement information is selected as output, so that the DVL data quality can be intuitively displayed, and the subsequent compensation and utilization are facilitated.

Step 2.3, expanding a noise uncertainty processing method based on VBKF, which comprises the following specific steps:

the constant filter parameters do not describe the statistical characteristics of the observed quantity variations. Classical kalman filtering algorithms treat all observations as the same feature, and when the system observations are abnormal or noise changes, the algorithm cannot adapt to the system changes. In order to accurately respond to possible anomalies, the system must respond specifically to unknown anomalies. When the measurement information is abnormal, the corresponding Marshall distance and R need to be obtained _k VBKF was introduced to characterize its variation. Unknown can be estimated based on VBKFTo improve the validity of ANFIS input information;

When the system noise is known and the measurement noise is unknown, the optimal bayesian filtering containing the measurement noise can be summarized as prediction and update:

p(x _k ,R _k |z _1:k-1 )

＝∫p(x _k |x _k-1 )p(R _k |R _k-1 )p(x _k-1 ,R _k-1 |z _1:k-1 )dx _k-1 dR _k-1

p(x _k ,R _k |z _1:k )∝p(z _k |x _k ,z _k )p(x _k ,R _k |z _1:k-1 )

since the Bayesian filtering is difficult to solve, a unified density q (x) _k ,R _k ) To approximate the true posterior probability distribution function p (x _k ,R _k |z _1:k ) Namely, a variational Bayesian algorithm:

p(x _k ,R _k |z _1:k )≈q(x _k )q(R _k )

where q (·) is an approximate posterior probability density function of p (·). The optimal solution of the expression can be determined by minimizing the true posterior probability density function (Probability Density Function, PDF) p (x _k ,R _k |z _1:k ) And approximate posterior PDFq (x _k )q(R _k ) The Kullback-Leibler divergence therebetween. q (x) _k )q(R _k ) The updates are gaussian distribution and inverse Wishart distribution:

the above method needs fixed-point iterative solution, q ⁽ⁱ⁺¹⁾ (R _k ) Can be updated as:

in the degree of freedom factorAnd inverse scale matrix->Can be expressed as: />

Wherein,can be expressed as:

it is desirable toExpressed as:

q ⁽ⁱ⁺¹⁾ (x _k ) The updating is as follows:

wherein the mean value is obtained by standard Kalman filteringAnd covariance matrix->

The method is simple derivation of VBKF, and more remarkable characteristic information is obtained through the VBKF;

step 2.4 shows an application case when part of the DVL beam is missing, and specifically comprises the following steps:

based on the assumption that DVL beams have the following characteristics:

At this time, it is required to satisfy that the AUV has no vertical velocity, and when the AUV fluctuates excessively with the ocean current, the formula is not established. The invention has the following limitation conditions for the application when part of DVL wave beams are absent:

1) Three beams are active: at this time, the complete pseudo beam information can be obtained through the formula, and error prediction compensation can be carried out through the invention;

2) Two orthogonal beams are active: at this time, the complete pseudo beam information can be obtained through the formula, and error prediction compensation can be carried out through the invention;

in other three cases, that is, that two parallel beams are effective or only one beam is effective and all the beams are invalid, DVL information has many defects, and the invalid DVL beam can be predicted by selecting different characteristic information (such as selecting information related to SINS solution);

step 3: normalizing the sample data collected in the step 2, and processing the sample data through a particle swarm optimization algorithm to realize the optimization of the parameters of the ANFIS model, so as to complete the training process of the ANFIS model;

step 3.1 normalize sample data:

z-score normalization of the innovation;

performing min-max standardization on the Markov distance and the measurement noise covariance matrix to enable the result to fall into a [0,1] interval;

Step 3.2 the specific ANFIS algorithm procedure is as follows:

for a simple TSK fuzzy system model: the manner in which functions combine or interact is called a rule, which includes a pre-parameter and a post-parameter. In order to realize the learning process of the TSK fuzzy model, the TSK fuzzy model is generally converted into a self-adaptive neural network, and the membership function of the neural network, namely the ANFIS, is obtained by training sample data. Given a pre-parameter, the output of an ANFIS may be represented as a linear combination of post-parameters;

ANFIS has five layers;

step 3.2.1 first layer, membership function layer of input variables:

each node i has an output function:

where in is an input, including x, y, z; m is M _i Is a fuzzy set comprising A _i ,B _i ,C _i ；Is a membership function of the fuzzy set M, and represents the degree to which a given input in meets M;

membership functions are of a wide varietyIncluding bell-shaped, gaussian-shaped, triangular, etc. In general we choose μ _M Is a generalized bell-shaped membership function:

wherein a is _i ,b _i ,c _i Is a set of parameters whose values change the shape of the bell-shaped function, resulting in different membership functions. These parameters are called front-piece parameters, which are adaptively adjusted during the learning process of the algorithm;

Step 3.2.2 second layer, regular intensity release layer:

each node i is responsible for multiplying the input signals:

wherein omega _i Is the output of each node, representing the trustworthiness of the rule;

step 3.2.3 normalization process of all rule intensities:

the i-th node calculates the ratio of the release strength of rule i to the sum of all rule release strengths:

step 3.2.4, calculating the output of the fuzzy rule in the fourth layer:

each node i of this layer is an adaptive node whose output is:

wherein,is the output of the third layer, according to the backward parameter { m } _i ,p _i ,q _i ,r _i Calculating the output of the fuzzy rule by the aid of the } and the membership function;

step 3.2.5 fifth layer, calculating the total output of the input signals:

as previously described, the output of an ANFIS may be represented as a linear combination of backward parameters, the front piece parameters having been given:

the training process of the ANFIS first extracts an initial blur model from the acquired sample data and then optimizes model parameters from Layer 1 to Layer 5. Wherein the node parameters of the first layer and the fourth layer are self-adaptive, and the node parameters of the second layer and the third layer are fixed;

the invention adopts PSO algorithm to optimize the front part and the back part parameters of the ANFIS model. From the first layer to the fourth layer, the back-piece parameters are calculated by least squares estimation, the error between the iteration value and the expected value of the training data is calculated, and in the reverse transmission process, the error signal is transmitted from the output layer back to the input layer, and the back-piece parameters are adjusted by PSO. In the process of changing the parameters, the shape of the membership function is continuously modified so as to achieve the aim of minimum output error in a set period;

Step 3.3 the specific PSO parameter optimizing algorithm process is as follows:

PSO is a random optimization algorithm, the solution of the problem is called particles, and the optimization result can be checked by simulating the cooperation and competition modes of individuals in the particle swarm;

for a population of N particles, there is an N-dimensional search space. In PSO, each particle is assigned a position vector x _i And a velocity vector v _i The corresponding objective function allows the particle to obtain fitness and to determine the fitness from the previous and current position (x _i ) Is selected to be the best position (p _best ) The method comprises the steps of carrying out a first treatment on the surface of the In addition, in one group, all particles areIts global optimum position (g) _best )；

The speed vector and position vector update of the ith particle are expressed as:

in the above-mentioned formula(s),a velocity vector representing the particle i at the d-dimensional kth time; />A position vector representing the particle i at the k-th moment of the d-dimension; omega represents an inertia weight; r is (r) ₁ And r ₂ Representing a random number from 0 to 1; c ₁ And c ₂ For acceleration coefficient, i.e. cognition coefficient (c ₁ ) And social coefficient (c) ₂ )；

The velocity vector equation mainly consists of three parts, namely cognition, society and inertia. Wherein the inertial component is a memory of the previous direction of motion that caused the particle to fly over its path at time k; the cognitive component is a velocity component generated by moving the particles to a previous optimal position; the social component is to evaluate the performance of a particle with respect to its neighbors and the whole population of particles. These three components define the trajectory of the particle throughout the search space;

Optimizing parameters of the ANFIS model by adopting a PSO algorithm according to the training data comprising the input data and the target output acquired in the step 2 to obtain a trained ANFIS model;

step 4: controlling the AUV to submerge under water, and carrying out online prediction on the four-beam absolute error of the DVL by adopting an improved PSO-ANFIS algorithm;

step 4.1, inertial navigation calculation is carried out through the initial information and the IMU information;

step 4.2, calculating pseudo measurement information corresponding to the velocity of the DVL four beams according to the DVL update frequency through inertial navigation calculation information;

step 4.3 filtering the difference between the calculated pseudo measurement information and the DVL four-beam measurement value as the observed quantity through VBKF, thereby obtaining three kinds of characteristic information v consistent with the step 2 _k ,λ _k ,As input to the ANFIS model;

step 4.4, carrying out a standardization process consistent with the step 3 on the collected characteristic information, and carrying out error prediction on DVL measurement information through an ANFIS model of PSO auxiliary training in the step 3;

step 5: based on an ANFIS prediction result, monitoring the characteristic change of the error, compensating the actual measurement of the DVL through an anomaly discrimination mechanism, and carrying out Kalman filtering on the compensated DVL quantity measurement and the pseudo observed quantity calculated by the SINS;

Step 5.1, analyzing training samples through Root-Mean-Square Error (RMSE) epsilon of predicted data, selecting 3 epsilon as a measurement abnormal threshold T of model prediction according to experience, and taking the value as a state discrimination standard of model prediction output;

step 5.2 modeling the predicted values according to the Bernoulli distribution:

step 5.3, compensating the DVL measured value according to the state discrimination standard:

wherein, gamma _k =0 means that DVL measurement is normal; gamma ray _k =1 represents abnormal DVL measurement, predicted by ANFISCompensating;

step 5.4 selecting the compensated DVL measurement Z _DVL,k And the difference of the corresponding pseudo observed information calculated by the SINS is used as an observed value, and the SINS/DVL tight combination navigation under the complex environment is realized by VBKF.

The technical means disclosed by the scheme of the invention is not limited to the technical means disclosed by the embodiment, and also comprises the technical scheme formed by any combination of the technical features.

Claims (2)

1. An improved PSO-ANFIS-assisted SINS/DVL tightly integrated navigation method, which is characterized by comprising the following steps:

step 1: establishing a state equation of the SINS/DVL integrated navigation system according to the system error equation, and establishing a measurement equation of the SINS/DVL integrated navigation system by taking the difference between pseudo measurement information calculated by navigation information calculated by the SINS, four-beam speed information measured by the DVL and depth information measured by the depth gauge as measurement; in the step 1, a state equation and a measurement equation of the SINS/DVL tightly combined navigation system are established, and the specific process is as follows:

Step 1.1, defining a coordinate system which needs to be used:

e-earth coordinate system: is fixedly connected with the earth, the origin is positioned at the earth center, x _e The axis passes through the intersection point of the primary meridian and the equator, z _e The axis pointing north, y _e Axis, x _e Axis, z _e The axes form a right hand coordinate system; n-a navigational coordinate system coincident with the east-north-day geographic coordinate system; b—carrier coordinate system: the origin being at the centre of the carrier, z _b Axis-vertical carrier up, x _b The axis pointing forward of the carrier, y _b Axis and x _b Axis, z _b The axes form a right hand coordinate system; d-an orthogonal coordinate system aligned with the beam center of the DVL, here denoted the beam system;

step 1.2, a state equation of the SINS/DVL tightly combined navigation system is established, and the specific steps are as follows:

taking the attitude error angle phi= [ phi ] _x φ _y φ _z ]Speed error δv= [ δv ] _E δV _N δV _U ]Position error δp= [ δlδλδh]Constant drift epsilon of gyroscope and random constant of accelerometerValue errorAs a state quantity of the SINS system, it is noted that:

wherein phi is _x Is the east misalignment angle phi _y Is the north misalignment angle phi _z Is the angle of the misalignment in the sky; δV (delta V) _E Is the east speed error, δV _N Is the north speed error, δV _U Is the error of the tangential velocity; δl is latitude error, δλ is longitude error, δh is altitude error; epsilon _x Is drift of the x-direction gyro, epsilon _y Is drift of the Y-direction gyro, epsilon _z Is z-direction gyro drift;is the random constant error of the x-direction accelerometer, < >>Is the random constant error of the y-direction accelerometer, < >>Is the random constant error of the z-direction accelerometer; noise of SINS system:

W _SINS ＝[ω _gx ω _gy ω _gz ω _ax ω _ay ω _az ] ^T

wherein omega _g Is the process noise vector, omega of the gyro _a Process noise vectors for the accelerometer; taking DVL four-beam velocity zero offset delta b= [ delta b ] ₁ δb ₂ δb ₃ δb ₄ ]The scale factor error δk is taken as a DVL system state variable and is noted as:

X _DVL ＝[δb ₁ δb ₂ δb ₃ δb ₄ δk] ^T

wherein δb ₁ Is the beam1 velocity zero offset, delta b ₂ Is beam2 velocity zero offset, δb ₃ Is the beam3 velocity zero offset, delta b ₄ Is beam4 velocity zero offset;

the noise of DVL system is omega _d The method comprises the steps of carrying out a first treatment on the surface of the Taking the offset delta b of the depth gauge _ps As state variables of the depth gauge:

X _PS ＝δb _ps

the noise of the depth gauge system is omega _ps ；

The state quantity of the integrated navigation system is expressed as:

X＝[X _SINS X _DVL X _PS ] ^T

obtaining a state equation from an error model of the navigation system:

wherein F is a state transition matrix, and W is system noise;

step 1.3, a measurement equation of the SINS/DVL tightly combined navigation system is established, and the specific steps are as follows:

in the case of neglecting sensor errors, the speed is defined as follows:

wherein,is the speed of SINS under n line, +.>Is the speed of SINS under b line, +.>SINS speed under beam line, < +. >Is the velocity of the DVL measurement; there is a relationship between them as follows:

wherein,represents the transformation matrix from n-series to b-series, < >>Representing a conversion matrix from b-series to beam-series, which specifically represents the following formula:

wherein b _i Is based on the geometrical relationship between DVL beam and AUV, slave V ^b To V ^d Is expressed as:

where α is the beam tilt angle of the DVL, a characteristic of DVL fixation;denoted as->Wherein the method comprises the steps ofDVL for "+" configuration, and +.>DVL for an "x" configuration;

the amount formed by SINS, DVL and depth gauge:

wherein,depth information calculated by SINS, +.>Is depth information measured by the depth gauge; defining a measurement error model of the depth gauge as follows:

wherein H is _PS Is a depth truth value; based on the above analysis, SINS solutionThe calculation formula is as follows:

defining a measurement error model of DVL as follows:

based on the above analysis, the SINS solution speed is converted into the beam systemThe calculation formula is as follows:

wherein, [. Times. ] represents a cross multiplication operation; the measurement equation of the integrated navigation system is obtained:

Z＝HX+V

wherein,

V＝[ω _d ω _ps ] ^T ；

step 2: controlling the diving depth of the AUV to reach the vicinity of the water surface, and after the initial alignment of the navigation positioning system is completed, assisting in collecting sample data containing various abnormal measurement types by means of GNSS information and a variational Bayesian Kalman filtering algorithm VBKF for training the ANFIS;

In step 2, the collecting of sample data including various abnormal measurement types is assisted by GNSS information and VBKF algorithm for training ANFIS, and specifically includes the following steps:

step 2.1, collecting the variable most relevant to the occurrence of the abnormality of the DVL measurement information, and taking the variable as characteristic information, wherein the specific steps are as follows:

innovation v in Kalman filtering in SINS/DVL integrated navigation system _k Mapping a difference between an estimated value of the system model and an actual measured value of the DVL; the computational expression is as follows:

wherein,the actual measurement information of DVL at the moment k; />Is a one-step predicted value for time k; we consider the system model to be accurately modeled, so that when the innovation suddenly gets large, the DVL measurement is not accurate enough and the measurement error increasesLarge; thus, the innovation is taken as one of the variables reflecting the DVL measurement error, as input to the ANFIS system;

constructing a second input based on the mahalanobis distance:

wherein lambda is _k Measuring abnormal characteristic information defined according to the mahalanobis distance; P _k/k-1 is a one-step prediction variance matrix; for convenience, lambda will be _k Known as mahalanobis distance;

when the DVL measurement information is abnormal, the measurement noise can change; a specific method of causing the measurement noise covariance matrix to show such a variation; measuring noise covariance matrix Is characteristic information representing measurement abnormality, and thus serves as a third input;

step 2.2, collecting absolute error of DVL measurement information as output of training data, specifically comprising the following steps:

the learning process of the model requires an accurate ideal output value; in the testing and verifying process, taking the sensor loaded on the AUV into consideration, selecting an accurate measurement error as output of training data;

this precise measurement error is obtained from the PHINS loaded by the AUV:

in the method, in the process of the invention,for the kth time DVAbsolute error of L measurement; PHINS is an optical fiber inertial navigation sensor which integrates GPS information and provides the most accurate attitude, speed and position information; z is Z _GPS，k For accurate pseudo-measurement information obtained at the kth time according to the posture and speed information provided by PHINS, and +.>The solving mode is the same;

step 2.3, expanding a noise uncertainty processing method based on VBKF, which comprises the following specific steps:

in order to accurately respond to possible anomalies, the system must respond specifically to unknown anomalies; when the measurement information is abnormal, the corresponding Marshall distance and R need to be obtained _k VBKF is introduced to characterize the change of the variable; VBKF-based estimation of unknownsTo improve the validity of ANFIS input information;

When the system noise is known and the measurement noise is unknown, the optimal bayesian filtering containing the measurement noise is summarized as prediction and update:

p(x _k ，R _k |z _1：k-1 )＝∫p(x _k |x _k-1 )p(R _k |R _k-1 )p(x _k-1 ，R _k-1 |z _1：k-1 )dx _k-1 dR _k-1 p(x _k ，R _k |z ₁ x _k )∝p(z _k |x _k ，z _k )p(x _k ，R _k |z _1：k-1 )

since the Bayesian filtering is difficult to solve, a unified density q (x) _k ，R _k ) To approximate the true posterior probability distribution function p (x _k ，R _k |z _1：k ) Namely, a variational Bayesian algorithm:

p(x _k ，R _k |z _1：k )≈q(x _k )q(R _k )

wherein q (·) is an approximate posterior probability density function of p (·); the optimal solution of the expression may be passed through a minimumThe true posterior probability density function PDF, p (x) _k ，R _k |z _1：k ) And approximate posterior PDF q (x _k )q(R _k ) The Kullback-Leibler divergence between; q (x) _k )q(R _k ) The updates are gaussian distribution and inverse Wishart distribution:

the above method needs fixed-point iterative solution, q ⁽ⁱ⁺¹⁾ (R _k ) Can be updated as:

in the degree of freedom factorAnd inverse scale matrix->Can be expressed as:

wherein,can be expressed as:

it is desirable toExpressed as:

q ⁽ⁱ⁺¹⁾ (x _k ) The updating is as follows:

wherein the mean value is obtained by standard Kalman filteringAnd covariance matrix->

The method is simple derivation of VBKF, and more remarkable characteristic information is obtained through the VBKF; step 2.4 shows an application case when part of the DVL beam is missing, and specifically comprises the following steps:

based on the assumption that DVL beams have the following characteristics:

at the moment, the requirement that the AUV has no vertical speed is met, and when the AUV is excessively large in up-and-down fluctuation amplitude along with ocean current, the formula is not established; the following constraints apply when part of the DVL beam is missing:

1) Three beams are active: at this time, the complete pseudo beam information can be obtained through the formula, and error prediction compensation can be performed;

2) Two orthogonal beams are active: at this time, the complete pseudo beam information can be obtained through the formula, and error prediction compensation can be performed;

in addition, the DVL information has a plurality of defects in three cases that two parallel beams are effective or only one beam is effective and all the beams are invalid, and at the moment, different characteristic information can be selected; the DVL beam that is failing is predicted,

step 3: normalizing the sample data collected in the step 2, and processing the sample data through a particle swarm optimization algorithm to realize the optimization of the parameters of the ANFIS model, so as to complete the training process of the ANFIS model; in the step 3, the sample data collected in the step 2 is standardized, and the sample data is processed by a particle swarm optimization algorithm to realize the optimization of the parameters of the ANFIS model, so as to complete the training process of the ANFIS model, and the specific steps are as follows:

step 3.1 normalize sample data: z-score normalization of the innovation; performing min-max standardization on the Markov distance and the measurement noise covariance matrix to enable the result to fall into a [0,1] interval; step 3.2 the specific ANFIS algorithm procedure is as follows:

For a simple TSK fuzzy system model: the manner in which functions combine or interact is called a rule, which includes a pre-parameter and a post-parameter; in order to realize the learning process of the TSK fuzzy model, the TSK fuzzy model is converted into a self-adaptive neural network, and a membership function of the neural network, namely ANFIS, is obtained by training sample data; given a pre-parameter, the output of an ANFIS may be represented as a linear combination of post-parameters;

ANFIS has five layers, and is specifically as follows;

step 3.2.1 first layer, membership function layer of input variables:

each node i has an output function:

where in is an input, including x, y, z; m is M _i Is a fuzzy set comprising A _i ，B _i ，C _i ；Is a membership function of the fuzzy set M, and represents the degree to which a given input in meets M;

common membership functions are numerous, including bell-shaped, gaussian-shaped, triangular; we select μ _M Is a generalized bell-shaped membership function:

wherein a is _i ，b _i ，c _i Is a parameter set, and the change of the parameter values changes the shape of the bell-shaped function, so as to obtain different membership functions; these parameters are called front-piece parameters, which are adaptively adjusted during the learning process of the algorithm;

step 3.2.2 second layer, regular intensity release layer:

Each node i is responsible for multiplying the input signals:

wherein omega _i Is the output of each node, representing the trustworthiness of the rule;

step 3.2.3 normalization process of all rule intensities:

the i-th node calculates the ratio of the release strength of rule i to the sum of all rule release strengths:

step 3.2.4, calculating the output of the fuzzy rule in the fourth layer:

each node i of this layer is an adaptive node whose output is:

wherein,is the output of the third layer, according to the backward parameter { m } _i ，p _i ，q _i ，r _i Calculating the output of the fuzzy rule by the aid of the } and the membership function;

step 3.2.5 fifth layer, calculating the total output of the input signals:

，

the output of an ANFIS may be expressed as a linear combination of backward parameters, the front piece parameters having been given:

firstly, extracting an initial fuzzy model through collected sample data in the training process of the ANFIS, and then optimizing model parameters from Layer 1 to Layer 5; wherein the node parameters of the first layer and the fourth layer are self-adaptive, and the node parameters of the second layer and the third layer are fixed;

adopting a PSO algorithm to optimize the front part and the back part parameters of the ANFIS model; from the first layer to the fourth layer, the back-piece parameters are calculated by least squares estimation, the error between the iteration value and the expected value of the training data is calculated, and in the reverse transmission process, error signals are transmitted back to the input layer from the output layer, and the back-piece parameters are adjusted through PSO; in the process of changing the parameters, the shape of the membership function is continuously modified so as to achieve the aim of minimum output error in a set period;

Step 3.3 the specific PSO parameter optimizing algorithm process is as follows:

PSO is a random optimization algorithm, the solution of the problem is called particles, and the optimization result can be checked by simulating the cooperation and competition modes of individuals in the particle swarm;

for a population of N particles, there is an N-dimensional search space; in PSO, each particle is assigned a position vector x _i And a velocity vector v _i The corresponding objective function allows the particle to obtain fitness and to obtain fitness from the previous position and the current position x _i Is selected to be the best position p _best The method comprises the steps of carrying out a first treatment on the surface of the In addition, in a cluster, all particles have their global optimal position g _best ；

The speed vector and position vector update of the ith particle are expressed as:

in the above-mentioned formula(s),a velocity vector representing the particle i at the d-dimensional kth time; />A position vector representing the particle i at the k-th moment of the d-dimension; omega represents an inertia weight; r is (r) ₁ And r ₂ Representing a random number from 0 to 1; c ₁ And c ₂ For acceleration factor, i.e. cognition factor c ₁ And social coefficient c ₂ ；

The velocity vector equation mainly comprises three parts of cognition, society and inertia; wherein the inertial component is a memory of the previous direction of motion that caused the particle to fly over its path at time k; the cognitive component is a velocity component generated by moving the particles to a previous optimal position; social components are the evaluation of the particle's performance with respect to its neighbors and the whole particle population; these three components define the trajectory of the particle throughout the search space;

Optimizing parameters of the ANFIS model by adopting a PSO algorithm according to the training data comprising the input data and the target output acquired in the step 2 to obtain a trained ANFIS model;

step 4: controlling the AUV to submerge under water, and predicting the four-beam absolute error of the DVL by adopting an improved PSO-ANFIS algorithm, wherein the characteristic information is required to be acquired on line; the specific steps of controlling the AUV to submerge under water and predicting the four-beam absolute error of the DVL by adopting the improved PSO-ANFIS algorithm in the step 4 are as follows:

step 4.1, inertial navigation calculation is carried out through the initial information and the IMU information;

step 4.2, calculating pseudo measurement information corresponding to the velocity of the DVL four beams according to the DVL update frequency through inertial navigation calculation information;

step 4.3 filtering the difference between the calculated pseudo measurement information and the DVL four-beam measurement value as the observed quantity through VBKF, thereby obtaining three kinds of characteristic information v consistent with the step 2 _k ，λ _k ，As input to the ANFIS model;

step 4.4, carrying out a standardization process consistent with the step 3 on the collected characteristic information, and carrying out error prediction on DVL measurement information through an ANFIS model of PSO auxiliary training in the step 3;

step 5: based on PSO-ANFIS prediction results, monitoring characteristic changes of errors, compensating actual measurement of DVL through an anomaly discrimination mechanism, and carrying out Kalman filtering on the compensated DVL quantity measurement and pseudo observed quantity calculated by SINS.

2. The improved PSO-ANFIS-based aided SINS/DVL tight-combined navigation method of claim 1, wherein in step 5, based on the PSO-ANFIS prediction result, the characteristic change of the error is monitored, the actual measurement of the DVL is compensated by an anomaly discrimination mechanism, and the compensated DVL amount measurement and the pseudo observed quantity calculated by the SINS are kalman filtered, and the specific steps are as follows:

step 5.1, analyzing a training sample through root mean square error epsilon of prediction data, selecting 3 epsilon as a measurement abnormal threshold T of model prediction according to experience, and taking the value as a state discrimination standard of model prediction output;

step 5.2 modeling the predicted values according to the Bernoulli distribution:

step 5.3, compensating the DVL measured value according to the state discrimination standard:

wherein, gamma _k =0 means that DVL measurement is normal; gamma ray _k =1 represents abnormal DVL measurement, predicted by ANFISCompensating;

step 5.4 selecting the compensated DVL measurement Z _DVL，k And the difference of the corresponding pseudo observed information calculated by the SINS is used as an observed value, and the SINS/DVL tight combination navigation under the complex environment is realized by VBKF.