Distributed inertia-based relative navigation method based on data chain

Abstract

The invention provides a distributed inertia-based relative navigation method based on a data link, and relates to the technical field of navigation. Firstly, a relative navigation error state equation based on speed error reconstruction is adopted, and a specific force term of high-frequency change is eliminated, so that the degree of dependence on inertial navigation data sharing between nodes is reduced; secondly, by designing and calculating an efficient information fusion method, the calculation complexity of the whole method is reduced; and finally, designing equivalent GNSS measurement for rapidly diffusing high-precision absolute navigation information. According to the invention, two core functions of accurate estimation of the relative state of the cluster node and rapid coverage of absolute information can be realized; through a simulation experiment, high-precision group inertial navigation error state propagation can be realized by sharing own inertial navigation data by cluster nodes at second-level intervals.

Description

Technical Field

[0001] The present invention belongs to the field of navigation technology, and in particular relates to a distributed inertial base relative navigation method based on a data link. Background Art

[0002] Relative navigation technology provides cluster nodes with accurate and reliable local spatiotemporal information (relative positioning and timing information), a prerequisite and important guarantee for the successful completion of tasks such as coordinated detection, coordinated capture, and coordinated strike. Currently, most cluster systems rely on the Global Navigation Satellite System (GNSS) to obtain precise absolute navigation information and achieve relative navigation through the distribution and sharing of this absolute navigation information. However, in complex environments such as indoors, in cities, and in forests, GNSS signals may be completely or intermittently lost due to obstructions such as tall buildings and trees, as well as atmospheric refraction and multipath effects, making this method unable to achieve stable relative navigation. Data links can achieve precise relative clock and relative distance measurements by measuring the round-trip time (RTT) and time of arrival (TOA) of the signal, making them an effective alternative to relative navigation sensors in GNSS denial or intermittent denial scenarios. The data link-based relative navigation method utilizes the data link time synchronization and relative ranging functions, combined with autonomous navigation sensors such as the Inertial Navigation System (INS) and altimeter, to achieve relative positioning and timing of cluster nodes, becoming an effective relative navigation method in GNSS-denied or intermittently denied environments.

[0003] In order to improve the relative navigation accuracy, predecessors proposed a relative navigation method based on measurement sharing. In this method, each node in the cluster will share the TOA measurement generated by itself with other nodes in the network, so that they can also use these measurements for information fusion. This method increases the number and types of TOA measurements that each node can obtain, effectively improving the observability and estimation accuracy of the relative navigation state. At the same time, in order to accurately fuse the TOA measurements from different nodes, each node in this method establishes a global inertial navigation state model and uses an extended Kalman filter (EKF) for information fusion to accurately maintain the correlation between the states of different nodes brought by the fused TOA measurements, ensuring the accuracy and reliability of information fusion. However, this method still faces three major problems: (1) In order to accurately model the inertial navigation error state of other nodes, each node needs to obtain the specific force information of other nodes in real time. The specific force output frequency of the current inertial sensor is usually 100Hz, and in practice, the data link is difficult to support such a high data transmission requirement. (2) Due to the modeling of the global inertial navigation error state, the computational complexity of the traditional EKF will increase exponentially with the increase of the cluster size. (3) This method only considers the relative navigation scenario in the GNSS denial state, and does not consider how to quickly spread high-precision absolute navigation information to the entire cluster through relative navigation when GNSS is available on some nodes. To address the above problems, we propose a distributed inertial-based relative navigation method based on data link to achieve accurate relative state estimation and rapid coverage of absolute information. Summary of the Invention

[0004] In order to solve the problems that the existing high-precision modeling of data link inertial-based relative navigation based on global modeling relies on high-frequency inertial navigation information sharing, has excessive computational complexity, and does not consider the available GNSS scenarios, the present invention provides a distributed inertial-based relative navigation method based on data link. This method can not only achieve accurate cluster relative navigation in GNSS-denied scenarios, but also quickly disseminate high-precision absolute information of clusters in scenarios where GNSS is available for some nodes, and has good versatility.

[0005] The present invention is implemented by the following technical solution: a distributed inertial base relative navigation method based on data link: the method specifically comprises the following steps:

[0006] Step 1: Establish the state equation of the relative navigation system for the nodes in the cluster;

[0007] Step 2: Based on the equivalent GNSS measurement of the rapidly diffused high-precision absolute navigation information, establish the measurement equation of the relative navigation system for each node in the cluster;

[0008] Step 3: Based on the time division multiple access round-robin broadcast communication mechanism, each node in the cluster shares the information data packet;

[0009] Step 4: Construct the time update equation and the measurement update equation to perform filter time update and measurement update;

[0010] Step 5: Use the state error estimated by the filter as feedback to correct the inertial navigation output and obtain the absolute state estimation information. The absolute state estimation information is then used to calculate the relative state estimation information of all members in the relative coordinate system to obtain the final navigation result.

[0011] Furthermore, in step 1,

[0012] Step 1.1, select the state quantity of node i modeling, the navigation state x of the node i It consists of its own navigation state and the navigation state of other modeled nodes. The specific form is as follows:

[0013]

[0014] in, is the navigation error state of node i itself, Indicates the state of the jth node among the n nodes modeled by node i except itself (1≤j≤n);

[0015] Step 1.2: Select the noise amount of node i, including its own noise and other node noise:

[0016]

[0017] in, is the noise amount of node i itself, represents the noise amount of the jth node among the n nodes modeled by node i except itself (1≤j≤n);

[0018] Step 1.3, define the velocity error as:

[0019]

[0020] Among them, δV i =[δV E,i δV N,i δV U,i ] T , represents the direction cosine matrix calculated from the true value of the node i trajectory, represents the direction cosine matrix calculated from the inertial guidance value of node i, is the northeastern sky velocity output by the inertial navigation value of node i, V i is the northeastern sky speed of the true value output of node i;

[0021] Step 1.4: Based on the state quantity and noise quantity modeled at node i, establish the error propagation equation and construct the state equation of node i.

[0022] Furthermore, in step 2,

[0023] Step 2.1: Construct an altitude measurement equation and use the Schmidt filter method to divide the overall node state into two parts: altitude state and other states. The altitude measurement only updates the state estimate and covariance corresponding to the altitude without affecting other state estimates.

[0024] z BA =h+w BA =h SINS -δh+w BA

[0025] Among them, h is the true height, h SINS is the inertial navigation indicated altitude, δh is the inertial navigation altitude error, w BA To measure noise;

[0026] Step 2.2: Construct the GNSS pseudorange measurement equation of node i as follows:

[0027]

[0028] in, are the positions of the satellite and node i in the earth coordinate system, δt i,GNSS is the clock error of node i, and ξ is the ranging noise of GNSS.

[0029] Furthermore, step 2 also includes:

[0030] Step 2.3: When the GNSS of some nodes in the cluster is available, an equivalent GNSS pseudorange measurement equation is constructed. The nodes that are unavailable use the equivalent GNSS measurement model to fuse the absolute measurement information.

[0031] Step 2.4, RTT clock synchronization measurement: measure the relative clock difference between nodes through the signal round trip time;

[0032] Step 2.5, direct TOA ranging measurement: the relative distance between nodes is measured by signal arrival time;

[0033] Step 2.6, shared TOA ranging measurement: When a node needs to use the TOA measurement information between other nodes, the shared TOA measurement model is used to obtain the information.

[0034] Furthermore, in step 3, nodes take turns broadcasting data packets at fixed time slots;

[0035] The contents of the data packet include: its own member ID, status sharing part, equivalent GNSS judgment conditions and measurement sharing part

[0036] The state sharing part includes the position, speed, attitude estimation of its own INS, the optimal estimation of its own position and celestial speed, and the covariance of altitude estimation;

[0037] The equivalent GNSS judgment condition is to obtain GNSS time;

[0038] The measurement sharing part includes another member ID related to the shared TOA, the shared TOA measurement, the INS position estimation of both parties at the time of shared TOA generation, and the optimal altitude estimation.

[0039] Furthermore, step 4 includes:

[0040] Step 4.1: Update the state equation constructed in step 1 and predict the state and covariance at the next moment;

[0041] In step 4.2, the measurement is updated according to the measurement equation constructed in step 2. Each node performs corresponding measurement updates when receiving altimeter measurement, GNSS measurement, equivalent GNSS measurement, RTT measurement, TOA measurement, and shared TOA measurement.

[0042] Furthermore, in step 5,

[0043] In step 5.1, the node feeds back the filter estimation error to the inertial navigation system to correct the absolute state information output by it.

[0044] In step 5.2, the node uses its own corrected absolute state information and combines it with the estimation errors of other nodes to calculate the relative state information of all nodes, and finally obtains the effective relative information of the relative navigation output.

[0045] A distributed inertial-based relative navigation system based on data link:

[0046] The navigation system includes a state modeling module, a measurement fusion module, an information sharing module, an information updating module and a correction navigation module;

[0047] The state modeling module establishes state equations of a relative navigation system for nodes in the cluster;

[0048] The measurement fusion module establishes the measurement equation of the relative navigation system for each node in the cluster based on the equivalent GNSS measurement of the rapidly diffused high-precision absolute navigation information;

[0049] The information sharing module is based on a time division multiple access round-robin broadcast communication mechanism, and each node in the cluster shares the information data packet;

[0050] The information update module is used to construct a time update equation and a measurement update equation to perform filter time update and measurement update;

[0051] The correction navigation module uses the state error estimated by the filter as feedback, corrects the inertial navigation output, obtains absolute state estimation information, and uses the absolute state estimation information to calculate the relative state estimation information of all members in the relative coordinate system to obtain the final navigation result.

[0052] An electronic device includes a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of the above method when executing the computer program.

[0053] A computer-readable storage medium is used to store computer instructions, which implement the steps of the above method when executed by a processor.

[0054] Beneficial effects of the present invention

