JP5939505B2 - Robot hand device and control method - Google Patents

Description

  The present invention relates to a robot hand apparatus that performs an operation on an object whose position and orientation change. The present invention also relates to a robot hand control method.

  Work on the object includes gripping the object, attaching an object to the object, or removing the object from the object.

  When such a work is performed by the robot hand, the robot hand is made to follow the object when the position or posture of the object changes. In this state, the robot hand performs work on the object. For example, when the object is being conveyed by the conveyor at a constant speed in a certain movement direction, the robot hand is caused to perform the same movement as the object. In this state, the robot hand grips the object being transported.

  Such control is described in Patent Document 1 below, for example. In Patent Document 1, an object is imaged with a camera, and a feature (for example, a hole) of the object is extracted from the captured image, and control is performed so that the robot hand follows the object based on this feature.

Japanese Patent No. 4265088

  The trajectory of the robot hand may be set in advance in accordance with the movement of the object. For example, control is performed so that the robot hand moves on the set trajectory so as to perform the same motion as the object. As a result, the robot hand works on the object in a state where the relative speed between the object and the robot hand is zero.

  Even when the set trajectory is used as described above, the position and orientation of the object can be sequentially measured, and the set trajectory can be changed according to the measurement result. This allows the position and orientation of the object to be used even when there is an error between the position and orientation of the object and the set trajectory, or when the change in the position or orientation of the object cannot be predicted. Can respond to changes in

  However, if the position or posture of the target object deviates significantly from the set orbit or posture of the robot hand due to unexpected sudden changes in the position or posture of the target object, it is necessary to change the set orbit or posture of the robot hand rapidly. There is. As a result, an operation command exceeding the operation capability of the robot hand is given to the robot hand. That is, the robot hand becomes inoperable. The problem caused by the sudden change in the position and orientation of the object is the same in the control that does not use the set trajectory of the robot hand.

  Therefore, an object of the present invention is to provide an apparatus and a method for continuing the operation of a robot hand so as to cope with a change in the position and posture of an object suddenly.

To achieve the above object, according to the present invention, a robot hand that performs an operation on an object;
An object measuring device for measuring the position and orientation of the object;
A hand measuring device for measuring the position and orientation of the robot hand;
A control device for operating the robot hand based on the measurement result of the object measuring device and the hand measuring device,
(A) In the current control cycle, the object measuring device measures the position and orientation of the object, and the control device determines the target position of the robot hand in the next control cycle based on the measured position and orientation. Search for the target posture,
(B) In the next control cycle, the hand measurement device measures the position and posture of the robot hand, and the control device sets the measured position and posture, and the target position and posture searched in (A). There is provided a robot hand device characterized by controlling the position and posture of the robot hand based on the above.

According to a preferred embodiment of the present invention, the control device has a storage unit for storing the set trajectory of the robot hand,
At each of a plurality of via positions in the set trajectory, the posture of the robot hand associated with the via position is set,
With the via position and setting posture associated with each other as a set, the control device, in (A),
(A1) The parallel movement and rotation of the set trajectory are performed so that the set position and posture corresponding to the next control cycle are moved toward the position and posture of the object measured in the current control cycle, respectively. Move the set trajectory by doing one or both,
(A2) determining whether or not the set of the via positions and the set posture moved toward the position and posture of the object measured in the current control cycle satisfy the operation restriction of the robot hand;
(A3) If it is determined that the motion constraint is not satisfied, another set of via position and set posture in the set trajectory after movement is selected,
(A4) Determine whether another set of via positions and set postures selected in (A3) satisfy the robot hand movement constraints,
(A3) and (A4) are repeated until a set of via positions and set postures that satisfy the robot hand movement constraints are found,
When a set of via positions and set postures satisfying the operation constraints are found in (A2) or (A4), the set of via positions and set postures are set as target positions and target postures in the next control cycle. select.

Preferably, in (A3), in the set trajectory after movement, the via position that is in front of the via position moved toward the position of the object measured in the current control cycle, and the setting of the via position Select posture.
Instead, in (A3), in the set trajectory after movement, from the via position moved toward the position of the object measured in the current control cycle, the via position existing within the set range, and the via A position setting posture may be selected.

