Motor control system of two-axis photoelectric pod and control method thereof

Abstract

The invention discloses a motor control system for a two-axis photoelectric pod, and the system comprises a current sampling EKF fusion filtering module which is used for carrying out the EKF fusion filtering of an obtained current original value, an angular velocity original value, and an angle original value of a motor, and then obtaining the angular velocity, the mechanical angle, and the current of the motor; the ADRC controller module is used for calculating a motor angular velocity expected value by adopting an ADRC active-disturbance-rejection control algorithm according to the filtered mechanical angle value and the filtered angle target value; a PID control module; and the controller is used for calculating a torque current target value according to the expected value of the angular velocity of the motor and the filtered angular velocity of the motor, and finally obtaining the torque voltage and the excitation voltage of the motor by combining the filtered current. Compared with the prior art, by modifying the control link and optimizing the controller algorithm, the stability and precision of the control of the photoelectric pod motor of the unmanned aerial vehicle are improved.

Description

Technical Field

[0001] This invention relates to the field of optoelectronic pod control technology, specifically providing a motor control system and control method for a two-axis optoelectronic pod. Background Technology

[0002] An optoelectronic pod is a multifunctional device that integrates optical imaging and a mechanical stabilization system. It is widely used in military reconnaissance, fire rescue, geographic surveying and mapping, and other fields. Its core function is to rely on a stable platform to counteract the interference of carrier movement and acquire target information through optical sensors to achieve high-precision observation and positioning.

[0003] The motor control system of the electro-optical pod is the core technology module for achieving precise and stable control and target tracking. Brushless DC motors are characterized by high stability, low maintenance, and lightweight design, therefore, they are commonly used in electro-optical pods. Existing brushless DC motor control technology combines an FOC control link with a PID controller. Traditional PID controllers suffer from drawbacks such as easy overshoot and slow convergence. Furthermore, to reduce the impact of sensor noise, only PI controllers can be used, further limiting the stability and accuracy of the motor control in UAV electro-optical pods. Summary of the Invention

[0004] The purpose of this invention is to provide a motor control system and control method for a two-axis optoelectronic pod. By modifying the control link and optimizing the controller algorithm, the stability and accuracy of motor control for the UAV optoelectronic pod are improved.

[0005] The technical solution for achieving the objective of this invention is: a motor control system for a two-axis photoelectric pod, comprising:

[0006] The current sampling EKF fusion filtering module is connected to the motor and is used to obtain the motor's angular velocity, mechanical angle, and current by fusing and filtering the original values ​​of the motor's current, angular velocity, and angle through EKF.

[0007] The ADRC controller module is connected to the output mechanical angle in the current sampling EKF fusion filter module. It is used to calculate the expected value of the motor angular velocity based on the filtered mechanical angle value and the target angle value using the ADRC active disturbance rejection control algorithm.

[0008] The PID control module connects to the output of the current sampling EKF fusion filter module, the output of the motor angular velocity and current, and the output of the ADRC controller module. It is used to calculate the target value of torque current based on the expected value of motor angular velocity and the filtered motor angular velocity, and then combine the filtered current to finally obtain the motor torque voltage and excitation voltage.

[0009] The significant advantages of this invention compared to existing technologies are:

[0010] (1) The photoelectric pod motor control system invented uses current, IMU, magnetic encoder and other sensors for fusion filtering, which solves the shortcomings of excessive noise of IMU and low update rate of magnetic encoder, improves the accuracy of sensor sampling, and improves the control precision and stability of motor.

[0011] (2) The photoelectric pod motor control system of the present invention adds an ADRC controller to the control loop, which can effectively suppress motor control overshoot, improve response speed, and improve the tracking performance of the photoelectric pod. Attached Figure Description

[0012] Figure 1 This is a block diagram of the two-axis photoelectric pod motor control system. Detailed Implementation

[0013] The terminology used in this invention is for illustrative purposes only and is not intended to limit the invention. The following description, in conjunction with the appendix, further clarifies this concept. Figure 1 The following is a detailed description of some embodiments of the present invention.

[0014] This invention provides a motor control system for a two-axis optoelectronic pod, such as... Figure 1 As shown, it includes a current sampling EKF fusion filter module, an ADRC controller module, and a PID control module;

[0015] The current sampling EKF fusion filtering module includes a three-phase current sampling module, a current Clark transformation module, a current Park module, a sensor IMU and a magnetic encoder, and an EKF calculation module.

