WO2019225358A1 - ロボットの直接教示装置及び直接教示方法 - Google Patents

ロボットの直接教示装置及び直接教示方法 Download PDF

Info

Publication number
WO2019225358A1
WO2019225358A1 PCT/JP2019/018766 JP2019018766W WO2019225358A1 WO 2019225358 A1 WO2019225358 A1 WO 2019225358A1 JP 2019018766 W JP2019018766 W JP 2019018766W WO 2019225358 A1 WO2019225358 A1 WO 2019225358A1
Authority
WO
WIPO (PCT)
Prior art keywords
arm
unit
orientation
constraint
calculation unit
Prior art date
Application number
PCT/JP2019/018766
Other languages
English (en)
French (fr)
Inventor
鉄也 田原
岩下　純久
野村　琢磨
広大 杉本
Original Assignee
アズビル株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by アズビル株式会社 filed Critical アズビル株式会社
Priority to CN201980032159.2A priority Critical patent/CN112118940B/zh
Priority to KR1020207031484A priority patent/KR102381594B1/ko
Publication of WO2019225358A1 publication Critical patent/WO2019225358A1/ja

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1653—Programme controls characterised by the control loop parameters identification, estimation, stiffness, accuracy, error analysis
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/19—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by positioning or contouring control systems, e.g. to control position from one programmed point to another or to control movement along a programmed continuous path
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/42—Recording and playback systems, i.e. in which the programme is recorded from a cycle of operations, e.g. the cycle of operations being manually controlled, after which this record is played back on the same machine