According to the reference example , in (A), the control device is within the set range from the position and posture of the object measured in the current control cycle, and is within the allowable range from the current position and posture of the robot hand. A certain position and posture are set as the target position and target posture of the robot hand in the next control cycle.

In order to achieve the above object, according to the present invention, there is provided a method for controlling a robot hand that performs an operation on an object,
(A) In the current control cycle, the position and orientation of the object are measured by the object measuring device, and the target position of the robot hand in the next control cycle is determined based on the measured position and posture by the control device. Search for the target posture,
(B) In the next control cycle, the hand measurement device measures the position and posture of the robot hand, and the control device uses the measured position and posture, and the target position and posture searched in (A). A robot hand control method characterized by controlling the position and orientation of the robot hand based on the above is provided.

  According to the present invention described above, the target position and posture of the robot hand in the next control cycle are searched based on the position and posture of the target measured in the current control cycle. Even if it changes, the operation of the robot hand can be continued to cope with this change.

It is a schematic block diagram of the robot hand apparatus by embodiment of this invention. 3 is a flowchart illustrating a robot hand control method according to an embodiment of the present invention. It is explanatory drawing of the setting track | orbit of a robot hand. It is a figure explaining the movement of a setting orbit. It is an example which shows the movement locus | trajectory of a target object. The position of the object with respect to time in the case of FIG. 5 is shown. FIG. 7 shows a set trajectory of the robot hand of FIG. 1 for the cases of FIGS. The simulation result in the case of FIGS. 5-7 is shown.

  A preferred embodiment of the present invention will be described with reference to the drawings. In addition, the same code | symbol is attached | subjected to the common part in each figure, and the overlapping description is abbreviate | omitted.

  FIG. 1 is a schematic configuration diagram of a robot hand apparatus 10 according to an embodiment of the present invention. The robot hand device 10 includes a robot hand 3, an object measuring device 5, a hand measuring device 7, and a control device 9.

  The robot hand 3 performs work on the object 1. The work on the target object 1 by the robot hand 3 is, for example, gripping the target object 1, attaching an object to the target object 1, or removing an object from the target object 1.

In the example of FIG. 1, the robot hand 3 includes first to fifth arms 3a to 3e and a work execution unit 3f.
One end of the first arm 3a is coupled to the hand support 11 so as to be rotatable about the first axis C1. One end of the second arm 3b is coupled to the other end of the first arm 3a so as to be rotatable about the second axis C2. One end of the third arm 3c is coupled to the other end of the second arm 3b so as to be rotatable about the third axis C3. One end of the fourth arm 3d is connected to the other end of the third arm 3c so as to be rotatable about the fourth axis C4. One end of a fifth arm 3e is connected to the other end of the fourth arm 3d so as to be rotatable about a fifth axis C5. A work execution unit 3f is coupled to the other end of the fifth arm 3e so as to be rotatable about a sixth axis C6.
The work execution unit 3 f performs work on the object 1. The work execution unit 3f may include a suction unit that holds the target 1 by sucking the target 1 and releases the held target 1 by stopping the suction. Instead, the work execution unit 3f has a plurality of claws, grips the object 1 by closing the claws, and releases the object 1 held by opening the claws. It may be a thing. The work execution unit 3f may have other configurations.

  The object measuring device 5 measures the position and orientation of the object 1. The object measuring device 5 may be, for example, a device that uses a camera that captures an area including the object 1. In this case, the position of the object 1 is obtained based on the position and size of the object 1 in the captured image, and the posture of the object 1 is obtained based on the orientation of the object 1 in the image. The object measuring device 5 may have other configurations.

