A design method for dead zone compensation and anti-interference control loop of hemispherical resonant gyroscope in full-angle mode

Abstract

The present invention relates to a method for designing a dead-zone compensation and anti-interference control loop for a hemispherical resonant gyroscope in full-angle mode. The method provides the hemispherical resonant gyroscope's amplitude, quadrature, velocity, and frequency-phase control variables based on signal demodulation. Based on Lynch's stochastic averaging theory, dynamic equations for the control variables are developed that take external interference into account. A measurement dead-zone model is constructed, and a disturbance observer is designed to estimate the composite interference. The estimated value is feedforward-compensated to the gyro control system to achieve dead-zone compensation and high-precision anti-interference control for the hemispherical resonant gyroscope. Furthermore, a design scheme for the hemispherical resonant gyroscope's amplitude, quadrature, velocity, and frequency-phase control loop is provided. The hemispherical resonant gyroscope's dead-zone compensation and anti-interference control loop designed in the present invention in full-angle mode can achieve high-precision estimation of the composite interference consisting of dead-zone error and external interference. The composite interference feedforward compensation and PI feedback control components are combined to form a composite control strategy to achieve gyro stability control.

Description

Technical Field

[0001] The invention relates to a method for designing a dead zone compensation and anti-interference control loop of a hemispherical resonant gyroscope in a full-angle mode, and belongs to the technical field of intelligent instruments and meters. Background Art

[0002] Hemispherical resonant gyroscopes (HRGs) are widely used in aerospace and navigation due to their high precision, high reliability, and long life. In full-angle mode, in particular, the gyroscope has a large dynamic range, making it suitable for highly maneuverable vehicles. However, when the gyroscope operates at low angular velocities, the transducer that senses output motion may not be able to detect changes in output, resulting in a measurement dead zone. Furthermore, the full-angle mode utilizes an open-loop control system, making it more susceptible to environmental loads such as vibration than the closed-loop force balance mode.

[0003] To address the measurement dead zone issue, there is currently no research on dead zone compensation for hemispherical resonant gyroscopes in full-angle mode. The paper "Application of Unknown Asymmetric Dead Zone Compensation Method in Three-Axis MEMS Gyroscope Control" (Zhuo Shufang, Huang Yanwei, He Yonghui, Guo Shinan, "Journal of Shandong University of Technology (Natural Science Edition)", 2020) uses a neural network to approximate the upper bound of the dead zone of the three-axis MEMS gyroscope and designs a feedforward control to compensate for the dead zone. However, the neural network calculations are complex, making it difficult to apply this strategy to hemispherical resonant gyroscopes. For external vibrations, adding a shock absorber is the mainstream solution in engineering applications. However, the shock absorber design increases the complexity of the structure. Summary of the Invention

[0004] Technical Problem: Taking into account the measurement dead zone and external vibration issues, this paper proposes a design method for dead zone compensation and interference rejection control loops for a HRG operating in full-angle mode. This method treats the unknown dead zone error and external interference as a composite disturbance, designs a disturbance observer to estimate the composite disturbance, and feeds the estimated value into the gyro control system for compensation. This achieves dead zone compensation and high-precision interference rejection control for the HRG. Furthermore, design schemes for the HRG's amplitude, quadrature, velocity, and frequency-phase control loops are presented.

[0005] Technical solution: The technical solution adopted by the present invention to solve its technical problem is: a method for designing a dead zone compensation and anti-interference control loop of a hemispherical resonant gyroscope in full-angle mode, comprising the following steps:

[0006] Step 1: Dynamic modeling of the HRG.

[0007] Assuming that the x-axis is the 0° electrode axis direction and the y-axis is the 45° electrode axis direction, in the full-angle mode, the vibration displacement equations of the equivalent mass point of the hemispherical resonator gyroscope in the x-direction and y-direction are:

[0008]

[0009] Among them, a is the main wave antinode, q is the orthogonal wave antinode, φ is the phase variable of the main wave, ω is the angular frequency of the hemispherical vibration, θ is the angle between the main wave antinode axis and the 0° electrode axis (x axis), and t is time.

[0010] Select the reference signal V with the same resonant frequency as the oscillator s With V c :

[0011]

[0012] Where A is the amplitude of the reference signal, φ r is the initial phase of the reference signal.

[0013] Using the reference signal V s With V c Perform multiplication demodulation on the x-axis and y-axis signals to obtain C x 、C y 、S x With S y The control variables E, Q, R, S, and L in full-width mode are

[0014]

[0015] Where, δφ=φ-φ r .

[0016] Therefore, the calculation can be obtained:

[0017]

[0018] According to Lynch's random average method theory, considering external interference, the dynamic equations of amplitude, orthogonality, speed, and frequency-phase control variables are:

[0019]

[0020] Where:

[0021]

[0022] Among them, f as 、f qc 、f qs 、f ac 、 for