Definitions

  • the present invention relates to a direct teaching apparatus and a direct teaching method for directly teaching a robot.
  • teaching In industrial robots, work called teaching (teaching) is performed in advance in order to make the robot work. As one of methods for teaching the robot, there is a method called direct teaching (direct teaching).
  • Patent Document 1 discloses a robot direct teaching method using a force sensor.
  • Patent Document 2 discloses a direct teaching method for a robot using a torque detection means.
  • a schematic configuration of the direct teaching apparatus disclosed in these patent documents is shown in FIG.
  • the external force detection unit 1101 detects an external force applied by the operator to the arm 2 of the robot using a force sensor, a torque sensor, or the like.
  • 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).
  • the driven control calculation unit 1102 may use parameters related to the position and orientation of the arm measured by a position and orientation measurement unit (not shown) in the driven control calculation.
  • the arm position / posture means at least one of the arm position and the arm posture.
  • the parameters include the arm position, the arm posture, the arm joint angle, and the like.
  • 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.
  • the drive control unit 1103 drives the arm 2 in accordance with the driven control command value notified by the driven control calculation unit 1102.
  • the direct teaching device 11 shown in FIG. 20 can be controlled so that the arm 2 moves according to the external force applied by the operator.
  • the direct teaching device 11 records the parameter at that time as a teaching point.
  • the teaching points recorded by the direct teaching device 11 are used when the robot works.
  • the direct teaching of the robot has the advantage of being intuitive and easy to understand because the operator directly operates the arm to teach the position and posture.
  • the feature that the arm moves according to the external force applied by the operator is not only an advantage.
  • Patent Documents 3 and 4 disclose techniques for solving the above-described 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.
  • position movement mode only the position can be moved while maintaining the posture of the tip of the arm.
  • direction movement mode only the posture can be changed while maintaining the position of the tip of the arm.
  • This direct teaching device can 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.
  • a direct teaching device in which an operation mode can be switched to a restraint mode or an omnidirectional movement mode by an input device.
  • the tip of the end effector In the restraint mode, the tip of the end effector can be moved along a particular restraint axis or restraint surface.
  • the omnidirectional movement mode normal direct teaching can be performed.
  • the tip of the arm By switching to the constraint mode in the middle of direct teaching, the tip of the arm can be accurately moved in the vertical direction, and the difficulty of operation in direct teaching is reduced.
  • the direct teaching devices disclosed in Patent Documents 3 and 4 have a mode for performing normal direct teaching without imposing restrictions on the operation of the arm and a mode for performing direct teaching with constraining that imposes restrictions on the operation of the arm. And switch.
  • the mode is switched by a mode switch, and in Patent Document 4, by an input device, the mode can be switched by an operator operating a switch or the like by hand.
  • the present invention has been made to solve the above problems, and provides a direct teaching device capable of switching between normal direct teaching and constrained direct teaching while an operator is operating an arm.
  • the purpose is that.
  • the direct teaching device 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 the movement of the arm according to the external force detected by the external force detection unit, and the position of the arm Alternatively, based on the calculation result of the position and orientation calculation unit that calculates the position and orientation of at least one of the orientations, and the position and orientation calculation unit, the position and orientation of the arm is set to the constraint target that is the constraint destination of the position and orientation of the arm.
  • the target value calculation unit that calculates the target value of the constraint control based on the calculation result by the constraint target and the position and orientation 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, and a constraint control limit unit that limits the calculation result by the constraint control calculation unit
  • the proximity determination unit determines that the target is approaching the constraint target
  • the calculation result by the constraint control calculation unit is validated.
  • the calculation by the constraint control calculation unit is performed.
  • Embodiment 1 of this invention It is a figure explaining the operation example of the target value calculation part in Embodiment 1 of this invention (in the case of attitude
  • 9A and 9B are diagrams showing an operation example of the driven control arithmetic unit in the second embodiment of the present invention.
  • 11A, 11B, and 11C are diagrams for explaining an operation example of the proximity determining unit according to Embodiment 2 of the present invention.
  • FIGS. 12A and 12B are diagrams for explaining an operation example of the target value calculation unit according to Embodiment 2 of the present invention, and are diagrams showing an example of calculation of the rotation axis.
  • FIGS. 13A and 13B are diagrams for explaining an operation example of the target value calculation unit according to Embodiment 2 of the present invention, and are diagrams showing an example of calculation of the rotation direction. It is a figure explaining the operation example of the target value calculation part in Embodiment 2 of this invention. It is a figure explaining the axial restraint by the direct teaching apparatus which concerns on Embodiment 3 of this invention. It is a flowchart which shows the operation example of the direct teaching apparatus which concerns on Embodiment 3 of this invention.
  • FIG. 1 is a diagram showing a configuration example of a direct teaching apparatus 1 according to Embodiment 1 of the present invention.
  • the direct teaching device 1 performs direct teaching of a robot.
  • the direct teaching device 1 includes, as a direct teaching method, normal direct teaching that does not impose a constraint on the position and orientation of the arm 2 that the robot has, and direct teaching with constraint that restricts the position and orientation of the arm 2 to a constraint target. Switching is possible.
  • the position and orientation 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 constraint target is a constraint destination of the position and orientation of the arm 2.
  • a constraint target for the position of the arm 2 there are a constraint surface 503 and a constraint shaft 504.
  • Types of the restraining surface 503 include a flat surface and a curved surface.
  • Types of the constraint axis 504 include a straight line and a curved line.
  • a constraint direction 505 is a constraint target for the posture of the arm 2.
  • the number of constraint targets used in the direct teaching device 1 is not limited to one and may be a plurality.
  • a reference robot coordinate system is set for the robot, and a tool coordinate system is set for the end effector 201 (see FIG. 6 and the like).
  • the direct teaching apparatus 1 includes a position / orientation measurement unit 101, an external force detection unit 102, a driven control calculation unit 103, a position / orientation calculation unit 104, a proximity determination unit 105, a switching unit 106, and a target value calculation unit. 107, a constraint control calculating unit 108, a constraint control limiting unit 109, a combining unit 110, and a drive control unit 111.
  • the driven control calculation unit 103, the position / orientation calculation unit 104, the proximity determination unit 105, the switching unit 106, the target value calculation unit 107, the constraint control calculation unit 108, the constraint control limit unit 109, the synthesis unit 110, and the drive control unit 111 are It 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.
  • LSI Large-Scale Integration
  • CPU Central Processing Unit
  • the position / orientation measurement unit 101 measures parameters related to the position / orientation of the arm 2.
  • the parameters related to the position and orientation 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.
  • 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.
  • 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 motor current 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 (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 / orientation measurement unit 101.
  • the position / orientation measurement unit 101, the external force detection unit 102, and the driven control calculation unit 103 are the same as those used in normal direct teaching without restriction, and are known techniques.
  • the position / orientation calculation unit 104 calculates the position / orientation of the arm 2 based on the calculation result by the driven control calculation unit 103.
  • the driven control calculation unit 103 uses the measurement result by the position / orientation measurement unit 101
  • the position / orientation calculation unit 104 uses the calculation result by the driven control calculation unit 103.
  • 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 / orientation measurement unit 101.
  • the position / orientation calculation unit 104 calculates the position / orientation of the arm 2 based on the measurement result of the position / orientation measurement unit 101.
  • the position / orientation calculation unit 104 may calculate the position / orientation of the arm 2 using both the calculation result by the driven control calculation unit 103 and the measurement result by the position / orientation measurement unit 101.
  • 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.
  • the switching unit 106 switches the operation of the constraint control calculation unit 108 according to the determination result by the proximity determination unit 105. That is, the switching unit 106 validates the processing by the constraint control calculation unit 108 when the proximity determination unit 105 determines that the position and orientation of the arm 2 is approaching the constraint target. On the other hand, when the proximity determination unit 105 determines that the position and orientation of the arm 2 is not approaching the constraint target, the switching unit 106 invalidates the processing by the constraint control calculation unit 108.
  • the switching unit 106 may also switch the operation (target constraint target) by the target value calculation unit 107 according to the determination result by the proximity determination unit 105.
  • the target value calculation unit 107 calculates a target value for constraint control based on the calculation result by the constraint target and position / orientation calculation unit 104.
  • the target value for 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 108 calculates the movement of the arm 2 (constraint control command value) that moves to the target value calculated by the target value calculation unit 107 based on the calculation result by the position / orientation calculation unit 104.
  • the constraint control restriction unit 109 restricts the calculation result by the constraint control calculation unit 108. That is, the constraint control limiting unit 109 imposes a limit on the calculation result by the constraint control calculation unit 108 so that the calculation result by the driven control calculation unit 103 is given priority over the calculation result by the constraint control calculation unit 108.
  • the constraint control calculation unit 108 and the constraint control restriction unit 109 are illustrated as separate components, but may be implemented as a single unit.
  • the synthesizing unit 110 synthesizes the calculation result (driven control command value) by the driven control calculation unit 103 and the restriction result (constraint control command value) by the constraint control limiting unit 109 into one command value.
  • the drive control unit 111 drives the arm 2 based on the synthesis result by the synthesis unit 110.
  • switching between normal direct teaching and direct teaching with restraint can be executed by operating the arm 2 by an operator.
  • This switching method is realized by the following two mechanisms.
  • the first is a mechanism in which the direct teaching device 1 automatically switches from normal direct teaching to direct teaching with restraint when the position and orientation of the arm 2 approaches the restraining target.
  • the direct teaching device 1 when it is desired to restrain the position of the arm 2 to a predetermined plane 506 (restraint surface 503), the direct teaching device 1 has a distance from the plane 506 to the position of the arm 2, for example, ⁇ If it is 50 [mm] or less, direct teaching with restraint is performed, otherwise normal direct teaching is performed. Also, for example, as shown in FIGS.
  • the direct teaching device 1 when the posture of the arm 2 is to be restrained in the directly downward direction 507 (restraining direction 505), the direct teaching device 1 has the posture of the arm 2 that is ⁇ 0. If it is 2 [rad] or less, direct teaching with restriction is performed, and otherwise normal direct teaching is performed. With this mechanism, the direct teaching device 1 can realize switching from normal direct teaching to restricted direct teaching only by operating the arm 2 by the operator.
  • the second is a mechanism that enables the operator to overcome the restraint control and operate the arm 2 by deliberately restraining the restraining force of the direct teaching device 1 during direct teaching with restraint.
  • the direct teaching device 1 suppresses the joint angular velocity ⁇ (dot) in the constraint control to, for example, 0.1 [rad / s] or less, and allows the operator to operate the arm 2 by applying an external force greater than that. . Thereby, the operator can deviate the arm 2 little by little from the restrained state, and finally can be removed from the execution condition of the restrained direct teaching.
  • the position and orientation of the arm 2 are maintained by strongly restraining the arm 2 to a restraining surface or the like in direct teaching with restraint.
  • the direct teaching device 1 according to the first embodiment by restraining the restraining force, as shown in FIGS. 2C, 2D, 3C, and 3D, only the operation of the arm 2 by the operator is performed. Switching from teaching to normal direct teaching can be realized.
  • the direct teaching device 1 can switch between normal direct teaching and restricted direct teaching while the operator is operating the arm 2. It becomes.
  • an operation example of the direct teaching apparatus 1 shown in FIG. 1 will be described with reference to FIG.
  • the position / orientation measurement unit 101 measures parameters related to the position / orientation of the arm 2 (step ST401).
  • the external force detection part 102 detects the external force applied to the arm 2 by the operator (step ST402).
  • the external force detected by the external force detection 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 sensor structure and the like. Therefore, in such a case, the external force detection unit 102 may calculate only the external force component by calculating a component caused by gravity and subtracting the component caused by the gravity from the detected external force. This is a known technique called gravity compensation, and is disclosed, for example, in Patent Document 5.
  • Japanese Patent Laid-Open No. 01-066715 Japanese Patent Laid-Open No. 01-066715
  • the driven control calculation unit 103 calculates the movement of the arm 2 (driven control command value) according to the external force detected by the external force detection unit 102 based on the measurement result by the position / orientation measurement unit 101 (step ST403).
  • 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. Based on the determination result and the position / orientation of the arm 2 measured by the position / orientation measurement unit 101, the driven control calculation unit 103 or the joint angular velocity ⁇ (dot) ) Etc. are calculated.
  • the driven control calculation method by the driven control calculation unit 103 is disclosed in, for example, Patent Literature 2 and the like, and various methods have been developed.
  • the position / orientation calculation unit 104 calculates the position / orientation of the arm 2 based on the calculation result by the driven control calculation unit 103 (step ST404).
  • the direct teaching device 1 performs position restraint, that is, when the direct teaching device 1 restrains the position of the arm 2 to the predetermined restraining surface 503 or the restraining shaft 504
  • the position / orientation calculation unit 104 is at least the arm.
  • the position of 2 needs to be calculated.
  • the direct teaching device 1 performs posture restraint that is, when the direct teaching device 1 restrains the posture of the arm 2 in the predetermined restraining direction 505
  • the position / posture calculation unit 104 calculates at least the posture of the arm 2. There is a need to.
  • the direct teaching device 1 performs position restraint and posture restraint, that is, when the direct teaching device 1 restrains the position and posture of the arm 2 in the predetermined restraint surface 503 or the restraint shaft 504 and the predetermined restraint direction 505.
  • the position / orientation calculation unit 104 needs to calculate the position and orientation of the arm 2.
  • the position / orientation calculation unit 104 performs the above calculation based on the calculation result (latest driven control command value) by the driven control calculation unit 103. This is because, when the direct teaching device 1 performs direct teaching with restraint, the component that moves out of the restraining surface 503 or the like out of the movement amount given to the arm 2 by the operator can be canceled simultaneously, and the restraint control is most effective. Because it works. Further, the position / orientation calculation unit 104 can perform the above calculation based on the measurement result of the position / orientation measurement unit 101 without using the calculation result of the driven control calculation unit 103.
  • the position / 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 / orientation measurement unit 101.
  • the position / orientation calculation unit 104 determines the position of the arm 2 by forward kinematics calculation. The posture can be calculated.
  • 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).
  • the purpose of constraint control is to fix the Z-axis coordinate of the tip of the end effector 201 at a position of 0.1 [m], and the approach determination threshold value is 0.05 [m].
  • the proximity determination unit 105 restrains the position of the arm 2 if the Z-axis coordinate of the tip of the end effector 201 calculated by the position / orientation calculation unit 104 is within a range of 0.05 to 0.15 [m]. It is determined that the target is approaching, and otherwise, it is determined that the position of the arm 2 is not approaching the constraint target.
  • the proximity determination unit 105 has an angle ⁇ formed by the upward direction (reference direction) 508 and the direction 502 of the end effector 201 equal to or greater than ( ⁇ 0.1) [rad], If the angle formed by 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, and otherwise, the arm 2 Is determined not to be close to the constraint target.
  • the proximity determination unit 105 performs the above determination for each constraint target, and the position of the arm 2 is at least one of the constraint targets.
  • the proximity determination unit 105 further evaluates the distance between the arm 2 and each constraint target, and notifies the switching unit 106 of information indicating the constraint target closest to the arm 2 together with the determination result. Good.
  • switching section 106 switches the operation of constraint control calculation section 108 in accordance with the determination result by proximity determination section 105 (step ST406). That is, the switching unit 106 validates the processing by the constraint control calculation unit 108 when the proximity determination unit 105 determines that the position and orientation of the arm 2 is approaching the constraint target. On the other hand, when the proximity determination unit 105 determines that the position and orientation of the arm 2 is not approaching the constraint target, the switching unit 106 stops the calculation by the constraint control calculation unit 108 or the calculation result of the arm 2 The processing by the constraint control calculation unit 108 is invalidated so as not to be reflected in the control.
  • the direct teaching device 1 automatically starts restraint control when the operator moves the position and orientation of the arm 2 closer to the restraint target. Further, the direct teaching device 1 automatically ends the restraint control when the operator moves the position and orientation of the arm 2 away from the restraint target by a combination with the restraint control restriction unit 109 described later. As a result, the direct teaching device 1 can switch on and off the constraint control while the arm 2 is held by the operator.
  • the switching unit 106 notifies the target value calculation unit 107 (to be described later) The operation of the target value calculation unit 107 may be switched by outputting information.
  • the target value calculation unit 107 calculates a target value for constraint control based on the calculation result by the constraint target and position / orientation calculation unit 104 (step ST407).
  • the purpose of constraint control is to fix the Z-axis coordinate of the tip of the end effector 201 at 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 It is assumed that [X P , Y P , Z P ].
  • the target value calculation unit 107 sets the target value for constraint control to [X P , Y P , 0.1].
  • the purpose of the constraint control is to align the X axis with the X axis of the robot coordinate system after setting the Z axis in the direction 502 of the end effector 201 to the direct downward direction 507 as shown in the lower part of FIG. .
  • the target posture which is the target value of the constraint control
  • the rotation matrix expression is expressed by the following equation (1). Note that the expression of the posture by the rotation matrix is disclosed in Non-Patent Document 1, for example, and details thereof are omitted.
  • the target value calculation unit 107 may calculate only one target value as the target value for constraint control, or may calculate a plurality of target values and select one target value from them. .
  • the target value calculation unit 107 [ X P , Y P , 0.1n] (n is an integer) is selected closest to the current position.
  • the purpose of the constraint control is to rotate the Z-axis in the direction 502 of the end effector 201 to the direct downward direction 507, and rotate it in ⁇ / 2 increments (in increments of 90 degrees) with this Z-axis as the rotation axis.
  • the target value calculation unit 107 selects a target value closest to the current posture as a target value for constraint control.
  • the target posture which is the target value of the constraint control
  • the target posture changes in four ways depending on n.
  • the target value calculation unit 107 selects one closest to the current posture from among them.
  • the target value calculation unit 107 can determine whether or not it is close to the current posture based on the magnitude of the rotation angle when rotating from the current posture to each target posture.
  • the target value calculation unit 107 may select a target value for constraint control from the information.
  • the constraint control calculation unit 108 calculates the movement (constraint control command value) of the arm 2 that moves to the target value calculated by the target value calculation unit 107 based on the calculation result by the position / orientation calculation unit 104 (step). ST408). At this time, the constraint control calculation unit 108 moves from the target value calculated by the target value calculation unit 107 to the amount of movement of the arm 2 or movement per unit time necessary to move the arm 2 from the current position and orientation to the target value. A joint angular velocity ⁇ (dot), which is a quantity, is calculated. Further, the constraint control calculation unit 108 may calculate a joint angle change amount ⁇ per control cycle instead of the joint angular velocity ⁇ (dot).
  • the constraint control limiting unit 109 limits the calculation result by the constraint control calculation unit 108 (step ST409). That is, the constraint control limiting unit 109 imposes a limit on the calculation result by the constraint control calculation unit 108 so that the calculation result by the driven control calculation unit 103 is given priority over the calculation result by the constraint control calculation unit 108.
  • the role of the restriction control restriction unit 109 is that the operator does not perform restriction control from the state where the direct teaching device 1 performs restriction control (the state during direct teaching with restriction) (the state of normal direct teaching). ).
  • the restraint control restriction unit 109 restricts the strength of the restraint control so that it can be removed from the restraint target if the operator applies a strong external force.
  • the restraint control restriction unit 109 restricts the restraint control speed to, for example, 0.05 [m / s] or less.
  • the arm 2 moves faster than 0.05 [m / s]
  • the arm 2 moves away from the restraint surface 503, and finally the proximity determination unit 105 does not approach the restraint surface 503. Can be moved until it is determined.
  • the operator tries to move the arm 2 later than 0.05 [m / s]
  • the arm 2 continues to be restrained by the restraining surface 503.
  • the restraint control restriction unit 109 sets the joint angular velocity ⁇ (dot) of restraint control to, for example, 0.1 [rad / s]. Restrict to: As a result, if the operator moves the arm 2 faster than 0.1 [rad / s], the arm 2 moves away from the restraint surface 503 and finally the proximity determination unit 105 does not approach the restraint surface 503. Can be moved until it is determined. On the other hand, if the operator tries to move the arm 2 later than 0.1 [rad / s], the arm 2 continues to be restrained by the restraining surface 503.
  • the constraint control restriction unit 109 places a restriction on the speed at which the posture is changed or the joint angular velocity ⁇ (dot).
  • the operator can perform an operation of releasing the restraint state.
  • the constraint control limiting unit 109 limits the strength of the constraint control, in any case, it is very preferable to maintain the direction in which the arm 2 is moved by the constraint control. For example, when the constraint control restriction unit 109 restricts the joint angular velocity ⁇ (dot) and exceeds a limit unless a certain joint angular velocity ⁇ (dot) is multiplied by 0.5, all joint angular velocities ⁇ (dots) ) Should be 0.5 times, and only the joint angular velocity ⁇ (dot) exceeding the limit should not be corrected.
  • the constraint control restriction unit 109 does not restrict all joint angular velocities ⁇ (dots) in the same way, the constraint control calculation unit 108 changes from the direction in which it is intended to move, so the accuracy of the constraint control deteriorates. .
  • the synthesizing unit 110 synthesizes the driven control command value output from the driven control calculating unit 103 and the constraint control command value output from the constraint control limiting unit 109 into one command value (step ST410).
  • the driven control calculation unit 103 outputs the joint angle theta D as a driven control command value
  • is restraining control limiting section 109 outputs the joint angle variation [Delta] [theta] C as restraining control command value
  • the synthesizer 110, ⁇ D + ⁇ C is output as a joint angle command value.
  • the driven control calculation unit 103 outputs the position coordinates [x D , y D , z D ] and the posture T D (rotation matrix expression) as the driven control command value
  • the constraint control limit unit 109 outputs the constraint control command value.
  • the combining unit 110 When the position change amount [ ⁇ x C , ⁇ y C , ⁇ z C ] and the posture change amount ⁇ T C (rotation matrix representation) are output as, the combining unit 110 outputs [x D + ⁇ x C , y D + ⁇ y C , z D + ⁇ z C ] is output as a position command value, and ⁇ T C T D (rotation matrix expression) is output as an attitude command value (composition of the attitude expressed in the rotation matrix is a matrix product).
  • the driven control calculation unit 103 outputs the torque ⁇ D as the driven control command value and the constraint control limit unit 109 outputs the torque change amount ⁇ C as the constraint control command value
  • the combining unit 110 ⁇ D + ⁇ C is output as a torque command value.
  • the drive control unit 111 drives the arm 2 based on the synthesis result by the synthesis unit 110 (step ST411). Since the drive of the arm 2 by the drive control unit 111 is a normal control of the arm 2, the details thereof are omitted.
  • the direct teaching device 1 includes the external force detection 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 detection unit 102.
  • the position-and-orientation calculation unit 104 that calculates the position and orientation of at least one of the positions or postures of the arm 2, and the position-and-orientation calculation unit 104, Based on the proximity determination unit 105 that determines whether the position and orientation of the arm 2 is approaching the constraint target that is the constraint destination of the position and orientation of the arm 2, and the constraint control based on the calculation result by the constraint target and position and orientation calculation unit 104
  • the target value calculated by the target value calculation unit 107 is transferred.
  • the proximity control unit 108 When it is determined by the proximity control unit 108 that calculates the movement of the arm 2 to be performed, the constraint control limit unit 109 that limits the calculation result by the constraint control calculation unit 108, and the proximity determination unit 105, A switching unit 106 that validates the calculation result by the constraint control calculation unit 108 and invalidates the calculation result by the constraint control calculation unit 108 when the proximity determination unit 105 determines that it is not approaching the constraint target, and a driven control calculation unit A combining unit 110 that combines the calculation result by 103 and the restriction result by the constraint control limiting unit 109, and a drive control unit 111 that drives the arm 2 based on the combining result by the combining unit 110 are provided.
  • the direct teaching device 1 according to the first embodiment can be switched between normal direct teaching and restricted direct teaching while the operator is operating the arm 2.
  • FIG. 1 the arm 2 is a vertical articulated robot arm, and the restraint method by the direct teaching device 1 is limited to only posture restraint.
  • the configuration example of the direct teaching apparatus 1 according to the second embodiment is the same as the configuration example of the direct teaching apparatus 1 according to the first embodiment, and will be described below using the configuration example shown in FIG.
  • the position / orientation calculation unit 104 calculates the attitude of the arm 2. Further, the proximity determination unit 105 according to the second embodiment determines whether the posture of the arm 2 is close to the restraining direction 505 based on the calculation result by the position / orientation calculation unit 104.
  • the direct teaching device 1 performs posture restraint such that the direction 502 (Z axis) of the end effector 201 is restrained in the direct downward direction 507 or the direct lateral direction 509. Further, it represents a unit vector of each axis of the robot coordinate system e X, e Y, in e Z. Further, it is assumed that the arm 2 has six axes.
  • the switching unit 106 switches the operations of the target value calculation unit 107 and the constraint control calculation unit 108.
  • the driven control calculation unit 103 calculates each joint angle ⁇ of the arm 2 as a driven control command value
  • the constraint control calculation unit 108 calculates each joint angular velocity ⁇ (dot) of the arm 2 as a constraint control command value. To do.
  • the position / orientation measurement unit 101 measures each joint angle ⁇ of the arm 2 (step ST801).
  • the position / 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.
  • the external force detection unit 102 detects the external force applied to the arm 2 by the operator (step ST802).
  • the processing by the external force detection unit 102 is the same as that in the first embodiment. It is assumed that the external force detection unit 102 performs gravity compensation.
  • driven control calculation unit 103 calculates each joint angle ⁇ (driven control command value) of arm 2 according to the external force detected by external force detection unit 102 based on the measurement result by position and orientation measurement unit 101 (step ST803). ). 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. 9A.
  • 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) .
  • the gain K T may be different or may be identical for each joint of the arm 2.
  • 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).
  • the follower control calculation unit 103 by applying a control period T S in joint angular velocity to the target theta (dots), for each joint of the arm 2, and calculates the joint angle variation ⁇ per one control period.
  • 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. To do.
  • the driven control calculating part 103 can calculate each joint angle (theta) of the arm 2 from the force F detected by the external force detection part 102, for example by the method shown by FIG. 9B.
  • the driven control calculation unit 103 first calculates the moving speed for each position and posture of the arm 2 by multiplying the force F detected by the external force detection unit 102 by the gain K F.
  • the gain K F may be the same or different for each component of the position and posture of the arm 2.
  • the driven control calculation unit 103 calculates a target joint angular velocity ⁇ (dot) for each joint of the arm 2 by multiplying the moving speed by the inverse matrix of the Jacobian matrix J.
  • the driven control calculation unit 103 multiplies the pseudo inverse matrix when the inverse matrix of the Jacobian matrix J does not exist.
  • 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).
  • the follower control calculation unit 103 by applying a control period T S in joint angular velocity to the target theta (dots), for each joint of the arm 2, and calculates the joint angle variation ⁇ per one control period.
  • 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. To do.
  • the position / orientation calculation unit 104 calculates the attitude of the arm 2 based on the calculation result by the driven control calculation unit 103 (step ST804).
  • the position / orientation calculation unit 104 can execute the calculation by forward kinematics.
  • the posture of the arm 2 calculated by the position / posture calculation unit 104 is represented by T by a rotation matrix.
  • a result obtained by decomposing T into a column vector or an element is represented by the following equation (3).
  • the proximity determining unit 105 determines whether the posture of the arm 2 is approaching the restraining direction 505 based on the calculation result by the position / orientation calculating unit 104 (step ST805).
  • the restraining direction 505 is the direct downward direction 507 or the direct lateral direction 509.
  • the approach determination threshold is assumed to be 0.1 [rad]. An operation example of the proximity determination unit 105 will be described with reference to FIGS.
  • the proximity determining unit 105 first calculates an angle ⁇ formed by the directly upward direction (reference direction) 508 and the direction 502 (Z axis) of the end effector 201. (Step ST1001). 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.
  • proximity determination section 105 determines whether angle ⁇ is within the range of ( ⁇ / 2 ⁇ 0.1) [rad] to ( ⁇ / 2 + 0.1) [rad] (step ST1002). In step ST1002, the proximity determination unit 105 determines that the angle ⁇ is within the range of ( ⁇ / 2 ⁇ 0.1) [rad] to ( ⁇ / 2 + 0.1) [rad] (for example, FIG. 11A). ), It is determined that the posture of the arm 2 is approaching the lateral direction 509 (step ST1003).
  • step ST1004 when the proximity determination unit 105 determines in step ST1002 that the angle ⁇ is not within the range of ( ⁇ / 2 ⁇ 0.1) [rad] to ( ⁇ / 2 + 0.1) [rad], It is determined whether the angle ⁇ is equal to or greater than ( ⁇ 0.1) [rad] (step ST1004).
  • step ST1004 when the proximity determination unit 105 determines that the angle ⁇ is equal to or larger than ( ⁇ 0.1) [rad] (for example, in the case of FIG. 11B), the posture of the arm 2 is set to the directly downward direction 507. It determines with approaching (step ST1005).
  • step ST1004 when the proximity determination unit 105 determines in step ST1004 that the angle ⁇ is not equal to or greater than ( ⁇ 0.1) [rad] (for example, in the case of FIG. 11C), the restraint direction in which the posture of the arm 2 is It determines with not approaching 505 (step ST1006).
  • switching section 106 switches the operation of target value calculation section 107 and constraint control calculation section 108 according to the determination result by proximity determination section 105 (step ST806).
  • the switching unit 106 sets the target posture in the lateral direction 509 as a target value to the target value calculation unit 107.
  • the calculation by the constraint control calculation unit 108 is validated.
  • the switching unit 106 calculates a target posture in the direct downward direction 507 as a target value to the target value calculation unit 107.
  • the processing by the constraint control calculation unit 108 is validated.
  • the switching unit 106 determines that the posture of the arm 2 is not approaching any constraint direction 505
  • the switching unit 106 does not require calculation by the target value calculation unit 107, and the constraint control calculation unit 108.
  • the processing by is invalidated.
  • the target value calculation unit 107 calculates a target posture based on the calculation result by the constraint target and position / posture calculation unit 104 (step ST807).
  • the target value calculation unit 107 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 for calculating the target posture by the target value calculation unit 107 will be described below.
  • the target value calculation unit 107 calculates a target angle theta * against e Z.
  • the target posture is the lateral direction 509
  • the target angle ⁇ * is ⁇ / 2
  • the target angle ⁇ * is ⁇ .
  • the target value calculation unit 107 based on the attitude of the arm 2, the rotation angle theta R calculates the smallest rotary shaft 510.
  • the rotary shaft 510 is a shaft perpendicular to and t Z is on the XY plane of the robot coordinate system, i.e., an axis orthogonal to both e Z and t Z. Therefore, the target value calculation portion 107 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 107 in the case of parallel and e Z and t Z can not be determined uniquely r. However, this is a case where the posture of the arm 2 is facing the upward direction 508 or the downward direction 507, and the target value calculation unit 107 calculates an arbitrary vector on the XY plane as the rotation axis 510.
  • the target value calculation unit 107 based on the target angle theta * and the rotary shaft 510 is calculated, to calculate the rotation angle theta R as the target position.
  • the magnitude of the rotation angle theta R is a theta * - [theta], the rotation as shown in FIG. 13 orientation It is necessary to consider (positive and negative). This is because the direct teaching device 1 according to the second embodiment uses the inner product when obtaining the angle ⁇ , and therefore the information on the orientation 502 of the end effector 201 is lost.
  • the rotation angle ⁇ R is expressed by the following equation (5).
  • the direction of rotation does not need to be reversed when e Z ⁇ t Z and r are in the same direction, and when e Z ⁇ t Z and r are in opposite directions, Inversion is necessary.
  • the target value calculation unit 107 is obtained by applying the rotation described above to the posture of the arm 2.
  • the target value calculation unit 107 calculates the target posture in which the direction 502 (Z axis) of the end effector 201 faces the downward direction 507 or the lateral direction 509 has been described.
  • the target value calculation unit 107 can further rotate from here to obtain a desired posture and set it as the target posture.
  • the target value calculation unit 107 can set the posture shown in FIG. 7 as the target posture by rotating the arm 2 around the Z-axis from the state in which the arm 2 faces the downward direction 507. Further, as shown in FIG.
  • the target value calculation unit 107 further rotates around the Z axis from the state in which the arm 2 faces the lateral direction 509, and t X or t Y is parallel to the Z axis of the robot coordinate system. It is also possible to set the posture to be the target posture.
  • the constraint control calculation unit 108 calculates the movement (constraint control command value) of the arm 2 that moves to the target posture calculated by the target value calculation unit 107 based on the calculation result by the position / orientation calculation unit 104 (step) ST808). 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 107 calculates the target posture, the constraint control calculation unit 108 does not change the arm in that way. 2. Calculate the movement of rotating 2. However, since the output of the constraint control calculation unit 108 needs to be the joint angular velocity ⁇ (dot), the constraint control calculation unit 108 determines that the rotation axis 510 calculated by the target value calculation unit 107 is r and the rotation angle ⁇ R. It is necessary to calculate the joint angular velocity ⁇ (dot) from this set. An example of the calculation is shown below.
  • the constraint control calculation unit 108 first determines the rotational angular velocity magnitude
  • a theta R. Next, the constraint control calculation unit 108 calculates a Jacobian matrix J at the current position and orientation of the arm 2. It is assumed that the following equation (6) holds for the Jacobian matrix J.
  • v X , v Y and v Z are the velocities in the X, Y and Z axis directions of the tip of the end effector 201
  • ⁇ X , ⁇ Y and ⁇ Z are the postures of the arm 2 in the X axis, Y axis
  • the angular velocities rotate about the Z axis
  • ⁇ 1 (dot) to ⁇ 6 (dots) are the joint angular velocities.
  • the constraint control calculation unit 108 calculates joint angular velocities ⁇ 1 (dots) to ⁇ 6 (dots) for each joint of the arm 2 that satisfy the following equation (7).
  • the constraint control calculation unit 108 calculates joint angular velocities ⁇ 1 (dots) to ⁇ 6 (dots) using a pseudo inverse matrix or the like when no inverse matrix exists in the Jacobian matrix J.
  • constraint control limiting section 109 limits the calculation result by constraint control calculation section 108 (step ST809).
  • the constraint control limiting unit 109 determines an upper limit ⁇ MAX (dot) for the joint angular velocity ⁇ (dot) for each joint of the arm 2 calculated by the constraint control calculation unit 108.
  • the upper limit ⁇ MAX (dot) may be the same or different for each joint.
  • the constraint control limiting unit 109 needs to make the upper limit ⁇ MAX (dot) smaller than the upper limit of the limiter.
  • the constraint control restriction unit 109 sets the upper limit ⁇ MAX (dot) to half or less than the upper limit of the limiter. If limited to that level, it is relatively easy to remove the constraint by an operation by the operator.
  • the constraint control restriction unit 109 restricts all the joint angular velocities ⁇ (dots) so as not to exceed the upper limit ⁇ MAX (dots). At this time, it is preferable to limit the direction of rotation so as not to change.
  • the upper limit of the i-th joint angular velocity ⁇ i (dot) is represented by ⁇ i, MAX (dot).
  • the constraint control limiting unit 109 sets the ratio c i between the magnitude of the joint angular velocity ⁇ i (dot) and the upper limit ⁇ i, MAX (dot) to
  • restraining control restriction unit 109 calculates the maximum value c of this ratio c i.
  • the constraint control restriction unit 109 calculates the joint angular velocity ⁇ i * (dot) after the restriction by dividing the joint angular velocity ⁇ i (dot) by the maximum value c for each joint of the arm 2. That is, the joint angular velocity after restriction ⁇ i * (dot) is ⁇ i (dot) / c.
  • the combining unit 110 combines 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 limiting unit 109.
  • the joint angle command value that is the output of the combining unit 110 is ⁇ + T S ⁇ (dot) from the control cycle T S.
  • m is the number of joints of the arm 2 which the drive control part 111 controls.
  • the drive control unit 111 drives the arm 2 based on the synthesis result (joint angle command value) by the synthesis unit 110 (step ST811).
  • Embodiment 3 In the direct teaching device 1 according to the second embodiment, the case where the restraint method is limited to only the posture restraint is shown.
  • Axial constraint means that the position constraint for constraining the position of the arm 2 to a predetermined constraint shaft 504 and the posture constraint for constraining the posture of the arm 2 in a predetermined constraint direction 505 are performed simultaneously. That is, the direct teaching device 1 according to the third embodiment performs position constraint in addition to the posture constraint described in the second embodiment.
  • the direct teaching apparatus 1 performs position constraint on the straight line 511 parallel to the Z axis of the robot coordinate system and posture constraint in the direct downward direction 507 as shown in FIG. 15 will be described.
  • 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.
  • the position and orientation calculation unit 104 in the third embodiment calculates the position and orientation of the arm 2. Further, the proximity determination unit 105 according to the third embodiment determines whether the position of the arm 2 is approaching the constraint axis 504 that is a constraint target based on the calculation result by the position / orientation calculation unit 104 and the posture of the arm 2 is It is determined whether the position and orientation of the arm 2 is approaching the constraint target by determining whether the constraint direction is approaching the constraint direction 505.
  • FIG. 16 is a flowchart showing an operation example of the direct teaching apparatus 1 according to the third embodiment.
  • the direct teaching apparatus 1 according to the third embodiment shown in FIG. 16 differs from the direct teaching apparatus 1 according to the second embodiment shown in FIG. 8 in that the position / orientation calculation unit 104, the proximity determination unit 105, and the target value are different.
  • the calculation unit 107 and the constraint control calculation unit 108, and only operation examples of these configurations will be described.
  • the position / 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 (step ST1604).
  • the position and posture calculation unit 104 needs to calculate both the position and the posture of the arm 2.
  • the position / orientation calculation unit 104 can execute the calculation by forward kinematics.
  • the proximity determination unit 105 determines whether the position of the arm 2 is approaching the restraining axis 504 and whether the posture of the arm 2 is approaching the restraining direction 505 based on the calculation result by the position and orientation calculation unit 104, 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 ST1605). For example, when the proximity determination unit 105 determines that the position of the arm 2 is approaching the restraint axis 504 and the posture of the arm 2 is approaching the restraint direction 505, the position and orientation of the arm 2 is approaching the restraint target. Is determined.
  • the determination with respect to the restraining direction 505 by the proximity determining unit 105 has been described in the second embodiment, and will be omitted.
  • the determination with respect to the constraint axis 504 by the proximity determining unit 105 will be described.
  • the proximity determination unit 105 lowers the vertical line 512 from the position 501 of the end effector 201 to the restraining shaft 504, and if the length of the vertical line 512 is smaller than a predetermined value, the arm 2 Is determined to be approaching the constraining shaft 504.
  • 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.
  • the length of the vertical line 512 is represented by the following formula (8).
  • the proximity determination unit 105 determines that the position of the arm 2 is approaching the restraint axis 504 if the length of the perpendicular line 512 shown in Expression (8) is smaller than a predetermined value, and otherwise. It is determined that the position of the arm 2 has not approached the restraint shaft 504.
  • the proximity determination unit 105 can determine the approach from the length of the perpendicular line with respect to the constraint axis 504 in the same manner even when the constraint axis 504 is not parallel to the Z axis of the robot coordinate system.
  • the target value calculation unit 107 calculates a target position when the position of the arm 2 is moved to the constraint shaft 504 (step ST1607). For example, the target value calculation unit 107 sets a position 513 where the perpendicular line 512 is lowered from the position 501 of the end effector 201 to the constraint shaft 504 as shown in FIG.
  • the target position is [x * , y * , z].
  • the constraint control calculation unit 108 calculates the joint angular velocity ⁇ (dot) for each joint of the arm 2 using the Jacobian matrix J as in the second embodiment (step ST1608).
  • the constraint control calculation unit 108 according to the third embodiment also performs position constraint in addition to posture constraint, the calculation procedure changes as follows with respect to the second embodiment.
  • the constraint control calculation unit 108 determines the magnitude of rotation angular velocity
  • the constraint control calculation unit 108 calculates a Jacobian matrix J at the current position and orientation of the arm 2.
  • the operations so far are the same as those in the second embodiment.
  • the constraint control calculation unit 108 determines the speed
  • ⁇ ⁇ (xx * ) 2 + (y ** ) 2 ⁇ . In addition, the constraint control calculation unit 108 may determine a certain constant v * to be
  • the constraint control calculation unit 108 calculates the joint angular velocity ⁇ (dot) for each joint of the arm 2 from the following equation (9).
  • Embodiment 4 FIG.
  • the direct teaching device 1 according to the second embodiment the case where the restraint method is limited to only the posture restraint is shown.
  • the direct teaching device 1 according to the fourth embodiment a case where the restraining method is limited to only the surface restraint is shown.
  • the surface constraint means that the position constraint for constraining the position of the arm 2 to a predetermined constraint surface 503 and the posture constraint for constraining the posture of the arm 2 in a predetermined constraint direction 505 are performed simultaneously. That is, the direct teaching apparatus 1 according to the fourth embodiment performs position constraint in addition to the posture constraint illustrated in the second embodiment.
  • the restraint surface 503 and the restraint direction 505 can be arbitrarily set.
  • the same method can be applied to the case of being represented.
  • the configuration example of the direct teaching apparatus 1 according to the fourth embodiment is the same as the configuration example of the direct teaching apparatus 1 according to the second embodiment, and will be described below using the configuration example shown in FIG.
  • the position and orientation calculation unit 104 calculates the position and orientation of the arm 2. Further, the proximity determination unit 105 according to the fourth embodiment determines whether the position of the arm 2 is approaching the constraint surface 503 that is the constraint target based on the calculation result by the position / orientation calculation unit 104 and the posture of the arm 2 is It is determined whether the position and orientation of the arm 2 is approaching the constraint target by determining whether the constraint direction is approaching the constraint direction 505.
  • FIG. 19 is a flowchart showing an operation example of the direct teaching apparatus 1 according to the fourth embodiment.
  • the direct teaching apparatus 1 according to the fourth embodiment shown in FIG. 19 differs in operation from the direct teaching apparatus 1 according to the third embodiment shown in FIG. 16 in that the proximity determining unit 105, the target value calculating unit 107, and the constraint control. Only the operation example of these configurations will be described.
  • the proximity determination unit 105 determines whether the position of the arm 2 is approaching the restraint surface 503 and whether the posture of the arm 2 is approaching the restraint direction 505 based on the calculation result by the position / orientation calculation unit 104, 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 approaches the restraint surface 503 and the posture of the arm 2 approaches the restraint direction 505, the position and orientation of the arm 2 approaches the restraint target. Is determined. The determination with respect to the restraining direction 505 by the proximity determination unit 105 has been described in the second embodiment, and will be omitted. Hereinafter, the determination with respect to the restraint surface 503 by the proximity determination unit 105 will be described.
  • the proximity determination unit 105 lowers a straight line parallel to the Z axis of the robot coordinate system from the position 501 of the end effector 201 to the restraint surface 503, and if the length of the straight line is smaller than a predetermined value, It is determined that the position is approaching the restraint 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 the predetermined value, the proximity determination unit 105 determines that the position of the arm 2 is approaching the restraint surface 503, and otherwise, the position of the arm 2 is the restraint surface 503. It is determined that it is not approaching.
  • the target value calculation unit 107 calculates a target position when the position of the arm 2 is moved to the restraint surface 503 in addition to the calculation of the target posture described in the second embodiment (step ST1907). For example, when the coordinates of the position of the arm 2 are [x, y, z], the target value calculation unit 107 replaces only the Z-axis coordinates with the coordinates on the curved surface [x, y, f (x, y)]. ] Is the target position.
  • the constraint control calculation unit 108 calculates the joint angular velocity ⁇ (dot) for each joint of the arm 2 using the Jacobian matrix J as in the second embodiment (step ST1908).
  • the constraint control calculation unit 108 according to the fourth embodiment also performs position constraint in addition to posture constraint, the calculation procedure changes as follows with respect to the second embodiment.
  • the constraint control calculation unit 108 determines the magnitude of rotation angular velocity
  • the constraint control calculation unit 108 calculates a Jacobian matrix J at the current position and orientation of the arm 2.
  • the operations so far are the same as those in the second embodiment.
  • the constraint control calculation unit 108 determines the speed
  • the direct teaching device is configured to be able to switch between normal direct teaching and restrained direct teaching while the operator is operating the arm, the robot can directly perform work on an industrial robot or the like. Suitable for use in the method taught.

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Manufacturing & Machinery (AREA)
  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

