Self-stabilization control photoelectric tracking turntable and method based on rate gyroscope

Abstract

The invention discloses a self-stabilization control photoelectric tracking rotary table and method based on a rate gyroscope. The method comprises the following steps: respectively acquiring an azimuth compensation angular velocity and a pitching compensation angular velocity of the photoelectric tracking rotary table under a rotary table coordinate system and a roll angular velocity interference variable of the photoelectric tracking rotary table under a platform coordinate system through the rate gyroscope; constructing a rotation matrix, converting the roll angular velocity interference variable of the photoelectric tracking rotary table under the platform coordinate system into the roll angular velocity interference variable of the photoelectric tracking rotary table under the rotary table coordinate system, and performing Kalman filtering processing to obtain the optimal estimated angular velocity of the photoelectric tracking rotary table; and obtaining the control quantity of the photoelectric tracking turntable according to the optimal estimated angular velocity, and adjusting the control voltage of the photoelectric tracking turntable. According to the method, the process noise covariance matrix and the observation noise covariance matrix are updated by using the adaptive noise covariance matrix, so that the problem of filtering divergence or estimation lag caused by the fact that Kalman filtering adopting fixed noise covariance parameters is difficult to adapt to a dynamic environment is avoided.

Description

Technical Field

[0001] The present invention relates to the technical field of automatic control, and more particularly, to a self-stabilizing control optoelectronic tracking turntable and method based on a rate gyroscope. Background Art

[0002] The movement of a moving carrier has a significant impact on the tracking line of sight of an optoelectronic tracking and launching turntable system. Since three additional velocity and acceleration components in three directions, namely yaw, pitch, and roll, are generated on the infrared line of sight during the movement of the carrier, as Figure 2 shown. Therefore, it is necessary to use three rate gyroscopes to measure the carrier disturbance, and respectively measure the velocities in the additional azimuth, elevation, and horizontal axis tilt directions caused by the sway. The existing infrared tracking system does not have roll motion and can only cancel this component through azimuth motion. Therefore, by measuring the additional velocities of the carrier vibration on the two axes of infrared tracking in real time and accurately, and introducing the velocity signal as a feedforward signal into the control system, the vibration interference can be effectively suppressed and high-precision tracking of the target can be achieved.

[0003] Currently, optoelectronic self-stabilizing platforms mostly use rate gyroscopes as the core inertial sensors, but their angular velocity signals have the following limitations: one is that they are easily affected by high-frequency noise interference, and the other is that mechanical installation deviation or non-orthogonality will cause coordinate system conversion errors, thus affecting the accuracy of attitude calculation.

[0004] If the disturbance caused by vibration cannot be effectively measured and overcome, the tracking accuracy will be severely reduced and the target will be lost. Summary of the Invention

[0005] The purpose of the present invention is to provide a self-stabilizing control optoelectronic tracking turntable and method based on a rate gyroscope, which can solve at least one of the above-mentioned technical problems. The specific solutions are as follows:

[0006] According to a specific embodiment of the present invention, a self-stabilizing control method for an optoelectronic tracking turntable based on a rate gyroscope is disclosed in the first aspect of the present invention, including: respectively obtaining the azimuth compensation angular velocity, pitch compensation angular velocity of the optoelectronic tracking turntable in the turntable coordinate system, and the roll angular velocity disturbance of the optoelectronic tracking turntable in the platform coordinate system through a rate gyroscope;

[0007] Constructing a rotation matrix to convert the roll angular velocity disturbance of the optoelectronic tracking turntable in the platform coordinate system into the roll angular velocity disturbance of the optoelectronic tracking turntable in the turntable coordinate system;

[0008] Performing Kalman filtering on the azimuth compensation angular velocity, pitch compensation angular velocity, and roll angular velocity disturbance in the turntable coordinate system, adaptively adjusting the process noise covariance parameter and the observation noise covariance parameter in the Kalman filtering algorithm, and obtaining the optimal estimated angular velocity of the optoelectronic tracking turntable;

[0009] Using the position deviation between the actual measured position of the optoelectronic tracking turntable and the target position as the input of speed loop control, combining the optimal estimated angular velocity and the speed feedback information of the optoelectronic tracking turntable, to obtain the control quantity of the optoelectronic tracking turntable;

[0010] Adjust the control voltage of the optoelectronic tracking turntable according to the control quantity of the optoelectronic tracking turntable to stabilize the optoelectronic tracking turntable.

[0011] Preferably, the turntable coordinate system takes the rotation center of the turntable as the origin, the motion direction of the optoelectronic tracking turntable as the x-axis, and the direction perpendicular to the motion direction in the sky as the z-axis; the plane perpendicular to the z-axis of the turntable coordinate system is the azimuth frame, the azimuth gyro for measuring the azimuth angle of the optoelectronic tracking turntable is set at the position of the azimuth frame, and the measuring axis of the azimuth gyro is parallel to the plane where the azimuth frame is located; the pitch gyro for measuring the pitch angle of the optoelectronic tracking turntable is set at the position of the azimuth frame, and the measuring axis of the pitch gyro is perpendicular to the plane where the azimuth frame is located;

[0012] The platform coordinate system takes an arbitrary fixed point of the optoelectronic tracking turntable as the origin, the motion direction of the optoelectronic tracking turntable as the x-axis, the span direction of the transverse two ends of the optoelectronic tracking turntable as the y-axis, and the direction perpendicular to the optoelectronic tracking turntable in the sky as the z-axis; the heading gyro for measuring the roll angle of the optoelectronic tracking turntable is set on the optoelectronic tracking turntable, and the measuring axis of the heading gyro is parallel to the motion direction of the optoelectronic tracking turntable.

