A hemispherical gyroscope phase-locked control optimization method and device

Abstract

The present invention discloses a method and device for optimizing phase-locked control of a hemispherical gyroscope. The method comprises: changing the mode azimuth of the hemispherical resonant gyroscope, collecting the phase-locked frequency in real time for each mode azimuth, and calculating the phase-locked optimized control force in real time based on the collected results. The phase-locked frequency and the phase-locked optimized control force are then substituted into an electrode drive excitation model to calculate a driving signal in real time and transmit it to the drive electrode. After the phase-locked frequency stabilizes, the stable phase-locked optimized control force corresponding to the mode azimuth is collected. A relationship model is constructed based on all mode azimuths and their corresponding stable phase-locked optimized control forces, and the relationship model is solved to obtain phase-locked optimized parameters. The phase-locked optimized control force is calculated based on the phase-locked optimized parameters and the relationship model, and the phase-locked optimized control force is substituted into the electrode drive excitation model to calculate the driving signal, thereby achieving compensation for the driving signal during normal operation of the hemispherical resonant gyroscope. The present invention can eliminate the influence of circumferential frequency difference on the output accuracy of the resonator.

Description

Technical Field

[0001] The present invention relates to the field of inertial navigation technology, and in particular to a hemispherical gyroscope phase-locked control optimization method and device. Background Art

[0002] A hemispherical resonator gyroscope (HRG) is an inertial sensing device with the advantages of small size, light weight, simple structure, and long life. It detects angular velocity by using the standing wave vibration of a hemispherical resonator. An electrostatic, electromagnetic, or piezoelectric actuator applies a sinusoidal electrostatic force to the edge of the resonator, causing it to vibrate at its natural frequency.

[0003] When the resonator vibrates, two signals are detected, modulated, and demodulated to obtain information such as their amplitude and phase. Combined calculations are then performed to determine the resonator's phase lock, amplitude, and quadrature error, thereby calculating the driving force required to maintain the resonator's vibration. Excitation control generally utilizes a combined drive method using X- and Y-path electrodes. Stable control of the phase-locked frequency directly determines the accuracy of the demodulation parameters, which in turn significantly impacts the resonator's output precision.

[0004] Due to the extremes of machining, the resonator will have a certain amount of frequency cracking, resulting in a circumferential frequency difference in the hemispherical resonator. In full-angle mode, when the external rotation occurs, the resonator vibration mode will rotate relative to the base due to inertia, and the resonant frequency will change. In particular, the greater the external angular velocity, the greater the rate of change of the resonant frequency, and the greater the tracking rate required for phase-locked control. However, due to the tracking delay of the phase-locked PID loop, short-term control errors will occur, and demodulation errors will also occur, resulting in sensitive angular velocity offsets. Summary of the Invention

[0005] In response to the problems in the background technology, the present invention proposes a hemispherical gyroscope phase-locked control optimization method to eliminate the influence of circumferential frequency difference on the output accuracy of the resonator, and also provides a device for implementing the above method.

[0006] The present invention adopts the following technical solutions:

[0007] A hemispherical gyroscope phase-locked control optimization method comprises the following steps:

[0008] S1: Change the vibration mode azimuth of the hemispherical resonant gyroscope, for each vibration mode azimuth , execute the phase-locked optimization control adjustment action, the phase-locked optimization control adjustment action includes: real-time acquisition of phase-locked frequency , and according to the collected phase-locked frequency and phase-locked optimization control algorithm to calculate phase-locked optimization control force in real time , and then the collected phase-locked frequency and the calculated phase-locked optimized control force Substitute it into the electrode drive excitation model to calculate the drive signal in real time and transmit it to the drive electrode. After the phase-locked frequency is stable, the azimuth angle of the vibration mode is collected. Corresponding stable phase-locked optimized control force ;

[0009] S2: Based on all vibration mode azimuths And its corresponding stable phase-locked optimization control force , construct a relationship model about the phase-locked optimization control force and the vibration mode azimuth angle, solve the relationship model, and obtain the phase-locked optimization parameters,

