Method for gyro automatic drift compensation of high-precision optical axis motion control device

Abstract

This invention discloses a method for automatic gyroscope drift compensation in a high-precision optical axis motion control device, belonging to the field of servo turntable control technology. This invention uses a three-axis fiber optic gyroscope mounted on the pitch axis to sense spatial angular velocity. Fiber optic gyroscopes eliminate errors caused by friction from rotating mechanical parts, resulting in high angular velocity measurement accuracy and a wide range of measurable angular velocities. The laser gyroscope used in this invention has a velocity measurement range of ±600° / s and a minimum sensitive angular velocity of less than ±0.001° / s. Fiber optic gyroscopes have a wide operating temperature range, from -50℃ to 60℃, requiring no heating, and exhibit short start-up time and fast system response. This invention proposes a method for automatic gyroscope drift compensation in a high-precision optical axis motion control device, employing a two-stage coarse-fine compensation method to achieve rapid compensation for fiber optic gyroscope drift. It first performs a coarse compensation, followed by up to five (the number of times is configurable) fine compensations. The algorithm is simple and efficient, with short compensation time, and does not consume significant CPU resources for complex calculations, thus improving optical axis stability.

Description

Technical Field

[0001] This invention relates to the field of servo turntable control technology, and more specifically to a method for automatic gyroscope drift compensation in a high-precision optical axis motion control device. Background Technology

[0002] A servo-stabilized turntable is an optoelectronic device installed on weapon platforms such as unmanned aerial vehicles or armored vehicles. Its main function is to observe, identify, aim, and track targets. Since it is necessary to collect the velocity in inertial space, a gyroscope is often used as a velocity feedback element. Through stabilization loop control, the stability of the optical axis in inertial space is ensured.

[0003] Currently, there are several types of gyroscopes: mechanical gyroscopes, fiber optic gyroscopes, MEMS gyroscopes, and other types. There are many types of gyroscopes, and each type has its specific application scenarios and characteristics. However, gyroscopes all have a common characteristic: they drift with temperature, geographical coordinates, and time. If this drift is not suppressed, it will affect the stability and accuracy of optical axis aiming. Therefore, gyroscope drift needs to be compensated before each mission.

[0004] The most common practice now is manual gyroscope compensation. A manual compensation button is set on the control handle. The left and right buttons compensate for azimuth drift, and the up and down buttons compensate for pitch drift. The compensation effect is determined by observing the movement of the optical axis. This is time-consuming and labor-intensive. At the same time, only one direction can be compensated, and azimuth and pitch cannot be compensated at the same time. The compensation accuracy is low and the efficiency is low.

[0005] Currently, there are several methods for implementing automatic gyroscope compensation. A patent application (publication number CN106441366A) filed by the Luoyang Electro-Optical Equipment Research Institute of the Aviation Industry Corporation of China discloses a method for automatic gyroscope drift compensation in a two-axis, four-frame optoelectronic pod. This method keeps the pod stationary after startup and self-test. It calculates the angular velocities of the inner and outer frames relative to the pod using differentiation, then calculates the optical axis angular velocity, and finally calculates the compensation parameters. When the static stability of the optical axis azimuth and pitch is less than a set threshold, the compensation parameters are stored, and automatic compensation is exited. While this method is more advanced than manual compensation, calculating angular velocity through differentiation is time-consuming and prone to errors. Furthermore, this method uses an analog-input gyroscope, requiring analog-to-digital and digital-to-analog conversion, resulting in low compensation accuracy. If the optical axis stability consistently fails to fall below the threshold, the pod remains in compensation mode and cannot quickly exit compensation mode. Summary of the Invention

[0006] To address the aforementioned problems, this invention provides a method for automatic gyroscope drift compensation in a high-precision optical axis motion control device.

[0007] This invention is achieved through the following technical solution:

[0008] A method for automatic drift compensation of a gyroscope in a high-precision optical axis motion control device includes the following steps:

[0009] (1) After the system is powered on, it receives the host computer command to trigger the automatic drift compensation mode, records the current encoder initial position of the azimuth axis and pitch axis, and initializes the compensation parameters;

[0010] (2) Entering the coarse compensation stage: within the preset first time period, the azimuth and pitch angular velocity data output by the fiber optic gyroscope are collected, the average drift velocity of the two axes is calculated, the initial compensation parameters are generated and input into the control system;

[0011] (3) Enter the fine compensation stage: collect gyroscope angular velocity data in real time within the preset second time period. If the azimuth and pitch drift velocities are both less than or equal to the threshold, record the final compensation parameters and exit the compensation; otherwise, adjust the compensation parameters according to the step size and perform fine compensation iteratively until the threshold condition is met or the maximum number of iterations is reached.

