Method for correcting attitude of unmanned aerial vehicle optoelectronic pod by MEMS-IMU dual mode

Abstract

The present application relates to the technical field of unmanned aerial vehicle photoelectric pod attitude measurement, and particularly relates to a MEMS-IMU dual-mode correction attitude measurement method for an unmanned aerial vehicle photoelectric pod. The method comprises the following steps: a shock absorption system second-order transfer function model is established to output a decay coefficient matrix; based on MEMS-IMU three-axis motion parameters, large maneuvering turning and straight flight states are distinguished; according to different states, the main inertial navigation is used to compensate the MEMS-IMU roll angle, pitch angle and heading angle respectively; the corrected attitude angle is converted into a quaternion, and the gradient descent method is used to optimize the output fusion quaternion; the fusion quaternion is taken as input to construct a seven-dimensional state space model, and the Kalman filter is used to output error compensation; the weight of the main inertial navigation is dynamically adjusted according to the root mean square of acceleration; the error compensation and the weight are combined to output the final attitude angle, which is verified by GNSS position inversion. The method has the advantages of avoiding cross interference, improving the attitude measurement precision and stability, and being suitable for the unmanned aerial vehicle photoelectric pod scene.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned aerial vehicle optoelectronic pod attitude measurement, and particularly relates to a MEMS-IMU dual-mode correction attitude measurement method for an unmanned aerial vehicle optoelectronic pod. BACKGROUND

[0002] In the application of the unmanned aerial vehicle optoelectronic pod, the rotors of the unmanned helicopter generate low-frequency vibration of 1-5 Hz, which seriously affects the precision of the attitude measurement equipment in the optoelectronic pod, and therefore the vibration needs to be isolated by a shock absorber. However, the non-rigid characteristics of the shock absorber distort the attitude data transmission process from the main inertial navigation system (installed on the unmanned aerial vehicle body) to the optoelectronic pod, so that the optoelectronic pod cannot directly use the high-precision attitude data of the main inertial navigation system.

[0003] At present, a low-cost micro-electromechanical inertial measurement unit (MEMS-IMU) is mostly used in the optoelectronic pod for attitude measurement, but the MEMS-IMU has the defect of unstable zero offset: in a large maneuvering flight scene (such as turning and slope flight), the pitch angle and roll angle errors will quickly accumulate, and the error value often exceeds 3°; in a straight flight scene, the heading angle drift problem is prominent, and the drift amount can reach more than 15° after 60 minutes of flight.

[0004] The attitude data fusion method in the prior art does not design an adaptive scheme for the physical scene of "shock absorber isolation", neither quantifies the attenuation effect of the shock absorber on attitude transmission, nor dynamically adjusts the correction strategy according to the flight state (large maneuvering / straight flight), so that the dynamic errors of the MEMS-IMU cannot be effectively compensated, and it is difficult to meet the demand of the optoelectronic pod for high-precision attitude measurement (attitude angle error ≤ 1°). Therefore, it is urgent to develop a high-precision attitude measurement method suitable for a vibration isolation environment, which combines the main inertial navigation system on the unmanned aerial vehicle and the low-cost MEMS-IMU in the optoelectronic pod. SUMMARY

[0005] The present application provides a MEMS-IMU dual-mode correction attitude measurement method for an unmanned aerial vehicle optoelectronic pod.

[0006] The present application aims to provide a MEMS-IMU dual-mode correction attitude measurement method for an unmanned aerial vehicle optoelectronic pod, which specifically comprises the following steps:

[0007] S1: According to the low-frequency vibration characteristics of the rotors of the unmanned helicopter and the dynamic parameters of the shock absorber, a second-order transfer function model of the shock absorption system is established, the amplitude attenuation and phase delay of the attitude angle of the shock absorber are quantified, and a decay coefficient matrix is output; the attitude angle includes the roll angle, the pitch angle and the heading angle;

[0008] S2: Accurately distinguish the UAV flight state through the angular velocity threshold criterion and acceleration energy detection, and output the state flag; the UAV flight state is large maneuvering turn or straight flight;

[0009] S3: When the UAV flight state is large maneuvering turn, the main inertial navigation is adopted to perform proportional-integral compensation on the roll angle and the pitch angle;

[0010] S4: When the UAV flight state is straight flight, the main inertial navigation is adopted to perform dynamic integral compensation on the heading angle output by the MEMS-IMU, so as to suppress the cumulative error caused by the gyro zero offset; the corrected heading angle is input to step S5;

[0011] S5: The corrected attitude angles output by steps S3 and S4 are converted into attitude quaternions, the attitude quaternions are iteratively optimized through the gradient descent method, and the fused quaternions are output;

[0012] S6: The fused quaternions are taken as observations to construct a seven-dimensional state space model, and the error compensation quantity is estimated and output in real time through Kalman filtering;

[0013] S7: The correction weight of the main inertial navigation is dynamically adjusted according to the real-time three-axis acceleration root mean square collected by the MEMS-IMU;

[0014] S8: The final attitude angle of the photoelectric pod is output according to the error compensation quantity of step S6 and the dynamic weight coefficient of step S7, and the external precision verification is performed through GNSS position inversion.

[0015] Preferably, the expression of the second-order transfer function model is as follows:

[0016] ;

[0017] In the formula: H ( s ) represents the transfer function of the damping system; represents the natural frequency of the shock absorber; represents the damping ratio, ranging from 0.1 to 0.3; s represents the Laplace complex variable;

[0018] The attenuation coefficient matrix is represented as: ;

