A method for controlling the speed of a robotic arm based on load torque feedforward

Abstract

This application relates to the field of robotic arm control technology, and more particularly to a robotic arm speed control method based on load torque feedforward. The method includes: acquiring the angular velocity and three-phase current of the motor, performing coordinate transformation to obtain the direct-axis current and quadrature-axis current; generating a voltage control signal using a current loop controller; generating a quadrature-axis current reference signal using a speed loop controller; constructing a load torque observer based on the angular velocity and quadrature-axis current to estimate the load torque in real time; detecting the acceleration start flag and deceleration start flag of the motor; adding the estimated load torque as a feedforward quantity to the input of the current loop controller within a first predetermined time after detecting the acceleration start flag; and removing the feedforward quantity from the input of the current loop controller within a second predetermined time after detecting the deceleration start flag. This achieves the goal of maintaining constant parameters of the current loop and speed loop controllers when the load and moment of inertia change, and smoothly suppressing speed fluctuations.

Description

Technical Field

[0001] This application relates to the field of robotic arm control technology, and in particular to a robotic arm speed control method based on load torque feedforward. Background Technology

[0002] In the field of industrial robotics and automation control, multi-joint robotic arms are widely used due to their flexibility. However, the posture of the robotic arm changes continuously during operation, causing the equivalent moment of inertia and equivalent load on each joint axis to change in real time. This makes the entire system a multi-inertia nonlinear system with significant cross-coupling characteristics, and its dynamic equations are extremely complex. Faced with this dynamic load change and strong nonlinear coupling, traditional control methods often use fixed-parameter speed loop and current loop PID controllers. Since the controller parameters are set once before operation and remain unchanged during operation, this fixed-parameter control strategy is difficult to effectively compensate when the actual load and moment of inertia deviate from the design conditions. Manually adjusting the speed loop PID parameters is time-consuming, difficult, and has poor performance. If traditional dynamic equations are used to identify the system inertia, the identification results are often biased due to cross-coupling characteristics, resulting in poor practical application effects or even adverse effects. Establishing multi-axis nonlinear dynamic equations also presents challenges in practical application due to computational complexity and MCU computing power limitations. Summary of the Invention

[0003] To address the above problems, this application provides a robotic arm speed control method based on load torque feedforward, comprising the following steps:

[0004] The angular velocity and three-phase current of the motor are collected, and the three-phase current is transformed by coordinate transformation to obtain the direct-axis current and quadrature-axis current;

[0005] A voltage control signal is generated based on the direct-axis current and the quadrature-axis current using a current loop controller, wherein the proportional-integral parameters of the current loop controller are preset based on the motor inductance and resistance and remain unchanged during operation;

[0006] A quadrature-axis current reference signal is generated based on the angular velocity using a speed loop controller, wherein the proportional-integral parameters of the speed loop controller are preset based on the motor's moment of inertia and torque coefficient and remain unchanged during operation;

[0007] A load torque observer is constructed based on the angular velocity and the cross-axis current to estimate the load torque in real time. The load torque observer uses only one adjustable observer coefficient, which is selected in the negative range according to the system moment of inertia and discrete sampling period to achieve fast convergence estimation of the load torque.

[0008] The system detects the acceleration start flag and deceleration start flag of the motor, adds the estimated load torque as a feedforward to the input of the current loop controller within a first predetermined time after detecting the acceleration start flag, and removes the feedforward from the input of the current loop controller within a second predetermined time after detecting the deceleration start flag.

[0009] Specifically, the coordinate transformation includes converting the three-phase current into a two-phase stationary coordinate system current through Clark transformation, and then converting the two-phase stationary coordinate system current into the direct-axis current and quadrature-axis current through Park transformation.

[0010] Specifically, when the current loop controller generates a voltage control signal based on the direct-axis current and the quadrature-axis current, the voltage control signal includes a direct-axis voltage control signal U. d and quadrature axis voltage control signal U q The current loop control function is:

[0011] ,

[0012] ,

