JP6891773B2 - Robot control device and robot inverse transformation processing method - Google Patents

Description

The present invention relates to a device for controlling the operation of a robot having an offset arm connecting the fourth axis and the sixth axis, and a method for inversely transforming a hand position.

A general vertical 6-axis type robot arm has a structure in which the axes of the 4th to 6th axes intersect at one point. In such a robot arm, since the fifth axis is rotated to displace the wrist portion including the sixth axis, a gap is formed in the portion of the tip of the arm that holds the wrist. If there is such a gap, there is a possibility that foreign matter may be caught. Therefore, an industrial robot in a form in which a gap in a portion where the wrist is displaced is eliminated by providing an offset arm for connecting the fourth axis and the sixth axis is disclosed in, for example, Design Registration 1583755. In this embodiment, due to the presence of the offset arm, the axis of the fourth axis and the axis of the sixth axis do not intersect and are in a parallel relationship with each other.

Assuming that an arm of such a form is subjected to an inverse transformation process for obtaining the angle of each axis from the position and posture of the hand, it is difficult to perform with the conventional method. For example, what is disclosed in Patent Document 1 is a technique that makes it possible to perform an inverse transformation process based on a DH parameter even when an error during robot assembly is included. In Patent Document 1, assuming that the length of the offset arm is d5, on the premise that d5 = 0, the convergence operation is performed by assuming a minute region corresponding to the error and linearizing it.

JP-A-2017-61022

Assuming that the method of Patent Document 1 is applied to a robot with d5 ≠ 0, d5 as an initial error of the convergence operation becomes a large value, so that the number of repetitions of the operation becomes enormous and the operation does not converge. There is a possibility of divergence. Further, since there is an offset portion, the offset portion is not correctly reflected even within the range that the reach originally reaches, and there is a possibility that it is erroneously determined that the reach does not reach.

The present invention has been made in view of the above circumstances, and an object of the present invention is to provide a robot control device capable of performing inverse transformation processing even for a robot provided with an offset arm, and a robot inverse transformation processing method. There is.

The robot control device according to claim 1 has a fifth axis and has an offset arm having a link length d5 that connects the fourth axis and the sixth axis, whereby the axis of the fourth axis and the sixth axis are provided. The control target is a robot equipped with a vertical 6-axis type arm having a structure parallel to the axis of the robot. Then, the tip of the arm is set as a control point, and the angle of each axis is calculated by performing inverse transformation processing on the target position and posture of the control point.

The position tentative determination unit tentatively determines the position of the sixth axis, and tentatively determines the position of the fifth axis to P5'based on the tentatively determined position P6'. The operation range determination unit determines whether or not the positions P5'and P6'are within the operation range based on the link parameter of the robot. If the positions P5'and P6'are within the operating range, the inverse transformation processing unit performs the inverse transformation processing by setting the link length d5 to "0" for the homogeneous transformation matrix based on those positions. Further, the forward conversion processing unit performs the forward conversion processing using the angles of the respective axes obtained by the reverse conversion processing.

_{The evaluation value calculation unit obtains the position matrix p E} of the difference between the target position of the control point and the position obtained by the forward conversion process, and rotates the rotation matrix corresponding to the target position by the forward conversion process. Multiply the inverse matrix of the matrix to find the _{rotation matrix R E. Then, when the norm of the position matrix p E} exceeds the threshold value or the angle obtained from the _{rotation matrix R E} exceeds the threshold value, the iterative calculation execution unit reflects the _{position matrix p E} and the rotation matrix R _E. The processing from the inverse conversion processing is repeatedly executed by the simultaneous conversion matrix. That is, if the norm of the _{position matrix p E is equal to or less than the threshold value and the angle obtained from the rotation matrix R E} is equal to or less than the threshold value, the inverse transformation process is completed. With this configuration, even for a robot having an offset arm with a link length d5 that connects the 4th axis and the 6th axis, the angle of the 6th axis is tentatively determined, the inverse transformation process is performed, and the processing result is obtained. _{By evaluating the matrix p E} and the matrix R _E obtained by the forward transformation process and converging the calculation, the inverse transformation process can be performed.

According to the robot control device according to claim 2, the inverse transformation processing unit has the positions P5'and P6'so that if the positions P5'and P6'are not within the operating range, they are located within the operating range. To fix. As a result, the inverse transformation process can be continuously executed.

