MEMS-IMU bimodal correction attitude determination method for photoelectric pod of unmanned aerial vehicle

Abstract

The invention relates to the technical field of attitude determination of a photoelectric pod of an unmanned aerial vehicle, in particular to an MEMS-IMU dual-mode correction attitude determination method for the photoelectric pod of the unmanned aerial vehicle. Comprising the following steps: establishing a damping system second-order transfer function model, and outputting an attenuation coefficient matrix; based on MEMS-IMU three-axis motion parameters, distinguishing a large maneuver turning state and a linear flight state; respectively compensating a roll angle, a pitch angle and a course angle of the MEMS-IMU by using the main inertial navigation according to different states; converting the corrected attitude angle into a quaternion, and optimizing and outputting a fused quaternion through a gradient descent method; taking the fused quaternion as an input, constructing a seven-dimensional state space model, and outputting an error compensation amount through Kalman filtering; dynamically adjusting the main inertial navigation weight according to the accelerated speed root mean square; and outputting a final attitude angle in combination with the error compensation amount and the weight, and verifying the final attitude angle through GNSS (Global Navigation Satellite System) position inversion. The method has the advantages that cross interference is avoided; therefore, the method is suitable for the photoelectric pod scene of the unmanned aerial vehicle.

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 rotor of the unmanned helicopter generates 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, resulting in 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 the scene of large maneuvering flight (such as turning and slope flight), the pitch angle and roll angle errors will quickly accumulate, and the error value often exceeds 3°; in the scene of straight flight, 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), resulting in that the dynamic error 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: S1: According to the low-frequency vibration characteristics of the rotor 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; S2: The flight state of the unmanned aerial vehicle is accurately distinguished by an angular velocity threshold criterion and acceleration energy detection, and a state flag is output; the flight state of the unmanned aerial vehicle is large maneuvering turning or straight flight; S3: When the flight state of the UAV is large maneuvering turn, the main inertial navigation is adopted to carry out proportional-integral compensation on the roll angle and the pitch angle; S4: When the flight state of the UAV is straight flight, the main inertial navigation is adopted to carry out 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; S5: The corrected attitude angles output by steps S3 and S4 are converted into attitude quaternions, the attitude quaternions are iteratively optimized by the gradient descent method, and the fused quaternions are output; S6: The fused quaternions are taken as observations to construct a seven-dimensional state space model, and the error compensation is estimated and output in real time by Kalman filtering; 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; S8: The final attitude angle of the photoelectric pod is output according to the error compensation of step S6 and the dynamic weight coefficient of step S7, and the external accuracy is verified by GNSS position inversion.

[0007] Preferably, the expression of the second-order transfer function model is as follows: ; 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; The attenuation coefficient matrix is represented as: ; 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 function ; ω is the angular frequency.

[0008] Preferably, step S2 specifically comprises: comparing the vector module lengths of the three-axis angular velocities ω x , ω y , ω z output by the MEMS-IMU with a preset threshold to realize preliminary state discrimination; The vector module lengths of the three-axis angular velocities: ; wherein: represents the three-axis angular velocity synthetic module length, unit rad / s, representing the overall rotation intensity; ω x ω y 、 ω z represents the MEMS-IMU roll, pitch, heading direction angular velocity respectively; 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 it is preliminarily determined as a large maneuver state, it is further verified by the three-axis acceleration root mean square (RMS) value; The three-axis acceleration root mean square 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: ; wherein: represents the three-axis acceleration root mean square value; , , represents the roll, pitch, and heading direction acceleration components at the i-th moment, respectively, unit m / s 2 ; N represents the sampling window size; i represents the sampling time index, ranging from 1 to N; 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; When it is preliminarily determined as a straight-line state, if > and lasts more than 3 seconds, it is determined as a vibration straight-line flight, and still maintains the straight-line state; The output state flag F mode ∈{0,1}, F mode =1 represents a large maneuver state, F mode =0 represents a straight-line state.