[0013] in, e d = I d * －I d It is the direct-axis current error. I d * It is the direct-axis reference current and I d * =0, I d It is a direct-axis current. e q = I q * －I q It is the quadrature-axis current error. I q * It is the quadrature axis reference current. I q It is a quadrature-axis current. K pI It is a proportional parameter, and the proportional parameter is the product of the motor's equivalent inductance and the current loop bandwidth. K iI It is an integral parameter, and the integral parameter is the product of the motor phase resistance and the current loop bandwidth.

[0014] Specifically, when the speed loop controller generates the quadrature-axis current reference signal based on the angular velocity, the control function is:

[0015] ,

[0016] in, It is the direct-axis current error. It is the target angular velocity. It is the angular velocity being collected. K pV It is a proportional parameter. K iV It is an integral parameter, which is related to the system's moment of inertia, torque coefficient, and damping factor.

[0017] Specifically, when constructing the load torque observer based on the angular velocity and the quadrature-axis current, the load torque observer is constructed based on the Gopinath principle, and the velocity loop state equation of the load torque observer is:

[0018] ,

[0019] in, It is angular acceleration. It is the angular velocity being collected. J It is the system's rotational inertia. T e It is electromagnetic torque. B It is the coefficient of friction. It is the friction torque, and the load torque estimated by the load torque observer is:

[0020] ,

[0021] in, The load torque estimated by the load torque observer. For observer coefficients, As an intermediate variable, , The observed angular velocity.

[0022] Specifically, the observer coefficients The value range is -2. J / T Between 0 and 0.

[0023] Specifically, the detection of the acceleration start flag and the deceleration start flag is based on the motor speed change rate or the deviation between the given speed and the actual speed.

[0024] Specifically, both the first and second predetermined times are 500 milliseconds.

[0025] Specifically, the feedforward quantity is applied after the motor starts running and is removed after the motor stops.

[0026] Specifically, the closed-loop transfer function of the current loop controller is G(s) = 1 / (1 + (L / Kp)S), where L is the equivalent inductance of the motor, Kp is the proportional control parameter of the current loop, and s is the complex frequency variable; the open-loop transfer function of the speed loop controller is:

[0027] ,

[0028] in K m For system gain, K t The torque coefficient, J Where W is the moment of inertia and Wn is the current closed-loop bandwidth.

[0029] This application has the following technical advantages:

[0030] By employing a strategy that combines a simplified load torque observer with adaptive feedforward control, the limitations of traditional fixed-parameter control in the dynamic operation of a robotic arm are effectively overcome. This allows the system to maintain constant current and speed loop controller parameters even when the load and moment of inertia change. This not only significantly reduces algorithm complexity and the computational power requirements of the controller, but also achieves smooth suppression of speed fluctuations through optimized feedforward timing control. Thus, while ensuring high dynamic response, the robustness and operational stability of the system are improved. Attached Figure Description

[0031] The above and other objects, features, and advantages of exemplary embodiments of this application will become readily understood by reading the following detailed description with reference to the accompanying drawings. Several embodiments of this application are illustrated in the drawings by way of example and not limitation, and the same or corresponding reference numerals denote the same or corresponding parts.

[0032] Figure 1 This is a schematic diagram of a robotic arm speed control method based on load torque feedforward in an embodiment of this application;

[0033] Figure 2 This is a structural diagram of a system used to implement a robotic arm speed control method based on load torque feedforward in an embodiment of this application;

[0034] Figure 3 These are the test results under experimental condition 1;

[0035] Figure 4 These are the test results under experimental condition 2;

[0036] Figure 5 These are the test results under test condition 3;

[0037] Figure 6 These are the test results under experimental condition 4. Detailed Implementation

[0038] The technical solutions in the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings.

