JP2011131362A - Control method and control program for seven-axes multi-jointed robot - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To avoid sudden changes in the configuration of a seven-axes multi-jointed robot when a wrist position is moved. <P>SOLUTION: A control method for a seven-axes multi-jointed robot includes steps of setting one of seven rotating shafts as a redundant shaft, three of the remaining rotating shafts as base shafts, and one of the three base shafts as a variable shaft, and solving a quadratic equation formalized with respect to a wrist position and the joint angle of the variable shaft to inversely convert the wrist position into the joint angle of the three base shafts, the wrist position being based on a linear distance from a base end to the wrist and on the distance from the base end to the wrist in the direction of extension of the rotating shaft closest to the base end. The steps of specifying and storing one of four first equation-solving arithmetic expressions, based on the initial configuration of the seven-axes multi-jointed robot when power is on, and performing inverse conversion using the stored one first equation-solving arithmetic expression after power is on, are performed to calculate the joint angles of the three base shafts based on the target position of the wrist, to thereby control the action of the seven-axes multi-jointed robot. <P>COPYRIGHT: (C)2011,JPO&INPIT

Description

  The present invention relates to a control method and a control program for a seven-axis articulated robot, and more particularly to a control method and a control program that suppress a sudden change in the form of the seven-axis articulated robot when the wrist position is moved.

  In recent years, control technology for industrial robots has progressed with the progress of computer technology, and as a result, the operations required for industrial robots have become increasingly complex and sophisticated, and the accuracy of the control technology has also increased. High speed and high precision are required. For example, in the case of a 6-axis articulated robot that has been widely used as an industrial robot in the past, it is sufficient for a purpose of moving to a certain position, but skillfully avoids various obstacles that exist in the moving space. However, it was difficult to perform complicated work. Therefore, development of a 7-axis articulated robot (see, for example, Patent Document 1) in which one axis (redundant axis) is further added to the 6-axis articulated robot has been actively performed in recent years.

  As a control method for the 7-axis articulated robot, as in the case of the conventional 6-axis articulated robot, a method for sequentially indicating points on the path to be followed by the wrist position at the tip of the robot arm, or the initial position of the wrist In addition, a method is used in which only the target position is indicated and the route on the way is not questioned. In addition, when these methods are used, inverse transformation (coordinate transformation) for obtaining each joint angle (joint angle coordinate) from the wrist position and posture (wrist coordinate) is performed.

JP-A-6-315880

  By the way, in the case of a 7-axis articulated robot, there is a redundant axis (1 axis) unlike a 6-axis articulated robot, so each joint angle obtained by inverse transformation at the same wrist position is not uniquely determined.

  For this reason, when the wrist position is moved from one point A to another point B, for example, the joint angle of the first joint at point A and point B differs by 180 °, and the reverse based on the coordinates of the point A The problem is that each joint angle obtained and specified by transformation and each joint angle obtained and specified by inverse transformation based on the coordinates of the point B are greatly deviated, and the form of the robot changes rapidly. was there.

  The technique disclosed in Patent Document 1 is a method of rotating the elbow part while maintaining the position and posture of the tip control point, and when the wrist position is moved, a rapid change in the shape of the robot is performed. There is no suggestion or disclosure about suppression.

  The present invention has been made to solve such a problem, and an object thereof is to suppress a rapid change in the form of the robot when the position of the wrist is moved.

  A control method of a seven-axis articulated robot according to the present invention for solving the above-described problem includes a wrist provided at a distal end and seven joints provided in order from the proximal end toward the wrist, A control method for a seven-axis articulated robot configured such that each of the seven joints has a rotation axis and the next joint is rotated around each rotation axis. One of them as a redundant axis, three of the remaining rotation axes as a base axis, and any one of the three base axes as a variable axis, and a straight line from the base end to the wrist A fourth order formulated with respect to the wrist position based on the distance and the distance from the base end to the wrist in the extending direction of the rotation axis closest to the base end and the joint angle of the joint having the variable axis Solve the equation to determine the position of the wrist And the step of inversely transforming into joint angles of each joint having the following equation, wherein the quartic equation can be solved by using four first solution calculation equations respectively corresponding to the four solutions, and The form that can be taken by the 7-axis articulated robot is determined in accordance with the four solutions, and thus the four first solution calculation formulas. When the power is turned on, the 7-axis articulated robot is turned on. One of the four first solution calculation formulas is specified and stored based on the initial form, and after the power is turned on, the inverse transformation is performed using the stored one first solution calculation formula And calculating the joint angle of each joint having the three basic axes from the target position of the wrist, thereby controlling the operation of the seven-axis articulated robot.

  In the above control method, when the power is turned on, the four first-order equations are used to solve the quaternary equation to obtain the four solutions, and each of the four solutions is obtained when the power is turned on. The wrist position is inversely transformed into at least one of the joint angles of each joint having the three base axes, and the joint angle of the joint having the one base axis and the 7-axis articulated robot when the power is turned on By comparing with the joint angle of the joint having one of the three basic axes corresponding to the initial form, one of the four first solving equations of the quaternary equation is specified. It is also possible to memorize.

  In the above control method, the rotation axes of all the adjacent joints are perpendicular to each other, and seven joints provided in order from the proximal end toward the wrist are respectively a first joint and a second joint. When the joint, the seventh joint, the third joint, the fourth joint, the fifth joint, and the sixth joint are defined, the redundant axis is a rotational axis of the seventh joint, and the three base axes are the first joint. The rotation axis of the second joint, the rotation axis of the third joint, and the rotation axis of the third joint, and the variable axis may be the rotation axis of the third joint.

  According to the above control method, the four first solution calculation expressions for obtaining four solutions of the quaternary equation are associated in advance with the four forms that can be taken according to the four solutions, and the power is turned on. Based on the initial form of the 7-axis articulated robot, any one of the four first solution calculation formulas is used in the inverse conversion process after the power is turned on. As a result, when the wrist position is moved to the target position, it is possible to suppress a sudden change in the form of the seven-axis articulated robot. Furthermore, in order to specify the initial form of the 7-axis articulated robot when the power is turned on, each of the four first solving equations of the quaternary equation may be calculated only once. In the inverse conversion process after power-on, it is not necessary to calculate the remaining three first solution calculation expressions other than the specified first solution calculation expression. Therefore, the control speed of the 7-axis articulated robot can be increased.

  In the above control method, 3 is formulated as a decomposition equation for solving the quartic equation based on the Ferrari solution when the quartic equation is solved using the four first solving equations. One real solution included in the three solutions of the quadratic equation may be obtained, and the four first solving equations may be calculated using one real solution of the cubic equation obtained.

  In the above control method, when one real solution included in three solutions of the cubic equation is obtained, if the discriminant of the cubic equation is 0 or positive, the solution is obtained by the Cardano solution. When the second solution calculation formula based on the Cardano solution corresponding to the one real solution among the one real solution and the two imaginary solutions of the cubic equation is used, and the discriminant of the cubic equation is negative Is a third solving equation based on the Vieta's solution corresponding to one real solution other than two real solutions having the same absolute value and different signs among the three real solutions of the cubic equation obtained by the Vieta's solution May be used.

  According to the above control method, in order to solve the quartic equation, the cubic equation formulated as a decomposition equation for solving the quartic equation based on the Ferrari solution is solved. Since it is an equation, there are always three solutions. Therefore, if one real number solution that is always included in the three solutions of the cubic equation is obtained in advance, the three solutions of the cubic equation will not be selected randomly when solving the quartic equation. It is possible to constantly obtain four solutions of the quartic equation. As a result, it is possible to further suppress the rapid fluctuation of the 7-axis articulated robot. Further, since only one real number solution is obtained when solving the cubic equation, it is not necessary to perform complex number calculation, the amount of calculation can be suppressed, and the implementation in the system becomes easy.

  A control program for a seven-axis articulated robot according to the present invention for solving the above-described problem includes a wrist provided at a distal end and seven joints provided in order from the proximal end toward the wrist, Control of a 7-axis articulated robot that causes a computer to perform a control method for a 7-axis articulated robot that is configured to have each of 7 joints have a rotation axis and rotate the next joint around each rotation axis In the program, the control method includes any one of the seven rotating shafts as a redundant shaft, three of the remaining rotating shafts as a base shaft, and any of the three base shafts. Position of the wrist based on a linear distance from the base end to the wrist and a distance from the base end to the wrist in the extending direction of the rotation shaft closest to the base end, with one as a variable axis And the variable axis Solving the quaternary equation formulated with respect to the joint angles of the joints, and converting the wrist position back to the joint angles of the joints having the three basic axes. It can be solved using the four first solving equations corresponding to each of the two solutions, and the form that the seven-axis articulated robot can take is that the four solutions and the four first finding solutions It is determined according to an arithmetic expression, and when the power is turned on, any one of the four first solving equations is specified based on the initial form of the seven-axis articulated robot when the power is turned on. And after the power is turned on, the inverse transformation step is performed using the stored first first solving equation, and the joint angles of the joints having the three base axes from the target position of the wrist And thereby Controlling the operation of the 7 shaft articulated robot is intended.

  When moving the position of the wrist, a rapid change in the form of the seven-axis articulated robot can be suppressed.

