Motor servo control method and device

Through the calculation of the three-ring control system and the speed ring introduced by the acceleration, the dynamic performance unstable of the servo system when the load changes is solved, and the precise control and stable operation of the motor is achieved. It is suitable for medical surgical robots and other applications.

CN119030396AActive Publication Date: 2024-11-26HARBIN SIZHERUI INTELLIGENT MEDICAL EQUIP CO LTD

Patent Information

Application Number
CN202411202390.0
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-29
Publication Date
2024-11-26
Estimated Expiration
2044-08-29

AI Technical Summary

Technical Problem

The existing servo systems have caused unstable dynamic performance of motor operation under severe load changes, especially in medical surgical robots.

Method used

The three-ring control system is adopted, including the position ring, the speed ring and the current ring. By receiving the command position signal and the motor feedback signal, the error value is calculated and the command current value is output, ensuring the motor is stable under various working conditions, and the acceleration is introduced. Speed ​​loop calculation to improve response speed.

Benefits of technology

Accurate control of the motor torque, speed and position is achieved, ensuring the motor runs stably under various operating conditions and accurately executes control instructions, improving dynamic performance and system stability.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119030396A_ABST
    Figure CN119030396A_ABST
Patent Text Reader

Abstract

The invention discloses a motor servo control method and device. The motor servo control method comprises the following steps: receiving an instruction position signal and a position signal fed back by a motor, obtaining a position loop error value according to the instruction position signal and the position signal fed back by the motor, and obtaining an instruction speed value according to the position loop error value; obtaining a speed value and an acceleration value fed back by the motor, obtaining a speed error value according to the instruction speed value and the speed value fed back by the motor, obtaining a speed loop calculation error value according to the speed error value and the acceleration value, and inputting the speed loop calculation error value into a speed loop to obtain an instruction current value; obtaining a current signal fed back by the motor, obtaining a current loop error value according to the current signal fed back by the motor and the instruction current value, and obtaining a current loop output value according to the current loop error value; and converting the current loop output value into a driving voltage to drive the motor to operate. The acceleration is introduced in the calculation of the speed ring, so that the dynamic performance of the motor is more stable.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The embodiments of the present invention relate to the field of motor control technology, and in particular to a motor servo control method and device. Background Art

[0002] Servo drive system (Servo System), referred to as servo system, is an automatic control system that uses mechanical position or angle as the control object. It can achieve precise control of the motion system. The control algorithm is the core part of the motion controller, which determines the performance and stability of the system and enables the robot to achieve automatic control.

[0003] In the current field of medical technology, the final execution component of the medical surgical robot is the instrument. The system adopts a master-slave follower mode, using the instrument to reproduce the doctor's operating actions and complete the patient's surgery. When the servo system in the prior art controls the motor, the input signal of the speed loop in the control program is the command speed and feedback speed, and the output is the current command. This method is prone to instability when the load changes drastically. If the servo system in the prior art is applied to the medical surgical robot, it will cause the dynamic performance of the motor in the instrument to be unstable. Summary of the invention

[0004] The present invention provides a motor servo control method and device to achieve more stable dynamic performance of the motor.

[0005] In a first aspect, an embodiment of the present invention provides a motor servo control method, which uses a three-loop control system to drive the motor to operate, wherein the three-loop control system includes a speed loop, and the method includes:

[0006] Receiving a command position signal and a position signal fed back by the motor, obtaining a position loop error value according to the command position signal and the position signal fed back by the motor, and obtaining a command speed value according to the position loop error value;

[0007] Obtaining the speed value and acceleration value fed back by the motor, obtaining a speed error value according to the command speed value and the speed value fed back by the motor, obtaining a speed loop calculation error value according to the speed error value and the acceleration value, and inputting the speed loop calculation error value into the speed loop to obtain a command current value;

[0008] Acquire the current signal fed back by the motor, obtain a current loop error value according to the current signal fed back by the motor and the command current value, and obtain a current loop output value according to the current loop error value;

[0009] The current loop output value is converted into a driving voltage, and the driving voltage is output to the motor to drive the motor to operate.

[0010] In a second aspect, an embodiment of the present invention further provides a motor servo control device, the motor servo control device is used to execute the motor servo control method described in any embodiment of the present invention, the motor servo control device comprises:

[0011] A position loop module, used for receiving a command position signal and a position signal fed back by the motor, obtaining a position loop error value according to the command position signal and the position signal fed back by the motor, and obtaining a command speed value according to the position loop error value;

[0012] A speed loop module, used for obtaining the speed value and acceleration value fed back by the motor, obtaining a speed error value according to the command speed value and the speed value fed back by the motor, obtaining a speed loop calculation error value according to the speed error value and the acceleration value, and inputting the speed loop calculation error value into the speed loop to obtain a command current value;

[0013] A current loop module, used for acquiring the current signal fed back by the motor, obtaining a current loop error value according to the current signal fed back by the motor and the command current value, and obtaining a current loop output value according to the current loop error value;

