CN111999747B - Robust fault detection method for inertial navigation-satellite integrated navigation system - Google Patents

Abstract

The invention provides a robust fault detection method for an inertial navigation-satellite integrated navigation system. It includes the following steps: adopting the volumetric Kalman filter algorithm based on singular value decomposition to optimize the estimation of the constructed strapdown inertial navigation system/global navigation satellite system integrated navigation system; introducing frame-based error detection filtering into the singular value decomposition-based In the volumetric Kalman filter algorithm, the integrated navigation system constructed is linearized, and in the process of error detection, the judgment threshold of the sensor for fault detection is calculated when the integrated navigation system is integrated navigation; the fault occurs is compared with the judgment threshold If the fault is small, the state estimation accuracy of the volumetric fault detection filter is improved by an adaptive algorithm based on the measurement residual and the scaling factor; if the fault is large, the robust batch processing H _∞ filter is used to ensure stability at this time. The invention can well guarantee the robustness of fault detection.

Description

A Robust Fault Detection Method for Inertial Navigation-Satellite Integrated Navigation System

technical field

The invention relates to the technical field of signal processing of shipborne navigation instruments, in particular to a robust fault detection method for an inertial navigation-satellite integrated navigation system.

Background technique

The inertial navigation system is an autonomous navigation system that does not rely on any external information and does not radiate energy to the outside. It has the characteristics of good concealment and can work in various complex environments such as air, ground, and underwater. The main points are There are two types of platform inertial navigation systems and strapdown inertial navigation systems. As the core navigation system of various ships and civil ships, the accuracy and reliability of the strapdown inertial navigation system directly affect the performance indicators of its equipment carrier.

The existing ship strapdown inertial navigation system and the global navigation satellite system (SINS/GNSS) integrated navigation system use the extended Kalman filter to form a signal estimation process of a specific time window, and adjust the nonlinear signal system to the signal estimation of the linear signal system process. The statistical model is used to effectively identify and detect the measured values in the measurement matrix reflecting the trajectory formed in the estimation process, so as to ensure that the abnormality of the measured values is judged under the influence of linearization residuals in the measurement matrix formed during the effective trajectory estimation process. Excluded, the existing technology is difficult to guarantee the robustness of fault detection.

Contents of the invention

According to the technical problems raised above, a robust fault detection method for an inertial navigation-satellite integrated navigation system is provided. The technical means adopted in the present invention are as follows:

A robust fault detection method for an inertial navigation-satellite integrated navigation system, comprising the steps of:

Step 1, adopt volumetric Kalman filter algorithm based on singular value decomposition to carry out optimal estimation to the strapdown inertial navigation system/global navigation satellite system integrated navigation system of construction;

Step 2. Introduce the frame-based error detection filter into the volumetric Kalman filter algorithm based on singular value decomposition, and the constructed strapdown inertial navigation system/global navigation satellite system integrated navigation system can be linearized. In the process of calculating the ship inertial navigation-satellite integrated navigation system integrated navigation, the judgment threshold of the sensor for fault detection;

Step 3, comparing the relationship between the failure and the judgment threshold, if the failure is small, then improve the state estimation accuracy of the volumetric fault detection filter through an adaptive algorithm based on measurement residuals and scaling factors; is large, the robust batch H _∞ filter is used to ensure the stability at this time.

Further, in the step 1, the constructed SINS/GNSS integrated navigation system is:

和C_k组成的单位模块矩阵，B_c＝B_cf，D_cf和D_c分别对应观测方程中系统故障f和系统噪声误差v的系数，且D_c＝Dc_f＝[0,I]^T，噪声v_k∈l₂[0,N]和ω_k∈[0,N]，l₂表示n维欧氏空间，f_k为系统携带的故障矩阵，下标k代表步长或时间；f _c is the error function of the nonlinear strapdown inertial navigation system, and B _c is the strapdown matrix

