Cluster collaborative navigation method based on particle distance fitness

Abstract

The invention provides a cluster collaborative navigation method based on particle distance fitness, and relates to the technical field of multi-agent collaborative navigation. The method comprises the following steps: in a multi-unmanned aerial vehicle cooperative formation consisting of a plurality of unmanned aerial vehicles, establishing a system motion model according to uniformly accelerated curve kinematics, and forming a system measurement model by relative distance measurement constraints measured by an ultra wide band; constructing a cluster collaborative navigation filtering model based on the constructed system motion model and system measurement model; based on the constructed cluster collaborative navigation filtering model, performing iterative filtering by adopting an error constraint quantum particle swarm optimization improved particle filtering algorithm based on particle distance fitness to obtain an optimization result of a state variable of each unmanned aerial vehicle; according to the method, the positioning precision and robustness of the cluster unmanned aerial vehicle under the condition that the global satellite navigation system is denial can be effectively improved.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-agent collaborative navigation, and in particular to a cluster collaborative navigation method based on particle distance fitness. Background Art

[0002] The swarm UAV collaborative navigation system (abbreviated as: swarm collaborative navigation system) is a system that completes navigation and task execution through communication and information exchange between UAVs. It can be widely used in military and civilian fields. At present, the Global Navigation Satellite System (GNSS) is still the main choice for most swarm UAVs to solve the positioning problem. However, in some environments where GNSS signals are interfered, restricted or even denied, the positioning accuracy of UAVs will be greatly reduced. Therefore, the collaborative navigation and positioning of UAV swarms in GNSS denied environments is one of the key issues to effectively improve the performance of UAV swarms.

[0003] The inertial navigation system (INS) based on the inertial measurement unit (IMU) has the advantages of high accuracy and easy implementation. However, it faces the problem of cumulative error and drift, which greatly limits its application. Existing methods mainly use a series of filtering-based methods to solve the above problems. However, they can only suppress the growth rate of cumulative errors to a certain extent, but cannot eliminate the cumulative errors. So far, it is still challenging to improve the accuracy of positioning algorithms using filtering technology. More importantly, in practical applications, the system model and measurement model are usually nonlinear. The extended Kalman filter estimates the mean and covariance of the state by linearizing the state equation, but it is often accompanied by the tedious calculation process of the Jacobian matrix. Since the error is introduced by linearization, the unscented Kalman filter is still not suitable for high-order nonlinear system models. Compared with other filtering methods, although the particle filter algorithm has a higher computational cost, it has better adaptability to nonlinear non-Gaussian systems. However, due to imperfect sampling, general particle filters have disadvantages such as particle impoverishment and sample size dependence. Summary of the invention

[0004] In order to solve the technical problems in the prior art of low positioning accuracy and poor robustness of clustered drones under the denial of the global satellite navigation system, an embodiment of the present invention provides a cluster collaborative navigation method based on particle distance fitness.

[0005] The embodiment of the present invention provides a cluster collaborative navigation method based on particle distance fitness, including:

[0006] S1. 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 based on the relative ranging constraints measured by ultra-wideband;

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

[0008] S3. Based on the constructed cluster cooperative navigation filter model, the error-constrained quantum particle swarm optimization based on particle distance fitness is used to improve the particle filter algorithm 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 in the storage medium, and the at least one instruction is loaded and executed by a processor to implement any one of the above-mentioned cluster collaborative navigation methods based on particle distance fitness.

[0010] The beneficial effects brought about by the technical solution provided by the embodiment of the present invention include at least:

[0011] According to an embodiment of the present invention, in a multi-UAV cooperative formation composed of multiple UAVs, a system motion model is established according to the kinematics of uniform acceleration curves, and a system measurement model is formed by relative ranging constraints measured by ultra-wideband; a cluster cooperative navigation filter model is constructed based on the constructed system motion model and the system measurement model; based on the constructed cluster cooperative navigation filter model, an error-constrained quantum particle swarm optimization improved particle filter algorithm based on particle distance fitness is used for iterative filtering to obtain the optimization results of the state variables of each UAV; in this way, the positioning accuracy and robustness of cluster UAVs in the case of global satellite navigation system denial can be effectively improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0012] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0013] Figure 1 This is a network topology diagram of a swarm UAV collaborative navigation system provided by an embodiment of the present invention;

