Inertial navigation collaborative method based on rank-deficient constrained cascade filtering

Through the method based on rank-deficiency constraint cascade filtering, the data link ranging and combined navigation filter are used to solve the accuracy problem of inertial navigation under satellite-free conditions, and high-precision correction and stability of inertial navigation are achieved, which is suitable for clusters Vehicles, underwater submarines and UAVs in the formation.

CN114964228BActive Publication Date: 2024-12-03SHANGHAI INST OF ELECTROMECHANICAL ENG
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202210481900.7
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-05-05
Publication Date
2024-12-03
Estimated Expiration
2042-05-05

AI Technical Summary

Technical Problem

Traditional inertial navigation methods are difficult to ensure high accuracy without satellite navigation. Especially in cluster formations, pure inertial navigation errors will accumulate rapidly, and when relies on high-precision node correction, if the satellite navigation is disturbed, the accuracy will decrease and cannot be effectively implemented. .

Method used

Using a method based on rank-deficiency constraint cascade filtering, the distance between nodes is measured through the data link, the rank-deficiency constraint least squares filter and first-order inertial smoothing filter are constructed, inertial error is estimated, and compensation and smoothing are performed, combined with combined navigation filtering The inertial guide closed-loop correction is carried out to realize real-time estimation and correction of inertial guide position and velocity errors.

Benefits of technology

The accuracy of inertial navigation is improved without high-precision nodes, suppressing the divergence of inertial errors, ensuring that high-precision navigation can be maintained when satellite navigation is disturbed, and meeting the navigation needs of motion carriers such as vehicles, underwater submarines and UAVs.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114964228B_ABST
    Figure CN114964228B_ABST
Patent Text Reader

Abstract

The present invention provides an inertial navigation collaboration method based on rank-deficient constrained cascade filtering, comprising the following steps: determining the number of nodes of an aircraft cluster network, measuring the relative distance between each node, and obtaining the inertial navigation error distribution of each node; determining the inertial navigation position error weight and rank-deficient constraint conditions of each node, and constructing a first-level filter of a cascade filter; estimating the inertial navigation error estimation value of each node, and using the inertial navigation error estimation value to compensate for the error of the pure inertial navigation position; constructing a second-level filter of a cascade filter, and smoothing the result of the compensated pure inertial navigation position; constructing a third-level filter of a cascade filter; estimating the inertial navigation position and velocity error estimation value of each node; and using the inertial navigation position and velocity error estimation value to perform inertial navigation closed-loop correction. The method of the present invention can still effectively improve and ensure the accuracy of inertial navigation when satellite navigation is interfered and cannot be used, so as to meet the high-precision navigation requirements of networked vehicles, underwater submersibles, UAVs and other moving carriers.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of inertial navigation cooperation technology, and in particular to an inertial navigation cooperation method based on rank-deficient constrained cascade filtering. Background Art

[0002] With the development of communication and ranging technology, and considering the characteristics of cluster formations such as large number, dispersion, decentralization, self-organization, low cost, and maneuverability, various types of aircraft represented by drones have begun to enter the era of cluster formations.

[0003] When traditional vehicles, underwater submersibles, UAVs and other moving carriers only use pure inertial navigation, since pure inertial navigation has high requirements on the performance of inertial devices and navigation algorithms, and navigation information is generated through integration, positioning errors will accumulate rapidly over time. To solve this problem, the traditional collaborative navigation method proposes that there is a high-precision node in the cluster formation, and the relative distance information of the nodes measured by the data link is used to correct the low-precision nodes of pure inertia using the position information of the high-precision node. The high-precision node relies on inertial / satellite combined navigation. However, when satellite navigation is interfered and cannot be used, the accuracy of the high-precision node decreases, and the inertial navigation accuracy of other nodes in the cluster will also be difficult to guarantee. This method cannot be effectively implemented.

[0004] The patent document with publication number CN107246868B discloses a collaborative navigation and positioning system and a navigation and positioning method, including a sensor data acquisition module, an RGB-D image generation module, a kinematic modeling module, a zero bias correction module, a posture tracking module, a global optimization module and a map storage module. However, the patent document still has the defect of low accuracy. Summary of the invention

[0005] In view of the defects in the prior art, the object of the present invention is to provide an inertial navigation collaboration method based on rank-deficient constrained cascade filtering.

[0006] According to the present invention, an inertial navigation collaboration method based on rank-deficient constrained cascade filtering includes the following steps:

[0007] Step 1: Determine the number of nodes in the aircraft cluster network, measure the relative distance between each of the nodes through the data link, and obtain the inertial navigation error distribution of each of the nodes;

[0008] Step 2: According to the inertial navigation error distribution, determine the inertial navigation position error weight and rank deficiency constraint condition of each node, and construct the first stage filter of the cascade filter through the rank deficiency constrained least squares filter;

[0009] Step 3: using the rank-deficient constrained least squares filter to estimate the inertial navigation error estimate of each node, and using the inertial navigation error estimate to compensate for the error of the pure inertial navigation position;

[0010] Step 4: constructing a second-stage filter of the cascade filter through a first-order inertial smoothing filter, and smoothing the result of the pure inertial navigation position after compensation through the first-order inertial smoothing filter;

[0011] Step 5: Based on the smoothed pure inertial navigation position information, a cascade filter third level filter is constructed by combining the navigation filter;

[0012] Step 6: using the integrated navigation filter to estimate the inertial navigation position and velocity error estimation value of each node;

[0013] Step 7: Perform inertial navigation closed-loop correction using the inertial navigation position and the velocity error estimate.

