A data link based distributed inertial-based relative navigation method

Abstract

The application provides a data chain-based distributed inertial base relative navigation method and relates to the technical field of navigation; first, a relative navigation error state equation based on speed error reconstruction is used to eliminate the high-frequency changing specific force term, thereby reducing the dependence on the sharing of inertial navigation data between nodes; second, an efficient information fusion method is designed to reduce the calculation complexity of the overall method; finally, an equivalent GNSS measurement for rapidly diffusing high-precision absolute navigation information is designed; the application can realize accurate estimation of the relative state of the cluster nodes and rapid coverage of the absolute information; through simulation experiments, the cluster nodes only need to share their inertial navigation data at a second-level interval to realize high-precision group inertial navigation error state propagation.

Description

Technical Field

[0001] This invention belongs to the field of navigation technology, specifically, it relates to a distributed inertial basis relative navigation method based on data links. Background Technology

[0002] Relative navigation technology provides swarm nodes with accurate and reliable local spatiotemporal information (relative positioning and timing information), which is a prerequisite and important guarantee for the swarm to successfully complete tasks such as coordinated detection, coordinated encirclement, and coordinated attack. Currently, most swarm systems rely on Global Navigation Satellite Systems (GNSS) to obtain accurate absolute navigation information and achieve relative navigation through the distribution and sharing of this absolute navigation information. However, in complex environments such as indoors, cities, and forests, GNSS signals may be completely or intermittently lost due to obstruction by tall buildings, trees, and the effects of atmospheric refraction and multipath effects, making this method unable to reliably achieve relative navigation. Data links can achieve accurate relative clock and relative distance measurements by measuring the round-trip time (RTT) and time of arrival (TOA) of signals, serving as an effective alternative to relative navigation sensors in scenarios where GNSS is completely or intermittently denied. Data link-based relative navigation methods utilize data link time synchronization and relative ranging capabilities, combined with autonomous navigation sensors such as inertial navigation systems (INS) and altimeters, to achieve relative positioning and timing of cluster nodes. This has become an effective relative navigation method in environments where GNSS is denied or intermittently denied.

[0003] To improve relative navigation accuracy, previous researchers proposed a relative navigation method based on measurement sharing. In this method, each node in the cluster shares its generated TOA measurements with other nodes in the network, enabling them to use these measurements for information fusion. This method increases the number and types of TOA measurements that each node can acquire, effectively improving the observability and estimation accuracy of the relative navigation state. At the same time, in order to accurately fuse 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 about by the fused TOA measurements, ensuring the accuracy and reliability of information fusion. However, this method still faces three main problems: (1) In order to accurately model the inertial navigation error state of other nodes, each node needs to acquire the relative force information of other nodes in real time. Currently, the relative force output frequency of inertial sensors is usually 100Hz, and in practice, data links can hardly support such high data transmission requirements. (2) Because the global inertial navigation error state is modeled, the computational complexity of traditional EKF will increase exponentially with the increase of cluster size. (3) This method only considers the relative navigation scenario of GNSS complete rejection, and does not consider how to quickly spread high-precision absolute navigation information to the entire cluster through relative navigation when GNSS is available at some nodes. In order to solve the above problems, we propose a distributed inertial basis relative navigation method based on data link to achieve accurate estimation of relative state and rapid coverage of absolute information. Summary of the Invention

[0004] To address the problems of existing data link-based inertial basis relative navigation methods that rely on high-frequency inertial navigation information sharing for high-precision modeling, have excessive computational load, and do not consider available GNSS scenarios, this invention provides a data link-based distributed inertial basis relative navigation method. This method can not only achieve accurate cluster relative navigation in GNSS-rejected scenarios, but also rapidly disseminate high-precision absolute information of the cluster in GNSS-available scenarios with some nodes, demonstrating good versatility.

[0005] This invention is achieved through the following technical solution: a distributed inertial basis relative navigation method based on data links; the method specifically includes the following steps:

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

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

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

[0009] Step 4: Construct the time update equation and measurement update equation, and 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, obtain the absolute state estimation information, and use 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.

[0011] Further, in step 1,

[0012] Step 1.1: Select the state variables for modeling node i, including the navigation state x of that node. i It consists of its own navigation state and the navigation states of other nodes it models, as shown in the following form:

[0013]

[0014] in, This represents the navigation error state of node i itself. This represents the state of the j-th node among the n nodes (excluding itself) modeled by node i (1≤j≤n);

