KR102116119B1 - Master robot and control method thereof - Google Patents

Abstract

The present invention relates to a master robot that can be operated smoothly by an operator while preventing gimbal lock, and a control method thereof.
According to the present invention, a method of determining a proportion to the first angle and the second angle is used instead of using a method of projecting a Jacobian to a null-space in order to determine an allowance angle. , It can solve the problem of falling into the gimbal by losing the degree of freedom in direction in a specific direction. Further, according to the present invention, when the third angle approaches 90 degrees or -90 degrees, the clearance angle is determined to be proportional to the first angle, so that the operator can operate the handle smoothly.

Description

Master robot and control method thereof

The present invention relates to a master robot that can be operated smoothly by an operator while preventing gimbal lock, and a control method thereof.

Medically, surgery refers to healing of the skin, mucous membranes, or other tissues by cutting, slitting or manipulating them using a medical machine. Particularly, open surgery to open and cut the skin on the surgical site to treat, shape, or remove the organs in the surgery area has recently been performed using robots due to problems such as bleeding, side effects, patient pain, and scars. It is spotlighted as an alternative.

The surgical robot may be divided into a master robot that generates and transmits a signal required by a doctor's manipulation, and a slave robot that receives a signal from the manipulation unit and directly applies a surgical operation to a patient. The robot and the slave robot may be classified as parts of one surgical robot, or may be configured as separate devices and disposed in the operating room.

The master robot for surgery is equipped with a device for doctor's manipulation. In the case of robotic surgery, the surgeon does not directly manipulate the instruments necessary for surgery, but manipulates the above-described devices to perform various operations required for surgery on the robot. Do it.

The master device, which is usually installed on the master robot, consists of a handle that the doctor holds by hand and moves, and a master arm that mediates the handle and the robot body, and the master arm handles when the doctor holds the handle and performs operations such as movement and rotation. It is composed of an arm in the form of a multi-joint link connecting the robot body and the handle to support the movement of the robot.

On the other hand, the master arm is designed to be able to move a sufficient range than the range of movement of the wrist of the operator. To this end, a free joint is additionally configured in the basic joint. The slack joint moves by following the operator's wrist movement at the proper position and speed.

In order to improve the performance of the surgical robot, a method for determining the follow angle (hereinafter, the angle of clearance) of the marginal joint that secures the operator's wrist movement range in the master arm, does not give resistance, and does not suffer from gimbal-lock Is required.

The problem to be solved by the present invention is to provide a method for determining a clearance angle to prevent gimbal from a master robot composed of a plurality of robot arms.

Another problem to be solved by the present invention is to provide a method for determining a clearance angle so as not to interfere with an operator's movement.

The problems of the present invention are not limited to the problems mentioned above, and other problems not mentioned will be clearly understood by those skilled in the art from the following description.

As a means for solving the above-described technical problem, the master robot according to an embodiment includes a lower arm mounted on a base portion to be rotatable with a yaw axis; A middle arm mounted on the lower arm to be rotatable on a pitch axis; An upper arm mounted on the intermediate arm to be rotatable with the yaw axis; A handle mounted to the upper arm to be rotatable with a roll shaft; And the lower arm from a first angle at which the handle rotates relative to the upper arm, a second angle at which the upper arm rotates relative to the intermediate arm, and a third angle at which the intermediate arm rotates relative to the lower arm. It includes a control unit for determining a fourth angle of rotation relative to the base portion.

In the master robot according to an embodiment, when the third angle has a value similar to 0 degrees, the controller determines the fourth angle from the second angle.

In the master robot according to an embodiment, when the third angle has a value similar to 90 degrees or -90 degrees, the controller determines the fourth angle from the first angle.

In a master robot according to an embodiment, the control unit may include: a first angular velocity of the angular velocity of the angular velocity at which the handle rotates with respect to the upper arm and a second axial velocity of the angular velocity at which the upper arm rotates about the intermediate arm; The fourth angle is determined from the angular velocity.

In the master robot according to an embodiment, the control unit determines the fourth angle to be proportional to the first angular velocity and the second angular velocity.

In a master robot according to an embodiment, the control unit determines the fourth angle to be proportional to the first angle and the second angle.

In the master robot according to an embodiment, the control unit determines the fourth angle to be proportional to the second angle with a greater weight than the first angle as the third angle approaches 0 degree. The fourth angle is determined such that the third angle approaches 90 degrees or -90 degrees and the weight is proportional to the first angle than the second angle.

