Distributed factor graph model solution method for cooperative positioning of UAV swarms

Abstract

This invention discloses a method for solving a distributed factor graph model for the collaborative positioning of unmanned aerial vehicle (UAV) swarms. The method comprises the following steps: S1: equipping a UAV swarm with sensors, simulating a UAV swarm information interaction graph, and establishing a local factor graph model for the UAVs; S2: solving the factor graph model using a global algorithm combined with a local algorithm; wherein: the global algorithm uses an adaptive genetic algorithm, configuring the genetic algorithm's crossover and mutation behaviors into an adaptive mode; the local algorithm uses an improved Gauss-Newton algorithm, using the information obtained by the adaptive genetic algorithm as the initial value. The improved Gauss-Newton algorithm searches for the optimal value in the local area, iteratively calculates the update vector, and obtains the final optimal position information. The proposed AGA-GNCG algorithm can improve UAV positioning accuracy, and the improved Gauss-Newton algorithm can significantly improve computational efficiency.

Description

Technical Field

[0001] The present invention relates to the field of navigation and positioning technology, and in particular to a method for solving a distributed factor graph model for collaborative positioning of unmanned aerial vehicle clusters. Background Art

[0002] Today's global military landscape is rapidly evolving towards intelligent capabilities, with increasingly complex combat environments and increasing operational difficulty. Drones, with their exceptional autonomy and flexibility, are playing an irreplaceable role, driving significant changes in modern warfare. Compared to individual drones, drone swarms, through information exchange and collaboration, can more flexibly accomplish complex and diverse missions. On the modern battlefield, real-time, high-precision positioning information is essential for the efficient and effective execution of coordinated drone swarm operations.

[0003] Existing positioning methods include stand-alone positioning and cluster collaborative positioning. For stand-alone positioning, the positioning system is usually composed of a combination of multi-source heterogeneous sensors, and combines multi-source information fusion technology to achieve high-precision positioning. Among them, fusion methods based on Kalman filtering and its variants are relatively common. However, when the information of a certain sensor fails, system reconstruction will be very complicated. Fusion methods based on graph optimization flexibly expand the sensors in the system through the concepts of factors and nodes, and can achieve plug-and-play of sensors. Compared with stand-alone positioning, cluster collaborative positioning technology has higher positioning accuracy and robustness. Each combat or detection unit is regarded as an independent network node. The nodes improve positioning accuracy by mutual observation and sharing of status information.

[0004] Currently, the Gauss-Newton algorithm is primarily used to solve factor graph models. Through QR decomposition, the optimal solution is iterated to obtain the final positioning information. However, this method requires high initial value accuracy, otherwise it is easy to fall into a local optimal solution. Therefore, it is urgent to develop a new factor graph model solution method to improve positioning accuracy. Summary of the Invention

[0005] Based on the technical problems existing in the background technology, the present invention proposes a distributed factor graph model solution method for the collaborative positioning of drone clusters, which realizes the accurate and reliable positioning of drone clusters.

[0006] The distributed factor graph model solution method for UAV cluster collaborative positioning proposed in this invention has the following steps:

[0007] S1: Equip the drone cluster with sensors, simulate the drone cluster information interaction graph, and establish a drone local factor graph model;

[0008] S2: Solve the factor graph model using a global algorithm combined with a local algorithm;

[0009] Among them: the global algorithm adopts an adaptive genetic algorithm, and the crossover and mutation behaviors of the genetic algorithm are configured into an adaptive mode;

[0010] The local algorithm adopts the improved Gauss-Newton algorithm, takes the information obtained by the adaptive genetic algorithm as the initial value, searches for the optimal value in the local area, and calculates the update vector in an iterative manner to obtain the final optimal position information.

[0011] Preferably, the drone local factor graph model established in S1 with drones as variable nodes and sensors as factor nodes realizes data fusion based on the posterior probability estimation theory, and converts it into solving a nonlinear least squares problem by taking the negative logarithm of the posterior probability estimation:

[0012]

[0013] Where: They represent the northeast celestial coordinate of UAV i at time t; m is the position information or distance information measured by the sensor; h(·) is the theoretical measurement function of the sensor; is the actual measurement value of the sensor associated with the factor node; Σ m is the covariance matrix corresponding to different measurement models.

