Unmanned aerial vehicle cluster cooperative positioning method based on dynamic performance evaluation

Abstract

The invention discloses an unmanned aerial vehicle cluster cooperative positioning method based on dynamic performance evaluation, and the method comprises the steps: enabling each unmanned aerial vehicle to complete the updating of the state quantity of a system through the output information of an airborne inertial sensor, and maintaining the absolute positioning precision in a short time; thirdly, completing inter-aircraft distance measurement and cooperative message transmission among the unmanned aerial vehicles by using a data link, comprehensively considering a coupling relationship among distance measurement errors, prior position error information and spatial geometric configurations, and constructing a weighted cooperative precision factor for representing a cooperative positioning performance lower bound by using all measurement information; the contribution degree of each adjacent unmanned aerial vehicle is calculated by successively eliminating the observation matrix of each adjacent unmanned aerial vehicle, and measurement information with high contribution degree is screened out to participate in cooperative positioning calculation. According to the method, the calculation complexity of a navigation system and the communication frequency of a data link can be reduced, the real-time performance and accuracy of cooperative positioning are improved, and the method can be used for high-precision navigation and positioning of an unmanned aerial vehicle cluster under satellite denial and complex environments.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned system cluster navigation and positioning, and specifically relates to a drone cluster collaborative positioning method based on dynamic performance evaluation. Background Art

[0002] In recent years, single UAVs have been limited by their carrier platforms and have been unable to adapt to various operational mission requirements in complex environments such as cities and hills. Clusters composed of multiple UAVs can effectively exert the performance of UAV clusters through technologies such as information interaction and collaborative interoperability between carriers. Among them, high-precision absolute positioning information is one of the prerequisites for UAV clusters to perform tasks.

[0003] In the current navigation and positioning system of drone clusters, each drone is equipped with an inertial sensor. The inertial navigation has strong anti-interference ability, and the use environment is not restricted by region. It can continuously output the inertial navigation position solution results in the global environment at all times, so inertial sensors are an important component of the existing combined navigation method. However, the disadvantage of inertial navigation is that the positioning error will continue to accumulate over time, and high-precision inertial devices are expensive and large in size. Due to load limitations, small drones can only carry low-precision micro-electromechanical system (MEMS) inertial sensors. It is necessary to use external information such as the global navigation satellite system (GNSS) and inertial data fusion to complete positioning error correction and compensation. However, GNSS exogenous navigation information is susceptible to interference and easy to be deceived, so collaborative positioning through inter-machine ranging is an effective method for drone cluster navigation at present, which can maintain the overall positioning accuracy of the cluster while also reducing the hardware cost of navigation sensors.

[0004] The cooperative positioning of drone clusters is similar to the cooperative positioning of wireless sensor networks. Through information sharing and inter-machine ranging, each drone can complete distributed positioning solution by using geometric constraints. The accuracy of cooperative positioning depends on factors such as the quality and quantity of observation information. In theory, when using a filter based on minimizing the mean square error for data fusion, the optimal estimate can be obtained after obtaining measurement information of different qualities and cooperating with its own motion model. However, the increase in communication overhead between drones and the increase in the complexity of positioning solution are likely to cause real-time performance to deteriorate. Therefore, a dynamic performance evaluation system for cooperative positioning of drone clusters is established, and measurement information with a large contribution to its own positioning is selected to participate in positioning solution, thereby improving the positioning accuracy of drone clusters, which has important scientific and application value. Summary of the invention

[0005] Purpose of the invention: In order to solve the problem of high-precision navigation and positioning of existing UAV clusters in satellite denial and complex environments, the present invention proposes a UAV cluster collaborative positioning method based on dynamic performance evaluation.

[0006] Technical solution: The method for cooperative positioning of a drone cluster based on dynamic performance evaluation described in the present invention comprises the following steps:

[0007] (1) Initialize the state of each UAV’s inertial navigation system and set the initial state estimation value and covariance matrix in the Kalman filter;

[0008] (2) Use the airborne MEMS inertial sensor to output the acceleration and angular velocity of the three orthogonal axes of the drone's own coordinate system at high frequency, and complete the one-step prediction calculation of the drone's attitude, velocity, and position.