In the master robot according to an embodiment, the control unit determines the fourth angle θ ₄ by the following equation,

(Mathematics)

θ ₄ = k ₁ * W ₁ * θ ₁ + k ₂ * W ₂ * θ ₂

In the above equation, θ ₁ is the first angle, θ ₂ is the second angle, k ₁ is a proportionality constant for adjusting the ratio of the fourth angle to the first angle, k ₂ Is a proportionality constant for adjusting the ratio of the fourth angle to the second angle, W ₁ is a weight for the first angle, and W ₂ is a weight for the second angle.

In the master robot according to an embodiment, as the third angle approaches 0 degrees, the W ₁ has a value closer to 0, the W ₂ has a value closer to 1, and the third angle is 90 degrees or As it approaches -90 degrees, W ₁ has a value closer to 1 and W ₂ has a value closer to 0.

In the master robot according to an embodiment, the W ₂ is determined as a cos function for the third angle, and the W ₁ is determined such that the sum of the W ₂ is 1.

In the master robot according to an embodiment, the W ₂ is determined as a polynomial function for the third angle, and the W ₁ is determined such that the sum of the W ₂ is 1.

In a master robot according to an embodiment, a slave arm on which a surgical tool is mounted is steered according to the manipulation of the handle.

In the control method of the master robot according to an embodiment, the master robot is mounted on the lower arm (lower arm) mounted on the base portion to be rotatable on the yaw axis, and the middle arm mounted on the lower arm to be rotatable on the pitch axis arm), an upper arm mounted to the intermediate arm to be rotatable with the yaw axis, and a handle mounted to the upper arm to be rotatable with a roll axis, wherein the handle is rotated relative to the upper arm Obtaining an angle; Obtaining a second angle at which the upper arm rotates relative to the intermediate arm; Obtaining a third angle at which the intermediate arm rotates relative to the lower arm; Determining a fourth angle at which the lower arm rotates with respect to the base portion from the first angle, the second angle, and the third angle; And controlling the lower arm to rotate relative to the base portion at the determined fourth angle.

In the control method of the master robot according to an embodiment, the fourth angle is determined from the second angle when the third angle has a value similar to 0 degrees, the control method of the master robot.

In the control method of the master robot according to an embodiment, the fourth angle is determined from the first angle when the third angle has a value similar to 90 degrees or -90 degrees.

In the control method of the master robot according to an embodiment, the fourth angle is a first angular velocity that is a yaw axis component of an angular velocity at which the handle rotates with respect to the upper arm and an angular velocity at which the upper arm rotates with respect to the intermediate arm. It is determined from the second angular velocity which is the yaw axis component.

In the control method of the master robot according to an embodiment, the fourth angle is determined to be proportional to the first angular velocity and the second angular velocity.

In the control method of the master robot according to an embodiment, the fourth angle is determined to be proportional to the first angle and the second angle.

In the control method of the master robot according to an embodiment, the fourth angle is proportional to the third angle as the second angle approaches 0 degrees, with a greater weight to the second angle than the first angle. It is determined that the closer the angle is to 90 degrees or -90 degrees, the more the first angle is weighted and proportional to the second angle.

In a master robot according to another embodiment, the lower arm (lower arm) mounted to the base portion to be rotatable in the yaw axis; A middle arm mounted on the lower arm to be rotatable on a pitch axis; An upper arm mounted on the intermediate arm to be rotatable with the yaw axis; A handle mounted to the upper arm to be rotatable with a roll shaft; And the lower arm rotates with respect to the base portion from a first angular velocity that is a yaw axis component of an angular velocity at which the handle rotates with respect to the upper arm and a second angular velocity that is a yaw axis component of an angular velocity at which the upper arm rotates with respect to the intermediate arm. It includes a control unit for determining the fourth angle.

In a master robot according to another embodiment, the control unit determines the fourth angle to be proportional to the first angular velocity and the second angular velocity.

According to the present invention, a method of determining a proportion to the first angle and the second angle is used instead of using a method of projecting a Jacobian to a null-space in order to determine an allowance angle. , It can solve the problem of falling into the gimbal by losing the degree of freedom in direction in a specific direction.

