CN114459477A - Improved PSO-ANFIS-assisted SINS/DVL tightly combined navigation method - 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 an SINS/DVL tight combination navigation system in the underwater diving process; collecting sample data on the water surface by means of GNSS and a variational Bayesian Kalman filtering algorithm, acquiring innovation containing various abnormal measurement types, Mahalanobis distance and a measured noise covariance matrix 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 navigating underwater, online prediction is carried out on the four-beam absolute error of the DVL by adopting an ANFIS model obtained by training; furthermore, 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 and used for the measurement updating process of the integrated navigation system. The method can improve the positioning accuracy and robustness of the SINS/DVL tightly-combined navigation system in the complex underwater environment.

Description

Improved PSO-ANFIS-assisted SINS/DVL tightly combined navigation method

Technical Field

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

Background

More and more countries take the exploration development of the ocean as a strategic target, and the realization of the exploration breadth and depth is always a global common pursuit. An accurate and stable underwater navigation system is very important for people to explore the ocean. Commonly used underwater navigation systems and sensors mainly include SINS, DVL, underwater acoustic positioning technology, geophysical navigation system, depth gauge, magnetometer and the like. Among them, SINS is the most central navigation system in underwater navigation because it has autonomy and concealment, but its independent use is limited by the problem of accumulation of errors over time. Unlike the ground and the air, the Global Navigation Satellite System (GNSS) cannot be used underwater due to the serious attenuation of electromagnetic waves underwater. The geophysical navigation technology needs to establish a matching database of a task area in advance, and the underwater sound positioning also needs to arrange a sound head in advance. Therefore, the combined navigation of the SINS and the DVL is a mainstream navigation mode of An Underwater Vehicle (AUV) to realize long-endurance and high-precision Underwater navigation.

For an SINS/DVL integrated system, factors influencing navigation accuracy of the system are many, such as installation angle errors, lever arm errors and speed measurement errors. After the SINS and the DVL are fixed, the error of the installation angle and the lever arm is relatively stable, and the calibration can be carried out before sailing. The DVL is an active sonar, and needs to receive external reflected sound waves, and the received acoustic signals have a great relationship with the surrounding acoustic environment. Therefore, the actual speed measurement accuracy of the DVL is very affected by the complex environment, which is the most significant part of the combined navigation accuracy. When bubbles are generated around the AUV due to marine organism blockage or rapid acceleration during the navigation process, DVL measurement data can be misaligned, and outliers and large interference noise can be generated. 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 detection is a wild value or not is judged by setting a certain threshold value. The scholars propose a Huber-M estimation-based Kalman filter, solve a Huber kernel function through innovation, further solve a weight matrix, reconstruct observed quantities to perform filtering updating, but certain experience is needed for parameter selection of the kernel function. Kalman filters based on the maximum correlation entropy criterion are also similar principles. The students propose a Student's-t distribution-based Kalman filter, which considers that measurement outliers in a complex underwater environment can cause measurement noise to have thick tail characteristics, models the measurement noise into Student's-t distribution, and iteratively solves state estimation through variational Bayes learning. The learners solve the problem of unknown interference noise through the adaptive filter, and the model parameters are adjusted in the iterative process to obtain a model which is more suitable for the actual situation, including 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 methods have no universality. In fact, when a deep groove or seabed sludge is encountered during the AUV navigation, part of the DVL beams cannot obtain effective reflected sound waves, which may cause irregular data updating and even short-term failure of part of the DVL beams. These errors are relatively small at the initial stage of occurrence, which leads to a reduction in the accuracy of the combined navigation. The current research on these factors is still in the initial stage and there is no effective enough solution. And high-precision navigation is realized in a complex marine environment, and the errors of the types need to be compensated, which is also the task of the invention.

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

Disclosure of Invention

In order to solve the above problems, the present invention discloses a Strapdown Inertial Navigation System (SINS)/Doppler Velocity Log (DVL) tightly-combined Navigation method based on improved Particle Swarm Optimization (PSO) -Adaptive Neuro-Fuzzy Inference System (ANFIS) assistance; an SINS/DVL tightly-combined navigation system is taken as a research object, the improved PSO-ANFIS algorithm is adopted to solve the speed error of the four beams of the DVL, and the actual DVL measurement value is compensated. And then the compensated measurement data and corresponding data calculated by inertial navigation are subjected to integrated navigation, so that high-precision navigation information is obtained.

In order to achieve the purpose, the invention provides the following technical scheme:

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

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

step 2: controlling the depth of the underwater vehicle of the AUV to reach the position near the water surface, after the initial alignment of a Navigation positioning System is completed, assisting in collecting sample data containing various abnormal measurement types by means of Global Navigation Satellite System (GNSS) information and a Variational Bayesian Kalman Filter (VBKF) and training ANFIS;

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

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

and 5: based on the ANFIS prediction result, the characteristic change of the error is monitored, the actual measurement of the DVL is compensated through an anomaly discrimination mechanism, and the compensated DVL measurement and the pseudo observed quantity calculated by the SINS are subjected to Kalman filtering.

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

step 1.1 defines the coordinate system to be used:

e-terrestrial coordinate system: is fixedly connected with the earth, with the origin at the center of the earth, x_eAxis passing through the intersection of the meridian and equator, z_eAxis directed north, y_eAxis x_e、z_eForming a right-hand coordinate system;

n-a navigational coordinate system coinciding with the east-north-sky geographic coordinate system;

b-carrier coordinate system: the origin being at the center of the vehicle, z_bAxis perpendicular to carrier up, x_bDirected forward of the vehicle, y_bAnd x_b、z_bForming 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, establishing a state equation of the SINS/DVL tightly-combined navigation system, which comprises the following specific steps:

taking the attitude error angle phi as [ 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 state quantities of the SINS system, they are:

wherein phi is_xIs the east misalignment angle, phi_yIs the north misalignment angle, phi_zIs the angle of the vertical misalignment; delta V_EIs east velocity error, δ V_NIs the north velocity error, δ V_UIs the speed error in the sky direction; δ L is the latitude error, δ λ is the longitude error, δ h is the altitude error; epsilon_xIs the x-direction gyro drift, ε_yIs a y-direction gyro drift, epsilon_zIs a z-direction gyroscopeHelical drift;

is the random constant error of the x-direction accelerometer,

is a 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, ω is_gIs the process noise vector, omega, of the gyro_aIs the process noise vector of the accelerometer.

Taking DVL four-beam velocity zero offset delta b as [ delta b [ ]₁ δb₂ δb₃ δb₄]And taking the scale coefficient error delta k as a DVL system state variable, and recording as:

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

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

noise of DVL system is ω_d；

Offset deltab of depth gauge_psAs 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

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

where F is the state transition matrix and W is the system noise;

specific formula F_SINSDerivation is not discussed in detail, and derivation processes exist in many documents,

step 1.3, establishing a measurement equation of the SINS/DVL tightly-combined navigation system, and specifically comprising the following steps:

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

wherein,

is the speed of the SINS under n,

is the velocity of the SINS under b,

is the SINS speed under the beam system,

is the velocity of the DVL measurement. There is a relationship between them:

wherein,

represents a transformation matrix from n system to b system,

represents a transformation matrix from b system to beam system, which is expressed as follows:

wherein b is_iIs based on the geometric relationship between DVL beam and AUV from V^bTo V^dThe direction vector of (a) can be expressed as:

where α is the beam tilt angle of the DVL, a fixed characteristic of the DVL.

Can be expressed as

Wherein

For a "+" configuration of the DVL, and

DVL for "x" configuration;

measurements made by SINS, DVL and depth gauge:

wherein,

is the depth information calculated by the SINS,

is the depth information measured by the depth gauge. Defining a measurement error model of the depth gauge as follows:

wherein H_PSIs the true depth value. Solved by the above analysis, SINS

The calculation formula is as follows:

the measurement error model for DVL is defined as:

obtained by converting the SINS calculation speed into beam system according to the analysis

The calculation formula is as follows:

wherein [. x ] represents a cross product operation. The measurement equation of the integrated navigation system can be obtained as follows:

Z＝HX+V

wherein,

V＝[ω_d ω_ps]^T

further, in the step 2, sample data containing various abnormal measurement types is collected with the aid of GNSS information and VBKF algorithm for training ANFIS, and the specific process is as follows:

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

step 2.1, collecting variables most relevant to the abnormal DVL measurement information, and taking the variables as characteristic information, wherein the specific steps are as follows:

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

wherein,

the DVL actually measures information for time k.

Is a one-step predicted value at time k. We consider the system model to be accurately modeled, so when the innovation suddenly becomes large, the DVL measurement is not accurate enough and the measurement error increases. Therefore, innovation is taken as one of the variables reflecting the DVL measurement error as input to the ANFIS system;

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

wherein λ is_kIs the measured abnormal characteristic information defined according to the mahalanobis distance.

P_k/k-1Is a one-step prediction variance matrix. For convenience, λ will be used herein_kReferred to as mahalanobis distance;

when the DVL measurement information is abnormal, measurement noise may change. The specific method by which the measurement noise covariance matrix can be made to show this variation will be described in step 2.3. Measuring a noise covariance matrix

Is characteristic information that may represent a measurement anomaly, and therefore serves as a third input;

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

the learning process of the model requires accurate ideal output values. During the testing and verification process, the accurate measurement error is selected as the output of the training data in consideration of the sensors loaded on the AUV. This precise measurement error can be obtained from the AUV loaded PHINS:

in the formula,

the absolute error measured for the DVL at the k-th instant. The PHINS is a fiber optic inertial navigation sensor that integrates GPS information to provide the most accurate attitude, velocity and position information. Z_GPS,kAccurate pseudo-measurement information obtained at the kth moment according to the attitude and speed information provided by the PHINS, and

the solving mode is the same;

in summary, to make the training of the target model more accurate, we select three feature information that best react to the measured outliers. They are new messages v_kLambda constructed based on mahalanobis distance_kAnd obtained based on VBKF

In addition, the absolute error of the DVL measurement information is selected as output, so that the data quality of the DVL can be visually displayed, and the subsequent compensation and utilization are facilitated.

Step 2.3, a noise uncertainty processing method based on VBKF is developed, and the specific steps are as follows:

constant filter parameters cannot describe the statistical characteristics of the observed quantity variation. The classic kalman filter algorithm treats all observations as the same feature, and cannot adapt to changes in the system when the system observations are abnormal or noise changes. To accurately respond to anomalies that may occur, the system must respond specifically to unknown anomalies. Corresponding mahalanobis distance and R are obtained when the abnormal measurement information is considered_kAnd introducing VBKF to characterize the change of the VBKF. Unknown can be estimated based on VBKF

To improve the effectiveness of the ANFIS input information;

when the system noise is known and the measurement noise is unknown, the optimal bayesian filtering including 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-1dR_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)

