A robot control system, method and robot

Abstract

The application discloses a robot control system, method and robot, and the robot control system is used for controlling a controlled joint of the robot, and the system comprises a position loop controller, a speed loop controller, an encoder and a motor driver, wherein: the encoder is arranged on the controlled joint, is used for determining an actual position of the controlled joint, and transmits the actual position to the speed loop controller; the position loop controller is used for determining a desired rotating speed according to a planned position and a planned speed, and transmitting the desired rotating speed to the speed loop controller; the speed loop controller is used for outputting a speed compensation parameter according to the actual position and the desired rotating speed, and transmitting the speed compensation parameter to the motor driver; and the motor driver is used for driving a motor of the controlled joint to rotate based on the speed compensation parameter. The motor of the controlled joint of the surgical arm is controlled to move only by the cooperation of the position loop controller and the speed loop controller, and the response speed of the control system is improved.

Description

Technical Field

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

[0002] Surgical robot technology has developed rapidly in recent years, and the functions of surgical robots have gradually improved. The current development trend is to optimize the basic functions based on the existing functions, so that the clinical use of surgical robots is more stable and controllable. Precise control of surgical robots is the core technology of surgical robots.

[0003] In the process of realizing this invention, it was found that at least the following technical problems exist in the prior art: when controlling the controlled joint motor of the surgical arm, the multi-port laparoscopic surgical robot in the industry generally uses the position loop, speed loop and current loop connected in series to control the robot, which results in a slow response speed. Summary of the Invention

[0004] This invention provides a robot control system, method, and robot to solve the technical problem of slow control response speed of controlled joints in existing robots, improve the control response speed of controlled joints, and thus achieve precise control of the robot.

[0005] According to one aspect of the present invention, a robot control system is provided for controlling the controlled joints of the robot, the system comprising a position loop controller, a velocity loop controller, an encoder, and a motor driver, wherein:

[0006] The encoder is mounted on the controlled joint and is used to determine the actual position of the controlled joint and transmit the actual position to the speed loop controller.

[0007] The position loop controller is used to determine the desired rotational speed based on the planned position and planned speed, and transmit the desired rotational speed to the speed loop controller;

[0008] The speed loop controller is used to output speed compensation parameters according to the actual position and the desired rotational speed, and transmit the speed compensation parameters to the motor driver;

[0009] The motor driver is used to drive the motor of the controlled joint to rotate based on the speed compensation parameters.

[0010] Optionally, based on the above scheme, the encoder includes an external absolute encoder and an incremental encoder, and determining the actual position of the controlled joint includes:

[0011] Obtain the first position of the external absolute encoder and the second position of the incremental encoder;

[0012] The actual position is determined based on the first position and the second position.

[0013] Optionally, based on the above scheme, the step of compensating for the output speed according to the actual position and the desired rotational speed includes:

[0014] Based on the actual position, speed compensation is performed with the desired rotational speed as a reference, and the speed compensation parameters are output.

[0015] Optionally, based on the above scheme, the step of performing speed compensation based on the actual position and using the desired rotational speed as a reference, and outputting the speed compensation parameters, includes:

[0016] Based on the actual position, the acceleration compensation value and the actual speed are output as the speed compensation parameters, with the desired rotational speed as the reference.

[0017] Optionally, based on the above scheme, the planned location is determined according to the actual location and the planned path.

[0018] Optionally, based on the above scheme, the planned speed is determined according to the actual location and the actual speed.

[0019] Optionally, based on the above scheme, the position loop controller is located in the outer loop, and the speed loop controller is located in the inner loop.

[0020] According to another aspect of the present invention, a robot control method is provided for controlling the controlled joints of the robot, the method comprising:

[0021] The actual position of the controlled joint is determined based on the encoder, and the actual position is transmitted to the speed loop controller;

[0022] The position loop controller determines the desired rotational speed based on the planned position and planned speed, and transmits the desired rotational speed to the speed loop controller;

[0023] The speed loop controller outputs speed compensation parameters based on the actual position and the desired rotational speed, and transmits the speed compensation parameters to the motor driver;

[0024] The motor driver drives the motor of the controlled joint to rotate based on the speed compensation parameters.