[0010] S3: Calculate the phase-lock optimization control force according to the phase-lock optimization parameters and the relationship model , and substituted it into the electrode drive excitation model to calculate the drive signal to achieve compensation of the drive signal when the hemispherical resonant gyroscope works normally.

[0011] Optionally, the phase-locked optimization control algorithm is specifically:

[0012] ,

[0013] in, Optimize the control force for the phase lock at the previous moment, and PID parameters for phase-locked optimization control algorithm;

[0014] , To set the target phase-locked frequency, is the phase-locked frequency at the current moment of acquisition.

[0015] Optionally, the initial mode shape azimuth In the corresponding phase-locked optimization control adjustment action, the initial When calculating Take 0, the next vibration mode azimuth corresponding phase-locked optimization control adjustment action, the initial When calculating Take the azimuth of the previous vibration mode The stable phase-locked optimization control force collected by the corresponding phase-locked optimization control adjustment action. Optionally, the relationship model is specifically:

[0016] ,in, Optimize parameters for phase locking.

[0017] Optionally, the electrode drive excitation model is:

[0018] ,

[0019] in, is the X electrode driving signal, is the Y electrode driving signal, is the phase-locked frequency, is the amplitude control force calculated by the amplitude control loop, is the orthogonal control force calculated by the orthogonal control loop, is the azimuth of the vibration mode of the oscillator, is the DDS driving signal reference phase, Optimize control force for phase locking;

[0020] In the driving signal calculation of step S3, the phase-locked frequency The phase-locked frequency calculated using the phase-locked loop.

[0021] Optionally, in step S1, the mode azimuth angle change rate of the hemispherical resonant gyroscope is 5° / s-25° / s, the acquisition frequency of the phase-locked frequency is 4000Hz-6000Hz, and the acquisition frequency of the phase-locked optimization control force is 80Hz-120Hz.

[0022] Optionally, the changing the vibration mode azimuth of the hemispherical resonant gyroscope specifically includes:

[0023] The hemispherical resonator gyroscope is placed on a turntable, and the turntable is rotated at a rate of 5° / s-25° / s to change the vibration mode azimuth of the hemispherical resonator gyroscope.

[0024] As a general inventive concept, the present invention further provides a device for implementing the above-mentioned hemispherical gyroscope phase-locked control optimization method, comprising:

[0025] An execution module is used to change the vibration mode azimuth of the hemispherical resonant gyroscope.

[0026] The acquisition module is used to collect the phase-locked frequency in real time and send it to the control chip.

[0027] The control chip is used to calculate the phase-locked optimization control force in real time for each mode azimuth according to the collected phase-locked frequency and the phase-locked optimization control algorithm, and then substitute the collected phase-locked frequency and the calculated phase-locked optimization control force into the electrode drive excitation model to calculate the drive signal in real time and transmit it to the drive electrode of the hemispherical resonant gyroscope until the phase-locked frequency is stable. The mode azimuth and the corresponding stable phase-locked optimization control force are sent to the host computer.

[0028] The host computer is used to construct a relationship model of the phase-locked optimization control force relative to the mode shape azimuth angle based on all mode shape azimuth angles and their corresponding stable phase-locked optimization control forces, solve the relationship model, obtain the phase-locked optimization parameters, and send them to the control chip;

[0029] The control chip is also used to calculate the phase-locked optimization control force according to the phase-locked optimization parameters and the relationship model, and substitute the calculated force into the electrode drive excitation model to calculate the drive signal, so as to achieve compensation for the drive signal when the hemispherical resonant gyroscope works normally.

[0030] Compared with the prior art, the advantages of the present invention are:

[0031] The hemispherical gyroscope phase-locked control optimization method of the present invention adds a phase-locked optimization control force to the driving signal. The phase-locked optimization control force is a function of the vibration mode azimuth angle, which can generate a force that actively adjusts the driving phase so that the frequency change and the phase change caused by the frequency split exactly offset each other, so that the phase-locked frequency is only positively correlated with the temperature change, and the influence of the circumferential frequency difference on the output accuracy of the resonator is eliminated. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] In order to make the present invention more easily understood, the present invention will be described in more detail with reference to the specific embodiments shown in the accompanying drawings. These drawings only depict typical embodiments of the present invention and should not be considered as limiting the scope of protection of the present invention.

[0033] Figure 1 This is a schematic diagram showing the change in the resonant frequency of the oscillator relative to time when an external angular velocity of 20 degrees / s is input.

[0034] Figure 2 This is a schematic diagram showing the resonator resonant frequency versus relative azimuth angle without temperature influence when the external angular velocity input is 20 degrees / s.

[0035] Figure 3 This is a system block diagram for calculating the a priori phase-locked control optimization force of the present invention.

[0036] Figure 4 This is a system block diagram of the phase-locked optimization control of the resonator according to the present invention.

[0037] Figure 5 Graph showing the relationship between the phase-locked optimization control force and the vibration mode azimuth angle according to an embodiment of the present invention.

[0038] Figure 6 A graph showing the test results of the resonator resonant frequency versus azimuth angle to enable the phase-locked optimization control function in an embodiment of the present invention. DETAILED DESCRIPTION

[0039] The following describes the embodiments of the present invention with reference to the accompanying drawings so that those skilled in the art can better understand the present invention and implement it. However, the enumerated embodiments are not intended to limit the present invention. Unless there is a conflict, the following embodiments and the technical features in the embodiments can be combined with each other, wherein the same components are represented by the same figure marks.

[0040] The vibration of the resonator causes the capacitance between the lip and the electrode to change. The circuit collects the signal of the capacitance change and sends it to the main control chip. After modulation and demodulation, the relevant controlled quantity is calculated, and the control force is generated through the loop control algorithm. After amplification by the DAC, it acts on the resonator electrode.

[0041] In full-angle mode, the vibration mode of the resonator rotates, and the vibration frequency of the resonator changes due to the existence of frequency cracking. At the same time, the phase-locked frequency of the resonator changes with temperature. Figure 1 and Figure 2 These are the schematic diagrams of the resonator resonant frequency changing with time and with azimuth angle without temperature influence when the external angular velocity input is 20 degrees / s.

[0042] In order to reduce the influence of control error caused by phase-locked PID control delay, this patent designs an electrode drive excitation model, which adds a phase-locked optimization control force to the drive signal. , the phase lock optimizes the control force is a function of the vibration mode azimuth, which can generate a force to actively adjust the driving phase so that the frequency change caused by the frequency split is consistent with The phase changes caused by this just cancel each other out, making the phase-locked frequency positively correlated only with temperature changes, eliminating the impact of circumferential frequency difference on the resonator output accuracy. The drive model is shown below:

[0043] ,

[0044] in, is the X electrode driving signal, is the Y electrode driving signal, is the phase-locked frequency, is the amplitude control force calculated by the amplitude control loop, is the orthogonal control force calculated by the orthogonal control loop, is the azimuth of the vibration mode of the oscillator, For time, is the DDS driving signal reference phase, Optimize control force for phase lock, .

[0045] In the above drive model, the phase-locked optimization control force and amplitude control in the same direction. When it is not 0, it will cause the phase of the driving signal to drift actively. When the phase is actively offset, the phase-locked error L will change, and the phase-locked loop will cause the resonant frequency to change. If the resonator vibration mode rotates, the phase-locked frequency will shift due to frequency cracking. However, if the rotation is too fast, the phase-locked PID will have a tracking delay, resulting in overshoot or lag in the phase-locked frequency adjustment. , a force that actively adjusts the driving phase can be generated to make the frequency change caused by the frequency splitting The phase changes caused by this just cancel each other out, so that the phase-locked frequency is only positively correlated with the temperature change, eliminating the influence of the circumferential frequency difference on the output accuracy of the resonator.