FIG. 1 is a diagram showing a configuration of a robot system including a seven-axis articulated robot and a robot control device according to an embodiment of the present invention. FIG. 2 is a diagram schematically showing the link structure of the seven-axis articulated robot shown in FIG. FIG. 3 is a flowchart showing a flow of processing for specifying any one of the four first solving equations for the quaternary equation. FIG. 4 is a flowchart showing the flow of the inverse transformation process executed using any one of the four solution equations of the stored quartic equation. FIG. 5 is a diagram illustrating a configuration of a seven-axis articulated robot in which a seventh joint is provided between the second joint and the third joint of the six-axis articulated robot. FIG. 6 is a diagram showing four forms of a seven-axis articulated robot that can be taken according to four solutions of a quartic equation.

  Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. In the following description, the same or corresponding elements are denoted by the same reference symbols throughout the drawings, and redundant description thereof is omitted.

(Concept of the present invention)
First, the concept of the present invention will be described.

  FIG. 5 shows a second joint JT2 in a six-axis articulated robot having first to sixth joints JT1 to JT6 having first to sixth rotation axes A1 to A6 in order from the proximal end to the wrist at the distal end. 7 shows a configuration of a seven-axis articulated robot 100 when a seventh joint JT7 having a seventh rotation axis JT7 is provided between the (second rotation axis A2) and the third joint JT3 (third rotation axis A3). It is a figure. In FIG. 5, the fourth to sixth joints JT4 to JT6 (wrist axis: fourth to sixth rotation axes A4 to A6) constituting the wrist of the seven-axis articulated robot 100 are omitted from the drawing.

  When the inverse transformation is performed on the 7-axis articulated robot 100, the wrist axes (A4 to A6) that define the wrist posture are ignored for the time being, and the remaining four rotation axes A1 that define the position of the wrist. , A2, A7, A3, one rotating shaft (for example, the seventh rotating shaft A7) is a redundant shaft, and the remaining three rotating shafts (for example, the first to third rotating shafts A1 to A3) are the base shafts. And a method of handling the joint angle of each joint having the base axis as an unknown variable is conceivable.

  Here, as shown in FIG. 5, the linear distance between the origin O (0, 0, 0) in the reference coordinate system (XYZ coordinate system) and the wrist position P of the 7-axis articulated robot 100 is Rh, and the reference The distance in the Z-axis direction of the wrist position P in the coordinate system (the distance from the base end to the wrist in the extending direction of the first rotation axis closest to the base end) is expressed as Zh. Further, when the wrist position P in the reference coordinate system is expressed as (Xp, Yp, Zp), the following equations are established for Rh and Zh.

Rh ² = Xp ² + Yp ² + Zp ² (1)
Zh = Zp Formula (1-2)
Rh and Zh are the link lengths of the seven-axis articulated robot 100 and the first to third and seventh rotation axes A1 to A3, separately from the above formulas (1-1) and (1-2). This can be expressed by using the joint angles θ1, θ2, θ3, θ7 of the first to third and seventh joints JT1 to JT3, JT7 having A7.

  Therefore, when these equations are arranged, a quaternary equation for the variable t (= tan θ3) can be obtained as follows.

t ⁴ + a · t ³ + b · t ² + c · t + d = 0 (2)
Note that the solution of the variable t in the quaternary equation of Equation (2) is the sum of the solutions t1, t2, t3, and t4 obtained from the following four first solving equations (3-1) to (3-4). There are four ways.

t1 = (− M1− (M1 ² −4 · N1) ^0.5 ) / 2−a / 4 (3-1)
t2 = (− M1 + (M1 ² −4 · N1) ^0.5 ) / 2−a / 4 (3-2)
t3 = (-M2- (M2 ² -4 · N2) ^0.5 ) / 2-a / 4 (3-3)
t4 = (− M2 + (M2 ² −4 · N2) ^0.5 ) / 2−a / 4 (3-4)
Therefore, by calculating “θ3 (n) = atan (t (n)): where n = 1 to 4” for each of the four solutions t1 to t4, the four solutions θ3 (1), θ3 (2), θ3 (3), and θ3 (4) can be obtained. If the joint angle θ3 is obtained, the joint angle θ1 and the joint angle θ2 are also obtained uniquely, so the four solutions θ3 (1), θ3 (2), θ3 (3), θ3 ( Four solutions θ1 (n) (where n = 1 to 4) of the joint angle θ1 corresponding to 4) and four solutions θ2 (n) (where n = 1 to 4) of the joint angle θ2 are obtained. As a result, even if the wrist position P is the same, there are four possible forms of the 7-axis articulated robot 100 as shown in FIG. In order to perform the operation, it is required to suppress a rapid change in the form of the seven-axis articulated robot.

By the way, in the first solution calculation formula (formula (3-1) to formula (3-4)) regarding the four solutions t1 to t4 of the quaternary equation of formula (2), M1, M2, N1, and N2 are , (A, b, c, d) in the left side of the quaternary equation of equation (2), and the decomposition equation of the quaternary equation of equation (2) based on “Ferrari's solution” of the quaternary equation 3 It is obtained by a solution u of the following equation. This cubic equation is
^{p = -3a 2/8 + b} ··· (4-1)
q = a ³ / 8−ab / 2 + c (4-2)
^{r = -3a 4/256 + a} 2 b / 16-ac / 4 + d ··· (4-3)
Is expressed as a cubic equation for the variable y, as in the following equation.

