Unmanned aerial vehicle autonomous navigation method based on sensitivity factor optimization set kalman filter

Abstract

The present application relates to the low-altitude economy unmanned aerial vehicle autonomous navigation technical field, especially unmanned aerial vehicle autonomous navigation method based on sensitivity factor optimization set Kalman filtering, including S1, the state quantity related to unmanned aerial vehicle navigation is acquired, the discrete state model and measurement model of unmanned aerial vehicle navigation control system containing uncertain parameter are established, and the corresponding noise statistical parameters are determined;S2, state set points are generated based on the discrete state model, prior state estimation and its statistics are calculated, and corresponding state sensitivity information is obtained for uncertain parameters.The present application introduces the sensitivity propagation mechanism in the set Kalman filtering framework, and the sensitivity weight matrix is adjusted online adaptively by using intelligent optimization algorithm, the effective inhibition of model parameter uncertainty is realized, and the unmanned aerial vehicle navigation precision is significantly improved.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation technology for unmanned aerial vehicles (UAVs) in the low-altitude economy, and particularly to an autonomous navigation method for UAVs based on sensitivity factor optimization ensemble Kalman filtering. Background Technology

[0002] Multi-rotor unmanned aerial vehicles (UAVs), with their unique advantages of vertical takeoff and landing, hovering, and agile maneuverability, have demonstrated enormous application potential in many fields such as military reconnaissance, logistics delivery, and agricultural plant protection. As the core of UAVs' autonomous flight, the accuracy, robustness, and real-time performance of their state estimation directly determine the overall performance and mission completion capability of the aircraft.

[0003] In real-world flight environments, UAV navigation systems face numerous challenges. First, their dynamic models exhibit significant uncertainties, including but not limited to perturbations of model parameters and unmodeled dynamics caused by atmospheric turbulence, mass variations, and aerodynamic coupling effects. Second, constrained by size, weight, and cost, measurement information from airborne sensors (such as microelectromechanical inertial measurement units and GPS receivers) is often accompanied by non-Gaussian noise and anomalous disturbances. These factors cause a sharp decline in the estimation accuracy of traditional filtering algorithms based on accurate models (such as standard Kalman filtering) in practical applications, and may even lead to filter divergence, seriously threatening flight safety.

[0004] To improve the robustness of estimation under model mismatch conditions, the Desensitized Kalman Filter (DKF) was proposed (Christopher D. Karlgaard, Hai Jun Shen. Desensitized Kalman Filtering[J]. IET Radar, Sonar & Navigation, 2013, 7(1):2-9). This method effectively suppresses the influence of parameter perturbations on the estimation results by introducing the sensitivity index of the state estimation error to the uncertain parameters of the model into the cost function and adjusting the filter gain accordingly. However, the performance of DKF and related methods largely depends on the artificially set sensitivity weight matrix, and the optimization problem of this matrix has not been systematically solved, which limits its application effect in complex nonlinear UAV systems. On the other hand, the Ensemble Kalman Filter (EnKF), as a Monte Carlo filtering method suitable for nonlinear systems, approximates the error covariance through the statistical properties of a set of states, without the need to calculate a complex Jacobian matrix, showing a natural advantage in dealing with the state estimation problem of high-dimensional nonlinear systems such as UAVs. However, the standard EnKF method itself does not include a dedicated mechanism for handling model parameter uncertainties. When the system has the aforementioned model perturbations, its estimation accuracy cannot be guaranteed. Therefore, there is a significant gap in the existing technology: the lack of an efficient filtering framework that can simultaneously handle system nonlinearity and actively suppress the effects of model parameter uncertainties.

[0005] While existing technologies have attempted to apply the concept of weak-sensitivity filtering to Kalman filtering, these methods are all designed for linear or low-dimensional systems, and the sensitivity weight matrix is ​​fixed, failing to adapt to dynamic changes in parameter perturbations. How to achieve online, adaptive, and optimal adjustment of the sensitivity weight matrix within the nonlinear filtering framework of ensemble Kalman filtering remains a pressing technical problem to be solved in this field.