[0015] Step 1.2: Select the noise quantity for modeling node i, including its own noise and noise from other nodes:

[0016]

[0017] in, Let be the noise level of node i itself. This represents the noise level of the j-th node among the n nodes (excluding itself) modeled by node i (1≤j≤n);

[0018] Step 1.3 defines the speed error as follows:

[0019]

[0020] Where, δV i =[δV E,i δV N,i δV U,i ] T , This represents the direction cosine matrix calculated from the true value of the trajectory of node i. This represents the direction cosine matrix calculated from the inertial guidance values ​​at node i. The northeastern celestial velocity output by node i inertial navigation is V. i The northeastern sky speed is the true value output of node i;

[0021] Step 1.4: Based on the state variables and noise variables 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 the height measurement equation, use the Schmidt filter method to divide the overall state of the node into two parts: height state and other states, and make the height measurement only update the state estimate and covariance corresponding to the height, without affecting the other state estimates;

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

[0025] Where h is the actual height, h SINS The inertial navigation system (INS) indicates the altitude, δh is the INS altitude error, and w is the inertial navigation system altitude indication. BA For measuring noise;

[0026] Step 2.2, construct the GNSS pseudorange measurement equation for node i as follows:

[0027]

[0028] in, δt represents the position of satellite and node i in the Earth coordinate system. i,GNSS Let ξ be the clock error of node i, and ξ be the ranging noise of GNSS.

[0029] Furthermore, step 2 also includes:

[0030] Step 2.3: When GNSS is available at some nodes within the cluster, construct an equivalent GNSS pseudorange measurement equation. For nodes where GNSS is unavailable, fuse this absolute measurement information using the equivalent GNSS measurement model.

[0031] Step 2.4, RTT clock synchronization measurement: Measure the relative clock difference between nodes by measuring the signal round-trip time;

[0032] Step 2.5, Direct TOA Distance 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 TOA measurement information between other nodes, it uses the shared TOA measurement model to obtain the information.

[0034] Furthermore, in step 3, the nodes broadcast data packets in turn according to fixed time slots;

[0035] The data packet includes: its own member ID, state sharing portion, equivalent GNSS determination conditions, and measurement sharing portion.

[0036] The state sharing component includes its own INS position, velocity, attitude estimation, optimal estimation of its own position and celestial velocity, and altitude estimation covariance.

[0037] The equivalent GNSS determination condition is the acquisition of GNSS time;

[0038] The measurement sharing component includes another member ID associated with the shared TOA, the shared TOA measurement, the INS estimation of the horizontal position of both parties at the time of the shared TOA generation, and the optimal height estimation.

[0039] Furthermore, step 4 includes:

[0040] Step 4.1: Update the state equation in step one to predict the state and covariance at the next time step.

[0041] Step 4.2: Update measurements based on the measurement equations constructed in Step 2. Each node updates its measurements upon receiving altimeter measurements, GNSS measurements, equivalent GNSS measurements, RTT measurements, TOA measurements, and shared TOA measurements.

[0042] Furthermore, in step 5,

[0043] Step 5.1: The node feeds back the error estimated by the filter to the inertial navigation system to correct the absolute state information output by the system.

[0044] Step 5.2: The node uses its own corrected absolute state information and combines it with the estimation error 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 basis 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 update module, and a navigation correction module;

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

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

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

[0050] The information update module is used to construct time update equations and measurement update equations, and to perform filter time updates and measurement updates.

[0051] The correction navigation module uses the state error estimated by the filter as feedback to correct 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, thus obtaining the final navigation result.

[0052] An electronic device includes a memory and a processor, the memory storing a computer program, the processor executing the computer program to implement the steps of the above method.

[0053] A computer-readable storage medium for storing computer instructions that, when executed by a processor, implement the steps of the above-described method.

[0054] Beneficial effects of the invention

[0055] This invention first employs a relative navigation error state equation based on velocity error reconstruction, eliminating the high-frequency changing force term and thus reducing the dependence on inter-node inertial navigation data sharing; secondly, it reduces the overall computational complexity by designing a computationally efficient information fusion method; and finally, it designs an equivalent GNSS measurement for rapidly disseminating high-precision absolute navigation information.