[0014] The driving module is used to convert the current loop output value into a driving voltage, and output the driving voltage to the motor to drive the motor to operate.

[0015] The present invention provides a motor servo control method and device. The motor servo control method adopts a three-loop control system to drive the motor to run, receives a command position signal and a position signal fed back by the motor, obtains a position loop error value according to the command position signal and the position signal fed back by the motor, and obtains a command speed value according to the position loop error value; introduces acceleration in the calculation of the speed loop, obtains a speed value and an acceleration value fed back by the motor, obtains a speed error value according to the command speed value and the speed value fed back by the motor, obtains a speed loop calculation error value according to the speed error value and the acceleration value, inputs the speed loop calculation error value into the speed loop to obtain a command current value, so that the loop response speed is faster and the dynamic performance is more stable; obtains a current loop error value according to the current signal fed back by the motor and the command current value, and obtains a current loop output value according to the current loop error value; converts the current loop output value into a driving voltage, and outputs the driving voltage to the motor, so as to realize accurate control of the motor torque, speed and position, and ensure that the motor can stably operate and accurately execute control instructions under various working conditions, and the present invention introduces acceleration in the calculation of the speed loop to make the dynamic performance of the motor more stable. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 A flow chart of a motor servo control method provided by an embodiment of the present invention.

[0017] Figure 2 A schematic diagram of the principle of the PID adjustment process provided by an embodiment of the present invention.

[0018] Figure 3 Flow chart of another motor servo control method provided by an embodiment of the present invention

[0019] Figure 4 A schematic structural diagram of a motor servo control device provided by an embodiment of the present invention.

[0020] Figure 5 A schematic diagram of the structure of a medical surgical robot provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0021] The present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It is to be understood that the specific embodiments described herein are only used to explain the present invention, rather than to limit the present invention. It should also be noted that, for ease of description, only parts related to the present invention, rather than all structures, are shown in the accompanying drawings.

[0022] Figure 1 Flow chart of a motor servo control method provided by an embodiment of the present invention. The motor servo control method of this embodiment adopts a three-loop control system to drive the motor to operate. The three-loop control system includes a speed loop, such as Figure 1 As shown, the motor servo control method specifically includes the following steps:

[0023] S110, receiving a command position signal and a position signal fed back by the motor, obtaining a position loop error value according to the command position signal and the position signal fed back by the motor, and obtaining a command speed value according to the position loop error value.

[0024] Among them, the three-loop control system includes the current loop, the speed loop and the position loop. The current loop, the speed loop and the position loop are the three-loop control systems of the motion servo, from the inside to the outside. The main goal of the current loop is to control the torque, which will affect the speed and thus the position. The speed loop is located in the outer layer of the current loop, and performs negative feedback PID adjustment by detecting the signal of the motor encoder. The PID output in the loop is directly the set value of the current loop. The speed loop ensures that the motor runs stably at a given speed and can respond quickly to speed changes. The position loop is the outermost control loop, which converts the external input pulse or position command into the position control signal of the motor to ensure that the motor reaches the specified position accurately. The three-loop control system realizes precise control of the motor torque, speed and position through the synergistic effect of the current loop, the speed loop and the position loop, ensuring that the motor can run stably and accurately execute control instructions under various working conditions.

[0025] Specifically, the command position signal is received by the command receiving module of the motor, and the command position signal is obtained after parsing the control command. After receiving the enable command of the host computer, the position loop is enabled. The position signal fed back by the motor is obtained after the encoder processing module connected to the motor obtains the position of the motor, and the encoder processing module sends the position signal fed back by the motor to the position loop. The position loop receives the command position signal and the position signal fed back by the motor, and calculates the position loop error value according to the command position signal and the position signal fed back by the motor. The position loop error value is PID-adjusted in the position loop to obtain the command speed value, and the command speed value is output to the speed loop.

[0026] S120, obtaining the speed value and acceleration value fed back by the motor, obtaining a speed error value according to the command speed value and the speed value fed back by the motor, obtaining a speed loop calculation error value according to the speed error value and the acceleration value, and inputting the speed loop calculation error value into the speed loop to obtain a command current value.

[0027] The encoder processing module can obtain the speed value and acceleration value fed back by the motor, and send the speed value and acceleration value fed back by the motor to the speed loop.

[0028] Specifically, after the speed loop receives the command speed value, the speed value fed back by the motor, and the acceleration value, the speed error value is calculated based on the command speed value and the speed value fed back by the motor. The speed error value is obtained by subtracting the speed value fed back by the motor from the command speed value. The speed loop calculated error value is calculated based on the speed error value and the acceleration value. The speed loop calculated error value is output as the command current value after PID adjustment in the speed loop, and the command current value is used as the given value of the current loop. The characteristic of this method is that in the control process of the speed loop, since the speed output of the three-loop control system is current, and the current corresponds to acceleration, the acceleration of the motor feedback is introduced in the speed loop control process, so that the response speed of the loop is faster and the dynamic performance is more stable.

[0029] S130, obtaining a current signal fed back by the motor, obtaining a current loop error value according to the current signal fed back by the motor and a command current value, and obtaining a current loop output value according to the current loop error value.

