A cluster cooperative navigation method based on particle distance fitness

Abstract

This invention provides a swarm cooperative navigation method based on particle distance fitness, relating to the field of multi-agent cooperative navigation technology. The method includes: in a multi-UAV cooperative formation composed of multiple UAVs, establishing a system motion model based on uniform acceleration curve kinematics and constructing a system measurement model using relative ranging constraints measured by ultra-wideband; based on the constructed system motion model and system measurement model, constructing a swarm cooperative navigation filtering model; and based on the constructed swarm cooperative navigation filtering model, using an error-constrained quantum particle swarm optimization improved particle filtering algorithm based on particle distance fitness for iterative filtering to obtain the optimized results of the state variables of each UAV. This invention can effectively improve the positioning accuracy and robustness of swarm UAVs in the event of global satellite navigation system rejection.

Description

Technical Field

[0001] This invention relates to the field of multi-agent cooperative navigation technology, and in particular to a swarm cooperative navigation method based on particle distance fitness. Background Technology

[0002] Collaborative navigation systems for swarm drones (hereinafter referred to as swarm collaborative navigation systems) are systems that enable drones to communicate and exchange information to jointly complete navigation and mission execution. They have wide applications in both military and civilian fields. Currently, the Global Navigation Satellite System (GNSS) remains the primary choice for most swarm drones to solve positioning problems. However, in environments where GNSS signals are interfered with or even denied, the positioning accuracy of drones will be significantly reduced. Therefore, collaborative navigation and positioning of drone swarms in GNSS-denied environments is one of the key issues for effectively improving the performance of drone swarms.

[0003] Inertial Navigation Systems (INS) based on Inertial Measurement Units (IMUs) offer advantages such as high accuracy and ease of implementation. However, they face the problems of accumulated error and drift, which significantly limit their application. Existing methods primarily employ a series of filtering-based approaches to address these issues. However, these methods can only suppress the growth rate of accumulated error to a certain extent, not eliminate it. Improving the accuracy of positioning algorithms using filtering techniques remains a challenge. More importantly, in practical applications, system and measurement models are often nonlinear. Extended Kalman filtering estimates the mean and covariance of the state by linearizing the state equations, but this often involves tedious calculations of the Jacobian matrix. Since the error is introduced by linearization, unscented Kalman filters are still unsuitable for high-order nonlinear system models. Compared to other filtering methods, particle filtering algorithms, while computationally more expensive, are better suited for nonlinear non-Gaussian systems. However, due to imperfect sampling, particle filtering generally suffers from drawbacks such as particle impoverishment and sample size dependence. Summary of the Invention

[0004] To address the technical problems of low positioning accuracy and poor robustness of swarmed UAVs in the event of global satellite navigation system denial, this invention provides a swarm cooperative navigation method based on particle distance fitness.

[0005] This invention provides a swarm cooperative navigation method based on particle distance fitness, comprising:

[0006] S1. In a multi-UAV cooperative formation consisting of multiple UAVs, a system motion model is established based on uniform acceleration curve kinematics, and a system measurement model is constructed using the relative ranging constraints obtained from ultra-wideband measurements.

[0007] S2. Based on the constructed system motion model and system measurement model, a cluster cooperative navigation filtering model is constructed.

[0008] S3. Based on the constructed cluster cooperative navigation filtering model, the improved particle filtering algorithm based on particle distance fitness error constraint quantum particle swarm optimization is used for iterative filtering to obtain the optimization results of the state variables of each UAV.

[0009] On the other hand, a computer-readable storage medium is provided, wherein at least one instruction is stored therein, the at least one instruction being loaded and executed by a processor to implement any of the above-described methods of cluster cooperative navigation based on particle distance fitness.

[0010] The beneficial effects of the technical solutions provided by the embodiments of the present invention include at least the following:

[0011] In this embodiment of the invention, in a multi-UAV cooperative formation composed of multiple UAVs, a system motion model is established based on uniform acceleration curve kinematics, and a system measurement model is constructed using relative ranging constraints obtained from ultra-wideband measurements. Based on the constructed system motion model and system measurement model, a cluster cooperative navigation filtering model is built. Based on the constructed cluster cooperative navigation filtering model, an error-constrained quantum particle swarm optimization improved particle filtering algorithm based on particle distance fitness is used for iterative filtering to obtain the optimized results of the state variables of each UAV. In this way, the positioning accuracy and robustness of cluster UAVs can be effectively improved in the case of global satellite navigation system rejection. Attached Figure Description