[0039] In the field of industrial robot control, achieving high-speed and high-precision operation of robotic arms has always been a core challenge. The root of this challenge lies in the inherent dynamic characteristics of the robotic arm itself. When its multi-joint structure undergoes complex movements in space, the equivalent moment of inertia and load torque of each joint are not constant but fluctuate continuously with the real-time changes in the robotic arm's posture. These time-varying dynamic parameters make the entire system a strongly coupled, nonlinear, multi-inertia system. Faced with such a complex controlled object, traditional control strategies are caught in a dilemma. If a fixed-parameter PID controller is used, its parameters are usually tuned based on a specific operating condition or average load. Once the robotic arm's posture changes, causing the actual inertia and load to deviate from the tuned operating condition, the control performance will deteriorate sharply, manifesting as speed overshoot, steady-state fluctuations, or response lag. If attempts are made to adjust the controller parameters in real time through online tuning or adaptive algorithms, problems such as algorithm complexity, large computational load, difficulty in guaranteeing real-time performance, and difficulty in system stability analysis arise. Especially on embedded microcontrollers with limited computing resources, implementing complex parameter adaptive algorithms is often not worthwhile. On the other hand, to suppress load disturbances, feedforward control can be introduced, but its effectiveness is highly dependent on the accuracy and speed of load torque observation. Existing load torque observers, such as those based on Luneburg or sliding mode theory, are theoretically feasible, but are usually complex in structure, containing multiple observer gains that require precise tuning. This not only relies heavily on the engineer's experience but also consumes a significant amount of the controller's computational resources. Even more challenging is the improper handling of the connection between the observer output and the system's feedforward channel, especially during the transient processes of system acceleration and deceleration. A coarse feedforward introduction strategy can easily cause current surges and speed jitter, thus compromising system stability. Therefore, how to design a load disturbance suppression scheme that is simple in structure, easy to tune parameters, has a low computational burden, and can smoothly integrate feedforward compensation while ensuring control performance has become a long-standing and unresolved technical bottleneck in this field.

[0040] Based on the above analysis, the inventors of this application have designed as follows: Figure 1 The illustrated method for controlling the speed of a robotic arm based on load torque feedforward includes the following steps:

[0041] The angular velocity and three-phase current of the motor are collected, and the coordinate transformation of the three-phase current is performed to obtain the direct-axis current and quadrature-axis current;

[0042] A voltage control signal is generated based on the direct-axis current and quadrature-axis current using a current loop controller. The proportional-integral parameters of the current loop controller are preset based on the motor inductance and resistance and remain unchanged during operation.

[0043] A quadrature-axis current reference signal is generated based on angular velocity using a speed loop controller, wherein the proportional-integral parameters of the speed loop controller are preset based on the motor's moment of inertia and torque coefficient and remain unchanged during operation;

[0044] A load torque observer is constructed based on angular velocity and quadrature axis current to estimate load torque in real time. The load torque observer uses only one adjustable observer coefficient, which is selected in the negative range according to the system's moment of inertia and discrete sampling period to achieve fast convergence estimation of load torque.

[0045] The system detects the acceleration start flag and deceleration start flag of the motor, adds the estimated load torque as a feedforward to the input of the current loop controller within a first predetermined time after detecting the acceleration start flag, and removes the feedforward from the input of the current loop controller within a second predetermined time after detecting the deceleration start flag.

[0046] In this embodiment, the electrical parameters of the permanent magnet synchronous motor are shown in the table below:

[0047]

[0048] In this embodiment, the system implementation block diagram is as follows: Figure 2 As shown, the coordinate transformation includes converting the three-phase current into a two-phase stationary coordinate system current through Clark transformation, and then converting the two-phase stationary coordinate system current into direct-axis current and quadrature-axis current through Park transformation.

