Motor torque control method and robot control device

Abstract

This motor torque control method is used in direct teaching of an articulated robot. During direct teaching: a sensor acquires joint angles and joint angular velocities of joints of the articulated robot when a hand tip of a robot arm has moved to an instructed target position; deviation of a hand tip variable of the robot arm and velocity deviation of the hand tip variable are calculated on the basis of the joint angles and the joint angular velocities of the joints acquired by the sensor; a joint torque control target value is calculated for each joint, on the basis of the deviation of the hand tip variable of the robot arm, the velocity deviation of the hand tip variable, and the joint angles of the robot arm, using a numerical formula from which an inertia compensation term has been omitted; and, motor torque of a motor built into each joint is controlled to the calculated joint torque control target value during the direct teaching of the articulated robot.

Description

Motor torque control method and robot control device

[0001] The technology disclosed in this specification relates to direct teaching of an articulated robot.

[0002] One method of teaching an articulated robot is the direct teaching method, in which an operator manually operates the robot and teaches it.

[0003] There are two types of direct teach: joint direct teach and Cartesian direct teach. Joint direct teach is a method of teaching a robot movement by freely moving each joint.

[0004] Cartesian direct teach is a method of teaching an articulated robot to move by freeing one or more of the following coordinates in a Cartesian coordinate space: x, y, z, roll, pitch, and yaw, while fixing the rest.

[0005] For example, the angle (roll, pitch, yaw) of the robot arm's hand is fixed, and only the position (x, y, z) can be moved. Cartesian direct teach makes it possible to specify the direction of movement in Cartesian space. This makes it easy to use for operations such as pick-and-place, where the workpiece's position is changed without changing its orientation. However, it has the disadvantage of calculating excessive commands in the singular orientation of an articulated robot, which can lead to unstable control.

[0006] Direct teaching also includes a method of controlling torque using position as input, and a method of detecting force to control position or speed.

[0007] Patent Documents that disclose techniques related to this subject include Patent Documents 1 and 2.

[0008] JP 2021-62436 A JP 2005-88114 A

[0009] An object of the present invention is to prevent excessive torque from being output near a singular point during direct teaching of an articulated robot.

[0010] A motor torque control method during direct teaching of an articulated robot, comprising: acquiring, by sensors, joint angles and joint angular velocities of each joint of the articulated robot when a hand of a robot arm moves to a teaching target position during direct teaching; calculating a hand variable deviation of the robot arm and a speed deviation of the hand variable based on the joint angle and joint angular velocity of each joint acquired by the sensors; calculating a joint torque control target value of each joint using the following mathematical formula (A) based on the hand variable deviation, speed deviation of the hand variable and joint angle of the robot arm; and controlling the motor torque of a motor built into each joint to the calculated joint torque control target value.

[0011] A robot control device for an articulated robot having motors built into its joints includes a sensor attached to each joint of the articulated robot, and a controller. The sensor acquires a joint angle and a joint angular velocity of each joint of the articulated robot when a hand of a robot arm moves to a teaching target position during direct teaching of the articulated robot, and the controller calculates a deviation of a hand variable of the robot arm and a velocity deviation of the hand variable based on the joint angle and joint angular velocity of each joint acquired by the sensor, calculates a control target value of the joint torque of each joint using the following formula (A) based on the deviation of the hand variable of the robot arm, the velocity deviation of the hand variable, and the joint angle of the robot arm, and controls the motor torque of the motor built into each joint to the calculated control target value of the joint torque.

[0012] The hand variables of the robot arm are the position and angle of the hand. The hand variable deviations are the position and angle deviations of the hand. The hand variable velocity deviations are the velocity and angular velocity deviations of the hand.

[0013] According to the technology disclosed in this specification, it is possible to prevent excessive torque from being output near a singular point during direct teaching of an articulated robot.

[0014] A perspective view of a robot. A block diagram showing the electrical configuration of a robot control device. A block diagram showing the electrical configuration of a robot control device. A simplified model of a robot arm. A flow chart of motor torque control.

[0015] <Embodiment 1> 1. Structure of the Articulated Robot Fig. 1 is a perspective view of an articulated robot 10. The articulated robot 10 is composed of a base 11 and a robot arm 12. The robot arm 12 is composed of a plurality of arms 13 to 17 and a plurality of joints 21A to 21F that connect the arms 13 to 17.

