Distributed factor graph model solving method for unmanned aerial vehicle cluster co-location
Through the distributed factor graph model solution method, combined with adaptive genetic algorithm and improved Gauss Newton's algorithm, the problem of high initial value accuracy requirements and local optimal solutions in collaborative positioning of drone clusters is solved, and high accuracy and robustness are achieved The root mean square error in the northeast direction is within 1m, which significantly improves the calculation efficiency.
Patent Information
- Application Number
- CN202411063591.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-05
- Publication Date
- 2024-11-15
- Estimated Expiration
- 2044-08-05
AI Technical Summary
The existing factor graph model solution methods require high initial value accuracy, easily fall into local optimal solutions, and it is difficult to achieve high-precision and robust positioning in the coordinated positioning of drones clusters.
Using the distributed factor graph model solution method, combining adaptive genetic algorithm and improved Gauss Newton's algorithm, a local factor graph model for drone is established through the combination of global and local algorithms, and global search is performed using adaptive genetic algorithm, and local area Iteratively solves, and finally calculates the update vector through the conjugate gradient algorithm to obtain the optimal position information.
The precise and reliable positioning of the drone cluster is achieved, the problem of local optimal solutions is avoided, the positioning accuracy and calculation efficiency are improved, and the root mean square error in the three directions of the Northeast Tian is maintained within 1m, which significantly improves the calculation efficiency.
Smart Images

