A self-stabilization control photoelectric tracking turntable and method based on rate gyro

Abstract

The application discloses a kind of self-stabilization control photoelectric tracking turntable and method based on rate gyro, method includes: by rate gyro, the azimuth compensation angular velocity of photoelectric tracking turntable under the turntable coordinate system, the pitch compensation angular velocity and the roll angular velocity disturbance of photoelectric tracking turntable under platform coordinate system are acquired respectively;Rotary matrix is constructed, the roll angular velocity disturbance of photoelectric tracking turntable under platform coordinate system is converted into the roll angular velocity disturbance of photoelectric tracking turntable under the turntable coordinate system and is handled with Kalman filtering, and the optimal estimated angular velocity of photoelectric tracking turntable is obtained;According to optimal estimated angular velocity, the control quantity of photoelectric tracking turntable is obtained, and the control voltage of photoelectric tracking turntable is adjusted.The application is updated to process noise covariance matrix and observation noise covariance matrix by using adaptive noise covariance matrix, and the problem that filtering divergence or estimation lag is caused by the fact that Kalman filtering with fixed noise covariance parameter is difficult to adapt to dynamic environment is avoided.

Description

Technical Field

[0001] This invention relates to the field of automatic control technology, and more specifically, to a self-stabilizing control photoelectric tracking turntable and method based on a rate gyroscope. Background Technology

[0002] The motion of the moving vehicle significantly affects the tracking line of sight of the electro-optical tracking and aiming turntable system. This is because the moving vehicle generates additional velocity and acceleration components in three directions on the infrared line of sight: yaw, pitch, and roll. Figure 2 As shown, three rate gyroscopes are needed to measure the carrier disturbance, specifically the additional azimuth, elevation, and horizontal tilt velocities caused by the swaying motion. Existing infrared tracking systems lack roll motion and can only compensate for this component through azimuth motion. Therefore, by accurately measuring the additional velocities of the two infrared tracking axes caused by carrier vibration in real time and introducing this velocity signal as a feedforward signal into the control system, vibration interference can be effectively suppressed, achieving high-precision target tracking.

[0003] Currently, most optoelectronic self-stabilizing platforms use rate gyroscopes as the core inertial sensors, but their angular velocity signals have the following limitations: first, they are susceptible to high-frequency noise interference; second, mechanical installation deviations or non-orthogonality can lead to coordinate system transformation errors, thereby affecting the accuracy of attitude calculation.

[0004] If the disturbances 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 this invention is to provide a self-stabilizing control photoelectric tracking turntable and method based on a rate gyroscope, which can solve at least one of the aforementioned technical problems. The specific solution is as follows:

[0006] According to specific embodiments disclosed in this invention, the first aspect of this invention discloses a self-stabilizing control method for an optoelectronic tracking turntable based on a rate gyroscope, comprising: acquiring the azimuth compensation angular velocity, pitch compensation angular velocity, and roll angular velocity disturbance of the optoelectronic tracking turntable in the turntable coordinate system and the platform coordinate system respectively through the rate gyroscope.

[0007] A rotation matrix is ​​constructed to convert the roll angular velocity interference of the photoelectric tracking turntable in the platform coordinate system into the roll angular velocity interference of the photoelectric tracking turntable in the turntable coordinate system.

[0008] The azimuth compensation angular velocity, pitch compensation angular velocity, and roll angular velocity interference in the turntable coordinate system are processed by Kalman filtering. The process noise covariance parameter and observation noise covariance parameter in the Kalman filtering algorithm are adaptively adjusted to obtain the optimal estimated angular velocity of the photoelectric tracking turntable.

[0009] The positional deviation between the actual measured position and the target position of the photoelectric tracking turntable is used as the input for the speed loop control. Combined with the optimal estimated angular velocity and the speed feedback information of the photoelectric tracking turntable, the control quantity of the photoelectric tracking turntable is obtained.

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