8y ³ -4py ² -8ry + 4pr-q ² = 0 (5)
However, since three solutions of u1, u2, and u3 are obtained as solutions of the cubic equation of equation (5), no matter which of these solutions u1 to u3 is used, the quaternary equation of equation (2) There arises a problem that the solutions t1, t2, t3, and t4 can be obtained. Therefore, if different ones of the three solutions u1, u2, and u3 are used every time the cubic equation of Equation (5) is solved, the four solutions t1 and t2 of the quaternary equation of Equation (2) are used. , T3, t4 change in combination.

  For example, Table 1 below shows from four solutions t1, t2, t3, and t4 of the quaternary equation of Equation (2) when each of the three solutions u1, u2, and u3 of the cubic equation of Equation (5) is used. It is the table | surface which showed the example of the combination of joint angle (theta) 3 obtained. In Table 1, when the solution u1 of the cubic equation of equation (5) is adopted, the combination of the joint angles θ3 obtained from the four solutions t1, t2, t3, t4 of the quaternary equation of equation (2) is (− 43 °, 51 °, −20 °, 28 °). On the other hand, when the solution u2 of the cubic equation is adopted, the combination of the joint angles θ3 obtained from the four solutions t1, t2, t3, and t4 of the quaternary equation is (−43 °, −20 °, 51 °, 28 °). ), And the joint angle θ3 corresponding to the solutions t2 and t3 of the quaternary equation is directly opposite to the case where the solution u1 of the cubic equation is employed. Alternatively, when the solution u3 of the cubic equation is adopted, the combination of the joint angles θ3 obtained from the solutions t1, t2, t3, and t4 of the quaternary equation is (−43 °, −20 °, 51 °, 28 °). Yes, compared to the case where the solution u1 of the cubic equation is adopted, the joint angle θ3 corresponding to the solutions t1 and t4 of the quartic equation is opposite.

  Therefore, the same solution (for example, solution u1) is always adopted among the three solutions u1, u2, and u3 in the cubic equation of Equation (5), and the four solutions t1 in the quaternary equation of Equation (2) are used. , T2, t3, and t4, unless the same solution (for example, the solution t1) is always used, the form of the seven-axis articulated robot 100 is greatly changed. More specifically, Table 1 represents four solutions t1, t2, t3, and t4 for every three solutions u1 to u3 corresponding to a wrist position Px, and four solutions t1 to t4 with respect to the wrist position Px. For example, it is assumed that the solution t1 is selected from among the above. In this case, when four solutions t1 to t4 are obtained by calculation for the wrist position Py slightly shifted from the wrist position Px, the values shown in Table 1 are slightly shifted from each other. As for the wrist position Py, there is no problem if the solution t1 (value slightly shifted) is selected as in the wrist position Px. However, if, for example, a solution t2 (value slightly shifted) other than the solution t1 (value slightly shifted) is selected, the joint angle θ3 and the joint angles θ1 and θ2 calculated using the joint angle θ3 are selected. The value is greatly different from the value at the wrist position Px. As a result, the movement amounts of the first to third joints JT1 to JT3 and the seventh joint JT7 increase, and it is necessary to avoid such a situation.

  Therefore, as one of the methods for suppressing the rapid change in the form of the seven-axis articulated robot 100, as shown in FIG. 6 based on the condition of each joint angle (eg, within ± 90 °) or the combination thereof. It is always possible to grasp which form is present among the four forms, and one solution that can obtain the grasped form is selected from the four solutions t1, t2, t3, and t4 A control method is possible. However, each time the inverse transformation process is performed, it is necessary to determine the complicated conditions as described above, which causes a problem that the inverse transformation process becomes complicated.

  As another method, the same solution (for example, solution u1) is always selected from the three solutions u1, u2, and u3 of the cubic equation of Equation (5) required for solving the quartic equation of Equation (2). By adopting, the four first solving equations (formulas (3-1) to (3-4)) and the four forms as shown in FIG. A method is conceivable. However, in this case, since it becomes necessary to handle complex numbers as a solution of the cubic equation of Equation (5), implementation in the system becomes difficult.

  Therefore, the present inventor preliminarily associates the four first solving calculation expressions (formulas (3-1) to (3-4)) with the four forms as shown in FIG. As a method of always adopting the same solution for the cubic equation (5), we considered using the “Cardano solution” and the “Vieta solution”, which are widely known as the solution of the cubic equation. Both solutions can obtain three solutions. However, as shown in the following Table 2, depending on the condition of the discriminant D of the cubic equation (less than 0, equal to 0, greater than 0), it may be negative. √ operation, that is, complex number operation may be required. As the conditions of the discriminant D of the cubic equation, there are the following three conditions C1, C2, and C3.

Condition C1 (D <0): Three real solutions are obtained Condition C2 (D = 0): One real solution (triple solution) is obtained Condition C3 (D> 0): One real solution and two real solutions Imaginary solution (conjugate complex number) is obtained

  As shown in Table 2, when the “cardano solution” is adopted, in the case of the condition C1, a negative √ operation (complex number operation) is required in all three real number solutions u1 to u3, and in the case of the condition C3 Is characterized in that a negative √ operation is required for the two imaginary solutions u2 and u3, and a negative √ operation is not required in the case of the condition C2.

  When the “Vieta's solution” is adopted, a negative √ operation may be required in the real solution u1 that is a triple solution in the case of the condition C2, and all solutions (the real solution u1 in the case of the condition C3). In the imaginary solution u2, u3), a negative √ operation may be required, and in the case of the condition C1, the negative √ operation is unnecessary.

  Since it is complicated and difficult to calculate the negative √ operation in the control program of the robot controller that controls the 7-axis articulated robot, it is necessary to use “Cardano's solution” and “Vieta's solution” properly. . Therefore, the present applicant has realized that in the case of the conditions C2 and C3, it is sufficient to use the “Cardano solution” in which the real solution u1 is obtained without requiring a negative √ operation.

  On the other hand, in the case of the condition C1, it is sufficient to use a “Vieta's solution” that does not require a negative √ operation. In this case, three real solutions (u1 to u3) are obtained. One real solution needs to be selected from the three real solutions. Furthermore, since it is necessary to always use the same solution u1 regardless of which of the conditions C1 to C3 is satisfied in order to suppress a change in the form of the robot, the three real solutions (u1 to u1) obtained by the “Vieta's solution” are used. It is necessary to determine which solution of u3) is associated with one real solution u1 obtained by the “Caedano solution”. Therefore, the real number solution u2 and the real number solution u3 are different from each other but have the same absolute value, and a solution that is not both is adopted as the real number solution u1.

  As a result, any of the conditions C1 to C3 of the discriminant D of the cubic equation is satisfied, the negative √ operation is not performed, and the same real number solution u1 regardless of which of the conditions C1 to C3 is satisfied. Is always required. Furthermore, since this real number solution u1 is always employed and a combination of four solutions t1, t2, t3, and t4 of the quaternary equation is obtained, a sudden change in the form of the 7-axis articulated robot is suppressed. .

