Method and system for calculating the external force disturbance of the pod based on servo current feedback

Abstract

This invention relates to the field of servo control algorithm design for aircraft airborne pods, providing a method and system for calculating external disturbance reactance based on servo motor current feedback. The method acquires three-phase currents Ia, Ib, and Ic, and electrical angle θ, and obtains two-phase currents Id and Iq through Clarke and Park transforms, defining Iq as the external disturbance reactance estimate IQ. A setpoint r(t) and deviation e(t) = r(t) - IQ are established, and the PID controller output u(t) is calculated. u(t) is used as the q-axis control reference and written to the driver. In voltage control mode, it is mapped to Vq and Vd for inverse transformation and PWM modulation; in current control mode, it is mapped to the q-axis current reference Iqref and converted into corresponding voltage commands by the driver's current loop, driving the servo motor to generate electromagnetic torque to cancel out disturbance reactance. This invention eliminates the need for complex motor models and high-end inertial sensors, has low computational complexity, and can quickly and accurately estimate disturbances, ensuring stable control performance of the aircraft pod in complex flight environments.

Description

Technical Field

[0001] This invention relates to the technical fields of electronic digital data processing, intelligent manufacturing equipment, and pod algorithm design, and in particular to a method and system for calculating the external force disturbance impedance of a pod based on the current feedback of a servo. Background Technology

[0002] Aircraft-borne pods (also known as airborne electro-optical pods or mission pods) are widely used in aerial reconnaissance, surveillance, targeting, navigation, and ground imaging missions. These pods typically maintain a stable attitude during flight via servo mechanisms to ensure the pointing accuracy and imaging clarity of electro-optical sensors, radar antennas, or optical targeting equipment. However, during flight, aircraft are subjected to various external forces such as airflow disturbances, maneuvering overloads, engine vibrations, and load eccentricity, inevitably subjecting the pod structure and servo system to complex disturbance torques. To maintain pod stability and high-precision pointing, these external disturbances need to be estimated and compensated for in real time. In existing technologies, a common approach is to use algorithms based on Active Disturbance Rejection Control (ADRC), establishing motor output models and gyroscope feedback models through an Extended State Observer (ESO) to estimate and compensate for external disturbances. However, this approach has the following drawbacks: strong model dependence: the structural parameters of the motor and pod change with flight environment, temperature, and load conditions, and model mismatch can easily lead to inaccurate disturbance estimation; high sensor requirements: high-performance inertial devices such as gyroscopes are required to provide high-bandwidth feedback, increasing system cost and complexity; high computing power consumption: the real-time operation of the ESO observer requires high processor computing power, increasing the burden on the airborne electronic system; insufficient robustness: when the mission environment changes abruptly or the payload is changed, the model and observer parameters often need to be readjusted, resulting in limited adaptability.

[0003] Therefore, existing aircraft pod servo control technology urgently needs an anti-disturbance method that does not require complex motor models and high-end inertial sensors, has less computational load, and can quickly and accurately estimate disturbances, in order to ensure the stable control performance of the pod in complex flight environments. Summary of the Invention

[0004] To address the shortcomings of the existing technology, this invention provides a method and system for calculating the external force disturbance immunity of the pod based on the current feedback of the servo. This method eliminates the need for complex motor models and high-end inertial sensors in the servo control of aircraft-borne pods, reduces the computational load, and can quickly and accurately estimate disturbances, ensuring the stable control performance of the pod in complex flight environments.

[0005] In a first aspect, the present invention provides a method for calculating the external force disturbance reactance of a pod based on the current feedback of a servo, comprising:

[0006] A servo motor, driver, three-phase current detection circuit and processor are set at each controlled rotating shaft of the pod. The processor synchronously acquires the three-phase currents Ia, Ib and Ic of the servo motor and the electrical angle θ of the controlled rotating shaft collected by the three-phase current detection circuit under a unified sampling period.

[0007] Performing an amplitude-invariant Clarke transform on Ia, Ib, and Ic yields two-phase stationary coordinate system currents Iα and Iβ, where the Clarke transform satisfies Iα = (2 / 3)Ia - (1 / 3)Ib - (1 / 3)Ic and Iβ = (1 / )(Ib-Ic) and the zero-sequence component I0=(1 / 3)(Ia+Ib+Ic) is constrained to 0 under three-phase symmetrical power supply;

[0008] Based on the electrical angle θ, Park transform is applied to Iα and Iβ to obtain Id and Iq, where Id = Iα·cosθ + Iβ·sinθ and Iq = -Iα·sinθ + Iβ·cosθ. Iq is defined as the external disturbance reactance estimate IQ to establish the setpoint r(t) and deviation e(t) = r(t) - IQ. The continuous-time form of the PID controller output u(t) satisfies:

[0009] ;in, This is the proportionality coefficient. The integral coefficient is... These are the differential coefficients;

[0010] The processor writes u(t) as the q-axis control reference to the driver. Specifically, when the driver is in voltage control mode, u(t) is mapped to the q-axis voltage command Vq and combined with the d-axis voltage command Vd through inverse Park / inverse Clarke transformation and synthesized into a three-phase modulation quantity by PWM. When the driver is in current control mode, u(t) is mapped to the q-axis current reference Iqref and converted into the corresponding voltage command by the current loop in the driver to drive the servo motor to generate electromagnetic torque opposite to the direction of external force interference, thereby achieving interference suppression.

[0011] Secondly, the present invention provides a system for calculating the external force disturbance impedance of a pod based on the current feedback of a servo, wherein the system for calculating the external force disturbance impedance of a pod based on the current feedback of a servo uses the above-described method for calculating the external force disturbance impedance of a pod based on the current feedback of a servo.

[0012] Compared with the prior art, the beneficial effects of this invention are as follows:

[0013] This invention provides a method and system for calculating the external force disturbance impedance of a pod based on servo current feedback. The method includes: setting up a servo motor, a driver, a three-phase current detection circuit, and a processor at each controlled rotating shaft of the pod; the processor synchronously acquiring the three-phase currents Ia, Ib, and Ic of the servo motor and the electrical angle θ of the controlled rotating shaft collected by the three-phase current detection circuit under a uniform sampling period; performing an amplitude-invariant Clarke transformation on Ia, Ib, and Ic to obtain two-phase stationary coordinate system currents Iα and Iβ, wherein the Clarke transformation satisfies Iα = (2 / 3)Ia - (1 / 3)Ib - (1 / 3)Ic and Iβ = (1 / (Ib-Ic) and the zero-sequence component I0=(1 / 3)(Ia+Ib+Ic) is constrained to 0 under three-phase symmetrical power supply; Id and Iq are obtained by performing Park transformation on Iα and Iβ based on the electrical angle θ, where Id=Iα·cosθ+Iβ·sinθ and Iq=-Iα·sinθ+Iβ·cosθ, and Iq is defined as the external force disturbance impedance estimate IQ to establish the setpoint r(t) and deviation e(t)=r(t)-IQ; the continuous-time form of the PID controller output u(t) satisfies:

[0014] ;in, This is the proportionality coefficient. The integral coefficient is... The processor writes u(t) as the q-axis control reference to the driver. Specifically, when the driver operates in voltage control mode, u(t) is mapped to the q-axis voltage command Vq and combined with the d-axis voltage command Vd through inverse Park / inverse Clarke transformation and PWM to synthesize a three-phase modulation quantity. When the driver operates in current control mode, u(t) is mapped to the q-axis current reference Iqref and converted into the corresponding voltage command by the current loop within the driver, which then drives the servo motor to generate electromagnetic torque opposite to the direction of external disturbance rejection, thereby achieving disturbance rejection suppression. The method of this invention eliminates the need for complex motor models and high-end inertial sensors in the servo control of aircraft airborne pods, reduces computational load, and can quickly and accurately estimate disturbances, ensuring stable control performance of the pod in complex flight environments. Attached Figure Description

[0015] The accompanying drawings, which are included to provide a further understanding of the invention and form part of this invention, illustrate exemplary embodiments of the invention and are used to explain the invention, but do not constitute an undue limitation of the invention. Some specific embodiments of the invention will be described in detail below with reference to the accompanying drawings in an exemplary and non-limiting manner. The same reference numerals in the drawings designate the same or similar parts or components. It should be understood by those skilled in the art that these drawings are not necessarily drawn to scale. In the drawings:

[0016] Figure 1 This is a flowchart illustrating a method for calculating the external force disturbance impedance of the pod based on the current feedback of the servo according to an embodiment of the present invention. Detailed Implementation

[0017] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments. Obviously, the described embodiments are merely some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0018] See Figure 1 This embodiment provides a method for calculating the external force disturbance reactance of the pod based on the current feedback of the servo, including the following steps:

[0019] A servo motor, driver, three-phase current detection circuit and processor are set at each controlled rotating shaft of the pod. The processor synchronously acquires the three-phase currents Ia, Ib and Ic of the servo motor and the electrical angle θ of the controlled rotating shaft collected by the three-phase current detection circuit under a unified sampling period.