[0009] Preferably, the correction formula of step S3 is as follows: ; ; wherein: , respectively represent the corrected roll angle and pitch angle; , respectively represent the decay coefficients of the roll angle and pitch angle; , respectively represent the roll angle and pitch angle of the MEMS-IMU; represents the proportional gain; represents the integral gain; t represents the integral time, in seconds.

[0010] Preferably, the correction formula of step S4 is as follows: ; wherein: represents the corrected heading angle; represents the original heading angle of the MEMS-IMU; represents the heading angle of the main inertial navigation system of the aircraft; α is a dynamic weight coefficient; t represents the cumulative time of entering the straight flight state; represents the heading angle deviation integral term; k I is a dynamic integral gain decaying over time, and the formula is as follows: t ; wherein: represents the initial integral gain; τ represents the decay time constant; represents the minimum integral gain; t represents the cumulative time of entering the straight flight state.

[0011] Preferably, the error compensation quantity includes a zero offset compensation value and an attitude residual error; The seven-dimensional state space model is composed of a state equation and an observation equation, as follows: ; wherein: A represents a state transition matrix, used to describe the evolution law of the state vector from k-1 time to 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 represent the time; w k represents the process noise, and the covariance matrix is Q; v k ​represents observation noise, and the covariance matrix is R; state vector x [ δ φ , δ θ , δ ψ , b gx , b gy , b gz , ε ]; δ φ 、 δ θ 、 δ ψ respectively represent the estimated errors of roll, pitch and yaw angle; b gx 、 b gy 、 b gz respectively represent the three-axis gyro zero biases; ε represents the vibration coupling term.

[0012] Preferably, the weight coefficient in step S7 is α The formula is as follows: ; In the formula: λ represents an adjustment coefficient; when the three-axis acceleration root mean square (RMS (a)) is greater than 5 m / s 2 , the weight is reduced to below 0.2; RMS (a) = ; In the formula, 、 、 respectively represent the three-axis acceleration components at the i th sampling time, and N is the sampling window size.

[0013] Preferably, the position inversion accuracy verification criterion in step S8 is as follows: Error pos =|| P GNSS - P calc ||; In the formula: Error pos represents the position inversion error; P GNSS represents the GNSS measured target position; P calc represents the attitude solution inversion position; If Error pos <0.5m, determine that the attitude calculation accuracy is qualified; if Error pos >1m, the system triggers an alarm and automatically switches to the direct output mode of the main inertial navigation.

[0014] Compared with the prior art, the present application can achieve the following beneficial effects: The prior art does not consider the attenuation and delay of the shock absorption system on the attitude transmission, and it is difficult to adapt to the physical isolation scene of the pod-airframe; 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 traditional methods.

[0015] The existing single filtering or fusion method is prone to insufficient accuracy or calculation lag; the present application combines quaternion gradient descent fusion (40% higher in calculation efficiency than Kalman filter, quickly suppresses cumulative error) with Kalman filter error observation (real-time estimation of zero bias and attitude residual), ensures real-time response while stably controlling the attitude error within 0.5°, and balances accuracy and efficiency.

[0016] The prior art is prone to instability in complex environments such as severe vibration, and lacks effective precision verification means; the present application uses vibration adaptive weight adjustment (dynamically adjusts the weight of the main inertial navigation with the change of acceleration) to resist vibration interference, and builds a closed loop with GNSS position inversion verification to ensure the stability and accuracy of the attitude under complex working conditions, and significantly improves the engineering applicability. BRIEF DESCRIPTION OF DRAWINGS

[0017] 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 present application. DETAILED DESCRIPTION

[0018] In the following description, the same reference numerals are used to represent the same elements in the drawings. Where appropriate, the same reference numerals are used to represent the same elements in the drawings. Therefore, the detailed description will not be repeated.

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

