WO2020050032A1

WO2020050032A1 - Robot direct teaching device and direct teaching method

Info

Publication number: WO2020050032A1
Application number: PCT/JP2019/032666
Authority: WO
Inventors: 鉄也田原
Original assignee: アズビル株式会社
Priority date: 2018-09-07
Filing date: 2019-08-21
Publication date: 2020-03-12
Also published as: KR102517598B1; JP7095944B2; CN112654469A; KR20210041034A; JP2020040136A

Abstract

This robot direct teaching device is provided with: an external force detecting unit (102) for detecting an external force; a driven control computing unit (103) for calculating movement of an arm (2) due to the external force; a position and attitude computing unit (104) for calculating the position and attitude of the arm (2); a proximity determining unit (105) for determining whether the position and attitude of the arm (2) are approaching a constraint target; a release operation determining unit (106) for determining whether a release operation has been performed; a return determining unit (107) for determining whether a return condition has been satisfied; a target value calculating unit (109) for calculating a target value for constraint control; a constraint control computing unit (110) for calculating the movement of the arm to the target value; a switching unit (108) for switching the operation of the constraint control computing unit (110) on the basis of the determination results obtained by the determining units (105) to (107); a combining unit (111) for combining the calculation result obtained by the driven control computing unit (103) and the computation result obtained by the constraint control computing unit (110); and a drive control unit (112) for driving the arm (2) on the basis of the combined result.

Description

Robot direct teaching device and direct teaching method

The present invention relates to a direct teaching device and a direct teaching method for directly teaching a robot.

In industrial robots, work called teaching is performed in advance to make the robot work. As one of the methods for teaching the robot, there is a method called direct teaching (direct teaching).

For example, Patent Document 1 discloses a direct teaching method of a robot using a force sensor. Further, for example, Patent Document 2 discloses a direct teaching method of a robot using a torque detecting unit. FIG. 23 shows a schematic configuration of the direct teaching device disclosed in these patent documents.

In the direct teaching device 11 shown in FIG. 23, first, the external force detecting unit 1101 detects an external force applied to the arm 2 of the robot by the operator using a force sensor or a torque sensor. Next, the driven control calculation unit 1102 calculates the movement of the arm according to the external force detected by the external force detection unit 1101 (driven control calculation). In addition, the driven control calculation unit 1102 may use parameters related to the position and orientation of the arm measured by the position and orientation measurement unit (not shown) in the driven control calculation. The position and orientation of the arm means at least one of the position of the arm and the orientation of the arm. The parameters include the position of the arm, the posture of the arm, the joint angle of the arm, and the like. Then, the driven control calculation unit 1102 notifies the drive control unit 1103 of the driven control command value updated based on the result of the driven control calculation. Next, the drive control unit 1103 drives the arm 2 according to the driven control command value notified by the driven control calculation unit 1102.
Through this series of operations, the direct teaching device 11 shown in FIG. 23 can control the arm 2 to move according to the external force applied by the operator. Then, when the position and the posture of the arm 2 are in the state intended by the operator by the control, the direct teaching device 11 records the above parameters at that time as a teaching point. The teaching points recorded by the direct teaching device 11 are used when the robot works.

As described above, the direct teaching of the robot has an advantage that the operator directly operates the arm to teach the position and the posture, so that the operator is intuitive and easy to understand. On the other hand, the feature that the arm moves according to the external force applied by the operator is not only an advantage.

For example, consider a case where the operator teaches the position of a certain point P to the robot and then teaches the position of another point Q immediately below the point P. In this case, the operator moves the arm directly downward (Z-axis direction). However, it is difficult for the operator to accurately apply an external force to the arm in a downward direction, and the X-axis coordinate and the Y-axis coordinate of the arm often shift. Although it is possible to later correct only the X-axis and Y-axis coordinates of the arm using a teaching pendant or the like, the advantage of direct teaching is reduced.
Further, for example, consider a case in which an operator teaches while keeping an end effector provided at the tip of an arm directly below. Also in this case, it is difficult for the operator to directly operate the arm while maintaining a state where the arm is directed directly downward, and the posture of the arm often shifts.
As described above, direct teaching is intuitive and easy to understand, but there are cases where operation is difficult.

On the other hand, for example, Patent Documents 3 and 4 disclose techniques for solving the above problems.

Patent Document 3 discloses a direct teaching device in which an operation mode can be switched to a position / direction movement mode, a position movement mode, or a direction movement mode by a mode changeover switch. In the position movement mode, only the position can be moved while maintaining the posture of the tip of the arm. In the direction movement mode, only the posture can be changed while maintaining the position of the tip of the arm. With this direct teaching device, it is possible to alleviate the difficulty of operation in direct teaching as described above. Note that normal direct teaching can be realized in the position / direction movement mode.

特許 Further, Patent Document 4 discloses a direct teaching device that allows an operation mode to be switched to a constrained mode or an omnidirectional movement mode by an input device. In the constraint mode, the tip of the end effector is movable along a specific constraint axis or constraint surface. In the omnidirectional movement mode, normal direct teaching can be performed. In this direct teaching device, by switching to the restraining mode during the direct teaching, the tip of the arm can be accurately moved in the vertical direction, and the difficulty of the operation in the direct teaching is reduced.

JP 05-204441 A JP 05-250029 A JP 05-285870 A JP 05-303425 A

As described above, the direct teaching devices disclosed in Patent Literatures 3 and 4 include a mode for performing normal direct teaching without imposing a constraint on the operation of an arm and a mode for performing direct teaching with a constraint that imposes a constraint on the operation of an arm. And switch. In Patent Document 3, the mode is switched by a mode changeover switch, and in Patent Document 4, the mode is switched by an input device. The mode can be switched by an operator manually operating a switch or the like.

However, in direct teaching, since the operator directly operates the arm by hand, it is not convenient to manually switch the mode. For example, when the operator operates the arm with both hands, when operating the mode switching, the operation of the arm is temporarily stopped and the arm is held with one hand. When the arm held by both hands in normal time is held by one hand, the position and orientation of the arm may be shifted. As a result, the effect of the constrained direct teaching may be reduced.

The present invention has been made to solve the above-described problem, and provides a direct teaching device capable of switching between normal direct teaching and constrained direct teaching while an operator is operating an arm. It is intended to be.

A direct teaching device according to the present invention includes an external force detection unit that detects an external force applied to an arm of a robot, a driven control calculation unit that calculates a movement of the arm according to the external force detected by the external force detection unit, and a position of the arm. Or, based on the calculation result by the position / posture calculation unit that calculates the position / posture that is at least one of the postures, the position / posture of the arm is set to the constraint target to which the position / posture of the arm is restricted. A proximity determination unit for determining whether approaching, a release operation determination unit for determining whether the operator has performed a release operation for releasing the restricted direct teaching, and a return to a state in which switching to the restricted direct teaching is possible. A target value for constraint control in the constrained direct teaching is calculated based on calculation results by a return determination unit that determines whether the condition is satisfied and a constraint target and a position and orientation calculation unit. A target value calculation unit, a constraint control calculation unit that calculates the movement of the arm that moves to the target value calculated by the target value calculation unit based on the calculation result by the position and orientation calculation unit, In some cases, when the proximity determination unit determines that the target is approaching the restriction target, the own device is switched to the second state, and when the own device is in the second state, the release operation determination unit releases the operator. When it is determined that the operation has been performed, the own device is switched to the third state. When the own device is in the third state, the own device is set to the first state when the return determination unit determines that the return condition is satisfied. The state is switched to the state, when the own machine is in the second state, the processing by the restraint control calculation unit is enabled, and when the own machine is in the first state or the third state, the processing by the restraint control calculation unit is performed. The switching unit to be invalidated and the driven control operation unit Calculation results and the combining unit for combining the calculation result of the restraint control calculation unit that, characterized by comprising a drive control unit for driving the arm based on the combined result of the synthesis section.

According to the present invention, since the present invention is configured as described above, it is possible to switch between the normal direct teaching and the restricted direct teaching while the operator is operating the arm.

FIG. 2 is a diagram showing a configuration example of a direct teaching device according to Embodiment 1 of the present invention. 2A to 2D are diagrams showing an example of switching between normal direct teaching and restricted direct teaching (in the case of position constraint) by the direct teaching device according to Embodiment 1 of the present invention. FIGS. 3A to 3D are diagrams showing an example of switching between normal direct teaching and restricted direct teaching (in the case of a posture constraint) by the direct teaching device according to Embodiment 1 of the present invention. 5 is a flowchart illustrating an operation example of the direct teaching device according to the first embodiment of the present invention. FIG. 5 is a diagram illustrating an operation example of the proximity determining unit according to the first embodiment of the present invention (in the case of a posture constraint). FIG. 4 is a diagram for explaining an operation example of a release operation determination unit according to Embodiment 1 of the present invention (when an external force is used). FIG. 5 is a diagram for explaining an operation example of a release operation determination unit according to Embodiment 1 of the present invention (when the position and orientation of an arm is used). FIG. 5 is a diagram illustrating an operation example of a switching unit according to the first embodiment of the present invention. FIG. 5 is a diagram for explaining an operation example of a target value calculating unit according to the first embodiment of the present invention (in the case of a posture constraint). FIG. 5 is a diagram for explaining an operation example of a target value calculating unit according to the first embodiment of the present invention (in the case of a posture constraint). 9 is a flowchart showing an operation example of the direct teaching device according to Embodiment 2 of the present invention. 12A and 12B are diagrams illustrating an operation example of a driven control calculation unit according to Embodiment 2 of the present invention. 9 is a flowchart illustrating an operation example of a proximity determination unit according to Embodiment 2 of the present invention. 14A, 14B, and 14C are diagrams illustrating an operation example of the proximity determining unit according to the second embodiment of the present invention. 15A and 15B are diagrams illustrating an operation example of the target value calculation unit according to the second embodiment of the present invention, and are diagrams illustrating a calculation example of the rotation axis. 16A and 16B are diagrams illustrating an operation example of the target value calculation unit according to the second embodiment of the present invention, and are diagrams illustrating a calculation example of the rotation direction. FIG. 13 is a diagram illustrating an operation example of a target value calculating unit according to Embodiment 2 of the present invention. FIG. 13 is a diagram for explaining shaft restraint by a direct teaching device according to Embodiment 3 of the present invention. 13 is a flowchart illustrating an operation example of the direct teaching device according to Embodiment 3 of the present invention. FIG. 14 is a diagram illustrating an operation example of a proximity determining unit according to Embodiment 3 of the present invention. FIG. 14 is a diagram for describing surface constraint by a direct teaching device according to Embodiment 4 of the present invention. 13 is a flowchart illustrating an operation example of the direct teaching device according to Embodiment 4 of the present invention. FIG. 11 is a diagram illustrating a configuration example of a conventional direct teaching device.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
Embodiment 1 FIG.
FIG. 1 is a diagram showing a configuration example of a direct teaching device 1 according to Embodiment 1 of the present invention.
The direct teaching device 1 directly teaches a robot. The direct teaching device 1 includes, as a direct teaching method, a normal direct teaching that does not impose a constraint on the position and orientation of the arm 2 of the robot, and a constrained direct teaching that restricts the position and orientation of the arm 2 to a constraint target. It can be switched.
The position and posture of the arm 2 means at least one of the position of the arm 2 and the posture of the arm 2. The position of the arm 2 means the position 501 of the end effector 201 provided at the tip of the arm 2, and the posture of the arm 2 means the direction 502 of the end effector 201. The restraint target is a restraint destination of the position and orientation of the arm 2. The constraint target for the position of the arm 2 includes a constraint surface 503 and a constraint shaft 504. The types of the constraint surface 503 include a flat surface and a curved surface. The types of the constraint shaft 504 include a straight line and a curve. The constraint target for the posture of the arm 2 is a constraint direction 505. Further, the number of restraint targets used in the direct teaching device 1 is not limited to one, but may be a plurality. Further, a reference robot coordinate system is set for the robot, and a tool coordinate system is set for the end effector 201 (see FIG. 9 and the like).