[0056] This invention enables two core functions: accurate estimation of the relative state of cluster nodes and rapid coverage of absolute information. Simulation experiments demonstrate that the method of this invention achieves high-precision propagation of group inertial navigation error state by having cluster nodes share their own inertial navigation data at second-level intervals. Furthermore, compared to traditional global modeling schemes, the computational complexity of this framework is reduced by an order of magnitude.

[0057] Furthermore, this method can not only achieve accurate cluster relative navigation in GNSS all-rejection scenarios, but also rapidly disseminate high-precision absolute information of the cluster in scenarios where GNSS is available on some nodes, demonstrating good versatility. Attached Figure Description

[0058] Figure 1 This is a simulation diagram of the cluster's operational trajectory according to the present invention;

[0059] Figure 2 This is a flowchart of the relative navigation algorithm of the present invention;

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

[0061] Figure 4 This is a relative navigation position error curve.

[0062] Figure 5 It is a distance error curve diagram for relative navigation;

[0063] Figure 6 It is a graph of the horizontal absolute error of relative navigation. Detailed Implementation

[0064] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

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

[0066] A distributed inertial basis relative navigation method based on data links:

[0067] (1) First, generate simulated aircraft cluster movement trajectories, such as Figure 1 The simulation generated the movement trajectory of the UAV swarm consisting of three formations: line formation, dense formation, and loose formation. The position, speed, and attitude information of the UAVs were imported into MATLAB as real-world data.

[0068] (2) Set up a relative navigation experimental environment, and set the parameters of each sensor as follows: inertial navigation frequency 100 Hz, gyroscope constant bias 0.01° / h, and gyroscope angle random walk. Implantation constant bias 50 μg, accelerator velocity random walk TOA pseudorange standard deviation 20m, RTT standard deviation 30ns (9m), altimeter standard deviation 10m, initial clock synchronization phase error 10ns (3m), initial clock synchronization frequency error 0.09m / s, initial position error [10m, 10m, 10m], initial velocity error [0.1m / s, 0.1m / s, 0.1m / s], initial attitude error [0.003°, 0.003°, 0.05°], 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 diagram shows the flowchart of the core algorithm for relative navigation, taking node i as an example. The specific steps are as follows:

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

[0072] Step one is as follows:

[0073] (1.1) Filtering State: Select the state variables for modeling node i, including its own state and the states of other nodes:

[0074] Assuming there are n+1 nodes in the cluster, taking node i as an example, the navigation state x of node i is... i It consists of its own navigation state and the navigation states of other nodes it models, as shown in the following form:

[0075]

[0076] in, This represents the navigation error state of node i itself. This represents the state of the j-th node among the n nodes (excluding itself) modeled by node i (1≤j≤n).

[0077] (1.1.1) Its own state is defined as follows:

[0078] Node i's own navigation error state Let it be: SINS latitude and longitude error [δλ] i δL i δh i ] T SINS Northeast-to-Heavy 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) The states of other nodes are defined as follows:

[0081] The state of the j-th node among the n nodes excluding itself, modeled by node i. Taken as: SINS horizontal geographic location error SINS Horizontal Geographic Velocity Error SINS Posture Misalignment Angle JTIDS receiver clock phase and frequency error There are 9 dimensions in total, as shown below:

[0082]

[0083] (1.1.3) The speed error of this invention is defined as follows:

[0084]

[0085] Where, δV i =[δV E,i δV N,i δV U,i ] T , This represents the direction cosine matrix calculated from the true value of the trajectory of node i. This represents the direction cosine matrix calculated from the inertial guidance values ​​at node i. The northeastern celestial velocity output by node i inertial navigation is V. i It is the northeastern sky speed output by node i.

[0086] This definition differs from the traditional velocity error definition. It transforms the velocity error state in the traditional EKF model of an integrated navigation system, replacing the original velocity error state with a new one. Consequently, the newly derived velocity error differential equation no longer contains a specific force term. This allows the system matrix update to no longer rely on frequently changing specific force information. Simulations show that this method can achieve high-precision time updates of the error states of other nodes with communication intervals down to the second level.

[0087] (1.2) Filtered noise: The noise level modeled for node i is selected, including its own noise and noise from other nodes:

[0088] Assuming there are n+1 nodes in the cluster, taking node i as an example, the noise level w of node i is... i It consists of its own noise and the noise of other nodes being modeled, and its specific form is shown below:

[0089]

[0090] in, Let be the noise level of node i itself. This represents the noise level of the j-th node among the n nodes (excluding itself) modeled by node i (1≤j≤n).

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

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