[0014] Preferably, the sensors in S1 include an ultra-wideband ranging sensor and a Beidou satellite navigation sensor;

[0015] The BeiDou satellite navigation sensor factor provides the location information of UAV i, and its expression is:

[0016]

[0017] Where: h BDS is the measurement function of the Beidou satellite navigation sensor, is the theoretical position information of UAV i, is the actual position information of UAV i measured by BeiDou satellite navigation sensor, Σ BDS is the covariance matrix of the BeiDou satellite navigation sensor.

[0018] The ultra-wideband ranging sensor factor provides the distance information between UAV i and UAV j, and its expression is:

[0019]

[0020] Where: is the measurement function of the ultra-wideband ranging sensor, is the theoretical position information of UAV i and UAV j, is the distance information between UAV i and UAV j actually measured by the ultra-wideband ranging sensor, Σ UWB is the covariance matrix of the ultra-wideband ranging sensor.

[0021] According to the Beidou satellite navigation sensor factor expression and the ultra-wideband ranging sensor factor expression, the total cost function of the following formula is obtained. At this time, the solution of X i The maximum a posteriori probability estimation is converted into a nonlinear least squares problem:

[0022]

[0023] Preferably, the steps of the adaptive genetic algorithm solution in S2 are as follows:

[0024] S211: Set the initial population number, population size, crossover rate, mutation rate and maximum number of iterations;

[0025] S212: Using the total cost function of S1 as the fitness function, when entering the Nth iteration, calculate the fitness values ​​of all individuals in the Nth generation population and output the optimal individual of the Nth generation;

[0026] S213: Determine whether the iteration stop condition is met. If so, stop the iteration and output the optimal position information corresponding to the optimal solution of the objective function; if not, proceed to S214;

[0027] S214: According to the elite selection strategy, the elite individuals in the Nth generation are retained; at the same time, random errors are introduced into the optimal individuals to form multiple new individuals, which together constitute the new population of the N+1th generation;

[0028] S215: Binary encode the new population and perform crossover, mutation, and decoding operations to complete the population update;

[0029] S216: Update the new population after genetic evolution and repeat S212-S216.

[0030] Preferably, in S215, a crossover probability and a mutation probability adaptive adjustment formula are designed during the crossover and mutation process, wherein:

[0031] The crossover probability adaptive adjustment formula is:

[0032]

[0033] Where: J min is the minimum fitness value in the group; J avg is the average fitness value of each generation group, J' is the smaller fitness value of the two individuals to be crossed; p c1 is the crossover probability of the initial setting;

[0034] The adaptive adjustment formula of mutation probability is:

[0035]

[0036] Where: J is the fitness value of the individual to be mutated; p m1 is the initial mutation probability.

[0037] Preferably, the steps of the improved Gauss-Newton algorithm solution are as follows:

[0038] S221: The theoretical measurement function h of the sensor m (·) performs a first-order Taylor expansion:

[0039]

[0040] Where: H m is the sensor's measurement function h m (·) At linear points The Jacobian matrix at Δ m is the state increment, that is, the incremental step size of the iteration;

[0041] S222: Substitute the Taylor expansion of S221 into the UAV local factor graph model to obtain the solution of the linear least squares problem Δ * for:

[0042]

[0043] S223: Exploitation Markov norm Converted into 2-norm, the standard least squares problem is:

[0044]

[0045] Where, is the Jacobian matrix of the sensor measurement function and The product of For The prediction error at The matrix A is the product of all A m A large matrix is ​​obtained by merging, and the vector b is all b m The high-dimensional vector obtained by merging together.

[0046] For Δ * The solution is to minimize By taking the derivative of Δ, we can get (A T A)Δ=A T b, and then use the conjugate gradient algorithm to iteratively solve Δ. The formula for iterative solution is as follows:

[0047] Δk+1 =Δ k +α k d k

[0048]

[0049] η k =η k-1 -α k-1 A T Ad k-1

[0050]

[0051] Where: k represents the number of iterations; α k is the step size of the kth iteration; d k is the search vector for the kth iteration; η k represents the error vector of the kth iteration; β k is represented as the kth conjugate parameter.