[0014] Figure 2 It is a flow chart of a cluster collaborative navigation method based on particle distance fitness provided by an embodiment of the present invention;

[0015] Figure 3It is a schematic diagram of the process of an improved particle filter algorithm based on error-constrained quantum particle swarm optimization and particle distance fitness provided by an embodiment of the present invention;

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

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

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

[0019] FIG. 7( a ) is a schematic diagram showing a comparison of the positioning errors in the longitude direction of one of the slave nodes in the present invention;

[0020] FIG7( b ) is a schematic diagram showing a comparison of the latitude positioning errors of one of the slave nodes in the present invention;

[0021] FIG7( c ) is a schematic diagram showing a comparison of the positioning errors in the height direction of one of the slave nodes in the present invention;

[0022] Figure 8 This is a schematic diagram of the positioning error simulation results of 10 slave machines in the present invention under different optimization methods. DETAILED DESCRIPTION

[0023] The technical solution of the present invention is described below in conjunction with the accompanying drawings.

[0024] In the embodiments of the present invention, words such as "exemplarily" and "for example" are used to indicate examples, illustrations or explanations. Any embodiment or design described as "example" in the present invention should not be interpreted as being more preferred or more advantageous than other embodiments or designs. Specifically, the use of the word "example" is intended to present the concept in a specific way. In addition, in the embodiments of the present invention, the meaning expressed by "and / or" can be both, or it can be either of the two.

[0025] In the embodiments of the present invention, "image" and "picture" can sometimes be used interchangeably. It should be noted that when the difference between them is not emphasized, the meanings they intend to express are the same. "of", "corresponding, relevant" and "corresponding" can sometimes be used interchangeably. It should be noted that when the difference between them is not emphasized, the meanings they intend to express are the same.

[0026] In the embodiments of the present invention, sometimes a subscript such as W1 may be written as a non-subscript such as W1. When the difference is not emphasized, the meanings to be expressed are consistent.

[0027] In order to make the technical problems, technical solutions and advantages to be solved by the present invention more clear, a detailed description will be given below with reference to the accompanying drawings and specific embodiments.

[0028] The embodiment of the present invention provides a cluster cooperative navigation method based on particle distance fitness, and the application scenario is the cooperative navigation of cluster UAVs under GNSS denial, wherein the cluster UAV cooperative navigation system is as follows: Figure 1 As shown in the figure, the swarm UAV collaborative navigation system includes 2 hosts and 10 slaves. Each UAV is equipped with an IMU sensor, an ultra-wideband (UWB) ranging sensor and a communication device. The host is equipped with a high-precision IMU device and GPS, and the 10 slaves are equipped with a lower-precision IMU device. The distance between the aircraft is measured by UWB sensors, and the measurement noise is considered to be Gaussian noise. In addition, the UAVs act as communication nodes in the network, and they can send and receive information with the UAV nodes connected in the topology map. Figure 2 The process flow chart of the cluster collaborative navigation method based on particle distance fitness shown in the figure may include the following steps:

[0029] S1. 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 based on the relative ranging constraints measured by ultra-wideband;

[0030] In this embodiment, the drones act as individual communication nodes in the swarm collaborative navigation system network, and drone i is referred to as node i;

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

[0032] The established system motion model is expressed as:

[0033]

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

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

[0036] Considering that the situation where all cluster drones are equipped with high-precision navigation equipment will cause the problem of excessive cost, in the cluster cooperative navigation system of this embodiment, the host is equipped with high-precision navigation equipment (referring to: high-precision IMU equipment and GPS), and all slaves are equipped with low-precision sensors (referring to: lower-precision IMU equipment). The slave uses the accurate position information obtained by the host to correct its own state through calculation, realizes the fusion of cooperative information, and suppresses error accumulation; if node i and node j can communicate at time t, then the system measurement model of the node is expressed as:

[0037]