[0030] Specifically, the motor feedback current signal is obtained from the motor phase line by the current signal processing module, and the current signal processing module sends the obtained motor feedback current signal to the current loop. After receiving the motor feedback current signal and the command current value, the current loop compares and calculates the current loop error value based on the motor feedback current signal and the command current value. The current loop error value is output to the motor after PID adjustment in the current loop to control the phase current of each phase of the motor and realize the control of the motor torque.

[0031] in addition, Figure 2The schematic diagram of the principle of the PID adjustment process provided by the embodiment of the present invention, the PID adjustment process in the current loop, speed loop and position loop can include a proportional link, an integral link and a differential link. Figure 2 As shown, after receiving the set value and the feedback signal, the current loop, speed loop and position loop perform difference calculation to obtain the error value, and then perform calculations in the proportional link, integral link and differential link, and then add the obtained value as the output signal, which is output to the next link. The feedback signal is usually obtained by a sensor. In the embodiment of the present invention, the feedback signal is obtained by an encoder processing module. Through the PID adjustment and synergy of the current loop, speed loop and position loop, the motor torque, speed and position are accurately controlled to ensure that the motor can operate stably and accurately execute control instructions under various working conditions.

[0032] S140, converting the current loop output value into a driving voltage, and outputting the driving voltage to the motor to drive the motor to operate.

[0033] Specifically, the current loop outputs the current loop output value to the drive module, and the drive module converts the current loop output value into a drive voltage, and outputs the drive voltage to the motor to drive the motor to operate. The characteristic of this method is that in the control process of the speed loop, the output of the speed is current, and the current corresponds to acceleration, so the feedback acceleration is introduced in the speed loop control method, so that the loop responds faster and the dynamic performance is more stable.

[0034] The embodiment of the present invention provides a motor servo control method. The motor servo control method adopts a three-loop control system to drive the motor to run, receives a command position signal and a position signal fed back by the motor, obtains a position loop error value according to the command position signal and the position signal fed back by the motor, and obtains a command speed value according to the position loop error value; introduces acceleration in the calculation of the speed loop, obtains a speed value and an acceleration value fed back by the motor, obtains a speed error value according to the command speed value and the speed value fed back by the motor, obtains a speed loop calculation error value according to the speed error value and the acceleration value, and inputs the speed loop calculation error value into the speed loop to obtain a command current value, so that the loop responds faster and has more stable dynamic performance; obtains a current loop error value according to the current signal fed back by the motor and the command current value, and obtains a current loop output value according to the current loop error value; converts the current loop output value into a driving voltage, and outputs the driving voltage to the motor, so as to realize precise control of the motor torque, speed and position, and ensure that the motor can stably operate and accurately execute control instructions under various working conditions, and the present invention introduces acceleration in the calculation of the speed loop to make the dynamic performance of the motor more stable.

[0035] Figure 3 A flowchart of another motor servo control method provided by an embodiment of the present invention is shown in FIG. Figure 3As shown, the motor servo control method specifically includes the following steps:

[0036] S210, receiving a command position signal and a position signal fed back by a motor, obtaining a position loop error value by subtracting the command position signal from the position signal fed back by the motor; inputting the position loop error value into the position loop, and obtaining a command speed value through a first transfer function operation in the position loop.

[0037] Specifically, after receiving the command position signal and the position signal fed back by the motor, the position loop obtains the position loop error value by subtracting the command position signal from the position signal fed back by the motor. The position loop error value is input into the position loop, and the command speed value is obtained by the first transfer function operation in the position loop.

[0038] The first transfer function is:

[0039]

[0040] Among them, e 1 is the position loop error value, u1 is the command speed value, t is the time, K p1 is the position loop proportional coefficient, T t1 is the position loop integral coefficient, T d1 is the differential coefficient of the position loop. The command speed value can be obtained by performing the proportional link, integral link and differential link through the first transfer function to ensure that the motor reaches the specified position accurately.

[0041] S220, obtaining the speed value and acceleration value fed back by the motor, obtaining a speed error value according to the command speed value and the speed value fed back by the motor, obtaining a speed loop calculation error value according to the speed error value and the acceleration value, and inputting the speed loop calculation error value into the speed loop to obtain a command current value.

[0042] Optionally, obtain the speed and acceleration values ​​fed back by the motor, including:

[0043] Get the number of revolutions of the motor within a fixed time. The speed value fed back by the motor is the ratio of the number of revolutions of the motor within a fixed time to the fixed time.

[0044] The first speed of the motor at the first time is obtained, the second speed of the motor at the second time interval with the first time is obtained, the speed difference between the first speed and the second speed is calculated, and the acceleration value is the ratio of the speed difference to the second time.