[0019] In the formula: represents the roll angle attenuation coefficient, ; represents the pitch angle attenuation coefficient, ; represents the heading angle attenuation coefficient, ; is the amplitude of the complex variable function .ω is the angular frequency.

[0020] Preferably, step S2 specifically comprises: calculating the vector module of the three-axis angular velocity output by the MEMS-IMU ω x , ω y , ω z to realize preliminary state discrimination by comparison with a preset threshold value;

[0021] The vector module of the three-axis angular velocity is: ;

[0022] In the formula: represents the resultant module of the three-axis angular velocity, with the unit of rad / s, representing the degree of overall rotation; ω x ω y 、 ω z respectively represent the roll, pitch and heading direction angular velocities of the MEMS-IMU;

[0023] If > ω th , it is preliminarily determined as a large maneuver state, otherwise it is preliminarily determined as a straight-line state; ω th is a threshold value; when the preliminary discrimination is a large maneuver state, further verification is performed through the three-axis acceleration root mean square (RMS) value;

[0024] The three-axis acceleration RMS value is used as a vibration energy threshold to filter the interference of residual vibration of the damping system on state recognition, and the expression is as follows:

[0025] ;

[0026] In the formula: represents the three-axis acceleration RMS value; , , respectively represent the roll, pitch and heading direction acceleration components at the i-th moment, with the unit of m / s 2 ; N represents the sampling window size; i represents the sampling moment index, ranging from 1 to N;

[0027] If > , the large maneuver state is maintained; if ≤ , it is determined as a false maneuver state, and is forced to switch to a straight-line state; is a vibration energy threshold value;

[0028] When initially determined to be a straight line, if > If the vibration lasts for more than 3 seconds, it is determined to be a straight-line flight and will remain in a straight line.

[0029] The output status flag F mode ∈{0,1}, F mode =1 indicates a high-maneuverability state. F mode =0 indicates a straight line state.

[0030] Preferably, the modified formula for step S3 is as follows:

[0031] ;

[0032] ;

[0033] In the formula: , These represent the corrected roll angle and pitch angle, respectively. , These represent the attenuation coefficients for roll angle and pitch angle, respectively. , These represent the roll angle and pitch angle of the MEMS-IMU, respectively. Indicates proportional gain; Indicates integral gain; t This represents the integration time, expressed in seconds (s).

[0034] Preferably, the modified formula for step S4 is as follows:

[0035] ;

[0036] In the formula: Indicates the corrected heading angle; This represents the initial heading angle of the MEMS-IMU; Indicates the aircraft's primary inertial navigation heading angle; α It is a dynamic weighting coefficient; t represents the cumulative time to enter straight flight mode; This represents the integral term of the heading angle deviation; k I ( t The gain is the dynamic integral gain that decays over time, and the formula is as follows:

[0037] ;

[0038] In the formula: Indicates the initial integral gain; τ Indicates the decay time constant; represents the minimum integral gain; t represents the cumulative time of entering the straight flight state.

[0039] Preferably, the error compensation quantity comprises a zero offset compensation value and a residual attitude error;

[0040] The seven-dimensional state space model is composed of a state equation and an observation equation, as follows:

[0041]

[0042] In the formula: A represents a state transition matrix, used to describe the evolution law of the state vector from the k-1 time to the k time; H represents an observation matrix, used to map the state space to the observation space; x represents a state vector; z represents an observation vector; k- 1 and k represents the time; w k represents the process noise, and the covariance matrix is Q; v k represents the observation noise, and the covariance matrix is R;

[0043] The state vector x = [ δ φ , δ θ , δ ψ , b gx , b gy , b gz , ε ]; δ φ , δ θ , δ ψ respectively represent the estimated errors of the roll, the pitch and the heading angle; b gx , b gy , b gz respectively represent the three-axis gyro zero offsets; ε represents the vibration coupling term.

[0044] Preferably, the weight coefficient in step S7 is α The formula is as follows:

[0045] ​​

[0046] wherein: λ represents the adjustment coefficient; when the three-axis acceleration root mean square (RMS (a)) > 5 m / s 2 , the weight is reduced to below 0.2;

[0047] RMS (a) = ;

[0048] wherein, , , are the three-axis acceleration components at the i-th sampling time, and N is the sampling window size.

[0049] Preferably, the position inversion accuracy verification criterion in step S8 is as follows:

[0050] Error pos =|| P GNSS - P calc ||;

[0051] wherein: Error pos represents the position inversion error; P GNSS represents the GNSS measured target position; P calc represents the attitude solution inversion position;

[0052] If Error pos < 0.5 m, it is determined that the attitude solution accuracy is qualified; if Error pos > 1 m, the system triggers an alarm and automatically switches to the main inertial navigation direct output mode.

[0053] Compared with the prior art, the present application can achieve the following beneficial effects:

[0054] The prior art does not consider the attenuation and delay of the shock absorption system on attitude transmission, and it is difficult to adapt to the physical isolation scene of the pod-fuselage; the present application quantifies the shock absorption effect by constructing a vibration isolation-attitude transmission coupling model, combines flight state recognition -> dual-mode segmented correction architecture, switches the correction strategy according to large maneuvering turning / straight flight, avoids correction cross interference under different working conditions, and solves the poor adaptability problem of the traditional method.

[0055] The existing single filtering or fusion method is prone to insufficient precision or calculation lag; the four quaternion gradient descent fusion (40% higher in calculation efficiency than Kalman filtering, and fast suppression of cumulative error) is combined with Kalman filtering error observation (real-time estimation of zero bias and attitude residual) in the application, so that the attitude error is stably controlled within 0.5 degrees while ensuring real-time response, and the precision and efficiency are considered.

