A method for cooperative navigation of a UAV cluster based on an NLOS environment

Abstract

The application discloses a kind of unmanned aerial vehicle cluster cooperative navigation methods based on NLOS environment, comprising: by the state parameter of unmanned aerial vehicle constructs the state model of unmanned aerial vehicle;Judge the communication NLOS environment between wingman and host;Based on the NLOS error compensation value and relative distance of calculating distance measurement, the NLOS error compensation value and relative distance are superimposed to obtain correction distance, and the correction distance and measured distance ρ li Construct initial observation model;Error covariance matrix R a→b Add to initial observation model to obtain position observation model;State model and position observation model are brought into extended Kalman filter formula and are iterated, and the final estimated position of host is obtained;Wireless communication is used to make host and wingman interact and adopt the way of wingman following host Cooperative navigation, the correction of navigation information, keep unmanned aerial vehicle cluster normal flight under NLOS environment.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle (UAV) navigation, specifically relating to a UAV swarm cooperative navigation method based on an NLOS environment. Background Technology

[0002] Unmanned aerial vehicles (UAVs) possess advantages such as lightness, maneuverability, and low cost, making them widely used in engineering exploration, military reconnaissance, and disaster relief. Compared to individual UAVs, UAV swarms offer redundancy, robustness, and scalability, while also exhibiting superior coordination, intelligence, and autonomy. Cooperative navigation, crucial for maintaining formation flight and precise positioning, has garnered significant attention. Multi-UAV cooperative navigation technology has greatly expanded the application scope of UAVs. Through information sharing and fusion in multi-UAV formation flight, relative positioning can be achieved, improving navigation accuracy.

[0003] The practical application scenarios of UAV swarms are complex and varied, often requiring accurate position estimation in NLOS environments. However, the cooperative navigation performance of UAV swarms is poor due to the influence of NLOS noise. Therefore, researching cooperative navigation methods for UAV swarms in complex NLOS environments and improving their performance is an urgent technical problem to be solved. Summary of the Invention

[0004] This invention provides a method for cooperative navigation of UAV swarms based on NLOS environment, which corrects navigation information and maintains normal flight of UAV swarms in NLOS environment.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0006] The first aspect of this invention provides a method for cooperative navigation of unmanned aerial vehicle (UAV) swarms based on an NLOS environment, comprising:

[0007] The initial position estimates and state parameters of each UAV in the UAV swarm are collected using a combination of GPS and INS. A state model of the UAVs is then constructed from these state parameters. The UAV swarm includes a master drone and wingmen. The theoretical distance ρ from the wingmen to the master drone is derived based on the theoretical positions of each UAV. li ;

[0008] The distances from each wingman to the main unit were obtained using UWB ranging. Based on measured distance The theoretical distance from each wingman to the main engine was derived. By measuring distance Theoretical distance The error covariance matrix R is derived from the initial position estimates of each UAV. a→b According to the measured distance Calculate the offset ΔD of the relative distance between the wingman and the main aircraft per unit time. i,k Compare the offset ΔD i,k The communication environment between the wingman and the host is determined by comparing the preset decision threshold T.

[0009] If the offset ΔD i,k When the distance is less than or equal to the decision threshold T, the communication LOS environment between the wingman and the host is determined; based on the theoretical distance... and theoretical distance ρ li Construct an initial observation model;

[0010] If the offset ΔD i,k When the distance exceeds the decision threshold T, determine the NLOS communication environment between the wingman and the host aircraft; based on the measured distance. Calculate NLOS error compensation value and relative distance Combine NLOS error compensation value and relative distance Superimposed to obtain correction distance From the corrected distance and measuring distance ρ li Construct an initial observation model;

[0011] The error covariance matrix R a→b The initial observation model is added to obtain the position observation model; the state model and the position observation model are substituted into the extended Kalman filter formula for iteration to obtain the final estimated position of the host; wireless communication is used to enable interaction between the host and the wingman and to coordinate navigation by having the wingman follow the host.

[0012] Furthermore, methods for constructing a UAV state model from the UAV's state parameters include:

[0013] State variables X(t) are established from state parameters; these state parameters include inertial navigation platform error angle, velocity error, position error, gyroscope noise, gyroscope bias noise, and accelerometer error; a state model of the UAV is constructed based on the state variables X(t). The formula is as follows:

[0014]

[0015] In the formula, F(t) represents the state coefficient matrix; G(t) represents the error coefficient matrix; W(t) represents the noise random error; the noise random error W(t) includes the white noise and colored noise of gyroscope drift and the white noise of accelerometer drift.

[0016] Furthermore, based on the theoretical positions of each UAV, the theoretical distance ρ from the wingman to the main unit is derived. li The formula is as follows:

[0017] ρ li =r i +e i1 δx+e i2 δy+e i3 δz

[0018]

[0019]

[0020]

[0021]

[0022] In the formula, (x, y, z) represents the theoretical coordinates of the host computer; Let represent the theoretical coordinates of the i-th wingman.

[0023] Furthermore, based on the measured distance The theoretical distance from each wingman to the main engine was derived. The formula is as follows:

[0024]

[0025] In the formula, bias is the crystal oscillator deviation. For measurement error in UWB ranging, This represents the measured distance from the i-th wingman to the host aircraft.

[0026] Furthermore, by measuring distance Theoretical distance The error covariance matrix R is derived from the initial position estimates of each UAV. a→b The specific process includes:

[0027] The theoretical distance from the i-th wingman to the main unit and the position x of the i-th wingman b The relationship can be expressed by the formula: The position x of the i-th wingman b for

[0028] The variance of the estimation error can be modeled as:

[0029]

[0030] In the formula, H a→b Represented as the constant term in the Taylor expansion formula; This represents the initial position estimate of the wingman; This is expressed as the variance of the wingman's position;

[0031] Error covariance matrix R a→b The formula for expressing it is:

[0032]

[0033] In the formula, P b These are the eigenvalues ​​on the diagonal of the posterior covariance matrix of the wingman.

[0034] Furthermore, based on the measured distance Calculate the offset ΔD of the relative distance between the wingman and the main aircraft per unit time. i,k The methods include:

[0035] The distance measured from the host to the i-th wingman at time k. The measured distance from the host to the i-th wingman at time k-1. By taking the difference, we can calculate the offset ΔD of the relative distance per unit time. i,k .

[0036] Furthermore, the formula for calculating the decision threshold T is as follows:

[0037] T = σ + Δd,

[0038] Where σ is the distance measured from each wingman to the main unit obtained through UWB ranging. The variance, Δd, is the measured distance per unit time. The maximum offset.

[0039] Furthermore, based on theoretical distance and theoretical distance ρ li The initial observation model is constructed, expressed by the following formula:

[0040] δρ=ρ li -ρ UWBi =e i1 δx+e i2 δy+e i3 δz+v i

[0041] δx=δhcosLcosλ-(R N +h)sinLcosλδL-(R N +h)cosLsinλδλ

[0042] δy=δhcosLsinλ-(R N +h)sinLsinλδL-(R N +h)cosLcosλδλ

[0043] δz=δhsinL+[R N (1-f) 2+h]cosLδL

[0044] In the formula, δL, δλ, and δh represent the error values ​​of the UAV's longitude, latitude, and altitude, respectively, and R... N Let L be the Earth's radius, and let L, λ, and h represent the longitude, latitude, and altitude of the UAV, respectively; f represents the Earth's ellipticity.

[0045] Furthermore, based on the measured distance Calculate NLOS error compensation value and relative distance Combine NLOS error compensation value and relative distance Superimposed to obtain correction distance The methods include:

[0046] Based on measured distance The formula for calculating the NLOS error compensation value is as follows:

[0047]

[0048] Based on measured distance Calculate NLOS error compensation value and relative distance The formula is as follows:

[0049]

[0050] Combine NLOS error compensation value and relative distance Superimposed to obtain correction distance The formula is as follows:

[0051]

[0052]