[0006] To address the aforementioned problems, this invention proposes an autonomous navigation method for unmanned aerial vehicles (UAVs) based on sensitivity factor optimization and ensemble Kalman filtering. The core of this method lies in integrating the robustness of weak-sensitivity filtering with the ability of ensemble Kalman filtering to handle nonlinear problems, and using an intelligent optimization algorithm to adaptively adjust the key sensitivity weight matrix online. Specifically, this invention introduces a sensitivity propagation mechanism into the prediction step of ensemble Kalman filtering, constructs a performance index function that includes filter residuals and state sensitivity, and uses an intelligent optimization algorithm to search for the optimal sensitivity factors, thereby achieving automated tuning of the sensitivity weight matrix. This method not only inherits the ability of ensemble Kalman filtering to handle nonlinear dynamic models of UAVs, but also significantly enhances robustness to model parameter uncertainties and sensor noise through the online optimization weak-sensitivity mechanism, thus providing a new technical approach for achieving high-precision and high-reliability autonomous navigation of UAVs in complex real-world environments. Summary of the Invention

[0007] To overcome the above shortcomings, this invention provides an autonomous navigation method for UAVs based on sensitivity factor optimization set Kalman filtering, aiming to improve the technical problems of low filtering accuracy and poor filtering stability in existing UAV navigation systems when affected by uncertainties.

[0008] To achieve the above objectives, the present invention provides the following technical solution: an autonomous navigation method for unmanned aerial vehicles based on sensitivity factor optimization ensemble Kalman filtering, comprising:

[0009] S1, Model Establishment: Obtain UAV navigation state variables, establish a discrete state model and measurement model containing uncertain parameters, and determine noise statistical parameters;

[0010] S2, Set Prediction and Sensitivity Calculation: Generate state set points based on the discrete state model, and obtain predicted set points through state equation propagation; calculate prior state estimates and their statistics based on the predicted set points; calculate the state sensitivity of each set point for the uncertain parameters, and obtain the sensitivity matrix of the prior state through sensitivity propagation.

[0011] S3, Adaptive Sensitivity Weight Adjustment: A sensitivity factor is introduced to adjust the sensitivity weight matrix; a performance index function containing filter residual terms and state sensitivity weighting terms is constructed; an intelligent optimization algorithm is used to search for the optimal sensitivity factor online to achieve adaptive adjustment of the sensitivity weight matrix;

[0012] S4, Calculate the Kalman gain: Introduce the adjusted sensitivity weight matrix into the cost function, and calculate the Kalman gain under the constraint of minimizing the cost function that simultaneously includes the state estimation error and the state sensitivity weighting term;

[0013] S5, Sensitivity Information Update: Using the Kalman gain, update the sensitivity information of the state estimation error, and calculate the posterior sensitivity variance matrix and the posterior sensitivity transfer matrix.

[0014] S6, State estimation output: Based on the Kalman gain and measurement information, calculate the posterior state estimate and posterior estimation error variance matrix of the UAV, and output the navigation state result.

[0015] Preferably, in step S2, the specific method of sensitivity propagation is as follows:

[0016] calculate The state sensitivity of each set point to uncertain parameters is calculated using the following formula:

[0017]

[0018] in, For the first The meeting point is at Sensitivity of steps For the first The posterior state of each set point A vector of uncertain parameters;

[0019] The sensitivity is propagated through the state equation, and its calculation formula is as follows:

[0020]

[0021] in, For the first The meeting point is at Predictive sensitivity of steps State transition function for Step control input;

[0022] The prior sensitivity matrix is ​​calculated using the following formula:

[0023]

[0024] in, The number of rendezvous points.

[0025] Preferably, in step S3, the performance index function is constructed as follows:

[0026]

[0027] in, For the filter residual, For k-step measurements, For the k-step prior measurement prediction value, Sensitivity factors This is the sensitivity weight matrix. The prior sensitivity matrix is ​​calculated in step S2. The trace of a matrix, indicated by a superscript This indicates the matrix transpose.

[0028] Preferably, the sensitivity weight matrix Determined in the following ways:

[0029]

[0030] in, Obtained through intelligent optimization algorithm search These are the initial weighting coefficients for each uncertain parameter. , The number of uncertain parameters. This represents a diagonal matrix.

[0031] Preferably, in step S4, the cost function takes the following form:

[0032]

[0033] in, for Step into the real state, for Step state estimate, for Step estimate of error covariance matrix, Represents the mathematical expectation;

[0034] By taking the partial derivative of the cost function and setting it to zero, the Kalman gain is obtained as follows:

[0035]

[0036] in, for Step Kalman gain, The cross-covariance between state and measurement. To measure variance, For the measurement matrix, superscript This represents finding the inverse of a matrix.

[0037] Preferably, in step S3, the intelligent optimization algorithm is a population-based adaptive optimization algorithm, including the following steps:

[0038] S3-1, Initialization: Set population size Maximum number of iterations Sensitivity factors Within the search range, an initial population is randomly generated;