外力を検知する外力検知部（１０２）と、外力に従うアーム（２）の動きを算出する従動制御演算部（１０３）と、アーム（２）の位置姿勢を算出する位置姿勢演算部（１０４）と、アーム（２）の位置姿勢が拘束目標に近づいているかを判定する近接判定部（１０５）と、拘束制御の目標値を算出する目標値算出部（１０７）と、目標値に移動するアーム（２）の動きを算出する拘束制御演算部（１０８）と、拘束制御演算部（１０８）による算出結果を制限する拘束制御制限部（１０９）と、拘束目標に近づいていると判定された場合に拘束制御演算部（１０８）を有効とし、拘束目標に近づいていないと判定された場合に拘束制御演算部（１０８）を無効とする切換部（１０６）と、従動制御演算部（１０３）による算出結果と拘束制御制限部（１０９）による制限結果とを合成する合成部（１１０）と、合成結果に基づいてアーム（２）を駆動する駆動制御部（１１１）とを備えた。

Description

ロボットの直接教示装置及び直接教示方法
　この発明は、ロボットの直接教示を行う直接教示装置及び直接教示方法に関する。
　産業用のロボットでは、ロボットに作業をさせるために、前もって教示（ティーチング）と呼ばれる作業が実施される。このロボットの教示を行う方法の１つとして、直接教示（ダイレクト教示）と呼ばれる方法がある。
　例えば特許文献１では、力センサを用いたロボットの直接教示方法が開示されている。また、例えば特許文献２では、トルク検出手段を用いたロボットの直接教示方法が開示されている。これらの特許文献で開示されている直接教示装置の概略構成を図２０に示す。
　図２０に示す直接教示装置１１では、まず、外力検知部１１０１が、力センサ又はトルクセンサ等を用いて、ロボットが有するアーム２に対して操作者により加えられた外力を検知する。次いで、従動制御演算部１１０２が、外力検知部１１０１により検知された外力に従うアームの動きを算出（従動制御演算）する。なお、従動制御演算部１１０２は、従動制御演算において、位置姿勢計測部（不図示）により計測されたアームの位置姿勢に関するパラメータを用いる場合もある。なお、アームの位置姿勢とは、アームの位置及びアームの姿勢のうちの少なくとも一方を意味する。また、上記パラメータとしては、アームの位置、アームの姿勢、又は、アームの関節角等が挙げられる。そして、従動制御演算部１１０２は、従動制御演算の結果に基づいて更新した従動制御指令値を駆動制御部１１０３に通知する。次いで、駆動制御部１１０３は、従動制御演算部１１０２により通知された従動制御指令値に従ってアーム２を駆動する。
　この一連の動作により、図２０に示す直接教示装置１１は、操作者により加えられた外力に従ってアーム２が動くように制御できる。そして、直接教示装置１１は、制御によりアーム２の位置及び姿勢が操作者の意図する状態となった場合に、その際の上記パラメータを教示点として記録する。この直接教示装置１１により記録された教示点はロボットが作業する際に使用される。
　このように、ロボットの直接教示は、操作者がアームを直接操作して位置及び姿勢を教示するため、操作者にとって直観的でわかりやすいという利点を持つ。一方で、アームが操作者により加えられた外力に従ってそのまま動くという特徴は利点ばかりではない。
　例えば、操作者が、ロボットに対して、ある点Ｐの位置を教示した後に、その真下の別の点Ｑの位置を教示するというケースを考える。このケースでは、操作者は、真下方向（Ｚ軸方向）にアームを動かすことになる。しかしながら、操作者がアームに対して正確に真下方向へ外力を加えることは難しく、アームのＸ軸座標及びＹ軸座標がずれることが多い。後からアームのＸ軸座標及びＹ軸座標だけをティーチングペンダント等を用いて修正することは可能であるが、それでは直接教示の利点が減ずる。
　また、例えば、操作者が、アームの先端に設けられたエンドエフェクタを真下に向けたまま教示を行うケースを考える。このケースにおいても、操作者がアームに対して正確に真下に向けた状態を維持したままアームを直接操作することは難しく、アームの姿勢がずれることが多い。
　以上のように、直接教示は、直観的で分かりやすい反面、操作が難しいケースも存在する。
　これに対し、例えば特許文献３，４では、上記のような課題を解決する技術が開示されている。
　特許文献３では、モード切換スイッチにより、動作モードを、位置方向移動モード、位置移動モード又は方向移動モードに切換え可能とする直接教示装置が開示されている。位置移動モードでは、アームの先端の姿勢を維持したまま位置のみを移動可能としている。また、方向移動モードでは、アームの先端の位置を維持したまま姿勢のみを変更可能としている。この直接教示装置により、上記のような直接教示における操作の難しさをある程度緩和可能となる。なお、通常の直接教示は位置方向移動モードで実現可能である。
　また、特許文献４では、入力装置により、動作モードを、拘束モード又は全方向移動モードに切換え可能とする直接教示装置が開示されている。拘束モードでは、エンドエフェクタの先端を特定の拘束軸又は拘束面に沿って移動可能としている。また、全方向移動モードでは、通常の直接教示を実施可能としている。この直接教示装置は、直接教示の途中で拘束モードに切換えることにより、アームの先端を正確に上下方向に動かすことが可能となり、直接教示における操作の難しさが軽減される。
特開平０５－２０４４４１号公報 特開平０５－２５００２９号公報 特開平０５－２８５８７０号公報 特開平０５－３０３４２５号公報
　上記のように、特許文献３，４に開示された直接教示装置は、アームの操作に拘束を課さない通常の直接教示を行うモードと、アームの操作に拘束を課す拘束付き直接教示を行うモードとを切換える。そして、特許文献３ではモード切換スイッチによって、また、特許文献４では入力装置によって、モードの切換えが行われており、操作者が手でスイッチ等を操作することで切換え可能となっている。
　しかしながら、直接教示では、操作者は手でアームを直接操作しているため、モードの切換えを手で操作するのは便利ではない。例えば、操作者が両手でアームを操作している場合、モードの切換えを操作する際にはアームの操作を一旦停止してアームを片手で保持することになる。通常時に両手で保持しているアームを片手で保持すると、アームの位置姿勢がずれることも考えられる。その結果、拘束付き直接教示の効果を減ずることにもなりかねない。
　この発明は、上記のような課題を解決するためになされたもので、操作者がアームを操作している状態のまま通常の直接教示と拘束付き直接教示を切換える可能な直接教示装置を提供することを目的としている。
　この発明に係る直接教示装置は、ロボットが有するアームに加えられた外力を検知する外力検知部と、外力検知部により検知された外力に従うアームの動きを算出する従動制御演算部と、アームの位置又は姿勢のうちの少なくとも一方である位置姿勢を算出する位置姿勢演算部と、位置姿勢演算部による算出結果に基づいて、アームの位置姿勢が、当該アームの位置姿勢の拘束先である拘束目標に近づいているかを判定する近接判定部と、拘束目標及び位置姿勢演算部による算出結果に基づいて、拘束制御の目標値を算出する目標値算出部と、位置姿勢演算部による算出結果に基づいて、目標値算出部により算出された目標値に移動するアームの動きを算出する拘束制御演算部と、拘束制御演算部による算出結果を制限する拘束制御制限部と、近接判定部により拘束目標に近づいていると判定された場合に拘束制御演算部による算出結果を有効とし、近接判定部により拘束目標に近づいていないと判定された場合に拘束制御演算部による算出結果を無効とする切換部と、従動制御演算部による算出結果と拘束制御制限部による制限結果とを合成する合成部と、合成部による合成結果に基づいてアームを駆動する駆動制御部とを備えたことを特徴とする。
　この発明によれば、上記のように構成したので、操作者がアームを操作している状態のまま通常の直接教示と拘束付き直接教示を切換え可能となる。
この発明の実施の形態１に係る直接教示装置の構成例を示す図である。 図２Ａ～図２Ｄは、この発明の実施の形態１に係る直接教示装置による通常の直接教示と拘束付き直接教示（位置拘束の場合）の切換えの一例を示す図である。 図３Ａ～図３Ｄは、この発明の実施の形態１に係る直接教示装置による通常の直接教示と拘束付き直接教示（姿勢拘束の場合）の切換えの一例を示す図である。 この発明の実施の形態１に係る直接教示装置の動作例を示すフローチャートである。 この発明の実施の形態１における近接判定部の動作例を説明する図である（姿勢拘束の場合）。 この発明の実施の形態１における目標値算出部の動作例を説明する図である（姿勢拘束の場合）。 この発明の実施の形態１における目標値算出部の動作例を説明する図である（姿勢拘束の場合）。 この発明の実施の形態２に係る直接教示装置の動作例を示すフローチャートである。 図９Ａ、図９Ｂは、この発明の実施の形態２における従動制御演算部の動作例を示す図である。 この発明の実施の形態２における近接判定部の動作例を示すフローチャートである。 図１１Ａ、図１１Ｂ、図１１Ｃは、この発明の実施の形態２における近接判定部の動作例を説明する図である。 図１２Ａ、図１２Ｂは、この発明の実施の形態２における目標値算出部の動作例を説明する図であり、回転軸の算出例を示す図である。 図１３Ａ、図１３Ｂは、この発明の実施の形態２における目標値算出部の動作例を説明する図であり、回転方向の算出例を示す図である。 この発明の実施の形態２における目標値算出部の動作例を説明する図である。 この発明の実施の形態３に係る直接教示装置による軸拘束を説明する図である。 この発明の実施の形態３に係る直接教示装置の動作例を示すフローチャートである。 この発明の実施の形態３における近接判定部の動作例を説明する図である。 この発明の実施の形態４に係る直接教示装置による面拘束を説明する図である。 この発明の実施の形態４に係る直接教示装置の動作例を示すフローチャートである。 従来の直接教示装置の構成例を示す図である。
　以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
　図１はこの発明の実施の形態１に係る直接教示装置１の構成例を示す図である。
　直接教示装置１は、ロボットの直接教示を行う。この直接教示装置１は、直接教示方法として、ロボットが有するアーム２の位置姿勢に対して拘束を課さない通常の直接教示と、アーム２の位置姿勢を拘束目標に拘束する拘束付き直接教示とを切換え可能である。
　なお、アーム２の位置姿勢とは、アーム２の位置及びアーム２の姿勢のうちの少なくとも一方を意味する。また、アーム２の位置とは、アーム２の先端に設けられたエンドエフェクタ２０１の位置５０１を意味し、アーム２の姿勢とはエンドエフェクタ２０１の向き５０２を意味する。また、拘束目標とは、アーム２の位置姿勢の拘束先である。アーム２の位置に対する拘束目標としては、拘束面５０３及び拘束軸５０４がある。拘束面５０３の種別としては、平面及び曲面がある。拘束軸５０４の種別としては、直線及び曲線がある。アーム２の姿勢に対する拘束目標としては、拘束方向５０５がある。また、直接教示装置１で用いる拘束目標は１つに限らず複数でもよい。また、ロボットに対しては基準となるロボット座標系が設定され、エンドエフェクタ２０１に対してはツール座標系が設定される（図６等参照）。
　この直接教示装置１は、図１に示すように、位置姿勢計測部１０１、外力検知部１０２、従動制御演算部１０３、位置姿勢演算部１０４、近接判定部１０５、切換部１０６、目標値算出部１０７、拘束制御演算部１０８、拘束制御制限部１０９、合成部１１０及び駆動制御部１１１を備えている。なお、従動制御演算部１０３、位置姿勢演算部１０４、近接判定部１０５、切換部１０６、目標値算出部１０７、拘束制御演算部１０８、拘束制御制限部１０９、合成部１１０及び駆動制御部１１１は、システムＬＳＩ（Ｌａｒｇｅ－Ｓｃａｌｅ　Ｉｎｔｅｇｒａｔｉｏｎ）等の処理回路、又はメモリ等に記憶されたプログラムを実行するＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）等により実現される。
　位置姿勢計測部１０１は、アーム２の位置姿勢に関するパラメータを計測する。なお、アーム２の位置姿勢に関するパラメータとしては、アーム２の位置、アーム２の姿勢、又は、アーム２の関節角θ等が挙げられる。
　外力検知部１０２は、操作者によりアーム２に加えられた外力を検知する。例えば、外力検知部１０２は、アーム２の先端に取付けられた力センサを用い、この力センサにより計測された力Ｆを上記外力として検知してもよい。また、例えば、外力検知部１０２は、アーム２のモータ駆動軸に取付けられたトルクセンサを用い、このトルクセンサにより計測されたトルクτを上記外力として検知してもよい。また、外力検知部１０２は、上記のようにセンサを用いて外力を直接計測するのではなく、アーム２が有するモータの電流又はアーム２の関節角θの計測値から間接的に外力を算出する外力オブザーバを用いて上記外力を検知してもよい。
　従動制御演算部１０３は、位置姿勢計測部１０１による計測結果に基づいて、外力検知部１０２により検知された外力に従うアーム２の動き（従動制御指令値）を算出する。
　なお、位置姿勢計測部１０１、外力検知部１０２及び従動制御演算部１０３は、拘束が無い通常の直接教示で用いられる構成と同様であり、公知技術である。
　位置姿勢演算部１０４は、従動制御演算部１０３による算出結果に基づいて、アーム２の位置姿勢を算出する。
　なお図１では、従動制御演算部１０３が位置姿勢計測部１０１による計測結果を用い、位置姿勢演算部１０４が従動制御演算部１０３による算出結果を用いている。
　しかしながら、これに限らず、従動制御演算部１０３は、位置姿勢計測部１０１による計測結果は用いず、外力検知部１０２により検知された外力に従うアーム２の動きを演算してもよい。この場合、位置姿勢演算部１０４は、位置姿勢計測部１０１による計測結果に基づいてアーム２の位置姿勢を算出する。
　また、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果及び位置姿勢計測部１０１による計測結果の両方を用いてアーム２の位置姿勢を算出してもよい。
　近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置姿勢が拘束目標に近づいているかを判定する。
　切換部１０６は、近接判定部１０５による判定結果に従い、拘束制御演算部１０８の動作を切換える。すなわち、切換部１０６は、近接判定部１０５によりアーム２の位置姿勢が拘束目標に近づいていると判定された場合には、拘束制御演算部１０８による処理を有効とする。一方、切換部１０６は、近接判定部１０５によりアーム２の位置姿勢が拘束目標に近づいていないと判定された場合には、拘束制御演算部１０８による処理を無効とする。
　また、切換部１０６は、拘束目標が複数存在する場合に、近接判定部１０５による判定結果に従い、目標値算出部１０７による動作（対象とする拘束目標）の切換えも実施してもよい。
　目標値算出部１０７は、拘束目標及び位置姿勢演算部１０４による算出結果に基づいて、拘束制御の目標値を算出する。なお、拘束制御の目標値とは、アーム２の目標位置及びアーム２の目標姿勢のうちの少なくとも一方を意味する。
　拘束制御演算部１０８は、位置姿勢演算部１０４による算出結果に基づいて、目標値算出部１０７により算出された目標値に移動するアーム２の動き（拘束制御指令値）を算出する。
　拘束制御制限部１０９は、拘束制御演算部１０８による算出結果を制限する。すなわち、拘束制御制限部１０９は、従動制御演算部１０３による算出結果が拘束制御演算部１０８による算出結果より優先されるよう拘束制御演算部１０８による算出結果に制限を課す。
　なお図１では、拘束制御演算部１０８と拘束制御制限部１０９を別々の構成要素として示しているが、一体として実装することも可能である。
　合成部１１０は、従動制御演算部１０３による算出結果（従動制御指令値）と拘束制御制限部１０９による制限結果（拘束制御指令値）とを一つの指令値に合成する。
　駆動制御部１１１は、合成部１１０による合成結果に基づいて、アーム２を駆動する。
　実施の形態１に係る直接教示装置１では、通常の直接教示と拘束付きの直接教示の切換えを、操作者によるアーム２の操作により実行可能としている。この切換え方法は、以下に示す２つの仕組みによって実現される。
　１つ目は、直接教示装置１が、アーム２の位置姿勢が拘束目標に近づいた際に、自動的に通常の直接教示から拘束付きの直接教示に切換える仕組みである。例えば図２Ａ，２Ｂに示すように、アーム２の位置を所定の平面５０６（拘束面５０３）に拘束させたい場合、直接教示装置１は、その平面５０６からアーム２の位置までの距離が例えば±５０［ｍｍ］以下である場合には拘束付き直接教示を行い、それ以外の場合には通常の直接教示を行う。また、例えば図３Ａ，３Ｂに示すように、アーム２の姿勢を真下方向５０７（拘束方向５０５）に拘束させたい場合、直接教示装置１は、アーム２の姿勢が真下方向５０７から例えば±０．２［ｒａｄ］以下である場合には拘束付き直接教示を行い、それ以外の場合には通常の直接教示を行う。この仕組みにより、直接教示装置１は、操作者によるアーム２の操作のみで、通常の直接教示から拘束付き直接教示への切換えを実現できる。
　２つ目は、直接教示装置１が、拘束付き直接教示の際に拘束する力を敢えて抑制することで、操作者が拘束制御に打ち勝ってアーム２を操作可能とする仕組みである。例えば、直接教示装置１は、拘束制御における関節角速度θ（ドット）を例えば０．１［ｒａｄ／ｓ］以下に抑制し、操作者がそれ以上の外力を加えることでアーム２を操作可能とする。これにより、操作者は、アーム２を、拘束された状態から少しずつ逸脱させることができ、最終的には拘束付き直接教示の実行条件から外すことができる。一般的な従来の直接教示装置では、拘束付き直接教示においてアーム２を拘束面等に強く拘束することでアーム２の位置姿勢を保持している。それに対し、実施の形態１に係る直接教示装置１では、拘束する力を敢えて弱めることで、図２Ｃ，２Ｄ，３Ｃ，３Ｄに示すように、操作者によるアーム２の操作のみで、拘束付き直接教示から通常の直接教示への切換えを実現できる。
　以上の２つの仕組みを組合わせることにより、実施の形態１に係る直接教示装置１は、操作者がアーム２を操作している状態のまま、通常の直接教示と拘束付き直接教示とを切換え可能となる。以下、図１に示す直接教示装置１の動作例について、図４を参照しながら説明する。
　図１に示す直接教示装置１の動作例では、図４に示すように、まず、位置姿勢計測部１０１は、アーム２の位置姿勢に関するパラメータを計測する（ステップＳＴ４０１）。
　また、外力検知部１０２は、操作者によりアーム２に加えられた外力を検知する（ステップＳＴ４０２）。なお、外力検知部１０２により検知される外力は、センサの構造等によっては、操作者によりアーム２に加えられた外力だけではなく、重力に起因する成分が重畳されている場合がある。そこで、このような場合には、外力検知部１０２は、重力に起因する成分を算出し、検知した外力から当該重力に起因する成分を差し引くことで、外力成分のみを算出するとよい。これは重力補償と呼ばれる公知技術であり、例えば特許文献５等に開示されている。
特開平０１－０６６７１５号公報
　次いで、従動制御演算部１０３は、位置姿勢計測部１０１による計測結果に基づいて、外力検知部１０２により検知された外力に従うアーム２の動き（従動制御指令値）を算出する（ステップＳＴ４０３）。この際、従動制御演算部１０３は、まず、外力検知部１０２により検知された外力の向き及び大きさから、操作者がどのようにアーム２を動かそうとしているのかを判断する。そして、従動制御演算部１０３は、その判断結果及び位置姿勢計測部１０１により計測されたアーム２の位置姿勢に基づいて、アーム２の移動量（関節角θ等の差分）又は関節角速度θ（ドット）等を算出する。なお、従動制御演算部１０３による従動制御演算方法は、例えば特許文献２等で開示されており、様々な方式が開発されているためその詳細な記述は省略する。
　次いで、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果に基づいて、アーム２の位置姿勢を算出する（ステップＳＴ４０４）。
　ここで、直接教示装置１が位置拘束を行う場合、すなわち、直接教示装置１がアーム２の位置を所定の拘束面５０３又は拘束軸５０４に拘束する場合には、位置姿勢演算部１０４は少なくともアーム２の位置を算出する必要がある。
　また、直接教示装置１が姿勢拘束を行う場合、すなわち、直接教示装置１がアーム２の姿勢を所定の拘束方向５０５に拘束する場合には、位置姿勢演算部１０４は少なくともアーム２の姿勢を算出する必要がある。
　また、直接教示装置１が位置拘束及び姿勢拘束を行う場合、すなわち、直接教示装置１がアーム２の位置及び姿勢を所定の拘束面５０３又は拘束軸５０４且つ所定の拘束方向５０５に拘束する場合には、位置姿勢演算部１０４はアーム２の位置及び姿勢を算出する必要がある。
　また、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果（最新の従動制御指令値）に基づいて上記算出を行うことが最も望ましい。これは、直接教示装置１が拘束付き直接教示を行う場合に、操作者がアーム２に対して与えた移動量のうち、拘束面５０３等から外れる成分を同時に相殺でき、拘束制御が最も有効に働くからである。
　また、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果を用いず、位置姿勢計測部１０１による計測結果に基づいて上記算出を行うことも可能である。一方、この場合には、拘束面５０３等から外れたことが計測されてから拘束制御が始まるので、拘束制御の効きが悪くなる。
　また、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果及び位置姿勢計測部１０１による計測結果の両方に基づいて上記算出を行ってもよい。
　なお、位置姿勢計測部１０１及び従動制御演算部１０３で関節角θのみが得られ、アーム２の位置姿勢を直接得られない場合、位置姿勢演算部１０４は、順運動学演算によってアーム２の位置姿勢を算出することが可能である。
　次いで、近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置姿勢が拘束目標に近づいているかを判定する（ステップＳＴ４０５）。
　例えば、拘束制御の目的がエンドエフェクタ２０１の先端のＺ軸座標を０．１［ｍ］の位置に固定することであり、また、接近判定のしきい値が０．０５［ｍ］であるとする。この場合、近接判定部１０５は、位置姿勢演算部１０４により算出されたエンドエフェクタ２０１の先端のＺ軸座標が０．０５～０．１５［ｍ］の範囲内であればアーム２の位置は拘束目標に近づいていると判定し、それ以外の場合にはアーム２の位置は拘束目標に近づいていないと判定する。
　また、拘束制御の目的がエンドエフェクタ２０１の向き５０２（Ｚ軸）を真下に向けることであり、また、接近判定のしきい値が０．１［ｒａｄ］であるとする。この場合、近接判定部１０５は、図５に示すように、真上方向（基準方向）５０８とエンドエフェクタ２０１の向き５０２との成す角度Θが（π－０．１）［ｒａｄ］以上、すなわち、真下方向５０７とエンドエフェクタ２０１の向き５０２との成す角度が０．１［ｒａｄ］未満であれば、アーム２の姿勢は拘束目標に近づいていると判定し、それ以外の場合にはアーム２の姿勢は拘束目標に近づいていないと判定する。
　また、拘束面５０３が複数存在する場合等のように拘束目標が複数存在する場合、近接判定部１０５は、拘束目標毎に上記判定を行い、各拘束目標のうちの１つでもアーム２の位置姿勢が近づいていると判定した場合には、アーム２の位置姿勢は拘束目標に近づいていると判定する。この際、更に、近接判定部１０５は、アーム２と各拘束目標との間の距離を評価し、上記判定の結果とともにアーム２に最も近い拘束目標を示す情報を切換部１０６に通知してもよい。
　次いで、切換部１０６は、近接判定部１０５による判定結果に従い、拘束制御演算部１０８の動作を切換える（ステップＳＴ４０６）。すなわち、切換部１０６は、近接判定部１０５によりアーム２の位置姿勢が拘束目標に近づいていると判定された場合には、拘束制御演算部１０８による処理を有効とする。一方、切換部１０６は、近接判定部１０５によりアーム２の位置姿勢が拘束目標に近づいていないと判定された場合には、拘束制御演算部１０８による演算を停止するか又は算出結果がアーム２の制御に反映されないようにして、拘束制御演算部１０８による処理を無効とする。
　このように、直接教示装置１は、操作者によりアーム２の位置姿勢が拘束目標に近づけられると、自動的に拘束制御を開始する。また、直接教示装置１は、後述する拘束制御制限部１０９との組合わせにより、操作者によりアーム２の位置姿勢が拘束目標から遠ざけられると、自動的に拘束制御を終了する。これにより、直接教示装置１は、操作者によりアーム２が保持された状態で拘束制御のオンオフを切換えることができる。
　なお、切換部１０６は、拘束目標が複数あって近接判定部１０５からアーム２に最も近い拘束目標を示す情報が通知された場合には、切換部１０６は、後述する目標値算出部１０７にその情報を出力して当該目標値算出部１０７の動作を切換えてもよい。
　次いで、目標値算出部１０７は、拘束目標及び位置姿勢演算部１０４による算出結果に基づいて、拘束制御の目標値を算出する（ステップＳＴ４０７）。
　例えば、拘束制御の目的がエンドエフェクタ２０１の先端のＺ軸座標を０．１［ｍ］の位置に固定することであり、位置姿勢演算部１０４により算出されたエンドエフェクタ２０１の先端の現在値が［ＸＰ，ＹＰ，ＺＰ］であるとする。この場合、目標値算出部１０７は、拘束制御の目標値を［ＸＰ，ＹＰ，０．１］とする。
　また、拘束制御の目的が、図６下段に示すようにエンドエフェクタ２０１の向き５０２のうちのＺ軸を真下方向５０７とした上でＸ軸をロボット座標系のＸ軸に合わせることであるとする。この場合、拘束制御の目標値である目標姿勢を回転行列表現で表したものは次式（１）のようになる。なお、回転行列による姿勢の表現は、例えば非特許文献１等に開示されており、その詳細については省略する。
Figure JPOXMLDOC01-appb-I000001
Ｊｏｈｎ　Ｊ．　Ｃｒａｉｇ　（三浦・下山訳）：ロボティクス，共立出版（１９９１）
　なお、目標値算出部１０７は、拘束制御の目標値として、１つの目標値のみを算出してもよいし、複数の目標値を算出してその中から１つの目標値を選択してもよい。例えば、拘束制御の目的がエンドエフェクタ２０１の先端のＺ軸座標を０．１［ｍ］刻みで最も近い位置に固定することである場合、目標値算出部１０７は、拘束制御の目標値として［ＸＰ，ＹＰ，０．１ｎ］（ｎは整数）の中で最も現在の位置に近いものを選択する。
　また、拘束制御の目的が図７に示すようにエンドエフェクタ２０１の向き５０２のうちのＺ軸を真下方向５０７とし且つこのＺ軸を回転軸としてπ／２刻み（９０度刻み）に回転させた姿勢で固定することである場合、目標値算出部１０７は、拘束制御の目標値として最も現在の姿勢に近いものを選択する。このとき、拘束制御の目標値である目標姿勢を回転行列表現で表したものは次式（２）のようになる。
Figure JPOXMLDOC01-appb-I000002
　この式（２）は図７に示すように、ｎによって目標姿勢が４通りに変わるが、目標値算出部１０７はその中から現在の姿勢に最も近いものを１つ選択する。目標値算出部１０７は、現在の姿勢に近いかどうかは、現在の姿勢から各目標姿勢へと回転させた場合の回転角の大小等から判断できる。また、近接判定部１０５がアーム２に最も近い拘束目標を示す情報を通知している場合には、目標値算出部１０７はその情報から拘束制御の目標値を選択してもよい。
　次いで、拘束制御演算部１０８は、位置姿勢演算部１０４による算出結果に基づいて、目標値算出部１０７により算出された目標値に移動するアーム２の動き（拘束制御指令値）を算出する（ステップＳＴ４０８）。この際、拘束制御演算部１０８は、目標値算出部１０７により算出された目標値から、アーム２を現在の位置姿勢から目標値に移動させるのに必要なアーム２の移動量又は単位時間当たり移動量である関節角速度θ（ドット）等を算出する。また、拘束制御演算部１０８は、関節角速度θ（ドット）の代わりに１制御周期あたりの関節角変化量Δθを演算してもよい。
　次いで、拘束制御制限部１０９は、拘束制御演算部１０８による算出結果を制限する（ステップＳＴ４０９）。すなわち、拘束制御制限部１０９は、従動制御演算部１０３による算出結果が拘束制御演算部１０８による算出結果より優先されるよう拘束制御演算部１０８による算出結果に制限を課す。
　この拘束制御制限部１０９の役割は、操作者が、直接教示装置１が拘束制御を行っている状態（拘束付き直接教示中の状態）から拘束制御を行っていない状態（通常の直接教示の状態）に切換えられるようにすることである。例えば、直接教示装置１が、拘束する力を強くして、拘束から外れるような操作者の操作を常時完全に打ち消していると、一旦拘束制御が開始すると拘束目標に常時拘束してしまい、拘束制御を終了できなくなる。そこで、拘束制御の終了を可能とするため、拘束制御制限部１０９により拘束制御の強さを制限して、操作者が強い外力を加えれば拘束目標から外すことができるようにする。
　例えば、直接教示装置１がアーム２の位置を所定の拘束面５０３に拘束している場合において、拘束制御制限部１０９は拘束制御の速度を例えば０．０５［ｍ／ｓ］以下に制限する。これにより、操作者が０．０５［ｍ／ｓ］より速くアーム２を移動すれば、アーム２は拘束面５０３から離れていき、最終的には近接判定部１０５が拘束面５０３に近づいていないと判定するまで移動させることができる。一方、操作者が０．０５［ｍ／ｓ］より遅くアーム２を移動しようとすれば、アーム２は拘束面５０３に拘束された状態を継続する。
　また、直接教示装置１がアーム２の位置を所定の拘束面５０３に拘束している場合において、拘束制御制限部１０９は拘束制御の関節角速度θ（ドット）を例えば０．１［ｒａｄ／ｓ］以下に制限する。これにより、操作者が０．１［ｒａｄ／ｓ］より速くアーム２を移動すれば、アーム２は拘束面５０３から離れていき、最終的には近接判定部１０５が拘束面５０３に近づいていないと判定するまで移動させることができる。一方、操作者が０．１［ｒａｄ／ｓ］より遅くアーム２を移動しようすれば、アーム２は拘束面５０３に拘束された状態を継続する。
　上記の他、例えば、直接教示装置１がアーム２の姿勢を真下方向５０７に向けている場合についても、拘束制御制限部１０９が姿勢を変化させる速度又は関節角速度θ（ドット）に制限を加えることで、操作者は拘束状態を解除する操作が可能となる。
　なお、拘束制御制限部１０９が拘束制御の強さを制限する際、何れの場合においても、拘束制御によってアーム２を動かす向きを維持することが非常に好ましい。
　例えば、拘束制御制限部１０９が関節角速度θ（ドット）を制限する場合であって、ある関節角速度θ（ドット）を０．５倍にしないと制限を超過する場合、全ての関節角速度θ（ドット）を０．５倍にすべきであり、制限を超えた関節角速度θ（ドット）だけを修正すべきではない。拘束制御制限部１０９が全ての関節角速度θ（ドット）に対して同じように制限しなかった場合、拘束制御演算部１０８が動かそうとする向きから変わってしまうため、拘束制御の精度が劣化する。
　次いで、合成部１１０は、従動制御演算部１０３から出力された従動制御指令値と拘束制御制限部１０９から出力された拘束制御指令値とを一つの指令値に合成する（ステップＳＴ４１０）。
　例えば、従動制御演算部１０３が従動制御指令値として関節角θＤを出力し、拘束制御制限部１０９が拘束制御指令値として関節角変化量ΔθＣを出力した場合には、合成部１１０は、θＤ＋ΔθＣを関節角指令値として出力する。
　また、例えば、従動制御演算部１０３が従動制御指令値として位置座標［ｘＤ，ｙＤ，ｚＤ］及び姿勢ＴＤ（回転行列表現）を出力し、拘束制御制限部１０９が拘束制御指令値として位置変化量［ΔｘＣ，ΔｙＣ，ΔｚＣ］及び姿勢変化量ΔＴＣ（回転行列表現）を出力した場合には、合成部１１０は、［ｘＤ＋ΔｘＣ，ｙＤ＋ΔｙＣ，ｚＤ＋ΔｚＣ］を位置指令値として、ΔＴＣＴＤ（回転行列表現）を姿勢指令値として出力する（回転行列表現された姿勢の合成は行列の積となる）。
　また、例えば、従動制御演算部１０３が従動制御指令値としてトルクτＤを出力し、拘束制御制限部１０９が拘束制御指令値としてトルク変化量ΔτＣを出力した場合には、合成部１１０は、τＤ＋ΔτＣをトルク指令値として出力する。
　次いで、駆動制御部１１１は、合成部１１０による合成結果に基づいて、アーム２を駆動する（ステップＳＴ４１１）。この駆動制御部１１１によるアーム２の駆動は、通常のアーム２の制御であるため、その詳細については省略する。
　以上のように、この実施の形態１によれば、直接教示装置１は、ロボットが有するアーム２に加えられた外力を検知する外力検知部１０２と、外力検知部１０２により検知された外力に従うアーム２の動きを算出する従動制御演算部１０３と、アーム２の位置又は姿勢のうちの少なくとも一方である位置姿勢を算出する位置姿勢演算部１０４と、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置姿勢が、当該アーム２の位置姿勢の拘束先である拘束目標に近づいているかを判定する近接判定部１０５と、拘束目標及び位置姿勢演算部１０４による算出結果に基づいて、拘束制御の目標値を算出する目標値算出部１０７と、位置姿勢演算部１０４による算出結果に基づいて、目標値算出部１０７により算出された目標値に移動するアーム２の動きを算出する拘束制御演算部１０８と、拘束制御演算部１０８による算出結果を制限する拘束制御制限部１０９と、近接判定部１０５により拘束目標に近づいていると判定された場合に拘束制御演算部１０８による算出結果を有効とし、近接判定部１０５により拘束目標に近づいていないと判定された場合に拘束制御演算部１０８による算出結果を無効とする切換部１０６と、従動制御演算部１０３による算出結果と拘束制御制限部１０９による制限結果とを合成する合成部１１０と、合成部１１０による合成結果に基づいてアーム２を駆動する駆動制御部１１１とを備えた。これにより、実施の形態１に係る直接教示装置１は、操作者がアーム２を操作している状態のまま通常の直接教示と拘束付き直接教示を切換え可能となる。
実施の形態２．
　実施の形態２では、アーム２が垂直多関節型ロボットアームであり、直接教示装置１による拘束方法が姿勢拘束のみに限定された場合を示す。なお、実施の形態２に係る直接教示装置１の構成例は、実施の形態１に係る直接教示装置１の構成例と同様であり、以下では図１に示す構成例を用いて説明を行う。
　なお、実施の形態２における位置姿勢演算部１０４は、アーム２の姿勢を算出する。
　また、実施の形態２における近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の姿勢が拘束方向５０５に近接しているかを判定する。
　次に、実施の形態２に係る直接教示装置１の動作例について、図８を参照しながら説明する。
　以下では、直接教示装置１は、エンドエフェクタ２０１の向き５０２（Ｚ軸）を真下方向５０７又は真横方向５０９に拘束するような姿勢拘束を行うものとする。また、ロボット座標系の各軸の単位ベクトルをｅＸ，eＹ，eＺで表す。また、アーム２は６軸であるとする。また、切換部１０６は、目標値算出部１０７及び拘束制御演算部１０８の動作を切換えるものとする。また、従動制御演算部１０３は従動制御指令値としてアーム２の各関節角θを算出し、拘束制御演算部１０８は拘束制御指令値としてアーム２の各関節角速度θ（ドット）を算出するものとする。
　実施の形態２に係る直接教示装置１の動作例では、図８に示すように、まず、位置姿勢計測部１０１は、アーム２の各関節角θを計測する（ステップＳＴ８０１）。例えば、位置姿勢計測部１０１は、アーム２が有するモータに取付けられたエンコーダ等を用いて、アーム２の関節毎に関節角θを計測する。
　次いで、外力検知部１０２は、操作者によりアーム２に加えられた外力を検知する（ステップＳＴ８０２）。この外力検知部１０２による処理は実施の形態１と同じである。なお、外力検知部１０２は重力補償を行うものとする。
　次いで、従動制御演算部１０３は、位置姿勢計測部１０１による計測結果に基づいて、外力検知部１０２により検知された外力に従うアーム２の各関節角θ（従動制御指令値）を算出する（ステップＳＴ８０３）。この従動制御演算部１０３の動作例について、図９を参照しながら説明する。
　従動制御演算部１０３は、例えば図９Ａに示される方法により、外力検知部１０２により検知されたトルクτからアーム２の各関節角θを算出できる。
　図９Ａでは、従動制御演算部１０３は、まず、外力検知部１０２により検知されたトルクτにゲインＫＴを掛けることで、アーム２の関節毎に目標とする関節角速度θ（ドット）を算出する。なお、ゲインＫＴは、アーム２の関節毎に同一でもよいし異なっていてもよい。なお図９Ａに示すように、従動制御演算部１０３は、目標とする関節角速度θ（ドット）にリミッタを適用し、従動制御の速さを制限してもよい。
　次に、従動制御演算部１０３は、目標とする関節角速度θ（ドット）に制御周期ＴＳを掛けることで、アーム２の関節毎に、１制御周期あたりの関節角変化量Δθを算出する。
　次に、従動制御演算部１０３は、アーム２の関節毎に、現時刻ｋの関節角θ（ｋ）に関節角変化量Δθを加算して次の時刻ｋ＋１の関節角θ（ｋ＋１）を算出する。
　また、従動制御演算部１０３は、例えば図９Ｂに示される方法により、外力検知部１０２により検知された力Ｆからアーム２の各関節角θを演算できる。
　図９Ｂの例では、従動制御演算部１０３は、まず、外力検知部１０２により検知された力ＦにゲインＫＦを掛けることで、アーム２の位置及び姿勢毎に移動速度を算出する。なお、ゲインＫＦは、アーム２の位置及び姿勢の各成分毎に同一でもよいし異なっていてもよい。
　次に、従動制御演算部１０３は、移動速度にヤコビ行列Ｊの逆行列を掛けることで、アーム２の関節毎に目標とする関節角速度θ（ドット）を算出する。なお、従動制御演算部１０３は、ヤコビ行列Ｊの逆行列が存在しない場合は疑似逆行列を掛ける。なお図９Ｂに示すように、従動制御演算部１０３は、目標とする関節角速度θ（ドット）にリミッタを適用し、従動制御の速さを制限してもよい。
　次に、従動制御演算部１０３は、目標とする関節角速度θ（ドット）に制御周期ＴＳを掛けることで、アーム２の関節毎に、１制御周期あたりの関節角変化量Δθを算出する。
　次に、従動制御演算部１０３は、アーム２の関節毎に、現時刻ｋの関節角θ（ｋ）に関節角変化量Δθを加算して次の時刻ｋ＋１の関節角θ（ｋ＋１）を算出する。
　次いで、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果に基づいて、アーム２の姿勢を算出する（ステップＳＴ８０４）。位置姿勢演算部１０４は、順運動学により上記算出を実行できる。以下、位置姿勢演算部１０４により算出されるアーム２の姿勢を回転行列によりＴで表す。また、Ｔを列ベクトルや要素に分解したものを次式（３）のように表す。
Figure JPOXMLDOC01-appb-I000003
　次いで、近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の姿勢が拘束方向５０５に近づいているかを判定する（ステップＳＴ８０５）。ここでは、拘束方向５０５は真下方向５０７又は真横方向５０９である。また、接近判定のしきい値は０．１［ｒａｄ］であるとする。近接判定部１０５の動作例について、図１０，１１を参照しながら説明する。
　近接判定部１０５の動作例では、図１０に示すように、近接判定部１０５は、まず、真上方向（基準方向）５０８とエンドエフェクタ２０１の向き５０２（Ｚ軸）とが成す角度Θを算出する（ステップＳＴ１００１）。すなわち、近接判定部１０５は、次式（４）のように、ｅＺとｔＺとの内積から角度Θを算出する。
Figure JPOXMLDOC01-appb-I000004
　そして、近接判定部１０５は、角度Θが（π／２－０．１）［ｒａｄ］から（π／２＋０．１）［ｒａｄ］の範囲内であるかを判定する（ステップＳＴ１００２）。
　このステップＳＴ１００２において、近接判定部１０５は、角度Θが（π／２－０．１）［ｒａｄ］から（π／２＋０．１）［ｒａｄ］の範囲内であると判定した場合（例えば図１１Ａの場合）には、アーム２の姿勢が真横方向５０９に近づいていると判定する（ステップＳＴ１００３）。
　一方、ステップＳＴ１００２において、近接判定部１０５は、角度Θが（π／２－０．１）［ｒａｄ］から（π／２＋０．１）［ｒａｄ］の範囲内ではないと判定した場合には、角度Θが（π－０．１）［ｒａｄ］以上であるかを判定する（ステップＳＴ１００４）。
　このステップＳＴ１００４において、近接判定部１０５は、角度Θが（π－０．１）［ｒａｄ］以上であると判定した場合（例えば図１１Ｂの場合）には、アーム２の姿勢が真下方向５０７に近づいていると判定する（ステップＳＴ１００５）。
　一方、ステップＳＴ１００４において、近接判定部１０５は、角度Θが（π－０．１）［ｒａｄ］以上ではないと判定した場合（例えば図１１Ｃの場合）には、アーム２の姿勢がどの拘束方向５０５にも近づいていないと判定する（ステップＳＴ１００６）。
　次いで、切換部１０６は、近接判定部１０５による判定結果に従い、目標値算出部１０７及び拘束制御演算部１０８の動作を切換える（ステップＳＴ８０６）。
　この際、切換部１０６は、近接判定部１０５によりアーム２の姿勢が真横方向５０９に近づいていると判定された場合には、目標値算出部１０７に目標値として真横方向５０９への目標姿勢を算出させ、拘束制御演算部１０８による処理を有効とする。
　また、切換部１０６は、近接判定部１０５によりアーム２の姿勢が真下方向５０７に近づいていると判定された場合には、目標値算出部１０７に目標値として真下方向５０７への目標姿勢を算出させ、拘束制御演算部１０８による処理を有効とする。
　また、切換部１０６は、近接判定部１０５によりアーム２の姿勢がどの拘束方向５０５にも近づいていないと判定された場合には、目標値算出部１０７による算出を不要し、拘束制御演算部１０８による処理を無効とする。
　次いで、目標値算出部１０７は、拘束目標及び位置姿勢演算部１０４による算出結果に基づいて、目標姿勢を算出する（ステップＳＴ８０７）。例えば、目標値算出部１０７は、真横方向５０９又は真下方向５０７であり、且つ、アーム２の現在の姿勢から最も小さい回転角度ΘＲで到達可能な目標姿勢を算出する。目標値算出部１０７による目標姿勢の算出方法について以下に示す。
　まず、目標値算出部１０７は、対象となる目標姿勢毎に、ｅＺに対する目標角度Θ＊を算出する。ここで、目標姿勢が真横方向５０９である場合には目標角度Θ＊はπ／２であり、目標姿勢が真下方向５０７である場合には目標角度Θ＊はπである。
　次に、目標値算出部１０７は、アーム２の姿勢に基づいて、回転角度ΘＲが最も小さい回転軸５１０を算出する。図１２に示すように、この回転軸５１０は、ロボット座標系のＸＹ平面上にあり且つｔＺと直交する軸であり、すなわち、ｅＺ及びｔＺの両方と直交する軸である。よって、目標値算出部１０７は、ｅＺ及びｔＺの両方と内積が０となるような単位ベクトルであるｒを１つ求め、それを回転軸５１０とする。なお、目標値算出部１０７は、ｅＺとｔＺとが平行の場合にはｒを一意に求めることはできない。しかしながら、これはアーム２の姿勢が真上方向５０８又は真下方向５０７を向いている場合であり、目標値算出部１０７は、ＸＹ平面上の任意のベクトルを回転軸５１０として算出する。
　次に、目標値算出部１０７は、算出した目標角度Θ＊及び回転軸５１０に基づいて、目標姿勢として回転角度ΘＲを算出する。ここで、エンドエフェクタ２０１の向き５０２が真上方向（基準方向）５０８から角度Θである場合、回転角度ΘＲの大きさはΘ＊－Θとなるが、図１３に示すように回転の向き（正負）を考慮する必要がある。これは、実施の形態２に係る直接教示装置１では、角度Θを求める際に内積を使っているため、エンドエフェクタ２０１の向き５０２の情報が失われているためである。回転の向きを考慮すると、回転角度ΘＲは次式（５）のようになる。すなわち、回転の向きは、ｅＺ×ｔＺとｒとが同じ向きである場合には符号の反転は不要であり、ｅＺ×ｔＺとｒとが逆の向きである場合には符号の反転が必要である。
Figure JPOXMLDOC01-appb-I000005
　目標値算出部１０７では、回転軸５１０を表すｒ＝［ｒｘ，ｒｙ，ｒｚ］Ｔ（Ｔは転置を表す）と回転角度ΘＲが得られれば、アーム２の姿勢から目標姿勢への回転が得られたことになるので、拘束目標が事実上得られたことになる。目標姿勢の具体的な表現が必要な場合は、目標値算出部１０７は、アーム２の姿勢に前述した回転を適用することで得られる。
　上記では、目標値算出部１０７が、エンドエフェクタ２０１の向き５０２（Ｚ軸）が真下方向５０７又は真横方向５０９を向くような目標姿勢を算出する方法について述べた。目標値算出部１０７は、ここから更に回転を加えて所望の姿勢にして、それを目標姿勢とすることも可能である。
　例えば、目標値算出部１０７は、アーム２が真下方向５０７を向いた状態からＺ軸周りに回転を加えることで、図７に示すような姿勢を目標姿勢とすることも可能である。また、目標値算出部１０７は、図１４に示すように、アーム２が真横方向５０９を向いた状態から更にＺ軸周りに回転を加え、ｔＸ又はｔＹがロボット座標系のＺ軸と平行となる姿勢を目標姿勢とすることも可能である。
　次いで、拘束制御演算部１０８は、位置姿勢演算部１０４による算出結果に基づいて、目標値算出部１０７により算出された目標姿勢に移動するアーム２の動き（拘束制御指令値）を算出する（ステップＳＴ８０８）。なお、アーム２を現在の姿勢から目標姿勢へと動かすような回転は、目標値算出部１０７が目標姿勢を算出する際に既に算出しているため、拘束制御演算部１０８は、そのようにアーム２を回転させる動きを算出する。ただし、拘束制御演算部１０８の出力は関節角速度θ（ドット）とする必要があるので、拘束制御演算部１０８は、目標値算出部１０７により算出された回転軸５１０であるｒと回転角度ΘＲの組から関節角速度θ（ドット）を算出する必要がある。以下にその計算の一例を示す。
　拘束制御演算部１０８は、まず、回転の角速度の大きさ｜ω｜を決める。例えば、制限を受けなければ１秒で目標姿勢に到達するように制御するのであれば、｜ω｜＝θＲとする。
　次に、拘束制御演算部１０８は、アーム２の現在の位置姿勢におけるヤコビ行列Ｊを算出する。ヤコビ行列Ｊについては次式（６）が成り立つものとする。
Figure JPOXMLDOC01-appb-I000006
　ここで、ｖＸ，ｖＹ，ｖＺはエンドエフェクタ２０１の先端のＸ，Ｙ，Ｚ軸方向の速度であり、ωＸ，ωＹ，ωＺはアーム２の姿勢がＸ軸，Ｙ軸，Ｚ軸について回転する角速度であり、θ１（ドット）～θ６（ドット）は各関節角速度である。
　次に、拘束制御演算部１０８は、回転軸５１０であるｒと回転速度の大きさ｜ω｜から姿勢に関する回転ベクトルω＝［ωＸ，ωＹ，ωＺ］Ｔを算出する。
　次に、拘束制御演算部１０８は、次式（７）を満たすようなアーム２の関節毎の関節角速度θ１（ドット）～θ６（ドット）を算出する。なお、拘束制御演算部１０８は、ヤコビ行列Ｊに逆行列が存在しない場合は疑似逆行列等を用いて関節角速度θ１（ドット）～θ６（ドット）を算出する。
Figure JPOXMLDOC01-appb-I000007
　次いで、拘束制御制限部１０９は、拘束制御演算部１０８による算出結果を制限する（ステップＳＴ８０９）。
　まず、拘束制御制限部１０９は、拘束制御演算部１０８により算出されたアーム２の関節毎の関節角速度θ（ドット）に対する上限θＭＡＸ（ドット）を決める。上限θＭＡＸ（ドット）は関節毎に同一でもよいし異なっていてもよい。また、従動制御演算部１０３でリミッタを適用している場合は、拘束制御制限部１０９は、そのリミッタの上限よりも上限θＭＡＸ（ドット）を小さくする必要がある。リミッタの上限よりも上限θＭＡＸ（ドット）を小さくしないと、操作者による操作が拘束制御よりも制限され、拘束付き直接教示から脱することができなくなる。現実的には、拘束制御制限部１０９は、上限θＭＡＸ（ドット）を、リミッタの上限に比して半分以下にすることが好ましい。その程度に制限すれば、操作者による操作で拘束を脱することは比較的容易になる。
　次に、拘束制御制限部１０９は、全ての関節角速度θ（ドット）が上限θＭＡＸ（ドット）を超えないように制限する。このとき、回転の方向を変えないように制限することが好ましい。そのような演算を行う手順の一例を以下に示す。なお、ｉ番目の関節角速度θｉ（ドット）の上限をθｉ，ＭＡＸ（ドット）で表す。
　拘束制御制限部１０９は、まず、アーム２の関節毎に、関節角速度θｉ（ドット）の大きさと上限θｉ，ＭＡＸ（ドット）の比ｃｉを｜θｉ（ドット）｜／θｉ，ＭＡＸ（ドット）で算出する。
　次に、拘束制御制限部１０９は、この比ｃｉの最大値ｃを算出する。
　次に、拘束制御制限部１０９は、アーム２の関節毎に、関節角速度θｉ（ドット）を最大値ｃで割って制限後の関節角速度θｉ ＊（ドット）を算出する。すなわち制限後の関節角速度θｉ ＊（ドット）はθｉ（ドット）／ｃである。
　次いで、合成部１１０は、従動制御演算部１０３から出力された従動制御指令値（関節角θ）と拘束制御制限部１０９から出力された拘束制御指令値（関節角速度θ（ドット））とを合成する（ステップＳＴ８１０）。
　例えば、従動制御演算部１０３が関節角θ＝［θ１，・・・，θｍ］Ｔを出力し、拘束制御制限部１０９が関節角速度θ（ドット）＝［θ１（ドット），・・・，θｍ（ドット）］Ｔを出力した場合、制御周期ＴＳから、合成部１１０の出力である関節角指令値はθ＋ＴＳθ（ドット）となる。ただし、ｍは駆動制御部１１１が制御するアーム２の関節数である。
　次いで、駆動制御部１１１は、合成部１１０による合成結果（関節角指令値）に基づいて、アーム２を駆動する（ステップＳＴ８１１）。
　なお上記では一部、６軸（関節数が６個）のアーム２を前提として説明及び例示を行ったが、６軸以外のアーム２であっても同様に実施可能である。
実施の形態３．
　実施の形態２に係る直接教示装置１では、拘束方法が姿勢拘束のみに限定された場合を示した。それに対し、実施の形態３に係る直接教示装置１では、拘束方法が軸拘束のみに限定された場合を示す。軸拘束とは、アーム２の位置を所定の拘束軸５０４に拘束する位置拘束とアーム２の姿勢を所定の拘束方向５０５に拘束する姿勢拘束とを同時に行うことを意味する。すなわち、実施の形態３に係る直接教示装置１は、実施の形態２で示した姿勢拘束に加え、位置拘束も行う。以下では、直接教示装置１が、図１５に示すようなロボット座標系のＺ軸に平行な直線５１１への位置拘束及び真下方向５０７への姿勢拘束を行う場合を示すが、拘束軸５０４及び拘束方向５０５は任意に設定可能である。
　なお、実施の形態３に係る直接教示装置１の構成例は、実施の形態２に係る直接教示装置１の構成例と同様であり、以下では図１に示す構成例を用いて説明を行う。
　なお、実施の形態３における位置姿勢演算部１０４は、アーム２の位置及び姿勢を算出する。
　また、実施の形態３における近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置が拘束目標である拘束軸５０４に近づいているかを判定し且つアーム２の姿勢が拘束目標である拘束方向５０５に近づいているかを判定することで、アーム２の位置姿勢が拘束目標に近づいているかを判定する。
　図１６は、実施の形態３に係る直接教示装置１の動作例を示すフローチャートである。図１６に示す実施の形態３に係る直接教示装置１において、図８に示す実施の形態２に係る直接教示装置１と動作が異なるのは、位置姿勢演算部１０４、近接判定部１０５、目標値算出部１０７及び拘束制御演算部１０８であり、これらの構成の動作例についてのみ説明を行う。
　まず、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果に基づいて、アーム２の位置及び姿勢を算出する（ステップＳＴ１６０４）。実施の形態３ではアーム２の位置及び姿勢の両方を拘束するため、位置姿勢演算部１０４は、アーム２の位置及び姿勢の両方を算出する必要がある。位置姿勢演算部１０４は、順運動学により上記算出を実行できる。
　また、近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置が拘束軸５０４に近づいているか及びアーム２の姿勢が拘束方向５０５に近づいているかをそれぞれ判定し、それらの判定結果を基にアーム２の位置姿勢が拘束目標に近づいているかどうかを総合的に判定する（ステップＳＴ１６０５）。例えば、近接判定部１０５は、アーム２の位置が拘束軸５０４に近づき且つアーム２の姿勢が拘束方向５０５に近づいていると判定した場合には、アーム２の位置姿勢は拘束目標に近づいていると判定する。近接判定部１０５による拘束方向５０５に対する判定については実施の形態２で説明しているので省略し、以下では、近接判定部１０５による拘束軸５０４に対する判定について説明する。
　この際、近接判定部１０５は、例えば図１７に示すように、エンドエフェクタ２０１の位置５０１から拘束軸５０４に対して垂線５１２をおろし、その垂線５１２の長さが所定の値より小さければアーム２の位置が拘束軸５０４に近づいていると判定する。例えばアーム２の位置の座標を［ｘ，ｙ，ｚ］で表し、直線５１１のＸ座標とＹ座標をそれぞれｘ＊，ｙ＊で表す。この場合、垂線５１２の長さは次式（８）のようになる。
Figure JPOXMLDOC01-appb-I000008
　そして、近接判定部１０５は、式（８）に示す垂線５１２の長さが所定の値よりも小さければ、アーム２の位置が拘束軸５０４に近づいていると判定し、それ以外の場合にはアーム２の位置が拘束軸５０４には近づいていないと判定する。なお、近接判定部１０５は、拘束軸５０４がロボット座標系のＺ軸に平行な場合以外でも、同様にして拘束軸５０４に対しておろした垂線の長さから接近判定が可能である。
　また、目標値算出部１０７は、実施の形態２で説明した目標姿勢の算出に加え、アーム２の位置を拘束軸５０４へと移動させる場合の目標位置の算出を行う（ステップＳＴ１６０７）。例えば、目標値算出部１０７は、エンドエフェクタ２０１の位置５０１から図１７に示すように拘束軸５０４へと垂線５１２をおろした位置５１３を目標位置とする。ここで、目標値算出部１０７は、アーム２の位置をロボット座標系のＺ軸に平行な拘束軸５０４に拘束させるのであれば、目標位置は［ｘ＊，ｙ＊，ｚ］となる。
　また、拘束制御演算部１０８は、実施の形態２と同様に、ヤコビ行列Ｊを用いてアーム２の関節毎に関節角速度θ（ドット）を算出する（ステップＳＴ１６０８）。ただし、実施の形態３における拘束制御演算部１０８では、姿勢拘束に加え位置拘束も行うので、演算手順が実施の形態２に対して以下のように変わる。
　まず、拘束制御演算部１０８は、回転の角速度の大きさ｜ω｜を決める。
　次に、拘束制御演算部１０８は、アーム２の現在の位置姿勢におけるヤコビ行列Ｊを算出する。
　次に、拘束制御演算部１０８は、回転軸５１０であるｒと回転速度の大きさ｜ω｜から姿勢に関する回転ベクトルω＝［ωＸ，ωＹ，ωＺ］Ｔを算出する。ここまでの動作は、実施の形態２と同様である。
　次に、拘束制御演算部１０８は、アーム２の位置を拘束軸５０４へと移動させる速さ｜ｖ｜を決める。例えば、制限を受けなければ１秒で目標位置に到達するように制御するのであれば、拘束制御演算部１０８は、｜ｖ｜＝√｛（ｘ－ｘ＊）２＋（ｙ－ｙ＊）２｝とする。また、拘束制御演算部１０８は、ある定数ｖ＊を決めて｜ｖ｜＝ｖ＊としてもよい。
　次に、拘束制御演算部１０８は、アーム２の位置に関する速度ベクトルｖ＝［ｖＸ，ｖＹ，０］Ｔを算出する。拘束制御演算部１０８は、ロボット座標系のＺ軸に平行な拘束軸５０４に拘束させる場合はｖ＝｜ｖ｜［ｘ＊－ｘ，ｙ＊－ｙ，０］／√｛（ｘ－ｘ＊）２＋（ｙ－ｙ＊）２｝より算出可能である。
　次に、拘束制御演算部１０８は、アーム２の関節毎に関節角速度θ（ドット）を次式（９）より算出する。
Figure JPOXMLDOC01-appb-I000009
　その他の動作は実施の形態２と同じになる。
　なお、実施の形態３に係る直接教示装置１において、姿勢拘束に関する演算を省略した場合には、アーム２の姿勢に関する拘束は行われず、アーム２の位置のみを指定された軸に拘束する制御となる。
実施の形態４．
　実施の形態２に係る直接教示装置１では、拘束方法が姿勢拘束のみに限定された場合を示した。それに対し、実施の形態４に係る直接教示装置１では、拘束方法が面拘束のみに限定された場合を示す。面拘束とは、アーム２の位置を所定の拘束面５０３に拘束する位置拘束とアーム２の姿勢を所定の拘束方向５０５に拘束する姿勢拘束とを同時に行うことを意味する。すなわち、実施の形態４に係る直接教示装置１は、実施の形態２で示した姿勢拘束に加え、位置拘束も行う。以下では、直接教示装置１が、図１８に示すようなＺ軸座標がＸ軸座標及びＹ軸座標の関数ｚ＝ｆ（ｘ，ｙ）で表される曲面５１３への位置拘束及び真下方向５０７への姿勢拘束を行う場合を示すが、拘束面５０３及び拘束方向５０５は任意に設定可能であり、例えば拘束面５０３が関数ｘ＝ｆ（ｙ，ｚ）又はｙ＝ｆ（ｚ，ｘ）で表される場合等についても同様の方法で実施可能である。
　なお、実施の形態４に係る直接教示装置１の構成例は、実施の形態２に係る直接教示装置１の構成例と同様であり、以下では図１に示す構成例を用いて説明を行う。
　なお、実施の形態４における位置姿勢演算部１０４は、アーム２の位置及び姿勢を算出する。
　また、実施の形態４における近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置が拘束目標である拘束面５０３に近づいているかを判定し且つアーム２の姿勢が拘束目標である拘束方向５０５に近づいているかを判定することで、アーム２の位置姿勢が拘束目標に近づいているかを判定する。
　図１９は、実施の形態４に係る直接教示装置１の動作例を示すフローチャートである。図１９に示す実施の形態４に係る直接教示装置１において、図１６に示す実施の形態３に係る直接教示装置１と動作が異なるのは、近接判定部１０５、目標値算出部１０７及び拘束制御演算部１０８であり、これらの構成の動作例についてのみ説明を行う。
　まず、近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置が拘束面５０３に近づいているか及びアーム２の姿勢が拘束方向５０５に近づいているかをそれぞれ判定し、それらの判定結果を基にアーム２の位置姿勢が拘束目標に近づいているかどうかを総合的に判定する（ステップＳＴ１９０５）。例えば、近接判定部１０５は、アーム２の位置が拘束面５０３に近づき且つアーム２の姿勢が拘束方向５０５に近づいていると判定した場合には、アーム２の位置姿勢は拘束目標に近づいていると判定する。近接判定部１０５による拘束方向５０５に対する判定については実施の形態２で説明しているので省略し、以下では、近接判定部１０５による拘束面５０３に対する判定について説明する。
　この際、近接判定部１０５は、エンドエフェクタ２０１の位置５０１から拘束面５０３に対してロボット座標系のＺ軸に平行な直線をおろし、その直線の長さが所定の値より小さければアーム２の位置が拘束面５０３に近づいていると判定する。例えばアーム２の位置の座標が［ｘ，ｙ，ｚ］である場合、上記直線の長さは｛ｚ－ｆ（ｘ，ｙ）｝となる。そして、近接判定部１０５は、この長さが所定の値よりも小さければ、アーム２の位置が拘束面５０３に近づいていると判定し、それ以外の場合にはアーム２の位置が拘束面５０３には近づいていないと判定する。
　また、目標値算出部１０７は、実施の形態２で説明した目標姿勢の算出に加え、アーム２の位置を拘束面５０３へと移動させる場合の目標位置の算出を行う（ステップＳＴ１９０７）。例えば、目標値算出部１０７は、アーム２の位置の座標が［ｘ，ｙ，ｚ］である場合、Ｚ軸座標だけを曲面上の座標に置き換えた［ｘ，ｙ，ｆ（ｘ，ｙ）］を目標位置とする。
　また、拘束制御演算部１０８は、実施の形態２と同様に、ヤコビ行列Ｊを用いてアーム２の関節毎に関節角速度θ（ドット）を算出する（ステップＳＴ１９０８）。ただし、実施の形態４における拘束制御演算部１０８では、姿勢拘束に加え位置拘束も行うので、演算手順が実施の形態２に対して以下のように変わる。
　まず、拘束制御演算部１０８は、回転の角速度の大きさ｜ω｜を決める。
　次に、拘束制御演算部１０８は、アーム２の現在の位置姿勢におけるヤコビ行列Ｊを算出する。
　次に、拘束制御演算部１０８は、回転軸５１０であるｒと回転速度の大きさ｜ω｜から姿勢に関する回転ベクトルω＝［ωＸ，ωＹ，ωＺ］Ｔを算出する。ここまでの動作は、実施の形態２と同様である。
　次に、拘束制御演算部１０８は、アーム２の位置を拘束軸５０４へと移動させる速さ｜ｖ｜を決める。例えば、制限を受けなければ１秒で目標位置に到達するように制御するのであれば、拘束制御演算部１０８は、｜ｖ｜＝｜ｚ－ｆ（ｘ，ｙ）｜とする。また、拘束制御演算部１０８は、速さ｜ｖ｜をある定数値にしてもよい。
　次に、拘束制御演算部１０８は、アーム２の位置に関する速度ベクトルｖ＝［０，０，ｖＺ］Ｔをｖ＝［０，０，｜ｖ｜］より算出する。
　次に、拘束制御演算部１０８は、アーム２の関節毎に関節角速度θ（ドット）を次式（１０）より算出する。
Figure JPOXMLDOC01-appb-I000010
　その他の動作は実施の形態２，３と同じになる。
　なお、実施の形態４に係る直接教示装置１において、姿勢拘束に関する演算を省略した場合には、アーム２の姿勢に関する拘束は行われず、アーム２の位置のみを指定された面に拘束する制御となる。また、実施の形態４に係る直接教示装置１において、アーム２の位置から拘束面５０３へ垂線を下した方向を目標姿勢とした場合には、エンドエフェクタ２０１が拘束面５０３に対して常に垂直となるような姿勢拘束になる。
　なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。
　本発明に係る直接教示装置は、操作者がアームを操作している状態のまま通常の直接教示と拘束付き直接教示を切換え可能に構成しているので、産業用ロボットなどのロボットに作業を直接教示する方法に用いるのに適している。
１　直接教示装置
２　アーム
１０１　位置姿勢計測部
１０２　外力検知部
１０３　従動制御演算部
１０４　位置姿勢演算部
１０５　近接判定部
１０６　切換部
１０７　目標値算出部
１０８　拘束制御演算部
１０９　拘束制御制限部
１１０　合成部
１１１　駆動制御部
２０１　エンドエフェクタ