[0013] Preferably, obtaining the azimuth compensation angular velocity, pitch compensation angular velocity of the optoelectronic tracking turntable in the turntable coordinate system and the roll angular velocity disturbance of the optoelectronic tracking turntable in the platform coordinate system includes:

[0014] Converting the angle data of the azimuth gyro, pitch gyro and heading gyro through a servo control algorithm to obtain the azimuth compensation angular velocity, pitch compensation angular velocity of the optoelectronic tracking turntable in the turntable coordinate system and the roll angular velocity disturbance of the optoelectronic tracking turntable in the platform coordinate system.

[0015] Preferably, constructing a rotation matrix to convert the roll angular velocity disturbance of the optoelectronic tracking turntable in the platform coordinate system into the roll angular velocity disturbance of the optoelectronic tracking turntable in the turntable coordinate system includes:

[0016] Construct a rotation matrix R:

[0017]

[0018] The roll angular velocity of the optoelectronic tracking turntable in the turntable coordinate system is expressed as:

[0019] θ tA =Rθ p A

[0020] Wherein, t represents the turntable coordinate system, p represents the platform coordinate system, A represents the heading gyroscope,

[0021] α represents the azimuth angle of the platform coordinate system relative to the turntable coordinate system;

[0022] β represents the pitch angle of the platform coordinate system relative to the turntable coordinate system;

[0023] γ represents the roll angle of the platform coordinate system relative to the turntable coordinate system.

[0024] Preferably, the steps of Kalman filter processing include:

[0025] Set the Kalman filter state vector matrix X, the process noise covariance matrix Q k associated state equation and the observation noise covariance matrix R k associated observation equation z k ;

[0026] Predict the Kalman filter state and the error covariance P k ;

[0027] According to the predicted error covariance observation matrix H k and the observation noise covariance matrix R k calculate the Kalman gain K at time k k : According to the Kalman gain K at time k k update the predicted Kalman filter state and the error covariance P k ;

[0028] Update the process noise covariance matrix Q k and the observation noise covariance matrix R k through an adaptive noise covariance matrix;

[0029] The expression of the adaptive noise covariance matrix ω k is:

[0030] The expression for updating the process noise covariance matrix is: Q k =τQ k-1 +(1 - τ)ζω k v k T

[0031] The expression for updating the measurement noise covariance matrix is: Rk = σR k-1 + (1 - σ)δω k ω k T

[0032] where τ, ζ, σ = 0.9 and δ is an adjustment parameter.

[0033] Preferably, set the Kalman filter state vector matrix X, the process noise covariance matrix Q k associated state equation, and the observation equation z k associated with the observation noise covariance matrix R k , including:

[0034] Set the Kalman filter state vector matrix

[0035] where the angular variable angular velocity variable angular acceleration

[0036] In the turntable coordinate system, the expression of the state equation is:

[0037]

[0038] where T is a 3×3 diagonal matrix of the sampling period t;

[0039] ε is the process noise, a Gaussian distribution with a dive mean of 0 and a process noise covariance matrix Q k ;

[0040] In the turntable coordinate system, the expression of the observation equation is:

[0041]

[0042] where z k is the observation value, including the measured values of the angle and angular velocity;

[0043] v k is the observation noise, following a Gaussian distribution with a mean of 0 and an observation noise covariance matrix R k .

[0044] Preferably, predict the Kalman filter state and the error covariance P k , including:

[0045] Predicted Kalman filter state:

[0046] Predicted error covariance:

[0047] Among them, T is a 3×3 diagonal matrix of the sampling period t;

[0048] λ is a forgetting factor, where 0 < λ < 1.

[0049] Preferably, the expression of the Kalman gain is:

[0050]

[0051] The updated Kalman filter state has the following expression:

[0052]

[0053] The updated error covariance P k has the following expression:

[0054] P k =(I - K k H k )P k-1 ,

[0055] Among them,

[0056] Preferably, using the position deviation between the actual measurement position and the target position of the optoelectronic tracking turntable as the input of the speed loop control, and combining the optimal estimated angular velocity and the speed feedback information of the optoelectronic tracking turntable to obtain the control quantity of the optoelectronic tracking turntable, including:

[0057] Obtain the position deviation between the actual measurement position and the target position of the optoelectronic tracking turntable;

[0058] Perform a differential operation on the generated position information of the optoelectronic tracking turntable according to the output signal of the speed loop control to obtain the feedback signal for the speed loop control;

[0059] Input the feedback signal of the speed loop control, the optimal estimated angular velocity, and the position deviation between the actual measurement position and the target position of the optoelectronic tracking turntable into the speed loop control to obtain the real-time control quantity of the optoelectronic tracking turntable.

[0060] According to the specific embodiments disclosed in the present invention, a second aspect of the present invention discloses a self-stabilizing control optoelectronic tracking turntable based on a rate gyroscope, including:

[0061] Input unit: respectively obtain the azimuth compensation angular velocity, pitch compensation angular velocity of the optoelectronic tracking turntable in the turntable coordinate system and the roll angular velocity disturbance amount of the optoelectronic tracking turntable in the platform coordinate system through a rate gyroscope;

[0062] Conversion unit: Construct a rotation matrix to convert the roll angular velocity disturbance of the optoelectronic tracking turntable in the platform coordinate system into the roll angular velocity disturbance of the optoelectronic tracking turntable in the turntable coordinate system;

[0063] Kalman filter unit: Perform Kalman filtering on the azimuth compensation angular velocity, pitch compensation angular velocity, and roll angular velocity disturbance in the turntable coordinate system, and adaptively adjust the process noise covariance parameter and observation noise covariance parameter in the Kalman filtering algorithm to obtain the optimal estimated angular velocity of the optoelectronic tracking turntable;

