JP2019093487A - Robot control device and robot reverse conversion processing method - Google Patents

Abstract

To provide a robot control device which can perform reverse conversion processing of a robot even when the same is mounted with an offset arm.SOLUTION: A robot control device controls a robot 2 comprising a vertical 6-axis arm with a structure where a third offset arm 8, which is provided with a fifth axis connecting a fourth and sixth axes and has a link length d5, enables axial centers of fourth and sixth axes to be parallel to each other. With a hand at a tip section of the arm as a control point, the robot control device calculates angles of respective axes through reverse conversion processing of a target position of the control point and a posture. Then, the robot control device; tentatively sets a position of the sixth axis; also tentatively sets the positions of the fifth axis at P5' on the basis of the tentatively set position P6' of the sixth axis; and determines whether or not the tentative positions P5' and P6' are within an operating range on the basis of a link parameter of the robot 2. When the tentative positions are determined to be within the operating range, the robot control device performs: the reverse conversion processing with the link length d5 set at 0 with respect to a simultaneous conversion matrix on the basis of the tentative positions; and forward conversion processing using the angles of respective axes obtained through the reverse conversion processing.SELECTED DRAWING: Figure 7

Description

  The present invention relates to an apparatus for controlling the operation of a robot having an offset arm connecting a fourth axis and a sixth axis, and a method for reverse conversion processing of the hand position.

  A typical vertical six-axis robot arm has a structure in which the axes of the fourth to sixth axes intersect at one point. In such a robot arm, in order to rotate the fifth axis to displace the wrist portion including the sixth axis, a gap is formed at the tip portion of the arm that holds the wrist. If there is such a gap, there is a possibility that the foreign matter will be caught. Therefore, an industrial robot is disclosed in, for example, design registration 1583755 or the like, in which the gap in the portion where the wrist is displaced is eliminated by providing an offset arm that connects the fourth axis and the sixth axis. In this form, the axis of the fourth axis and the axis of the sixth axis are in parallel with each other without crossing due to the presence of the offset arm.

  If it is assumed that an inverse conversion process of obtaining the angle of each axis from the position / orientation of the hand is performed on the arm of such a form, it is difficult in the conventional method. For example, Patent Document 1 discloses a technology that makes it possible to perform inverse transformation processing based on DH parameters even when an error in assembling a robot is included. In Patent Document 1, assuming that the length of the offset arm is d5, the convergence operation is performed by linearizing the minute region corresponding to the error on the assumption that d5 = 0.

Unexamined-Japanese-Patent No. 2017-61022

  Assuming that the method of Patent Document 1 is applied to a robot of d5 ≠ 0, d5 as the initial error of the convergence operation becomes a large value, so the number of iterations of the operation becomes enormous, or the operation does not converge. There is a possibility of divergence. In addition, due to the presence of the offset, the offset may not be correctly reflected even if the reach is originally reached, and it may be erroneously determined that the reach does not reach.

  The present invention has been made in view of the above situation, and an object thereof is to provide a control apparatus of a robot capable of performing reverse conversion processing even on a robot provided with an offset arm, and a reverse conversion processing method of the robot. It is.

  The control device of the robot according to claim 1 has an offset arm of link length d5 in which the fifth axis is disposed and which links the fourth axis and the sixth axis, whereby the axial center of the fourth axis and the sixth axis The control target is a robot including a vertical six-axis arm having a structure parallel to the axis of the robot. Then, with the tip of the arm as a control point, the angle and the position of each axis are calculated by performing an inverse conversion process on the position and attitude that are the targets of the control point.

  The position temporary determination unit temporarily determines the position of the sixth axis, and temporarily determines the position of the fifth axis to P5 'based on the temporarily determined position P6'. The movement range determination unit determines whether or not the positions P5 'and P6' are within the movement range based on the link parameters of the robot. When the positions P5 'and P6' are within the operation range, the inverse transformation processing unit performs the inverse transformation process by setting the link length d5 to "0" for the homogeneous transformation matrix based on the positions. Further, the forward conversion processing unit performs the forward conversion process using the angle of each axis obtained by the reverse conversion process.

