阅读说明：本技术 一种基于统计相似度量的组合导航鲁棒滤波方法 (Combined navigation robust filtering method based on statistical similarity measurement ) 是由 徐博 郭瑜 赵玉新 吴磊 胡俊苗 陈崇 王连钊 李盛新 王潇雨 王朝阳 于 2021-09-23 设计创作，主要内容包括：本发明属于非理想条件下的组合导航技术领域,具体涉及一种基于统计相似度量的组合导航鲁棒滤波方法。本发明考虑SINS/DVL紧组合系统中正常的DVL波束测量信息和大误差的波束测量信息同时出现的情形,针对目前现有的组合导航系统鲁棒滤波器对量测信息处理粗糙,导致正常量测信息易丢失的问题,基于多维量测方程分解和统计相似度量提出了新的鲁棒滤波方法。本发明在将SINS/DVL紧组合导航系统的多维量测方程进行分解的同时,引入统计相似度量(SSM)理论,协助每个波束的测量噪声方差在大测量误差出现时完成各自的自适应更新,最终保证每个波束测量信息处理的独立性。本发明可用于非理想条件下的水下航行器组合导航领域。(The invention belongs to the technical field of integrated navigation under non-ideal conditions, and particularly relates to an integrated navigation robust filtering method based on statistical similarity measurement. The invention provides a novel robust filtering method based on multi-dimensional measurement equation decomposition and statistical similarity measurement, which considers the situation that normal DVL beam measurement information and large-error beam measurement information in an SINS/DVL tight combination system occur simultaneously and aims at solving the problem that the conventional robust filter of the combined navigation system processes the measurement information coarsely to cause easy loss of the normal measurement information. The invention decomposes the multidimensional measurement equation of the SINS/DVL tight combination navigation system, and introduces a Statistic Similarity Measurement (SSM) theory to assist the measurement noise variance of each beam to complete respective self-adaptive updating when a large measurement error occurs, thereby finally ensuring the independence of the measurement information processing of each beam. The invention can be used in the field of underwater vehicle integrated navigation under non-ideal conditions.)

1. A combined navigation robust filtering method based on statistical similarity measurement is characterized by comprising the following steps:

step 1: defining a coordinate system;

defining a carrier coordinate system to be represented by b, a geocentric coordinate system to be represented by e, a geographic coordinate system of 'east-north-sky' as a navigation system to be represented by n, an inertial coordinate system to be represented by i, and a DVL frame coordinate system to be represented by d;

defining velocities in different coordinate systems:

wherein the content of the first and second substances,representing the velocity vector of the strapdown inertial navigation system in a b system;representing the velocity vector of the strapdown inertial navigation system in the n systems;representing a velocity vector of the strapdown inertial navigation system in a system d;representing a velocity vector of the DVL beam in the d-system;

step 2: establishing an SINS/DVL tightly-combined navigation system state equation;

selecting SINS error as state including misalignment angle phi and speed error delta VⁿPosition error delta p and gyro constant drift epsilon^bAnd accelerometer constant zero offsetThe equation of state is as follows:

wherein epsilon^bRepresenting a gyro constant drift vector;is a gyro random drift vector;is the accelerometer constant zero offset vector;is the accelerometer random zero offset vector; phi denotes a misalignment angle vector consisting of pitch, roll and yaw misalignment angles;is a directional cosine matrix representing the transformation from the b-system to the n-system;represents the angular velocity vector of b relative to i under b, and the corresponding calculation error isIs the vector of the velocity of the carrier in the n series with respect to the ground, the corresponding error vector is δ vⁿ；f^bIs the output of the accelerometer; δ gⁿIs the projection of the gravity error vector in the n system; δ p represents a position error vector composed of a latitude error δ L, a longitude error δ λ, and an altitude error δ h; r_MAnd R_NRespectively representing the curvature radius of the meridian circle and the initial vertical circle;

and step 3: establishing a SINS/DVL tight combination navigation system measurement equation and decomposing;

the measurement equation is as follows:

Z＝HX+v

wherein the content of the first and second substances,representing measurement noise; i is_3×3Is a three-dimensional identity matrix; [. radix Et rhizoma Rhei]A cross product operation representing a vector;α is the horizontal angle between the beam and the carrier;set at 0 ° or 45 °;