[0020] Performing an amplitude-invariant Clarke transform on Ia, Ib, and Ic yields two-phase stationary coordinate system currents Iα and Iβ, where the Clarke transform satisfies Iα = (2 / 3)Ia - (1 / 3)Ib - (1 / 3)Ic and Iβ = (1 / )(Ib-Ic) and the zero-sequence component I0=(1 / 3)(Ia+Ib+Ic) is constrained to 0 under three-phase symmetrical power supply;

[0021] Based on the electrical angle θ, Park transform is applied to Iα and Iβ to obtain Id and Iq, where Id = Iα·cosθ + Iβ·sinθ and Iq = -Iα·sinθ + Iβ·cosθ. Iq is defined as the external disturbance reactance estimate IQ to establish the setpoint r(t) and deviation e(t) = r(t) - IQ. The continuous-time form of the PID controller output u(t) satisfies:

[0022] ;in, This is the proportionality coefficient. The integral coefficient is... These are the differential coefficients;

[0023] The processor writes u(t) as the q-axis control reference to the driver. Specifically, when the driver is in voltage control mode, u(t) is mapped to the q-axis voltage command Vq and combined with the d-axis voltage command Vd through inverse Park / inverse Clarke transformation and synthesized into a three-phase modulation quantity by PWM. When the driver is in current control mode, u(t) is mapped to the q-axis current reference Iqref and converted into the corresponding voltage command by the current loop in the driver to drive the servo motor to generate electromagnetic torque opposite to the direction of external force interference, thereby achieving interference suppression.

[0024] In some specific examples, the method for calculating the external force disturbance reactance of the pod based on the servo's current feedback may include the following steps:

[0025] The three-phase currents Ia, Ib, and Ic and the electrical angle θ are acquired synchronously (with a unified sampling reference); the Clarke transform is used to obtain Iα and Iβ, and the zero-sequence I0≈0; the Park transform (with θ as the reference) is used to obtain Id and Iq.

[0026] Define the external disturbance resistivity estimator IQ=Iq, and the error e(t)=r(t)-IQ;

[0027] The PID controller generates u(t); in voltage control mode, u(t) is mapped to Vq, and then synthesized into a three-phase modulation quantity by inverse Park / inverse Clarke and PWM; in current control mode, u(t) is mapped to Iq_ref, and then driven by voltage conversion from the driver current loop.

[0028] The motor outputs electromagnetic torque opposite to external disturbances to compensate for them, thereby achieving pod stability and high-precision pointing / imaging.