The hand measuring device 7 measures the position and posture of the robot hand 3. In the example of FIG. 1, the hand measuring device 7 includes the rotation angle of the first to fifth arms 3a to 3e about the first to fifth axes C1 to C5 and the work execution unit 3f about the sixth axis C6. And the position and posture of the robot hand 3 (work execution unit 3f) are obtained based on these rotation angles.
The hand measurement device 7 includes a plurality of angle sensors that respectively measure the rotation angles of the first to fifth arms 3a to 3e and the work execution unit 3f. However, in FIG. Only the angle sensor 7 for measuring the rotation angle 3a is shown.

  The control device 9 operates the robot hand 3 based on the measurement results of the object measuring device 5 and the hand measuring device 7 and the set trajectory of the robot hand 3. This causes the robot hand 3 to perform the above operation.

  In the present embodiment, the posture of the robot hand 3 associated with each via position is set for each of a plurality of via positions in the set trajectory. The route position and the setting posture of each set are stored in the storage unit 9a of the control device 9 with the route position and the setting posture associated with each other as one set.

  In the example of FIG. 1, the control device 9 rotates the first to fifth arms 3a to 3e about the first to fifth axes C5, respectively, and moves the work execution unit 3f about the sixth axis C6. Rotate. These rotations may be performed by the controller 9 controlling a driver (not shown) of a servo motor that rotationally drives the first to fifth arms 3a to 3e and the work execution unit 3f.

  The object measuring device 5, the hand measuring device 7, and the control device 9 repeat the following processes (A) and (B) in this order.

(A) In the current control cycle, the object measuring device 5 measures the position and orientation of the object 1, and the control device 9 determines the robot hand 3 in the next control cycle based on the measured position and orientation. Search the target position and target posture.

(B) In the next control cycle, the hand measuring device 7 measures the position and posture of the robot hand 3, and the control device 9 determines the measured position and posture and the target position searched in (A). The position and posture of the robot hand 3 are controlled based on the target posture. For example, the control device 9 controls the position of the robot hand 3 so as to reduce the difference between the position of the robot hand 3 measured in the next control cycle and the target position (preferably to eliminate this difference). Then, the posture of the robot hand 3 is controlled so as to reduce the difference between the posture of the robot hand 3 and the target posture measured in the next control cycle (preferably so as to eliminate this difference).

  By repeating the control cycle many times, the robot hand 3 approaches the target object 1 from the standby position and performs an operation on the target object 1. For example, one control cycle is 10 milliseconds, and the time required for the robot hand 3 to work is 4 seconds.

  FIG. 2 is a flowchart illustrating a robot hand control method according to an embodiment of the present invention.

This control method is performed after moving the robot hand 3 to a position where the distance from the object 1 is equal to or less than the set value. In this case, the control device 9 may control the robot hand 3 so that the robot hand 3 moves on the set trajectory until the robot hand 3 is moved to this position.
However, when the distance between the standby position of the robot hand 3 and the object 1 is equal to or less than the set value, the control method of this embodiment may be performed from the time when the robot hand 3 is operated from the standby position.

  In step S1, the object measurement device 5 measures the position and orientation of the object 1 in the current control cycle, and the control device 9 determines the robot hand in the next control cycle based on the measured position and orientation. 3 target position and target posture are searched.

  Next, after the control cycle i shifts to the next control cycle (i + 1), the process proceeds to step S2. Note that i is a positive integer and indicates a control cycle number (the same applies hereinafter).

  In step S2, the hand measuring device 7 measures the position and posture of the robot hand 3, and the control device 9 performs the robot hand based on the measured position and posture, and the target position and target posture searched in step S1. 3 position and orientation are controlled. For example, the control device 9 controls the position of the robot hand 3 so as to reduce (preferably eliminate this difference) the difference between the measured position of the robot hand 3 and the target position obtained in step S1. . Similarly, in step S2, the controller 9 reduces the difference between the measured posture of the robot hand 3 and the target posture obtained in step S1 (preferably to eliminate this difference). To control the attitude.

  Step S1 includes steps S11 to S16.

  In step S <b> 11, the position and orientation of the object 1 are measured by the object measuring device 5.