[0009] (3) Each UAV broadcasts its own information through the data link, and also receives data link coordination information from neighboring UAVs;

[0010] (4) The UAV receives the measurement information of n neighboring UAVs and determines whether the number of neighboring UAVs is greater than a preset threshold. If the condition is met, the process proceeds to step (5); if the condition is not met, the process directly proceeds to step (7);

[0011] (5) Comprehensively consider the coupling relationship between ranging error, prior position error information, and spatial geometric configuration, and use all measurement information to construct a weighted collaborative precision factor to characterize the lower bound of collaborative positioning performance;

[0012] (6) By successively eliminating the observation matrix of each adjacent UAV, the contribution of each adjacent UAV is calculated respectively, and the measurement information with high contribution is selected to participate in the collaborative positioning solution;

[0013] (7) Use the Kalman filter to complete the system measurement update, correct the inertial navigation positioning error, output the collaborative positioning result in the strapdown inertial navigation solution platform, and encode the relevant information and transmit it to the data communication equipment, return to step (2) to start the next cycle until the UAV completes the positioning solution.

[0014] Furthermore, the state quantity of the UAV inertial navigation system in step (1) is:

[0015]

[0016] in, is the platform error angle of the inertial navigation system in the east, north and celestial directions, δv = [δv E δv N δv U ] is the speed error of the UAV in the east, north and sky directions, δp=[δL δλ δh] is the latitude, longitude and altitude error of the UAV, ε b =[ε bx ε by ε bz] is the gyro constant drift error, ε r =[ε rx ε ry ε rz ] is the first-order Markov drift error of the gyro, is the first-order Markov state quantity of the accelerometer;

[0017] Set the initial value X of the state quantity according to the position, speed, attitude, and inertial sensor characteristics of the drone at the start time (0) and system noise W (0) , initial value of covariance matrix P (0) It needs to be set according to the initial uncertainty of the state quantity to ensure the stability and convergence speed of the filter.

[0018] Furthermore, the three state quantities in step (2) are predicted in one step by constructing a discrete state transfer equation based on the inertia error differential equation for solution. The mathematical expression is:

[0019] X (k) =Φ (k|k-1) X (k-1) +G (k-1) W (k-1)

[0020] Among them, Φ (k|k-1) is the error state transfer matrix, G (k-1) is the system noise matrix, W (k-1) is the system noise.

[0021] Furthermore, the implementation process of step (3) is as follows:

[0022] The output position of the drone is p = [Lλh], and the velocity is v = [v E v N v U ], attitude a=[φθγ]; at the same time, each UAV encodes its own number, timestamp, three-dimensional position, and prior position error information and transmits them to the data communication equipment for broadcast.

[0023] Furthermore, the implementation process of step (5) is as follows:

[0024] (51) Constructing a collaborative positioning error propagation model:

[0025] The position of the UAV in the Earth-centered Earth-fixed coordinate system is x = (x, y, z). Assume that there are n adjacent UAVs that can establish data communication with it, and their positions are x ri =(x ri ,y ri ,z ri ), the actual distance between the adjacent UAV and the UAV is di =||xx ri ||, considering the prior position error δx of adjacent drones ri and its own position error δx, measuring distance d i ′ is expressed as:

[0026] d i ′=||x+δx-(x ri +δx ri )||+ε i

[0027] Where δx is the position correction vector [δx,δy,δz] T ,δx ri is the prior position error of the neighboring UAV i, ε i is the ranging error;

[0028] (52) Geometric representation of ranging information:

[0029] After linearizing the above equation and ignoring the higher-order terms, we get:

[0030]

[0031] The i-th row element of the ranging residual vector v is v i =d i ′-d i , substituting the linear approximation into , we can get:

[0032]

[0033] in,

[0034] The ranging residuals of all adjacent drones are expressed in vector form:

[0035] v=Gδx+Jδx ri +ε

[0036] Among them, G is a geometric matrix with a dimension of n×3; J is the adjacent UAV position error influence matrix with a dimension of n×3n, and ε is the ranging error vector;

[0037] (53) Objective function solution:

[0038] By introducing the weight matrix W, we can get the regression estimation under different variance observation data. The weight matrix W is as follows:

[0039]

[0040] in, is the variance of the total error of adjacent UAVs; construct the objective function:

[0041] min δx (v-Gδx) T W(v-Gδx)

[0042] Usually G is full of primes, which can ensure the uniqueness and stability of the solution. Taking the derivative of the objective function with respect to δx and setting the derivative to zero, we get:

[0043] δx=(G T WG) -1 G T W

[0044] (54) Definition of systematic error term: The covariance matrix of δx directly reflects the distribution of the estimation error and is defined as:

[0045] Cov(δx)=E[(δx-E[δx])(δx-E[δx]) T ]

[0046] Where E[·] is the mean or expected value, Cov(·) is the covariance;

[0047] Ranging error ε i Satisfies normal distribution, its mean is zero, the variance is positively correlated with the distance, and each ranging error is independent of each other; the prior position error δx of each adjacent drone ri Independent of each other;

[0048] From the collaborative positioning error propagation model, we can know that the covariance matrix of the ranging residual vector v is:

[0049] Cov(v)=JCov(δx r )J T +Cov(ε)

[0050] Among them, Cov(δx r ) is Cov(δx) which reflects the position error distribution of each adjacent UAV. ri ),Cov(δx ri )for Cov(ε) is the covariance of the ranging error. From the ranging error model, we know that its variance is positively correlated with the distance. get Where k is the proportionality factor;

[0051] (55) The lower bound of collaborative positioning performance WCDOP is solved as follows:

[0052] Substituting Cov(v) into Cov(δx), expanding and solving it, we can get:

[0053] Cov(δx)=(G T WG)-1 [G T WJCov(δx r )J T WG

[0054] +σ 2 G T WRWG](G T WG) -1

[0055] The position covariance Cov(δx) includes the contribution of all influencing factors, where the diagonal elements directly quantify the positioning accuracy of the unknown UAV in the three-axis direction, and the WCDOP is defined as:

[0056] WCDOP=trace(Cov(δx))

[0057] WCDOP describes the uncertainty of the positioning solution and provides a basis for optimizing the collaborative positioning performance.

[0058] Furthermore, the contribution calculation method of each adjacent drone in step (6) is as follows:

[0059] First, the position information of all adjacent drones is converted to the ECEF coordinate system using the rotation matrix to determine whether the adjacent drones are collinear with the unknown drone or the angle between them is less than the set threshold. If the above conditions are met, the adjacent drones with a large sum of prior position error and ranging error need to be eliminated; then the observation vector g of each drone is removed in turn i , calculate the contribution of all UAVs to the collaborative positioning solution, and the change value of WCDOP is defined as:

[0060]

[0061] According to the contribution of each adjacent UAV, a dynamic performance evaluation system is established to screen the measurement information with high contribution to participate in collaborative positioning solution.

[0062] Furthermore, the system measurement update method in step (7) is as follows:

[0063] Using the transformation matrix Establish a unified dimension for position errors in two coordinate systems:

[0064]

[0065] Among them, R N is the radius of curvature of the y-axis, and f is the flattening of the earth;

[0066] When the measurement information of the four adjacent drones after screening is used in the positioning solution, the measurement equation of the collaborative positioning system is as follows:

[0067]

[0068] Among them, V (k) Resolve measurement noise of UAVs involved in collaborative localization.

[0069] Furthermore, the preset threshold in step (4) is 4.

[0070] Furthermore, in step (6), the number of the high-contribution measurement information is greater than or equal to a preset threshold.

[0071] Beneficial effects: Compared with the prior art, the beneficial effects of the present invention are as follows: Based on the collaborative positioning using inter-machine ranging, the present invention comprehensively analyzes the influence of three influencing factors, namely ranging error, prior position error information and spatial geometric configuration, on the collaborative positioning performance; according to the principle of solving the unknown UAV position by a closed-form analytical algorithm, a WCDOP for characterizing the lower bound of the collaborative positioning performance is constructed, and the coupling relationship between various influencing factors is considered; the contribution of each adjacent UAV is calculated and a dynamic performance evaluation system is established based on this, and the measurement information with high contribution is screened to participate in the collaborative positioning solution, and finally the absolute position information of the UAV is output; the present invention can reduce the computational complexity of the navigation system and the data link communication frequency, improve the real-time and accuracy of collaborative positioning, and can be used for high-precision navigation and positioning of UAV clusters in satellite denial and complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 It is an architecture diagram of the UAV cluster collaborative positioning system based on dynamic performance evaluation constructed by the present invention;