The evaluation value calculation unit obtains a 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 the rotation obtained by the forward conversion process to a rotation matrix corresponding to the target position. The rotation matrix R _E is obtained by multiplying the inverse matrix of the matrix. Then, the repetitive operation execution unit reflects the position matrix p _E and the rotation matrix R _E when the norm of the position matrix p _E exceeds the threshold or the angle obtained from the rotation matrix R _E exceeds the threshold. The processes from the inverse transformation process are repeatedly executed using the simultaneous transformation matrix. That is, if the norm of the position matrix p _E is less than or equal to the threshold and the angle obtained from the rotation matrix R _E is less than or equal to the threshold, the inverse transformation process is completed. According to this structure, the angle of the sixth axis is tentatively determined for the robot having the offset arm of link length d5 connecting the fourth axis and the sixth axis, the reverse conversion processing is performed, and the processing result is The inverse transformation process can be performed by evaluating the matrix p _E and the matrix R _E obtained by the forward transformation process and converging the operation.

  According to the robot control device of the second aspect, the inverse transformation processing unit is configured to position the position P5 'and P6' so as to be within the operation range if the positions P5 'and P6' are not within the operation range. Correct the Thereby, reverse conversion processing can be performed continuously.

  According to the robot control device of the third aspect, the repetitive operation execution unit determines whether or not the position of the control point determined by the homogeneous 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 corrected so as to be within the operating range. In this way, it is possible to continue to execute repetitive calculations.

  According to the robot control device of the fourth aspect, the form determining unit calculates the determinant of the Jacobian matrix for the result of the inverse transformation process. Then, the wrist form of the inverse transformation processing result is determined depending on whether or not the sign of the determinant matches the previously specified wrist form. For example, in the case where the movable range of each axis is -180 degrees to 180 degrees, when five axes are moved to visually confirm the wrist form, a combination of at most two arm forms, two elbow forms and four wrist forms It turns out that it becomes.

  And the points separating the boundaries of the four wrist forms should be singular points, and the Jacobian matrix has a determinant of zero at the singular points. Therefore, it is possible to determine whether or not the form is designated based on whether or not the wrist form of the inverse transformation processing result corresponds to the sign of the determinant of the Jacobian matrix for the combination of each form. That is, by combining the angle of four axes to whichever of -90 degrees to 90 degrees, -180 degrees to -90 degrees, or 90 degrees to 180 degrees, it is possible to determine at most four wrist forms.

