An unmanned aerial vehicle dense formation autonomous cooperative relative navigation method and system

Abstract

The application relates to the technical field of unmanned aerial vehicle navigation, and provides an unmanned aerial vehicle dense formation autonomous cooperative relative navigation method and system. The method comprises the following steps: acquiring inter-aircraft ranging information and airborne inertial measurement unit measurement information; using an interpolation fitting algorithm to derive the relative distance change rate by fitting and deriving time-series ranging data; taking the product of the relative distance change rate and the relative distance as a composite observation quantity to construct an observation equation; constructing an extended Kalman filter based on the observation equation and the measurement information to perform state estimation and output a relative position; and when the relative distance is greater than a preset threshold, constructing an adaptive gain matrix correction quantity measurement update process composed of a correlation coefficient. The application solves the problems of weak observability under two-point ranging conditions and decreased long-distance solving capability, and realizes high-precision cooperative relative navigation.

Description

Technical Field

[0001] This application relates to the field of unmanned aerial vehicle (UAV) navigation technology, specifically to UAV dense formation relative navigation mission planning technology, and particularly to an autonomous cooperative relative navigation method and system for UAV dense formation. Background Technology

[0002] In recent years, the research and application of unmanned aerial vehicles (UAVs) in surveying, inspection, scientific research, and logistics have continued to deepen. Although a single UAV performs well in performing a mission, its capabilities are still limited, making it difficult to handle complex and diverse tasks. Therefore, researchers have begun to focus on researching UAV formation and coordination technology, aiming to improve mission efficiency and quality through collaborative operations among multiple UAVs.

[0003] Existing multi-sensor fusion algorithms can generally obtain relatively accurate positions of individual nodes, but obtaining the relative positions between nodes still requires cooperative localization methods. Most current cooperative localization methods are based on information transmission between nodes, but they all require at least three nodes, which imposes constraints on the number of nodes, is computationally complex, and has high requirements for communication quality. Optimal algorithms such as neural networks have excessively high requirements for hardware computing power and environmental conditions.

[0004] In navigation-denied environments, aircraft formation cooperative navigation typically relies on the interaction information between multiple nodes to achieve relative positioning. Most existing cooperative positioning methods require at least three participating nodes or depend on external auxiliary anchor points to maintain system observability. However, in practical applications, limitations in communication bandwidth, node deployment conditions, or mission concealment requirements often limit the acquisition of distance information between only two points. With only two-point distance measurement, the system geometry is simplistic, resulting in weak observability. Especially as the relative distance between aircraft increases, the observation geometry degrades significantly, and conventional filtering algorithms are prone to decreased computational power or even divergence, making it difficult to meet the demands of high-precision cooperative navigation. Summary of the Invention

[0005] Objectives of the Invention: Addressing the problems of weak observability under two-point ranging conditions and decreased filtering and calculation capabilities as the relative distance increases in existing technologies, the first objective of this application is to provide an autonomous cooperative relative navigation method for dense formations of unmanned aerial vehicles (UAVs), and the second objective is to provide an autonomous cooperative relative navigation system for dense formations of UAVs. This application utilizes an interpolation fitting algorithm to reconstruct composite observations and combines this with an adaptive gain matrix to correct the measurement update process, achieving high-precision cooperative relative navigation relying solely on two-point ranging data.

[0006] To achieve the above objectives, this application adopts the following technical solution: A method for autonomous cooperative relative navigation in dense formations of unmanned aerial vehicles includes: Acquire inter-aircraft ranging information between aircraft and use the inter-aircraft ranging information as relative distance, and acquire measurement information from the airborne inertial measurement unit; An interpolation fitting algorithm is used to fit the noisy time-series ranging data to obtain the curve of relative distance change with time, and the derivative of the curve is obtained to obtain the rate of change of relative distance. The product of the relative distance change rate and the relative distance is used as a composite observation, and the observation equation is constructed by combining the inter-machine ranging information. An extended Kalman filter is constructed based on the observation equation and the measurement information. State estimation is performed through the extended Kalman filter, and the relative positions between the aircraft are output. When the relative distance between the aircraft is greater than a preset threshold, an adaptive gain matrix is ​​constructed to correct the measurement update process of the extended Kalman filter; The adaptive gain matrix consists of a first coefficient related to the relative distance, the length of the distance sensor mounting baseline, and the measurement accuracy, and a second coefficient related to the relative distance change rate and the measurement accuracy.

[0007] Furthermore, the specific operation of obtaining the relative distance change rate by differentiating the change curve is as follows: the noisy time-series ranging data obtained by cubic spline interpolation is used to perform curve fitting to obtain the relative distance change curve with respect to time; the change curve is differentiated, and the target time point is substituted into the derivative expression to obtain the relative distance change rate.

[0008] Furthermore, the interpolation function of the cubic spline interpolation method Defined as:

[0009] in, , ; and They are nodes and The second derivative value at; The step size between adjacent nodes. ; and They are nodes and The corresponding ranging data; the interpolation function with respect to Differentiating yields the first derivative. :

[0010] Set the second derivative to 0 at both ends of the interval, and solve for the value of the second derivative with respect to the interval. The system of linear equations, substituting the target time point into the first derivative. The rate of change of the relative distance is obtained.

[0011] Furthermore, the process of constructing an adaptive gain matrix to correct the measurement update process of the extended Kalman filter includes: when the relative distance between the aircraft is greater than the preset threshold, the measurement update equation of the extended Kalman filter is corrected as follows:

[0012] in, For the first State estimate at time 10:00 For the first The predicted state value at time 10:00. For the first Kalman gain coefficient at time 10:00 For the first The system observation vector at time t, It is a nonlinear observation function; Let be the adaptive gain matrix.

[0013] Furthermore, the adaptive gain matrix From the first coefficient Second coefficient Composition, wherein: the first coefficient The expression is:

[0014] in, For the first Relative distance at any given moment This is the position vector of the ranging sensor installed on the aircraft. This represents the squared magnitude of the vector. For measurement accuracy, and The preset gain adjustment coefficient; the second coefficient The expression is:

[0015] in, and As a preset constant, For the first The rate of change of relative distance at any given time.

[0016] Furthermore, the specific process of constructing the extended Kalman filter includes: adding the constant errors of the accelerometers of aircraft A and aircraft B as additional state variables to the system state variables of the extended Kalman filter for augmentation, and constructing the augmented system state differential equation; performing a one-step prediction based on the augmented system state differential equation to obtain the state prediction value and the state prediction covariance matrix.

[0017] Based on the above scheme, the augmented system state variables for:

[0018] in, Let be the relative position of aircraft B in the carrier coordinate system of aircraft A. Let be the relative velocity of aircraft B in the coordinate system of aircraft A. and The constant errors of the accelerometers for aircraft A and B are respectively; the augmented system state differential equation is... for:

[0019] in, This is the coordinate transformation matrix from the global coordinate system to the aircraft A carrier coordinate system. and These are the accelerations of aircraft A and B in the global coordinate system, respectively. This is the projection of the Earth's rotational angular velocity onto the coordinate system of spacecraft A. Let be the projection of the angular velocity of the vehicle A coordinate system relative to the global coordinate system onto the vehicle A coordinate system. Angular acceleration, This is the projection of the relative position in the coordinate system of aircraft A. The relative velocity is the projection of the relative velocity in the coordinate system of aircraft A. It is a zero vector.

[0020] Furthermore, the observation equation for:

[0021] in, For the first ranging sensor installed at the center of aircraft A The distance to the center point of aircraft B at any time For a second ranging sensor installed at another location on aircraft A The distance to the center point of aircraft B at any time for exist The rate of change of a moment with respect to time, It is the product of the rate of change of the relative distance and the relative distance. This is the measurement noise matrix of the system.

[0022] In addition, this application also provides an autonomous cooperative relative navigation system for dense formation of unmanned aerial vehicles, including: The data acquisition module is used to acquire inter-aircraft ranging information between aircraft and use the inter-aircraft ranging information as relative distance, as well as to acquire measurement information from the airborne inertial measurement unit; The observation reconstruction module is used to perform curve fitting on noisy time-series ranging data using an interpolation fitting algorithm to obtain the curve of relative distance change with time, and to obtain the rate of change of relative distance by differentiating the curve; the product of the rate of change of relative distance and the relative distance is used as a composite observation, and the observation equation is constructed by combining the inter-machine ranging information; The filtering estimation module is used to construct an extended Kalman filter based on the observation equation and the measurement information, perform state estimation through the extended Kalman filter, and output the relative positions between the aircraft. An adaptive correction module is used to construct an adaptive gain matrix to correct the measurement update process of the extended Kalman filter when the relative distance between the aircraft is greater than a preset threshold. The adaptive gain matrix consists of a first coefficient related to the relative distance, the installation baseline length of the ranging sensor, and the measurement accuracy, and a second coefficient related to the relative distance change rate and the measurement accuracy.

[0023] Beneficial Effects: This application introduces higher-order kinematic constraints by using the product of the relative distance change rate and the relative distance as a composite observation, under the condition of only two-point ranging information. Compared with using only the original ranging values, this composite observation can effectively enhance the constraint ability of the observation equation on the relative position state, suppress the positioning instability caused by inertial navigation integral drift, and thus improve the filtering convergence and navigation accuracy of the system at conventional distances. On the other hand, this application solves the solution problem caused by observation geometric degradation at long distances by constructing an adaptive gain matrix composed of a first coefficient and a second coefficient, and triggering measurement update correction when the relative distance is greater than a preset threshold. The first coefficient comprehensively considers the relative distance, the installation baseline length, and the measurement accuracy, while the second coefficient combines the distance change rate and the measurement accuracy. The two work together to dynamically adjust the filtering gain, enabling the system to maintain a stable state estimation capability even when observability decreases, avoiding filter divergence, and broadening the effective range of cooperative relative navigation. Attached Figure Description

[0024] Figure 1 This is a schematic diagram of the aircraft cooperative relative navigation system model and vector relationships according to an embodiment of this application; Figure 2 This is a three-dimensional distribution diagram of the simulated flight trajectory of the aircraft formation according to an embodiment of this application; Figure 3 This is a solution error curve obtained based on the method described in this invention; Figure 4 It is a prediction curve using only airborne inertial navigation. Detailed Implementation

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

[0026] like Figure 1 As shown, this embodiment provides an autonomous cooperative relative navigation method for dense formations of unmanned aerial vehicles. This method is applied to dual-vehicle or multi-vehicle formation scenarios in GNSS-denied environments, aiming to achieve high-precision relative state estimation by relying solely on inter-vehicle ranging and inertial measurement information.

[0027] Figure 1 The paper presents a model of a cooperative relative navigation system for aircraft and a schematic diagram of vector relationships according to an embodiment of this application. It defines the geocentric coordinate system (e-system) and the carrier coordinate systems of aircraft A and aircraft B, and illustrates the geometric relationship between the relative position vector and the sensor installation position.