[0093]

[0094] (1.2.2) Other node noise is defined as follows:

[0095] The process noise of the j-th node among the n nodes excluding itself in the modeling of node i. The random error of the three-axis accelerometer and gyroscope of Northeast Tian is taken as: Noise status of JTIDS receiver There are 7 dimensions in total, as shown below:

[0096]

[0097] (1.3) Error Propagation Equation

[0098] (1.3.1) Based on the state variables The error equation for the continuous-time model can be written as:

[0099]

[0100] Among them, F i 0 (t) is specifically composed as follows:

[0101]

[0102]

[0103] in, The specific components are as follows:

[0104]

[0105] (1.3.2) Based on the state variables The error equation for the continuous-time model can be written as:

[0106]

[0107] Among them, F i 0 By removing the dimensions corresponding to the upward velocity, altitude, and GNSS receiver clock phase and frequency error from the (t) matrix, we can obtain F. i j(t),F i j(t) is specifically composed as follows:

[0108]

[0109] in, The specific components are as follows:

[0110]

[0111] Compared to the traditional strapdown inertial navigation error equations, the redefinition of velocity error ensures that the velocity error propagation equations do not contain specific force terms, but are instead replaced by gravity terms, i.e., matrix F ve It does not contain a force term. Therefore, it avoids the problem of inaccurate system matrix calculation caused by high-frequency changes in the force at other nodes.

[0112] (1.4) System State Model

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

[0114]

[0115] By selecting an appropriate discretization time t s The system matrix F i 0 F i j and noise-driven array Discretization yields the state transition matrix of node i's own navigation error. and noise-driven array

[0116] Step 2: Each node in the cluster establishes the measurement equations relative to the designed navigation system, which includes an equivalent GNSS measurement designed for rapidly disseminating high-precision absolute navigation information;

[0117] Step two is as follows:

[0118] (2.1) Constructing the equation for altitude measurement

[0119] The measurement update process of the barometric altimeter differs from that of the traditional Kalman filter. It uses the Schmitt filter method to divide the overall state of the node into two parts: the altitude state and the other states. The altimeter measurement updates only the state estimate and covariance corresponding to the altitude, without affecting the estimates of other states.

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

[0121] Where h is the actual height, h SINS The inertial navigation system (INS) indicates the altitude, δh is the INS altitude error, and w is the inertial navigation system altitude indication. BA For measuring noise. BA It follows a Gaussian distribution, and the measurement noise variance R = E[w] BA w BA T ]. H U As shown below:

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

[0123] (2.2) Constructing the GNSS pseudorange measurement equation

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

[0125]

[0126] in, δt represents the position of satellite and node i in the Earth coordinate system. i,GNSS Let ξ be the clock error of node i, and ξ be the ranging noise of GNSS.

[0127] Performing a Taylor expansion of the measurement equation at the a priori estimate, followed by a transformation from the latitude, longitude, and altitude coordinate system to the Earth's Cartesian coordinate system, yields:

[0128]

[0129] in,

[0130]

[0131]

[0132] Measurement noise variance R GNSS To measure the variance R0 corresponding to the noise ξ:

[0133] R GNSS=R0

[0134] H is obtained from the expansion of the measurement equation. 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 it belongs to (column 12).

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

[0138] When GNSS becomes available at some nodes within the cluster, the equivalent GNSS measurement model will be triggered. First, the GNSS-available node (let's say node l, and the j-th node modeled for node i) shares its precise absolute position estimate in the next broadcast. Then, the GNSS-unavailable node (let's say node i) fuses this absolute measurement information using the equivalent GNSS measurement model. This improves the accuracy of the absolute state estimated by node i, enabling rapid dissemination of absolute navigation information. The equivalent geodetic equation is shown below:

[0139]

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

[0141]

[0142] Where -1 corresponds to the column number δλ i j , The dimension of the state variables in node i is modeled.

[0143] (2.4) RTT Measurement Equation

[0144] RTT directly measures the difference in data link clock speeds between the sending and receiving nodes, from which its measurement equation can be derived:

[0145]

[0146] Where, δt i,JTIDS Where is the clock difference of node i. Let w be the clock bias of the time reference node NTR (default is 0), and w be the measurement noise, which follows a Gaussian distribution and has a measurement noise variance R = E[ww] Τ ]. H RTT As shown below:

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

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

[0149] (2.5) Direct TOA Measurement Equation

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

[0151]

