An INS networking information fusion method based on a recursive algorithm

Abstract

The application provides an INS networking information fusion method based on a recursive algorithm. The standard deviation of the right gyro drift estimation value of a low-precision INS carrier system of a Kalman filter is calculated respectively to determine the weight of a first set of high-precision INS and a second set of high-precision INS in INS networking, and the position information after the first fusion is obtained. When the information is fused again, the standard deviation of the right gyro drift estimation value of the low-precision INS carrier system is updated by using three parameters, and the updated weight is calculated to update the fused position information. In the information fusion process of the two sets of high-precision INS in the INS networking, the concealment is high, and the weight of the two sets of high-precision INS is calculated by using only three parameters.

Description

TECHNICAL FIELD

[0001] The application relates to an INS networking information fusion method based on a recursive algorithm and belongs to the field of INS networking information fusion. BACKGROUND

[0002] An inertial navigation system (INS) is a completely autonomous navigation system, which is not affected by the external environment and is not interfered by the external electromagnetic field. The working environment of the inertial navigation system includes the air, the ground and the water. In order to ensure the reliability of long-time work, a configuration scheme of multiple INSs is commonly used at present, and an INS networking structure is formed. Through processing of the information of multiple INSs, on one hand, in the case that a device fails or navigation output information has a large error, fault identification and system switching can be performed in time, so that the reliability of navigation information and the effectiveness of the system are ensured; on the other hand, the output information in the INS networking is fused, so that the accuracy of navigation is improved.

[0003] In the INS networking, a typical configuration is one set of low-precision INS and two sets of high-precision INS. When there is no external measurement reference, the navigation positioning results of the two sets of high-precision INS are fused according to the subjective experience of the staff to obtain the current reference navigation positioning result, and the low-precision INS serves as a hot backup system. In actual work, the navigation errors of the two sets of high-precision INS are not the same, and the information fusion cannot be performed according to the experience. In order not to waste the resources of the INS networking configuration, the two sets of high-precision INS need to be assigned weights in real time, and the current reference navigation positioning result is obtained.

[0004] In the patent of He Wei et al. of Beijing Automation Control Equipment Institute disclosed in 2021, an multi-source information fusion combined navigation system and method (patent application number: CN202110012499.8) is proposed. In this patent, other navigation sources are involved in addition to the INS, which can weaken the concealment of the system. In the patent of Ma Zheng et al. of Beijing Tuguo Intelligent Technology Co., Ltd. disclosed in 2020, a fault detection and correction method for an open-pit mine unmanned inertial navigation system (patent application number: CN202010298879.8) is proposed. In this patent, real-time information fusion of each navigation system is not performed. Therefore, the INS networking information fusion method based on the recursive algorithm has innovation and practical engineering value. SUMMARY

[0005] The application provides an INS networking information fusion method based on a recursive algorithm.

[0006] The object of the present application is achieved in that the steps are as follows:

[0007] Step 1, respectively determine the combined error state of the low-precision INS and the first set of high-precision INS, and the combined error state of the low-precision INS and the second set of high-precision INS;

[0008] Step 2, respectively determine the combined state space model composed of the low-precision INS and the first set of high-precision INS, and the combined state space model composed of the low-precision INS and the second set of high-precision INS;

[0009] Step 3, in order to perform Kalman filtering algorithm, discretize the two combined state space models in step 2 respectively;

[0010] Step 4, respectively perform Kalman filtering algorithm on the discretized combined state space models of the low-precision INS and the first set of high-precision INS, and the low-precision INS and the second set of high-precision INS, the low-precision INS and the first set of high-precision INS form Kalman filter 1, and the low-precision INS and the second set of high-precision INS form Kalman filter 2;