(Embodiment)
Next, an embodiment of the present invention based on the above-described concept of the present invention will be described.
[Robot system configuration]
FIG. 1 is a diagram showing a configuration of a robot system including a seven-axis articulated robot and a robot control device according to an embodiment of the present invention. FIG. 2 is a diagram schematically showing the link structure of the seven-axis articulated robot 100 shown in FIG.

  As shown in FIGS. 1 and 2, the 7-axis articulated robot 100 includes a wrist provided at the tip and seven joints JT1 to JT7 provided in order from a predetermined base end toward the wrist, Seven joints JT1 to JT7 have first to seventh rotation axes A1 to A7, respectively, and are configured to rotate the next joint around each rotation axis. In the present embodiment, the seven-axis articulated robot 100 is configured as a so-called floor-standing vertical articulated robot in which the base 2 is installed on the floor surface below the work space. Further, the seven-axis articulated robot 100 includes six joints JT1 to JT6 provided in order from the base end to the wrist, and the six joints JT1 to JT6 respectively have the first to sixth rotation axes A1 to A6. In the 6-axis multi-joint robot having the rotation and the next joint around each rotation axis, the second joint JT2 (second rotation axis A2) and the third joint JT3 (third rotation axis) A seventh joint JT7 (seventh rotation axis A7) is provided between the third rotation axis A7 and the seventh rotation axis A7 of the seventh joint JT7 is perpendicular to the second rotation axis A2 and the third rotation axis A3. It is comprised by.

  In addition, the code | symbol provided to seven joints and seven rotating shafts was attached | subjected for convenience, and what code | symbol may be sufficient as long as each of seven joints and seven rotating shafts can be identified.

  In the present embodiment, an orthogonal coordinate system based on the X0 axis, the Y0 axis, and the Z0 axis, which will be described later, is referred to as a “reference coordinate system”, which is denoted as Σ0. Further, an orthogonal coordinate system based on an X6 axis, a Y6 axis, and a Z6 axis, which will be described later, is referred to as a “wrist coordinate system” and is denoted as ΣR. The displacement of the ΣR origin in the X0, Y0, and Z0 axis directions relative to the Σ0 origin is called “wrist position”, and the angular displacement of the ΣR relative to Σ0 around the X0, Y0, and Z0 axes is called “wrist posture”. I will do it.

  The tip of the robot is called “hand”. In the present embodiment, the tip of the tool member 11 is the tip. Also, assuming that the portion that mainly affects the wrist posture is called “wrist”, in the seven-axis articulated robot 100 shown in FIG. 1, the origin of ΣR is the fourth to sixth rotation axes A4 as described later. Since A6 to A6 intersect at a single point, the portion related to the fourth to sixth joints JT4 to JT6 is the wrist. On the other hand, it is the portion related to the first to third joints JT1 to JT3 and the seventh joint JT7 that affects the wrist position. “Wrist” includes attachments, hands, tools, end effectors, and the like. In the present embodiment, “wrist” corresponds to a wrist device 10 described later.

  If the hand position at ΣR is grasped, the hand position at Σ0 can be obtained when the wrist position and the wrist posture are given. Further, when the wrist posture, the hand position at Σ0, and the angle of the redundant axis are given, the joint angles θ1 to θ6 of the first to sixth joints JT1 to JT6 that can realize this can be obtained.

  In the present embodiment, the triaxial intersection corresponds to one point where the four axes of the fourth to sixth rotation axes A4 to A6 as wrist axes intersect. As a feature of the three-axis intersection, the position of the three-axis intersection with respect to the tool member 11 is constant regardless of the relative angle between the tool member 11 and the arm member 8 (that is, the rotation angle of the sixth rotation axis A6). 6 is also constant regardless of the relative angle between the arm member 6 and the arm member 7 (that is, the rotation angle of the fourth rotation axis A4). In this way, when a desired position / posture of the hand (tip of the tool member 11) is given, after determining the remaining axis other than the wrist axis that realizes the position of the wrist axis, There is an advantage that it is possible to determine a wrist axis that realizes a desired position and posture.

  On the base 2, a swivel base 3, arm members (links) 4, 5, 6, 7, 8 and an attachment 9 are connected in this order. A tool member 11 appropriately selected according to various work contents is detachably attached to the flange surface forming the tip of the attachment 9. The continuous members 2 to 9 from the base 2 to the attachment 9 are connected so as to be rotatable relative to each other.

  More specifically, rotation (turning) around the first rotation axis A <b> 1 of the turntable 3 with respect to the base 2 is allowed in the first joint JT <b> 1 that is a connecting portion between the base 2 and the turntable 3. Note that the origin O (0, 0, 0) of the reference coordinate system (orthogonal coordinate system based on the X0 axis, the Y0 axis, and the Z0 axis) is set as the base end at the center of the upper surface of the base 2 (see FIG. 2). ). The first joint JT1 is set with a joint coordinate system of the first joint JT1 (orthogonal coordinate system based on the X1, Y1, and Z1 axes).

  In the second joint JT2, which is a connecting portion between the upper end portion of the swivel base 3 and one end portion of the arm member 4, rotation (turning) of the arm member 4 around the second rotation axis A2 with respect to the swivel base 3 is allowed. The second joint JT2 is set with a joint coordinate system (orthogonal coordinate system based on the X2, Y2, and Z2 axes) of the second joint JT2. Here, the distance in the Z-axis direction of the second joint JT2 in the reference coordinate system (hereinafter referred to as the first link length) is represented as L0. Further, a distance in the Y-axis direction (hereinafter referred to as a second link length) between the first joint JT1 and the second joint JT2 in the reference coordinate system is represented as L1.

  In the seventh joint JT7, which is a connecting portion between the other end portion of the arm member 4 and one end portion of the arm member 5, rotation (turning) around the seventh rotation axis A7 of the arm member 5 with respect to the arm member 4 is allowed. The seventh joint JT7 is set with the joint coordinate system of the seventh joint JT7 (orthogonal coordinate system based on the X7 axis, the Y7 axis, and the Z7 axis).

  In the third joint JT3 that is a connecting portion between the other end portion of the arm member 5 and one end portion of the arm member 6, the arm member 6 is allowed to rotate (turn) around the third rotation axis A3 with respect to the arm member 5. . The third joint JT3 is set with a joint coordinate system (orthogonal coordinate system based on the X3 axis, the Y3 axis, and the Z3 axis) of the third joint JT3. Here, a distance in the Z-axis direction (hereinafter referred to as a third link length) between the second joint JT2 and the third joint JT3 in the reference coordinate system is represented as L2.

  In the fourth joint JT4 that is a connecting portion between the other end of the arm member 6 and one end of the arm member 7, the arm member 7 is allowed to rotate (turn) around the fourth rotation axis A4 with respect to the arm member 6. The fourth joint JT4 is set with a joint coordinate system of the fourth joint JT4 (orthogonal coordinate system based on the X4 axis, the Y4 axis, and the Z4 axis). Here, a distance in the Z-axis direction (hereinafter referred to as a fourth link length) between the third joint JT3 and the fourth joint JT4 in the reference coordinate system is represented as L3. Further, a distance in the Y-axis direction (hereinafter referred to as a fifth link length) between the third joint JT3 and the fourth joint JT4 in the reference coordinate system is represented as L4.

  In the fifth joint JT5, which is a connecting portion between the other end of the arm member 7 and one end of the arm member 8, the arm member 8 is allowed to rotate (turn) around the fifth rotation axis A5 with respect to the arm member 7. . The fifth joint JT5 is set with the joint coordinate system of the fifth joint JT5 (orthogonal coordinate system based on the X5 axis, the Y5 axis, and the Z5 axis).

  The sixth joint JT6, which is a connecting portion between the other end portion of the arm member 8 and one end portion of the attachment 9, is allowed to rotate (turn) around the sixth rotation axis A6 of the attachment 9 with respect to the arm member 8. In the sixth joint JT6, an orthogonal coordinate system based on the X6 axis, the Y6 axis, and the Z6 axis is set as the joint coordinate system of the sixth joint JT6. The orthogonal coordinate system using the X6 axis, the Y6 axis, and the Z6 axis has a point at which the fourth to sixth rotation axes A4 to A6 intersect at one point as the origin. Here, the distance in the Z-axis direction (hereinafter referred to as the sixth link length) between the fifth joint JT5 and the sixth joint JT6 in the reference coordinate system is represented as L5.

  When the base 2 is properly installed on the floor surface, the first rotation axis A1 closest to the base 2 is oriented in the vertical direction (extending direction from the base 2 to the first joint JT1), and the second rotation. The axis A2 is oriented in the horizontal direction (direction parallel to the floor surface). The seventh rotation axis A7 is oriented in the direction orthogonal to the second rotation axis A2 and in the extending direction of the arm member 4. Further, the third rotation axis A3 is a direction orthogonal to the seventh rotation axis A7 and is oriented in the horizontal direction. Further, the fourth rotation axis A4 is oriented in the direction orthogonal to the third rotation axis A3 and in the extending direction of the arm member 6. The fifth rotation axis A5 is a direction orthogonal to the fourth rotation axis A4 and is directed in the horizontal direction. The sixth rotation axis A6 is oriented in the direction orthogonal to the fifth rotation axis A5 and in the extending direction of the arm member 8. Therefore, the first to seventh rotation axes A1 to A7 are arranged so that the rotation axes of all adjacent joints are perpendicular to each other.

  The arm members 7 and 8 and the attachment 9 constitute a structure 10 (hereinafter referred to as a wrist device) resembling a human wrist for causing the tool member 11 attached to the attachment 9 to perform a fine operation. . The first rotation axis A1, the second rotation axis A2, the seventh rotation axis A7, and the third rotation axis A3 are used as rotation axes for horizontally turning or swinging the tool member 11 together with the wrist device 10. , The main axis of the seven-axis articulated robot 100. The fourth to sixth rotation axes A4 to A6 are rotation axes set in the wrist device 10, and form a so-called RBR (Roll-Bend-Roll) type wrist axis. The wrist shaft of the wrist device 10 is not limited to the RBR type, and may be a so-called BBR (Bend-Bend-Roll) type or 3R (Roll-Roll-Roll) type wrist shaft.

  The first to seventh joints JT1 to JT7 are provided with servo motors M1 to M7 and position detectors D1 to D7, respectively. The position detectors D1 to D7 are composed of, for example, a rotary encoder. By driving the servo motors M1 to M7, the first to seventh joints JT1 to JT7 are allowed to rotate around the first to seventh rotation axes A1 to A7, respectively. Each servo motor M1 to M7 can be driven independently of each other. When the servo motors M1 to M7 are driven, the position detectors D1 to D7 set the rotational positions around the first to seventh rotation axes A1 to A7 of the servo motors M1 to M7. Detection is performed.

  The robot control device 200 is configured to include at least an arithmetic unit 210 and a storage unit 220, and is disposed around the base 2 of the seven-axis articulated robot 100. The robot control device 200 is configured by a computer, for example, and the arithmetic unit 210 and the storage unit 220 are each configured by a CPU and an internal memory. Note that the robot control device 200 may be disposed remotely from the 7-axis articulated robot 100, or may be physically connected to the 7-axis articulated robot 100 in a detachable form.

  The robot control device 200 controls the operation of the seven-axis articulated robot 100. Specifically, the robot control device 200 moves the tool member 11 attached to the attachment 9 to an arbitrary position by servo control of the servo motors included in the first to seventh joints JT1 to JT7 of the 7-axis articulated robot 100. And move along any path to posture.

