Motor servo control method and device

Abstract

The application 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. In the calculation of the speed loop, the acceleration is introduced, so that the dynamic performance of the motor is more stable.

Description

Technical Field

[0001] This invention relates to the field of motor control technology, and in particular to a motor servo control method and apparatus. Background Technology

[0002] A servo system is an automatic control system that uses mechanical position or angle as the controlled 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 a surgical robot is the instrument. The system employs a master-slave follow-up approach, using the instrument to replicate the surgeon's actions and complete the patient's surgery. In existing servo systems, the speed loop input signals in the control program consist of command speed and feedback speed, with the output being a current command. This method is prone to instability under drastic load changes. Applying existing servo systems to surgical robots would lead to dynamic performance instability in the instrument's motor operation. Summary of the Invention

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

[0005] In a first aspect, embodiments of the present invention provide a motor servo control method, which employs a three-loop control system to drive the motor, the three-loop control system including a speed loop, and the method comprising:

[0006] Receive the command position signal and the position signal fed back by the motor, obtain the position loop error value based on the command position signal and the position signal fed back by the motor, and obtain the command speed value based on the position loop error value;

[0007] The speed and acceleration values ​​fed back by the motor are obtained. A speed error value is obtained based on the commanded speed value and the speed value fed back by the motor. A speed loop calculation error value is obtained based on the speed error value and the acceleration value. The speed loop calculation error value is input into the speed loop to obtain the commanded current value.

[0008] The current signal fed back by the motor is obtained, and the current loop error value is obtained based on the current signal fed back by the motor and the command current value. The current loop output value is obtained based on the current loop error value.

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

[0010] Secondly, embodiments of the present invention also provide a motor servo control device, which is used to execute the motor servo control method described in any embodiment of the present invention. The motor servo control device includes:

[0011] The position loop module is used to receive the command position signal and the position signal fed back by the motor, obtain the position loop error value based on the command position signal and the position signal fed back by the motor, and obtain the command speed value based on the position loop error value;

[0012] The speed loop module is used to obtain the speed and acceleration values ​​fed back by the motor, obtain the speed error value based on the command speed value and the speed value fed back by the motor, obtain the speed loop calculation error value based on the speed error value and the acceleration value, and input the speed loop calculation error value into the speed loop to obtain the command current value;

[0013] The current loop module is used to acquire the current signal fed back by the motor, obtain the current loop error value based on the current signal fed back by the motor and the command current value, and obtain the current loop output value based on the current loop error value.

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

[0015] This invention provides a motor servo control method and apparatus. The motor servo control method employs a three-loop control system to drive the motor. It receives command position signals and motor feedback position signals, obtains a position loop error value based on these signals, and then calculates the command speed value based on the position loop error value. By introducing acceleration into the speed loop calculation, it obtains the motor feedback speed and acceleration values, calculates the speed error value based on the command speed value and the motor feedback speed value, and then calculates the speed loop error value based on the speed error value and acceleration value. This speed loop error value is input into the speed loop to obtain the command current value, resulting in a faster response speed and more stable dynamic performance. Furthermore, by acquiring the motor feedback current signal and obtaining the current loop error value based on the motor feedback current signal and the command current value, it calculates the current loop output value. The current loop output value is converted into a drive voltage and output to the motor, achieving precise control of the motor torque, speed, and position. This ensures stable operation of the motor under various operating conditions and accurate execution of control commands. The invention also incorporates acceleration into the speed loop calculation to further stabilize the motor's dynamic performance. Attached Figure Description

[0016] Figure 1 This is a flowchart of a motor servo control method provided in an embodiment of the present invention.

[0017] Figure 2 This is a schematic diagram illustrating the principle of the PID control process provided in an embodiment of the present invention.

[0018] Figure 3 A flowchart of another motor servo control method provided in an embodiment of the present invention.

[0019] Figure 4 This is a schematic diagram of the structure of a motor servo control device provided in an embodiment of the present invention.

[0020] Figure 5 This is a schematic diagram of the structure of a medical surgical robot provided in an embodiment of the present invention. Detailed Implementation

[0021] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and not intended to limit it. Furthermore, it should be noted that, for ease of description, the accompanying drawings show only the parts relevant to the present invention, and not all of the structures.