[0014] Preferably, in step 2, the inertial navigation position error weight, the rank deficiency constraint condition, and the rank deficiency constraint least squares filter all include a measurement equation, the node is a plurality of nodes, and the plurality of nodes are 1, ..., n respectively; the measurement equation is specifically:

[0015] L=HX+Δ,

[0016] In the formula, is the inertial navigation error vector to be estimated, representing the inertial navigation error of each node in the aircraft cluster network in the navigation coordinate system; Respectively represent the latitude, longitude, and altitude of the inertial navigation solution of node i; Indicates the latitude error of the inertial navigation solution of node i; Indicates the longitude error of the inertial navigation solution of node i; represents the inertial navigation solution height error of node i; i represents the node number, i = 1, ..., n; L is dimensional observation matrix; H is dimensional observation coefficient matrix; Δ is dimensional observation noise matrix, It represents the number of combinations of 2 in n.

[0017] Preferably, the observation matrix L is:

[0018]

[0019] In the formula, is the data link measurement distance between node i and node j, in meters, i,j=1,...,n and i≠j, is the position calculated by the inertial navigation of node i Calculate the position with the inertial navigation of node j The distance between is the coordinate of node i, is the coordinate of node j.

[0020] Preferably, the observation coefficient matrix H is:

[0021]

[0022] In the above formula, the coordinate transformation matrix D a for

[0023]

[0024] In the formula, They represent the latitude, longitude, and altitude of the inertial navigation solution of the node numbered i, respectively. n represents the radius of curvature of the earth's circumplex, e represents the curvature of the earth, is the x-axis coordinate of node i, is the y-axis coordinate of node i, is the z-axis coordinate of node i.

[0025] Preferably, in step 2, the rank deficiency constraint condition includes:

[0026] C T W x D a X=0,

[0027] Where X is the inertial navigation error vector to be estimated; D a is the coordinate transformation matrix; C is composed of the matrix H T W x The eigenvectors corresponding to the zero eigenvalues ​​of H are composed of dimensional matrix, C T represents the transpose of C; H T represents the transpose of H; W x is a 3n×3n weight matrix determined according to the accuracy level of each inertial navigation system, as shown in the following formula:

[0028] W x =[Δ1 … Δ n ] T ,

[0029] In the formula, Δ i is the error weighting coefficient of node i, i = 1, 2, …, n.

[0030] Preferably, in step 2, the first stage filtering of the cascade filtering is specifically:

[0031] X=(H T RH+W x CC TW x ) -1 H T RL,

[0032] Where R is the observation noise covariance matrix, (H T RH+W x CC T W x ) is a column full rank matrix, and the inertial navigation error vector X to be estimated of each node is estimated in real time by the least square method of the above formula.

[0033] Preferably, in step 3, compensating the error of the pure inertial navigation position is specifically:

[0034]

[0035]

[0036]

[0037] in, Indicates the pure inertial navigation latitude, longitude, and altitude after compensation of the node numbered i.

[0038] Preferably, in step 4, smoothing the result of the compensated pure inertial navigation position is specifically performed as follows:

[0039] m1=(2*b1-b2*T) / (2*b1+b2*T),

[0040] n0=(2*a1+a2*T) / (2*b1+b2*T),

[0041] n1=(-2*a1+a2*T) / (2*b1+b2*T),

[0042]

[0043]

[0044]

[0045] Where: k = 2, 3....; a1 = 0, a2 = 1, b1 = 1, b2 = 1, T = 0.1; m1, n0, n1 are smoothing coefficients; They represent the smoothed latitude, longitude, and altitude of the node numbered i, respectively.

[0046] Preferably, in step 5, the integrated navigation filter includes a state equation, and the state equation is specifically:

[0047]

[0048] In the formula, Filter state vector X s Each element is the north misalignment angle East misalignment angle Misalignment angle Eastward velocity error δV E , north velocity error δV N , celestial velocity error δV U ; Latitude error δL, longitude error δλ, height error δh; F is the state transfer matrix, W is the system noise;

[0049] The combined navigation filter also includes a measurement equation, which is as follows:

[0050] Z=AX s +V,

[0051] Z is the system observation:

[0052]

[0053] A is the observation matrix:

[0054]

[0055] In the matrix Z and matrix A, L I , I 、h I Respectively represent the latitude, longitude and altitude of the current node; L p , p 、h p Respectively represent the smoothed latitude, longitude, and altitude of the current node; R M is the principal radius of curvature of the Earth’s meridian, R N represents the radius of curvature of the Earth’s angular momentum, and V is the 3×1 observation noise.

[0056] Preferably, in step 7, the closed-loop calibration process is as follows:

[0057] Combined navigation position correction:

[0058] L Z =L I -X s (7)

[0059] λ Z =λ I -X s (8)

[0060] h Z =h I -X s (9)

[0061] Among them, L Z , Z 、h Z They represent the latitude, longitude, and altitude obtained by closed-loop correction of the current node.

[0062] Integrated navigation speed correction:

[0063]

[0064]

[0065]

[0066] in, They represent the eastward velocity, northward velocity, and celestial velocity obtained by closed-loop correction of the current node respectively; They respectively represent the eastward speed, northward speed and celestial speed calculated by the inertial navigation of the current node.

[0067] Compared with the prior art, the present invention has the following beneficial effects:

[0068] 1. The present invention does not require high-precision nodes in the cluster network, but only relies on inertial navigation solution and data link networking distance measurement, designs error weights and rank deficiency constraints, and constructs a cascade filter based on the rank deficiency constraint method;