[0038] Among them, Z t represents the measured value of the system, P and V are the absolute position and velocity information of the slave obtained by the inertial measurement unit (IMU), S, θ and They are the observed values ​​of relative distance, relative heading angle and relative pitch angle between UAVs;

[0039] The measurement equation for determining relative navigation information is:

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

[0041]

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

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

[0044] S3, based on the constructed cluster cooperative navigation filtering model, adopt the error-constrained quantum particle swarm optimization based on particle distance fitness to improve the particle filtering algorithm for iterative filtering, and obtain the optimization results of the state variables of each UAV; taking any slave as an example, step S3 is described in detail, as follows: Figure 3 As shown, the following steps may be specifically included:

[0045] S31, for any slave i, as in the traditional particle filter algorithm, at time t = 0, according to the initial state vector Generate N particles using Gaussian random distribution, that is Among them, represents the initial state of 1 to N particles, and ~ means subject to represents the prior probability. The value of the number of particles N mainly depends on the balance between the required accuracy and the 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 achieve a balance. In this embodiment, N is set to 500;

[0046] S32. When the slave device i moves, update the initial particle state using the system motion model and calculate the prior probability to obtain the predicted result of the initial state In this step of the embodiment of the present invention, there is no need to calculate the weight information required by the traditional particle filtering 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 under the condition 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 decline 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 on 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] where 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] Established with The error ellipsoid centered on is expressed as:

[0055]

[0056] Among them, λ1, λ2 and λ3 are the three eigenvalues ​​of the covariance matrix C; μ x , μ y , μ z is the estimated center position, that 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 Figure 4 As shown;

[0057] After obtaining the particle distribution derived After the error ellipsoid is centered, a group of particles are selected in the error ellipsoid Where M≤N, is the particle group, which represents the M particles sampled within the error ellipsoid.

[0058] In this embodiment, on the basis of estimating the center and scale of the system motion model, an error constraint is used to define an error ellipsoid based on the covariance matrix, and particles that meet the conditions are screened out to ensure that the particles are distributed in a high-confidence area, thereby realizing distance constraints based on geometric positions, which is beneficial to the accuracy and stability of positioning.

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

[0060] In order to Sampling obtains a set of optimally selected particles to estimate the state of the target. This embodiment transforms the resampling problem into a problem of finding the optimal solution. First, initialize a quantum particle group Q = {Q1, Q1, ..., Q np}; where np is the number of quantum particles, and each quantum particle Q i is an m-dimensional real-valued vector 1≤j≤M, M and particle swarm The number of M is the same; for each component of the quantum particle Its value represents the particle swarm The probability of the corresponding particle being selected in is ; the quantum particle swarm optimization algorithm will perform multiple iterations 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 optimal value remains unchanged after half of the maximum number of iterations;

[0061] For a randomly initialized Q k , each quantum particle By applying random observations, it becomes an M-dimensional binary particle as shown below in, It is expressed as:

[0062]

[0063] Among them, Q k represents the quantum particle after k iterations, the binary particle D i Represents particle swarm The resampling result of The value indicates whether it is selected, that is, 1 indicates selected and 0 indicates unselected;

[0064] Each binary particle D i The fitness function based on particle distance is Fit i Defined as:

[0065]

[0066] Among them, Dis j express and The distance between represents the jth particle in the quantum particle swarm optimization algorithm, and B is The variance of F i,min 、F i,max Respectively represent F i The minimum and maximum values ​​of ; the higher the fitness, the more reliable the sampling results;

[0067] After obtaining the fitness of each binary particle, the quantum particle swarm can be updated according to the following formula at the kth iteration:

[0068]

[0069] in, and They represent quantum particles Q i Personal and global historical optimal solutions at the k+1th iteration; represents the quantum particle after k+1 iterations; ε∈[0,1] represents the control parameter; is an M-dimensional unit vector; and Denote binary particles D i The personal and global optimal solutions at the kth iteration are given 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, where c1 ∈ [0, 1], c2 ∈ [0, 1], and 0 < c1 + c2 < 1; in experience, the algorithm can exhibit better performance when ε > 0.7. 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, and 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 particle degradation problems, reduces the dependence on 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 t = 0