[0055] The present invention first adopts a relative navigation error state equation based on velocity error reconstruction, eliminating the high-frequency varying specific force term, thereby reducing the reliance on inertial navigation data sharing between nodes. Secondly, by designing a computationally efficient information fusion method, the computational complexity of the overall method is reduced. Finally, an equivalent GNSS measurement is designed for the rapid dissemination of high-precision absolute navigation information.

[0056] This invention achieves two core functions: accurate estimation of the relative states of cluster nodes and rapid coverage of absolute information. Simulation experiments demonstrate that cluster nodes only need to share their inertial navigation data at intervals of seconds to achieve high-precision group inertial navigation error state propagation. Furthermore, compared to traditional global modeling solutions, this framework reduces computational complexity by an order of magnitude.

[0057] In addition, this method can not only achieve accurate cluster relative navigation in GNSS-denied scenarios, but also quickly disseminate high-precision absolute information of clusters in scenarios where GNSS is available for some nodes, and has good versatility. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 It is a cluster operation trajectory diagram of the simulation of the present invention;

[0059] Figure 2 is a flow chart of the relative navigation algorithm of the present invention;

[0060] Figure 3 is a schematic diagram of error correction of the present invention;

[0061] Figure 4 is the position error curve of relative navigation;

[0062] Figure 5 It is the distance error curve of relative navigation;

[0063] Figure 6 It is the horizontal absolute error curve of relative navigation. DETAILED DESCRIPTION

[0064] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0065] The experimental methods used in the following examples are conventional methods unless otherwise specified. The materials, reagents, methods, and instruments used are conventional in the art and can be obtained commercially by those skilled in the art unless otherwise specified.

[0066] A distributed inertial-based relative navigation method based on data link:

[0067] (1) First, generate the simulated aircraft cluster motion trajectory, such as Figure 1 The simulation shown generates the motion trajectory of a UAV cluster consisting of three formations: a straight formation, a dense formation, and a loose formation. The position, velocity, and attitude information of the UAVs are imported into MATLAB as the real state.

[0068] (2) Build a relative navigation experimental environment, and set the sensor parameters as follows: inertial navigation frequency 100 Hz, gyro constant bias 0.01° / h, gyro angle random walk Add constant bias 50μg, add speed random walk The TOA pseudorange standard deviation is 20m, the RTT standard deviation is 30ns (9m), the altimeter standard deviation is 10m, the initial clock synchronization phase error is 10ns (3m), the initial clock synchronization frequency error is 0.09m / s, the initial position error is [10m, 10m, 10m], the initial velocity error is [0.1m / s, 0.1m / s, 0.1m / s], the initial attitude error is [0.003°, 0.003°, 0.05°], and the communication interval is 0.3s / total number of nodes.

[0069] (3) Deploy the core relative navigation algorithm and verify the accuracy of the results.

[0070] like Figure 2 The following is a flowchart of the relative navigation core algorithm, taking node i as an example. The specific steps are as follows:

[0071] Step 1: Each node in the cluster establishes a state equation for the designed relative navigation system. Compared to traditional inertial navigation error state equations, this equation does not contain high-frequency force terms, reducing the need for nodes to share inertial navigation data.

[0072] Step 1 is as follows:

[0073] (1.1) Filter state: Select the state quantity of node i modeling, including its own state and the state of other nodes:

[0074] Assume that there are n+1 nodes in the cluster. Take node i as an example. The navigation state of the node is x i It consists of its own navigation state and the navigation state of other modeled nodes. The specific form is as follows:

[0075]

[0076] in, is the navigation error state of node i itself, Represents the state of the jth node among the n nodes modeled by node i except itself (1≤j≤n).

[0077] (1.1.1) The self-state is defined as follows:

[0078] Node i's own navigation error state Taken as: SINS latitude and longitude height error [δλ i δL i δh i ] T ; SINS northeast celestial velocity error [δV E,i δV N,i δV U,i ] T ; SINS three-axis attitude misalignment angle [φ E,i φ N,i φ U,i ] T ; JTIDS receiver clock phase and frequency error [δt i,JTIDS δf i,JTIDS ] T , GNSS receiver clock phase and frequency error [δt i,GNSS δf i,GNSS ] T . There are 13 dimensions in total, as shown below:

[0079]

[0080] (1.1.2) Other node status definitions are as follows:

[0081] The state of the jth node among the n nodes modeled by node i except itself Taken as: SINS horizontal geographic location error SINS horizontal geographic velocity error SINS attitude misalignment angle JTIDS receiver clock phase and frequency errors There are 9 dimensions in total, as shown below:

[0082]

[0083] (1.1.3) The velocity error of the present invention is defined as:

[0084]

[0085] Among them, δV i =[δV E,i δV N,i δV U,i ] T , represents the direction cosine matrix calculated from the true value of the node i trajectory, represents the direction cosine matrix calculated from the inertial guidance value of node i, is the northeastern sky velocity output by the inertial navigation value of node i, V i is the northeastern sky velocity of the true value output of node i.

[0086] This definition differs from the traditional velocity error. By transforming the velocity error state in the traditional integrated navigation system's EKF model, replacing it with a new velocity error state, the newly derived velocity error differential equation no longer contains the specific force term. This eliminates the dependency of system matrix updates on high-frequency specific force information. Simulations show that this method can achieve high-precision time updates of the error states of other nodes with communication intervals in the order of seconds.