[0053] In the formula, I represents the identity matrix; Δf represents the NLOS error compensation value, and r UWBi,k This represents the distance measured from the host aircraft to the i-th wingman at time k using UWB ranging; r UWBi,k-1 This represents the distance from the host to the i-th wingman measured at time k-1 using UWB ranging; the distance from the host to the i-th wingman is under LOS conditions, a1 to a... n All are 1; when the host to the i-th wingman is in an NLOS environment, a1 to a n All are 0.

[0054] Furthermore, the error covariance matrix R a→b The formula for expressing the location observation model, which is added to the initial observation model, is as follows:

[0055] Z ρ =δρ=H ρ(t)X(t)+V(t)

[0056] H ρ =[0 9×6 H 9×3 0 9×9 ]

[0057]

[0058] a i1 =(R N +h)[-e i1 sinLcosλ-e i2 sinLsinλ]+[R N (1-f) 2 +h]e i3 cosL

[0059] a i2 =(R N +h)[e i2 cosLcosλ-e i1 cosLsinλ]

[0060] a i3 =e i1 cosLcosλ+e i2 cosLsinλ+e i3 sinL

[0061] In the formula, X(t) represents the state variable established by the state parameters; V(t) represents the system noise, where the error covariance matrix Rt a→b Add to system noise.

[0062] In a second aspect, the present invention provides an electronic device including a storage medium and a processor; the storage medium is used to store instructions; the processor is used to operate according to the instructions to perform the method described in the first aspect.

[0063] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0064] This invention is based on measuring distance Calculate NLOS error compensation value and relative distance Combine NLOS error compensation value and relative distance Superimposed to obtain correction distance From the corrected distance and measuring distance ρ li Construct an initial observation model; define the error covariance matrix R. a→bThe initial observation model is added to obtain the position observation model; the state model and position observation model are then iteratively applied to the extended Kalman filter formula to obtain the final estimated position of the host drone; wireless communication is used to establish interaction between the host and wingmen, and wingmen follow the host for collaborative navigation; an information exchange network is used to transform the drone swarm into a host-wingmen system, enabling accurate sharing of position and velocity information; even in NLOS environments, the drone swarm maintains high positioning accuracy. Reliable positioning accuracy is maintained even in complex NLOS environments with weak GPS signals. Attached Figure Description

[0065] Figure 1 This is a flowchart of a UAV swarm cooperative navigation method based on the NLOS environment provided in the embodiment. Detailed Implementation

[0066] The present invention will be further described below with reference to the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and should not be used to limit the scope of protection of the present invention.

[0067] Example 1

[0068] like Figure 1 As shown, a UAV swarm cooperative navigation method based on NLOS environment includes: acquiring the initial position estimates and state parameters of each UAV in the swarm using a GPS / INS combination. In this embodiment, the GPS / INS combination involves mutual assistance between the GPS receiver and the IMU. GPS provides the IMU with accurate position and velocity information, helping to overcome the accumulation of IMU drift errors; the IMU provides the GPS with real-time position and velocity information, assisting the GPS tracking loop and improving GPS dynamic tracking capability and anti-interference capability. With the assistance of the IMU, as much satellite information as possible is received from GPS to improve the accuracy of filtering correction, while also detecting the integrity of GPS receiver information. Compact combination is a complex and relatively advanced combination method that can obtain a relatively accurate position of the UAV in a given coordinate system.

[0069] The navigation output parameter error of the inertial navigation system (IMU) is taken as the state. The inertial navigation system has nine navigation output parameters: three inertial navigation platform error angles, denoted as... The three velocity errors are denoted as δv. E ,δv N ,δv UThree positional errors are denoted as δL, δλ, and δh. Errors in inertial instruments, such as installation errors, calibration coefficient errors, and instrument random errors, must also be considered; these errors are typically colored noise. Assume the gyroscope drift error consists of a first-order Markov process random error and white noise error; assume the accelerometer error is a first-order Markov process random error. It is also assumed that the error models for the gyroscope and accelerometer are identical across the three axes. Because inertial navigation requires three gyroscopes and three accelerometers to acquire data in three directions, the gyroscope noise and gyroscope bias noise can be expanded into three dimensions, denoted as ε. bx ,ε by ,ε bz ,ε rx ,ε ry ,ε rz Extending the accelerometer error to three dimensions, denoted as...

