Unmanned aerial vehicle inertial navigation model parameter identification method based on robust adaptive filtering

Abstract

The invention discloses an unmanned aerial vehicle inertial navigation model parameter identification method based on robust adaptive filtering. The method comprises the following steps: 1, establishing an unmanned aerial vehicle motion model and a maneuvering action library; 2, simulating and generating an ideal flight path of the unmanned aerial vehicle; 3, simulating error characteristics of the inertial navigation assembly to generate a gyroscope, and adding inertial navigation assembly output; 4, setting an initial estimation error value of the navigation physical quantity of the inertial navigation system; 5, adding the set navigation error into the reference trajectory to obtain actual navigation data; step 6, setting an initial parameter of a robust adaptive filtering algorithm; and the like. According to the method, identification of the error state quantity of the inertial navigation model of the unmanned aerial vehicle is realized, a software platform supporting modeling simulation and parameter identification of the inertial navigation model of the unmanned aerial vehicle is also established, and inertial navigation model parameter identification with good identification precision, real-time performance and reliability is realized; therefore, the inertial navigation model capable of supporting high-fidelity flight autonomous navigation simulation of the unmanned aerial vehicle is established.

Description

Technical Field

[0001] The invention relates to a method for identifying parameters of an unmanned aerial vehicle inertial navigation model based on robust adaptive filtering, and belongs to the technical field of unmanned aerial vehicle system identification and autonomous navigation. Background Art

[0002] In practical applications, drones are affected by the uncertainty of parameters, structures, and environmental disturbances, and need to acquire their own situation through situation fusion between sensors. Among the sensors that undertake the autonomous navigation function, the inertial navigation system (INS) plays a vital role. The measurement process of INS applied to autonomous navigation of drones does not rely on external information. It relies on inertial elements such as accelerometers (hereinafter referred to as accelerometers) and gyroscopes (hereinafter referred to as gyroscopes) to measure the linear acceleration and angular acceleration of the drone body in the inertial reference system, and obtains the speed, position, attitude and other information of the drone in the navigation coordinate system through time integration and coordinate system transformation.

[0003] The navigation sensor in the UAV usually forms a combined navigation system of inertial navigation and global satellite positioning system (GNSS, hereinafter referred to as satellite). Inertial navigation has the advantages of small size, low cost, low power consumption and comprehensive information acquisition. However, if the inertial navigation is used alone, without any error estimation, correction and compensation, the attitude angle, velocity and position information output by the inertial navigation will produce cumulative errors over time. In order to reduce the error accumulation effect caused by time integration in the inertial navigation, it is usually necessary to carry out system-level calibration and compensation of the inertial navigation, and obtain the error characteristic parameters in the measurement model of the inertial navigation and other sensors through parameter identification and other means, so as to establish an accurate sensor measurement mathematical model.

[0004] In research, in order to reduce the navigation measurement deviation of the inertial sensor device during use, the inertial navigation transfer mechanism is often rotated at the system level to simulate the rotation characteristics of the IMU during flight maneuvers, and the inertial navigation is calibrated by self-alignment and other methods, that is, to obtain the measurement error characteristics of the inertial navigation system and the measurement accuracy of the inertial measurement unit (IMU). In essence, it is to identify the measurement model of the inertial navigation sensor to correct the state information with various measurement errors used in the autonomous navigation process. The self-alignment calibration of the inertial sensor can be completed using methods such as the least squares method, the Kalman filter method, or the frequency domain identification method. It is necessary to set the calibration transfer sequence and perform the solution and error parameter identification based on the data collected by the turntable test.

[0005] Most existing studies use some nonlinear filtering algorithms derived from Kalman filtering to reduce the navigation error of the integrated navigation system by estimating the system error and measurement error covariance, but do not provide the estimation results of the measurement error characteristic parameters of sensors such as inertial navigation. Therefore, although these methods can be used as combined navigation algorithms for inertial navigation and satellite sensors in actual navigation systems, they cannot provide the mathematical model of sensors used in UAV flight simulation, which is not conducive to the construction of simulation models before actual experiments. The method discussed in this paper is a Kalman filtering derivative algorithm based on adaptive theory. It can identify multiple error measurement parameters of IMU and integrated navigation systems while being used in the combined navigation system formed by inertial navigation and satellites, so as to establish a mathematical model of sensors and support high-fidelity UAV flight simulation.

[0006] Therefore, how to design a UAV inertial navigation model parameter identification method with adaptive estimation performance is an urgent problem to be solved.

[0007] If the prior information of the error characteristics of the UAV navigation sensor is known, the principle of the turntable can be used to design the UAV's maneuverable flight movements and sample the flight data, so as to obtain the experimental data samples required to drive model identification, activate the measurement characteristics of each degree of freedom of the sensor during the navigation process, and perform damping inversion on the inertial navigation solution process. The robust adaptive Kalman filter algorithm (RAKF) is designed to complete the a posteriori estimation of the sensor measurement error characteristics and realize the online identification of the inertial navigation model parameters. Summary of the invention

[0008] The present invention provides a method for identifying parameters of an unmanned aerial vehicle inertial navigation model based on robust adaptive filtering. The purpose of the method is to design a method for identifying parameters of an unmanned aerial vehicle inertial navigation model with adaptive estimation performance. Under the input of the unmanned aerial vehicle maneuvering flight trajectory, the method realizes the identification of a total of 28 state quantities of the unmanned aerial vehicle inertial navigation model based on a robust adaptive filtering algorithm, builds a software platform supporting modeling simulation and parameter identification of the unmanned aerial vehicle inertial navigation model, realizes the identification of inertial navigation model parameters with good identification accuracy, real-time performance and reliability, and establishes an inertial navigation model that can support high-fidelity flight autonomous navigation simulation of the unmanned aerial vehicle.

[0009] Aiming at the problem of parameter identification of unmanned aerial vehicle inertial navigation model, the present invention invents a method for identifying the parameters of unmanned aerial vehicle inertial navigation model based on robust adaptive filtering, aiming to realize the identification of 28 inertial navigation model error parameters. Firstly, the error characteristic equation of IMU is established, and then the error propagation differential equation of inertial navigation attitude, velocity and position is derived. The inertial navigation measurement information is input as the state information, and the GNSS navigation information is input as the measurement information. Based on the Kalman filtering principle and combined with adaptive theoretical mechanisms such as variational Bayes, it is applied to inertial navigation and satellite integrated navigation systems, and an inertial navigation model parameter identification framework based on RAKF estimation algorithm is established to improve the identification accuracy of the unmanned aerial vehicle inertial navigation model parameters.

[0010] The UAV inertial navigation model parameter identification method based on robust adaptive filtering designed by the present invention is as follows Figure 1 As shown in the figure, it is an identification algorithm framework designed for UAV inertial navigation. The error characteristics such as the platform misalignment angle of the inertial navigation model, the speed and position navigation errors of the inertial navigation sensor measurement values ​​relative to the actual flight state values, the instrument installation error of IMU (hereinafter referred to as: installation error), the scale factor error (hereinafter referred to as: scale error) and the zero drift error (hereinafter referred to as: zero bias error) are taken as system state quantities, and the speed and position navigation information deviations output by two different sensors, the inertial navigation and the satellite, are taken as system observation quantities. The variational Bayesian (VB) filter is used to estimate the measurement noise covariance matrix (MNCM), and the multiple fading factors (MFF) are used to construct the strong tracking filter (STF). The Suga-Husa filtering mechanism based on the weighted sliding window is used to adaptively adjust the system noise covariance matrix (PNCM), and the design of the inertial navigation model parameter identification algorithm based on RAKF is completed.

[0011] Based on the proposed parameter identification method, the present invention also designs a software functional architecture of a simulation software platform that supports the identification of UAV inertial navigation model parameters. The platform is developed based on the Matlab GUI tool and has the functions of generating UAV motion trajectories, identifying inertial navigation model parameters, storing results, and replaying and displaying data. Its framework architecture, module composition, and operation method are as follows: Figure 2 shown.

[0012] According to the functional modules, the simulation platform consists of several modules, including model parameter and input setting module, UAV motion trajectory generation module, identification algorithm setting module, simulation and result storage module, data playback and display module, etc. The functions and usage instructions of each sub-module are as follows:

[0013] 1) Model parameter and input setting module, including trajectory generation setting and inertial navigation model parameter setting. The former is used by the user to specify the maneuvering action from the UAV maneuvering action library, and the maneuvering action sequence and the duration of each action can be freely arranged to form a trajectory script file; the latter is used to set the IMU error characteristic parameters and the navigation system error characteristic parameters. The specific error types included can be referred to Figure 2 .

[0014] 2) The UAV motion trajectory generation module selects and loads the written trajectory script file. The software platform can combine the maneuvering actions and durations set in the file with the actions in the maneuvering action library in sequence and duration, and generate a UAV motion trajectory and load it into the Matlab workspace.

[0015] 3) Identification algorithm setting module, the user can select one of the three methods from the classic Kalman filter (KF), the classic variational Bayesian adaptive Kalman filter (VBAKF) and the robust adaptive filter (RAKF) proposed in the present invention, and set the filter algorithm parameters and navigation error prior knowledge parameters, which are specifically described in detail in the following steps.

[0016] 4) Simulation and result storage module. After completing the identification algorithm setting, the algorithm can be run to identify the UAV inertial navigation model. After the simulation is completed, the results of the identification algorithm are displayed in the form of images and text. If the results satisfy the user, the results can be stored as a mat file, which can be named and stored in the specified path. The software platform supports the storage of UAV motion trajectories and identification results.

[0017] 5) Data playback and display module, which supports drawing images such as the measurement output of the inertial navigation model for the UAV motion trajectory, the inertial navigation model identification results, etc., and can load the stored results to complete data playback; this module is also equipped with an operation log recording module, which records the corresponding operation logs when the user uses the interactive interface of the simulation platform to perform operations including parameter setting, data loading, parameter identification simulation and data saving. The user can trace back from the operation log to view all the operations he has performed, as well as view the specific parameters set for a simulation and the path location of the data after saving the results.

[0018] The above modules of the algorithm simulation platform are used to realize functions such as inertial navigation model parameter setting, UAV motion trajectory generation, inertial navigation model parameter identification, result output drawing and storage, data playback and display, etc.

[0019] The present invention proposes a method for identifying parameters of an unmanned aerial vehicle inertial navigation model based on robust adaptive filtering. The specific implementation steps are as follows:

[0020] Step 1: Create a drone motion model and maneuver library

[0021] It includes establishing a UAV maneuver library, a UAV motion model, and an IMU sensor measurement model that includes a gyroscope and an adder measurement model.

[0022] <Sub-step 1>: Establishing the UAV motion model:

[0023] The UAV motion model includes linear motion equations and angular motion equations. The linear motion equation integrates the acceleration vector in the body coordinate system in the form of a nonlinear differential equation to obtain the "east-north-sky" velocity v in the navigation coordinate system. n and position p n The angular motion equations are calculated by integrating the attitude angular rate to obtain the Euler angles. The dimensions are all three-dimensional, which can be specifically expressed as:

[0024]

[0025] Where w is the attitude angular velocity, is the linear acceleration of the body coordinate system, is the attitude angle, v n and p n are the speed and position of the UAV under the navigation system, M is the attitude rotation matrix of the drone body coordinate system rotating to the navigation coordinate system. pv is the position error coefficient matrix, which can be expressed as:

[0026]

[0027] Where L is latitude, R N and R M is the intermediate variable involved in the calculation. For the specific solution method, please refer to formula (8).

[0028] use and f b Respectively represent the time interval [t k-1 ,t k The body attitude angular rate and specific force (linear acceleration) within the range of ] are the derivatives of the ideal output of the IMU and can be calculated by formula (1) when the aircraft is in motion. Through coordinate transformation, the time interval [t m-1 ,t m ] The angular increment of the inner body Δθ m and velocity increment Δv m :

[0029]

[0030] <Sub-step 2>: Establishing the UAV maneuvering action library:

[0031] The underlying maneuvers involved in the drone maneuver library include the most basic movements in the six-degree-of-freedom aircraft movements, including 13 different maneuvers, including constant-speed level flight, accelerated level flight, decelerated level flight, head up, head down, climb, glide, left yaw, right yaw, left roll, right roll, left coordinated turn, right coordinated turn, etc.

[0032] Step 2: Generate UAV motion trajectory based on the maneuver library

[0033] Specifically: treat the drone as a rigid body with translational and rotational behaviors, ignore its nonlinear aerodynamic characteristics and the motion coupling characteristics of the longitudinal and lateral channels, and ignore the interference caused by gusts and atmospheric turbulence in the environment, and only consider relatively ideal conditions. According to the drone maneuvering action library established in step 1, select the movement actions in a reasonable way and set the appropriate acceleration Angular rate w and motion duration kT s Based on the linear motion and angular motion model of the UAV given in step 1, the fourth-order Runge-Kutta (4-RK) method is used to update the differential equations in formula (1). Integrate and convert to the navigation coordinate system to obtain the linear velocity and position in the "east-north-sky" direction, and integrate w to obtain the attitude angle This generates the flight trajectory of the drone and the speed and other states within the corresponding time.

[0034] Step 3: Simulate the gyro output according to the error characteristics of the inertial measurement unit

[0035] Establish an IMU sensor measurement model, including the sensor measurement error model of the gyro and the meter. The error model established for the gyro includes the zero bias error w g , random walk error ε r , scale error Installation error etc., where δK GThe diagonal elements are the scale factors of each rotation axis of the IMU, and the other elements are all 0. However, δG is not 0 except for the diagonal, and the diagonal elements are 0. The error model of the accelerator is consistent with that of the gyroscope. The random walk error, zero bias error, scale error, and installation error involved in the accelerator are represented by symbols as follows: w a ,δK A ,δA. After setting the error parameters of the gyro and the accelerometer and substituting them into the established IMU sensor measurement model, the acceleration specific force f in the body coordinate system is generated based on the UAV motion trajectory generated by the maneuvering action library in step 2 b and the angular rate of rotation The above measurement error is added to the attitude angular rate output by the gyro and the accelerator. With the comparative term f b The error model is as follows:

[0036]

[0037] Then the true output of the IMU sensor measurement model can be recorded as:

[0038]

[0039] The random walk error ε in formulas (4) and (5) r and Modeled as a first-order Markov process:

[0040]

[0041] In formula (6) and (7), T g and T a are the random walk time constants of the gyroscope and the accelerator, ε b is the white noise error of the gyro, ε b It is the sum of the gyro's bias error and random walk error.

[0042] Step 4: Set the initial navigation error estimate of the inertial navigation model

[0043] Set the inertial navigation sensor to measure the attitude angle Φ relative to the ideal flight state. nb , speed v n , position p n The initial estimated error is the platform misalignment angle Speed ​​error and position error The initial estimation error of the navigation state quantity will be used as the adjustable initial parameter of the RAKF algorithm proposed in the present invention. Adjusting the initial estimation error can directly affect the convergence speed and identification accuracy of the RAKF algorithm, and affect the posterior estimation value of PNCM in the subsequent steps. and the MNCM posterior estimate The solution of , to a certain extent, affects the parameter identification results.

[0044] Step 5: Establish the navigation error model of inertial navigation and satellite, establish the time synchronization error model of inertial navigation and satellite combined navigation, establish the space arm effect error model between inertial navigation and satellite navigation receivers, and obtain the sensor navigation information output of inertial navigation and satellite.

[0045] First, some coordinate transformation variables involved in the INS update are calculated based on some geographic parameters and the current actual flight status of the UAV. Second, the INS error model is established using the mathematical characteristics of the sensor of the INS system. The INS error model is based on the UAV motion trajectory simulated in step 2 and the INS initial navigation error estimate set in step 4. and According to the inertial navigation error model, the sensor measurement output of the inertial navigation is updated. v n +δv n and p n +δp n Finally, a satellite navigation error model is established, and the satellite sensor measurement error is superimposed on the UAV motion trajectory. The inertial navigation and satellite combined navigation time synchronization error model is further established, as well as the space arm effect error model between the inertial navigation and the satellite receiver antenna. After superimposing the measurement errors of these two sensors, the speed output by the satellite sensor is obtained. and location Navigation data. The specific process is as follows:

[0046] <Sub-step 1>: Based on the basic geographic parameters of the earth and the current real flight status information of the drone, calculate some variables in the earth coordinate system and the angular rate of rotation between the coordinate systems:

[0047]

[0048]

[0049] Formula (8) gives the expressions of various calculation variables in the Earth system, where f represents the Earth's flattening, e represents the Earth's equivalent elliptical focal length, and R e is the semi-major axis of the earth's equivalent ellipse, L is the latitude, R N and R M is the intermediate variable involved in the calculation; Formula (9) gives the rotation angular rate between the coordinate systems related to the rotation of the earth. The variables involved in the calculation are, ω ie is the Earth's rotation angular rate, h is the altitude of the drone, and is the north and east navigation coordinate system speed; the solution of formula (9) is Each variable represents the angular rate of rotation of the earth, the angular rate of rotation of the navigation coordinate system, the angular rate of revolution of the navigation coordinate system, and the angular rate of rotation of the body coordinate system around the navigation coordinate system; It is the attitude rotation matrix of the UAV body coordinate system to the navigation coordinate system, and its transpose is the rotation matrix from the navigation coordinate system to the body coordinate system.

[0050] <Sub-step 2>: According to the initial platform misalignment angle set in step 4 Initial speed error And the initial position error According to the INS navigation error model, the 4-RK method is used to calculate the INS navigation error output. The established INS navigation error model is:

[0051]

[0052] In formula (10) and As defined above, is the gyro attitude angular rate measurement error obtained by formula (4), is the angular rate error of the navigation coordinate system; in formula (11), f n is the specific force f measured by addition b The result of the transformation to the navigation coordinate system; the state quantity in formula (12) can be combined into the position vector δp n =[δLδλδh] T The navigation error of the inertial navigation model is updated by formula (8) to formula (12), and the actual output of the inertial navigation can be obtained by superimposing it with the motion trajectory of the UAV.

[0053] <Sub-step 3>: Establish a satellite navigation error model. First, establish the time synchronization error model of the inertial navigation and satellite integrated navigation system, then consider the arm effect error between the inertial navigation and the satellite receiver antenna, establish a spatial arm effect error model, and add data transmission time delay and first-order Markov process. The final satellite model is given as follows:

[0054]

[0055] In formula (13), and Represents the sensor measurement output v of the inertial navigation n +δv n and p n +δp n , a n represents the linear acceleration of the UAV in the navigation coordinate system, δT d is the time synchronization error; in formula (14), Indicates the rotation angular rate of the space arm coordinate system relative to the drone body coordinate system, δr b is the spatial arm effect error; τ in formula (15) v , τ p and T s are the velocity and position time constants and the sampling time interval for the slow loop update of the measurement equation, respectively. V1 and V2 represent the white noise of the satellite receiver velocity and position measurement, respectively.

[0056] Step 6: Set the initial parameters of the robust adaptive filtering algorithm

[0057] The RAKF algorithm designed by the present invention is an adaptive identification algorithm based on the Kalman filter framework. The initial parameters of the algorithm include: the parameters of the classic Kalman filter algorithm, including the initial value of PNCM MNCM initial value Initial value of the Predicted Error Covariance Matrix (PECM) The system state vector dimension n and the measurement vector dimension m, etc.; the parameters of the sliding window Sage-Husa filter (SWF), including the sliding window length N w , adaptive control operator b a ; Strong Tracking Filter (STF) parameters, including the forgetting factor ρ a ; Variational Bayesian Adaptive KalmanFilter (VBAKF) adaptive parameters, including the number of variational iterations N vb , variational control factor ζ vb , shape parameter α of the inverse Gamma distribution k , the scale parameter of the inverse Gamma distribution β k The RAKF algorithm introduces VBAKF to estimate MNCM and PECM, uses multiple fading factors (MFF) to establish STF, and introduces SWF to estimate PNCM.

[0058] Step 7: Iteratively update the state equation fast loop with a shorter sampling time and the measurement equation slow loop with a longer sampling time to obtain the a posteriori estimate of the system state vector as the inertial navigation model parameter identification result

[0059] Update the state equation fast loop with shorter sampling time (hereinafter referred to as: fast loop) and the measurement equation slow loop with longer sampling time (hereinafter referred to as: slow loop); load the sensor measurement output of the inertial navigation and satellite calculated in step 5, calculate the deviation and establish the new information sequence, use SWF to give the adaptive estimate of the new information sequence, use the new information sequence estimate to construct the multiple fading factor matrix, and use STF to obtain the one-step state transfer matrix; use VBAKF to update the posterior estimate of PECM and MNCM, and finally output the posterior estimate of the system state vector as the parameter identification result of the inertial navigation model. The specific process is as follows:

[0060] <Sub-step 1>: Initialize the a posteriori estimates of the system state variables to all zero vectors According to the initial value setting of the estimated error of the inertial navigation and satellite given in step 4, set the initial estimated value of PNCM and the initial estimate of MNCM for:

[0061]

[0062] Among them, σ(w g ) and σ(w a ) are the three-dimensional covariances of the initially given gyro and the added white noise, 0 28×1 is a 28-dimensional all-zero column vector, and They are respectively the initial values ​​of the deviation of the velocity and position of the navigation coordinate system measured by the inertial navigation and satellite in the initially given measurement equations.

[0063] Note that in the present invention, if the subscript is in the form of k|k-1, it represents a priori estimator, and if the subscript is written in the form of k|k, it represents a posterior estimator.

[0064] <Sub-step 2>: Based on the initial values ​​of the inertial navigation measurement output and the geographic coordinate parameters calculated by formula (8) and formula (9), combined with the state equation and its discretized form x k =F k-1 x k-1 , take the value of k = 1 time step to calculate the initial value F0 of the discrete state coefficient matrix, where x k is the system state vector, which can be written as:

[0065]

[0066] in They represent the scale error and installation error matrix of the gyro and gage, respectively.

[0067] Preset variational Bayes estimation parameters ρ, ψ0, α 0|0 ,β 0|0, and its initial value is set as follows: α 0|0 =[1,...,1] T , Initialize PECM to Initialize Kalman filter gains Set the state equation update loop as a fast loop with a sampling update frequency of 10Hz; set the measurement equation update loop as a slow loop with a sampling update frequency of 1Hz. According to the RAKF parameters set in step 6, traverse all sampling time periods corresponding to the UAV motion trajectory for iterative updates.

[0068] State equation fast loop update: specifically the following sub-steps 3-5

[0069] <Sub-step 3>: Calculate the sensor measurement values ​​of the gyro and the accelerometer using formulas (3) to (7), further calculate the sensor measurement output of the inertial navigation system according to formulas (8) to (12), and then calculate the state coefficient matrix F based on the inertial navigation system measurement values ​​and the attitude angular rate and rotation matrix obtained by formulas (8) and (9). k , the one-step state transfer matrix is ​​obtained by approximate calculation using the Taylor expansion formula

[0070] <Sub-step 4>: Based on the Bayesian posterior estimate m of the system state vector obtained in the previous iteration k-1k-1 , calculate the system state vector x k A priori estimate of :

[0071] m k|k-1 =Φ k|k-1 m k-1|k-1 (16)

[0072] The measurement vector z obtained by formula (24) at the sampling time is k , calculate the measurement equation coefficient matrix H according to the system state vector k , and then the calculation of the new information sequence is obtained:

[0073]

[0074] Calculate the L2 norm of the new information sequence and introduce the forgetting factor as the weight to update the PNCM:

[0075]

[0076] Further, according to the one-step state transfer matrix Φ calculated in this round of iteration k|k-1 PNCM And the PECM posterior estimate P obtained in the previous round k-1|k-1 The a priori estimate of PECM is calculated as:

[0077]

[0078] <Sub-step 5>: Using variational Bayesian estimation theory, the system state and The joint posterior estimate of is expanded into the product of independent Gaussian distribution and inverse Gamma distribution through fractional order calculation:

[0079]

