JP5980084B2 - Error detection method for robot hand - Google Patents

Description

  The present invention relates to an error detection method for a robot hand, and more particularly to an error detection method that can be suitably used for error detection of an articulated robot hand.

  In an articulated robot hand, an error generated in each joint appears as an error at the tip of the robot hand that performs a work while holding a tool or the like. Therefore, an error detection method for specifying an error occurring in each joint has been proposed. For example, in Patent Document 1, a fixed object point near the base of the robot is photographed with a camera attached to the tip of the robot hand, When the robot hand is in different postures, it calculates the difference between the position of the fixed object image captured in each posture and the theoretical position, and detects the error in each joint based on these differences. How to do is shown.

JP-A-5-8185

  However, the above-described conventional error detection method has a problem that the absolute value of the angle deviation (error angle) between the arms of each joint cannot be detected.

  Therefore, the present invention solves such a problem, and provides a robot hand error detection method capable of detecting the absolute value of the angle deviation (error angle) of the arm at each joint constituting the robot hand. For the purpose.

  In order to achieve the above object, the first invention provides an error of a robot hand (1) composed of a plurality of arms (L1 to L6) connected in series so as to be relatively rotatable by joints (P1 to P6). In this detection method, the tip position vector of the tip (P7) of the robot hand when each of the arms (L1 to L6) is independently rotated is measured at three points, and the measured tip position vector (P7An *) is measured. , P7Bn *, P7Cn *), respectively, calculating the rotation axis vector (kn *) and the center vector (On *) of the rotation surface when the arms (L1 to L6) are rotated, and the rotation axis vector. calculating a joint position vector (Pn *) of each joint (P1 to P6) and an arm length vector (Ln *) of each arm (L1 to L6) from (kn *) and the center vector (On *) And the calculated function Calculating a joint position error vector (ΔPn *), which is a difference between the position vector and a master joint position vector stored in advance, the joint position error vector (ΔPn *), the rotation axis vector (kn *), and the arm length Calculating an error angle (Δθn) between the angle of each arm (L1 to L6) and the master angle stored in advance from the vector (Ln *).

  According to the first aspect of the present invention, it is possible to accurately detect the absolute value of the angular deviation (error angle) of the arms of each joint constituting the robot hand.

  In the second invention, the error angle (Δθn) is used to compensate at least one of the joint position vector (Pn *), the arm length vector (Ln *), and the joint position error vector (ΔPn *). The method further includes a step.

  According to the second aspect of the present invention, at least one of the joint position vector, the arm length vector, and the joint position error vector can be favorably compensated based on the arm error angle of each joint.

  As described above, according to the error detection method of the robot hand of the present invention, the absolute value of the angle deviation (error angle) of the arm in each joint constituting the robot hand can be detected with high accuracy.

It is a figure which shows the structure of the apparatus which implements the method of this invention. It is a conceptual diagram of an articulated robot hand. It is a figure which shows the relationship between a reference | standard coordinate system and a plane coordinate system. It is a figure which shows the geometric positional relationship on a plane coordinate system. It is a figure which shows the geometric positional relationship on a plane coordinate system. It is a conceptual diagram of a part of a robot hand.

  The embodiment described below is merely an example, and various design improvements made by those skilled in the art without departing from the gist of the present invention are also included in the scope of the present invention.

  FIG. 1 shows a configuration of an apparatus for carrying out the present invention. A measuring jig 2 attached to the tip of an articulated robot hand 1 is imaged by a camera 3. The camera 3 is connected to a computer 4, and error detection and error compensation procedures described below are executed by a program of the computer 4. The computer 4 captures a fixed reference point St (which becomes the origin of the reference coordinate system) via the camera 3 as necessary, and maintains the accuracy of three-dimensional recognition.

  FIG. 2 conceptually shows an example of the articulated robot hand 1. The joints P1 to P6 of the robot hand 1 are at six locations from the ground side, and P7 is the tip of the robot hand 1. The joints P1 to P6 and the tip P7 (tips of the measurement jig) are connected by arms L1 to L6. In this embodiment, P1 is a fixed joint, and P2, P3, and P5 are arms L2, L3, and L5, respectively. , P4 and P6 are joints that rotate the arms L4 and L6 around their axes, respectively.