the measurement noise of each beam of the DVL is usually uncorrelated, and its measurement noise covariance matrix R ═ E [ vv ═^T]Is a diagonal matrix, so the measurement equation is equivalently decomposed:

wherein H^jRepresents the jth row of the measurement matrix; v. of^jThe measurement noise is corresponding measurement noise, the measurement noise of different DVL beams is uncorrelated, and here, the four beam measurement values of the DVL are assumed to be obtained by respectively measuring by different sensors;

and 4, step 4: constructing a cost function of state estimation based on the statistical similarity measurement;

wherein the content of the first and second substances,andrespectively representing a one-step prediction result and a one-step prediction covariance matrix of the state; n (X; X)_k+1,P_k+1) Representing a Gaussian distribution with respect to X, the mean being X_k+1Variance is P_k+1(ii) a Similarity function f_x(t) is selected as f_x(t) — 0.5 t; similarity function f_z(t) is selected asω is a degree of freedom parameter, m is a measurement dimension, and m is 1; q is 4;

and 5: solving the optimal value of the cost function by applying Gaussian-Newton iteration;

step 5.1: setting the initial iteration number as 0

Step 5.2: calculating filter gain

Wherein the content of the first and second substances, andauxiliary parameters for adjusting the one-step prediction covariance matrix and the measurement noise variance;

step 5.3: calculating posterior states

Step 5.4: computing estimation error covariance matrix

Step 5.5: judging whether the requirements are metIf not, making t equal to t +1, and returning to the step 5.2; otherwise, it ordersOutputting an error X of the SINS;

step 6: feeding back the estimated SINS error X to the SINS for correction;

obtaining a misalignment angle phi and a speed error delta V according to the estimated error X of the SINSⁿPosition error δ p; attitude matrix for SINS solutionSpeed of rotationPosition ofThe feedback correction method of the attitude error comprises the following steps:

speed error delta VⁿAnd the position error δ p is directly subtracted from the output of the SINS:

Technical Field

The invention belongs to the technical field of integrated navigation under non-ideal conditions, and particularly relates to an integrated navigation robust filtering method based on statistical similarity measurement.

Background

A strap-down inertial navigation/Doppler velocity measurement log (SINS/DVL) integrated navigation system is one of the most common integrated navigation methods for underwater vehicles at present. The tight combination mode can utilize original beam measurement information of the DVL, has good fault tolerance and is gradually and widely used. DVL measures the velocity of a carrier along the direction of the acoustic beam by calculating the difference between the frequency of the acoustic wave transmitted towards the water bottom and the frequency of the reflected acoustic wave received, using the doppler shift principle. However, in some complicated cases, the DVL may have large measurement errors, for example, the sound wave beam emitted by the DVL is blocked by marine organisms and cannot reach the seabed, or when the underwater vehicle travels through a sea ditch, the distance between the vehicle and the seabed exceeds the range of the sound wave beam of the DVL, or strong wave absorbing materials (such as silt and the like) exist in the seabed, and the emitted sound wave beam cannot be reflected back. Theoretically, when a large measurement error occurs in a certain beam of the DVL, the filtering algorithm of the tight combination system needs to be able to automatically reduce the utilization rate of the error information of the beam. However, most of the robust filters used by the current integrated navigation system uniformly adjust the utilization rate of all measurement information, and for a typical multi-dimensional measurement system such as an SINS/DVL tight integrated navigation system, reducing the utilization rate of beam information with a single large measurement error may cause other normal beam measurement information to be not fully used in the system, thereby losing useful information.

Therefore, there is a need to invent a robust filtering method that reduces the influence of beam information containing large measurement errors without affecting the use of normal beam information by the navigation system.

Disclosure of Invention

The invention aims to provide a combined navigation robust filtering method based on statistical similarity measurement.

The purpose of the invention is realized by the following technical scheme: the method comprises the following steps:

step 1: defining a coordinate system;

defining a carrier coordinate system to be represented by b, a geocentric coordinate system to be represented by e, a geographic coordinate system of 'east-north-sky' as a navigation system to be represented by n, an inertial coordinate system to be represented by i, and a DVL frame coordinate system to be represented by d;

defining velocities in different coordinate systems:

wherein the content of the first and second substances,representing the velocity vector of the strapdown inertial navigation system in a b system;representing the velocity vector of the strapdown inertial navigation system in the n systems;representing a velocity vector of the strapdown inertial navigation system in a system d;representing a velocity vector of the DVL beam in the d-system;

step 2: establishing an SINS/DVL tightly-combined navigation system state equation;

selecting SINS error as state including misalignment angle phi and speed error delta VⁿPosition error delta p and gyro constant drift epsilon^bAnd accelerometer constant zero offsetThe equation of state is as follows:

wherein epsilon^bRepresenting a gyro constant drift vector;is a gyro random drift vector;is the accelerometer constant zero offset vector;is the accelerometer random zero offset vector; phi denotes a misalignment angle vector consisting of pitch, roll and yaw misalignment angles;is a directional cosine matrix representing the transformation from the b-system to the n-system;represents the angular velocity vector of b relative to i under b, and the corresponding calculation error is ⁿ＝[v_E v_N v_h]^TIs the vector of the velocity of the carrier in the n series with respect to the ground, the corresponding error vector is δ vⁿ；f^bIs the output of the accelerometer; δ gⁿIs the projection of the gravity error vector in the n system;δ p represents a position error vector composed of a latitude error δ L, a longitude error δ λ, and an altitude error δ h; r_MAnd R_NRespectively representing the curvature radius of the meridian circle and the initial vertical circle;

and step 3: establishing a SINS/DVL tight combination navigation system measurement equation and decomposing;

the measurement equation is as follows:

Z＝HX+v

wherein the content of the first and second substances,representing measurement noise; i is_3×3Is a three-dimensional identity matrix; [. radix Et rhizoma Rhei]A cross product operation representing a vector;α is the horizontal angle between the beam and the carrier;set at 0 ° or 45 °;

the measurement noise of each beam of the DVL is usually uncorrelated, and its measurement noise covariance matrix R ═ E [ vv ═^T]Is a diagonal matrix, so the measurement equation is equivalently decomposed:

wherein H^jRepresents the jth row of the measurement matrix; v. of^jThe measurement noise is corresponding measurement noise, the measurement noise of different DVL beams is uncorrelated, and here, the four beam measurement values of the DVL are assumed to be obtained by respectively measuring by different sensors;

and 4, step 4: constructing a cost function of state estimation based on the statistical similarity measurement;

wherein the content of the first and second substances,andrespectively representing a one-step prediction result and a one-step prediction covariance matrix of the state; n (X; X)_k+1,P_k+1) Representing a Gaussian distribution with respect to X, the mean being X_k+1Variance is P_k+1(ii) a Similarity function f_x(t) is selected as f_x(t) — 0.5 t; similarity function f_z(t) is selected asω is a degree of freedom parameter, m is a measurement dimension, and m is 1; q is 4;

and 5: solving the optimal value of the cost function by applying Gaussian-Newton iteration;

step 5.1: setting the initial iteration number as 0

Step 5.2: calculating filter gain

Wherein the content of the first and second substances, andauxiliary parameters for adjusting the one-step prediction covariance matrix and the measurement noise variance;

step 5.3: calculating posterior states

Step 5.4: computing estimation error covariance matrix

Step 5.5: judging whether the requirements are metIf not, making t equal to t +1, and returning to the step 5.2; otherwise, it ordersOutputting an error X of the SINS;

step 6: feeding back the estimated SINS error X to the SINS for correction;

based on the estimated error X of the SINS, obtainTaking out the misalignment angle phi and the speed error delta VⁿPosition error δ p; attitude matrix for SINS solutionSpeed of rotationPosition ofThe feedback correction method of the attitude error comprises the following steps:

speed error delta VⁿAnd the position error δ p is directly subtracted from the output of the SINS:

the invention has the beneficial effects that:

the invention provides a novel robust filtering method based on multi-dimensional measurement equation decomposition and statistical similarity measurement, which considers the situation that normal DVL beam measurement information and large-error beam measurement information in an SINS/DVL tight combination system occur simultaneously and aims at solving the problem that the conventional robust filter of the combined navigation system processes the measurement information coarsely to cause easy loss of the normal measurement information. The invention decomposes the multidimensional measurement equation of the SINS/DVL tight combination navigation system, and introduces a Statistic Similarity Measurement (SSM) theory to assist the measurement noise variance of each beam to complete respective self-adaptive updating when a large measurement error occurs, thereby finally ensuring the independence of the measurement information processing of each beam. The invention can be used in the field of underwater vehicle integrated navigation under non-ideal conditions.