[0049] In this embodiment, the goal of the current loop controller is to enable the direct-axis and quadrature-axis currents to quickly and accurately track their given values. A key design feature of this invention is that the proportional-integral (PI) parameters of the current loop are preset at the factory and remain unchanged throughout the entire operation. Specifically, the proportional parameter is set as the product of the motor's equivalent inductance and the desired current loop bandwidth, while the integral parameter is the product of the motor's phase resistance and the same bandwidth. This design method ensures that the current loop has consistent and predictable dynamic response characteristics, and its closed-loop transfer function exhibits typical first-order inertial characteristics. The bandwidth of this loop is directly determined by the ratio of the proportional parameter to the inductance, meaning that once the bandwidth is selected according to the system response speed requirements, the control parameters are uniquely determined, thus fundamentally avoiding the tedious manual parameter tuning work afterwards. Based on the direct-axis current error and the quadrature-axis current error, the current loop controller calculates the voltage control signals for the direct and quadrature axes respectively through proportional-integral operations. Finally, after space vector pulse width modulation, the signals drive the inverter to generate the required three-phase voltages applied to the motor.

[0050] When the current loop controller generates voltage control signals based on the direct-axis current and quadrature-axis current, the voltage control signals include the direct-axis voltage control signal U. d and quadrature axis voltage control signal U q The current loop control function is:

[0051] ,

[0052] ,

[0053] in, e d = I d * －I d It is the direct-axis current error. I d * It is the direct-axis reference current and I d * =0, I d It is a direct-axis current. e q = I q * －I q It is the quadrature-axis current error. I q * It is the quadrature axis reference current. I q It is a quadrature-axis current. K pI It is a proportional parameter, and the proportional parameter is the product of the motor's equivalent inductance and the current loop bandwidth. K iI It is an integral parameter, and the integral parameter is the product of the motor phase resistance and the current loop bandwidth.

[0054] At the speed loop level, a proportional-integral (PI) controller with fixed parameters is also employed. The setpoint for the speed loop is the desired joint angular velocity, while the feedback value is the previously acquired actual angular velocity. The output of the speed loop controller is a reference signal for the quadrature-axis current, which determines the electromagnetic torque required by the motor. Similar to the current loop, the parameters of the speed loop controller are also preset based on control system theory. The calculation of its proportional and integral parameters is closely related to the system's equivalent moment of inertia, the motor torque coefficient, and the desired speed loop bandwidth and damping characteristics. By treating the speed loop as a typical Type II system, its open-loop transfer function has a specific form, balancing the system's tracking performance and disturbance rejection performance. This model-based design method ensures that even when changes in the robotic arm's posture cause changes in the joint's equivalent inertia, the speed loop maintains relatively stable control performance without requiring online parameter adjustments, significantly improving the system's applicability and ease of use.

[0055] When the speed loop controller generates the quadrature-axis current reference signal based on the angular velocity, the control function is:

[0056] ,

[0057] in, It is the direct-axis current error. It is the target angular velocity. It is the angular velocity being collected. K pV It is a proportional parameter. K iV It is an integral parameter, which is related to the system's moment of inertia, torque coefficient, and damping factor.

[0058] The closed-loop transfer function of the current loop controller is G(s) = 1 / (1 + (L / Kp)S), where L is the equivalent inductance of the motor, Kp is the proportional control parameter of the current loop, and s is the complex frequency variable; the open-loop transfer function of the speed loop controller is:

[0059] ,

[0060] in K m For system gain, K t The torque coefficient, J For rotational inertia, ω n This is the current closed-loop bandwidth.

[0061] To address the dynamic load variations of a robotic arm, this invention proposes a real-time observation and feedforward compensation mechanism for load torque. First, a load torque observer based on the Gopinath principle is constructed. The observer's unique advantage lies in its extremely simplified structure, relying solely on an adjustable observer coefficient. The observer's construction begins with the mechanical motion equations of the motor, which reveal the dynamic balance between angular acceleration, electromagnetic torque, frictional torque, and load torque. The Gopinath observer introduces an intermediate variable and uses the observation error of the angular velocity to drive its update. Finally, through a simple linear combination—the intermediate variable plus the product of the observer coefficient, moment of inertia, and actual angular velocity—the observed load torque value is output in real time. The observer coefficient is limited to a negative range determined by the system's moment of inertia and the control sampling period. This range ensures the observer's stability and fast convergence. For multi-joint robotic arms, where the equivalent inertia is difficult to know precisely, in practice, this coefficient can be selected as five to ten times the critical value, achieving a good balance between system stability and observation speed. This single-parameter observer structure greatly simplifies engineering design and reduces the computational requirements of the microcontroller compared to traditional observers that require tuning multiple gains.