[0020] The present application provides a MEMS-IMU dual-mode correction attitude measurement method for an unmanned aerial vehicle optoelectronic pod, which specifically comprises the following steps: S1: Constructing the vibration isolation-attitude transmission coupling model: combining the low-frequency vibration characteristics (1-5 Hz) 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 roll angle, pitch angle, and heading angle of the shock absorber, and an attenuation coefficient matrix is output for subsequent flight state recognition. The expression of the second-order transfer function model is as follows: ; In the formula: H ( s ) represents the transfer function of the shock absorption system (dimensionless); represents the natural frequency of the shock absorber (unit: rad / s), and the typical value is 2π×2 rad / s (corresponding to 2 Hz); represents the damping ratio (dimensionless), and the measured range is 0.1-0.3; s represents the Laplace complex variable (unit: s -1 ); 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; The attenuation coefficient matrix is specifically used in the attitude correction link of steps S3 and S4; the attenuation coefficient matrix is represented as: ; Among them: 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 function (representing the ratio of the output signal amplitude to the input signal amplitude when the signal with an angular frequency of ω passes through the shock absorption system).

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

[0022] Brief Description of the Principle: This step addresses the core issue of attitude data attenuation caused by the vibration damping system. Low-frequency vibrations (1-5 Hz) of the unmanned helicopter rotor force the electro-optical pod to use a vibration damper to isolate the vibration; however, the dynamic characteristics of the damper distort the attitude transmission process. The 1-5 Hz low-frequency vibrations of the unmanned helicopter rotor are transmitted through the fuselage to the input of the damper. The elastic deformation and damping dissipation of the damper cause amplitude attenuation and phase delay of the attitude signals (roll, pitch, and yaw) when transmitted to the MEMS-IMU of the electro-optical pod, forming a coupled link of vibration input, damping response, and attitude distortion. By establishing a second-order transfer function model (adaptive mathematical model), the amplitude attenuation and phase delay effects of the damper on roll, pitch, and yaw angles are quantified; for the first time, the damping ratio-natural frequency coupling parameter is introduced (…). , This addresses the shortcomings of traditional methods that neglect the frequency response characteristics of shock absorbers. Performance verification: Actual measurements show that the model's predicted attenuation error is ≤3% (frequency range 1-10 Hz), providing a physical basis for subsequent corrections.

[0023] S2: Flight Status Recognition: Based on the three-axis motion parameters collected by the MEMS-IMU of the optoelectronic pod, a dual mechanism of angular velocity threshold criterion and acceleration energy detection is designed. The two criteria adopt a hierarchical series and main-auxiliary fusion strategy to accurately distinguish between the UAV's high-maneuver turning and straight flight status, and output a status flag to trigger the corresponding correction module. The specific implementation logic is as follows: Specifically, by calculating the three-axis angular velocity output by the MEMS-IMU ω x , ω y , ω z The vector magnitude is compared with a preset threshold to achieve preliminary state discrimination (primary criterion), as shown in the following expression: 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 This represents the angular velocity of the MEMS-IMU in the roll direction, in rad / s, and the rotational speed around the X-axis. ω y This represents the pitch angular velocity of the MEMS-IMU, in rad / s, and the rotational speed about the Y-axis. ω z This represents the angular velocity of the MEMS-IMU in the heading direction, in rad / s, and the rotational speed about the Z-axis. like > ω th (ω th If the threshold is 0.5 rad / s, it is preliminarily determined as a large maneuver state, otherwise it is preliminarily determined as a straight-line state; Secondary confirmation (auxiliary criterion): When the preliminary determination is a large maneuver state, further verification is needed through the acceleration RMS value to prevent misjudgment.

[0024] The three-axis acceleration root mean square (RMS) is used as a vibration energy threshold to filter the residual vibration of the damping system that interferes with state recognition, and the expression is as follows: ; In the formula: represents the three-axis acceleration root mean square value, with a unit of m / s 2 , representing the strength of vibration energy; represents the roll direction acceleration component at the i-th moment, with a unit of m / s 2 , X-axis instantaneous acceleration; represents the pitch direction acceleration component at the i-th moment, with a unit of m / s 2 , Y-axis instantaneous acceleration; represents the heading direction acceleration component at the i-th moment, with a unit of m / s 2 , Z-axis instantaneous acceleration; N represents the sampling window size (dimensionless), and the larger the window, the better the smoothing effect; i represents the sampling time index, ranging from 1 to N, and the current sampling point in the window sequence number; If > ( is the vibration energy threshold, and the typical value is 2.0 m / s 2 ), the large maneuver state is maintained; if ≤ , it is determined as a false maneuver state (such as a transient angular velocity change caused by wind disturbance), and it is forced to switch to a straight-line state. When the preliminary determination is a straight-line state, if > and lasts for more than 3 seconds, it is determined as a vibrating straight-line flight, and the straight-line state is still maintained, but the weight adjustment in step S7 is triggered.

