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

Abstract

The invention discloses an automatic gyroscope drift compensation method for a high-precision optical axis motion control device, and belongs to the technical field of servo turntable control. According to the invention, a three-axis fiber-optic gyroscope is used and is installed on a pitch axis system to sense the space angular velocity. The fiber-optic gyroscope has no error caused by friction of a mechanical rotating part, the angular velocity measurement precision is high, the measured angular velocity range is large, the velocity measurement range of the laser gyroscope can reach + / -600 degrees per second, and the minimum sensitive angular velocity is smaller than + / -0.001 degrees per second. The operating temperature range of the fiber-optic gyroscope is wide from-50 DEG C to 60 DEG C, heating is not needed, the starting time is short, and the system response time is short. According to the automatic gyroscope drift compensation method of the high-precision optical axis motion control device, the drift of the fiber-optic gyroscope is rapidly compensated by adopting a coarse-fine two-stage compensation method, coarse compensation is firstly performed once, then fine compensation is performed at most five times (the times can be set), the algorithm is simple and efficient, the compensation time is short, and the method is suitable for large-scale popularization and application. The optical axis stability can be improved without consuming a large number of CPU resources to carry out complex calculation.

Description

Technical Field

[0001] The present invention relates to the technical field of servo turntable control, and particularly relates to a method for automatically compensating gyro drift of a high-precision optical axis motion control device. Background Art

[0002] A servo-stabilized turntable is an optoelectronic device installed on weapon platforms such as unmanned aircraft or combat vehicles. Its main functions are to observe, identify, aim at, and track targets. Since it is necessary to collect the speed in the inertial space, gyroscopes are often used as speed measurement feedback elements, and through stable loop control, the stability of the optical axis in the inertial space is ensured.

[0003] Currently, there are mainly the following types of gyroscopes: mechanical gyroscopes, fiber optic gyroscopes, MEMS gyroscopes, and other types of gyroscopes. There are a wide variety of gyroscopes, and each type of gyroscope has its specific application scenarios and characteristics. However, gyroscopes have a common characteristic that they will 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, gyro drift compensation needs to be performed before each mission.

[0004] Currently, the more common practice is manual gyro compensation. Manual compensation buttons are set on the control handle. The left and right buttons compensate for the drift in the azimuth direction, and the up and down buttons compensate for the drift in the pitch direction. The compensation effect is determined by observing the movement of the optical axis. It is time-consuming and laborious, and only one direction can be compensated at the same time. It cannot compensate for azimuth and pitch simultaneously, and the compensation accuracy is low and the efficiency is low.

[0005] Currently, there are also some implementation methods for automatic gyro compensation. The patent with the publication number CN106441366A applied by Luoyang Electro-Optical Equipment Research Institute of Aviation Industry Corporation of China discloses a method for realizing automatic gyro drift compensation of a two-axis four-frame optoelectronic pod. This method keeps the pod stationary after startup and self-check, calculates the angular velocity of the inner and outer frames of the pod relative to the pod through differentiation, then calculates the angular velocity of the optical axis, and finally calculates the compensation parameters. When the static stability of the optical axis in azimuth and pitch is less than the set threshold, the compensation parameters are stored and the automatic compensation is exited. Although this method is more advanced than manual compensation, calculating the angular velocity through differential position takes a long time and has a large error. In addition, the gyro used in this method is an analog input gyro, which requires analog-to-digital conversion and digital-to-analog conversion, and the compensation accuracy is not high. If the optical axis stability cannot be less than the threshold all the time, it will always be in the compensation state and cannot quickly exit the compensation state. Summary of the Invention

[0006] In order to solve the above problems, the present invention provides a method for automatically compensating gyro drift of a high-precision optical axis motion control device.