[0152] in, Let δt represent the positions of node i and node j in the Earth coordinate system. i,JTIDS ,δt j,JTIDS Let ξ represent the clock errors at nodes i and j, respectively, and ξ be the TOA ranging noise. Performing a Taylor expansion of the measurement equation at the prior estimate, followed by a transformation from latitude, longitude, and height coordinates to the Earth's Cartesian coordinate system, we obtain:

[0153]

[0154] in,

[0155]

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

[0157]

[0158] in, Let Rj be the element in the third row and third column of the covariance matrix of node j, and R0 be the variance corresponding to the measurement noise ξ. H is obtained from the expansion of the measurement equation. 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 (first three columns) of node i's own latitude, longitude, and altitude modeling; position 1 corresponds to δt. i,JTIDS The dimension it belongs to (column 10). H j Take the first two columns of H2, H j The two columns correspond to the latitude and longitude of node i modeling node j, and -1 corresponds to the position 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) Information (this shared TOA measurement is formed by node k broadcasting and node q receiving):

[0163]

[0164] in, Let δt represent the positions of nodes q and k in the Earth coordinate system. q,JTIDS ,δt k,JTIDS Let be the clock errors of nodes q and k, respectively, and ξ be the TOA ranging noise.

[0165] The derivation process of the shared TOA measurement equation is the same as that of the TOA measurement equation described above. The expansion of the measurement equation, r0, H1, and H2, only requires replacing the position corresponding to i with q and the position corresponding to j with k to obtain the final result. The difference lies in 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, Let be the element in the third row and third column of the covariance matrix of node q. Let Rk be the element in the third row and third column of the covariance matrix of node k, and R0 be the variance corresponding to the measurement noise ξ. H is obtained from the expansion of the measurement equation. TOA′ :

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

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

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

[0171] Under the time-division multiple access round-robin broadcast communication mechanism (nodes broadcast data packets in turn according to fixed time slots to avoid collisions), the information data packets shared by each node in the cluster are shown in the following table:

[0172]

[0173] Table 1. Information data shared by each node

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

[0175] (4.1) Based on the state equation constructed in step one, the time update equation and the specific execution steps are described below:

[0176] The specific formula for calculating the one-step prediction of the state is as follows:

[0177]

[0178] in, The one-step prediction matrix for the state of the i-th node. Model the state transition matrix of node i for its own state. The state transition matrix of the j-th node among the n nodes excluding itself, which is used to model node i.

[0179] The formula for calculating the one-step prediction error covariance matrix of the state is as follows:

[0180]

[0181] in, Let be the one-step prediction error covariance matrix corresponding to the state of the i-th node. and The known covariance matrices are divided into blocks according to the state of each node. and noise-driven array The sub-block corresponding to the m-th row and n-th column. Model the state transition matrix of node i for its own state. The state transition matrix of the j-th node among the n nodes excluding itself, used to model node i. and This represents the noise driving matrix corresponding to the modeling state.

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

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

[0184] Suppose node i obtains measurements about nodes i and j. Let p = {i,j} and q = {1,...,N} 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 approximations reduce the computational cost of this method, which can be reduced by an order of magnitude compared to 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, obtain the absolute state estimation information, and use the absolute state estimation information to calculate the relative state estimation information of all members in the relative coordinate system.

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

[0191] like Figure 3 The diagram shows an error correction scheme. By updating the error state of the node model using the filter of this invention, the absolute information of the inertial navigation solution output is corrected, and then the effective relative information is calculated.

[0192] (5.1) Output the relative position error curve.

[0193] Figure 4 This is a graph showing the relative navigation position error of the NC members in the cluster formation control system. The graphs, from top to bottom, represent the maximum, average, and minimum values, respectively. The specific calculation method is shown below:

[0194] Let node i be the origin of the grid, and let the true value of the UVW coordinates of node j at time k be... The estimated value is the estimate of the filter at node i with respect to j, denoted as: The average relative position error of node i 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 at time k is calculated as follows:

[0197]

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

[0199]

[0200] (5.2) Output the relative distance error curve.

[0201] Figure 5 This is a graph showing the relative navigation distance error of the NC members in the cluster formation control system. The graphs, from top to bottom, represent the maximum, average, and minimum values, respectively. The specific calculation method is shown below:

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

[0203]

[0204] Define the estimated relative distance as:

[0205]

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

[0207]

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

[0209]

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

[0211]

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

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