[0064] Velocity loop control unit: Use the position deviation between the actual measured position and the target position of the optoelectronic tracking turntable as the input of the velocity loop control, and combine the optimal estimated angular velocity and the velocity feedback information of the optoelectronic tracking turntable to obtain the control quantity of the optoelectronic tracking turntable;

[0065] Drive unit: Adjust the control voltage of the optoelectronic tracking turntable according to the control quantity of the optoelectronic tracking turntable to stabilize the optoelectronic tracking turntable.

[0066] Compared with the prior art, the above scheme disclosed by the present invention has at least the following beneficial effects:

[0067] The present invention updates the process noise covariance matrix Q k and the observation noise covariance matrix R k by using an adaptive noise covariance matrix, avoiding the problems of filtering divergence or estimation lag caused by the difficulty of the Kalman filter with fixed noise covariance parameters in adapting to a dynamic environment. At the same time, by integrating with the velocity loop control, the dynamic accuracy and anti-interference ability of the system are significantly improved. Description of the drawings

[0068] The drawings herein are incorporated into the specification and form a part of this specification, showing embodiments consistent with the disclosure of the present invention, and are used together with the specification to explain the principles of the disclosure of the present invention. Obviously, the drawings in the following description are only some embodiments of the disclosure of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts. In the drawings:

[0069] Figure 1 is a flowchart of a self-stabilization control method for an optoelectronic tracking turntable based on a rate gyroscope according to an embodiment of the present invention;

[0070] Figure 2 is a schematic diagram of the three-direction motion components generated by the optoelectronic tracking turntable on the infrared line of sight in the prior art;

[0071] Figure 3 is a schematic diagram of the azimuth frame and pitch frame according to an embodiment of the present invention;

[0072] Figure 4 It is a schematic diagram of the deflection angle between the platform coordinate system and the turntable coordinate system in the embodiment of the present invention;

[0073] Figure 5 It is a schematic diagram of the principle of the speed loop control unit in the embodiment of the present invention;

[0074] Figure 6 It is a schematic diagram of the structure of the self-stabilizing control optoelectronic tracking turntable based on a rate gyro in the embodiment of the present invention;

[0075] Figure 7 It is a schematic diagram of the structure of the electronic device provided in this embodiment. Specific embodiments

[0076] In order to make the purpose, technical solutions and advantages of the present invention disclosure clearer, the present invention will be further described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention disclosure, rather than all of the embodiments. Based on the embodiments disclosed in the present invention, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the scope of protection of the present invention disclosure.

[0077] The terms used in the embodiments of the present invention disclosure are only for the purpose of describing specific embodiments, and are not intended to limit the present invention disclosure. The singular forms "a", "the" and "said" used in the embodiments of the present invention disclosure and the appended claims are also intended to include the plural forms, unless the context clearly indicates otherwise. "Plural" generally includes at least two.

[0078] It should be understood that the term "and / or" used herein is only a description of the association relationship of associated objects, indicating that there can be three relationships. For example, A and / or B can represent: A exists alone, A and B exist simultaneously, and B exists alone. In addition, the character " / " in this article generally represents an "or" relationship between the associated objects before and after.

[0079] It should be understood that although terms such as first, second, and third may be used in the embodiments of the present invention disclosure for description, these descriptions should not be limited to these terms. These terms are only used to distinguish the descriptions. For example, without departing from the scope of the embodiments of the present invention disclosure, the first can also be called the second, and similarly, the second can also be called the first.

[0080] Depending on the context, as used herein, the terms "if" and "when" may be interpreted as "when", "while", "in response to determining", or "in response to detecting". Similarly, depending on the context, the phrases "if determined" or "if detecting (stated condition or event)" may be interpreted as "when determined", "in response to determining", "when detecting (stated condition or event)", or "in response to detecting (stated condition or event)".

[0081] It should also be noted that the term "comprising", "including" or any other variant thereof is intended to cover non-exclusive inclusion, such that a commodity or device comprising a series of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such commodity or device. Without further limitation, an element defined by the statement "comprising an..." does not exclude the presence of additional identical elements in the commodity or device comprising said element.

[0082] The optional embodiments disclosed in the present invention will be described in detail below with reference to the accompanying drawings.

[0083] Embodiment 1

[0084] An optoelectronic tracking turntable is a complex system integrating optics, mechanics, electronics, and automation technologies, and is widely used in military, aerospace, meteorological observation, and other fields. Through the coordinated operation of a gyroscope and a motor, it is ensured that the line of sight (the aiming line of the optical device) of the optoelectronic tracking turntable remains stable when the carrier is in motion. Usually, three rate gyros are used to measure the carrier perturbation, respectively measuring the perturbations existing in the yaw, pitch, and roll coordinate axes directions of the optoelectronic payload on the load platform, such as an aircraft or a ship. By measuring the angular velocity components in the three coordinate axes directions, and then calculating the perturbation angular velocities of the yaw (azimuth) and pitch (elevation) of the optoelectronic tracking turntable, the compensation angular velocities of the servo system in the azimuth and elevation directions can be obtained.

[0085] Since the optoelectronic tracking turntable can only generate motion in the yaw and pitch directions and cannot move in the roll direction, it can only be offset by the azimuth motion component. Therefore, in this embodiment, the installation positions of the three-degree-of-freedom rate gyros are set so that there is no coupling relationship between the perturbation signals, eliminating the need for complex coordinate transformation and decoupling. After decomposing the three-degree-of-freedom measurement signals, they are directly introduced into the speed loop control unit to achieve high-precision feedforward composite control and line-of-sight stability. Moreover, fiber optic gyros are selected as the rate gyros, which have strong shock resistance and anti-acceleration motion capabilities, a long service life, and stable signals, and can well adapt to high-precision application environments.