[0052] Preferably, the iterative solution method steps in S223 are as follows:

[0053] S2231: When the number of iterations k = 0, input matrix A T A, vector A T b, and the initialized random vector Δ0, set the maximum number of iterations, use the negative gradient direction of f(Δ) as the initial error vector η0, and let the initial search vector d0 equal the initial error vector η0;

[0054] S2232: Calculate α0, Δ1, and η1, and determine whether η1 is less than the set threshold ε. If so, output the optimal solution Δ1 = Δ0 + α0d0. Otherwise, proceed to S2233.

[0055] S2233: When the number of iterations k>0, calculate α k , Δ k+1 and η k+1 , judge the iteration stop condition, that is, whether the maximum number of iterations is reached, or judge η k+1 Is it less than the threshold ε? If it is satisfied, then output the optimal solution Δ * =Δ k+1 , otherwise go to S2234;

[0056] S2234: Calculate conjugate parameter β k And the k+1th search vector d k+1 , and returns to S2233 for the next iteration until the iteration stop condition is met.

[0057] Beneficial technical effects of the present invention:

[0058] The present invention adopts a solution that combines a small number of lead aircraft equipped with high-precision sensors with a large number of wingmen equipped with low-precision sensors, balancing the contradiction between the load and positioning accuracy of the rotorcraft UAV; the proposed UAV cluster collaborative positioning method abstracts the measurement information of the UAV and sensors into variable nodes and factor nodes to establish a factor graph model, which can achieve accurate fusion of multi-sensor information; adopts a distributed computing method, and each UAV establishes its own local factor graph model, avoiding the problem of over-reliance on the central node; designs the AGA-GNCG algorithm for solving the factor graph model, and uses the AGA algorithm to globally The search is carried out, and the initial solution of the AGA algorithm is used as the starting point of the GNCG algorithm, and the solution is iterated locally, which not only ensures the extensiveness of the global search, but also ensures the accuracy of local optimization; by configuring the crossover and mutation behavior of the genetic algorithm into an adaptive mode, the problem of falling into the local optimal solution is avoided; the Gauss-Newton algorithm is improved by the conjugate gradient iteration method, which effectively shortens the running time of the algorithm; the position information solved by the AGA-GNCG algorithm has a root mean square error of less than 1m in the three directions of the northeast sky; the GNCG algorithm has a significant improvement in computational efficiency compared with the GNQR algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 This is the UAV cluster collaborative positioning framework proposed by the present invention;

[0060] Figure 2 This is the UAV cluster information interaction diagram proposed by the present invention;

[0061] Figure 3 This is the UAV local factor graph model diagram proposed by the present invention;

[0062] Figure 4 Flowchart of the adaptive genetic algorithm proposed by the present invention;

[0063] Figure 5 This is the flight trajectory diagram of the UAV cluster proposed by the present invention;

[0064] Figure 6 This is the easting error map of the UAV proposed in the present invention;

[0065] Figure 7 This is the north error map of the UAV proposed in the present invention;

[0066] Figure 8 This is the azimuth error diagram of the UAV proposed in the present invention;

[0067] Figure 9 This is a comparison chart of the processing time of different algorithms proposed in this invention. DETAILED DESCRIPTION

[0068] The present invention will be further explained below with reference to specific embodiments.

[0069] Reference Figure 1 The present invention proposes a distributed factor graph model solution method for UAV cluster collaborative positioning, which uses a distributed method to equip each UAV with a plug-and-play Beidou satellite navigation sensor (BDS) and an ultra-wideband ranging sensor (UWB). Among them, BDS and UWB are used as factor nodes, and each UAV is used as a variable node to establish a local factor graph model. The factor graph model is solved using a combination strategy of global and local. The global algorithm uses the AGA algorithm, and the local algorithm uses the GNCG algorithm. During the whole process, all UAVs broadcast their own positions through the wireless network and receive the latest positions of other UAVs, and finally obtain the optimal position information. The specific method steps are as follows:

[0070] (1) Simulate the information interaction graph of drone clusters and establish a local factor graph model of drones