As shown in FIG. 1, the direct teaching device 1 includes a position and orientation measurement unit 101, an external force detection unit 102, a driven control calculation unit 103, a position and orientation calculation unit 104, a proximity determination unit 105, a release operation determination unit 106, a return determination. It includes a unit 107, a switching unit 108, a target value calculation unit 109, a constraint control calculation unit 110, a synthesis unit 111, and a drive control unit 112. Note that the driven control calculation unit 103, the position and orientation calculation unit 104, the proximity determination unit 105, the release operation determination unit 106, the return determination unit 107, the switching unit 108, the target value calculation unit 109, the constraint control calculation unit 110, the synthesis unit 111, The drive control unit 112 is realized by a processing circuit such as a system LSI (Large-Scale Integration) or a CPU (Central Processing Unit) that executes a program stored in a memory or the like.

The position and orientation measurement unit 101 measures parameters relating to the position and orientation of the arm 2. The parameters related to the position and posture of the arm 2 include the position of the arm 2, the posture of the arm 2, the joint angle θ of the arm 2, and the like.

The external force detection unit 102 detects an external force applied to the arm 2 by the operator. For example, the external force detection unit 102 may use a force sensor attached to the tip of the arm 2 and detect the force F measured by the force sensor as the external force. Further, for example, the external force detection unit 102 may use a torque sensor attached to the motor drive shaft of the arm 2 and detect the torque τ measured by the torque sensor as the external force. The external force detection unit 102 does not directly measure the external force using the sensor as described above, but indirectly calculates the external force from the current of the motor of the arm 2 or the measured value of the joint angle θ of the arm 2. The external force may be detected using an external force observer.

The driven control calculation unit 103 calculates the movement (the driven control command value) of the arm 2 according to the external force detected by the external force detection unit 102 based on the measurement result by the position and orientation measurement unit 101.

The position and orientation measurement unit 101, the external force detection unit 102, and the driven control calculation unit 103 have the same configuration as that used for normal direct teaching without restriction, and are known technologies.

The position and orientation calculation unit 104 calculates the position and orientation of the arm 2 based on the calculation result by the driven control calculation unit 103.

In FIG. 1, the driven control calculation unit 103 uses the measurement result obtained by the position and orientation measurement unit 101, and the position and orientation calculation unit 104 uses the calculation result obtained by the driven control calculation unit 103.
However, the invention is not limited thereto, and the driven control calculation unit 103 may calculate the movement of the arm 2 according to the external force detected by the external force detection unit 102 without using the measurement result by the position and orientation measurement unit 101. In this case, the position and orientation calculation unit 104 calculates the position and orientation of the arm 2 based on the measurement result by the position and orientation measurement unit 101.
Further, the position and orientation calculation unit 104 may calculate the position and orientation of the arm 2 using both the calculation result by the driven control calculation unit 103 and the measurement result by the position and orientation measurement unit 101.

The proximity determining unit 105 determines whether the position and orientation of the arm 2 is approaching the constraint target based on the calculation result by the position and orientation calculation unit 104.

(4) The release operation determination unit 106 determines whether the operator has performed a release operation for releasing restricted direct teaching. At this time, for example, the release operation determination unit 106 detects the detection result (the magnitude of the external force, the direction of the external force or the pattern of the external force) by the external force detection unit 102 or the calculation result (the position / posture or the position / posture of the position / posture) by the position / posture calculation unit 104. It is determined whether the operator has performed the release operation based on the pattern or the like.

The return determination unit 107 determines whether a predetermined return condition for returning to a state (a first state described later) that can be switched to the restricted direct teaching is satisfied. At this time, for example, the return determination unit 107 determines that the fixed time has elapsed after the operator performed the release operation, or that the position and orientation of the arm 2 calculated by the position and attitude calculation unit 104 does not approach the constraint target. In this case, it is determined that the return condition is satisfied.

The switching unit 108 switches the operation of the constraint control calculation unit 110 according to the determination result by the proximity determination unit 105, the determination result by the release operation determination unit 106, and the determination result by the return determination unit 107. That is, when the direct teaching device 1 is in the first state (during proximity determination), the switching unit 108 determines that the position and orientation of the arm 2 is approaching the constraint target by the proximity determination unit 105. The direct teaching device 1 is switched to the second state (during release operation determination). When the direct operation of the teaching device 1 is in the second state, and the release operation determination unit 106 determines that the operator has performed the release operation, the switching unit 108 directly switches the direct instruction device 1 to the third state. (During return determination). The switching unit 108 switches the direct teaching device 1 to the first state when the return determining unit 107 determines that the return condition is satisfied in the case where the direct teaching device 1 is in the third state. In addition, the switching unit 108 enables the processing by the constraint control calculation unit 110 when the direct teaching device 1 is in the second state, and sets the constraint control when the direct teaching device 1 is in any other state. The processing by the arithmetic unit 110 is invalidated.

In addition, when there are a plurality of constraint targets, the switching unit 108 may switch the operation (target constraint target) by the target value calculation unit 109 according to the determination result by the proximity determination unit 105.

The target value calculation unit 109 calculates a target value of the constraint control in the constrained direct teaching based on the calculation result by the constraint target and the position and orientation calculation unit 104. The target value of the constraint control means at least one of the target position of the arm 2 and the target posture of the arm 2.

The constraint control calculation unit 110 calculates the movement (restriction control command value) of the arm 2 that moves to the target value calculated by the target value calculation unit 109 based on the calculation result by the position and orientation calculation unit 104.

The combining unit 111 combines the calculation result (the driven control command value) by the driven control calculation unit 103 and the calculation result (the restraint control command value) by the constraint control calculation unit 110 into one command value.

The drive control unit 112 drives the arm 2 based on the result of the synthesis by the synthesis unit 111.

In the direct teaching device 1 according to the first embodiment, switching between normal direct teaching and restricted direct teaching can be executed by operating the arm 2 by an operator. This switching method is realized by the following three mechanisms.

The first is a mechanism in which the direct teaching device 1 automatically switches from normal direct teaching to restricted direct teaching when the position and orientation of the arm 2 approaches the constraint target. For example, as shown in FIGS. 2A and 2B, a case is considered where the direct teaching device 1 restricts the position of the arm 2 to a predetermined plane 506 (restriction surface 503). In this case, if the distance from the plane 506 to the position of the arm 2 becomes less than or equal to ± 50 [mm], for example, in the state where the direct teaching device 1 is performing the normal direct teaching (first state), Switching to constrained direct teaching is performed; otherwise, normal direct teaching is maintained. 3A and 3B, the case where the direct teaching device 1 restrains the posture of the arm 2 in the downward direction 507 (restriction direction 505) is considered. In this case, in a state where the direct teaching device 1 is performing a normal direct teaching, when the posture of the arm 2 becomes, for example, ± 0.2 [rad] or less from the downward direction 507, the direct teaching device 1 switches to the restricted direct teaching. Switch, otherwise leave normal direct teaching. According to the first mechanism, the direct teaching device 1 can realize switching from normal direct teaching to restricted direct teaching only by the operation of the arm 2 by the operator.

The second is a mechanism in which the direct teaching device 1 releases the restricted direct teaching by detecting the release operation by the operator. For example, when the direct teaching device 1 detects that the operator has applied a force of 5.0 [N] or more to the arm 2 in the state of performing the restricted direct teaching (second state), When it is determined that the release operation has been performed, the restricted direct teaching is stopped. When a force of 5.0 [N] or more is not detected, the restricted direct teaching is continued. Alternatively, when the direct teaching device 1 detects that the operator has applied clockwise a rotational torque of 1.0 [Nm] or more to the arm 2 in the state where the direct teaching with constraint is being performed, the release operation is performed. The direct teaching with restriction is stopped when it is determined that there has been detected, and when the clockwise rotation torque of 1.0 [Nm] or more is not detected, the direct teaching with restriction is continued.

When the direct teaching device 1 determines that the release operation has been performed, the direct teaching device 1 does not return to the initial state (the first state), but shifts to the third state in which the return determination is being performed. The direct teaching device 1 executes normal direct teaching during the return determination, but does not execute switching to the restricted direct teaching even when the position and orientation of the arm 2 approaches the constraint target. This is because, in the direct teaching device 1, the position and orientation of the arm 2 coincides with the constraint target immediately after the restrained direct teaching is released, and if the state is switched to the restrained direct teaching in this state, the direct teaching is performed. Cannot be switched. Therefore, when detecting the release operation by the operator, the direct teaching device 1 temporarily stops switching to the restricted direct teaching. This is why the third state is needed.
Then, during the return determination, the operator can move the arm 2 away from the constraint target as shown in FIGS. 2C and 3C. With this second mechanism, the direct teaching device 1 can release the restricted direct teaching.

Third, when the direct teaching device 1 satisfies a predetermined return condition during the return determination, the direct teaching device 1 returns to a state (first state) where the direct teaching device 1 can be switched to the restricted direct teaching. The second mechanism allows the direct teaching device 1 to release the constrained direct teaching, but cannot perform the constrained direct teaching again only by this. Therefore, for example, the direct teaching device 1 returns to the first state when the position and orientation of the arm 2 is sufficiently far from the constraint target. Alternatively, for example, the direct teaching device 1 returns to the first state when a predetermined time or more has passed since the restraint of the arm 2 was released. By this third mechanism, the direct teaching device 1 can not only release the restricted direct teaching but also switch to the restricted direct teaching again as shown in FIGS. 2D and 3D.