According to the robot control device according to claim 3, the iterative calculation execution unit determines whether or not the position of the control point determined by the linear transformation matrix is within the operation range as in the operation range determination unit. Then, if the position of the control point is not within the operating range, the homogeneous transformation matrix is modified so that it is located within the operating range. As a result, the iterative operation can be continuously executed.

According to the robot control device according to claim 4, the morphological determination unit calculates the determinant of the Jacobian matrix with respect to the result of the inverse transformation processing. Then, the wrist form of the inverse transformation processing result is determined depending on whether or not the code of the determinant matches the wrist form specified in advance. For example, when the movable range of each axis is -180 degrees to 180 degrees, when the wrist morphology is visually confirmed by moving the five axes, a maximum of two arm morphologies, two elbow morphologies and four wrist morphologies can be combined. It turns out that

The point that separates the boundaries of the four wrist forms should be a singular point, and the Jacobian matrix has a determinant of zero at the singular point. Therefore, for each combination of forms, it can be determined whether or not the form is specified by whether or not the wrist form of the inverse transformation processing result and the sign of the determinant of the Jacobian matrix correspond to each other. That is, a maximum of four wrist morphologies can be discriminated by combining with whether the angles of the four axes belong to -90 degrees to 90 degrees, -180 degrees to -90 degrees, or 90 degrees to 180 degrees.

The figure which is 1st Embodiment and shows the structure of the robot system. Diagram showing the coordinate system of the robot The figure which shows the structure of a robot in xz plane and yz plane The figure which shows the arm form of a robot by xy coordinates and αr coordinates The figure which shows the arm form of a robot by αz coordinates The figure which shows each form of the arm defined by αz coordinates Flowchart showing the inverse transformation process performed by the controller The figure explaining the operating range when d2 = d3 The figure explaining four wrist forms which a robot arm can take about the same hand position.

Hereinafter, one embodiment will be described with reference to FIGS. 1 to 9. As shown in FIG. 1, the robot system 1 includes a vertically articulated robot 2 and a controller 3 for controlling the robot 2 inside the base 4. This robot system 1 is used for general industrial use. The robot 2 is a so-called 6-axis vertical articulated robot. On the base 4, a first axis having an axis in the Z direction; a shoulder 5 is rotatably connected in the horizontal direction via J1. The shoulder 5 is provided with a second axis having an axis in the Y direction; J2, and the lower end portion of the first arm 7 extending upward can be rotated in the vertical direction via the second offset arm 6 extending in the Y direction. It is connected. The tip of the first arm 7 is provided with a third axis having an axis in the Y direction; J3, and the second arm 9 can rotate in the vertical direction via a third offset arm 8 extending in the −Y direction. It is connected. The second arm 9 includes a base portion 9a and a tip portion 9b.

The second arm 9 includes a fourth axis having an axis in the X direction; J4, and the tip portion 9b is twistably and rotatably connected to the base portion 9a. The tip of the second arm 9 is provided with a fifth axis; J5 having an axis in the Y direction, and the wrist 11 is rotatably connected in the vertical direction via a fifth offset arm 10 extending in the −Y direction. ing. A flange and a hand 12 shown in FIG. 2 are rotatably connected to the wrist 11 via a sixth axis having an axis in the X direction; J6. Each axis provided in the robot 2; J1 to J6 are provided with motors (not shown) serving as drive sources corresponding to the respective axes.

The controller 3 is a control device for the robot 2, and controls the robot 2 by executing a computer program in a control means including a computer composed of a CPU, a ROM, a RAM, and the like (not shown). Specifically, the controller 3 includes a drive unit composed of an inverter circuit or the like, and each of them is controlled by feedback, for example, based on the rotation position of the motor detected by the encoder provided corresponding to each motor. Drive the motor.

The controller 3 includes a CPU, a ROM, a RAM, a drive circuit, a position detection circuit, and the like. The ROM stores a system program, an operation program, and the like of the robot 2. The RAM stores parameter values and the like when executing these programs. The controller 3 corresponds to an angle temporary determination unit, a direction calculation unit, a temporary target position calculation unit, an inverse transformation processing unit, an evaluation unit, and a form determination unit. A detection signal of each encoder (not shown) provided at each joint of the robot 2 is input to the position detection circuit. The position detection circuit detects the rotation angle position of the motor provided in each joint based on the detection signal of each encoder.