[0029] In this embodiment, the processor synchronously acquires the three-phase currents Ia, Ib, and Ic, as well as the controlled shaft electrical angle θ, under a unified sampling period. This solves the phase error and estimation jitter problems caused by the asynchronous current sampling and angle measurement, achieving a consistent time reference for current to disturbance estimation and improving the transient accuracy and stability of disturbance estimation. A Clarke transformation with invariant amplitude is performed on Ia, Ib, and Ic to obtain Iα and Iβ, and the zero-sequence component I0 is constrained to 0 under three-phase symmetrical power supply. This solves the problems of strong coupling, amplitude distortion, and difficulty in suppressing unbalanced disturbances caused by direct participation of three-phase quantities in control, achieving amplitude preservation and zero-sequence suppression in the αβ stationary coordinate system. A Park transformation is performed on Iα and Iβ based on the electrical angle θ to obtain Id and Iq, solving the problem of difficulty in directly utilizing torque and excitation coupling in the stator coordinate system. This decouples the torque channel (q-axis) from the excitation channel (d-axis) in the dq rotating coordinate system, creating mathematical observability for torque estimation based on current. Defining Iq as the external disturbance reactance estimate IQ and establishing the setpoint r(t) and deviation e(t) = r(t) - IQ can solve the problems of gyroscope / model-dependent disturbance observers (ESOs) being sensitive to controlled object parameters and costly. It enables the direct acquisition of an estimate equivalent to the external disturbance torque using only current feedback, transforming disturbance compensation into a standard error adjustment task and reducing reliance on high-end inertial devices and accurate models. The PID controller is then... The generated control input can solve the problem of difficulty in balancing steady-state error and overshoot when pure proportional or fixed feedforward inputs are used to handle rapidly changing disturbances. It achieves a comprehensive effect where P provides stiffness to suppress external disturbances, I eliminates steady-state residuals, and D predicts changes to suppress overshoot and oscillations, balancing speed and steady-state accuracy. The processor writes u(t) as the q-axis control reference to the driver. In voltage control mode, it is mapped to Vq and synthesized into a three-phase modulation input via inverse Park, inverse Clarke, and PWM. This solves the problem of not being able to directly use the current reference for torque compensation on drivers without an internal current loop, enabling low-modification integration of the disturbance rejection channel on voltage-type drive platforms, improving the versatility and portability of the solution. In current control mode, u(t) is mapped to the q-axis current reference Iqref and converted into voltage commands by the driver's current loop to drive the servo motor. This solves the problem of residual jitter caused by limited torque loop bandwidth and compensation lag, achieving fast, low-latency torque output based on the high bandwidth of the driver's internal current loop, enhancing the ability to suppress sudden external disturbances. Compensation using electromagnetic torque generated by the q-axis current in the opposite direction to external disturbances can solve the problem of the equivalent external torque caused by aerodynamic disturbances, fuselage vibrations, and load eccentricity acting directly on the rotating shaft during flight, which is difficult to suppress. This achieves active cancellation of external disturbances at the torque level, significantly improving pod attitude stability, pointing accuracy, and imaging clarity. Standardized coordinate transformations of Clarke and Park with unchanged amplitude can solve the problem of inconsistent control laws and calibrations between different motor parameters and different axis systems, achieving consistent control scales and allowing parameters to be reused across axes, reducing the tuning workload of multi-axis pods and improving maintenance efficiency. Directly comparing IQ and r(t) under unified sampling, coordinate decoupling, and PID closed-loop can solve the problem of frequent retuning of observer parameters when mission environment changes abruptly or load changes. This allows external disturbance estimation and compensation to be encapsulated into a universal error loop, mainly for tuning... / / This results in stable performance with enhanced adaptability and maintainability. The standard power conversion output of inverse Park and inverse Clarke, along with PWM, resolves the issues of inconsistent interfaces and uncontrollable delays between disturbance rejection control and power stage drive, achieving deterministic transmission and low-latency execution of control commands to the three-phase bridge arms, ensuring compensation bandwidth and dynamic tracking capability. Constraints on the zero-sequence component I0 address the difficulty in detecting interferences to estimation accuracy caused by current detection anomalies, cable faults, and power supply imbalances, enabling online consistency verification using the physical prior of I0 being 0, thus improving the system's self-monitoring and fault tolerance. Applying the disturbance rejection estimate IQ directly to the q-axis solves the problems of excessively long external compensation paths in the position and velocity loops and insufficient phase margin in cascaded components, achieving shorter torque control loops and higher effective suppression bandwidth, reducing low-frequency drift and high-frequency chattering. Overall, this embodiment uses current to estimate torque and q-axis direct compensation to transform the complex external disturbance observation problem into a standard PID error adjustment problem. Without relying on high-end gyroscopes and complex object models, it achieves low-computing-power, robust, and easily tuned external disturbance suppression, thereby ensuring pod stability and pointing accuracy in complex flight environments.

[0030] Preferably, the three-phase current detection circuit includes a shunt sampling resistor connected in series with the three phases U, V, and W of the servo motor, a differential instrumentation amplifier, and an analog-to-digital converter channel. The two ends of the shunt sampling resistor are respectively connected to the input terminal of the differential instrumentation amplifier, and the output terminal of the differential instrumentation amplifier is electrically connected to the analog-to-digital converter channel. The processor performs synchronous sampling on the three analog-to-digital converter channels and performs zero drift compensation and temperature drift correction at the beginning of each sampling cycle. The zero drift compensation is achieved by acquiring static samples in the motor de-excitation window to obtain the bias and subtracting it online. The temperature drift correction is achieved by establishing a mapping table between sampling gain and temperature during the power-on self-test phase and interpolating based on the temperature sensor readings during operation, so that Ia, Ib, and Ic entering the Clarke transform meet the requirements of amplitude balancing and phase consistency.

[0031] In this embodiment, in the three-channel measurement link consisting of a three-phase series shunt resistor (U, V, W), a differential instrumentation amplifier, and an analog-to-digital converter, the processor performs synchronous sampling across the three channels and executes zero-drift compensation and temperature drift correction at the beginning of each sampling cycle. This ensures that Ia, Ib, and Ic entering the Clarke converter satisfy amplitude balancing and phase consistency. This solves the problems of amplitude mismatch and phase error caused by device bias, amplifier zero drift, temperature drift, channel inconsistency, and asynchrony in the current sensing link, which in turn cause distortion in the Clarke and Park outputs, and bias and jitter in the disturbance rejection estimate IQ. This achieves consistency and repeatability comparable to high-end sensing links while keeping hardware costs under control. It effectively reduces systematic errors caused by DC bias and cross-temperature gain changes, improves the predictability of disturbance estimation and subsequent PID tuning, and reduces the risk of misjudgment and miscompensation.

