JP4640499B2 - Grip control device - Google Patents

Description

The present invention relates to a gripping control device for a robot hand having a plurality of fingers that grip an object of arbitrary shape.

As a conventional technique for gripping an object by a robot, for example, one described in Patent Document 1 is known. The robot controller described in this document manually operates the robot and memorizes the robot tip position, thereby referring to a reference point representing a representative position / posture on the movement path and an operating position of the robot tip during operation. It teaches the relative position with respect to a point, designates the movement position path of the robot tip during operation with a row of reference points, and grips the workpiece according to the movement position path.
JP-A-2-59285

  However, in the above-described prior art, it is necessary to determine by teaching what kind of operation position path is appropriate every time the type of an object (work) to be grasped changes. For this reason, if there are many types of workpieces, the teaching work takes time and the efficiency becomes poor. Moreover, it is difficult to deal with a workpiece having an arbitrary shape.

  An object of the present invention is to provide a grip control device for a robot hand that can reliably and efficiently grip an object having an arbitrary shape.

Gripping control device of the present invention, there is provided a gripping control apparatus for a robot hand having a plurality of fingers for gripping an object having an arbitrary shape, and a plurality of actuators for driving the respective finger joints, by image recognition, the object The contact position setting means for determining the target contact point in contact with the fingertip part of each finger and obtaining the normal of the surface of the object at the target contact point, and the point set on the curved surface of the fingertip part for each finger The direction of the normal of the curved surface is determined, and among the points set on the surface portion, the error between the direction of the normal of the curved surface at the point and the direction of the normal of the surface of the object at the target contact point the calculated, and the candidate position location means that the point at which the error is minimum and the candidate contact point, a gripping attitude calculation means for calculating a target angle of the joint so that the target contact point and the candidate contact point is contact for each finger , Joint target angle And a drive control means for controlling the actuator in accordance with.

  According to the grip control device for a robot hand of the present invention, the target angle of the finger joint is obtained so that the target contact point with the closest normal direction and the candidate contact point are in contact with each other. If the finger joint is driven, an object having an arbitrary shape can be held.

  In the grip control device of the present invention, it is also preferable that the grip posture calculation means obtains the joint target angle so as to reduce the coordinate difference between the target contact point and the candidate contact point in the arbitrary coordinate system. Since the target angle of the finger joint is obtained so as to reduce the coordinate difference between the target contact point and the candidate contact point, the target angle can be calculated by simple calculation.

  According to the present invention, an object having an arbitrary shape can be reliably and efficiently gripped.

  The knowledge of the present invention can be easily understood by considering the following detailed description with reference to the accompanying drawings shown for illustration only. Subsequently, embodiments of the present invention will be described with reference to the accompanying drawings. Where possible, the same parts are denoted by the same reference numerals, and redundant description is omitted.

  FIG. 1 is a perspective view showing an external appearance of a main part of a robot hand to which an embodiment of a grip control device according to the present invention is applied. In the figure, a robot hand 1 is attached to a robot arm (not shown) and has four fingers 3 (thumb 3A, index finger 3B, middle finger 3C, ring finger 3D) for gripping an object 2 having an arbitrary shape. ing. These fingers 3 are connected to a hand palm portion (not shown).

  The thumb 3A is a four-degree-of-freedom link system, and includes four joints 4A and four motors 5A that drive each joint 4A. The index finger 3B is a link system with three degrees of freedom, and includes four joints 4B and three motors 5B that drive each joint 4B. The most distal joint 4B has an interlocking joint structure. The middle finger 3C is a link system with three degrees of freedom, and includes four joints 4C and three motors 5C that drive each joint 4C. The most distal joint 4C has an interlocking joint structure. The ring finger 3D is a link system having three degrees of freedom, and includes four joints 4D and three motors 5D that drive the joints 4D. The most distal joint 4D has an interlocking joint structure. Each finger 3A to 3D has curved fingertip portions 6A to 6D (hereinafter referred to as fingertip portion 6) made of an elastic body.

  FIG. 2 is a configuration diagram showing a grip control device that controls the grip posture of each finger 3 of the robot hand 1. In the figure, the grip control device 7 has a plurality of six-axis force sensors 8, a plurality of encoders / potentiometers 9, an image recognition unit 10, a robot hand control unit 11, and a plurality of motor drivers 12. Yes.

  The 6-axis force sensor 7 is provided at the base of each finger 3 (see FIG. 1) and detects a gripping reaction force. The encoder / potentiometer 9 is provided in each of the motors 5A to 5D (hereinafter referred to as the motor 5), and detects an angle (joint angle) θ of each of the joints 4A to 4D (hereinafter referred to as the joint 4). As shown in FIG. 3, the joint angle θ is an angle at which lines extending in the longitudinal direction of the two links 13 connected to the joint 4 intersect.