Claims (7)

  1. 　ロボットが有するアームに加えられた外力を検知する外力検知部と、
    　前記外力検知部により検知された外力に従う前記アームの動きを算出する従動制御演算部と、
    　前記アームの位置又は姿勢のうちの少なくとも一方である位置姿勢を算出する位置姿勢演算部と、
    　前記位置姿勢演算部による算出結果に基づいて、前記アームの位置姿勢が、当該アームの位置姿勢の拘束先である拘束目標に近づいているかを判定する近接判定部と、
    　拘束目標及び前記位置姿勢演算部による算出結果に基づいて、拘束制御の目標値を算出する目標値算出部と、
    　前記位置姿勢演算部による算出結果に基づいて、前記目標値算出部により算出された目標値に移動する前記アームの動きを算出する拘束制御演算部と、
    　前記拘束制御演算部による算出結果を制限する拘束制御制限部と、
    　前記近接判定部により拘束目標に近づいていると判定された場合に前記拘束制御演算部による算出結果を有効とし、前記近接判定部により拘束目標に近づいていないと判定された場合に前記拘束制御演算部による算出結果を無効とする切換部と、
    　前記従動制御演算部による算出結果と前記拘束制御制限部による制限結果とを合成する合成部と、
    　前記合成部による合成結果に基づいて前記アームを駆動する駆動制御部と
    　を備えた直接教示装置。
  2. 　前記アームの位置姿勢に関するパラメータを計測する位置姿勢計測部を備え、
    　前記従動制御演算部は、前記位置姿勢計測部による計測結果に基づいて、前記外力検知部により検知された外力に従う前記アームの動きを算出し、
    　前記位置姿勢演算部は、前記従動制御演算部による算出結果に基づいて、前記アームの位置姿勢を算出する
    　ことを特徴とする請求項１記載の直接教示装置。
  3. 　前記アームの位置姿勢に関するパラメータを計測する位置姿勢計測部を備え、
    　前記位置姿勢演算部は、前記位置姿勢計測部による計測結果に基づいて、前記アームの位置姿勢を算出する
    　ことを特徴とする請求項１記載の直接教示装置。
  4. 　前記アームは垂直多関節型ロボットアームであり、
    　前記位置姿勢演算部は、前記アームの姿勢を算出し、
    　前記近接判定部は、前記位置姿勢演算部による算出結果に基づいて、前記アームの姿勢が、拘束目標である拘束方向に近づいているかを判定する
    　ことを特徴とする請求項１から請求項３のうちの何れか１項記載の直接教示装置。
  5. 　前記アームは垂直多関節型ロボットアームであり、
    　前記位置姿勢演算部は、前記アームの位置及び姿勢を算出し、
    　前記近接判定部は、前記位置姿勢演算部による算出結果に基づいて、前記アームの位置が拘束目標である拘束軸に近づいているかを判定し且つ当該アームの姿勢が拘束目標である拘束方向に近づいているかを判定することで、当該アームの位置姿勢が拘束目標に近づいているかを判定する
    　ことを特徴とする請求項１から請求項３のうちの何れか１項記載の直接教示装置。
  6. 　前記アームは垂直多関節型ロボットアームであり、
    　前記位置姿勢演算部は、前記アームの位置及び姿勢を算出し、
    　前記近接判定部は、前記位置姿勢演算部による算出結果に基づいて、前記アームの位置が拘束目標である拘束面に近づいているかを判定し且つ当該アームの姿勢が拘束目標である拘束方向に近づいているかを判定することで、当該アームの位置姿勢が拘束目標に近づいているかを判定する
    　ことを特徴とする請求項１から請求項３のうちの何れか１項記載の直接教示装置。
  7. 　外力検知部が、ロボットが有するアームに加えられた外力を検知するステップと、
    　従動制御演算部が、前記外力検知部により検知された外力に従う前記アームの動きを算出するステップと、
    　位置姿勢演算部が、前記アームの位置又は姿勢のうちの少なくとも一方である位置姿勢を算出するステップと、
    　近接判定部が、前記位置姿勢演算部による算出結果に基づいて、前記アームの位置姿勢が、当該アームの位置姿勢の拘束先である拘束目標に近づいているかを判定するステップと、
    　目標値算出部が、拘束目標及び前記位置姿勢演算部による算出結果に基づいて、拘束制御の目標値を算出するステップと、
    　拘束制御演算部が、前記位置姿勢演算部による算出結果に基づいて、前記目標値算出部により算出された目標値に移動する前記アームの動きを算出するステップと、
    　拘束制御制限部が、前記拘束制御演算部による算出結果を制限するステップと、
    　切換部が、前記近接判定部により拘束目標に近づいていると判定された場合に前記拘束制御演算部による算出結果を有効とし、前記近接判定部により拘束目標に近づいていないと判定された場合に前記拘束制御演算部による算出結果を無効とするステップと、
    　合成部が、前記従動制御演算部による算出結果と前記拘束制御制限部による制限結果とを合成するステップと、
    　駆動制御部が、前記合成部による合成結果に基づいて前記アームを駆動するステップと
    　を有する直接教示方法。