[0032] Preferably, the processor triggers three-phase sampling outside the dead zone after the voltage vector update, based on the switching period Ts, to avoid the current ripple peak. The sampling result is first passed through a first-order or second-order low-pass digital filter to suppress high-frequency switching harmonics and maintain a bandwidth not less than twice the motor current control bandwidth, and then sent to Clarke and Park transforms. The filter coefficients of the digital filter are adaptively adjusted according to Ts to maintain a consistent phase margin at different PWM frequencies.

[0033] In this embodiment, sampling is triggered outside the dead zone after the voltage vector update, using the switching period Ts as a reference, to avoid sampling the peak of the current ripple. First, high-frequency switching harmonics are suppressed by first-order and second-order low-pass digital filters while maintaining a bandwidth ≥ twice that of the current loop. Then, the data is fed into Clarke and Park, and the filter coefficients are made adaptive with Ts to maintain a consistent phase margin under different PWM frequencies. This can solve the problems of high-frequency noise, equivalent measurement delay, and phase uncertainty caused by improper sampling timing and PWM harmonic injection, which make disturbance rejection estimation and torque loop prone to oscillation. It achieves low-noise and low-delay equivalent sampling outside the influence of commutation and dead zone. It maintains constant control phase characteristics and stability margin under variable frequency PWM conditions. It takes into account both noise suppression and bandwidth requirements, making IQ estimation clean and fast, thereby improving the dynamic controllability and robustness of the disturbance rejection loop.

[0034] Preferably, when acquiring the electrical angle θ, if the system is equipped with a position sensor, the mechanical angle θm is output by an absolute encoder or rotary transformer, converted into an electrical angle θ = p·θm by the number of pole pairs p, and the coordinates are aligned according to the phase sequence and zero offset calibration value to form a Park transformation angle reference; if the system is running in a sensorless mode, the rotational speed ωe and electrical angle θ are estimated by an observer based on the voltage and current model in the stator α and β stationary coordinate system, and the back EMF integration stage is limited and discharged in the low-speed region to suppress integration drift.

[0035] In this embodiment, when a sensor is present, the mechanical angle θm is output by an absolute encoder or resolver, converted to θ=p·θm using pole pairs, and the coordinates are aligned according to the phase sequence and zero-position offset. In the sensorless mode, an observer based on a voltage and current model estimates ωe and θ in the αβ system, and sets a limit and discharge to suppress integral drift in the back EMF integration in the low-speed region. This can solve the problems of inconsistent electrical angle acquisition under different hardware configurations, dq axis misalignment caused by phase sequence / zero-position mismatch, and angle drift caused by weak back EMF and integral drift in sensorless low-speed conditions. It can achieve a reliable Park angle regardless of whether a sensor is present or not, ensuring the consistency of dq axis decoupling and Iq equivalent torque. It suppresses angle observation drift in the low-speed and start-up phases to avoid incorrect compensation direction. It shortens the calibration time and enhances cross-platform reuse capability, providing a stable angle reference for IQ estimation and q-axis compensation.

[0036] Preferably, after obtaining Id and Iq, the processor sets the target current of the d-axis to Id*, either zero or given according to the load flux requirement, and uses a PI loop to adjust Id to tend towards Id* to reduce cross-axis coupling; in the q-axis channel, a PID loop is used to calculate u(t) and its output is subject to symmetrical saturation and rate limiting, while an inverse integral saturation strategy is used to freeze the integral term or back to the feasible region when |u(t)| reaches the limit.

[0037] In this embodiment, after obtaining Id and Iq, the target Id of the d-axis is set to zero or given according to the flux linkage requirement, and Id is adjusted by a PI loop to reduce cross-axis coupling. On the q-axis, PID is used to calculate u(t) and implement symmetrical saturation and rate limiting. At the same time, inverse integral saturation is used to freeze or roll back the integral when |u(t)| reaches the limit. This can solve the problems of mutual influence of regulation caused by excitation and torque coupling, and the problems of control quantity saturation and integral accumulation caused by external disturbances, which lead to system overshoot and oscillation. It achieves complete decoupling of the dq axis and fast and stable torque regulation of the q axis. Under voltage / current limiting, it avoids integral overshoot and large reverse overshoot during recovery. It improves the anti-saturation robustness and recoverability against sudden disturbances, thereby maintaining smooth torque output and stable attitude under a wider range of operating conditions.

[0038] Preferably, the processor calculates the equivalent q-axis component Iload of the static gravitational torque on the motor shaft based on the pod geometry parameters, load mass and gravity direction, and sets the set value r(t) as Iload or its amplification factor, so that the PID only operates on the incremental part of the external disturbance.