[0012] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0013] Figure 1 This is a network topology diagram of the clustered unmanned aerial vehicle (UAV) cooperative navigation system provided in an embodiment of the present invention;

[0014] Figure 2 This is a flowchart of a cluster cooperative navigation method based on particle distance fitness provided in an embodiment of the present invention;

[0015] Figure 3This is a schematic diagram of the improved particle filtering algorithm for error-constrained quantum particle swarm optimization based on particle distance fitness provided in an embodiment of the present invention;

[0016] Figure 4 This is a schematic diagram of the error ellipsoid provided in an embodiment of the present invention;

[0017] Figure 5 This is a schematic diagram of the UAV position distribution for simulation and experimental analysis provided in an embodiment of the present invention;

[0018] Figure 6 This is a schematic diagram of a drone trajectory for simulation and experimental analysis provided in an embodiment of the present invention;

[0019] Figure 7(a) is a schematic diagram comparing the positioning errors in the longitude direction of one of the slave nodes in this invention;

[0020] Figure 7(b) is a schematic diagram comparing the latitude positioning errors of one of the slave nodes in this invention;

[0021] Figure 7(c) is a schematic diagram comparing the positioning errors in the height direction of one of the slave nodes in this invention;

[0022] Figure 8 This is a schematic diagram of the simulation results of the positioning error of 10 slave devices under different optimization methods in this invention. Detailed Implementation

[0023] The technical solution of the present invention will now be described with reference to the accompanying drawings.

[0024] In embodiments of the present invention, words such as "exemplarily," "for example," etc., are used to indicate that something is an example, illustration, or description. Any embodiment or design described as "exemplary" in the present invention should not be construed as being more preferred or advantageous than other embodiments or designs. Specifically, the use of the word "exemplary" is intended to present the concept in a concrete manner. Furthermore, in embodiments of the present invention, the meaning expressed by "and / or" can be both, or either one.

[0025] In the embodiments of this invention, the terms "image" and "picture" may sometimes be used interchangeably. It should be noted that, without emphasizing the distinction between them, their intended meanings are consistent. Similarly, the terms "of," "corresponding (relevant)," and "corresponding" may sometimes be used interchangeably. It should be noted that, without emphasizing the distinction between them, their intended meanings are consistent.

[0026] In this embodiment of the invention, sometimes a subscript such as W1 may be written in a non-subscript form such as W1. When the difference is not emphasized, the meaning they express is the same.

[0027] To make the technical problems, technical solutions and advantages of the present invention clearer, a detailed description will be given below in conjunction with the accompanying drawings and specific embodiments.

[0028] This invention provides a swarm cooperative navigation method based on particle distance fitness, applied in the scenario of swarm UAV cooperative navigation under GNSS denial conditions, wherein the swarm UAV cooperative navigation system is as follows: Figure 1 As shown, this swarm drone cooperative navigation system comprises 2 master drones and 10 slave drones. Each drone is equipped with an IMU sensor, an Ultra Wide Band (UWB) ranging sensor, and communication equipment. The master drones are equipped with high-precision IMU devices and GPS, while the 10 slave drones are equipped with lower-precision IMU devices. Distance is measured between the drones using UWB sensors, and the measurement noise is considered to be Gaussian noise. Furthermore, the drones act as communication nodes in the network, enabling them to send and receive information with other drone nodes connected in the topology. Figure 2 The flowchart shown is for a cluster cooperative navigation method based on particle distance fitness. The processing flow of this method may include the following steps:

[0029] S1. In a multi-UAV cooperative formation consisting of multiple UAVs, a system motion model is established based on uniform acceleration curve kinematics, and a system measurement model is constructed using the relative ranging constraints obtained from ultra-wideband measurements.

[0030] In this embodiment, the UAV acts as a communication node in the clustered collaborative navigation system network, and UAV i is referred to as node i.