[0022] Figure 1 This is a flowchart of a motor servo control method provided in an embodiment of the present invention. The motor servo control method in this embodiment uses a three-loop control system to drive the motor. 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: Receives the command position signal and the position signal fed back by the motor, obtains the position loop error value based on the command position signal and the position signal fed back by the motor, and obtains the command speed value based on the position loop error value.

[0024] The three-loop control system comprises a current loop, a speed loop, and a position loop. These three loops, from the inside out, form the three-loop control system of a motion servo system. The primary objective of the current loop is to control torque, which affects speed and thus position. The speed loop, located outside the current loop, uses negative feedback PID control by detecting signals from the motor's encoder. Its PID output is directly the setpoint of the current loop, ensuring stable motor operation at a given speed and rapid response to speed changes. The position loop, the outermost control loop, converts externally input pulses or position commands into position control signals for the motor, ensuring accurate arrival at the designated position. Through the coordinated action of the current, speed, and position loops, the three-loop control system achieves precise control of the motor's torque, speed, and position, ensuring stable operation and accurate execution of control commands under various operating conditions.

[0025] Specifically, the command position signal is obtained by the motor's command receiving module receiving control commands from the host computer, parsing the control commands, and enabling the position loop upon receiving an enable command from the host computer. The position feedback signal from the motor is obtained by the encoder processing module connected to the motor after acquiring the motor's position, and the encoder processing module sends the motor feedback position signal to the position loop. The position loop receives the command position signal and the motor feedback position signal, calculates the position loop error value based on these two signals, performs PID adjustment on the position loop to obtain the command speed value, and then outputs the command speed value to the speed loop.

[0026] S120. Obtain the speed and acceleration values ​​fed back by the motor. Based on the commanded speed value and the speed value fed back by the motor, obtain the speed error value. Based on the speed error value and the acceleration value, obtain the speed loop calculation error value. Input the speed loop calculation error value into the speed loop to obtain the commanded current value.

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

[0028] Specifically, after receiving the commanded speed value, the motor feedback speed value, and the acceleration value, the speed loop calculates the speed error value based on the commanded speed value and the motor feedback speed value. The speed error value is obtained by subtracting the motor feedback speed value from the commanded speed value. Based on the speed error value and the acceleration value, the speed loop calculation error value is calculated. After PID adjustment within the speed loop, the commanded current value is output and used as the setpoint for the current loop. The characteristic of this method is that, in the speed loop control process, since the speed output of the three-loop control system is current, and current corresponds to acceleration, the acceleration fed back from the motor is introduced into the speed loop control process, making the loop response faster and the dynamic performance more stable.

[0029] S130. Obtain the current signal fed back by the motor, obtain the current loop error value based on the current signal fed back by the motor and the command current value, and obtain the current loop output value based on the current loop error value.

[0030] Specifically, the current signal fed back from the motor is obtained from the motor's phase lines by the current signal processing module, which then sends the obtained current signal to the current loop. Upon receiving the current signal and the commanded current value, the current loop compares them to calculate the current loop error value. This error value is then processed by a PID controller within the current loop and output to the motor to control the current in each phase, thereby controlling the motor's torque.

[0031] in addition, Figure 2This is a schematic diagram illustrating the principle of the PID control process provided in an embodiment of the present invention. The PID control processes in the current loop, speed loop, and position loop can all include proportional, integral, and derivative components. Figure 2 As shown, after receiving the setpoint and feedback signal, the current loop, speed loop, and position loop calculate the error value by comparing the difference. Then, they perform proportional, integral, and derivative calculations, summing the results to obtain the output signal, which is then output to the next stage. The feedback signal is typically obtained from a sensor; in this embodiment, it is obtained from the encoder processing module. Through the PID regulation and synergistic effect of the current loop, speed loop, and position loop, precise control of the motor torque, speed, and position is achieved, ensuring stable operation and accurate execution of control commands under various operating conditions.

[0032] S140: Convert the current loop output value into a drive voltage and output the drive voltage to the motor to drive the motor to run.

[0033] Specifically, the current loop outputs its value to the drive module, which then converts it into a drive voltage and outputs it to the motor to drive it. The key feature of this method is that in the speed loop control process, the speed output is current, and current corresponds to acceleration. Therefore, feedback acceleration is introduced into the speed loop control method, resulting in a faster response speed and more stable dynamic performance.