[0073] Figure 2 It is a flow chart of the present invention;

[0074] Figure 3 It is the initial position distribution map of the drone cluster constructed by the present invention;

[0075] Figure 4 The graphs are surface diagrams showing the changes in the cooperative positioning performance of UAVs under different influencing factors according to the method of the present invention, wherein (a) is a surface diagram showing the changes in positioning performance after ignoring the influencing factors of the position errors of adjacent UAVs, and (b) is a surface diagram showing the changes in positioning performance after considering the influencing factors of the position errors of adjacent UAVs; (c) is a surface diagram showing the changes in positioning performance when UAV No. 2 has a smaller prior position error; (d) is a surface diagram showing the changes in positioning performance when UAV No. 2 has a larger prior position error;

[0076] Figure 5 It is a flight trajectory diagram of the drone cluster constructed by the present invention;

[0077] Figure 6It is a comparison diagram of the three-axis positioning errors of the method of the present invention, the optimal configuration screening method, and the shortest distance screening method;

[0078] Figure 7 It is a comparison diagram of the error cumulative distribution of the method of the present invention, the optimal configuration screening method, and the shortest distance screening method. DETAILED DESCRIPTION

[0079] The present invention is further described in detail below with reference to the accompanying drawings.

[0080] like Figure 1 , Figure 2 As shown, the present invention provides a method for cooperative positioning of UAV clusters based on dynamic performance evaluation. Each UAV uses the output information of the airborne inertial sensor to complete the system state update and maintain absolute positioning accuracy in a short time. Then, the UAVs use the data link to complete the inter-machine distance measurement and message transmission, and establish a dynamic performance evaluation system based on the cooperative messages received from adjacent UAVs. The contribution of each adjacent UAV is calculated and the measurement information with high contribution is selected to participate in the cooperative positioning solution, which reduces the system calculation complexity and communication frequency while ensuring the overall positioning accuracy of the cluster. It solves the problem of high-precision navigation and positioning of existing UAV clusters in satellite denial and complex environments. The specific implementation methods are as follows:

[0081] Step 1: Initialize the state of each UAV's inertial navigation system and set the initial state estimation value and covariance matrix in the Kalman filter.

[0082] The state quantity of the UAV inertial navigation system is:

[0083]

[0084] in, is the platform error angle of the inertial navigation system in the east, north and celestial directions, δv = [δv E δv N δv U ] is the speed error of the UAV in the east, north and sky directions, δp=[δL δλ δh] is the latitude, longitude and altitude error of the UAV, ε b =[ε bx ε by ε bz ] is the gyro constant drift error, ε r =[ε rx ε ry ε rz ] is the first-order Markov drift error of the gyro, is the first-order Markov state of the accelerometer.

[0085] Set the initial value X of the state quantity according to the position, speed, attitude, and inertial sensor characteristics of the drone at the start time (0) and system noise W (0) , initial value of covariance matrix P (0) It needs to be set according to the initial uncertainty of the state quantity to ensure the stability and convergence speed of the filter.

[0086] Step 2: Use the airborne MEMS inertial sensor to output the acceleration and angular velocity in the three orthogonal axes of the drone's own coordinate system at high frequency, and complete the one-step prediction calculation of the three state quantities of the drone's attitude, speed, and position.

[0087] The one-step prediction of the system state quantity can be solved by constructing a discrete state transfer equation based on the inertia error differential equation. The mathematical expression is:

[0088] X (k) =Φ (k|k-1) X (k-1) +G (k-1) W (k-1)

[0089] Among them, Φ (k|k-1) is the error state transfer matrix, G (k-1) is the system noise matrix, W (k-1) is the system noise.

[0090] Step 3: Each UAV broadcasts its own number, three-dimensional position, and prior position error information through the data link, and also receives data link coordination information from adjacent UAVs.