[0031] The state vector of node i at time t is defined as Let i represent the three-dimensional position state of node i at time t. Let represent the three-dimensional velocity state of node i at time t. This represents the three-dimensional acceleration information of node i at time t;

[0032] The established system motion model is represented as follows:

[0033]

[0034] Where F is the state transition matrix; T is the sampling time; Let i be the state vector of node i at time t-1; Let be the system process noise at node i at time t-1, which has a mean of 0 and a variance of Q. t The Gaussian distribution.

[0035] In this embodiment, the relative ranging constraint obtained by ultra-wideband measurement is constructed from the relative measurement information between each node and the position information obtained by communication between nodes;

[0036] Considering the excessive cost of equipping all swarm drones with high-precision navigation equipment, in this embodiment's swarm cooperative navigation system, the master unit is equipped with high-precision navigation equipment (i.e., high-precision IMU and GPS), while all slave units are equipped with low-precision sensors (i.e., lower-precision IMU). The slave units utilize the accurate position information acquired by the master unit to correct their own state variables through calculation, achieving the fusion of cooperative information and suppressing error accumulation. If node i and node j can communicate at time t, then the system measurement model of the nodes is expressed as:

[0037]

[0038] Among them, Z t The system's measured values ​​are represented by P and V, which are the absolute position and velocity information of the slave device calculated by the inertial measurement unit (IMU), respectively, and S, θ, and These are the observations of the relative distance, relative heading angle, and relative pitch angle between the UAVs;

[0039] The measurement equation for relative navigation information is determined as follows:

[0040] z t ′=h′(X t )+R t ′

[0041]

[0042] Among them, z t ′ represents the measurement equation for relative navigation information, h′(X t () is an intermediate expression. This represents the position information of node i at time t; Represents the position information of node j at time t; R t ′ represents relative measurement noise.

[0043] S2. Based on the constructed system motion model and system measurement model, a cluster cooperative navigation filtering model is constructed.

[0044] S3. Based on the constructed cluster cooperative navigation filtering model, an improved particle filtering algorithm based on particle distance fitness and error constraint quantum particle swarm optimization is used for iterative filtering to obtain the optimization results of the state variables of each UAV. Taking any slave UAV as an example, step S3 is explained in detail, such as... Figure 3 As shown, the specific steps may include:

[0045] S31. For any slave device i, similar to the traditional particle filter algorithm, at time t=0, based on the initial state vector... N particles are generated using a Gaussian random distribution, i.e. Among them, represents the initial state of 1 to N particles, and ∼ represents following, represents the prior probability. The value of the number of particles N mainly depends on the balance between the required accuracy and computing resources. The larger N is, the higher the accuracy, but the greater the computational amount. Therefore, the number of particles will be adjusted during the experiment to balance. In this embodiment, N is set to 500;

[0046] S32. When the slave device i moves, use the system motion model to update the initial particle state and calculate the prior probability to obtain the initial state prediction result In this step of the embodiment of the present invention, there is no need to calculate the weight information required by the traditional particle filter algorithm;

[0047] S33. Resample the particles using resampling based on error constraint and quantum particle swarm optimization to obtain the optimal resampling result; specifically, it may include the following steps:

[0048] S331. Establish an error ellipsoid based on the covariance matrix using error constraint, and select a group of particles within the error ellipsoid;

[0049] The application scenario of the embodiment of the present invention is cooperative navigation in the case of global satellite navigation system denial. The absolute positioning of the unmanned aerial vehicle is realized by using the IMU, which will cause the positioning accuracy to decrease over time. Therefore, in this stage, error constraint is used to define an ellipsoid range based on the covariance matrix, and the qualified particles are screened out to ensure that the particles are distributed in a high-confidence region, thereby improving the accuracy and stability of the algorithm.

[0050] In this embodiment, when performing interval estimation of the scale (i.e., estimating the value range of the scale), if a small probability β is given in advance, then the (1-β)% confidence interval of the s scale accuracy is defined as the interval <s1, s2>; where s1 and s2 are the lower confidence limit and the upper confidence limit respectively, Pr(s1 < s < s2) = 1-β, and Pr(s1 < s < s2) represents the probability that s falls within a certain interval <s1, s2>. The probability β represents the significance level, and 1-β represents the confidence level. This confidence interval is used as the error constraint;

[0051] Determine the covariance matrix C of N particles at a certain moment as:

[0052]

[0053] Among them, cov(x, x), cov(y, y), and cov(z, z) are the variances in the x-axis, y-axis, and z-axis directions respectively, and cov(x, y), cov(x, z), and cov(y, z) represent the correlations in different directions;

[0054] Establish with The error ellipsoid centered at the center is represented as:

[0055]

[0056] Where λ1, λ2, and λ3 are the three eigenvalues ​​of the covariance matrix C; μ x μ y μ z The estimated center location is: s is the scale of the ellipsoid; x, y, z are the three-dimensional coordinates of the error ellipsoid; where the error ellipsoid is as follows: Figure 4 As shown;

[0057] After obtaining the result derived from the particle distribution, After centered on the error ellipsoid, select a group of particles within the error ellipsoid. Where M≤N, Let M be the particle swarm, representing the M particles sampled within the error ellipsoid.

[0058] In this embodiment, based on the estimation center and scale of the system motion model, an error ellipsoid based on the covariance matrix is ​​defined using error constraints. Particles that meet the conditions are selected to ensure that the particles are distributed in a high-confidence region. This achieves distance constraints based on geometric position, which is beneficial to the accuracy and stability of positioning.

[0059] S332. Use the quantum particle swarm optimization algorithm to resample the particles selected within the error ellipsoid to obtain the optimal resampling result.

[0060] In order to The state of the target is estimated by sampling to obtain a set of optimally selected particles. In this embodiment, the resampling problem is transformed into the problem of finding the optimal solution. First, a quantum particle swarm Q = {Q1, Q2, ..., Q3} is initialized. np}; where np is the number of quantum particles, and each quantum particle Q i They are all m-dimensional real-valued vectors 1≤j≤M, M and particle swarm The number of M components is the same; for each component of the quantum particle Its value represents the particle swarm. The probability of the corresponding particle being selected; the quantum particle swarm optimization algorithm will iterate multiple times until its stopping condition is met, that is, the maximum number of iterations of the quantum particle swarm optimization algorithm is reached or the global optimum remains unchanged after half of the maximum number of iterations.