[0028] A method for autonomous cooperative relative navigation in dense formations of unmanned aerial vehicles (UAVs) includes the following steps: (1) Obtain the inter-aircraft ranging information between aircraft and use the inter-aircraft ranging information as the relative distance, and obtain the measurement information of the airborne inertial measurement unit.

[0029] In this embodiment, the inter-machine ranging information is typically acquired by an ultra-wideband (UWB) ranging sensor, which features strong resistance to multipath effects and high temporal resolution. For example... Figure 1 As shown, at least two ranging sensors (e.g., UWB1 located at the center and UWB2 located at another position) are installed on aircraft A to measure the distance between it and the center point of aircraft B. This multi-point ranging layout provides the physical basis for the subsequent construction of spatial geometric constraints. Simultaneously, the onboard inertial measurement unit (IMU) acquires the angular velocity and acceleration information of the aircraft in real time to calculate the attitude and position changes of the aircraft, providing a high-frequency dynamic reference benchmark for relative navigation. In this step, the specific internal state definition and dynamic modeling method for constructing the extended Kalman filter based on the observation equations and measurement information are as follows: The constant errors of the accelerometers of aircraft A and B are added as additional state variables to the system state variables of the extended Kalman filter for augmentation, constructing an augmented system state differential equation. Based on this augmented system state differential equation, a one-step prediction is performed to obtain the predicted state value and the state prediction covariance matrix. The core objective of this design is to solve the inherent zero-bias drift problem of the inertial measurement unit (IMU). In GNSS denied environments, if only position and velocity are used as state variables, the constant errors of the accelerometers accumulate with time through quadratic integration, causing the relative position estimate to diverge rapidly. By incorporating the constant errors of the accelerometers of both aircraft into the state vector, the filter can utilize the distance change rate information contained in inter-aircraft ranging and composite observations to inversely calculate and estimate these error terms in real time. This proactively compensates for IMU drift during state updates, significantly improving the accuracy and stability of long-term cooperative navigation.

[0030] To fully support the implementation of the above augmentation strategy, this application defines specific 12-dimensional augmented state variables. and its corresponding state differential equation. Augmented system state variables. for:

[0031] in, Let be the relative position of aircraft B in the carrier coordinate system of aircraft A. Let be the relative velocity of aircraft B in the coordinate system of aircraft A. and These represent the constant errors of the accelerometers for aircraft A and B, respectively. In this state vector, the first six dimensions describe the relative kinematic state between the two aircraft, while the latter six dimensions describe the error state of the sensors themselves. This combination allows the filter to sense not only the external relative motion but also the internal sensor health status, enabling simultaneous navigation calculations and sensor calibration.

[0032] Correspondingly, the augmented system state differential equation for:

[0033] in, This is the coordinate transformation matrix from the global coordinate system to the aircraft A carrier coordinate system. and These are the accelerations of aircraft A and B in the global coordinate system, respectively. This is the projection of the Earth's rotational angular velocity onto the coordinate system of spacecraft A. Let be the projection of the angular velocity of the vehicle A coordinate system relative to the global coordinate system onto the vehicle A coordinate system. Angular acceleration, This is the projection of the relative position in the coordinate system of aircraft A. The relative velocity is the projection of the relative velocity in the coordinate system of aircraft A. It is a zero vector.

[0034] In this differential equation, the derivative of relative position is directly equal to the relative velocity, which conforms to the basic definition of kinematics. The derivative term of relative velocity contains rich dynamic information: This represents the projection of the difference in force between the two machines onto the coordinate system of machine A, where... and The estimated accelerometer constant error was explicitly transformed from the carrier coordinate system back to the global coordinate system for calculation, and then projected back to the A-frame, thus achieving real-time correction of the actual acceleration. Subsequent... , and These represent the effects of centrifugal force, Coriolis force, and Euler force, respectively. These terms are crucial for the relative navigation accuracy during high-speed maneuvers or long-baseline formation flight, ensuring the rigor of the dynamic model in a non-inertial frame.

[0035] It is particularly important to note that the last two lines of the equation... Indicates the constant error of the accelerometer and The derivative is set to zero. This is based on the commonly used stochastic constant model assumption in engineering practice, which states that the zero bias of the accelerometer is considered a slowly changing constant within the filter update period or a short time window. Although actual devices may exhibit random walk characteristics, modeling it as constant under the condition that the observability of two-point ranging is inherently weak helps to enhance the observability of the state and avoid filter instability caused by introducing too many noise driving terms. Meanwhile, the coordinate transformation matrix... , , Instead of fixed parameters, the attitude angles output by the airborne IMU are calculated in real time, ensuring that the dynamic equations are always expressed in the accurate carrier coordinate system at the current moment. Through the combination of the augmented state and the refined dynamic model, the extended Kalman filter can effectively suppress the accumulation of inertial navigation errors in the absence of an absolute position reference, providing a reliable basis for state prediction of composite observations and adaptive corrections.

[0036] (2) Use interpolation fitting algorithm to fit the noisy time series ranging data to obtain the curve of relative distance change with time, and take the derivative of the curve to obtain the rate of change of relative distance; use the product of the rate of change of relative distance and relative distance as a composite observation, and combine it with the inter-machine ranging information to construct the observation equation.