due to the above-mentioned Bayesian filteringDifficult to solve, so a uniform density q (x) of a plurality of known distributions is used_k,R_k) To approximate the true posterior probability distribution function p (x)_k,R_k|z_1:k) I.e. variational bayes 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 obtained by minimizing the Probability Density Function (PDF) p (x)_k,R_k|z_1:k) And approximate posterior PDFq (x)_k)q(R_k) With a Kullback-Leibler divergence in between. q (x)_k)q(R_k) Updating to a Gaussian distribution and an inverse Wishart distribution:

the above equation requires fixed-point iterative solution, q⁽ⁱ⁺¹⁾(R_k) Can be updated as:

in the formula, the factor of freedom

And inverse scale matrix

Can be expressed as:

wherein,

can be expressed as:

expectation of

Expressed as:

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

wherein the mean value is obtainable by standard Kalman filtering

Sum covariance matrix

The VBKF is simply deduced, and more obvious characteristic information is obtained through the VBKF;

step 2.4 shows the application situation when part of DVL beam is missing, the specific steps are as follows:

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

at the moment, it needs to be satisfied that the AUV has no vertical speed, and when the AUV fluctuates with the ocean current in an up-and-down mode, the formula does not work. The present invention has the following constraints for the application when a portion of the DVL beam is missing:

1) three beams are active: at the moment, complete pseudo-beam information can still be obtained through the formula, and error prediction compensation can be performed through the method;

2) two orthogonal beams are active: at the moment, complete pseudo-beam information can still be obtained through the formula, and error prediction compensation can be performed through the method;

in addition, under three conditions of effectiveness of two parallel beams or effectiveness of only one beam and total failure, the DVL information has a lot of defects, and the failed DVL beam can be predicted by selecting different characteristic information (such as information related to SINS calculation), so that the method is not applicable;

further, the sample data collected in step 3 is standardized, and processed by PSO algorithm to realize ANFIS model parameter optimization, completing the training process of ANFIS model, and specifically comprising the following steps:

step 3.1 standardizing the sample data:

z-score normalization of the innovation;

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

step 3.2 the specific ANFIS algorithm process is as follows:

for a simple TSK fuzzy system model: the way 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 an adaptive neural network, and a membership function of the neural network, namely ANFIS, is obtained by training sample data. Given the pre-parameters, the output of ANFIS can be expressed as a linear combination of post-parameters;

the ANFIS has five layers in total,

step 3.2.1 first layer, input variable membership function layer:

each node i has an output function:

where in is an input, including x, y, z; m_iIs a fuzzy set comprising A_i,B_i,C_i；

Is a membership function of the fuzzy set M, which represents the degree to which a given input in satisfies M;

there are many types of membership functions including bell, gaussian, triangular, etc. In general, we choose μ_MAs generalized bell membership functions:

wherein, a_i,b_i,c_iIs a set of parameters whose changes in value change the shape of the bell-shaped function and thus result in different membership functions. The parameters are called as front-part parameters and can be adaptively adjusted in the learning process of the algorithm;

step 3.2.2 second layer, regular strength release layer:

each node i is responsible for multiplying the input signals:

wherein, ω is_iIs each timeThe output of each node represents the credibility of the rule;

step 3.2.3 layer three, normalization process of all regular intensities:

the ith node calculates the ratio of the release strength of rule i to the sum of all the release strengths of the rules:

step 3.2.4, fourth layer, calculating fuzzy rule output:

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_iComputing the output of the fuzzy rule by the membership function;

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

as previously mentioned, the output of ANFIS can be expressed as a linear combination of backward parameters, the forward parameters having been given:

the training process of the ANFIS firstly extracts an initial fuzzy model through the collected sample data, and then optimizes the model parameters from Layer 1 to Layer 5. 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 a PSO algorithm to optimize the front-part and 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, in the reverse transmission process, the error signal is propagated from the output layer back to the input layer, and the front-piece parameters are adjusted by the PSO. In the process of changing the parameters, the shape of the membership function is continuously modified so as to achieve the purpose of minimizing the output error in a set period.

Step 3.3 the specific PSO parameter optimization 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 individual cooperation and competition modes 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_iAnd a velocity vector v_iThe corresponding objective function allows the particle to obtain fitness and from the previous position and the current position (x)_i) Selecting the best position (p)_best) (ii) a In addition, in a cluster, all particles have their global optimal position (g)_best)；

The updating mode of the velocity vector and the position vector of the ith particle is expressed as follows:

in the above-mentioned formula,

representing the velocity vector of the particle i at the kth time in d dimension;

indicating that the particle i is in d dimensionA position vector at time k; omega represents an inertia weight; r is₁And r₂Represents a random number from 0 to 1; c. C₁And c₂As acceleration factor, i.e. cognition factor (c)₁) And social coefficient (c)₂)；

The velocity vector equation is mainly composed of 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 through 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 an assessment of the performance of a particle relative to its neighbors and the entire 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 which is acquired in the step 2 and comprises input data and target output to obtain a trained ANFIS model;

further, the step 4 of controlling the AUV to submerge underwater and predicting the four-beam absolute error of the DVL by adopting an improved PSO-ANFIS algorithm comprises the following specific steps:

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

step 4.2, calculating pseudo measurement information corresponding to the four-beam velocity of the DVL through inertial navigation resolving information according to the DVL updating frequency;

step 4.3, filtering the difference value of the solved pseudo measurement information and the DVL four-beam measurement value as an observed quantity through VBKF so as to obtain three kinds of characteristic information v consistent with the step 2_k,λ_k,

As input to the ANFIS model;

step 4.4, performing a standardization process consistent with the step 3 on the collected characteristic information, and performing DVL measurement information error prediction through the ANFIS model subjected to PSO assisted 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 an anomaly discrimination mechanism, and the compensated DVL measurement and the pseudo-observed quantity calculated by the SINS are subjected to kalman filtering, which specifically includes the following steps:

step 5.1, analyzing a training sample by belonging to Root-Mean-Square Error (RMSE) of prediction data, selecting 3 belonging to the group as a measurement abnormal threshold T of model prediction according to experience, and using the value as a state discrimination standard of model prediction output;

step 5.2, modeling the predicted value according to Bernoulli distribution:

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

wherein, γ_k0 indicates DVL measurement is normal; gamma ray_k1 indicates abnormal DVL measurement, predicted by ANFIS

Performing compensation;

step 5.4 selecting compensated DVL measurement value Z_DVL,kAnd the difference between the corresponding pseudo-observation information calculated by the SINS is used as an observation value, and the SINS/DVL tight combination navigation under the complex environment is realized by the VBKF.

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

1. and (3) acquiring high-quality ANFIS input information by using a variational Bayes-based adaptive filtering method. The innovation, the mahalanobis distance and the measurement noise covariance matrix are originally related to the measured value, the mahalanobis distance and the measurement noise covariance matrix in the ANFIS input information can be more remarkable when encountering abnormal values by the self-adaptive filtering method, and the training quality of the ANFIS is improved.

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

3. Instead of rejecting error points, a hierarchical approach to ANFIS prediction data is selected. Therefore, when the DVL measurement value encounters continuous errors such as irregular measurement information updating, short-time failure of partial beams and the like, the information of the DVL can be utilized to the maximum extent, and the reduction of navigation accuracy caused by continuous data point elimination is avoided.

Drawings

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

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

FIG. 3 is a technical route of the SINS/DVL integrated navigation system under a complex environment provided by the present invention.

Detailed Description

The present invention will be further illustrated with reference to the accompanying drawings and specific embodiments, which are to be understood as merely illustrative of the invention and not as limiting the scope of the invention. It should be noted that the terms "front," "back," "left," "right," "upper" and "lower" used in the following description refer to directions in the drawings, and the terms "inner" and "outer" refer to directions toward and away from, respectively, the geometric center of a particular component.

The invention relates to an application method of an improved PSO-ANFIS assisted SINS/DVL tightly combined navigation method in AUV navigation, the flow of which is shown in figure 3, and the application method comprises the following steps:

step 1: establishing a state equation of the SINS/DVL integrated navigation system according to a system error equation, and establishing a measurement equation of the SINS/DVL integrated navigation system by taking the difference between pseudo measurement information calculated according to navigation information solved by the SINS and four-beam velocity information measured by the DVL and depth information measured by a depth meter as measurement quantity, as shown in FIG. 1;

step 1.1 defines the coordinate system to be used:

e-terrestrial coordinate system: is fixedly connected with the earthThe point is located at the geocentric, x_eAxis passing through the intersection of the meridian and equator, z_eAxis directed north, y_eAxis x_e、z_eForming a right-hand coordinate system;

n-a navigational coordinate system coinciding with the east-north-sky geographic coordinate system;

b-carrier coordinate system: the origin being at the center of the vehicle, z_bAxis perpendicular to carrier up, x_bDirected forward of the vehicle, y_bAnd x_b、z_bForming 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, establishing a state equation of the SINS/DVL tightly-combined navigation system, which comprises the following specific steps:

taking the attitude error angle phi as [ 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 state quantities of the SINS system, they are:

wherein phi_xIs the east misalignment angle, phi_yIs the north misalignment angle, phi_zIs the angle of the vertical misalignment; delta V_EIs east velocity error, δ V_NIs the north velocity error, δ V_UIs the speed error in the sky direction; δ L is the latitude error, δ λ is the longitude error, δ h is the altitude error; epsilon_xIs the x-direction gyro drift, ε_yIs a y-direction gyro drift, epsilon_zIs z-direction gyro drift;

is the random constant error of the x-direction accelerometer,

is a 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, ω is_gIs the process noise vector, omega, of the gyro_aIs the process noise vector of the accelerometer.

Taking DVL four-beam velocity zero offset delta b as [ delta b [ ]₁ δb₂ δb₃ δb₄]And taking the scale coefficient error delta k as a DVL system state variable, and recording as:

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

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

noise of DVL system is ω_d；

Offset deltab of depth gauge_psAs 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

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

wherein, FIs the state transition matrix, W is the system noise;

specific formula F_SINSDerivation is not discussed in detail, and derivation processes exist in many documents,

step 1.3, establishing a measurement equation of the SINS/DVL tightly-combined navigation system, and specifically comprising the following steps:

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

wherein,

is the speed of the SINS under n,

is the velocity of the SINS under b,

is the SINS speed under the beam system,

is the velocity of the DVL measurement. There is a relationship between them:

wherein,

represents a transformation matrix from n system to b system,

represents a transformation matrix from b system to beam system, which is expressed as follows:

wherein b is_iIs based on the geometric relationship between DVL beam and AUV from V^bTo V^dThe direction vector of (a) can be expressed as:

where α is the beam tilt angle of the DVL, a fixed characteristic of the DVL.

Can be expressed as

Wherein

For a "+" configuration of the DVL, and

DVL for "x" configuration;

measurements made by SINS, DVL and depth gauge:

wherein,

is the depth information calculated by the SINS,

is the depth information measured by the depth gauge. Defining a measurement error model of the depth gauge as follows:

wherein H_PSIs the true depth value. Solved by the above analysis, SINS

The calculation formula is as follows:

the measurement error model for DVL is defined as:

obtained by converting the SINS calculation speed into beam system according to the analysis

The calculation formula is as follows:

wherein [. x ] represents a cross product operation. The measurement equation of the integrated navigation system can be obtained as follows:

Z＝HX+V

wherein,

V＝[ω_d ω_ps]^T

step 2: controlling the depth of the underwater vehicle of the AUV to reach the position near the water surface, and after the initial alignment of a 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 ANFIS;

step 2.1, collecting variables most relevant to the abnormal DVL measurement information, and taking the variables as characteristic information, wherein the specific steps are as follows:

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

wherein,

the DVL actually measures information for time k.

Is a one-step predicted value at time k. We consider the system model to be accurately modeled, so when the innovation suddenly becomes large, the DVL measurement is not accurate enough and the measurement error increases. Therefore, innovation is taken as one of the variables reflecting the DVL measurement error as input to the ANFIS system;

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

wherein λ is_kIs the measured abnormal characteristic information defined according to the mahalanobis distance.

P_k/k-1Is a one-step prediction variance matrix. For convenience, λ will be used herein_kReferred to as mahalanobis distance;

when DVL measurement informationIn the event of an anomaly, the measurement noise will change. The specific method by which the measurement noise covariance matrix can be made to show this variation will be described in step 2.3. Measuring a noise covariance matrix

Is characteristic information that may represent a measurement anomaly, and therefore serves as a third input;

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

the learning process of the model requires accurate ideal output values. During the testing and verification process, the accurate measurement error is selected as the output of the training data in consideration of the sensors loaded on the AUV. This precise measurement error can be obtained from the AUV loaded PHINS:

in the formula,

the absolute error measured for the DVL at the k-th instant. The PHINS is a fiber optic inertial navigation sensor that integrates GPS information to provide the most accurate attitude, velocity and position information. Z_GPS,kAccurate pseudo-measurement information obtained at the kth moment according to the attitude and speed information provided by the PHINS, and

the solving mode is the same;

in summary, to make the training of the target model more accurate, we select three feature information that best react to the measured outliers. They are new messages v_kLambda constructed based on mahalanobis distance_kAnd obtained based on VBKF

In addition, the absolute error of the DVL measurement information is selected as the output, so that the DVL number can be displayed intuitivelyAnd according to the quality, the subsequent compensation and utilization are convenient.

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

constant filter parameters cannot describe the statistical characteristics of the observed quantity variation. The classic kalman filter algorithm treats all observations as the same feature, and cannot adapt to changes in the system when the system observations are abnormal or noise changes. To accurately respond to anomalies that may occur, the system must respond specifically to unknown anomalies. Corresponding mahalanobis distance and R are obtained when the abnormal measurement information is considered_kVBKF is introduced to characterize its variations. Unknown can be estimated based on VBKF

To improve the effectiveness of the ANFIS input information;

when the system noise is known and the measurement noise is unknown, the optimal bayesian filtering including 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-1dR_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 described above is difficult to solve, a uniform density q (x) of multiple known distributions is employed_k,R_k) To approximate the true posterior probability distribution function p (x)_k,R_k|z_1:k) I.e. variational bayes 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 obtained by minimizing the Probability Density Function (PDF) p (x)_k,R_k|z_1:k) And approximate posterior PDFq (x)_k)q(R_k) With a Kullback-Leibler divergence in between. q (x)_k)q(R_k) Updating to a Gaussian distribution and an inverse Wishart distribution:

the above equation requires fixed-point iterative solution, q⁽ⁱ⁺¹⁾(R_k) Can be updated as:

in the formula, the factor of freedom

And inverse scale matrix

Can be expressed as:

wherein,

can be expressed as:

desire to

Expressed as:

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

wherein the mean value is obtainable by standard Kalman filtering

Sum covariance matrix

The VBKF is simply deduced, and more obvious characteristic information is obtained through the VBKF;

step 2.4 shows the application situation when part of DVL beam is missing, the specific steps are as follows:

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

at the moment, it needs to be satisfied that the AUV has no vertical speed, and when the AUV fluctuates with the ocean current in an up-and-down mode, the formula does not work. The present invention has the following constraints for the application when a portion of the DVL beam is missing:

1) three beams are active: at the moment, complete pseudo-beam information can still be obtained through the formula, and error prediction compensation can be performed through the method;

2) two orthogonal beams are active: at the moment, complete pseudo-beam information can still be obtained through the formula, and error prediction compensation can be performed through the method;

in addition, under three conditions of effectiveness of two parallel beams or effectiveness of only one beam and total failure, the DVL information has a lot of defects, and the failed DVL beam can be predicted by selecting different characteristic information (such as information related to SINS calculation), so that the method is not applicable;

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

step 3.1 standardizing the sample data:

z-score normalization of the innovation;

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

step 3.2 the specific ANFIS algorithm process is as follows:

for a simple TSK fuzzy system model: the way 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 an adaptive neural network, and a membership function of the neural network, namely ANFIS, is obtained by training sample data. Given the pre-parameters, the output of ANFIS can be expressed as a linear combination of post-parameters;

ANFIS has five layers;

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

each node i has an output function:

where in is an input, including x, y, z; m_iIs a fuzzy set comprising A_i,B_i,C_i；

Is a membership function of the fuzzy set M, which represents the degree to which a given input in satisfies M;

there are many types of membership functions including bell, gaussian, triangular, etc. In general, we choose μ_MAs generalized bell membership functions:

wherein, a_i,b_i,c_iIs a set of parameters whose changes in value change the shape of the bell-shaped function and thus result in different membership functions. The parameters are called as front-part parameters and can be adaptively adjusted in the learning process of the algorithm;

step 3.2.2 second layer, regular strength release layer:

each node i is responsible for multiplying the input signals:

wherein, ω is_iIs the output of each node, representing the trustworthiness of the rule;