[0039] In this embodiment, the processor calculates the equivalent q-axis component Iload of the static gravitational torque on the axis based on the pod's geometric parameters, load mass, and gravity direction. The setpoint r(t) is set as Iload or its amplification factor, so that the PID only operates on the incremental part of external disturbances. This can solve the long-standing problem that the static bias torque forces the integral term to accumulate continuously, causing chronic saturation and increased heat load, and making it difficult to distinguish between dynamic disturbances and static bias. It achieves feedforward cancellation of constant gravity load, significantly reducing the static pressure of the error loop; reduces the workload of the PID and increases the dynamic margin, so that the system's main efforts are focused on suppressing true random / time-varying disturbances; reduces heat generation and energy consumption, and improves steady-state accuracy and following speed.

[0040] Preferably, the method is applicable to the coordinated control of pitch, roll and yaw axes of a multi-axis pod. The processor performs current acquisition, coordinate transformation, disturbance rejection estimation and PID adjustment for each axis, and uses a coupling compensation matrix between the three axes to counteract the inter-axis torque coupling caused by load eccentricity. The elements of the compensation matrix are obtained according to the assembly calibration and can be updated online during operation based on inertial parameter estimation.

[0041] In this embodiment, the method for calculating the external force disturbance resistance of the pod based on the servo current feedback is applicable to pitch, roll, and yaw three-axis coordination. The processor performs sampling, transformation, disturbance resistance estimation, and PID control for each axis separately. A coupling compensation matrix is ​​used between the three axes to cancel the inter-axis torque coupling caused by load eccentricity. The matrix elements are obtained based on assembly calibration and can be updated online according to inertial parameter estimation. This can solve the problems of strong inter-axis coupling in multi-axis pods under eccentric / asymmetric loads, and the easy mutual restraint, cross oscillation, and accumulation of pointing errors caused by independent single-axis control. It achieves active decoupling at the control level, weakens the propagation path of external disturbances between axes, and corrects the coupling model online with load / attitude changes to maintain long-term consistent dynamics. It can still achieve high-precision stability and fast convergence under heavy maneuvering or heavy load conditions, significantly improving the imaging and aiming stability under three-axis coordination.

[0042] Preferably, during the acquisition of the three-phase currents Ia, Ib, and Ic of the servo motor, when the current sampling channel experiences saturation, disconnection, abnormal noise, or theta mismatch, the processor, based on the residual threshold and consistency check, causes IQ to enter a degraded mode, freezing the differential term and reducing... It also limits the integral growth and smoothly transitions u(t) to a safety control command based on the speed or position loop. After the sampling returns to normal, the degradation mode and safety control are released according to the hysteresis criterion.

[0043] In this embodiment, when the current sampling channel experiences saturation, disconnection, abnormal noise, or theta mismatch, the processor, based on the residual threshold and consistency check, causes IQ to enter a degradation mode, freezing the differential term and reducing... By limiting integral growth and smoothly transitioning u(t) to speed or position environmentally friendly control, and releasing it according to the hysteresis criterion after sampling returns to normal, it can solve the problem of violent oscillations and runaway risks caused by continuing high-gain torque compensation when the sensor link fails or the angle is misaligned. It achieves rapid and smooth fault-tolerant degradation and avoids abnormal short-term impacts. In the stage of unreliable information, it maintains basic stability and safety with a low-bandwidth bottom-line strategy. By avoiding frequent jittery switching through hysteresis, it significantly enhances the safety and task continuity of the system under extreme conditions and sensor anomalies.

[0044] Preferably, the processor injects a step current into the q-axis under no-load or light-load conditions and measures the speed response. Based on the steady-state balance relationship, it estimates the motor torque constant Kt and establishes the conversion coefficient from Iq to the equivalent torque on the shaft. At the same time, it determines the U, V, W phase sequence and encoder zero-position bias through the minimum variance criterion and writes them into the non-volatile memory.

[0045] In this embodiment, a stepped current is injected into the q-axis under no-load or light-load conditions, and the rotational speed response is measured. The torque constant Kt is estimated based on the steady-state relationship, and a conversion coefficient from Iq to the equivalent torque on the shaft is established. At the same time, the minimum variance criterion is used to determine the N, V, M phase sequence and encoder zero-position offset, and write N, V, M. This can solve the problems of Kt uncertainty caused by manufacturing dispersion and assembly errors, phase sequence reversal or zero-position offset causing dq-axis misalignment, and the need for repeated manual adjustment. It can achieve rapid self-calibration after power-on or maintenance to obtain accurate current-torque mapping and correct phase registration; reduce manual calibration time and human error; and improve the consistency and absolute accuracy of disturbance rejection estimation and torque compensation.