[0061] For randomly initialized Q k Each quantum particle By applying random observation as shown in the following formula, it transforms into an M-dimensional binary particle. in, Represented as:

[0062]

[0063] Among them, Q k Denotes the quantum particle after k iterations, the binary particle D. i Particle swarm Resampling results; components The value indicates whether the selection is selected, i.e., 1 indicates selection and 0 indicates no selection;

[0064] Each binary particle D i Fitness function based on particle distance. i Defined as:

[0065]

[0066] Among them, Dis j express and The distance between them Let B represent the j-th particle in the quantum particle swarm optimization algorithm. The variance, F i,min F i,max F i The minimum and maximum values; higher fitness indicates more reliable sampling results;

[0067] After obtaining the fitness of each binary particle, the quantum particle swarm can be updated in the k-th iteration according to the following formula:

[0068]

[0069] in, and They represent quantum particles Q and Q respectively. i The personal and global historical optimal solutions at the (k+1)th iteration; represents the quantum particle after k+1 iterations; ε∈[0,1] represents the control parameter; It is an M-dimensional unit vector; and They represent binary particles D respectively. i The individual and global optimal solutions at the k-th iteration are determined by the binary particle D. kDetermination of the fitness value set; the coefficients c1 and c2 represent the degree of belief in oneself and the personal optimal solution, and they satisfy c1 ∈ [0, 1], c2 ∈ [0, 1], and 0 < c1 + c2 < 1; in experience, when ε > 0.7, the algorithm can exhibit better performance. A low ε value indicates slow convergence, and a high ε value indicates low convergence performance. At the same time, according to experience, it can be observed that the weight ratio of the best global position factor (1 - c1 - c2) is more important than c1 and c2. A lower value indicates a slower convergence speed, and a higher value indicates a lack of diversity. When c1 = c2 = 0.2 and (1 - c1 - c2) = 0.6, the algorithm has better performance;