The controller 3 controls the position and orientation of the control point at the tip of the arm based on the position information input from the position detection circuit by executing a preset operation program. In the present embodiment, the controller 3 performs CP (Continuous Path) control. In CP control, when the control point at the tip of the arm is operated to the target, the target position and posture of the control point, that is, the operation trajectory is set as a time function. The target position and posture include, in addition to the taught position and posture, the position and posture interpolated based on the taught position and posture. The controller 3 controls the angle of each joint in the arm by CP control so that the position and posture of the control point follow the operation trajectory. In the control of the position and the posture, the controller 3 performs an inverse transformation process for calculating the angles of the first axis to the sixth axis for realizing the target position and the posture currently instructed.

As shown in FIG. 2, each joint of the robot 2, first to sixth coordinate system Σ ₁ ~Σ ₆ is defined as a three-dimensional orthogonal coordinate system. The origins of the coordinate systems Σ _{1 to Σ 6} are defined at predetermined positions on the first to sixth axes J1 to J6. Z1~Z6 axis is the z-axis of each coordinate system Σ ₁ ~Σ ₆ coincides with the first to sixth axis J1 to J6.

The base 4 defines a 0th coordinate system Σ ₀ , which is a robot coordinate system. The 0th coordinate system Σ ₀ is a coordinate system that does not change even if the 1st to 6th axes rotate. In the present embodiment, _{the origin of the coordinate system Σ 0} is defined on the first axis J1. Further, Z0 axis is the z axis of the coordinate system sigma ₀ is coincident with the first axis J1.

D1 to d6, a2 and a3 shown in FIG. 2 are defined as follows.
d1: Link length from the origin of the 0th coordinate system Σ _{0 to} the origin of the 1st coordinate system Σ ₁ d2: Link length from the origin of the 1st coordinate system Σ ₁ to the base of the 1st arm 7 a2: 1st arm 7 Link length from the base to the tip of the second coordinate system Σ _{2 to} the origin of the second coordinate system Σ ₂ d3: Link length from the origin of the second coordinate system Σ 2 to the tip of the third offset arm 8 a3: Third axis J3, 3rd Distance between the axes of the 4-axis J4 d4: _{Link length from the origin of the 3rd coordinate system Σ 3 to} the origin of the 4th coordinate system Σ ₄ d5: From the origin of the 4th coordinate system Σ _{4 to} the origin of the 5th coordinate system Σ ₅ Link length to d6: See FIG. 3 for the link length a3 _{from the origin of the 5th coordinate system Σ 5 to} the origin of the 6th coordinate system Σ _6. The d5 corresponds to the link length of the fifth offset arm 10.

First, the forward conversion process will be described as a premise for explaining the inverse conversion process in the present embodiment.
<Forward conversion process>
First, the DH parameters were determined as shown in Table 1 by coordinate transformation in the order of z-axis rotation, z-axis movement, x-axis movement, and x-axis rotation. θ _i is the rotation angle of each joint from the state shown in FIG.

The homogeneous transformation matrix from the base coordinates Σ ₀ to the mechanical interface coordinates Σ _{6 is as follows.} n, o, and a indicate a normal vector, an orient vector, and an approach vector, respectively. In order to simplify the notation, sinθ _i and cosθ _i are described as s _i and c _i , respectively. Further, for example, S ₂₃ indicates sin (θ ₂ + θ ₃ ).

When the equations (8) to (10) are expanded, the equations (13) to (15) are obtained.

The normal, orient, and approach vectors and position coordinates for the 5th and 6th axes are as follows.

Since it is a homogeneous transformation matrix, it can be converted to a position vector at _{base coordinates Σ 0} by multiplying the position vector on the tool coordinates by the product from the left. That is, the tip position in the base coordinates can be obtained from the joint angle and the DH parameter. The attitude angle is represented by the normal, orient, and approach vectors n ₆ , o ₆ , and a _{6 on the sixth axis.}

<Inverse conversion processing>
Next, the inverse transformation process will be described. It is assumed that the position and orientation of the hand of the robot arm are given by the simultaneous transformation matrix. _{Let p i be} the position vector at the base coordinate Σ ₀ in FIG. First, the wrist position p ₅ is obtained from the approach vector a _6.
p ₅ = p ₆ −d ₆ a ₆ … (20)

