A motor closed-loop control device and method for a surgical robot

Abstract

The present invention relates to a motor closed-loop control device and method for a surgical robot, comprising an execution side connected to a base, the execution side is located at the top of the driving side, and the execution side is detachably connected to the driving side, the driving side output mechanism is provided with a first magnet, and a first sensor is provided at a position corresponding to the first magnet in the base; a second magnet is provided at the axis of the driven wheel of the execution side, and a second sensor is installed at a position corresponding to the second magnet in the base, and two groups of sensors respectively obtain the rotation speed output by the driving side and the rotation speed output by the execution side. Both sensors are located inside the base of the driving side, and the process of obtaining the rotation speed data under the action of the magnetic field does not require the execution side to have direct contact with the sensor body, which is conducive to the sterilization operation and aseptic isolation of the execution side, and the data obtained by the two groups of sensors can be directly exported using the serial port on the base, so that the process of obtaining the rotation speed of the execution side no longer requires wires, and can meet the aseptic isolation requirements of surgical instruments.

Description

Technical Field

[0001] The present invention relates to the field of surgical robots, and in particular to a motor closed-loop control device and method for a surgical robot. Background Art

[0002] The statements in this section merely provide background information related to the present invention and do not necessarily constitute prior art.

[0003] For assisted surgical equipment and assisted surgical systems using robotic technology, during surgery, non-sterile equipment and sterilized equipment must be separated by a sterile isolation cover, and non-sterile equipment / system components are covered in the sterile isolation cover, while sterilized equipment / system components are located outside the sterile isolation cover and in contact with the patient. This type of surgical robot usually includes a non-sterile drive side and a sterilized execution side. The end of the execution side carries different actuators (such as needle holders, clip applicators, stripping forceps, monopolar hooks, etc.). The drive side and the execution side need to be connected by a mechanical coupling mechanism to transmit the rotational power of the drive side motor to drive the actuator at the end of the execution side.

[0004] The surgical robot has high requirements for the control accuracy of the end effector. There is an error between the rotational motion output by the driving side motor and the motion finally output by the execution side, which affects the control accuracy of the end effector. The error here comes from the gap between multiple mechanical parts inside the execution side (for example, the tooth surface gap between two adjacent sets of gears when the gears are engaged). This type of motion error is small and can be ignored for general mechanical equipment. However, for surgical robots, the motion error generated by this structure causes the motion control instructions issued by the driving side to have slight position deviations or angle deviations when they are output by the execution side, which ultimately affects the accuracy of the surgical robot's movements, posture accuracy and repeatability, and even the delay time of the master-slave control.

[0005] The prior art solves the above problem through closed-loop speed detection on the driving side and the execution side. There are two conventional methods for the existing motor drive closed-loop method. The first is to install a speed detection sensor on the motor output shaft and calculate the output speed of the execution side through the transmission ratio designed by the system; the second is to install a speed detection sensor on the motor output shaft and install a wired sensor on the execution side to detect the output speed.

[0006] The first method can only detect the rotation speed of the motor output shaft, but does not detect the rotation speed of the execution side output. The output rotation speed calculated only by the transmission ratio designed by the system is still a theoretical value, which will introduce a large error and cannot minimize the error caused by the mechanical clearance of the transmission mechanism. Therefore, the accuracy of this method cannot meet the drive control requirements of surgical instruments.

[0007] Although the second method detects the rotation speed of the driving side and the execution side at the same time, it requires a connecting cable and cannot achieve sterile isolation of the driving side and the execution side, and cannot meet the requirements of the surgical environment. Summary of the invention

[0008] In order to solve the technical problems existing in the above-mentioned background technology, the present invention provides a motor closed-loop control device and method for a surgical robot, by simultaneously installing magnets at the axis of the output shaft on the driving side and the driven gear shaft on the execution side, and respectively installing non-contact magnetic rotary encoders at positions corresponding to the magnets in the base, and simultaneously detecting the rotational speed of the output on the driving side and the rotational speed of the output on the execution side, and minimizing the error caused by the mechanical idle clearance of the transmission mechanism through a software algorithm, thereby improving the motion control accuracy and meeting the use requirements of the surgical robot.

[0009] In order to achieve the above object, the present invention adopts the following technical solution:

[0010] The first aspect of the present invention provides a motor closed-loop control device for a surgical robot, including an execution side connected to a base, the execution side is located on the top of the driving side, and the execution side and the driving side are detachably connected, the driving side output mechanism is provided with a first magnet, and a first sensor is provided at a position corresponding to the first magnet in the base; a second magnet is provided at the axis of a driven wheel on the execution side, and a second sensor is installed at a position corresponding to the second magnet in the base, and the two groups of sensors respectively obtain the rotational speed output by the driving side and the rotational speed output by the execution side.