[0046] The following is to verify the phase-locked optimization control force The derivation process related to the vibration mode azimuth angle:

[0047] The phase-locked optimization control force will cause the time-domain phase shift of the main vibration mode. Assuming the shift rate is , the signal detection model can be written as follows:

[0048] ,

[0049] in, is the main vibration mode amplitude, is the secondary mode amplitude, is the angular frequency of hemispherical vibration, and are the relative phase-locked frequency differences with the resonant frequencies of the primary and secondary modes, respectively. The demodulation is performed with the reference signal, and the model is written as:

[0050]

[0051] remember

[0052]

[0053] The phase-locked error is calculated as

[0054]

[0055] When phase locked, L=0, then

[0056]

[0057] After stable control, q=0, at this time,

[0058]

[0059] That is, by adjusting The phase-locked frequency can be adjusted, and the phase-locked error L can be kept constant at 0 at the same time. That is, the phase-locked optimization control force is added to the driving signal, which can offset the influence of frequency cracking on the phase-locked state.

[0060] The following is the technical approach to determining the functional relationship model between the phase-locked optimization control force and the vibration mode azimuth angle:

[0061] The resonator frequency splitting is sinusoidally distributed with the azimuth angle, so the phase-locked optimization control force needs to be obtained a priori through test experiments. The flow chart of the method for obtaining the phase-locked control force a priori is as follows: Figure 3As shown in the figure, a phase-locked optimization control loop is added to the driving signal. When the resonant frequency is basically stable, a similar target resonant frequency value is set, the control loop is opened, the turntable speed is set to rotate, and the change in the output value of the frequency control force and the azimuth angle of the resonator vibration mode are obtained.

[0062] After the vibration mode rotates one circle, the relationship between the phase-locked optimization control force and the vibration mode azimuth angle can be obtained, and the output parameters of the phase-locked optimization control force can be obtained using the least squares model.

[0063] like Figure 4 As shown, the specific technical solutions are as follows:

[0064] A hemispherical resonant gyroscope is placed on a turntable, which rotates at a rate of 5° / s-25° / s to change the vibration mode azimuth of the hemispherical resonant gyroscope. For each vibration mode azimuth, the acquisition module collects the phase-locked frequency in real time and transmits it to the control chip. The control chip substitutes the phase-locked frequency into the phase-locked optimization control algorithm to calculate the phase-locked optimization control force in real time. The calculated phase-locked optimization control force is substituted into the electrode drive excitation model to calculate the drive signal in real time and transmit it to the drive electrode. After the phase-locked frequency stabilizes, the vibration mode azimuth and its corresponding phase-locked optimization control force are sent to the host computer.

[0065] The specific phase-locked optimization control algorithm is:

[0066] ,

[0067] in, Optimize the control force for the phase lock at the previous moment, and PID parameters for phase-locked optimization control algorithm;

[0068] , is the target phase-locked frequency, is the phase-locked frequency at the current moment of acquisition.

[0069] Initial adjustment of the first round of phase-locked optimization control Calculating, Take 0, the initial value of the next round of phase-locked optimization control adjustment Calculating, Take the last round of stable phase-locked optimization control force.

[0070] As the vibration mode azimuth of the hemispherical resonant gyroscope changes, the phase-locked frequency at the current moment Due to the presence of frequency cracking, the control chip will perform a new round of phase-locked optimization control adjustments, adjusting the frequency between 4000Hz and 6000Hz. This new round of adjustment and optimization control force will return the phase-locked frequency to the target resonant frequency. The current mode angle and the current output phase-locked optimization control force are collected and sent to the host computer via the serial port at a transmission rate of 80Hz to 120Hz.

[0071] Since the phase-locked optimization control adjustment process can converge in a very short time, the above-mentioned sending frequency can ensure that the phase-locked optimization control force sent to the host computer is the phase-locked optimization control force after the phase-locked frequency is stabilized.

