Relative navigation method and system suitable for measurement asynchronization and communication delay processing

Abstract

The invention discloses a relative navigation method and system suitable for measurement asynchronization and communication delay processing, and relates to the field of navigation. The method solves the problems of measurement asynchronization, communication delay and the like of an existing measurement sharing relative navigation method in practical application. The method comprises the following steps: establishing a total error state equation of a relative navigation system for nodes in a cluster; establishing a multi-source measurement equation of a relative navigation system for the nodes in the cluster; based on a time division multiple access in-turn broadcast type communication mechanism, information data packets are shared by each node in the cluster; constructing a time updating equation and a measurement updating equation, and performing filter time updating and measurement updating; carrying out inertial navigation position extrapolation aiming at measurement asynchronization and communication delay to realize accurate alignment of measurement information containing lagging and an inertial navigation position in time; correcting the error feedback and outputting a relative navigation result. The method is also suitable for the application fields of relative navigation algorithms for measuring asynchronization and communication delay processing and the like.

Description

Technical Field

[0001] This invention relates to the field of navigation technology, and more specifically to a relative navigation method and system suitable for handling asynchronous measurement and communication delays. Background Technology

[0002] Unmanned aerial vehicle (UAV) swarms can efficiently complete tasks such as large-scale reconnaissance and parallel operations, achieving adaptation to complex dynamic environments and efficient and reliable execution of complex tasks. Accurate and reliable relative positioning is a crucial prerequisite and important guarantee for the successful collaborative operation of the swarm. Currently, most swarm systems still rely on the Global Navigation Satellite System (GNSS) to obtain accurate absolute navigation information. However, in complex environments such as indoors, cities, forests, and canyons, GNSS signals may be completely lost or intermittently lost, making it difficult for this method to reliably achieve relative navigation. Relative navigation (RN), which measures the round-trip time (RTT) and time of arrival (TOA) of signals to achieve accurate relative clock and relative distance measurements, is an effective solution for scenarios where GNSS is completely or intermittently denied. Combined with inertial navigation systems (INS), altimeters, and other autonomous navigation sensors, it can achieve relative positioning and timing of swarm nodes, thereby ensuring the system's swarm operation capability.

[0003] To improve relative navigation accuracy, a measurement-sharing-based relative navigation method has been proposed in existing technologies. In this method, each node in the cluster employs time-division multiple access (TDMA) communication, achieving ordered broadcasting through pre-defined time slot planning. Each node only packages its own navigation estimate, clock information (forming a TOA measurement at the receiver), and all shared TOA measurements from the most recent round (including the corresponding navigation estimate) into a data packet at a fixed allocated broadcast time. This data packet is then shared with other nodes in the network, enabling them to utilize these measurements for information fusion. However, this method still faces two main problems in practical engineering applications: (1) Asynchronous measurement problem. Since the data link terminal is connected to sensors such as inertial navigation system, altimeter, and GNSS receiver via bus, there is a delay of tens to hundreds of milliseconds in the data transmission from the sensor to the data link processing software. This causes the measurement time to lag behind the current processing time. If the current inertial navigation position is used directly for measurement update, it will introduce time alignment deviation and cause inaccurate navigation error correction.

[0004] (2) Communication delay problem. From the time the broadcast node generates a data packet to the time the receiving node actually uses the packet for TOA measurement fusion, it goes through the process of message packaging, broadcasting, receiving and processing, which will also generate a delay of tens of milliseconds. This causes the time corresponding to the navigation estimated position shared by the broadcaster to lag behind the time of TOA measurement generation. If no compensation is made, it will cause errors in the relative TOA measurement fusion and affect the accuracy of relative position estimation.

[0005] In summary, this invention primarily addresses the asynchronous measurement issues caused by asynchronous sensor sampling and the non-fixed communication delays resulting from cross-node data transmission in practical applications of existing measurement-sharing relative navigation methods. These problems lead to misalignment between measurement information and inertial navigation state on the time axis, resulting in filter update deviations and severely impacting relative positioning accuracy. Summary of the Invention