[0073] In this embodiment, when slave i obtains 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 optimizing the filter at t = 0.

[0074] S35, According to the final state estimate obtained after the quantum particle swarm optimization resampling process The weighted consistency algorithm is used to update the acceleration information for updating the state quantity in the prediction stage of the next moment.

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

[0076] In this embodiment, for the selected slave, the acceleration information of the slave is updated using the master and slave states communicating with it, so as to be used for the state quantity update in the next moment prediction phase. The weighted consistency algorithm is expressed as:

[0077]

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

[0079] In this embodiment, if there is an edge between the slave and the master, then d ij Equal to the weight value a on this edge h_s , the weight of the edge between the slave and the master is expressed as:

[0080] a h_s =W h ×C s

[0081] Among them, W h Represents the connection weight of the host, specifically the number of connections between slave i and the host; C s Indicates the number of other slaves connected to slave i;

[0082] If there is an edge between slaves, then a ij Equal to the weight value a on this edge s_s , the weight of the edge between slaves is expressed as:

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

[0084] Among them, W h,i Indicates the number of hosts connected to slave i, W h,j Indicates the number of hosts connected to slave j, Q i , Q j They 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 consistency algorithm is used to optimize the information interaction between drones in collaborative navigation, and a new method for calculating communication weights is adopted on the cluster drone communication topology map. Further processing is performed based on filtering to ensure that the navigation data in the cluster system can be integrated more accurately and effectively.

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

[0087] Figure 7(a)-7(c) is a result diagram of the absolute value of the positioning error of one of the slave nodes in the present invention, Figure 8 The simulation result diagram of the positioning error of 10 slaves in the present invention under different optimization methods is shown. It can be seen from the figure that compared with the situation without coordination, the collaborative navigation algorithm of the cluster drones of the present invention can significantly improve the positioning level of the cluster aircraft. Moreover, the collaborative navigation algorithm proposed in the present invention has a certain improvement in positioning longitude compared with the collaborative navigation based on particle filter (PF) and the collaborative navigation method based on unscented particle filter (UPF).

[0088] In order to quantitatively analyze the positioning error, the root mean square error (RMSE) of the aircraft positioning is statistically analyzed. The statistical results are shown in Table 1. The calculation formula of the position estimation error in Table 1 is:

[0089]

[0090] Among them, E represents the root mean square error of the drone, Err lon , Err lat and Err alt Represents the errors in longitude, latitude and altitude respectively.

[0091] Table 1. Statistical results of positioning error

[0092]

[0093] As can be seen from Table 1, in the comparison of positioning errors of different methods, the errors of the method of the present invention in the three dimensions of longitude, latitude and altitude are significantly lower than those of other methods. Compared with the uncoordinated, PF and UPF methods, the method of the present invention shows obvious advantages in positioning accuracy in each dimension. On the whole, the method of the present invention also shows higher accuracy and reliability in terms of overall position error, which is far superior to other methods. Therefore, the qualitative and quantitative experimental results prove that compared with other algorithms, the method of the present invention has significant improvements and enhancements in positioning accuracy.

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

[0095] The above embodiments can be implemented in whole or in part by software, hardware (such as circuits), firmware or any other combination. When implemented by software, the above embodiments can be implemented in whole or in part in the form of 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, the process or function described in the embodiment of the present invention is generated in whole or in part. 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 computer-readable storage medium. For example, the computer instructions can be transmitted from one website, computer, server or data center to another website, computer, server or data center by wired (such as infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that can be accessed by a computer or a data storage device such as a server or data center that contains one or more available media sets. The available medium can be a magnetic medium (for example, a floppy disk, a hard disk, a tape), an optical medium (for example, a DVD), or a semiconductor medium. The semiconductor medium can be a solid-state hard disk.

[0096] It should be understood that the term "and / or" in this article is only a description of the association relationship of associated objects, indicating that there can be three relationships. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. A and B can be singular or plural. In addition, the character " / " in this article generally indicates that the associated objects before and after are in an "or" relationship, but it may also indicate an "and / or" relationship. Please refer to the context for specific understanding.

[0097] In the present 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 refers to any combination of these items, including any combination of single or plural items. For example, at least one of a, b, or c can be represented by: a, b, c, ab, ac, bc, or abc, where a, b, c can be single or multiple.

[0098] It should be understood that in various embodiments of the present invention, the size of the serial numbers of the above-mentioned processes does not mean 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 appreciate that the units and algorithm steps of each example 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 performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0100] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the above-described equipment, devices and units can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0101] In the several embodiments provided by the present invention, it should be understood that the disclosed devices, apparatuses and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another device, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

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

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