[0016] In this embodiment, a six-joint robot with six joints is exemplified, and from the side closest to the base 11, the joints are the first joint 21A, the second joint 21B, the third joint 21C, the fourth joint 21D, the fifth joint 21E, and the sixth joint 21F.

[0017] The fourth joint 21D is a so-called elbow joint, and the fifth joint 21E and sixth joint 21F correspond to joints from the arm to the wrist. A load attachment part is provided on the hand 12A of the robot arm 12, and a load 50 can be attached thereto.

[0018] As shown in FIG. 2, motors 23A to 23F are built into the joints 21A to 21F, respectively, and the joint angles (angles of the arms) can be adjusted arbitrarily by driving the motors.

[0019] As shown in FIG. 3, the robot control device 30 is made up of a first sensor 31 A to a sixth sensor 31 F and a controller 32 .

[0020] As shown in FIG. 2, the first to sixth sensors 31A to 31F are provided at the joints 21A to 21F, respectively, and measure and output the joint angles and joint angular velocities of the joints 21A to 21F.

[0021] The controller 32 includes a calculation unit 33, a memory 34, and an input unit 35. The input unit 35 is a user interface for an operator, and is provided for inputting various information to the controller 32.

[0022] The memory 34 stores data necessary for torque control during direct teaching of the articulated robot 10. Specifically, data (A) to (F) are stored in order to obtain the control target value of the joint torque from formula (A).

[0023] <Data for calculating joint torque control target value> (A) Taught target value of hand variable of hand 12A, speed command value of hand variable (B) Calculation formula for gravity torque applied to each joint of articulated robot 10 (C) Calculation formula for Coriolis torque applied to each joint of articulated robot 10 (D) Stiffness matrix representing the apparent stiffness (springiness) of hand 12A (E) Viscosity matrix representing the apparent viscosity (damperiness) of hand 12A (F) Jacobian matrix of articulated robot 10, transpose matrix of Jacobian matrix

[0024] 2. Jacobian Matrix Generally, the relationships expressed by Equations (1) and (2) hold between the hand variables and joint variables of the robot arm 12. The joint variables are the joint angles of each of the joints 21A to 21G. Figure 4 shows the hand variables and joint variables using a simplified model of a robot arm.

[0025]

[0026] J(q) is the Jacobian matrix. The Jacobian matrix is ​​a matrix that converts the differential value (velocity) of the joint variable into the differential value (velocity) of the hand variable of the robot arm 12. The Jacobian matrix can be calculated using the joint variables as variables and the arm length of each arm, etc.

[0027] 3. Cartesian Impedance Control Method The Cartesian impedance control method is a method for controlling the mechanical impedance of the robot arm 12 in an orthogonal space. The mechanical impedance is the ratio of the external force acting on the tip of the robot arm 12 to the response speed, and includes an inertial component, a viscous component (damper property), and a rigidity component (spring property).

[0028]

[0029] The above equation (3) is the equation of motion for the articulated robot 10. The left side is the joint torque, the first term on the right side is the gravity term, the second term is the Coriolis term, the third term is the inertia compensation term, and the fourth term is the mechanical impedance control term (see Table 1). The gravity term is the torque (weight torque) applied to the joint due to the weight of the robot arm 12. The Coriolis term is the torque (Coriolis torque) applied to the joint due to the Coriolis force associated with the rotation of the robot arm 12.

[0030]

[0031] In equation (3), by setting the reference force and command acceleration in the four terms on the right side to zero and setting the mechanical impedance in the direction in which you want to move or rotate the hand 12A to zero, you can move the hand 12A of the robot arm 12 in that direction, and perform direct teaching in Cartesian space (orthogonal space).

[0032] For example, by setting the mechanical impedance of the position (x, y, z) to 0 and the mechanical impedance of the angle (roll, pitch, yaw) to a large value, it is possible to teach only the position of the hand 12A without changing the angle (posture) of the hand 12A.

[0033] The following equation (4) is an equation of motion when the reference force and command acceleration are set to zero. As with equation (3), the first term on the right side is the gravity term, the second term is the Coriolis term, the third term is the inertia compensation term, and the fourth term is the mechanical impedance control term (see Table 1).

[0034]