[0091] The drone can output position p = [Lλh], velocity v = [v E v N V U ], attitude a=[φθγ], these three types of navigation and positioning information are output. At the same time, each UAV encodes its own number, timestamp, three-dimensional position, and prior position error information and transmits them to the data communication equipment for broadcast.

[0092] Step 4: The UAV will receive the measurement information of n neighboring UAVs and determine whether the number of neighboring UAVs is greater than 4. When this condition is met, all measurement information is used to construct a weighted collaborative precision factor (WCDOP) to characterize the lower bound of collaborative positioning performance; when this condition is not met, the Kalman filter is directly used to complete the system measurement update.

[0093] Step 5: Construct a collaborative positioning dynamic performance evaluation system: Comprehensively consider the coupling relationship between ranging error, prior position error information, and spatial geometric configuration, and use all measurement information to construct WCDOP to characterize the lower bound of collaborative positioning performance.

[0094] (5.1) System model construction:

[0095] The position of the UAV in the Earth-centered Earth-fixed coordinate system (ECEF) is x = (x, y, z). Assume that there are n adjacent UAVs that can establish data communication with it, and their positions are x ri =(x ri ,y ri , z ri ), the actual distance between the adjacent UAV and the UAV is d i =||xx ri ||, considering the prior position error δx of adjacent drones ri and its own position error δx, measuring distance d′ i It is expressed as:

[0096] d′ i =||x+δx-(x ri +δx ri )||+ε i

[0097] Where δx is the position correction vector [δx, δy, δz] T ,δx ri is the prior position error of the neighboring UAV i, ε i is the ranging error.

[0098] (5.2) Geometric representation of ranging information:

[0099] After linearizing the above equation and ignoring the higher-order terms, we get:

[0100]

[0101] The i-th row element of the ranging residual vector v is v i =d i ′-d i , substituting the linear approximation into , we can get:

[0102]

[0103] in,

[0104] The ranging residuals of all adjacent drones are expressed in vector form:

[0105] v=Gδx+Jδx ri +ε

[0106] Among them, G is a geometric matrix with a dimension of n×3; J is the adjacent UAV position error influence matrix with a dimension of n×3n, and ε is the ranging error vector.

[0107] (5.3) Objective function solution:

[0108] By introducing the weight matrix W, we can get the regression estimation under different variance observation data. The weight matrix W is as follows:

[0109]

[0110] in, is the variance of the total error of neighboring UAVs.

[0111] Construct the objective function:

[0112] min δx (v-Gδx) T W(v-Gδx)

[0113] Usually G is full of primes, which can ensure the uniqueness and stability of the solution. Taking the derivative of the objective function with respect to δx and setting the derivative to zero, we get:

[0114] δx=(G T WG) -1 G T W

[0115] (5.4) Definition of systematic error term:

[0116] The covariance matrix of δx directly reflects the distribution of the estimation error and is defined as:

[0117] Cov(δx)=E[(δx-E[δx])(δx-E[δx]) T ]

[0118] Where E[·] is the mean or expected value and Cov(·) is the covariance.

[0119] Ranging error ε i It satisfies the normal distribution, its mean is zero, the variance is positively correlated with the distance, and each ranging error is independent of each other; the prior position error δx of each adjacent drone ri Independent of each other.

[0120] From the system error propagation model, we can know that the covariance matrix of the ranging residual vector v is:

[0121] Cov(v)=JCov(δx r )J T +Cov(ε)

[0122] Among them, Cov(δx r ) is Cov(δx) which reflects the position error distribution of each adjacent UAV. ri ),Cov(δx ri )for Cov(ε) is the covariance of the ranging error. From the ranging error model, we know that its variance is positively correlated with the distance. get

[0123] (5.5) The lower bound of collaborative positioning performance WCDOP is solved as:

[0124] Substituting Cov(v) into Cov(δx), expanding and solving it, we can get:

[0125] Cov(δx)=(G T WG) -1 [G T WJCov(δx r )J T WG

[0126] +σ 2 G T WRWG](G T WG) -1

[0127] The position covariance Cov(δx) includes the contribution of all influencing factors, where the diagonal elements directly quantify the positioning accuracy of the unknown UAV in the three-axis direction, and the WCDOP is defined as:

[0128] WCDOP=trace(Cov(δx))

[0129] WCDOP describes the uncertainty of the positioning solution and can provide a basis for optimizing the collaborative positioning performance.

[0130] Step 6: Calculate the contribution of each adjacent UAV by eliminating the observation matrix of each adjacent UAV one by one, calculate the contribution of each adjacent UAV respectively, and select the measurement information of four high contribution metrics to participate in the collaborative positioning solution.

[0131] The position information of all adjacent drones is converted to the ECEF coordinate system using the rotation matrix to determine whether the adjacent drones are collinear with the unknown drone or the angle between them is less than the set threshold. If the above conditions are met, the adjacent drones with a large sum of prior position error and ranging error need to be eliminated. Then remove the observation vector g of each drone in turn. i , calculate the contribution of all UAVs to the collaborative positioning solution, and the change value of WCDOP is defined as:

[0132]

[0133] According to the contribution of each adjacent UAV, a dynamic performance evaluation system is established to screen the measurement information with high contribution to participate in collaborative positioning solution.

[0134] When the measurement information of the four adjacent drones with high contribution is used in the positioning solution, the measurement equation of the collaborative positioning system is as follows:

[0135]

[0136] Among them, V (k) Resolve measurement noise of UAVs involved in collaborative localization.

[0137] Since the system measurement model is based on the Earth-centered Earth-fixed coordinate system, the state quantity in the inertial navigation system uses the Northeast Sky Geographic System, and the conversion matrix Establishing a unified dimension for position errors in two coordinate systems

[0138]

[0139] Among them, R N is the radius of curvature of the ecliptic circle, and f is the flattening of the earth.

[0140] Figure 3 It is an initial position distribution map of the drone cluster constructed by the present invention, wherein the cross symbol represents an unknown drone and the circle symbol represents an adjacent drone. Figure 4 This is a surface diagram showing the variation of the UAV collaborative positioning performance under different influencing factors according to the method of the present invention. Figure 3 Among them, No. 1 and No. 3 are reference drones with high-precision position information, and No. 2, No. 4 and No. 5 have large position errors. The simulation results of the positioning performance changes of unknown drones on a plane with a height of 25m are given respectively. Figure 4 (a) is a surface diagram showing the change in positioning performance after ignoring the influence of the position error of adjacent UAVs. Figure 4 (b) is a surface diagram of the positioning performance change after considering the influence of the position error of adjacent drones. Since drone No. 2 has a better elevation angle than other drones, considering only the spatial geometric configuration, this drone has a greater contribution to the improvement of collaborative positioning performance. Figure 4 (c) is a surface diagram showing the positioning performance change of UAV 2 with a small prior position error. Figure 4 (d) is a surface diagram showing the change in positioning performance of UAV 2 when it has a large prior position error. Figure 4 The results show that the dynamic performance evaluation WCDOP proposed in this invention reasonably characterizes the coupling relationship between the position error and the three influencing factors, and can provide a basis for the screening of high-contribution measurement information.

[0141] Figure 5 It is a flight trajectory diagram of a drone cluster constructed by the present invention, in which six drones maneuver according to a preset trajectory. Figure 6This is a comparison chart of the three-axis positioning errors of the method of the present invention, the optimal configuration screening method, and the closest distance screening method; among them, the curve with an asterisk marker is the screening method based on dynamic performance evaluation in the present invention, the curve with a square marker is the optimal configuration screening method, and the curve with a circular marker is the closest distance screening method. The method proposed by the present invention improves the accuracy of UAV collaborative positioning by more than 15%, reduces the dependence of UAV cluster collaborative navigation and positioning on large bandwidth and high frequency of data link communication, and reduces the computational complexity while ensuring positioning accuracy.

[0142] Figure 7 This is a comparison chart of the error cumulative distribution of the method of the present invention, the optimal configuration screening method, and the closest distance screening method; wherein, the curve with an asterisk marker is the cumulative distribution curve of the positioning error based on the dynamic performance evaluation in the present invention, the curve with a five-pointed star marker is the cumulative distribution curve of the positioning error of the optimal configuration screening method, and the curve with a diamond marker is the cumulative distribution curve of the positioning error of the closest distance screening method. The method proposed by the present invention improves the overall positioning accuracy of the drone cluster, and can be used for high-precision navigation and positioning of drone clusters in satellite denial and complex environments.