[0045] Specifically, the encoder processing module obtains the number of revolutions of the motor within a fixed time and calculates the speed value of the motor feedback. The speed value of the motor feedback is the ratio of the number of revolutions of the motor within a fixed time to the fixed time. The encoder processing module obtains the first speed of the motor at the first time, obtains the second speed of the motor at the second time interval with the first time, calculates the speed difference between the first speed and the second speed, and the acceleration value is the ratio of the speed difference to the second time. Exemplarily, the encoder processing module has two speed calculation methods, one is: the fixed time is T, the encoder count is M within the T time, then the motor speed n = M / T, the other is: the current value change of the encoder processing module is T1 from the last value change, and the motor speed n = 1 / T1. These two calculation methods have their own advantages and disadvantages. Only by comprehensive application can the motor speed value n be more accurately obtained. The obtained motor speed value n is the speed value of the motor feedback. The encoder processing module obtains the acceleration calculation method of the motor feedback as follows: the interval time is T, the motor speed before T is n1, and the motor speed after T is n2, then the motor acceleration a = (n2-n1) / T. After the speed loop obtains the speed error value and the acceleration value, the speed loop calculation error value is calculated, and the speed loop calculation error value is input into the speed loop to obtain the command current value, so as to achieve a rapid response to the speed change of the motor and ensure the stable operation of the motor at a given speed. In addition, the acceleration of the motor feedback is introduced in the speed loop control process, so that the loop responds faster and the dynamic performance is more stable.

[0046] Optionally, a speed error value is obtained according to the command speed value and the speed value fed back by the motor, and a speed loop calculation error value is obtained according to the speed error value and the acceleration value, including: obtaining the speed error value by subtracting the command speed value from the speed value fed back by the motor; and obtaining the speed loop calculation error value by calculation, wherein the speed loop calculation error value satisfies:

[0047] e=V 0 +k*a;

[0048] Among them, e is the speed loop calculation error value, V 0 is the velocity error value, k is the acceleration correction coefficient, and a is the acceleration value. 0 Adding the product of the acceleration value a and the acceleration correction coefficient k, we can get the speed loop calculation error value e.

[0049] Specifically, the speed error value is obtained by subtracting the motor feedback speed value from the command speed value, and the speed loop calculation error value is calculated by the formula of the speed loop calculation error value. In the formula of the speed loop calculation error value, the acceleration correction coefficient k is a fixed value, and the value of the acceleration correction coefficient k is determined through multiple tests to make the calculation of the speed loop calculation error value more accurate.

[0050] Optionally, the speed loop calculated error value is input into the speed loop to obtain the command current value, including:

[0051] The speed loop error value is input into the speed loop, and the command current value is obtained through the second transfer function operation in the speed loop.

[0052] The second transfer function is:

[0053]

[0054] Where, u is the command current value, t is the time, K p is the proportionality coefficient, T t is the integration coefficient, T d is the differential coefficient.

[0055] Specifically, after the speed loop error value is input into the speed loop, the speed loop is PID-adjusted through the second transfer function to calculate the output value of the speed loop, that is, the command current value. The command current value is also the input value of the current loop, ensuring that the motor runs stably at a given speed and can quickly respond to speed changes of the motor.

[0056] In addition, the traditional servo driver receives the control instruction of the host computer and transmits it to the control unit. The control unit calculates the control data output to the motor according to the feedback data of the motor, and converts it into the control current of the actual motor through the drive circuit to realize the closed-loop control of the motor. In the traditional motor speed loop control method, the input signal of the speed loop is the command speed and the feedback speed, and the output is the current command. This method is prone to instability when the load changes drastically. Therefore, the embodiment of the present invention analyzes the speed loop, and the output of the speed loop is current. The current actually corresponds to the acceleration in the system. Therefore, acceleration is introduced in the calculation of the speed loop, and the acceleration value obtained by calculation corresponds to the current output of the system, so that the loop responds faster and the dynamic performance is more stable.

[0057] S230, obtaining the current signal fed back by the motor, subtracting the current signal fed back by the motor from the command current value to obtain a current loop error value; inputting the current loop error value into the current loop, and obtaining a current loop output value through a third transfer function operation in the current loop; wherein the current loop output value is a PWM opening signal.

[0058] Specifically, the current loop obtains the current signal fed back by the motor through the current signal processing module, and obtains the current loop error value by subtracting the command current value from the current signal fed back by the motor. The current loop error value is calculated by the third transfer function in the current loop to obtain the current loop output value, and the third transfer function is:

[0059]

[0060] Among them, e3 is the current loop error value, u3 is the current loop output value, t is the time, K p3 is the current loop proportionality coefficient, T t3 is the current loop integral coefficient, T d3 is the differential coefficient of the current loop. The current loop output value can be obtained by calculating the proportional link, integral link and differential link through the third transfer function, so as to achieve the purpose of making the output current of the current loop as close to the set current as possible and realize the control of the motor torque.

[0061] S240: Convert the PWM opening signal into a driving voltage, and output the driving voltage to the motor to drive the motor to run.

[0062] Specifically, the output value of the current loop is a PWM opening signal. The drive module converts the PWM opening signal into the actual driving voltage of the motor and outputs the driving voltage to the motor, driving the motor to run at a preset torque, speed and position, ensuring that the motor can operate stably and accurately execute control instructions under various working conditions.