[0070] Generate a new quantum particle from the above formula Replace Perform iteration, and at the same time convert it into a binary particle For updating And Until the quantum particle swarm optimization algorithm meets its stopping condition, that is, reaches the maximum number of iterations of the quantum particle swarm optimization algorithm or the global optimal value remains unchanged after half of the maximum number of iterations, then the final optimal solution D can be obtained * , which illustrates an optimal resampling result. After performing the copy operation on all particles corresponding to D * , the number of particle N remains unchanged. The selected particle set will be used to obtain the final estimate

[0071] In this embodiment, in the resampling stage, quantum particle swarm optimization (QPSO) is used to replace the traditional weight-based resampling method. QPSO simulates quantum behavior, maintains particle diversity, avoids the problem of particle degradation, reduces the dependence on the sample size, and improves the performance of particle filtering; at the same time, a new fitness function based on particle distance is adopted in the process of quantum particle swarm optimization to fuse the distance information between particles to improve the positioning accuracy.

[0072] S34. According to the obtained optimal resampling result, use the system measurement model and the measurement equation to perform measurement update on the particles to obtain the final state estimate at time t = 0

[0073] In this embodiment, when slave i obtains the node information that can interact with it, including its position information P and the observed values of the relative distance, relative heading angle, and relative pitch angle between the two, use the system measurement model and the measurement equation to perform measurement update on the particles to obtain the final state estimate Represents the final value of the state quantity obtained by optimized filtering at time t = 0.

[0074] S35. According to the final state estimate obtained after the quantum particle swarm optimization resampling process A weighted consensus algorithm is used to update acceleration information for updating state variables in the next time step prediction stage.

[0075] In this embodiment, a weighted consensus algorithm is used to exchange the states between UAVs, and the cooperative navigation system in this embodiment adopts a multi-navigation mode.

[0076] In this embodiment, for the selected slave device, the acceleration information of the slave device is updated using the states of the master and slave devices with which it communicates, so as to be used for the state quantity update in the prediction stage of the next time step. The weighted consensus algorithm is expressed as follows:

[0077]

[0078] in, The derivative of the slave device's velocity represents the slave device's acceleration information. n represents the total number of nodes; P i F and Both represent the location information of the slave device; Indicates the host's location information; V i F and Both represent the speed information of the slave device; This represents the host's speed information; γ0 and γ1 are control parameters; j∈G ff G represents the node j that communicates with slave i. ff a represents the set of UAV nodes that communicate with slave i; ij Indicates the communication status between slave devices; d ij This indicates the communication status between the slave device and the master device.

[0079] In this embodiment, if there is an edge between the slave and the master, then d ij It equals the weight value a of this edge. h_s The weight of the edge between the slave and the master is represented as:

[0080] a h_s =W h ×C s

[0081] Among them, W h The connection weight of the master is represented by the number of connections between slave i and the master; C s Indicates the number of other slave devices connected to slave device i;

[0082] If there is an edge between slave devices, then a ij It equals the weight value a of this edge. s_s The weights of the edges between slave devices are represented as follows:

[0083] a s_s =(W h,i ×Q i )+(W h,j ×Q j ))

[0084] Among them, W h,i W represents the number of hosts connected to slave device i. h,j Q represents the number of masters connected to slave j. i Q j These represent the number of connections between slave i and other slaves, and the number of connections between slave j and other slaves, respectively.

[0085] In this embodiment, a weighted consensus algorithm is used to optimize the information interaction between UAVs in cooperative navigation. A new method for calculating communication weights is adopted on the communication topology map of the UAV cluster. Further processing is performed on the basis of filtering to ensure that the navigation data in the cluster system can be more accurately and effectively integrated.

[0086] The initial position of the drone swarm is as follows: Figure 5 As shown, the trajectory of one of the drones is as follows: Figure 6 As shown.

[0087] Figures 7(a)-7(c) This is a graph showing the absolute value of the positioning error of one of the slave nodes in this invention. Figure 8 The simulation results of positioning errors of 10 slave drones under different optimization methods are shown in the figure. As can be seen from the figure, compared with the case without cooperation, the swarm UAV cooperative navigation algorithm of the present invention can significantly improve the positioning level of the swarm aircraft. Moreover, the cooperative navigation algorithm proposed in this invention has a certain improvement in positioning longitude compared with cooperative navigation based on particle filter (PF) and cooperative navigation methods based on unscented particle filter (UPF).