[0006] To overcome the problems of asynchronous measurement and communication delay in existing relative navigation methods based on measurement sharing, which can easily lead to inaccurate navigation error correction and affect the accuracy of relative position estimation, this invention proposes a relative navigation algorithm suitable for handling asynchronous measurement and communication delay. This algorithm utilizes sensor timestamps and message generation times to construct an inertial navigation system (INS) position extrapolation compensation mechanism, achieving precise alignment of spatiotemporal information without the need for additional complex synchronization hardware. This effectively eliminates the time deviation caused by asynchrony and delay, ensuring high accuracy and reliability of relative navigation state estimation and improving the algorithm's engineering applicability. Specifically, this invention proposes a relative navigation algorithm suitable for handling asynchronous measurement and communication delay. By using sensor timestamps and message generation times for targeted INS position extrapolation compensation during measurement processing, it effectively eliminates the time deviation caused by asynchrony and delay, ensuring the reliability of navigation error correction and the accuracy of relative TOA measurement updates. Simultaneously, it maintains high-precision relative navigation state estimation and improves the algorithm's engineering applicability without the need for additional complex synchronization hardware.

[0007] To solve the above-mentioned technical problems, the present invention is achieved through the following technical solution: Option 1: This invention proposes a relative navigation method suitable for asynchronous measurement and communication delay processing, the method comprising the following steps: Step 1: Establish the full error state equations for the nodes in the cluster relative to the navigation system; Step 2: Establish multi-source measurement equations relative to the navigation system for the nodes in the cluster; 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 time update equations to update the filter time; Step 5: Extrapolate the inertial navigation position to account for asynchronous measurement and communication delay, so as to achieve accurate time alignment between the measurement information with lag and the inertial navigation position. In measurement processing, the sensor's built-in timestamp is used to extrapolate the current inertial navigation position to the time of generation on both sides to achieve precise alignment; in communication delay processing, the message generation time is added to the P message and extrapolated to the TOA generation time. Step 6: Construct measurement update equations to update filter measurements; Step 7: Perform error feedback correction and output relative navigation results for the updated measurement information from Step 6.

[0008] Furthermore, a preferred embodiment is provided, wherein step 1 specifically includes: Step 1.1: Select Nodes The state variables in the model, the navigation state of node i Based on its own navigation state and others The navigation error state of each node is constituted, and the calculation method is as follows:

[0009] in, Represents a node Navigation error status, , Represents a node i The modeling of the rest besides itself n The node in the node j The state of each node ; Step 1.2, corresponding to the above global error state, node The global process noise vector is defined as:

[0010] in, For nodes The process noise of self-navigation error includes random drift of inertial devices, data link clock noise and GNSS receiver clock noise; For nodes For other nodes Equivalent process noise introduced during state modeling; Step 1.3: Based on the full-error state-space modeling method, redefine the nodal northeast-sky velocity error:

[0011] in, , , This represents the traditional velocity error vector; This represents the inertial navigation system solution result; Step 1.4, Node-based The state variables and noise variables in the model are used to construct the global error discrete-time propagation equation, which has the following form:

[0012] in, and Representing nodes respectively In time and time The state vector; and They represent time. The state transition matrix and the noise driving matrix; The variance is The system's white noise.

[0013] Furthermore, a preferred embodiment is provided, wherein the multi-source measurement in step 2 includes RTT clock synchronization measurement, direct TOA ranging measurement, and shared TOA ranging measurement. The RTT clock synchronization measurement is used to measure the clock difference between the data links of the broadcast node and the receiving node. The direct TOA ranging measurement is used when the node Received from node Direct TOA measurements are generated during broadcasting; The shared TOA ranging measurement is used to set nodes. At any moment Obtain a message about the node TOA measurement, and at time The measurement and related information will be broadcast together; node After receiving the message, TOA measurement at time and current time The state establishes a correspondence, that is, when a node needs to use the TOA measurement information between other nodes, it uses the shared TOA measurement model to obtain the information.

[0014] Furthermore, a preferred embodiment is provided, wherein step 2 specifically includes: Step 2.1: Construct a barometric altimeter measurement model.

[0015] in, It is an altitude estimate based on INS indications; It is the white noise of the altimeter measurement, which obeys... ; Step 2.2: Construct a GNSS pseudorange measurement model, when the node When a GNSS signal is received, its pseudorange measurement equation can be expressed as:

[0016] in, Represents a node exist The position below; Indicates that the satellite is The position below; Represents the speed of light; This represents the white noise of GNSS pseudorange measurements, which follows the... distributed.