The robot control device 200 can be connected to an external device (not shown) such as a teach pendant. By this external device, the robot control device 200 receives a command for translating or rotating the 7-axis articulated robot 100 in accordance with the operation of the operator. The computing unit 210 calculates a target position where the tool member 11 should be positioned after the control time has elapsed, based on the above-described command input every control time. Such a target position is calculated based on a control distance and a movement distance obtained from a predetermined limit movement speed of the tool member 11, and the tool coordinate system (Xt axis, Yt so that the calculator 210 can calculate the target position. Is converted into a format of coordinate data defined in an orthogonal coordinate system based on an axis and a Zt axis. Further, the computing unit 210 performs an inverse conversion process of the coordinate data of the target position, and calculates joint angles θ1 to θ7 necessary for moving the tool member 11 to the target position after the control time has elapsed. The computing unit 210 then calculates the first to seventh joints JT1 to JT7 based on the deviation between the calculated joint angles θ1 to θ7 and the rotational position at the time of power-on detected by the position detectors D1 to D7. The command value of the operation amount of the servo motor provided in is calculated and supplied to each servo motor. Thereby, the tool member 11 will be moved to a target position for every progress of control time.
[Formulation of inverse transformation formula]
Hereinafter, the formulation of the inverse transformation formula of the seven-axis articulated robot 100 will be described. Note that the wrist axis (A4 to A6) that defines the posture of the wrist device 10 is ignored for the time being, and the remaining four rotation axes A1, A2, A7, and A3 that define the position of the wrist device 10 are the seventh. The rotation axis A7 is used as a redundant axis, the remaining first to third rotation axes A1 to A3 are used as base axes, and the joint angles θ1 to θ3 of the joints JT1 to JT3 having the base axes A1 to A3 are handled as unknown variables. The third rotation axis A3 is a variable axis that defines a joint angle θ3 used as a variable of a quaternary equation described later.

First, the position vector [x0, y0, z0] of the first joint JT1, the position vector [x1, y1, z1] of the second joint JT2, and the position vector [x3, y3 of the third joint JT3 as viewed from the reference coordinate system. z3] are represented as “xyz0”, “xyz1”, and “xyz3”, respectively, and the joint of the jth joint JTj (jth rotational axis Aj) viewed from the joint coordinate system of the i th joint JTi (ith rotational axis Ai) When the coordinate system is expressed using “ ⁱ T _j ” that is a homogeneous transformation matrix (coordinate transformation matrix), the following equation holds. The homogeneous transformation matrix ⁱ T _j is a matrix that combines a position vector and a rotation matrix using Euler angles, and is a matrix that is widely used in the formulation of robot kinematics.

xyz1 = ¹ T ₂ · ² T ₇ · ⁷ T ₃ · xyz ₃ (6-1)
xyz0 = ⁰ T ₁ · ¹ T ₂ · ² T ₇ · ⁷ T ₃ · xyz ₃
= ⁰ T ₁ xyz1 (6-2)
When multiplying both sides of Equation (6-2) to ⁽⁰ _T ^{1) -1} from the left, the following equation holds.

_{^{(0 T 1) -1 · xyz0}} = xyz1 ··· (7)
Further, according to the equation (7), the relationship of the following equations is obtained for each of the elements x1, y1, and z1 of the position vector of the second joint JT2. In the following equation, si represents sin (θ _i ) corresponding to the joint angle θ _i of the i-th joint, and ci represents cos (θ _i ) corresponding to the joint angle θ _i of the i-th joint. Represents.

x1 = c1 · x0 + s1 · y0 (8-1)
y1 = −s1 · x0 + c1 · y0 (8-2)
z1 = z0−L0 (8-3)
Here, when the joint coordinate system of the first joint JT1 is used as a reference, the joint coordinate system of the fourth joint JT4 extended from the third joint JT3 assumed to be the position of the wrist device 10 from the origin O of the reference coordinate system. About the distance Rh and distance Zh to an origin, the relationship of following Formula is materialized using x1, y1, and z1 represented by Formula (8-1)-Formula (8-3).

Rh ² = x1 ² + y1 ² + z1 ² = x0 ² + y0 ² + (z0−L0) ² (9-1)
Zh = z0−L0 (9-2)
On the other hand, with respect to x1, y1, and z1, which are elements of the position vector of the second joint JT2, the following formulas are respectively obtained by formula (6-1) separately from formulas (8-1) to (8-3). The relationship is obtained.