PCT/JP2019/018766 2018-05-23 2019-05-10 ロボットの直接教示装置及び直接教示方法 WO2019225358A1 (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201980032159.2A CN112118940B (zh) 2018-05-23 2019-05-10 机械手的直接教示装置以及直接教示方法
KR1020207031484A KR102381594B1 (ko) 2018-05-23 2019-05-10 로봇의 직접 교시 장치 및 직접 교시 방법

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2018-098891 2018-05-23
JP2018098891A JP7048176B2 (ja) 2018-05-23 2018-05-23 ロボットの直接教示装置及び直接教示方法

Publications (1)

Publication Number Publication Date
WO2019225358A1 true WO2019225358A1 (ja) 2019-11-28

Family

ID=68616049

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2019/018766 WO2019225358A1 (ja) 2018-05-23 2019-05-10 ロボットの直接教示装置及び直接教示方法

Country Status (4)

Country Link
JP (1) JP7048176B2 (ja)
KR (1) KR102381594B1 (ja)
CN (1) CN112118940B (ja)
WO (1) WO2019225358A1 (ja)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP7131087B2 (ja) * 2018-05-31 2022-09-06 セイコーエプソン株式会社 ロボットシステムの制御方法およびロボットシステム
JP2022062859A (ja) * 2020-10-09 2022-04-21 川崎重工業株式会社 手術支援システム、手術支援システムの制御装置および手術支援システムの制御方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0337701A (ja) * 1989-07-05 1991-02-19 Toshiba Corp ロボットの制御方法及び装置
JPH09141580A (ja) * 1995-11-22 1997-06-03 Yaskawa Electric Corp 直接教示ロボットの動作範囲制限装置
US6385508B1 (en) * 2000-10-31 2002-05-07 Fanuc Robotics North America, Inc. Lead-through teach handle assembly and method of teaching a robot assembly
JP2005293098A (ja) * 2004-03-31 2005-10-20 Fanuc Ltd ロボット教示装置
JP2015202537A (ja) * 2014-04-14 2015-11-16 ファナック株式会社 力に応じて動かすロボットのロボット制御装置およびロボットシステム
US9804593B1 (en) * 2014-12-12 2017-10-31 X Development Llc Methods and systems for teaching positions to components of devices
WO2018149502A1 (en) * 2017-02-17 2018-08-23 Abb Schweiz Ag A method for controlling an industrial robot during lead-through programming of the robot and an industrial robot

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3207231B2 (ja) 1992-01-24 2001-09-10 松下電器産業株式会社 ロボットの直接教示方法
JPH05250029A (ja) 1992-03-04 1993-09-28 Kobe Steel Ltd 産業用ロボット
JPH05285870A (ja) 1992-04-03 1993-11-02 Matsushita Electric Ind Co Ltd 多関節ロボットの直接教示装置
JPH05303425A (ja) * 1992-04-27 1993-11-16 Nippon Telegr & Teleph Corp <Ntt> 直接教示方式ロボット
CN1060715C (zh) * 1995-09-14 2001-01-17 株式会社安川电机 机器人的示教装置
JP5636119B2 (ja) 2011-11-30 2014-12-03 パナソニック株式会社 ロボットの教示装置、ロボット装置、ロボットの教示装置の制御方法、ロボットの教示装置の制御プログラム
JP6706489B2 (ja) * 2015-11-24 2020-06-10 川崎重工業株式会社 ロボットのダイレクト教示方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0337701A (ja) * 1989-07-05 1991-02-19 Toshiba Corp ロボットの制御方法及び装置
JPH09141580A (ja) * 1995-11-22 1997-06-03 Yaskawa Electric Corp 直接教示ロボットの動作範囲制限装置
US6385508B1 (en) * 2000-10-31 2002-05-07 Fanuc Robotics North America, Inc. Lead-through teach handle assembly and method of teaching a robot assembly
JP2005293098A (ja) * 2004-03-31 2005-10-20 Fanuc Ltd ロボット教示装置
JP2015202537A (ja) * 2014-04-14 2015-11-16 ファナック株式会社 力に応じて動かすロボットのロボット制御装置およびロボットシステム
US9804593B1 (en) * 2014-12-12 2017-10-31 X Development Llc Methods and systems for teaching positions to components of devices
WO2018149502A1 (en) * 2017-02-17 2018-08-23 Abb Schweiz Ag A method for controlling an industrial robot during lead-through programming of the robot and an industrial robot

Also Published As

Publication number Publication date
JP7048176B2 (ja) 2022-04-05
CN112118940B (zh) 2023-05-30
KR20200139217A (ko) 2020-12-11
JP2019202383A (ja) 2019-11-28
KR102381594B1 (ko) 2022-04-04
CN112118940A (zh) 2020-12-22

Similar Documents

Publication Publication Date Title
Park et al. Compliance-based robotic peg-in-hole assembly strategy without force feedback
US10434646B2 (en) Robot control apparatus, robot, and robot system
JP5946859B2 (ja) 力に応じて動かすロボットのロボット制御装置およびロボットシステム
JP6044511B2 (ja) ロボットの制御方法及びロボットシステム
CN109129525B (zh) 机器人的负载重心位置推定装置及负载重心位置推定方法
CN110394817B (zh) 使用机器人推断负载重量和重心位置的装置、方法及程序
WO2019225358A1 (ja) ロボットの直接教示装置及び直接教示方法
JP2010076074A (ja) ロボットの制御方法
CN105643619B (zh) 一种采用框架描述的工业机器人工具位姿控制方法
JP2011062793A (ja) ロボットの制御装置
JP6697544B2 (ja) 最適化装置及びそれを備えた垂直型多関節ロボット
JP2018158418A (ja) ロボットの負荷重心位置推定装置及びロボットの負荷重心位置推定方法
JP2014240106A (ja) ロボット、ロボット制御装置およびロボットの駆動方法
JP2020171989A (ja) ロボット教示システム
JP2005100143A (ja) モータ駆動装置の制御方法及び制御装置
WO2020050032A1 (ja) ロボットの直接教示装置及び直接教示方法
JP5521506B2 (ja) ロボット
JP2017077600A (ja) マニピュレータ装置
JP2012051043A (ja) ロボットシステムまたはロボット制御装置
JP4305340B2 (ja) ロボットに取り付けられた負荷の質量と重心位置の算出方法
JP2024111358A (ja) 直接教示装置及び直接教示方法
JP2005034960A (ja) 搬送装置
JPS63305407A (ja) ロボットの軌跡制御方式
JP2022114472A (ja) ロボットの直接教示装置及び直接教示方法
Umemoto et al. Research of Improvement of Path Follow-Up Performance of Robot

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 19807043

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 20207031484

Country of ref document: KR

Kind code of ref document: A

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 19807043

Country of ref document: EP

Kind code of ref document: A1