[0017] Furthermore, a preferred embodiment is provided, wherein step 4 specifically includes: Step 4.1: Update the time based on the total error state equation constructed in Step 1; Step 4.2: Propagate the error covariance matrix based on step 4.1.

[0018] Furthermore, a preferred embodiment is provided, wherein step 6 includes: Step 6.1: Perform relative measurement updates based on the measurement equations constructed in Step 2. Each node performs corresponding partial measurement updates when it receives RTT measurements, direct TOA ranging measurements, and shared TOA ranging measurements. The partial update strategy only performs accurate updates on sub-states directly related to the measurements, while the other sub-states retain their prior values. Step 6.2: Update the absolute measurements based on the constructed measurement equations.

[0019] Furthermore, a preferred embodiment is provided, wherein step 7 specifically includes: Step 7.1, Node The error estimated by the filter is fed back to the inertial navigation system to correct its output absolute state information; Step 7.2: After obtaining the absolute state estimates of all nodes, the nodes... Calculate its value for any node The relative state.

[0020] Option 2: A relative navigation system suitable for asynchronous measurement and communication delay processing, the system being implemented based on the method described in Option 1, the system comprising: The input module is used to establish the full error state equations relative to the navigation system for the nodes in the cluster. The multi-source measurement module is used to establish multi-source measurement equations relative to the navigation system for nodes in the cluster. The information sharing module is used for information data packets shared by each node in the cluster based on a time-division multiple access round-robin broadcast communication mechanism; The time update module is used to construct time update equations for filter time updates; The collaborative alignment module is used to extrapolate the inertial navigation position for asynchronous measurements and communication delays, so as to achieve accurate temporal alignment between the measurement information with hysteresis and the inertial navigation position. In measurement processing, the sensor's built-in timestamp is used to extrapolate the current inertial navigation position to the time of generation on both sides to achieve precise alignment; in communication delay processing, the message generation time is added to the P message and extrapolated to the TOA generation time. The measurement update module is used to construct measurement update equations for filter measurement updates. The output module is used to perform error feedback correction and output relative navigation results for the updated measurement information in the measurement update module.

[0021] Option 3: A computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the method described in Option 1.

[0022] Option 4: A computer device, including a memory and a processor, wherein the memory stores a computer program, and when the processor runs the computer program stored in the memory, the processor executes the method described in Option 1.

[0023] The advantages of this invention are: This invention proposes a multi-UAV cooperative relative navigation algorithm applicable to the problems of asynchronous measurement and communication delay in practical engineering. By introducing a targeted time alignment compensation mechanism in measurement processing and message design, the algorithm effectively eliminates the error effects caused by asynchrony and delay, ensuring the accuracy and reliability of navigation error correction and relative TOA measurement updates.

[0024] This invention addresses the problem of asynchronous measurements. It utilizes the timestamp inherent in the sensor data packet to extrapolate the current inertial navigation system (INS) position to the actual measurement generation time, achieving precise temporal alignment between measurement information and INS position. This correctly corrects navigation errors and avoids measurement update deviations caused by time asynchrony in traditional methods. Regarding communication delays, this invention adds a message generation time field to the P message, enabling the receiver to extrapolate the broadcaster's estimated navigation position to the TOA generation time, ensuring correct fusion and updating of relative TOA measurements even with communication delays.

[0025] In summary, this invention can maintain high-precision relative navigation state estimation in real-world engineering environments, avoiding the negative impact of time alignment errors on cluster cooperative positioning performance. Furthermore, it requires no additional complex synchronization hardware support, is easy to implement in engineering, and possesses good versatility in multi-UAV cooperative missions where GNSS is restricted or denied.

[0026] This invention is also applicable to fields such as relative navigation algorithms that handle asynchronous measurement and communication delays. Attached Figure Description

[0027] Figure 1 This is a simulated cluster running trajectory diagram in the relative navigation method applicable to asynchronous measurement and communication delay processing as described in Implementation Method 1.

[0028] Figure 2 This is a schematic diagram of the parameter chart used in Implementation Method 1.

[0029] Figure 3 This is a flowchart of the relative navigation method applicable to asynchronous measurement and communication delay processing as described in Implementation Method 1.

[0030] Figure 4 This is a flowchart of the asynchronous measurement and communication delay model and processing method described in Implementation Method 1.

[0031] Figure 5 The diagram shows the relative position estimation accuracy under different engineering problems and processing methods provided in Implementation Method 1. Detailed Implementation