[0011] Preferably, the turntable coordinate system has the rotation center of the turntable as the origin, the direction of motion of the photoelectric tracking turntable as the x-axis, and the direction perpendicular to the direction of motion upwards as the z-axis; the plane perpendicular to the z-axis of the turntable coordinate system is the azimuth frame, and an azimuth gyroscope for measuring the azimuth angle of the photoelectric tracking turntable is set at the position of the azimuth frame, with the measurement axis of the azimuth gyroscope parallel to the plane of the azimuth frame; a pitch gyroscope for measuring the pitch angle of the photoelectric tracking turntable is set at the position of the azimuth frame, with the measurement axis of the pitch gyroscope perpendicular to the plane of the azimuth frame;

[0012] The platform coordinate system has an origin at any fixed point of the photoelectric tracking turntable, the x-axis is the direction of motion of the photoelectric tracking turntable, the y-axis is the horizontal span direction of the two ends of the photoelectric tracking turntable, and the z-axis is perpendicular to the celestial direction of the photoelectric tracking turntable. A directional gyroscope for measuring the roll angle of the photoelectric tracking turntable is set on the photoelectric tracking turntable, and the measuring axis of the directional gyroscope is parallel to the direction of motion of the photoelectric tracking turntable.

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

[0014] The azimuth gyroscope, pitch gyroscope, and yaw gyroscope angle data are converted using a servo control algorithm to obtain the azimuth compensation angular velocity, pitch compensation angular velocity, and roll angular velocity interference of the photoelectric tracking turntable in the turntable coordinate system and the platform coordinate system.

[0015] Preferably, a rotation matrix is ​​constructed to convert the roll angular velocity interference of the photoelectric tracking turntable in the platform coordinate system into the roll angular velocity interference of the photoelectric tracking turntable in the turntable coordinate system, including:

[0016] Construct the rotation matrix R:

[0017]

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

[0019] θ tA =Rθ p A

[0020] Where t represents the turntable coordinate system, p represents the platform coordinate system, and A represents the yaw 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 Kalman filtering process includes:

[0025] Based on the initial state of the rate gyroscope, set the Kalman filter state vector matrix X and the process noise covariance matrix Q. k The relevant state equations and the covariance matrix R with the observation noise k The relevant observation equation z k ;

[0026] Kalman filter state and error covariance P k Make predictions;

[0027] Based on 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 For the predicted Kalman filter state and error covariance P k Update;

[0028] The process noise covariance matrix Q is obtained by using an adaptive noise covariance matrix. k and the observation noise covariance matrix R k Update;

[0029] The adaptive noise covariance matrix ω k The expression 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, the Kalman filter state vector matrix X and the process noise covariance matrix Q are set according to the initial state of the rate gyroscope. k The relevant state equations and the covariance matrix R with the observation noise k The relevant observation equation z k ,include:

[0034] Set the Kalman filter state vector matrix

[0035] Among them, angle variable angular velocity variable angular acceleration

[0036] In the rotary table coordinate system, the expression for the state equation is:

[0037]

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

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

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

[0041]

[0042] Among them, z k These are observed values, including measurements of angle and angular velocity;

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

[0044] Preferably, the Kalman filter state and error covariance P are... k Making predictions includes:

[0045] Predicted Kalman filter state:

[0046] Prediction error covariance:

[0047] in, T is a 3×3 diagonal matrix with a sampling period t;

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

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

[0050]

[0051] Updated Kalman filter state The expression is:

[0052]

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

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

[0055] in,

[0056] Preferably, the step of using the positional deviation between the actual measured position and the target position of the photoelectric tracking turntable as the input for velocity loop control, and combining the optimal estimated angular velocity and the velocity feedback information of the photoelectric tracking turntable to obtain the control quantity of the photoelectric tracking turntable includes:

[0057] Obtain the positional deviation between the actual measured position and the target position of the photoelectric tracking turntable;

[0058] The position information of the photoelectric tracking turntable generated based on the output signal of the speed loop control is used to perform a differential operation on the generated position information of the photoelectric tracking turntable to obtain a feedback signal for the speed loop control.

[0059] 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 photoelectric tracking turntable are input into the speed loop control to obtain the real-time photoelectric tracking turntable control quantity.

[0060] According to specific embodiments disclosed in this invention, a second aspect of this invention discloses a self-stabilizing control photoelectric tracking turntable based on a rate gyroscope, comprising:

[0061] Input unit: The azimuth compensation angular velocity, pitch compensation angular velocity and roll angular velocity interference of the photoelectric tracking turntable in the turntable coordinate system and the platform coordinate system are obtained by the rate gyroscope respectively.