Further, according to the present invention, when the third angle approaches 90 degrees or -90 degrees, the clearance angle is determined to be proportional to the first angle, so that the operator can operate the handle smoothly.

The effects according to the present invention are not limited by the contents exemplified above, and more various effects are included in the present specification.

1 is a plan view showing the overall structure of a surgical robot according to an embodiment of the present invention.
Figure 2 shows a master robot according to an embodiment of the present invention.
FIG. 3 is a diagram briefly expressing each component and its connection relationship in the master robot illustrated in FIG. 2.
4 is a view showing a gimbal lock state.
5 and 6 are views showing a state in which the third angle is 90 degrees or -90 degrees.
7 and 8 are flowcharts illustrating an example of a method in which the control unit determines a clearance angle (fourth angle).
9 is a flowchart illustrating another example of a method in which the control unit determines a clearance angle (fourth angle).

Hereinafter, exemplary embodiments will be described in detail with reference to the accompanying drawings. It goes without saying that the following description is only for the purpose of embodying the embodiments and does not limit or limit the scope of the invention. From the detailed description and examples, what can be easily inferred by experts in the art is interpreted as belonging to the scope of rights.

As used herein, terms such as 'consist of' or 'comprises' are not to be construed as including all of the various components or various steps described in the specification, and some components or some of them. It should be construed that they may not be included, or may further include additional components or steps.

The terms used in the present specification have selected general terms that are currently widely used as possible while considering functions in the present invention, but this may be changed according to the intention or precedent of a person skilled in the art or the appearance of new technologies. In addition, in certain cases, some terms are arbitrarily selected by the applicant, and in this case, their meanings will be described in detail in the description of the applicable invention. Therefore, the terms used in the present invention should be defined based on the meanings of the terms and the contents of the present invention, not simply the names of the terms.

1 is a plan view showing the overall structure of a surgical robot according to an embodiment of the present invention.

Referring to FIG. 1, the surgical robot 1 allows the operator O to remotely control the slave robot 10 and the slave robot 10 performing surgery on the patient P lying on the operating table 2. It includes a master console (20). In addition, the surgical robot 1 may include a vision cart 30. Through the display unit 35 of the vision cart 30, the assistant A can check the progress of the surgery.

The slave robot 10 may include one or more robot arms 11. In general, a robot arm refers to a device having a function similar to a human arm and / or wrist and capable of attaching a predetermined tool to the wrist. In this specification, the robot arm 11 may be defined as a concept encompassing all components such as upper arm, lower arm, wrist, and elbow, and surgical instruments coupled to the wrist. As such, the robot arm 11 of the slave robot 10 may be implemented to be driven with multiple degrees of freedom. The robot arm 11 is, for example, a surgical instrument that is inserted into the surgical site of the patient P, a transport drive unit that moves the surgical instrument in the longitudinal direction, a rotary drive unit that rotates the surgical instrument, and is installed at the end of the surgical instrument to perform surgery It may be configured to include a surgical instrument drive for cutting or cutting the lesion. However, the configuration of the robot arm 11 is not limited to this, it should be understood that these examples do not limit the scope of the present invention. Here, the detailed description of the actual control process such as the robot arm 11 rotates and moves in the corresponding direction by the operator O operating the operation lever is omitted.

The slave robot 10 may be used as one or more to operate the patient P, and the surgical tool 12 for allowing the surgical site to be displayed as an image image through a display member (not shown) is an independent slave robot. (10).

In addition, as described above, embodiments of the present invention can be used universally for surgery using various surgical endoscopes (eg, thoracoscopy, arthroscopy, parenteral, etc.) other than laparoscopic.

The master console 20 and the slave robot 10 do not necessarily need to be separated into separate physically independent devices, and can be integrated into one and configured integrally.

The master console 20 includes a master robot (100 in FIG. 2) and a display member (not shown). In addition, the master console 20 may further include an external display device 25 capable of displaying the state of the operator O on the outside.

In detail, the master console 20 is equipped with a master robot so that the operator O can operate. The master robot may be implemented with one or more handles, and an operation signal according to the handle manipulation of the operator O is transmitted to the slave robot 10 through a wired or wireless communication network, and the robot arm 11 is controlled. do. That is, a surgical operation such as a position movement, rotation, or cutting operation of the robot arm 11 may be performed by operating the handle of the operator O.