[0214] Absolute-relative diffusion time refers to the time required for the average absolute position error of all aircraft in a cluster to decrease to a certain threshold after several aircraft within the cluster have obtained absolute satellite navigation measurements under satellite denial conditions. Figure 6 The absolute-relative diffusion time is 7s (there are a total of 12 nodes, and nodes 1, 8, and 12 activate GPS between 300s and 600s).

[0215] In summary, this invention discloses a distributed inertial basis relative navigation method based on a data link, which can achieve accurate estimation of the relative state of cluster nodes and rapid coverage of absolute information. In this invention, cluster nodes only need to share their own inertial navigation data at second-level intervals to achieve high-precision propagation of group inertial navigation error states; simultaneously, the use of a computationally efficient filter update method reduces computational complexity by an order of magnitude; and it can rapidly disseminate high-precision absolute information of the cluster in scenarios where GNSS is available for some nodes, demonstrating good versatility.

[0216] A distributed inertial basis 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 update module, and a navigation correction module;

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

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

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

[0221] The information update module is used to construct time update equations and measurement update equations, and to perform filter time updates and measurement updates.

[0222] The correction navigation module uses the state error estimated by the filter as feedback to correct 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, thus obtaining the final navigation result.

[0223] An electronic device includes a memory and a processor, the memory storing a computer program, the processor executing the computer program to implement the steps of the above method.

[0224] A computer-readable storage medium for storing computer instructions that, when executed by a processor, implement the steps of the above-described method.

[0225] The memory in the embodiments of this application can be volatile memory or non-volatile memory, or may include both volatile and non-volatile memory. 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. Volatile memory can be random access memory (RAM), which is used as an external cache. By way of example, but not limitation, many forms of RAM are available, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate synchronous DRAM (DDR SDRAM), enhanced synchronous DRAM (ESDRAM), synchronous linked DRAM (SLDRAM), and direct rambus RAM (DR RAM). It should be noted that the memory of the methods described in this invention is intended to include, but is not limited to, these and any other suitable types of memory.

[0226] In the above embodiments, implementation can be achieved entirely or partially through software, hardware, firmware, or any combination thereof. When implemented using software, it can be implemented entirely or partially 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 this 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 wired means such as coaxial cable, optical fiber, digital subscriber line, DSL, or wireless means such as infrared, wireless, microwave, etc. The computer-readable storage medium can be any available medium that a computer can access 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 disk, SSD, etc.

[0227] In implementation, each step of the above method can be completed by integrated logic circuits in the processor's hardware or by instructions in software. The steps of the method disclosed in the embodiments of this application can be directly implemented by a hardware processor, or by a combination of hardware and software modules in the processor. The software modules can reside in random access memory, flash memory, read-only memory, programmable read-only memory, electrically erasable programmable memory, registers, or other mature storage media in the art. This storage medium is located in memory, and the processor reads information from the memory and, in conjunction with its hardware, completes the steps of the above method. To avoid repetition, detailed descriptions are omitted here.

[0228] It should be noted that the processor in the embodiments of this application can be an integrated circuit chip with signal processing capabilities. During implementation, each step of the above method embodiments can be completed by the integrated logic circuits in the processor's hardware or by instructions in software form. The 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 devices, discrete gate or transistor logic devices, or discrete hardware components. It can implement or execute the methods, steps, and logic block diagrams disclosed in the embodiments of this application. The general-purpose processor can be a microprocessor or any conventional processor. The steps of the methods disclosed in the embodiments of this application can be directly embodied as execution by a hardware decoding processor, or as execution by a combination of hardware and software modules in the decoding processor. The software modules can be located in random access memory, flash memory, read-only memory, programmable read-only memory, electrically erasable programmable memory, registers, or other mature storage media in the art. This storage medium is located in memory; the processor reads information from the memory and, in conjunction with its hardware, completes the steps of the above methods.

[0229] The above provides a detailed description of the distributed inertial basis relative navigation method based on data links proposed in this invention, and elucidates the principles and implementation methods of this invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of this invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this invention. Therefore, the content of this specification should not be construed as a limitation of this invention.

Claims