[0062] Transformation unit: Constructs a rotation matrix to convert the roll angular velocity interference of the photoelectric tracking turntable in the platform coordinate system into the roll angular velocity interference of the photoelectric tracking turntable in the turntable coordinate system.

[0063] Kalman filtering unit: performs Kalman filtering on the azimuth compensation angular velocity, pitch compensation angular velocity and roll angular velocity interference in the turntable coordinate system, adaptively adjusts the process noise covariance parameter and observation noise covariance parameter in the Kalman filtering algorithm, and obtains the optimal estimated angular velocity of the photoelectric tracking turntable.

[0064] Speed ​​loop control unit: The position deviation between the actual measured position and the target position of the photoelectric tracking turntable is used as the input of the speed loop control. Combined with the optimal estimated angular velocity and the speed feedback information of the photoelectric tracking turntable, the control quantity of the photoelectric tracking turntable is obtained.

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

[0066] Compared with the prior art, the above-described solution disclosed in this invention has at least the following beneficial effects:

[0067] This invention utilizes an adaptive noise covariance matrix to optimize the process noise covariance matrix Q. k and the observation noise covariance matrix R k The update avoids the problem of filter divergence or estimation lag caused by Kalman filtering with fixed noise covariance parameters failing to adapt to dynamic environments. Simultaneously, by integrating with velocity loop control, the system's dynamic accuracy and anti-interference capability are significantly improved. Attached Figure Description

[0068] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with the disclosure of this invention and, together with the description, serve to explain the principles of the invention. It is obvious that the drawings described below are merely some embodiments of the invention, and those skilled in the art can obtain other drawings based on these drawings without any inventive effort. In the drawings:

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

[0070] Figure 2 This is a schematic diagram of the motion components in three directions generated by the photoelectric tracking turntable on the infrared line of sight in the existing technology;

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

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

[0073] Figure 5 This is a schematic diagram of the speed loop control unit according to an embodiment of the present invention;

[0074] Figure 6 This is a schematic diagram of the structure of a self-stabilizing control photoelectric tracking turntable based on a rate gyroscope according to an embodiment of the present invention;

[0075] Figure 7 This is a schematic diagram of the structure of the electronic device provided in this embodiment. Detailed Implementation

[0076] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are merely a part of the embodiments disclosed in this invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments disclosed in this invention without creative effort are within the scope of protection of this invention.

[0077] The terminology used in the disclosed embodiments of this invention is for the purpose of describing particular embodiments only and is not intended to limit the scope of the invention. The singular forms “a,” “the,” and “the” as used in the disclosed embodiments and appended claims are also intended to include the plural forms, and “multiple” generally includes at least two unless the context clearly indicates otherwise.

[0078] It should be understood that the term "and / or" used in this article is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, and B existing alone. Additionally, the character " / " in this article generally indicates that the preceding and following related objects have an "or" relationship.

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

[0080] Depending on the context, the words “if” or “suppose” as used here can be interpreted as “when” or “in response to determination” or “in response to detection.” Similarly, depending on the context, the phrases “if determination” or “if detection (of the stated condition or event)” can be interpreted as “when determination” or “in response to determination” or “when detection (of the stated condition or event)” or “in response to detection (of the stated condition or event).”

[0081] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that an article or device that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such an article or device. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the article or device that includes said element.

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

[0083] Example 1

[0084] An optoelectronic tracking turntable is a complex system integrating optics, mechanics, electronics, and automation technologies, widely used in military, aerospace, and meteorological observation fields. Through the coordinated operation of gyroscopes and motors, the eye axis (the aiming line of the optical equipment) of the optoelectronic tracking turntable remains stable during carrier movement. Typically, three rate gyroscopes are used to measure carrier disturbances, specifically measuring the disturbances of the optoelectronic load along the yaw, pitch, and roll axes of the load platform, such as an aircraft or ship. By measuring the angular velocity components along the three axes, the yaw (azimuth) and pitch (pitch) disturbance angular velocities of the optoelectronic tracking turntable are calculated, thus obtaining the compensation angular velocities of the servo system in the azimuth and pitch directions.