x1 = -s3 * s7 * 3-c3 * s7 * y3-c7 * z3 (10-1)
y1 = (c2, s3, c7 + s2, c3), x3 + (c2, c3, c7-s2, s3), y3-c2, s7, z3 + L1 + L2, s2 (10-2)
z1 = (− s2 · s3 · c7 + c2 · c3) · x3- (s2 · c3 · c7 + c2 · s3) · y3 + s2 · s7 · z3 + L2 · c2 (10-3)
When these are used, Rh ² in the formula (9-1) can be represented by the following formula.

Rh ² = x1 ² + y1 ² + z1 ²
= 2 * L1 * [(c3 * x3-s3 * y3 + L2) * s2-{-c7 * (s3 * x3 + c3 * y3) + s7 * z3} * c2] + 2 * L2 * c3 * x3-2 * L2 * s3 * y3 + L1 ² + L2 ² + x3 ² + y3 ² + z3 ² (11-1)
Zh can be expressed as the following equation by modifying z1.

Zh = (c3 * x3-s3 * y3 + L2) * c2 + {-c7 * (s3 * x3 + c3 * y3) + s7 * z3} * c2] * s2 (11-2)
here,
F1 = c3 · x3-s3 · y3 + L2 (12-1)
F2 = −c7 · (s3 · x3 + c3 · y3) + s7 · z3 (12-2)
F3 = 2 · L2 · c3 · x3−2 · L2 · s3 · y3 + L1 ² + L2 ² + x3 ² + y3 ² + z3 ² (12-3)
Then, about Formula (11-1) and Formula (11-2), it can each deform | transform as following Formula.

(Rh ² −F 3) / (2 · L 1) = F 1 · s ² −F 2 · c 2 (13-1)
Zh = F1 · c2 + F2 · s2 (13-2)
Here, when the sides of the equations (13-1) and (13-2) are squared and summed, the following equation is established.

{(Rh ² −F 3) / (2 · L 1)} ² + Zh ² = F1 ² + F2 ² (14)
Then, by substituting Equation (11-1), Equation (11-2), Equation (12-1) to Equation (12-3) into Equation (14) and arranging for s3 and c3, the following equation is obtained. It will be established.

A · c3 ² + B · s3 ² + C · s3 · c3 + D · c3 + E · s3 + F = 0 (15)
In addition, A, B, C, D, E, and F in Formula (15) are as follows.

A = 4 · L1 ² · (x3 ² + y3 ² · c7 ² ) -4 · L2 ² · x3 ² (16-1)
B = 4 * L1 ² * (y3 ² + x3 ² * c7 ² ) -4 * L2 ² * y3 ² (16-2)
C = 4 · L1 ² · (−2 · x3 · y3 + 2 · x3 · y3 · c7 ² ) + 8 · L2 ² · x3 · y3 (16-3)
D = 4 · L1 ² · (2 · x3 · L2-2 · y3 · z3 · s7 · c7) -4 · L2 · x3 · K (16-4)
E = 4 · L1 ² · (−2 · y3 · L2-2 · x3 · z3 · s7 · c7) + 4 · L2 · y3 · K (16-5)
F = 4 * L1 ² * (L2 ² + z3 ² * s7 ² −Zh ² ) −K ² (16-6)
In Expressions (16-1) to (16-6), L1 represents the first link length and L2 represents the second link length. K is a value obtained by the following equation.

K = L1 ² + L2 ² + x3 ² + y3 ² + z3 ² -Rh ² (17)
Each value of Expression (16-1) to Expression (16-6) is a value irrelevant to s1, c1, s2, and c2, and can be obtained without information on each joint angle.

  Next, when t = tan (θ3), the following equations are established for c3 (cosine) and s3 (sine) related to the joint angle θ3 of the third joint JT3.

c3 = 2 · t / (1 + t ² ) (18-1)
s3 = (1-t ² ) / (1 + t ² ) (18-2)
Here, when the expressions (18-1) and (18-2) are substituted into the expression (15) and the terms are arranged for each order of the variable t (= tan (θ3)), the following expression is established.

(B−E + F) · t ⁴ +2 (−C + D) · t ³ + (2A−B + F) · t ² +2 (C + D) · t + (B + E + F) = 0 (19)
As described above, the joint angle θ3 can be obtained by calculating the variable t by solving the quartic equation represented by the equation (19) and calculating θ3 = atan (t).

  On the other hand, the joint angle θ2 can be obtained as follows. First, when the joint angle θ3 obtained by Expression (19) is substituted into Expression (12-1) and Expression (12-2), F1 and F2 are obtained. Here, since Zh is known, the joint angle θ2 is obtained by the following equation from the equation (13-2). It should be noted that as a calculation result of the following equation, a result satisfying the equations (13-1) and (13-2) is selected.

θ2 = atan2 (F2, F1) ± atan2 ((F2 ² + F1 ² −Zh ² ) ^0.5 , Zh) (20)
Further, the joint angle θ ₁ can be obtained as follows. First, when x1 is deleted using Expressions (8-1) to (8-3) and Expressions (10-1) to (10-3), the following expression is established.

c1 * x0 + s1 * y0 = -s3 * s7 * x3-c3 * s7 * y3-c7 * z3 (21)
Here, assuming that the right side of Equation (21) is Q and substituting the joint angle θ3 obtained by Equation (19) for both sides of Equation (21), θ1 is obtained by the following equation. It should be noted that as a calculation result of the following equation, a value satisfying the equations (8-1) to (8-3) is selected.

θ1 = atan2 (y0, x0) ± atan2 ((y0 ² + x0 ² −Q ² ) ^0.5 , Q) (22)
As described above, the inverse transformation formula of the seven-axis articulated robot 100 is formulated with respect to the joint angles θ1, θ2, and θ3 as shown in the equations (19), (21), and (22). The inverse conversion formula is formulated in advance, and the formulated inverse conversion formula is incorporated in the control program.
[Association of four first solving equations and four forms]
Next, the correspondence between the four first solution calculation expressions for obtaining the four solutions of the quaternary equation of Expression (19) and the four forms of the seven-axis articulated robot 100 will be described.

  First, the coefficients of the quaternary equation represented by Expression (19) are arranged and converted into the following expression.

t ⁴ + at ³ + bt ² + ct + d = 0 (23)
Also,
^{p = -3a 2/8 + b} ··· (24-1)
q = a ³ / 8−ab / 2 + c (24-2)
^{r = -3a 4/256 + a} 2 b / 16-ac / 4 + d ··· (24-3)
far. Here, based on “Ferrari's solution”, which is well known as a solution of the quaternary equation, a cubic equation for the variable y, which is a decomposition equation of the quaternary equation of Equation (23), is formulated as the following equation: .

8y ³ -4py ² -8ry + 4pr-q ² = 0 (25)
In addition, each coefficient of the cubic equation of Formula (25) is arranged and converted into the following formula.

x ³ + fx ² + gx + h = 0 (26)
Here, the discriminant D of the equation (26) is expressed as the following equation.

D = q ² + p ³ (27)
Note that p and q are as follows.

^{p = (g-f 2/} 3) / 3 ··· (28-1)
^{q = (h-g · f} / 3 + 2 · f 3/27) / 2 ··· (28-2)
Further, the solution of the cubic equation of Expression (25) is classified for each of the three conditions C1, C2, and C3 based on the solution of the discriminant D of Expression (27).