[0011] The first sensor and the second sensor are both non-contact magnetic rotary encoders.

[0012] The driving side includes a motor connected to the base and a driving side output mechanism. The motor is connected to the driving side output mechanism to output power to drive the end effector of the surgical robot. A first magnet is installed on the output shaft of the driving side output mechanism.

[0013] The first sensor is located at a position in the base corresponding to the first magnet, and the first sensor obtains the rotation speed output by the driving side under the influence of the first magnet.

[0014] The execution side includes a driving wheel and a driven wheel installed in the shell. The driving wheel is connected to the rotating shaft. The output shaft of the output mechanism on the driving side is detachably connected to the rotating shaft to transmit the power of the driving side. The rotating shaft drives the driving wheel to move, the driving wheel is meshed with the driven wheel, and the driven wheel outputs the power of the driving side.

[0015] The execution side also has an end effector, and the driven wheel is connected to the end effector.

[0016] A second magnet is arranged at the axis center of the driven wheel. When the execution side is located at the top of the driving side, the second sensor is located at a position corresponding to the second magnet in the base.

[0017] The second sensor acquires the rotation speed output by the driven wheel on the execution side under the influence of the second magnet.

[0018] A second aspect of the present invention provides a method for implementing motor closed-loop control based on the above device, comprising the following steps:

[0019] Obtaining the speed data of the output of the driving side and the speed data of the driven wheel of the execution side;

[0020] The filter determines whether the speed exceeds the threshold. If it exceeds the threshold, the motor is controlled to rotate to the set position after the gap elimination algorithm.

[0021] The steps of the gap elimination algorithm are:

[0022] The processor cyclically acquires the filtered rotary encoder data of the driving side and the execution side at a set period;

[0023] Compare the sensor data on the drive side and the actuator side to obtain the mechanical idle stroke of the motor transmission gap;

[0024] Use mechanical idle travel as a correction factor for this operation to eliminate the gap;

[0025] During the gap elimination process, the sum of the driving side motor rotation angle setting value and the correction factor is used as the execution side target rotation angle.

[0026] Compared with the prior art, one or more of the above technical solutions have the following beneficial effects:

[0027] 1. Both sensors are located inside the base on the drive side and are non-contact magnetic rotary encoders. The process of obtaining speed data under the action of the magnetic field does not require direct contact between the execution side and the sensor body, which is conducive to the sterilization operation and aseptic isolation of the execution side. The data obtained by the two sets of sensors can be directly exported using the serial port on the base, so that the process of obtaining the execution side speed no longer requires wires, which can meet the aseptic isolation requirements of surgical instruments.

[0028] 2. The drive side and the execution side are isolated through the magnetic field and non-contact magnetic rotary encoder, and the speed of both is detected at the same time. The mechanical backlash of the transmission mechanism is eliminated through software algorithms to minimize the error of the transmission mechanism and improve the motion control accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] The accompanying drawings in the specification, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0030] Figure 1 is a schematic diagram of the overall structure provided by one or more embodiments of the present invention;

[0031] Figure 2 is a schematic diagram of the bottom structure of the execution side provided by one or more embodiments of the present invention;

[0032] Figure 3 is a schematic diagram of the meshing structure of the driving wheel and the driven wheel on the execution side provided by one or more embodiments of the present invention;

[0033] Figure 4 is a flow chart of implementing motor closed-loop control provided by one or more embodiments of the present invention;

[0034] In the figure: 1. base; 2. execution side; 3. drive side output mechanism; 21. rotating shaft; 22. driving wheel; 23. driven wheel; 24. end effector. DETAILED DESCRIPTION

[0035] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0036] It should be noted that the following detailed descriptions are exemplary and are intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0037] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0038] As described in the background technology, the power generated by the motor on the driving side of the surgical robot reaches the rotating shaft on the execution side through the gear reduction box and mechanical coupling mechanism, and the rotating shaft is then converted into the power to pull the end surgical instrument (end effector) through the gear meshing driven wheel. The system contains a gear reduction box and a transmission coupling mechanism and the gear meshing between the master and driven wheels, and there is a mechanical backlash, which brings a large error to the precise control of the system operation.

[0039] Therefore, in the following embodiments, a magnet is installed on the output shaft of the motor gear reducer on the driving side, a non-contact magnetic rotary encoder is installed at a position corresponding to the magnet in the base, a magnet is installed at the axis of the driven gear of the rotating shaft on the execution side, and a non-contact magnetic rotary encoder is installed at a position corresponding to the magnet in the base, and the magnet and the non-contact magnetic rotary encoder are used to obtain the rotation speed of the driving side output and the rotation speed of the execution side output.