Drawings

FIG. 1 is a block diagram of a SINS/DVL tightly-integrated navigation system.

FIG. 2 is a velocity relationship diagram under different coordinate systems.

FIG. 3 is a diagram of experimental traces of lake test.

FIG. 4 is a graph comparing velocity error and positioning error.

Fig. 5 is a plot of DVL beam measurements versus auxiliary parameter changes.

FIG. 6 is a flow chart of the present invention.

Detailed Description

The invention is further described below with reference to the accompanying drawings.

The invention aims to provide a robust filtering method capable of independently adjusting the measurement noise variance of each DVL wave beam aiming at an SINS/DVL tight combination navigation system, namely, when a certain measurement has a larger error, the corresponding measurement noise variance matrix in a filter can be automatically increased, and the measurement noise variance matrix of other normal measurements is hardly adjusted.

The method comprises the following steps: establishing an SINS/DVL tightly combined navigation system model;

firstly, a common coordinate system is defined, a carrier coordinate system is represented by b, a geocentric coordinate system is represented by e, a geographical coordinate system of east-north-sky is represented by n as a navigation system, an inertial coordinate system is represented by i, and a DVL frame coordinate system is represented by d.

FIG. 1 depicts the structure of a SINS/DVL tightly-integrated navigation system. SINS is a primary navigation device that provides speed, attitude, and position information. As an auxiliary navigation device, the DVL provides a four-dimensional measurement vector, i.e. the velocity of the DVL relative to the seafloor in four beam directions. And converting the SINS solution into a difference value of the velocity in the d coordinate system and the velocity of the DVL beam to be used as the observed quantity of the navigation filter. The state is then estimated and fed back to the SINS to reduce the SINS navigation error.

In the present invention, it is assumed that the stagger angle error between the SINS and the DVL and the scaling factor of the DVL have been calibrated and compensated after the installation of the device is completed. The residual mounting angle error and scale factor error are negligible. Therefore, the invention only selects the SINS error as the state, including misalignment angle phi and speed error delta VⁿPosition error delta p and gyro constant drift epsilon^bAnd accelerometer constant zero offsetThe equation of state is as follows:

in the formula of^bRepresents a constant drift vector of the gyro,is a gyro random drift vector. Is a constant zero offset vector of an accelerometer Is the accelerometer random zero offset vector. Phi denotes the misalignment angle vector consisting of pitch, roll and yaw misalignment angles.Is a direction cosine matrix representing the transformation from the b-system to the n-system.Represents the angular velocity vector of b relative to i under b, and the corresponding calculation error isvⁿ＝[v_E v_N v_h]^TIs the vector of the velocity of the carrier in the n series with respect to the ground, the corresponding error vector is δ vⁿ。f^bIs the output of the accelerometer. δ gⁿIs the projection of the gravity error vector in the n system. δ p denotes a position error vector composed of a latitude error δ L, a longitude error δ λ, and an altitude error δ h. R_MAnd R_NThe radii of curvature of the meridian circle and the initial vertical circle are shown, respectively.

Before introducing a measurement equation, the present invention defines velocities in different coordinate systems, taking a four-beam configuration DVL as an example, as shown in fig. 2.Is the velocity vector of the strapdown inertial navigation system in the b system.And representing the velocity vector of the strapdown inertial navigation system in the n systems.Representing the velocity vector of the strapdown inertial navigation system in the d system.Representing the velocity vector of the DVL beam in the d-system.

The relationship between the velocity of the strapdown inertial navigation system under the navigation system and the DVL beam velocity under the d system is as follows

Where α is the horizontal angle between the beam and the carrier. In general,set at 0 ° or 45 °.

DVL beam measurements are modeled as follows:

whereinRepresenting the measurement noise of the DVL. The result of converting the speed output of the strapdown inertial navigation system to d system is

In the formula I_3×3Is a three-dimensional identity matrix. [. radix Et rhizoma Rhei]Representing a cross product operation of the vector.