[0086] The specific working process is as follows: When the rate gyro senses the disturbance of the optoelectronic tracking turntable, after signal amplification and DC torque calculation, the rate gyro outputs a feedback signal; then, through the servo control algorithm, the sensor data is processed and converted to obtain the required control voltage, which drives the DC torque motor to generate a counteracting torque that can overcome the disturbance torque and stabilize the platform.

[0087] In an embodiment of the present invention, the installation positions of the three-degree-of-freedom rate gyros are determined by establishing a coordinate system as Figure 3 shown. Among them, the two-dimensional plane perpendicular to the Z-axis is the azimuth frame, the two-dimensional plane perpendicular to the Y-axis is the pitch frame, and the pitch frame and the azimuth frame are perpendicular to each other and intersect.

[0088] Specifically, as Figure 4 shown, the origin of the turntable coordinate system is the rotation center of the turntable, and the origin of the platform coordinate system is an arbitrary fixed point of the optoelectronic tracking turntable. It is set that the origins of the two coordinate systems coincide.

[0089] In the platform coordinate system p, the x p axis is along the longitudinal direction of the load platform, that is, the forward direction of the optoelectronic tracking turntable (taking an aircraft as an example, the direction of the line connecting the nose and the tail); the y p axis is along the transverse extension direction of the optoelectronic tracking turntable (taking an aircraft as an example, the direction of the line connecting the two wing tips); the z p axis is the direction perpendicular to the plane where the optoelectronic tracking turntable is located (taking an aircraft as an example, the plane formed by the line connecting the two wing tips and the line connecting the nose and the tail is the plane where the optoelectronic tracking turntable is located).

[0090] In the turntable coordinate system t, the direction of the z t axis is the same as the direction of the z p axis. The direction of the x t axis is the movement direction of the optoelectronic tracking turntable, which is also the direction of the azimuth axis of the turntable. The plane where the azimuth axis is located and perpendicular to the z t axis is the plane where the pitch frame is located. The direction of the y t axis is the direction of the pitch axis of the turntable. The plane where the pitch frame is located is perpendicular to the plane where the azimuth frame is located, and at the same time, the plane where the pitch frame is located is perpendicular to the pitch axis. The rotation angle of the pitch frame around the pitch axis is the pitch angle, and the rotation angle of the azimuth frame around the z t axis is the azimuth angle. The optoelectronic tracking turntable cannot rotate around the azimuth axis of the turntable, the x t axis direction, so the optoelectronic tracking turntable can only move in the yaw and pitch directions and cannot move in the roll direction.

[0091] In this embodiment, the azimuth gyro for measuring the azimuth angle of the optoelectronic tracking turntable is arranged at the azimuth frame position, and the measuring axis of the azimuth gyro is parallel to the plane where the azimuth frame is located, that is, the measuring axis of the azimuth gyro is perpendicular to the z tThe axis. The pitch gyro for measuring the pitch angle of the optoelectronic tracking turntable is also set at the azimuth frame position, and the measuring axis of the pitch gyro is parallel to the plane where the pitch frame is located. By directly installing the azimuth gyro and the pitch gyro in the direction of the azimuth frame, the additional velocity components generated due to the carrier's swing in the azimuth and pitch axis tilting directions can be directly measured, so there is no need to use a computer for complex coordinate transformation. For the rate gyros installed in this way, since their speeds with respect to the azimuth axis and pitch axis of the optoelectronic tracking turntable are decoupled, the functions of these two parts of signals are equivalent to the feedforward control of the composite system. That is, by directly measuring the interference information generated by the carrier's movement, the control of the tracking turntable is adjusted in advance to better achieve the tracking of the target and reduce the influence of the carrier's movement on the tracking accuracy. The composite system here emphasizes the overall system in which the tracking turntable and the carrier's movement are interrelated and interact with each other. Feedforward control is based on the gyro measurement information as the system input signal or disturbance signal, and control actions are taken in advance to reduce or eliminate the influence of the disturbance on the system output, without relying on the feedback information of the system output.

[0092] In this embodiment, the heading gyro for measuring the roll angle of the optoelectronic tracking turntable is set on the optoelectronic tracking turntable, and the measuring axis of the heading gyro is parallel to the movement direction of the optoelectronic tracking turntable. The heading gyro installed on the optoelectronic tracking turntable is transformed through the coordinate system as shown in Figure 4 to convert the interference amount of the optoelectronic tracking turntable into the turntable coordinate system. The gyro can directly measure the roll angular velocity interference amount of the load platform around the y p axis, denoted as ω py . In the figure, the azimuth angle α, pitch angle β, and roll angle γ of the load platform relative to the turntable (here, the change rate of the roll angle γ corresponding to the measured roll angular velocity interference amount ω py ).

[0093] Based on the above-established coordinate system and the installation positions of the heading gyro A, pitch gyro C, and azimuth gyro B, a fiber optic rate gyro is used to implement a rate gyro-based optoelectronic tracking turntable self-stabilization control method of the present invention. The method includes the following steps:

[0094] Step S102: Obtain the azimuth compensation angular velocity, pitch compensation angular velocity of the optoelectronic tracking turntable in the turntable coordinate system, and the roll angular velocity interference amount of the optoelectronic tracking turntable in the platform coordinate system through the rate gyro respectively.

[0095] Specifically, through the servo control algorithm, the angle signals of the pitch gyro and azimuth gyro installed at the azimuth frame position are converted and processed to obtain the azimuth compensation angular velocity and pitch compensation angular velocity of the optoelectronic tracking turntable in the turntable coordinate system.