[0087] (1.2) Filter noise: Select the noise amount of node i modeling, including its own noise and other node noise:

[0088] Assume that there are n+1 nodes in the cluster. Taking node i as an example, the noise amount of this node is w i It is composed of its own noise and the noise of other modeled nodes, and its specific form is as follows:

[0089]

[0090] in, is the noise amount of node i itself, It represents the noise amount of the jth node among the n nodes modeled by node i except itself (1≤j≤n).

[0091] (1.2.1) Self-noise is defined as follows:

[0092] Process noise of node i's own navigation error Taken as: random error of the three-axis accelerometer and gyroscope in the northeast sky [ω ax,i ω ay,i ω az,i ω gx,i ω gy,i ω gz,i ] T ; Noise state of the JTIDS receiver w J,i , the noise state w of the GNSS receiver G,i . There are 8 dimensions in total, as shown below:

[0093]

[0094] (1.2.2) Other node noises are defined as follows:

[0095] The process noise of the jth node among the n nodes modeled by node i except itself Taken as: random error of the three-axis accelerometer and gyroscope of the Northeast Sky Noise status of the JTIDS receiver There are 7 dimensions in total, as shown below:

[0096]

[0097] (1.3) Error propagation equation

[0098] (1.3.1) According to the state quantity The error equation for the continuous-time model can be written as:

[0099]

[0100] Among them, F i 0 (t) The specific composition is as follows:

[0101]

[0102]

[0103] in, The specific composition is as follows:

[0104]

[0105] (1.3.2) According to the state quantity The error equation for the continuous-time model can be written as:

[0106]

[0107] Among them, F i 0 The corresponding dimensions of the celestial velocity, altitude, GNSS receiver clock phase and frequency error in the (t) matrix are removed, and F can be obtained. i j(t),F i The specific composition of j(t) is as follows:

[0108]

[0109] in, The specific composition is as follows:

[0110]

[0111] Compared with the traditional strapdown inertial navigation error equation, due to the redefinition of velocity error, it can be ensured that the velocity error propagation equation does not contain the specific force term, but is replaced by the gravity term, that is, the matrix F ve The specific force term is not included in the equation. Therefore, the inaccurate calculation of the system matrix caused by high-frequency changes in the specific force of other nodes can be avoided.

[0112] (1.4) System state model

[0113] Assuming that the states of each node in the model are independent of each other, the state equation of the global model can be written as the block diagonal form of the state equation of each node, and the system state model is established. The state equation of node i is constructed as follows:

[0114]

[0115] By choosing a suitable discretization time t s , the system matrix F i 0 、F i j and noise driven array Discretize to get the state transfer matrix of node i's own navigation error and noise driven array

[0116] Step 2: Each node in the cluster establishes the measurement equations for the designed relative navigation system, which includes equivalent GNSS measurements for rapid dissemination of high-precision absolute navigation information.

[0117] Step 2 is as follows:

[0118] (2.1) Constructing the altimeter measurement equation

[0119] The measurement update process of the barometric altimeter is different from the traditional Kalman filter. It adopts the Schmidt filter method to divide the overall state of the node into two parts: the altitude state and the remaining states. The altimeter measurement only updates the state estimate and covariance corresponding to the altitude without affecting other state estimates.

[0120] z BA =h+w BA =h SINS -δh+w BA

[0121] Among them, h is the true height, h SINS is the inertial navigation indicated altitude, δh is the inertial navigation altitude error, w BA is the measurement noise. BA Obeying Gaussian distribution, and the measurement noise variance R=E[w BA w BA T ]. U As shown below:

[0122] H U =[0 0 -1…0]

[0123] (2.2) Constructing GNSS pseudorange measurement equation

[0124] The GNSS pseudorange measurement equation of node i is as follows:

[0125]

[0126] in, are the positions of the satellite and node i in the earth coordinate system, δt i,GNSS is the clock error of node i, and ξ is the ranging noise of GNSS.

[0127] The measurement equation is Taylor expanded at the prior estimate and then converted from the latitude and longitude coordinate system to the Earth Cartesian coordinate system to obtain:

[0128]

[0129] in,

[0130]

[0131]

[0132] Measurement noise variance R GNSS is the variance R0 corresponding to the measurement noise ξ:

[0133] R GNSS=R0

[0134] According to the expanded form of the measurement equation, H GNSS :

[0135] H GNSS =[H i …1…]

[0136] Among them, H i Take the first three columns of H1, the position corresponding to 1 is δt i,JTIDS The dimension in which it is located (the twelfth column).

[0137] (2.3) Constructing the equivalent GNSS pseudorange measurement equation

[0138] When GNSS is available for some nodes within the cluster, the equivalent GNSS measurement model is triggered. First, the GNSS-available node (assuming it is node l, and the jth node modeled for node i) shares its precise absolute position estimate in the next broadcast. Then, the GNSS-unavailable node (assuming it is node i) uses the equivalent GNSS measurement model to fuse this absolute measurement information. This improves the accuracy of node i's estimated absolute state and enables rapid diffusion of absolute navigation information. The equivalent geodetic equation is as follows:

[0139]

[0140] in, represents the optimal estimate of broadcaster l’s own latitude and longitude, which receiver i can obtain from the shared data packet sent by broadcaster l; It represents the latitude and longitude indicated by the inertial navigation of broadcaster l, which receiver i can obtain from the shared data packet sent by broadcaster l; H represents the latitude and longitude inertial navigation error state of the jth node modeled in the receiver i filter; w is the equivalent Gaussian white noise, and its covariance is determined by the GNSS accuracy. G As shown below:

[0141]

[0142] Among them, the number of columns corresponding to -1 is δλ i j , The dimension in which the state is modeled at node i.

[0143] (2.4) RTT measurement equation

[0144] RTT directly measures the difference between the data link clocks of the sending node and the receiving node. From this, the measurement equation can be obtained as:

[0145]

[0146] Among them, δt i,JTIDS where is the clock difference of node i, is the clock error of the time reference node NTR (the default is 0), w is the measurement noise, which obeys Gaussian distribution, and the measurement noise variance R=E[ww Τ ]. RTT As shown below:

[0147] H RTT =[…1…-1…]

[0148] Among them, 1 is in the tenth column, and the position corresponding to -1 is the time reference node NTR of node i modeling. The dimension in which it is located.

[0149] (2.5) Direct TOA measurement equation

[0150] Node j broadcasts, node i receives and forms the TOA measurement, which is then fused in the filter of node i. The direct TOA measurement equation can be written as follows:

[0151]

[0152] in, are the positions of node i and node j in the earth coordinate system, δt i,JTIDS ,δt j,JTIDS are the clock errors of node i and node j respectively, and ξ is the TOA ranging noise. Taylor expansion of the measurement equation at the prior estimate and then conversion of the latitude and longitude coordinate system to the Earth Cartesian coordinate system yields:

[0153]

[0154] in,

[0155]

[0156] Measurement noise variance R toa As shown below (the height covariance is considered as noise because the heights of other nodes are not modeled):

[0157]

[0158] in, is the element in the third row and third column of the covariance matrix of node j, and R0 is the variance corresponding to the measurement noise ξ. According to the expansion form of the measurement equation, H TOA :

[0159] H TOA =[-H i …1…H j ...-1...]

[0160] Among them, H i Take the first three columns of H1, -H i The corresponding position is the dimension corresponding to the latitude and longitude of node i modeling itself (the first three columns), and the position corresponding to 1 is δt i,JTIDS The dimension in which it is located (the tenth column). j Take the first two columns of H2, H j The corresponding position of the two columns is the dimension corresponding to the longitude and latitude of node i modeling node j, and the position corresponding to -1 is the dimension corresponding to the longitude and latitude of node i modeling node j. The dimension in which it is located.

[0161] (2.6) Shared TOA measurement equation

[0162] When node i needs to use the TOA measurement information between node q and node k, it needs to use the shared TOA measurement model to obtain z toa(k,q) (This shared TOA measurement is broadcast by node k and received by node q):

[0163]

[0164] in, are the positions of node q and node k in the earth coordinate system, δt q,JTIDS ,δt k,JTIDS are the clock errors of node q and node k respectively, and ξ is the TOA ranging noise.

[0165] The derivation process of the shared TOA measurement equation is the same as the above TOA measurement equation. The expansion of the measurement equation, r0, H1, H2, only needs to replace the position corresponding to i with q and the position corresponding to j with k to obtain the final result. The difference is that the measurement noise variance R toa (Since the heights of other nodes are not modeled, the height covariance is considered as noise):

[0166]

[0167] in, is the element in the third row and third column of the covariance matrix of node q, is the element in the third row and third column of the covariance matrix of node k, and R0 is the variance corresponding to the measurement noise ξ. According to the expansion form of the measurement equation, H TOA′ :

[0168] H TOA′ =[…-H q …1…H k ...-1...]

[0169] Among them, H q Take the first two columns of H1, -H qThe corresponding position of the two columns is the longitude and latitude corresponding to the node i modeling node q, and the corresponding position of 1 is the node i modeling node q. The dimension in which it is located. H k Take the first two columns of H2, H k The corresponding position of the two columns is the dimension corresponding to the longitude and latitude of node i modeling node k, and the corresponding position of -1 is the dimension corresponding to the longitude and latitude of node i modeling node k. The dimension in which it is located.

[0170] Step 3: Construct an information data packet shared by each node in the cluster under the time division multiple access round-robin broadcast communication mechanism;

[0171] In the present invention, under the time division multiple access round-robin broadcast communication mechanism (nodes broadcast data packets in turn according to fixed time slots to avoid conflicts), the information data packets shared by each node in the cluster are as follows:

[0172]

[0173] Table 1 Information data shared by each node

[0174] Step 4: Based on a computationally efficient information fusion method, construct the time update equation and the measurement update equation to perform filter time update and measurement update;

[0175] (4.1) Based on the state equation constructed in step 1, the time update equation and the specific execution steps are explained as follows:

[0176] The one-step prediction of the calculation state is as follows:

[0177]

[0178] in, is the one-step prediction matrix of the state of the i-th node, The state transition matrix for node i to model its own state, The state transition matrix of the jth node among the n nodes modeled for node i except itself.

[0179] Calculate the one-step prediction error covariance matrix of the state. The specific formula is as follows:

[0180]

[0181] in, is the one-step prediction error covariance matrix corresponding to the i-th node state, and are the known covariance matrices divided into blocks according to the state of each node and noise driven array The sub-block corresponding to the mth row and nth column of . The state transition matrix for node i to model its own state, The state transition matrix of the jth node among the n nodes modeled by node i, excluding itself, and represents the noise driving matrix corresponding to the corresponding modeled state.

[0182] (4.2) Based on the measurement equation constructed in step 2, the measurement update equation and the specific execution steps are described as follows:

[0183] Each node performs corresponding measurement updates upon receiving altimeter measurements, GNSS measurements, equivalent GNSS measurements, RTT measurements, TOA measurements, and shared TOA measurements.

[0184] Assume that node i obtains the measurement about node i and node j Define p = {i, j} and let q = {1,...,N}\{i, j} represent the N-2 remaining cluster nodes that did not participate in the relative measurement. Then the measurement update equation is:

[0185]

[0186] in,

[0187]

[0188] The two-term approximation reduces the computational complexity of this method, which can be reduced by an order of magnitude compared with traditional methods when the number of members is large.

[0189] Step 5: Use the state error estimated by the filter as feedback to correct the inertial navigation output and obtain the absolute state estimation information. The absolute state estimation information is then used to calculate the relative state estimation information of all members in the relative coordinate system.

[0190] Because the present invention employs a full modeling approach, each node in the cluster can estimate the state error of the current node relative to all other nodes. This estimated navigation parameter error is used as a correction for the inertial navigation system. By subtracting this error from the information output by the inertial navigation system, the absolute state information of all nodes calculated based on the current node's error state can be obtained. By then subtracting the calculated absolute state information of other nodes from the node's own absolute state information, the information can be converted into relative state information, ultimately yielding the effective relative information output by the relative navigation system.

[0191] like Figure 3 The figure shows a schematic diagram of error correction. The filter of the present invention is used to update the error state of the node modeling, thereby correcting the absolute information output by the inertial navigation solution and then calculating the effective relative information.

[0192] (5.1) Output relative position error curve

[0193] Figure 4 This is a curve chart of the relative navigation position error of the NC of the cluster formation control member. The curve chart has the maximum value, average value, and minimum value from top to bottom. The specific calculation method is as follows:

[0194] Let node i be the origin of the grid, and the true value of the UVW coordinate of node j at time k is The estimated value is the estimated value of the filter at node i with respect to j, expressed as The average relative position error of node i in the cluster at time k is calculated as follows (N is the total number of nodes in the cluster):

[0195]

[0196] The maximum relative positioning error of node i in the cluster at time k is calculated as follows:

[0197]

[0198] The minimum relative positioning error of node i in the cluster at time k is calculated as follows:

[0199]

[0200] (5.2) Output relative distance error curve

[0201] Figure 5 This is a curve chart of the relative navigation distance error of the NC of the cluster formation control member. From top to bottom, the curve chart shows the maximum value, average value, and minimum value. The specific calculation method is as follows:

[0202] Let node i be the origin of the grid, and the true value of the UVW coordinate of node i at time k is The estimated value is the self-filter estimate The true value of the UVW coordinate of node j at time k is The estimated value is the estimated value of the filter at node i with respect to j, expressed as The true relative distance between node i and node j is defined as (N is the total number of nodes in the cluster):

[0203]

[0204] The estimated relative distance is defined as:

[0205]

[0206] Taking all nodes as the grid origin, the average relative ranging error of node i in the cluster at time k is calculated as follows (abs means absolute value):

[0207]

[0208] The maximum relative distance error of node i in the cluster at time k is calculated as follows:

[0209]

[0210] The minimum relative distance error of node i in the cluster at time k is calculated as follows:

[0211]

[0212] (5.3) Output absolute-relative diffusion time

[0213] Figure 6 The horizontal absolute error curve of node 3 is shown. The absolute-relative diffusion time is calculated as follows:

[0214] The absolute-relative diffusion time refers to the time required for the average absolute position error of all aircraft in the cluster to drop to a certain threshold after several aircraft in the cluster obtain absolute measurements under satellite guidance denial conditions for a period of time. Figure 6 The absolute-relative diffusion time is 7s (a total of 12 nodes, node 1, node 8, and node 12 turn on GPS between 300s and 600s).

[0215] In summary, the present invention discloses a data link-based distributed inertial-based relative navigation method that enables accurate estimation of the relative states of cluster nodes and rapid coverage of absolute information. Cluster nodes only need to share their own inertial navigation data at intervals of seconds to achieve high-precision group inertial navigation error state propagation. A computationally efficient filter update method reduces computational complexity by an order of magnitude. Furthermore, the method can rapidly disseminate high-precision absolute cluster information in scenarios where GNSS is available for some nodes, demonstrating excellent versatility.