[0080] The system state vector m k|k-1 and PECMP k|k-1 The prior estimate of is given by the Gaussian distribution, and the prior estimate of MNCM is given by the shape factor α of the inverse Gamma distribution. k|k-1,j and the inverse scaling factor β k|k-1,j Given.

[0081] Find the a priori estimated parameters of MNCM, that is, the two scale factors of the inverse Gamma distribution, using a Bayesian estimation coefficient ρ j (j=1,2,...,m) to calculate its prior estimate:

[0082] α k|k-1,j =ρ j α k-1|k-1,j j=1,2,…,m (22)

[0083] β k|k-1,j =ρ j β k-1|k-1,j j=1,2,…,m (23)

[0084] Slow loop update of measurement equation: specifically, the following sub-steps 6-10;

[0085] <Sub-step 6>: If the slow loop sampling time point is reached, the satellite sensor measurement output is calculated according to formulas (13) to (15), that is, the satellite speed measured in the navigation coordinate system and location Combined with the inertial navigation sensor measurement output calculated by formulas (10) to (12), the navigation speed v measured by the UAV inertial navigation is n +δv n And the navigation position p measured by inertial navigation n +δp n , calculate its deviation to obtain the measurement vector z in the slow loop k , which can be expressed as:

[0086]

[0087] <Sub-step 7>: Prior estimation of PECM P k|k-1In this step, a strong tracking filter with a fading factor matrix is ​​used to update. Using the new information sequence obtained by equation (18), a weighted sliding window is used to adaptively estimate the new information sequence, which is expressed as follows:

[0088]

[0089] The innovation term constructs multiple fading factor matrices, which gives the algorithm strong tracking properties:

[0090]

[0091] Where χ represents the forgetting factor, which takes values ​​in the interval [0.95, 0.99].

[0092] Apply the STF method for suboptimal estimation to calculate the one-step state transfer matrix ψ with strong tracking characteristics k , first calculate the multiple fading factor matrix, and then obtain ψ by weighting it k :

[0093]

[0094] ψ k =diag{ψ k,11 ,ψ k,22 ,...,ψ k,nn} (31)

[0095] Where diag{·} is the matrix diagonalization operator, tr[·] represents the matrix trace operation, μ k represents the desired multiple vanishing factors, is the weight coefficient that needs to be set, which is usually set to no less than 1. The one-step transfer matrix with strong tracking characteristics obtained by formulas (27)-(31) is used to predict the estimated value of PNCM calculated in formula (19): And the PNCM posterior estimate P obtained in the previous round k-1k-1 , in the slow loop, the fast loop formula (20) can be replaced by the following formula to obtain the prior update value of PECM:

[0096]

[0097] <Sub-step 8>: Initialize the Bayesian posterior estimate and PECM posterior estimate of the system state vector in this iteration to:

[0098]

[0099] Set the number of iterations from n = 0 to n = N VB -1, in the nth round, the shape factor of the inverse Gamma distribution is updated and the inverse scale parameter is initialized as:

[0100]

[0101] In the formula, represents the jth component of the shape factor of the Gamma distribution in the nth iteration, Represents the initial value of the j-th component of the inverse scale factor.

[0102] <Sub-step 9>: The number of iterations is from n=0 to n=N VB -1, the a posteriori estimation calculation method of the system state vector and PECM is given as:

[0103]

[0104] In the above two equations, the posterior estimate of MNCM is The calculation method is:

[0105]

[0106] First, use formula (39) to update the inverse scaling factor β in the nth iteration: k|k-1,j , and then use the variational Bayesian mechanism of formulas (35), (39) and (40) to preliminarily estimate MNCM. Use the sequential filtering of formulas (41) and (42) to prevent MNCM from becoming non-positive definite during the adaptive update process, and finally obtain the posterior estimate of MNCM. Note that the subscript j in the lower right corner of all the formulas represents the jth element of the vector, and the subscript jj represents the jth diagonal value of the matrix. represents the jth component of the inverse scaling factor in the nth iteration, Represents the posterior estimate of MNCM The jth diagonal element of the i-th iteration before the sequential filtering correction is performed. After completing the update of formulas (37)-(42), let the number of variational Bayesian estimation iterations be updated to n=n+1, and return to formula (35) for a new round of iterative calculation until the number of iterations reaches n=N VB .

[0107] <Sub-step 10>: Adaptive estimation of PNCM, PECM and MNCM is completed based on the proposed RAKF through formulas (19), (32) and (40)-(42). VB After the update is completed, the traditional Kalman filter can be used to update the Kalman gain, state vector and posterior estimate of PECM:

[0108]

[0109] P k|k =P k|k-1 -K k H k Pk|k-1 (45)

[0110] Among them, K k , and P k|k They are Kalman gain, system state vector a posteriori estimate, and system PECM a posteriori estimate. After completing the a posteriori estimate update, the time step is set to k = k + 1, and then return to <Sub-step 4> to predict and estimate the next time step. If the time step reaches the maximum time of the UAV motion trajectory, the iteration is stopped, and the output system state vector and PECM a posteriori estimate are:

[0111]

[0112] In the formula, They represent the system state vector and the posterior estimation results of PECM respectively.

[0113] A posteriori estimate of the system state vector Each item represents, Indicates the platform misalignment angle, represents the velocity measurement error of the navigation coordinate system, represents the position measurement error of the navigation coordinate system, represents the gyro bias error, Indicates the addition of zero bias error, represents the spatial arm effect error, The augmented vector representing the time synchronization error of the integrated navigation system composed of inertial navigation and satellite, the gyro installation error and the scale error can be written as:

[0114] in and are the gyro scale errors, and the remaining elements are the gyro installation errors. The augmented vector with the installation error and scale error can be written as in and are the added scale errors, and the remaining elements are the added installation errors.

[0115] The posterior estimate P of PECM k|k Each item represents, is the estimated covariance of the platform misalignment angle, is the velocity estimation error covariance of the navigation coordinate system, is the navigation coordinate system position estimation error covariance, is the gyro bias error estimation error covariance, is the error covariance of the estimated error with added bias error, is the spatial lever-arm effect error estimation covariance, is the estimated covariance of the time synchronization error of the inertial navigation and satellite integrated navigation system, is the augmented covariance matrix of gyro installation error and scale error, is the augmented covariance matrix with the installation error and the scaling error added.

[0116] Step 8: Output, store and display parameter identification results

[0117] By extracting the a posteriori estimation result of the system state vector after step 7, the output value of the identification parameters can be obtained, including the misalignment angle of the inertial navigation platform Gyro zero drift ε b , plus zero drift Spatial arm effect error δr b and time synchronization error δT d , gyro scale error δK G and installation error δG, plus scale error δK A And installation error δA.

[0118] The posterior estimated value of the system state vector output after the inertial navigation model is identified based on the RAKF method can be substituted into the combined navigation model of inertial navigation and satellite, and the sensor measurement values ​​of the flight state such as attitude, speed and position of the UAV motion trajectory in the navigation coordinate system driven by the parameter identification results of the model are output. The speed filtering error δv is obtained by subtracting the measured value from the original data of the actual flight state of the UAV. n And the position filtering error δp n During the time period of traversing the entire motion trajectory, the inertial navigation model identification parameters and navigation errors at each sampling moment are recorded and displayed graphically.

[0119] The present invention establishes a parameter identification framework for an unmanned aerial vehicle inertial navigation model based on a robust adaptive filtering algorithm. The method can take an input of an ideal unmanned aerial vehicle maneuvering flight trajectory as a benchmark, and based on the robust adaptive filtering algorithm, use a weighted sliding window Sage-Husa adaptive filter to give the observation value of a new information sequence composed of measurement vector deviations, and construct a multiple fading factor matrix based on this, use a strong tracking filter to obtain a one-step state transfer matrix, and use variational Bayesian estimation to update and obtain the posterior estimate of the system state prediction error covariance matrix and the measurement error covariance matrix, and finally realize the identification of a total of 28 error state quantities of the unmanned aerial vehicle inertial navigation model. The present invention also builds a software platform that supports modeling, simulation and parameter identification of unmanned aerial vehicle inertial navigation models, and realizes inertial navigation model parameter identification with good identification accuracy, real-time performance and reliability, so as to establish an inertial navigation model that can support high-fidelity flight autonomous navigation simulation of unmanned aerial vehicles. BRIEF DESCRIPTION OF THE DRAWINGS