By combining the above three mechanisms, the direct teaching device 1 according to the first embodiment can switch between the normal direct teaching and the restricted direct teaching while the operator is operating the arm 2. Becomes Hereinafter, an operation example of the direct teaching device 1 shown in FIG. 1 will be described with reference to FIG.

In the operation example of the direct teaching device 1 shown in FIG. 1, as shown in FIG. 4, first, the position and orientation measurement unit 101 measures parameters relating to the position and orientation of the arm 2 (step ST401).

Further, external force detecting section 102 detects an external force applied to arm 2 by the operator (step ST402). The external force detected by the external force detecting unit 102 may include not only the external force applied to the arm 2 by the operator but also a component due to gravity, depending on the structure of the sensor. Therefore, in such a case, the external force detection unit 102 may calculate only the external force component by calculating the component due to gravity and subtracting the component due to gravity from the detected external force. This is a known technique called gravity compensation, which is disclosed in, for example, Patent Document 5 and the like.
JP-A-01-066715

Next, the driven control calculation unit 103 calculates the movement (the driven control command value) of the arm 2 according to the external force detected by the external force detection unit 102 based on the measurement result by the position and orientation measurement unit 101 (step ST403). At this time, the driven control calculation unit 103 first determines how the operator intends to move the arm 2 from the direction and magnitude of the external force detected by the external force detection unit 102. Then, the driven control calculation unit 103 determines the movement amount of the arm 2 (difference such as the joint angle θ) or the joint angular velocity θ (dot) based on the determination result and the position and orientation of the arm 2 measured by the position and orientation measurement unit 101. ) Is calculated. Note that the driven control calculation method by the driven control calculation unit 103 is disclosed in, for example, Patent Document 2, and various methods have been developed.

Next, position / posture calculation section 104 calculates the position / posture of arm 2 based on the calculation result by driven control calculation section 103 (step ST404).
Here, when the direct teaching device 1 restricts the position (that is, when the direct teaching device 1 restricts the position of the arm 2 to the predetermined restricting surface 503 or the restricting shaft 504), the position and orientation calculation unit 104 includes at least the arm. It is necessary to calculate the position of No. 2.
Further, when the direct teaching device 1 performs the posture constraint (that is, when the direct teaching device 1 restricts the posture of the arm 2 in the predetermined constraint direction 505), the position / posture calculation unit 104 calculates at least the posture of the arm 2. There is a need to.
Further, when the direct teaching device 1 performs the position constraint and the posture constraint (that is, when the direct teaching device 1 constrains the position and the posture of the arm 2 to the predetermined constraint surface 503 or the constraint shaft 504 and the predetermined constraint direction 505). In other words, the position and orientation calculation unit 104 needs to calculate the position and orientation of the arm 2.

It is most desirable that the position and orientation calculation unit 104 performs the calculation based on the calculation result (the latest driven control command value) by the driven control calculation unit 103. This is because, when the direct teaching device 1 performs the direct teaching with the constraint, components out of the constraint surface 503 and the like out of the movement amount given to the arm 2 by the operator can be simultaneously canceled, and the constraint control is most effectively performed. Because it works.
Further, the position and orientation calculation unit 104 can perform the above calculation based on the measurement result by the position and orientation measurement unit 101 without using the calculation result by the driven control calculation unit 103. On the other hand, in this case, since the restraint control starts after it is measured that it has deviated from the restraint surface 503 or the like, the effectiveness of the restraint control deteriorates.
Further, the position and orientation calculation unit 104 may perform the calculation based on both the calculation result by the driven control calculation unit 103 and the measurement result by the position and orientation measurement unit 101.

When only the joint angle θ is obtained by the position and orientation measurement unit 101 and the driven control calculation unit 103 and the position and orientation of the arm 2 cannot be directly obtained, the position and orientation calculation unit 104 determines the position of the arm 2 by forward kinematics calculation. The posture can be calculated.

Next, the proximity determination unit 105 determines whether the position and orientation of the arm 2 is approaching the constraint target based on the calculation result by the position and orientation calculation unit 104 (step ST405).

For example, if the purpose of the constraint control is to fix the Z-axis coordinate of the tip of the end effector 201 to a position of 0.1 [m], and if the threshold value of the approach determination is 0.05 [m], I do. In this case, the proximity determination unit 105 restricts the position of the arm 2 if the Z-axis coordinate of the tip of the end effector 201 calculated by the position and orientation calculation unit 104 is within the range of 0.05 to 0.15 [m]. It is determined that the target is approaching the target, otherwise, it is determined that the position of the arm 2 is not approaching the constraint target.

。 It is also assumed that the purpose of the constraint control is to direct the direction 502 (Z axis) of the end effector 201 directly downward, and the threshold value for the approach determination is 0.1 [rad]. In this case, as shown in FIG. 5, the proximity determination unit 105 determines that the angle Θ formed by the upward direction (reference direction) 508 and the direction 502 of the end effector 201 is equal to or more than (π−0.1) [rad] (ie, If the angle between the direct downward direction 507 and the direction 502 of the end effector 201 is less than 0.1 [rad], it is determined that the posture of the arm 2 is approaching the constraint target. Is determined not to be approaching the restraint target.

When there are a plurality of constraint targets, such as when there are a plurality of constraint surfaces 503, the proximity determination unit 105 performs the above-described determination for each constraint target, and determines the position of the arm 2 even in one of the constraint targets. When it is determined that the posture is approaching, it is determined that the position and posture of the arm 2 is approaching the constraint target. At this time, the proximity determination unit 105 further evaluates the distance between the arm 2 and each constraint target, and notifies the switching unit 108 of information indicating the constraint target closest to the arm 2 along with the result of the above determination. Good.

解除 The release operation determination unit 106 determines whether the operator has performed a release operation (step ST406).

最も The most preferable determination method by the release operation determination unit 106 is a determination method using external force. An example of the simplest determination method using the external force by the release operation determination unit 106 is to determine that the operator has performed the release operation when the external force is equal to or more than a predetermined force (for example, 5.0 [N]). This is a determination method based on the magnitude of the external force. The release operation determination unit 106 also considers time in addition to the magnitude of the external force, and determines that the operator has performed the release operation when the external force is equal to or more than a predetermined force and continues for a predetermined time or more. You may.

FIG. 6 shows an example of a more complicated determination method using an external force by the release operation determination unit 106.
As illustrated in FIG. 6A, the release operation determination unit 106 may determine that the operator has performed the release operation when the external force is away from the constraint surface 503 and is equal to or greater than a predetermined force. This determination method can be realized by detecting the magnitude of a component of the external force orthogonal to the constraint surface 503 in the external force detection unit 102. As described above, as the determination method by the release operation determination unit 106, a determination method that considers not only the magnitude of the external force but also the direction of the external force can be implemented.

As shown in FIG. 6B, the release operation determination unit 106 determines that the operator has performed the release operation when the external force is a rotational force about the axis of the end effector 201 and is equal to or greater than a predetermined force. May be. This determination method can be realized by the external force detection unit 102 detecting a rotational force about the axis of the end effector 201.

Further, as shown in FIG. 6C, the release operation determination unit 106 determines that the operator has performed the release operation when the external force is a force applied alternately in the left-right direction and is equal to or greater than a predetermined force. Is also good. In this determination method, since the release operation determination unit 106 considers not only the magnitude and direction of the external force but also a time-series pattern, there is a possibility that the release operation is accidentally determined to be a release operation by an unintended operation. Probability) can be expected to decrease. Various methods such as known pattern recognition, machine learning, and neural network can be used as a method for the release operation determination unit 106 to determine whether the pattern is intended for the release operation.

The release operation determination unit 106 may determine whether the operator has performed the release operation using the position and orientation of the arm 2 without being limited to the determination based on the external force. However, since the direct teaching device 1 restrains the position and orientation of the arm 2 during the direct teaching with restraint, the arm 2 can be moved only within the restrained range. Therefore, the determination method using the position and orientation of the arm 2 by the release operation determination unit 106 is limited to some extent. FIG. 7 illustrates an example of a determination method using the position and orientation of the arm 2 by the release operation determination unit 106.

As shown in FIGS. 7A to 7C, it is assumed that the direct teaching device 1 restrains the arm 2 on a plane 506 (restriction surface 503) having a constant Z coordinate.
In this case, as shown in FIG. 7A, when the X coordinate or the Y coordinate of the arm 2 is outside the plane 506 (for example, outside the range of ± 0.5 [m]), the release operation determination unit 106 determines May have determined that the release operation has been performed. Further, the release operation determination unit 106 may determine that the operator has performed the release operation when the distance from the workpiece to the tip of the end effector 201 is larger than, for example, 0.2 [m].

Also, as shown in FIG. 7B, the release operation determination unit 106 determines that the operator has performed the release operation when the arm 2 has moved to a predetermined shape (a triangular shape in FIG. 7B) on the constraint surface 503. For example, the determination may be made based on a time-series pattern of the position of the end effector 201. Various methods such as known pattern recognition, machine learning, and neural network can be used as a method for the release operation determination unit 106 to determine whether the pattern is intended for the release operation.

Also, as shown in FIG. 7C, the release operation determination unit 106 determines the posture of the end effector 201 in a time-series manner, such as determining that the operator has performed the release operation when the arm 2 makes one rotation on the constraint surface 503. The determination may be made based on the pattern. However, this determination method may not be implemented in a state where the direct teaching device 1 is performing the direct teaching with the constraint. Various methods such as known pattern recognition, machine learning, and neural network can be used as a method for the release operation determination unit 106 to determine whether the pattern is intended for the release operation.

Also, as shown in FIGS. 7D and 7E, a case is considered in which the direct teaching device 1 restrains the arm 2 on an axis parallel to the Z axis (a restraining axis 504).
In this case, as shown in FIG. 7D, when the Z coordinate of the end of the end effector 201 is larger than a predetermined position (for example, 0.3 [m]), the release operation determination unit 106 performs the release operation by the operator. May be determined. The release operation determination unit 106 may determine that the operator has performed the release operation when the distance from the workpiece to the tip of the end effector 201 is large (that is, when the arm 2 has moved away from the workpiece).