[0025] State output logic: output state flag (flight state flag bit) F mode ∈{0,1}, F mode =1 represents a large maneuver flight state, F mode =0 represents a straight-line flight state; output to the subsequent step S3 (large maneuver correction) or step S4 (straight-line correction) module; the pseudo code is as follows: if:

[0026] {if: > , F mode =1, #Confirm high maneuver status Else: F mode =0, #Pseudo-maneuver, forced straight line} else {if: > , F mode =0, # Vibrate the straight line, keep the straight line but reduce the weight. Else: F mode =0, #normal straight line} 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.

[0027] 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. The corrected formula 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. 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). Principle: MEMS-IMU is easily affected by factors such as vibration during high maneuvering flight, and low-frequency vibration error accumulates rapidly. This step solves the problem of MEMS-IMU low-frequency vibration error accumulation during high maneuvering. The high-precision characteristics of the main inertial navigation in roll angle and pitch angle are used to quickly respond to the attitude deviation between the current main inertial navigation and MEMS-IMU through the proportional term, and the integral term gradually eliminates the long-term cumulative error; At the same time, the decay coefficient obtained in step S1 is introduced to weight the proportional term, which avoids excessive correction caused by deviation error judgment between the main inertial navigation and MEMS-IMU due to factors such as shock absorption; The integral limit design (|∫e dt|≤10°) prevents integral saturation.

[0028] S4: Straight flight heading angle correction: For the problem of MEMS-IMU heading angle drift in the scene of unmanned aerial vehicle straight flight, when the flight state flag F mode =0 (straight flight state), the main inertial navigation heading angle ( ) is used to dynamically integrate and compensate the heading angle output by MEMS-IMU, and the cumulative error caused by gyro zero drift is suppressed; The corrected heading angle is input to the quaternion gradient descent fusion module of step S5; The dynamic integral gain k I ( t ) is introduced in the formula as follows: ; In the formula: represents the corrected heading angle, unit rad, output to the heading angle of S5; represents the original heading angle of MEMS-IMU, unit rad, that is, the uncorrected heading measurement value; represents the main inertial navigation heading angle of the aircraft, unit rad, which is the high-precision heading as the correction reference; α is a dynamic weight coefficient, that is, the weight from S7; t represents the cumulative time (unit s) of entering the straight flight state, which starts from the time of cutting into the straight state; represents the heading angle deviation integral term, unit rad·s, that is, the cumulative error after limit protection; k I ( t ) is a dynamic integral gain that decays with time, unit s - ¹, which is a real-time calculation value, controls the correction strength, and is designed as follows: ; In the formula: represents the initial integral gain (typical value 0.05, unit s - ¹), which represents the gain at the start of straight flight; τrepresents the time constant of the decay (typical value 300, unit s), represents the time required for the gain to decay to 37%; represents the minimum integral gain (typical value 0.01, unit s - ¹), ensuring that there is still a weak correction ability in the long term; t represents the cumulative time into the straight flight state (unit s), starting from the cut-in straight flight state.

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

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

[0031] S5: quaternion gradient descent fusion: to solve the singularity of Euler angles and the coupling problem of multi-sensor noise, the corrected attitude angle output by steps S3 and S4 is converted into attitude quaternion as the 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 attitude solution, realize high-precision attitude fusion and update; finally output the fused quaternion q fuse ; The time rate of change of the attitude quaternion is expressed as follows:

[0032] 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 ); denotes the norm (length) of the gradient, used for normalization to prevent too large steps; denotes the conjugate of the estimated quaternion; denotes 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 3are given by ; where q 0is the scalar part, q 1、 q 2、 q 3are the vector parts; The complete calculation steps are as follows: (1) Calculate the accelerometer gradient: ; (2) Calculate the magnetometer gradient: ; (3) Fuse the gradient: ; (4) Normalize the update: ; (5) Adaptive step size adjustment: ; where denotes the accelerometer gradient; denotes the magnetometer gradient; denotes the fused gradient; denotes the weight coefficient of the accelerometer gradient; denotes the weight coefficient of the magnetometer gradient; q k denotes the quaternion at the kth iteration; q k+1 denotes the quaternion at the k+1th iteration; β denotes the step size of gradient descent; β 0denotes the initial gradient step size, i.e. the reference step size in static or low vibration, β 0=0.1; λ denotes the step size adjustment coefficient, the stronger the vibration, the larger the step size, and the stronger the correction force, λ = 0.05.

[0033] Principle: Euler angles have singularity problem 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 based on the natural evolution of the quaternion driven by angular velocity, combined with the gradient information of accelerometer and magnetometer data, the quaternion is continuously corrected by gradient descent method; improve the adaptive mechanism of gradient step, dynamically adjust the step according to the vibration situation, and enhance the correction strength to suppress noise when the vibration is enhanced, so as to improve the stability of the attitude solution.

[0034] S6: Kalman filter error observation: in order to eliminate the time-varying error of MEMS-IMU zero offset, improve the long-term attitude solution accuracy, based on the fused quaternion output in step S5 q fuse As an observation input, a seven-dimensional state space model containing attitude residual and gyro zero offset is constructed, and the error compensation quantity is estimated and output in real time through Kalman filter; the error compensation quantity includes zero offset compensation value And attitude residual; The seven-dimensional state space model is composed of state equation and observation equation; as follows: ; In the formula: A State transition matrix (7x7) is used to describe the evolution rule of state vector from k-1 time to k time, and its elements are determined by attitude kinematics equation and zero offset random walk model (usually first-order Gauss-Markov process); H Observation matrix (4x7) is used to map state space to observation space; x State vector; z Observation vector; k- 1 and k Indicates the time; w k Process noise is used to describe model uncertainty, and its covariance matrix is Q; v k Observation noise is used to describe sensor measurement noise, and its covariance matrix is R; State vector x =[ δ φ , δ θ , δ ψ , b gx , b gy , b gz , ε ] New vibration coupling term ε ; δ φ, δ θ , δ ψ respectively represent the estimation errors of roll, pitch, and yaw angle; b gx , b gy , b gz respectively represent the three-axis gyro bias; ε represents the shock coupling term (dimensionless), which is used to model the influence of the vibration that is not completely isolated by the damping system on the attitude error.

[0035] S7: Dynamic weight self-adaptive adjustment: in order to optimize the reliability of the fusion of the main inertial navigation and the MEMS-IMU in the severe vibration environment, the correction weight of the main inertial navigation 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 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: ; In the formula: λ represents the adjustment coefficient (unit: s 2 / m), and the optimized value is 0.2, λ is dynamically adjusted by the bias compensation value and the attitude residual error; when RMS(a)>5m / s 2 , the weight is reduced to below 0.2; RMS(a)= ; In the formula, , , respectively represent the three-axis acceleration components at the i th sampling time, and N is the sampling window size.

[0036] Principle: under the condition of severe vibration, the relative motion between the unmanned aerial vehicle body and the photoelectric pod is intensified, and when the main inertial navigation attitude data is transmitted to the pod, a significant phase delay and amplitude distortion will be generated. The exponential weight decay law designed in this step can dynamically adjust the main inertial navigation weight according to the vibration energy: when the vibration energy is low (very small), it tends to 1, and the main inertial navigation fully plays the 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 for solving after being optimized by S5, so as to guarantee the attitude stability. Effect verification: in the strong vibration environment (RMS(a)=8 m / s 2 ), after adopting the dynamic weight adjustment strategy, the attitude angle error output by the system can still be controlled within the range of ≤0.8°.