[0016] The three-phase current sampling module is connected to the motor to collect its current. It is sequentially connected to the current Clark transformation module, the current Park module, and the EKF calculation module. The initial current sampling yields the raw three-phase current values. , The output after passing through the current Clark conversion module is shaft and Original value of shaft current , Then, it is converted into the original torque current value by the current Park module. Original value of excitation current The data is then input into the EKF calculation module. The sensor IMU and magnetic encoder are set up side-by-side, each connected to the motor at one end and the EKF calculation module at the other; the sensor IMU acquires the raw angular velocity value. The magnetic encoder obtains the raw angle value. All values ​​are input into the EKF calculation module. The EKF calculation module uses fusion filtering to calculate and obtain the motor angular velocity. Mechanical angle Excitation current Torque current Mechanical angle Excitation current Torque current Used for current loop calculations. Motor angular velocity. Angular velocity is used in velocity loop calculations;

[0017] The ADRC controller module includes: a tracking differentiator (TD) module, an ESO state observer module, and an NLSEF state error feedback module. The angle setpoint (target value) is first processed by the tracking differentiator (TD) module to obtain the position estimate. Speed ​​estimate The position estimates output by the ESO state observer module are compared with those of the ESO state observer module. The first-order state estimate of the system The difference is obtained by performing error calculation. , All values ​​are input into the NLSEF state error feedback module. The total system disturbance estimate calculated by the ESO state observer module is subtracted from the output value of the NLSEF state error feedback module. Divide by control gain Obtain the expected value of the velocity. Expected speed value Multiply by control gain and mechanical angle As input to the ESO state observer module.

[0018] The PID control module includes: a current PID control module, a torque voltage PID control module, an excitation voltage PID control module, an inverse Park transform module, and an SVPWM debugging module; the output of the ESO state observer module is connected to the current PID control module, and the output of the current PID control module... and the output of the EKF calculation module The error is calculated and input into the torque-voltage PID control module, and the output of the current PID control module is also calculated. and the output of the EKF calculation module Perform error calculation and input the excitation voltage into the PID control module; speed value expected value With motor angular velocity The motor angular velocity error value is obtained by performing error calculation. This value is then processed by a current PID control module to obtain the target torque current value. and the target value of excitation current Target value of torque current and torque current The torque voltage is calculated by the torque voltage PID control module. Obtain the target value of the excitation current. and excitation current respectively The torque voltage is calculated by the excitation voltage PID control module. Torque voltage and torque voltage After passing through the inverse Park transform module and the SVPWM debugging module, a three-phase PWM wave is generated to control the motor movement.

[0019] A method for the motor control system of a two-axis optoelectronic pod is as follows:

[0020] Step 1: The current sampling module acquires the raw sampled values ​​of phase A and phase B currents. , First, it is transformed by Clark into shaft and Original value of shaft current , Then, it is transformed into the original value of torque current through Park transformation. Original value of excitation current The IMU acquired the raw angular velocity value. The magnetic encoder obtains the raw angle value. And the electrical angle was calculated. All raw data were fused and filtered using EKF to obtain the motor angular velocity and mechanical angle. Torque current Excitation current .

[0021] 1.1 Clark Transformation of Current:

[0022] ;

[0023] 1.2 Current Park Transformation:

[0024] ;

[0025] 1.3. The EKF algorithm is used to fuse current sensor, IMU, and magnetic encoder filtering processes. The system state equation is: ,in, Angular velocity, For excitation current, For torque current, Let represent the biases of the gyroscope and the current sensor, respectively. The observation equation is: ,in, These represent the original sampled values ​​of angular velocity, excitation current, torque current, and angle, respectively. Based on the torque current... Excitation current and motor parameters inductance , Extreme logarithm Calculated electromagnetic torque P is the extreme logarithm. and These are the flux linkages along the d-axis and q-axis, respectively, and the electromagnetic torque is... The load torque electromagnetic torque is The excitation voltage and torque voltage are respectively and , Where J is the viscous friction torque, and J is the moment of inertia. The stator resistance is given. According to the motor model, the system state change rate is:

[0026]

[0027]

[0028]

[0029]

[0030] in, , , , Represents the rate of change of angular velocity, excitation current, torque current, and angle.

[0031] 1.4 EKF Filtering Algorithm for State Prediction renew:

[0032]

[0033]

[0034]

[0035]

[0036] in, The time interval between the next moment and the current moment. , , These represent the current angular velocity, excitation current, torque current, and rate of change of angle, respectively. , , , These are the angular velocity, excitation current, torque current, and angle value at the next moment, respectively.