なお、この法線ベクトルＶ₀は、ロボットハンド１の手首（図示せず）に固定された絶対座標系における座標である。 Although not shown, the image recognition unit 10 includes a light source, an imaging unit, an image processing unit, and the like. The image recognition unit 10 determines the coordinates of the target contact point P in contact with the fingertip portion 6 of each finger 3 on the object 2 as shown in FIG. 3, and determines the normal vector V ₀ of the object curved surface at the target contact point P. Ask. The normal vector V ₀ is given by the following equation.

The normal vector V ₀ is a coordinate in an absolute coordinate system fixed to the wrist (not shown) of the robot hand 1.

The robot hand control unit 11 includes a gripping posture calculation unit 14 and a control calculation unit 15. The gripping posture calculation unit 14 inputs the coordinates of the target contact point P on the object preset by the image recognition unit 10 and the normal vector V ₀ , performs a predetermined calculation, and a finger 3 for gripping the object 2 The target angle of each joint 4 (target joint angle) is obtained. The calculation processing by the gripping posture calculation unit 14 will be described in detail later.

  The control calculation unit 15 creates a time-series joint angle command value corresponding to the target joint angle of each finger 3 obtained by the gripping posture calculation unit 14, and sends this command value to the motor driver 12. At this time, the detection signal of the encoder / potentiometer 9 is input, and the angle of the joint 4 (joint angle) is feedback-controlled. Alternatively, the detection signal of the six-axis force sensor 8 may be input, and the gripping force by each finger 3 may be feedback controlled.

  The motor driver 12 supplies each motor 5 with a drive current corresponding to the joint angle command value created by the control calculation unit 15 to drive each motor 5 to rotate.

  Next, the concept of obtaining the gripping posture of the robot hand 1 in the gripping posture calculation unit 14 of the robot hand control unit 11 will be described below.

(1) Element division of fingertip portion As shown in FIG. 4, many lattice points Q are generated by dividing the surface of the fingertip portion 6 into elements. Then, a coordinate database of each lattice point Q in the link coordinate system (local coordinate system) fixed to the fingertip link 16 with the rotation center of the joint 4 as the origin is created in advance.

  (2) Calculation of lattice point coordinates on the fingertip part in the absolute coordinate system By performing coordinate conversion between the link coordinate system and the absolute coordinate system (described above), the lattice point Q on the fingertip part 6 in the absolute coordinate system Calculate the coordinates.

FIG. 5 shows how to take the coordinate system of the index finger 3b, the middle finger 3c, and the ring finger 3d. In the figure, the x _s y _s z _s coordinate system is an absolute coordinate system fixed to the wrist of the robot hand 1 as described above. The x ₀ y ₀ z ₀ coordinate system is a relative coordinate system fixed to the stationary part of the motor in the first joint 4a. The x _i y _i z _i (i = 1 to 4) coordinate system is a link coordinate system fixed to the rotation axes of the second joint 4b, the third joint 4c, and the fourth joint 4d.

The origin of the x _i y _i z _i coordinate system is located at the center point of the joints 4b to 4d or the motor. The z axis of the x _i y _i z _i coordinate system corresponds to the rotation axis of the joints 4b to 4d. The x axis of the x _i y _i z _i coordinate system is an axis orthogonal to the rotation axis of the joints 4b to 4d and extending in the longitudinal direction of the links 16b to 16d on the fingertip side. x _i y _i z _i coordinate system y axis is an axis perpendicular to the x _i y _i z _i coordinate system x-axis and z-axis. The length of each link 16a~16c is L ₁ ~L _3, the joint angles of the joints 4a~4d is theta ₁ through? _4. Here, the joint 4c, 4d is because it is interlocked joints, which is equal to the joint angle theta ₄ of the joint angle theta ₃ and joint 4d of the joint 4c.