[0040] The data obtained by the non-contact magnetic rotary encoders at the above two positions are read in real time through the SPI interface (serial peripheral interface) set on the base. The rotation position of the motor is further corrected by relying on the built-in filtering algorithm and gap elimination compensation algorithm of the MCU on the main control board in the base to ensure the accuracy of the motor rotation position and the movement of the end device. At the same time, the distance between the execution side magnet and the drive side non-contact magnetic rotary encoder can be used to determine in real time whether the execution side mechanism is well installed, so as to ensure the safety and reliability of the device during operation.

[0041] Embodiment 1:

[0042] like Figure 1-3 As shown, a motor closed-loop control device of a surgical robot includes an execution side 2 connected to a base 1, the execution side 2 is detachably connected to a driving side, a first magnet is provided at an output mechanism of the driving side, a first sensor is provided at a position corresponding to the first magnet in the base 1; a second magnet is provided at the axis of a driven wheel on the execution side, a second sensor is installed at a position corresponding to the second magnet in the base, and the two groups of sensors respectively obtain the rotation speed output by the driving side and the rotation speed output by the execution side.

[0043] In this embodiment, both the first sensor and the second sensor are non-contact magnetic rotary encoders.

[0044] The driving side includes a motor connected to the base 1 and a driving side output mechanism 3. In this embodiment, the driving side output mechanism 3 is a gear reducer. The motor is connected to the gear reducer to output power to drive the end effector of the surgical robot. A first magnet is installed on the output shaft of the driving side output mechanism 3.

[0045] The first sensor is located at a position in the base 1 corresponding to the first magnet. The first sensor obtains the rotation speed output by the driving side under the influence of the first magnet.

[0046] The execution side includes a driving wheel 22 and a driven wheel 23 installed in the shell. The driving wheel 22 is connected to the rotating shaft 21. The output shaft of the driving side output mechanism 3 is detachably connected to the rotating shaft 21 to transmit the power of the driving side. The rotating shaft 21 drives the driving wheel 22 to move, and the driving wheel 22 is meshed with the driven wheel 23, and the driven wheel 23 outputs the power of the driving side.

[0047] The execution side further comprises an end effector 24 , and the driven wheel 23 is connected to the end effector 24 .

[0048] A second magnet is provided at the axis of the driven wheel 23. When the execution side is located at the top of the driving side, the second sensor is located at a position corresponding to the second magnet in the base.

[0049] The second sensor acquires the rotation speed output by the driven wheel on the execution side under the influence of the second magnet.

[0050] In the above structure, both sensors are located inside the base on the driving side and are non-contact magnetic rotary encoders. The process of obtaining the rotational speed data under the action of the magnetic field does not require a direct connection between the execution side and the sensor body, which is beneficial to the sterilization operation and aseptic isolation of the execution side. The data obtained by the two sets of sensors can be directly exported using the serial port on the base. The process of obtaining the rotational speed of the execution side no longer requires wires, which can meet the aseptic isolation requirements of surgical instruments.

[0051] The core technology of the non-contact magnetic rotary encoder in this embodiment is the Hall sensor, and the chip parameters are as follows:

[0052] The AS5145 is a 12-bit programmable, contactless magnetic rotary encoder for accurate measurement of angles through a full 360-degree rotation. It is a system-on-chip that integrates Hall elements, analog front-end and digital signal processing in a single device. To measure the angle, a simple bipolar magnet is all that is needed, rotating in the center of the chip. The magnet can be placed on both sides of the integrated circuit. Absolute angle measurement provides instant indication of the angular position of the magnet with a resolution of 0.08790 = 4096 positions per revolution.

[0053] Embodiment 2:

[0054] This embodiment provides a motor closed-loop control method for a surgical robot based on the above detection device, comprising the following steps:

[0055] Obtaining the speed data of the output of the driving side and the speed data of the driven wheel of the execution side;

[0056] The filtering algorithm is used to determine whether the speed exceeds the threshold. If it exceeds the threshold, the motor is controlled to rotate to the set position after the gap elimination algorithm.

[0057] The principle of filtering algorithm:

[0058] The rotation angle measured by the non-contact magnetic rotary encoder is a 12-bit digital quantity, which will have certain quantization deviation and jitter. This embodiment processes the data through oversampling and digital filtering to reduce the error and make it closer to the true value and more accurate. The processed sensor data will be used as input data for the gap elimination program.

[0059] Principle of gap elimination algorithm (program):

[0060] When the motor is started, the processor records the rotary encoder data processed by the filter algorithm on the drive side and the execution side. The processor reads the rotary encoder data processed by the filter algorithm on the drive side and the execution side in a certain period for comparison, and obtains the mechanical idle stroke of the motor transmission gap as the correction factor for this operation. The gap is eliminated according to the following formula:

[0061] α=β+θ;