[0037] 1.5 Calculate the Jacobian matrix of the current state transition matrix. Then calculate the covariance matrix of the predicted state. , Let be the covariance matrix at the current time. Let be the process noise covariance matrix at the current time.

[0038] 1.6 Calculate the measurement residuals ,in This is the observation matrix for the next time step, i.e., the actual measured values. This is the observation model function. Calculate the measurement matrix for the next time step. Next, the covariance of the measurement residuals is calculated. ,in The observation noise covariance matrix is ​​then calculated. The Kalman gain is then calculated. .

[0039] 1.7 Update the state estimate and covariance matrix using the measurement data, where... The state value at the next moment. The covariance matrix for the next time step:

[0040] ;

[0041] ;

[0042] Step 2: Based on the filtered angle value and the target angle value, the expected value of the motor angular velocity is calculated using the ADRC active disturbance rejection control algorithm.

[0043] 2.1 Calculate the output of the TD tracking differentiator:

[0044]

[0045]

[0046] in, For the current moment , For the current moment , The integration step size is... The estimated position for the next moment. For the next moment fhan is the fastest control synthesis function. denoted as the target angle value, and r as the velocity factor.

[0047] 2.2 Calculate the ESO state observer:

[0048]

[0049]

[0050]

[0051]

[0052] in, , , These are the estimated position at the current time, the estimated first-order state of the system, and the estimated total system disturbance, respectively. , , denoted as observer gain parameter, b as control gain, u as control input, and e as observation error. , , These are the estimated position at the next time step, the estimated first-order state of the system, and the estimated total disturbance of the system, respectively. This is the perturbation threshold.

[0053] 2.3 Calculate NLSEF state error feedback:

[0054]

[0055]

[0056]

[0057]

[0058] in, , These are the position error value at the next time step and the first-order state error value of the system, respectively. , The measured values ​​are the position and the first-order state of the system at the next moment. , For observer gain parameters, , It is a nonlinear factor. b is the disturbance threshold, and b is the control gain. This represents the expected value of the motor's angular velocity.

[0059] Step 3: Calculate the target value of torque current:

[0060]

[0061]

[0062] in, This represents the current motor angular velocity error value. The target value for torque current. , For controller coefficients, , These are the motor angular velocity error values ​​for the first two moments. The target value for the motor's angular velocity. This is the actual value of the motor's angular velocity.

[0063] Step 4: Calculate the torque voltage and excitation voltage:

[0064]

[0065]

[0066]

[0067]

[0068] in, These are the target values ​​for torque current, torque voltage, and excitation voltage, respectively. , These are the actual values ​​of torque current and excitation current. , For controller coefficients, , These are the error values ​​of torque current and excitation current at the current moment, respectively. , , , These are the error values ​​of torque current and excitation current at the first two moments.

[0069] Step 5: The torque voltage and excitation voltage undergo inverse Park transformation, and a three-phase PWM wave is generated through SVPWM debugging to control the motor movement.

[0070] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention without departing from the principles and spirit of the present invention.

Claims

1. A motor control system for a two-axis optoelectronic pod, characterized in that, It comprises: The current sampling EKF fusion filtering module is connected with the motor, and is used for obtaining the motor angular velocity, mechanical angle and current after EKF fusion filtering of the acquired current raw value, angular velocity raw value and angle raw value of the motor; The ADRC controller module is connected with the output mechanical angle in the current sampling EKF fusion filtering module, and is used for calculating the motor angular velocity expected value according to the filtered mechanical angle value and the angle target value by using the ADRC self-disturbance control algorithm; The PID control module is connected with the output motor angular velocity and current in the current sampling EKF fusion filtering module and the output in the ADRC controller module, and is used for calculating the torque current target value according to the motor angular velocity expected value and the filtered motor angular velocity, and then obtaining the torque voltage and excitation voltage of the motor in combination with the filtered current.

2. The motor control system of claim 1, wherein The current sampling EKF fusion filtering module comprises a three-phase current sampling module, a current Clark conversion module, a current Park module, a sensor IMU and a magnetic encoder, and an EKF calculation module; the three-phase current sampling module is connected with the motor and used for collecting the current of the motor; the three-phase current sampling module is connected with the current Clark conversion module, the current Park module and the EKF calculation module in sequence; the first sampling current obtains three-phase current sampling original values, which are output by the current Clark conversion module as the shaft and the shaft current original values, and are converted into torque current original values and excitation current original values by the current Park module and input into the EKF calculation module; the sensor IMU and the magnetic encoder are arranged side by side, one end of each is connected with the motor, and the other end of each is connected with the EKF calculation module; the sensor IMU collects angular velocity original values, and the magnetic encoder obtains angle original values, which are input into the EKF calculation module; The EKF calculation module fuses and calculates to obtain the motor angular velocity, mechanical angle, excitation current and torque current; Mechanical angle, field current, torque current are used for current loop calculation; motor angular velocity is used for speed loop calculation. Angular velocity is used for speed loop calculation.