[0069] 2. The first-level rank-deficient constrained least squares filter of the present invention estimates the inertial navigation position error and compensates the pure inertial navigation position result, solving the measurement rank deficiency problem of multi-node inertial navigation collaboration; the second-level first-order inertial smoothing filter smoothes the compensated pure inertial navigation position result to solve the problem of unstable data link ranging results; the third-level combined navigation filter only uses position measurement to recursively estimate the inertial navigation position error and speed error, and performs inertial navigation closed-loop correction to suppress the inertial navigation error of each node in the cluster network and improve the pure inertial navigation accuracy;

[0070] 3. The method of the present invention can still effectively improve and ensure the accuracy of inertial navigation when satellite navigation is interfered and cannot be used, so as to meet the high-precision navigation requirements of moving carriers such as vehicles, underwater submersibles, and UAVs. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:

[0072] Figure 1 It is a flow chart of the inertial navigation cooperation method based on rank-deficient constrained cascade filtering of the present invention;

[0073] Figure 2This is a process diagram for updating the inertial navigation collaborative method based on rank-deficient constrained cascade filtering of the present invention. DETAILED DESCRIPTION

[0074] The present invention is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several changes and improvements can also be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0075] Embodiment 1:

[0076] like Figure 1 and Figure 2 As shown, this embodiment provides an inertial navigation collaboration method based on rank-deficient constrained cascade filtering, comprising the following steps:

[0077] Step 1: Determine the number of nodes in the aircraft cluster network, measure the relative distance between each node through the data link, and obtain the inertial navigation error distribution of each node.

[0078] Step 2: According to the inertial navigation error distribution, determine the inertial navigation position error weight and rank deficiency constraint conditions of each node, and construct the first-level filter of the cascade filter through the rank deficiency constrained least squares filter;

[0079] The inertial navigation position error weight, the rank deficiency constraint condition, and the rank deficiency constrained least squares filter all include a measurement equation, and the nodes are multiple nodes, and the multiple nodes are 1,…,n respectively. The measurement equation is specifically:

[0080] L=HX+Δ,

[0081] In the formula, is the inertial navigation error vector to be estimated, which represents the inertial navigation error of each node in the aircraft cluster network in the navigation coordinate system; Respectively represent the latitude, longitude, and altitude of the inertial navigation solution of node i; Indicates the latitude error of the inertial navigation solution of node i; Indicates the longitude error of the inertial navigation solution of node i; represents the inertial navigation solution height error of node i; i represents the node number, i = 1, ..., n; L is dimensional observation matrix; H is dimensional observation coefficient matrix; Δ is dimensional observation noise matrix, It represents the number of combinations of taking 2 from n;

[0082] The observation matrix L is:

[0083]

[0084] In the formula, is the data link measurement distance between node i and node j, in meters, i,j=1,...,n and i≠j, is the position calculated by the inertial navigation of node i Calculate the position with the inertial navigation of node j The distance between is the coordinate of node i, is the coordinate of node j;

[0085] The observation coefficient matrix H is:

[0086]

[0087] In the above formula, the coordinate transformation matrix D a for

[0088]

[0089] In the formula, They represent the latitude, longitude, and altitude of the inertial navigation solution of the node numbered i, respectively. n represents the radius of curvature of the earth's circumplex, e represents the curvature of the earth, is the x-axis coordinate of node i, is the y-axis coordinate of node i, is the z-axis coordinate of node i;

[0090] The rank deficiency constraints include:

[0091] C T W x D a X=0,

[0092] Where X is the inertial navigation error vector to be estimated; D a is the coordinate transformation matrix; C is composed of the matrix H T W x The eigenvectors corresponding to the zero eigenvalues ​​of H are composed of dimensional matrix, C T represents the transpose of C; H T represents the transpose of H; W x is a 3n×3n weight matrix determined according to the accuracy level of each inertial navigation system, as shown in the following formula:

[0093] W x =[Δ1 … Δ n ] T ,

[0094] In the formula, Δ i is the error weight coefficient of node i, i = 1, 2, ..., n;

[0095] The first stage of cascade filtering is as follows:

[0096] X=(H T RH+W x CC T W x ) -1 H T RL,

[0097] Where R is the observation noise covariance matrix, (H T RH+W x CC T W x ) is a column full rank matrix, and the inertial navigation error vector X to be estimated at each node is estimated in real time by the least square method in the above formula;

[0098] Step 3: Use the rank-deficient least squares filter to estimate the inertial navigation error estimate of each node, and use the inertial navigation error estimate to compensate for the error of the pure inertial navigation position;

[0099] The error of the pure inertial navigation position compensation is specifically:

[0100]

[0101]

[0102]

[0103] in, Indicates the pure inertial navigation latitude, longitude, and altitude after compensation of the node numbered i.

[0104] Step 4: The second stage of cascade filtering is constructed by a first-order inertial smoothing filter, and the result of the compensated pure inertial navigation position is smoothed by the first-order inertial smoothing filter;

[0105] The specific method for smoothing the compensated pure inertial navigation position is as follows:

[0106] m1=(2*b1-b2*T) / (2*b1+b2*T),

[0107] n0=(2*a1+a2*T) / (2*b1+b2*T),

[0108] n1=(-2*a1+a2*T) / (2*b1+b2*T),

[0109]

[0110]

[0111]

[0112] Where: k = 2, 3....; a1 = 0, a2 = 1, b1 = 1, b2 = 1, T = 0.1; m1, n0, n1 are smoothing coefficients; They represent the smoothed latitude, longitude, and altitude of the node numbered i, respectively.