[0039] S3-2, Fitness Assessment: Assessing the sensitivity factors for each individual in the population. Substitute the performance index function from step S3 to calculate the fitness value and record the globally optimal individual;

[0040] S3-3, Adaptive Factor Calculation: Calculate the adaptive factor based on the current population diversity D. This is used to dynamically adjust the exploration and development ratio;

[0041] S3-4, Population Update: Execute a three-stage update strategy sequentially. The first stage uses probability... Execute exploratory updates, the second phase is based on probability. The search is guided by the globally optimal individual, and the third stage uses probability. Perform an update using the available resources;

[0042] S3-5, Determine the termination condition: If the maximum number of iterations is reached or the convergence condition is met, output the optimal sensitivity factor that minimizes the performance index function. Otherwise, return to step S3-2.

[0043] Preferably, in step S1, the discrete state model is:

[0044] Equations of state:

[0045]

[0046] Measurement equation:

[0047]

[0048] in, for Step state vector, For dependent on uncertain parameters The state transition matrix, To control the input matrix, To control the input, For process noise, For measurement vectors, For the measurement matrix, For measuring noise.

[0049] Preferably, the UAV navigation state variables include three-axis velocity, three-axis attitude angle, and three-axis attitude angular velocity, and the uncertain parameters include coefficients in the state transition matrix that are affected by environmental disturbances, mass changes, or aerodynamic coupling.

[0050] Preferably, in step S5, the formula for calculating the posterior sensitivity transfer matrix is:

[0051]

[0052] in, for Step posterior sensitivity matrix, It is the identity matrix. for Step Kalman gain, For the measurement matrix, for The prior sensitivity matrix is ​​used to characterize the influence of uncertain parameters on the posterior state estimation.

[0053] The present invention has the following beneficial effects:

[0054] 1. Sensitivity propagation and online optimization mechanism for steps S2-S3: By propagating state sensitivity in parallel during the prediction step of the ensemble Kalman filter, and using an intelligent optimization algorithm to adjust the weight matrix in real time based on the filter residual and sensitivity information, the defect of the weight matrix relying on human experience in traditional weakly sensitive filters is avoided, enabling the filter to adapt to different degrees of parameter perturbation.

[0055] 2. Cost function design and gain calculation in step S4: By introducing an optimized sensitivity weight matrix into the weakly sensitive cost function, a dynamic balance between state estimation accuracy and parameter sensitivity suppression is achieved. Compared with the fixed-weight weakly sensitive filtering method, this invention significantly reduces the impact of parameter uncertainty on the estimation results while ensuring the filtering convergence speed, and avoids estimation lag caused by excessively weak sensitivity.

[0056] 3. For the nonlinear adaptability of the ensemble Kalman filter framework: This invention does not require the calculation of a complex Jacobian matrix. It directly obtains sensitivity information through ensemble point sampling, which has high computational efficiency and is suitable for real-time navigation applications of high-dimensional nonlinear systems such as UAVs. Attached Figure Description

[0057] Figure 1 This is a flowchart of the UAV autonomous navigation method based on sensitivity factor optimization set Kalman filter proposed in this invention;

[0058] Figure 2 This is a flowchart illustrating the algorithm principle of the UAV autonomous navigation method based on sensitivity factor optimization set Kalman filter proposed in this invention.

[0059] Figure 3 The image shows the root mean square error of the UAV velocity in the UAV autonomous navigation method based on sensitivity factor optimization set Kalman filter proposed in this invention.

[0060] Figure 4This is a simulation diagram of the root mean square error of the attitude angle of the UAV in the UAV autonomous navigation method based on sensitivity factor optimization set Kalman filter proposed in this invention. Detailed Implementation

[0061] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0062] Example 1

[0063] In a first embodiment of the present invention, the present invention provides an autonomous navigation method for unmanned aerial vehicles based on sensitivity factor optimization ensemble Kalman filtering, such as... Figure 1 As shown, it includes the following steps:

[0064] S1. Obtain the navigation-related state variables of the UAV, establish a discrete state model and measurement model of the UAV navigation control system containing uncertain parameters, and determine the corresponding noise statistical parameters.

[0065] Further, in step S1, the steps of establishing the discrete state model and measurement model of the UAV navigation control system containing uncertain parameters include: obtaining the speed, attitude angle and attitude angular velocity of the UAV as navigation state variables; constructing the state equation and measurement equation of the UAV navigation control system containing uncertain parameters based on the navigation state variables; discretizing the state equation and measurement equation, and determining the system noise variance matrix and the measurement noise variance matrix.