[0120] Figure 1This is the flow chart of the UAV inertial navigation model parameter identification algorithm based on robust adaptive filtering.

[0121] Figure 2 This is the functional structure diagram of the UAV inertial navigation model parameter identification simulation platform based on robust adaptive filtering.

[0122] Figure 3 It is the comparison between the UAV motion trajectory and the navigation data output of the inertial navigation model obtained by identification. Among them;

[0123] (a) Comparison between the UAV’s 2D plane motion trajectory and the trajectory output by the inertial navigation identification model;

[0124] (b) Comparison of the UAV’s 3D motion trajectory and the trajectory output by the inertial navigation identification model;

[0125] (c) Comparison between the actual speed of the UAV and the speed of the inertial navigation identification model;

[0126] (d) Comparison between the actual motion position of the UAV and the position of the INS identification model;

[0127] Figure 4 is the IMU zero bias error identification curve. Among them;

[0128] (a) Gyro bias error identification curve;

[0129] (b) Add zero bias error identification curve;

[0130] Figure 5 is the IMU installation error identification curve. Among them;

[0131] (a) Gyro installation error identification curve;

[0132] (b) Add installation error identification curve;

[0133] Figure 6 is the IMU scale error identification curve. Where;

[0134] (a) Gyro scale error identification curve;

[0135] (b) Added scale error identification curve;

[0136] Figure 7 is the misalignment angle of the inertial navigation platform and the error identification curve of the inertial and satellite integrated navigation system.

[0137] (a) Misalignment angle identification curve of inertial navigation platform;

[0138] (b) Identification curve of space arm effect error and time synchronization error of integrated navigation system. The numbers and symbols in the figure are explained as follows:

[0139] P N——North position of the navigation coordinate system;

[0140] P E ——East position of the navigation coordinate system;

[0141] h——height of navigation coordinate system;

[0142] V N / V E / V U ——North, east and celestial speeds of the navigation coordinate system;

[0143] ΔL / Δλ / ΔH——Latitude, longitude and altitude distance of the navigation coordinate system;

[0144] ε bx / ε by / ε bz ——Gyro X, Y, Z direction zero bias error;

[0145] ——Add zero bias error in X, Y and Z directions;

[0146] δG ij (i=x,y,z;j=x,y,z;i≠j)——gyro installation error in X,Y and Z directions;

[0147] δA ij (i=x,y,z;j=x,y,z;i≠j)——add installation error in X,Y and Z directions;

[0148] δK Gi (i=x,y,z)——Gyro scale error in X, Y, and Z directions;

[0149] δK Ai (i=x,y,z)——add the scale errors in the X, Y, and Z directions;

[0150] ——North, east and celestial platform misalignment angles;

[0151] r x / r y / r z ——Space arm effect error of inertial navigation and satellite integrated navigation system in X, Y and Z directions;

[0152] δT——time synchronization clock error of inertial navigation and satellite integrated navigation system;

[0153] μg——unit of added zero bias error, 1μg=1×10 -6 g;

[0154] ——Unit of gyro bias error, degree / hour;

[0155] sec——unit of scale error, arc second, 1sec=1"=(1 / 3600)°;

[0156] ppm——installation error unit, parts per million, dimensionless, 1ppm=1×10 -6 ;

[0157] min——unit of platform misalignment angle, arc minute, 1min=1'=(1 / 60)°;

[0158] Φ k|k-1 ——Kalman filter one-step state transfer matrix;

[0159] P k|k-1 ——The state error prior estimate covariance matrix;

[0160] P k|k ——The state error posterior estimate covariance matrix;

[0161] x k|k-1 ,m k|k-1 ——A priori estimate of the system state vector;

[0162] x k|k ,m k|k ——A posteriori estimate of the system state vector. DETAILED DESCRIPTION

[0163] The effectiveness of the UAV inertial navigation model parameter identification method based on robust adaptive filtering proposed in the present invention is verified by a specific simulation example. In this example, the simulation environment is configured with an Intel Core i7-10700 processor, 2.90Ghz main frequency, 16G memory, and the parameter identification algorithm simulation program and software platform can support running on MATLAB 2014a and above.

[0164] Step 1: Establish a UAV motion library, which involves the most basic movements of six-degree-of-freedom aircraft, including 13 different maneuvers, including constant speed level flight, accelerated level flight, decelerated level flight, head up, head down, climb, glide, left yaw, right yaw, left roll, right roll, left coordinated turn, right coordinated turn, etc. Establish a UAV motion model.

[0165] Step 2: Select the action sequence in the maneuver action library, and solve the UAV motion trajectory through the 4th-order Runge-Kutta method based on the constructed UAV linear motion and angular motion model. The UAV motion trajectory constructed in this simulation example is as follows: Figure 3 As shown, Figure 3The solid lines in the two sub-figures (a) and (b) show the aircraft motion trajectory curves generated in this step in the two-dimensional latitude and longitude plane and three-dimensional space respectively. The initial setting of the UAV motion trajectory is as follows: the initial posture is φ=[φ0 θ0 ψ0] T =[0 0 0] T , initial velocity (east-north-sky direction) The initial position (latitude-latitude-altitude) is

[0166] Step 3: Set the error parameters of the gyroscope and the accelerometer, substitute them into the established IMU sensor measurement model, and generate the acceleration specific force f in the body coordinate system based on the UAV motion trajectory generated by the maneuvering action library in step 2 b and the angular rate of rotation Wait for the flight state, and f b and After the ideal flight state quantity is superimposed with the IMU measurement error, the gyro angle increment measurement output and the added velocity increment output can be obtained respectively. The IMU error model parameters are set as follows:

[0167] The gyro bias error is set to w g =[0.05deg / h 0.05deg / h 0.05deg / h] T , the white noise error covariance of the gyro is: The scale error of the gyro is set to:

[0168] The installation error of the gyro is set to: δG xy =10ppm,δG xz =10ppm,δG yz =10ppm,δG yx =10ppm,δG zx =10ppm,δG zy =10ppm.

[0169] The accelerometer bias error is set to: w a =[100μg 100μg 100μg] T , set the white noise error characteristic parameters of the accelerometer to: The scale error of the accelerometer is set as:

[0170] The added installation error is set as:

[0171] δA yx =200ppm, δA zx =200ppm, δA zy= 200ppm. Using the IMU error equation, the measurement errors of the gyroscope and the accelerometer are superimposed on the angular rate and specific force terms of the ideal motion state of the drone to obtain the IMU sensor measurement output.

[0172] Step 4: Set the initial estimate of the inertial navigation model navigation output error: The initial platform misalignment angle is set to:

[0173] The velocity measurement error in the initial navigation coordinate system is:

[0174] The position measurement error in the initial navigation coordinate system is set as: The initial estimation error value is an adjustable parameter, which can directly affect the convergence speed and identification accuracy of the inertial navigation model parameter identification algorithm.

[0175] Step 5: Based on the UAV motion trajectory generated in step 2, according to the established inertial navigation and satellite sensor navigation measurement error models, derive the sensor measurement output with different error characteristics of the two added respectively.

[0176] Step 6: Set the initial parameters of the robust adaptive filtering algorithm. The initial parameters of the algorithm proposed in the present invention are set as follows: initial system noise covariance matrix Initial measurement noise covariance matrix Where σ(ε b )and The error parameters in step 3 have been set, which are the white noise error covariance of the gyro and the gage, and In step 4, the initial values ​​of speed and position navigation errors, the dimension of the system state vector n = 34, and the dimension of the measurement vector m = 6 have also been set; the parameters of the Sage-Husa adaptive filter based on the weighted sliding window are set, including the sliding window length N w =10, adaptive control operator b a = 0.9; strong tracking adaptive filter parameters, including forgetting factor ζ k =0.98; Adaptive filtering parameters based on variational Bayes, including the number of variational iterations N vb =20, variational control factor ρ vb =0.95. Other parameters are set as follows: Gamma distribution shape parameter α 0|0 =[1,...,1] T , the scale parameter of the inverse Gamma distribution State estimation error covariance matrix Can be initialized as:

[0177]

[0178] Step 7-8: Load Figure 3 The UAV motion trajectory and identification algorithm parameter settings are shown in the figure. After running the identification algorithm simulation, the results are as follows: Figures 4 to 7 As shown in the figure, it can be seen that the identification algorithm proposed in the present invention can simultaneously process the identification of multiple parameters in the inertial navigation model, and the designed UAV motion trajectory can meet the identification requirements of most parameters, except for the platform misalignment angle (day direction). Add zero bias error (z axis) Except for the gyro three-axis scaling error, the identification results of other inertial navigation system model parameters are good. The above parameters have certain identification static errors or insufficient tracking speed in the identification process.

Claims

1. A method for identifying parameters of an unmanned aerial vehicle inertial navigation model based on robust adaptive filtering, characterized in that: The steps of this method are as follows: Step 1: Create a drone motion model and maneuver library Specifically: establish a UAV maneuver action library. The UAV maneuver action library involved in step 1 includes the most basic movements of 13 six-degree-of-freedom aircraft movements; establish a UAV motion model, including linear motion equations and angular motion equations; in the linear motion equation, integrate the acceleration vector in the body coordinate system and obtain the velocity v in the navigation coordinate system through coordinate transformation. n and position p n ; The Euler angle is obtained by integrating the attitude angular rate of the diagonal motion equation During the movement of the drone, obtain the angular velocity in its body coordinate system and Comparison b That is, the linear acceleration and the integral to get the angular increment Δθ m and velocity increment Δv m ; Step 2: Generate UAV motion trajectory based on the maneuver library Specifically: According to the drone maneuvering action library established in step 1, select the movement action and set the acceleration Angular rate w and motion duration kT s Parameters, and arrange the maneuvers in an appropriate order, and based on the UAV linear motion and angular motion model given in step 1, Integrate and convert to the navigation coordinate system to obtain the linear velocity and position in the "east-north-sky" direction, and integrate w to obtain the attitude angle Thus, the flight trajectory of the UAV and the speed state in the corresponding time are generated; Step 3: Simulate the gyro output according to the error characteristics of the inertial measurement unit Specifically: establish the IMU sensor measurement model, including the gyro and the accelerometer measurement model; in the gyro's measurement of the attitude angular rate, introduce the zero bias error ε into the model b , random walk error w g , scaling error δK G , installation error δG; the error model of the added error is consistent with that of the gyro, and the added errors of zero bias error, random walk, scale, and installation error are represented by symbols respectively: w a ,δK A ,δA; Set the error parameters of the gyro and the accelerometer, substitute them into the established IMU sensor measurement model, and generate the acceleration specific force f in the body coordinate system based on the UAV motion trajectory generated by the maneuvering action library in step 2 b and the angular velocity f b and After the ideal flight state quantity is superimposed with the IMU measurement error, the gyro angle increment measurement output and the accelerometer velocity increment measurement output can be obtained respectively; Step 4: Set the initial navigation error estimate of the inertial navigation system Specifically: the settings include the inertial navigation output relative to the attitude angle Φ in the ideal flight state nb , speed v n , position p n The initial estimation error is expressed as the platform misalignment angle Speed ​​error δv n 、Position error δp n ; Step 5: Establish the navigation error model of inertial navigation and satellite, establish the time synchronization error model of inertial navigation and satellite integrated navigation, establish the space lever effect error model between inertial navigation and satellite navigation receiver, and obtain the sensor navigation information output of inertial navigation and satellite; Specifically: First, the coordinate transformation variables involved in the INS update are calculated based on the geographic parameters and the current actual flight status of the UAV; second, the INS navigation error model is established using the mathematical characteristics of the INS sensor, based on the UAV motion trajectory simulated in step 2 and the INS initial navigation error estimate set in step 4 and According to the inertial navigation error model, the sensor measurement output of the inertial navigation is updated. v n +δv n and p n +δp n ; Finally, combining the measurement principle of satellite, the navigation error model of satellite is established, and the satellite sensor measurement error is superimposed on the UAV motion trajectory. The inertial navigation and satellite combined navigation time synchronization error model and the space arm effect error model between the inertial navigation and the satellite receiver antenna are further established. After superimposing the measurement errors of these two sensors, the speed output by the satellite sensor is obtained. and location Navigation data; Step 6: Set the initial parameters of the robust adaptive filtering algorithm The initial parameters of the algorithm include: classic Kalman filter algorithm parameters, including the initial system noise covariance matrix PNCM Initial measurement noise covariance matrix MNCM Initial state error covariance matrix PECM The system state vector dimension n and the measurement vector dimension m; the parameters of the sliding window-based Sage-Husa filter SWF, including the sliding window length N w , adaptive control operator b a ; Strong tracking filter STF parameters, including the forgetting factor ρ a ; Variational Bayesian adaptive filter VBAKF adaptive parameters, including the number of variational iterations N vb , variational control factor ζ vb , inverse Gamma distribution shape parameter α k , the scale parameter of the inverse Gamma distribution β k ; The RAKF algorithm introduces VBAKF to estimate MNCM and PECM, uses multiple fading factors MFF to establish STF, and introduces SWF to estimate PNCM; Step 7: Iteratively update the state equation fast loop with a shorter sampling time and the measurement equation slow loop with a longer sampling time to obtain the a posteriori estimate of the system state vector as the parameter identification result of the inertial navigation system. Specifically: update the state equation fast loop with shorter sampling time and the measurement equation slow loop with longer sampling time; load the sensor measurement output of the inertial navigation and satellite calculated in step 5, calculate the deviation and establish the new information sequence, use SWF to give the adaptive estimate of the new information sequence, use it to construct the multiple fading factor matrix, and use STF to obtain the one-step state transfer matrix; use VBAKF to update the posterior estimate of PECM and MNCM; finally, after traversing and completing the update of all sampling time periods of the state equation fast loop and the measurement equation slow loop, finally output the posterior estimate of the system state vector as the parameter identification result of the inertial navigation; Step 8: Output, store and display parameter identification results After completing the iterative update of step seven, output the posterior estimation value of the system state vector as the inertial navigation model identification result, store and plot a comparison curve between the preset error parameter and the parameter identification result, and run the model with the identification result as the inertial navigation error parameter to obtain the sensor navigation measurement value of the integrated navigation system of the inertial navigation and the satellite, and calculate the navigation error of the identification model relative to the actual model; Specifically, by extracting the a posteriori estimation result of the system state vector after step 7, the output value of the identification parameters can be obtained, including the misalignment angle of the inertial navigation platform. Gyro zero drift ε b , plus zero drift Spatial arm effect error δr b and time synchronization error δT d , gyro scale error δK G and installation error δG, plus scale error δK A and installation error δA; The a posteriori estimate of the system state vector output after the inertial navigation model is identified based on the RAKF method is substituted into the combined navigation model of inertial navigation and satellite, and the sensor measurement values ​​of the flight state such as attitude, speed and position of the UAV motion trajectory in the navigation coordinate system are output under the drive of the model parameter identification results. The speed filtering error δv is obtained by subtracting the measured value from the original data of the actual flight state of the UAV. n And the position filtering error δp n , during the time period of traversing the entire motion trajectory, the inertial navigation model identification parameters and navigation errors at each sampling moment are recorded.