[0037] Since raw ranging data inevitably contains measurement noise, directly calculating the distance change rate using differential methods amplifies noise interference, leading to a decrease in filtering performance. Therefore, this embodiment employs an interpolation fitting algorithm to smooth the time-series ranging data within a time window, constructing a continuous distance-time function, and then obtaining a high-precision relative distance change rate through analytical differentiation. Based on this, this application innovatively constructs a composite observation system. Specifically, the observation equation... Defined as:

[0038] in, For the first ranging sensor installed at the center of aircraft A The distance to the center point of aircraft B at any time For a second ranging sensor installed at another location on aircraft A The distance to the center point of aircraft B at any time for exist The rate of change of a moment with respect to time, It is the product of the rate of change of relative distance and the relative distance. This is the measurement noise matrix of the system.

[0039] This application introduces a product term. The core of this composite observation lies in addressing the weak observability problem under two-point ranging conditions. With only distance information, the system is insensitive to relative motion perpendicular to the line-of-sight direction; while the simple rate of change of distance includes radial velocity information, it exhibits strong coupling nonlinearity with the relative distance. By multiplying the two to construct a new observation, a higher-order kinematic constraint is introduced into the observation equation. This effectively decouples the correlation between distance and velocity, significantly enhancing the filter's convergence capability for relative position states. Particularly during aircraft maneuvers, this composite observation provides more stable geometric constraints than either distance or velocity alone.

[0040] (3) Based on the observation equation and measurement information, an extended Kalman filter is constructed. The state is estimated by the extended Kalman filter, and the relative positions between the aircraft are output.

[0041] The extended Kalman filter (EKF) uses the acceleration and angular velocity information measured by the IMU to establish a relative motion dynamic model and complete the state prediction in one step; then the measurement is updated using the observation equation containing composite observations constructed in step (2) to correct the prediction error.

[0042] This application achieves continuous and stable estimation of relative position by integrating the high-frequency dynamic characteristics of inertial navigation with the absolute constraint of ranging information through a prediction-correction mechanism.

[0043] The specific operation of the one-step prediction process is as follows: Based on the UAV relative position model constructed above, the system state model is expressed as a nonlinear equation of discrete-time state:

[0044] In the formula, and The system at the 1st and The state vector at time t, The nonlinear equations representing the system, For the system in the first Time-selectable system control inputs yes The output vector of the system at any given time. express Time and system state vector Related observation equations; Use the state information from the previous time step to make a one-step prediction of the system: ,

[0045] In the formula, Represented as Time's up The predicted state value at time t, through System state estimation vector at time t The differential equation is obtained by integration; The initial covariance matrix for system state prediction. It is a state equation Jacobian matrix; For the corresponding state vector The covariance matrix.

[0046] The measurement update process is represented as follows:

[0047]

[0048]

[0049] In the formula, The Kalman gain coefficient is... It is the observation equation Jacobian matrix, This is the system measurement noise variance matrix.

[0050] (4) Optimize measurement information using a relative distance change rate fitting algorithm.

[0051] This step utilizes an interpolation fitting algorithm to obtain the relative distance change rate. Cubic spline interpolation is used to fit a curve to the noisy time-series ranging data, yielding a curve showing the relative distance change over time. The derivative of this curve is then calculated, and the target time point is substituted into the derivative expression to obtain the relative distance change rate. The specific mathematical expression of the cubic spline interpolation function is given below.

[0052] Given Time nodes and its corresponding ranging data Then in any subinterval Above, cubic spline interpolation function Defined as:

[0053] in, , ; and They are nodes and The second derivative value at that point is the key parameter to be solved. The step size between adjacent nodes is... ; and They are nodes and The corresponding raw distance measurement data.

[0054] Based on this, the interpolation function Regarding the independent variable Taking the derivative, we can obtain the first derivative expression used to calculate the rate of change of relative distance. :

[0055] This derivative expression shows that as long as the second derivative values ​​at each node are determined... So that you can do it at any time. The rate of change of distance can be calculated analytically without the need for numerical difference operations that amplify noise.

[0056] The reason why this application prefers cubic spline interpolation over ordinary high-order polynomial fitting is mainly based on the following two considerations: First, the cubic spline function has continuous function values ​​at each node, and its first and second derivatives are also continuous. This means that the fitted distance-time curve has extremely high smoothness and can truly reflect the physical continuity of the relative motion of the aircraft. Second, when there are many sampling nodes, high-order global polynomial fitting is prone to violent oscillations at the interval edges (i.e., Runge phenomenon), which leads to serious distortion of the derivative results. Cubic splines, on the other hand, are constructed using piecewise low-order polynomials, which fundamentally avoids this problem and is particularly suitable for processing UWB time series data containing measurement noise.

[0057] In practical engineering solutions, in order to determine the unknown parameters in the above formulas Boundary conditions need to be introduced. This embodiment uses natural spline boundary conditions, that is, setting the second derivative values ​​at both ends of the interval to 0, satisfying... and This physical assumption implies that the rate of change of relative acceleration between the aircraft is considered zero or a minimum at the beginning and end of the observation time window. This is a reasonable and robust approximation for relative navigation within a short sliding window. Introducing this boundary condition, combined with the continuity condition of the spline function at its first derivative, allows us to construct a... The system of tridiagonal linear equations has a strict diagonal dominance property, and its unique solution can be quickly obtained through efficient numerical algorithms such as the pursuit method, thus ensuring the feasibility and determinism of real-time solution by the airborne processor.