In step S12, a set of via positions and set postures in the set trajectory portion that the robot hand 3 (work execution unit 3f) is scheduled to pass in the next control cycle are specified. The set of the via position and the set posture are preferably an initial target position and an initial target posture that are predetermined for the next control cycle in a predetermined initial set trajectory, respectively. The set trajectory consists of a via position determined for each elapsed time (each time point) from the start of the operation control of the robot hand 3. Accordingly, “the portion of the set trajectory that the robot hand 3 is scheduled to pass in the next control cycle” means a plurality of transit positions corresponding to the next control cycle. In this regard, the upper part of FIG. 3 shows a set trajectory through which the reference point of the work execution unit 3f in the case of FIG. 1 passes, and the lower part of FIG. It is an enlarged view. In the example of the lower part of FIG. 3, a passing position (black circle in FIG. 3) is determined for each time point t _{m + 1 to} t _{m + 10} , where m is a positive integer. In FIG. 3, for example, the initial target position in the current control cycle i is a via position for t _{m + 5} , and the initial target position in the next control cycle (i + 1) is a via position for t _{m + 10} .
1 and 3 show an orthogonal coordinate system having an X axis that faces the horizontal direction, a Y axis that faces the horizontal direction perpendicular to the paper surface, and a Z axis that faces the vertical direction.

In step S13, toward the position and orientation of the object 1 measured in step S11 (that is, the current control cycle) (in this embodiment, to the position and orientation of the object 1 measured in step S11), step S12. The set trajectory is moved by performing one or both of the parallel movement of the set trajectory and the rotation of the set trajectory so as to move the set of via positions and the set posture specified in (1). Here, the measured position of the target object 1 is also the position of the robot hand 3 (work execution unit 3f) that can work on the target object 1. The measured posture of the target object 1 is relative to the target object 1. It is also the posture of the robot hand 3 (work execution unit 3f) that can work.
Note that when the via position of the set identified in step S12 matches the position measured in step S11, the amount by which the via position is moved in step S13 is zero. Similarly, when the set posture specified in step S12 matches the posture measured in step S11, the amount by which the set posture is moved in step S14 is zero.

  Step S13 will be described in detail later.

  In step S14, a set of transit positions and set trajectories in the set trajectory moved in step S13 are selected. In the first step S14 in the current control cycle, the via position and the set posture specified in step S12 in the set trajectory after movement are selected.

  In step S <b> 15, it is determined whether or not the set of via positions and set postures selected in step S <b> 14 satisfy the operation constraints of the robot hand 3. If it is determined that the operation restriction of the robot hand 3 is satisfied, the process proceeds to step S2, and if not, the process proceeds to step S16.

  Step S15 will be described in detail later.

  In step S16, it is determined whether step S14 has been performed a set number of times (an integer greater than or equal to 2) in the current control cycle. Here, if this determination is affirmative, the operation of the robot hand 3 is ended as an error, and if not, the operation returns to step S14, and in the returned step S14, the setting moved in step S13. Another set of via positions and set postures in the trajectory (the set trajectory after movement) is selected, and in the next step S15, whether or not the via positions and set postures of the selected set satisfy the operation restriction of the robot hand. to decide.

Preferably, in the returned step S14, in the set trajectory after the movement, the route that is in front of the route position moved toward the position of the object 1 in step S13 (that is, the route position specified in step S12). The position and the setting posture of the via position in front of the position are selected. Here, “front” means the side of the transit position determined with respect to the above-described operation control start time in the set trajectory. More preferably, in step S14 after the third time, a route position in front of the route position selected in the previous step S14 and a setting posture of the route position in front of the route position are selected.
Instead, in the returned step S14, in the set trajectory after the movement, from the transit position moved toward the position of the object 1 in step S13, the transit position existing within the set range and the set posture of the transit position And may be selected. Here, “within the set range” means that the distance from the transit position moved toward the position of the object 1 in step S13 is smaller than the set value. Further, the “setting range” is not limited to a range in front of the via position moved toward the position of the object 1 in step S13.

  Subsequent processing is the same as described above, and steps S14 to S16 described above are repeated.

  In addition, when progressing to step S2 from step S15, after moving from the present control period i to the next control period (i + 1), the next step S2 is performed. In the next step S2, the set of via positions and set postures selected in the immediately preceding step S14 are used as the target position and target posture.

  Step S2 includes step S21 and step S22.