The unit module matrix composed of and C _k , B _c ＝B _cf , D _cf and D _c respectively correspond to the coefficients of system failure f and system noise error v in the observation equation, and D _c ＝Dc _f ＝[0,I] ^T , Noise v _k ∈ _{l 2} [0,N] and ω _k ∈ [0,N], l ₂ represents the n-dimensional Euclidean space, f _k is the fault matrix carried by the system, and the subscript k represents the step size or time;

The volumetric Kalman filter algorithm based on singular value decomposition is used to perform Kalman filtering. The specific operation steps are as follows:

Step 11. Time update the volumetric Kalman filter based on singular value decomposition, specifically,

Where k=1,...n, and χ are volume sampling points, svd represents a singular value decomposition matrix algorithm, and the above sampling points are propagated in the following way:

以及它的协方差矩阵，其中Q_k为系统噪声观测矩阵，详细运算过程如下：where f _c is the system matrix in system (1), and the system state is obtained with the updated sampling points

and its covariance matrix, where Q _k is the system noise observation matrix, and the detailed operation process is as follows:

Step 12, update the process using the volumetric Kalman filter based on singular value decomposition, specifically,

The predicted observation vector and associated covariance matrix are updated as follows, where R _k is the observation noise matrix:

可用如下方式得到：。Updated cross-covariance matrix

It can be obtained as follows: .

以及用以下次更新的协方差矩阵P_k|k。Step 13. Calculate the matrix gain K _k , the updated state matrix

And use the covariance matrix P _k|k for the next update.

Further, in the step 2, the SINS/GNSS integrated navigation system to be constructed can be linearized as:

e_k和ε_k是保持正态分布项的非线性性质的统计线性化误差以及状态和测量函数具有零均值和协方差的矩阵，计算方式如下。

e _k and ε _k are statistical linearization errors preserving the nonlinear nature of the normally distributed terms and matrices with zero mean and covariance for the state and measurement functions, computed as follows.

具体应为：/>

Parameters H _k and

Specifically should be: />

以及权重W_k的选用：At this time, the system (8) satisfies the form of the volumetric fault detection filter as follows, which can be used for reference from the prior probability distribution

And the selection of weight W _k :

Then system (11) can be restructured as follows:

J _N is the calculation method of the threshold value when performing fault detection, and the update method is as follows.

Some parameters in (12) are updated as follows:

Further, in the step 3, the adaptive part specifically includes:

The adaptive R _k is as follows:

The adaptive Q _k is as follows:

in:

Further, in the step 3, the robustness optimization specifically includes:

The optimization and adjustment of γ is as follows:

Under the assumption that the norm of process disturbance and measurement noise is bounded, the present invention proposes a volumetric fault detection filter (fault detection filter, FDF) that considers both disturbance sensitivity and robustness. Observer gains are used for fault detection and state estimates for suboptimal solutions are obtained. An adaptive algorithm based on measurement residuals and scaling factors is used to improve the state estimation accuracy of volumetric FDF. In order to ensure the stability when the fault is too large, the framework uses a robust H _∞ volumetric Kalman filter. The invention can well guarantee the robustness of fault detection.

Description of drawings

In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings that need to be used in the description of the embodiments or the prior art. Obviously, the accompanying drawings in the following description These are some embodiments of the present invention. For those skilled in the art, other drawings can also be obtained according to these drawings without any creative effort.

Fig. 1 is the flow chart of the present invention.

Detailed ways

In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments It is a part of embodiments of the present invention, but not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by persons of ordinary skill in the art without making creative efforts belong to the protection scope of the present invention.

This embodiment discloses a robust fault detection method for an inertial navigation-satellite integrated navigation system, including the following steps:

Step 1, adopt volumetric Kalman filter algorithm based on singular value decomposition to carry out optimal estimation to the strapdown inertial navigation system/global navigation satellite system integrated navigation system of construction;

Step 2. Introduce frame-based error detection filtering into the volumetric Kalman filter algorithm based on singular value decomposition, and linearize the constructed strapdown inertial navigation system/global navigation satellite system integrated navigation system. In the process, when calculating the ship inertial navigation-satellite integrated navigation system integrated navigation, the judgment threshold of the sensor for fault detection;