[0096] The angular signal of the heading gyro installed on the optoelectronic tracking turntable is processed through a servo control algorithm to obtain the roll angular velocity interference of the optoelectronic tracking turntable in the platform coordinate system.

[0097] The servo algorithm is a method for obtaining the compensated angular velocity in the prior art and will not be elaborated here.

[0098] Step S104: Construct a rotation matrix to convert the roll angular velocity interference of the optoelectronic tracking turntable in the platform coordinate system into the roll angular velocity interference of the optoelectronic tracking turntable in the turntable coordinate system.

[0099] Specifically, the rotation matrix R is constructed as follows:

[0100]

[0101] The roll angular velocity of the optoelectronic tracking turntable in the turntable coordinate system is expressed as:

[0102] θ t A = Rθ p A

[0103] Where, t represents the turntable coordinate system, p represents the platform coordinate system, A represents the heading gyro,

[0104] α represents the azimuth angle of the platform coordinate system relative to the turntable coordinate system;

[0105] β represents the pitch angle of the platform coordinate system relative to the turntable coordinate system;

[0106] γ represents the roll angle of the platform coordinate system relative to the turntable coordinate system.

[0107] Where, γ measures the change rate of the roll angle corresponding to the interference amount ω of the roll angular velocity. py The corresponding change rate of the roll angle.

[0108] Step S106: Perform Kalman filtering processing on the azimuth compensation angular velocity, pitch compensation angular velocity, and roll angular velocity interference in the turntable coordinate system, and adaptively adjust the process noise covariance parameter and observation noise covariance parameter in the Kalman filtering algorithm to obtain the optimal estimated angular velocity of the optoelectronic tracking turntable. It includes the following steps:

[0109] Step S106-1: Initialization step: Set the Kalman filter state vector matrix according to the initial state of the rate gyro Where, the state variables of the rate gyro include:

[0110] Angle variable Angular velocity variable [[ID=5�]]Angular acceleration

[0111] In the turntable coordinate system, the expression of the state equation is:

[0112]

[0113] Where T is a 3×3 diagonal matrix with sampling period t;

[0114] ε is the process noise, which has a mean of 0 and a process noise covariance matrix Q k Gaussian distribution;

[0115] In the turntable coordinate system, the expression of the observation equation is:

[0116]

[0117] Among them, z k are observations, including angle and angular velocity measurements;

[0118] v k is the observation noise, which has mean 0 and the observation noise covariance matrix R k Gaussian distribution.

[0119] Step S106-2, prediction step: specifically including Kalman filter state prediction and error covariance prediction.

[0120] (1) Kalman filter state prediction:

[0121] in, T is a 3×3 diagonal matrix with sampling period t.

[0122] (2) Error covariance:

[0123] λ is the forgetting factor, 0<λ<1; 0<λ<1, usually set to 0.9 or 0.95, k represents the current moment, and k-1 represents the previous moment.

[0124] Step S106-3: Kalman filter update step.

[0125] According to the predicted covariance matrix Observation Matrix and the observation noise covariance matrix R k , calculate the Kalman gain K k .

[0126]

[0127] The updated Kalman filter state is The expression is:

[0128]

[0129] Updated error covariance P k The expression is:

[0130] P k =(IK k H k )P k-1 ,

[0131] in,

[0132] Step S106-4, the step of adjusting the adaptive noise covariance matrix, adjusting the process noise covariance matrix Q by the adaptive noise covariance matrix k and the observation noise covariance matrix R k to update.

[0133] Set the adaptive noise covariance matrix ω k , whose expression is:

[0134] Then the updated process noise covariance matrix is obtained, which is expressed as: Q k =τQ k-1 +(1-τ)ζω k v k T .

[0135] Get the updated measurement noise covariance matrix, expressed as: R k =σR k-1 +(1-σ)δω k ω k T ,

[0136] Among them, τ, ζ, σ = 0.9, and δ is the adjustment parameter.

[0137] Furthermore, according to preliminary experiments, if the adjustment process of the noise covariance is expected to be relatively smooth and slow, τ and σ can be set to be larger, for example, τ = 0.9, σ = 0.9, and β and δ can be set to be smaller, for example, ζ = 0.1, δ = 0.1. If the system is expected to have complex environmental noise and the filter needs to adapt quickly to the new noise characteristics, τ and σ can be set to be smaller, and ζ and δ can be set to be larger.

[0138] In this embodiment, by updating the covariance matrix, it is ensured that the Kalman filter can continuously adjust the estimation of the state variables according to new observation data and maintain an appropriate confidence level in the estimation.

[0139] Step S108: Using the position deviation between the actual measured position and the target position of the optoelectronic tracking turntable as the input for speed loop control, and combining the optimal estimated angular velocity and the speed feedback information of the optoelectronic tracking turntable, obtain the control quantity x(k) of the optoelectronic tracking turntable.

[0140] Speed loop control is a closed-loop feedback control system used to precisely control the speed of the system and is often applied in fields such as motor control and robot control. According to the optimal estimated angular velocities of the heading gyro A, pitch gyro C, and azimuth gyro B input, PID algorithm is used for adjustment. The output value after PID adjustment is used as the reference input of the current loop, that is, the control quantity of the optoelectronic tracking turntable. By controlling the motor current through the current loop, the corresponding torque is generated to drive the motor to reach the target speed.

[0141] In this embodiment, the dynamic accuracy index of the rate gyro used is less than 1 / 3 of the speed loop accuracy, and the bandwidth is more than 10 times the system bandwidth to meet the requirements of the control system's stable loop bandwidth, motion rate, and control accuracy. Set the sampling frequency of the fiber optic gyro to 1 kHz to assist the accelerometer in compensating for gravity interference. After measuring the disturbance of the platform by the rate gyro, convert it into the disturbance of the line of sight of the optoelectronic device. And adopt a compound feedforward method. On the basis of speed feedforward, introduce the actual speed of the turntable obtained by filtering the encoder differential into the speed loop to offset the influence of torque disturbance.