[0037] S8: Real-time attitude output and verification: As the final step of system attitude solution and accuracy verification, the final high-precision attitude angles [φ final , θ final , ψ final ] of the optical-electric pod are outputted by integrating the error compensation amount of step S6 and the dynamic weight coefficient of step S7, and the external accuracy verification is performed through GNSS position inversion; the implementation method is as follows: S81: Error compensation amount application Step S6 Kalman filter output 7-dimensional state vector x =[ , , , b gx , b gy , b gz , ε ] is decomposed and processed: S811: Zero offset compensation (real-time feedback to S5): ; in the formula, represents the compensated gyro angular velocity; represents the original measured angular velocity of the gyroscope; δ gx , δ gy , δ gz respectively represent the zero offset compensation amount of the x, y and z axes of the gyroscope; S812: Attitude residual correction: = ; = ; = ; in the formula, , , respectively represent the corrected roll angle, pitch angle and yaw angle residual.

[0038] S82: Weighted fusion output: the final attitude angle is converted into Euler angle after the fusion quaternion output by S5 q fuse is weighted and fused with the attitude residual of S6: ; wherein: 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 the gradient descent method of S5 quaternion; α t ​) is the time-varying weight coefficient, that is, the dynamic weight coefficient output by S7; It is the roll angle estimation error, in rad, derived from the S6 Kalman filter state vector; It is the final output pitch angle, in rad, which is the final pitch attitude output of the S8 system; It is the pitch angle calculated by the gradient descent method, in rad, derived from the pitch angle obtained by the S5 quaternion gradient descent method; This is the pitch angle estimation error, in rad, derived from the S6 Kalman filter state vector; It is the final output heading angle, in rad, which is the final heading and attitude output of the S8 system; This is the heading angle calculated by the gradient descent method, in rad, derived from the heading angle obtained by the S5 quaternion gradient descent method. This is the heading angle estimation error, in rad, derived from the S6 Kalman filter state vector; The residual terms are compensated after weight adjustment to avoid overcorrection.

[0039] S83: Vibration coupling term suppression: 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 ε.

[0040] The detailed method for external accuracy verification via GNSS position inversion in step S8 is as follows: 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.