[0035] Here, if the rank of the matrix J(q) decreases (if the order of the matrix decreases), 1 / J(q) in the third term on the right side (the inertia compensation term) becomes large, and the magnitude of the joint torque approaches infinity, causing the operation of the articulated robot 10 to become unstable near a singular posture.

[0036] The only term affected by the singular posture is the third term on the right side of Equation 4 (the inertia compensation term), which is a term that dynamically compensates for inertia. During Cartesian direct teach, the operator supports the vicinity of the tip of the robot arm 12 with his or her hand, so even if the term that dynamically compensates for inertia (Equation (5) below) is omitted, it is considered that the effect on performance is small.

[0037]

[0038] Therefore, by calculating the joint torque and controlling the motor torque using equation (A), which omits the inertia compensation term from the equation of motion of equation (4), excessive joint torque is not output near the singular point, and Cartesian direct teach can be performed safely even near the singular point.

[0039]

[0040] As shown in formulas (B) to (F), the joint torque, gravity torque, Coriolis torque, and hand variables are multidimensional, specifically six-dimensional vectors. As shown in formulas (G) to (I), the Jacobian matrix (transpose), viscosity matrix, and stiffness matrix are six-row, six-column matrices. The angular deviation (rotational deviation) of the hand 12A can be calculated using a quaternion or angular axis vector representation using a quaternion, which does not have a singular point in the representation. Patent Publication No. 2021-194734 is a reference regarding angular axis vector representation.

[0041]

[0042]

[0043]

[0044]

[0045]

[0046]

[0047]

[0048]

[0049] By changing the coefficients of the stiffness matrix Kd, it is possible to control the movement direction and rotation direction of the hand 12A of the robot arm 12. For example, if it is desired to move the hand 12A in the x direction, the coefficient Kx is set to zero, and the coefficients Ky and Kz are set to values ​​other than zero, as shown below.

[0050] Similarly, to move the hand 12A in the y direction, the coefficient Ky is set to zero, and the coefficients Kx and Kz are set to values ​​other than 0. Also, by setting any of the coefficients Kr, Kp, and Kyaw to zero, the hand 12A can be rotated in that direction.

[0051] Furthermore, by adjusting the coefficients of the viscosity matrix Dd, the ease of movement of the hand 12A can be changed.

[0052] Torque control during Cartesian direct teaching will now be described with reference to the flowchart of Fig. 5. Here, a case will be described in which the angle of the hand 12A is fixed and only the position of the hand 12A is taught.

[0053] First, the operator touches the robot arm 12 with his / her hand and moves the hand 12A to a teaching target position (S10).

[0054] When the hand 12A moves to the teaching target position, the sensors 31A to 31F detect the joint angle and joint angular velocity of each of the joints 21A to 21F of the robot arm 12. The detected joint angle and joint angular velocity data of each of the joints 21A to 21F is input to the controller 32 (S20).

[0055] The controller 32 calculates the position of the hand 12A of the robot arm 12 using equation (1) based on the joint angles of the joints 21A to 21F, and compares it with the taught target position to calculate the position deviation.

[0056] Furthermore, the velocity of the hand 12A of the robot arm 12 is calculated from the joint angular velocity of each of the joints 21A to 21F using equation (2), and the velocity deviation of the hand 12A is calculated by comparing it with the command velocity (S30).

[0057] The controller 32 calculates the control target values ​​of the joint torques of the joints 21A to 21G using the above-mentioned formula (A) based on the information in (1) and (2) (S40).

[0058] (1) Joint angles and joint angular velocities of the joints 21A to 21F (2) Position deviation and velocity deviation of the hand 12A

[0059] Thereafter, the controller 32 controls the motor torque of the motors 23A to 23G built into the joints 21A to 21G to the joint torque control target value calculated in S40 (S50).

[0060] By repeating these steps (S10 to S50), the motor torques of the motors 23A to 23G are controlled during Cartesian teaching.

[0061] 3. Effects According to this embodiment, it is possible to prevent excessive joint torque from being output near a singular point during Cartesian direct teach. Therefore, it is possible to safely perform Cartesian direct teach even near a singular point.