[0034] This invention provides a motor servo control method. The method employs a three-loop control system to drive the motor. It receives command position signals and motor feedback position signals, calculates a position loop error value based on these signals, and then calculates a command speed value based on the position loop error value. By introducing acceleration into the speed loop calculation, it obtains the motor feedback speed and acceleration values, calculates a speed error value based on the command speed and motor feedback speed values, and then calculates a speed loop error value based on the speed error value and acceleration value. This speed loop error value is input into the speed loop to obtain the command current value, resulting in a faster response speed and more stable dynamic performance. Furthermore, by acquiring the motor feedback current signal and calculating a current loop error value based on the motor feedback current signal and command current value, it calculates a current loop output value based on the current loop error value. The current loop output value is converted into a drive voltage and output to the motor, achieving precise control of the motor's torque, speed, and position. This ensures stable operation and accurate execution of control commands under various operating conditions. The invention also incorporates acceleration into the speed loop calculation to further stabilize the motor's dynamic performance.

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

[0036] S210: Receive the command position signal and the position signal fed back by the motor; obtain the position loop error value by subtracting the command position signal from the position signal fed back by the motor; input the position loop error value into the position loop, and obtain the command speed value through the first transfer function in the position loop.

[0037] Specifically, after receiving the commanded position signal and the position signal fed back from the motor, the position loop obtains the position loop error value by subtracting the commanded position signal from the motor feedback position signal. This position loop error value is then input into the position loop and processed by the first transfer function within the position loop to obtain the commanded speed value.

[0038] The first transfer function is:

[0039]

[0040] Where e1 is the position loop error value, u1 is the commanded speed value, t is time, and K is the position loop error value. p1 T is the position ring scaling factor. t1 T represents the position loop integral coefficient. d1 This represents the differential coefficient of the position loop. The commanded speed value can be obtained by performing proportional, integral, and differential operations using the first transfer function, ensuring the motor accurately reaches the specified position.

[0041] S220: Obtain the speed and acceleration values ​​fed back by the motor; obtain the speed error value based on the commanded speed value and the speed value fed back by the motor; obtain the speed loop calculation error value based on the speed error value and the acceleration value; input the speed loop calculation error value into the speed loop to obtain the commanded current value.

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

[0043] The number of rotations of the motor within a fixed time period is obtained, and the speed value fed back by the motor is the ratio of the number of rotations of the motor within the fixed time period to the fixed time period.

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

[0045] Specifically, the encoder processing module calculates the motor's feedback speed value by acquiring the number of motor rotations within a fixed time interval. The feedback speed value is the ratio of the number of motor rotations within the fixed time interval to the fixed time itself. The encoder processing module also acquires the motor's first rotational speed at a first time interval and its second rotational speed at a second time interval, calculating the speed difference between the two. The acceleration value is the ratio of this speed difference to the second time interval. For example, the encoder processing module uses two speed calculation methods: one is: with a fixed time T, if the encoder counts M within T, then the motor speed n = M / T; the other is: with the time T1 between the current value change and the previous value change in the encoder processing module, then the motor speed n = 1 / T1. Each method has its advantages and disadvantages; a combination of these methods is needed to more accurately determine the motor speed value n, which is the motor's feedback speed value. The encoder processing module obtains the acceleration calculation method based on motor feedback as follows: The interval time is T; the motor speed before time T is n1; and the motor speed after time T is n2. Therefore, the motor acceleration a = (n2 - n1) / T. After obtaining the speed error value and acceleration value in the speed loop, the speed loop calculation error value is calculated. This error value is input into the speed loop to obtain the command current value, enabling rapid response to motor speed changes and ensuring stable motor operation at a given speed. Furthermore, introducing motor feedback acceleration during speed loop control makes the loop response faster and its dynamic performance more stable.

[0046] Optionally, a speed error value is obtained based on the commanded speed value and the speed value fed back by the motor. A speed loop calculation error value is then obtained based on the speed error value and the acceleration value, including: subtracting the commanded speed value from the speed value fed back by the motor to obtain the speed error value; calculating the speed loop calculation error value, which satisfies the following:

[0047] e = V0 + k * a;

