A consistent multi-center relative navigation method under resource constraints

Abstract

The application is a consistent multi-center relative navigation method under resource limitation. The application relates to the technical field of relative navigation, and constructs a multi-center relative navigation error state modeling and a global error discrete time propagation equation; establishes an absolute measurement and a relative measurement model; constructs a process noise covariance upper bound matrix based on a maximum speed boundary; performs consistent multi-center extended Kalman filtering based on the upper bound matrix; and outputs error feedback correction and relative navigation results. The method can realize consistent propagation of process noise without relying on high-frequency information of other machines, significantly improves the stability and robustness of multi-center relative navigation, and has important practical application value.

Description

Technical Field

[0001] This invention relates to the field of relative navigation technology, and is a consistent multi-center relative navigation method for resource-constrained environments. Background Technology

[0002] Compared to single drones, drone swarms offer higher efficiency and greater robustness, and accurate and reliable relative position information is crucial for achieving tasks such as formation flight, target tracking, coordinated reconnaissance, and coordinated jamming. In existing engineering projects, drones typically rely on absolute position information provided by the Global Navigation Satellite System (GNSS). However, in obstructed environments such as cities and canyons, or in scenarios with strong electromagnetic interference / deception, GNSS signals are easily interfered with, making it difficult to guarantee positioning accuracy and continuity, thus weakening swarm coordination capabilities. Therefore, relative navigation (RN) has become the mainstream approach in GNSS-denied environments: by fusing Inertial Navigation System (INS) information, inter-drone relative measurements, and available absolute measurements, accurate estimation of the relative positions between swarm nodes can be achieved.

[0003] Compared to traditional relative navigation schemes, multi-center relative navigation based on measurement sharing can fully utilize all information in the cluster, maintaining formation geometry constraints and relative positioning accuracy even when GNSS is unavailable, thanks to inter-machine relative measurements. Within this framework, each UAV needs to simultaneously predict its own state and the states of other nodes, while the process noise driver in the time update depends on the real-time motion information of each node, such as velocity and attitude. Due to factors such as communication bandwidth, link latency, and packet loss, the states of other UAVs are difficult to obtain in a timely and reliable manner. In engineering practice, a delayed state approximation is often used, leading to an underestimation of the process noise covariance and overconfidence in the filter. This can even cause filter divergence when relative measurements are insufficient or maneuvers are enhanced. Furthermore, in a multi-center architecture, each node needs to predict and update the states of all nodes. If online calculation is still performed in the traditional way, the computational burden will increase significantly, further limiting the deployability of the algorithm in practical systems.

[0004] In summary, for multi-center relative navigation systems of UAV swarms with limited communication and computing resources, the traditional filtering time update framework has the following two main problems: (1) the process noise covariance matrix depends on the real-time information of other nodes, the covariance propagation is prone to distortion, and the consistency of filtering is difficult to guarantee; (2) the multi-center prediction computation overhead is large, and it is difficult to balance real-time performance and deployability. Summary of the Invention

[0005] This invention aims to solve the problem of covariance propagation distortion and filtering inconsistency caused by the difficulty in accurately and synchronously obtaining the noise driving matrix during the time update process of multi-center relative navigation under limited communication bandwidth and computing resources. It discloses a consistent multi-center relative navigation method for resource-constrained conditions.

[0006] This invention provides the following technical solutions: A consistent multi-center relative navigation method for resource-constrained environments, the method comprising the following steps: Step 1: Construct multi-center relative navigation error state modeling and global error discrete-time propagation equation; Step 2: Establish absolute and relative measurement models; Step 3: Construct the upper bound matrix of process noise covariance based on the maximum velocity boundary; Step 4: Uniform multicenter extended Kalman filter based on upper bound matrix; Step 5: Output error feedback correction and relative navigation results.

[0007] Preferably, step 1 specifically comprises: Step 1.1, configure the cluster to have a total of Deploy drones, as nodes As the fusion center of multi-center relative navigation filters, it simultaneously models itself and others in the filters of each node. The navigation error status of each node, node global error state vector Recorded as:

[0008] in, Represents a node Navigation error status; Represents a node Other nodes within the modeled cluster The navigation error status is as follows:

[0009]

[0010] in, and They represent drones and other nodes modeled Longitude error, latitude error, and altitude error; and They represent drones and other nodes modeled The speed error in the northeast sky; and They represent drones and other nodes modeled The angle of attitude misalignment; and They represent drones and other nodes modeled Data link receiver clock phase and frequency error; and They represent drones and other nodes modeled GNSS receiver clock phase and frequency error; Step 1.2, corresponding to the global error state described above, node The global process noise vector is defined as:

[0011] 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: The traditional velocity error is reconstructed using a full error state-space modeling method based on the right-invariant error of the Lie group.

[0012] in, at the same time ; This represents the traditional velocity error vector; This represents the inertial navigation system solution result; Step 1.4: Construct the global error discrete-time propagation equation, specifically in the following form:

[0013] 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.

[0014] Preferably, step 2 specifically comprises: Step 2.1, construct the barometric altimeter measurement model, whose equation is expressed as:

[0015] in, For altitude estimation of inertial navigation indication; For altimeter measurement white noise, follow ; Step 2.2, Build Nodes The GNSS pseudorange measurement model, when the node When a GNSS signal is received, its pseudorange measurement equation is expressed as:

[0016] in, Represents a node exist The position below; Indicates that the satellite is in The position below; Represents the speed of light; The Gaussian white noise representing GNSS pseudorange measurements obeys the following rules: distributed; Step 2.3, Build Nodes The RTT clock synchronization measurement model; RTT measurement directly reflects the clock difference between the data links of the broadcast node and the receiving node; with the time reference node NTR as a reference, the node... The RTT measurement equation for NTR is expressed as follows:

[0017] in, The white noise representing the RTT measurement follows a certain order. distributed; Step 2.4, Build Nodes The direct TOA measurement model, when the node Received from node The broadcast generates a direct TOA measurement; a direct TOA measurement is a relative measurement, its physical meaning being the signal propagation time, corresponding to the spatial distance between nodes, and the measurement equation is expressed as:

[0018] in, White noise measured for TOA, obeying distributed; For nodes The horizontal position error, expressed by the Jacobian matrix, is:

[0019] Step 2.5, Build Nodes An indirect TOA measurement model is introduced to avoid real-time broadcasting of every TOA measurement when communication resources are limited and TOA measurements are frequent.

[0020] Preferably, nodes are set. 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 If a correspondence is established between the states, then the indirect TOA measurement is represented as: .

[0021] Preferably, step 3 specifically comprises: Step 3.1, for each node in the cluster Set the maximum velocity vector:

[0022] in, and Representing nodes respectively The upper bounds of the maximum eastward, northward, and celestial velocity components in the navigation coordinate system; the upper bounds are given by mission requirements, flight control limitations, historical statistics, or safety redundancy, and satisfy the upper bound constraint on the absolute value of the actual velocity components:

[0023] Step 3.2: Construct the velocity correlation matrix based on the maximum velocity boundary vector. This is used to characterize the amplification effect of velocity-related terms on inertial navigation process noise under maximum maneuver conditions;

[0024] in, Defined as:

[0025]

[0026] Step 3.3: Based on the above parameters, construct the nodes. The inertial navigation process noise covariance submatrix:

[0027] in, and These represent the single-axis or average noise variance of the accelerometer and gyroscope, respectively. Defined as:

[0028] Step 3.4: Construct the overall process noise covariance matrix for each node. ,Will By combining the process noise terms with those related to clock drift and other states, we obtain the nodal process noise covariance matrix. Then all nodes The overall process noise covariance matrix is ​​composed of blocks in a diagonal form. :

[0029]

[0030] Step 3.5, for the fusion center node The filter state vector is defined by, from Delete nodes Calculate the corresponding rows and columns of the altitude and ascent speed of all external drones. .

[0031] Preferably, step 4 specifically comprises: Step 4.1: Perform block filter time update based on the global error state model and discretized state transition matrix established in Step 1. ,node At any moment The prior state estimate is:

[0032] By using a block diagonal structure, the above high-dimensional operations can be decomposed into low-dimensional operations on each sub-block:

[0033] Step 4.2, use the upper bound matrix obtained in step 3. To propagate the error covariance matrix, divide the covariance matrix into sub-blocks based on node indices:

[0034] Propagate the covariance matrix:

[0035] The upper bound of the entire variance term is derived using the expected maximum speed of the drone. Based on this upper bound, the state propagation equation can be rewritten as:

[0036] Step 4.3, the method for partially updating relative measurements; in Time Node Get a link to the node State-related relative measurements The unified representation is:

[0037] in, Represents a relative measurement function; This represents Gaussian white noise, i.e. ;make

[0038]

[0039] These represent the sets of nodes participating in the relative measurement and the sets of nodes not participating in the relative measurement, respectively. A set of nodes, then in the nodes In the filter, the global state is divided into Rewritten as:

[0040]

[0041] The EKF update steps for the complete error state model are represented as follows:

[0042] Step 4.4: Update the absolute measurement based on the constructed measurement equation, when the node... When obtaining altitude measurements or precise GNSS measurements, a partial update method similar to that used for relative measurements is employed. Step 4.5 integrates time updates, relative measurement updates, and absolute measurement updates to form the efficient multi-center extended Kalman filter process of this invention.

[0043] Preferably, the sub-states not involved in the measurement are approximated using a partial update:

[0044] This update strategy only updates the sub-states that are directly related to the measurement, while keeping the prior values ​​of the remaining sub-states. This significantly reduces the computational complexity of relative measurement updates while ensuring the consistency of the estimates.

[0045] Preferably, step 5 specifically comprises: Step 5.1, Node The error estimated by the filter is fed back to the inertial navigation system to correct its output absolute state information; Step 5.2, after obtaining the absolute state estimates of all nodes, the nodes... Calculate its value for any node The relative state.

[0046] A computer-readable storage medium having a computer program stored thereon, which is executed by a processor to implement a consistent multicenter relative navigation method for resource-constrained environments.

[0047] A computer device includes a memory and a processor, the memory storing a computer program, the processor executing the computer program to implement a consistent multi-center relative navigation method under resource constraints.

[0048] The present invention has the following beneficial effects: This invention addresses the multi-center relative navigation problem in UAV swarms under conditions of limited communication and computing resources, proposing a consistent multi-center relative navigation method for resource-constrained environments. During the time-updated covariance propagation process, this method constructs an upper bound for the process noise covariance using a maximum velocity boundary, and replaces the process noise driving term in the traditional form with this upper bound matrix. This avoids the need for high-frequency motion information such as attitude, velocity, and force of other nodes, significantly reducing the communication burden and improving availability in intermittent communication scenarios. In practical applications, communication latency and packet loss make it difficult to obtain the high-frequency status of other UAVs in a timely manner. Traditional time updates are prone to underestimation of covariance, inconsistent filtering, and even filter divergence. The proposed method can achieve consistent propagation of process noise without relying on high-frequency information from other UAVs, significantly improving the stability and robustness of multi-center relative navigation, and has significant practical application value. Attached Figure Description

[0049] To more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the specific embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0050] Figure 1 A general flowchart of a consistent multi-center relative navigation method under resource constraints is provided for the implementation of this invention. Figure 2 This is the trajectory diagram used for testing in this invention; Figure 3 The parameter values ​​used in the implementation of this invention; Figure 4 Consistency curve for relative position estimation provided for implementation of the present invention; Figure 5This invention provides relative position estimation accuracy in a GNSS-completely denied environment for the implementation of the present invention. Detailed Implementation

[0051] The technical solution of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. 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.

[0052] The present invention will be described in detail below with reference to specific embodiments. Specific Implementation Example 1: according to Figures 1 to 5 As shown, the specific optimized technical solution adopted by the present invention to solve the above-mentioned technical problems is: The present invention relates to a consistent multi-center relative navigation method under resource constraints.