[0062] The speed loop state equation of the load torque observer is:

[0063] ,

[0064] in, It is angular acceleration. It is the angular velocity being collected. J It is the system's rotational inertia. T e It is electromagnetic torque. B It is the coefficient of friction. It is the friction torque. The load torque estimated by the load torque observer is:

[0065] ,

[0066] in, The load torque estimated by the load torque observer. For observer coefficients, As an intermediate variable, , The observed angular velocity. Observer coefficients. The value range is -2. J / T Between 0 and 0 =(5-10) 0, 0 = -2 J / T).

[0067] In addition, the acceleration and deceleration start indicators of the robotic arm are detected by real-time judgment of the trend of angular velocity changes or the deviation between the given value and the actual value. Once the acceleration start indicator is detected, the observed load torque value is gradually added as a feedforward to the input of the current loop controller over the next 500 milliseconds. Correspondingly, when the deceleration start indicator is detected, the feedforward is gradually removed over the subsequent predetermined time. This method of using a time window for smooth transition in critical transient processes effectively avoids current surges and torque fluctuations caused by sudden changes in the feedforward, making the start-up and stopping process of the robotic arm smoother. During the constant speed operation phase, the feedforward continues to act to compensate for the steady-state load and improve speed stability.

[0068] The effectiveness of this invention is demonstrated below through actual operation experiments of the joint module. The PI parameters and load torque observer parameters involved in the experiment are as follows (PWM carrier frequency 16KHz):

[0069]

[0070] Experimental test bench: rotates under load with a 0.5m swing arm.

[0071] Experimental Condition 1: Under no-load conditions, load torque is directly applied / removed, with 0%, 50%, and 80% load torque values ​​applied respectively, and the given speed is 20Hz;