[0048] Where e is the velocity loop calculation error value, V0 is the velocity error value, k is the acceleration correction coefficient, and a is the acceleration value. The velocity loop calculation error value e can be obtained by adding the product of the velocity error value V0, the acceleration value a, and the acceleration correction coefficient k.

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

[0050] Optionally, the speed loop calculation error value can be 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 by the second transfer function in the speed loop.

[0052] The second transfer function is:

[0053]

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

[0055] Specifically, after the speed loop error value is input into the speed loop, the speed loop performs PID regulation through the second transfer function to calculate the output value of the speed loop, namely 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 changes in the motor speed.

[0056] Furthermore, traditional servo drives receive control commands from a host computer and transmit them to a control unit. The control unit then calculates the control data to be output to the motor based on feedback data from the motor, converting it into actual motor control current via the drive circuit to achieve closed-loop motor control. In traditional motor speed loop control methods, the input signals to the speed loop are the commanded speed and the feedback speed, and the output is a current command. This method is prone to instability under drastic load changes. Therefore, this invention analyzes the speed loop, where the output is current. This current corresponds to the acceleration in the system. Therefore, acceleration is introduced into the speed loop calculation, and the calculated acceleration value corresponds to the system's current output, resulting in a faster loop response and more stable dynamic performance.

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

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

[0059]

[0060] Where e3 is the current loop error value, u3 is the current loop output value, t is time, and K is the current loop error value. p3 T is the proportionality coefficient of the current loop.t3 T is the integral coefficient of the current loop. d3 This represents the differential coefficient of the current loop. The output value of the current loop can be obtained by calculating the proportional, integral, and differential components using the third transfer function, aiming to make the output current of the current loop as close as possible to the set current, and thus controlling the motor torque.

[0061] S240 converts the PWM opening signal into a drive voltage and outputs the drive voltage to the motor to drive the motor to run.

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

[0063] The characteristic of the motor servo control method in this embodiment of the invention lies in the fact that, during the speed loop control process, the speed output is current, and the current corresponds to acceleration. Therefore, feedback acceleration is introduced into the speed loop control method, making the loop response faster and the dynamic performance more stable. By adopting a speed loop with introduced acceleration, the dynamic characteristics and static stability of the system containing the motor servo control method are superior under varying load conditions.

[0064] It should be noted that the motor servo control method in the embodiments of the present invention can run on various programmable CPUs and field programmable gate array (FPGA) devices.

[0065] This invention provides a motor servo control method that employs a three-loop feedback control and introduces a speed closed-loop control algorithm with feedback acceleration. This enables precise control of motor torque, speed, and position, ensuring stable operation and accurate execution of control commands under various operating conditions. Furthermore, by introducing acceleration into the speed loop calculation, this invention achieves faster speed loop response and more stable dynamic performance of the motor.

[0066] Figure 4 This is a schematic diagram of a motor servo control device provided in an embodiment of the present invention. The motor servo control device in 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 the position loop error value based on the command position signal and the position signal fed back by the motor, and obtain the command speed value based on the position loop error value.

[0068] The speed loop module 120 is used to obtain the speed and acceleration values ​​fed back by the motor, obtain the speed error value based on the command speed value and the speed value fed back by the motor, obtain the speed loop calculation error value based on the speed error value and the acceleration value, and input the speed loop calculation error value into the speed loop to obtain the command current value.

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

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

[0071] Specifically, the position loop module 110 receives the command position signal and the position signal fed back by the motor. Based on these signals, it calculates the position loop error value. This error value is then used for PID adjustment within the position loop to obtain the command speed value, which is output to the speed loop. The speed loop module 120 receives the command speed value, the motor feedback speed value, and the acceleration value. Based on these values, it calculates the speed error value, which is obtained by subtracting the motor feedback speed value from the command speed value. Based on the speed error value and the acceleration value, it calculates the speed loop calculation error value. This error value is then used for PID adjustment within the speed loop, and the command current value is output as the setpoint for the current loop. The current loop module 130 receives the motor feedback current signal and the command current value. Based on these two values, it calculates the current loop error value. This error value is then used for PID adjustment within the current loop and output to the motor to control the current in each phase, thereby controlling the motor's torque. The drive module 140 converts the current loop output value into a drive voltage and outputs the drive voltage to the motor to drive the motor to run. The feature of this method is that in the speed loop control process, the speed output is current, and the current corresponds to acceleration. Therefore, feedback acceleration is introduced into the speed loop control method, which makes the loop response faster and the dynamic performance more stable.