ここで、^sＴ₄は、ｘ₄ｙ₄ｚ₄座標系からｘ_sｙ_sｚ_s座標系への座標変換行列であり、次式のように隣り合うリンク座標系の座標変換行列により求められる。

ここで、ｘ₀ｙ₀ｚ₀座標系は、ｘ_sｙ_sｚ_s座標系の各座標軸の並進移動を施すことによって得られるので、^sＴ₀は次式で表される。

The coordinates (x _j ⁽⁴⁾ , y _j ⁽⁴⁾ , z _j ⁽⁴⁾ ) in the x ₄ y ₄ z ₄ coordinate system (link coordinate system of the fingertip link 16d) of an arbitrary lattice point j on the fingertip part 6 are This is a known amount that is determined in advance from the database without changing according to the joint angle θ _i . At this time, the coordinates (x _j ^(s) , y _j ^(s) , z _j ^(s) ) of the lattice point j in the x _s y _s z _s coordinate system are expressed by the following equations.

Here, ^s T ₄ is a coordinate transformation matrix from the x ₄ y ₄ z ₄ coordinate system to the x _s y _s z _s coordinate system, and is obtained by a coordinate transformation matrix of adjacent link coordinate systems as in the following equation. .

Here, since the x ₀ y ₀ z ₀ coordinate system is obtained by performing translational movement of each coordinate axis of the x _s y _s z _s coordinate system, ^s T ₀ is expressed by the following equation.

The x _i y _i z _i coordinate system is obtained by performing the following transformation on the x _i−1 y _i−1 z _i−1 coordinate system.
(A) Translation by a _i-1 along x _i-1 axis (b) Rotation by α _i-1 around x _i-1 axis (c) z _i-1 axis after rotation (ie, z _i axis) Rotate by θ _i around the z _i-1 axis (ie, z _i axis) after translation (d) rotation by d _i along

The coordinate transformation matrix ^i-1 T _i from the x _i y _i z _i coordinate system to the x _i-1 y _i-1 z _i-1 coordinate system is the product of the above-mentioned homogeneous transformations (a) to (d). It can be expressed as

The parameters of the above equation (4) are shown in Table 1.

By substituting each parameter of Table 1 into the equation (4), the coordinate transformation matrices ⁰ T ₁ , ¹ T ₂ , ² T ₃ , and ³ T ₄ are calculated. Further, the coordinate transformation matrix ^s T ₄ is calculated by substituting the equations (3) and (4) into the equation (2). Finally, the equation (2) is substituted into the equation (1) to obtain the coordinates of the arbitrary point j on the fingertip portion 6 in the x _s y _s z _s coordinate system (absolute coordinate system).

(3) Calculation of normal vector of arbitrary point on fingertip part The normal vector of the arbitrary point j on the fingertip part 6 is calculated as follows by the same method as described above. ^s T ₄ is a 4 × 4 matrix, and ^s T ′ ₄ is an upper left 3 × 3 matrix.

On the other hand, the normal vector V _j ⁽⁴⁾ in the x ₄ y ₄ z ₄ coordinate system of the link 4 at the arbitrary point j is a known amount determined by the fingertip phase and is expressed by the following equation.

The normal vector V _j ^(S) in the x _s y _s z _s coordinate system (absolute coordinate system) is calculated by the following equation.

When each joint angle θ _i is given, the coordinates of the arbitrary point j on the fingertip surface and the normal vector are calculated by equations (1) and (7), respectively.

指先面上の全ての点における法線方向誤差ｅを比較し、ｅが最小値となる点を候補接触点Ａとする。 (4) Selection of candidate contact points on the fingertip part A point having a normal direction parallel to the normal vector V ₀ of the object curved surface at the target contact point P and defining a point on the fingertip part 6 as a candidate contact point To do. In order to obtain the candidate contact point A, the normal direction error e is calculated for all the points on the fingertip surface by the following equation.

The normal direction errors e at all points on the fingertip surface are compared, and a point where e becomes the minimum value is set as a candidate contact point A.