[0063] The motor servo control method in the embodiment of the present invention is characterized in that in the control process of the speed loop, the output of the speed is current, and the current corresponds to acceleration, so the feedback acceleration is introduced into the speed loop control method, so that the loop responds faster and the dynamic performance is more stable. By adopting the speed loop introducing acceleration, the dynamic characteristics and static stability of the system in which the motor servo control method is located are better under the condition of variable load.

[0064] It should be noted that the motor servo control method in the embodiment of the present invention can be executed in various programmable CPU and Field Programmable Gate Array (FPGA) components.

[0065] The embodiment of the present invention provides a motor servo control method, adopts three-closed-loop feedback control, and introduces a speed closed-loop control algorithm with feedback acceleration, so as to achieve precise control of the motor torque, speed and position, and ensure that the motor can operate stably and accurately execute control instructions under various working conditions. In addition, the present invention introduces acceleration in the calculation of the speed loop, so that the response speed of the speed loop is faster and the dynamic performance of the motor is more stable.

[0066] Figure 4 Schematic diagram of the structure of a motor servo control device provided by an embodiment of the present invention. The motor servo control device of this embodiment is used to execute the motor servo control method of any of the above embodiments, such as Figure 4 As shown, the motor servo control device includes:

[0067] The position loop module 110 is used to receive the command position signal and the position signal fed back by the motor, obtain a position loop error value according to the command position signal and the position signal fed back by the motor, and obtain a command speed value according to the position loop error value.

[0068] The speed loop module 120 is used to obtain the speed value and acceleration value fed back by the motor, obtain the speed error value according to the command speed value and the speed value fed back by the motor, obtain the speed loop calculated error value according to the speed error value and the acceleration value, and input the speed loop calculated error value into the speed loop to obtain the command current value.

[0069] The current loop module 130 is used to obtain the current signal fed back by the motor, obtain the current loop error value according to the current signal fed back by the motor and the command current value, and obtain the current loop output value according to the current loop error value.

[0070] The driving module 140 is used to convert the current loop output value into a driving voltage, and output the driving voltage to the motor to drive the motor to operate.

[0071] Specifically, the position loop module 110 receives the command position signal and the position signal fed back by the motor, and calculates the position loop error value according to the command position signal and the position signal fed back by the motor. The position loop error value is PID-regulated in the position loop to obtain the command speed value, and the speed loop outputs the command speed value. After receiving the command speed value, the speed value fed back by the motor and the acceleration value, the speed loop module 120 calculates the speed error value according to the command speed value and the speed value fed back by the motor, and the speed error value is obtained by subtracting the speed value fed back by the motor from the command speed value. The speed loop calculation error value is calculated according to the speed error value and the acceleration value, and the speed loop calculation error value is PID-regulated in the speed loop to output the command current value, and the command current value is used as the given value of the current loop. After receiving the current signal fed back by the motor and the command current value, the current loop module 130 compares and calculates the current loop error value according to the current signal fed back by the motor and the command current value, and the current loop error value is PID-regulated in the current loop and output to the motor, and controls the current of each phase of the motor to achieve the control of the torque of the motor. The driving module 140 converts the current loop output value into a driving voltage, and outputs the driving voltage to the motor to drive the motor to operate. The characteristic of this method is that in the control process of the speed loop, the output of the speed is current, and the current corresponds to acceleration, so the feedback acceleration is introduced into the speed loop control method, so that the loop responds faster and the dynamic performance is more stable.

[0072] The embodiment of the present invention provides a motor servo control device. The motor servo control device receives a command position signal and a position signal fed back by a motor through a position loop module, obtains a position loop error value according to the command position signal and the position signal fed back by the motor, and obtains a command speed value according to the position loop error value; introduces acceleration into the calculation of the speed loop through the speed loop module, obtains a speed value and an acceleration value fed back by the motor, obtains a speed error value according to the command speed value and the speed value fed back by the motor, obtains a speed loop calculation error value according to the speed error value and the acceleration value, and inputs the speed loop calculation error value into the speed loop to obtain a command current value, so that the loop responds faster and has more stable dynamic performance; obtains a current signal fed back by the motor through a current loop module, obtains a current loop error value according to the current signal fed back by the motor and the command current value, and obtains a current loop output value according to the current loop error value; the drive module converts the current loop output value into a drive voltage, and outputs the drive voltage to the motor, so as to realize precise control of the motor torque, speed and position, and ensure that the motor can stably operate and accurately execute control instructions under various working conditions, and the present invention introduces acceleration into the calculation of the speed loop to make the dynamic performance of the motor more stable.

[0073] For further information, please refer to Figure 4 The position loop module 110 includes a first differential module 111 and a position loop 112 ; the speed loop module 120 includes a second differential module 121 and a speed loop 122 ; and the current loop module 130 includes a third differential module 131 and a current loop 132 .

[0074] The first difference module 111 is used to subtract the command position signal from the position signal fed back by the motor to obtain a position loop error value. The position loop 112 is used to substitute the position loop error value into the first transfer function to obtain a command speed value.