[0058] Through the above process, the system completes the conversion from a discrete, noisy ranging sequence to a continuous, smooth distance change rate. It should be understood that this cubic spline-based differentiation method is essentially equivalent to a low-pass filter based on kinematic model constraints. It utilizes ranging information from multiple adjacent time steps to jointly determine the rate of change at the current time step, effectively smoothing out the inherent high-frequency random measurement noise of the UWB sensor and avoiding the noise amplification effect caused by direct differential calculus. This provides a high-quality, low-jitter composite observation input for the subsequent extended Kalman filter, significantly improving the system's state estimation accuracy and convergence stability in dynamic environments.

[0059] (5) When the relative distance between the aircraft is greater than the preset threshold, an adaptive gain matrix is ​​constructed to correct the measurement update process of the extended Kalman filter. The adaptive gain matrix consists of a first coefficient related to the relative distance, the length of the distance sensor installation baseline and the measurement accuracy, and a second coefficient related to the relative distance change rate and the measurement accuracy.

[0060] In actual formation flying, as the relative distance between aircraft increases, the angle between the baseline lengths of the ranging sensors and the distance decreases sharply, leading to a degradation of the observation geometry. The fixed gain of a conventional EKF (Electronic Kalman Filter) is ill-suited to this time-varying observability variation, easily resulting in filter divergence. Therefore, this embodiment sets a preset threshold as a trigger condition. When the relative distance exceeds this threshold, the system automatically switches to adaptive correction mode. This mode adjusts the standard Kalman gain by constructing a dynamic adaptive gain matrix. The first coefficient primarily reflects the combined effect of the degree of geometry degradation and sensor accuracy, used to compensate for information loss due to increased distance; the second coefficient combines the rate of distance change and measurement accuracy to evaluate the observation quality under current dynamic conditions. These two coefficients work synergistically, enabling the filter to maintain an appropriate gain level even at long distances and under weak observation conditions. This avoids introducing excessive noise due to excessive gain and prevents state estimation lag or divergence due to insufficient gain, thus ensuring the robustness of cooperative relative navigation throughout the entire flight. It should be understood that the specific value of the preset threshold can be flexibly set according to the sensor performance and task requirements. For example, it can be set to a number of times the distance measurement baseline length. This application does not limit this.

[0061] Specifically, the following section details the measurement update process for constructing the adaptive gain matrix for the extended Kalman filter.

[0062] When the relative distance between aircraft exceeds a preset threshold, it indicates that the system has entered a long-range, weak observation region. At this point, the standard extended Kalman filter gain may no longer be applicable due to increased linearization error and geometric degradation. Therefore, this embodiment modifies the measurement update equation as follows:

[0063] in, For the first State estimate at time 10:00 For the first The predicted state value at time 10:00. For the first The standard Kalman gain coefficient at time 10:00. For the first The system observation vector at time t, For nonlinear observation functions, This is the adaptive gain matrix. (Introduced) Essentially, it superimposes a robust compensation mechanism based on physical environment awareness on top of the standard Kalman gain. When the system is in a close-range, strong observation region, To approximate the identity matrix, the algorithm degenerates into the standard EKF algorithm to ensure optimality; however, when the system enters a distant, weakly observable region... The gain is dynamically scaled based on real-time geometry and dynamic conditions to prevent filter divergence or over-reliance on noise measurements due to model mismatch.

[0064] Furthermore, the adaptive gain matrix From the first coefficient Second coefficient The configuration involves jointly adjusting the filter gain from two dimensions: static geometric configuration and dynamic observation quality. The first coefficient... It mainly reflects the degree of geometric degradation, and its expression is:

[0065] In this formula, For the first The relative distance at any given time is used to characterize the current observation scale; This is the position vector of the ranging sensor installed on the aircraft. This represents the square of the magnitude of the vector, i.e., the square of the ranging baseline length. Baseline length is a key hardware parameter determining the observability of a two-point ranging system. A longer baseline results in a larger observation angle at the same distance, and stronger geometric constraints. Therefore, this parameter is in the denominator, meaning that with a long baseline configuration, the system has a higher tolerance for long-distance degradation, and the required gain compensation is correspondingly reduced. For measurement accuracy, meters are typically used. The nonlinear value is because the ranging error distribution of sensors such as UWB is often non-uniform. Logarithmic transformation can compress the dynamic range of accuracy changes, making the gain adjustment curve smoother and avoiding drastic gain jumps caused by small fluctuations in accuracy indicators. and This is a preset gain adjustment coefficient used to balance the dimensional differences and weight allocation between the distance and baseline accuracy terms. To meet full disclosure requirements, and These are not arbitrarily selected empirical values, but rather obtained through offline simulation calibration: within a typical flight envelope, the filter convergence boundary is statistically analyzed for different combinations of distances and baseline lengths, and the parameter combination that minimizes the root mean square error is selected as the recommended value. For example, for a typical UAV formation scenario with a baseline length of 0.5m to 1m and a ranging accuracy of 0.1m to 0.3m, The recommended value range is 0.8 to 1.2. The recommended value range is 0.5 to 0.9.

[0066] Second coefficient It mainly reflects the observation quality under dynamic conditions, and its expression is:

[0067] in, For the first The rate of change of relative distance at any given moment is directly related to the intensity of relative radial motion between the aircraft. From an information theory perspective, when there is significant relative motion between the two aircraft, the observability of the system is actually better than in a relatively static state, because dynamic excitation helps decouple position and velocity states. Therefore, As a denominator term, this coefficient automatically adjusts under high dynamic conditions to accommodate the increased amount of observational information; conversely, under quasi-static long-distance conditions, the coefficient increases to provide a more conservative gain strategy. This is a preset constant. Its primary function is to serve as a regularization term to prevent the denominator from being zero. Secondly, it also represents the system's baseline confidence level in static observations. It is recommended that the value range be 0.1 to 0.5. The gain coefficient related to measurement accuracy, and the first coefficient similar, It can also be determined through simulation statistics, with a recommended value range of 0.6 to 1.0.