[0041] Input parameters: pod attitude angle [φ] final ,θ final ,ψ final ]; pod height h (laser rangefinder or barometer data, unit: m) pod GNSS position [ L uav , Buav , H uav (Latitude, Longitude, Altitude, unit: ° / m); Camera mounting matrix (Mounting angle between camera and IMU, fixed value) Calculation process: S841. Constructing the pod - Ground vector r b (Body coordinate system):

[0042] In the formula, r b The pod-to-ground vector (aircraft system) represents the vector pointing from the pod to the ground target; θ represents the final pitch angle θ. final ; Indicates the final roll angle φ final h is the height of the pod above the ground, whether measured by laser ranging or by air pressure. S842. Coordinate System Transformation: Convert the body coordinate system vector to the geographic coordinate system (Northeast ENU): ; in: r e It is a vector in a geographic coordinate system, specifically the Northeast-Eastern (ENU) coordinate system; The attitude matrix from the aircraft to the navigation system is composed of quaternions. q final The constructed 3×3 matrix.

[0043] S843: Target Position Calculation: Based on the pod's GNSS position, the geographic coordinates of the target point are calculated through geodetic coordinate transformation. ; Where: Δ L Indicates the change in latitude; Δ B Indicates the change in longitude; L UAV Indicates the real-time latitude of the pod. B UAV Indicates the real-time longitude of the pod. H UAV The real-time altitude of the pod is indicated by the GNSS receiver. L calc , B calc These represent the target latitude and longitude, respectively, derived from the inversion calculation results; Re The radius of the Earth (6,378,137 m) is used for radian-distance conversion; S844: Accuracy Verification Calculate the position error: Error pos =|| P GNSS -P calc ||; In the formula: Error pos represents the position inversion error, that is, the attitude solution accuracy evaluation index; P GNSS represents the GNSS measured target position, which is the real position as a reference; P calc represents the attitude solution inversion position, which is the calculated position based on the attitude output; If Error pos <0.5m, it is determined that the attitude solution accuracy is qualified; if Error pos >1m, the system triggers an alarm and automatically switches to a direct output mode (Bypass mode) of the main inertial navigation.

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

[0045] Principle: this step forms the engineering closed loop of the system; on the one hand, the optimization and compensation information of the upstream module are integrated to generate the final attitude angle which can be directly used for control and display; on the other hand, high-precision GNSS is introduced as an external reference source to evaluate the final accuracy of the entire attitude solution link. By comparing the target position inverted based on the attitude data with 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.

[0046] 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.

[0047] The advantages of the application are: 1. Dynamic correction architecture under physical isolation: a vibration isolation-attitude transmission coupling model is constructed to quantify the attenuation effect of the shock absorber on the attitude transmission; state recognition, segmented correction, and fusion verification processes (S2-S4) are adopted to 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.

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

[0049] 3. Strong reinforcement of engineering applicability: through vibration self-adaptive weight adjustment (S7) to adapt to complex vibration environment, combined with GNSS position inversion verification (S8) to ensure accuracy and reliability, and to strengthen the applicability and stability of the method in actual engineering scenarios.

[0050] It should be understood that various forms of flow shown above can be used to reorder, add or delete steps. For example, each step described in the present disclosure can be executed 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.

[0051] The above specific embodiments do not constitute a limitation on the scope of protection 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 modifications, equivalent replacements and improvements made within the spirit and principles of the present application shall be included in the scope of protection 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 comprising the following steps: S1: According to the low-frequency vibration characteristics of unmanned helicopter rotor and the dynamic parameters of shock absorber, a second-order transfer function model of shock absorption system is established, the amplitude attenuation and phase delay of attitude angle are quantified, and an attenuation coefficient matrix is output; the attitude angle includes roll angle, pitch angle and heading angle; S2: The flight state of unmanned aerial vehicle is accurately distinguished by angle velocity threshold criterion and acceleration energy detection, and a state flag is output; the flight state of unmanned aerial vehicle is large maneuvering turn or straight flight; S3: When the flight state of unmanned aerial vehicle is large maneuvering turn, the main inertial navigation is used to carry out proportional-integral compensation on roll angle and pitch angle; S4: When the flight state of unmanned aerial vehicle is straight flight, the main inertial navigation is used to carry out dynamic integral compensation on the heading angle output by MEMS-IMU, so as to suppress the cumulative error caused by gyro zero offset; the corrected heading angle is input into step S5; S5: The corrected attitude angle output by steps S3 and S4 is converted into attitude quaternion, the attitude quaternion is iteratively optimized by gradient descent method, and the fused quaternion is output; S6: The fused quaternion is taken as observation input to construct a seven-dimensional state space model, and the error compensation is estimated and output in real time by Kalman filtering; S7: According to the real-time three-axis acceleration root mean square collected by MEMS-IMU, the correction weight of main inertial navigation is dynamically adjusted; S8: According to the error compensation of step S6 and the dynamic weight coefficient of step S7, the final attitude angle of photoelectric pod is output, and external accuracy verification is carried out through GNSS position inversion. 2.The method of claim 1, wherein the method further comprises: determining a first attitude of the UAV based on the first attitude information; determining a second attitude of the UAV based on the second attitude information; and determining a third attitude of the UAV based on the first attitude and the second attitude. The expression of the second-order transfer function model is as follows: ； where: H s represents the shock system transfer function; represents the shock natural frequency; represents the damping ratio, in the range 0.1-0.3; s represents the Laplace complex variable;​ The attenuation coefficient matrix is represented as: ; wherein: represents the roll angle decay coefficient, ; represents the pitch angle decay coefficient, ; represents the yaw angle decay coefficient, ; is the magnitude of the complex function ; and ω is the angular frequency. 3.The method of claim 1, wherein the method further comprises: determining a first attitude of the UAV based on the first attitude information; determining a second attitude of the UAV based on the second attitude information; and determining a third attitude of the UAV based on the first attitude and the second attitude. The step S2 specifically comprises: comparing the vector module of the three-axis angular velocity output by the MEMS-IMU with a preset threshold to realize preliminary state discrimination. ω x , ω y , ω z ​ Triaxial angular velocity vector module: ; In the formula: represents the three-axis angular velocity synthesis module length, unit rad / s, representing the overall rotation intensity; ω x 、ω y 、ω z respectively represent the MEMS-IMU roll, pitch, heading direction angular velocity; If ω th , the preliminary determination is a large maneuver state, otherwise the preliminary determination is a straight-line state; ω th is a threshold value; when the preliminary determination is a large maneuver state, further verification is performed through the three-axis acceleration root mean square (RMS) value;​ The three-axis acceleration root mean square is used as vibration energy threshold to filter the interference of residual vibration of shock absorption system on state recognition, and the expression is as follows: ； In the formula: represents the three-axis acceleration root mean square value; , , respectively represent the roll, pitch, and yaw 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; 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 the preliminary identification is a straight line state, if and lasts more than 3 seconds, it is determined to be a shaking straight line flight, still maintaining a straight line state;​ the output state flag F mode ∈ {0,1}, F mode = 1 indicates a large maneuver state, F mode = 0 indicates a straight state.

4. The method of claim 1, wherein the method is a dual-mode correction method for a micro electro mechanical system (MEMS) - inertial measurement unit (IMU) of an unmanned aerial vehicle (UAV) opto-pod. The correction formula of step S3 is as follows: ； ； wherein: , respectively denote the corrected roll angle, pitch angle; , respectively denote the decay coefficients of the roll angle, pitch angle; , respectively denote the roll angle, pitch angle of the MEMS-IMU; denotes the proportional gain; denotes the integral gain; t denotes the integral time in s.

5. The method of claim 1, wherein the method is a dual-modality correction method for a MEMS-IMU of an unmanned aerial vehicle (UAV) optoelectronic pod. The correction formula of 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 formulae: represents an initial integral gain; τ represents a decay time constant; represents a minimum integral gain; t represents a cumulative time of entry into a straight flight state.

6. The method of claim 1, wherein the method is a dual-modality correction method for a MEMS-IMU of an unmanned aerial vehicle (UAV) optoelectronic pod. The error compensation includes zero offset compensation value and attitude residual error; The seven-dimensional state space model is composed of state equation and observation equation, which are as follows respectively: ； In the formula: A denotes a state transition matrix, used to describe the evolution rule of a state vector from k-1 time to k time; H denotes an observation matrix, used to map a state space to an observation space; x denotes a state vector; z denotes an observation vector; k- 1 and k denotes a time; w k denotes process noise, and the covariance matrix is Q; v k denotes observation noise, and the covariance matrix is R; state vector x [ δ φ , δ θ , δ ψ , b gx , b gy , b gz , ε ]; δ φ , δ θ , δ ψ denote the estimation errors of roll, pitch, yaw angle, respectively; b gx , b gy , b gz denote the three-axis gyro bias; ε denote the vibration coupling terms.

7. The method of claim 1, wherein the method is a dual-modality correction method for a micro electro mechanical system (MEMS) inertial measurement unit (IMU) of an unmanned aerial vehicle (UAV) opto-electrical pod. The weight coefficient in the step S7 α The formula is as follows: ； In the formula: λ represents a regulation coefficient; when the three-axis acceleration root mean square (RMS (a)) > 5 m / s 2 , the weight is reduced to 0.2 or less; RMS(a)= ; wherein, , , are the three-axis acceleration components at the i-th sampling time, and N is the sampling window size.

8. The method of claim 1, wherein the method is a dual-modality correction method for a MEMS-IMU of an unmanned aerial vehicle (UAV) optoelectronic pod. The position inversion accuracy verification criterion in step S8 is as follows: Error pos =|| P GNSS - P calc ||； wherein: Error pos represents the position inversion error; P GNSS represents the GNSS measured target position; P calc represents the attitude solution inversion position; If Error pos <0.5 m, determine 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.

Images