[0056] The prior art is prone to instability in complex environments such as severe vibration, and lacks effective precision verification means; the application resists vibration interference through vibration adaptive weight adjustment (dynamically adjusts the main inertial navigation weight according to acceleration changes), and builds a closed loop with GNSS position inversion verification to ensure the attitude stability and precision reliability in complex working conditions, and the engineering applicability is significantly improved. BRIEF DESCRIPTION OF DRAWINGS

[0057] Figure 1 It is a flow chart of a MEMS-IMU dual-mode correction attitude measurement method for an unmanned aerial vehicle optoelectronic pod according to an embodiment of the application. DETAILED DESCRIPTION

[0058] In the following description, the same reference numerals are used to represent the same elements throughout the drawings. Where possible, the same reference numerals are used to represent the same elements in the same drawing. Therefore, detailed descriptions thereof will not be repeated.

[0059] In order to make the purpose, technical scheme and advantages of the application clearer, further detailed description will be made in combination with the drawings and specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the application and do not constitute a limitation on the application.

[0060] The application provides a MEMS-IMU dual-mode correction attitude measurement method for an unmanned aerial vehicle optoelectronic pod, which specifically comprises the following steps:

[0061] S1: Construct a vibration isolation-attitude transmission coupling model: combine the rotor low-frequency vibration characteristics (1-5Hz) of the unmanned helicopter with the dynamic parameters of the shock absorber ( , ), and output an attenuation coefficient matrix for subsequent flight state recognition by establishing a second-order transfer function model of the shock absorption system to quantify the amplitude attenuation and phase delay of the roll angle, pitch angle and heading angle of the shock absorber.

[0062] The expression of the second-order transfer function model is as follows:

[0063] ;

[0064] In the formula, H ( s ) represents the transfer function of the shock absorption system (dimensionless). represents the inherent frequency of the shock absorber (unit: rad / s), and a typical value is 2π×2 rad / s (corresponding to 2 Hz); represents the damping ratio (dimensionless), and a measured range is 0.1-0.3; s represents the Laplace complex variable (unit: s -1 );

[0065] When analyzing its characteristics in the frequency domain (let s = ), the amplitude characteristic of the transfer function represents the ratio of the output signal amplitude to the input signal amplitude;

[0066] The attenuation coefficient matrix is specifically used for the attitude correction link in steps S3 and S4; the attenuation coefficient matrix is represented as: ;

[0067] wherein: represents the roll angle attenuation coefficient, which is used to calculate the main inertial navigation correction weight in step S2, ; represents the pitch angle attenuation coefficient, ; represents the heading angle attenuation coefficient, ; is the amplitude of the complex variable function (representing the ratio of the output signal amplitude to the input signal amplitude when a signal with an angular frequency of ω passes through the shock absorbing system).

[0068] For example, if the amplitude of the input roll angle is A in , after passing through the shock absorbing system, the amplitude of the output roll angle is A out = × A in , thereby establishing the attenuation relationship between the input amplitude and the output amplitude.

[0069] Principle: This step solves the core problem of the shock absorption system leading to the attenuation of the main inertial navigation attitude data. The low-frequency vibration (1-5 Hz) of the unmanned helicopter rotor forces the optical pod to use shock absorbers to isolate vibration, but the dynamic characteristics of the shock absorber will distort the attitude transmission process. The low-frequency vibration (1-5 Hz) of the unmanned helicopter rotor is transmitted to the shock absorber input through the fuselage, and the elastic deformation and damping dissipation of the shock absorber cause the attitude signal (roll angle, pitch angle, heading angle) to be attenuated in amplitude and delayed in phase when transmitted to the MEMS-IMU of the optical pod, forming a coupled link of vibration input-shock response-attitude distortion. By establishing a second-order transfer function model (adaptive mathematical model), the amplitude attenuation and phase delay effects of the shock absorber on the roll angle, pitch angle, and heading angle are quantified; the damping ratio-natural frequency coupling parameter (Kd) is first introduced to solve the defect of traditional methods that ignore the frequency response characteristics of the shock absorber. Effect verification: The actual measurement shows that the model prediction attenuation error is ≤3% (frequency range 1-10 Hz), providing a physical basis for subsequent correction. , ,

[0070] S2: Flight state recognition: According to the three-axis motion parameters collected by the MEMS-IMU of the optical pod, through the design of angular velocity threshold criterion and acceleration energy detection dual mechanism, the two criteria use a hierarchical series, main and auxiliary combined fusion strategy to accurately distinguish between large maneuvering turning and straight flight state of the unmanned aerial vehicle, and output the state flag to trigger the corresponding correction module, the specific implementation logic is as follows:

[0071] Specifically, by calculating the vector module of the three-axis angular velocity ω x , ω y , ω z output by the MEMS-IMU, and comparing it with the preset threshold to realize preliminary state discrimination (main criterion), the expression is as follows:

[0072] The vector module of the three-axis angular velocity is: ;

[0073] In the formula: represents the combined module of the three-axis angular velocity, with a unit of rad / s, representing the overall rotation intensity; ω x represents the MEMS-IMU roll direction angular velocity, with a unit of rad / s, representing the rotation rate around the X-axis; ω y represents the MEMS-IMU pitch direction angular velocity, with a unit of rad / s, representing the rotation rate around the Y-axis; ω z represents the MEMS-IMU heading direction angular velocity, with a unit of rad / s, representing the rotation rate around the Z-axis;