[0011] Step 5, take the standard deviation of the right gyro drift estimation value of the low-precision INS carrier system calculated by the two Kalman filters after filtering as the evaluation index, the smaller the standard deviation of the right gyro drift estimation value of the low-precision INS carrier system, the greater the weight value of the corresponding high-precision INS in information fusion, the greater the standard deviation of the right gyro drift estimation value of the low-precision INS carrier system, the smaller the weight value of the corresponding high-precision INS in information fusion, and the ratio of the weights of the two sets of high-precision INS in information fusion is inversely proportional to the standard deviations of the right gyro drift estimation values of the low-precision INS carrier system calculated by the respective Kalman filters;

[0012] After the Kalman filter is filtered stably, a period of data is taken for the first weight distribution, the standard deviation σ1 of the right gyro drift estimation value of the low-precision INS carrier system of Kalman filter 1 and the standard deviation σ2 of the right gyro drift estimation value of the low-precision INS carrier system of Kalman filter 2 are calculated, and the formula is as follows:

[0013]

[0014]

[0015] Wherein, N is the number of data of the right gyro drift estimation value of the low-precision INS carrier system participating in the calculation in this period, ε x,1 (i) is the i-th right gyro drift estimation value of the low-precision INS carrier system in this period obtained by Kalman filter 1, ε x,2(i) is the right gyro drift estimation value of the i th low-precision INS carrier system in the time period obtained by Kalman filter 2, μ 1 is the average value of N low-precision INS carrier system right gyro drift estimation values in the time period obtained by Kalman filter 1, and μ 2 is the average value of N low-precision INS carrier system right gyro drift estimation values in the time period obtained by Kalman filter 2;

[0016] The standard deviation of the low-precision INS carrier system right gyro drift estimation value calculated by Kalman filters 1 and 2 is used to obtain the weight of the information fusion of the first set of high-precision INS and the second set of high-precision INS, and the formula of the information fusion of the two sets of high-precision INS is as follows:

[0017] P ref =K1P1+K2P2

[0018] Wherein, P ref is the position information after the fusion of the two sets of high-precision INS, is the weight of the first set of high-precision INS, P 1 is the position information of the first set of high-precision INS at this time of information fusion, is the weight of the second set of high-precision INS, and P 2 is the position information of the second set of high-precision INS at this time of information fusion;

[0019] Step 6, when the weight is assigned again, only the low-precision INS carrier system right gyro drift estimation value of Kalman filter, N obtained when the weight is assigned last time, and the average value of the low-precision INS carrier system right gyro drift estimation value obtained when the weight is assigned last time are needed, and the standard deviation at this time can be recursively obtained according to the last standard deviation, and then used to update the weight coefficient at this time of information fusion, and the recursive formula of the standard deviation is as follows:

[0020]

[0021]

[0022] Wherein, ε x,1 (m) is the low-precision INS carrier system right gyro drift estimation value obtained by Kalman filter 1 at the current m moment, μ' 1 is the recursive update value of μ 1, σ' 1 is the recursive update value of σ 1, ε x,2 (m) is the low-precision INS carrier system right gyro drift estimation value obtained by Kalman filter 2 at the current m moment, μ' 2 is the recursive update value of μ 2, and σ' 2 is the recursive update value of σ 2.

[0023] According to the recursively obtained standard deviation, the information fusion of the first set of high-precision INS and the second set of high-precision INS is carried out:

[0024] P'ref = K'1P'1+ K'2P'2

[0025] Wherein, P r ' ef is the position information of the two sets of high-precision INS after this information fusion update, is the weight of the updated first set of high-precision INS, P1' is the position information of the first set of high-precision INS at this time of information fusion, is the weight of the updated second set of high-precision INS, P2' is the position information of the first set of high-precision INS at this time of information fusion.

[0026] Compared with the prior art, the beneficial effects of the present application are: the present application obtains the weight matched with the navigation error of the two sets of high-precision INS by real-time recursive updating of the standard deviation instead of repeated calculation of the standard deviation, so as to perform real-time information fusion. Compared with the prior art, the beneficial effects of the present application are: due to the characteristics that the INS neither affects the external environment nor is interfered by the external electromagnetic field, the configuration mode of the multiple sets of INS in the network is high in concealment; the weight of the two sets of high-precision INS in the INS network information fusion can be distributed in real time, and the current referenceable position information is obtained; in the INS network information fusion, the weight is updated by using the recursive algorithm, only three data need to be stored, and the operation amount can be reduced; in the INS network information fusion, the weight is updated by using the recursive algorithm, and the error caused by rounding can be better avoided. BRIEF DESCRIPTION OF DRAWINGS