[0070] State variables X(t) are established from state parameters; these state parameters include inertial navigation platform error angle, velocity error, position error, gyroscope noise, gyroscope bias noise, and accelerometer error; a state model of the UAV is constructed based on the state variables X(t). The formula is as follows:

[0071]

[0072]

[0073] W = [w gx w gy w gz w rx w ry w rz w ax w ay w az ]

[0074] In the formula, F(t) represents the state coefficient matrix; G(t) represents the error coefficient matrix; W(t) represents the noise random error; the noise random error W(t) includes white noise and colored noise from gyroscope drift and white noise from accelerometer drift; w gx w gy and w gz These represent the white noise caused by the gyroscope drift in the x, y, and z axes, respectively; w rx w ry and w rz The colored noise caused by gyroscope drift in the x, y, and z axes; w ax w ay and w azThis refers to the white noise generated by the accelerometer drift in the x, y, and z axes.

[0075] The drone swarm includes a main drone and wingmen. The theoretical distance ρ from the wingmen to the main drone is derived based on the theoretical positions of each drone. li The formula is as follows:

[0076] ρ li =r i +e i1 δx+e i2 δy+e i3 δz

[0077]

[0078]

[0079]

[0080]

[0081] In the formula, (x, y, z) represents the theoretical coordinates of the host computer; Let represent the theoretical coordinates of the i-th wingman.

[0082] The distances from each wingman to the main unit were obtained using UWB ranging. Based on measured distance The theoretical distance from each wingman to the main engine was derived. The formula is as follows:

[0083]

[0084] In the formula, bias is the crystal oscillator deviation. For measurement error in UWB ranging, This represents the measured distance from the i-th wingman to the host aircraft.

[0085] By measuring distance Theoretical distance The error covariance matrix R is derived from the initial position estimates of each UAV. a→b The specific process includes:

[0086] The theoretical distance from the i-th wingman to the main unit and the position x of the i-th wingman b The relationship can be expressed by the formula: The position x of the i-th wingman b for

[0087] The variance of the estimation error can be modeled as:

[0088]

[0089] In the formula, H a→b Represented as the constant term in the Taylor expansion formula; This represents the initial position estimate of the wingman; This is expressed as the variance of the wingman's position;

[0090] Error covariance matrix R a→b The formula for expressing it is:

[0091]

[0092] In the formula, P b These are the eigenvalues ​​on the diagonal of the posterior covariance matrix of the wingman.

[0093] According to the measured distance Calculate the offset ΔD of the relative distance between the wingman and the main aircraft per unit time. i,k The method includes: measuring the distance from the host to the i-th wingman at time k. The measured distance from the host to the i-th wingman at time k-1. By taking the difference, we can calculate the offset ΔD of the relative distance per unit time. i,k .

[0094] Compare offset ΔD i,k The communication environment between the wingman and the host is determined by comparing the data with a preset decision threshold T. The decision threshold T is calculated as follows: T = σ + Δd, where σ is the distance measured from each wingman to the host obtained by UWB ranging. The variance, Δd, is the measured distance per unit time. The maximum offset.

[0095] If the offset ΔD i,k When the distance is less than or equal to the decision threshold T, the communication LOS environment between the wingman and the host is determined; based on the theoretical distance ρ UWBi and theoretical distance ρ li The initial observation model is constructed, expressed by the following formula:

[0096] δρ=ρ li -ρ UWBi =e i1 δx+e i2 δy+e i3 δz+v i

[0097] δx=δhcosLcosλ-(R N +h)sinLcosλδL-(R N +h)cosLsinλδλ

[0098] δy=δhcosLsinλ-(R N +h)sinLsinλδL-(R N +h)cosLcosλδλ

[0099] δz=δhsinL+[R N (1-f) 2 +h]cosLδL

[0100] In the formula, i = 1, 2, 3, 4, 5, 6, 7, 8, 9; δL, δλ, and δh represent the error values ​​of the UAV's longitude, latitude, and altitude, respectively, and R... N Let L be the Earth's radius, and let L, λ, and h represent the longitude, latitude, and altitude of the UAV, respectively; f represents the Earth's ellipticity.