[0046] Preferably, to adapt to low-computing-power embedded processors, the calculation of cosθ and sinθ adopts the CORDIC rotation algorithm or lookup table interpolation algorithm. Clarke, Park, inverse Park and inverse Clarke transformations use fixed-point Q format operations and are combined with shift scaling to avoid overflow. At the same time, the PID parameters are tuned offline in the fixed-point domain according to dimensional consistency and online fine-tuning step size is provided during operation.

[0047] In this embodiment, cosθ and sinθ are calculated using CORDIC or lookup table interpolation, while Clarke, Park, and inverse transforms use fixed-point Q-formats combined with shift scaling to avoid overflow. Simultaneously, the PID parameters are tuned offline in the fixed-point domain with dimensional consistency and online fine-tuning step size is provided. This addresses the problems of limited computing power and storage in airborne embedded systems, high floating-point costs and difficulty in guaranteeing real-time performance, and the ease of overflow / quantization distortion in fixed-point implementation. It enables stable and predictable full control bandwidth operation even on low-computing-power platforms; the numerical range is controlled, avoiding overflow and jitter; parameter tuning and online fine-tuning have a unified scale and traceability, thus supporting high-bandwidth disturbance rejection control with lower hardware costs and improving engineering feasibility and reliability.

[0048] Preferably, when changes in the statistical characteristics of pod attitude, load, or environmental disturbance are detected, the processor updates r(t) and PID parameters online based on the mean and variance of IQ within a time window, while keeping Id* unchanged or adjusting it to a weak magnetic target as needed.

[0049] In this embodiment, when changes in attitude, load, or disturbance statistical characteristics are detected, the processor updates r(t) and PID parameters online based on the mean and variance of IQ within the time window, and keeps Id* unchanged or adjusts it to a weak magnetic target as needed. This can solve the problems of performance degradation of fixed parameter controllers, reappearance of static bias, and tuning mismatch caused by changes in dynamic disturbance spectrum due to changes in mission environment and load over time. It realizes the absorption of the long-term mean of IQ into the setpoint feedforward and the mapping of variance changes to gain / bandwidth self-tuning, enabling the system to adaptively maintain speed and steady-state accuracy over a long period of time. At high speeds or in specific attitudes, it can be used in conjunction with the weak magnetic strategy to maintain voltage margin, thereby reducing the frequency of maintenance and tuning, and ensuring stable pointing and imaging quality throughout complex flight missions.

[0050] It should be noted that the embodiments of the present invention also propose a system for calculating the external force disturbance impedance of the pod based on the current feedback of the servo. The system for calculating the external force disturbance impedance of the pod based on the current feedback of the servo uses the above-described method for calculating the external force disturbance impedance of the pod based on the current feedback of the servo.

[0051] It should be noted that the above embodiments are merely preferred embodiments of the present invention, and the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention, and the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A method of calculating the external force disturbance of a pod from servoed current feedback, characterized by, The application relates to a method for controlling a servo motor, which comprises the following steps: A servo motor, a driver, a three-phase current detection circuit and a processor are arranged at each controlled rotating shaft of a pod, the processor synchronously acquires three-phase phase currents Ia, Ib and Ic of the servo motor and an electric angle theta of the controlled rotating shaft collected by the three-phase current detection circuit under a unified sampling period; Performing a magnitude-invariant Clarke transformation on Ia, Ib, Ic results in two-phase stationary coordinate system currents Ia, Iβ, which satisfy Ia = (2 / 3)Ia - (1 / 3)Ib - (1 / 3)Ic, Iβ = (1 / 3)Ia + (1 / 3)Ib - (1 / 3)Ic, and the zero sequence component I0 = (1 / 3)(Ia + Ib + Ic) is constrained to 0 under three-phase symmetric power supply. Ib - Ic) and the zero sequence component I0 = (1 / 3)(Ia + Ib + Ic) is constrained to 0 under three-phase symmetric power supply. Park transformation is performed on Ia and Ib based on the electric angle theta to obtain Id and Iq, wherein Id=Ia*cos theta+Ib*sin theta, Iq=-Ia*sin theta+Ib*cos theta, the Iq is defined as an external force disturbance estimation IQ to establish a set value r(t) and a deviation e(t)=r(t)-IQ; and a continuous time form of a PID controller output u(t) satisfies: ; wherein is a proportional coefficient, is an integral coefficient, is a derivative coefficient; The processor writes u(t) as a q-axis control reference into the driver, specifically: when the driver works in a voltage control mode, u(t) is mapped into a q-axis voltage instruction Vq and is combined with a d-axis voltage instruction Vd to be inversely Park / Clarke transformed and PWM-synthesized into three-phase modulation quantities; when the driver works in a current control mode, u(t) is mapped into a q-axis current reference Iqref and is converted into a corresponding voltage instruction by a current loop in the driver to drive the servo motor to generate an electromagnetic torque opposite to the direction of the external force disturbance, thereby realizing disturbance suppression.