[0143] The above-mentioned embodiments only express the implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the attached claims.

Claims

1. A UAV cluster collaborative positioning method based on dynamic performance evaluation, characterized in that: The following steps are involved: (1) Initialize the state of each UAV’s inertial navigation system and set the initial state estimation value and covariance matrix in the Kalman filter; (2) Use the airborne MEMS inertial sensor to output the acceleration and angular velocity of the three orthogonal axes of the drone's own coordinate system at high frequency, and complete the one-step prediction calculation of the drone's attitude, velocity, and position. (3) Each UAV broadcasts its own information through the data link, and also receives data link coordination information from neighboring UAVs; (4) The UAV receives the measurement information of n neighboring UAVs and determines whether the number of neighboring UAVs is greater than a preset threshold. If the condition is met, the process proceeds to step (5); if the condition is not met, the process directly proceeds to step (7); (5) Comprehensively consider the coupling relationship between ranging error, prior position error information, and spatial geometric configuration, and use all measurement information to construct a weighted collaborative precision factor to characterize the lower bound of collaborative positioning performance; (6) By successively eliminating the observation matrix of each adjacent UAV, the contribution of each adjacent UAV is calculated respectively, and the measurement information with high contribution is selected to participate in the collaborative positioning solution; (7) Use the Kalman filter to complete the system measurement update, correct the inertial navigation positioning error, output the collaborative positioning result in the strapdown inertial navigation solution platform, and encode the relevant information and transmit it to the data communication equipment, return to step (2) to start the next cycle until the UAV completes the positioning solution.

2. The method for cooperative positioning of drone clusters based on dynamic performance evaluation according to claim 1 is characterized in that: The state quantity of the UAV inertial navigation system in step (1) is: in, is the platform error angle of the inertial navigation system in the east, north and celestial directions, δv = [δv E δv N δv U ] is the speed error of the UAV in the east, north and sky directions, δp=[δL δλ δh] is the latitude, longitude and altitude error of the UAV, ε b =[ε bx ε by ε bz ] is the gyro constant drift error, ε r =[ε rx ε ry ε rz ] is the gyro first-order Markov drift error, is the first-order Markov state quantity of the accelerometer; Set the initial value X of the state quantity according to the position, speed, attitude, and inertial sensor characteristics of the drone at the start time (0) and system noise W (0) , initial value of covariance matrix P (0) It needs to be set according to the initial uncertainty of the state quantity to ensure the stability and convergence speed of the filter.

3. The method for cooperative positioning of drone clusters based on dynamic performance evaluation according to claim 1 is characterized in that: The three state quantities in step (2) are predicted in one step. According to the inertia error differential equation, a discrete state transfer equation is constructed and solved. The mathematical expression is: X (k) =Φ (k|k-1) X (k-1) +G (k-1) W (k-1) Among them, Φ (k|k-1) is the error state transfer matrix, G (k-1) is the system noise matrix, W (k-1) is the system noise.

4. The method for cooperative positioning of drone clusters based on dynamic performance evaluation according to claim 1 is characterized in that: The implementation process of step (3) is as follows: The output position of the drone is p = [Lλh], and the velocity is v = [v E v N v U ], attitude a=[φθγ]; at the same time, each UAV encodes its own number, timestamp, three-dimensional position, and prior position error information and transmits them to the data communication equipment for broadcast.