[0113] Step 5: Based on the smoothed pure inertial navigation position information, the third level of cascade filtering is constructed by combining the navigation filter;

[0114] The integrated navigation filter includes a state equation, which is specifically:

[0115]

[0116] In the formula, Filter state vector X s Each element is the north misalignment angle East misalignment angle Misalignment angle Eastward velocity error δV E , north velocity error δV N , celestial velocity error δV U ; Latitude error δL, longitude error δλ, height error δh; F is the state transfer matrix, W is the system noise;

[0117] The integrated navigation filter also includes a measurement equation, which is as follows:

[0118] Z=AX s +V,

[0119] Z is the system observation:

[0120]

[0121] A is the observation matrix:

[0122]

[0123] In the matrix Z and matrix A, L I , I 、h I Respectively represent the latitude, longitude and altitude of the current node; L p , p 、h p Respectively represent the smoothed latitude, longitude, and altitude of the current node; R M is the principal radius of curvature of the Earth’s meridian, R N represents the radius of curvature of the Earth’s angular momentum, and V is the 3×1 observation noise.

[0124] Step 6: Use the integrated navigation filter to estimate the inertial navigation position and velocity error of each node.

[0125] Step 7: Use the estimated values ​​of the inertial navigation position and velocity errors to perform inertial navigation closed-loop correction. The closed-loop correction process is as follows:

[0126] Combined navigation position correction:

[0127] L Z =L I -X s (7)

[0128] λ Z =λ I -X s (8)

[0129] h Z =h I -X s (9)

[0130] Among them, L Z , Z 、h Z They represent the latitude, longitude, and altitude obtained by closed-loop correction of the current node.

[0131] Integrated navigation speed correction:

[0132]

[0133]

[0134]

[0135] in, They represent the eastward velocity, northward velocity, and celestial velocity obtained by closed-loop correction of the current node respectively; They respectively represent the eastward speed, northward speed and celestial speed calculated by the inertial navigation of the current node.

[0136] Embodiment 2:

[0137] Those skilled in the art may understand this embodiment as a more specific description of Embodiment 1.

[0138] like Figure 1 and Figure 2 As shown, the present embodiment provides an inertial navigation collaboration method based on rank-deficient constrained cascade filtering, comprising the following steps:

[0139] Step 1: Determine the number of nodes in the aircraft cluster network, measure the relative distance between each node through the data link, and obtain the inertial navigation error distribution of each node.

[0140] Step 2: According to the inertial navigation error distribution, determine the inertial navigation position error weight and rank deficiency constraint conditions of each node, and construct the first-level filter of the cascade filter through the rank deficiency constrained least squares filter;

[0141] The inertial navigation position error weight, rank deficiency constraint condition and rank deficiency constrained least squares filter all include measurement equations, with four nodes, namely a, b, c and d. The specific measurement equations are:

[0142] L=HX+Δ,

[0143] In the formula, is the inertial navigation error vector to be estimated, representing the inertial navigation longitude and latitude height error of each node in the aircraft cluster network in the navigation coordinate system; Indicates the latitude error of the inertial navigation solution of node i; Indicates the longitude error of the inertial navigation solution of node i; represents the inertial navigation solution height error of node i; where i represents the node number, i = a, ..., d; L is a 6 × 1 dimensional observation matrix; H is a 6 × 12 dimensional observation coefficient matrix; Δ is a 6 × 1 dimensional observation noise matrix;

[0144] The observation matrix L is:

[0145]

[0146] In the formula, is the data link measurement distance between node i and node j, in meters; i,j=a,…,d and i≠j, the three-axis position calculated by the inertial navigation of node i Calculate the position with the inertial navigation of node j The distance between

[0147] The observation coefficient matrix H is:

[0148]

[0149] In the above formula, D a for

[0150]

[0151] In the formula, They represent the latitude, longitude, and altitude of the inertial navigation solution of the node numbered i, respectively. n represents the radius of curvature of the earth's angular circle, and e represents the curvature of the earth;

[0152] The rank deficiency constraints include:

[0153] C T W x D aX=0,

[0154] Where X is the inertial navigation error vector to be estimated; D a is the coordinate transformation matrix, as shown above; C is composed of the matrix H T W x The 12×6 dimensional matrix consisting of the eigenvectors corresponding to the zero eigenvalues ​​of H, C T represents the transpose of C; H T represents the transpose of H; W x is a 12×12 dimensional weight matrix determined according to the accuracy level of each inertial navigation system, as shown in the following formula:

[0155] W x =diag[Δ a Δ b Δ c Δ d ] 12×12 ,

[0156] In the formula, Δ a , Δ b , Δ c , Δ d are the error weighting coefficients of nodes a to d respectively;

[0157] The first stage of cascade filtering is as follows:

[0158] X=(H T RH+W x CC T W x ) -1 H T RL,

[0159] In the formula, (H T RH+W x CC T W x ) is a column full rank matrix, and the inertial navigation error vector X to be estimated of each node is estimated in real time by the least square method of the above formula.

[0160] Step 3: Use the rank-deficient least squares filter to estimate the inertial navigation error estimate of each node, and use the inertial navigation error estimate to compensate for the error of the pure inertial navigation position;

[0161] The error of the pure inertial navigation position compensation is specifically:

[0162]

[0163]

[0164]

[0165] Step 4: The second stage of cascade filtering is constructed by a first-order inertial smoothing filter, and the result of the compensated pure inertial navigation position is smoothed by the first-order inertial smoothing filter;

[0166] The specific method for smoothing the compensated pure inertial navigation position is as follows:

[0167] m1=(2*b1-b2*T) / (2*b1+b2*T),

[0168] n0=(2*a1+a2*T) / (2*b1+b2*T),

[0169] n1=(-2*a1+a2*T) / (2*b1+b2*T),

[0170]

[0171]

[0172]

[0173] Where: k = 2, 3....; a1 = 0, a2 = 1, b1 = 1, b2 = 1, T = 0.1; m1, n0, n1 are smoothing coefficients; are the smoothed latitude, longitude, and altitude, respectively.

[0174] Step 5: Based on the smoothed pure inertial navigation position information, the third level of cascade filtering is constructed by combining the navigation filter;

[0175] The integrated navigation filter includes a state equation, which is specifically:

[0176]

[0177] In the formula, Filter state vector X s Each element is the north misalignment angle East misalignment angle Misalignment angle Eastward velocity error δV E , north velocity error δV N , celestial velocity error δV U ; Latitude error δL, longitude error δλ, height error δh; F is the state transfer matrix, W is the system noise;

[0178] The integrated navigation filter also includes a measurement equation, which is as follows:

[0179] Z=AX s +V,

[0180] Z is the system observation:

[0181]

[0182] A is the observation matrix:

[0183]

[0184] In the matrix Z and matrix A, L I , I 、h I Respectively represent the latitude, longitude and altitude of the current node; L p , p 、h p Respectively represent the smoothed latitude, longitude, and altitude of the current node; R M is the principal radius of curvature of the Earth’s meridian, R N represents the radius of curvature of the Earth’s angular momentum, and V is the 3×1 observation noise.

[0185] Step 6: Use the integrated navigation filter to estimate the inertial navigation position and velocity error of each node.

[0186] Step 7: Use the estimated values ​​of the inertial navigation position and velocity errors to perform inertial navigation closed-loop correction. The closed-loop correction process is as follows:

[0187] Combined navigation position correction:

[0188] L Z =L I -X s (7)

[0189] λ Z =λ I -X s (8)

[0190] h Z =h I -X s (9)

[0191] Among them, L Z , Z 、h Z They respectively represent the latitude, longitude and altitude of the current node after closed-loop correction.

[0192] Integrated navigation speed correction:

[0193]

[0194]

[0195]

[0196] in, They represent the eastward velocity, northward velocity and celestial velocity of the current node after closed-loop correction respectively; They respectively represent the eastward velocity, northward velocity and celestial velocity calculated by the inertial navigation of the current node.

[0197] Embodiment 3:

[0198] Those skilled in the art may understand this embodiment as a more specific description of Embodiment 1.

[0199] like Figure 1 and Figure 2 As shown, this embodiment provides an inertial navigation collaboration method based on rank-deficient constrained cascade filtering, comprising the following steps:

[0200] Step S1: Determine the number of nodes in the aircraft cluster network and measure the relative distance between each node through the data link;

[0201] Step S2: According to the inertial navigation error distribution of each node, determine the inertial navigation position error weight and rank deficiency constraint condition of each node in the cluster network, and construct the first level filter of the cascade filter: rank deficiency constrained least squares filter;

[0202] Step S3: using the estimated value of the inertial navigation error of each node estimated by the rank-deficient least squares filter to compensate the pure inertial navigation position error;

[0203] Step S4: construct the second level filter of the cascade filter: a first-order inertial smoothing filter to smooth the compensated pure inertial navigation position result;

[0204] Step S5: constructing the third level filter of the cascade filter based on the smoothed pure inertial navigation position information: a combined navigation filter;

[0205] Step S6: using the integrated navigation filter to estimate the inertial navigation position and velocity error estimation value of each node;

[0206] Step S7: Perform inertial navigation closed-loop correction using the inertial navigation position and velocity error estimates obtained in step S6.

[0207] The error weight, rank deficiency constraint and rank deficiency constrained least squares filter in step S2 include a measurement equation. Taking four nodes as an example, the measurement equation is specifically:

[0208] L=HX+Δ,

[0209] In the formula, is the inertial navigation error vector to be estimated, representing the inertial navigation longitude and latitude height error of each node in the aircraft cluster network in the navigation coordinate system; L is a 6×1 dimensional observation matrix; H is a 6×12 dimensional observation coefficient matrix; Δ is a 6×1 dimensional observation noise matrix;

[0210] Among them, the observed quantity is:

[0211]

[0212] In the formula, The data link measurement distance between node i and node j, in meters.

[0213] Measurement matrix:

[0214]

[0215] In the above formula, D a for

[0216]

[0217] In the formula, R n represents the radius of curvature of the earth's ecliptic, and e represents the curvature of the earth.

[0218] The rank deficiency constraints in step S2 include:

[0219] C T W x D a X=0,

[0220] Where X is the inertial navigation error vector to be estimated; D a is the coordinate transformation matrix, as shown above; C is composed of the matrix H T W x The 12×6 dimensional matrix consisting of the eigenvectors corresponding to the zero eigenvalues ​​of H, C T represents the transpose of C; H T represents the transpose of H; W x is a 12×12 dimensional weight matrix determined according to the accuracy level of each inertial navigation system, as shown in the following formula:

[0221] W x =diag[Δ a Δ b Δ c Δ d ] 12×12 ,

[0222] Δ a , Δ b , Δ c , Δ d are the error weighting coefficients of nodes a to d respectively.

[0223] The error weight, rank deficiency constraint condition and the first-stage rank deficiency constrained least squares filter of the cascaded filter in step S2 include the first-stage rank deficiency constrained least squares filter of the cascaded filter, and the rank deficiency constrained least squares filter is specifically:

[0224] X=(H T RH+W x CC T W x ) -1 H T RL,

[0225] Where R is the observation noise covariance matrix, H is the observation coefficient matrix, L is the observation matrix, and the weight matrix W is x With the matrix C T Same as above; (H T RH+W x CC T W x ) is a column full rank matrix, and the inertial navigation error vector X to be estimated of each node is estimated in real time by the least square method of the above formula.

[0226] Step S3 uses the estimated value X of the inertial navigation error of each node estimated by the rank-deficient least squares filter to compensate the pure inertial navigation position error as follows:

[0227]

[0228]

[0229]

[0230] The method of using the second-stage first-order inertial smoothing filter of the cascade filter to process the pure inertial navigation position compensation result in step S4 is as follows:

[0231] m1=(2*b1-b2*T) / (2*b1+b2*T),

[0232] n0=(2*a1+a2*T) / (2*b1+b2*T),

[0233] n1=(-2*a1+a2*T) / (2*b1+b2*T),

[0234]

[0235]

[0236]

[0237] Where: k = 2, 3....; a1 = 0, a2 = 1, b1 = 1, b2 = 1, T = 0.1; m1, n0, n1 are smoothing coefficients; are the smoothed latitude, longitude, and altitude, respectively.

[0238] Step S5 constructs a cascade filtering third-stage filtering combined navigation filter based on the smoothed pure inertial navigation position information. The combined navigation filter includes a state equation, which is as follows:

[0239]

[0240] In the formula, F is the state transfer matrix, W is the system noise;

[0241] F 9×9 The non-zero elements of the array are as follows:

[0242]

[0243] F 27 =-ω ie sin L I ;

[0244]

[0245] F 42 =-f u , F 43 =f N ,

[0246] F 51 =f u , F 53 =-f E ,

[0247] F 61 =-f N , F 62 =f E ,

[0248]

[0249]

[0250] F 96 =1;

[0251] Where: R M =R e (1-2f+3f sin 2 L I ), R N =R e (1+f sin 2 L I ), Re =6378137m, f=1 / 298.257, ω ie =0.000072915rad / s,

[0252] Add table output increment for this node, T INS is the inertial measurement output cycle, where position, velocity and attitude are the inertial navigation results of this node.

[0253] The combined navigation equation also includes the measurement equation. The measurement equation of a node in the cluster network is as follows:

[0254] Z=AX s +V,

[0255] Z is the system observation:

[0256]

[0257] A is the observation matrix:

[0258]

[0259] Step S7: The integrated navigation solution result is used for integrated navigation closed-loop correction. The process of pure inertial navigation position and velocity closed-loop correction is as follows:

[0260] Combined navigation position correction:

[0261] L Z =L I -X s (7)

[0262] λ Z =λ I -X s (8)

[0263] h Z =h I -X s (9)

[0264] Integrated navigation speed correction:

[0265]

[0266]

[0267]

[0268] This embodiment provides a collaborative navigation method based on rank-deficit constrained cascade filtering. To solve the long-term accumulation problem of inertial navigation error, the network distance measurement between nodes in the cluster is used, and the error weight matrix and rank-deficit constraint conditions are designed to construct a cascade filtering method. The first-level rank-deficit constrained least squares filter of the cascade filtering estimates the inertial navigation error and compensates the pure inertial navigation position result; the second-level first-order inertial smoothing filter of the cascade filtering smoothes the compensated pure inertial navigation position result; the third-level combined navigation filter of the cascade filtering estimates the position error and velocity error of the inertial navigation using only position measurement, and performs closed-loop correction to suppress the divergence of the pure inertial navigation error.

[0269] Embodiment 4:

[0270] Those skilled in the art may understand this embodiment as a more specific description of Embodiment 1.

[0271] like Figure 1 and Figure 2 As shown, in this embodiment, a collaborative navigation method based on rank-deficient constrained cascade filtering is provided, comprising the following steps:

[0272] Firstly, the nodes involved in the networking are determined according to the geometric configuration, and the relative distances of the nodes in the cluster network are measured through data link and UWB, which are used as the measured distances between the nodes in the rank-deficient constraint filter for ranging networking.

[0273] Secondly, according to the inertial navigation error distribution of each node, the inertial navigation position error weight and rank deficiency constraint of each node in the cluster network are determined, and the first-level rank deficiency constrained least squares filter of the cascade filter is constructed, including the measurement equation, rank deficiency constraint condition and rank deficiency constraint filter equation. Among them, the measurement equation taking four nodes as an example is:

[0274] L=HX+Δ,

[0275] In the formula, is the inertial navigation error vector to be estimated, representing the inertial navigation longitude and latitude height error of each node in the aircraft cluster network in the navigation coordinate system; L is a 6×1 dimensional observation matrix; H is a 6×12 dimensional observation coefficient matrix; Δ is a 6×1 dimensional observation noise matrix;

[0276] Among them, the observed quantity is:

[0277]

[0278] In the formula, The data link measurement distance between node i and node j, in meters.

[0279] Measurement matrix:

[0280]

[0281] In the above formula, D a for

[0282]

[0283] In the formula, R n represents the radius of curvature of the earth's ecliptic, and e represents the curvature of the earth.

[0284] The specific rank deficiency constraint conditions are:

[0285] C T W x D a X=0,

[0286] Where X is the inertial navigation error vector to be estimated; D a is the coordinate transformation matrix, as shown above; C is composed of the matrix H T W x The 12×6 dimensional matrix consisting of the eigenvectors corresponding to the zero eigenvalues ​​of H, C T represents the transpose of C; H T represents the transpose of H; W x is a 12×12 dimensional weight matrix determined according to the accuracy level of each inertial navigation system, as shown in the following formula:

[0287] W x =diag[Δ a Δ b Δ c Δ d ] 12×12 ,

[0288] The rank deficiency constraint filtering equation is specifically:

[0289] X=(H T RH+W x CC T W x ) -1 H T RL,

[0290] Where R is the observation noise covariance matrix, H is the observation coefficient matrix, L is the observation matrix, and the weight matrix W is x With the matrix C T Same as above; (H T RH+W x CC T W x ) is a column full rank matrix, and the inertial navigation error vector X to be estimated of each node is estimated in real time by the least square method of the above formula.

[0291] Subsequently, the pure inertial navigation error is compensated by using the estimated value of the inertial navigation error of each node estimated by rank-deficient least squares filtering.

[0292] Specifically, in this embodiment, the method for compensating the pure inertial navigation error of the estimated value of the inertial navigation position error of each node in the cluster network is as follows:

[0293]

[0294]

[0295]

[0296] The second-stage first-order inertial filter smoothing method of cascade filtering is used to smooth the compensated pure inertial navigation position results.

[0297] Specifically, in this embodiment, the method of smoothing the compensated pure inertial navigation position result using the first-order inertial smoothing filtering method is:

[0298] m1=(2*b1-b2*T) / (2*b1+b2*T),

[0299] n0=(2*a1+a2*T) / (2*b1+b2*T),

[0300] n1=(-2*a1+a2*T) / (2*b1+b2*T),

[0301]

[0302]

[0303]

[0304] Where: k = 2, 3....; a1 = 0, a2 = 1, b1 = 1, b2 = 1, T = 0.1; m1, n0, n1 are smoothing coefficients; are the smoothed latitude, longitude, and altitude, respectively.

[0305] Based on the smoothed pure inertial navigation position information, a cascade filtering third-level combined navigation filter is constructed, including state equations and measurement equations.

[0306] Specifically, in this embodiment, the state equation of the third-stage combined navigation filter of the cascade filtering is:

[0307]

[0308] In the formula, F is the state transfer matrix, and W is the system noise.

[0309] F 9×9 The non-zero elements of the array are as follows:

[0310]

[0311] F 27 =-ω ie sI I ;

[0312]

[0313] F 42 =-f u , F 43 =f N ,

[0314] F 51 =f u , F 53 =-f E ,

[0315] F 61 =-f N ,

[0316]

[0317]

[0318] F 96 =1;

[0319] Where: R M =R e (1-2f+3f sin 2 L I ), R N =R e (1+f sin 2 L I ), R e =6378137m, f=1 / 298.257, ω ie =0.000072915rad / s.

[0320] Add table output increment for this node, T INS is the inertial measurement output cycle, where position, velocity and attitude are the inertial navigation results of this node.

[0321] Specifically, in this embodiment, the measurement equation of the third-stage combined navigation filter of the cascade filtering is:

[0322] Z=AXs +V,

[0323] Z is the system observation:

[0324]

[0325] A is the observation matrix:

[0326]

[0327] Specifically, in the embodiment of the present invention, the method for closed-loop correction of pure inertial navigation position and velocity error by the estimated values ​​of inertial navigation position and velocity error of each node in the cluster network is as follows:

[0328] Combined navigation position correction:

[0329] L Z =L I -X s (7)

[0330] λ Z =λ I -X s (8)

[0331] h Z =h I -X s (9)

[0332] Integrated navigation speed correction:

[0333]

[0334]

[0335]

[0336] This embodiment provides an inertial navigation collaboration method based on rank-deficit constrained cascade filtering, which only relies on inertial navigation and data link ranging equipment, designs an inertial navigation error weight matrix and a networking rank-deficit constraint condition, establishes a rank-deficit constrained cascade filtering method, uses a first-level rank-deficit constrained least squares filter to obtain an inertial navigation position error estimate and compensates for a pure inertial navigation position result, uses a second-level first-order inertial smoothing filter to smooth the compensated pure inertial navigation position result, constructs a third-level combined navigation filter based on pure inertial navigation position information, delivers the inertial navigation position and velocity error estimates of each node in real time, and performs closed-loop correction on the position and velocity solved by the pure inertial navigation, suppresses the divergence of the pure inertial navigation error of each node, and realizes high-precision navigation of each node in the cluster network.

[0337] In the absence of high-precision nodes in the cluster network, the present invention only relies on pure inertial navigation solution and data link networking ranging, designs error weights and rank deficiency constraints, constructs a three-stage cascade filter to estimate the pure inertial navigation position and velocity errors and performs closed-loop correction, effectively suppresses the divergence of the pure inertial navigation position error and velocity error of each node, and obtains the real-time optimal estimation of the position and velocity information of each node in the cluster network.

[0338] In the description of the present application, it should be understood that the terms "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc., indicating orientations or positional relationships, are based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present application and simplifying the description, and do not indicate or imply that the referred device or element must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as a limitation on the present application.