[0072] This invention provides a motor servo control device. The device receives command position signals and motor feedback position signals via a position loop module. It obtains a position loop error value based on these signals and then calculates a command speed value. A speed loop module incorporates acceleration into its calculations to obtain the motor feedback speed and acceleration values. It calculates a speed error value based on the command speed and the motor feedback speed, and then calculates a speed loop error value based on the speed error and acceleration values. This speed loop error value is input into the speed loop to obtain the command current value, resulting in faster response and more stable dynamic performance. A current loop module obtains the motor feedback current signal and calculates a current loop error value based on the motor feedback current signal and the command current value. A current loop output value is then calculated based on the current loop error value. A drive module converts the current loop output value into a drive voltage and outputs the drive voltage to the motor, achieving precise control of the motor's torque, speed, and position. This ensures stable operation and accurate execution of control commands under various operating conditions. Furthermore, the invention incorporates acceleration into the speed loop calculations to further stabilize the motor's dynamic performance.

[0073] Further reference 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 differential module 111 is used to subtract the commanded position signal from the position signal fed back by the motor to obtain the position loop error value. The position loop 112 is used to substitute the position loop error value into the first transfer function to obtain the commanded speed value.

[0075] The second differential module 121 is used to calculate the speed error value by subtracting the commanded speed value from the motor feedback speed value. The calculated speed loop error value satisfies the following conditions:

[0076] e = V0 + k * a;

[0077] Where e is the velocity loop calculation error value, V0 is the velocity error value, k is the acceleration correction coefficient, and a is the acceleration value.

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

[0079] The third differential module 131 is used to subtract the current signal fed back from the motor from the commanded current value to obtain the current loop error value. The current loop 132 is used to substitute the current loop error value into the third transfer function to obtain the current loop output value.

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

[0081]

[0082] Where e1 is the position loop error value, u1 is the commanded speed value, t is time, and K is the position loop error value. p1 T is the position ring scaling factor. t1 T represents the position loop integral coefficient. d1 This represents the differential coefficient of the position loop. The commanded speed value can be obtained by performing proportional, integral, and differential operations using the first transfer function, ensuring the motor accurately reaches the specified position.

[0083] The second differential module 121 subtracts the motor feedback speed value from the commanded speed value to obtain the speed error value, and calculates the speed loop calculation error value, which satisfies the following:

[0084] e = V0 + k * a;

[0085] Where e is the velocity loop calculation error value, V0 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, which is determined through multiple experiments 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, the speed loop performs PID control through the second transfer function to calculate the output value of the speed loop, i.e., the command current value. The command current value is also the input value of the current loop, ensuring stable motor operation at a given speed and rapid response to speed changes. The second transfer function is:

[0087]

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

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

[0090]

[0091] Where e3 is the current loop error value, u3 is the current loop output value, t is time, and K is the current loop error value. p3 T is the proportionality coefficient of the current loop. t3 T is the integral coefficient of the current loop. d3 This represents the differential coefficient of the current loop. The output value of the current loop can be obtained by calculating the proportional, integral, and differential components using the third transfer function, aiming to make the output current of the current loop as close as possible to the set current, and thus controlling the motor torque.

[0092] Further reference Figure 4 The motor servo control device also includes an encoder signal processing module 160 and a current signal processing module 150;

[0093] The input terminal of the encoder signal processing module 160 is connected to the motor. The first output terminal of the encoder signal processing module 160 is connected to the first input terminal of the first differential module 111. The second output terminal of the encoder signal processing module 160 is connected to the first input terminal of the second differential module 121. The second output terminal of the encoder signal processing module 160 is connected to the second input terminal of the second differential module 121. The encoder signal processing module 160 is used to acquire the position signal, the speed value, and the acceleration value fed back by the motor, and sends the position signal fed back by the motor to the first input terminal of the first differential module 111, sends the speed value fed back by the motor to the first input terminal of the second differential module 121, and sends the acceleration value to the second input terminal 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 terminal of the current signal processing module 150 is connected to the first input terminal 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 terminal of the third differential module 131.