[0104] If the functions are implemented in the form of 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 can be essentially or partly embodied in the form of a software product that contributes to the prior art. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods described in various embodiments of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0105] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A cluster collaborative 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 uniformly accelerated curvilinear kinematics and construct a system measurement model from the relative ranging constraints measured by ultra-wideband; S2. Based on the constructed 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 constraints based on particle distance fitness of quantum particle swarm optimization for iterative filtering to obtain the optimized results of the state variables of each UAV.

2. The cluster collaborative navigation method based on particle distance fitness according to claim 1 is 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 represents the three-dimensional position state of node i at time t, represents the three-dimensional velocity state of node i at time t, 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 transfer matrix; T is the sampling time; is the state vector of node i at time t-1; is the system process noise of node i at time t-1.

3. The cluster collaborative navigation method based on particle distance fitness according to claim 2 is 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 to achieve the fusion of collaborative information and suppress 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 represents the measured value of the system, P and V are the absolute position and velocity information of the slave obtained by the inertial measurement unit, S, θ and They are the observed values ​​of relative distance, relative heading angle and relative pitch angle between 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 of relative navigation information, h′(X t ) is an intermediate expression, Represents the location information of node i at time t; Represents the location information of node j at time t; R t ′ is the relative measurement noise.

4. The cluster collaborative navigation method based on particle distance fitness according to claim 3 is characterized in that: The step of using an improved particle filtering algorithm with error constraints based on particle distance fitness of quantum particle swarm optimization for iterative filtering based on the constructed cluster collaborative navigation filtering model to obtain the optimized results of the state variables of each UAV includes: S31, for any slave i, at time t = 0, according to the initial state vector Generate N particles using Gaussian random distribution, that is in, represents the initial state of particles 1 to N, ~ represents obedience, represents the prior probability; S32, when slave i moves, the initial particle state is updated using the system motion model to calculate the prior probability Get the initial state prediction result S33. Resample the particles using resampling based on error constraints and quantum particle swarm optimization to obtain the optimal resampling result; S34, based on the optimal resampling result, the system measurement model and measurement equation are used to measure and update the particles to obtain the final state estimate at time t = 0 S35, the final state estimate obtained after resampling according to quantum particle swarm optimization The weighted consistency algorithm is used to update the acceleration information for updating the state quantity in the prediction stage of the next moment.

5. The cluster collaborative navigation method based on particle distance fitness according to claim 4 is characterized in that: The step of resampling the particles using resampling based on error constraints and quantum particle swarm optimization to obtain the optimal resampling result includes: Establish an error ellipsoid based on the covariance matrix using error constraints 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.

6. The cluster collaborative navigation method based on particle distance fitness according to claim 5 is characterized in that: The step of establishing an error ellipsoid based on the covariance matrix using error constraints 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 accurate for the s scale 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 an error constraint; Determine the covariance matrix C of N particles at a certain moment as: where cov(x, x), cov(y, y), cov(z, z) are the variances in the x-axis, y-axis, and z-axis directions respectively, and cov(x, y), cov(x, z), cov(y, z) represent the correlations in different directions; Established with The error ellipsoid centered on is expressed as: Among them, λ1, λ2 and λ3 are the three eigenvalues ​​of the covariance matrix C; μ x , μ y , μ z is the estimated center position, that 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, is the particle group, which represents the M particles sampled within the error ellipsoid.