[0339] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. In the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. An inertial navigation collaborative method based on rank-deficient constrained cascade filtering, characterized in that: The steps include: Step 1: Determine the number of nodes in the aircraft cluster network, measure the relative distance between each of the nodes through the data link, and obtain the inertial navigation error distribution of each of the nodes; Step 2: According to the inertial navigation error distribution, determine the inertial navigation position error weight and rank deficiency constraint condition of each node, and construct the first stage filter of the cascade filter through the rank deficiency constrained least squares filter; Step 3: using the rank-deficient constrained least squares filter to estimate the inertial navigation error estimate of each node, and using the inertial navigation error estimate to compensate for the error of the pure inertial navigation position; Step 4: constructing a second-stage filter of the cascade filter through a first-order inertial smoothing filter, and smoothing the result of the pure inertial navigation position after compensation through the first-order inertial smoothing filter; Step 5: Based on the smoothed pure inertial navigation position information, a cascade filter third level filter is constructed by combining the navigation filter; Step 6: using the integrated navigation filter to estimate the inertial navigation position and velocity error estimation value of each node; Step 7: Performing inertial navigation closed-loop correction using the inertial navigation position and the velocity error estimate; In the step 2, the inertial navigation position error weight, the rank deficiency constraint condition, and the rank deficiency constraint least squares filter all include a measurement equation, the node is a plurality of nodes, and the plurality of nodes are 1, ..., n respectively; the measurement equation is specifically: L=HX+Δ, In the formula, is the inertial navigation error vector to be estimated, representing the inertial navigation error of each node in the aircraft cluster network in the navigation coordinate system; Respectively represent the latitude, longitude, and altitude of the inertial navigation solution of node i; Indicates the latitude error of the inertial navigation solution of node i; Indicates the longitude error of the inertial navigation solution of node i; represents the inertial navigation solution height error of node i; i represents the node number, i = 1, ..., n; L is dimensional observation matrix; H is dimensional observation coefficient matrix; Δ is dimensional observation noise matrix, It represents the number of combinations of taking 2 from n; The observation matrix L is: In the formula, is the data link measurement distance between node i and node j, in meters, i,j=1,...,n and i≠j, is the position calculated by the inertial navigation of node i Calculate the position with the inertial navigation of node j The distance between is the coordinate of node i, is the coordinate of node j; The observation coefficient matrix H is: In the above formula, the coordinate transformation matrix D a for In the formula, They represent the latitude, longitude, and altitude of the inertial navigation solution of the node numbered i, respectively. n represents the radius of curvature of the earth's circumplex, e represents the curvature of the earth, is the x-axis coordinate of node i, is the y-axis coordinate of node i, is the z-axis coordinate of node i; In step 2, the rank deficiency constraint condition includes: C T W x D a X=0, Where X is the inertial navigation error vector to be estimated; D a is the coordinate transformation matrix; C is composed of the matrix H T W x The eigenvectors corresponding to the zero eigenvalues ​​of H are composed of dimensional matrix, C T represents the transpose of C; H T represents the transpose of H; W x is a 3n×3n weight matrix determined according to the accuracy level of each inertial navigation system, as shown in the following formula: W x =[Δ1…Δ n ] T , In the formula, Δ i is the error weight coefficient of node i, i = 1, 2, ..., n; In step 2, the first stage of filtering of the cascade filtering is specifically: X=(H T RH+W x CC T W x ) -1 H T RL, Where R is the observation noise covariance matrix, (H T RH+W x CC T W x ) is a column full rank matrix, and the inertial navigation error vector X to be estimated of each node is estimated in real time by the least square method of the above formula.

2. The inertial navigation collaborative method based on rank-deficient constrained cascade filtering according to claim 1, characterized in that: In step 3, the error of the pure inertial navigation position is compensated as follows: in, Indicates the pure inertial navigation latitude, longitude, and altitude after compensation of the node numbered i.

3. The inertial navigation collaborative method based on rank-deficient constrained cascade filtering according to claim 2, characterized in that: In step 4, smoothing the result of the pure inertial navigation position after compensation is specifically performed as follows: m1=(2*b1-b2*T) / (2*b1+b2*T), n0=(2*a1+a2*T) / (2*b1+b2*T), n1=(-2*a1+a2*T) / (2*b1+b2*T), Where: k = 2, 3....; a1 = 0, a2 = 1, b1 = 1, b2 = 1, T = 0.1; m1, n0, n1 are smoothing coefficients; They represent the smoothed latitude, longitude, and altitude of the node numbered i, respectively.

4. The inertial navigation collaborative method based on rank-deficient constrained cascade filtering according to claim 3, characterized in that: In step 5, the integrated navigation filter includes a state equation, and the state equation is specifically: In the formula, Filter state vector X s Each element is the north misalignment angle East misalignment angle Misalignment angle Eastward velocity error δV E , north velocity error δV N , celestial velocity error δV U ; Latitude error δL, longitude error δλ, height error δh; F is the state transfer matrix, W is the system noise; The combined navigation filter also includes a measurement equation, which is as follows: Z=AX s +V, Z is the system observation: A is the observation matrix: In the matrix Z and matrix A, L I , I 、h I Respectively represent the latitude, longitude and altitude of the current node; L p , p 、h p Respectively represent the smoothed latitude, longitude, and altitude of the current node; R M is the principal radius of curvature of the Earth’s meridian, R N represents the radius of curvature of the Earth’s angular momentum, and V is the 3×1 observation noise.

5. The inertial navigation collaborative method based on rank-deficient constrained cascade filtering according to claim 4, characterized in that: In step 7, the closed-loop calibration process is as follows: Combined navigation position correction: L Z =L I -X s (7), l Z =λ I -X s (8), h Z =h I -X s (9), Among them, L Z , Z 、h Z Respectively represent the latitude, longitude, and altitude obtained by closed-loop correction of the current node; Integrated navigation speed correction: in, They represent the eastward velocity, northward velocity, and celestial velocity obtained by closed-loop correction of the current node respectively; They respectively represent the eastward speed, northward speed and celestial speed calculated by the inertial navigation of the current node.