[0054] This invention aims to address the problem of covariance propagation distortion and filtering inconsistency caused by the difficulty in accurately and synchronously acquiring the noise driving matrix during the time update process of multi-center relative navigation under limited communication bandwidth and computing resources. It proposes a consistent multi-center relative navigation method for resource-constrained conditions.

[0055] This method introduces a maximum velocity boundary and constructs an upper bound matrix offline that can be used to replace the process noise driving term, ensuring that the noise driving term is conservatively estimated at any given time. This achieves consistent time updates for multi-center filtering without increasing communication overhead or requiring real-time transmission of high-frequency attitude and velocity information from other nodes. The method includes the following steps: Step 1: Multi-center relative navigation error state modeling and global error discrete-time propagation equation construction; Step 2: Establishment of absolute and relative measurement models; Step 3: Construct the upper bound matrix of process noise covariance based on the maximum velocity boundary; Step 4: Uniform multicenter extended Kalman filter based on upper bound matrix; Step 5: Error feedback correction and relative navigation result output.

[0056] Further, in step 1, Step 1.1, assuming there are a total of Deploy drones. (Based on nodes) For example, this node is used as the fusion center of the multi-center relative navigation filter of this invention, and the filter at this node simultaneously models itself and others. Navigation error status of each node. global error state vector Recorded as:

[0057] Furthermore, Represents a node Navigation error status; Represents a node Other nodes within the modeled cluster The navigation error status is as follows:

[0058]

[0059] in, and They represent drones and other nodes modeled Longitude error, latitude error, and altitude error; and They represent drones and other nodes modeled The speed error in the northeast sky; and They represent drones and other nodes modeled The angle of attitude misalignment; and They represent drones and other nodes modeled Data link receiver clock phase and frequency error; and They represent drones and other nodes modeled The clock phase and frequency error of the GNSS receiver.

[0060] Step 1.2, corresponding to the global error state described above, node The global process noise vector is defined as:

[0061] 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.

[0062] Step 1.3, to reduce the dependence on high-frequency specific force information within the multi-center framework while ensuring the consistency of the global error state model, this invention employs a full error state space modeling method based on Lie group right-invariant error to reconstruct the traditional velocity error:

[0063] in, at the same time ; This represents the traditional velocity error vector; This represents the inertial navigation system solution result.

[0064] Step 1.4: Construct the global error discrete-time propagation equation, specifically in the following form:

[0065] 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.

[0066] Furthermore, in step 2, Step 2.1, construct the barometric altimeter measurement model, whose equation can be expressed as:

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

[0068] Step 2.2, Build Nodes GNSS pseudorange measurement model. When the node When GNSS signals can be received, the pseudorange measurement equation can be expressed as:

[0069] in, Represents a node exist The position below; Indicates that the satellite is in The position below; Represents the speed of light; The Gaussian white noise representing GNSS pseudorange measurements obeys the following rules: distributed.

[0070] Step 2.3, Build Nodes The RTT clock synchronization measurement model. RTT measurement directly reflects the clock difference between the data links of the broadcast node and the receiving node. Using the time reference node NTR as a reference, the node... The RTT measurement equation for NTR can be expressed as:

[0071] in, The white noise representing the RTT measurement follows a certain order. distributed.

[0072] Step 2.4, Build Nodes Direct TOA measurement model. When node Received from node Direct TOA measurements are generated during broadcasting. Direct TOA measurement is a relative measurement; its physical meaning is the signal propagation time, corresponding to the spatial distance between nodes. The measurement equation can be expressed as:

[0073] in, White noise measured for TOA, obeying distributed; For nodes The horizontal position error, measured by the Jacobian matrix, can be expressed as:

[0074] Step 2.5, Build Nodes An indirect TOA measurement model is introduced in this invention to avoid real-time broadcasting of every TOA measurement when communication resources are limited and TOA measurements are frequent.

[0075] 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 If a correspondence is established between the states, then indirect TOA measurement can be expressed as:

[0076] Furthermore, in step 3, Step 3.1, for each node in the cluster Set the maximum velocity vector:

[0077] in, and Representing nodes respectively The upper bounds of the maximum eastward, northward, and celestial velocity components in the navigation coordinate system. These upper bounds can be given by mission requirements, flight control limitations, historical statistics, or safety redundancy, and must satisfy upper bound constraints on the absolute values ​​of the actual velocity components.

[0078] Step 3.2: Construct the velocity correlation matrix based on the maximum velocity boundary vector. It is used to characterize the amplification effect of velocity-related terms on inertial navigation process noise under maximum maneuver conditions.

[0079]

[0080] in, It can be defined as:

[0081]

[0082] Step 3.3: Based on the above parameters, construct the nodes. The inertial navigation process noise covariance submatrix:

[0083] in, and These represent the single-axis or average noise variance of the accelerometer and gyroscope, respectively. It can be defined as:

[0084] Step 3.4: Construct the overall process noise covariance matrix for each node. ,Will By combining the process noise terms with those related to clock drift and other states, we obtain the nodal process noise covariance matrix. Then all nodes will be... The overall process noise covariance matrix is ​​composed of blocks in a diagonal form. :

[0085]

[0086] Step 3.5, for the fusion center node The filter state vector is defined by, from Delete nodes Calculate the corresponding rows and columns of the altitude and ascent speed of all external drones. .

[0087] Furthermore, in step 4, Step 4.1: Perform block filter time update based on the global error state model and discretized state transition matrix established in Step 1. ,node At any moment The prior state estimate is:

[0088] Using a block diagonal structure, the above high-dimensional operations can be decomposed into low-dimensional operations on each sub-block:

[0089] This method can significantly reduce the complexity of matrix operations, while not introducing approximation errors and ensuring the same estimation accuracy as the traditional EKF.

[0090] Step 4.2, use the upper bound matrix obtained in step 3. To propagate the error covariance matrix, divide the covariance matrix into sub-blocks based on node indices:

[0091] Propagate the covariance matrix:

[0092] The upper bound of the entire variance term is derived using the expected maximum speed of the drone. Based on this upper bound, the state propagation equation can be rewritten as:

[0093] Step 4.3, the method for partially updating relative measurements. Time Node Get a link to the node State-related relative measurements , can be uniformly represented as:

[0094] in, Represents a relative measurement function; This represents Gaussian white noise, i.e. .make

[0095]

[0096] These represent the sets of nodes participating in the relative measurement and the sets of nodes not participating in the relative measurement, respectively. A set of nodes, then in the nodes In the filter, the global state can be divided into Rewritten as:

[0097]

[0098] Furthermore, the EKF update steps for the complete error state model can be expressed as:

[0099] Substates not involved in this measurement are approximated using a partial update:

[0100] This update strategy only updates the sub-states that are directly related to the measurement, while keeping the prior values ​​of the remaining sub-states. This significantly reduces the computational complexity of relative measurement updates while ensuring the consistency of the estimates.

[0101] Step 4.4: Update the absolute measurement based on the constructed measurement equation, when the node... When obtaining altitude measurements or precise GNSS measurements, a partial update method similar to that used for relative measurements is employed.

[0102] Step 4.5 integrates time updates, relative measurement updates, and absolute measurement updates to form the efficient multi-center extended Kalman filter process of this invention.

[0103] Furthermore, in step 5, Step 5.1, Node The error estimated by the filter is fed back to the inertial navigation system to correct its output absolute state information.

[0104] Step 5.2, after obtaining the absolute state estimates of all nodes, the nodes... It can be calculated for any node The relative state.

[0105] The present invention also provides a computer-readable storage medium having a computer program stored thereon, which is executed by a processor to implement a consistent multi-center relative navigation method for resource-constrained environments.