[0023] The controller to be designed, d E d Q d θ d δφ are the external disturbances on each control variable, θ τ and θ ωare the angles between the damping principal axis and the stiffness principal axis, Ω is the input angular velocity, k is the angular gain, τ1 and τ2 are the decay times of the two cracking modes, ω1 and ω2 are the resonant frequencies of the two cracking modes, ω and Δω are the mean and variance of ω1 and ω2, respectively.

[0024] Step 2: Modeling the dead zone of the HRG.

[0025] Set the driving force f qc =0. f as 、f qc 、f qs The following dead zone nonlinear relationship is presented:

[0026]

[0027] Among them, u as 、u qc 、u qs is the signal to be designed, b as 、 b qc 、 b qs 、 b ac is the dead zone parameter.

[0028] f as 、f qc 、f qs It can also be written as

[0029]

[0030] Where:

[0031]

[0032] Substituting the dead zone model (7) into the first three formulas of the dynamic equation (5) of the control variable, equation (5) can be rewritten as

[0033]

[0034] The composite interference is

[0035] D δφ =d δφ

[0036] Step 3: Hemispherical resonant gyro composite disturbance estimation and composite control strategy.

[0037] Define the tracking error of the amplitude, quadrature, and velocity control loops as

[0038] e E =EE r ,e Q =QQ r ,e θ =θ-θ r (9)

[0039] Among them, E r is the reference signal of the amplitude control loop, Q r is the reference signal of the orthogonal control loop, θ r It is the reference signal of the speed control loop.

[0040] Considering the dynamic equation (8) of the control variable, the composite controller is designed as

[0041]

[0042] Among them, K P1 , K P2 , K P3 , K P4 , K I1 , K I2 , K I3 , K I4 is a positive constant to be designed, It's D E 、D Q 、D θ 、D δφ The estimated value of is obtained by the following disturbance observer:

[0043]

[0044] Where P1, P2, P3, and P4 are positive constants to be designed, and E1, Q1, θ1, and δφ1 are intermediate variables.

[0045] Step 4: Design the amplitude, quadrature, velocity, and frequency-phase control loops for the hemispherical resonant gyroscope.

[0046] The hemispherical resonant gyroscope control system consists of three parts: the resonator, the analog front end, and the FPGA. x 、f y The displacement signals x and y are amplified and converted into digital-to-analog and analog-to-digital signals. The FPGA implements the composite controller (7) (10) and the disturbance observer (11) in hardware.

[0047] Beneficial effects: Compared with the prior art, the present invention has the following beneficial effects:

[0048] (1) Aiming at the unknown measurement dead zone and external disturbance, a disturbance observer is designed to realize the online estimation of the composite disturbance composed of dead zone error and external disturbance.

[0049] (2) The composite disturbance estimation value is fed forward to the amplitude, quadrature, velocity, and frequency-phase control loops, and a design scheme for the hemispherical resonant gyroscope controller is proposed to achieve dead zone compensation and high-precision control under the influence of external disturbances.

[0050] (3) Combined with signal demodulation, a design scheme for the amplitude, quadrature, speed, and frequency-phase control loops of the hemispherical resonant gyroscope based on FPGA is given to achieve dead zone compensation, interference compensation, and high-precision control of the hemispherical resonant gyroscope. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 This is the vibration trajectory of the equivalent mass point of the hemispherical resonant gyroscope resonator in full-angle mode.

[0052] Figure 2 The invention discloses a design scheme for a hemispherical resonant gyroscope dead zone compensation and anti-interference controller.

[0053] Figure 3 The invention provides a design scheme for a hemispherical resonant gyroscope control system in full-angle mode. DETAILED DESCRIPTION

[0054] In order to deepen the knowledge and understanding of the present invention, the solution is described in detail below with reference to the accompanying drawings and implementation methods.

[0055] Example 1:

[0056] The specific embodiment of the method for designing a dead zone compensation and anti-interference control loop of a hemispherical resonant gyroscope in full-angle mode proposed by the present invention is as follows:

[0057] Figure 2 The dead zone compensation and anti-interference controller design scheme for the hemispherical resonant gyroscope designed by the present invention is as follows:

[0058] Step 1: Dynamic modeling of the HRG.

[0059] Figure 1 is the vibration trajectory of the equivalent mass point of the hemispherical resonator gyroscope in full-angle mode. Assuming that the x-axis is the 0° electrode axis direction and the y-axis is the 45° electrode axis direction, the vibration displacement equation of the equivalent mass point of the hemispherical resonator gyroscope in the x-direction and y-direction in full-angle mode is

[0060]

[0061] Among them, a is the main wave antinode, q is the orthogonal wave antinode, φ is the aroma variable of the main wave, ω is the angular frequency of hemispherical vibration, θ is the angle between the main wave antinode axis and the 0° electrode axis (x axis), and t is time.