[0088] To quantitatively analyze positioning errors, the root mean square error (RMSE) of aircraft positioning was statistically analyzed. The statistical results are shown in Table 1. The formula for calculating the position estimation error in Table 1 is as follows:

[0089]

[0090] Where E represents the root mean square error of the UAV, Err lon Err lat and Err alt These represent the errors in the longitude, latitude, and altitude directions, respectively.

[0091] Table 1. Statistical Results of Positioning Errors

[0092]

[0093] As shown in Table 1, in the comparison of positioning errors among different methods, the method of this invention has significantly lower errors in all three dimensions (longitude, latitude, and altitude) than other methods. Compared with the non-cooperative, PF, and UPF methods, the method of this invention demonstrates a clear advantage in positioning accuracy across all dimensions. Overall, the method of this invention also exhibits higher accuracy and reliability in terms of overall positional error, far surpassing other methods. Therefore, qualitative and quantitative experimental results prove that the method of this invention has a significant improvement and enhancement in positioning accuracy compared to other algorithms.

[0094] In summary, under the condition of global satellite navigation system denial, the method of the present invention enables each UAV to perform cooperative navigation by combining its own IMU information and the information of UAVs communicating with it, effectively improving the positioning accuracy and robustness of the swarm of UAVs.

[0095] The above embodiments can be implemented, in whole or in part, by software, hardware (such as circuits), firmware, or any other combination thereof. When implemented using software, the above embodiments can be implemented, in whole or in part, as a computer program product. The computer program product includes one or more computer instructions or computer programs. When the computer instructions or computer programs are loaded or executed on a computer, all or part of the processes or functions described in the embodiments of the present invention are generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired (e.g., infrared, wireless, microwave, etc.) means. The computer-readable storage medium can be any available medium that a computer can access or a data storage device such as a server or data center that includes one or more sets of available media. The available medium can be a magnetic medium (e.g., floppy disk, hard disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium. A semiconductor medium can be a solid-state drive.

[0096] It should be understood that the term "and / or" in this article is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, or B existing alone. A and B can be singular or plural. Additionally, the character " / " in this article generally indicates an "or" relationship between the preceding and following related objects, but it can also represent an "and / or" relationship. Please refer to the context for a more accurate understanding.

[0097] In this invention, "at least one" means one or more, and "more than one" means two or more. "At least one of the following" or similar expressions refer to any combination of these items, including any combination of a single item or a plurality of items. For example, at least one of a, b, or c can represent: a, b, c, ab, ac, bc, or abc, where a, b, and c can be a single item or multiple items.

[0098] It should be understood that, in various embodiments of the present invention, the order of the above-mentioned process numbers does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0099] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0100] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the devices, apparatuses, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0101] In the several embodiments provided by this invention, it should be understood that the disclosed devices, apparatuses, and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another device, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between devices or units may be electrical, mechanical, or other forms.

[0102] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0103] In addition, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit.

[0104] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0105] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A swarm cooperative navigation method based on particle distance fitness, characterized in that, The method includes: S1. In a multi-UAV collaborative formation composed of multiple UAVs, establish a system motion model based on uniform acceleration curve kinematics and construct a system measurement model from the relative ranging constraints measured by ultra-wideband; S2. Based on the established system motion model and system measurement model, construct a cluster collaborative navigation filtering model; S3. Based on the constructed cluster collaborative navigation filtering model, use an improved particle filtering algorithm with error constraint quantum particle swarm optimization based on particle distance fitness to perform iterative filtering to obtain the optimized results of the state variables of each UAV; Among them, the step of using an improved particle filtering algorithm with error constraint quantum particle swarm optimization based on particle distance fitness to perform iterative filtering to obtain the optimized results of the state variables of each UAV includes: S31. For any slave device i, at time t=0, according to the initial state vector N particles are generated using a Gaussian random distribution, i.e. in, This represents the initial state of 1 to N particles, and ~ indicates obedience. Represents prior probability; S32. When slave device i moves, update the initial particle state using the system motion model and calculate the prior probability. Obtain the initial state prediction result S33. Resample the particles using resampling based on error constraint and quantum particle swarm optimization to obtain the optimal resampling result; S34. Based on the obtained optimal resampling results, the particle measurements are updated using the system measurement model and measurement equations to obtain the final state estimate at time t=0. S35, The final state estimate obtained after quantum particle swarm optimization resampling. A weighted consensus algorithm is used to update acceleration information for updating state variables in the next time step prediction stage.