[0066] Specifically, the speeds of the three axes of the UAV navigation and control system are obtained. , and Three-axis attitude angles , and Three-axis attitude angular velocity , and Constructing the state equations of the UAV navigation and control system

[0067] ;

[0068] in , and For uncertain parameters, It is a control input. It is a constant matrix.

[0069] By measuring the state of the UAV at a constant sampling time interval, the measurement equations for the induction motor system are established:

[0070] ;

[0071] Where the observation matrix It is a 9×9 identity matrix. It is zero-mean Gaussian white noise.

[0072] Based on the state equations and measurement equations of the above-mentioned UAV navigation and control system, a discrete mathematical model of the UAV navigation and control system is constructed:

[0073] ;

[0074] ;

[0075] in, and They are mutually independent zero-mean Gaussian white noise sequences, and The variances are respectively , The variance is And satisfy The system noise variance matrix of the UAV navigation and control system model can be obtained through statistical methods. and measurement noise variance matrix .

[0076] S2 generates state set points based on the discrete state model, calculates prior state estimates and their statistics, and obtains corresponding state sensitivity information for uncertain parameters.

[0077] Furthermore, in step S2, the step of generating state set points includes: generating a set point of UAV state based on the state estimation information of the UAV during flight; and performing state transfer on the state set point to obtain the set point after propagation by the state equation.

[0078] Further, in step S2, the step of calculating the prior state estimate and its statistics includes: calculating the prior state estimate based on the propagated set point; and calculating the corresponding state error variance, measurement error variance, and cross-covariance between the state and the measurement based on the deviation between the set point and its mean.

[0079] Furthermore, in step S2, the step of obtaining the corresponding state sensitivity information includes: calculating the state sensitivity of the set point for the uncertain parameters; propagating the state sensitivity to obtain the sensitivity information corresponding to the prior state.

[0080] Specifically, based on the state of the UAV during flight, the set point of the state, as well as the prior state estimate and prior error variance, are obtained.

[0081] Based on the above discrete mathematical model, by the first State estimates of the unmanned aerial vehicle (UAV) sum of error variance matrix Calculate the first Meeting point of steps:

[0082] ;

[0083] The superscript "+" indicates its posterior estimate. The error variance matrix The square root of the first Column, satisfying , The number of sampling points;

[0084] The rendezvous point after function passing is:

[0085] ;

[0086] Then its corresponding prior state estimate:

[0087] ;

[0088] ;

[0089] Wherein, the superscript "-" indicates a prior estimate of the variable;

[0090] Then the state variance Measurement variance and their mutual covariance They are respectively

[0091] ;

[0092] ;

[0093] ;

[0094] in, This is the state error matrix, which represents the state values ​​at each set point. and average The difference between them is defined as:

[0095] ;

[0096] The above applies to uncertain parameters The corresponding sensitivity matrix and transfer equation are as follows:

[0097] First, calculate Sensitivity of step set points:

[0098] ;

[0099] in, The number of uncertain parameters;

[0100] Secondly, the sensitivity of the propagation rendezvous point:

[0101] ;

[0102] Then, the sensitivity of the prior states is calculated separately:

[0103] ;

[0104] ;

[0105] S3, based on the preset filtering performance indicators, uses an intelligent optimization algorithm to optimize the sensitivity weight matrix, obtains the optimal sensitivity factor through search, and adjusts the sensitivity weight matrix.

[0106] Furthermore, in step S3, the step of optimizing the sensitivity weight matrix using an intelligent optimization algorithm includes: introducing a sensitivity factor to adjust the sensitivity weight matrix; constructing a performance index function based on the filtering residual and state sensitivity; and using the intelligent optimization algorithm to search for the optimal sensitivity factor to adjust the sensitivity weight matrix.

[0107] Specifically, based on the set performance indicators, the sensitivity factors of the sensitivity weight matrix are obtained using the improved building design IBOA optimization strategy, and then the sensitivity weight matrix is ​​automatically adjusted.

[0108] A sensitivity factor is introduced in this invention. Used to adjust the sensitivity weight matrix Furthermore, the IBOA algorithm is used to find an optimal sensitivity factor in real time to meet the optimal requirements of the filtering process. The fitness function used in the optimization algorithm is:

[0109] ;

[0110] ;

[0111] In the formula Sensitivity factor feasible domain, For matrix The Line number Column elements, The residual matrix , It is a forgetting factor.