[0012] (4) The encoder position information is updated in real time during the compensation process, and the compensation parameters are combined with the stabilization loop and the current loop to form a closed-loop control.

[0013] Preferably, the preset first duration of the coarse compensation stage in step (2) is 1.5 seconds, and the average drift velocity is calculated after accumulating to 1500 samples by a counter.

[0014] Preferably, the preset second duration of the fine compensation stage in step (3) is 4 seconds, the maximum number of iterations is 5, and the single compensation step size is 0.0001° / s.

[0015] Preferably, the fiber optic gyroscope is a three-axis digital fiber optic gyroscope that outputs digital angular velocity signals at a 1ms period via an RS422 interface, with a dynamic measurement range of ±600° / s and a minimum sensitive angular velocity of ±0.001° / s.

[0016] Preferably, the compensation parameter adjustment adopts a multi-axis synchronous compensation mechanism, which realizes independent iterative update of the dual-axis compensation parameters by solving the coupled drift components of the azimuth and pitch axes.

[0017] Preferably, the threshold is set to 0.001° / s.

[0018] Compared with the prior art, the beneficial effects of the present invention are:

[0019] This invention uses a three-axis fiber optic gyroscope mounted on the pitch axis to sense spatial angular velocity. Fiber optic gyroscopes eliminate errors caused by friction from rotating mechanical parts, resulting in high angular velocity measurement accuracy and a wide range of measurable angular velocities. The laser gyroscope used in this invention has a velocity measurement range of ±600° / s, with a minimum sensitive angular velocity of less than ±0.001° / s. The fiber optic gyroscope also boasts a wide operating temperature range, from -50℃ to 60℃, requiring no heating, and features short start-up time and fast system response.

[0020] The fiber optic gyroscope of this invention uses digital output carriers to output angular velocity information. It can output angular velocity information once every 1ms, eliminating the need for a precision analog-to-digital converter. It outputs speed digital signals through an RS422 digital interface, which can save costs and improve control accuracy.

[0021] This invention proposes a method for automatic gyroscope drift compensation in a high-precision optical axis motion control device. It adopts a two-stage coarse and fine compensation method to quickly compensate for fiber optic gyroscope drift. First, a coarse compensation is performed, followed by up to five (the number of times can be set) fine compensations. The algorithm is simple and efficient, with short compensation time. It can improve the stability of the optical axis without consuming a lot of CPU resources for complex calculations. Attached Figure Description

[0022] Figure 1 This is a block diagram of the automatic gyroscope compensation function of the present invention;

[0023] Figure 2 This is a block diagram of the automatic compensation startup of the gyroscope according to the present invention;

[0024] Figure 3 This is a flowchart of the automatic drift compensation algorithm of the present invention. Detailed Implementation

[0025] The present invention will be further described below with reference to the accompanying drawings:

[0026] As shown in the attached diagram of the instruction manual. Figure 1 As shown, the servo turntable based on this invention is a two-axis, two-frame opto-stabilized turntable. The servo turntable has a frame for each axis of orientation and pitch, and each frame has a motor and an encoder. The fiber optic gyroscope is mounted on the pitch axis.

[0027] After the stabilized turntable is powered on, it operates in a stable working mode. At this time, the stabilized turntable receives speed commands and the servo turntable operates in a dual closed loop of optical axis stabilization loop and current loop. The stabilization loop uses the angular velocity feedback closed loop uploaded by the fiber optic gyroscope. The gyroscope automatic drift compensation algorithm works together with the stabilization loop and the current loop to improve the stability of the optical axis.

[0028] As shown in the attached diagram of the instruction manual. Figure 2 As shown, the start and stop process of the gyroscope's automatic drift compensation is as follows:

[0029] After the system is powered on, it operates in a gyro-stabilized loop. Upon receiving a command from the host computer, it parses the command. When the received command is for automatic drift compensation, it records the current azimuth and pitch encoder positions, sets the compensation flag Acom to 1 (Acom is initialized to 0), sets the compensation state to Start, and starts the automatic compensation algorithm. When the received command is to exit automatic drift compensation, it sets the compensation flag Acom to 0, sets the compensation state to Start, and exits compensation.

[0030] As shown in the attached diagram of the instruction manual. Figure 3 As shown, the automatic gyroscope compensation algorithm is implemented as follows:

[0031] The system receives gyroscope angular velocity information every 1ms. When the compensation flag Acom is 1, it enters the gyroscope automatic compensation algorithm; otherwise, it records the compensation parameters, sets the compensation flag Acom to 0, and exits the automatic compensation process.