[0062] Figure 1 Medium a and f q are the driving forces along the major and minor axes of the elliptical vibration trajectory, respectively, and

[0063]

[0064] Among them, f x and f y are the driving forces along the x-axis and y-axis respectively.

[0065] f in the following formula (5) as 、f ac The driving force f a Components on the phase and quadrature control loops, f qs 、f qc The driving force f q Components on the phase and quadrature control loops.

[0066] In full-angle mode, the control variables E, Q, R, S, and L of the hemispherical resonant gyroscope are:

[0067]

[0068] Where, δφ=φ-φ r ,φ r is the initial phase of the reference signal. In this embodiment, φ is set r =0rad.

[0069] According to the third and fourth equations of formula (3), the angle between the antinode axis of the main wave and the 0° electrode axis (x axis) can be calculated as

[0070]

[0071] According to Lynch's random average method theory, considering external interference, the dynamic equations of amplitude, orthogonality, speed, and frequency-phase control variables are:

[0072]

[0073] Where:

[0074]

[0075] Among them, f as 、f qc 、f qs 、f ac 、 for

[0076] The controller to be designed, d E d Q d θ d δφ are the external disturbances on each control variable, θ τ and θ ω are the angles between the damping axis and the stiffness axis, Ω is the input angular velocity to be measured, k is the angle gain, τ1 and τ2 are the decay times of the two cracking modes, ω1 and ω2 are the resonant frequencies of the two cracking modes, ω and Δω are the mean and variance of ω1 and ω2 respectively. In this embodiment, θ is set to τ =45.1°,θ ω =49.9°, k=1.0001, τ1=0.15s, τ2=0.16s, ω1=10rad / s, ω2=10.2rad / s.

[0077] Step 2: Modeling the dead zone of the HRG.

[0078] Design driving force qc =0N,f as 、f qc 、f qs The following dead zone nonlinear relationship is presented:

[0079]

[0080] Among them, u as 、u qc 、u qs is the signal to be designed, b as 、 b qc 、 b qs 、 b ac is the dead zone parameter. In this embodiment,

[0081] b as =0.5N, b qc =0.5N, b qs =0.5N, b ac =0.5N.

[0082] f as 、f qc 、f qs It can also be written as

[0083]

[0084] Where:

[0085]

[0086] Substituting the dead zone model (7) into the first three formulas of the dynamic equation (5) of the control variable, equation (5) can be rewritten as

[0087]

[0088] The composite interference is

[0089] D δφ =d δφ

[0090] Step 3: Hemispherical resonant gyro composite disturbance estimation and composite control strategy.

[0091] Define the tracking error of the amplitude, quadrature, and velocity control loops as

[0092] e E =EE r ,e Q =QQ r ,e θ =θ-θ r (9)

[0093] Among them, E r It is the reference signal of the amplitude control loop, which is generally selected as a positive constant; Q r is the reference signal of the orthogonal control loop, which is generally selected to be zero; θ r It is the reference signal of the speed control loop, which is generally selected as a constant. In this embodiment, E is set r =1.2, Q r =0,θ r =0.5.

[0094] Considering the dynamic equation (6) of the control variable, the composite controller is designed as

[0095]

[0096] Among them, K P1 , K P2 , K P3 , K P4 , K I1 , K I2 , KI3 , K I4 Is a positive constant to be designed. In this embodiment, K is set P1 =10, K P2 =10, K P3 =10, K P4 =10, K I1 =12, K I2 =12, K I3 =12, K I4 =12. It's D E 、D Q 、D θ 、D δφ The estimated value of is obtained by the following disturbance observer:

[0097]

[0098] Wherein, P1, P2, P3, and P4 are positive constants to be designed, and E1, Q1, θ1, and δφ1 are intermediate variables. In this embodiment, P1=20, P2=20, P3=20, and P4=20 are set.

[0099] According to the dynamic equation (5) of the control variable, the controller (7) (10) and the disturbance observer (11) are used to realize the dead zone compensation and anti-interference stability control of the hemispherical resonant gyroscope input. At this time, the measured angular rate is

[0100]

[0101] Here, θ0 is the initial value of θ. In this embodiment, θ0=0 rad is set.

[0102] Figure 3 The design scheme of the hemispherical resonant gyroscope control system in full-angle mode designed by the present invention. The hemispherical resonant gyroscope control system includes three links: resonator, analog front end, and FPGA. Among them, the resonator is the control object, and the analog front end controls the driving force f x 、f y The displacement signals x and y are amplified and converted into digital-to-analog and analog-to-digital signals. The FPGA implements the controller (7) (10) and the disturbance observer (11) in hardware.

[0103] It should be noted that the above embodiments are not intended to limit the scope of protection of the present invention, and equivalent replacements or substitutions made on the basis of the above shall fall within the scope of protection of the claims of the present invention.