[0075] The second difference module 121 is used to obtain a speed error value by subtracting the command speed value from the speed value fed back by the motor. The speed loop calculation error value is calculated, and the speed loop calculation error value satisfies:

[0076] e=V 0 +k*a;

[0077] Among them, e is the speed loop calculation error value, V 0 is the velocity error value, k is the acceleration correction coefficient, and a is the acceleration value.

[0078] The speed loop 122 is used to substitute the speed loop calculated error value into the second transfer function to obtain the command current value.

[0079] The third difference module 131 is used to subtract the motor feedback current signal from the command current value to obtain a current loop error value. The current loop 132 is used to substitute the current loop error value into the third transfer function to obtain a current loop output value.

[0080] Specifically, after the first difference module 111 subtracts the command position signal from the position signal fed back by the motor to obtain the position loop error value, the position loop 112 substitutes the position loop error value into the first transfer function to obtain the command speed value. The first transfer function is:

[0081]

[0082] Among them, e 1 is the position loop error value, u1 is the command speed value, t is the time, K p1 is the position loop proportional coefficient, T t1 is the position loop integral coefficient, T d1 is the differential coefficient of the position loop. The command speed value can be obtained by performing the proportional link, integral link and differential link through the first transfer function to ensure that the motor reaches the specified position accurately.

[0083] The second difference module 121 obtains a speed error value by subtracting the speed value fed back by the motor from the command speed value, and obtains a speed loop calculation error value by calculation. The speed loop calculation error value satisfies:

[0084] e=V 0 +k*a;

[0085] Among them, e is the speed loop calculation error value, V 0 is the velocity error value, k is the acceleration correction coefficient, and a is the acceleration value. The acceleration correction coefficient k is a fixed value, and the value of the acceleration correction coefficient k is determined through multiple tests to make the calculation of the velocity loop calculation error value more accurate.

[0086] After the speed loop 122 inputs the speed loop error value into the speed loop, the speed loop performs PID adjustment through the second transfer function to calculate the output value of the speed loop, that is, the command current value, which is also the input value of the current loop, to ensure that the motor runs stably at a given speed and can quickly respond to the speed change of the motor. The second transfer function is:

[0087]

[0088] Where, u is the command current value, t is the time, K p is the proportionality coefficient, T t is the integration coefficient, T d is the differential coefficient.

[0089] After the third difference module 131 obtains the current loop error value by subtracting the command current value from the current signal fed back by the motor, the current loop 132 substitutes the current loop error value into the third transfer function to obtain the current loop output value. The third transfer function is:

[0090]

[0091] Among them, e 3 is the current loop error value, u3 is the current loop output value, t is the time, K p3 is the current loop proportionality coefficient, T t3 is the current loop integral coefficient, T d3 is the differential coefficient of the current loop. The current loop output value can be obtained by calculating the proportional link, integral link and differential link through the third transfer function, so as to achieve the purpose of making the output current of the current loop as close to the set current as possible and realize the control of the motor torque.

[0092] For further information, please refer to Figure 4 , the motor servo control device further includes an encoder signal processing module 160 and a current signal processing module 150;

[0093] The input end of the encoder signal processing module 160 is connected to the motor, the first output end of the encoder signal processing module 160 is connected to the first input end of the first differential module 111, the second output end of the encoder signal processing module 160 is connected to the first input end of the second differential module 121, and the second output end of the encoder signal processing module 160 is connected to the second input end of the second differential module 121. The encoder signal processing module 160 is used to obtain the position signal fed back by the motor, the speed value and the acceleration value fed back by the motor, and send the position signal fed back by the motor to the first input end of the first differential module 111, send the speed value fed back by the motor to the first input end of the second differential module 121, and send the acceleration value to the second input end of the second differential module 121.

[0094] The current signal processing module 150 is connected to the phase line of the motor, and the output end of the current signal processing module 150 is connected to the first input end of the third differential module 131. The current signal processing module 150 is used to obtain the current signal fed back by the motor from the phase line of the motor, and send the current signal fed back by the motor to the first input end of the third differential module 131.

[0095] The encoder signal processing module 160 has the function of real-time monitoring and feedback, and can monitor the motor speed, the relative position of the circumferential direction of the rotating shaft and other key parameters in real time, so as to accurately grasp the running state of the motor and its traction equipment, and form a closed-loop control by converting these monitored parameters into digital pulse signals and feeding them back to the control system. This feedback mechanism enables the control system to adjust the running state of the motor according to real-time data to ensure the accuracy and stability of the motor operation.

[0096] Specifically, the input end of the encoder signal processing module 160 is connected to the motor, and the position signal, speed value and acceleration value fed back by the motor can be obtained, and the position signal fed back by the motor is sent to the first differential module 111, and the speed value and acceleration value fed back by the motor are sent to the second differential module 121, so that the position loop 112, the speed loop 122 and the current loop 132 can achieve closed-loop control. And the acceleration is fed back to the speed loop module 120 through the encoder signal processing module 160, so that the speed loop 122 responds faster and the dynamic performance is more stable.