In step S21, the position and orientation of the robot hand 3 are measured by the hand measuring device 7.
In step S22, the control device 9 sets the position and posture of the robot hand 3 measured in step S21 and the set of passing positions and set postures selected in the immediately preceding step S14 as the target position and target posture. Then, the position and posture of the robot hand 3 are controlled based on the target posture.
When step S22 is completed, the process returns to step S1.

  Next, a specific example of step S13 described above will be described in detail.

In the set trajectory moved in step S13, the set of via positions and set postures selected in step S14 can be expressed by the following Qt.

Qt = S [i] Q [c]

S [i]: A homogeneous transformation matrix representing the position and orientation measured in step S11. This S [i] is expressed in the coordinate system of the robot hand 3.
Q [c]: A homogeneous transformation matrix representing a set of transit positions and set postures selected in step S14 in the set trajectory moved in step S13. This Q [c] is expressed in the coordinate system of the object 1.
Qt: a homogeneous transformation matrix representing the position and orientation of the robot hand 3. This Qt is expressed in the coordinate system of the robot hand 3.

In FIG. 4, the broken line indicates a part of the set trajectory before being moved in step S13, the black circle on the broken line indicates the via position, and the broken circle written concentrically with the black circle on the broken line is in step S12. An arrow D that indicates the specified via position and extends from the broken circle indicates the set posture (direction of the robot hand 3) of the via position specified in step S12.
In FIG. 4, the solid line indicates a part of the setting trajectory moved from a part of the broken setting trajectory in step S13, and the black circle on the solid line indicates the via position on the setting trajectory after the movement. The solid circle written concentrically with the black circle indicates the via position specified in step S12 in the set trajectory after movement, and the arrow D extending from the dashed circle indicates step S12 in the set trajectory after movement. The setting posture (direction of the robot hand 3) of the via position specified in (5) is shown.

Further, as shown in FIG. 4, via positions (black circles in FIG. 4) are determined for each time point t _{m + 3 to} t _{m + 10} . In FIG. 4, the via position identified in step S12 is the via position for t _{m + 10} .

ここで、ロボットハンド３の座標系が、ｘ軸とｙ軸とｚ軸を有するとする。
この［数１］において、ｘ、ｙ、ｚは、対象物１の位置を示し、それぞれ、ｘ軸座標、ｙ軸座標、ｚ軸座標であり、α、β、γは、対象物１の姿勢を示し、それぞれ、ｘ軸回りの回転角度、ｙ軸回りの回転角度、ｚ軸回りの回転角度である。 S [i] is expressed by the following [Equation 1].

Here, it is assumed that the coordinate system of the robot hand 3 has an x axis, a y axis, and a z axis.
In [Equation 1], x, y, and z indicate the position of the object 1, and are x-axis coordinates, y-axis coordinates, and z-axis coordinates, respectively, and α, β, and γ are the postures of the object 1. These are the rotation angle around the x-axis, the rotation angle around the y-axis, and the rotation angle around the z-axis, respectively.

When Ax, Ay, Az, and P are defined for [Expression 1] as in [Expression 2] below, S [i] can be expressed by [Expression 3] below.