[0062] Second Embodiment In the first embodiment, a six-axis (six joints) articulated robot was the control target. Since the degrees of freedom of the hand are six, the six-axis articulated robot has zero redundant degrees of freedom. Below, a seven-axis (seven joints) articulated robot will be exemplified to disclose a control method for a robot with redundant degrees of freedom.

[0063] (1) In the case of a seven-axis articulated robot, the joint torque, gravity torque, and Coriolis torque are all seven-dimensional vectors. By simply setting the Jacobian to a 6-by-7 matrix, the joint torque of each joint can be calculated using Equation A disclosed in the first embodiment.

[0064] (2) However, when formula A is used, the restoring force that constrains the redundant degrees of freedom does not work.

[0065] (3) For example, in a robot in which one axis is added to a general six-axis robot, the elbow position becomes a redundant degree of freedom. Humans can also move their elbow position while keeping their hands fixed, so the elbow position becomes a redundant degree of freedom.

[0066] (4) In other words, in a 7-axis articulated robot, even if all the diagonal components of Kd are set to large values, the hand position remains fixed and the elbow can move freely.

[0067] In a seven-axis articulated robot, direct teaching of redundant degrees of freedom can be performed using the methods described in (1) to (4).

[0068] When performing direct teaching on a 7-axis articulated robot by moving the position and angle of the hand, if you reduce the corresponding coefficient of the stiffness matrix Kd using the method above, the reduced direction and redundant degrees of freedom will become operational.

[0069] For example, when trying to move the position of the hand in three orthogonal axial directions (x, y, z), the elbow position also moves freely, requiring the operator to apply force to constantly hold the elbow, reducing operability.

[0070] Therefore, in a seven-axis articulated robot, if the robot is not to be moved in the direction of the redundant degree of freedom, the controller 32 controls the position of the redundant joint during Cartesian direct teaching, and restricts displacement in the direction of the redundant degree of freedom.

[0071] By restricting the displacement of the redundant joints in the direction of the redundant degrees of freedom through position control by the controller 32, the redundant joints can be treated in the same way as six axes, and deterioration of operability can be suppressed during Cartesian direct teaching.

[0072] Furthermore, when moving a redundant joint in the direction of the redundant degree of freedom, direct teaching can be performed using methods (1) to (4). In this way, by changing the control method depending on the direction of movement, it is possible to naturally expand the degree of freedom to seven without the existence of singular points.

[0073] Although the embodiments have been described in detail above, these are merely examples and do not limit the scope of the claims. The technology described in the claims includes various modifications and alterations of the specific examples exemplified above.

[0074] (1) In the above embodiment, the articulated robot 10 has been described as having six joints. The number of joints does not necessarily have to be six. It may be seven or eight.

[0075] REFERENCE SIGNS LIST 10 Robot 11 Base 12 Robot arm 12A Hand 13 to 18 Arms 21A to 21G Joints 23A to 23G Motors 30 Robot control device 32 Controller 33 Calculation unit 34 Memory 35 Input unit

Claims

1. A motor torque control method during direct teaching of a multi-joint robot, comprising the steps of: acquiring, by a sensor, the joint angle and joint angular velocity of each joint of the multi-joint robot when the endoscope of a robot arm moves to a teaching target position during direct teaching; calculating a deviation in the endoscope variable of the robot arm and a velocity deviation of the endoscope variable based on the joint angle and joint angular velocity of each joint acquired by the sensor; calculating a control target value of the joint torque of each joint using the following formula (A) based on the deviation in the endoscope variable of the robot arm, the velocity deviation of the endoscope variable and the joint angle of the robot arm; and controlling the motor torque of a motor built into each joint to the calculated control target value of the joint torque.

2. A robot control device for a multi-joint robot having motors built into each joint, comprising: a sensor attached to each joint of the multi-joint robot; and a controller, wherein the sensor acquires the joint angle and joint angular velocity of each joint of the multi-joint robot when the hand of the robot arm moves to a teaching target position during direct teaching of the multi-joint robot, and the controller calculates a deviation in the hand variable of the robot arm and a velocity deviation of the hand variable based on the joint angle and joint angular velocity of each joint acquired by the sensor, calculates a control target value of the joint torque of each joint using the following formula (A) based on the deviation in the hand variable of the robot arm, the velocity deviation of the hand variable and the joint angle of the robot arm, and controls the motor torque of the motor built into each joint to the calculated control target value of the joint torque.

Images