[0085] Since the photoelectric tracking turntable can only move in the yaw and pitch directions, and not in the roll direction, it can only be canceled out by the azimuth component. Therefore, in this embodiment, the installation positions of the three degree-of-freedom rate gyroscopes are set so that there is no coupling between the disturbance signals. This eliminates the need for complex coordinate transformations and decoupling, and the three degree-of-freedom measurement signals are directly introduced into the velocity loop control unit after decomposition, achieving high-precision feedforward composite control and line-of-sight stabilization. Furthermore, fiber optic gyroscopes are selected for their strong shock and acceleration resistance, long service life, and stable signal, making them well-suited for high-precision application environments.

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

[0087] In one embodiment of the present invention, by establishing as Figure 3 The coordinate system shown determines the installation positions of the three-degree-of-freedom rate gyroscopes. The two-dimensional plane perpendicular to the Z-axis is the azimuth frame, and the two-dimensional plane perpendicular to the Y-axis is the pitch frame. The pitch and azimuth frames are perpendicular to each other and intersect.

[0088] Specifically, such as Figure 4 As shown, the turntable coordinate system takes the rotation center of the turntable as its origin, and the platform coordinate system takes any fixed point of the photoelectric tracking turntable as its origin. The origins of the two coordinate systems are set to coincide.

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

[0090] In the rotary table coordinate system t, z t Axial direction and z p The axes are in the same direction. x t The azimuth axis is the direction of motion of the photoelectric tracking turntable, and also the azimuth axis direction of the turntable. The azimuth axis is perpendicular to the z-axis. t The plane of the axis is the plane containing the pitch frame. y t The axis direction is the pitch axis direction of the turntable. The plane containing the pitch frame is perpendicular to the plane containing the azimuth frame, and the plane containing the pitch frame is also perpendicular to the pitch axis. The angle of rotation of the pitch frame around the pitch axis is the pitch angle, and the angle of rotation of the azimuth frame around the z-axis is the pitch angle. t The rotation angle of the axis is the azimuth angle. The photoelectric tracking turntable cannot rotate around the turntable's azimuth axis; x... t Since the axis rotates, the photoelectric tracking turntable can only move in the yaw and pitch directions, and cannot move in the roll direction.

[0091] In this embodiment, an azimuth gyroscope for measuring the azimuth angle of the photoelectric tracking turntable is positioned at the azimuth frame location. The measurement axis of the azimuth gyroscope is parallel to the plane containing the azimuth frame, that is, the measurement axis of the azimuth gyroscope is perpendicular to the z-axis. tThe pitch gyroscope, which measures the pitch angle of the photoelectric tracking turntable, is also positioned at the azimuth frame, with its measurement axis parallel to the plane of the pitch frame. By directly mounting the azimuth and pitch gyroscopes in the direction of the azimuth frame, the additional velocity components generated by the carrier's swaying in the azimuth and pitch axis tilt directions can be directly measured, eliminating the need for complex coordinate transformations using a computer. Because the rate gyroscopes mounted in this manner are decoupled from the azimuth and pitch axis velocities of the photoelectric tracking turntable, the effect of these two signals is equivalent to feedforward control in a composite system. That is, by directly measuring the disturbance information generated by the carrier's motion, the control of the tracking turntable is adjusted in advance to better track the target and reduce the impact of carrier motion on tracking accuracy. Here, the composite system emphasizes the overall system where the tracking turntable and carrier motion are interconnected and mutually influential. Feedforward control uses gyroscope measurement information as the system input signal or disturbance signal, taking control actions in advance to reduce or eliminate the impact of disturbances on the system output, without relying on feedback information from the system output.

[0092] In this embodiment, a yaw gyroscope for measuring the roll angle of the photoelectric tracking turntable is mounted on the turntable, and the measuring axis of the yaw gyroscope is parallel to the direction of motion of the photoelectric tracking turntable. The yaw gyroscope mounted on the photoelectric tracking turntable is controlled by, for example... Figure 4 The coordinate system shown is transformed to convert the interference of the photoelectric tracking turntable into the turntable coordinate system. The gyroscope can directly measure the load platform's rotation around the y-axis. p The roll velocity disturbance of the shaft is denoted as ω. py The figure shows the azimuth angle α, pitch angle β, and roll angle γ of the load platform relative to the turntable (here, the disturbance ω of the roll rate is measured). py (The corresponding rate of change of the roll angle γ).