[0032] After entering automatic compensation, the current compensation state is determined. When the compensation state is Start, the counter TL_cnt is incremented by 1. When the counter TL_cnt ≥ 1500 (1.5 seconds), a coarse compensation is performed, calculating the average velocity of the azimuth and pitch frames, and solving for the drift velocity of the azimuth and pitch gyroscopes. The azimuth and pitch gyroscope compensation parameters are calculated and input into the system. The compensation state is set to ACT, the counter TL_cnt is cleared, and the positions of the azimuth and pitch encoders are recorded. The counter is cleared when entering a new compensation stage or exiting the compensation process.

[0033] When the compensation state is ACT, the counter TL_cnt is incremented by 1. When the counter TL_cnt≥4000, the compensation count is incremented by 1, and the compensation state is set to Check.

[0034] When the compensation state is Check, calculate the average velocity of the azimuth and pitch frames within 4 seconds and solve for the drift velocity of the azimuth and pitch gyroscopes. When the drift velocities of the azimuth and pitch gyroscopes are both less than or equal to 0.001° / s, record the compensation parameters, set the compensation flag Acom to 0, clear the compensation count N, clear TL_cnt, and exit compensation. Otherwise, enter fine compensation. When the azimuth gyroscope velocity is greater than 0.001° / s, adjust the azimuth gyroscope compensation parameters by one step (the step size can be adjusted according to the actual situation; in this example, the step size is set to 0.0001° / s) and input it into the system. Record the current encoder position. When the compensation count is less than or equal to 5 (which can be set), set the compensation state to ACT, clear the counter TL_cnt, and restart the 4-second run. When the pitch gyroscope speed is greater than 0.001° / s, the pitch gyroscope compensation parameter is adjusted by one step (the step size can be adjusted according to the actual situation; in this example, the step size is 0.0001° / s) and input into the system. The current encoder position is recorded. When the number of compensation attempts N is less than or equal to 5 (which can be set), the compensation state is set to ACT, the counter TL_cnt is cleared, and the 4-second run is restarted. Fine compensation can be performed a maximum of 5 times. The number of fine compensation attempts can be set according to actual use; in this example, 5 times are used until the compensation effect reaches a point where both the azimuth and pitch gyroscope drift speeds are less than or equal to 0.001° / s. The compensation parameters are then recorded, and compensation is exited. If the number of azimuth or pitch compensation attempts is greater than five, the compensation parameters are recorded, the compensation flag Acom is set to 0, the number of compensation attempts N is cleared, TL_cnt is cleared, and compensation is exited.

[0035] In summary, these are merely preferred embodiments of the present invention and are not intended to limit the scope of the invention. All equivalent variations and modifications made in accordance with the shape, structure, features and spirit described in the claims of the present invention should be included within the scope of the claims of the present invention.

Claims

1. A method for automatic drift compensation of a gyroscope in a high-precision optical axis motion control device, characterized in that: Includes the following steps: (1) After the system is powered on, it receives the host computer command to trigger the automatic drift compensation mode, records the current encoder initial position of the azimuth axis and pitch axis, and initializes the compensation parameters; (2) Entering the coarse compensation stage: within the preset first time period, the azimuth and pitch angular velocity data output by the fiber optic gyroscope are collected, the average drift velocity of the two axes is calculated, the initial compensation parameters are generated and input into the control system; (3) Enter the fine compensation stage: collect gyroscope angular velocity data in real time within the preset second time period. If the azimuth and pitch drift velocities are both less than or equal to the threshold, record the final compensation parameters and exit the compensation; otherwise, adjust the compensation parameters according to the step size and perform fine compensation iteratively until the threshold condition is met or the maximum number of iterations is reached. (4) The encoder position information is updated in real time during the compensation process, and the compensation parameters are combined with the stabilization loop and the current loop to form a closed-loop control; The compensation parameter adjustment adopts a multi-axis synchronous compensation mechanism, which realizes independent iterative update of dual-axis compensation parameters by solving the coupled drift components of azimuth and pitch axes.

2. The method for automatic gyroscope drift compensation in a high-precision optical axis motion control device according to claim 1, characterized in that: The preset first duration of the coarse compensation stage in step (2) is 1.5 seconds. The average drift velocity is calculated after the counter accumulates to 1500 samples.

3. The method for automatic gyroscope drift compensation in a high-precision optical axis motion control device according to claim 1, characterized in that: In step (3), the preset second duration of the fine compensation stage is 4 seconds, the maximum number of iterations is 5, and the single compensation step size is 0.0001° / s.

4. The method for automatic gyroscope drift compensation in a high-precision optical axis motion control device according to claim 1, characterized in that: The fiber optic gyroscope is a three-axis digital fiber optic gyroscope that outputs digital angular velocity signals at a 1ms period via an RS422 interface. Its dynamic measurement range is ±600° / s, and its minimum sensitive angular velocity is ±0.001° / s.

5. The method for automatic gyroscope drift compensation in a high-precision optical axis motion control device according to claim 1, characterized in that: The threshold is set to 0.001° / s.

Images