(4) Sequential calculation of target joint angle The coordinate difference between the target contact point P and the candidate contact point A in the x _s y _s z _s coordinate system, which is an absolute coordinate system fixed to the wrist of the robot hand 1, is expressed as Δx, Assume Δy and Δz. If gripping at the target contact point P, the coordinate differences Δx, Δy, Δz are simultaneously zero. In other words, if each joint angle is adjusted and the coordinate differences Δx, Δy, Δz are simultaneously set to zero, the gripping state is surely achieved.

  In the present embodiment, the configuration is further simplified by omitting the fourth joint 4d of the robot hand 1 described above, and a sequential calculation method will be described using a finger having three degrees of freedom as an example. This sequential calculation method is based on mechanistic consideration of the contribution ratio of the effect of the adjustment amount of each joint angle of a finger having three degrees of freedom on the coordinate differences Δx, Δy, Δz. Based on this consideration, an adjustment rule is considered in which the coordinate differences Δx, Δy, Δz are converged to zero with a simple adjustment rule, and the candidate contact point A is matched with the target contact point P to be in the gripping state.

First, FIG. 7 shows the contribution ratio for each joint angle along each direction of the coordinate differences Δx, Δy, Δz (that is, the x-axis direction, the y-axis direction, and the z-axis direction in the x _s y _s z _s coordinate system). A description will be given with reference to FIG. FIG. 7 is a diagram in which the relationship between the target contact point P and the candidate contact point A is projected on the xz plane. FIG. 8 is a diagram in which the relationship between the target contact point P and the candidate contact point A is projected onto the yz plane.

ただし、Ｓ_１は、第１関節４ａの回転中心から候補接触点Ａまでの距離がｘｚ平面に投影された長さである。 In FIG. 7, if only the first joint 4a is rotated by a minute angle Δθ ₁ , the displacement amounts Δx _θ1 , Δy _θ1 , Δz _θ1 of the candidate contact point A in the three directions Δx, Δy, Δz are given by the following equations.

However, S ₁ is the length of the distance from the rotation center to the candidate contact point A of the first joint 4a is projected to the xz plane.

ただし、Ｓ_２は、第２関節４ｂの回転中心から候補接触点Ａまでの距離がｙｚ平面に投影された長さである。また、αはｙ軸からのＳ_２の立ち上がり角度と等しい角度である。 Further, in FIG. 8, when rotating only the second joint 4b small angle [Delta] [theta] _2, [Delta] x, [Delta] y, the amount of displacement of the candidate contact point A in the three directions of _{Δz Δx θ2, Δy θ2, Δz} θ2 is given by: It is done.

However, S ₂ is the length of the distance from the rotation center to the candidate contact point A of the second joint 4b is projected to the yz plane. Α is an angle equal to the rising angle of S ₂ from the y-axis.

ただし、Ｓ_３は、第３関節４ｃの回転中心から候補接触点Ａまでの距離がｙｚ平面に投影された長さである。また、βはｙ軸からのＳ_３の立ち上がり角度と等しい角度である。 Further, in FIG. 8, when rotating only the third joint 4c small angle [Delta] [theta] _3, [Delta] x, [Delta] y, displacement [Delta] x _.theta.3 candidate contact point A in the three directions of Δz, Δy _θ3, Δz _θ3 is given by: It is done.

However, S ₃ is the length of the distance from the rotation center to the candidate contact point A of the third joint 4c is projected to the yz plane. Β is an angle equal to the rising angle of S ₃ from the y-axis.

尚、ｙ方向変位及びｚ方向変位についても同様に定義できる。 Here, the contribution ratio of the rotation angle Δθ _i (i = 1 to 3) of an arbitrary joint in the Δx direction is defined by the following equation.

The y-direction displacement and the z-direction displacement can be defined similarly.

Together above (9) to (12), [Delta] x, [Delta] y, in each direction of Delta] z, rotation angle [Delta] [theta] ₁ of the first joint 4a, the rotation angle [Delta] [theta] ₂ of the second joint 4b, the rotation angle of the third joint 4c The contribution ratio of Δθ ₃ is summarized as shown in the following table.