[0027] Figure 1 is a flowchart of the INS network information fusion method based on the recursive algorithm. DETAILED DESCRIPTION

[0028] The present application will be further described in detail below in combination with the drawings and specific embodiments.

[0029] The present application comprises the following steps:

[0030] Step 1, respectively determine the joint error state of the low-precision INS and the first set of high-precision INS, and the joint error state of the low-precision INS and the second set of high-precision INS:

[0031] (1) the joint error state between the low-precision INS and the first set of high-precision INS is:

[0032]

[0033] Wherein, X1(t) is the joint error state between the low-precision INS and the first set of high-precision INS, t is continuous time, φ E1 is the difference between the east direction attitude error of the low-precision INS and the east direction attitude error of the first set of high-precision INS, φ N1φ is the difference between the low-precision INS east attitude error and the second set of high-precision INS east attitude error, φ U1 δv is the difference between the low-precision INS north attitude error and the second set of high-precision INS north attitude error, δv E1 δv is the difference between the low-precision INS east velocity error and the second set of high-precision INS east velocity error, δv N1 δL2 is the difference between the low-precision INS latitude error and the second set of high-precision INS latitude error, δλ2 is the difference between the low-precision INS longitude error and the second set of high-precision INS longitude error, ε x , ε y , ε z are the right, forward, and upward gyro constant drifts in the low-precision INS body coordinate system, respectively, ε x1 , ε y1 , ε z1 are the right, forward, and upward gyro constant drifts in the first set of high-precision INS body coordinate system, respectively, are the right and forward accelerometer constant biases in the low-precision INS body coordinate system, respectively, are the right and forward accelerometer constant biases in the first set of high-precision INS body coordinate system, respectively, and the superscript T represents transposition.

[0034] (2) The combined error state between the low-precision INS and the second set of high-precision INS is:

[0035]

[0036] wherein X2(t) is the combined error state between the low-precision INS and the second set of high-precision INS, φ E2 φ is the difference between the low-precision INS east attitude error and the second set of high-precision INS east attitude error, φ N2 φ is the difference between the low-precision INS north attitude error and the second set of high-precision INS north attitude error, φ U2 δv is the difference between the low-precision INS north attitude error and the second set of high-precision INS north attitude error, δv E2 δv is the difference between the low-precision INS east velocity error and the second set of high-precision INS east velocity error, δv N2 δL2 is the difference between the low-precision INS latitude error and the second set of high-precision INS latitude error, δλ2 is the difference between the low-precision INS longitude error and the second set of high-precision INS longitude error, ε x2 , ε y2 , ε z2are respectively the right, forward and upward gyro constant drifts of the second set of high-precision INS body coordinate system, are respectively the right and forward accelerometer constant biases of the second set of high-precision INS body coordinate system.

[0037] Step 2, determine the joint state space model of the low-precision INS and the first set of high-precision INS, and the low-precision INS and the second set of high-precision INS respectively:

[0038] (1) The joint state space model of the system composed of the low-precision INS and the first set of high-precision INS is:

[0039]

[0040] Wherein, t is continuous time, F1(t) is the state matrix of the system, G1(t) is the noise driving matrix of the system, w1(t) is the noise vector of the system, H is the measurement matrix, V(t) is the measurement noise vector, Z1(t) = [v E -v E1 v N -v N1 L-L1λ-λ1] is the measurement vector of the system, v E is the eastward output velocity of the low-precision INS, v E1 is the eastward output velocity of the first set of high-precision INS, v N is the northward output velocity of the low-precision INS, v N1 is the northward output velocity of the first set of high-precision INS, L is the output latitude of the low-precision INS, L1 is the output latitude of the first set of high-precision INS, λ represents the output longitude of the low-precision INS, and λ1 is the output longitude of the first set of high-precision INS.