Further, as shown in FIG. 7E, when the arm 2 reciprocates up and down on the constraint shaft 504 in a cycle shorter than a predetermined cycle, the release operation determination unit 106 determines that the operator has performed the release operation. Is also good.

Note that the release operation determination unit 106 can use parameters related to the position and orientation of the arm 2 measured by the position and orientation measurement unit 101, instead of the position and orientation of the arm 2 calculated by the position and orientation calculation unit 104. .

Further, when the implementation environment of the direct teaching device 1 is an environment in which the voice of the operator can be acquired, the direct teaching device 1 acquires the voice of the operator using a microphone or the like, and the release operation determination unit 106 performs the voice recognition. It may be determined whether the operator has performed the release operation. In this determination method, the operator can perform the release operation without releasing the hand from the arm 2.

Further, when the environment in which the direct teaching device 1 is implemented is an environment in which an image of the operator can be acquired as a moving image, the direct teaching device 1 acquires an image of the lips of the operator using a camera or the like, and the release operation determination unit 106 May determine whether the operator has performed the release operation by recognizing the words spoken by the operator from the image. Also in this method, the operator can perform the release operation without releasing the hand from the arm 2.

{Circle around (2)} Return determination section 107 determines whether or not a predetermined return condition for returning to a state where switching to restricted direct teaching is possible is satisfied (step ST407). As described above, if the direct teaching device 1 shifts to the first state immediately after the release operation is performed by the operator, even if the release operation is performed, the direct teaching device 1 immediately returns to the restricted direct teaching. We need to avoid that. The return determination unit 107 exists to solve such a problem.

One of the most preferable determination methods by the return determination unit 107 is a method of determining that the return condition is satisfied after waiting for the proximity determination unit 105 to stop determining that the position and orientation of the arm 2 is approaching the constraint target. For example, in FIG. 2, the return determination unit 107 determines that when the arm 2 is separated from the restraining surface 503 by ± 50 [mm] or more, the angle between the downward direction 507 and the posture of the arm 2 in FIG. When the distance is 0.2 [rad] or more, it may be determined that the return condition is satisfied. Thereby, the direct teaching device 1 can suppress that the restricted direct teaching becomes valid again against the operator's intention after the release operation is performed by the operator.

Note that the return determination unit 107 may perform the determination using a threshold value which has a margin more than the threshold value for the proximity determination, instead of using the threshold value for the proximity determination as it is. It is considered that the operation is performed. For example, in FIG. 2, the return determination unit 107 determines that when the arm 2 is separated from the constraint surface 503 by ± 55 [mm] or more, in FIG. When the distance is 0.22 [rad] or more, it may be determined that the return condition is satisfied.

The return determination unit 107 may determine that the return condition has been satisfied when a predetermined time has elapsed after the operator performed the release operation. For example, the return determination unit 107 may determine that the return condition has been satisfied when 10 [seconds] have elapsed after the operator performed the release operation. In this case, when the operator wants to return to the normal direct teaching, the operator only has to operate the arm 2 to an area where the proximity determination condition is not satisfied within 10 [seconds]. When the operator wants to resume the restricted direct teaching, the operator may leave the arm 2 as it is and wait for the time to elapse.

The return determination unit 107 may determine that the return condition is satisfied when a certain period of time has elapsed after the position and orientation of the arm 2 has moved away from the constraint target by combining the above two methods.

The operator can perform the return operation without releasing the hand from the arm 2 regardless of whether the return determination unit 107 uses the distance from the constraint target or the time from the release operation.

Next, switching section 108 switches the operation of constraint control calculation section 110 according to the determination result by proximity determination section 105, the determination result by release operation determination section 106, and the determination result by return determination section 107 (step ST408).

The switching unit 108 manages the direct teaching device 1 in three states (first to third states) as shown in FIG. Then, the switching unit 108 directly switches the state of the teaching device 1 based on the determination results of the proximity determination unit 105, the release operation determination unit 106, and the return determination unit 107. The first state is a state in which the proximity determination is being performed in which only the determination by the proximity determination unit 105 is valid, and the direct teaching device 1 starts operating from this state. The second state is a state where the release operation is being determined in which only the determination by the release operation determination unit 106 is valid. The third state is a state in which a return determination is being performed in which only the determination by the return determination unit 107 is valid. The direct teaching device 1 is in one of these three states.

When the direct teaching device 1 is in the first state, the switching unit 108 refers only to the determination result by the proximity determining unit 105. Then, the switching unit 108 directly switches the teaching device 1 to the second state when the proximity determination unit 105 determines that the position and orientation of the arm 2 has approached the constraint target.

When the direct teaching device 1 is in the second state, the switching unit 108 refers only to the determination result by the release operation determination unit 106. Then, when the release operation determination unit 106 determines that the operator has performed the release operation, the switching unit 108 directly switches the teaching device 1 to the third state.

When the direct teaching device 1 is in the third state, the switching unit 108 refers only to the determination result by the return determination unit 107. Then, the switching unit 108 directly switches the teaching device 1 to the first state when the return determination unit 107 determines that the return condition is satisfied.

Then, the switching unit 108 switches the processing by the constraint control calculation unit 110 to valid or invalid according to the state of the direct teaching device 1. That is, when the direct teaching device 1 is in the second state, the switching unit 108 enables the processing by the constraint control calculation unit 110. When the direct teaching device 1 is in the first state or the third state, the switching unit 108 stops the calculation by the constraint control calculation unit 110 or prevents the calculation result from being reflected in the control of the arm 2. Then, the processing by the constraint control calculation unit 110 is invalidated.
Also, as shown in FIG. 8, when the direct teaching device 1 is in the second state, the switching unit 108 causes the target value calculating unit 109 to calculate the target value, and the direct teaching device 1 Alternatively, in the case of the third state, switching may be performed so that the calculation of the target value by the target value calculation unit 109 is stopped.

As described above, the direct teaching device 1 automatically starts the restraint control when the position and orientation of the arm 2 is brought closer to the restraint target by the operator. Further, the direct teaching device 1 automatically terminates the constraint control when the operator performs the operation of releasing the constrained direct teaching. Further, when the operator again brings the position and orientation of the arm 2 closer to the constraint target after satisfying the return condition, the direct teaching device 1 automatically starts constraint control. Accordingly, the direct teaching device 1 can switch on / off the constraint control while the arm 2 is held by the operator.
When there are a plurality of constraint targets and information indicating a constraint target closest to the arm 2 is notified from the proximity determination unit 105, the switching unit 108 outputs the information to a target value calculation unit 109 described later. The operation of the target value calculation unit 109 may be switched.

The proximity determining unit 105, the release operation determining unit 106, and the return determining unit 107 may stop the processing when the switching unit 108 does not need the determination result. For example, when the direct teaching device 1 is in the second state, the switching unit 108 does not use the determination result by the proximity determination unit 105, and therefore, there is no problem even if the proximity determination unit 105 stops processing. When the direct teaching device 1 is in the first state or the third state, the switching unit 108 does not use the determination result of the release operation determination unit 106, and the release operation determination unit 106 stops the process. No problem. Further, when the direct teaching device 1 is in the first state or the second state, the switching unit 108 does not use the determination result by the return determination unit 107. There is no. When the return determination unit 107 uses the determination result of the proximity determination unit 105 for the return determination, the proximity determination unit 105 needs to perform the process even when the direct teaching device 1 is in the third state. There is.

Next, the target value calculation unit 109 calculates a target value of the constraint control based on the calculation result by the constraint target and the position and orientation calculation unit 104 (step ST409).

For example, the purpose of the constraint control is to fix the Z-axis coordinate of the tip of the end effector 201 to a position of 0.1 [m], and the current value of the tip of the end effector 201 calculated by the position and orientation calculation unit 104 is _{_{_{[X P, Y P, Z}}} P] is to be. In this case, the target value calculation unit 109, a target value of the constraint control _{_{[X P, Y P, 0.1}} ] and.

Further, it is assumed that the purpose of the constraint control is to set the Z axis of the direction 502 of the end effector 201 to the directly downward direction 507 and align the X axis with the X axis of the robot coordinate system as shown in the lower part of FIG. . In this case, a target attitude, which is a target value of the constraint control, represented by a rotation matrix expression is represented by the following equation (1). The expression of the posture by the rotation matrix is disclosed, for example, in Non-Patent Document 1 and the like, and the details thereof will be omitted.

John J. Craig (translated by Miura and Shimoyama): Robotics, Kyoritsu Publishing (1991)

Note that the target value calculation unit 109 may calculate only one target value as the target value of the constraint control, or may calculate a plurality of target values and select one target value from the calculated target values. . For example, if the purpose of the constraint control is to fix the Z-axis coordinate of the tip of the end effector 201 to the nearest position in steps of 0.1 [m], the target value calculation unit 109 sets the target value of the constraint control as [ _{_{X P, Y P, 0.1n]}} (n is an integer) selects the closest to the current position in the.

10, the Z-axis of the direction 502 of the end effector 201 is set to a directly downward direction 507, and the Z-axis is rotated at π / 2 intervals (90-degree intervals) as the rotation axis. In the case of fixing the posture, the target value calculation unit 109 selects the target value of the constraint control that is closest to the current posture. At this time, a target attitude, which is a target value of the constraint control, expressed in a rotation matrix expression is as shown in the following equation (2).

In Equation (2), as shown in FIG. 10, the target posture changes in four ways depending on n, but the target value calculation unit 109 selects one of them that is closest to the current posture. The target value calculation unit 109 can determine whether or not the current attitude is close to the current attitude based on the magnitude of the rotation angle when rotating from the current attitude to each of the target attitudes. When the proximity determination unit 105 has notified information indicating the constraint target closest to the arm 2, the target value calculation unit 109 may select a target value for constraint control from the information.

Next, the constraint control calculation unit 110 calculates the movement (restriction control command value) of the arm 2 that moves to the target value calculated by the target value calculation unit 109 based on the calculation result by the position / posture calculation unit 104 (step). ST410). At this time, the constraint control calculation unit 110 calculates the amount of movement of the arm 2 or the movement per unit time required to move the arm 2 from the current position and orientation to the target value based on the target value calculated by the target value calculation unit 109. A joint angular velocity θ (dot), which is an amount, is calculated. Further, the constraint control calculation unit 110 may calculate the joint angle change amount Δθ per control cycle instead of the joint angular velocity θ (dot).

Next, the combining unit 111 combines the driven control command value output from the driven control calculation unit 103 and the constraint control command value output from the constraint control calculation unit 110 into one command value (step ST411).