[0071] The information interaction diagram of the drone cluster simulated by the present invention is as follows: Figure 2 As shown in Figure 1, a cluster of 8 drones is designed for collaborative positioning, with 2 lead drones equipped with high-precision BDS, 6 wingmen equipped with low-precision BDS, and each drone equipped with UWB. BDS and UWB provide each drone with location information and distance information between drones, respectively. All drones exchange information through wireless networks. Taking drone 1 as an example, the drone is used as a variable node, and the BDS and UWB measurement information is used as a factor node. At any time t, the local factor graph model of drone 1 is established as follows: Figure 3 shown.

[0072] (2) Factor graph model fusion

[0073] Take any wingman as an example, record it as number i, and the position information of drone i at time t is X i , They represent the northeastern celestial coordinates of UAV i at time t, and the measurement information set of all sensors is Z i , based on the above information, establish the posterior probability P(X i |Z i ), by maximizing the posterior probability, we can obtain X i The maximum a posteriori estimate of :

[0074]

[0075] In the formula, according to Bayesian theory, P(Z i |X i ) represents the observation probability, P(X i ) represents X iThe prior probability of Z i The prior probability of X i The maximum a posteriori estimate of is not affected by P(Zi), so the above formula can be written as:

[0076]

[0077] For navigation and positioning systems, the measurement information between each sensor is independent of each other. According to the Markov hypothesis, the state information at a certain moment t = k is only related to the previous moment t = k-1, and has nothing to do with the previous state. Therefore, the observation probability P(Z i |X i ) and the prior probability P(X i ) can be written as follows:

[0078]

[0079] Where P(X0) represents the prior information. Since each factor is represented as an independent term, the maximum a posteriori probability estimate can be converted into the form of a product of multiple local functions:

[0080]

[0081] Where, f m represents the factor node, m represents the location information measured by BDS or the distance information measured by UWB, Represents the factor node f m Related variable nodes. Each factor node f m Each corresponds to an error function f m Defined as:

[0082]

[0083] Where d(·) is the cost function corresponding to the factor node, h(·) represents the theoretical measurement function of the sensor, For and f m The actual measurement value of the relevant sensor.

[0084] Considering that the sensor measurement noise is Gaussian white noise, the above formula can be written as:

[0085]

[0086] Where, Σ m is the covariance matrix corresponding to different measurement models, represents the Mahalanobis squared distance.

[0087] Since the maximum posterior probability P(X i |Z i) is proportional to the product of all factor nodes in the factor graph, that is:

[0088]

[0089] Taking the negative logarithm of the above formula can transform the maximum a posteriori probability estimation problem into a nonlinear least squares problem:

[0090]

[0091] The factor nodes in the factor graph model are established by BDS and UWB. The BDS factor provides the location information of UAV i, and the measurement model of BDS can be expressed as:

[0092]

[0093] Where h BDS is the theoretical measurement function of BDS, n BDS is the measurement noise of BDS. The BDS factor node is expressed as:

[0094]

[0095] The UWB ranging factor provides the distance information between UAV i and UAV j. The UWB ranging measurement model can be expressed as:

[0096]

[0097] Where, d i,j is the theoretical distance between UAV i and UAV j, n UWB is the UWB ranging noise, and the UWB theoretical ranging measurement function between drones is:

[0098]

[0099] Therefore, the ranging factor node can be expressed as:

[0100]

[0101] According to the derivation of the above formula, we can solve X i The maximum a posteriori estimation problem is converted into a nonlinear least squares problem:

[0102]

[0103] (3) Use the adaptive genetic algorithm to solve the factor graph model. The process of the adaptive genetic algorithm is as follows: Figure 4 The specific steps are as follows:

[0104] S1: Initialize the population

[0105] Set the population size to 200, using location information as the individuals in the population. Introduce random errors in the three directions of the BDS's actual measured position, creating a population size of 200 x 3. Also, set the crossover rate to 0.6, the mutation rate to 0.1, and the maximum number of iterations to 100.

[0106] S2: Calculation of fitness value

[0107] by As the fitness function, that is, the objective function; when entering the Nth iteration, calculate the fitness values ​​of all individuals in the Nth generation population and output the optimal individual of the Nth generation