[0072] Experimental results: such as Figure 3 As shown ( Figure 3 The upper half represents the feedback speed, and the lower half represents the identified load torque.

[0073] Experimental Condition 2: Under 80% load conditions, load torque is directly applied / removed, with 0%, 50%, and 80% load torque values ​​applied respectively, and the given speed is 20Hz;

[0074] Experimental results: such as Figure 4 As shown ( Figure 4 The upper half represents the feedback speed, and the lower half represents the identified load torque.

[0075] Experimental Condition 3: Under 80% load conditions, load torque is directly applied / removed, 80% load torque feedforward value is applied, and the given speed is 20Hz;

[0076] Experimental results: such as Figure 5 As shown ( Figure 5 The upper half represents the feedback speed, and the lower half represents the identified load torque.

[0077] Experimental condition 4: Under 80% load conditions, load torque feedforward is added / removed within 500ms during acceleration and deceleration. The 80% load torque feedforward value is added, and the given speed is 20Hz.

[0078] Experimental results: such as Figure 6 As shown ( Figure 6 The upper half represents the feedback speed, and the lower half represents the identified load torque.

[0079] from Figures 3 to 6 It can be seen that the load torque feedback effect is not significant under no-load / light-load conditions. At 80% rated load, the larger the applied load torque feedback value, the smaller the speed deviation, effectively improving speed jitter and enhancing system stability. The application of slowly adding / removing the load torque feedback value over 500ms during acceleration and deceleration effectively improves torque fluctuations during startup and shutdown, enhancing the system's anti-disturbance capability.

[0080] Obviously, the embodiments described above are only some, not all, of the embodiments of this application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.

[0081] It should be understood that when the terms "first," "second," etc., are used in the claims, description, and drawings of this application, they are only used to distinguish different objects and not to describe a specific order. The terms "comprising" and "including" used in the description and claims of this application indicate the presence of the described features, integrals, steps, operations, elements, and / or components, but do not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components, and / or collections thereof.

Claims

1. A robot arm velocity control method based on load torque feedforward, characterized by, The method comprises the following steps: collecting angular velocity and three-phase current of the motor, and performing coordinate transformation on the three-phase current to obtain direct-axis current and quadrature-axis current; generating a voltage control signal based on the direct-axis current and the quadrature-axis current by using a current loop controller, wherein proportional integral parameters of the current loop controller are preset based on motor inductance and resistance and remain unchanged during operation; generating a quadrature-axis current reference signal based on the angular velocity by using a speed loop controller, wherein proportional integral parameters of the speed loop controller are preset based on motor moment of inertia and torque coefficient and remain unchanged during operation; constructing a load torque observer based on the angular velocity and the quadrature-axis current to estimate load torque in real time, wherein the load torque observer only uses one adjustable observer coefficient, and the observer coefficient is selected in a negative value range according to system moment of inertia and discrete sampling period to quickly converge the estimation of the load torque; detecting an acceleration start flag and a deceleration start flag of the motor, adding the estimated load torque as a feedforward quantity into an input of the current loop controller within a first predetermined time after detecting the acceleration start flag, and removing the feedforward quantity from the input of the current loop controller within a second predetermined time after detecting the deceleration start flag.

2. The method of claim 1, wherein, The coordinate transformation comprises converting the three-phase current into two-phase static coordinate system current by Clark transformation, and converting the two-phase static coordinate system current into the direct-axis current and the quadrature-axis current by Park transformation.

3. The method of claim 1, wherein, The voltage control signals include a direct-axis voltage control signal U d and a quadrature-axis voltage control signal U q , and the current loop control function is ， ， wherein e d = I d * －I d is a direct axis current error, I d * is a direct axis reference current and I d * = 0, I d is a direct axis current, e q = I q * － I q is a quadrature axis current error, I q * is a quadrature axis reference current, I q is a quadrature axis current, K pI is a proportional parameter and the proportional parameter is a product of a machine equivalent inductance and a current loop bandwidth, K iI is an integral parameter and the integral parameter is a product of a machine phase resistance and the current loop bandwidth.

4. The method of claim 1, wherein, When the speed loop controller generates the quadrature-axis current reference signal based on the angular velocity, the control function is: ， wherein is the direct axis current error, is the target angular velocity, is the acquired angular velocity, K pV is a proportional parameter, K iV is an integral parameter, related to the moment of inertia, torque coefficient and damping factor of the system.

5. The method of claim 1, wherein, When the load torque observer is constructed based on the angular velocity and the quadrature-axis current, the load torque observer is constructed based on Gopinath principle, and a speed loop state equation of the load torque observer is: ， wherein is the angular acceleration, is the angular velocity of the acquisition, J is the system moment of inertia, T e is the electromagnetic torque, B is the friction coefficient, is the friction torque, the load torque observer estimating a load torque of: ， wherein L load torque estimated by the load torque observer, observer coefficient, intermediate variable, , observed angular velocity.

6. The method of claim 5, wherein, The observer coefficients have a value range between -2 J / T and 0.

7. The method of claim 1, wherein, The detection of the acceleration start flag and the deceleration start flag is based on motor speed change rate or deviation between given speed and actual speed.

8. The method of claim 1, wherein, The first predetermined time and the second predetermined time are both 500 milliseconds.

9. The method of claim 1, wherein, The feedforward quantity is loaded after the motor starts running and is removed after the motor stops running.

10. The method of claim 3 or 4, wherein, The closed loop transfer function of the current loop controller is ; L is equivalent inductance of the motor, Kp is a current loop proportional control parameter, and s is a complex frequency variable; The open-loop transfer function of the speed loop controller is: ， wherein K m =Kt / J G is the system gain, K t K is the torque coefficient, J J is the moment of inertia, Wn is the current closed loop bandwidth, K pV Kp is a proportional parameter, K iV Ki is an integral parameter.

Images