[0112] The intelligent optimization algorithm in this invention can be a global optimization algorithm that minimizes the performance index function through search, such as Particle Swarm Optimization (PSO), Genetic Algorithm (GA), Differential Evolution (DE), or Ant Colony Optimization (ACO). This embodiment employs an improved Building Design Optimization Algorithm (IBOA), which effectively avoids getting trapped in local optima and achieves high search efficiency by introducing an adaptive factor to dynamically adjust the exploration-development ratio. However, those skilled in the art should understand that the other optimization algorithms described above can also be applied to the sensitivity factor optimization process of this invention, and their implementation does not exceed the scope of protection of this invention.

[0113] The calculation process of the IBOA algorithm is as follows:

[0114] (1) Initialization parameters: population size N, maximum number of iterations T, variable dimension m, boundary wait.

[0115] (2) Initialize the population ,in , This indicates the generation of random numbers in the range [0,1].

[0116] (3) Calculate the fitness of the initial population And record the globally optimal individual.

[0117] (4) Calculate the adaptive factor α

[0118] ;

[0119] Where D represents the diversity of the current population, calculated as follows:

[0120] ;

[0121] in Adaptive factor boundary, Population diversity threshold.

[0122] (5) In the first phase, the exploration is enhanced by an adaptive factor α, and the exploration update is performed with probability p1.

[0123] ;

[0124] in An integer randomly selected from the set {1,2}.

[0125] (6) In the second stage, the step size is adjusted using an adaptive factor (1-α) and an intermediate adjustment is performed with probability p2. In this stage, the search is guided by the current global best individual.

[0126] ;

[0127] in A random number in the range [0,1] .

[0128] (7) In the third stage, (1-α) is used to enhance the utilization, and the utilization update is performed with probability p3.

[0129] ;

[0130] Where is a random number in the range [0,1]. This represents the current iteration number. Pi is a constant.

[0131] (8) Check the boundary and update the fitness and global optimum.

[0132] (9) Output the global optimal solution .

[0133] S4, calculate the Kalman gain based on the statistics and the adjusted sensitivity weight matrix;

[0134] Further, in step S4, the step of calculating the Kalman gain includes: calculating the Kalman gain by minimizing the corresponding weak sensitivity cost function based on the prior state statistics and the cross-covariance between the state and the measurement, combined with the adjusted sensitivity weight matrix.

[0135] Specifically, the weak sensitivity cost function is:

[0136]

[0137] By taking the partial derivative of the cost function and setting it to zero, we obtain the Kalman gain:

[0138] ;

[0139] S5. Using Kalman gain, calculate the posterior sensitivity variance matrix and the posterior sensitivity transfer matrix.

[0140] Further, in step S5, the steps of calculating the posterior sensitivity variance matrix and the posterior sensitivity transfer matrix include: updating the sensitivity information of the state estimation error based on the Kalman gain; calculating the corresponding posterior sensitivity variance matrix; and calculating the posterior sensitivity transfer matrix to characterize the influence relationship of uncertain parameters on the state estimation.

[0141] Specifically, the corresponding posterior sensitivity variance matrix and sensitivity transfer matrix are calculated.

[0142] ;

[0143] ;

[0144] S6, based on Kalman gain and measurement information, calculates the posterior state estimate and posterior estimation error variance matrix of the UAV, and outputs the state results for UAV autonomous navigation.

[0145] Furthermore, in step S6, the step of outputting the state result for the UAV's autonomous navigation includes: updating the state estimate of the UAV based on the Kalman gain and measurement information; calculating the corresponding posterior estimation error variance matrix; and using the posterior state estimate as the state output result for the UAV's autonomous navigation.

[0146] Specifically, the posterior state estimate and the posterior estimation error variance matrix are calculated.

[0147] ;

[0148] ;

[0149] Example 2:

[0150] This embodiment provides a specific application case to verify the technical effect of the method of the present invention.

[0151] For ease of engineering implementation and technical exchange, this embodiment refers to the core filtering process as OFDEnKF.

[0152] (Optimized Fast Desensitized Ensemble Kalman Filter) algorithm.

[0153] This invention redefines the state error sensitivity matrix and constructs a weakly sensitive cost function within the ensemble Kalman filtering framework. The optimal gain is obtained by minimizing this cost function, effectively reducing the negative impact of model uncertainty on filtering performance. Specifically, this invention further improves the accuracy and robustness of UAV state estimation by introducing a sensitivity factor and utilizing an intelligent optimization algorithm for online adjustment. The specific implementation steps are as follows:

[0154] Step 1: Analyze the impact of unknown parameters on navigation status during UAV flight.

[0155] Drone flight is affected by a variety of interference factors, such as systematic errors and random errors of sensors, uncertainties in model parameters, etc. These uncertainties seriously affect the navigation accuracy of drone navigation systems and cannot guarantee the flight accuracy and safety of drones.