For example, if the driven control calculation unit 103 outputs the joint angle theta _D as a driven control command value, restraining control arithmetic unit 110 outputs the joint angle variation [Delta] [theta] _C as restraining control command value, the combining unit 111, θ _D + Δθ _C is output as a joint angle command value.
Further, for example, and it outputs the position coordinates follower control calculation unit 103 as a driven control command value _{_{_{[x D, y D, z}}} D] and attitude _{T D} (the rotation matrix representation), restraining control arithmetic unit 110 is restraining control command value When the position change amount [Δx _C , Δy _C , Δz _C ] and the posture change amount ΔT _C (rotation matrix expression) are output as the expression, the combining unit 111 outputs [x _D + Δx _C , y _D + Δy _C , z _D]. + Δz _C ] is used as a position command value, and ΔT _C T _D (represented by a rotation matrix) is output as a posture command value (composition of a posture represented by a rotation matrix is a product of matrices).
Further, for example, when the driven control calculation unit 103 outputs the torque τ _D as the driven control command value and the constraint control calculation unit 110 outputs the torque change amount Δτ _C as the constraint control command value, the combining unit 111 τ _D + Δτ _C is output as a torque command value.

Next, drive control section 112 drives arm 2 based on the result of synthesis by synthesis section 111 (step ST412). The drive of the arm 2 by the drive control unit 112 is a normal control of the arm 2, and thus the details thereof are omitted.

As described above, according to the first embodiment, the direct teaching device 1 includes the external force detecting unit 102 that detects the external force applied to the arm 2 of the robot, and the arm that follows the external force detected by the external force detecting unit 102. Based on the calculation results of the driven control calculation unit 103 that calculates the movement of the second position, the position and orientation calculation unit 104 that calculates the position and orientation of at least one of the position and the orientation of the arm 2, and the position and orientation calculation unit 104 A proximity determination unit 105 that determines whether the position and posture of the arm 2 is approaching a restriction target to which the position and posture of the arm 2 are restricted; and whether an operator has performed a release operation for releasing the restricted direct teaching. , A return determination unit 107 for determining whether or not a return condition to a state capable of switching to the restricted direct teaching is satisfied, a constraint target and a position A target value calculation unit 109 that calculates a target value of the constraint control in the constrained direct teaching based on the calculation result by the posture calculation unit 104, and a calculation by the target value calculation unit 109 based on the calculation result by the position and posture calculation unit 104 A constraint control calculation unit 110 for calculating the movement of the arm 2 moving to the set target value, and a self-control unit when the proximity determination unit 105 determines that the own device is approaching the constraint target in the first state. The device is switched to the second state, and when the release operation determination unit 106 determines that the operator has performed the release operation when the device is in the second state, the device is switched to the third state. When the return determination unit 107 determines that the return condition is satisfied when the own device is in the third state, the own device is switched to the first state, and when the own device is in the second state, A switching unit 108 that enables processing by the control arithmetic unit 110 and disables processing by the constraint control arithmetic unit 110 when the own machine is in the first state or the third state; and a calculation result by the driven control arithmetic unit 103. A combining unit 111 for combining the calculation result of the constraint control calculation unit 110 with the driving control unit 112 for driving the arm 2 based on the combining result of the combining unit 111. Thereby, the direct teaching device 1 according to the first embodiment can switch between normal direct teaching and restricted direct teaching while the operator is operating the arm 2.

Embodiment 2 FIG.
In the second embodiment, a case is described in which the arm 2 is a vertical articulated robot arm, and the restraining method by the direct teaching device 1 is limited to only the posture restraint. The configuration example of the direct teaching device 1 according to the second embodiment is the same as the configuration example of the direct teaching device 1 according to the first embodiment, and the following description will be given using the configuration example shown in FIG.

Note that the position and orientation calculation unit 104 according to the second embodiment calculates the orientation of the arm 2.
In addition, the proximity determination unit 105 according to Embodiment 2 determines whether the posture of the arm 2 is close to the constraint direction 505 based on the calculation result by the position and posture calculation unit 104.

Next, an operation example of the direct teaching device 1 according to the second embodiment will be described with reference to FIG.
Hereinafter, it is assumed that the direct teaching device 1 performs a posture constraint such that the direction 502 (Z-axis) of the end effector 201 is restricted in the downward direction 507 or the lateral direction 509. Further, it represents a unit vector of each axis of the robot coordinate system _e _X, e Y, in _{e Z.} The arm 2 has six axes. The switching unit 108 switches the operations of the target value calculation unit 109 and the constraint control calculation unit 110. Further, the driven control calculation unit 103 calculates each joint angle θ of the arm 2 as a driven control command value, and the constraint control calculation unit 110 calculates each joint angular velocity θ (dot) of the arm 2 as a constraint control command value. I do.

In the operation example of the direct teaching device 1 according to the second embodiment, as illustrated in FIG. 11, first, the position and orientation measurement unit 101 measures each joint angle θ of the arm 2 (step ST1101). For example, the position and orientation measurement unit 101 measures the joint angle θ for each joint of the arm 2 using an encoder or the like attached to the motor of the arm 2.

Next, the external force detecting unit 102 detects the external force applied to the arm 2 by the operator (step ST1102). The processing by the external force detecting unit 102 is the same as in the first embodiment. Note that the external force detection unit 102 performs gravity compensation.

Next, driven control calculation section 103 calculates each joint angle θ (driven control command value) of arm 2 according to the external force detected by external force detection section 102 based on the measurement result by position and orientation measurement section 101 (step ST1103). ). An example of the operation of the driven control calculation unit 103 will be described with reference to FIG.

The driven control calculation unit 103 can calculate each joint angle θ of the arm 2 from the torque τ detected by the external force detection unit 102 by, for example, the method shown in FIG. 12A.
In Figure 12A, the driven control calculation unit 103 first by the torque τ detected by the external-force detecting unit 102 multiplies the gain K _T, calculates joint angular velocity θ of the target for each joint of the arm 2 (dot) . Note that the gain K _T may be the same or different for each joint of the arm 2. Note that, as shown in FIG. 12A, the driven control calculation unit 103 may limit the speed of the driven control by applying a limiter to the target joint angular velocity θ (dot).
Next, the driven control calculation unit 103 calculates a joint angle change amount Δθ per control cycle for each joint of the arm 2 by multiplying the target joint angular velocity θ (dot) by the control cycle T _S.
Next, the driven control calculation unit 103 calculates the joint angle θ (k + 1) at the next time k + 1 by adding the joint angle change amount Δθ to the joint angle θ (k) at the current time k for each joint of the arm 2. I do.

Further, the driven control calculation unit 103 can calculate each joint angle θ of the arm 2 from the force F detected by the external force detection unit 102, for example, by a method illustrated in FIG. 12B.
In the example of FIG. 12B, the driven control calculation unit 103 first, by multiplying the gain K _F to the force F detected by the external-force detecting unit 102 calculates the moving speed for each position and orientation of the arm 2. The gain K _F can be different may be the same for each component of the position and orientation of the arm 2.
Next, the driven control calculation unit 103 calculates a target joint angular velocity θ (dot) for each joint of the arm 2 by multiplying the moving velocity by the inverse matrix of the Jacobi matrix J. When there is no inverse matrix of the Jacobi matrix J, the driven control calculation unit 103 multiplies the pseudo-inverse matrix. Note that, as shown in FIG. 12B, the driven control calculation unit 103 may apply a limiter to the target joint angular velocity θ (dot) to limit the speed of the driven control.
Next, the driven control calculation unit 103 calculates a joint angle change amount Δθ per control cycle for each joint of the arm 2 by multiplying the target joint angular velocity θ (dot) by the control cycle T _S.
Next, the driven control calculation unit 103 calculates the joint angle θ (k + 1) at the next time k + 1 by adding the joint angle change amount Δθ to the joint angle θ (k) at the current time k for each joint of the arm 2. I do.

Next, position / posture calculation section 104 calculates the posture of arm 2 based on the calculation result by driven control calculation section 103 (step ST1104). The position and orientation calculation unit 104 can execute the above calculation using forward kinematics. Hereinafter, the posture of the arm 2 calculated by the position / posture calculation unit 104 is represented by T using a rotation matrix. Also, T obtained by decomposing T into column vectors and elements is represented as the following equation (3).

Next, the proximity determination unit 105 determines whether the posture of the arm 2 is approaching the constraint direction 505 based on the calculation result by the position / posture calculation unit 104 (step ST1105). Here, the restraining direction 505 is the downward direction 507 or the lateral direction 509. Also, it is assumed that the threshold value for approach determination is 0.1 [rad]. An operation example of the proximity determination unit 105 will be described with reference to FIG.

In the operation example of the proximity determining unit 105, as illustrated in FIG. 13, the proximity determining unit 105 first calculates an angle す formed by a direction directly above (reference direction) 508 and a direction 502 (Z axis) of the end effector 201. (Step ST1301). That is, the proximity determination unit 105, as in the following equation (4), calculates an angle Θ from the inner product of the _{e Z} and _{t Z.}

Then, proximity determination section 105 determines whether angle Θ is in the range from (π / 2-0.1) [rad] to (π / 2 + 0.1) [rad] (step ST1302).
In this step ST1302, the proximity determining unit 105 determines that the angle Θ is in the range of (π / 2-0.1) [rad] to (π / 2 + 0.1) [rad] (for example, FIG. 14A). ), It is determined that the posture of the arm 2 is approaching the horizontal direction 509 (step ST1303).

On the other hand, in step ST1302, when the proximity determining unit 105 determines that the angle Θ is not in the range from (π / 2-0.1) [rad] to (π / 2 + 0.1) [rad], It is determined whether angle Θ is equal to or more than (π−0.1) [rad] (step ST1304).
In this step ST1304, when the proximity determining unit 105 determines that the angle Θ is equal to or more than (π−0.1) [rad] (for example, in the case of FIG. 14B), the posture of the arm 2 is set in the downward direction 507. It is determined that it is approaching (step ST1305).

On the other hand, in step ST1304, when the proximity determining unit 105 determines that the angle Θ is not equal to or more than (π−0.1) [rad] (for example, in the case of FIG. 14C), It is determined that it is not approaching 505 (step ST1306).

解除 The release operation determination unit 106 determines whether the operator has performed a release operation (step ST1106). Here, when performing the determination using the external force, the release operation determination unit 106 can apply the same method as in the first embodiment. On the other hand, when performing the determination using the position and orientation of the arm 2, the release operation determination unit 106 directly applies the teaching device 1 in the second embodiment to restrict the posture of the arm 2. Is limited to a determination method compatible with the posture constraint. For example, as shown in FIG. 7C, the release operation determination unit 106 according to Embodiment 2 cannot apply the determination method using the pattern of the posture of the arm 2.