[0025] Optionally, based on the above scheme, the encoder includes an external absolute encoder and an incremental encoder, and determining the actual position of the controlled joint includes:

[0026] Obtain the first position of the external absolute encoder and the second position of the incremental encoder;

[0027] The actual position is determined based on the first position and the second position.

[0028] According to another aspect of the present invention, a robot is provided, wherein the controlled joints of the robot are controlled by a robot control system provided in any embodiment of the present invention.

[0029] The robot control system provided in this embodiment of the invention is used to control the controlled joints of the robot. The system includes a position loop controller, a speed loop controller, an encoder, and a motor driver. The encoder, mounted on the controlled joint, determines the actual position of the controlled joint and transmits the actual position to the speed loop controller. The position loop controller determines the desired rotational speed based on the planned position and planned speed and transmits the desired rotational speed to the speed loop controller. The speed loop controller outputs speed compensation parameters based on the actual position and desired rotational speed and transmits the speed compensation parameters to the motor driver. The motor driver drives the motor of the controlled joint to rotate based on the speed compensation parameters. By controlling the movement of the controlled joint motor of the surgical arm solely through the coordinated action of the position loop controller and the speed loop controller, the response speed of the control system is improved.

[0030] It should be understood that the description in this section is not intended to identify key or essential features of the embodiments of the present invention, nor is it intended to limit the scope of the invention. Other features of the invention will become readily apparent from the following description. Attached Figure Description

[0031] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0032] Figure 1 This is a schematic diagram of the structure of a robot control system provided in Embodiment 1 of the present invention;

[0033] Figure 2 This is a flowchart illustrating a robot control method provided in Embodiment 2 of the present invention;

[0034] Figure 3 This is a flowchart illustrating a robot control method provided in Embodiment 3 of the present invention. Detailed Implementation

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

[0036] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0037] Example 1

[0038] Figure 1 This is a schematic diagram of a robot control system provided in Embodiment 1 of the present invention. This embodiment is applicable to the control of controlled joints of a robot, especially to the control of controlled joints of the surgical arm of a surgical robot. Figure 1 As shown, the system includes: a position loop controller 120, a speed loop controller 130, an encoder 110, and a motor driver 140, wherein:

[0039] The encoder 110 is disposed on the controlled joint and is used to determine the actual position of the controlled joint and transmit the actual position to the speed loop controller.

[0040] The position loop controller 120 is used to determine the desired rotational speed based on the planned position and planned speed, and transmit the desired rotational speed to the speed loop controller;

[0041] The speed loop controller 130 is used to output speed compensation parameters according to the actual position and the desired rotational speed, and transmit the speed compensation parameters to the motor driver;

[0042] The motor driver 140 is used to drive the motor of the controlled joint to rotate based on the speed compensation parameters.

[0043] To address the slow response speed issue in existing technologies that use a series control loop consisting of a position loop, a velocity loop, and a current loop, this embodiment ignores the innermost current loop and uses only the position loop and velocity loop to control the movement of the robot's surgical arm joint motors. Simultaneously, the actual position of the controlled joint is obtained through an encoder, and control is achieved by combining the actual position with the position and velocity loops. This enables precise control of the controlled joint without using a current loop.

[0044] In one embodiment of the present invention, the position control loop controller is located in the outer loop, and the speed control loop controller is located in the inner loop. The overall control logic is as follows: the outer loop is the position control loop, which takes the desired position (planned position) as its input. The position control loop controller inputs the calculated desired rotational speed to the speed control loop, which is the inner loop. The speed control loop controller inputs the calculated speed compensation parameters to the motor driver, thereby controlling the motor to rotate. The actual rotation position is fed back by the pulse value of the external encoder of the motor, and the position fed back is used as the input of the speed control loop controller. The speed control loop controller performs speed compensation based on the real-time position of the motor fed back by the position control loop, using the desired rotational speed of the motor calculated by the position control loop as a reference. This ensures that the speed of the surgical arm joint motor of the surgical robot moves more smoothly towards the target position, and the external absolute encoder of the motor ensures the accuracy of the movement.

[0045] The planned position is determined based on the actual position and the planned path. The planned position of the controlled joint can be determined based on the actual position of the controlled joint and the pre-planned path. The planned path can be a path planned based on the robot performing a task / action. The planned speed is determined based on the actual position and the actual speed.