Figure CN118972940A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of navigation and positioning technology, and in particular to a distributed factor graph model solving method for collaborative positioning of unmanned aerial vehicle clusters. Background Art
[0002] The military field in today's world is rapidly developing towards intelligence, the combat environment is becoming increasingly complex, and the difficulty of combat is also rising. Drones play an irreplaceable role with their outstanding autonomy and flexibility, and continue to promote major changes in the mode of modern warfare. Compared with individual drones, drone swarms can complete complex and diverse tasks more flexibly through information interaction and mutual cooperation. In the modern battlefield, real-time high-precision positioning information is a necessary prerequisite for drone swarms to efficiently complete tasks when they are in coordinated combat.
[0003] Existing positioning methods include single-machine positioning and cluster collaborative positioning. For single-machine positioning, the positioning system is usually composed of a combination of multi-source heterogeneous sensors, and combined with multi-source information fusion technology to achieve high-precision positioning. Among them, fusion methods based on Kalman filtering and its variants are more common, but when the information of a certain sensor fails, system reconstruction will be very complicated. The fusion method based on graph optimization flexibly expands the sensors in the system through the concepts of factors and nodes, and can realize plug-and-play of sensors; compared with single-machine positioning, cluster collaborative positioning technology has higher positioning accuracy and robustness. Each combat or detection unit is regarded as an independent network node, and the nodes improve positioning accuracy by mutual observation and sharing of status information.
[0004] At present, the solution to the factor graph model mainly uses the Gauss-Newton algorithm, which iterates the optimal solution through QR decomposition to obtain the final positioning information. However, this method has requirements on the accuracy of the initial value, otherwise it is easy to fall into the local optimal solution. Therefore, it is urgent to develop a new factor graph model solution method to improve the accuracy of positioning. 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 solving method for UAV cluster collaborative positioning proposed in the present invention has the following steps:
[0007] S1: Equip the drone cluster with sensors, simulate the information interaction graph of the drone cluster, and establish a local factor graph model of the drone;
[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 UAV local factor graph model established in S1 with the UAV as the variable node and the sensor as the factor node realizes data fusion based on the posterior probability estimation theory, and converts it into solving the nonlinear least squares problem by taking the negative logarithm of the posterior probability estimation:
[0012]
[0013] Where: They represent the northeast celestial coordinates 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 sensor in S1 includes 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 position information of UAV i actually 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 following total cost function is obtained. At this time, the solution of X i The maximum a posteriori probability estimation is transformed into a nonlinear least squares problem:
[0022]
[0023] Preferably, the method steps of solving the problem by the adaptive genetic algorithm 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, J' is the smaller fitness value of the two individuals to be crossed; p c1 is the crossover probability set initially;
[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 set mutation probability.
[0037] Preferably, the steps of the method for solving the problem by improving the Gauss-Newton algorithm 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 measurement function h m (·) At the linear point The Jacobian matrix at Δ m is the state increment, i.e. the incremental step of iteration;
[0041] S222: Substitute the Taylor expansion of S221 into the local factor graph model of the UAV to obtain the solution of the linear least squares problem Δ * for:
[0042]
[0043] S223: Exploitation Mahalanobis norm Converted into 2-norm, the standard least squares problem is:
[0044]
[0045] In the formula, 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 The merged large matrix, vector b is all b m The resulting high-dimensional vector is merged together.
[0046] For Δ * The solution is to minimize By taking the derivative of Δ, we 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 Denoted 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 set the initial search vector d0 equal to the initial error vector η0;
[0054] S2232: Calculate α0, Δ1 and η1, and determine whether η1 is less than the set threshold ε. If it is less than ε, output the optimal solution Δ1=Δ0+α0d0, otherwise go to S2233;
[0055] S2233: When the number of iterations k>0, calculate α k , Δ k+1 and η k+1 , determine the iteration stop condition, that is, whether the maximum number of iterations has been reached, or determine η 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 the 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, which balances the contradiction between the load and positioning accuracy of the rotorcraft UAV; the UAV cluster collaborative positioning method provided by the invention abstracts the measurement information of the UAV and the sensor into variable nodes and factor nodes to establish a factor graph model, which can realize the accurate fusion of multi-sensor information; adopts a distributed computing method, 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 is kept within 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 a diagram of 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 The local factor graph model diagram of the UAV proposed by the present invention;
[0062] Figure 4 The flowchart of the adaptive genetic algorithm proposed by the present invention;
[0063] Figure 5 The flight trajectory diagram of the drone cluster proposed by the present invention;
[0064] Figure 6 This is the eastward error map of the UAV proposed by the present invention;
[0065] Figure 7 This is the north error diagram of the UAV proposed by the present invention;
[0066] Figure 8 This is the azimuth error diagram of the UAV proposed in the present invention;
[0067] Fig. 9 This is a comparison chart of the processing time of different algorithms proposed in the present invention. DETAILED DESCRIPTION
[0068] The present invention will be further explained below in conjunction with specific embodiments.
[0069] Reference Figure 1 The distributed factor graph model solving method for cooperative positioning of drone clusters proposed in the present invention adopts a distributed method to equip each drone 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 drone is used as a variable node to establish a local factor graph model. A combination strategy of global and local is used to solve the factor graph model. The global algorithm adopts the AGA algorithm, and the local algorithm adopts the GNCG algorithm. During the whole process, all drones broadcast their own positions through the wireless network and receive the latest positions of other drones, 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 the figure, a cluster of 8 drones is designed for collaborative positioning, of which 2 lead drones are equipped with high-precision BDS, 6 wingmen are equipped with low-precision BDS, and each drone is equipped with UWB. BDS and UWB provide location information and distance information between drones for each drone 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 are used as factor nodes. 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, denoted as number i, and the position information of drone i at time t is X i , They represent the northeast 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 ) indicates 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 the navigation and positioning system, the measurement information between each sensor is independent of each other. According to the Markov hypothesis, the state information at a certain time t = k is only related to the previous time 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] In the formula, 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 the product of multiple local functions:
[0080]
[0081] In the formula, 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 measured 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] In the formula, Σ m is the covariance matrix corresponding to different measurement models, represents the Mahalanobis squared distance.
[0087] Since the maximum a posteriori 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] In the formula, 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 As shown, the specific steps are as follows:
[0104] S1: Initialize the population
[0105] The population size is set to 200, and the position information is used as the individuals of the population. Random errors are introduced in the three directions of the actual measurement position of the BDS to form a population size of 200*3. At the same time, the crossover rate is set to 0.6, the mutation rate is set to 0.1, and the maximum number of iterations is set 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. In the crossover and mutation process, design the crossover probability and mutation probability adaptive adjustment formula. 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, 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 set mutation probability.
[0116] S6: Update the new population after genetic evolution and repeat S2-S6.
[0117] (4) Using the optimal solution searched 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 measurement function h m (·) At the linear point The Jacobian matrix at m Increment the state.
[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 Mahalanobis 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 uses 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 high complexity. The CG algorithm guides the search direction of this iteration by using the information of the previous iteration in each 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 CG algorithm iterative solution formula 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 Denoted 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 , determine the iteration stop condition, that is, whether the maximum number of iterations has been reached, or determine η 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 conjugation 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 through 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, the simulation time is 2400s, the flight process includes accelerated climb, uniform level flight, decelerated descent, decelerated turn 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 effects of GNQR algorithm, GA algorithm and AGA-GNCG algorithm. Taking UAV 2 as an example, the eastward error comparison under the three algorithms is as follows: Figure 6 As shown in Figure 2, the north error is Figure 7 As shown in the figure, the azimuth error is Figure 8 As shown in the figure, during the entire simulation period, the GNQR algorithm has the largest error fluctuation, followed by the GA algorithm, 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. The calculation formula is:
[0154]
[0155] Where: P Ru represents the theoretical reference position of the UAV, P Eu represents the position of the drone solved by the algorithm, and V represents the total number of all theoretical reference positions. RMSE is used to reflect the deviation between the reference value and the estimated value. The smaller the RMSE, the higher the measurement accuracy.
[0156] After calculation, the root mean square errors of the GNQR algorithm in the three directions of northeast and sky are 1.274m, 1.486m, and 1.783m respectively; the root mean square errors of the GA algorithm in the three directions of northeast and sky are 1.025m, 1.239m, and 1.571m respectively; the root mean square errors of the AGA-GNCG algorithm in the three directions of northeast and sky are 0.509m, 0.598m, and 0.883m respectively. The GNQR algorithm has the largest root mean square error, followed by the GA algorithm, and the AGA-GNCG algorithm has the smallest root mean square error, and all of them are kept within 1m.
[0157] (6) Perform GNCG operation rate simulation verification
[0158] The first 1200 seconds of the flight trajectory are selected for analysis to compare the running rates of the GNCG algorithm and the GNQR algorithm. The algorithm processing time is compared. Fig. 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 calculation rate of the GNCG algorithm is 58.1% higher than that of 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 information interaction graph of the drone cluster, and establish a local factor graph model of the drone; 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; 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 local factor graph model of the drone is established with the drone as the variable node and the sensor as the factor node. This model realizes data fusion based on the theory of posterior probability estimation, and converts it into solving the nonlinear least squares problem by taking the negative logarithm of the posterior probability estimation: Where: They represent the northeast celestial coordinates 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.
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; The Beidou satellite navigation sensor factor provides the location information of UAV i, and its expression is: Where: h BDS is the measurement function of the Beidou satellite navigation sensor, is the theoretical position information of UAV i, is the position information of UAV i actually measured by Beidou satellite navigation sensor, Σ BDS is the covariance matrix of the BeiDou satellite navigation sensor measurement model; The ultra-wideband ranging sensor factor provides the distance information between UAV i and UAV j, and its expression is: 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; 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 of X i The maximum a posteriori probability estimation is transformed 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: 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; 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, 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; 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 method for solving the distributed factor graph model for cooperative positioning of drone clusters according to claim 4 is characterized in that: In S215, a crossover probability and a mutation probability adaptive adjustment formula are designed during the crossover and mutation process, wherein: The crossover probability adaptive adjustment formula is: Where: J min is the minimum fitness value in the group; J avg is the average fitness value of each generation, J' is the smaller fitness value of the two individuals to be crossed; p c1 is the crossover probability set initially; The adaptive adjustment formula of mutation probability is: Where: J is the fitness value of the individual to be mutated; p m1 is the initial set mutation probability.
6. The distributed factor graph model solving method for UAV cluster collaborative positioning according to claim 2 is characterized in that: The steps of improving the Gauss-Newton algorithm are as follows: S221: The theoretical measurement function h of the sensor m (·) performs a first-order Taylor expansion: Where: H m is the sensor measurement function h m (·) At the linear point The Jacobian matrix at Δ m is the state increment, i.e. the incremental step of iteration; S222: Substitute the Taylor expansion of S221 into the local factor graph model of the UAV to obtain the solution of the linear least squares problem Δ * for: S223: Exploitation Mahalanobis norm Converted into 2-norm, the standard least squares problem is: In the formula, 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; vector b is all b m The high-dimensional vector obtained by merging together; For Δ * The solution is to minimize By taking the derivative of Δ, we 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: D k+1 =D k +a k d k or k =the k-1 -a k-1 A T Advertisement k-1 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 Denoted as the kth conjugate parameter.
7. The method for solving the distributed factor graph model for cooperative positioning of drone clusters according to claim 6 is characterized in that: The steps of iterative solution in S223 are as follows: 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 set the initial search vector d0 equal to the initial error vector η0; S2232: Calculate α0, Δ1 and η1, and determine whether η1 is less than the set threshold ε. If it is less than ε, output the optimal solution Δ1=Δ0+α0d0, otherwise go to S2233; S2233: When the number of iterations k>0, calculate α k , Δ k+1 and η k+1 , determine the iteration stop condition, that is, whether the maximum number of iterations has been reached, or determine η k+1 Is it less than the threshold ε? If it is satisfied, then output the optimal solution Δ * =Δ k+1 , otherwise go to S2234; S2234: Calculate the 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.
Citation Information
Patent Citations
Formation unmanned aerial vehicle cooperation positioning method based on posteriori linearization confidence propagation
CN109901108A
Unmanned support vehicle path planning method
CN116976534A
Rapid and automatic iteration method and system for trajectory parameters of carrier rocket
CN117113838A
Indoor mobile robot sensor fusion factor graph positioning method
CN117889849A
Gaussian Newton positioning method based on TOF fusion particle swarm under constant gradient sound velocity profile
CN117933066A
Cited By
Formation grouping optimization method for multi-node cluster cooperative positioning
CN119596977A
Three-dimensional space positioning method based on wireless mesh network and server
CN119854936A
A three-dimensional space positioning method based on a wireless mesh network and a server
CN119854936B
Offshore efficient communication method based on integrated unmanned aerial vehicle STAR-RIS
CN120264325A
An efficient offshore communication method based on integrated UAV STAR-RIS
CN120264325B