In the case of a conventional 6-axis robot, since the rotation axes of each of the 4, 5 and 6 axes are orthogonal to each other at the wrist position, the 1, 2 and 3 axes can be analytically obtained first with respect to the wrist position. All joint angles can be obtained. That is, since the wrist position is on the rotation axes of the 4, 5, and 6 axes, the wrist position does not change even if the angles of the 4, 5, and 6 axes are changed. On the other hand, in the case of the 6-axis robot 2 of the present embodiment, p ₄ changes depending on the 4, 5, and 6-axis angles, so that it cannot be solved by the same method as the conventional method.

Therefore, a method of solving by assuming a 6-axis angle is shown. Assuming that the rotation range of the 6 axes is, for example, −180 degrees to 180 degrees, the search is performed assuming an appropriate step size within that range. This rotation range may be referred to as SINGLE. Once the six-axis angle, (11), it can be determined p ₄ from (12).
p ₄ = a ₄ d ₆ + p ₆
= O ₆ d ₆ + p ₆
= (O ₆ s ₆ + o ₆ c ₆ ) d ₆ + p ₆ … (21)

Here, as shown in FIG. 4, the αr coordinate is taken on the axis obtained by rotating the xy axis by one axis angle. However, since the link parameter a ₁ = 0, the formula is different when _{a 1 ≠ 0.} Since p _{4 is fixed, the α coordinate of p 4} can be expressed by an equation that does not use the uniaxial angle. Here, the arm morphology of _{α 14} ≧ 0 is defined as LEFTY, and the arm morphology of _{α 14 <0 is defined as RIGHTY.} If α ₁₄ is an imaginary number, there is no solution because the reach does not reach.

_{Note that l 14 in} Eq. (24) is the distance from the origin of the αr coordinate to p _4.
Therefore, the trigonometric function of the axial angle can be obtained as in Eqs. (25) to (28).

Next, as shown in FIG. 5, the robot 2 is considered in the αz plane seen from the side surface. 2,3 axis angle satisfying p ₄ are present two sets. Equations (29) and (30) are established from the arm lengths from p ₁ to p _{2 and the arm lengths from p 2} to p _4.

By expanding Eq. (30) and arranging it according to Eq. (29), p ₂ can be obtained.

If k ₂ <0, there is no solution because the reach does not reach. z _{12 is} also obtained.

Substituting Eqs. (44) and (45) into Eq. (29), the condition is that the equal sign of Eq. (52) is established.

Since the following equation holds from FIG. 5, it is possible to obtain trigonometric functions of two or three axes.
By using atan2 (y, x), the 1, 2, and 3 axis angles can be obtained.

Next, as shown in FIG. 6, when the arm forms BELOW and ABOVE are defined,
In the case of LEFTY _{, ABOVE is obtained when θ 3} > φ3, and BELOW is obtained _{when θ 3 <φ3.} Also,
In the case of RIGHTY, when θ ₃ > φ3, it becomes BELOW, and when θ ₃ <φ3, it becomes ABOVE. It can also be determined by the code of _{z 12.} φ3 is represented by the equation (64).
φ3 = atan2 (a ₃ , d ₄ )… (64)

_{Then angle .theta.5,} seek theta _6.

_{The angle θ 5} can be obtained from Eq. (* 4). _{Further, since the sin θ 6} and the cos θ ₆ can be obtained from the equations (* 4), (* 1), and (* 2), the angle θ ₆ can be obtained from them.

_{Since sin θ 4} and cos θ ₄ can be obtained from the equations (* 7) and (* 8) _{, the angle θ 4} can be obtained from this. In addition, although there is a double number of ± in the formula (* 4), a code that satisfies the condition of the 4-axis angle or the 5-axis angle is adopted in the designation of the wrist form. Further, since the angle θ ₅ is obtained first, the equation (* 8) _{may be divided by cos θ 5} , or the equations (* 9) and (* 10) may be used instead.

The above is the outline of the inverse transformation process. Next, the operation of this embodiment will be described with reference to FIGS. 7 and 8. FIG. 7 is a flowchart showing the contents of the inverse transformation process performed by the controller 3. First, if the position of the 6-axis is tentatively determined to be P6', the position of the 5-axis is tentatively determined from the position P6'to be P5'. Then, the homogeneous transformation matrix T'is determined using these (S1). Then, it is determined whether or not the tentatively determined position P5'is within the operating range (S2).