For example, the operator O can operate the slave robot arm 11 or the surgical tool 12 using a handle-type operation lever. Such an operation lever may have various mechanical configurations according to its operation method, a master handle for operating operations such as a slave robot arm 11 or a surgical tool 12, and a master for operating functions of the entire system. Various types of input tools such as joysticks, keypads, trackballs, and touch screens added to the console 20 may be provided in various forms for operating the robot arm 11 and / or other surgical equipment of the slave robot 10. have.

The image captured through the surgical tool 12 is displayed on the display member of the master console 20 as an image image. In addition, a predetermined virtual operation panel may be displayed on the display member together with an image photographed through the surgical tool 12 or may be displayed independently.

The display member may be provided in various forms in which the operator O can check the image. For example, a display device may be installed to correspond to both eyes of the operator O. As another example, one or more monitors may be configured, and information necessary for surgery may be individually displayed on each monitor. The number of display members may be variously determined according to the type or type of information requiring display.

The vision cart 30 is installed spaced apart from the slave robot 10 or the master console 20, and the progress of the operation can be checked from the outside through the display unit 35.

The image displayed on the display unit 35 may be the same as the image displayed on the display member of the operator O. The assistant A may assist the operator O with the surgical operation while checking the image of the display unit 35. For example, the assistant A can replace the surgical tool 12 in the instrument cart 3 according to the progress of the surgery.

The central control unit 40 is connected to the slave robot 10, the master console 20 and the vision cart 30 to transmit and receive each signal.

Figure 2 shows a master robot according to an embodiment of the present invention. The master robot according to an embodiment includes a lower arm (140), a middle arm (130), an upper arm (upper arm 120), a handle 110, and a control unit (not shown).

On the roll axis (y), the pitch axis (x), and the yaw axis (z) of the inertial coordinate system for analyzing the movement of the master robot 100, the lower arm 140 is mounted on the base portion 200 so as to be rotatable with the yaw axis do. In addition, the middle arm 130 is mounted to the lower arm 140 to be rotatable with a pitch axis. In addition, the upper arm 120 is mounted on the intermediate arm 130 to be rotatable with the yaw axis. In addition, the handle 110 is mounted to the upper arm 120 to be rotatable with a roll axis.

3 is a diagram briefly expressing each component and a connection relationship in the master robot 100 shown in FIG. 2.

3, one end 140a of the lower arm is connected to the base portion 200, one end 130a of the middle arm is connected to the other end 140b of the lower arm, and one end 120a of the upper arm is It is connected to the other end 130b of the middle arm, and the handle 110 is connected to the other end 120b of the upper arm.

2 and 3 are views showing the initial posture of the master robot 100, the handle 110, the upper arm 120, the middle arm 130, and the lower arm 140 of the master robot 100 Since they are configured to have rotational degrees of freedom with respect to each other, the posture of the master robot 100 can be changed as the operator operates the handle 110.

For example, the operator can rotate the handle 110 by a roll axis, a yaw axis, and a pitch axis. When the handle 110 is rotated by a roll axis, the handle 110 is rotated by a first angle θ1 with respect to the upper arm 120. ), And when the handle 110 is rotated on the yaw axis, the upper arm 120 rotates by a second angle θ2 with respect to the middle arm 130, and when the handle 110 is rotated on the pitch axis, the middle The arm 130 is rotated by the third angle θ3 with respect to the lower arm 140.

At this time, in order to extend the range that the operator can operate with the handle 110, the control unit determines the clearance angle (fourth angle) so that the lower arm 140 rotates relative to the base unit 200, and the determined fourth angle ( The lower arm 140 is controlled to rotate relative to the base portion 200 with θ4).

Here, the signs of the first angle θ1, the second angle θ2, and the fourth angle θ4 are all set to be the same when rotated in the same direction with respect to the roll axis or the yaw axis based on the initial posture. The third angle θ3 may also be set to have the same sign for the same rotation direction as other angles.

For example, based on the initial posture, when the handle 110 rotates clockwise with respect to the roll axis, the first angle θ1 has a positive value, and the upper arm 120 rotates clockwise with respect to the yaw axis. 2 The angle θ2 has a positive value, and when the lower arm 140 rotates clockwise with respect to the yaw axis, the fourth angle θ4 has a positive value. In this case, the clockwise direction means a direction in which the fingers of the right hand are wound when the thumb of the right hand is placed in the direction that each axis is facing.