[0108] S3: Determine whether the iteration stop condition is met, that is, the change in the fitness value of the optimal individual in two consecutive generations is less than the set threshold ζ or the maximum number of iterations is reached; if it is met, the iteration is stopped and the optimal position information corresponding to the optimal solution of the objective function is output; if it is not met, enter S4.

[0109] S4: According to the elite selection strategy, 10% of the individuals with smaller fitness values ​​in the Nth generation are retained, namely the elite individuals; at the same time, random errors are introduced into the optimal individuals to form multiple new individuals, which together constitute the new population of the N+1th generation.

[0110] S5: Binary encode the new population and perform crossover, mutation, and decoding operations to complete the population update. During the crossover and mutation process, the crossover probability and mutation probability adaptive adjustment formula are designed. The crossover probability adaptive adjustment formula is:

[0111]

[0112] Among them, J min is the minimum fitness value in the group; J avg is the average fitness value of each generation group, J' is the smaller fitness value of the two individuals to be crossed; p c1 is the crossover probability set initially.

[0113] The adaptive adjustment formula of mutation probability is:

[0114]

[0115] Among them, J is the fitness value of the individual to be mutated; p m1 is the initial mutation probability.

[0116] S6: Update the new population after genetic evolution and repeat S2-S6.

[0117] (4) Using the optimal solution found by the adaptive genetic algorithm as the initial value, the improved Gauss-Newton algorithm searches for the optimal value in the local area. The specific steps are as follows:

[0118] The theoretical measurement function h m (·) performs a first-order Taylor expansion:

[0119]

[0120] Among them, H m is the sensor's measurement function h m (·) At linear points The Jacobian matrix at Δ m Increment the status.

[0121] h m Substituting the first-order Taylor expansion of (·) into In this case, the linear least squares problem is obtained. At this time, the solution of this problem is Δ * for:

[0122]

[0123] use Markov norm Converting to the 2-norm, we get the standard least squares problem:

[0124]

[0125] For Δ * The solution is to minimize

[0126]

[0127] Taking the derivative of Δ, we get (A T A)Δ=A T b.

[0128] The existing GNQR algorithm is based on the linear point X i,0 The iteration starts at , and in each iteration, the step size Δ is calculated by performing QR decomposition on the matrix A and then inverting it, and the linear point is updated X i,t+1 =X i,t +Δ, until the iterative convergence condition is met.

[0129] The present invention adopts the CG algorithm to iteratively solve Δ, thereby avoiding the matrix inversion operation with higher complexity. The CG algorithm uses the information of the previous iteration in each iteration to guide the search direction of this iteration, thereby converging to the optimal solution within a limited number of steps.

[0130] Using CG algorithm to solve (A T A)Δ=A T The problem of b is transformed into solving a quadratic optimization problem:

[0131]

[0132] The iterative solution formula of the CG algorithm is as follows:

[0133] Δ k+1 =Δ k +α k d k

[0134]

[0135] η k =η k-1 -α k-1 A T Ad k-1

[0136]

[0137] Where: k represents the number of iterations; α k is the step size of the kth iteration; d k is the search vector for the kth iteration; η k represents the error vector of the kth iteration; β k is represented as the kth conjugate parameter.

[0138] The specific steps of the CG algorithm iterative solution to Δ are as follows:

[0139] S1: When the number of iterations k = 0, input matrix A T A, vector A T b, and the initialized random vector Δ0. Set the maximum number of iterations, use the negative gradient direction of f(Δ) as the initial error vector η0, and set the initial search vector d0 equal to the initial error vector η0; where:

[0140]

[0141] S2: Calculate α0, Δ1, η1, and determine whether η1 is less than the set threshold ε. If it is less than ε, output the optimal solution Δ1 = Δ0 + α0d0, otherwise enter S3; where:

[0142]

[0143] η1=η0-α0A T Ad0

[0144] S3: When the number of iterations k>0, calculate α k , Δ k+1 , η k+1 , judge the iteration stop condition, that is, whether the maximum number of iterations is reached, or judge η k+1Is it less than the threshold ε? If it is satisfied, then output the optimal solution Δ * =Δ k+1 Otherwise, go to S4; where:

[0145] η k+1 =η k -α k A T Ad k

[0146] S4: Calculate the conjugate parameter β k And the k+1th search vector d k+1 , and returns to S3 for the next iteration until the iteration stop condition is met.