[0007] The present invention is realized through the following technical solutions: A method for automatic drift compensation of a gyroscope in a high-precision optical axis motion control device, comprising the following steps: (1) After the system is powered on, receive the host computer instruction to trigger the automatic drift compensation mode, record the initial positions of the encoders of the azimuth axis and the pitch axis, and initialize the compensation parameters; (2) Enter the coarse compensation stage: Collect the azimuth and pitch angular velocity data output by the fiber optic gyroscope within a preset first time period, calculate the average drift velocity of the two axes, generate the initial compensation parameters and input them into the control system; (3) Enter the fine compensation stage: Collect the gyro angular velocity data in real time within a preset second time period. If the azimuth and pitch drift velocities are both less than or equal to the threshold value, record the final compensation parameters and exit the compensation; otherwise, adjust the compensation parameters in steps and iteratively execute the fine compensation until the threshold condition is met or the maximum number of iterations is reached; (4) Update the encoder position information in real time during the compensation process, and form a closed-loop control in combination with the compensation parameters, the stable loop and the current loop.

[0008] Preferably, the preset first time period in the coarse compensation stage in step (2) is 1.5 seconds, and the average drift velocity calculation is completed after the counter accumulates to 1500 samples.

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

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

[0011] Preferably, the compensation parameter adjustment adopts a multi-axis synchronous compensation mechanism, and the independent iterative update of the biaxial compensation parameters is realized by solving the coupled drift components of the azimuth and pitch axes.

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

[0013] Compared with the prior art, the beneficial effects of the present invention are: The present invention uses a three-axis fiber optic gyroscope, which is installed on the pitch axis system to sense the space angular velocity. The fiber optic gyroscope has no errors caused by the friction of mechanical rotating parts, has high angular velocity measurement accuracy, and a large measured angular velocity range. The laser gyroscope used in the present invention has a speed measurement range of up to ±600° / s, and the minimum sensitive angular velocity is less than ±0.001° / s. The fiber optic gyroscope has a wide operating temperature range, from -50°C to 60°C, does not require heating, has a short startup time, and a fast system response time.

[0014] The fiber optic gyroscope of the present invention uses a digital quantity to output the angular velocity information of the carrier, and can output the angular velocity information once every 1 ms. Without a precision analog-to-digital converter, it outputs a speed digital signal through an RS422 digital interface, which can save costs and improve control accuracy.

[0015] The present invention proposes a method for automatically compensating the drift of a gyroscope in a high-precision optical axis motion control device. The method of coarse and fine two-stage compensation is adopted to quickly compensate for the drift of the fiber optic gyroscope. First, a rough compensation is performed, and then up to five times (the number of times can be set) of fine compensation are performed. The algorithm is simple and efficient, the compensation time is short, and it does not consume a large amount of CPU resources for complex calculations, and can improve the stability of the optical axis. Description of the Drawings

[0016] Figure 1 It is the functional block diagram of the gyroscope automatic compensation of the present invention; Figure 2 It is the start block diagram of the gyroscope automatic compensation of the present invention; Figure 3 It is the flow chart of the automatic drift compensation algorithm of the present invention. Detailed Embodiments

[0017] The present invention will be further described below with reference to the accompanying drawings: As shown in the accompanying drawings of the specification Figure 1 As shown, the servo turntable based on the present invention is a two-axis and two-frame optoelectronic stable turntable. There is one frame for each axial direction of the azimuth and pitch of the servo turntable. Each frame has one motor and one encoder, and the fiber optic gyroscope is installed on the pitch axis system.

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

[0019] As shown in the accompanying drawings of the specification Figure 2 As shown, the start and exit processes of the gyroscope automatic drift compensation are as follows: After the system is powered on, the system is in the operation of the gyroscope stable loop. After receiving the command from the host computer, the command from the host computer is parsed. When the received command is automatic drift compensation, the current azimuth and pitch encoder positions are recorded, the compensation flag Acom is set to 1 (Acom is initialized to 0), the compensation state is set to Start, and the automatic compensation algorithm is started; when the received command is to exit the automatic drift compensation, the compensation flag Acom is set to 0, the compensation state is set to Start, and the compensation is exited.

[0020] As shown in the accompanying drawings of the specification Figure 3 As shown, the implementation of the gyroscope automatic compensation algorithm is as follows: Receive gyro angular velocity information once every 1ms. When the compensation flag Acom is 1, enter the gyro automatic compensation algorithm; otherwise, record the compensation parameters, set the compensation flag Acom to 0, and exit the automatic compensation process.