[0046] Generally, the encoders that come with robot joint motors are mostly incremental encoders. Incremental encoders use two photosensitive receivers to convert the timing and phase relationship of the angle code disk to obtain the increase (positive direction) or decrease (negative direction) of the angle displacement of the angle code disk. However, after an incremental encoder breaks down and is powered on again, it cannot determine its kinematic zero position, and will lose position information. Its encoder resolution is also low, and the position information is not accurate enough.

[0047] In one embodiment of the present invention, the encoder includes an external absolute encoder and an incremental encoder, and determining the actual position of the controlled joint includes:

[0048] Obtain the first position of the external absolute encoder and the second position of the incremental encoder;

[0049] The actual position is determined based on the first position and the second position.

[0050] To prevent position information loss after power failure and improve position accuracy, an external absolute encoder can be used in conjunction with an incremental encoder. An absolute encoder outputs an absolute value; its encoding is determined by the mechanical position, requiring no memory, no reference point, and no continuous counting. It offers high accuracy and does not lose data after power failure. Using an external absolute encoder in conjunction with an incremental encoder to determine the actual position makes the position determination more accurate.

[0051] When the surgical arm joint moves, the angle by which the controlled joint rotates due to the feedback from the incremental encoder can be used as the second position. The external absolute encoder restores the joint position before the power failure during the robot's power-off and power-on operation as the first position. Combining the first and second positions to determine the actual position can avoid data loss caused by power failure, which would lead to inaccurate position determination. At the same time, the high-precision external absolute encoder can further improve the accuracy of the actual position.

[0052] In one embodiment, the step of compensating for the output speed based on the actual position and the desired rotational speed includes:

[0053] Based on the actual position, speed compensation is performed with the desired rotational speed as a reference, and the speed compensation parameters are output.

[0054] An external absolute encoder on the motor provides pulse value feedback on the actual rotational position, which is then used as input to the speed loop controller. Based on the real-time motor position feedback from the position control loop, the speed loop controller performs speed compensation using the desired motor speed calculated by the position control loop as a reference, outputting speed compensation parameters. This allows the motor driver to control the motor rotation based on these parameters. This ensures smoother movement of the surgical robot's arm joint motor towards the target position, while the external absolute encoder guarantees the accuracy of the motion.

[0055] Optionally, the step of performing speed compensation based on the actual position and using the desired rotational speed as a reference, and outputting the speed compensation parameters, includes:

[0056] Based on the actual position, the acceleration compensation value and the actual speed are output as the speed compensation parameters, with the desired rotational speed as the reference.

[0057] The acceleration compensation value and the actual speed can be determined as speed compensation parameters based on the actual position and the desired rotational speed.

[0058] The robot control system provided in this embodiment of the invention is used to control the controlled joints of the robot. The system includes a position loop controller, a speed loop controller, an encoder, and a motor driver. The encoder, mounted on the controlled joint, determines the actual position of the controlled joint and transmits the actual position to the speed loop controller. The position loop controller determines the desired rotational speed based on the planned position and planned speed and transmits the desired rotational speed to the speed loop controller. The speed loop controller outputs speed compensation parameters based on the actual position and desired rotational speed and transmits the speed compensation parameters to the motor driver. The motor driver drives the motor of the controlled joint to rotate based on the speed compensation parameters. By controlling the movement of the controlled joint motor of the surgical arm solely through the coordinated action of the position loop controller and the speed loop controller, the response speed of the control system is improved.

[0059] Example 2

[0060] Figure 2 This is a flowchart illustrating a robot control method according to Embodiment 2 of the present invention. This embodiment is applicable to situations where the controlled joints of a robot are to be controlled, such as... Figure 2 As shown, the method includes:

[0061] S210. Determine the actual position of the controlled joint based on the encoder, and transmit the actual position to the speed loop controller.

[0062] S220. The position loop controller determines the desired rotational speed based on the planned position and planned speed, and transmits the desired rotational speed to the speed loop controller.

[0063] S230. The speed loop controller outputs speed compensation parameters based on the actual position and the desired rotational speed, and transmits the speed compensation parameters to the motor driver.