{Circle around (2)} Return determination section 107 determines whether or not a return condition to a state where switching to restricted direct teaching can be satisfied is satisfied (step ST1107). In Embodiment 2, since the teaching device 1 directly restrains the posture of the arm 2, when performing the determination based on the position and posture of the arm 2, the return determination unit 107 determines based on the value of the angle Θ. It is possible. For example, the return determination unit 107 determines that the angle Θ is not in the range of (π / 2-0.1) [rad] to (π / 2 + 0.1) [rad], and that the angle Θ is (π−0.1). When it is less than [rad], it can be determined that the force of the arm 2 is away from the restrained posture and the return condition is satisfied.

In the above description, the return determination unit 107 sets (π / 2-0.1) [rad], (π / 2 + 0.1) [rad], and (π-0.1) [rad] as threshold values for return determination. rad] was used. However, the present invention is not limited to this, and the return determination unit 107 sets the threshold values of the return determination as (π / 2-0.12) [rad], (π / 2 + 0.12) [rad], and (π-0. 12) A value slightly wider than the above threshold such as [rad] may be used.

On the other hand, the return determination unit 107 must not use a value smaller than the threshold value used by the proximity determination unit 105 as the threshold value for the return determination. This is because when the return determination unit 107 uses the above narrow value, the proximity determination unit 105 determines that the posture of the arm 2 is approaching the constraint direction 505 immediately after the return determination unit 107 determines that the return condition is satisfied. This is because the direct teaching device 1 cannot leave the restricted direct teaching.

In the case where the return determination unit 107 uses only the time without using the position and orientation of the arm 2 for the determination, the same method as in the first embodiment can be applied.

Next, the switching unit 108 switches the operation of the target value calculation unit 109 and switches the operation of the constraint control calculation unit 110 (step ST1108). That is, the switching unit 108 switches the operation of the target value calculation unit 109 according to the determination result by the proximity determination unit 105. The switching unit 108 switches the operation of the constraint control calculation unit 110 according to the determination result by the proximity determination unit 105, the determination result by the release operation determination unit 106, and the determination result by the return determination unit 107.

At this time, if the proximity determination unit 105 determines that the posture of the arm 2 is approaching the lateral direction 509, the switching unit 108 sets the target posture in the lateral direction 509 as a target value to the target value calculation unit 109. Let it be calculated.
When the proximity determining unit 105 determines that the posture of the arm 2 is approaching the downward direction 507, the switching unit 108 calculates the target posture in the downward direction 507 as a target value by the target value calculating unit 109. Let it.
When the proximity determining unit 105 determines that the posture of the arm 2 is not approaching any of the restraining directions 505, the switching unit 108 does not require the calculation by the target value calculating unit 109.
The switching of the operation of the constraint control calculation unit 110 by the switching unit 108 is the same as in the first embodiment.

Next, target value calculation section 109 calculates a target attitude based on the calculation results by constraint target and position and attitude calculation section 104 (step ST1109). For example, the target value calculation unit 109 is a sideways direction 509 or below direction 507, and calculates the target posture reachable smallest rotation angle theta _R from the current position of the arm 2. A method of calculating the target posture by the target value calculation unit 109 will be described below.

First, the target value calculation unit 109, for each target attitude of interest, calculates a target angle theta _* against e _Z. Here, when the target posture is the horizontal direction 509, the target angle Θ _* is π / 2, and when the target posture is the downward direction 507, the target angle Θ _* is π.

Then, the target value calculation unit 109, based on the attitude of the arm 2, the rotation angle theta _R calculates the smallest rotary shaft 510. As shown in FIG. 15, the rotary shaft 510 is an axis orthogonal to the located and t _Z on the XY plane of the robot coordinate system (i.e., an axis orthogonal to the both e _Z and t _Z). Therefore, the target value calculation portion 109 obtains one r dot product and both e _Z and t _Z is a unit vector such that 0 to it and the rotating shaft 510. The target value calculation unit 109, in the case of parallel and e _Z and t _Z can not be determined uniquely r. However, this is the case where the posture of the arm 2 is pointing directly upward 508 or directly downward 507, and the target value calculation unit 109 calculates an arbitrary vector on the XY plane as the rotation axis 510.

Then, the target value calculation unit 109, based on the target angle theta _* and the rotary shaft 510 is calculated, to calculate the rotation angle theta _R as the target position. Here, when the direction 502 of the end effector 201 is at an angle theta from directly above (reference direction) 508, the magnitude of the rotation angle theta _R is a theta _* - [theta], the rotation as shown in FIG. 16 orientation (Positive or negative) must be considered. This is because the direct teaching device 1 according to the second embodiment uses the inner product when obtaining the angle Θ, and thus loses the information of the direction 502 of the end effector 201. Considering the rotation direction, rotation angle theta _R is given by the following equation (5). That is, the orientation of the rotation, if the the e _Z × t _Z and r are the same orientation sign inversion is not necessary, if the the e _Z × t _Z and r is the reverse of the orientation of the code Inversion is required.

The target value calculation unit 109, _r = representing the rotation shaft _{_{510 [r x, r y,}} r z] as long T (T denotes the transpose) and obtain the rotation angle theta _R is, to the target posture from the posture of the arm 2 Is obtained, so that the restraint target is effectively obtained. When a specific expression of the target posture is required, the target value calculation unit 109 can be obtained by applying the above-described rotation to the posture of the arm 2.

In the above, the method in which the target value calculation unit 109 calculates the target posture in which the direction 502 (Z axis) of the end effector 201 is directed to the downward direction 507 or the lateral direction 509 is described. The target value calculation unit 109 can further apply a rotation from here to a desired posture, and can use it as a target posture.
For example, the target value calculation unit 109 can also set the posture shown in FIG. 10 as the target posture by applying a rotation around the Z axis from a state in which the arm 2 faces the downward direction 507. The target value calculation unit 109, as shown in FIG. 17, an additional rotation about the Z-axis from the state where the arm 2 facing sideways direction 509, t _X or t _Y is the Z axis of the robot coordinate system parallel It is also possible to set the posture which becomes as the target posture.

Next, the constraint control calculation unit 110 calculates the movement (restriction control command value) of the arm 2 that moves to the target posture calculated by the target value calculation unit 109 based on the calculation result by the position and posture calculation unit 104 (step). ST1110). Note that since the rotation that moves the arm 2 from the current posture to the target posture has already been calculated when the target value calculation unit 109 calculates the target posture, the constraint control calculation unit 110 2 is calculated. However, since the output of the constraint control calculation unit 110 needs to be the joint angular velocity θ (dot), the constraint control calculation unit 110 calculates the rotation axis 510 r and the rotation angle Θ _R calculated by the target value calculation unit 109. It is necessary to calculate the joint angular velocity θ (dot) from the set of. An example of the calculation is shown below.

The constraint control calculation unit 110 first determines the magnitude | ω | of the angular velocity of rotation. For example, to control so as to reach the target posture 1 second unless restricted, | omega | = a theta _R. If it is not preferable that the angular velocity is too large, the constraint control calculation unit 110 may appropriately limit the angular velocity.
Next, the constraint control calculation unit 110 calculates a Jacobi matrix J at the current position and orientation of the arm 2. It is assumed that the following equation (6) holds for the Jacobi matrix J.

Here, v _X , v _Y , and v _Z are the velocities of the tip of the end effector 201 in the X, Y, and Z directions, and ω _X , ω _Y , and ω _Z are the postures of the arm 2 in the X axis, the Y axis, The angular velocity rotating about the Z axis, and θ ₁ (dot) to θ ₆ (dot) are the angular velocities of the respective joints.

Next, the constraint control calculation unit 110 calculates a rotation vector ω = [ω _X , ω _Y , ω _Z ] ^T with respect to the posture from the rotation axis 510 r and the magnitude of the rotation speed | ω |.
Next, the constraint control calculation unit 110 calculates joint angular velocities θ ₁ (dots) to θ ₆ (dots) for each joint of the arm 2 such that the following equation (7) is satisfied. When there is no inverse matrix in the Jacobian matrix J, the constraint control calculation unit 110 calculates joint angular velocities θ ₁ (dots) to θ ₆ (dots) using a pseudo inverse matrix or the like.

Note that the constraint control calculation unit 110 may apply a limiter in order to prevent the obtained joint angular velocity from becoming excessive. In this case, it is preferable that the constraint control calculation unit 110 limits all joint angles by multiplying them by the same constant (a value between 0 and 1) so as not to change the direction of rotation.

Next, the synthesis unit 111 synthesizes the driven control command value (joint angle θ) output from the driven control calculation unit 103 and the constraint control command value (joint angular velocity θ (dot)) output from the constraint control calculation unit 110. (Step ST1111).
For example, the driven control calculation unit 103 outputs the joint angle θ = [θ ₁ ,..., Θ _m ] ^T , and the constraint control calculation unit 110 outputs the joint angular velocity θ (dot) = [θ ₁ (dot),. ·, Θ _m (dot)] When ^T is output, the joint angle command value output from the combining unit 111 is θ + T _S θ (dot) from the control cycle T _S. Here, m is the number of joints of the arm 2 controlled by the drive control unit 112.

Next, the drive control unit 112 drives the arm 2 based on the synthesis result (joint angle command value) by the synthesis unit 111 (step ST1112).

In the above description, the description and the examples have been partially based on the assumption that the arm 2 has six axes (the number of joints is six). However, the present invention can be similarly applied to an arm 2 other than six axes.

Embodiment 3 FIG.
In the direct teaching device 1 according to the second embodiment, the case where the restraining method is limited to only the posture restraint has been described. On the other hand, in the direct teaching device 1 according to the third embodiment, a case is shown in which the constraint method is limited to only the axial constraint. The axial constraint means that the position constraint for restricting the position of the arm 2 to the predetermined constraint shaft 504 and the posture constraint for restricting the posture of the arm 2 in the predetermined constraint direction 505 are performed simultaneously. That is, the direct teaching device 1 according to the third embodiment also performs the position constraint in addition to the posture constraint described in the second embodiment. In the following, a case is described in which the direct teaching device 1 performs a position constraint on a straight line 511 parallel to the Z-axis of the robot coordinate system as shown in FIG. The direction 505 can be set arbitrarily.
The configuration example of the direct teaching device 1 according to the third embodiment is the same as the configuration example of the direct teaching device 1 according to the second embodiment, and will be described below using the configuration example shown in FIG.