step 3.2.3 layer three, normalization process of all regular intensities:

the ith node calculates the ratio of the release strength of rule i to the sum of all the release strengths of the rules:

step 3.2.4, fourth layer, calculating fuzzy rule output:

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_iComputing the output of the fuzzy rule by the membership function;

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

as previously mentioned, the output of ANFIS can be expressed as a linear combination of backward parameters, the forward parameters having been given:

the training process of the ANFIS firstly extracts an initial fuzzy model through the collected sample data, and then optimizes the model parameters from Layer 1 to Layer 5. 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 a PSO algorithm to optimize the front-part and 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, in the reverse transmission process, the error signal is propagated from the output layer back to the input layer, and the front-piece parameters are adjusted by the PSO. In the process of changing the parameters, the shape of the membership function is continuously modified so as to achieve the purpose of minimum output error in a set period;

step 3.3 the specific PSO parameter optimization 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 individual cooperation and competition modes 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_iAnd a velocity vector v_iThe corresponding objective function allows the particle to obtain fitness and from the previous position and the current position (x)_i) Selecting the best position (p)_best) (ii) a In addition, in a cluster, all particles have their global optimal position (g)_best)；

The updating mode of the velocity vector and the position vector of the ith particle is expressed as follows:

in the above-mentioned formula,

representing the velocity vector of the particle i at the kth time in d dimension;

a position vector representing a particle i at a kth time in d-dimension; omega represents an inertia weight; r is₁And r₂Represents a random number from 0 to 1; c. C₁And c₂As acceleration factor, i.e. cognition factor (c)₁) And social coefficient (c)₂)；

The velocity vector equation is mainly composed of 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 through 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 an assessment of the performance of a particle relative to its neighbors and the entire 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 which is acquired in the step 2 and comprises input data and target output to obtain a trained ANFIS model;

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

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

step 4.2, calculating pseudo measurement information corresponding to the four-beam velocity of the DVL through inertial navigation resolving information according to the DVL updating frequency;

step 4.3, filtering the difference value of the solved pseudo measurement information and the DVL four-beam measurement value as an observed quantity through VBKF so as to obtain three kinds of characteristic information v consistent with the step 2_k,λ_k,

As input to the ANFIS model;

step 4.4, performing a standardization process consistent with the step 3 on the collected characteristic information, and performing DVL measurement information error prediction through the ANFIS model subjected to PSO assisted training in the step 3;

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

step 5.1, analyzing a training sample by belonging to Root-Mean-Square Error (RMSE) of prediction data, selecting 3 belonging to the group as a measurement abnormal threshold T of model prediction according to experience, and using the value as a state discrimination standard of model prediction output;

step 5.2, modeling the predicted value according to Bernoulli distribution:

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

wherein, γ_k0 indicates DVL measurement is normal; gamma ray_k1 indicates abnormal DVL measurement, predicted by ANFIS

Compensation is carried out;

step 5.4 selecting compensated DVL measurement value Z_DVL,kAnd the difference between the corresponding pseudo-observation information calculated by the SINS is used as an observation value, and the SINS/DVL tight combination navigation under the complex environment is realized by the VBKF.

The technical means disclosed in the invention scheme are not limited to the technical means disclosed in the above embodiments, but also include the technical scheme formed by any combination of the above technical features.

Claims (6)

1. An improved PSO-ANFIS assisted SINS/DVL tightly combined 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 a system error equation, and establishing a measurement equation of the SINS/DVL integrated navigation system by taking the difference between pseudo measurement information calculated according to navigation information solved by the SINS and four-beam velocity information measured by the DVL and depth information measured by the depth meter as measurement;

step 2: controlling the depth of the underwater vehicle of the AUV to reach the position near the water surface, and after the initial alignment of a 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 ANFIS;

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

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

and 5: based on the ANFIS prediction result, the characteristic change of the error is monitored, the actual measurement of the DVL is compensated through an anomaly discrimination mechanism, and the compensated DVL measurement and the pseudo observed quantity calculated by the SINS are subjected to Kalman filtering.

2. The improved PSO-ANFIS-assisted SINS/DVL tightly-integrated navigation method according to claim 1, wherein the state equation and the measurement equation of the SINS/DVL tightly-integrated navigation system are established in step 1 by the following steps:

step 1.1 defines the coordinate system to be used:

e-terrestrial coordinate system: is fixedly connected with the earth, with the origin at the center of the earth, x_eAxis passing through the intersection of the meridian and equator, z_eAxis directed north, y_eAxis x_e、z_eForming a right-hand coordinate system; n-a navigational coordinate system coinciding with the east-north-sky geographic coordinate system; b-carrier coordinate system: the origin being at the center of the vehicle, z_bAxis perpendicular to carrier up, x_bDirected forward of the vehicle, y_bAnd x_b、z_bForming 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, establishing a state equation of the SINS/DVL tightly-combined navigation system, which comprises the following specific steps:

taking the attitude error angle phi as [ 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 state quantities of the SINS system, they are:

wherein phi is_xIs the east misalignment angle phi_yIs the north misalignment angle, phi_zIs the angle of the vertical misalignment; delta V_EIs east velocity error, δ V_NIs the north velocity error, δ V_UIs the speed error in the sky direction; δ L is the latitude error, δ λ is the longitude error, δ h is the altitude error; epsilon_xIs the x-direction gyro drift, ε_yIs y-direction gyro drift, ε_zIs z-direction gyro drift;

is the random constant error of the x-direction accelerometer,

is a 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, ω is_gIs the process noise vector, omega, of the gyro_aIs the process noise vector of the accelerometer; taking DVL four-beam velocity zero offset delta b as [ delta b [ ]₁ δb₂ δb₃ δb₄]And taking the scale coefficient error delta k as a DVL system state variable, and recording as:

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

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

noise of DVL system is ω_d(ii) a Offset deltab of depth gauge_psAs 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:

where F is the state transition matrix and W is the system noise;

specific formula F_SINSDerivation is not discussed in detail, and derivation processes exist in many documents,

step 1.3, establishing a measurement equation of the SINS/DVL tightly-combined navigation system, and specifically comprising the following steps:

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

wherein,

is the speed of the SINS under n,

is the velocity of the SINS under b,

is the SINS speed under the beam system,

is the velocity of the DVL measurement; there is a relationship between them:

wherein,

represents a transformation matrix from n system to b system,

represents a transformation matrix from b system to beam system, which is expressed as follows:

wherein b is_iIs based on the geometric relationship between DVL beam and AUV from V^bTo V^dThe direction vector of (c) can be expressed as:

where α is the beam tilt angle of the DVL, a fixed characteristic of the DVL;

can be expressed as

Wherein

For "+" fittingDVL placed in, and

DVL for "x" configuration;

measurements made by SINS, DVL and depth gauge:

wherein,

is the depth information calculated by the SINS,

is depth information measured by a depth gauge; defining a measurement error model of the depth gauge as follows:

wherein H_PSIs a true depth value; solved by the above analysis, SINS

The calculation formula is as follows:

the measurement error model for DVL is defined as:

obtained by converting the SINS calculation speed into beam system according to the analysis

The calculation formula is as follows:

wherein [. x ] represents a cross product operation; the measurement equation of the integrated navigation system can be obtained as follows:

Z＝HX+V

wherein,

V＝[ω_d ω_ps]^T。

3. the improved PSO-ANFIS-assisted-based SINS/DVL tightly-combined navigation method according to claim 1, wherein the step 2 of collecting sample data containing various abnormal measurement types with the aid of GNSS information and VBKF algorithm for training ANFIS comprises the following steps:

step 2.1, collecting variables most relevant to the abnormal DVL measurement information, and taking the variables as characteristic information, wherein the specific steps are as follows:

innovation v in Kalman filtering in SINS/DVL integrated navigation systems_kThe difference between the estimated value of the system model and the actual measured value of DVL can be mapped; the calculation expression is as follows:

wherein,

actual measurement information for the DVL at time k;

is a one-step predicted value at time k; we consider the system model to be accurately modeled, so when the innovation suddenly becomes large, the DVL measurement is not accurate enough and the measurement error increases; therefore, 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 mahalanobis distance:

wherein λ is_kThe measured abnormal characteristic information is defined according to the Mahalanobis distance;

P_k/k-1is a one-step prediction variance matrix; for convenience, λ will be used herein_kReferred to as mahalanobis distance;

when the DVL measurement information is abnormal, the measurement noise changes; a specific method that can make the measurement noise covariance matrix show such variations; measuring a noise covariance matrix

Is characteristic information that may represent a measurement anomaly, and therefore serves as a third input;

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

the learning process of the model requires accurate ideal output values; in the testing and verifying process, considering a sensor loaded on the AUV, and selecting an accurate measurement error as the output of training data; this precise measurement error can be obtained from the AUV loaded PHINS:

in the formula,

absolute error measured for the DVL at time k; the PHINS is an optical fiber inertial navigation sensor, can integrate GPS information and provides the most accurate attitude, speed and position information; z_GPS，kAccurate pseudo-measurement information obtained at the kth moment according to the attitude and speed information provided by the PHINS, and

the solving mode is the same;

step 2.3, a noise uncertainty processing method based on VBKF is developed, and the specific steps are as follows:

in order to accurately respond to anomalies that may occur, the system must respond specifically to unknown anomalies; corresponding mahalanobis distance and R are obtained when the abnormal measurement information is considered_kIntroducing VBKF to characterize the change; unknown can be estimated based on VBKF

To improve the effectiveness of the ANFIS input information;

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

since the Bayesian filtering described above is difficult to solve, a uniform density q (x) of multiple known distributions is employed_k，R_k) To approximate the true posterior probability distribution function p (x)_k，R_k|z_1：k) I.e. variational bayes 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 can be obtained by minimizing the 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 the two parts; q (x)_k)q(R_k) Updating to a Gaussian distribution and an inverse Wishart distribution:

the above equation requires fixed point iterative solution, q⁽ⁱ⁺¹⁾(R_k) Can be updated as:

in the formula, the factor of freedom

And inverse scale matrix

Can be expressed as:

wherein,

can be expressed as:

expectation of

Expressed as:

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

wherein the mean value is obtainable by standard Kalman filtering

Sum covariance matrix

The VBKF is simply deduced, and more obvious characteristic information is obtained through the VBKF;

step 2.4 shows the application situation when part of DVL beam is missing, the specific steps are as follows:

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

at the moment, it is required to meet the condition that the AUV has no vertical speed, and when the fluctuation amplitude of the AUV along with the ocean current is too large, the formula does not hold; the present invention has the following constraints for the application when a portion of the DVL beam is missing:

1) three beams are active: at the moment, complete pseudo-beam information can still be obtained through the formula, and error prediction compensation can be performed through the method;

2) two orthogonal beams are active: at the moment, complete pseudo-beam information can still be obtained through the formula, and error prediction compensation can be performed through the method;

in addition, under three conditions of effectiveness of two parallel beams or effectiveness of only one beam and total failure, DVL information has a lot of defects, and different characteristic information can be selected at the moment; predicting failed DVL beams, the present invention is not applicable.