This contribution ratio analysis shows which joint is efficient to move in order to reduce the error in each direction of Δx, Δy, and Δz. That is, in the direction of Δx, the contribution ratio cos θ ₁ of Δθ ₁ is the largest, so it is most efficient to rotate the first joint 4a. Similarly, since the contribution rate sin α of Δθ ₂ is the largest in the direction of Δy, it is most efficient to rotate the second joint 4b. Similarly, in the direction of Δz, since the contribution ratio cos β of Δθ ₃ is the largest, it is most efficient to rotate the third indirect 4c.

ここで、ｋ１、ｋ２、ｋ３は定数で、各方向における誤差が減少するように正負が設定される。 From the above consideration, it has been found that the angle adjustment value of each joint may be set in proportion to the error in each direction of Δx, Δy, and Δz. A specific adjustment algorithm is shown in the following equation.

Here, k1, k2, and k3 are constants, and positive and negative are set so that an error in each direction is reduced.

  Subsequently, a calculation processing procedure by the gripping posture calculation unit 14 of the robot hand control unit 11 will be described with reference to FIG. FIG. 9 is a flowchart illustrating a calculation processing procedure of the gripping posture calculation unit 14. In this description as well, the fourth joint 4d of the robot hand 1 described above is omitted to further simplify the form, and a sequential calculation method will be described using a finger having three degrees of freedom as an example.

First, the coordinates of the target contact point P on the object 2 are determined by the image recognition unit 10, and the normal vector V ₀ at the target contact point P is detected (step S01).

Then, the coordinates of the target contact point P on the object 2 and the normal vector V ₀ are input to the gripping posture calculation unit 14. The gripping posture calculation unit 14 sets initial values of the finger joint angles θ _{1 to} θ ₃ (step S02).

Subsequently, the initial values of the joint angles θ _{1 to} θ ₃ are substituted into the above equation (1), and the coordinates of each point i on the fingertip portion 6 are calculated. Further, the normal vector V _i at each point i on the fingertip portion 6 is calculated by the equation (7) (step S03).

  Subsequently, the normal direction error e is calculated based on the equation (8), the normal direction errors e at all points on the fingertip surface are compared, and the point where e is the minimum value is set as the candidate contact point A. (Step S04).

ここで、εは閾値である。この（１４）式を満たさない場合には、関節角を次式に基づいて調整する（ステップＳ０７）。

その後、更に関節角を調整すべくステップＳ０３に戻る。 Subsequently, coordinate differences Δx, Δy, Δz between the candidate contact point P and the candidate contact point A are calculated (step S05). It is determined whether or not the coordinate differences Δx, Δy, Δz satisfy the following equation (step S06).

Here, ε is a threshold value. If this equation (14) is not satisfied, the joint angle is adjusted based on the following equation (step S07).

Thereafter, the process returns to step S03 to further adjust the joint angle.

On the other hand, if the expression (14) is satisfied in step S06, the joint angles θ _{1 to} θ ₃ given at that time are set as the target joint angles of the respective joints 4 (step S08).

  By executing the arithmetic processing as described above for each finger 3A to 3D, the target joint angle of each joint 4 of the fingers 3A to 3D is obtained, and the gripping posture angle of the robot hand 1 as a whole is obtained.

  Thus, the target joint angle of each joint 4 of the fingers 3A to 3D obtained by the gripping posture calculation unit 14 is sent to the control calculation unit 15, and a joint angle command value is created in the control calculation unit 15. The joint angle command value is sent to each motor driver 12, and each motor 5 is driven and controlled by each motor driver 12. As a result, the fingers 3A to 3D of the robot hand 1 are in the holding posture, and the object 2 is held by the fingers 3A to 3D (step S09). Further, the gripping force is controlled based on information from the six-axis force sensor 8 (step S10).