Note that the position and orientation calculation unit 104 according to the third embodiment calculates the position and orientation of the arm 2.
In addition, the proximity determination unit 105 according to Embodiment 3 determines whether the position of the arm 2 is approaching the constraint shaft 504 that is the constraint target, based on the calculation result by the position and orientation calculation unit 104, and determines whether the position of the arm 2 is It is determined whether the position and the posture of the arm 2 are approaching the constraint target by determining whether the position is approaching the constraint direction 505 that is the constraint target.

FIG. 19 is a flowchart showing an operation example of the direct teaching device 1 according to the third embodiment. The operation of the direct teaching device 1 according to the third embodiment shown in FIG. 19 differs from that of the direct teaching device 1 according to the second embodiment shown in FIG. The determination unit 106, the return determination unit 107, the target value calculation unit 109, and the constraint control calculation unit 110, and only an operation example of these configurations will be described.

First, position / posture calculation section 104 calculates the position and posture of arm 2 based on the calculation result by driven control calculation section 103 (step ST1904). In the third embodiment, since both the position and the posture of the arm 2 are restricted, the position and posture calculation unit 104 needs to calculate both the position and the posture of the arm 2. The position and orientation calculation unit 104 can execute the above calculation using forward kinematics.

In addition, the proximity determination unit 105 determines whether the position of the arm 2 is approaching the constraint shaft 504 and whether the posture of the arm 2 is approaching the constraint direction 505 based on the calculation result by the position and orientation calculation unit 104, respectively. Based on these determination results, it is comprehensively determined whether or not the position and orientation of the arm 2 is approaching the constraint target (step ST1905). For example, when the proximity determination unit 105 determines that the position of the arm 2 is approaching the constraint axis 504 and the posture of the arm 2 is approaching the constraint direction 505, the position and orientation of the arm 2 is approaching the constraint target. Is determined. The determination of the constraint direction 505 by the proximity determining unit 105 has been described in the second embodiment, and therefore will not be described. The following describes the determination of the constraint axis 504 by the proximity determining unit 105.

At this time, for example, as shown in FIG. 20, the proximity determining unit 105 lowers the perpendicular 512 from the position 501 of the end effector 201 to the constraint shaft 504, and if the length of the perpendicular 512 is smaller than a predetermined value, the arm 2 Is determined to be approaching the constraint shaft 504. For example, the coordinates of the position of the arm 2 are represented by [x, y, z], and the X and Y coordinates of the straight line 511 are represented by x ^* and y ^* , respectively. In this case, the length of the perpendicular 512 is represented by the following equation (8).

If the length of the perpendicular 512 shown in the equation (8) is smaller than a predetermined value, the proximity determining unit 105 determines that the position of the arm 2 is approaching the constraint shaft 504, and otherwise, It is determined that the position of the arm 2 is not approaching the constraint shaft 504. Note that the proximity determination unit 105 can similarly perform the proximity determination based on the length of the perpendicular drawn to the constraint axis 504 even when the constraint axis 504 is not parallel to the Z axis of the robot coordinate system.

Next, release operation determination section 106 determines whether or not the operator has performed a release operation (step ST1906). Here, when performing the determination using the external force, the release operation determination unit 106 can apply the same method as in the first embodiment. On the other hand, when the release operation determination unit 106 performs the determination using the position and orientation of the arm 2, since the teaching device 1 directly restricts the axis of the arm 2 in the third embodiment, an applicable determination method Is limited to a determination method compatible with the axial constraint. For example, in the example of FIG. 7, the release operation determination unit 106 according to the third embodiment can apply only the methods illustrated in FIGS. 7D and 7E. On the other hand, the release operation determination unit 106 in the third embodiment is not applicable because the method shown in FIGS. 7A to 7C cannot be performed while the arm 2 is restrained by the shaft.

Next, return determination section 107 determines whether or not a return condition for returning to a state where switching to restricted direct teaching can be satisfied is satisfied (step ST1907). In the third embodiment, since the direct teaching device 1 performs the axial restraint of the arm 2, the return determination unit 107 determines the position of the arm 2 and the restraint shaft 504 when performing the determination based on the position and orientation of the arm 2. And the relationship between the posture of the arm 2 and the constraint direction 505. The determination of the restraint direction 505 by the return determination unit 107 has been described in the second embodiment, and therefore will not be described. In the following, the determination of the restraint axis 504 by the return determination unit 107 will be described.

The return determination unit 107 makes a determination on the constraint axis 504 based on the length of the perpendicular 512, as in the proximity determination unit 105. That is, the return determination unit 107 determines that the return condition is satisfied when the length of the perpendicular 512 is larger than the predetermined value. Note that the predetermined value needs to be equal to or larger than the threshold value used by the proximity determination unit 105. This is because if the return determination unit 107 uses a value smaller than the threshold value, the proximity determination unit 105 determines that the position and orientation of the arm 2 is approaching the constraint target immediately after the return determination unit 107 determines that the return condition is satisfied. Is determined, and the direct teaching device 1 cannot be separated from the restricted direct teaching.

Further, in addition to the calculation of the target posture described in the second embodiment, target value calculation section 109 also calculates a target position for moving the position of arm 2 to constraint shaft 504 (step ST1909). For example, the target value calculation unit 109 sets a position 513 where the perpendicular 512 is lowered from the position 501 of the end effector 201 to the constraint shaft 504 as shown in FIG. Here, if the target value calculation unit 109 restricts the position of the arm 2 to the restriction axis 504 parallel to the Z axis of the robot coordinate system, the target position is [x ^* , y ^* , z].

Also, as in the second embodiment, the constraint control calculation unit 110 calculates the joint angular velocity θ (dot) for each joint of the arm 2 using the Jacobian matrix J (step ST1910). However, since the constraint control calculation unit 110 according to the third embodiment also performs a position constraint in addition to a posture constraint, the computation procedure changes as follows with respect to the second embodiment.

First, the constraint control calculation unit 110 determines the magnitude | ω | of the angular velocity of rotation.
Next, the constraint control calculation unit 110 calculates a Jacobi matrix J at the current position and orientation of the arm 2.
Next, the constraint control calculation unit 110 calculates a rotation vector ω = [ω _X , ω _Y , ω _Z ] ^T with respect to the posture from the rotation axis 510 r and the magnitude of the rotation speed | ω |. The operation so far is the same as in the second embodiment.

Next, the constraint control calculation unit 110 determines the speed | v | at which the position of the arm 2 is moved to the constraint shaft 504. For example, if control is performed so as to reach the target position in one second if no restriction is imposed, the constraint control calculation unit 110 calculates | v | = √ ｛(xx ^* ) ² + (yy ^* ) ² ｝. Further, the constraint control calculation unit 110 may determine a certain constant v ^* and set | v | = v ^* .
Next, restraining control calculation unit 110 calculates the velocity vector regarding the position of the arm _{_{2 v = [v X, v}} Y, 0] T. When constraining the constraining control unit 110 to the constraining axis 504 parallel to the Z axis of the robot coordinate system, v = | v | [x ^* -x, y ^* -y, 0] / √ ｛(xx ^* ) ² + (yy ^* ) ² }.
Next, the constraint control calculation unit 110 calculates the joint angular velocity θ (dot) for each joint of the arm 2 using the following equation (9). Note that the constraint control calculation unit 110 may apply a limiter to the obtained joint angular velocity.

Other operations are the same as in the second embodiment.
In the direct teaching device 1 according to the third embodiment, when the calculation relating to the posture constraint is omitted, the constraint relating to the posture of the arm 2 is not performed, and the control for restricting only the position of the arm 2 to the designated axis is performed. Become.

Embodiment 4 FIG.
In the direct teaching device 1 according to the second embodiment, the case where the restraining method is limited to only the posture restraint has been described. On the other hand, in the direct teaching device 1 according to the fourth embodiment, a case where the constraint method is limited to only the surface constraint is shown. The term “surface constraint” means that the position constraint for restricting the position of the arm 2 on the predetermined constraint surface 503 and the posture constraint for restricting the posture of the arm 2 in the predetermined constraint direction 505 are simultaneously performed. That is, the direct teaching device 1 according to the fourth embodiment also performs a position constraint in addition to the posture constraint described in the second embodiment. In the following, the direct teaching device 1 performs position constraint on the curved surface 513 in which the Z-axis coordinate is represented by a function z = f (x, y) of the X-axis coordinate and the Y-axis coordinate as shown in FIG. In this case, the restraint surface 503 and the restraint direction 505 can be set arbitrarily. For example, the restraint surface 503 is a function x = f (y, z) or y = f (z, x). The same method can be applied to the case where it is expressed.
The configuration example of the direct teaching device 1 according to the fourth embodiment is the same as the configuration example of the direct teaching device 1 according to the second embodiment, and will be described below using the configuration example shown in FIG.

Note that the position and orientation calculation unit 104 according to the fourth embodiment calculates the position and orientation of the arm 2.
Further, proximity determining section 105 according to Embodiment 4 determines whether the position of arm 2 is approaching restraining surface 503, which is a restraint target, based on the calculation result by position and attitude calculating section 104, and determines whether the attitude of arm 2 is high. It is determined whether the position and the posture of the arm 2 are approaching the constraint target by determining whether the position is approaching the constraint direction 505 that is the constraint target.

FIG. 22 is a flowchart showing an operation example of the direct teaching device 1 according to the fourth embodiment. The operation of the direct teaching device 1 according to the fourth embodiment shown in FIG. 22 differs from that of the direct teaching device 1 according to the third embodiment shown in FIG. The unit 107, the target value calculation unit 109, and the constraint control calculation unit 110, and only an operation example of these configurations will be described.

First, the proximity determination unit 105 determines whether the position of the arm 2 is approaching the constraint surface 503 and whether the posture of the arm 2 is approaching the constraint direction 505 based on the calculation result by the position and orientation calculation unit 104, respectively. Based on these determination results, it is comprehensively determined whether the position and orientation of the arm 2 is approaching the constraint target (step ST2205). For example, when the proximity determination unit 105 determines that the position of the arm 2 is approaching the constraint surface 503 and that the posture of the arm 2 is approaching the constraint direction 505, the position and orientation of the arm 2 is approaching the constraint target. Is determined. The determination on the constraint direction 505 by the proximity determination unit 105 has been described in the second embodiment, and therefore will not be described. Hereinafter, the determination on the constraint surface 503 by the proximity determination unit 105 will be described.