[0106] The present invention also provides a computer device, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement a consistent multi-center relative navigation method for resource-constrained environments. Specific Implementation Example 2: The only difference between Embodiment 2 and Embodiment 1 of this application is that: This invention discloses a consistent multi-center relative navigation method for resource-constrained environments, comprising: Step 1: Establish a multi-center error state model for the filter of each node in the UAV swarm, including the inertial navigation error and clock deviation of all nodes; Step 2: Under the multi-center framework, establish absolute measurement models such as indirect TOA / RTT relative measurement models, barometric altimeter measurement models, and GNSS pseudorange measurement models. Step 3: Design a data link communication mechanism and data packet format based on time division multiple access round-robin broadcasting to realize the transmission of raw TOA measurements, filtered state estimation, and GNSS measurements between nodes; Step 4: Set maximum speed boundaries for each node in the cluster, and construct the overall process noise covariance matrix offline accordingly. Then, the upper bound matrix is ​​obtained by pruning according to the filtering state of the fusion center. .

[0108] Step 5: Perform time propagation on the global error state, and replace the traditional process noise driving term with the upper bound matrix of the process noise covariance to achieve consistent propagation of the covariance; when the measurement arrives, use a partial update strategy to complete the measurement fusion and output the relative navigation result.

[0109] In step one above, the state is first defined: Assuming there are a total of There are 1 node, numbered as Choose any node as the multi-center fusion node, and model the navigation error state of the entire cluster uniformly in its filter. For ease of representation, a local navigation coordinate system under the geographic coordinate system (such as the East-North-Sky system, denoted as...) is adopted. Position and velocity are described using a coordinate system (denoted as ). (Series) Description of the first The posture of the drone.

[0110] For any node In this regard, its nominal navigation state can be represented as position. ,speed and attitude quaternions The corresponding error states are denoted as position errors. Speed ​​error and attitude misalignment angle

[0111] Then the global error state vector established in the multi-center fusion node. It can be written as:

[0112] in, and They represent drones Longitude error, latitude error, and altitude error; and Indicates the speed error in the northeast sky; and Indicates the attitude misalignment angle; and For data link receiver clock phase and frequency error; and This refers to the clock phase and frequency error of the GNSS receiver.

[0113] It should be noted that in a multi-center framework, each node can construct a global error state vector of the above form, with the same structure, only the error estimation values ​​are different; in any specific instance, only one node can be used as a representative for description.

[0114] In step two above, various measurement models are established: 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:

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

[0116] (2) Direct / Indirect TOA Relative Distance Measurement Model For one-way ranging measurements based on TOA, the observed quantity can be written as:

[0117] 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:

[0118] (3) Absolute measurement model of barometric altimeter No. The barometric altimeter observations at each node can be expressed as:

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

[0120] (4) GNSS absolute measurement model When node When GNSS satellite signals can be received, the pseudorange measurement equation can be expressed as:

[0121] 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.

[0122] In step three above, the communication mechanism and data packet format are constructed: 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 content of the data packets from different nodes includes, but is not limited to: (1) Data packets broadcast by ordinary nodes Node ID and timestamp; optimal estimates of position, velocity and attitude obtained from its own nominal navigation solution; the most recently generated or received raw TOA / RTT measurement.

[0123] (2) Data packets broadcast by nodes with GNSS information In addition to the data from the ordinary nodes, the observations and covariance of the most recent GNSS measurement are also included.

[0124] In step four above, the upper bound matrix of the process noise covariance is constructed offline: (1) Set maximum speed boundary: Set the maximum speed boundary for each node in the cluster. This boundary can be determined by mission requirements, flight control limitations, historical statistics or safety redundancy.

[0125] (2) Constructing the overall process noise covariance matrix: Based on the maximum velocity boundary of each node and the noise parameters of the inertial devices, construct the process noise covariance sub-blocks of each node respectively, and combine them in a diagonal form to obtain the overall process noise covariance matrix. .

[0126] (3) Pruning to obtain the upper bound matrix: Based on the filtering state definition of the fusion center node, the overall matrix is... After pruning, the upper bound matrix of the process noise covariance used for time updates of this fusion center is obtained. .

[0127] In step five above, consistent time updates and relative measurement updates are performed based on the transformation of the upper bound of the process noise covariance: Within each filtering cycle, the multi-center fusion nodes sequentially complete time updates and partial updates of relative measurements, specifically as follows: (1) Time update and covariance propagation Based on the specific force and angular velocity output by the inertial navigation system, the global error state is propagated in discrete time. Simultaneously, in the covariance propagation, the upper bound matrix of the process noise covariance, calculated offline in step four, is used. Instead of the traditional process noise driving term, it achieves conservative and consistent propagation of the process noise term, obtaining the prior estimate of the current time step. and .