2. The swarm cooperative navigation method based on particle distance fitness according to claim 1, characterized in that, UAVs act as individual communication nodes in the cluster collaborative navigation system network; The state vector of node i at time t is defined as Let i represent the three-dimensional position state of node i at time t. Let represent the three-dimensional velocity state of node i at time t. This represents the three-dimensional acceleration information of node i at time t; The established system motion model is expressed as: Where F is the state transition matrix; T is the sampling time; Let i be the state vector of node i at time t-1; Let be the system process noise at node i at time t-1.

3. The swarm cooperative navigation method based on particle distance fitness according to claim 2, characterized in that, The relative ranging constraints measured by ultra-wideband are constructed from the relative measurement information between nodes and the position information obtained through communication between nodes; In the cluster collaborative navigation system, the host is equipped with high-precision navigation equipment, and all slave UAVs are equipped with low-precision sensors. The slave UAVs use the accurate position information obtained by the host to correct their own state variables through calculation, realizing the fusion of collaborative information and suppressing error accumulation. If node i and node j can communicate at time t, then the system measurement model of the node is expressed as: Among them, Z t The system's measured values ​​are represented by P and V, which are the absolute position and velocity information of the slave device calculated by the inertial measurement unit, respectively, and S, θ, and These are the observations of the relative distance, relative heading angle, and relative pitch angle between the UAVs; The measurement equation for determining the relative navigation information is: z t ′=h′(X t )+R t ′ Among them, z t ′ represents the measurement equation for relative navigation information, h′(X t () is an intermediate expression. This represents the position information of node i at time t; Represents the position information of node j at time t; R t ′ represents relative measurement noise.

4. The swarm cooperative navigation method based on particle distance fitness according to claim 1, characterized in that, The step of resampling the particles using resampling based on error constraint and quantum particle swarm optimization to obtain the optimal resampling result includes: Establish an error ellipsoid based on the covariance matrix using error constraint, and select a set of particles within the error ellipsoid; Use the quantum particle swarm optimization algorithm to resample the particles selected within the error ellipsoid to obtain the optimal resampling result.

5. The swarm cooperative navigation method based on particle distance fitness according to claim 4, characterized in that, The step of establishing an error ellipsoid based on the covariance matrix using error constraint and selecting a set of particles within the error ellipsoid includes: When performing interval estimation on the scale, if a small probability β is given in advance, define the (1-β)% confidence interval for the s scale accuracy as the interval <s1, s2>; where s1 and s2 are the confidence lower limit and confidence upper limit respectively, Pr(s1 < s < s2) = 1-β, and Pr(s1 < s < s2) represents the probability that s falls within a certain interval <s1, s2>. The probability β represents the significance level, and 1-β represents the confidence level. This confidence interval is used as the error constraint; Determine the covariance matrix C of N particles at a certain moment as: Among them, cov(x, x), cov(y, y), and cov(z, z) are the variances in the x-axis, y-axis, and z-axis directions respectively, and cov(x, y), cov(x, z), and cov(y, z) represent the correlations in different directions; Establish with The error ellipsoid centered at the center is represented as: Where λ1, λ2, and λ3 are the three eigenvalues ​​of the covariance matrix C; μ x μ y μ z The estimated center location is: s is the scale of the ellipsoid; x, y, z are the three-dimensional coordinates of the error ellipsoid; Select a set of particles within the error ellipsoid Where M≤N, Let M be the particle swarm, representing the M particles sampled within the error ellipsoid.