[0068] In actual operation, the first coefficient With the second coefficient Collaborative work. It focuses on compensating for the loss of inherent geometric information due to increased distance and belongs to the static environmental adaptation item; This focuses on evaluating the instantaneous impact of motion excitation on observation quality at the current moment and belongs to the dynamic process adaptation term. Both factors act together on the adaptive gain matrix. This design enables the filter to maintain a gain level that matches the statistical characteristics of the innovation sequence under complex operating conditions such as long distances, weak geometry, and variable dynamics. This design not only avoids the objective risk of purely empirical formulas lacking physical support, but more importantly, it solves the "overcorrection" or "undercorrection" problems that traditional fixed-gain EKFs are prone to in the long-range segment, ensuring the continuity and stability of cooperative relative navigation throughout the entire flight.

[0069] Example 2: This example provides an autonomous cooperative relative navigation system for dense formations of unmanned aerial vehicles (UAVs). This system is typically deployed on the onboard computing platform of the UAV and is used to execute the autonomous cooperative relative navigation method for dense formations of UAVs described in Example 1. It should be understood that the modules in this example do not refer to independent physical hardware circuits, but rather to software program units or firmware logic components stored in a computer-readable storage medium and loaded and executed by a processor to achieve specific functions.

[0070] The dense formation autonomous cooperative relative navigation system for unmanned aerial vehicles includes a data acquisition module, an observation reconstruction module, a filtering estimation module, and an adaptive correction module.

[0071] The data acquisition module is used to acquire inter-aircraft ranging information and use it as relative distance, as well as to acquire measurement information from the onboard inertial measurement unit (IMU). In actual operation, this module acts as a data gateway between the system and external sensors. It is responsible not only for reading raw distance data from UWB ranging sensors but also for reading acceleration and angular velocity data from the IMU. More importantly, this module also performs timestamp alignment and cache management logic for multi-source heterogeneous data, ensuring that the ranging sequences used in subsequent processing are strictly synchronized with the inertial navigation data on the time base, laying a data foundation for high-precision relative navigation calculations.

[0072] The observation reconstruction module uses an interpolation fitting algorithm to perform curve fitting on noisy time-series ranging data, obtaining a curve showing the change of relative distance over time. It then differentiates this curve to obtain the rate of change of relative distance. The product of the rate of change of relative distance and the relative distance is used as a composite observation, combined with inter-machine ranging information to construct the observation equation. Specifically, the module calls a cubic spline interpolation algorithm subroutine to smooth and analytically differentiate the noisy ranging sequence output by the data acquisition module, thereby obtaining a low-noise rate of change of relative distance. Subsequently, this module performs the observation equation calculation, assembling the original distance, redundant distance, and the product of distance and rate of change into a standardized observation vector. Through this reconstruction, the module transforms the original physical measurements into information-enhanced observations containing higher-order kinematic constraints, which are then directly fed to the back-end filter.

[0073] The filtering estimation module is used to construct an extended Kalman filter based on observation equations and measurement information. This extended Kalman filter is then used for state estimation, outputting the relative positions between the aircraft. This module carries the core solution logic for relative navigation, including processing and calculating the 12-dimensional augmented state vector and its covariance matrix, and periodically performing state prediction and measurement updates. In the prediction phase, it uses IMU data to drive the relative motion dynamics model; in the update phase, it receives composite observations output from the observation reconstruction module to calculate information and correct the state. The final output of this module is a high-precision estimate of the relative position and velocity between the two aircraft, which can be directly used in formation control laws.

[0074] The adaptive correction module is used to correct the measurement update process of the extended Kalman filter when the relative distance between aircraft exceeds a preset threshold. It constructs an adaptive gain matrix. The adaptive gain matrix consists of a first coefficient related to the relative distance, the distance sensor mounting baseline length, and measurement accuracy, and a second coefficient related to the relative distance change rate and measurement accuracy. This module, acting as a robustness assurance unit, is tightly coupled with the filter estimation module. It monitors the current relative distance estimate in real time, and activates the correction logic once the distance exceeds the preset threshold. At this point, the module dynamically calculates the first and second coefficients based on parameters such as the current distance, baseline length, measurement accuracy, and distance change rate, thereby generating the adaptive gain matrix. This matrix is ​​injected into the measurement update stage of the filter estimation module to scale and adjust the standard Kalman gain.

[0075] In terms of timing coordination, the aforementioned modules form a closed-loop data flow under the scheduling of the onboard processor: the data acquisition module continuously fills the data buffer according to the sampling rate; the observation reconstruction module triggers a fitting and reconstruction operation after accumulating data of sufficient window length; the filtering estimation module runs prediction at the inertial navigation frequency and performs updates when new reconstructed observations are received; and the adaptive correction module evaluates the triggering conditions and dynamically adjusts the gain parameters before each update. This loosely coupled and highly cohesive software architecture not only facilitates independent debugging and upgrades in engineering implementation but also enables the device claims of this application to fully cover the entire process of software product distribution, sales, and operation, providing comprehensive legal protection for technological innovation.