[0128] (2) RTT clock synchronization measurement update When an RTT clock synchronization measurement between a pair of nodes is received, the corresponding observation matrix is ​​constructed. 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.

[0129] (3) Update of barometric altimeter measurement When a node obtains a local barometric altimeter measurement, it selects the altitude-related state component of the corresponding node from the global state vector and updates the standard EKF according to the altitude measurement model.

[0130] (4) TOA relative distance measurement section update When a direct or indirect TOA relative ranging measurement is received, a relative distance observation equation is constructed based on the measurement model established in step two. Only the position error, velocity error and other sub-states of the two nodes related to the measurement are updated using a partial update strategy. The sub-states of the remaining nodes keep their prior values ​​unchanged, but the corresponding cross covariance is updated, thereby significantly reducing the amount of computation while maintaining the consistency of the filter.

[0131] (5) Error feedback and relative navigation result output The fusion center uses the error from the filtering estimation to correct the local inertial navigation output; after obtaining the absolute state estimate of each node, it calculates and outputs the relative navigation results such as the relative position and relative velocity between any nodes in the relative coordinate system.

[0132] The above description is merely a preferred embodiment of a consistent multi-center relative navigation method under resource constraints. The scope of protection for a consistent multi-center relative navigation method under resource constraints is not limited to the above embodiments; all technical solutions falling within this conceptual framework are within the protection scope of this invention. It should be noted that for those skilled in the art, any improvements and variations made without departing from the principles of this invention should also be considered within the protection scope of this invention.

Claims

1. A consistent multi-center relative navigation method for resource-constrained environments, characterized by: The method includes the following steps: Step 1: Construct multi-center relative navigation error state modeling and global error discrete-time propagation equation; Step 2: Establish absolute and relative measurement models; Step 3: Construct the upper bound matrix of process noise covariance based on the maximum velocity boundary; Step 3 specifically involves: Step 3.1, for each node in the cluster Set the maximum velocity vector: in, and Representing nodes respectively The upper bounds of the maximum eastward, northward, and celestial velocity components in the navigation coordinate system; the upper bounds are given by mission requirements, flight control limitations, historical statistics, or safety redundancy, and satisfy the upper bound constraint on the absolute value of the actual velocity components: Step 3.2: Construct the velocity correlation matrix based on the maximum velocity boundary vector. This is used to characterize the amplification effect of velocity-related terms on inertial navigation process noise under maximum maneuver conditions; in, Defined as: Step 3.3: Based on the above parameters, construct the nodes. The inertial navigation process noise covariance submatrix: in, and These represent the single-axis or average noise variance of the accelerometer and gyroscope, respectively. Defined as: Step 3.4: Construct the overall process noise covariance matrix for each node. ,Will By combining the process noise term with the clock drift state, we obtain the nodal process noise covariance matrix. Then all nodes The overall process noise covariance matrix is ​​composed of blocks in a diagonal form. : Step 3.5, for the fusion center node The filter state vector is defined by, from Delete nodes Calculate the corresponding rows and columns of the altitude and ascent speed of all external drones. ; Step 4: Based on the discrete-time propagation equation in Step 1, the measurement model in Step 2, and the upper bound matrix in Step 3, a consistent multicenter extended Kalman filter based on the upper bound matrix is ​​constructed. Step 5: Output error feedback correction and relative navigation results; Step 5 specifically involves: Step 5.1, Node The error estimated by the filter is fed back to the inertial navigation system to correct its output absolute state information; Step 5.2, after obtaining the absolute state estimates of all nodes, the nodes... Calculate its value for any node The relative state.