(Error detection of swiveling and rotating joints)
First, each joint Pn (n = 2 to 6) is rotated, and three vector coordinates P7An *, P7Bn *, P7Cn * are obtained from the locus of the tip P7 at that time. In the following, symbols marked with * indicate that they are vectors. Subsequently, the turning radius dn of the turning trajectory passing through the three points G1 (n) *, G2 (n) *, G3 (n) * represented by the following equations (1) to (3) is calculated. The turning radius dn can be calculated only for the joint Pn (n = 2, 3, 5) that rotates. In the following formulas (1) to (3), m> n.

The rotation radius dn is calculated according to the following procedure.
First, the center vector On * of the rotation surface where the rotation trajectory passing through the three points G1 (n) *, G2 (n) *, G3 (n) * exists is calculated as follows.

  As shown in FIG. 3, a plane (planar coordinate system) including the three points G1 (n) *, G2 (n) *, and G3 (n) * ((n) is omitted in the figure). If the origin is G1 (n) *, the vector between points G1 (n) * and G2 (n) * is V1 *, and the vector between points G1 (n) * and G3 (n) * is V2 * The unit vectors i *, j *, k * of the coordinate system are obtained by the following equations (4) to (6). Here, k * is a unit direction vector indicating the direction of the rotation axis perpendicular to the plane.

  The coordinate value of the point G1 (n) * in the reference coordinate system is (X1, Y1, Z1), and the coordinate values of the unit vectors i *, j *, k * are respectively (ix, ii, iz) (jx , jy, jz) (kx, ky, kz), the transformation matrix Tp is expressed by the following equation (7).

  Thereby, the points G2 (n) *, G3 (n) * are converted into points G2 (n) * ′, G3 (n) * ′ on the plane coordinate system by the following equations (8), (9) ( FIG. 3).

  The coordinates (uc, vc, 0) of the center On * 'on the plane coordinate system of the circle (turning trajectory) passing through the three points G1 (n) *', G2 (n) * ', G3 (n) *' ) Is represented by the following equation (10) from the geometric positional relationship shown in FIG.

  The vectors from the center On * ′ to the points G1 (n) * ′, G2 (n) * ′, G3 (n) * ′ are shown in FIG. 5 ((n) is omitted in the figure). ) VcG1 * ′, VcG2 * ′, and VcG * 3 ′, respectively, the angle θ12 between the points G1 (n) * ′ and G2 (n) * ′, and G2 (n) * ′ and G3 (n) The angle θ23 between * ′ is expressed by the following equations (11) and (12).

The center On * ′ whose position is specified on the plane coordinate system is converted into the center On * on the reference coordinate system by the following equation (13).
On * = Tp · On * ′ (13)
Thus, the rotation radius dn is calculated by dn = | G1 * -On * |.

Subsequently, the arm length Ln (n = 2, 3, 5) of each arm L2, L3, L5 is lower than the design angle φn (φn ≠ 0) with respect to the rotation axis of each arm and the rotation radius dn. It calculates with Formula (14).
Ln = dn / sinφn (14)
Note that Ln (n = 4, 6) when φn = 0 is measured in advance.

  Thereafter, the position vector Pn * (n = 1 to 6) of each joint and the length vector Ln * of each arm are calculated by the following equations (15) and (16).

In the above equation (15), kn * is a unit direction vector of the rotation axis of the joint Pn. The above equations (15) and (16) are calculated in the order of n = 6 → 1.
The values of Pn * and Ln * calculated in the above procedure at the initial installation of the robot hand 1 are stored in the computer 4 in advance as master values.

  In the process of using the robot hand 1, the position vectors Pn * (n = 1 to 6) of the joints P1 to P6 are calculated in the same procedure as described above, and the error ΔPn between the position vector Pn * and the master value is calculated. * Is calculated. Then, the following formulas (17) and (18) detect (calculate) an error (deviation) angle Δθn of the rotation axis of each joint Pn (n = 1 to 3) from the master angle. When the detected error angle Δθn is larger than a predetermined value, the corresponding joint Pn (n = 1 to 6) is notified as an abnormality of the robot hand 1. When the detected error angle Δθn is smaller than a predetermined value, error compensation described later is performed.

  In the above equation (17), ΔPmn * represents the position error vector of the mth joint Pm affected by the position error of the nth joint Pn. Further, m> n ≧ 1.