[0216] A distributed inertial-based relative navigation system based on data link:

[0217] The navigation system includes a state modeling module, a measurement fusion module, an information sharing module, an information updating module and a correction navigation module;

[0218] The state modeling module establishes state equations of a relative navigation system for nodes in the cluster;

[0219] The measurement fusion module establishes the measurement equation of the relative navigation system for each node in the cluster based on the equivalent GNSS measurement of the rapidly diffused high-precision absolute navigation information;

[0220] The information sharing module is based on a time division multiple access round-robin broadcast communication mechanism, and each node in the cluster shares the information data packet;

[0221] The information update module is used to construct a time update equation and a measurement update equation to perform filter time update and measurement update;

[0222] The correction navigation module uses the state error estimated by the filter as feedback, corrects the inertial navigation output, obtains absolute state estimation information, and uses the absolute state estimation information to calculate the relative state estimation information of all members in the relative coordinate system to obtain the final navigation result.

[0223] An electronic device includes a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of the above method when executing the computer program.

[0224] A computer-readable storage medium is used to store computer instructions, which implement the steps of the above method when executed by a processor.

[0225] The memory in the embodiments of the present application can be volatile memory or non-volatile memory, or can include both volatile and non-volatile memory. Among them, the non-volatile memory can be read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), or flash memory. The volatile memory can be random access memory (RAM), which is used as an external cache. By way of example and not limitation, many forms of RAM are available, such as static random access memory (SRAM), dynamic random access memory (DRAM), synchronous dynamic random access memory (SDRAM), double data rate synchronous dynamic random access memory (DDR SDRAM), enhanced synchronous dynamic random access memory (ESDRAM), synchronous link dynamic random access memory (SLDRAM), and direct RAM bus random access memory (DR RAM). It should be noted that memory of the methods described herein is intended to comprise, but not be limited to, these and any other suitable types of memory.

[0226] In the above embodiments, all or part of the embodiments can be implemented using software, hardware, firmware, or any combination thereof. When implemented using software, all or part of the embodiments can be implemented in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer instructions are loaded and executed on a computer, all or part of the processes or functions described in the embodiments of the present application are generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via a wired connection such as a coaxial cable, optical fiber, digital subscriber line (DSL), or wireless connection such as infrared, wireless, or microwave. The computer-readable storage medium can be any available medium that can be accessed by a computer, or a data storage device such as a server or data center that integrates one or more available media. The available medium can be a magnetic medium such as a floppy disk, hard disk, magnetic tape, an optical medium such as a high-density digital video disc (DVD), or a semiconductor medium such as a solid-state disc (SSD).

[0227] During implementation, each step of the above method can be completed by an integrated logic circuit of the hardware in the processor or by instructions in the form of software. The steps of the method disclosed in conjunction with the embodiments of the present application can be directly embodied as being executed by a hardware processor, or can be executed by a combination of hardware and software modules in the processor. The software module can be located in a storage medium mature in the art such as a random access memory, a flash memory, a read-only memory, a programmable read-only memory or an electrically erasable programmable memory, a register, etc. The storage medium is located in the memory, and the processor reads the information in the memory and completes the steps of the above method in conjunction with its hardware. To avoid repetition, it will not be described in detail here.

[0228] It should be noted that the processor in the embodiments of the present application can be an integrated circuit chip with signal processing capabilities. During implementation, each step of the above-described method embodiment can be completed by hardware integrated logic circuits in the processor or by software instructions. The above-described processor can be a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA), or other programmable logic device, discrete gate or transistor logic device, or discrete hardware components. The various methods, steps, and logic block diagrams disclosed in the embodiments of the present application can be implemented or executed. The general-purpose processor can be a microprocessor or any conventional processor. The steps of the methods disclosed in the embodiments of the present application can be directly implemented and executed by a hardware decoding processor, or by a combination of hardware and software modules in the decoding processor. The software module can be located in a storage medium well-known in the art, such as random access memory, flash memory, read-only memory, programmable read-only memory, electrically erasable programmable memory, registers, etc. The storage medium is located in the memory, and the processor reads the information in the memory and, in conjunction with its hardware, completes the steps of the above-described method.

[0229] The above is a detailed introduction to the distributed inertial base relative navigation method based on data link proposed in the present invention, and the principles and implementation methods of the present invention are explained. The description of the above embodiments is only used to help understand the method and core ideas of the present invention. At the same time, for those skilled in the art, according to the ideas of the present invention, there may be changes in the specific implementation methods and application scopes. In summary, the contents of this specification should not be understood as limiting the present invention.

Claims

1. A distributed inertial-based relative navigation method based on a data link, characterized by: The method specifically comprises the following steps: Step 1: Establish the state equation of the relative navigation system for the nodes in the cluster; Step 2: Based on the equivalent GNSS measurement of the rapidly diffused high-precision absolute navigation information, establish the measurement equation of the relative navigation system for each node in the cluster; Step 3: Based on the time division multiple access round-robin broadcast communication mechanism, each node in the cluster shares the information data packet; Step 4: Construct the time update equation and the measurement update equation to perform filter time update and measurement update; Step 5: Use the state error estimated by the filter as feedback to correct the inertial navigation output and obtain the absolute state estimation information. The absolute state estimation information is then used to calculate the relative state estimation information of all members in the relative coordinate system to obtain the final navigation result.