[0076] Example 3: To verify the effectiveness of the method described in this application, this example constructs a dual-aircraft formation cooperative navigation scenario under GNSS denial conditions in a numerical simulation environment, and compares the solution results of this method with a scheme using only airborne inertial navigation. The simulation scenario is set as dual-aircraft formation flight with a flight duration of 600 seconds. To better reflect actual engineering practice and fully stimulate the observability of the system, the motion trajectory design includes various typical maneuvers such as acceleration, turning, and climbing. Figure 2 As shown, aircraft A first initiates flight motion, and after 5 seconds, aircraft B begins to move. The two maintain coordinated maneuvers in three-dimensional space.

[0077] Regarding simulation parameter settings, the airborne sensors and initial error parameters are configured as follows: IMU sampling rate is 0.01 seconds, UWB sampling rate is 0.05 seconds; gyroscope constant drift is 0.1 degrees / hour, accelerometer constant drift is 100 micrograms; UWB ranging error standard deviation is 0.1 meters; initial position error is set to (10, 10, 10) meters, and initial velocity error is set to (0.1, 0.1, 0.1) meters / second. The UWB sensor installation positions are as follows: UWB1 is located at the origin of the aircraft's coordinate system at (0, 0, 0) meters, and UWB2 is located at (-0.45, 0.75, 0) meters. The resulting ranging baseline length is approximately 0.87 meters, providing a physical basis for the geometric constraints of two-point ranging. The initial relative position true value is set to (1, 1, 1) meters, and the initial relative velocity is (0, 0, 0) meters / second.

[0078] Figure 3 and Figure 4 The comparison curves of prediction errors for relative navigation calculations using the method of this application and those using only airborne inertial navigation methods are shown under the above simulation conditions. From Figure 4 It can be seen that the scheme using only airborne inertial navigation, lacking external absolute measurement information for correction, results in the accumulation of accelerometer constant drift over time through double integration, leading to a significant divergence trend in the relative position errors across all three axes. Particularly in the y-axis direction, the error amplitude fluctuates dramatically over time, reaching a minimum close to -600 meters; while the z-axis error remains relatively stable for most of the time, a step change occurs at the end; the x-axis error peaks at approximately 300 seconds and, although it declines, remains in the hundreds of meters range. This indicates that under GNSS rejection and with only two-point ranging, relying solely on inertial navigation cannot maintain high-precision relative positioning over extended periods.

[0079] In comparison, Figure 3The solution error curves using the method proposed in this application are shown. It can be observed that throughout the entire 600-second flight, the absolute values ​​of the three-axis prediction errors remained within the convergence range without divergence. Specifically, after the system entered steady state, the estimation errors of the y-axis and z-axis stabilized within ±1 meter, and the x-axis error stabilized within ±6 meters. This significant performance improvement verifies the effectiveness of the technical solution proposed in this application. Its main working principle is as follows: On the one hand, the relative distance change rate is obtained through cubic spline interpolation fitting, and multiplied by the relative distance to construct a composite observation, effectively enhancing the constraint ability of the observation equation on the relative position state and overcoming the weak observability problem of pure distance observation in the vertical line-of-sight direction; on the other hand, when the relative distance increases, leading to geometric configuration degradation, the adaptive gain matrix dynamically adjusts the filter gain according to the real-time distance, baseline length, measurement accuracy, and distance change rate, avoiding filter divergence caused by model mismatch or decreased observation quality, thereby ensuring the stability of navigation accuracy throughout the entire flight.

[0080] Furthermore, in actual engineering deployments, the selection of the preset threshold has a significant impact on system performance. This threshold determines the triggering timing of the adaptive correction mechanism. If the threshold is set too low, the system will frequently activate adaptive correction in close-range strong observation areas, potentially introducing unnecessary computational overhead or losing some optimality due to excessive parameter adjustment. If the threshold is set too high, a brief decrease in accuracy or even instability may occur during the transition phase when the system has entered a weak observation area but has not yet triggered correction. Based on the simulation statistical experience of this embodiment, it is recommended to set the preset threshold to 5 to 10 times the length of the ranging sensor installation baseline. For example, in this embodiment, when the baseline length is approximately 0.87 meters, the preset threshold can be set between 4.35 meters and 8.7 meters. Within this range, the system can smoothly transition to a long-range robust estimation mode while ensuring optimal estimation at close range, achieving continuous and stable cooperative relative navigation performance across the entire airspace. It should be understood that the above simulation parameters and recommended threshold values ​​are merely illustrative examples. In practical applications, adaptive adjustments can be made according to specific sensor performance indicators, aircraft dynamics characteristics, and mission accuracy requirements, which does not depart from the scope of protection of this application.

[0081] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any person skilled in the art can easily conceive of variations or substitutions within the technical scope disclosed in this application. For example, equivalent transformations or simple modifications can be made to the specific selection of the interpolation fitting algorithm, the mathematical expression of the coefficients in the adaptive gain matrix, the augmentation dimension of the state variables, or the setting strategy of the preset threshold, etc., as long as they do not depart from the essence of the technical solution defined by the claims of this application, they should all be covered within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A method for autonomous cooperative relative navigation in dense formations of unmanned aerial vehicles, characterized in that, include: Acquire inter-aircraft ranging information between aircraft and use the inter-aircraft ranging information as relative distance, and acquire measurement information from the airborne inertial measurement unit; An interpolation fitting algorithm is used to fit the noisy time-series ranging data to obtain the curve of relative distance change with time, and the derivative of the curve is obtained to obtain the rate of change of relative distance. The product of the relative distance change rate and the relative distance is used as a composite observation, and the observation equation is constructed by combining the inter-machine ranging information. An extended Kalman filter is constructed based on the observation equation and the measurement information. State estimation is performed through the extended Kalman filter, and the relative positions between the aircraft are output. When the relative distance between the aircraft is greater than a preset threshold, an adaptive gain matrix is ​​constructed to correct the measurement update process of the extended Kalman filter; the adaptive gain matrix consists of a first coefficient related to the relative distance, the installation baseline length of the ranging sensor and the measurement accuracy, and a second coefficient related to the relative distance change rate and the measurement accuracy. The correction process for the measurement update of the extended Kalman filter is as follows: When the relative distance between the aircraft exceeds the preset threshold, the measurement update equation of the extended Kalman filter is corrected as follows: in, For the first State estimate at time 10:00 For the first The predicted state value at time 10:

00. For the first Kalman gain coefficient at time 10:00 For the first The system observation vector at time t, It is a nonlinear observation function; The adaptive gain matrix; The adaptive gain matrix From the first coefficient Second coefficient Composition, wherein: the first coefficient The expression is: in, For the first Relative distance at any given moment This is the position vector of the ranging sensor installed on the aircraft. This represents the squared magnitude of the vector. For measurement accuracy, and This is the preset gain adjustment coefficient; The second coefficient The expression is: in, and As a preset constant, For the first The rate of change of relative distance at any given time; The observation equation for: in, For the first ranging sensor installed at the center of aircraft A The distance to the center point of aircraft B at any time For a second ranging sensor installed at another location on aircraft A The distance to the center point of aircraft B at any time for exist The rate of change of a moment with respect to time, It is the product of the rate of change of the relative distance and the relative distance. This is the measurement noise matrix of the system.

2. The method for autonomous cooperative relative navigation in dense formations of unmanned aerial vehicles according to claim 1, characterized in that, The process of obtaining the relative distance change rate includes: The cubic spline interpolation method was used to perform curve fitting on the noisy time-series distance measurement data to obtain the curve of relative distance change with time; Differentiate the change curve and substitute the target time point into the derivative expression to obtain the rate of change of the relative distance.

3. The method for autonomous cooperative relative navigation in dense formations of unmanned aerial vehicles according to claim 2, characterized in that, The interpolation function of the cubic spline interpolation method Defined as: in, , ; and They are nodes and The second derivative value at; The step size between adjacent nodes. ; and They are nodes and The corresponding distance measurement data; Regarding the interpolation function with respect to Differentiating yields the first derivative. : Set the second derivative to 0 at both ends of the interval, and solve for the value of the second derivative with respect to the interval. The system of linear equations, substituting the target time point into the first derivative. The rate of change of the relative distance is obtained.

4. The method for autonomous cooperative relative navigation in dense formations of unmanned aerial vehicles according to claim 1, characterized in that, The construction of the extended Kalman filter based on the observation equation and the measurement information includes: The constant errors of the accelerometers of aircraft A and aircraft B are used as additional state variables and added to the system state variables of the extended Kalman filter for augmentation, thereby constructing the augmented system state differential equation. Based on the augmented system state differential equation, a one-step prediction is performed to obtain the state prediction value and the state prediction covariance matrix.

5. The method for autonomous cooperative relative navigation in dense formations of unmanned aerial vehicles according to claim 4, characterized in that, The augmented system state variables for: in, Let be the relative position of aircraft B in the carrier coordinate system of aircraft A. Let be the relative velocity of aircraft B in the coordinate system of aircraft A. and These are the constant errors of the accelerometers for aircraft A and B, respectively. The augmented system state differential equation for: in, This is the coordinate transformation matrix from the global coordinate system to the aircraft A carrier coordinate system. and These are the accelerations of aircraft A and B in the global coordinate system, respectively. This is the projection of the Earth's rotational angular velocity onto the coordinate system of spacecraft A. Let be the projection of the angular velocity of the vehicle A coordinate system relative to the global coordinate system onto the vehicle A coordinate system. Angular acceleration, This is the projection of the relative position in the coordinate system of aircraft A. The relative velocity is the projection of the relative velocity in the coordinate system of aircraft A. It is a zero vector.

6. A dense formation autonomous cooperative relative navigation system for unmanned aerial vehicles, characterized in that, The system is used to execute the autonomous cooperative relative navigation method for dense formation of unmanned aerial vehicles as described in any one of claims 1-5; the system includes: The data acquisition module is used to acquire inter-aircraft ranging information between aircraft and use the inter-aircraft ranging information as relative distance, as well as to acquire measurement information from the airborne inertial measurement unit; The observation reconstruction module is used to perform curve fitting on noisy time-series ranging data using an interpolation fitting algorithm to obtain the curve of relative distance change with time, and to obtain the rate of change of relative distance by differentiating the curve; the product of the rate of change of relative distance and the relative distance is used as a composite observation, and the observation equation is constructed by combining the inter-machine ranging information; The filtering estimation module is used to construct an extended Kalman filter based on the observation equation and the measurement information, perform state estimation through the extended Kalman filter, and output the relative positions between the aircraft. An adaptive correction module is used to construct an adaptive gain matrix to correct the measurement update process of the extended Kalman filter when the relative distance between the aircraft is greater than a preset threshold. The adaptive gain matrix consists of a first coefficient related to the relative distance, the installation baseline length of the ranging sensor, and the measurement accuracy, and a second coefficient related to the relative distance change rate and the measurement accuracy.

Images