(Error compensation)
Next, the influence of Δθn is removed from each Ln *, kn *, Pn *, ΔPn * where n = 1 to 3 (error compensation). This is performed as follows using a rotation matrix Rot (k *, θ) represented by the following equation (19).
Rot (k *, θ) = cos θ · I 3 + (1−cos θ) k * · tk * + sin θ [k * ×] (19)
Here, I3 is a cubic unit matrix, tk * is a transposed vector of the unit vector k *, and [k * ×] is an outer product matrix of the unit vector k *.

That is, assuming that Ln *, kn *, Pn *, and ΔPn * after error compensation are Lm * ′, km * ′, Pm * ′, and ΔPm * ′, respectively, the following equations (20) to (23) are used. Can be calculated (compensated).
Lm * ′ = Rot (kn *, −Δθn) Lm * (20)
km * ′ = Rot (kn *, −Δθn) km * (21)
Pm * ′ = Rot (kn *, −Δθn) (Pm * −Pn *) + Pn * (22)
ΔPm * ′ = Rot (kn *, −Δθn) ΔPm * (23)

(Error detection and compensation for joints rotating around the axis)
When using the above equations (20) to (23), the joints of Pn (n = 4, 6) rotate the arms Ln (n = 4, 6) around their axes, respectively. In the case of n = 4, 6), since the rotation axis and the arm are parallel, | kn * × Ln * | = 0 in the above equation (18), and Δθn cannot be calculated.

Therefore, in this case, in FIG. 6, | P5 * −P4 * | does not change with the rotation of the arm L4, and | O6 * −P5 * | does not change with the rotation of the arm L6. In view of the fact that | O6 * −P4 * | is not affected by Δθn other than Δθ5 because there is no influence of n = 1 to 3), Δθ5 is expressed by the following equation (24) from the geometrical relationship shown in FIG. ).

  Error compensation of L5 *, k5 *, P5 *, and ΔP5 * is performed by the above equations (20) to (23) based on the calculated (detected) Δθ5, or the abnormality of the joint P6 is notified.

Next, Δθ4 is calculated (detected). Since O6 * is not affected by Δθn other than Δθ4, Δθ4 is calculated by the following equation (25), and error compensation of L4 *, k4 *, P4 *, ΔP4 * is performed by the above equations (20) to (23). Or the abnormality of the joint P4 is notified.
| ΔO6 * | ² = 2 | R4 * × (O6 * −P4 *) | ² (1-cosΔθ4) (25)

  For Δθ6, by adjusting the measurement jig in advance so that P7 * and O6 * do not overlap (see FIG. 6), Δθ6 is calculated (detected) by the above equations (1) to (18), and then Then, error compensation of L6 *, k6 *, P6 *, ΔP6 * is performed by the above equations (20) to (23), or the abnormality of the joint P4 is notified.

DESCRIPTION OF SYMBOLS 1 ... Robot hand, 2 ... Measuring jig, 3 ... Camera, 4 ... Computer, P1, P2, P3, P4, P5, P6 ... Joint, L1, L2, L3, L4, L5, L6 ... Arm.

Claims (2)

An error detection method for a robot hand composed of a plurality of arms connected in series with each other so as to be relatively rotatable by a joint, wherein three tip position vectors of the robot hand tip when each of the arms is rotated independently Performing point measurement and calculating a rotation axis vector and a center vector of the rotation surface at the time of rotation of each arm based on the measured tip position vectors, and each joint from the rotation axis vector and the center vector Calculating a joint position vector and an arm length vector of each arm, calculating a joint position error vector that is a difference between the calculated joint position vector and a pre-stored master joint position vector, and From the position error vector, the rotation axis vector and the arm length vector, Angle and error detecting method of a robot hand and a step of calculating an error angle between the pre-stored master angle. The robot hand error detection method according to claim 1, further comprising the step of compensating at least one of the joint position vector, the arm length vector, and the joint position error vector using the error angle.

Images