[0093] Based on the established coordinate system and the installation positions of the azimuth gyroscope A, pitch gyroscope C, and azimuth gyroscope B, a self-stabilizing control method for an optoelectronic tracking turntable based on a rate gyroscope, as described in this invention, is implemented using fiber optic rate gyroscopes. The method includes the following steps:

[0094] Step S102: Obtain the azimuth compensation angular velocity, pitch compensation angular velocity, and roll angular velocity interference of the photoelectric tracking turntable in the turntable coordinate system and the platform coordinate system respectively using the rate gyroscope.

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

[0096] By processing the angle signal of the heading gyroscope mounted on the photoelectric tracking turntable using a servo control algorithm, the roll velocity disturbance of the photoelectric tracking turntable in the platform coordinate system is obtained.

[0097] The servo algorithm is a method for obtaining the compensated angular velocity in the existing technology, which will not be elaborated here.

[0098] Step S104: Construct a rotation matrix to convert the roll angular velocity interference of the photoelectric tracking turntable in the platform coordinate system into the roll angular velocity interference of the photoelectric 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 photoelectric 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, and A represents the yaw gyroscope.

[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] Among them, γ measures the disturbance ω of the roll angular velocity. py The corresponding rate of change of the roll angle.

[0108] Step S106: Perform Kalman filtering on the azimuth compensation angular velocity, pitch compensation angular velocity, and roll angular velocity interference in the turntable coordinate system. 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 photoelectric tracking turntable. This 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 gyroscope. The state variables of the rate gyroscope include:

[0110] Angle variable angular velocity variable angular acceleration

[0111] In the rotary table coordinate system, the expression for the state equation is:

[0112]

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

[0114] ε represents 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 for the observation equation is:

[0116]

[0117] Among them, z k These are observed values, including measurements of angle and angular velocity;

[0118] v k It is observation noise, with a mean of 0 and an observation noise covariance matrix R. k The Gaussian distribution.

[0119] Step S106-2, the prediction steps: 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 time, and k-1 represents the previous time.

[0124] Step S106-3, Kalman filter update steps.

[0125] Based on 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 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: Adjustment of the adaptive noise covariance matrix, which involves adjusting the process noise covariance matrix Q using the adaptive noise covariance matrix. k and the observation noise covariance matrix R k Update.

[0133] Define the adaptive noise covariance matrix ω k Its expression is:

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

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

[0136] Where τ, ζ, σ = 0.9, and δ is an adjustment parameter.

[0137] Furthermore, based on preliminary experiments, if it is desired that the adjustment process of the noise covariance is relatively smooth and slow, τ and σ can be set to be larger, for example, τ = 0.9 and σ = 0.9, and β and δ can be set to be smaller, for example, ζ = 0.1 and δ = 0.1. If it is desired that the system has complex environmental noise and the filter needs to quickly adapt to new noise characteristics, then τ 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 based on 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 photoelectric tracking turntable as the input of the speed loop control, and combining the optimal estimated angular velocity and the speed feedback information of the photoelectric tracking turntable, the control quantity x(k) of the photoelectric tracking turntable is obtained.

[0140] Speed ​​loop control is a closed-loop feedback control system used for precise speed control, commonly applied in motor control, robot control, and other fields. Based on the optimal estimated angular velocities from the input yaw gyroscope A, pitch gyroscope C, and azimuth gyroscope B, a PID algorithm is used for adjustment. The output value after PID adjustment serves as the reference input for the current loop, i.e., the control input for the photoelectric tracking turntable. The current loop controls the motor current, thereby generating corresponding torque to drive the motor to the target speed.

[0141] In this embodiment, the dynamic accuracy of the rate gyroscope used is less than 1 / 3 of the velocity loop accuracy, and its bandwidth is more than 10 times the system bandwidth to meet the requirements of the control system's stability loop bandwidth, motion rate, and control accuracy. A fiber optic gyroscope sampling frequency of 1kHz is used to assist the accelerometer in compensating for gravity interference. After the platform's disturbance is measured by the rate gyroscope, it is converted into a line-of-sight disturbance of the photoelectric device. A composite feedforward method is adopted, whereby, based on velocity feedforward, the actual turntable speed obtained from the encoder differential is filtered and then introduced into the velocity loop to counteract the influence of torque disturbance.