2. The method according to claim 1, characterized in that: Step 1 specifically involves: Step 1.1, configure the cluster to have a total of Deploy drones, as nodes As the fusion center of multi-center relative navigation filters, it simultaneously models itself and others in the filters of each node. The navigation error status of each node, node global error state vector Recorded as: in, Represents a node Navigation error status; Represents a node Other nodes within the modeled cluster The navigation error status is as follows: in, and They represent drones and other nodes modeled Longitude error, latitude error, and altitude error; and They represent drones and other nodes modeled The speed error in the northeast sky; and They represent drones and other nodes modeled The angle of attitude misalignment; and They represent drones and other nodes modeled Data link receiver clock phase and frequency error; and They represent drones and other nodes modeled GNSS receiver clock phase and frequency error; Step 1.2, corresponding to the global error state described above, 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: The traditional velocity error is reconstructed using a full error state-space modeling method based on the right-invariant error of the Lie group. in, at the same time ; This represents the traditional velocity error vector; This represents the inertial navigation system solution result; Step 1.4: Construct the global error discrete-time propagation equation, specifically in 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 The system's white noise.

3. The method according to claim 2, characterized in that: Step 2 specifically involves: Step 2.1, construct the barometric altimeter measurement model, whose equation is expressed as: in, For altitude estimation of inertial navigation indication; For altimeter measurement white noise, follow ; Step 2.2, Build Nodes The 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; The Gaussian white noise representing GNSS pseudorange measurements obeys the following rules: distributed; Step 2.3, Build Nodes The RTT clock synchronization measurement model; RTT measurement directly reflects the clock difference between the data links of the broadcast node and the receiving node; with the time reference node NTR as a reference, the node... The RTT measurement equation for NTR is expressed as follows: in, The white noise representing the RTT measurement follows a certain order. distributed; Step 2.4, Build Nodes The direct TOA measurement model, when the node Received from node The broadcast generates a direct TOA measurement; a direct TOA measurement is a relative measurement, its physical meaning being the signal propagation time, corresponding to the spatial distance between nodes, and the measurement equation is expressed as: in, White noise measured for TOA, obeying distributed; For nodes The horizontal position error, expressed by the Jacobian matrix, is: Step 2.5, Build Nodes An indirect TOA measurement model is introduced to avoid real-time broadcasting of every TOA measurement when communication resources are limited and TOA measurements are frequent.

4. The method according to claim 3, characterized in that: set up node 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 broadcast message, TOA measurement at time and current time If a correspondence is established between the states, then the indirect TOA measurement is represented as: 。 5. The method according to claim 4, characterized in that: Step 4 specifically involves: Step 4.1: Perform block filter time update based on the global error state model and discretized state transition matrix established in Step 1. ,node At any moment The prior state estimate is: By using a block diagonal structure, the above high-dimensional operations can be decomposed into low-dimensional operations on each sub-block: Step 4.2, use the upper bound matrix obtained in step 3. To propagate the error covariance matrix, divide the covariance matrix into sub-blocks based on node indices: Propagate the covariance matrix: The upper bound of the entire variance term is derived using the expected maximum speed of the drone. Based on this upper bound, the state propagation equation can be rewritten as: Step 4.3, the method for partially updating relative measurements; in Time Node Get a link to the node State-related relative measurements The unified representation is: in, Represents a relative measurement function; This represents Gaussian white noise, i.e. ;make These represent the sets of nodes participating in the relative measurement and the sets of nodes not participating in the relative measurement, respectively. A set of nodes, then in the nodes In the filter, the global state is divided into Rewritten as: The EKF update steps for the complete error state model are represented as follows: Step 4.4: Update the absolute measurement based on the constructed measurement equation, when the node... When obtaining altitude measurements or precise GNSS measurements, a partial update method similar to that used for relative measurements is employed. Step 4.5 integrates time updates, relative measurement updates, and absolute measurement updates to form an efficient multi-center extended Kalman filter process.

6. The method according to claim 5, characterized in that: Substates not involved in this measurement are approximated using a partial update: This update strategy only updates the sub-states that are directly related to the measurement, while keeping the prior values ​​of the remaining sub-states. This significantly reduces the computational complexity of relative measurement updates while ensuring the consistency of the estimates.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that, The program is executed by the processor to implement the method as claimed in any one of claims 1-6.

8. A computer 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 method of any one of claims 1-6.

Images