[0095] The encoder signal processing module 160 features real-time monitoring and feedback, enabling it to monitor key parameters such as motor speed and the relative position of the shaft in the circumferential direction. This allows for accurate understanding of the motor's and its driven equipment's operating status. By converting these monitored parameters into digital pulse signals and feeding them back to the control system, a closed-loop control is formed. This feedback mechanism allows the control system to adjust the motor's operating status based on real-time data, ensuring the accuracy and stability of motor operation.

[0096] Specifically, the input terminal of the encoder signal processing module 160 is connected to the motor, which can acquire the position signal, speed value, and acceleration value fed back by the motor. The position signal is sent to the first differential module 111, and the speed and acceleration values ​​are sent to the second differential module 121, enabling closed-loop control of the position loop 112, speed loop 122, and current loop 132. Furthermore, the encoder signal processing module 160 feeds back the acceleration to the speed loop module 120, resulting in a faster response speed and more stable dynamic performance for the speed loop 122.

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

[0098] The various modules of the medical surgical robot serve the following functions: **Surgeon's Console:** The surgeon operates from the surgeon's console, inputting surgical instructions and desired surgical poses. The surgeon's console is the interface between the surgeon and the robot, allowing the surgeon to precisely control the robot's movements. **Operation Pose Generation:** Instructions received from the surgeon's console are converted into specific operation pose information. This step involves translating the surgeon's intentions into specific parameters that the robot can understand and execute, such as position, posture, and speed. **Communication Module:** Operation pose information is transmitted at high speed and reliably via the ETHERCAT communication protocol (a real-time industrial Ethernet communication protocol). ETHERCAT communication ensures real-time data exchange between the surgeon's console and the robot system, enabling rapid response to the surgeon's instructions during surgical operations. **Execution Pose Generation:** Upon receiving the operation pose information, the robot system internally generates the execution pose. This step may involve complex algorithms and calculations to ensure the robot moves according to the surgeon's desired trajectory and posture. **Motor Drive Module:** The execution pose information is transmitted to the motor drive module, which controls the robot's motors to generate the corresponding movements. The motor drive module is a crucial component for the robot's motion execution. It drives the motors to rotate based on received commands, thereby moving the robot's joints and end effector. Position output from the pose motor: Ultimately, the robot's end effector (such as a surgical instrument) reaches the surgeon's desired pose and performs the corresponding surgical procedure. This process is achieved through precise motor control and feedback mechanisms, ensuring the accuracy and safety of the surgical operation. The motor servo control device in this embodiment can be applied to the motor drive module in a medical surgical robot to improve the robot's dynamic characteristics and static stability, achieving precise control and efficient execution of surgical operations.

[0099] This invention provides a motor servo control device that achieves precise control of motor torque, speed, and position through the coordinated control of a position loop, speed loop, and current loop. This ensures stable operation of the motor and accurate execution of control commands under various operating conditions. Furthermore, by introducing acceleration into the speed loop calculation, this invention enables faster speed loop response and more stable dynamic performance of the motor, thereby improving the dynamic characteristics and static stability of the system containing the motor servo control device. This achieves precise control and efficient execution of the system.

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

Claims

1. A motor servo control method, characterized in that, A three-loop control system is used to drive the motor. This motor servo control method is applied to the motor drive module in a medical surgical robot. The three-loop control system includes a current loop, a speed loop, and a position loop arranged sequentially from the inside out. The method includes: Receive the command position signal and the position signal fed back by the motor, obtain the position loop error value based on the command position signal and the position signal fed back by the motor, and obtain the command speed value based on the position loop error value; The process involves obtaining the speed and acceleration values ​​fed back by the motor, calculating a speed error value based on the commanded speed value and the speed value fed back by the motor, calculating a speed loop error value based on the speed error value and the acceleration value, and inputting the speed loop error value into the speed loop to obtain the commanded current value. Specifically, obtaining the speed and acceleration values ​​fed back by the motor includes: obtaining the number of rotations of the motor within a fixed time period, where the speed value fed back by the motor is the ratio of the number of rotations of the motor within the fixed time period to the fixed time period; obtaining the first rotational speed of the motor at a first time point; obtaining the second rotational speed of the motor at a second time point relative to the first time interval; calculating the speed difference between the first rotational speed and the second rotational speed, where the acceleration value is the ratio of the speed difference to the second time period. The current signal fed back by the motor is obtained, and the current loop error value is obtained based on the current signal fed back by the motor and the command current value. The current loop output value is obtained based on the current loop error value. The current signal fed back by the motor is obtained from the phase line of the motor. The current loop output value is converted into a drive voltage, and the drive voltage is output to the motor to drive the motor to run; The process of obtaining a speed error value based on the commanded speed value and the speed value fed back by the motor, and obtaining a speed loop calculation error value based on the speed error value and the acceleration value, includes: The speed error value is obtained by subtracting the commanded speed value from the speed value fed back by the motor. The calculated speed loop error value satisfies the following conditions: ； Where e is the velocity loop calculation error value, V0 is the velocity error value, k is the acceleration correction coefficient, and a is the acceleration value.