Step 3, comparing the relationship between the failure and the judgment threshold, if the failure is small, then improve the state estimation accuracy of the mode volume failure detection filter through an adaptive algorithm based on the measurement residual and the scaling factor; if the failure If it is larger, the robust batch processing H _∞ filter is used to ensure the stability at this time.

As shown in Figure 1, the specific steps of this embodiment are as follows:

The strapdown inertial navigation system/global navigation satellite system integrated navigation system (SINS/GNSS) constructed is:

和C_k组成的单位模块矩阵，B_c＝B_cf，D_cf和D_c分别对应观测方程中系统故障f和系统噪声误差v的系数，且D_c＝D_cf＝[0,I]^T，噪声v_k∈l₂[0,N]和ω_k∈[0,N]，l₂表示n维欧氏空间，f_k为系统携带的故障矩阵，下标k代表步长或时间；f _c is the error function of the nonlinear strapdown inertial navigation system, and B _c is the strapdown matrix

The unit module matrix composed of and C _k , B _c ＝B _cf , D _cf and D _c respectively correspond to the coefficients of system failure f and system noise error v in the observation equation, and D _c ＝D _cf ＝[0,I] ^T , Noise v _k ∈ _{l 2} [0,N] and ω _k ∈ [0,N], l ₂ represents the n-dimensional Euclidean space, f _k is the fault matrix carried by the system, and the subscript k represents the step size or time;

If you want to estimate it optimally, considering the complex sea surface environment and the characteristics of ship motion, the system is a strongly nonlinear system, and the volumetric Kalman filter (SVD-CKF) algorithm based on singular value decomposition can be used to implement the Kalman Mann filtering. The specific operation steps are as follows:

Step 11. Time update the volumetric Kalman filter based on singular value decomposition, specifically,

Where k=1,...n, and χ is the volume sampling point, and svd represents the singular value decomposition matrix algorithm, which is used to improve the robustness of the algorithm. The above sampling points are propagated in the following ways:

以及它的协方差矩阵，其中Q_k为系统噪声观测矩阵，详细运算过程如下：where f _c is the system matrix in system (1), and the system state is obtained with the updated sampling points

and its covariance matrix, where Q _k is the system noise observation matrix, and the detailed operation process is as follows:

Step 12. The volumetric Kalman filter based on singular value decomposition is used for process update. Specifically, the sampling points need to be updated again. The specific method is as follows:

The predicted observation vector and associated covariance matrix are updated as follows, where R _k is the observation noise matrix:

可用如下方式得到：Updated cross-covariance matrix

It can be obtained as follows:

以及用以下次更新的协方差矩阵P_k|k。Step 13. Calculate the matrix gain K _k , the updated state matrix

And use the covariance matrix P _k|k for the next update.

The error detection filter based on the H _i /H _∞ framework is introduced into the traditional SVD-CKF. System (1) can be linearized as:

e_k和ε_k是保持正态分布项的非线性性质的统计线性化误差以及状态和测量函数具有零均值和协方差的矩阵，计算方式如下：Combined SVD-CKF results.

e _k and ε _k are the statistical linearization errors preserving the nonlinear nature of the normally distributed terms and the state and measurement functions have zero mean and covariance matrices, computed as follows:

具体应为：/>

Parameters H _k and

Specifically should be: />

以及权重W_k的选用：At this time, the system (8) satisfies the form of the volumetric fault detection filter as follows, which can be used for reference from the prior probability distribution

And the selection of weight W _k :

Then system (11) can be restructured as follows:

J _N is the calculation method of the threshold value when performing fault detection, and the update method is as follows:

Some parameters in (12) are updated as follows:

Although the SVD-CKF is integrated into the above error detection filter based on the H _i /H _∞ framework, the sensor failure during the ship's SINS/GNSS integrated navigation can be detected. Although it can take into account the sensitivity of fault diagnosis and the robustness of error interference, but for the accuracy of state estimation, the method is slightly lacking, because the algorithm is based on the method of suboptimal estimation. For this reason, QR adaptive optimization is performed on this method to improve the estimation accuracy.

The adaptive R _k is as follows:

The adaptive Q _k is as follows:

in:

Adaptive optimization ensures that SVD-CKF can perform accurate state estimation after incorporating the error detection filter based on the H _i /H _∞ framework, but SINS/GNSS will produce singular values when there are large faults and errors. If the error is too large, it will also cause the integrated navigation to fail. In order to improve the stability of the algorithm as much as possible, the adaptive optimization is abandoned for systems with large errors or faults, and the robust optimization is used instead. The threshold for judging adaptive optimization and robust optimization can refer to J _N,k in fault detection filtering. At this point, the specific update process is as follows:

The optimization and adjustment of γ is as follows:

Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present invention, rather than limiting them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: It is still possible to modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements for some or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the technical solutions of the various embodiments of the present invention. scope.

Claims (1)

1. A robust fault detection method of an inertial navigation-satellite integrated navigation system is characterized by comprising the following steps:

step 1, optimizing and estimating a constructed strapdown inertial navigation system/global navigation satellite system combined navigation system by adopting a volume Kalman filter algorithm based on singular value decomposition;

step 2, introducing error detection filtering based on a frame into the volume Kalman filter algorithm based on singular value decomposition, linearizing a built strapdown inertial navigation system/global navigation satellite system integrated navigation system, and calculating a judging threshold value of fault detection by a sensor when the ship inertial navigation-satellite integrated navigation system integrated navigation is calculated in the error detection process;

step 3, comparing the relation between the faults and the judging threshold, and if the faults are smaller, improving the state estimation precision of the volume fault detection filter through a self-adaptive algorithm based on the measurement residual error and the scale factors; if the fault is large, robust batch processing H is adopted _∞ Filtering to ensure the stability at this time;

in the step 1, the built strapdown inertial navigation system/global navigation satellite system integrated navigation system is as follows:

f _c is a nonlinear strapdown inertial navigation system error function, B _c Is composed of strapdown matrix

And C _k Unit module matrix formed by B _c ＝B _cf ，D _cf And D _c Respectively corresponding to the coefficients of the system fault f and the system noise error v in the observation equation, and D _c ＝D _cf ＝[0,I] ^T Noise v _k ∈l ₂ [0,N]And omega _k ∈[0,N]，l ₂ Representing an n-dimensional Euclidean space, f _k For a fault matrix carried by a system, the subscript k represents step length or time;

the Kalman filtering is executed by adopting a volume Kalman filter algorithm based on singular value decomposition, and the specific operation steps are as follows:

step 11, time updating the volume kalman filter based on singular value decomposition, specifically,

where k=1,..n, and χ are volume sampling points, svd represents a singular value decomposition matrix algorithm, which propagates by:

wherein f _c For the system matrix in the system (1), the updated sampling points are used to obtain the system state

And its covariance matrix, wherein Q _k For the system noise observation matrix, the detailed operation process is as follows:

step 12, updating a volume Kalman filter based on singular value decomposition, wherein the method comprises the following steps:

the predicted observation vector and associated covariance matrix are updated as follows, whereinR _k To observe the noise matrix:

updated cross covariance matrix

The method is characterized by comprising the following steps:

step 13, calculating matrix gain K _k Updated state matrix

Covariance matrix P updated with next time _k|k ：

In the step 2, linearizing the built strapdown inertial navigation system/global navigation satellite system integrated navigation system into:

e _k and epsilon _k Is a matrix with zero mean and covariance of statistical linearization error and state and measurement functions that maintains the nonlinear properties of normal distribution terms, calculated as follows:

parameter H _k And

the method comprises the following steps: />

At this time, the form of the volume fault detection filter satisfied by the formula (8) is as follows, and the prior probability distribution can be used as a reference

Weight W _k Is selected from the following:

the system (11) is reconfigured as follows:

J _N in order to calculate the threshold value during fault detection, the update method is as follows:

(12) The specific update of part of the parameters is as follows:

in the step 3, the adaptive portion specifically includes:

self-adaptive R _k The following are provided:

self-adapting Q _k The following are provided:

wherein:

in the step 3, the robustness optimization specifically includes:

the gamma is optimally adjusted as follows:

Images