[0156] By analyzing the uncertainties in the parameters of the UAV model, three parameters with significant impact were identified. Based on experience, their calibration values ​​are as follows: , and The corresponding variances are 1.141, 1.841 and 0.701, respectively.

[0157] Step 2: Establish the state equations and measurement equations for the UAV flight model.

[0158] Select the speed of the drone ( , and ), attitude angular velocity ( , and ) and attitude angle ( , and Using as state variables, construct a discrete dynamics system for UAV flight:

[0159] ;

[0160] ;

[0161] In the formula, and These are the system's state vector and measurement vector, respectively. and These are the system's state transition matrix and observation matrix, respectively. This is an uncertain parameter vector. and The process noise of the system (variance is...) ) and measurement noise (variance is ), and satisfy , Let be the control vector, where This is a constant matrix with the following values:

[0162] ;

[0163] Step 3: Specific implementation of the method of the present invention and output of UAV flight state estimation

[0164] The specific implementation steps of the method of the present invention are as follows:

[0165] S101: Initialize the UAV flight model

[0166] The initial parameters of the drone flight model are:

[0167] ;

[0168] ;

[0169] Initial state estimation error variance matrix The initial state value and the estimated state value are as follows:

[0170] ;

[0171] ;

[0172] The initial value of the sensitivity matrix of the state estimation error is Since sufficient state information has not yet been accumulated, it is usually set to The sensitivity matrix is ​​initialized to zero or based on the nominal values ​​of the model parameters. In this embodiment, the initial value of the sensitivity matrix is ​​set to... (A 9×3 zero matrix, corresponding to 9 state variables and 3 uncertain parameters), this matrix will be gradually updated through a sensitivity propagation mechanism during the filtering iteration process. The estimated value of the system at time t is The error variance is The sensitivity matrix is .

[0173] Scheme 1 (the method of this invention, referred to as OFDEnKF in engineering implementation): Employing a weakly sensitive ensemble Kalman filter with dynamically adjusted sensitivity factors. In this embodiment, the optimal sensitivity weight parameters obtained through online optimization using an intelligent optimization algorithm (the IBOA algorithm is used in this embodiment) are: , and These correspond to the sensitivity weights of the three uncertain parameters (Jx, Jy, Jz) to the state estimation. During the filtering iteration process, these weights are dynamically adjusted through the sensitivity factor β (β is obtained online by minimizing the performance index function), thereby achieving adaptive adjustment of the sensitivity weight matrix.

[0174] Option 2 (comparison method, denoted as DEnKF): Employs a weakly sensitive ensemble Kalman filter with fixed weights. For

[0175] For each uncertain parameter, the same fixed weight matrix is ​​used (set empirically, without an online optimization process):

[0176] ;

[0177] Each element of the diagonal matrix corresponds to a fixed weight of 9 state variables (u, v, w, p, q, r, φ, θ, ψ), which remains unchanged throughout the filtering process.

[0178] Through the above comparison, it can be verified that the "online optimization mechanism for sensitivity factors" introduced in this invention is superior to traditional fixed-function mechanisms.

[0179] The significant effect of the fixed weight method on improving navigation accuracy.

[0180] S102: Based on the state of the UAV during flight, obtain the set point of the state, the prior state estimate, and the prior error variance.

[0181] Acquire drones Meeting point of steps:

[0182] ;

[0183] The superscript "+" indicates its posterior estimate. The error variance matrix The square root of the first Column, satisfying , The number of sampling points;

[0184] The rendezvous point after function passing is:

[0185] ;

[0186] Then its corresponding prior state estimate:

[0187] ;

[0188] ;

[0189] Wherein, the superscript "-" indicates a prior estimate of the variable;

[0190] Then the state variance Measurement variance and their mutual covariance They are respectively

[0191] ;

[0192] ;

[0193] ;

[0194] in, This is the state error matrix, which represents the state values ​​at each set point. and average The difference between them is defined as:

[0195] ;

[0196] The above applies to uncertain parameters The corresponding sensitivity matrix and transfer equation are as follows:

[0197] First, calculate Sensitivity of step set points:

[0198] ;

[0199] in, The number of uncertain parameters;

[0200] Secondly, the sensitivity of the propagation rendezvous point:

[0201] ;

[0202] Then, the sensitivity of the prior states is calculated separately:

[0203] ;

[0204] ;

[0205] S103: Based on the set performance indicators, the sensitivity factors of the sensitivity weight matrix are obtained using the IBOA optimization strategy, and then the sensitivity weight matrix is ​​automatically adjusted.