Condition C1 (when D <0): 3 real solutions
Condition C2 (when D = 0): real multiple solution and other real solution or real triple solution Condition C3 (when D> 0): one real solution and two imaginary solutions (conjugate complex number)
Here, in the case of the condition C2 or the condition C3, the solution of the cubic equation of the equation (25) is obtained according to the “cardano solution” that does not require the negative √ operation. The Cardano solution is a typical solution of a cubic equation. Specifically, first,
W1 = (− 1 + 3 ^0.5 · i) / 2 (29-1)
W2 = W1 ² (29-2)
U1 = −q + D ^0.5 (29-3)
U2 = −q−D ^0.5 (29-4)
In other words, the three solutions u1, u2, and u3 of the cubic equation of Equation (25) in the Cardano solution are obtained by the following equations.

u1 = 1 · U1 ^(1/3) + 1 · U2 ^(1/3) −a / 3 (30-1)
u2 = W1 · U1 ^(1/3) + W2 · U2 ^(1/3) −a / 3 (30-2)
u3 = W2 · U1 ^(1/3) + W1 · U2 ^(1/3) −a / 3 (30-3)
In addition, although U1 ^(1/3) and U2 ^(1/3) in Formula (30-1)-Formula (30-3) can consider three values, what is a real value shall be used. . In other words, when U1 or U2 is negative, the cube root of these absolute values is obtained and then multiplied by −1 to avoid calculation of an imaginary number. In the case of the condition C2 or C3, only the solution u1 that is a real number solution among the three solutions (u1, u2, u3) is calculated. In the case of the condition C2 or C3, the equation (30-1) for obtaining the real number solution u1 corresponds to the “second solution calculation equation” according to the present invention.

  Next, in the case of the condition C1, the solution of the cubic equation of the equation (25) is obtained by “the solution method of Vieta”. The Vieta's solution is a known solution of a cubic equation which is different from the Cardano's method using the triple angle formula of the trigonometric function. Specifically, the following formula is established when formula (28-1) and formula (28-2) are used. Note that Φ in the following expression represents one of the Euler angles used when defining a homogeneous transformation matrix.

cos3Φ = −q / {(− p) ³ } ^(1/2) (31)
Moreover, following Formula is materialized by Formula (31).

Φ = arccos [−q / {(− p) ³ } ^(1/2) ] / 3 (32)
In the condition C1, since the discriminant D <0 in the equation (27) is “p <0”, the content of the square root on the right side of the equation (32) is a positive number. Therefore, a negative √ operation is not performed. Three solutions u1, u2, and u3 of the cubic equation of Equation (25) in the Vieta's solution method are obtained by the following equations.

u1 = 2 (−p) ^0.5 · cos Φ−a / 3 (33-1)
^{u2 = 2 (-p) 0.5 ·} cos (Φ + 2π / 3) -a / 3 ··· (33-2)
u3 = 2 (−p) ^0.5 · cos (Φ + 4π / 3) −a / 3 (33-3)
In this case, all the solutions u1, u2, and u3 are real solutions, but any solution can be used unconditionally, and it is necessary to select a solution that provides a real solution even if the condition C2 or C3 is satisfied. Therefore, in the case of the condition C1, three solutions u1 are obtained by using the feature that the two real solutions that are conjugate complex numbers (imaginary numbers) in the condition C2 or C3 have the same absolute value and different signs (±). , U2, u3, only the remaining one real solution u1 other than the two real solutions u2, u3 having the same absolute value and different signs is specified and calculated. Note that, in the case of the condition C1, the equation (33-1) for obtaining the real number solution u1 corresponds to the “third solution calculation equation” according to the present invention.

  One real solution of the cubic equation of the equation (25) calculated by the “Cardano solution” or the “Vieta solution” as described above is represented by u. Furthermore, M1, M2, N1, and N2 are defined as follows.

M1 =-(2up) ^0.5 ... (34-1)
M2 = (2u−p) ^0.5 (34-2)
N1 = (2up−p) ^0.5 · q / 2 (2up−p) + u ... (34-3)
N2 = − (2up−p) ^0.5 · q / 2 (2up−p) + u ... (34-4)
At this time, the four solutions of the quaternary equation of Equation (20) are obtained by the first solving equation of the following equation.