[0032] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of them.

[0033] Implementation Method 1, see [link] Figures 1 to 5 This embodiment describes a relative navigation method suitable for handling asynchronous measurements and communication delays. The method specifically includes the following steps: (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. (2) Set up the relative navigation experimental environment, and set the parameters of each sensor as follows: Figure 2 As shown.

[0034] (3) Deploy a relative navigation algorithm suitable for asynchronous measurement and communication delay processing and verify the results.

[0035] like Figure 3 The diagram shows the flowchart of the core algorithm for relative navigation, taking node i as an example. The specific steps are as follows: Step 1 above: Establish the full error state equations for the nodes in the cluster relative to the navigation system, specifically as follows: (1.1) Filtering state: Select node The modeled state variables include the state of the model itself and the states of other nodes: Assuming there are a total of Each node, with nodes For example, the navigation status 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:

[0036] 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 ( ).

[0037] (1.1.1) Its own state is defined as follows: node Self-navigation error status Taken as: SINS latitude and longitude error SINS Northeast-to-Heavy Velocity Error SINS three-axis attitude misalignment angle JTIDS receiver clock phase and frequency error GNSS receiver clock phase and frequency error . There are 13 dimensions in total, as shown below:

[0038] (1.1.2) The states of other nodes are defined as follows: node The modeling of the rest besides itself The node in the node The state of each node Taken as: SINS horizontal geographic location error SINS horizontal geographic velocity error SINS attitude misalignment angle JTIDS receiver clock phase and frequency error . There are 9 dimensions in total, as shown below:

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

[0040] in, , 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 is the output value of the inertial navigation system at node i. It is the northeastern sky speed output by node i.

[0041] This definition differs from traditional velocity error. By transforming the velocity error state in the traditional EKF model of an integrated navigation system, a new velocity error state is used to replace the original 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 depend 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.

[0042] (1.2) Noise filtering: Selecting nodes The noise level in the model includes its own noise and noise from other nodes: Assuming there are a total of Each node, with nodes For example, the noise level of this node It consists of its own noise and the noise of other nodes being modeled, and its specific form is shown below:

[0043] 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 ( ).

[0044] (1.2.1) Self-noise is defined as follows: node Process noise of self-navigation error The random error of the three-axis accelerometer and gyroscope of Northeast Tian is taken as: Noise status of the JTIDS receiver Noise status of GNSS receiver . There are 8 dimensions in total, as shown below:

[0045] (1.2.2) Other node noise is defined as follows: node The modeling of the rest besides itself The node in the node Process noise at each node The random error of the three-axis accelerometer and gyroscope of Northeast Tian is taken as: Noise status of the JTIDS receiver . There are 7 dimensions in total, as shown below:

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

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

[0048]

[0049]

[0050]

[0051]

[0052]

[0053]

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

[0055]

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

[0057] Among them, By removing the dimensions corresponding to the celestial velocity, altitude, and GNSS receiver clock phase and frequency error from the matrix, we can obtain... , The specific components are as follows:

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

[0059]

[0060] 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., the matrix... 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.

[0061] (1.4) System State Model 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:

[0062] By selecting an appropriate discretization time , system matrix and noise-driven array Discretization is performed to obtain nodes State transition matrix of self-navigation error and noise-driven array :

[0063] Step 2 above: Establish multi-source measurement equations relative to the navigation system for the nodes in the cluster, specifically as follows: Based on the above-mentioned multi-center error state modeling, this invention establishes a unified relative measurement and absolute measurement model in the same coordinate system, including: 1. RTT bidirectional ranging clock synchronization measurement model For nodes With nodes The RTT measurement between them can be expressed as a function of the round-trip time and the local clock reading, which can be simplified as follows:

[0064] in, The white noise representing the RTT measurement follows a certain order. Distribution. After linearization, the error state can be obtained. The observation equation.

[0065] 2. Absolute Measurement Model of Barometric Altimeter No. The barometric altimeter observations at each node can be expressed as:

[0066] in, For altitude estimation of inertial navigation indication; For altimeter measurement white noise, follow .

[0067] 3. GNSS and equivalent GNSS absolute measurement model When node When GNSS satellite signals can be received, the pseudorange measurement equation can be expressed as:

[0068] in, Represents a node exist The position below; Indicates that the satellite is The position below; Represents the speed of light; This represents the white noise of GNSS pseudorange measurements, which follows the... distributed.

[0069] In challenging GNSS environments, only a subset of nodes can acquire GNSS measurements in real time. To enable the rapid dissemination of absolute navigation information in multi-center relative navigation systems, this invention constructs an equivalent GNSS measurement model, allowing high-precision position error estimates of GNSS-enabled nodes to be used as "pseudo-GNSS measurements" by other nodes.

[0070] When node At any moment After obtaining GNSS measurements, the horizontal position error can be estimated. and the corresponding covariance This was then broadcast via data link as an equivalent GNSS measurement. For the time... Any node that receives this message can establish the following equivalent GNSS measurement model:

[0071] 4. Direct / Indirect TOA Relative Distance Measurement Model For one-way ranging measurements based on TOA, the observed quantity can be written as:

[0072] When using the indirect TOA method, set the node At any moment Get a message about the node TOA measurement, and at time The measurement and related information are broadcast together via data link; node After receiving the message, TOA measurement at time and current time By establishing a correspondence between the states, an indirect TOA measurement can be constructed, which can be written in the form of:

[0073] Step 3 above: Based on time-division multiple access (TDMA) round-robin broadcast communication, each node in the cluster shares information data packets, specifically as follows: This invention is based on a time-division multiple access (TDMA) round-robin broadcast communication mechanism, where each node in the cluster sequentially occupies the channel and sends data packets according to a preset order and broadcast period. The information data packets shared by each node in the cluster are shown in the table below:

[0074] Table 1. Information data shared by each node Step 4 above: Constructing the time update equation for filter time update, specifically as follows: The specific formula for calculating the one-step prediction of the state is as follows:

[0075] in, For the first The state prediction matrix of each node in one step For nodes Model the state transition matrix of its own state. For nodes The modeling of the rest besides itself The node in the node The state transition matrix of each node.

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

[0077] in, For the first The one-step prediction error covariance matrix corresponding to the state of each 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. For nodes Model the state transition matrix of its own state. For nodes The modeling of the rest besides itself The node in the node The state transition matrix of each node and This represents the noise driving matrix corresponding to the modeling state.

[0078] Step 5 above: Extrapolate the inertial navigation system (INS) position to account for measurement asynchrony and communication delay, achieving precise temporal alignment between the delayed measurement information and the INS position. Specifically: In practical engineering, factors such as communication delay and asynchronous measurement exist and need to be considered and addressed in the design of relative navigation algorithms. This invention develops a relative navigation algorithm suitable for handling asynchronous measurement and communication delay, which is used to solve these two types of practical engineering problems.

[0079] Asynchronous measurement: such as Figure 4 As shown, the data link terminal and sensors such as GPS receivers, altimeters, and inertial navigation systems are connected via a bus. The transmission of data from these sensors to the data link terminal via the bus causes delays ranging from tens to hundreds of milliseconds. Specifically, the inertial navigation system delay is in the tens of milliseconds, while the GPS receiver and altimeter delay are in the hundreds of milliseconds. This causes the data link message processing software to receive the measurement generation time lag behind the current time. To address this issue, this solution utilizes the measurement generation time (…) in the data packet sent by the sensor to the terminal. Figure 4 At time t1, the current inertial navigation position ( Figure 4 Extrapolate from time t2 to the measurement generation time, and use the measurement information containing lag to correctly correct the navigation error at the current time.

[0080] Specifically, regarding the asynchronous measurement issue, the local machine The current inertial navigation measurement time t2 is extrapolated to the local GPS and altimeter measurement time t1 to compensate for the time deviation caused by the asynchronous measurement of inertial navigation and other sensors.

[0081]

[0082] in, . The longitude, latitude, and altitude at time t3, in rad, rad, and m respectively. The eastward, northward, and skyward velocities at time t3 are in m / s. The coordinates are the longitude, latitude, and altitude after the repositioning at time t4, in rad, rad, and m. t3 is the push time, i.e. the time difference of communication delay, t4 is the time when the P message is generated, and t4 is the time when the TOA is generated.

[0083] Communication delay: such as Figure 4 As shown, from the broadcaster Data link message processing software generates P messages to the receiver. The data link message processing software needs to go through multiple processes to utilize the P message, including message packaging, channel access, encoding and modulation, electromagnetic wave transmission, and decoding and demodulation, which will cause communication delays of tens of milliseconds. This causes the receiver to obtain the navigation estimated position (TOA) from the P message after a lag compared to the TOA generation time. To address this issue, this solution adds the P message generation time (…) to the P message. Figure 4 At time t3, the receiver can extrapolate the inertial navigation position sent by the broadcaster to the time of TOA generation. Figure 4 At time t4 (shared by the broadcaster), the correct update of the relative TOA measurement is ensured even with communication delays.

[0084] Specifically, regarding the communication latency issue, other machine members Received from broadcaster member at time t4 The P message generated a TOA measurement. Firstly, for broadcaster members... The communication delay issue requires extrapolating the inertial navigation position at time t3 when the P message is generated to time t4 when the TOA is generated. Secondly, for the receiver members... The measurement delay issue is that the inertial navigation data is delayed, so it is necessary to extrapolate the inertial navigation measurement data generation time t5 to the TOA generation time t4.

[0085] Firstly, the push-order processing for communication message delays is divided into two parts: broadcaster members Positioning can be performed based on the original inertial navigation position or based on the optimal estimated position.

[0086] Broadcaster inertial navigation position extrapolation:

[0087] in, . The longitude, latitude, and altitude at time t3, in rad, rad, and m respectively. The eastward, northward, and skyward velocities at time t3 are in m / s. The coordinates are the longitude, latitude, and altitude after the repositioning at time t4, in rad, rad, and m. t3 is the push time, i.e. the time difference of communication delay, t4 is the time when the P message is generated, and t4 is the time when the TOA is generated.

[0088] Broadcaster optimal position extrapolation:

[0089] in, Highly usable directly. The calculation will not be repeated. The optimal longitude and latitude at time t3, in rad, rad, and m; The eastward and northward velocities at time t3 are in m / s. The coordinates are the longitude and latitude after the coordinate shift at time t4, in rad, rad, and m. t3 is the push time, i.e. the time difference of communication delay, t4 is the time when the P message is generated, and t4 is the time when the TOA is generated.

[0090] Secondly, regarding the recipient members The problem of self-measurement delay in positioning processing arises because its inertial navigation data has a delay. It is necessary to adjust the inertial navigation measurement data generation time t5 ( Figure 4 Extrapolate from time t5 to time t4 when TOA is generated.

[0091]

[0092] in, . The longitude, latitude, and altitude at time t5, in rad, rad, and m. The eastward, northward, and skyward velocities at time t5 are in m / s. The coordinates are the longitude, latitude, and altitude after the repositioning at time t4, in rad, rad, and m. The time difference is the push-ahead time, i.e., the communication delay time difference. t4 is the TOA generation time, and t5 is the member's time. The moment when inertial navigation measurement data is generated.

[0093] By utilizing sensor timestamps and message generation times for targeted inertial navigation position extrapolation compensation during measurement processing, the time deviation caused by asynchronous measurement and communication delay is effectively eliminated, ensuring the reliability of navigation error correction and the accuracy of relative TOA measurement updates. At the same time, high-precision relative navigation state estimation is maintained, and the engineering practicality of the algorithm is improved without the need for additional complex synchronization hardware.

[0094] Step 6 above: Construct the measurement update equation to update the filter measurement, specifically as follows: Within each filtering cycle, the multi-center fusion nodes sequentially complete time updates and partial updates of relative measurements, specifically as follows: Assuming nodes i Obtained relevant nodes i and nodes j Measurement ,definition And make Let N-2 be the remaining cluster nodes that did not participate in the relative measurement. Then the measurement update equation is:

[0095]

[0096]

[0097]

[0098]

[0099] in, , , , . , The two approximations reduce the computational cost of this method, and can reduce it by an order of magnitude compared to traditional methods when the number of members is large.

[0100] This includes the following types of measurement information: 1. RTT clock synchronization measurement update When an RTT clock synchronization measurement between a pair of nodes is received, the inertial navigation position data after asynchronous measurement processing is used to construct the corresponding observation matrix. Only the sub-states of the two nodes related to the measurement are selected for updating. For other irrelevant sub-states, a partial update strategy that preserves prior values ​​is adopted, and the relevant covariance blocks are updated accordingly.

[0101] 2. Barometric altimeter update When a node obtains a local barometric altimeter measurement, it uses the inertial navigation position data after asynchronous measurement processing to select the corresponding node's altitude-related state components from the global state vector and performs a standard EKF update according to the altitude measurement model.

[0102] 3. GNSS Measurement Update When a node obtains direct GNSS pseudorange measurements, it uses the inertial navigation position data after asynchronous measurement processing to select the corresponding node's position error (and clock error, etc.) sub-state. Partial EKF updates are performed according to the pseudorange measurement model in step 2, while the prior values ​​of the remaining node sub-states remain unchanged, and the relevant covariance is updated. When an equivalent GNSS measurement broadcast from another node is received, the position error sub-state of the broadcasting node is partially updated according to the equivalent measurement model, while the remaining sub-states remain unchanged. Simultaneously, the covariance is updated, achieving rapid dissemination of absolute navigation information.

[0103] 4. TOA relative distance measurement section updated When a direct or indirect TOA relative ranging measurement is received, the inertial navigation position data after communication delay processing is used. Based on the measurement model established in step 2, a relative distance observation equation is constructed. After inertial navigation position extrapolation through measurement asynchrony and communication delay, only the position error, velocity error and other sub-states of the two nodes related to the measurement are partially updated. The sub-states of the remaining nodes remain unchanged with prior values, and only the corresponding cross covariance is updated.

[0104] In step 7 above, the error feedback correction and relative navigation result output are specifically as follows: 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.

[0105] like Figure 5 The figure shown is a graph of relative position estimation accuracy under different engineering problems and processing methods provided by the present invention, i.e., the output relative position error curve. The specific calculation method is as follows: Set nodes i The origin of the grid, nodes j exist k The true value of the UVW coordinates at time t is The estimated value is a node. i Regarding the filter j The estimated value is expressed as Then the cluster is Time Node i The average relative position error is calculated as follows ( (Total number of nodes in the cluster) .

[0106] This implementation proposes a relative navigation algorithm suitable for asynchronous measurement and communication delay processing. By utilizing sensor timestamps and message generation times for targeted inertial navigation position extrapolation compensation during measurement processing, the algorithm effectively eliminates the time deviation caused by asynchrony and delay, ensuring the reliability of navigation error correction and the accuracy of relative TOA measurement updates. At the same time, it maintains high-precision relative navigation state estimation and improves the engineering practicality of the algorithm without the need for additional complex synchronization hardware.

[0107] Those skilled in the art will understand that the above description is merely a preferred embodiment of the present invention, and the features described in the various embodiments and / or claims of this disclosure can be combined or combined in various ways, even if such combinations or combinations are not explicitly described in this disclosure. This is not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

[0108] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including both the preferred embodiments and all changes and modifications falling within the scope of the invention. Clearly, those skilled in the art can make various alterations and modifications to the invention without departing from its spirit and scope. Thus, if these modifications and modifications of the invention fall within the scope of the claims and their equivalents, the invention is also intended to include these modifications and modifications.

Claims

1. A relative navigation method suitable for asynchronous measurement and communication delay processing, characterized in that, The method includes the following steps: Step 1: Establish the full error state equations for the nodes in the cluster relative to the navigation system; Step 2: Establish multi-source measurement equations relative to the navigation system for the nodes in the cluster; 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 time update equations to update the filter time; Step 5: Extrapolate the inertial navigation position to account for asynchronous measurement and communication delay, so as to achieve accurate time alignment between the measurement information with lag and the inertial navigation position. In measurement processing, the sensor's built-in timestamp is used to extrapolate the current inertial navigation position to the time of generation on both sides to achieve precise alignment; in communication delay processing, the message generation time is added to the P message and extrapolated to the TOA generation time. Step 6: Construct measurement update equations to update filter measurements; Step 7: Perform error feedback correction and output relative navigation results for the updated measurement information from Step 6.

2. The relative navigation method for handling asynchronous measurement and communication delays according to claim 1, characterized in that, Step 1 specifically includes: Step 1.1: Select Nodes The state variables in the model, the navigation state of node i Based on its own navigation state and others The navigation error state of each node is constituted, and the calculation method is as follows: in, Represents a node Navigation error status, , Represents a node i The modeling of the rest besides itself n The node in the node j The state of each node ; Step 1.2, corresponding to the above global error state, node The global process noise vector is defined as: in, For nodes The process noise of self-navigation error includes random drift of inertial devices, data link clock noise and GNSS receiver clock noise; For nodes For other nodes Equivalent process noise introduced during state modeling; Step 1.3: Based on the full-error state-space modeling method, redefine the nodal northeast-sky velocity error: in, , , This represents the traditional velocity error vector; This represents the inertial navigation system solution result; Step 1.4, Node-based The state variables and noise variables in the model are used to construct the global error discrete-time propagation equation, which has the following form: in, and Representing nodes respectively In time and time The state vector; and They represent time. The state transition matrix and the noise driving matrix; The variance is expressed as The system's white noise.

3. The relative navigation method for handling asynchronous measurement and communication delays according to claim 1, characterized in that, The multi-source measurement in step 2 includes RTT clock synchronization measurement, direct TOA ranging measurement, and shared TOA ranging measurement. The RTT clock synchronization measurement is used to measure the clock difference between the data links of the broadcast node and the receiving node. The direct TOA ranging measurement is used when the node Received from node Direct TOA measurements are generated during broadcasting; The shared TOA ranging measurement is used to set nodes. At any moment Obtain a message about the node TOA measurement, and at time The measurement and related information will be broadcast together; node After receiving the message, TOA measurement at time and current time The state establishes a correspondence, that is, when a node needs to use the TOA measurement information between other nodes, it uses the shared TOA measurement model to obtain the information.

4. The relative navigation method for handling asynchronous measurement and communication delays according to claim 3, characterized in that, Step 2 specifically includes: Step 2.1: Construct a barometric altimeter measurement model. in, It is an altitude estimate based on INS indications; It is the white noise of the altimeter measurement, which obeys... ; Step 2.2: Construct a GNSS pseudorange measurement model, when the node When a GNSS signal is received, its pseudorange measurement equation is expressed as: in, Represents a node exist The position below; Indicates that the satellite is in The position below; Represents the speed of light; This represents the white noise of GNSS pseudorange measurements, which follows the... distributed.

5. The relative navigation method for handling asynchronous measurement and communication delays according to claim 1, characterized in that, Step 4 specifically includes: Step 4.1: Update the time based on the total error state equation constructed in Step 1; Step 4.2: Propagate the error covariance matrix based on step 4.

1.

6. The relative navigation method for handling asynchronous measurement and communication delays according to claim 1, characterized in that, Step 6 includes: Step 6.1: Perform relative measurement updates based on the measurement equations constructed in Step 2. Each node performs corresponding partial measurement updates when it receives RTT measurements, direct TOA ranging measurements, and shared TOA ranging measurements. The partial update strategy only performs accurate updates on sub-states directly related to the measurements, while the other sub-states retain their prior values. Step 6.2: Update the absolute measurements based on the constructed measurement equations.

7. The relative navigation method for handling asynchronous measurement and communication delays according to claim 1, characterized in that, Step 7 specifically includes: Step 7.1, Node The error estimated by the filter is fed back to the inertial navigation system to correct its output absolute state information; Step 7.2: After obtaining the absolute state estimates of all nodes, the nodes... Calculate its value for any node The relative state.

8. A relative navigation system suitable for asynchronous measurement and communication delay processing, characterized in that, The system is implemented based on the method of claim 1, and the system includes: The input module is used to establish the full error state equations relative to the navigation system for the nodes in the cluster. The multi-source measurement module is used to establish multi-source measurement equations relative to the navigation system for nodes in the cluster. The information sharing module is used for information data packets shared by each node in the cluster based on a time-division multiple access round-robin broadcast communication mechanism; The time update module is used to construct time update equations for filter time updates; The collaborative alignment module is used to extrapolate the inertial navigation position for asynchronous measurements and communication delays, so as to achieve accurate temporal alignment between the measurement information with hysteresis and the inertial navigation position. In measurement processing, the sensor's built-in timestamp is used to extrapolate the current inertial navigation position to the time of generation on both sides to achieve precise alignment; in communication delay processing, the message generation time is added to the P message and extrapolated to the TOA generation time. The measurement update module is used to construct measurement update equations for filter measurement updates. The output module is used to perform error feedback correction and output relative navigation results for the updated measurement information in the measurement update module.

9. A computer storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the method described in any one of claims 1-7.

10. A computer device, characterized in that, include: A memory, a processor, and a computer program stored in the memory and executable on the processor, the processor executing the program to implement the method of any one of claims 1-7.

Images