6. The swarm cooperative navigation method based on particle distance fitness according to claim 5, characterized in that, The step of using the quantum particle swarm optimization algorithm to resample the particles selected within the error ellipsoid to obtain the optimal resampling result includes: Initialize a quantum particle group Q = {Q1, Q2, ..., Qn} np }; where np is the number of quantum particles, and each quantum particle Q i They are all m-dimensional real-valued vectors 1≤j≤M, M and particle swarm The number of M components is the same; for each component of the quantum particle Its value represents the particle swarm. The probability that the corresponding particle is selected; For randomly initialized Q k Each quantum particle By applying random observation as shown in the following formula, it transforms into an M-dimensional binary particle. in, Represented as: Among them, Q k Denotes the quantum particle after k iterations, the binary particle D. i Particle swarm Resampling results; components The value indicates whether the selection is selected, i.e., 1 indicates selection and 0 indicates no selection; Each binary particle D i Fitness function based on particle distance. i Defined as: Among them, Dis j express and The distance between them Let B represent the j-th particle in the quantum particle swarm optimization algorithm. The variance, F i,min F i,max F i The minimum and maximum values; After obtaining the fitness of each binary particle, the quantum particle swarm can be updated in the k-th iteration according to the following formula: in, and They represent quantum particles Q and Q respectively. i The personal and global historical optimal solutions at the (k+1)th iteration, 1≤i≤np; represents the quantum particle after k+1 iterations; ε∈[0,1] represents the control parameter; It is an M-dimensional unit vector; and They represent binary particles D respectively. i The individual and global optimal solutions at the k-th iteration are determined by the binary particle D. k The fitness value set is determined; coefficients c1 and c2 represent the degree of confidence in oneself and the individual optimal solution, satisfying c1∈[0,1], c2∈[0,1], and 0. <c1+c2<1; New quantum particles are generated from the above equation. replace Iterate through the process, while simultaneously transforming it into binary particles. Used for updating and The final optimal solution D is obtained when the quantum particle swarm optimization algorithm meets its stopping condition, that is, when the maximum number of iterations of the quantum particle swarm optimization algorithm is reached or the global optimum remains unchanged after half of the maximum number of iterations. * This illustrates an optimal resampling result.

7. The swarm cooperative navigation method based on particle distance fitness according to claim 6, characterized in that, The process of updating particle measurements using the system measurement model and measurement equations based on the obtained optimal resampling results to obtain the final estimate at time t=0 includes: When slave device i obtains information from nodes it can interact with, including their position information P and the observations of their relative distance, relative heading angle, and relative pitch angle, it uses the system measurement model and measurement equations to update the particle's measurements, thus obtaining the final state estimate. This represents the final value of the state variable obtained through optimized filtering at time t=0.

8. The swarm cooperative navigation method based on particle distance fitness according to claim 7, characterized in that, The final state estimate obtained after quantum particle swarm optimization resampling processing The weighted consensus algorithm is used to update acceleration information for state updates in the next time step prediction phase, including: For the selected slave, the slave's acceleration information is updated using the states of the master and slave with which it communicates, so that it can be used for the state quantity update in the next prediction phase. The weighted consensus algorithm is expressed as: in, The derivative of the slave device's velocity represents the slave device's acceleration information. n represents the total number of nodes; P i F and Both represent the location information of the slave device; Indicates the host's location information; V i F and Both represent the speed information of the slave device; This represents the host's speed information; γ0 and γ1 are control parameters; j∈G ff G represents the node j that communicates with slave i. ff a represents the set of UAV nodes that communicate with slave i; ij Indicates the communication status between slave devices; d ij This indicates the communication status between the slave device and the master device.

9. The swarm cooperative navigation method based on particle distance fitness according to claim 8, characterized in that, If there is an edge between the slave and the master, then d ij It equals the weight value a of this edge. h_s The weight of the edge between the slave and the master is represented as: a h_s =W h ×C s Among them, W h The connection weight of the master is represented by the number of connections between slave i and the master; C s Indicates the number of other slave devices connected to slave device i; If there is an edge between slave devices, then a ij It equals the weight value a of this edge. s_s The weights of the edges between slave devices are represented as follows: a s_s =(W h,i ×Q i )+(W h,j ×Q j ) Among them, W h,i W represents the number of hosts connected to slave device i. h,j Q represents the number of masters connected to slave j. i Q j These represent the number of connections between slave i and other slaves, and the number of connections between slave j and other slaves, respectively.

Images