[0074] If > ω th If ω th is threshold value, typical value 0.5 rad / s, preliminary determine large maneuver state, otherwise preliminary determine straight state;

[0075] Secondly confirm (auxiliary criterion): when preliminary determine large maneuver state, need further verify through acceleration RMS value, prevent misjudgment.

[0076] Three-axis acceleration root mean square (RMS) as vibration energy threshold, filter residual vibration of damping system to state recognition interference, expression as follows:

[0077] ;

[0078] In the formula: Indicate three-axis acceleration root mean square value, unit m / s 2 , vibration energy intensity; Indicate the i moment roll direction acceleration component, unit m / s 2 , X-axis instantaneous acceleration; Indicate the i moment pitch direction acceleration component, unit m / s 2 , Y-axis instantaneous acceleration; Indicate the i moment heading direction acceleration component, unit m / s 2 , Z-axis instantaneous acceleration; N Indicate sampling window size (dimensionless), window is bigger, smoothing effect is better; i indicates sampling time index, range 1~N, current sampling point in window in serial number;

[0079] If > ( is vibration energy threshold, typical value 2.0 m / s 2 ), maintain large maneuver state; if ≤ , determine as false maneuver state (such as only wind disturbance causes instantaneous angular velocity change), forced switch to straight state;

[0080] When preliminary determine straight state, if > And last more than 3 seconds, determine as vibration straight flight, still maintain straight state, but trigger step S7 weight adjustment.

[0081] State output logic: output state flag (flight state flag bit) F mode ∈{0,1}, Fmode =1 indicates a high-maneuver flight state. F mode =0 indicates straight flight; output to subsequent step S3 (large maneuver correction) or step S4 (straight line correction) module; pseudocode is as follows:

[0082] if:

[0083] {if: > , F mode =1, #Confirm high maneuver status

[0084] Else: F mode =0, #Pseudo-maneuver, forced straight line}

[0085] else

[0086] {if: > , F mode =0, # Vibrate the straight line, keep the straight line but reduce the weight.

[0087] Else: F mode =0, #normal straight line}

[0088] Advantages of this step design: (1) Angular velocity criterion as the main criterion: directly responds to the maneuver characteristics of the UAV and has strong real-time performance; (2) RMS criterion as the auxiliary criterion: effectively filters residual vibration (1-5Hz) of the shock absorption system and wind disturbance interference, reducing the false trigger rate; (3) Hierarchical processing: avoids state jump caused by single-point noise and improves recognition robustness.

[0089] S3: High-maneuver attitude correction: High-maneuver attitude includes roll angle and pitch angle; when F mode When =1, the high-precision roll angle of the main inertial navigation system is adopted ( ), pitch angle ( The attitude angles output by the MEMS-IMU are proportionally-integral compensated to suppress drift caused by vibration; the corrected attitude angles (roll and pitch angles) are input to the quaternion gradient descent fusion module in step S5.

[0090] The corrected formula is as follows:

[0091] ;

[0092] ;

[0093] In the formula: , These represent the corrected roll angle and pitch angle, respectively. , These represent the attenuation coefficients for roll angle and pitch angle, respectively. , These represent the roll angle and pitch angle of the MEMS-IMU, respectively. This represents the proportional gain (dimensionless), with an optimized value of 0.8. Integral gain (unit: s) -1 ), optimized value 0.05; t Indicates the integration time, in seconds (s).

[0094] Brief Principle Description: During high-maneuver flight, MEMS-IMUs are susceptible to factors such as vibration, leading to rapid accumulation of low-frequency oscillation errors. This step addresses the issue of low-frequency oscillation error accumulation in MEMS-IMUs during high-maneuver flight. Utilizing the high precision of the main inertial navigation system (INS) in roll and pitch angles, a proportional term rapidly responds to the current attitude deviation between the INS and the MEMS-IMU, while an integral term gradually eliminates long-term accumulated errors. Simultaneously, a weighted proportional term based on the attenuation coefficient obtained in step S1 is introduced to avoid over-correction due to misjudgment caused by deviations between the INS and the MEMS-IMU due to factors such as vibration reduction. An integral limiting design (|∫e dt|≤10°) prevents integral saturation.

[0095] S4: Straight Flight Heading Angle Correction: Addresses the MEMS-IMU heading angle drift issue in straight flight scenarios for drones, when the flight status flag... F mode When =0 (straight flight state), the main inertial navigation azimuth angle is used ( The heading angle output by the MEMS-IMU is dynamically integrated to compensate for the cumulative error caused by the gyroscope's zero bias; the corrected heading angle is then input to the quaternion gradient descent fusion module in step S5.

[0096] The formula introduces dynamic integral gain. k I ( t The formula is as follows:

[0097] ;

[0098] In the formula: This represents the corrected heading angle in rad, and is the heading angle output to S5. This represents the raw heading angle of the MEMS-IMU, in rad, which is the uncorrected heading measurement. This represents the aircraft's primary inertial navigation heading angle, in rad, and serves as a high-precision heading reference for correction. αis the dynamic weight coefficient, i.e. the weight from S7; t represents the cumulative time (unit: s) of entering the straight flight state, which is counted from the time of cutting into the straight state; represents the heading angle deviation integral term, unit: rad·s, i.e. the cumulative error after the amplitude protection; k I ( t ) is a dynamic integral gain decaying with time, unit: s - ¹, which is a real-time calculation value and controls the correction strength, and is designed as follows:

[0099] ;

[0100] In the formula: represents the initial integral gain (typical value 0.05, unit: s - ¹), which represents the gain at the initial time of straight flight; τ represents the decay time constant (typical value 300, unit: s), which represents the time required for the gain to decay to 37%; represents the minimum integral gain (typical value 0.01, unit: s - ¹), which ensures a weak correction ability in the long term; t represents the cumulative time (unit: s) of entering the straight flight state, which is counted from the time of cutting into the straight state.

[0101] The specific application method of step S4 includes: (1) initialization: when F mode the timer t is reset and starts counting again; (2) real-time calculation: calculate k I ( t ) every period, which exponentially decays with time; (3) amplitude protection: the integral term is limited within ±10° to prevent integral saturation caused by the installation deviation of the main inertial navigation system and the MEMS-IMU; (4) weight coordination: the dynamic integral gain is multiplied by the weight coefficient α S7 to realize double adaptive adjustment.

[0102] The design principle of this step is: (1) initial strong correction: the gyro zero offset accumulates quickly in the initial stage of straight flight, and strong integral action is needed to quickly suppress the error; (2) later weak dependence: as the Kalman filter converges, the system gradually trusts the MEMS-IMU self-solution, reduces the dependence on the main inertial navigation system, and avoids the influence of long-term drift of the main inertial navigation system.

[0103] S5: Quaternion gradient descent fusion: To solve the problem of Euler angle singularity and multi-sensor noise coupling, the corrected attitude angle output by steps S3 and S4 is converted into an attitude quaternion as an initial attitude input; combined with the accelerometer and magnetometer data, the attitude quaternion is iteratively optimized through the gradient descent method to improve the stability of the attitude solution and achieve high-precision attitude fusion and update; finally, the fused quaternion is output q fuse ;

[0104] Time rate of change of attitude quaternion The expression is as follows:

[0105]

[0106] In the formula: represents the quaternion multiplication operator; β is the gradient step / learning rate, which controls the convergence speed and stability; represents the gradient of the target function, which is calculated from the accelerometer and magnetometer data, i.e., the accelerometer / magnetometer data gradient (unit: s -1 ); represents the gradient norm (modulus), which is used for normalization to prevent the step size from being too large; represents the conjugate of the estimated quaternion; represents the quaternion update rate, i.e., the amount of quaternion change per second; =[ q 0, q 1, q 2, q 3], where q 0、 q 1、 q 2、 q 3The expression is as follows:

[0107] ;

[0108] In the formula: q 0 is the scalar part, q 1、 q 2、 q 3 are the vector parts;

[0109] The complete calculation steps are as follows:

[0110] (1) Calculate the accelerometer gradient:

[0111] ;

[0112] (2) Calculate the magnetometer gradient:

[0113] ;

[0114] (3) Fusion gradient: ;

[0115] (4) Normalized update: ;

[0116] (5) Adaptive step adjustment: ;

[0117] In the formula: represents the accelerometer gradient; represents the magnetometer gradient; represents the fusion gradient; represents the weight coefficient of the accelerometer gradient; represents the weight coefficient of the magnetometer gradient; q k represents the quaternion at the kth iteration; q k+1 represents the quaternion at the k+1th iteration; β represents the step of gradient descent; β 0 represents the initial gradient step, i.e., the reference step in static or low vibration, β 0=0.1; λ represents the step adjustment coefficient, the stronger the vibration, the larger the step, and the stronger the correction strength, λ = 0.05.

[0118] Principle: Euler angles have singularity problems and are easily affected by multi-sensor noise coupling. This step first converts the Euler angles corrected in the previous step into quaternion form, then drives the natural evolution of the quaternion based on the angular velocity, combines the gradient information of the accelerometer and magnetometer data, and continuously corrects the quaternion through the gradient descent method; improve the adaptive mechanism of the gradient step, dynamically adjust the step according to the vibration, and enhance the correction strength when the vibration is enhanced to suppress noise, thereby improving the stability of the attitude solution.

[0119] S6: Kalman filter error observation: In order to eliminate the time-varying error of the MEMS-IMU zero offset and improve the long-term attitude solution accuracy, based on the fusion quaternion output by step S5 q fuse as observation input, a seven-dimensional state space model containing attitude residual and gyro zero offset is constructed, and error compensation is estimated and output in real time through Kalman filtering; the error compensation includes zero offset compensation value and attitude residual;

[0120] The seven-dimensional state space model is composed of state equation and observation equation; they are as follows:

[0121] ;

[0122] In the formula: A represents a state transition matrix (7x7) for describing the evolution rule of the state vector from k-1 time to k time, and the elements thereof are determined by the attitude kinematics equation and the zero-mean random walk model (usually a first-order Gaussian-Markov process); H represents an observation matrix (4x7) for mapping the state space to the observation space; x represents a state vector; z represents an observation vector; k- 1 and k represents a time; w k represents process noise for describing model uncertainty, and the covariance matrix thereof is Q; v k represents observation noise for describing sensor measurement noise, and the covariance matrix thereof is R;

[0123] state vector x [ δ φ , δ θ , δ ψ , b gx , b gy , b gz , ε ]new vibration coupling term ε ; δ φ , δ θ , δ ψ respectively represent the estimated errors of roll, pitch and yaw angles; b gx , b gy , b gz respectively represent the three-axis gyro zero biases; ε represents a vibration coupling term (dimensionless) for modeling the influence of the vibration not completely isolated by the damping system on the attitude error.

[0124] S7: Dynamic weight adaptive adjustment: in order to optimize the reliability of the fusion of the main inertial navigation system and the MEMS-IMU in the severe vibration environment, the correction weight of the main inertial navigation system is dynamically adjusted according to the real-time three-axis acceleration root mean square (RMS(a)) collected by the MEMS-IMU; the correction weight of the main inertial navigation system is dynamically reduced according to the real-time vibration energy, so as to avoid the vibration transmission error from polluting the attitude solution of the MEMS-IMU; the weight coefficient α The formula is as follows:

[0125] ;

[0126] In the formula: λ Adjustment coefficient (unit: s 2 / m), optimal value 0.2, λ Compensated by zero bias value and attitude residual dynamic adjustment; when RMS(a)>5m / s 2 , the weight is reduced to below 0.2;

[0127] RMS(a)= ;

[0128] In the formula, , , The three-axis acceleration components at the i-th sampling time, and N is the sampling window size.

[0129] Principle: Under severe vibration conditions, the relative motion between the UAV body and the photoelectric pod is intensified, and the main inertial attitude data transmitted to the pod will produce significant phase delay and amplitude distortion. The exponential weight decay law designed in this step can dynamically adjust the weight of the main inertial navigation system according to the vibration energy: when the vibration energy is low (very small), it tends to 1, and the main inertial navigation system fully exhibits high precision characteristics; when the vibration energy is severe (very large), it is quickly reduced, and the system relies more on the MEMS-IMU itself optimized by S5 to solve, to ensure the stability of the attitude. Effect verification: in a strong vibration environment (RMS(a)=8 m / s 2 ), after using the dynamic weight adjustment strategy, the attitude angle error of the system output can still be controlled within ≤0.8°.

[0130] S8: Real-time attitude output and verification: As the final attitude solving and precision verification link of the system, the final high-precision attitude angle [φ final , θ final , ψ final ] of the photoelectric pod is output, and external precision verification is performed through GNSS position inversion; the implementation method is as follows:

[0131] S81: Error compensation step S6 Kalman filter output 7-dimensional state vector x =[ , , , b gx , b gy , b gz , ε ] is decomposed and processed:

[0132] S811: Zero bias compensation (real-time feedback to S5): ; where, represents the compensated gyro angular velocity; represents the original measured angular velocity of the gyro; δ gx , δ gy , δ gz represents the zero bias compensation of the gyro x, y, z axis respectively;

[0133] S812: Attitude residual correction: = ; = ; = ; where, , , represents the corrected roll angle, pitch angle and yaw angle residual respectively.

[0134] S82: Weighted fusion output: the final attitude angle is converted into Euler angle from the fused quaternion output by S5 q fuse and weighted fusion with the attitude residual of S6:

[0135] ;

[0136] where: is the final output roll angle, unit rad, which is the final roll attitude output of S8 system; is the roll angle calculated by gradient descent method, unit rad, which is the roll angle calculated by quaternion gradient descent method from S5; α ( t ) is the time-varying weight coefficient, i.e. the dynamic weight coefficient output by S7; is the roll angle estimation error, unit rad, which is from the Kalman filter state vector of S6; is the final output pitch angle, unit rad, which is the final pitch attitude output of S8 system; is the pitch angle calculated by gradient descent method, unit rad, which is the pitch angle calculated by quaternion gradient descent method from S5; is the pitch angle estimation error, unit rad, which is from the Kalman filter state vector of S6; is the final output heading angle, unit rad, which is the final heading attitude output of S8 system; is the heading angle calculated by gradient descent method, unit rad, which is the heading angle calculated by quaternion gradient descent method from S5; This is the heading angle estimation error, in rad, derived from the S6 Kalman filter state vector;

[0137] The residual terms are compensated after weight adjustment to avoid overcorrection.

[0138] S83: Vibration coupling term suppression:

[0139] Vibration coupling term ε Used to correct quaternion updates: q final = q fuse ·Δ q ( ε In the formula, q final The final output quaternion is the final attitude including vibration compensation; q fuse It is the quaternion after S5 fusion, i.e., the basic fused pose; Δ q ( ε ) is a small-angle perturbation quaternion, which separates the vibration error from the attitude calculation and is constructed from the vibration coupling term ε.

[0140] The detailed method for external accuracy verification via GNSS position inversion in step S8 is as follows:

[0141] Basic principle: Utilizing the pitch angle of the electro-optical pod θ and heading angle ψ A line-of-sight vector (LOS) is constructed, and combined with the pod height h, the geographic coordinates of the ground target are retrieved and compared with the GNSS measured coordinates.

[0142] Input parameters: pod attitude angle [φ] final ,θ final ,ψ final ]; pod height h (laser rangefinder or barometer data, unit: m) pod GNSS position [ L uav , B uav , H uav (Latitude, Longitude, Altitude, unit: ° / m); Camera mounting matrix (Mounting angle between camera and IMU, fixed value)

[0143] Calculation process:

[0144] S841. Constructing the pod - Ground vector r b (Body coordinate system):

[0145] In the formula, r bis the pod-ground vector, which is the vector from the pod pointing to the ground target; θ is the final pitch angle θ final ; is the final roll angle φ final ; h is the pod height from the ground, which is obtained by laser ranging or barometric height;

[0146] S842. Coordinate system conversion: convert the body coordinate system vector to the geographic coordinate system (East-North-Up, ENU): ;

[0147] wherein: r e is the vector in the geographic coordinate system, East-North-Up (ENU) coordinate system; is the attitude matrix from the body to the navigation system, which is constructed by the quaternion q final .

[0148] S843: Target position calculation: based on the pod GNSS position, calculate the target point geographic coordinates through geodetic coordinate conversion:

[0149] ;

[0150] wherein: Δ L represents the change in latitude; Δ B represents the change in longitude; L UAV represents the real-time latitude of the pod, B UAV represents the real-time longitude of the pod, H UAV represents the real-time height of the pod, all from the GNSS receiver; L calc , B calc respectively represent the calculated target latitude and longitude, from the inversion calculation result; Re is the Earth's radius (6378137 m), used for radian-distance conversion;

[0151] S844: Accuracy verification

[0152] Calculate the position error: Error pos =|| P GNSS - P calc ||;

[0153] wherein: Error pos represents the position inversion error, which is the attitude solution accuracy evaluation index; P GNSSGNSS measured target position, as a reference, the true position; P calc attitude solution inversion position, calculated position based on attitude output;

[0154] If Error pos <0.5m, determine that the attitude solution accuracy is qualified; if Error pos >1m, the system triggers an alarm and automatically switches to the main inertial navigation direct output mode (Bypass mode).

[0155] The inversion method of the application adds the following parts on the basis of the traditional dead reckoning method: (1) high dynamic compensation: introduce laser ranging real-time correction h, eliminate the influence of terrain undulation; (2) online calibration of installation error: use the vibration coupling term of S6 to estimate the camera-IMU installation angle deviation, periodically update , improve the inversion accuracy.

[0156] Principle: this step forms the engineering closed loop of the system; on the one hand, integrate the optimization and compensation information of the upstream module to generate the final attitude angle that can be directly used for control and display; on the other hand, introduce high-precision GNSS as an external reference source to evaluate the final accuracy of the entire attitude solution link. By comparing the target position based on attitude data inversion and the GNSS measured position, it can be objectively judged whether the system accuracy meets the task requirements. When the error exceeds the preset threshold (such as 1m), the system triggers an alarm to prompt that there may be sensor failure or abnormal flight conditions.

[0157] The above test results show that the modified attitude determination method of the application can significantly reduce the attitude angle solution error and cumulative drift in the typical flight scene of an unmanned helicopter, and has excellent engineering application effect.

[0158] The advantages of the application are:

[0159] 1. Dynamic correction architecture under physical isolation: build a vibration isolation-attitude transmission coupling model to quantify the attenuation effect of the shock absorber on attitude transmission; adopt state recognition, segmented correction and fusion verification process (S2-S4), switch the double-mode correction mode according to the flight state, avoid cross interference, and solve the problem that the traditional method cannot adapt to the physical isolation scene.

[0160] 2. Multi-source data collaborative optimization: combine the quaternion gradient descent method (S5) and Kalman filter (S6), the former suppresses the cumulative error of MEMS-IMU and has a calculation efficiency 40% higher than Kalman filter, and the latter estimates the error compensation in real time, reduces the attitude error under the premise of ensuring real-time.

[0161] 3. Engineering applicability enhancement: The complex vibration environment is adapted through vibration self-adaptive weight adjustment (S7), and the accuracy and reliability are ensured through GNSS position inversion verification (S8), so as to enhance the applicability and stability of the method in the actual engineering scene.

[0162] It should be understood that the various forms of flow shown above can be reordered, added to, or steps deleted. For example, the steps described in the present disclosure can be performed in parallel, sequentially, or in a different order, as long as the desired results of the technical solutions of the present disclosure can be achieved, which is not limited herein.

[0163] The above specific embodiments do not constitute a limitation on the protection scope of the present application. Those skilled in the art should understand that various modifications, combinations, sub-combinations and substitutions can be made according to design requirements and other factors. Any modification, equivalent replacement and improvement made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A dual-mode attitude measurement method for an unmanned aerial vehicle (UAV) optoelectronic pod MEMS-IMU, characterized in that: Specifically, the steps include the following: S1: Based on the low-frequency vibration characteristics of the unmanned helicopter rotor and the dynamic parameters of the shock absorber, a second-order transfer function model of the shock absorption system is established to quantify the amplitude attenuation and phase delay of the shock absorber on the attitude angle, and the attenuation coefficient matrix is ​​output; the attitude angle includes roll angle, pitch angle, and yaw angle. S2: Accurately distinguish the flight state of the UAV by using angular velocity threshold criteria and acceleration energy detection, and output a status flag; the UAV flight state is either high-maneuver turn or straight flight. S3: When the UAV is in a high-maneuver turn, the main inertial navigation system is used to perform proportional-integral compensation for the roll and pitch angles. S4: When the UAV is flying in a straight line, the main inertial navigation system is used to dynamically integrate and compensate the heading angle output by the MEMS-IMU to suppress the cumulative error caused by the gyroscope zero bias; the corrected heading angle is input to step S5. S5: Convert the corrected attitude angles output from steps S3 and S4 into attitude quaternions, iteratively optimize the attitude quaternions using gradient descent, and output the fused quaternions. S6: Using the fused quaternion as the observation input, a seven-dimensional state-space model is constructed, and the error compensation is estimated and output in real time through Kalman filtering. S7: Dynamically adjust the correction weights of the main inertial navigation system based on the real-time triaxial root mean square acceleration data collected by the MEMS-IMU. S8: Based on the error compensation amount in step S6 and the dynamic weighting coefficient in step S7, output the final attitude angle of the electro-optical pod and perform external accuracy verification through GNSS position inversion.

2. The method for dual-mode attitude correction of a UAV optoelectronic pod MEMS-IMU according to claim 1, characterized in that: The expression for the second-order transfer function model is as follows: ； In the formula: H ( s () represents the transfer function of the damping system; Indicates the natural frequency of the shock absorber; This indicates the damping ratio, ranging from 0.1 to 0.

3. s Represents the Laplace complex variable; The attenuation coefficient matrix is ​​represented as follows: ; In the formula: This represents the roll angle attenuation factor. ; This represents the pitch attenuation coefficient. ; This represents the heading angle attenuation coefficient. ; It is a complex function The amplitude; ω ω is the angular frequency.

3. The dual-mode attitude correction method for an unmanned aerial vehicle (UAV) optoelectronic pod MEMS-IMU according to claim 1, characterized in that: Step S2 specifically includes: calculating the triaxial angular velocity output by the MEMS-IMU. ω x , ω y , ω z The vector magnitude is compared with a preset threshold to achieve preliminary state discrimination; Triaxial angular velocity vector magnitude: ; In the formula: The combined modulus of the three-axis angular velocities is expressed in rad / s and represents the degree of overall rotational intensity. ω x ω y ω z These represent the angular velocities of the MEMS-IMU in the roll, pitch, and yaw directions, respectively. like > ω th If it is determined to be a high-maneuver state, it is determined to be a straight-line state otherwise. ω th The threshold is used; when it is initially determined to be a high-maneuver state, it is further verified by the root mean square (RMS) value of the three-axis acceleration. The root mean square of triaxial acceleration is used as a vibration energy threshold to filter out interference from residual vibration in the damping system on state identification. The expression is as follows: ； In the formula: This represents the root mean square value of triaxial acceleration; , , These represent the roll, pitch, and yaw acceleration components at time i, respectively, in m / s². 2 ; N Indicates the sampling window size; i represents the sampling time index, ranging from 1 to N; like > Then maintain a high maneuverability state; if ≤ If so, it is determined to be a pseudo-maneuver state and is forcibly switched to a straight-line state; The vibration energy threshold; When initially determined to be a straight line, if > If the vibration lasts for more than 3 seconds, it is determined to be a straight-line flight and will remain in a straight line. The output status flag F mode ∈{0,1}, F mode =1 indicates a high-maneuverability state. F mode =0 indicates a straight line state.

4. The dual-mode attitude correction method for an unmanned aerial vehicle (UAV) optoelectronic pod MEMS-IMU according to claim 1, characterized in that: The correction formula for step S3 is as follows: ； ； In the formula: , These represent the corrected roll angle and pitch angle, respectively. , These represent the attenuation coefficients for roll angle and pitch angle, respectively. , These represent the roll angle and pitch angle of the MEMS-IMU, respectively. Indicates proportional gain; Indicates integral gain; t This represents the integration time, expressed in seconds (s).

5. The method for dual-mode attitude correction of a UAV optoelectronic pod MEMS-IMU according to claim 1, characterized in that: The correction formula for step S4 is as follows: ； In the formula: Indicates the corrected heading angle; This represents the initial heading angle of the MEMS-IMU; Indicates the aircraft's primary inertial navigation heading angle; α It is a dynamic weighting coefficient; t represents the cumulative time to enter straight flight mode; This represents the integral term of the heading angle deviation; k I ( t The gain is the dynamic integral gain that decays over time, and the formula is as follows: ； In the formula: Indicates the initial integral gain; τ Indicates the decay time constant; Indicates the minimum integral gain; t This indicates the cumulative time spent in straight-line flight.

6. The method for dual-mode attitude correction of a UAV optoelectronic pod MEMS-IMU according to claim 1, characterized in that: The error compensation amount includes the zero bias compensation value and the attitude residual; The seven-dimensional state-space model consists of state equations and observation equations, as follows: ； In the formula: A This represents the state transition matrix, used to describe the evolution of the state vector from time k-1 to time k; H This represents the observation matrix, used to map the state space to the observation space; x Represents the state vector; z Represents the observation vector; k- 1 and k Indicates time; w k The process noise is represented by Q; v k Represents observation noise, with covariance matrix R; State vector x =[ δ φ , δ θ , δ ψ , b gx , b gy , b gz , ε ]; δ φ , δ θ , δ ψ These represent the estimation errors for roll, pitch, and yaw angles, respectively. b gx , b gy , b gz These represent the zero bias of the three-axis gyroscope; ε This indicates a vibration coupling term.

7. The method for dual-mode attitude correction of a UAV optoelectronic pod MEMS-IMU according to claim 1, characterized in that: The weighting coefficient in step S7 α The formula is as follows: ； In the formula: λ This represents the adjustment coefficient; when the root mean square (RMS) of the triaxial acceleration is greater than 5 m / s². 2 At that time, the weight drops below 0.2; RMS(a)= ; In the formula, , , These are the triaxial acceleration components at the i-th sampling time, and N is the sampling window size.

8. The dual-mode attitude correction method for an unmanned aerial vehicle (UAV) optoelectronic pod MEMS-IMU according to claim 1, characterized in that: The location inversion accuracy verification criterion in step S8 is as follows: Error pos =|| P GNSS - P calc ||； In the formula: Error pos Indicates the position inversion error; P GNSS Indicates the actual target location measured by GNSS; P calc Indicates the position obtained from the attitude calculation inversion; like Error pos <0.5m, the attitude calculation accuracy is deemed acceptable; like Error pos If the distance is greater than 1m, the system will trigger an alarm and automatically switch to the main inertial navigation direct output mode.

Images