2. The method of claim 1, wherein, The three-phase current detection circuit comprises shunt sampling resistors connected in series with three phases U, V and W of the servo motor, differential instrument amplifiers and analog-to-digital converter channels, two ends of the shunt sampling resistors are respectively connected with input ends of the differential instrument amplifiers, and output ends of the differential instrument amplifiers are electrically connected with the analog-to-digital converter channels; The processor performs synchronous sampling on the three analog-to-digital converter channels, and executes zero drift compensation and temperature drift correction at the beginning of each sampling period, wherein the zero drift compensation is achieved by collecting static samples in a motor de-excitation window to obtain a bias and online deduction, and the temperature drift correction is achieved by establishing a mapping table of sampling gain and temperature in a self-checking stage at startup and performing interpolation correction according to temperature sensor readings in a running process, so that Ia, Ib and Ic entering the Clarke transformation meet the requirements of amplitude matching and phase consistency.

3. The method of claim 1, wherein, The processor triggers three-phase sampling at a time point outside a dead zone after a voltage vector is updated based on a switching period Ts to avoid current ripple peaks, the sampling results are first sent into a first-order or second-order low-pass digital filter to suppress high-frequency switching harmonics and maintain a bandwidth not less than twice of a motor current control bandwidth, and then are sent into Clarke and Park transformations, and filter coefficients of the digital filter are adaptively adjusted according to Ts to maintain a consistent phase margin under different PWM frequencies.

4. The method of claim 1, wherein, When the electric angle theta is acquired, if a position sensor is configured, a mechanical angle theta m is output by an absolute encoder or a rotary transformer, is converted into the electric angle theta=p*theta m according to the number of pole pairs p, and is coordinate-aligned according to a phase sequence and a zero-position bias calibration value to form a Park transformation angle reference; if the system runs in a position sensorless mode, a speed observer based on a voltage and current model estimates a rotating speed omega e and the electric angle theta in a stator alpha-beta stationary coordinate system, and amplitude limiting and bleeder are set to an integral link of a back electromotive force to suppress integral drift in a low-speed area.

5. The method of claim 1, wherein, The processor sets the d-axis target current as Id* after obtaining Id and Iq, takes zero or gives a value according to the load flux demand, and adjusts Id to Id* by a PI loop to reduce the cross-axis coupling; in the q-axis channel, a PID loop is used to calculate u(t) and implement symmetric saturation and rate limitation on the output, and an anti-integration saturation strategy is used to freeze the integral term or back to the feasible region when |u(t)| reaches the limit.

6. The method of claim 1, wherein, The method is suitable for coordinated control of three axes of pitch, roll and heading of a multi-axis nacelle, the processor performs current collection, coordinate transformation, disturbance estimation and PID regulation for each axis, and uses a coupling compensation matrix among the three axes to offset the axis torque coupling caused by load eccentricity, the elements of the compensation matrix are obtained according to assembly calibration and can be updated online according to inertia parameter estimation during operation.

7. The method of claim 1, wherein, In the process of collecting the three-phase phase currents Ia, Ib, Ic of the servo motor, when the current sampling channel appears saturation, broken line, abnormal noise or θ mismatch sampling abnormal state, the processor makes IQ enter the degradation mode according to the residual threshold and consistency check, freezes the differential term, reduces and limits the integral growth, and smoothly transitions u(t) to the bottom control instruction based on the speed or position ring, and then releases the degradation mode and the bottom control according to the hysteresis criterion after the sampling recovers to normal.

8. The method of claim 1, wherein, The processor injects a step current into the q-axis under no-load or light-load conditions and measures the speed response, estimates the motor torque constant Kt according to the steady-state balance relationship and establishes a conversion coefficient from Iq to the equivalent torque on the shaft, and determines the U, V and W phase sequence and the encoder zero position offset by the least variance criterion and writes them into the non-volatile memory.

9. A system for calculating the external force disturbance of a pod based on servoed current feedback, the system comprising: The system for calculating the external force disturbance of the nacelle according to the current feedback of the servo uses the method for calculating the external force disturbance of the nacelle according to the current feedback of the servo according to any one of claims 1-8.

Images