FIG. 8 is a diagram illustrating the operating range, which is premised on the case where d2 = d3. A sphere A having a radius L1 and a sphere B having a radius (L1 + L2) are set. L1 and L2 are defined by Eqs. (* 11) and (* 12), respectively.

If the position P5'is outside the sphere A (S2; outside the operating range), the distance from the position determined by the homogeneous transformation matrix T'to the boundary of the operating range is L (S9). Subsequently, it is determined whether or not the distance L is the link length d5 or less (S10). This is a determination as to whether or not the tentatively determined position P6'is within the operating range. If the distance L exceeds the link length d5, a (NO) error occurs and the process ends.

On the other hand, if the distance L is the link length d5 or less (YES), the position P5'is modified to move inside the sphere A (S14). For example, the position P5'may be moved to the intersection of the straight line connecting the position P1 and the position P5'and the sphere A. As the position P5'is moved, the position P6'is also moved. In step S10, it may be determined whether or not the position P5'is outside the sphere B by setting L2 = d5.

If d2 ≠ d3, it is determined whether or not the operation range is within or outside the operation range as follows. The operation range is out of the range in step S2 when the inside of the square root in the equation (24) or the equation (44) becomes negative. Here, if a modification is made so as to be within the operating range, in the case of equation (24), l ₁₄ ² = r ₁₄ ²
It should be done. Therefore, if L = | r ₁₄ |-| l ₁₄ |
w = L / | l ₁₄ |, [wx _{4 by 40} ] ^T
You only have to correct the position.

Also, in the case of formula (44)

When it is determined in step S2 that it is within the operating range, and when step S14 is executed, the inverse conversion process is performed (S3). Here, g ^-1 (T', F) is the _{simultaneous transformation matrix T', where d 5} = 0, and the angle θ'= [θ ₁ ', θ ₂ ', θ ₃ ', θ _{4 from the form F of the robot 2.} ', θ ₅ ', θ ₆ '] This is a function to find ^T.
Next, the forward transformation process is performed using the angle θ'obtained as a result of the inverse transformation (S4). Let the simultaneous transformation matrix obtained as a result of this forward transformation process be T "and the form be F". The simultaneous transformation matrix T "includes a rotation matrix R" and a position matrix p "as elements.

Next, the difference between the target position p and the above position p "is obtained, and the position error vector p _E is obtained. Further, the rotation matrix R corresponding to the target position p is multiplied by the inverse matrix of the rotation matrix R" for evaluation. The rotation matrix R _E is obtained (S5). The norm || p _E || Do less than the threshold value p _EM vector p _E (S6), also if the (S7) to each determined angle || R _E || rotation matrix R _E is the threshold R _EM less .. _{Here, || p E ||, || R} E || , respectively (* 13) is defined by (* 14).