[0142] Specifically, such as Figure 5 The principle of the speed loop control unit is to obtain the position deviation between the actual measured position of the photoelectric tracking turntable and the target position using existing technology, and use this deviation as the input to the speed loop controller. The speed controller generates a control signal u(k) to adjust the system's operating speed through PID control.

[0143] The turntable servo mechanism drives the turntable to rotate based on the output control signal from the speed loop controller.

[0144] This control process involves two dynamic feedback processes:

[0145] (1) Differential velocity measurement, through methods such as Figure 3 The transformers D and E, installed on the X and Z axes, are used to perform differential calculations on the position information and other signals output by the measurement system to obtain the speed signal, which provides speed feedback information for the speed controller.

[0146] (2) Attitude disturbance compensation: Based on the optimal estimated angular velocity of the photoelectric tracking turntable obtained by Kalman filtering, the attitude disturbance of the system is compensated to reduce the impact of the disturbance on the system performance.

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

[0148] Based on the photoelectric tracking turntable control quantity x(k), drive the azimuth motor M a and pitch actuator motor M f This stabilizes the photoelectric tracking turntable.

[0149] Example 2

[0150] The present invention also provides an apparatus embodiment that follows the above embodiments, for implementing the method steps described in the above embodiments. The interpretation of the same names is the same as that of the above embodiments, and the same technical effects are achieved as those of the above embodiments. Therefore, it will not be described again here.

[0151] like Figure 6 As shown, this invention discloses a self-stabilizing control photoelectric tracking turntable based on a rate gyroscope, comprising:

[0152] Input unit 301: The azimuth compensation angular velocity, pitch compensation angular velocity and roll angular velocity interference of the photoelectric tracking turntable in the turntable coordinate system and the platform coordinate system are obtained by the rate gyroscope respectively.

[0153] Conversion unit 302: Constructs a rotation matrix to convert the roll angular velocity interference of the photoelectric tracking turntable in the platform coordinate system into the roll angular velocity interference of the photoelectric tracking turntable in the turntable coordinate system.

[0154] Kalman filter unit 303: performs Kalman filtering on the azimuth compensation angular velocity, pitch compensation angular velocity and roll angular velocity interference in the turntable coordinate system, adaptively adjusts the process noise covariance parameter and observation noise covariance parameter in the Kalman filtering algorithm, and obtains the optimal estimated angular velocity of the photoelectric tracking turntable.

[0155] Speed ​​loop control unit 304: The position deviation between the actual measured position and the target position of the photoelectric tracking turntable is used as the input of the speed loop control. Combined with the optimal estimated angular velocity and the speed feedback information of the photoelectric tracking turntable, the control quantity of the photoelectric tracking turntable is obtained.

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

[0157] Example 3

[0158] like Figure 7As shown, 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 at least one processor, which are executed by the at least one processor to enable the at least one processor to perform the method steps described in the above embodiment.

[0159] Example 4

[0160] The present invention discloses a non-volatile computer storage medium storing computer-executable instructions that can perform the steps described in the above embodiments.

[0161] Example 5

[0162] The following is for reference. Figure 7 The diagram illustrates a structural schematic suitable for implementing embodiments of the present invention. The terminal devices in the embodiments of the present invention may include, but are not limited to, mobile terminals such as mobile phones, laptops, digital broadcast receivers, PDAs (personal digital assistants), PADs (tablet computers), PMPs (portable multimedia players), in-vehicle terminals (e.g., in-vehicle navigation terminals), and fixed terminals such as digital TVs and desktop computers. Figure 7 The electronic device shown is merely an example and should not be construed as limiting the functionality and scope of use of the disclosed embodiments of the present invention.