t1 = (− M1− (M1 ² −4 · N1) ^0.5 ) / 2−a / 4 (35-1)
t2 = (− M1 + (M1 ² −4 · N1) ^0.5 ) / 2−a / 4 (35-2)
t3 = (-M2- (M2 ² -4 · N2) ^0.5 ) / 2-a / 4 (35-3)
t4 = (− M2 + (M2 ² −4 · N2) ^0.5 ) / 2−a / 4 (35-4)
Here, the four first solution calculation formulas (formulas (35-1) to (35-4)) for obtaining the four solutions t1 to t4 are represented by the four forms of the robot as shown in FIG. Assume that they are associated. The four first solution calculation formulas (formula (35-1) to formula (35-4)) for obtaining the four solutions t1 to t4 are the three solutions u1 of the cubic equation of formula (25), Since only the real number solution u1 is used among u2 and u3, the combination of the four solutions t1 to t4 is uniquely determined. That is, the solutions u1 to u3 are not randomly selected, and the four first solving equations (Equations (35-1) to (35-4)) are always based only on the same real solution u1. Since calculations are performed and four solutions t1 to t4 are obtained, a sudden change in the form of the seven-axis articulated robot 100 can be suppressed.
[Identification of solution of quaternary equation according to form (any one of t1 to t4)]
FIG. 3 is a flowchart showing a flow of processing for specifying one of the four first solution calculation formulas (formula (35-1) to formula (35-4)).

  First, when the powers of the seven-axis articulated robot 100 and the robot controller 200 are turned on (step S301), the calculator 210 of the robot controller 200 detects the positions provided in the first to seventh joints JT1 to JT7. The joint angles θ1 to θ7 are acquired by the devices D1 to D7. Then, the arithmetic unit 210 performs forward conversion processing on the acquired joint angles θ1 to θ7, thereby calculating the tip position (position of the wrist device 10) when the power of the robot arm of the seven-axis articulated robot 100 is turned on (step). S302).

  Next, the arithmetic unit 210 performs the above-described inverse conversion process based on the calculated tip position of the robot arm. Specifically, for the cubic equation of equation (25), only the real solution u1 is calculated according to the Vieta's solution when the condition is C1, and the real solution u1 according to the Cardano solution when the conditions are C2 and C3. Calculate only.

  Next, the computing unit 210 uses only the real number solution u1 of the calculated cubic equation of the equation (25) for the quaternary equation of the equation (20) to obtain four first solving equations (equation (35− The four solutions t1 to t4 are respectively calculated by calculating (1) to (35-4)) (step S303). Further, the joint angle θ3 corresponding to each of the calculated four solutions t1 to t4 is calculated (step S304).

  Then, the calculator 210 specifies the joint angle θ3 closest to the joint angle θ3 acquired by the position detector D3 among the four joint angles θ3 calculated in step S304, and determines the specified joint angle θ3. One of the four solutions t1 to t4 used when calculating is specified. In order to specify the one solution, the joint angle θ1 or the joint angle θ2 may be used instead of the joint angle θ3. Further, one first solution calculation formula (formula (35-1) to formula (35-4)) for obtaining the specified one solution t1 to t4 is stored in the storage device 220 (step S305). As a result, one first solving equation (Equation (35-1) to Equation (35-4)) stored in the storage device 220 is continuously used in the inverse conversion process after the power is turned on.

  FIG. 4 shows the flow of the inverse conversion process executed by using any one of the four first solution solving expressions (expressions (35-1) to (35-4)) stored in the storage device 220. It is a flowchart which shows. It is assumed that only the first solution equation (35-1) for obtaining the solution t1 of the quaternary equation is stored in the storage device 220 by the power-on process shown in FIG.

  First, when the computing unit 210 obtains the target position of the tool member 11 from an external device (step S401), using the obtained target position, three solutions (u1 to u3) of the cubic equation of Expression (25) are obtained. Only the real number solution u1 is calculated (step S402).

  Next, the arithmetic unit 210 calculates the first solution calculation formula (35-1) related to the solution t1 of the quaternary equation of the formula (20) stored in the storage unit 220 using the calculated real solution u1. Thereby, the solution t1 of the quaternary equation of Expression (20) is calculated (Step S403).

  Then, the joint angle θ3 corresponding to the calculated solution t1 is calculated (step S404). In addition, using the calculated joint angle θ3, the joint angle θ1 is calculated by the equation (22), and the joint angle θ2 is calculated by the equation (21) (step S405).

  As described above, the four first solving equations (formulas (35-1) to (35-4)) and the four solutions t1 for obtaining the four solutions t1 to t4 of the quaternary equation of the formula (20). The four forms 1 to 4 that can be taken according to ~ t4 are associated in advance, and the four first solving equations (formulas (35-1) to (35-4)) are based on the form when the power is turned on. Any one of them is used in the inverse conversion process after the power is turned on. As a result, it is possible to suppress a sudden change in the form of the seven-axis articulated robot 100.

  Furthermore, in order to specify the initial form of the seven-axis articulated robot 100 when the power is turned on, each of the four first solving equations (Equations (35-1) to (35-4)) is calculated only once. In the inverse conversion process after power-on, it is not necessary to calculate the remaining three first solution calculation expressions other than the specified first solution calculation expression, so the amount of calculation required for the inverse conversion process Thus, the control of the seven-axis articulated robot 100 can be speeded up.

  Further, in order to solve the quaternary equation of Equation (20), the cubic equation of Equation (25) is solved. However, as long as it is the cubic equation, three solutions u1 to u3 are generated. Therefore, if any one of the three solutions u1 to u3 is selected in advance, it is possible to suppress variations in the combination of the four solutions t1 to t4 of the quaternary equation.

  In addition, when solving the quartic equation when the power is turned on, it is not necessary to perform the complex number calculation by using the Cardano method and the Vieta method separately, so the amount of calculation can be reduced. This makes it easy to mount on a robot system.

  From the foregoing description, many modifications and other embodiments of the present invention are apparent to persons skilled in the art. Accordingly, the foregoing description should be construed as illustrative only and is provided for the purpose of teaching those skilled in the art the best mode of carrying out the invention. The details of the structure and / or function may be substantially changed without departing from the spirit of the invention.

  The present invention is useful for the inverse transformation processing of a seven-axis articulated robot.

2 base 3 swivel 4 to 8 arm member 9 attachment 10 wrist device 11 tool members JT1 to JT7 first to seventh joints A1 to A7 first to seventh rotation shafts M1 to M7 servo motors D1 to D7 position detector L0 ˜L5 1st to 6th link length 100 7-axis articulated robot 200 Robot control device 210 Computing unit 220 Memory

Claims (6)

A wrist provided at the distal end and seven joints provided in order from the proximal end toward the wrist, the seven joints each having a rotation axis, and a next joint around each rotation axis. A control method for a seven-axis articulated robot configured to rotate,
Any one of the seven rotation axes as a redundant axis, three of the remaining rotation axes as a base axis, and any one of the three base axes as a variable axis; and The joint position having the variable axis and the position of the wrist based on the linear distance from the base end to the wrist and the distance from the base end to the wrist in the extending direction of the rotation shaft closest to the base end Solving the quaternary equation formulated with respect to angles and inversely transforming the wrist position into the joint angles of each of the joints having the three base axes;
The quaternary equation can be solved using the four first solution calculation equations corresponding to the four solutions, respectively, and the form that the 7-axis articulated robot can take is the four The solution is determined corresponding to the four first solving equations,
When turning on the power, based on the initial form of the 7-axis articulated robot at the time of turning on the power, any one of the four first solution calculation formulas is specified and stored,
After the power is turned on, the inverse transformation step is performed using the stored first first solving equation to calculate the joint angle of each joint having the three base axes from the wrist target position, Thereby, the control method of the 7-axis articulated robot, wherein the operation of the 7-axis articulated robot is controlled.   When the power is turned on, the four first-order equations are used to solve the quartic equation to obtain the four solutions, and the position of the wrist when the power is turned on for each of the four solutions is determined based on the three basic axes. Is converted back to at least one of the joint angles of each joint having the above-mentioned three joint angles corresponding to the joint angle of the joint having the one base axis and the initial form of the seven-axis articulated robot when the power is turned on 2. The method according to claim 1, wherein one of the four first solving equations of the quaternary equation is specified and stored by comparing a joint angle of a joint having one of the basic axes. The control method of the 7-axis articulated robot as described. The rotation axes of all adjacent joints are perpendicular to each other;
Seven joints provided in order from the proximal end toward the wrist are defined as a first joint, a second joint, a seventh joint, a third joint, a fourth joint, a fifth joint, and a sixth joint, respectively. The redundant axis is a rotation axis of the seventh joint, the three base axes are a rotation axis of the first joint, a rotation axis of the second joint, and a rotation axis of the third joint; and The method of controlling a seven-axis articulated robot according to claim 1, wherein the variable axis is a rotation axis of the third joint.   Included in the three solutions of the cubic equation formulated as a decomposition equation for solving the quartic equation based on the Ferrari solution when the quartic equation is solved using the four first solving equations. 4. The 7-axis articulated robot according to claim 1, wherein one real solution is calculated, and the four first solution calculation equations are calculated using one real solution of the calculated cubic equation. Control method.   When obtaining one real solution included in the three solutions of the cubic equation, if the discriminant of the cubic equation is 0 or positive, one real solution of the cubic equation obtained by the Cardano solution And the second solution calculation equation based on the Cardano solution corresponding to the one real solution among the two imaginary solutions, and when the discriminant of the cubic equation is negative, the solution obtained by the Vieta solution 5. The third solving equation based on the Vieta solution corresponding to one real solution other than two real solutions having the same absolute value and different signs among the three real solutions of the cubic equation is used. The control method of the 7-axis articulated robot as described. A wrist provided at the distal end and seven joints provided in order from the proximal end toward the wrist, the seven joints each having a rotation axis, and a next joint around each rotation axis. A control program for a 7-axis articulated robot that causes a computer to execute a control method for a 7-axis articulated robot configured to rotate,
The control method is:
Any one of the seven rotation axes as a redundant axis, three of the remaining rotation axes as a base axis, and any one of the three base axes as a variable axis; and The joint position having the variable axis and the position of the wrist based on the linear distance from the base end to the wrist and the distance from the base end to the wrist in the extending direction of the rotation shaft closest to the base end Solving the quaternary equation formulated with respect to angles and inversely transforming the wrist position into the joint angles of each of the joints having the three base axes;
The quaternary equation can be solved using the four first solution calculation equations corresponding to the four solutions, respectively, and the form that the 7-axis articulated robot can take is the four The solution is determined corresponding to the four first solving equations,
When turning on the power, based on the initial form of the 7-axis articulated robot at the time of turning on the power, any one of the four first solution calculation formulas is specified and stored,
After the power is turned on, the inverse transformation step is performed using the stored first first solving equation to calculate the joint angle of each joint having the three base axes from the wrist target position, Thereby, a control program for the seven-axis articulated robot for controlling the operation of the seven-axis articulated robot.

Images