2. The method for identifying parameters of an unmanned aerial vehicle inertial navigation model based on robust adaptive filtering according to claim 1 is characterized in that: The step 1 specifically includes the following sub-steps: <Sub-step 1>: Establishing the UAV motion model: The UAV motion model includes linear motion equations and angular motion equations. The linear motion equation integrates the acceleration vector in the body coordinate system in the form of a nonlinear differential equation to obtain the "east-north-sky" velocity v in the navigation coordinate system. n and position p n ; The angular motion equation calculates the Euler angle by integrating the attitude angular rate The dimensions are all three-dimensional, which can be specifically expressed as: Where w is the attitude angular velocity, is the linear acceleration of the machine system, is the attitude angle, v n and p n are the speed and position of the UAV in the navigation coordinate system, M is the attitude rotation matrix of the drone body coordinate system rotating to the navigation coordinate system. pv is the position error coefficient matrix, which can be expressed as: Where L is latitude, R N and R M is the intermediate variable involved in the calculation. For the specific solution method, refer to formula (8); use and f b Respectively represent the time interval [t k-1 ,t k The body attitude angular rate and specific force, i.e., linear acceleration, are the derivatives of the ideal output of the IMU and can be calculated by formula (1) when the aircraft is in motion. Through coordinate transformation, the time interval [t m-1 ,t m ] The angular increment Δθ of the inner body m and velocity increment Δv m : <Sub-step 2>: Establishing the UAV maneuvering action library: The underlying maneuvers involved in the drone maneuver library include the most basic movements in the six-degree-of-freedom aircraft movement, including 13 different maneuvers, including constant speed level flight, accelerated level flight, decelerated level flight, head up, head down, climb, glide, left yaw, right yaw, left roll, right roll, left coordinated turn, and right coordinated turn.

3. The method for identifying parameters of an unmanned aerial vehicle inertial navigation model based on robust adaptive filtering according to claim 1 is characterized in that: The specific process of step three is as follows: Establish the IMU sensor measurement error model, and the gyro error model includes the zero bias error w g 、Random walk error ε r , scale error Installation error etc., where δK G The diagonal elements of are the scale factors of each rotation axis, and the other elements are all 0, while δG is not 0 except for the diagonal, and the diagonal elements are 0; the error model of the addition is consistent with that of the gyro, and the random walk error, zero bias error, scale error, and installation error involved in the addition are represented by symbols as w a ,δK A ,δA; Set the error parameters of the gyroscope and the accelerometer, substitute them into the established IMU sensor measurement model, and generate the acceleration specific force f in the body coordinate system based on the UAV motion trajectory generated by the maneuvering action library in step 2 b and the angular velocity The above measurement error is added to the attitude angular rate output by the gyro and the accelerator. With the comparative term f b The error model is as follows: Then the actual measurement output of IMU can be recorded as: The random walk error ε in formula (4) and (5) r and Modeled as a first-order Markov process: In formula (6) and (7), T g and T a are the random walk time constants of the gyroscope and the accelerator, ε b is the white noise error of the gyro, ε b It is the sum of the gyro's bias error and random walk error.

4. The method for identifying parameters of an unmanned aerial vehicle inertial navigation model based on robust adaptive filtering according to claim 1 is characterized in that: The step 5 specifically includes the following sub-steps: <Sub-step 1>: Based on the basic geographic parameters of the earth and the current real flight status information of the drone, calculate some variables in the earth coordinate system and the angular rate of rotation between the coordinate systems: Formula (8) gives the expressions of various calculation variables in the Earth system, where f represents the Earth's flattening, e represents the Earth's equivalent elliptical focal length, and R e is the semi-major axis of the earth's equivalent ellipse, L is the latitude, R N and R M is the intermediate variable involved in the calculation; Formula (9) gives the rotation angular rate between the coordinate systems related to the rotation of the earth. The variables involved in the calculation are, ω ie is the Earth's rotation angular rate, h is the altitude of the drone, and is the north and east navigation coordinate system speed; the solution of formula (9) is The variables represent the angular rate of the earth's rotation, the angular rate of the navigation coordinate system's rotation, the angular rate of the navigation coordinate system's revolution, and the angular rate of the aircraft system's rotation around the navigation coordinate system. is the attitude rotation matrix of the UAV body coordinate system to the navigation coordinate system, which is transposed to the rotation matrix from the navigation coordinate system to the body coordinate system; <Sub-step 2>: According to the initial platform misalignment angle set in step 4 Initial speed error And the initial position error According to the INS navigation error model, the 4-RK method is used to calculate the INS navigation error output. The established INS navigation error model is: In formula (10) and As defined above, is the gyro attitude angular rate measurement error obtained by formula (4), is the angular rate error of the navigation system; in formula (11), f n is the specific force f measured by addition b The result of the transformation to the navigation coordinate system; the state quantity in formula (12) can be combined into the position vector δp n =[δL δλ δh] T To express; the navigation error of the inertial navigation is updated by formula (8) to formula (12), and the actual output of the inertial navigation is obtained by superimposing it with the motion trajectory of the UAV; <Sub-step 3>: Establish a satellite navigation error model. First, establish the time synchronization error model of the inertial navigation and satellite integrated navigation system, then consider the arm effect error between the inertial navigation and the satellite receiver antenna, establish a spatial arm effect error model, and add data transmission time delay and first-order Markov process. The final satellite model is given as follows: In formula (13), and Represents the sensor measurement output v of the inertial navigation n +δv n and p n +δp n , a n represents the linear acceleration of the UAV in the navigation coordinate system, δT d is the time synchronization error; in formula (14), Indicates the rotation angular rate of the space arm coordinate system relative to the drone body coordinate system, δr b is the spatial arm effect error; τ in formula (15) v , τ p and T s are the velocity and position time constants and the sampling time interval for the slow loop update of the measurement equation, respectively. V1 and V2 represent the white noise of the satellite receiver velocity and position measurement, respectively.

5. The method for identifying parameters of an unmanned aerial vehicle inertial navigation model based on robust adaptive filtering according to claim 1 is characterized in that: The step seven includes the following sub-steps: <Sub-step 1>: Initialize the a posteriori estimates of the system state variables to all zero vectors According to the initial value setting of the estimated error of the inertial navigation and satellite given in step 4, set the initial estimated value of PNCM and the initial estimate of MNCM for: Among them, σ(w g ) and σ(w a ) are the three-dimensional covariances of the initially given gyro and the added white noise, 0 28×1 is a 28-dimensional all-zero column vector, and are the initial deviation values ​​of the velocity and position of the navigation coordinate system measured by the inertial navigation and the satellite in the initially given measurement equations; Wherein, in the present invention, if the subscript is in the form of k|k-1, it indicates a priori estimator, and if the subscript is written in the form of k|k, it indicates a posteriori estimator; <Sub-step 2>: Based on the initial values ​​of the inertial navigation measurement output and the geographic coordinate parameters calculated by formula (8) and formula (9), combined with the state equation and its discretized form x k =F k-1 x k-1 , take the value of k = 1 time step to calculate the initial value F0 of the discrete state coefficient matrix, where x k is the system state vector, which can be written as: in represent the scale and installation error matrices of the gyro and gage, respectively; Preset variational Bayes estimation parameters ρ, ψ0, α 00 ,β 00 , and its initial value is set as follows: α 0|0 =[1,...,1] T , Initialize PECM to Initialize Kalman filter gains Set the sampling update frequency of the state equation fast loop to 10Hz; set the sampling update frequency of the measurement equation slow loop to 1Hz; according to the RAKF parameters set in step 6, traverse all sampling time periods corresponding to the UAV motion trajectory for iterative update; State equation fast loop update: specifically the following sub-steps 3-5 <Sub-step 3>: Calculate the sensor measurement values ​​of the gyro and the accelerator using formulas (3) to (7), further calculate the sensor measurement output of the inertial navigation system according to formulas (8) to (12), and then calculate the state coefficient matrix F based on the inertial navigation system measurement values ​​and the attitude angular rate and rotation matrix obtained by formulas (8) and (9). k , the one-step state transfer matrix is ​​obtained by approximate calculation using the Taylor expansion formula <Sub-step 4>: Based on the Bayesian posterior estimate m of the system state vector obtained in the previous iteration k-1|k-1 , calculate the system state vector x k A priori estimate of : m k|k-1 =Φ k|k-1 m k-1|k-1 (16) The measurement vector z obtained by formula (24) at the sampling time is k , calculate the measurement equation coefficient matrix H according to the system state vector k , and then the calculation of the new information sequence is obtained: Calculate the L2 norm of the new information sequence and introduce the forgetting factor as the weight to update the PNCM: Further, according to the one-step state transfer matrix Φ calculated in this round of iteration k|k-1 PNCM And the PECM posterior estimate P obtained in the previous round k-1|k-1 The a priori estimate of PECM is calculated as: <Sub-step 5>: Using variational Bayesian estimation theory, the system state and The joint posterior estimate of is expanded into the product of independent Gaussian distribution and inverse Gamma distribution through fractional order calculation: The system state vector m kk-1 and PECMP kk-1 The prior estimate of is given by the Gaussian distribution, and the prior estimate of MNCM is given by the shape factor α of the inverse Gamma distribution. kk-1,j and the inverse scaling factor β kk-1,j given; Find the a priori estimated parameters of MNCM, that is, the two scale factors of the inverse Gamma distribution, using a Bayesian estimation coefficient ρ j (j=1,2,...,m) to calculate its prior estimate: a kk-1,j =ρ j a k-1k-1,j j=1,2,...,m (22) b kk-1,j =ρ j b k-1k-1,j j=1,2,...,m (23) Slow loop update of measurement equation: specifically, the following sub-steps 6-10; <Sub-step 6>: If the measurement equation slow loop sampling time point is reached, the satellite sensor measurement output is calculated according to formulas (13) to (15), that is, the satellite speed measured in the navigation coordinate system and location The navigation coordinate system velocity v calculated by the inertial navigation system using formulas (10) to (12) is n +δv n And the navigation coordinate system position p n +δp n , calculate its deviation to obtain the measurement vector z in the slow loop of the measurement equation k , which can be expressed as: <Sub-step 7>: Prior estimation of PECM P kk-1 In this step, a strong tracking filter with a fading factor matrix is ​​used to update; using the new information sequence obtained by equation (18), a weighted sliding window is used to adaptively estimate the new information sequence, which is expressed as follows: The innovation term constructs multiple fading factor matrices, which gives the algorithm strong tracking properties: Where χ represents the forgetting factor, which takes values ​​in the interval [0.95, 0.99]; Apply the STF suboptimal estimation method to calculate the one-step state transfer matrix ψ with strong tracking characteristics k , first calculate the multiple fading factor matrix, and then obtain ψ by weighting it k : ψ k =diag{ψ k,11 ,ψ k,22 ,...,ψ k,nn } (31) Where diag{·} is the matrix diagonalization operator, tr[·] represents the matrix trace operation, μ k represents the desired multiple vanishing factors, is the weight coefficient that needs to be set, which is usually set to no less than 1; the one-step transfer matrix with strong tracking characteristics obtained by formulas (27)-(31) is used to predict the estimated value of PNCM calculated in formula (19) And the PNCM posterior estimate P obtained in the previous round k-1k-1 , in the measurement equation slow loop, the following formula can be used to replace the equation (20) of the state equation fast loop to obtain the prior update value of PECM: <Sub-step 8>: Initialize the Bayesian posterior estimate and PECM posterior estimate of the system state vector in this iteration to: Set the number of iterations from n = 0 to n = N VB -1, in the nth round, the shape factor of the inverse Gamma distribution is updated and the inverse scale parameter is initialized as: In the formula, represents the jth component of the shape factor of the Gamma distribution in the nth iteration, represents the initial value of the jth component of the inverse scale factor; <Sub-step 9>: The number of iterations is from n=0 to n=N VB -1, the a posteriori estimation calculation method of the system state vector and PECM is given as: In the above two equations, the posterior estimate of MNCM is The calculation method is: First, use formula (39) to update the inverse scaling factor β in the nth iteration: k|k-1,j , and then use the variational Bayesian mechanism of formulas (35), (39) and (40) to preliminarily estimate MNCM; use the sequential filtering of formulas (41) and (42) to prevent MNCM from becoming non-positive definite during the adaptive update process, and finally obtain the posterior estimate of MNCM; Note that the subscript j in the lower right corner of all the formulas represents the jth element of the vector, and the subscript jj represents the jth diagonal value of the matrix, represents the jth component of the inverse scaling factor in the nth iteration, Represents the posterior estimate of MNCM The jth diagonal element of the i-th iteration before the sequential filtering correction is performed; after completing the update of formulas (37)-(42), let the number of variational Bayesian estimation iterations be updated to n=n+1, and return to formula (35) for a new round of iterative calculation until the number of iterative updates reaches n=N VB ; <Sub-step 10>: Adaptive estimation of PNCM, PECM and MNCM is completed based on the proposed RAKF through formulas (19), (32) and (40)-(42); when n=N VB After the update is completed, the traditional Kalman filter can be used to update the Kalman gain, state vector and posterior estimate of PECM: P k|k =P k|k-1 -K k H k P k|k-1 (45) Among them, K k , and P k|k They are Kalman gain, system state vector a posteriori estimate and system PECM a posteriori estimate respectively; after completing the a posteriori estimate update, the time step is set to k=k+1, and then return to <sub-step 4> to predict and estimate the next time step; if the time step reaches the maximum time of the UAV motion trajectory, the iteration is stopped, and the output system state vector and PECM a posteriori estimate are: In the formula, Represent the system state vector and the posterior estimation results of PECM respectively; A posteriori estimate of the system state vector Each item represents, Indicates the platform misalignment angle, represents the velocity measurement error of the navigation coordinate system, represents the position measurement error of the navigation coordinate system, represents the gyro bias error, Indicates the addition of zero bias error, represents the spatial arm effect error, The time synchronization error of the integrated navigation system composed of inertial navigation and satellite, the augmented vector of gyro installation error and scale error can be written as in and are the gyro scale errors, and the remaining elements are the gyro installation errors. The augmented vector with the installation error and scale error can be written as in and are the added scale errors, and the remaining elements are the added installation errors; The posterior estimate P of PECM kk Each item represents, is the estimated covariance of the platform misalignment angle, is the velocity estimation error covariance of the navigation coordinate system, is the navigation coordinate system position estimation error covariance, is the gyro bias error estimation error covariance, is the error covariance of the estimated error with added bias error, is the spatial lever-arm effect error estimation covariance, is the estimated covariance of the time synchronization error of the inertial navigation and satellite integrated navigation system, is the augmented covariance matrix of gyro installation error and scale error, is the augmented covariance matrix with added fitting and scaling errors.

6. A UAV inertial navigation model parameter identification system based on robust adaptive filtering, characterized by: The identification system is used to run the method described in any one of claims 1 to 5, and the system is specifically as follows: The system has the functions of UAV motion trajectory generation, inertial navigation model parameter identification, result storage, data playback and display; According to the functional modules, the simulation platform consists of several modules, including model parameter and input setting module, UAV motion trajectory generation module, identification algorithm setting module, simulation and result storage module, data playback and display module, etc. The functions and usage instructions of each sub-module are as follows: 1) Model parameter and input setting module, including trajectory generation setting and inertial navigation model parameter setting. The former is used by the user to specify the maneuvering action from the UAV maneuvering action library, and the maneuvering action sequence and the duration of each action can be freely arranged to form a trajectory script file. The latter is used to set the error characteristic parameters in the IMU sensor measurement model and the navigation system error characteristic parameters; 2) The UAV motion trajectory generation module selects and loads the written trajectory script file. The software platform can combine the maneuvering actions and durations set in the file with the actions in the maneuvering action library in sequence and duration, and generate a UAV motion trajectory and load it into the Matlab workspace; 3) Identification algorithm setting module, the user can select one of the three methods: classic Kalman filtering, classic variational Bayesian adaptive filtering and robust adaptive filtering proposed in the present invention, and set the filtering algorithm parameters and navigation error prior knowledge parameters; 4) Simulation and result storage module: After the identification algorithm is set, the algorithm can be run to identify the UAV inertial navigation model. After the simulation is completed, the results of the identification algorithm are displayed in the form of images and text; If the result satisfies the user, the result can be stored as a mat file, which can be named and stored in the specified path. The software platform supports the storage of drone motion trajectory and recognition results. 5) Data playback and display module, which supports drawing the measurement output of the inertial navigation model for the UAV motion trajectory and the inertial navigation model identification result image, and can load the stored results to complete data playback; this module is also equipped with an operation log recording module, which records the corresponding operation log when the user uses the interactive interface of the simulation platform to perform operations including parameter setting, data loading, parameter identification simulation and data saving. The user can trace back from the operation log to view all the operations he has performed, as well as view the specific parameters set for a simulation and the path location of the data after saving the results.

Images