[0147] (5) Perform positioning accuracy simulation verification

[0148] By solving the factor graph model using the AGA-GNCG algorithm, the optimal position information of the UAV can be obtained. Figure 5 As shown in Figure 1, the flight trajectory of the UAV cluster is simulated, and the simulation time is 2400s. The flight process includes acceleration climbing, constant speed level flight, deceleration descent, deceleration turning and other maneuvers, and the sensor parameters are configured as shown in Table 1.

[0149] Table 1 Sensor parameter configuration

[0150]

[0151]

[0152] The experiment verified the positioning effect of GNQR algorithm, GA algorithm and AGA-GNCG algorithm. Taking UAV 2 as an example, the easting error comparison under the three algorithms is as follows: Figure 6 As shown, the north error is Figure 7 As shown, the celestial error is Figure 8 As shown in the figure, during the entire simulation period, the GNQR algorithm has the largest error fluctuation, the GA algorithm has the second largest error fluctuation, and the AGA-GNCG algorithm has the smallest positioning error fluctuation and the highest accuracy.

[0153] The root mean square error is used to verify the positioning accuracy, and the calculation formula is:

[0154]

[0155] Where: P Ru represents the theoretical reference position of the UAV, P Eu represents the drone position calculated by the algorithm, and V represents the total number of theoretical reference positions. RMSE reflects the deviation between the reference value and the estimated value. The smaller the RMSE, the higher the measurement accuracy.

[0156] Calculations show that the GNQR algorithm's RMS positioning errors in the three northeast and sky directions are 1.274m, 1.486m, and 1.783m, respectively; the GA algorithm's RMS positioning errors in the three northeast and sky directions are 1.025m, 1.239m, and 1.571m, respectively; and the AGA-GNCG algorithm's RMS positioning errors in the three northeast and sky directions are 0.509m, 0.598m, and 0.883m, respectively. The GNQR algorithm has the largest RMS positioning error, followed by the GA algorithm, and the AGA-GNCG algorithm has the smallest RMS positioning error, all of which remain within 1m.

[0157] (6) Perform GNCG operation rate simulation verification

[0158] The first 1200S of the flight trajectory is selected for analysis to compare the running speed of the GNCG algorithm and the GNQR algorithm. The algorithm processing time is compared. Figure 9 As shown in the figure, the GNCG algorithm takes less time to run than the GNQR algorithm. The average running time of the GNQR algorithm is 0.0086s; the average running time of the GNCG algorithm is 0.0036s. The GNCG algorithm has an average computational speed improvement of 58.1% compared to the GNQR algorithm.

Claims

1. A distributed factor graph model solution method for UAV cluster collaborative positioning, characterized by: The steps are as follows: S1: Equip the drone cluster with sensors, simulate the drone cluster information interaction graph, and establish a drone local factor graph model; S2: Solve the factor graph model using a global algorithm combined with a local algorithm; Among them: the global algorithm adopts an adaptive genetic algorithm, and the crossover and mutation behaviors of the genetic algorithm are configured into an adaptive mode; In the adaptive genetic algorithm solution in S2, the crossover probability and mutation probability adaptive adjustment formula are designed during the crossover and mutation process, where: The crossover probability adaptive adjustment formula is: ， Where: is the minimum fitness value in the group; is the average fitness value of the population in each generation, is the smaller fitness value of the two bodies to be crossed; is the crossover probability of the initial setting; The adaptive adjustment formula of mutation probability is: ， Where: is the fitness value of the individual to be mutated; is the mutation probability of the initial setting; The local algorithm adopts the improved Gauss-Newton algorithm, takes the information obtained by the adaptive genetic algorithm as the initial value, searches for the optimal value in the local area, and calculates the update vector in an iterative manner to obtain the final optimal position information.

2. The distributed factor graph model solving method for UAV cluster collaborative positioning according to claim 1 is characterized in that: In S1, the drone local factor graph model is established with drones as variable nodes and sensors as factor nodes. This model realizes data fusion based on the posterior probability estimation theory and converts it into solving a nonlinear least squares problem by taking the negative logarithm of the posterior probability estimate: ， Where: , Representing drones exist Northeast celestial coordinate of the moment; Position information or distance information measured by the sensor; is the theoretical measurement function of the sensor; is the actual measurement value of the sensor associated with the factor node; is the covariance matrix corresponding to different measurement models.