At this time, the proximity determination unit 105 draws a straight line parallel to the Z axis of the robot coordinate system from the position 501 of the end effector 201 to the constraint surface 503, and if the length of the straight line is smaller than a predetermined value, the arm 2 It is determined that the position is approaching the constraint surface 503. For example, when the coordinates of the position of the arm 2 are [x, y, z], the length of the straight line is {z−f (x, y)}. If the length is smaller than a predetermined value, the proximity determining unit 105 determines that the position of the arm 2 is approaching the constraint surface 503, and otherwise determines that the position of the arm 2 is close to the constraint surface 503. Is determined not to be approaching.

Next, the release operation determination unit 106 determines whether the operator has performed a release operation (step ST2206). Here, when performing the determination using the external force, the release operation determination unit 106 can apply the same method as in the first embodiment. On the other hand, when the release operation determination unit 106 performs the determination using the position and orientation of the arm 2, the direct teaching device 1 directly restrains the arm 2 on the curved surface in the fourth embodiment. Is limited to the determination method compatible with the curved surface constraint. For example, in FIG. 7, the release operation determination unit 106 in the fourth embodiment is applicable only to the method shown in FIGS. 7A and 7B. On the other hand, the method shown in FIG. 7C cannot be applied to the release operation determination unit 106 in Embodiment 4 because the posture of the arm 2 cannot be freely changed. 7D and 7E cannot be applied to the release operation determination unit 106 in the fourth embodiment because the arm 2 cannot be separated from the curved surface 513.

Next, return determination section 107 determines whether or not a return condition to a state in which switching to restricted direct teaching can be performed is satisfied (step ST2207). In the fourth embodiment, since the direct teaching device 1 performs the curved surface constraint on the arm 2, the return determination unit 107 determines the position of the arm 2 and the constraint surface 503 when performing the determination based on the position and orientation of the arm 2. And the relationship between the posture of the arm 2 and the constraint direction 505. The determination of the restraint direction 505 by the return determination unit 107 has been described in the second embodiment, and therefore will not be described. In the following, the determination of the restraint surface 503 by the return determination unit 107 will be described.

In the determination with respect to the constraint surface 503, the return determination unit 107 draws a straight line parallel to the Z axis of the robot coordinate system from the position 501 of the end effector 201 to the constraint surface 503 in the same manner as the proximity determination unit 105. Perform based on length. That is, when the length is larger than the predetermined value, the return determination unit 107 determines that the return condition is satisfied. Note that the predetermined value needs to be equal to or larger than the predetermined value used by the proximity determination unit 105. The reason is the same as in the third embodiment.

{Circle around (2)} In addition to the calculation of the target posture described in the second embodiment, target value calculation section 109 also calculates a target position when moving the position of arm 2 to constraint surface 503 (step ST2209). For example, when the coordinates of the position of the arm 2 are [x, y, z], the target value calculation unit 109 replaces only the Z-axis coordinates with the coordinates on the curved surface [x, y, f (x, y)]. ] Is the target position.

Also, as in the second embodiment, the constraint control calculation unit 110 calculates the joint angular velocity θ (dot) for each joint of the arm 2 using the Jacobian matrix J (step ST2210). However, since the constraint control calculation unit 110 according to the fourth embodiment also performs the position constraint in addition to the posture constraint, the computation procedure changes as follows with respect to the second embodiment.

Next, the constraint control calculation unit 110 determines the speed | v | at which the position of the arm 2 is moved to the constraint shaft 504. For example, if control is performed so as to reach the target position in one second if no restriction is imposed, the constraint control calculation unit 110 sets | v | = | z−f (x, y) |. The constraint control calculation unit 110 may set the speed | v | to a certain constant value.
Next, restraining control calculation unit 110, a velocity vector v = _[0,0, v ^Z] on the position of the arm 2 ^{T v = [0,0, | v} |] is calculated from.
Next, the constraint control calculation unit 110 calculates the joint angular velocity θ (dot) for each joint of the arm 2 using the following equation (10). Note that the constraint control calculation unit 110 may apply a limiter to the obtained joint angular velocity.

Other operations are the same as those of the second and third embodiments.
In the direct teaching device 1 according to the fourth embodiment, when the calculation related to the posture constraint is omitted, the constraint related to the posture of the arm 2 is not performed, and the control for restricting only the position of the arm 2 to the designated surface is performed. Become. In the direct teaching device 1 according to the fourth embodiment, when the target posture is a direction in which a perpendicular line is drawn from the position of the arm 2 to the constraint surface 503, the end effector 201 is always perpendicular to the constraint surface 503. It becomes such a posture constraint.

In addition, within the scope of the present invention, the present invention can freely combine any of the embodiments, modify any of the components in each of the embodiments, or omit any of the components in each of the embodiments. is there.

The robot direct teaching device according to the present invention can switch between normal direct teaching and constrained direct teaching while the operator is operating the arm, and can be used for a direct teaching device or the like that directly teaches a robot. Suitable for.

DESCRIPTION OF SYMBOLS 1 Direct teaching device 2 Arm 101 Position / posture measurement unit 102 External force detection unit 103 Follow-up control calculation unit 104 Position / posture calculation unit 105 Proximity determination unit 106 Release operation determination unit 107 Return determination unit 108 Switching unit 109 Target value calculation unit 110 Constraint control calculation Unit 111 combining unit 112 drive control unit 201 end effector

Claims

An external force detection unit that detects an external force applied to an arm of the robot,
A driven control calculation unit that calculates the movement of the arm according to the external force detected by the external force detection unit,
A position and orientation calculation unit that calculates a position and orientation that is at least one of the position or the orientation of the arm,
A proximity determination unit that determines whether the position and orientation of the arm is approaching a constraint target to which the position and orientation of the arm is restricted, based on a calculation result by the position and orientation calculation unit;
A release operation determination unit that determines whether the operator has performed a release operation for releasing the constrained direct teaching,
A return determination unit that determines whether a return condition to a state that can be switched to the constrained direct teaching is satisfied,
A target value calculation unit that calculates a target value of the constraint control in the constrained direct teaching based on the constraint target and the calculation result by the position and orientation calculation unit;
A constraint control calculation unit that calculates a movement of the arm that moves to the target value calculated by the target value calculation unit based on a calculation result by the position and orientation calculation unit;
When the own device is in the first state, the own device is switched to the second state when it is determined that the approach target is approaching the restriction target, and when the own device is in the second state. When the release operation determination unit determines that the operator has performed the release operation, the own device is switched to the third state, and when the own device is in the third state, the return determination unit satisfies the return condition. When it is determined that the self-machine is in the first state, when the self-machine is in the second state, the processing by the constraint control calculation unit is enabled, and the self-machine is in the first state or the third state. A switching unit for invalidating the processing by the constraint control calculation unit when in the state,
A combining unit that combines the calculation result by the driven control calculation unit and the calculation result by the constraint control calculation unit;
A drive control unit that drives the arm based on a result of the synthesis by the synthesis unit.
A position and orientation measurement unit that measures parameters related to the position and orientation of the arm,
The driven control calculation unit calculates the movement of the arm according to the external force detected by the external force detection unit based on the measurement result by the position and orientation measurement unit,
The direct teaching device according to claim 1, wherein the position / posture calculation unit calculates the position / posture of the arm based on a calculation result by the driven control calculation unit.
A position and orientation measurement unit that measures parameters related to the position and orientation of the arm,
The direct teaching device according to claim 1, wherein the position / posture calculation unit calculates the position / posture of the arm based on a measurement result by the position / posture measurement unit.
2. The direct teaching according to claim 1, wherein the release operation determination unit determines whether the operator has performed a release operation based on a detection result by the external force detection unit or a calculation result by the position and orientation calculation unit. 3. apparatus.
The return determination unit is configured to perform a return condition when a predetermined time has elapsed after the operator performed the release operation, or when the position and orientation of the arm calculated by the position and orientation calculation unit does not approach the constraint target. The direct teaching device according to claim 1, wherein it is determined that the condition is satisfied.
The arm is a vertical articulated robot arm,
The position and orientation calculation unit calculates an orientation of the arm,
The said proximity determination part determines whether the attitude | position of the said arm is approaching the restraint direction which is a restraint target based on the calculation result by the said position-and-posture calculation part. The Claims 1-5 characterized by the above-mentioned. A direct teaching device according to any one of the preceding claims.
The arm is a vertical articulated robot arm,
The position and orientation calculation unit calculates the position and orientation of the arm,
The proximity determination unit determines whether the position of the arm is approaching a constraint axis that is a constraint target based on a calculation result by the position and orientation calculation unit, and determines whether the posture of the arm is approaching a constraint direction that is a constraint target. The direct teaching device according to any one of claims 1 to 5, wherein it is determined whether or not the position and orientation of the arm is approaching the constraint target by determining whether the arm is in the restraint target.
The arm is a vertical articulated robot arm,
The position and orientation calculation unit calculates the position and orientation of the arm,
The approach determination unit determines whether the position of the arm is approaching a constraint surface that is a constraint target based on a calculation result by the position and orientation calculation unit, and determines whether the posture of the arm is approaching a constraint direction that is a constraint target. The direct teaching device according to any one of claims 1 to 5, wherein it is determined whether or not the position and orientation of the arm is approaching the constraint target by determining whether the arm is in the restraint target.
An external force detecting unit for detecting an external force applied to an arm of the robot;
A driven control calculation unit that calculates a movement of the arm according to the external force detected by the external force detection unit;
A position and orientation calculation unit that calculates a position and orientation that is at least one of a position and an orientation of the arm;
A step of determining whether the position and orientation of the arm is approaching a constraint target to which the position and orientation of the arm is restricted, based on a calculation result by the position and orientation calculation unit;
A step of determining whether the operator has performed a release operation for releasing the constrained direct teaching,
A step of determining whether or not a return determination unit satisfies a return condition to a state that can be switched to the constrained direct teaching;
A target value calculation unit that calculates a target value of the constraint control in the constrained direct teaching based on the constraint target and the calculation result by the position and orientation calculation unit;
A step of calculating a movement of the arm that moves to the target value calculated by the target value calculation unit, based on a calculation result by the position and orientation calculation unit,
A switching unit that switches the own device to the second state when the proximity determination unit determines that the own device is approaching the restriction target when the own device is in the first state; In the case where it is determined that the operator has performed the release operation by the release operation determination unit, the own device is switched to the third state, and when the own device is in the third state, the return determination unit When it is determined that the return condition is satisfied, the own device is switched to the first state, and when the own device is in the second state, the processing by the constraint control calculation unit is enabled, and the own device is in the first state. Or a step of invalidating the processing by the constraint control calculation unit when in the third state,
A combining unit combining the calculation result by the driven control calculation unit and the calculation result by the constraint control calculation unit;
A driving control unit for driving the arm based on a result of the synthesis by the synthesizing unit.