[0142] Specifically, as Figure 5 the principle of the speed loop control unit, the position deviation between the actual measured position and the target position of the optoelectronic tracking turntable can be obtained through the prior art and used as the input of the speed loop controller. The speed controller generates a control signal u(k) for regulating the operating speed of the system through PID control.

[0143] The turntable servo mechanism drives the turntable to rotate according to the output control signal of the speed loop controller.

[0144] There are two dynamic feedback processes in this control process:

[0145] (1) Differential speed measurement. By using the transformers D and E installed on the X-axis and Z-axis as shown in Figure 3 to perform differential operations on signals such as the position information output by the measurement system to obtain the speed signal and provide speed feedback information for the speed controller.

[0146] (2) Attitude disturbance compensation. Based on the optimal estimated angular velocity of the optoelectronic tracking turntable obtained by Kalman filtering, compensate for the attitude disturbance existing in the system to reduce the influence of the disturbance on the system performance.

[0147] Step S110: Adjust the control voltage of the optoelectronic tracking turntable according to the control quantity of the optoelectronic tracking turntable to stabilize the optoelectronic tracking turntable.

[0148] According to the control quantity x(k) of the optoelectronic tracking turntable, drive the azimuth motor M a and the pitch actuator motor M f to stabilize the optoelectronic tracking turntable.

[0149] Embodiment 2

[0150] The present invention also provides a device embodiment that continues from the above embodiments, which is used to implement the method steps described in the above embodiments. Based on the same interpretation of the name meaning as the above embodiments, it has the same technical effects as the above embodiments, and will not be elaborated here.

[0151] As Figure 6 shown, the present invention discloses a self-stabilizing control optoelectronic tracking turntable based on a rate gyroscope, including:

[0152] Input unit 301: respectively obtain the azimuth compensation angular velocity, pitch compensation angular velocity of the optoelectronic tracking turntable in the turntable coordinate system, and the roll angular velocity disturbance amount of the optoelectronic tracking turntable in the platform coordinate system through the rate gyroscope;

[0153] Conversion unit 302: construct a rotation matrix to convert the roll angular velocity disturbance amount of the optoelectronic tracking turntable in the platform coordinate system into the roll angular velocity disturbance amount of the optoelectronic tracking turntable in the turntable coordinate system;

[0154] Kalman filter unit 303: perform Kalman filter processing on the azimuth compensation angular velocity, pitch compensation angular velocity, and roll angular velocity disturbance amount in the turntable coordinate system, adaptively adjust the process noise covariance parameter and observation noise covariance parameter in the Kalman filter algorithm, and obtain the optimal estimated angular velocity of the optoelectronic tracking turntable;

[0155] Velocity loop control unit 304: use the position deviation between the actual measured position and the target position of the optoelectronic tracking turntable as the input of the velocity loop control, combine the optimal estimated angular velocity and the velocity feedback information of the optoelectronic tracking turntable to obtain the control quantity of the optoelectronic tracking turntable;

[0156] Drive unit 305: adjust the control voltage of the optoelectronic tracking turntable according to the control quantity of the optoelectronic tracking turntable to stabilize the optoelectronic tracking turntable.

[0157] Embodiment 3

[0158] As Figure 7As shown in the figure, this embodiment provides an electronic device, which includes: at least one processor; and a memory communicatively connected to the at least one processor; wherein, the memory stores instructions executable by the one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the method steps described in the above embodiment.

[0159] Embodiment 4

[0160] The disclosed embodiment of the present invention provides a non-volatile computer storage medium, and the computer storage medium stores computer-executable instructions, and the computer-executable instructions can execute the method steps described in the above embodiment.

[0161] Embodiment 5

[0162] Reference is made below Figure 7 , which shows a schematic structural diagram of an electronic device suitable for implementing the disclosed embodiments of the present invention. The terminal device in the disclosed embodiments of the present invention may include, but is not limited to, mobile terminals such as mobile phones, laptop computers, digital broadcast receivers, PDAs (Personal Digital Assistants), PADs (Tablet Computers), PMPs (Portable Multimedia Players), in-vehicle terminals (such as in-vehicle navigation terminals), etc., and fixed terminals such as digital TVs, desktop computers, etc. Figure 7 The electronic device shown is only an example and should not impose any limitation on the functions and usage scope of the disclosed embodiments of the present invention.

[0163] As Figure 7 shown, the electronic device may include a processing device (such as a central processing unit, a graphics processing unit, etc.) 401, which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM) 402 or a program loaded from a storage device 408 into a random access memory (RAM) 403. In the RAM 403, various programs and data required for the operation of the electronic device are also stored. The processing device 401, the ROM 402, and the RAM 403 are connected to each other through a bus 404. An input / output (I / O) interface 405 is also connected to the bus 404.

[0164] Generally, the following devices may be connected to the I / O interface 405: an input device 406 including, for example, a touch screen, a touchpad, a keyboard, a mouse, a camera, a microphone, an accelerometer, a gyroscope, etc.; an output device 407 including, for example, a liquid crystal display (LCD), a speaker, a vibrator, etc.; a storage device 408 including, for example, a magnetic tape, a hard disk, etc.; and a communication device 409. The communication device 409 can allow the electronic device to communicate with other devices wirelessly or wireline to exchange data. Although Figure 7An electronic device having various devices is shown, but it should be understood that it is not required to implement or have all the shown devices. Instead, more or fewer devices may be implemented or had.