[0072] The host computer constructs a relationship model of the phase-locked optimization control force relative to the vibration mode azimuth angle based on all vibration mode azimuth angles and their corresponding phase-locked optimization control forces:

[0073] ,

[0074] Solve the above relationship model to obtain the phase-locked optimization control force Output Parameters .

[0075] Therefore, the host computer will set the parameters The phase-locked optimization control force is transmitted to the control chip, and the control chip calculates the phase-locked optimization control force according to the phase-locked optimization parameters and the above-mentioned relationship model, and substitutes it into the electrode drive excitation model to calculate the drive signal to achieve compensation for the drive signal when the hemispherical resonant gyroscope is working normally. When the hemispherical resonant gyroscope is working normally, the phase-locked frequency in the drive model adopts the phase-locked frequency calculated by the phase-locked loop.

[0076] The following is a specific process of obtaining and compensating the phase-locked optimized control force using the present invention in a hemispherical resonant gyroscope:

[0077] S01. Place the hemispherical gyroscope on a turntable, power it on and test it normally for 30 minutes until the resonant frequency has no obvious temperature drift.

[0078] S02, set the turntable input to rotate at 20° / s, and open the phase-locked optimization control loop at the same time. The target phase-locked resonance frequency is set to 7414.2925Hz. After opening, the phase-locked frequency should not change with the change of the vibration mode azimuth angle. In the phase-locked optimization control algorithm of this example, =1, =100.

[0079] S03, record the phase-locked optimization control force and vibration mode azimuth, and stop the test after the vibration mode rotates one circle. Figure 5 As shown in Figure 2, it can be found that the phase-locked optimization control force has an obvious sinusoidal distribution relationship with the vibration mode azimuth angle.

[0080] S04. Obtain the model parameters of the phase-locked optimization control force relative vibration mode azimuth angle through least squares

[0081] , and stored in the control chip.

[0082] S05, then close the phase-locked optimization control loop, burn the phase-locked optimization compensation parameters into the control chip, and enable the phase-locked optimization control function. The test results are as follows: Figure 5 As shown in the figure (the sawtooth shape is phase-locked noise. The phase-locked frequency of the hemispherical resonant gyroscope will change each time it is turned on. The initial phase-locked frequency in this test is about 7414.4 Hz). Figure 2 and Figure 6 By comparison, it can be seen that the phase-locked frequency in the measured results only changes with temperature, which shows that the present invention achieves phase-locked control optimization and responds promptly to changes in the resonant frequency relative to the azimuth angle, making the control more stable.

[0083] The embodiments described above are merely preferred embodiments of the present invention. The phrases "in one embodiment," "in another embodiment," "in yet another embodiment," or "in other embodiments" used in this specification may refer to one or more of the same or different embodiments of the present disclosure. Any common changes and substitutions made by those skilled in the art within the scope of the present invention are intended to be encompassed within the scope of protection of the present invention.

Claims

1. A hemispherical gyroscope phase-locked control optimization method, characterized in that: The following steps are involved: S1: Change the vibration mode azimuth of the hemispherical resonant gyroscope , for each vibration mode azimuth, perform phase-locked optimization control adjustment action, the phase-locked optimization control adjustment action includes: real-time acquisition of phase-locked frequency , and according to the collected phase-locked frequency and phase-locked optimization control algorithm to calculate phase-locked optimization control force in real time , and then the collected phase-locked frequency and the calculated phase-locked optimized control force Substitute it into the electrode drive excitation model to calculate the drive signal in real time and transmit it to the drive electrode. After the phase-locked frequency is stable, the azimuth angle of the vibration mode is collected. Corresponding stable phase-locked optimized control force ; S2: Based on all vibration mode azimuths And its corresponding stable phase-locked optimization control force , construct a relationship model about the phase-locked optimization control force and the vibration mode azimuth angle, solve the relationship model, and obtain the phase-locked optimization parameters, S3: Calculate the phase-lock optimization control force according to the phase-lock optimization parameters and the relationship model , and substituted it into the electrode drive excitation model to calculate the drive signal to achieve compensation of the drive signal when the hemispherical resonant gyroscope works normally.