5. The method for cooperative positioning of drone clusters based on dynamic performance evaluation according to claim 1 is characterized in that: The implementation process of step (5) is as follows: (51) Constructing a collaborative positioning error propagation model: The position of the UAV in the Earth-centered Earth-fixed coordinate system is x = (x, y, z). Assume that there are n adjacent UAVs that can establish data communication with it, and their positions are x ri =(x ri ,y ri ,z ri ), the actual distance between the adjacent UAV and the UAV is d i =||xx ri ||, considering the prior position error δx of adjacent drones ri and its own position error δx, measuring distance d′ i It is expressed as: d′ i =||x+δx-(x ri +δx ri )||+ε i Where δx is the position correction vector [δx,δy,δz] T ,δx ri is the prior position error of the neighboring UAV i, ε i is the ranging error; (52) Geometric representation of ranging information: After linearizing the above equation and ignoring the higher-order terms, we get: The i-th row element of the ranging residual vector v is v i =d′ i -d i , substituting the linear approximation into , we can get: in, The ranging residuals of all adjacent drones are expressed in vector form: v=Gδx+Jδx ri +e Among them, G is a geometric matrix with a dimension of n×3; J is the adjacent UAV position error influence matrix with a dimension of n×3n, and ε is the ranging error vector; (53) Objective function solution: By introducing the weight matrix W, we can get the regression estimation under different variance observation data. The weight matrix W is as follows: in, is the variance of the total error of adjacent UAVs; construct the objective function: min δx (v-Gδx) T W(v-Gδx) Usually G is full of primes, which can ensure the uniqueness and stability of the solution. Taking the derivative of the objective function with respect to δx and setting the derivative to zero, we get: δx=(G T WG) -1 G T Wv (54) Definition of systematic error term: The covariance matrix of δx directly reflects the distribution of the estimation error and is defined as: Cov(δx)=E[(δx-E[δx])(δx-E[δx]) T ] Where E[·] is the mean or expected value, Cov(·) is the covariance; Ranging error ε i Satisfies normal distribution, its mean is zero, the variance is positively correlated with the distance, and each ranging error is independent of each other; the prior position error δx of each adjacent drone ri Independent of each other; From the collaborative positioning error propagation model, we can know that the covariance matrix of the ranging residual vector v is: Cov(v)=JCov(δx r )J T +Cov(ε) Among them, Cov(δx r ) is Cov(δx) which reflects the position error distribution of each adjacent UAV. ri ),Cov(δx ri )for Cov(ε) is the covariance of the ranging error. From the ranging error model, we know that its variance is positively correlated with the distance. get Where k is the proportionality factor; (55) The lower bound of collaborative positioning performance WCDOP is solved as follows: Substituting Cov(v) into Cov(δx), expanding and solving it, we can get: Cov(δx)=(G T WG) -1 [G T WJCov(δx r )J T WG+σ 2 G T WRWG](G T WG) -1 The position covariance Cov(δx) includes the contribution of all influencing factors, where the diagonal elements directly quantify the positioning accuracy of the unknown UAV in the three-axis direction, and the WCDOP is defined as: WCDOP=trace(Cov(δx)) WCDOP describes the uncertainty of the positioning solution and provides a basis for optimizing the collaborative positioning performance.

6. The method for cooperative positioning of drone clusters based on dynamic performance evaluation according to claim 1 is characterized in that: The contribution calculation method of each adjacent drone in step (6) is as follows: First, the position information of all adjacent drones is converted to the ECEF coordinate system using the rotation matrix to determine whether the adjacent drones are collinear with the unknown drone or the angle between them is less than the set threshold. If the above conditions are met, the adjacent drones with a large sum of prior position error and ranging error need to be eliminated; then the observation vector g of each drone is removed in turn i , calculate the contribution of all UAVs to the collaborative positioning solution, and the change value of WCDOP is defined as: According to the contribution of each adjacent UAV, a dynamic performance evaluation system is established to screen the measurement information with high contribution to participate in collaborative positioning solution.

7. The method for cooperative positioning of drone clusters based on dynamic performance evaluation according to claim 1 is characterized in that: The system measurement update method in step (7) is as follows: Using the transformation matrix Establish a unified dimension for position errors in two coordinate systems: Among them, R N is the radius of curvature of the y-axis, and f is the flattening of the earth; When the measurement information of the four adjacent drones after screening is used in the positioning solution, the measurement equation of the collaborative positioning system is as follows: Among them, V (k) Resolve measurement noise of UAVs involved in collaborative localization.

8. The method for cooperative positioning of drone clusters based on dynamic performance evaluation according to claim 1, characterized in that: The preset threshold in step (4) is 4.

9. The method for cooperative positioning of drone clusters based on dynamic performance evaluation according to claim 1, characterized in that: In step (6), the number of high-contribution measurement information is greater than or equal to a preset threshold.

Images