[0206] The fitness function used in the optimization algorithm is:

[0207] ;

[0208] ;

[0209] In the formula Sensitivity factor feasible domain, For matrix The Line number Column elements, The residual matrix Forgetting factor Values .

[0210] The calculation process of the IBOA algorithm:

[0211] (1) Initialization parameters: Population size N=30 (determined based on the search space dimension of the sensitivity factor and the balance of computational resources), maximum number of iterations T=10 (balancing optimization accuracy and real-time requirements, ensuring optimization is completed within one filtering cycle), variable dimension m=1 (the sensitivity factor is a scalar), boundary (The value range of the sensitivity weight matrix and the requirements for filtering stability are determined. If the value is too small, the weak sensitivity effect will be insufficient, and if the value is too large, the filtering gain will be degraded.)

[0212] (2) Initialize the population ,in , This indicates the generation of random numbers in the range [0,1].

[0213] (3) Calculate the fitness of the initial population And record the globally optimal individual.

[0214] (4) Calculate the adaptive factor α

[0215] ;

[0216] Where D represents the diversity of the current population, calculated as follows:

[0217] ;

[0218] Where the adaptive factor boundary Population diversity threshold .

[0219] (5) In the first phase, the exploration is enhanced by an adaptive factor α, and the exploration update is performed with probability p1.

[0220] ;

[0221] in An integer randomly selected from the set {1,2}.

[0222] (6) In the second stage, the step size is adjusted using an adaptive factor (1-α) and an intermediate adjustment is performed with probability p2. In this stage, the search is guided by the current global best individual.

[0223] ;

[0224] in A random number in the range [0,1] .

[0225] (7) In the third stage, (1-α) is used to enhance the utilization, and the utilization update is performed with probability p3.

[0226] ;

[0227] Where is a random number in the range [0,1]. This represents the current iteration number. Pi is a constant.

[0228] (8) Check the boundary and update the fitness and global optimum.

[0229] (9) Output the global optimal solution .

[0230] S104: Calculate the Kalman gain. Kalman gain after introducing the sensitivity factor. It is obtained by minimizing the cost function:

[0231] ;

[0232] in, Minimizing the above expression yields... :

[0233] ;

[0234] S105: Calculate the posterior sensitivity variance matrix and the sensitivity transfer matrix.

[0235] ;

[0236] ;

[0237] S106: Calculate the posterior state estimate and the posterior state estimate error:

[0238] ;

[0239] ;

[0240] By iterating through steps S102 to S106 above, the state output by the UAV navigation system can be obtained.

[0241] This experimental example is used to verify the technical effect of the method of the present invention. For ease of comparison and analysis, the method of the present invention is referred to as "Scheme 1 (abbreviated as OFDEnKF in engineering implementation)" and the traditional fixed weight method is referred to as "Scheme 2 (abbreviated as DEnKF)".

[0242] The simulation parameters were set as follows: sampling time Ts = 0.02s, sampling frequency fs = 50Hz, simulation duration 200 seconds, and the uncertainty range of the model parameters was ±30% of the nominal value. The RMSE of the UAV flight model's velocity, yaw angle, and yaw angle angular velocity in each direction were calculated. As can be seen from the figure, the RMSE of the UAV state obtained by the proposed OFDEnKF algorithm is less than that obtained by the DEnKF algorithm, meaning that the proposed OFDEnKF algorithm has higher navigation accuracy.

[0243] This invention significantly reduces computational complexity and improves computational efficiency by redefining the sensitivity matrix and deriving an analytical form of the gain matrix within the ensemble Kalman filtering framework. Furthermore, this invention innovatively introduces a sensitivity factor to dynamically adjust the sensitivity weight matrix and utilizes an intelligent optimization algorithm to obtain the optimal sensitivity factor online. This effectively reduces the negative impact of parameter perturbations when there are uncertainties in the UAV navigation model parameters, further enhancing the accuracy and robustness of UAV navigation.