[0101] If the offset ΔD i,k When the distance exceeds the decision threshold T, determine the NLOS communication environment between the wingman and the host aircraft; based on the measured distance. Calculate NLOS error compensation value and relative distance Combine NLOS error compensation value and relative distance Superimposed to obtain correction distance From the corrected distance and measuring distance ρ li Construct an initial observation model;

[0102] Based on measured distance The formula for calculating the NLOS error compensation value is as follows:

[0103]

[0104] Based on measured distance Calculate NLOS error compensation value and relative distance The formula is as follows:

[0105]

[0106] Combine NLOS error compensation value and relative distance Superimposed to obtain correction distance The formula is as follows:

[0107]

[0108]

[0109] In the formula, I represents the identity matrix; Δf represents the NLOS error compensation value. This is represented as the distance from the host to the i-th wingman measured at time k using UWB ranging. This represents the distance from the host to the i-th wingman measured at time k-1 using UWB ranging; the distance from the host to the i-th wingman is under LOS conditions, a1 to a... n All are 1; when the host to the i-th wingman is in an NLOS environment, a1 to a n All are 0;

[0110] The error covariance matrix R a→b The formula for expressing the location observation model, which is added to the initial observation model, is as follows:

[0111] Z ρ =δρ=H ρ (t)X(t)+V(t)

[0112] H ρ =[0 9×6 H 9×3 0 9×9 ]

[0113]

[0114] a i1 =(R N +h)[-e i1 sinLcosλ-e i2 sinLsinλ]+[R N (1-f) 2 +h]e i3 cosL

[0115] a i2 =(R N +h)[e i2 cosLcosλ-e i1 cosLsinλ]

[0116] a i3 =e i1 cosLcosλ+e i2 cosLsinλ+e i3 sinL

[0117] In the formula, X(t) represents the state variable established by the state parameters; V(t) represents the system noise, where the error covariance matrix Rt a→b Add to system noise.

[0118] The state model and position observation model are substituted into the extended Kalman filter formula for iteration to obtain the final estimated position of the host; wireless communication is used to enable interaction between the host and the wingman, and the wingman follows the host for collaborative navigation.

[0119] First, the main aircraft and its wingman synchronize their time to ensure real-time accuracy. Then, the wingman merges its own navigation information with the main aircraft's navigation information and uses filtering algorithms to reduce navigation errors. This process corrects the navigation information, maintains the correct formation flight path, and ensures high-precision positioning overall.

[0120] Example 2

[0121] In a second aspect, the present invention provides an electronic device including a storage medium and a processor; the storage medium is used to store instructions; the processor is used to operate according to the instructions to execute the method described in Embodiment 1.

[0122] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0123] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0124] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0125] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0126] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the technical principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A method for cooperative navigation of unmanned aerial vehicle (UAV) swarms based on an NLOS environment, characterized in that, include: The initial position estimates and state parameters of each UAV in the UAV swarm are collected using a combination of GPS and INS. A state model of the UAVs is then constructed from these state parameters. The UAV swarm includes a master drone and wingmen. The theoretical distance from the wingmen to the master drone is derived based on the theoretical positions of each UAV. The formula is as follows: ； ； ； ； ； In the formula, (x, y, z) represent the theoretical coordinates of the host computer; , , Let ) represent the theoretical coordinates of the i-th wingman; The distances from each wingman to the main unit were obtained using UWB ranging. Based on measured distance The theoretical distance from each wingman to the main aircraft was derived. ; by measuring distance Theoretical distance The error covariance matrix is ​​derived from the initial position estimates of each UAV. According to the measured distance Calculate the offset of the relative distance between the wingman and the main aircraft per unit time. Compare offsets The communication environment between the wingman and the host is determined by comparing the preset decision threshold T. If offset When the distance is less than or equal to the decision threshold T, the communication between the wingman and the host is determined to be in a LOS (Longest in Sight) environment; based on the theoretical distance... Distance from theory The initial observation model is constructed, expressed by the following formula: ； ； In the formula, , and These represent the error values ​​for the longitude, latitude, and altitude of the UAV, respectively. For the Earth's radius, , and These represent the longitude, latitude, and altitude of the drone, respectively; f represents the ellipticity of the Earth. Measurement error in UWB ranging; If offset When the distance exceeds the decision threshold T, the communication environment between the wingman and the host is determined to be NLOS (Neutral Noise, Noise, and Orthogonal); based on the measured distance. Calculate NLOS error compensation value and relative distance NLOS error compensation value and relative distance Superimposed to obtain correction distance ; by correction distance and measuring distance Construct an initial observation model; The error covariance matrix The initial observation model is added to obtain the position observation model; the state model and the position observation model are substituted into the extended Kalman filter formula for iteration to obtain the final estimated position of the host; wireless communication is used to enable interaction between the host and the wingman and to coordinate navigation by having the wingman follow the host.