[0165] In particular, according to the embodiments disclosed in the present invention, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, the embodiments disclosed in the present invention include a computer program product that includes a computer program carried on a computer-readable medium, and the computer program contains program codes for performing the methods shown in the flowcharts. In such an embodiment, the computer program can be downloaded and installed from a network through a communication device 409, or installed from a storage device 408, or installed from a ROM 402. When the computer program is executed by a processing device 401, the above functions defined in the methods of the embodiments disclosed in the present invention are executed.

[0166] It should be noted that the computer-readable medium disclosed in the present invention can be a computer-readable signal medium or a computer-readable storage medium or any combination of the two. The computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination of the above. More specific examples of the computer-readable storage medium can include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present invention, the computer-readable storage medium can be any tangible medium that contains or stores a program, and the program can be used by or in combination with an instruction execution system, apparatus, or device. In the present invention, the computer-readable signal medium can include a data signal propagated in a baseband or as part of a carrier wave, which carries computer-readable program codes. Such a propagated data signal can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. The computer-readable signal medium can also be any computer-readable medium other than the computer-readable storage medium, and the computer-readable signal medium can send, propagate, or transmit a program for use by or in combination with an instruction execution system, apparatus, or device. The program codes contained on the computer-readable medium can be transmitted by any appropriate medium, including but not limited to: wires, optical cables, RF (radio frequency), etc., or any suitable combination of the above.

[0167] The above computer-readable medium can be included in the above electronic device; or it can exist separately and not be assembled into the electronic device.

[0168] Computer program code for performing the operations disclosed in the present invention can be written in one or more programming languages or combinations thereof. The programming languages include object-oriented programming languages such as Java, Smalltalk, C++, and also include conventional procedural programming languages such as the "C" language or similar programming languages. The program code can be executed entirely on the user's computer, partially on the user's computer, executed as a stand-alone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In the case of a remote computer, the remote computer can be connected to the user's computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or it can be connected to an external computer (e.g., by using an Internet service provider to connect through the Internet).

[0169] The flowcharts and block diagrams in the accompanying drawings illustrate the possible architectures, functions, and operations of systems, methods, and computer program products according to various embodiments disclosed in the present invention. In this regard, each block in the flowchart or block diagram can represent a module, a program segment, or a part of code that contains one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the blocks can occur in a different order than that marked in the accompanying drawings. For example, two consecutive blocks shown can actually be executed substantially in parallel, and they can sometimes be executed in the reverse order, depending on the functions involved. It should also be noted that each block in the block diagram and / or flowchart, and the combination of blocks in the block diagram and / or flowchart, can be implemented by a dedicated hardware-based system that performs the specified function or operation, or can be implemented by a combination of dedicated hardware and computer instructions.

[0170] The units described in the embodiments disclosed in the present invention can be implemented in software or in hardware. Among them, the name of the unit does not constitute a limitation to the unit itself in some cases.

Claims

1. An optoelectronic tracking turntable self-stabilization control method based on a rate gyro, characterized in that Including: Obtaining the azimuth compensation angular velocity, pitch compensation angular velocity of the optoelectronic tracking turntable in the turntable coordinate system and the roll angular velocity disturbance of the optoelectronic tracking turntable in the platform coordinate system respectively through rate gyros; Constructing a rotation matrix to convert the roll angular velocity disturbance of the optoelectronic tracking turntable in the platform coordinate system into the roll angular velocity disturbance of the optoelectronic tracking turntable in the turntable coordinate system; Performing Kalman filtering processing on the azimuth compensation angular velocity, pitch compensation angular velocity and roll angular velocity disturbance in the turntable coordinate system, adaptively adjusting the process noise covariance parameter and observation noise covariance parameter in the Kalman filtering algorithm, and obtaining the optimal estimated angular velocity of the optoelectronic tracking turntable; Taking the position deviation between the actual measured position and the target position of the optoelectronic tracking turntable as the input of speed loop control, combining the optimal estimated angular velocity and the speed feedback information of the optoelectronic tracking turntable to obtain the control quantity of the optoelectronic tracking turntable; Adjusting the control voltage of the optoelectronic tracking turntable according to the control quantity of the optoelectronic tracking turntable to stabilize the optoelectronic tracking turntable.

2. The method according to claim 1, wherein The turntable coordinate system takes the rotation center of the turntable as the origin, takes the motion direction of the optoelectronic tracking turntable as the x-axis, and takes the direction perpendicular to the motion direction as the z-axis; taking the plane perpendicular to the z-axis of the turntable coordinate system as the azimuth frame, the azimuth gyro for measuring the azimuth angle of the optoelectronic tracking turntable is arranged at the position of the azimuth frame, and the measuring axis of the azimuth gyro is parallel to the plane where the azimuth frame is located; The pitch gyro for measuring the pitch angle of the optoelectronic tracking turntable is arranged at the position of the azimuth frame, and the measuring axis of the pitch gyro is perpendicular to the plane where the azimuth frame is located; The platform coordinate system takes any fixed point of the optoelectronic tracking turntable as the origin, takes the motion direction of the optoelectronic tracking turntable as the x-axis, takes the span direction of the transverse two ends of the optoelectronic tracking turntable as the y-axis, and takes the direction perpendicular to the optoelectronic tracking turntable as the z-axis; the heading gyro for measuring the roll angle of the optoelectronic tracking turntable is arranged on the optoelectronic tracking turntable, and the measuring axis of the heading gyro is parallel to the motion direction of the optoelectronic tracking turntable.