Simultaneous (19) and (20) can yield a measurement equation

Z＝HX+v (22)

Representing the measurement noise.

Step two: decomposing a multi-dimensional measurement equation of the SINS/DVL tight combination navigation system;

the measurement noise of each beam of the DVL is usually uncorrelated, and its measurement noise covariance matrix R ═ E [ vv ═^T]Is a diagonal matrix. Therefore, the measurement equation (22) can be equivalently decomposed in the present invention.

Wherein H^jRepresenting the jth row of the measurement matrix. v. of^jIs the corresponding measurement noise, the measurement noise of the different DVL beams is uncorrelated. Here, it is assumed that four beam measurement values of the DVL are obtained by measurement by different sensors, respectively.

Step three: introducing a concept of statistical similarity measurement, and constructing a cost function of state estimation based on the statistical similarity measurement;

step 3.1: introduction of Statistical Similarity Measures (SSM)

The statistical similarity measure s (a, b) can be used to represent the similarity between two random vectors a and b, defined as follows:

s(a,b)＝E[f(||a-b||²)]＝∫∫f(||a-b||²)p(a,b)dadb (25)

where | l | · | | represents the euclidean norm, and p (a, b) represents the joint probability density between random vectors a and b. f (-) represents a similarity function, and satisfies the following three conditions.

a) f (-) is a continuous function within the definition domain [0, + ∞).

b) f (-) is a monotonically decreasing function, i.e.

c) The second derivative of f (-) is non-negative, i.e.

The statistical similarity measure conforms to the general definition of a similarity measure. The higher the similarity between random vectors, the larger the statistical similarity measure. If f (t) is selected to be f (t) ═ t, SSM indicates a differenceNegative mean square error between random vectors. When f (t) is selected to beWhen, SSM represents the correlation entropy between different random vectors. By selecting different similarity functions, different SSMs can be implemented.

Step 3.2: establishing a robust cost function

First, assume a system model of

X_k+1＝f_k(X_k)+n_k (26)

Wherein k represents time, X_k+1Representing a p-dimensional state.Denotes the jth measurement, f_k(. a) andrepresenting the state transition function and the measurement function. n is_kAndrepresenting process noise and metrology noise. Q_kAndrepresenting the process noise nominal covariance matrix and the jth measured noise nominal covariance matrix, q is 4 for the SINS/DVL tightly-combined navigation system described herein.

Standard Kalman Filtering (KF) is essentially a weighting of the state prediction and measurement information to obtain an optimal estimate of the state, whose weighted least squares cost function reflects the mean square error between the state and the predicted state and between the measurement and the predicted measurement. The present invention constructs a cost function reflecting the SSM between the state and predicted states and between the measurements and predicted measurements.

In the formula (I), the compound is shown in the specification,andrepresenting the one-step prediction result and the one-step prediction covariance matrix of the state,andrepresenting the variance of the measured noiseSum-step prediction covariance matrixRoot mean square decomposition of.

In order to solve the cost function shown in (28), the posterior distribution is approximated to a gaussian distribution, and the lower bound of the cost function in the above equation is solved by using Jensen inequality as a new cost function.

In the formula, N (X; X)_k+1,P_k+1) Representing a Gaussian distribution with respect to X, the mean being X_k+1Variance is P_k+1。

Equation of when measuringIn case of non-linearity, a sigma point transform, such as an unscented transformation rule or a cubic sphere radial volume rule, may be used for the approximate solution. When the measurement equation is linear, (31) can be written as

Step four: solving the optimal value of the cost function by applying Gaussian-Newton iteration;

the extreme value of the cost function is solved by deriving the cost function and making the derivative zero.

Since equation (33) is a nonlinear equation, gaussian-newton iteration method is used to solve (33) in the present invention.

According to the Gauss-Newton iteration method, for the nonlinear equation (33), the numerical value of the solution is updated by

In the formula, the superscript t represents the t-th iteration. Initial value of iteration is set asRepresents the cost function J (X)_k+1,P_k+1) Approximate Hessian (Hessian) matrix.

Wherein the content of the first and second substances,

is finished to obtain

In the above formula, the filter gain is

WhereinAndcorresponding to the auxiliary parameters used to adjust the one-step prediction covariance matrix and the measured noise variance.

Estimate error of