FIG. 1 shows the configuration of a robot system according to a first embodiment. Diagram showing robot's coordinate system The figure which shows the composition of the robot by xz plane and yz plane The figure which shows the arm form of the robot by xy coordinates and αr coordinates The figure which shows the arm form of the robot by the az coordinate Diagram showing each form of arm defined by αz coordinate Flow chart showing reverse conversion process performed by controller Diagram for explaining the operation range in the case of d2 = d3 The figure explaining four wrist forms which a robot arm can take about the same hand tip position

  Hereinafter, an embodiment will be described with reference to FIGS. 1 to 9. As shown in FIG. 1, the robot system 1 includes a vertical articulated robot 2 and a controller 3 for controlling the robot 2 in a base 4. The robot system 1 is used for general industrial use. The robot 2 is a so-called six-axis vertical articulated robot. A shoulder 5 is rotatably connected horizontally on the base 4 via a first axis J having an axis in the Z direction. The shoulder 5 is provided with a second axis J2 having an axis in the Y direction, and the lower end portion of the first arm 7 extending upward is rotatable in the vertical direction via the second offset arm 6 extending in the Y direction. It is connected. The third arm J has an axial center in the Y direction at the tip of the first arm 7 and the second arm 9 can be rotated in the vertical direction via the third offset arm 8 extending in the -Y direction It is connected. The second arm 9 comprises a base 9a and a tip 9b.

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

  The controller 3 is a control device of the robot 2 and controls the robot 2 by executing a computer program in a control means composed of a computer not shown CPU, ROM, RAM and the like. Specifically, the controller 3 includes a drive unit configured of an inverter circuit or the like, and for example, performs feedback control based on the rotational 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 a temporary angle determination unit, a direction calculation unit, a temporary target position calculation unit, an inverse conversion processing unit, an evaluation unit, and a form determination unit. Detection signals of encoders (not shown) provided at joints of the robot 2 are input to the position detection circuit. The position detection circuit detects the rotational angle position of the motor provided at each joint based on the detection signal of each encoder.

  The controller 3 executes a preset operation program to control the position and attitude of the control point at the tip of the arm based on the position information input from the position detection circuit. In the present embodiment, the controller 3 performs CP (Continuous Path) control. In CP control, when moving the control point at the tip of the arm to the target, the position and posture that are the target of the control point, that is, the motion trajectory is set as a time function. The target position and posture include, in addition to the taught position and posture, the interpolated position and posture 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 attitude of the control point follow the motion trajectory. The controller 3 performs inverse transformation processing to calculate the angles of the first axis to the sixth axis for realizing the currently designated target position and orientation in the control of the position and orientation.

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 1} to 6 6 are determined at predetermined positions on the first to sixth axis lines J _{1 to} J ₆ . 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 zeroth coordinate system ₀ 0 which is a robot coordinate system. The zeroth coordinate system _{0 0} is a coordinate system which does not change even if the first to sixth axes rotate. In the present embodiment, the origin of the coordinate system _{0 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 zeroth coordinate system _{0 0 to} the origin of the first coordinate system _{1 1} d 2: Link length from the origin of the first coordinate system _{1 1} to the base of the first arm 7 a 2: First arm 7 the tip from the base of the link length to the second coordinate system sigma ₂ origin d3: link length from the second coordinate system sigma ₂ the origin to the tip portion of the third offset arm 8 a3: third axis J3, the Center-to-center distance between four axes J4 d4: Link length from the origin of the third coordinate system _{3 3 to} the origin of the fourth coordinate system _{4 4} d5: Origin of the fourth coordinate system _{4 4 to} the origin of the fifth coordinate system _{5 5} link length to d6: see Figure 3 for link length a3 of the fifth coordinate system sigma ₅ origin to the origin of the sixth coordinate system sigma _6. The d5 corresponds to the link length of the fifth offset arm 10.

First, forward conversion processing will be described as a premise for describing inverse conversion processing in the present embodiment.
<Forward conversion process>
First, DH parameters were determined as shown in Table 1 by coordinate conversion 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 of FIG.

A homogeneous transformation matrix from base coordinates Σ ₀ to mechanical interface coordinates _{6 6} is as follows. n, o and a respectively indicate a normal vector, an orient vector and an approach vector. Incidentally, to simplify the notation, it is denoted sin [theta _i, cos [theta] _i respectively s _i, and c _i. Further, for example, S ₂₃ indicates the _{sin (θ 2 + θ 3)} .

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

  The normal, orient, approach vectors and position coordinates for the fifth and sixth axes are as follows.

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

<Inverse conversion process>
Next, reverse conversion processing will be described. It is assumed that the position and orientation of the hand of the robot arm is given by a simultaneous transformation matrix. Let p _i be a position vector at base coordinates _{0 0} in FIG. First, the approach vector a _6, obtains the wrist position p _5.
p ₅ = p ₆ −d ₆ a ₆ (20)

In the case of the conventional six-axis robot, since the rotation axes of the 4, 5, and 6 axes are orthogonal to each other at the wrist position, the 1, 2, 3 axes are analytically determined first for the wrist position. All joint angles can be determined. That is, since the wrist position is on the rotational 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 six-axis robot 2 of the present embodiment, p ₄ changes depending on the 4, 5, and 6 axes angles, so that it can not be solved by the same method as the conventional one.

Therefore, a method to solve by assuming a 6-axis angle is shown. Assuming that the rotation range of six axes is, for example, -180 degrees to 180 degrees, the search is performed assuming that the step size is appropriate 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 an axis obtained by rotating the xy axis by one axis angle. However, since the link parameter a ₁ = 0, the equation is different if a ₁ ≠ 0. Since p ₄ is fixed, the α coordinate of p ₄ can be represented by an equation that does not use a single axis angle. Also here, the arms form of alpha ₁₄ ≧ 0 LEFTY, defined as RIGHTY arms form of alpha ₁₄ <0. If α ₁₄ is an imaginary number, no solution is obtained because the reach does not reach.

Here, l ₁₄ in the equation (24) is a distance from the origin of the α r coordinate to p ₄ .
Therefore, the trigonometric function of the axis angle can be obtained as in equations (25) to (28).

Next, as shown in FIG. 5, the robot 2 is considered in an αz plane viewed from the side. 2,3 axis angle satisfying p ₄ are present two sets. From the p ₁ and the arm length of p _2, from p ₂ and the arm length of the _{p 4, (29), (} 30) equation is satisfied.

By expanding the equation (30) and arranging the equation by the equation (29), p ₂ can be obtained.

In the case of k ₂ <0, there is no solution because the reach does not reach. z _{12 is} also required.

  If Eqs. (44) and (45) are substituted into Eq. (29), since the equality of Eq. (52) is satisfied, the sign and the absolute value are removed, and the following combination is made in the same order.

Since the following equation is established from FIG. 5, trigonometric functions of two and three axes can be obtained.
If a tan 2 (y, x) is used, 1, 2 and 3 axis angles can be determined.

Next, as shown in FIG. 6, when arm configurations BELOW and ABOVE are defined,
In the case of LEFTY, ABOVE occurs with θ ₃ > φ 3 and BELOW with θ ₃ <φ ₃ . Also,
In the case of RIGHTY, BELOW is established with θ ₃ > φ 3 and ABOVE is established with θ ₃ <φ ₃ . even the sign of z ₁₂ can be determined. φ3 is expressed by equation (64).
φ 3 = a tan 2 (a ₃ , d ₄ ) (64)

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

(* 4) from the equation, the angle θ ₅ is required. Further, (* 4) and (* 1), (* 2) Since the obtained and sin [theta ₆ and cos [theta] ₆ from expression, thereby the angle theta ₆ is obtained.

(* 7), since it is required and sin [theta ₄ and cos [theta] ₄ by (* 8), thereby the angle theta ₄ is obtained. Incidentally, although there is a compound sign of ± in the equation (* 4), a code satisfying the condition of the 4-axis angle or the 5-axis angle is adopted in the specification of the wrist form. Further, since the angle theta ₅ above is sought, (* 8) to equation may be divided by cos [theta] _5, and instead of these (* 9), may be used (* 10).

  The above is the outline of the reverse conversion process. Next, the operation of the present embodiment will be described with reference to FIGS. 7 and 8. FIG. 7 is a flowchart showing the contents of the reverse conversion process performed by the controller 3. First, assuming that the positions of six axes are temporarily determined to be P6 ', the positions of five axes from the position P6' are temporarily determined to be P5 '. Then, the homogeneous transformation matrix T 'is determined using these (S1). Then, it is determined whether the temporarily determined position P5 'is within the operation range (S2).

FIG. 8 is a diagram for explaining the operation range, which is based on the case where d2 = d3. A sphere A of radius L1 and a sphere B of radius (L1 + L2) are set. L1 and L2 are respectively defined by equations (* 11) and (* 12).

  Then, 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 the distance L is equal to or less than the link length d5 (S10). This is a determination as to whether or not the temporarily determined position P6 'is within the operating range. If the distance L exceeds the link length d5 (NO), the process ends as an error.

  On the other hand, if the distance L is equal to or less than the link length d5 (YES), the position P5 'is corrected to move to the inside of the sphere A (S14). For example, the position P5 'may be moved to the intersection point 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 out of the sphere B, with L2 = d5.

When d2 ≠ d3, the inside and outside of the operation range are determined as follows. The case where the inside of the square root in the equation (24) or (44) becomes negative is out of the operation range in step S2. Here, if correction is made so as to be within the operating range, l ₁₄ ² = r ₁₄ ^{2 in} the case of equation (24)
You should do. Then, if L = | r ₁₄ | − | l ₁₄ |
w = L / | l ₁₄ |, [wx ₄ wy ₄ 0] ^T
You only need to correct the position.

  Also, in the case of equation (44)

If it is determined in step S2 that it is "within the operating range", and if step S14 is executed, reverse conversion processing is performed (S3). Here, g ⁻¹ (T ′, F) is the simultaneous conversion matrix T ′ with d ₅ = 0, and from the form F of the robot 2, the angle θ ′ = [θ ₁ ′, θ ₂ ′, θ ₃ ′, θ ₄ ', Θ ₅ ', θ ₆ '] This is a function for obtaining ^T.
Next, forward conversion processing is performed using the angle θ ′ obtained as a result of the inverse conversion (S4). The simultaneous conversion matrix obtained as a result of this forward conversion process is T ′ ′, and the form is F ′ ′. The simultaneous conversion matrix T ′ ′ includes a rotation matrix R ′ ′ and a position matrix p ′ ′ as elements.

Next, the difference between the target position p and the position p ′ ′ is obtained to obtain a position error vector p _{E. 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). Then, it is determined whether the norm │p _E │ of the vector p _E is smaller than or equal to the threshold p _EM (S6) and whether the angle │R _E │ of the rotation matrix R _E is smaller than the threshold R _EM (S7). . Here, || p _E || and || R _E || are respectively defined by the equations (* 13) and (* 14).

If "YES" is determined in the step S7, it is determined whether or not the wrist form determined from the position p "and the rotation matrix R" is the specified form (S8). If the specified form is obtained, the reverse conversion process is ended. Do. On the other hand, if "NO" is determined in step S6 or S7, it is determined whether the number of times of repetitive execution of the reverse conversion process and the forward conversion process exceeds a predetermined number (S11). Here, if the specified number is exceeded (YES), the process ends as an error. If the specified number is not exceeded (NO), the rotation matrix R ′ is multiplied by the rotation matrix R _E for evaluation, the position error vector p _E is added to the position matrix p ′, and the simultaneous transformation matrix T ′ is updated (S12) . Then, the inside and outside of the operation range is determined as in step S2 (S13). If it is within the operation range, the process proceeds to step S3. If it is outside the operation range, the process proceeds to step S14.

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

  The code of the Jacobian matrix and the conventional way of thinking of the wrist form, the combination of the 4-axis angle being -90 degrees to 90 degrees, -180 degrees to -90 degrees or 90 degrees to 180 degrees, a maximum of four The present wrist forms FLIP +, FLIP-, NONFLIP +, NONFLIP- can be determined.

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

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

  As described above, according to the present embodiment, the controller 3 is provided with the third offset arm 8 of the link length d5 in which the five axes are disposed and the four axes and the six axes are coupled, and A robot 2 provided with a vertical six-axis arm having a structure parallel to the six axes of axes is to be controlled. Then, the controller 3 uses the hand that is the tip of the arm as a control point, and performs inverse conversion processing on the position and orientation that is the target of the control point to calculate the angle of each axis.

  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 ′. It is determined whether it is within the operation range based on the link parameter. When the positions P5 'and P6' are within the operation range, the link length d5 is set to "0" and the reverse conversion processing is performed on the homogeneous conversion matrix based on the positions. Then, forward conversion processing is performed using the angle of each axis obtained by the reverse conversion processing.

Furthermore, 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 determined, and the rotation matrix corresponding to the target position is the inverse of the rotation matrix obtained by the forward conversion process. The rotation matrix R _E is obtained by multiplying the matrix. Then, if the norm of the position error vector p _E exceeds the threshold or the angle obtained from the rotation matrix R _E exceeds the threshold, a simultaneous conversion matrix reflecting the position error vector p _E and the rotation matrix R _E Process repeatedly executes the process from the reverse conversion process. If the norm of the position error vector p _E is less than or equal to the threshold and the angle determined from the rotation matrix R _E is less than or equal to the threshold, the inverse transformation process is complete.

With such a configuration, the angle of the sixth axis is tentatively determined for the robot 2 having the offset arm of link length d5 connecting the fourth axis and the sixth axis, and the inverse conversion processing is performed, and the processing result The inverse transformation process can be performed by evaluating the matrix p _E and the matrix R _E obtained by performing the forward transformation process on the above to converge the operation.

In addition, the controller 3 corrects the positions P5 'and P6' so as to be located in the operation range if the positions P5 'and P6' are not in the operation range. Similarly, with respect to 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 positioned within the operating range. Fix it. In this way, it is possible to continuously execute the reverse conversion process and the repetitive operation.
Then, the controller 3 calculates the determinant of the Jacobian matrix J for the result of the inverse transformation process. Then, the wrist form of the inverse transformation processing result can be uniquely determined depending on whether or not the sign of the determinant matches the previously specified wrist form.

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

  In the drawings, 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, 9 is a second arm, 10 indicates a fifth offset arm, 11 indicates a wrist, and 12 indicates a hand.

Claims (8)

The fifth axis is disposed, and by having an offset arm of link length d5 connecting the fourth axis and the sixth axis, the vertical axis of the structure in which the axis center of the fourth axis and the axis center of the sixth axis are parallel It is applied to a robot having a six-axis arm, and the tip of the arm is used as a control point, and the angle and the position of each control point are inversely transformed to calculate the angle of each axis.
A position temporary determination unit which temporarily determines the position of the sixth axis and temporarily determines the position of the fifth axis as P5 ′ based on the temporarily determined position P6 ′;
An operation range determination unit that determines whether the positions P5 'and P6' are within an operation range based on link parameters of the robot;
If the positions P5 ′ and P6 ′ are within the operation range, an inverse transformation processing unit that performs an inverse transformation process by setting the link length d5 to “0” for the homogeneous transformation matrix based on the positions;
A forward conversion processing unit that performs a forward conversion process using the angle of each axis determined by the inverse conversion process;
A 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 determined, and a rotation matrix corresponding to the target position is obtained by using the rotation matrix obtained by the forward conversion process. An evaluation value calculator, which obtains a rotation matrix R _E by multiplying the inverse matrix,
When the norm of the position matrix p _E exceeds a threshold or the angle obtained from the rotation matrix R _E exceeds a threshold, a simultaneous transformation matrix reflecting the position matrix p _E and the rotation matrix R _E A control device of a robot comprising: a repeat operation execution unit which repeatedly executes a process from the inverse conversion process according to   The control device for a robot according to claim 1, wherein the inverse transformation processing unit corrects the positions P5 'and P6' so as to be located within the operation range if the positions P5 'and P6' are not within the operation range. .   The repetitive operation 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 The control device of a robot according to claim 1 or 2, wherein the homogeneous transformation matrix is corrected to be located within a range.   A form determining unit that calculates a determinant of a Jacobian matrix for the result of the inverse conversion process, and determines a wrist form of the result of the inverse conversion process depending on whether or not the sign of the determinant matches the previously specified wrist form The control apparatus of the robot as described in any one of Claim 1 to 3 provided with. The fifth axis is disposed, and by having an offset arm of link length d5 connecting the fourth axis and the sixth axis, the vertical axis of the structure in which the axis center of the fourth axis and the axis center of the sixth axis are parallel It is applied to a robot having a six-axis arm, and the tip of the arm is used as a control point, and the angle and the position of each control point are inversely transformed to calculate the angle of each axis.
Temporarily determining the position of the sixth axis, and temporarily determining the position of the fifth axis to P5 'based on the temporarily determined position P6';
Determining whether the positions P5 'and P6' are within an operating range based on link parameters of the robot;
If the positions P5 'and P6' are within the operation range, the reverse conversion process is performed by setting the link length d5 to "0" for the homogeneous transformation matrix based on the positions;
Performing a forward conversion process using the angle of each axis obtained by the reverse conversion process;
A 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 determined, and a rotation matrix corresponding to the target position is obtained by using the rotation matrix obtained by the forward conversion process. Obtaining the rotation matrix R _E by multiplying the inverse matrix;
When the norm of the position matrix p _E exceeds a threshold or the angle obtained from the rotation matrix R _E exceeds a threshold, a simultaneous transformation matrix reflecting the position matrix p _E and the rotation matrix R _E A robot reverse conversion processing method that is repeatedly executed from the step of performing the reverse conversion processing according to   6. The robot reverse conversion processing method according to claim 5, wherein the positions P5 'and P6' are corrected so as to be within the operation range if the positions P5 'and P6' are not within the operation 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 positioned within the operating range. The robot inverse transformation processing method according to claim 5 or 6, wherein the homogeneous transformation matrix is corrected.   A form determining unit which 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 previously designated wrist form The robot reverse conversion processing method according to any one of claims 5 to 7, comprising:

Images