3. The method according to claim 2, wherein The obtaining the azimuth compensation angular velocity, pitch compensation angular velocity of the optoelectronic tracking turntable in the turntable coordinate system and the roll angular velocity disturbance of the optoelectronic tracking turntable in the platform coordinate system includes: Converting the angle data of the azimuth gyro, pitch gyro and heading gyro through a servo control algorithm to obtain the azimuth compensation angular velocity, pitch compensation angular velocity of the optoelectronic tracking turntable in the turntable coordinate system and the roll angular velocity disturbance of the optoelectronic tracking turntable in the platform coordinate system.

4. The method according to claim 1, wherein The constructing a rotation matrix to convert the roll angular velocity disturbance of the optoelectronic tracking turntable in the platform coordinate system into the roll angular velocity disturbance of the optoelectronic tracking turntable in the turntable coordinate system includes: Constructing a rotation matrix R: The roll angular velocity of the optoelectronic tracking turntable in the turntable coordinate system is expressed as: θ t A = Rθ p A wherein, t represents the turntable coordinate system, p represents the platform coordinate system, A represents the heading gyro, α represents the azimuth angle of the platform coordinate system relative to the turntable coordinate system; β represents the pitch angle of the platform coordinate system relative to the turntable coordinate system; γ represents the roll angle of the platform coordinate system relative to the turntable coordinate system.

5. The method according to claim 1, wherein The steps of the Kalman filtering process include: Set the Kalman filter state vector matrix X and the process noise covariance matrix Q according to the initial state of the rate gyro k The related state equation and the observation noise covariance matrix R k The related observation equation z k ; Predict the Kalman filter state and the error covariance P k Perform prediction; According to the predicted error covariance Observation matrix H k And the observation noise covariance matrix R k Calculate the Kalman gain K at time k k : According to the Kalman gain K at time k k Update the predicted Kalman filter state And the error covariance P k For updating; Update the process noise covariance matrix Q k and the observation noise covariance matrix R k by means of an adaptive noise covariance matrix; The adaptive noise covariance matrix ω k has the following expression: The expression for updating the process noise covariance matrix is: Q k = τQ k-1 + (1 - τ)ζω k v k T The expression for updating the measurement noise covariance matrix is: R k = σR k-1 +(1 - σ)δω k ω k T where τ, ζ, σ = 0.9, and δ is an adjustment parameter.

6. The method according to claim 5, wherein Setting the Kalman filter state vector matrix X and the process noise covariance matrix Q according to the initial state of the rate gyro k The related state equation and the observation noise covariance matrix R k The related observation equation z k , including: Set the Kalman filter state vector matrix Among them, the angular variable the angular velocity variable the angular acceleration In the turntable coordinate system, the expression of the state equation is: where T is a 3×3 diagonal matrix of the sampling period t; ε is the process noise, with a Gaussian distribution having a dive mean of 0 and a process noise covariance matrix Q k ; In the turntable coordinate system, the expression of the observation equation is: where z k is the observed value, including the measured values of angles and angular velocities; v k is the observation noise, which follows a Gaussian distribution with a mean of 0 and an observation noise covariance matrix R k .

7. The method according to claim 5, characterized in that, Predicting the Kalman filter state and error covariance P k includes: Predicted Kalman filter state: Predicted error covariance: Among them, T is a 3×3 diagonal matrix of the sampling period t; λ is a forgetting factor, 0 < λ < 1.

8. The method according to claim 5, wherein The expression of the Kalman gain is: Updated Kalman filter state The expression is as follows: The updated error covariance P k has the following expression: P k = (I - K k H k )P k-1 , Among them, 9. The method according to claim 1, wherein Using the position deviation between the actual measured position and the target position of the optoelectronic tracking turntable as the input of the speed loop control, and combining the optimal estimated angular velocity and the speed feedback information of the optoelectronic tracking turntable to obtain the control quantity of the optoelectronic tracking turntable, including: Obtaining the position deviation between the actual measured position and the target position of the optoelectronic tracking turntable; Performing a differential operation on the generated position information of the optoelectronic tracking turntable according to the output signal of the speed loop control to obtain a feedback signal as the speed loop control; Inputting the feedback signal of the speed loop control, the optimal estimated angular velocity, and the position deviation between the actual measured position and the target position of the optoelectronic tracking turntable into the speed loop control to obtain the real-time control quantity of the optoelectronic tracking turntable.

10. An optoelectronic tracking turntable with self-stabilizing control based on a rate gyro, characterized in that, Including: Input unit: Obtaining the azimuth compensation angular velocity, pitch compensation angular velocity of the optoelectronic tracking turntable in the turntable coordinate system, and the roll angular velocity disturbance of the optoelectronic tracking turntable in the platform coordinate system through rate gyros respectively; Conversion unit: Constructing a rotation matrix to convert the roll angular velocity disturbance of the optoelectronic tracking turntable in the platform coordinate system into the roll angular velocity disturbance of the optoelectronic tracking turntable in the turntable coordinate system; Kalman filtering unit: Performing Kalman filtering on the azimuth compensation angular velocity, pitch compensation angular velocity, and roll angular velocity disturbance in the turntable coordinate system, and adaptively adjusting the process noise covariance parameter and observation noise covariance parameter in the Kalman filtering algorithm to obtain the optimal estimated angular velocity of the optoelectronic tracking turntable; Speed loop control unit: Using the position deviation between the actual measured position and the target position of the optoelectronic tracking turntable as the input of the speed loop control, and combining the optimal estimated angular velocity and the speed feedback information of the optoelectronic tracking turntable to obtain the control quantity of the optoelectronic tracking turntable; Drive unit: Adjusting the control voltage of the optoelectronic tracking turntable according to the control quantity of the optoelectronic tracking turntable to stabilize the optoelectronic tracking turntable.

Images