[0041] (2) The joint state space model of the system composed of the low-precision INS and the second set of high-precision INS is:

[0042]

[0043] Wherein, F2(t) is the state matrix of the system, G2(t) is the noise driving matrix of the system, w2(t) is the noise vector of the system, Z2(t) = [v E -v E2 v N -v N2 L-L2λ-λ2] is the measurement vector of the system, v E is the eastward output velocity of the low-precision INS, v E2 is the eastward output velocity of the second set of high-precision INS, v N is the northward output velocity of the low-precision INS, v N2L is the output latitude of the low-precision INS, L2 is the output latitude of the second high-precision INS, λ represents the output longitude of the low-precision INS, and λ2 represents the output longitude of the second high-precision INS.

[0044] Step 3, in order to perform Kalman filter estimation, the discrete joint state space model needs to be discretized respectively:

[0045] (1) The discrete joint state space model of the system composed of the low-precision INS and the first high-precision INS is as follows:

[0046]

[0047] wherein k and k-1 are the discrete time of the continuous time t, X1(k) and X1(k-1) are the quantities after the discretization of the joint error state X1(t), Φ1(k-1) is the matrix after the discretization of the system state matrix F1(t), Γ1(k-1) is the matrix after the discretization of the system noise driving matrix G1(t), W1(k-1) is the vector after the discretization of the system noise vector w1(t), Z1(k) is the vector after the discretization of the system measurement vector Z1(t), and V(k) is the vector after the discretization of the measurement noise vector V(t).

[0048] (2) The discrete joint state space model of the system composed of the low-precision INS and the second high-precision INS is as follows:

[0049]

[0050] wherein X2(k) and X2(k-1) are the quantities after the discretization of the joint error state X2(t), Φ2(k-1) is the matrix after the discretization of the system state matrix F2(t), Γ2(k-1) is the matrix after the discretization of the system noise driving matrix G2(t), W2(k-1) is the vector after the discretization of the system noise vector w2(t), and Z2(k) is the vector after the discretization of the system measurement vector Z2(t).

[0051] Step 4, Kalman filter algorithm is performed on the discrete joint state space models of the low-precision INS and the first high-precision INS and the low-precision INS and the second high-precision INS respectively:

[0052] (1) The system state estimation and state estimation mean square error matrix X1(0) and P1(0) at the initial time of the system are set, and Kalman filter is performed on the joint state system composed of the low-precision INS and the first high-precision INS. The algorithm equation of this Kalman filter 1 is as follows:

[0053]

[0054] wherein, X1(k-1) and X1(k) are the system state estimation of the system at k-1 time and k time, P1(k-1) and P1(k) are the state estimation mean square error matrix of the system at k-1 time and k time, Q1(k-1) and Q1(k) are the system noise variance matrix of the system at k-1 time and k time, R(k) is the measurement noise variance matrix at k time, and K1(k) is the gain matrix of the system at k time.

[0055] (2) Setting the system state estimation and state estimation mean square error matrix X2(0) and P2(0) at the initial time of the system, Kalman filtering is performed on the combined state system composed of the low-precision INS and the second high-precision INS, and the algorithm equation of the Kalman filter 2 is as follows:

[0056]

[0057] wherein, X2(k-1) and X2(k) are the system state estimation of the system at k-1 time and k time, P2(k-1) and P2(k) are the state estimation mean square error matrix of the system at k-1 time and k time, Q2(k-1) and Q2(k) are the system noise variance matrix of the system at k-1 time and k time, and K2(k) is the gain matrix of the system at k time.