3. The motor control system of claim 2, wherein, The ADRC controller module comprises a tracking differentiator TD module, an ESO state observer module and an NLSEF state error feedback module; the angle set value first enters the tracking differentiator TD module to calculate a position estimation value and a speed estimation value, and the position estimation value and the speed estimation value are respectively subjected to error calculation with a position estimation value and a system first-order state estimation value output by the ESO state observer module to obtain a difference value, and all are input into the NLSEF state error feedback module; a speed value expected value is obtained by subtracting a system total disturbance estimation calculated by the ESO state observer module from an output value of the NLSEF state error feedback module and dividing by a control gain; the speed value expected value is multiplied by the control gain and a mechanical angle as an input of the ESO state observer module.

4. The motor control system of claim 3, wherein, The PID control module comprises a current PID control module, a torque voltage PID control module, an excitation voltage PID control module, an inverse Park conversion module and an SVPWM debugging module; the output end of the ESO state observer module is connected with the current PID control module, the output of the current PID control module and the output of the EKF calculation module are subjected to error calculation and are respectively input into the torque voltage PID control module and the excitation voltage PID control module; the speed value expected value and the motor angular velocity are subjected to error calculation to obtain the motor angular velocity error value, the motor angular velocity error value is subjected to current PID control to obtain the torque current target value and the excitation current target value; the torque current target value and the torque current are subjected to torque voltage PID control to obtain the torque voltage, and the excitation current target value is obtained to be subjected to excitation voltage PID control to obtain the torque voltage; the torque voltage and the torque voltage are subjected to inverse Park conversion and SVPWM debugging to generate three-phase PWM waves to control the motor movement.

5. A method of motor control system of a two-axis opto-electrical pod, suitable for the motor control system of any one of claims 1-5, characterized in that, The specific steps are: Step 1, the three-phase current sampling module obtains A-phase, B-phase current original sampling values, converts the original sampling values into torque current original values and excitation current original values through Clark transformation and Park transformation; the IMU collects angular velocity original values, the magnetic encoder obtains an electric angle, and all original collection data are fused and filtered through an EKF to obtain motor angular velocity, mechanical angle and torque current, ; Step 2, according to the filtered angle value and the angle target value, the ADRC self-disturbance control algorithm is used to calculate the motor angular velocity expected value; Step 3, calculate the torque current target value; Step 4, calculate the torque voltage and excitation voltage; Step 5, the torque voltage and excitation voltage are subjected to inverse Park conversion and SVPWM debugging to generate three-phase PWM waves to control the motor movement.

6. The method of electric machine control system of claim 5, wherein, In step 1, the EKF algorithm is used to fuse and filter the current sensor, IMU and magnetic encoder; The state change rate of the motor model system is: ； ； ； ； wherein, , , , represents the angular velocity, the field current, the torque current, the rate of change of angle; P is the number of pole pairs, the motor parameter inductance , and are the flux linkage of d-axis and q-axis respectively, is the angular velocity, is the field current, is the torque current, the electromagnetic torque is , the load torque electromagnetic torque is , the field voltage and the torque voltage are and , is the viscous friction torque, J is the moment of inertia, is the stator resistance.

7. The method of electric machine control system of claim 6, wherein, The formula for calculating the torque current target value in step 3 is: ； ； wherein, is the motor angular velocity error value at the current time instant, is the torque current target value, , is the controller coefficient, , are the motor angular velocity error values at the previous two time instants, respectively; is the motor angular velocity target value, is the motor angular velocity actual value.

8. The method of electric machine control system of claim 6, wherein, The formula for calculating the torque current target value in step 4 is: Calculate the torque voltage and excitation voltage: ； ； ； ； Wherein, are target values of torque current, torque voltage, and excitation voltage, respectively, , are actual values of torque current and excitation current. , is a controller coefficient, , are error values of torque current and excitation current at the current moment, respectively; , , , are error values of torque current and excitation current at the previous two moments.

Images