If "YES" is determined in step S7, it is determined whether or not the wrist form determined by the position p "and the rotation matrix R" is the specified form (S8), and if the specified form is obtained, the inverse transformation process is terminated. To do. On the other hand, if it is determined as "NO" in steps S6 or S7, it is determined whether or not the number of repeated executions of the inverse conversion process and the forward conversion process exceeds a predetermined number (S11). Here, if the specified number is exceeded, the process ends as a (YES) error. If the specified number is not exceeded (NO), the rotation matrix _{R'is multiplied by the evaluation rotation matrix R E} , and the position error vector p _E is added to the position matrix p'to update the simultaneous transformation matrix T'(S12). .. Then, the inside / outside of the operating range is determined in the same manner as in step S2 (S13), and if it is within the operating range, the process proceeds to step S3, and if it is outside the operating range, the process proceeds to step S14.

Next, the processing determination in step S8 will be described. As shown in FIG. 9, in the case of a robot having d5 ≠ 0 by having the third offset arm 8 as in the present embodiment, there are a maximum of four wrist forms for one hand position, and in step S9. There are a maximum of four 6-axis angles that satisfy the evaluation. Therefore, in step S8, the Jacobian matrix J is used in order to uniquely determine the wrist morphology obtained by the inverse transformation process. The points that divide these boundaries should be singular points for the above-mentioned four existing 6-axis angles, and the Jacobian matrix J has a determinant of zero at the singular points.

A maximum of four by combining the sign of the Jacobi matrix, the conventional way of thinking of the wrist form, and whether the four-axis angle is -90 degrees to 90 degrees, -180 degrees to -90 degrees, or 90 degrees to 180 degrees. The existing wrist morphology FLIP +, FLIP-, NONFLIP +, NONFLIP- can be identified.

That is, in addition to the arm morphology RIGHTY, LEFTY and the elbow morphology ABOVE, BELOW, there are combinations of wrist morphology FLIP +, FLIP-, NONFLIP +, NONFLIP-. Therefore, in step S8, the determinant of the Jacobian matrix J is calculated. The Jacobian matrix J is represented by equations (87) to (99).

Then, it is determined whether or not the code of the calculated determinant matches the designated wrist form.

As described above, according to the present embodiment, the controller 3 has 5 axes arranged and has a third offset arm 8 having a link length d5 that connects the 4 axes and the 6 axes, so that the controller 3 has the axis of the 4 axes. The control target is a robot 2 having a vertical 6-axis type arm having a structure in which the 6-axis axes are parallel to each other. Then, the controller 3 uses the hand, which is the tip of the arm, as a control point, and calculates the angle of each axis by performing inverse transformation processing on the target position and posture of the control point.

Further, when the controller 3 tentatively determines the position of the sixth axis and tentatively determines the position of the fifth axis to P5'based on the tentatively determined position P6', the positions P5'and P6'are set to the robot 2. Determine if it is within the operating range based on the link parameters. If the positions P5'and P6'are within the operating range, the link length d5 is set to "0" for the homogeneous transformation matrix based on those positions, and the inverse transformation process is performed. Then, the forward transformation process is performed using the angles of each axis obtained by the inverse transformation process.

_{Further, the position error vector p E} of the difference between the target position of the control point and the position obtained by the forward conversion process is obtained, and the rotation matrix corresponding to the target position is the inverse of the rotation matrix obtained by the forward conversion process. Multiply the matrix to find the _{rotation matrix R E.} Then, when the norm of the position error vector p _E exceeds the threshold value or the _{angle obtained from the rotation matrix R E} exceeds the threshold value, the simultaneous transformation matrix reflecting _{the position error vector p E} and the rotation matrix R _{E is reflected.} Repeats the process from the inverse conversion process. If the norm of the position error vector p _{E is equal to or less than the threshold value and the angle obtained from the rotation matrix R E} is equal to or less than the threshold value, the inverse transformation process is completed.

With this configuration, even for the robot 2 having an offset arm with a link length d5 that connects the 4th axis and the 6th axis, the angle of the 6th axis is tentatively determined and the inverse transformation process is performed, and the processing result is obtained. _{By evaluating the matrix p E} and the matrix R _E obtained by the forward transformation process and converging the calculation, the inverse transformation process can be performed.

Further, if the positions P5'and P6'are not within the operating range, the controller 3 modifies the positions P5'and P6' so that they are located within the operating range. Similarly, for the position of the control point determined by the homogeneous transformation matrix T', it is determined whether or not it is within the operating range, and if it is not within the operating range, the homogeneous transformation matrix T'is located within the operating range. Fix it. As a result, the inverse transformation process and the iterative operation can be continuously executed.
Then, the controller 3 calculates the determinant of the Jacobian matrix J with respect to the result of the inverse transformation processing. Then, the wrist form of the inverse transformation processing result can be uniquely determined depending on whether or not the code of the determinant matches the wrist form specified in advance.

The present invention is not limited to the embodiments described above or in the drawings, and the following modifications or extensions are possible.
The controller 3 may be arranged outside the base 4 of the robot 2.

In the drawing, 1 is a robot system, 2 is a robot, 3 is a controller, 4 is a base, 5 is a shoulder, 6 is a second offset arm, 7 is a first arm, 8 is a third offset arm, and 9 is a second arm. 10 indicates a fifth offset arm, 11 indicates a wrist, and 12 indicates a hand.

Claims (8)

A vertical structure in which the fifth axis is arranged and the axis of the fourth axis and the axis of the sixth axis are parallel to each other by having an offset arm having a link length d5 that connects the fourth axis and the sixth axis. It is applied to a robot equipped with a 6-axis type arm, and the angle of each axis is calculated by inversely transforming the target position and posture of the control point with the tip of the arm as a control point.
A position tentative determination part that tentatively determines the position of the 6th axis and tentatively determines the position of the 5th axis to P5'based on the tentatively determined position P6'.
An operation range determination unit that determines whether or not the positions P5'and P6'are within the operation range based on the link parameter of the robot.
If the positions P5'and P6'are within the operating range, the inverse transformation processing unit that performs the inverse transformation processing by setting the link length d5 to "0" for the homogeneous transformation matrix based on those positions.
A forward conversion processing unit that performs forward conversion processing using the angles of each axis obtained by the reverse conversion processing, and a forward conversion processing unit.
_{The position matrix p E} of the difference between the target position of the control point and the position obtained by the forward conversion process is obtained, and the rotation matrix corresponding to the target position is the rotation matrix obtained by the forward conversion process. Multiply the inverse matrix to obtain the rotation matrix R _E.
If the norm of the position matrix p _E exceeds the threshold value, or if the _{angle obtained from the rotation matrix R E} exceeds the threshold value, the simultaneous transformation matrix reflecting the _{position matrix p E} and the rotation matrix R _{E is reflected.} A robot control device including a repetitive calculation execution unit that repeatedly executes a process from the inverse conversion process. The robot control device according to claim 1, wherein the inverse transformation processing unit modifies the positions P5'and P6'so that the positions P5'and P6'are located within the operating range if the positions P5'and P6'are not within the operating range. .. The iterative calculation execution unit determines whether or not the position of the control point determined by the homogeneous transformation matrix is within the operating range, and if the position of the control point is not within the operating range, the operation is performed. The robot control device according to claim 1 or 2, wherein the homogeneous transformation matrix is modified so as to be located within the range. A morphological determination unit that calculates the determinant of the Jacobian matrix for the result of the inverse transformation processing and determines the wrist form of the inverse transformation processing result depending on whether or not the sign of the determinant matches the wrist form specified in advance. The robot control device according to any one of claims 1 to 3, further comprising. A vertical structure in which the fifth axis is arranged and the axis of the fourth axis and the axis of the sixth axis are parallel to each other by having an offset arm having a link length d5 that connects the fourth axis and the sixth axis. It is applied to a robot equipped with a 6-axis type arm, and the angle of each axis is calculated by inversely transforming the target position and posture of the control point with the tip of the arm as a control point.
A step of tentatively determining the position of the 6th axis and tentatively determining the position of the 5th axis to P5'based on the tentatively determined position P6'.
A step of determining whether or not the positions P5'and P6'are within the operating range based on the link parameter of the robot, and
If the positions P5'and P6'are within the operating range, the step of performing the inverse transformation process by setting the link length d5 to "0" for the homogeneous transformation matrix based on those positions.
A step of performing forward conversion processing using the angles of each axis obtained by the reverse conversion processing, and
_{The position matrix p E} of the difference between the target position of the control point and the position obtained by the forward conversion process is obtained, and the rotation matrix corresponding to the target position is the rotation matrix obtained by the forward conversion process. The step of multiplying the inverse matrix to obtain the rotation matrix R _E,
If the norm of the position matrix p _E exceeds the threshold value, or if the _{angle obtained from the rotation matrix R E} exceeds the threshold value, the simultaneous transformation matrix reflecting the _{position matrix p E} and the rotation matrix R _{E is reflected.} A method for reverse conversion processing of a robot that is repeatedly executed from the step of performing the reverse conversion processing. The inverse transformation processing method for a robot according to claim 5, wherein if the positions P5'and P6'are not within the operating range, the positions P5'and P6' are modified so as to be located within the operating range. It is determined whether or not the position of the control point determined by the homogeneous transformation matrix is within the operating range, and if the position of the control point is not within the operating range, it is located within the operating range. The inverse transformation processing method for a robot according to claim 5 or 6, wherein the homogeneous transformation matrix is modified. A morphological determination unit that calculates the determinant of the Jacobian matrix for the result of the inverse transformation processing and determines the wrist form of the inverse transformation processing result depending on whether or not the determinant of the determinant matches the wrist form specified in advance. The inverse transformation processing method for a robot according to any one of claims 5 to 7.

Images