[0097] Optionally, the motor servo control device in the embodiment of the present invention may be applied to a medical surgical robot to improve the dynamic characteristics and static stability of the medical surgical robot. Figure 5 A schematic diagram of the structure of a medical surgical robot provided by an embodiment of the present invention is shown in FIG. Figure 5 As shown, the medical surgical robot includes a doctor's console, operation posture generation, a communication module, execution posture generation, a motor drive module and posture motor position output.

[0098] Among them, the various modules of the medical surgical robot have the following functions: Doctor console: The surgeon operates on the doctor console and inputs surgical instructions and desired surgical postures. The doctor console is the interface for the surgeon to interact with the robot, allowing the surgeon to accurately control the movements of the surgical robot. Operation posture generation: The instructions received by the doctor console are converted into specific operation posture information. This step involves converting the doctor's operation intentions into specific parameters that the robot can understand and execute, such as position, posture, speed, etc. Communication module: The operation posture information is transmitted at high speed and reliably through the ETHERCAT communication protocol (a real-time industrial Ethernet communication protocol). ETHERCAT communication ensures real-time data exchange between the doctor console and the robot system, so that the surgical operation can respond quickly to the doctor's instructions. Execution posture generation: After receiving the operation posture information, the robot system generates the execution posture internally. This step may involve complex algorithms and calculations to ensure that the robot can move according to the trajectory and posture expected by the doctor. Motor drive module: The execution posture information is passed to the motor drive module, which is responsible for controlling the robot's motor to generate the corresponding movement. The motor drive module is a key part of the robot's execution of actions. It drives the motor to rotate according to the received instructions, thereby driving the robot's joints and end effectors to move. Posture motor position output: Ultimately, the robot's end effector (such as a surgical instrument) reaches the posture expected by the doctor and performs the corresponding surgical operation. This process is achieved through the precise control and feedback mechanism of the motor to ensure the accuracy and safety of the surgical operation. The motor servo control device in the embodiment of the present invention can be applied to the motor drive module in the medical surgical robot to improve the dynamic characteristics and static stability of the medical surgical robot, and realize the precise control and efficient execution of surgical operations.

[0099] The embodiment of the present invention provides a motor servo control device, which can achieve precise control of the motor torque, speed and position through the coordinated control of the three loops of position loop, speed loop and current loop, ensuring that the motor can operate stably and accurately execute control instructions under various working conditions. In addition, the present invention introduces acceleration in the calculation of the speed loop, so that the response speed of the speed loop is faster and the dynamic performance of the motor is more stable, thereby improving the dynamic characteristics and static stability of the system where the motor servo control device is located, and achieving precise control and efficient execution of the system where the motor servo control device is located.

[0100] Note that the above are only preferred embodiments of the present invention and the technical principles used. Those skilled in the art will understand that the present invention is not limited to the specific embodiments herein, and that various obvious changes, readjustments and substitutions can be made by those skilled in the art without departing from the scope of protection of the present invention. Therefore, although the present invention has been described in more detail through the above embodiments, the present invention is not limited to the above embodiments, and may include more other equivalent embodiments without departing from the concept of the present invention, and the scope of the present invention is determined by the scope of the appended claims.

Claims

1. A motor servo control method, It is characterized in that A three-loop control system is used to drive the motor to operate, wherein the three-loop control system includes a speed loop, and the method includes: Receiving a command position signal and a position signal fed back by the motor, obtaining a position loop error value according to the command position signal and the position signal fed back by the motor, and obtaining a command speed value according to the position loop error value; Obtaining the speed value and acceleration value fed back by the motor, obtaining a speed error value according to the command speed value and the speed value fed back by the motor, obtaining a speed loop calculation error value according to the speed error value and the acceleration value, and inputting the speed loop calculation error value into the speed loop to obtain a command current value; Acquire the current signal fed back by the motor, obtain a current loop error value according to the current signal fed back by the motor and the command current value, and obtain a current loop output value according to the current loop error value; The current loop output value is converted into a driving voltage, and the driving voltage is output to the motor to drive the motor to operate.

2. The motor servo control method according to claim 1, It is characterized in that The step of obtaining a speed error value according to the command speed value and the speed value fed back by the motor, and obtaining a speed loop calculation error value according to the speed error value and the acceleration value includes: Subtracting the command speed value from the speed value fed back by the motor to obtain the speed error value; The speed loop calculation error value is obtained by calculation, and the speed loop calculation error value satisfies: e=V 0 +k*a; Where, e is the speed loop calculation error value, V 0 is the velocity error value, k is the acceleration correction coefficient, and a is the acceleration value.

3. The motor servo control method according to claim 2, It is characterized in that The step of inputting the speed loop calculated error value into the speed loop to obtain a command current value comprises: Inputting the speed loop error value into the speed loop, and obtaining the command current value through a second transfer function calculation in the speed loop; The second transfer function is: Wherein, u is the command current value, t is the time, K p is the proportionality coefficient, T t is the integration coefficient, T d is the differential coefficient.