[0058] Step 5, after the Kalman filter is stabilized, a period of data is taken for the first time weight distribution, the standard deviation σ1 of the right gyro drift estimation value of the low-precision INS carrier system of the Kalman filter 1 and the standard deviation σ2 of the right gyro drift estimation value of the low-precision INS carrier system of the Kalman filter 2 are calculated, and the formula is as follows:

[0059]

[0060]

[0061] wherein, N is the number of data of the right gyro drift estimation value of the low-precision INS carrier system participating in the calculation in the period, ε x,1 (i) is the i-th right gyro drift estimation value of the low-precision INS carrier system in the period obtained by the Kalman filter 1, ε x,2 (i) is the i-th right gyro drift estimation value of the low-precision INS carrier system in the period obtained by the Kalman filter 2, μ1 is the average value of the N right gyro drift estimation values of the low-precision INS carrier system in the period obtained by the Kalman filter 1, and μ2 is the average value of the N right gyro drift estimation values of the low-precision INS carrier system in the period obtained by the Kalman filter 2.

[0062] The low-precision INS carrier body right gyro drift estimation value standard deviation calculated by Kalman filters 1 and 2 is used to obtain the weight of the information fusion of the first set of high-precision INS and the second set of high-precision INS, and the information fusion formula of the two sets of high-precision INS is as follows:

[0063] P ref =K1P1+K2P2

[0064] Wherein, P ref is the position information after the fusion of the two sets of high-precision INS, is the weight of the first set of high-precision INS, P1 is the position information of the first set of high-precision INS at this time of information fusion, is the weight of the second set of high-precision INS, P2 is the position information of the second set of high-precision INS at this time of information fusion;

[0065] Step 6, when the weight is allocated again, only the low-precision INS carrier body right gyro drift estimation value of the Kalman filter, N obtained when the weight is allocated last time, and the low-precision INS carrier body right gyro drift estimation value average obtained when the weight is allocated last time are needed to recursively obtain the standard deviation this time, and then the weight coefficient at this time of information fusion is updated, and the standard deviation recursive formula is as follows:

[0066]

[0067]

[0068] Wherein, ε x,1 (m) is the low-precision INS carrier body right gyro drift estimation value obtained by Kalman filter 1 at current m moment, μ'1 is the recursive update value of μ1, σ1' is the recursive update value of σ1, ε x,2 (m) is the low-precision INS carrier body right gyro drift estimation value obtained by Kalman filter 2 at current m moment, μ'2 is the recursive update value of μ2, and σ2' is the recursive update value of σ2.

[0069] According to the recursively obtained standard deviation, the information fusion of the first set of high-precision INS and the second set of high-precision INS is carried out:

[0070] P′ ref =K′1P′1+K′2P′2

[0071] Wherein, P r ′ ef is the updated position information of the two sets of high-precision INS after this time of information fusion, is the updated weight of the first set of high-precision INS, P1' is the position information of the first set of high-precision INS at this time of information fusion, P2' is the position information of the first set of high-precision INS at this time of information fusion.

[0072] When information fusion is needed again, step 6 is repeated.

[0073] In summary, the present application provides an INS networking information fusion method based on a recursive algorithm. The present application determines the weights of the first set of high-precision INS and the second set of high-precision INS in INS networking by respectively calculating the standard deviation of the right gyro drift estimation value of the low-precision INS carrier system of the Kalman filter, and obtains the position information after the first fusion. When information fusion is needed again, the standard deviation of the right gyro drift estimation value of the low-precision INS carrier system is updated recursively using three parameters, and the updated weight is calculated to update the fused position information. The present application has high concealment and only uses three parameters to recursively calculate the weights of the two sets of high-precision INS in real time during the information fusion process of the two sets of high-precision INS in INS networking.

Claims