[0163] like Figure 7 As shown, the electronic device may include a processing unit (e.g., 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. The RAM 403 also stores various programs and data required for the operation of the electronic device. The processing unit 401, ROM 402, and RAM 403 are interconnected via a bus 404. An input / output (I / O) interface 405 is also connected to the bus 404.

[0164] Typically, the following devices can be connected to I / O interface 405: input devices 406 including, for example, touchscreens, touchpads, keyboards, mice, cameras, microphones, accelerometers, gyroscopes, etc.; output devices 407 including, for example, liquid crystal displays (LCDs), speakers, vibrators, etc.; storage devices 408 including, for example, magnetic tapes, hard disks, etc.; and communication devices 409. Communication device 409 allows electronic devices to communicate wirelessly or wiredly with other devices to exchange data. Although Figure 7Electronic devices with various devices are shown, but it should be understood that it is not required to implement or have all of the devices shown. More or fewer devices may be implemented or have instead.

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

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

[0167] The aforementioned computer-readable medium may be included in the aforementioned electronic device; or it may exist independently and not assembled into the electronic device.

[0168] Computer program code for performing the operations disclosed herein can be written in one or more programming languages ​​or a combination thereof. These programming languages ​​include object-oriented programming languages—such as Java, Smalltalk, and C++—and 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, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving remote computers, the remote computer can be connected to the user's computer via any type of network—including a local area network (LAN) or a wide area network (WAN)—or can be connected to an external computer (e.g., via the Internet using an Internet service provider).

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

[0170] The units described in the embodiments of this invention can be implemented in software or hardware. The names of the units are not, in some cases, intended to limit the specific unit.

Claims

1. A self-stabilizing control method for an optoelectronic tracking turntable based on a rate gyroscope, characterized in that, include: The azimuth compensation angular velocity, pitch compensation angular velocity and roll angular velocity interference of the photoelectric tracking turntable in the turntable coordinate system and the platform coordinate system are obtained by rate gyroscope. A rotation matrix is ​​constructed to convert the roll angular velocity interference of the photoelectric tracking turntable in the platform coordinate system into the roll angular velocity interference of the photoelectric tracking turntable in the turntable coordinate system. The azimuth compensation angular velocity, pitch compensation angular velocity, and roll angular velocity interference in the turntable coordinate system are processed by Kalman filtering. The process noise covariance parameter and observation noise covariance parameter in the Kalman filtering algorithm are adaptively adjusted to obtain the optimal estimated angular velocity of the photoelectric tracking turntable. The positional deviation between the actual measured position and the target position of the photoelectric tracking turntable is used as the input for the speed loop control. Combined with the optimal estimated angular velocity and the speed feedback information of the photoelectric tracking turntable, the control quantity of the photoelectric tracking turntable is obtained. Adjust the control voltage of the photoelectric tracking turntable according to the control quantity of the photoelectric tracking turntable to stabilize the photoelectric tracking turntable.

2. The method according to claim 1, characterized in that, The turntable coordinate system has the rotation center of the turntable as the origin, the direction of motion of the photoelectric tracking turntable as the x-axis, and the direction perpendicular to the direction of motion upward as the z-axis; the plane perpendicular to the z-axis of the turntable coordinate system is the azimuth frame, and the azimuth gyroscope for measuring the azimuth angle of the photoelectric tracking turntable is set at the position of the azimuth frame, with the measurement axis of the azimuth gyroscope parallel to the plane of the azimuth frame. A pitch gyroscope for measuring the pitch angle of the photoelectric tracking turntable is positioned at the azimuth frame location, and the measuring axis of the pitch gyroscope is perpendicular to the plane containing the azimuth frame. The platform coordinate system has an origin at any fixed point of the photoelectric tracking turntable, the x-axis is the direction of motion of the photoelectric tracking turntable, the y-axis is the horizontal span direction of the two ends of the photoelectric tracking turntable, and the z-axis is perpendicular to the celestial direction of the photoelectric tracking turntable. A directional gyroscope for measuring the roll angle of the photoelectric tracking turntable is set on the photoelectric tracking turntable, and the measuring axis of the directional gyroscope is parallel to the direction of motion of the photoelectric tracking turntable.

3. The method according to claim 2, characterized in that, The acquisition of the azimuth compensation angular velocity, pitch compensation angular velocity, and roll angular velocity interference of the photoelectric tracking turntable in the turntable coordinate system and the platform coordinate system includes: The azimuth gyroscope, pitch gyroscope, and yaw gyroscope angle data are converted using a servo control algorithm to obtain the azimuth compensation angular velocity, pitch compensation angular velocity, and roll angular velocity interference of the photoelectric tracking turntable in the turntable coordinate system and the platform coordinate system.

4. The method according to claim 1, characterized in that, The construction of the rotation matrix, which converts the roll angular velocity interference of the photoelectric tracking turntable in the platform coordinate system into the roll angular velocity interference of the photoelectric tracking turntable in the turntable coordinate system, includes: Construct the rotation matrix R: The roll angular velocity of the photoelectric tracking turntable in the turntable coordinate system is expressed as: i t A =Rθ p A Where t represents the turntable coordinate system, p represents the platform coordinate system, and A represents the yaw gyroscope. α 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, characterized in that, The Kalman filtering process includes the following steps: Based on the initial state of the rate gyroscope, set the Kalman filter state vector matrix X and the process noise covariance matrix Q. k The relevant state equations and the covariance matrix R with the observation noise k The relevant observation equation z k ; Kalman filter state and error covariance P k Make predictions; Based on 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 For the predicted Kalman filter state and error covariance P k Update; The process noise covariance matrix Q is obtained by using an adaptive noise covariance matrix. k and the observation noise covariance matrix R k Update; The adaptive noise covariance matrix ω k The expression is: 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, characterized in that, The Kalman filter state vector matrix X is set according to the initial state of the rate gyroscope, and the process noise covariance matrix Q is... k The relevant state equations and the covariance matrix R with the observation noise k The relevant observation equation z k ,include: Set the Kalman filter state vector matrix Among them, angle variable angular velocity variable angular acceleration In the rotary table coordinate system, the expression for the state equation is: Where T is a 3×3 diagonal matrix of sampling period t; ε represents process noise, with a dive mean of 0, and the process noise covariance matrix Q. k Gaussian distribution; In the turntable coordinate system, the expression for the observation equation is: Among them, z k These are observed values, including measurements of angle and angular velocity; v k It is observation noise, with a mean of 0 and an observation noise covariance matrix R. k The Gaussian distribution.

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

8. The method according to claim 5, characterized in that, The expression for the Kalman gain is: Updated Kalman filter state The expression is: Updated error covariance P k The expression is: P k =(I-K k H k )P k-1 , in, 9. The method according to claim 1, characterized in that, The step of using the position deviation between the actual measured position and the target position of the photoelectric tracking turntable as the input for the velocity loop control, combined with the optimal estimated angular velocity and the velocity feedback information of the photoelectric tracking turntable, yields the control quantity for the photoelectric tracking turntable, including: Obtain the positional deviation between the actual measured position and the target position of the photoelectric tracking turntable; The position information of the photoelectric tracking turntable generated based on the output signal of the speed loop control is used to perform a differential operation on the generated position information of the photoelectric tracking turntable to obtain a feedback signal for the speed loop control. 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 photoelectric tracking turntable are input into the speed loop control to obtain the real-time photoelectric tracking turntable control quantity.

10. A self-stabilizing control photoelectric tracking turntable based on a rate gyroscope, characterized in that, include: Input unit: The azimuth compensation angular velocity, pitch compensation angular velocity and roll angular velocity interference of the photoelectric tracking turntable in the turntable coordinate system and the platform coordinate system are obtained by the rate gyroscope respectively. Transformation unit: Constructs a rotation matrix to convert the roll angular velocity interference of the photoelectric tracking turntable in the platform coordinate system into the roll angular velocity interference of the photoelectric tracking turntable in the turntable coordinate system. Kalman filtering unit: performs Kalman filtering on the azimuth compensation angular velocity, pitch compensation angular velocity and roll angular velocity interference in the turntable coordinate system, adaptively adjusts the process noise covariance parameter and observation noise covariance parameter in the Kalman filtering algorithm, and obtains the optimal estimated angular velocity of the photoelectric tracking turntable. Speed ​​loop control unit: The position deviation between the actual measured position and the target position of the photoelectric tracking turntable is used as the input of the speed loop control. Combined with the optimal estimated angular velocity and the speed feedback information of the photoelectric tracking turntable, the control quantity of the photoelectric tracking turntable is obtained. Drive unit: Adjusts the control voltage of the photoelectric tracking turntable according to the control quantity of the photoelectric tracking turntable to stabilize the photoelectric tracking turntable.

Images