In the above, the image recognition unit 10 constitutes a contact position setting unit that determines a target contact point in contact with the fingertip portion 6 of each finger 3 on the object 2 and obtains a normal vector V ₀ at the target contact point. The gripping posture calculation unit 14 of the robot hand control unit 11 selects the normal in the direction closest to the direction of the line along the normal vector V ₀ from the arbitrary points set on the fingertip part 6 of each finger 3. It functions as a candidate position selection means using a point it has as a candidate contact point.

  The gripping posture calculation unit 14 of the robot hand control unit 11 constitutes a gripping posture calculation unit that calculates a target angle of the joint 4 such that the target contact point and the candidate contact point substantially contact each other with respect to each finger 3. The control calculation unit 15 and the motor driver 12 of the robot hand control unit 11 constitute drive control means for controlling the motor 5 as an actuator according to the target angle of the joint 4 for each finger 3.

  As described above, in the present embodiment, candidate contact points on the fingertip portion 6 having normals in the direction along the normal direction at the target contact points set on the object 2 having an arbitrary shape are determined. A target joint angle is obtained so that the target contact point and the candidate contact point are in contact with each other, and the motor 5 of each finger 3 is driven and controlled according to the target joint angle. Therefore, even if the fingertip portion 6 of each finger 3 has a curved surface shape, the robot hand 1 can reliably hold the object 2 having any shape. At this time, since the target joint angle of each finger 3 is automatically obtained by calculation, teaching work relating to the operation of the robot hand 1 is not necessary, and the object 2 can be gripped efficiently.

  In addition, since a simple mathematical expression such as the expression (15) can be used to determine the adjustment amount Δθ of the joint angle, for example, the calculation is simpler than the technique of obtaining the derivative of the evaluation function. An adjustment amount can be obtained.

  Since the adjustment amount Δθ of the joint angle is proportional to the coordinate difference, when the object is away from the fingertip, the joint angle can be quickly approached in a large step. On the other hand, when the fingertip approaches the object, the search efficiency is improved by accurately converging at the Komaki step.

  In addition, the robot hand of the above embodiment has four fingers, but the number of fingers may be plural, and the number of joints and the number of degrees of freedom of each finger are particularly limited to those described above. Absent.

It is a figure which shows the external appearance of the principal part of the robot hand of this embodiment. It is a figure which shows the structure of the holding | grip control apparatus of this embodiment. It is a figure which shows the position and orientation relationship between the finger | toe of a robot hand and an object. It is a figure which shows the element division | segmentation of a robot hand. It is a figure which shows the absolute coordinate system and link coordinate system of the index finger of a robot hand, a middle finger, and a ring finger. It is a figure which shows the absolute coordinate system and link coordinate system of the thumb of a robot hand. It is a figure for demonstrating the method of converging the error of the candidate contact point of a robot hand, and a target contact point. It is a figure for demonstrating the method of converging the error of the candidate contact point of a robot hand, and a target contact point. It is a flowchart for demonstrating the calculation processing procedure of the holding | grip attitude | position calculating part shown in FIG.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1 ... Robot hand, 2 ... Object, 3 ... Finger, 3A ... Thumb, 3B ... Index finger, 3C ... Middle finger, 3D ... Ring finger, 4 ... Joint, 5 ... Motor, 6 ... Finger tip part, 7 ... Grip control apparatus, 8 ... 6-axis force sensor, 10 ... image recognition unit, 11 ... robot hand control unit, 12 ... motor driver, 14 ... gripping posture calculation unit, 15 ... control calculation unit.

Claims (2)

A non-cylindrical body comprising a plurality of fingers for gripping an object of arbitrary shape and a plurality of actuators for driving the joints of the fingers, each finger having a curved surface portion for gripping the object at the tip A gripping control device for a robot hand having a fingertip portion ,
A contact position setting means for determining a target contact point in contact with a fingertip portion of each finger on the object by image recognition, and obtaining a normal line of the surface of the object at the target contact point;
For each finger, the direction of the normal of the curved surface at a point set on the curved surface of the surface of the fingertip is obtained, and the direction of the normal of the curved surface at that point among the points set on the surface And a candidate position selecting means for determining a point between which the error is a minimum value as a candidate contact point ,
A gripping posture calculation means for obtaining a target angle of the joint so that the target contact point and the candidate contact point are in contact with each other;
A grip control device comprising: a drive control unit that controls the actuator according to a target angle of the joint for each finger.   The grip control apparatus according to claim 1, wherein the grip posture calculation unit obtains a target angle of the joint so as to reduce a coordinate difference between the target contact point and the candidate contact point in an arbitrary coordinate system.

Images