[0244] Finally, it should be noted that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. An autonomous navigation method for unmanned aerial vehicles based on sensitivity factor optimization ensemble Kalman filter, characterized in that, Includes the following steps: S1, Model Establishment: Obtain UAV navigation state variables, establish a discrete state model and measurement model containing uncertain parameters, and determine noise statistical parameters; S2, Set Prediction and Sensitivity Calculation: Generate state set points based on the discrete state model, and obtain prediction set points through state equation propagation; calculate prior state estimates and their statistics based on the prediction set points; For the uncertain parameters, calculate the state sensitivity of each set point, and obtain the sensitivity matrix of the prior state through sensitivity propagation; S3, Adaptive Sensitivity Weight Adjustment: A sensitivity factor is introduced to adjust the sensitivity weight matrix; a performance index function containing filter residual terms and state sensitivity weighting terms is constructed; an intelligent optimization algorithm is used to search for the optimal sensitivity factor online to achieve adaptive adjustment of the sensitivity weight matrix; S4, Calculate the Kalman gain: Introduce the adjusted sensitivity weight matrix into the cost function, and calculate the Kalman gain under the constraint of minimizing the cost function that simultaneously includes the state estimation error and the state sensitivity weighting term; S5, Sensitivity Information Update: Using the Kalman gain, update the sensitivity information of the state estimation error, and calculate the posterior sensitivity variance matrix and the posterior sensitivity transfer matrix. S6, State estimation output: Based on the Kalman gain and measurement information, calculate the posterior state estimate and posterior estimation error variance matrix of the UAV, and output the navigation state result.

2. The method according to claim 1, characterized in that, In step S2, the specific method of sensitivity propagation is as follows: calculate The state sensitivity of each set point to uncertain parameters is calculated using the following formula: in, For the first The meeting point is at Sensitivity of steps For the first The posterior state of each set point A vector of uncertain parameters; The sensitivity is propagated through the state equation, and its calculation formula is as follows: in, For the first The meeting point is at Predictive sensitivity of steps State transition function for Step control input; The prior sensitivity matrix is ​​calculated using the following formula: in, The number of rendezvous points.

3. The method according to claim 1, characterized in that, In step S3, the performance index function is constructed as follows: in, For the filter residual, For k-step measurements, For the k-step prior measurement prediction value, Sensitivity factors This is the sensitivity weight matrix. The prior sensitivity matrix is ​​calculated in step S2. The trace of a matrix, indicated by a superscript This indicates the matrix transpose.

4. The method according to claim 3, characterized in that, The sensitivity weight matrix Determined in the following ways: in, Obtained through intelligent optimization algorithm search These are the initial weighting coefficients for each uncertain parameter. , The number of uncertain parameters. This represents a diagonal matrix.

5. The method according to claim 1, characterized in that, In step S4, the specific form of the cost function is as follows: in, for Step into the real state, for Step state estimate, for Step estimate of error covariance matrix, Represents the mathematical expectation; By taking the partial derivative of the cost function and setting it to zero, the Kalman gain is obtained as follows: in, for Step Kalman gain, The cross-covariance between state and measurement. To measure variance, For the measurement matrix, superscript This represents finding the inverse of a matrix.

6. The method according to claim 1, characterized in that, In step S3, the intelligent optimization algorithm is a population-based adaptive optimization algorithm, which includes the following steps: S3-1, Initialization: Set population size Maximum number of iterations Sensitivity factors Within the search range, an initial population is randomly generated; S3-2, Fitness Assessment: Assessing the sensitivity factors for each individual in the population. Substitute the performance index function from step S3 to calculate the fitness value and record the globally optimal individual; S3-3, Adaptive Factor Calculation: Calculate the adaptive factor based on the current population diversity D. This is used to dynamically adjust the exploration and development ratio; S3-4, Population Update: Execute a three-stage update strategy sequentially. The first stage uses probability... Execute exploratory updates, the second phase is based on probability. The search is guided by the globally optimal individual, and the third stage uses probability. Perform an update using the available resources; S3-5, Determine the termination condition: If the maximum number of iterations is reached or the convergence condition is met, output the optimal sensitivity factor that minimizes the performance index function. Otherwise, return to step S3-2.

7. The method according to claim 1, characterized in that, In step S1, the discrete state model is: Equations of state: Measurement equation: in, for Step state vector, For dependent on uncertain parameters The state transition matrix, To control the input matrix, To control the input, For process noise, For measurement vectors, For the measurement matrix, For measuring noise.

8. The method according to claim 1 or 7, characterized in that, The UAV navigation state variables include three-axis velocity, three-axis attitude angle, and three-axis attitude angular velocity. The uncertain parameters include coefficients in the state transition matrix that are affected by environmental disturbances, mass changes, or aerodynamic coupling.

9. The method according to claim 1, characterized in that, In step S5, the formula for calculating the posterior sensitivity transfer matrix is: in, for Step posterior sensitivity matrix, It is the identity matrix. for Step Kalman gain, For the measurement matrix, for The prior sensitivity matrix is ​​used to characterize the influence of uncertain parameters on the posterior state estimation.

Images