[0021] After entering the automatic compensation, judge the current compensation status. When the compensation status is Start, increment the counter TL_cnt by 1. When the counter TL_cnt ≥ 1500 (1.5 seconds), perform a rough compensation, calculate the average speeds of the azimuth and elevation frames, calculate the azimuth and elevation gyro drift speeds, calculate the azimuth and elevation gyro compensation parameters and input them into the system, set the compensation status to ACT, clear the counter TL_cnt, and record the azimuth and elevation encoder positions. The counter is cleared when entering a new compensation stage or exiting the compensation process.

[0022] When the compensation status is ACT, increment the counter TL_cnt by 1. When the counter TL_cnt ≥ 4000, increment the compensation count by 1, and set the compensation status to Check; When the compensation status is Check, calculate the average speeds of the azimuth and elevation frames within 4s, and calculate the azimuth and elevation gyro drift speeds. When the azimuth and elevation gyro drift speeds 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 the compensation; otherwise, enter the fine compensation. When the azimuth gyro speed is greater than 0.001° / s, adjust the azimuth gyro compensation parameter by one step (the step size can be adjusted according to the actual situation, and the step size in this example 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 (can be set), set the compensation status to ACT, clear the counter TL_cnt, and run for 4s again. When the elevation gyro speed is greater than 0.001° / s, adjust the elevation gyro compensation parameter by one step (the step size can be adjusted according to the actual situation, and the step size in this example is 0.0001° / s) and input it into the system, record the current encoder position. When the compensation count N is less than or equal to 5 (can be set), set the compensation status to ACT, clear the counter TL_cnt, and run for 4s again. The fine compensation is performed at most 5 times, and the number of fine compensation times can be set according to actual use. In this example, it is used 5 times until the compensation effect reaches that the azimuth and elevation gyro drift speeds are both less than or equal to 0.001° / s, and then record the compensation parameters and exit the compensation. If the azimuth or elevation compensation count is greater than five, record the compensation parameters, set the compensation flag Acom to 0, clear the compensation count N, clear TL_cnt, and exit the compensation.

[0023] In summary, the above are only the preferred embodiments of the present invention and are not intended to limit the scope of implementation of the present invention. All equivalent changes and modifications made to the shape, structure, features and spirit described in the scope of 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 gyro drift compensation of a high-precision optical axis motion control device, characterized in that: The following steps are involved: (1) After the system is powered on, it receives the host computer command to trigger the automatic drift compensation mode, records the initial position of the encoder of the current azimuth axis and pitch axis, and initializes the compensation parameters; (2) Entering the coarse compensation stage: collecting the azimuth and pitch angular velocity data output by the fiber optic gyroscope within a preset first time period, calculating the average drift velocity of the two axes, generating initial compensation parameters and inputting them into the control system; (3) Entering the fine compensation stage: collect gyro angular velocity data in real time within the preset second time length. If the azimuth and pitch drift speeds are both less than or equal to the threshold, record the final compensation parameters and exit compensation; otherwise, adjust the compensation parameters according to the step size and iterate to perform fine compensation until the threshold condition is met or the maximum number of iterations is reached; (4) During the compensation process, the encoder position information is updated in real time, and the compensation parameters are combined with the stabilization loop and current loop to form a closed-loop control.

2. The method for automatic gyro drift compensation of 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, and the average drift speed calculation is completed after the counter accumulates 1500 samples.

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

4. The method for automatic gyro drift compensation of 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, which outputs a digital angular velocity signal with a period of 1 ms through an RS422 interface, has a dynamic measurement range of ±600° / s, and a minimum sensitive angular velocity of ±0.001° / s.

5. The method for automatic gyro drift compensation of a high-precision optical axis motion control device according to claim 1, characterized in that: The compensation parameter adjustment adopts a multi-axis synchronous compensation mechanism, and realizes independent iterative update of dual-axis compensation parameters by solving the coupled drift components of the azimuth and pitch axes.

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

Images