2. The UAV swarm cooperative navigation method according to claim 1, characterized in that, Methods for constructing a drone's state model from its state parameters include: State variables X(t) are established from state parameters; these state parameters include inertial navigation platform error angle, velocity error, position error, gyroscope noise, gyroscope bias noise, and accelerometer error; a state model of the UAV is constructed based on the state variables X(t). The formula is as follows: ； In the formula, F(t) represents the state coefficient matrix; G(t) represents the error coefficient matrix; W(t) represents the noise random error; the noise random error W(t) includes the white noise and colored noise of gyroscope drift and the white noise of accelerometer drift.

3. The UAV swarm cooperative navigation method according to claim 1, characterized in that, Based on measured distance The theoretical distance from each wingman to the main aircraft was derived. The formula is as follows: ； In the formula, This is due to crystal oscillator deviation. Measurement error in UWB ranging This represents the measured distance from the i-th wingman to the host aircraft.

4. The UAV swarm cooperative navigation method according to claim 3, characterized in that, By measuring distance Theoretical distance The error covariance matrix is ​​derived from the initial position estimates of each UAV. The specific process includes: The theoretical distance from the i-th wingman to the main unit and the position of the i-th wingman The relationship can be expressed by the formula: The position of the i-th wingman for( , , ); The variance model for the estimation error is as follows: ； In the formula, Represented as the constant term in the Taylor expansion formula; This represents the initial position estimate of the wingman; This is expressed as the variance of the wingman's position; Error covariance matrix The formula for expressing it is: ； In the formula, These are the eigenvalues ​​on the diagonal of the posterior covariance matrix of the wingman.

5. The UAV swarm cooperative navigation method according to claim 1, characterized in that, The formula for calculating the decision threshold T is: ； in, To obtain the distances from each wingman to the main unit using UWB ranging. variance Measuring distance per unit time The maximum offset.

6. The UAV swarm cooperative navigation method according to claim 3, characterized in that, Based on measured distance Calculate NLOS error compensation value and relative distance NLOS error compensation value and relative distance Superimposed to obtain correction distance The methods include: Based on measured distance The formula for calculating the NLOS error compensation value is as follows: ； Based on measured distance Calculate NLOS error compensation value and relative distance The formula is as follows: ； Combine NLOS error compensation value and relative distance Superimposed to obtain correction distance The formula is as follows: ； ； In the formula, Represented as an identity matrix; This is expressed as the NLOS error compensation value. This is represented as the distance from the host to the i-th wingman measured at time k using UWB ranging. This represents the distance from the host aircraft to the i-th wingman measured at time k-1 using UWB ranging; the distance from the host aircraft to the i-th wingman is under LOS conditions. to All are 1; when the host and the i-th wingman are in an NLOS environment, to All are 0.

7. The UAV swarm cooperative navigation method according to claim 1, characterized in that, The error covariance matrix The formula for expressing the location observation model, which is added to the initial observation model, is as follows: ； ； ； ； In the formula, Represented as state variables established from state parameters; Represented as system noise, where the error covariance matrix is... Add to system noise.

8. An electronic device, comprising a storage medium and a processor; said storage medium for storing instructions; said processor for operating according to said instructions to perform the method of any one of claims 1 to 7.

Images