2. The hemispherical gyroscope phase-locked control optimization method according to claim 1, characterized in that: The specific phase-locked optimization control algorithm is: ， in, Optimize the control force for the phase lock at the previous moment, and PID parameters for phase-locked optimization control algorithm; , To set the target phase-locked frequency, is the phase-locked frequency at the current moment of acquisition.

3. The hemispherical gyroscope phase-locked control optimization method according to claim 2, characterized in that: Initial vibration mode azimuth In the corresponding phase-locked optimization control adjustment action, the initial When calculating Take 0, the azimuth angle of the next vibration mode In the corresponding phase-locked optimization control adjustment action, the initial When calculating Take the azimuth of the previous vibration mode The stable phase-locked optimization control force collected by the corresponding phase-locked optimization control adjustment action.

4. The hemispherical gyroscope phase-locked control optimization method according to claim 1, characterized in that: In step S2, the relationship model is specifically: ,in, Optimize parameters for phase locking.

5. The hemispherical gyroscope phase-locked control optimization method according to claim 1, characterized in that: In steps S1 and S3, the electrode drive excitation model is: ， in, is the X electrode driving signal, is the Y electrode driving signal, is the phase-locked frequency, is the amplitude control force calculated by the amplitude control loop, is the orthogonal control force calculated by the orthogonal control loop, is the azimuth of the vibration mode of the oscillator, is the DDS driving signal reference phase, Optimize control force for phase locking; In the driving signal calculation of step S3, the phase-locked frequency The phase-locked frequency calculated using the phase-locked loop.

6. The hemispherical gyroscope phase-locked control optimization method according to claim 1, characterized in that: In step S1, the rate of change of the mode azimuth angle of the hemispherical resonant gyroscope is 5° / s-25° / s, the acquisition frequency of the phase-locked frequency is 4000Hz-6000Hz, and the acquisition frequency of the phase-locked optimization control force is 80Hz-120Hz.

7. The hemispherical gyroscope phase-locked control optimization method according to claim 1, characterized in that: The changing of the vibration mode azimuth of the hemispherical resonant gyroscope specifically includes: The hemispherical resonator gyroscope is placed on a turntable, and the turntable is rotated at a rate of 5° / s-25° / s to change the vibration mode azimuth of the hemispherical resonator gyroscope.

8. A device for implementing the hemispherical gyroscope phase-locked control optimization method according to any one of claims 1 to 7, characterized in that: include: An execution module is used to change the vibration mode azimuth of the hemispherical resonant gyroscope. The acquisition module is used to collect the phase-locked frequency in real time and send it to the control chip. The control chip is used to calculate the phase-locked optimization control force in real time for each mode azimuth according to the collected phase-locked frequency and the phase-locked optimization control algorithm, and then substitute the collected phase-locked frequency and the calculated phase-locked optimization control force into the electrode drive excitation model to calculate the drive signal in real time and transmit it to the drive electrode of the hemispherical resonant gyroscope until the phase-locked frequency is stable. The mode azimuth and the corresponding stable phase-locked optimization control force are sent to the host computer. The host computer is used to construct a relationship model of the phase-locked optimization control force relative to the mode shape azimuth angle based on all mode shape azimuth angles and their corresponding stable phase-locked optimization control forces, solve the relationship model, obtain the phase-locked optimization parameters, and send them to the control chip; The control chip is also used to calculate the phase-locked optimization control force according to the phase-locked optimization parameters and the relationship model, and substitute the calculated force into the electrode drive excitation model to calculate the drive signal, so as to achieve compensation for the drive signal when the hemispherical resonant gyroscope works normally.

Images