Since the controller must be able to perform basic logic operations, it is possible to use semiconductor devices capable of logic operations such as central processing unit (CPU), micro controller unit (MCU), microprocessor, field programmable gate array (FPGA), and application specific integrated circuit (ASIC). However, it is not limited thereto.

Conventionally, in order to calculate the clearance angle, a method of projecting Jacobian, which is a derivative mapping relationship between a Cartesian space velocity and a joint space velocity, into a null-space was used. This method is generally used well in robotics, but has a large amount of computation, and has a problem of falling into a gimbal that loses directional freedom in a specific direction as shown in FIG. 4.

In addition, the conventional method, as shown in Figures 5 and 6, when the middle arm 130 rotates about 90 or -90 degrees relative to the lower arm 140, there is a problem that the operator feels resistance to operate the handle 110 have.

In the present invention, the control unit determines the clearance angle, that is, the fourth angle θ4, so that the operator can smoothly operate the handle 110 without generating gimbal in the master robot 100. Hereinafter, the master shown in FIG. 2 Based on the configuration of the robot 100 looks at how to determine the clearance angle.

7 is a flowchart illustrating an example of a method in which the control unit determines a clearance angle (fourth angle).

First, a first angle θ1 that is a rotation angle with respect to the upper arm 120 of the handle 110, a second angle θ2 that is a rotation angle with respect to the middle arm 130 of the upper arm 120, and a middle arm A third angle θ3, which is a rotation angle with respect to the lower arm 140 of 130, is obtained (S11, S12, S13).

At this time, the rotation angle can be detected by the encoder. Various types of encoders, such as incremental encoders, absolute encoders, magnetic encoders, and potentiometers, can be used for angle detection.

Next, the fourth angle θ4 is determined from the obtained first angle θ1, second angle θ2, and third angle θ3 (S14). The lower arm 140 may be controlled to rotate at the determined fourth angle θ4 (S15).

Referring to FIG. 8 to look at step 14 (S14) in more detail, the control unit determines whether the third angle θ3 has a value similar to 0 degrees (S141). Whether it has a value similar to 0 degree can be determined as whether the third angle θ3 is within the range of -a degree to + a degree, where a is any constant having a value of 0 or its vicinity.

If the third angle θ3 has a value similar to 0 degrees, it is determined that the fourth angle θ4 is proportional to the second angle θ2 (S142). At this time, the fourth angle θ4 is determined to have the same sign as the second angle θ2.

If the third angle θ3 does not have a value similar to 0 degrees, it is determined whether the third angle θ3 has a value similar to 90 degrees or -90 degrees (S143). Whether the third angle θ3 has a value similar to 90 degrees or -90 degrees may be determined in a similar manner to step 141 (S141).

If the third angle θ3 has a value similar to 90 degrees or -90 degrees, it is determined that the fourth angle θ4 is proportional to the first angle θ1 (S144). At this time, the fourth angle θ4 is determined to have the same sign as the first angle θ1.

As such, when the third angle θ3 has a value similar to 90 degrees or -90 degrees, that is, when the master robot 100 is operated in the posture shown in FIGS. 5 and 6, the clearance angle (fourth angle) If is determined to be proportional to the first angle (θ1) rather than the second angle (θ2), even if the posture of the master robot 100 changes, the clearance angle (fourth angle) is determined from the rotational component about the yaw axis. It is possible to operate the master robot 100 smoothly.

If the third angle θ3 does not have a value similar to 90 degrees or -90 degrees, it is determined that the fourth angle θ4 is proportional to both the first angle θ1 and the second angle θ2 (S145). At this time, as the third angle θ3 has a value close to 0, the fourth angle θ4 is determined by increasing the weight of the second angle θ2 rather than the first angle θ1, and the third angle θ3 As the value has a value close to 90 degrees or -90 degrees, the fourth angle θ4 may be determined by increasing the weight of the first angle θ1 rather than the second angle θ2.

A method of determining the clearance angle (fourth angle) by the controller according to the above description may be expressed by the following equation (1).

(Equation 1)

θ4 = k1 * W1 * θ1 + k2 * W2 * θ2

In Equation 1, k1 is a proportionality constant for adjusting the ratio of the fourth angle θ4 to the first angle θ1, and k2 is the ratio of the fourth angle θ4 to the second angle θ2. The proportional constant for doing so, W1 represents the weight for the first angle θ1, and W2 represents the weight for the second angle θ2. Also, k1, k2, W1, and W2 are positive numbers.