1. A method for INS networking information fusion based on a recursive algorithm, characterized in that: Here are the steps: Step 1: Determine the joint error state between the low-precision INS and the first set of high-precision INS, and between the low-precision INS and the second set of high-precision INS; Step 2: Determine the joint state space model consisting of the low-precision INS and the first set of high-precision INS, and the low-precision INS and the second set of high-precision INS; Step 3: In order to perform the Kalman filter algorithm, the two joint state space models in step 2 are discretized respectively; Step 4: Perform Kalman filtering on the discretized joint state space model of the low-precision INS and the first set of high-precision INS, and the low-precision INS and the second set of high-precision INS. The low-precision INS and the first set of high-precision INS constitute the first Kalman filter, and the low-precision INS and the second set of high-precision INS constitute the second Kalman filter. Step 5: After the two sets of Kalman filters are stable, take a period of data to perform the first weight distribution, and calculate the standard deviation σ1 of the low-precision INS carrier system right gyro drift estimation value of the first Kalman filter and the standard deviation σ2 of the low-precision INS carrier system right gyro drift estimation value of the second Kalman filter: Where N is the number of data points of the low-precision INS carrier system right gyro drift estimation value involved in the calculation within this time period; ε x,1 (i) is the estimated right gyro drift of the i-th low-precision INS carrier system obtained by the first Kalman filter during the time period; ε x,2 (i) is the estimated right gyro drift of the i-th low-precision INS onboard system obtained by the second Kalman filter during the time period; μ1 is the average of the N low-precision right gyro drift estimates of the INS onboard system obtained by the first Kalman filter during the time period; μ2 is the average of the N low-precision right gyro drift estimates of the INS onboard system obtained by the second Kalman filter during the time period; The standard deviation of the right gyro drift estimate of the low-precision INS carrier system, calculated after two Kalman filters are stabilized, is used as the evaluation index. The smaller the standard deviation of the right gyro drift estimate of the low-precision INS carrier system, the greater the weight of the corresponding high-precision INS during information fusion. The larger the standard deviation of the right gyro drift estimate of the low-precision INS carrier system, the smaller the weight of the corresponding high-precision INS during information fusion. The weight ratio of the two high-precision INS during information fusion is the inverse ratio of the standard deviation of the right gyro drift estimate of the low-precision INS carrier system calculated by their respective Kalman filters. Step 6: When allocating weights again, the Kalman filter needs to be used to calculate the estimated right gyro drift of a low-precision INS carrier system, N obtained during the previous weight allocation, and the average value of the estimated right gyro drift of the low-precision INS carrier system obtained during the previous weight allocation. The standard deviation of this time is recursively calculated based on the previous standard deviation, and then used to update the weight coefficient for this information fusion. The standard deviation recursive formula is as follows: Among them, ε x,1 (m) is the estimated right gyro drift of the low-precision INS carrier system obtained by the first Kalman filter at the current time m, μ1′ is the recursive update value of μ1, σ1′ is the recursive update value of σ1, ε x,2 (m) is the estimated right gyro drift of the low-precision INS carrier system obtained by the second Kalman filter at the current time m, μ2′ is the recursive update value of μ2, and σ2′ is the recursive update value of σ2; The information fusion of the first set of high-precision INS and the second set of high-precision INS is performed based on the derived standard deviation: P′ ref =K′1P′1+K′2P′2 Among them, P′ ref This is the updated position information after the information fusion of the two sets of high-precision INS. is the weight of the first set of high-precision INS updated, P1′ is the position information of the first set of high-precision INS during this information fusion, is the weight of the updated second set of high-precision INS, and P2′ is the position information of the first set of high-precision INS during this information fusion.

2. The INS network information fusion method based on recursive algorithm according to claim 1 is characterized in that: Step 5 also includes: The standard deviation of the right gyro drift estimate of the low-precision INS carrier system calculated by the first and second Kalman filters is used to obtain the weight when fusing the first and second high-precision INS information. The formula for fusing the two high-precision INS information is as follows: P ref =K1P1+K2P2 Among them, P ref In order to fuse the position information of two sets of high-precision INS, is the weight of the first set of high-precision INS, P1 is the position information of the first set of high-precision INS during this information fusion, is the weight of the second set of high-precision INS, and P2 is the position information of the second set of high-precision INS during this information fusion.

Images