2. The motor servo control method according to claim 1, characterized in that, The step of inputting the speed loop calculation error value into the speed loop to obtain the command current value includes: The speed loop error value is input into the speed loop, and the command current value is obtained by the second transfer function in the speed loop. The second transfer function is: ； Where u is the command current value, t is time, and K p T is the proportionality coefficient. t T is the integral coefficient. d is the differential coefficient.

3. The motor servo control method according to claim 1, characterized in that, The step of obtaining the position loop error value based on the commanded position signal and the position signal fed back by the motor, and obtaining the commanded speed value based on the position loop error value, includes: The position loop error value is obtained by subtracting the commanded 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 in the position loop.

4. The motor servo control method according to claim 1, characterized in that, The process of obtaining the current loop error value based on the current signal fed back by the motor and the command current value, and obtaining the current loop output value based on the current loop error value, includes: The current loop error value is obtained by subtracting the current signal fed back by the motor from the commanded current value. The current loop error value is input into the current loop, and the current loop output value is obtained by the third transfer function in the current loop. The current loop output value is the PWM opening signal.

5. The motor servo control method according to claim 4, characterized in that, The step of converting the current loop output value into a drive voltage and outputting the drive voltage to the motor to drive the motor to run 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 run.

6. A motor servo control device, characterized in that, The motor servo control device is used to execute the motor servo control method according to any one of claims 1-5, and the motor servo control device includes: The position loop module is used to receive the command position signal and the position signal fed back by the motor, obtain the position loop error value based on the command position signal and the position signal fed back by the motor, and obtain the command speed value based on the position loop error value; The speed loop module is used to obtain the speed and acceleration values ​​fed back by the motor, obtain the speed error value based on the command speed value and the speed value fed back by the motor, obtain the speed loop calculation error value based on the speed error value and the acceleration value, and input the speed loop calculation error value into the speed loop to obtain the command current value; The current loop module is used to acquire the current signal fed back by the motor, obtain the current loop error value based on the current signal fed back by the motor and the command current value, and obtain the current loop output value based on the current loop error value. The drive module is used to convert the current loop output value into a drive voltage and output the drive voltage to the motor to drive the motor to run.

7. The motor servo control device according to claim 6, characterized in that, The position loop module includes a first differential module and a position loop; the velocity loop module includes a second differential module and a velocity 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 obtain the command speed value; The second differential module is used to obtain the speed error value by subtracting the commanded speed value from the speed value fed back by the motor; The calculated speed loop error value satisfies the following conditions: ； Where e is the velocity loop calculation error value, V0 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 calculation error value into the second transfer function to obtain the command current value; The third differential 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 obtain the current loop output value.

8. The motor servo control device according to claim 7, characterized in that, It also includes an encoder signal processing module and a current signal processing module; The input terminal of the encoder signal processing module is connected to the motor, the first output terminal of the encoder signal processing module is connected to the first input terminal of the first differential module, the second output terminal of the encoder signal processing module is connected to the first input terminal of the second differential module, and the second output terminal of the encoder signal processing module is connected to the second input terminal of the second differential module. The encoder signal processing module is used to acquire 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 terminal of the first differential module, send the speed value fed back by the motor to the first input terminal of the second differential module, and send the acceleration value to the second input terminal of the second differential module. The current signal processing module is connected to the phase line of the motor, and the output terminal of the current signal processing module is connected to the first input terminal 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 terminal of the third differential module.

Images