W1 and W2 are determined by the third angle θ3, and in detail, as the third angle θ3 approaches 0 degree, W1 has a value close to 0, W2 has a value close to 1, and a third angle As (θ3) approaches 90 degrees or -90 degrees, W1 may be determined to have a value close to 1 and W2 to a value close to 0.

For example, the relationship between W1, W2, and the third angle θ3 may be determined by Equation 2 below.

(Equation 2)

W2 = k * cos (θ3)

W1 = 1-W2

As another example, the relationship between W1, W2, and the third angle θ3 may be determined by Equation 3 below.

(Equation 3)

+ bn-1*(θ3) + bnW2 = b0 * (θ3) n + b1 * (θ3) n-1 +

+ bn-1 * (θ3) + bn

W1 = 1-W2

When the clearance angle (fourth angle) is determined using Equations 1 to 3, as the third angle θ3 approaches 0 degree, the second angle θ2 for determining the fourth angle θ4 The specific gravity increases, and as the third angle θ3 approaches 90 degrees or -90 degrees, the specific gravity of the first angle θ1 for determining the fourth angle θ4 increases.

Therefore, as the third angle θ3 approaches 90 degrees or -90 degrees, that is, when the master robot 100 is operated in the posture shown in FIGS. 5 and 6, the specific gravity of the second angle θ2 is lowered and Since the specific gravity is increased by increasing the specific gravity of the first angle θ1, the operator can smoothly operate the master robot 100.

9 is a flowchart illustrating another example of a method in which the control unit determines the clearance angle (fourth angle).

First, the first angular velocity component w1 of the yaw axis angular velocity component at the rotational angular velocity with respect to the upper arm 120 of the handle 110 and the second angular velocity component of the yaw axis angular velocity component at the rotational angular velocity of the upper arm 120 with respect to the middle arm 130 (w2) is obtained (S21, S22).

At this time, the first angular velocity and the second angular velocity may be calculated using a rotation angle detected by the encoder or may be obtained from a value sensed by the inertial sensor.

Next, the fourth angle θ4 is determined from the obtained first angular velocity w1 and second angular velocity w2 (S23). The lower arm 140 may be controlled to rotate at the determined fourth angle θ4 (S24).

The fourth angle may be determined to be proportional to the first angular velocity and the second angular velocity, for example, by Equation 4 below.

(Equation 4)

θ4 = k3 * w1 + k4 * w2

In Equation 4, k3 is a proportionality constant for adjusting the ratio of the fourth angle θ4 to the first angular velocity w1, and k4 is the ratio of the fourth angle θ4 to the second angular velocity w2. It shows proportional constant for doing.

When the clearance angle (fourth angle) is determined according to the above method and the lower arm is rotated at the determined clearance angle (fourth angle), a method of projecting Jacobian to null-space is used. Unlike the conventional method, it can be free from the problem of falling into a gimbal lock.

Although the embodiments have been described in detail above, the scope of the present invention is not limited thereto, and various modifications and improvements of those skilled in the art using the basic concept of the present invention defined in the following claims are also provided. Belongs to

100: master robot 110: handle
120: upper arm 130: middle arm
140: lower arm 200: base

Claims (21)