[0062] α: Target rotation angle on the execution side;

[0063] β: driving side motor rotation angle setting value;

[0064] θ: gap elimination correction factor.

[0065] In the above processing, the data processing of the rotary encoder is more accurate, and the obtained gap elimination factor is used to perform real-time control on the motor.

[0066] Each position of the encoder during the movement process has a certain digital code as the corresponding position. In the sensor data obtained by the driving side and the execution side of this embodiment, the digital codes of the two groups of encoders correspond one to one, representing the one-to-one correspondence between the rotational positions of the driving side and the execution side. If a certain rotational position of the driving side does not correspond to the execution side, it means that "mechanical idleness" has appeared on the execution side at this position, then one or several digital codes representing the driving side position cannot be corresponded by the execution side, and an artificial "correction factor" is required to eliminate this part of the "mechanical idleness".

[0067] Therefore, in this embodiment, the transmission ratio of the output structure of the driving side (i.e., the first magnet installation location) and the second magnet installation location of the execution side is 1:1, and the driving side and the execution side use the same non-contact magnetic rotary encoder, so in the absence of a gap, the increments of the digital codes on the driving side and the execution side are consistent, that is, the driving side changes several digital quantities, and the execution side also changes the same digital quantities, which is the "one-to-one correspondence between the digital codes of the two sets of encoders" described above. The method of monitoring the increment is not limited by the mechanical installation error of the sensor and the mechanical transmission error. As long as the increments on the driving side and the execution side are consistent, the gap is considered to be eliminated. Similarly, due to the existence of mechanical gaps, the increments on the execution side may be inconsistent with those on the driving side, so the introduction of the correction process can minimize the error of the transmission mechanism, thereby improving the accuracy of motion control.

[0068] The processor can be integrated into the surgical robot body without restrictions on specific specifications and models; the drive side and the execution side are isolated through magnets and non-contact magnetic rotary encoders, and the speeds of both are detected at the same time. The mechanical backlash of the transmission mechanism is eliminated through software algorithms to minimize the error of the transmission mechanism and improve motion control accuracy.

[0069] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A motor closed-loop control device for a surgical robot, characterized in that: It includes an execution side connected to the base, the execution side is located at the top of the driving side, and the execution side is detachably connected to the driving side. The driving side output mechanism is provided with a first magnet, and a first sensor is provided at a position corresponding to the first magnet in the base; a second magnet is provided at the axis of the driven wheel of the execution side, and a second sensor is installed at a position corresponding to the second magnet in the base, and the two sets of sensors respectively obtain the rotation speed output by the driving side and the rotation speed output by the execution side; The execution side includes a driving wheel and a driven wheel installed in the housing, the driving wheel is connected to the rotating shaft, the output shaft of the driving side output mechanism is detachably connected to the rotating shaft, and transmits the power of the driving side, the rotating shaft drives the driving wheel to move, the driving wheel is meshed with the driven wheel, and the driven wheel outputs the power of the driving side; A first magnet is installed on the output shaft of the drive side output mechanism; a first sensor is located in the base at a position corresponding to the first magnet; the first sensor obtains the rotation speed of the drive side output under the influence of the first magnet; A second magnet is provided at the axis of the driven wheel. When the execution side is located at the top of the driving side, the second sensor is located at a position corresponding to the second magnet in the base. The second sensor obtains the rotation speed output by the execution side driven wheel under the influence of the second magnet. The driving side includes a motor connected to the base and a driving side output mechanism, and the motor is connected to the driving side output mechanism to output power to drive the end effector of the surgical robot to move; The execution side also has an end effector, and the driven wheel is connected to the end effector; The installation status of the execution side is determined in real time based on the distance between the second magnet and the second sensor.

2. The motor closed-loop control device of a surgical robot according to claim 1, characterized in that: The first sensor and the second sensor are both non-contact magnetic rotary encoders.

3. A method for realizing a motor closed-loop control of a surgical robot based on the device of claim 1, characterized in that: The following steps are involved: Obtaining the speed data of the output of the driving side and the speed data of the driven wheel of the execution side; The filter determines whether the speed exceeds the threshold. If it exceeds the threshold, the motor is controlled to rotate to the set position after the gap elimination algorithm.

4. The control method according to claim 3, characterized in that: The gap elimination algorithm comprises the following steps: The processor cyclically acquires the filtered rotary encoder data of the driving side and the execution side at a set period; Compare the sensor data on the drive side and the actuator side to obtain the mechanical idle stroke of the motor transmission gap; Use mechanical lost motion as a correction factor for this operation to eliminate backlash.

5. The control method according to claim 4, characterized in that: Specifically, during the gap elimination process, the sum of the set value of the driving side motor rotation angle and the correction factor is used as the execution side target rotation angle.

Images