[0064] S240. The motor driver drives the motor of the controlled joint to rotate based on the speed compensation parameters.

[0065] In one embodiment of the present invention, the encoder includes an external absolute encoder and an incremental encoder, and determining the actual position of the controlled joint includes:

[0066] Obtain the first position of the external absolute encoder and the second position of the incremental encoder;

[0067] The actual position is determined based on the first position and the second position.

[0068] Optionally, the step of outputting speed compensation parameters based on the actual position and the desired rotational speed includes:

[0069] Based on the actual position, speed compensation is performed with the desired rotational speed as a reference, and the speed compensation parameters are output.

[0070] Based on the above scheme, the step of performing speed compensation based on the actual position and the desired rotational speed, and outputting the speed compensation parameters, includes:

[0071] Based on the actual position, the acceleration compensation value and the actual speed are output as the speed compensation parameters, with the desired rotational speed as the reference.

[0072] Optionally, the planned location is determined based on the actual location and the planned path.

[0073] Optionally, the planned speed is determined based on the actual position and the actual speed.

[0074] In one implementation, the position loop controller is located in the outer loop, and the speed loop controller is located in the inner loop.

[0075] The method provided in this invention determines the actual position of the controlled joint using an encoder and transmits the actual position to a speed loop controller. The position loop controller determines the desired rotational speed based on the planned position and speed and transmits the desired rotational speed to the speed loop controller. The speed loop controller outputs speed compensation parameters based on the actual position and desired rotational speed and transmits the speed compensation parameters to a motor driver. The motor driver drives the motor of the controlled joint to rotate based on the speed compensation parameters. By controlling the movement of the controlled joint motor of the surgical arm solely through the coordinated action of the position loop controller and the speed loop controller, the response speed of the control system is improved.

[0076] Example 3

[0077] This embodiment provides a robot whose controlled joints are controlled by a robot control system provided in any embodiment of the present invention. The structure of the robot provided in this embodiment can refer to existing robot structures and is not limited thereto. For example, when the robot is a multi-port laparoscopic surgical robot, the structure provided in this embodiment can refer to the structure of a multi-port laparoscopic surgical robot, while also including the robot control system provided in any embodiment of the present invention.

[0078] The robot provided in this embodiment of the invention controls the robot joints through the robot control system provided in any embodiment of the invention. By controlling the movement of the controlled joint motors of the surgical arm through the coordinated action of only the position loop controller and the speed loop controller, the response speed of the robot joint control is improved, and the response speed of the robot is further improved.

[0079] Example 4

[0080] Figure 3This is a flowchart illustrating a robot control method according to Embodiment 4 of the present invention. Based on the above embodiments, this embodiment provides a preferred embodiment. Taking a multi-port laparoscopic surgical robot as an example, the control of the robot is described in detail.

[0081] Controlling the position of the surgical arm joint motors in a multi-port laparoscopic surgical robot requires that the surgical arm joint motors maintain both motion accuracy and smoothness during movement to avoid serious consequences caused by loss of control of the surgical arm joint motors.

[0082] like Figure 3 As shown, this embodiment provides a dual closed-loop control method for the position of the surgical arm joint motor in a multi-port laparoscopic surgical robot. The innermost current loop is ignored, and the movement of the surgical arm joint motor is controlled solely by the coordinated action of the position loop and the velocity loop. Since incremental encoders lose their position after power failure and power restoration, and have low resolution, an external absolute encoder is used in conjunction with the incremental encoder for position feedback. During surgical arm joint movement, the incremental encoder can provide feedback on the angle by which the motor drives the surgical arm joint to rotate, while the absolute encoder can restore the joint position before power failure during the power-off and power-back operation of the multi-port laparoscopic surgical robot. Furthermore, the position feedback from the higher-resolution absolute encoder is more accurate than that from the incremental encoder. Moreover, the cascade control based on the position and velocity loops reduces the calculation process of the current loop compared to the traditional three-loop cascade control of position-velocity-current loop, thus improving the response speed of the control system.