Thereby directly obtaining estimation error covariance matrix

Typically, for a SINS/DVL tightly-combined navigation system, the process noise made up of the random drift of the gyros and accelerometers is Gaussian noise with an accurate covariance. Thus, the similarity function f_x(t) is selected as f_x(t) — 0.5t, i.e. the degree of similarity between the state and the predicted state is measured using negative mean square error. The DVL beam measurement noise may be non-Gaussian noise caused by outliers, so the similarity function f_z(t) is selected asω is the degree of freedom parameter and m is the measurement dimension, which is 1 in the present invention.

Step five: and feeding back the estimated navigation error to the strapdown inertial navigation.

Through a robust Kalman filter, an estimated misalignment angle phi and a speed error delta V can be obtainedⁿAnd the position error delta p, the attitude matrix, the speed and the position calculated by assuming the pure inertia solution of the strapdown inertial navigation are respectivelyBecause the feedback correction is adopted to correct the strapdown inertial navigation, the error amount of the inertial navigation is kept small all the time, and the feedback correction method of the attitude error comprises

Speed error delta VⁿAnd directly subtracting the position error delta p from the output of the strapdown inertial navigation.

Step six: the proposed algorithm was validated using lake test experimental data.

FIG. 3 shows the flight path of a lake test. In the experiment, self-research strapdown inertial navigation and DVL are used as devices to be tested, and a GPS receiver and a national imported PHINS inertial navigation combination are used as a reference.

Fig. 4 shows the velocity error and horizontal positioning error of the proposed algorithm and the existing robust filtering algorithm applied in the SINS/DVL tightly combined system. Wherein RSTKF represents an existing robust Kalman filter based on Student's t distribution modeling. MCKF represents an existing Kalman filter based on the maximum correlation entropy criterion. The SSMKF represents the existing Kalman filter based on statistical similarity measurement, and the SSMKF is the most different from the robust Kalman filter provided by the invention in that the SSMKF only uses one auxiliary parameter to uniformly adjust the utilization rate of all the measurement components, while the robust Kalman filter provided by the invention respectively adjusts the utilization rates of different measurement components by using a plurality of auxiliary parameters. As can be seen from fig. 4, the positioning error and the velocity error of the proposed algorithm are both minimal, and the estimation accuracy of the proposed algorithm is significantly better than that of the SSMKF based on the same statistical similarity metric criterion.

Fig. 5 shows a DVL beam measurement and the variation of the auxiliary parameter corresponding to each beam in the experiment. It can be seen from the figure that when a large outlier occurs, the auxiliary parameters corresponding to the DVL beam are significantly reduced, so that the noise variance of the beam measurement is increased, and the proportion of the large outlier measurement in the estimation result is reduced. And the auxiliary parameters corresponding to the normal DVL wave beam can not be obviously changed, so that the algorithm can reduce the influence of error measurement and ensure the use of normal measurement.

The invention provides a novel robust filtering method based on multi-dimensional measurement equation decomposition and statistical similarity measurement, which considers the situation that normal DVL beam measurement information and large-error beam measurement information in an SINS/DVL tight combination system occur simultaneously and aims at solving the problem that the conventional robust filter of the combined navigation system processes the measurement information coarsely to cause easy loss of the normal measurement information.

The invention decomposes the multidimensional measurement equation of the SINS/DVL tight combination navigation system, and introduces a Statistic Similarity Measurement (SSM) theory to assist the measurement noise variance of each beam to complete respective self-adaptive updating when a large measurement error occurs, thereby finally ensuring the independence of the measurement information processing of each beam. The invention can be used in the field of underwater vehicle integrated navigation under non-ideal conditions.

The main advantages of the invention are as follows:

(1) the method decomposes the measurement equation of the SINS/DVL tight integrated navigation system, and establishes the cost function by using the statistic similarity measurement concept, thereby improving the utilization efficiency of the robust information fusion method of the integrated navigation system on the normal measurement information under the condition of abnormal measurement;

(2) the invention carries out iterative solution on the nonlinear cost function, thereby reducing numerical calculation errors;

(3) the system is considered as a nonlinear system when the cost function is established and solved, so that the robust information fusion method provided by the invention has a wide application range, and can be used for both a linear system and a nonlinear system.

The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.