3. The distributed factor graph model solving method for UAV cluster collaborative positioning according to claim 2 is characterized in that: The sensors in the S1 include an ultra-wideband ranging sensor and a BeiDou satellite navigation sensor; Beidou satellite navigation sensor factors provide drones The position information is expressed as: ， Where: is the measurement function of the Beidou satellite navigation sensor, For drones The theoretical position information of UAV for actual measurement of Beidou satellite navigation sensors location information, is the covariance matrix of the BeiDou satellite navigation sensor measurement model; Ultra-wideband ranging sensor factors in drones and drones The distance information is expressed as follows: ， Where: is the measurement function of the ultra-wideband ranging sensor, For drones and drones The theoretical position information of UAVs actually measured for ultra-wideband ranging sensors and drones distance information, is the covariance matrix of the ultra-wideband ranging sensor; According to the Beidou satellite navigation sensor factor expression and the ultra-wideband ranging sensor factor expression, the following total cost function is obtained. At this time, the solution is The maximum a posteriori probability estimation is converted into a nonlinear least squares problem: 。 4. The distributed factor graph model solving method for UAV cluster collaborative positioning according to claim 3 is characterized in that: The steps of the adaptive genetic algorithm solution in S2 are as follows: S211: Set the initial population number, population size, crossover rate, mutation rate and maximum number of iterations; S212: Take the total cost function of S1 as the fitness function, when entering the Iteration, calculate the The fitness values ​​of all individuals in the generation population are output The best individual of the generation; S213: Determine whether the iteration stop condition is met. If so, stop the iteration and output the optimal position information corresponding to the optimal solution of the objective function; if not, proceed to S214; S214: According to the elite selection strategy, retain the Elite individuals in the generation; at the same time, random errors are introduced into the optimal individuals to form multiple new individuals, which together constitute the first The new population of the generation; S215: Binary encode the new population and perform crossover, mutation, and decoding operations to complete the population update; S216: Update the new population after genetic evolution and repeat S212-S216.

5. The distributed factor graph model solving method for UAV cluster collaborative positioning according to claim 2 is characterized in that: The steps for improving the Gauss-Newton algorithm are as follows: S221: The theoretical measurement function of the sensor Perform a first-order Taylor expansion: ， ， ， Where: is the measurement function of the sensor At the linear point The Jacobian matrix at ; is the state increment, that is, the incremental step size of the iteration; S222: Substitute the Taylor expansion of S221 into the UAV local factor graph model to obtain the solution to the linear least squares problem for: ； S223: Exploitation Markov norm Converted into 2-norm, the standard least squares problem is: ； Where, is the Jacobian matrix of the sensor measurement function and The product of For The prediction error at The product of matrix Is to put all A large matrix obtained by merging; vector Is to put all The high-dimensional vector obtained by merging together; for The solution is to minimize , through Derivative , and then use the conjugate gradient algorithm to iteratively solve , the formula for iterative solution is as follows: ， ， ， ， ， Where: Indicates the number of iterations; For the The step size of the iteration; For the first time Iterative search vector; Indicates the The error vector of the iteration; Expressed as times the conjugate parameter.

6. The distributed factor graph model solving method for UAV cluster collaborative positioning according to claim 5 is characterized in that: The iterative solution method steps in S223 are as follows: S2231: When the number of iterations When the input matrix ,vector , and the initialized random vector , set the maximum number of iterations to The negative gradient direction is used as the initial error vector , and let the initial search vector Equal to the initial error vector ; S2232: Calculation 、 and , and judge Is it less than the set threshold? , if less than , then output the optimal solution , otherwise go to S2233; S2233: When the number of iterations When calculating 、 and , judge the iteration stop condition, that is, whether the maximum number of iterations is reached, or judge Is it less than the threshold? ; If satisfied, output the optimal solution , otherwise go to S2234; S2234: Calculate conjugate parameters and The search vector , and returns to S2233 for the next iteration until the iteration stop condition is met.

Images