4. The motor servo control method according to claim 1, It is characterized in that The obtaining of the speed value and the acceleration value fed back by the motor includes: Obtaining the number of rotations of the motor within a fixed time, wherein the speed value fed back by the motor is a ratio of the number of rotations of the motor within the fixed time to the fixed time; Obtain a first speed of the motor at a first time, obtain a second speed of the motor at a second time interval from the first time interval, calculate a speed difference between the first speed and the second speed, and the acceleration value is a ratio of the speed difference to the second time.

5. The motor servo control method according to claim 1, It is characterized in that The step of obtaining a position loop error value according to the command position signal and the position signal fed back by the motor, and obtaining a command speed value according to the position loop error value comprises: The position loop error value is obtained by subtracting the command position signal from the position signal fed back by the motor; The position loop error value is input into the position loop, and the command speed value is obtained through a first transfer function operation in the position loop.

6. The motor servo control method according to claim 1, It is characterized in that The step of obtaining a current loop error value according to the current signal fed back by the motor and the command current value, and obtaining a current loop output value according to the current loop error value comprises: Subtracting the current signal fed back by the motor from the command current value to obtain the current loop error value; Inputting the current loop error value into the current loop, and obtaining the current loop output value through a third transfer function operation in the current loop; Wherein, the current loop output value is a PWM opening signal.

7. The motor servo control method according to claim 6, It is characterized in that The step of converting the current loop output value into a driving voltage and outputting the driving voltage to the motor to drive the motor to operate includes: The PWM opening signal is converted into the driving voltage, and the driving voltage is output to the motor to drive the motor to operate.

8. A motor servo control device, It is characterized in that The motor servo control device is used to execute the motor servo control method according to any one of claims 1 to 7, and the motor servo control device comprises: A position loop module, used for receiving a command position signal and a position signal fed back by the motor, obtaining a position loop error value according to the command position signal and the position signal fed back by the motor, and obtaining a command speed value according to the position loop error value; A speed loop module, used for obtaining the speed value and acceleration value fed back by the motor, obtaining a speed error value according to the command speed value and the speed value fed back by the motor, obtaining a speed loop calculation error value according to the speed error value and the acceleration value, and inputting the speed loop calculation error value into the speed loop to obtain a command current value; A current loop module, used for acquiring the current signal fed back by the motor, obtaining a current loop error value according to the current signal fed back by the motor and the command current value, and obtaining a current loop output value according to the current loop error value; The driving module is used to convert the current loop output value into a driving voltage, and output the driving voltage to the motor to drive the motor to operate.

9. The motor servo control device according to claim 8, It is characterized in that The position loop module includes a first differential module and a position loop; the speed loop module includes a second differential module and the speed loop; the current loop module includes a third differential module and a current loop; The first differential module is used to subtract the command position signal from the position signal fed back by the motor to obtain the position loop error value; The position loop is used to substitute the position loop error value into the first transfer function to calculate and obtain the command speed value; The second differential module is used to obtain the speed error value by subtracting the command speed value from the speed value fed back by the motor; The speed loop calculation error value is obtained by calculation, and the speed loop calculation error value satisfies: e=V 0 +k*a; Where, e is the speed loop calculation error value, V 0 is the velocity error value, k is the acceleration correction coefficient, and a is the acceleration value; The speed loop is used to substitute the speed loop calculated error value into the second transfer function to calculate and obtain the command current value; The third difference module is used to subtract the current signal fed back by the motor from the command current value to obtain the current loop error value; The current loop is used to substitute the current loop error value into the third transfer function to calculate and obtain the current loop output value.

10. The motor servo control device according to claim 9, It is characterized in that It also includes an encoder signal processing module and a current signal processing module; The input end of the encoder signal processing module is connected to the motor, the first output end of the encoder signal processing module is connected to the first input end of the first differential module, the second output end of the encoder signal processing module is connected to the first input end of the second differential module, and the second output end of the encoder signal processing module is connected to the second input end of the second differential module. The encoder signal processing module is used to obtain the position signal fed back by the motor, the speed value fed back by the motor, and the acceleration value, and send the position signal fed back by the motor to the first input end of the first differential module, send the speed value fed back by the motor to the first input end of the second differential module, and send the acceleration value to the second input end of the second differential module; The current signal processing module is connected to the phase line of the motor, and the output end of the current signal processing module is connected to the first input end of the third differential module. The current signal processing module is used to obtain the current signal fed back by the motor from the phase line of the motor, and send the current signal fed back by the motor to the first input end of the third differential module.

Citation Information

Patent Citations

  • Anti-resonance control system used for power lithium battery preparation device and method thereof

    CN106655957A

  • Control system parameter tuning method applicable to hybrid mechanism

    CN107390525A

  • Alternating current servo driving method

    CN113467229A

  • Servo control method and system suitable for parallel non-circular turning control system

    CN114326592A

  • Integrally-driven automatic focusing microscopic control system

    CN118330868A

Cited By

  • Barrier gate control method and device

    CN120281241A