Claims

1. A method for designing a dead zone compensation and anti-interference control loop for a hemispherical resonant gyroscope in full-angle mode, characterized in that: The method comprises the following steps: Step 1: Dynamic modeling of the HRG, based on Lynch's stochastic averaging theory, taking into account external disturbances, dynamic equations of amplitude, quadrature, velocity, and frequency-phase control variables; Step 2: Modeling the dead zone of the HRG. Construct a dead zone model, consider the unknown dead zone error and external disturbance as a composite disturbance, and design a disturbance observer to estimate the composite disturbance. Step 3: Feedforward the composite disturbance estimation value to the gyro control system and combine it with the PI feedback control part to form a composite control strategy to achieve dead zone compensation and high-precision anti-interference control of the hemispherical resonant gyroscope; Step 4: Provide the design scheme of the amplitude, quadrature, velocity, and frequency-phase control loops of the hemispherical resonant gyroscope; Among them, step 3: hemispherical resonant gyro composite interference estimation and composite control is as follows: Define the tracking error of the amplitude, quadrature, and velocity control loops as And E =EE r ,And Q =QQ r ,And θ =θ-θ r (9) Among them, E r is the reference signal of the amplitude control loop, Q r is the reference signal of the orthogonal control loop, θ r It is the reference signal of the speed control loop; Considering the dynamic equation (8) of the control variable, design u as 、u qc 、u qs 、 for Among them, K P1 , K P2 , K P3 , K P4 , K I1 , K I2 , K I3 , K I4 is a positive constant to be designed, It's D E 、D Q 、D θ 、D δφ The estimated value of is obtained by the following disturbance observer: Where P1, P2, P3, and P4 are positive constants to be designed, and E1, Q1, θ1, and δφ1 are intermediate variables.

2. The method for designing a dead zone compensation and anti-interference control loop for a hemispherical resonant gyroscope in full-angle mode according to claim 1, wherein: Step 1: Dynamic modeling of the hemispherical resonant gyroscope, as follows: Assuming that the x-axis is the 0° electrode axis direction and the y-axis is the 45° electrode axis direction, in the full-angle mode, the vibration displacement equations of the equivalent mass point of the hemispherical resonator gyroscope in the x-direction and y-direction are: Among them, a is the antinode of the main wave, q is the antinode of the orthogonal wave, φ is the phase variable of the main wave, ω is the angular frequency of the hemispherical vibration, θ is the angle between the antinode axis of the main wave and the 0° electrode axis, and t is time; Select the reference signal V with the same resonant frequency as the oscillator s With V c : Where A is the amplitude of the reference signal, φ r is the initial phase of the reference signal; Using the reference signal V s With V c Perform multiplication demodulation on the x-axis and y-axis signals to obtain C x 、C y 、S x With S y , the control variables E, Q, R, S, and L in full-width mode are Where, δφ = φ-φ r ; Therefore, the calculation can be obtained: According to Lynch's random average method theory, considering external interference, the dynamic equations of amplitude, orthogonality, speed, and frequency-phase control variables are: Where: Among them, f as 、f qc 、f qs 、f ac 、 is the controller to be designed, d E d Q d θ d δφ are the external disturbances on each control variable, θ τ and θ ω are the angles between the damping principal axis and the stiffness principal axis, Ω is the input angular velocity, k is the angular gain, τ1 and τ2 are the decay times of the two cracking modes, ω1 and ω2 are the resonant frequencies of the two cracking modes, ω and Δω are the mean and variance of ω1 and ω2, respectively.

3. The method for designing a dead zone compensation and anti-interference control loop for a hemispherical resonant gyroscope in full-angle mode according to claim 2, wherein: Step 2: Modeling the dead zone of the HRG is as follows: Set the driving force f qc =0;f as 、f qc 、f qs The following dead zone nonlinear relationship is presented: Among them, u as 、u qc 、u qs is the signal to be designed, b as 、 b qc 、 b qs 、 b ac is the dead zone parameter, f as 、f qc 、f qs It can also be written as Where: Substituting the dead zone model (7) into the first three formulas of the dynamic equation (5) of the control variable, equation (5) can be rewritten as The composite interference is 4. The method for designing a dead zone compensation and anti-interference control loop for a hemispherical resonant gyroscope in full-angle mode according to claim 1, wherein: Step 4: Design the amplitude, quadrature, velocity, and frequency-phase control loops for the hemispherical resonant gyroscope. The hemispherical resonant gyroscope control system consists of three parts: the resonator, the analog front end, and the FPGA. The resonator is the control object, and the analog front end controls the driving force f. x 、f y The detected displacement signals x and y are amplified and converted into digital-to-analog and analog-to-digital signals, and the FPGA realizes the composite controller (10) and the disturbance observer (11) in hardware.

Images