[0083] The control process mainly includes: acquiring the actual position of the motor through an external absolute encoder and incremental encoder; generating a planned position and planned speed based on the actual position and the planned path; sending the planned speed and planned position to the position controller; the position controller outputting a speed compensation value (desired rotational speed) based on the planned position and planned speed; the speed controller receiving the speed compensation value, determining the acceleration compensation value and actual speed based on the speed compensation value, and sending the acceleration compensation value and actual speed to the motor driver, so that the motor driver controls the movement of the controlled joint according to the acceleration compensation value and actual speed. This process is repeated cyclically to achieve precise and rapid response control of the joint motor position of the surgical arm of the multi-port laparoscopic surgical robot.

[0084] This embodiment provides a dual closed-loop control method for the position of the surgical arm joint motor in a multi-port laparoscopic surgical robot. It adopts a dual closed-loop control mode of position and speed, omitting the calculation process of the current loop controller, improving the response speed, and using an external absolute encoder for the surgical arm joint motor to ensure the accuracy of the surgical arm joint motor movement, further improving the precision of joint control.

[0085] It should be understood that the various forms of processes shown above can be used, with steps reordered, added, or deleted. For example, the steps described in this invention can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution of this invention can be achieved, and this is not limited herein.

[0086] The specific embodiments described above do not constitute a limitation on the scope of protection of this invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A robot control system, characterized in that, The system is used to control the controlled joints of the robot. It includes a position loop controller, a velocity loop controller, an encoder, and a motor driver, wherein: The encoder is mounted on the controlled joint and is used to determine the actual position of the controlled joint and transmit the actual position to the speed loop controller. The position loop controller is used to determine the desired rotational speed based on the planned position and planned speed, and transmit the desired rotational speed to the speed loop controller; The speed loop controller is used to output speed compensation parameters according to the actual position and the desired rotational speed, and transmit the speed compensation parameters to the motor driver; The motor driver is used to drive the motor of the controlled joint to rotate based on the speed compensation parameters; The encoder includes an external absolute encoder and an incremental encoder. Determining the actual position of the controlled joint includes: Obtain the first position of the external absolute encoder and the second position of the incremental encoder; The actual position is determined based on the first position and the second position; Wherein, the robot is a surgical robot; the controlled joint is a surgical arm joint; The output speed compensation parameters based on the actual position and the desired rotational speed include: Based on the actual position, speed compensation is performed with the desired rotational speed as a reference, and the speed compensation parameters are output. The process of performing speed compensation based on the actual position and using the desired rotational speed as a reference, and outputting the speed compensation parameters, includes: Based on the actual position, the acceleration compensation value and the actual speed are output as the speed compensation parameters, with the desired rotational speed as the reference. The planned location is determined based on the actual location and the planned path; The planned speed is determined based on the actual location and the actual speed.

2. The system according to claim 1, characterized in that, The position loop controller is located in the outer loop, and the speed loop controller is located in the inner loop.

3. A robot control method, characterized in that, The method for controlling the controlled joints of the robot includes: The actual position of the controlled joint is determined based on the encoder, and the actual position is transmitted to the speed loop controller; The position loop controller determines the desired rotational speed based on the planned position and planned speed, and transmits the desired rotational speed to the speed loop controller; The speed loop controller outputs speed compensation parameters based on the actual position and the desired rotational speed, and transmits the speed compensation parameters to the motor driver; The motor driver drives the motor of the controlled joint to rotate based on the speed compensation parameters; The encoder includes an external absolute encoder and an incremental encoder. Determining the actual position of the controlled joint includes: Obtain the first position of the external absolute encoder and the second position of the incremental encoder; The actual position is determined based on the first position and the second position; Wherein, the robot is a surgical robot; the controlled joint is a surgical arm joint; The output speed compensation parameters based on the actual position and the desired rotational speed include: Based on the actual position, speed compensation is performed with the desired rotational speed as a reference, and the speed compensation parameters are output. The process of performing speed compensation based on the actual position and using the desired rotational speed as a reference, and outputting the speed compensation parameters, includes: Based on the actual position, the acceleration compensation value and the actual speed are output as the speed compensation parameters, with the desired rotational speed as the reference. The planned location is determined based on the actual location and the planned path; The planned speed is determined based on the actual location and the actual speed.

4. A robot, characterized in that, The controlled joints of the robot are controlled by the robot control system according to any one of claims 1-2.

Images