2. The navigation method according to claim 1, wherein: In step 1, Step 1.1, select the state quantity of node i modeling, the navigation state x of the node i It consists of its own navigation state and the navigation state of other modeled nodes. The specific form is as follows: in, is the navigation error state of node i itself, Indicates the state of the jth node among the n nodes modeled by node i except itself (1≤j≤n); Step 1.2: Select the noise amount of node i, including its own noise and other node noise: in, is the noise amount of node i itself, represents the noise amount of the jth node among the n nodes modeled by node i except itself (1≤j≤n); Step 1.3, define the velocity error as: Among them, δV i =[δV E,i δV N,i δV U,i ] T , represents the direction cosine matrix calculated from the true value of the node i trajectory, represents the direction cosine matrix calculated from the inertial guidance value of node i, is the northeastern sky velocity output by the inertial navigation value of node i, V i is the northeastern sky speed of the true value output of node i; Step 1.4: Based on the state quantity and noise quantity modeled at node i, establish the error propagation equation and construct the state equation of node i.

3. The navigation method according to claim 2, wherein: In step 2, Step 2.1: Construct an altitude measurement equation and use the Schmidt filter method to divide the overall node state into two parts: altitude state and other states. The altitude measurement only updates the state estimate and covariance corresponding to the altitude without affecting other state estimates. z BA =h+w BA =h SINS -δh+w BA Among them, h is the true height, h SINS is the inertial navigation indicated altitude, δh is the inertial navigation altitude error, w BA To measure noise; Step 2.2: Construct the GNSS pseudorange measurement equation of node i as follows: in, are the positions of the satellite and node i in the earth coordinate system, δt i,GNSS is the clock error of node i, and ξ is the ranging noise of GNSS.

4. The navigation method according to claim 3, characterized in that: Also included in step 2: Step 2.3: When the GNSS of some nodes in the cluster is available, an equivalent GNSS pseudorange measurement equation is constructed. The nodes that are unavailable use the equivalent GNSS measurement model to fuse the absolute measurement information. Step 2.4, RTT clock synchronization measurement: measure the relative clock difference between nodes through the signal round trip time; Step 2.5, direct TOA ranging measurement: the relative distance between nodes is measured by signal arrival time; Step 2.6, shared TOA ranging measurement: When a node needs to use the TOA measurement information between other nodes, the shared TOA measurement model is used to obtain the information.

5. The navigation method according to claim 4, characterized in that: In step 3, Nodes take turns broadcasting data packets at fixed time slots; The contents of the data packet include: its own member ID, status sharing part, equivalent GNSS judgment conditions and measurement sharing part The state sharing part includes the position, speed, attitude estimation of its own INS, the optimal estimation of its own position and celestial speed, and the covariance of altitude estimation; The equivalent GNSS judgment condition is to obtain GNSS time; The measurement sharing part includes another member ID related to the shared TOA, the shared TOA measurement, the INS position estimation of both parties at the time of shared TOA generation, and the optimal altitude estimation.

6. The navigation method according to claim 5, characterized in that: In step 4 include: Step 4.1: Update the state equation constructed in step 1 and predict the state and covariance at the next moment; In step 4.2, the measurement is updated according to the measurement equation constructed in step 2. Each node performs corresponding measurement updates when receiving altimeter measurement, GNSS measurement, equivalent GNSS measurement, RTT measurement, TOA measurement, and shared TOA measurement.

7. The navigation method according to claim 6, characterized in that: In step 5, In step 5.1, the node feeds back the filter estimation error to the inertial navigation system to correct the absolute state information output by it. In step 5.2, the node uses its own corrected absolute state information and combines it with the estimation errors of other nodes to calculate the relative state information of all nodes, and finally obtains the effective relative information of the relative navigation output.

8. A navigation system for executing the data link-based distributed inertial base relative navigation method according to any one of claims 1 to 7, characterized in that: The navigation system includes a state modeling module, a measurement fusion module, an information sharing module, an information updating module and a correction navigation module; The state modeling module establishes state equations of a relative navigation system for nodes in the cluster; The measurement fusion module establishes the measurement equation of the relative navigation system for each node in the cluster based on the equivalent GNSS measurement of the rapidly diffused high-precision absolute navigation information; The information sharing module is based on a time division multiple access round-robin broadcast communication mechanism, and each node in the cluster shares the information data packet; The information update module is used to construct a time update equation and a measurement update equation to perform filter time update and measurement update; The correction navigation module uses the state error estimated by the filter as feedback, corrects the inertial navigation output, obtains absolute state estimation information, and uses the absolute state estimation information to calculate the relative state estimation information of all members in the relative coordinate system to obtain the final navigation result.

9. An electronic device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium for storing computer instructions, characterized in that: When the computer instructions are executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images