A lower arm mounted on the base portion to be rotatable with a yaw axis;
A middle arm mounted on the lower arm to be rotatable on a pitch axis;
An upper arm mounted on the intermediate arm to be rotatable with the yaw axis;
A handle mounted to the upper arm to be rotatable with a roll shaft; And
The lower arm is from the first angle at which the handle rotates relative to the upper arm, the second angle at which the upper arm rotates relative to the intermediate arm, and the third angle at which the intermediate arm rotates relative to the lower arm. And a control unit for determining a fourth angle of rotation with respect to the base unit. According to claim 1,
The control unit,
When the third angle has a value similar to 0 degrees, the master robot determines the fourth angle from the second angle. According to claim 1,
The control unit,
When the third angle has a value similar to 90 degrees or -90 degrees, the master robot determines the fourth angle from the first angle. According to claim 1,
The control unit,
The master robot determines the fourth angle from the first angular velocity, which is the yaw axis component of the angular velocity at which the handle rotates relative to the upper arm, and the second angular velocity, which is the yaw axis component of the angular velocity at which the upper arm rotates relative to the intermediate arm. The method of claim 4,
The control unit,
The master robot determines the fourth angle to be proportional to the first angular velocity and the second angular velocity. According to claim 1,
The control unit,
The master robot determines the fourth angle to be proportional to the first angle and the second angle. The method of claim 6,
The control unit,
The fourth angle is determined so as to be proportional to the second angle with a greater weight than the first angle as the third angle approaches 0 degree,
The master robot determines that the fourth angle is proportional to the third angle by placing a greater weight on the first angle than the second angle as the third angle approaches 90 degrees or -90 degrees. The method of claim 6,
The control unit,
The fourth angle θ ₄ is determined by the following equation,
(Mathematics)
θ ₄ = k ₁ * W ₁ * θ ₁ + k ₂ * W ₂ * θ ₂
In the above equation, θ ₁ is the first angle, θ ₂ is the second angle, k ₁ is a proportionality constant for adjusting the ratio of the fourth angle to the first angle, k ₂ Is a proportionality constant for adjusting the ratio of the fourth angle to the second angle, W ₁ is a weight for the first angle, and W ₂ is a weight for the second angle, the master robot . The method of claim 8,
As the third angle approaches 0 degree, W ₁ has a value close to 0, and W ₂ has a value close to 1,
The third angle is 90 ° or closer to 90 degrees, a value close to the W ₁ is 1, the W ₂ is having a value close to 0, the master robot. The method of claim 9,
The W ₂ is determined by the cos function for the third angle,
The W ₁ is determined to be the sum of W ₂ and 1, the master robot. The method of claim 9,
The W ₂ is determined by the polynomial function for the third angle,
The W ₁ is determined to be the sum of W ₂ and 1, the master robot. According to claim 1,
A master robot in which a slave arm equipped with a surgical tool is steered according to the operation of the handle. In the control method of the master robot,
The master robot is mounted on the lower arm mounted on the base to be rotatable on the yaw axis, the middle arm mounted on the lower arm to be rotatable on the pitch axis, and on the intermediate arm to be rotatable on the yaw axis The upper arm is mounted, and includes a handle mounted to the upper arm so as to be rotatable with a roll shaft,
Obtaining a first angle at which the handle rotates relative to the upper arm;
Obtaining a second angle at which the upper arm rotates relative to the intermediate arm;
Obtaining a third angle at which the intermediate arm rotates relative to the lower arm;
Determining a fourth angle at which the lower arm rotates with respect to the base portion from the first angle, the second angle, and the third angle; And
And controlling the lower arm to rotate with respect to the base portion at the determined fourth angle. The method of claim 13,
The fourth angle, when the third angle has a value similar to 0 degrees, is determined from the second angle, the control method of the master robot. The method of claim 13,
The fourth angle is determined from the first angle when the third angle has a value similar to 90 degrees or -90 degrees, the control method of the master robot. The method of claim 12,
The fourth angle is determined from the first angular velocity of the yaw axis component of the angular velocity at which the handle rotates with respect to the upper arm and the second angular velocity of the yaw axis component of the angular velocity at which the upper arm rotates about the intermediate arm. Control method. The method of claim 16,
The fourth angle is determined to be proportional to the first angular velocity and the second angular velocity, the control method of the master robot. The method of claim 13,
The fourth angle is determined to be proportional to the first angle and the second angle, the control method of the master robot. The method of claim 18,
The fourth angle is proportional to the third angle closer to 0 degrees with a greater weight to the second angle than to the first angle, so that the third angle is closer to 90 degrees or -90 degrees. A master robot, which is determined to be proportional to the first angle with a greater weight than the second angle. A lower arm mounted on the base portion to be rotatable with a yaw axis;
A middle arm mounted on the lower arm to be rotatable on a pitch axis;
An upper arm mounted on the intermediate arm to be rotatable with the yaw axis;
A handle mounted to the upper arm to be rotatable with a roll shaft; And
The lower arm rotates with respect to the base portion from a first angular velocity that is a yaw axis component of an angular velocity at which the handle rotates with respect to the upper arm and a second angular velocity that is a yaw axis component of an angular velocity at which the upper arm rotates with respect to the intermediate arm. And a control unit for determining a fourth angle. The method of claim 20,
The control unit,
The master robot determines the fourth angle to be proportional to the first angular velocity and the second angular velocity.

Images