4. The improved PSO-ANFIS-assisted SINS/DVL tightly-combined navigation method according to claim 1, wherein in step 3, the sample data collected in step 2 is normalized, and the particle swarm optimization algorithm is used to process the sample data to realize parameter optimization of ANFIS model, thereby completing the training process of ANFIS model, specifically comprising the following steps:

step 3.1 standardizing the sample data: z-score normalization of the innovation; performing min-max standardization on the Mahalanobis distance and the measured noise covariance matrix to enable the result to fall into a [0,1] interval;

step 3.2 the specific ANFIS algorithm process is as follows:

for a simple TSK fuzzy system model: the mode of function combination or interaction is called a rule, and the rule comprises 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 an adaptive neural network, and a membership function of the neural network, namely ANFIS, is obtained by training sample data; given the pre-parameters, the output of ANFIS can be expressed as a linear combination of post-parameters;

the ANFIS has five layers, which are as follows;

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

each node i has an output function:

where in is an input, including x, y, z; m_iIs a fuzzy set comprising A_i，B_i，C_i；

Is a membership function of the fuzzy set M, which represents the degree to which a given input in satisfies M;

there are many common membership functions including bell shape, gaussian shape, and triangle; in general, we choose μ_MAs generalized bell membership functions:

wherein, a_i，b_i，c_iIs a parameter set, and the change of the parameter values can change the shape of a bell-shaped function, thereby obtaining different membership functions; the parameters are called as front-part parameters and can be adaptively adjusted in the learning process of the algorithm;

step 3.2.2 second layer, regular strength release layer:

each node i is responsible for multiplying the input signals:

wherein, ω is_iIs the output of each node, representing the trustworthiness of the rule;

step 3.2.3 layer three, normalization process of all regular intensities:

the ith node calculates the ratio of the release strength of rule i to the sum of all the release strengths of the rules:

step 3.2.4, fourth layer, calculating fuzzy rule output:

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_iComputing the output of the fuzzy rule by the membership function;

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

as previously mentioned, the output of ANFIS can be expressed as a linear combination of backward parameters, the forward parameters having been given:

in the training process of the ANFIS, firstly, an initial fuzzy model is extracted through collected sample data, and then model parameters are optimized from Layer 1 to Layer 5; 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 back-part parameters of ANFIS model; from the first layer to the fourth layer, the back-piece parameters are calculated by least square estimation, the error between an iteration value and an expected value of training data is calculated, in the reverse transmission process, an error signal is transmitted back to the input layer from the output layer, and the front-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 purpose of minimum output error in a set period;

step 3.3 the specific PSO parameter optimization 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 individual cooperation and competition modes 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_iAnd a velocity vector v_iThe corresponding objective function allows the particle to obtain fitness and from the previous position and the current position (x)_i) Selecting the best position (p)_best) (ii) a In addition, in a cluster, all particles have their global optimal position (g)_best)；

The updating mode of the velocity vector and the position vector of the ith particle is expressed as follows:

in the above-mentioned formula,

representing the velocity vector of particle i at the kth time in d-dimension；

A position vector representing a particle i at a kth time in d-dimension; omega represents an inertia weight; r is₁And r₂Represents a random number from 0 to 1; c. C₁And c₂As acceleration factor, i.e. cognition factor (c)₁) And social coefficient (c)₂) (ii) a 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 through 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 an assessment of the performance of the particle relative to its neighbors and the entire population of particles; these three components define the trajectory of the particle throughout the search space;

and (3) optimizing the parameters of the ANFIS model by adopting a PSO algorithm according to the training data which is acquired in the step (2) and comprises the input data and the target output to obtain the trained ANFIS model.

5. The improved PSO-ANFIS-assisted SINS/DVL tight-coupled navigation method according to claim 1, wherein the step 4 of controlling the AUV to submerge comprises the following steps of predicting the absolute error of four beams of DVL by using the improved PSO-ANFIS algorithm:

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

step 4.2, calculating pseudo measurement information corresponding to the four-beam velocity of the DVL through inertial navigation resolving information according to the DVL updating frequency;

step 4.3, filtering the difference value of the solved pseudo measurement information and the DVL four-beam measurement value as an observed quantity through VBKF so as to obtain three kinds of characteristic information v consistent with the step 2_k，λ_k，

As input to the ANFIS model;

and 4.4, performing a standardization process consistent with the step 3 on the collected characteristic information, and performing error prediction on the DVL measurement information through the ANFIS model trained by the PSO in the step 3.

6. The improved PSO-ANFIS-assisted SINS/DVL tightly-integrated navigation method according to claim 1, wherein the step 5 of monitoring the characteristic variation of the error based on the ANFIS prediction result, compensating the actual measurement of DVL by the anomaly determination mechanism, and performing kalman filtering on the compensated DVL measurement and the pseudo-observation amount calculated by the SINS comprises the following steps:

step 5.1, analyzing a training sample by belonging to Root-Mean-Square Error (RMSE) of prediction data, selecting 3 belonging to the group as a measurement abnormal threshold T of model prediction according to experience, and using the value as a state discrimination standard of model prediction output;

step 5.2, modeling the predicted value according to Bernoulli distribution:

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

wherein, γ_k0 indicates DVL measurement is normal; gamma ray_k1 indicates abnormal DVL measurement, predicted by ANFIS

Compensation is carried out;

step 5.4 selecting the compensated DVL measurement value Z_DVL，kAnd the difference between the corresponding pseudo-observation information calculated by the SINS is used as an observation value, and the SINS/DVL tight combination navigation under the complex environment is realized by the VBKF.

Images