7. The cluster collaborative navigation method based on particle distance fitness according to claim 6 is 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, Q1, ..., Q np }; where np is the number of quantum particles, and each quantum particle Q i is an m-dimensional real-valued vector 1≤j≤M, M and particle swarm The number of M is the same; for each component of the quantum particle Its value represents the particle swarm The probability of the corresponding particle being selected; For a randomly initialized Q k , each quantum particle By applying random observations, it becomes an M-dimensional binary particle as shown below in, It is expressed as: Among them, Q k represents the quantum particle after k iterations, the binary particle D i Represents particle swarm The resampling result of The value indicates whether it is selected, that is, 1 indicates selected and 0 indicates unselected; Each binary particle D i The fitness function based on particle distance is Fit i Defined as: Among them, Dis j express and The distance between represents the jth particle in the quantum particle swarm optimization algorithm, and B is The variance of i,min 、F i,max Respectively represent F i The minimum and maximum values ​​of After obtaining the fitness of each binary particle, the quantum particle swarm can be updated according to the following formula at the kth iteration: in, and They represent quantum particles Q i The personal and global historical optimal solutions at the k+1th iteration, 1≤i≤np; represents the quantum particle after k+1 iterations; ε∈[0,1] represents the control parameter; is an M-dimensional unit vector; and Denote binary particles D i The personal and global optimal solutions at the kth iteration are given by the binary particle D k The fitness value set is determined; the coefficients c1 and c2 represent the degree of belief in oneself and the personal optimal solution, which satisfies c1∈[0,1], c2∈[0,1], and 0 <c1+c2<1; Generate new quantum particles from the above formula replace Iterate and convert it into binary particles For update and Until the quantum particle swarm optimization algorithm meets its stopping condition, that is, the maximum number of iterations of the quantum particle swarm optimization algorithm is reached or the global optimal value remains unchanged after half of the maximum number of iterations, the final optimal solution D can be obtained. * , which illustrates an optimal resampling result.

8. The cluster collaborative navigation method based on particle distance fitness according to claim 7 is characterized in that: According to the obtained optimal resampling result, the particle is measured and updated using the system measurement model and the measurement equation to obtain the final estimate at time t=0, which includes: When slave i obtains information from the node that can interact with it, including its position information P and the relative distance, relative heading angle and relative pitch angle between the two, the system measurement model and measurement equation are used to measure and update the particles to obtain the final state estimate Represents the final value of the state quantity obtained by optimizing filtering at time t=0.

9. The cluster collaborative navigation method based on particle distance fitness according to claim 8, characterized in that: The final state estimation obtained after resampling according to quantum particle swarm optimization The weighted consistency algorithm is used to update the acceleration information. The state quantity update for the next moment prediction stage includes: For the selected slave, the slave's acceleration information is updated using the master and slave states that communicate with it, so as to be used for the state update in the next prediction phase. The weighted consistency algorithm is expressed as: in, The derivative of the slave's speed, i.e. the slave's acceleration information n represents the total number of nodes; P i F and Both represent the position information of the slave; Indicates the location information of the host; V i F and Both indicate the speed information of the slave; represents the speed information of the host; γ0 and γ1 are control parameters; j∈G ff represents the node j that communicates with slave i, G ff represents the set of drone nodes that communicate with slave i; a ij Indicates the communication status between slaves; d ij Indicates the communication status between the slave and the master.

10. The cluster collaborative navigation method based on particle distance fitness according to claim 9, characterized in that: If there is an edge between the slave and the master, then d ij Equal to the weight value a on this edge h_s , the weight of the edge between the slave and the master is expressed as: a h_s =W h ×C s Among them, W h Represents the connection weight of the host, specifically the number of connections between slave i and the host; C s Indicates the number of other slaves connected to slave i; If there is an edge between slaves, then a ij Equal to the weight value a on this edge s_s , the weight of the edge between slaves is expressed as: a s_s =(W h,i ×Q i )+(W h,j ×Q j )) Among them, W h,i Indicates the number of hosts connected to slave i, W h,j Indicates the number of hosts connected to slave j, Q i , Q j They represent the number of connections between slave i and other slaves, and the number of connections between slave j and other slaves respectively.

Images