1. A distributed inertial basis relative navigation method based on data links, characterized in that: The method specifically includes the following steps: Step 1: Establish the state equations for the nodes in the cluster relative to the navigation system; Step 1.1, Select Nodes The state variables in the model, the navigation state of this node. It consists of its own navigation state and the navigation states of other nodes it models, as shown in the following form: in, For nodes Self-navigation error status Represents a node The modeling of the rest besides itself The node in the node The state of each node ( ); Step 1.2, Select Nodes The noise level in the model includes its own noise and noise from other nodes: in, For nodes Self-noise level, Represents a node The modeling of the rest besides itself The node in the node Noise level of each node ( ); Step 1.3 defines the speed error as follows: in, , This represents the direction cosine matrix calculated from the true value of the trajectory of node i. Indicates that by node The direction cosine matrix calculated from the inertial guidance value. The northeastern celestial velocity is the output value of the inertial navigation system at node i. The true value output of node i is the northeastern sky speed; Step 1.4, based on nodes Modeling state variables and noise variables, establishing error propagation equations, and constructing nodes. The equation of state; Step 2: Based on the equivalent GNSS measurements of rapidly disseminated high-precision absolute navigation information, establish measurement equations for each node in the cluster relative to the navigation system; Step 3: Based on the time-division multiple access round-robin broadcast communication mechanism, each node in the cluster shares information data packets; Step 4: Construct the time update equation and measurement update equation, and 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, obtain the absolute state estimation information, and use 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.

2. The navigation method according to claim 1, characterized in that: In step 2, Step 2.1: Construct the height measurement equation, use the Schmidt filter method to divide the overall state of the node into two parts: height state and other states, and make the height measurement only update the state estimate and covariance corresponding to the height, without affecting the other state estimates; in, For the actual height, For inertial navigation to indicate altitude, For inertial navigation altitude error, For measuring noise; Step 2.2, Build Nodes The equation for GNSS pseudorange measurement is as follows: in, Satellites and nodes respectively Position in Earth's coordinate system For nodes Clock error, This refers to the ranging noise of GNSS.

3. The navigation method according to claim 2, characterized in that: Step 2 also includes: Step 2.3: When GNSS is available at some nodes within the cluster, construct an equivalent GNSS pseudorange measurement equation. For nodes where GNSS is unavailable, fuse this absolute measurement information using the equivalent GNSS measurement model. Step 2.4, RTT clock synchronization measurement: Measure the relative clock difference between nodes by measuring the signal round-trip time; Step 2.5, Direct TOA Distance 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 TOA measurement information between other nodes, it uses the shared TOA measurement model to obtain the information.

4. The navigation method according to claim 3, characterized in that: In step 3, Nodes broadcast data packets in turn according to fixed time slots; The data packet includes: its own member ID, state sharing portion, equivalent GNSS determination conditions, and measurement sharing portion. The state sharing component includes its own INS position, velocity, attitude estimation, optimal estimation of its own position and celestial velocity, and altitude estimation covariance. The equivalent GNSS determination condition is the acquisition of GNSS time; The measurement sharing component includes another member ID associated with the shared TOA, the shared TOA measurement, the INS estimation of the horizontal position of both parties at the time of the shared TOA generation, and the optimal height estimation.

5. The navigation method according to claim 4, characterized in that: Step 4 includes: Step 4.1: Update the state equation in step one to predict the state and covariance at the next time step. Step 4.2: Update measurements based on the measurement equations constructed in Step 2. Each node updates its measurements upon receiving altimeter measurements, GNSS measurements, equivalent GNSS measurements, RTT measurements, TOA measurements, and shared TOA measurements.

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

7. A navigation system for performing the data link-based distributed inertial basis relative navigation method as described in any one of claims 1 to 6, characterized in that: The navigation system includes a state modeling module, a measurement fusion module, an information sharing module, an information update module, and a navigation correction module; The state modeling module establishes state equations for the nodes in the cluster relative to the navigation system; The measurement fusion module establishes a measurement equation relative to the navigation system for each node in the cluster based on the equivalent GNSS measurement of rapidly disseminated high-precision absolute navigation information. The information sharing module is based on a time-division multiple access round-robin broadcast communication mechanism, where each node in the cluster shares information data packets. The information update module is used to construct time update equations and measurement update equations, and to perform filter time updates and measurement updates. The correction navigation module uses the state error estimated by the filter as feedback to correct 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, thus obtaining the final navigation result.

8. An electronic device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the method according to any one of claims 1 to 6.

9. A computer-readable storage medium for storing computer instructions, characterized in that, When the computer instructions are executed by the processor, they implement the steps of the method according to any one of claims 1 to 6.

Images