When Q [c] is the via position and the set attitude specified in step S12 (that is, the via position and the set attitude with respect to the time point t _{m + 10} in FIG. 4), Q [c] (hereinafter, Q [10] and Can be expressed by the following homogeneous transformation matrix of [Equation 4].

In this case, through the position relative to the time t _{m + 9} and setting attitude in FIG. 4 (hereinafter, referred to as Q [9]) is determined from the relative position and orientation between the time t _{m + 10} in the setting track. For example, it is assumed that Q [9] is expressed by the following [Equation 5].

Here, it is assumed that S [i] is the following [Equation 6]. In this case, S [i] Q [10] is represented by the following [Equation 7], and S [i] Q [9] is represented by the following [Equation 8].

  The same applies to other via positions and set postures.

  Next, the operation restriction in step S15 will be described in detail.

  As the operation constraints used in step S14, there are the following (1) to (7). Preferably, the operation restriction includes at least (1) among (1) to (7). However, the operation restriction may be any one of (1) to (7) or a plurality of selections selected from (1) to (7).

(1) The magnitude of the operation speed of the robot hand 3 is smaller than the threshold value.
Here, the operation speed may be a value obtained by dividing the required movement amount from the target position and target posture of the current control cycle to the target position and target posture of the next control cycle by the time of one control cycle.
In the example of FIG. 1, the operation speed is the rotation speed ω of each of the first to fifth arms 3a to 3e and the work execution unit 3f (hereinafter referred to as a rotation unit). In this case, each rotational speed ω is expressed by the following equation.

ω = (θt−θc) / T

In this equation, θt is the rotation angle of the rotating unit corresponding to the rotation speed ω when the robot hand 3 is in the target position and target posture of the next control cycle, and θc is the current value of the robot hand 3 This is the rotation angle of the rotation unit corresponding to the rotation speed ω when the target position and the target posture of the control cycle are present. T is the time of one control cycle (hereinafter the same).

(2) The magnitude of the motion acceleration of the robot hand 3 is smaller than the threshold value.
Here, the motion acceleration may be a value obtained by dividing the difference between the motion speed of the robot hand 3 in the current control cycle and the motion speed of the robot hand 3 in the next control cycle by the time of one control cycle. .
In the example of FIG. 1, the operation acceleration is the rotation acceleration a of each of the rotation units 3 a to 3 f. In this case, each rotational acceleration a is expressed by the following equation.

a = (ω−ω ₀ ) / T

In this equation, the rotational speed ω is the same as ω in (1) above, and ω ₀ is the target rotational speed of the rotating unit in the current control cycle. The target rotational speed ω ₀ is expressed by the following formula.

ω ₀ = (θ _i −θ _i−1 ) / T

In this equation, θ _i is the rotation angle of the rotating unit when the robot hand 3 is at the target position and target posture in the current control cycle i, and θ _i−1 is the robot hand 3 in the current control cycle. This is the rotation angle of the rotating unit when the target position and the target posture are in the previous control cycle (i-1).

(3) the magnitude of the relative velocity v _r of the robot hand 3 with respect to the object 1, less than the threshold. The v _r, for example, represented by the following formula.

v _r = v _h −v _t

v _h is the amount of movement of the robot hand 3 (work execution unit 3f) required to make the target position and posture in the current control cycle the target position and posture in the next control cycle, and the time of the control cycle. The value divided by T.
v _t is an object necessary to make the measured position and posture of the object 1 in the control cycle immediately before the current control cycle the measured position and posture of the object 1 in the current control cycle. This is a value obtained by dividing the movement amount of the object 1 by the time T of the control cycle.

(4) The magnitude of relative acceleration of the robot hand 3 with respect to the object 1 is smaller than the threshold value.
(5) The set of target position and target posture selected in step S14 does not become a singular point.
Here, the singular point means that there is no inverse matrix in the matrix (Jacobi matrix) for converting the rotational speed of each rotary part of the robot hand 3 into the translational movement speed of the work execution part 3f, that is, the translation of the work execution part 3f. It means that the rotational speed of each rotating part cannot be obtained from the moving speed. When the absolute value of the eigenvalue of the Jacobian matrix or the absolute value of the determinant is smaller than the threshold value, it can be determined that the Jacobian matrix does not have an inverse matrix.
(6) The robot hand 3 moves out of the setting range.
(7) The axes C1 to C6 of the robot hand 3 move out of the setting range.

According to the embodiment described above, the target position and target posture of the robot hand 3 in the next control cycle are searched based on the position and posture of the target 1 measured in the current control cycle. Even if the posture changes rapidly, the robot hand 3 can be controlled in response to this change.
Moreover, since the target position and target orientation of the robot hand 3 in the next control cycle are searched from the set trajectory moved toward the measured position and orientation of the target object 1, the robot hand 3 reflecting the set trajectory is searched. Operation is possible.
In step S14 for the second and subsequent times, in the set trajectory after movement, a transit position that is in front of the transit position selected in the first step S14 and a set posture of the transit position that is in front are selected. The via position and the set posture selected in this way are likely to be closer to the current position and posture of the robot hand 3. As a result, for example, the above-described operation constraints (1) and (2) are easily satisfied.

  Further, the set trajectory and the set posture need not exactly match the movement trajectory and posture of the object 1. That is, even if the set trajectory is roughly determined, the set trajectory is moved toward the measured position and orientation of the object 1, so that it is possible to cope with changes in the position and orientation of the object 1.

FIG. 5 is an example showing the movement trajectory of the object 1. In the case of FIG. 5, as shown in FIG. 1, the object 1 moves the pendulum around the fulcrum in the XZ plane. This movement assumes the movement of the object 1 suspended from the fulcrum by a rope. That is, when the fulcrum moves horizontally and stops, the object 1 moves as a pendulum.
FIG. 6 shows the position of the object 1 with respect to time in the case of FIG. In this example, the posture of the object 1 is constant.

FIG. 7 shows the set trajectory of the robot hand 3 of FIG. 1 for the cases of FIGS. In this example, the set trajectory is roughly defined. In FIG. 7, the horizontal axis represents time, and the vertical axis represents the distance from the work execution unit 3f to the center position of the pendulum motion (corresponding to the origin in FIG. 5) in the X-axis direction.
FIG. 8 shows a simulation result of the control method according to the above-described embodiment. FIG. 8A shows the relationship between the rotation speed (angular velocity) of the second arm 3b and time, and FIG. 8B shows the relationship between the rotation speed (angular velocity) of the third arm 3c and time. In this example, the other arms 3a, 3d, 3e and the work execution unit 3f are not rotated, and the work execution unit 3f is moved in the XZ plane.
The solid lines in FIGS. 8A and 8B show the case where the above-described operation restriction is not used. That is, the solid lines in FIGS. 8A and 8B show a case where steps S15 and S16 are omitted and step S21 is performed after step S14. The broken lines in FIGS. 8A and 8B show the case where the above-described operation restriction is used. That is, the broken lines in FIGS. 8A and 8B show the case of following the flowchart of FIG. Here, the operation restriction is that the magnitude of the angular velocity of the second and third arms 3b and 3c is 75 degrees / second or less. 8A and 8B, the broken line overlaps the solid line except for the range of about 0.2 to 0.9 seconds.

  According to FIGS. 6, 8 </ b> A, and 8 </ b> B, the work execution unit 3 f operates so as to follow the pendulum movement of the object 1.

The present invention is not limited to the above-described embodiment, and various changes can be made without departing from the scope of the present invention .

( Reference example )
The set trajectory of the robot hand 3 may not be used. In this reference example, in step S1 (that is, the process of (A)), the position and orientation of the object 1 are measured by the object measuring device 5 and measured by the control device 9 in the current control cycle. The position and posture within the set range from the position and posture and within the allowable range from the current position and posture of the robot hand 3 are set as the target position and target posture of the robot hand 3 in the next control cycle. In this case, in the range including the range in which the robot hand 3 moves, a large number of candidate positions are set at intervals, and the controller 9 determines the robot hand 3 in the next control cycle from these candidate positions. A target position may be selected. Also, a large number of candidate postures of the robot hand 3 may be set, and the target posture of the robot hand 3 in the next control cycle may be selected by the control device 9 from these candidate postures. Note that the current position and orientation of the robot hand 3 may be the latest measured values of the position and orientation of the robot hand 3. Step S2 is the same as described above .

( Example of change )
In the above-described embodiment, in step S13, the route position specified in step S12 is moved to the position of the target 1 measured in step S11. Instead, in step S13, the route specified in step S12 is used. The position may be moved toward the position of the target 1 measured in step S11 so as to approach the position.
Similarly, in step S13 described above, the set posture specified in step S12 is moved (rotated) to the posture of the object 1 measured in step S11. Instead, in step S13, the specified posture is specified in step S12. The set posture may be moved toward the posture of the target object 1 measured in step S11 so as to approach the posture.
In this case, the other points may be the same as described above, or may be appropriately selected from the above description within the scope of the present invention.

DESCRIPTION OF SYMBOLS 1 Object, 3 Robot hand, 3a-3e arm, 3f Work execution part, 5 Object measuring apparatus, 7 Hand measuring apparatus, 9 Control apparatus, 9a Memory | storage part, 10 Robot hand apparatus, 11 Hand support part

Claims (5)

A robot hand that performs work on an object;
An object measuring device for measuring the position and orientation of the object;
A hand measuring device for measuring the position and orientation of the robot hand;
A control device for operating the robot hand based on the measurement result of the object measuring device and the hand measuring device,
(A) In the current control cycle, the object measuring device measures the position and orientation of the object, and the control device determines the target position of the robot hand in the next control cycle based on the measured position and orientation. Search for the target posture,
(B) In the next control cycle, the hand measurement device measures the position and posture of the robot hand, and the control device sets the measured position and posture, and the target position and posture searched in (A). Based on the position and posture of the robot hand ,
The control device has a storage unit for storing the set trajectory of the robot hand,
At each of the plurality of via positions in the set trajectory, the posture of the robot hand that forms a pair with the via position is set,
In (A), the control device
Based on the position and orientation of the object measured in the current control cycle, move the set trajectory by performing one or both of parallel movement and rotation of the set trajectory,
A robot hand device that selects, from a set trajectory after movement, a set of via positions and set postures that satisfy the robot hand movement constraints as target positions and target postures in the next control cycle . A robot hand that performs work on an object;
An object measuring device for measuring the position and orientation of the object;
A hand measuring device for measuring the position and orientation of the robot hand;
A control device for operating the robot hand based on the measurement result of the object measuring device and the hand measuring device,
(A) In the current control cycle, the object measuring device measures the position and orientation of the object, and the control device determines the target position of the robot hand in the next control cycle based on the measured position and orientation. Search for the target posture,
(B) In the next control cycle, the hand measurement device measures the position and posture of the robot hand, and the control device sets the measured position and posture, and the target position and posture searched in (A). Based on the position and posture of the robot hand,
The control device has a storage unit for storing the set trajectory of the robot hand,
At each of a plurality of via positions in the set trajectory, the posture of the robot hand associated with the via position is set,
With the via position and setting posture associated with each other as a set, the control device, in (A),
(A1) The parallel movement and rotation of the set trajectory are performed so that the set position and posture corresponding to the next control cycle are moved toward the position and posture of the object measured in the current control cycle, respectively. Move the set trajectory by doing one or both,
(A2) determining whether or not the set of the via positions and the set posture moved toward the position and posture of the object measured in the current control cycle satisfy the operation restriction of the robot hand;
(A3) If it is determined that the motion constraint is not satisfied, another set of via position and set posture in the set trajectory after movement is selected,
(A4) Determine whether another set of via positions and set postures selected in (A3) satisfy the robot hand movement constraints,
(A3) and (A4) are repeated until a set of via positions and set postures that satisfy the robot hand movement constraints are found,
When a set of via positions and set postures satisfying the operation constraints are found in (A2) or (A4), the set of via positions and set postures are set as target positions and target postures in the next control cycle. When selected, the robot hand device. In (A3), in the set trajectory after movement, the via position that is in front of the via position moved toward the position of the object measured in the current control cycle, and the set posture of the via position select, robot hand apparatus according to claim 2. In (A3), in the set trajectory after the movement, from the via position moved toward the position of the object measured in the current control cycle, the via position existing within the set range and the setting of the via position selecting the posture, the robot hand apparatus according to claim 2. A method for controlling a robot hand that performs work on an object,
(A) In the current control cycle, the position and orientation of the object are measured by the object measuring device, and the target position of the robot hand in the next control cycle is determined based on the measured position and posture by the control device. Search for the target posture,
(B) In the next control cycle, the hand measurement device measures the position and posture of the robot hand, and the control device uses the measured position and posture, and the target position and posture searched in (A). Based on the position and posture of the robot hand ,
The control device has a storage unit that stores a set trajectory of the robot hand, and each of a plurality of via positions in the set trajectory is set with the posture of the robot hand that forms a pair with the via position,
In (A), the control device
Based on the position and orientation of the object measured in the current control cycle, move the set trajectory by performing one or both of parallel movement and rotation of the set trajectory,
A robot hand control method , wherein a set of via positions and set postures that satisfy a robot hand movement constraint are selected from a set trajectory after movement as target positions and target postures in the next control cycle .

Images