CN114800437A - Direct teaching device and direct teaching method - Google Patents

Abstract

The direct teaching device and the direct teaching method of the robot of the present invention can achieve both the teaching precision and the operation feeling of the mechanical arm. The direct teaching device of the present invention includes: a slave control calculation unit (103) for calculating a slave control command value; a constraint control calculation unit (108) for calculating a constraint control command value; and a constraint control adjustment unit (107) for The speed of the motion of the manipulator (2) calculated by the constraint control calculation unit (108) can be adjusted continuously or in stages in at least three stages or more; a synthesis unit (109), which slave control calculation unit (103) ) and the calculation result given by the constraint control arithmetic part (108) are combined; and a drive control part (110), which drives the robot arm (2) according to the combined result given by the combining part (109).

Description

Direct teaching device and direct teaching method

technical field

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

Background technique

In an industrial robot, in order for the robot to perform an operation, an operation called teaching is performed in advance. As one of the methods of teaching the robot, there is a method called direct teaching.

For example, Patent Document 1 discloses a direct teaching device for a robot using a force sensor or a torque sensor. The feature of this direct teaching device is that direct teaching with constraints and direct teaching without constraints can be switched.

The purpose of direct teaching with constraints is to improve the teaching accuracy. A general direct teaching device can intuitively operate and teach the robot. On the other hand, the position and posture of the robot are easily changed by external force, so it is difficult to improve the teaching accuracy. On the other hand, in direct teaching with constraints, operations and teaching are constrained so that the fingertips face vertically downward, or the fingertips are constrained to lie on a certain plane or curved surface. , or constrained by the fingertip on a certain axis, the robot can only be operated and taught within the scope of the specified constraints. Thereby, teaching can be performed so that the posture is accurately vertically downward, and the position can be accurately positioned on a predetermined plane, curved surface, or axis, and the problem that it is difficult to improve the teaching accuracy can be solved.

On the other hand, direct teaching with constraints is not always required. Therefore, it is desirable to be able to switch to unconstrained direct teaching when not required, and to be free to operate and teach without constraints. Therefore, in the direct teaching device of Patent Document 1, it is automatically switched to direct teaching with constraints when approaching the posture or position to be restrained, and direct teaching without constraints otherwise.

FIG. 13 shows a schematic configuration diagram of the direct teaching device 1b of Patent Document 1. As shown in FIG.

In this direct teaching device 1b, first, the position and posture measurement unit 101b measures parameters related to the position and posture of the robot arm 2 included in the robot. In addition, as the parameter related to the position and posture of the robot arm 2, the position of the robot arm 2, the posture of the robot arm 2, the joint angle of the robot arm 2, and the like can be mentioned.

In addition, the external force detection unit 102b detects the external force applied to the robot arm 2 by the operator.

Next, the slave control calculation unit 103b calculates the motion (slave control command value) of the robot arm 2 according to the external force detected by the external force detection unit 102b based on the measurement result given by the position and orientation measurement unit 101b.

Next, the position and posture calculation unit 104b calculates the position and posture of the robot arm 2 based on the calculation result given by the slave control calculation unit 103b. The position and posture of the robot arm 2 means at least one of the position and the posture of the robot arm 2 .

Next, the approach determination unit 105b determines whether or not the position and attitude of the robot arm 2 has approached the constraint target based on the calculation result given by the position and attitude calculation unit 104b.

Next, the switching unit 106b switches the operation of the constraint control computing unit 108b according to the determination result given by the proximity determination unit 105b. That is, when the proximity determination unit 105b determines that the position and posture of the robot arm 2 has approached the constraint target, the switching unit 106b enables the processing of the constraint control calculation unit 108b. On the other hand, when the proximity determination unit 105b determines that the position and orientation of the robot arm 2 has not approached the constraint target, the switching unit 106b disables the processing of the constraint control calculation unit 108b.

Next, the target value calculating unit 107b calculates the target value of the restraint control based on the restraint target and the calculation result given by the position and attitude calculating unit 104b. For example, the target value calculation unit 107b calculates the position and posture of the robot arm 2 conforming to a predetermined posture (constrained posture), a predetermined curved surface or plane (constrained surface), or a predetermined axis or curve (constrained axis) as the target value.

Next, the restraint control calculation unit 108b calculates the motion (restraint control command value) of the robot arm 2 moving toward the target value calculated by the target value calculation unit 107b based on the calculation result given by the position and orientation calculation unit 104b.

Next, the constraint control restriction unit 109b restricts the calculation result given by the constraint control calculation unit 108b. That is, the constraint control restriction unit 109b restricts the calculation result given by the constraint control calculation unit 108b so that the calculation result given by the slave control calculation unit 103b takes precedence over the calculation result given by the constraint control calculation unit 108b.

Next, the combining unit 110b combines the calculation result (slave control command value) given by the slave control computing unit 103b and the restriction result (restraint control command value) given by the restraint control limiting unit 109b into one command value.

Next, the drive control unit 111b drives the robot arm 2 according to the synthesis result given by the synthesis unit 110b.

In the direct teaching apparatus 1b shown in FIG. 13, the control that the robot arm 2 moves in accordance with an instruction given by a human by force is performed through a series of operations as described above. After that, when the position and posture of the robot arm 2 are in the state desired by the operator, the direct teaching device 1b records the joint angle or the position and posture at that time as a teaching point. The recorded teaching points are used when the robot performs work. Further, in the direct teaching device 1b, by switching the operation of the constraint control computing unit 108b by the switching unit 106b, direct teaching can be performed while switching between direct teaching with constraints and direct teaching without constraints.

【Existing technical documents】

【Patent Literature】

[Patent Document 1] Japanese Patent Laid-Open No. 2019-202383

SUMMARY OF THE INVENTION

【Problems to be Solved by Invention】

However, since the direct teaching device of Patent Document 1 selects whether or not to perform the constraint control computation, the robot arm may move suddenly or vibrate when switching the on/off of the constraint control computation. Therefore, there is a case where the operator's sense of operation is impaired.

This problem can be alleviated by reducing the force that causes the robot to follow the constraint pose, constraint surface, or constraint axis, but then the performance of improving the teaching accuracy by following the constraint pose, constraint surface, or constraint axis will be degraded. Therefore, it is difficult to balance the teaching accuracy and the operability.

The present invention is made in order to solve the above-mentioned problems, and an object thereof is to provide a direct teaching device which can achieve both the teaching accuracy and the operational feeling of the robot arm.

【Technical means to solve the problem】

The direct teaching device of the present invention is characterized by comprising: an external force detection unit that detects an external force applied to a robot arm included in the robot; and a slave control calculation unit that calculates a robot arm that conforms to the external force detected by the external force detection unit The movement of the robot arm; the position and attitude calculation part, which calculates the current value or the predicted value of the position and attitude as at least one of the position and the attitude of the robot arm; the target value calculation part, which is based on the constraint target and the calculation result given by the position and attitude calculation part to calculate the target value of the restraint control; the restraint control calculation unit, which calculates the motion of the robot arm moving toward the target value calculated by the target value calculation unit according to the calculation result given by the position and attitude calculation unit; the restraint control adjustment unit, which can The speed of the motion of the manipulator calculated by the constraint control calculation part is adjusted continuously or in stages in at least three stages or more; the synthesis part, the calculation result given by the active control calculation part and the constraint control calculation part are given. and a driving control unit, which drives the manipulator according to the combined result given by the combining unit.

【Effect of invention】

According to the present invention, since it is configured as described above, it is possible to achieve both the teaching accuracy and the operational feeling of the robot arm.

Description of drawings

FIG. 1 is a diagram showing a configuration example of the direct teaching device according to the first embodiment.

FIG. 2 is a diagram showing a configuration example of a slave control calculation unit in Embodiment 1. FIG.

FIG. 3 is a diagram showing a configuration example of a constraint control calculation unit in Embodiment 1. FIG.

4 is a flowchart showing an example of the operation of the direct teaching device according to the first embodiment.

5 is a diagram showing an example of the operation of the target value calculation unit in Embodiment 1 (in the case of posture constraints).

6 is a diagram showing an example of the operation of the target value calculation unit in Embodiment 1 (in the case of posture constraints).

FIG. 7 is a diagram showing an example of a constraint control area used in the direct teaching device according to Embodiment 1. FIG.

FIG. 8 is a diagram showing an example of the operation of the restraint control adjustment unit in Embodiment 1. FIG.

FIG. 9 is a diagram showing a configuration example of a constraint control calculation unit in Embodiment 2. FIG.

FIG. 10 is a diagram showing an example of the operation of the restraint control adjustment unit in Embodiment 2. FIG.

FIG. 11 is a diagram showing a configuration example of a direct teaching device according to Embodiment 3. FIG.

FIG. 12 is a diagram showing a configuration example of a direct teaching device according to Embodiment 4. FIG.

FIG. 13 is a diagram showing a configuration example of a conventional direct teaching device.

Detailed ways

Hereinafter, embodiments of the present invention will be described in detail with reference to the accompanying drawings.

Embodiment 1.

FIG. 1 is a diagram showing a configuration example of the direct teaching apparatus 1 according to the first embodiment.

The direct teaching device 1 performs direct teaching of the robot. As shown in FIG. 1 , the direct teaching device 1 includes a position and attitude measurement unit 101, an external force detection unit 102, a slave control calculation unit 103, a position and attitude calculation unit 104, a target value calculation unit 105, a distance calculation unit 106, and a constraint control adjustment unit. unit 107 , constraint control calculation unit 108 , synthesis unit 109 , and drive control unit 110 . In addition, the direct teaching device 1 is realized by a processing circuit such as a system LSI (Large-Scale Integration) or a CPU (Central Processing Unit) that executes a program stored in a memory or the like.

The position and posture measuring unit 101 measures parameters related to the position and posture of the robot arm 2 included in the robot. In addition, the position and posture of the robot arm 2 means at least one of the position of the robot arm 2 and the posture of the robot arm 2 . In addition, the position of the robot arm 2 refers to the position of the end effector 3 (see FIG. 5 and the like) provided at the tip of the robot arm 2 , and the posture of the robot arm 2 refers to the orientation of the end effector 3 . In addition, as a parameter related to the position and posture of the robot arm 2, the joint angle (angle of each joint) of the robot arm 2 can be mentioned. This joint angle is measured using an encoder or the like mounted on the rotating shaft of the motor. In addition, the position and posture measuring unit 101 may measure the position of the robot arm 2 or the posture of the robot arm 2 as a parameter related to the position and posture of the robot arm 2 .

The external force detection unit 102 detects the external force applied by the operator to the robot arm 2 . Examples of the external force detected by the external force detection unit 102 include the magnitude and direction of the force, the magnitude of the torque, and the like.

For example, the external force detection unit 102 can use a force sensor attached to the tip of the robot arm 2 to detect the magnitude and direction of the force measured by the force sensor as the external force. In addition, for example, the external force detection unit 102 may use a torque sensor attached to a motor drive shaft of each joint of the robot arm 2 to detect the magnitude of the torque measured by the torque sensor as the external force. In addition, the external force detection unit 102 may detect the external force by using an external force observer that indirectly detects the external force based on the current of the motor included in the robot arm 2 or the measurement value of the joint angle of the robot arm 2, etc., instead of using the above-mentioned external force. sensor to directly measure the external force.

In addition, when the external force detection unit 102 uses a sensor to detect the external force, depending on the structure of the sensor, etc., there are cases in which the detection is performed in a state where not only the external force exerted by a person but also the force or torque due to gravity is superimposed. . Therefore, in such a case, it is preferable that the external force detection unit 102 obtains only the external force component by estimating and deducting the component due to gravity. This is a known technique called gravity compensation, and is disclosed in a number of documents such as Patent Document 2.

[Patent Document 2] Japanese Patent Laid-Open No. 01-066715

The slave control calculation unit 103 calculates the motion (slave control command value) of the robot arm 2 in compliance with the external force detected by the external force detection unit 102 . At this time, first, the slave control calculation unit 103 determines how the operator intends to move the robot arm 2 based on the external force detected by the external force detection unit 102 . Next, the slave control calculation unit 103 calculates a command value (slave control command value) for driving the robot arm 2 based on the above determination result. As the slave control command value, for example, the angular velocity of each joint of the robot arm 2 or the amount of movement (difference in the angle of each joint) can be cited.

The position and orientation calculation unit 104 calculates the current value or predicted value of the position and orientation of the robot arm 2 based on the measurement result given by the position and orientation measurement unit 101 .

In addition, in FIG. 1 , the position and orientation calculation unit 104 uses the measurement result given by the position and orientation measurement unit 101 and does not use the calculation result given by the slave control calculation unit 103 . However, it is not limited to this, and the position and posture calculation unit 104 may use the calculation result given by the slave control calculation unit 103 to calculate the position and posture reflecting the movement of the robot arm 2 according to the external force detected by the external force detection unit 102 . .

The target value calculation unit 105 calculates the target value of the restraint control based on the restraint target and the calculation result given by the position and attitude calculation unit 104 . In addition, the so-called constraint target is a constraint target of the position and posture of the robot arm 2 .

As a constraint target for the position of the robot arm 2, there are a constraint point, a constraint axis, and a constraint surface. The constraint point is a position when the tip of the robot arm 2 or a position determined based on the tip is constrained to a certain point. As types of constraint axes, there are straight lines and curved lines. As the types of constraint surfaces, there are planes and surfaces.

As a constraint target for the posture of the robot arm 2, that is, a constraint posture, there are a constraint direction and a constraint rotation axis. The constraint direction is the posture when the posture of the robot arm 2 is completely constrained to a certain direction. The constraint rotation axis is a rotation axis in the case of performing a constraint that allows the rotation of the posture along the designated one or two rotation axes. In this case, the posture of the robot arm 2 can be changed along the designated rotation axis, but cannot take all postures and is in a restricted state, so this is also a type of posture constraint.

In addition, the constraint target used in the direct teaching device 1 is not limited to one, but may be plural. In addition, the target value of the constraint control means at least one of the target position of the robot arm 2 and the target posture of the robot arm 2 .

The distance calculation unit 106 calculates the distance to the constraint control area based on the calculation result given by the position and attitude calculation unit 104 . The so-called restraint control area is the area in which restraint control is performed.

The restraint control adjustment unit 107 adjusts the speed (strength) of the motion of the robot arm 2 calculated by the restraint control calculation unit 108 continuously or in stages based on the calculation result given by the distance calculation unit 106 . In addition, when adjusting the speed of the movement of the robot arm 2 stepwise, the restraint control adjusting unit 107 performs the adjustment in at least three steps or more.

The restraint control calculation unit 108 calculates the motion (restraint control command value) of the robot arm 2 moving toward the target value calculated by the target value calculation unit 105 based on the calculation result given by the position and orientation calculation unit 104 .

The combining unit 109 combines the calculation result (slave control command value) given by the slave control arithmetic unit 103 and the calculation result (restraint control command value) given by the constraint control arithmetic unit 108 into one command value.

The drive control unit 110 drives the robot arm 2 according to the synthesis result given by the synthesis unit 109 .

Next, a configuration example of the slave control calculation unit 103 will be described with reference to FIG. 2 .

The slave control arithmetic unit 103 includes, for example, a multiplication unit 1031 as shown in FIG. 2 .

The multiplier 1031 multiplies the external force (magnitude of torque for each joint) detected by the external force detection section 102 by a gain, thereby obtaining a command value (driven control command value) of the angular velocity for each joint. In addition, the external force is obtained by removing external disturbances such as gravity components by the external force detection unit 102 . Also, the gain is usually a diagonal matrix. Also, the gain may be different for each joint.

表示外力检测部102所检测到的转矩的大小，K_T表示增益，

表示从动控制运算部103所算出的角速度的指令值(从动控制指令值)。Furthermore, in Figure 2,

represents the magnitude of the torque detected by the external force detection unit 102, K _T represents the gain,

The command value (slave control command value) representing the angular velocity calculated by the slave control arithmetic unit 103 .

Furthermore, the slave control calculation unit 103 may limit the speed of the slave control by applying a limiter to the obtained command value of the angular velocity of each joint.

In addition, the case where the magnitude of the torque is detected by the external force detection unit 102 as the external force, and the command value of the angular velocity of each joint is calculated by the slave control calculation unit 103 as the slave control command value is shown above, but it is not limited to this. .

For example, when the magnitude and direction of the force applied to the fingertip of the robot arm 2 (or the force applied to the tip of the end effector 3 ) are detected by the external force detection unit 102 as the external force, a similar control calculation (for example, refer to Patent Document 1). In addition, the slave control calculation unit 103 may calculate the amount of movement of the fingertip of the robot arm 2, the drive torque of each joint, and the like as the slave control command value. This control operation is disclosed in a number of documents and is well known, and various methods have been developed, so further details are omitted.

Next, a configuration example of the constraint control calculation unit 108 will be described with reference to FIG. 3 .

The constraint control calculation unit 108 includes, for example, a deviation calculation unit 1081 , a command value calculation unit 1082 , a variable conversion unit 1083 , and a restriction unit 1084 as shown in FIG. 3 . In this case, the constraint control adjustment unit 107 adjusts the behavior of the restriction unit 1084 .

The deviation calculation unit 1081 calculates a deviation between the target value calculated by the target value calculation unit 105 and the current value or predicted value of the position and orientation calculated by the position and orientation calculation unit 104 .

The command value calculation unit 1082 calculates the constraint control command value before the restriction based on the calculation result given by the deviation calculation unit 1081 . That is, the command value calculation unit 1082 calculates a command value such that the position and attitude of the robot arm 2 gradually increases toward the target value calculated by the target value calculation unit 105 based on the deviation calculated by the deviation calculation unit 1081 . The command value calculated by the command value calculation unit 1082 is referred to as a constraint control command value before restriction. In addition, the command value calculated by the command value calculation unit 1082 is limited by the limitation unit 1084 to be the constraint control command value obtained by the constraint control calculation unit 108 .

The variable conversion unit 1083 converts the command value calculated by the command value calculation unit 1082 into a variable suitable for synthesis in the synthesis unit 109 .

Furthermore, FIG. 3 shows a case where the variable conversion unit 1083 is provided in the constraint control calculation unit 108 . However, the variable conversion unit 1083 is not an essential configuration of the constraint control calculation unit 108 , and the variable conversion unit 1083 may not be provided in the constraint control calculation unit 108 .

The restriction unit 1084 restricts the pre-restriction constraint control command value calculated by the command value calculation unit 1082 to be within a predetermined range. Furthermore, when the variable is converted by the variable conversion unit 1083, the restriction unit 1084 restricts the pre-restriction constraint control command value after the conversion so that it falls within a predetermined range.

Furthermore, in Embodiment 1, the restraint control adjustment unit 107 adjusts the restriction range of the restriction unit 1084 to adjust the speed of the movement of the robot arm 2 .

Next, an operation example of the direct teaching device 1 of Embodiment 1 shown in FIG. 1 will be described with reference to FIG. 4 . In addition, hereinafter, the constraint control calculation unit 108 has the configuration shown in FIG. 3 .

In the operation example of the direct teaching device 1 according to the first embodiment shown in FIG. 1 , as shown in FIG. 4 , first, the position and posture measuring unit 101 measures parameters related to the position and posture of the robot arm 2 included in the robot (step ST401 ). ).

Then, the external force detection unit 102 detects the external force applied by the operator to the robot arm 2 (step ST402 ).

Then, the slave control calculation unit 103 calculates the motion (slave control command value) of the robot arm 2 in compliance with the external force detected by the external force detection unit 102 (step ST403 ).

Then, the position and posture calculation unit 104 calculates the current value or predicted value of the position and posture of the robot arm 2 based on the measurement result given by the position and posture measurement unit 101 (step ST404 ).

Here, when the direct teaching device 1 constrains the position, that is, when the direct teaching device 1 constrains the position of the robot arm 2 to a predetermined constraint point, constraint axis, or constraint surface, the position and attitude calculation unit 104 must at least Calculate the position of the robot arm 2.

In addition, when the direct teaching device 1 performs posture constraint, that is, when the direct teaching device 1 constrains the posture of the robot arm 2 to a predetermined constraint direction, or performs constraint only allowing rotation along the posture of the predetermined constraint axis In the case of , the position and posture calculation unit 104 needs to calculate at least the posture of the robot arm 2 .

In addition, when the direct teaching device 1 performs the position constraint and the posture constraint, the position and posture calculation unit 104 needs to calculate both the position and the posture of the robot arm 2 .

What the position and orientation calculation unit 104 obtains may be a current value based only on the measured value, or may be a future predicted value to which the calculation result given by the slave control calculation unit 103 is also introduced.

Here, the position and attitude calculation unit 104 may calculate the current value of the position and attitude by using forward kinematics operation based on the angle of each joint measured by the position and attitude measurement unit 101 . Further, the position and orientation calculation unit 104 uses the slave control command value obtained by the slave control calculation unit 103 in addition to the above, thereby calculating a future predicted value of the position and orientation assuming no constraint control.

Then, the target value calculation unit 105 calculates the target value of the restraint control based on the restraint target and the calculation result given by the position and orientation calculation unit 104 (step ST405 ).

For example, the purpose of the constraint control is to fix the Z-axis coordinate of the tip of the end effector 3 at a position of 0.1 [m], and the current value of the tip of the end effector 3 calculated by the position and attitude calculation unit 104 is [Xp, Yp, Zp]. In this case, the target value calculation unit 105 sets the target value of the constraint control as [Xp, Yp, 0.1] (in addition, in this case, the posture is not changed, so the description is omitted).

In addition, as shown in FIG. 5 , for example, the purpose of the constraint control is to set the Z-axis in the orientation of the end effector 3 to the directly downward direction (the direction indicated by reference numeral 502 ), and then set the X-axis to the X-axis of the robot coordinate system. Consistent. In addition, in FIG. 5, the code|symbol 501 shows the straight upward direction (reference direction). In this case, the target attitude, which is the target value of the constraint control, is represented by the following equation (1) in terms of a rotation matrix (in addition, in this case, the position is not changed, so the description is omitted).

It should be noted that the expression of the posture under the rotation matrix is disclosed in, for example, Non-Patent Document 1, and the details thereof are omitted.

[Non-Patent Document 1] John J.Craig (translated by Miura Shimoyama): Robotics, Kyoritsu Publishing (1991)

In addition, as the target value of the constraint control, the target value calculation unit 105 may calculate only one target value, or may calculate a plurality of target values and select one target value among them.

For example, when the purpose of the restraint control is to fix the Z-axis coordinate of the tip of the end effector 3 at the nearest position in units of 0.1 [m], the target value calculation unit 105 selects [Xp, Yp, 0.1n](n The value closest to the current position in the integer) is used as the target value of the constraint control.

In addition, as shown in FIG. 6 , the purpose of the restraint control is to set the Z axis in the direction of the end effector 3 as the direct downward direction, and use the Z axis as the rotation axis to rotate by π/2 units (90 degree units). When the obtained posture is fixed, the target value calculation unit 105 selects a value closest to the current posture as the target value of the constraint control. In this case, the target value of the constraint control, that is, the target posture is expressed by the following equation (2) expressed by a rotation matrix.

In this formula (2), as shown in FIG. 6 , there are four kinds of changes in the target posture depending on n, and the target value calculation unit 105 selects one value closest to the current posture among them. The target value calculation unit 105 can determine whether or not the current posture is approached based on the magnitude of the rotation angle when the target posture is rotated from the current posture to each target posture.

Then, the distance calculation unit 106 calculates the distance from the current value or predicted value of the position and posture of the robot arm 2 to the constraint control area based on the calculation result given by the position and posture calculation unit 104 (step ST406 ).

For example, when the direct teaching device 1 performs both the axis restraint in the Z-axis direction and the vertically downward posture restraint, first, the distance calculation unit 106 defines the axis restrained by the axis as a restraint control area. Next, the distance calculation unit 106 calculates the distance from the current value or the predicted value of the position and attitude of the robot arm 2 to the above-mentioned axis as shown in the following formula (3). Furthermore, in formula (3), l represents the distance to the axis constrained by the axis, x and y represent the X coordinate and the Y coordinate of the current value or predicted value of the position of the robot arm 2, and x _a and y _a represent the axis constrained. The X and Y coordinates of the axes.

l=√{(xx _a ) ² +(yy _a ) ² } (3)

Further, for example, when the direct teaching device 1 performs a vertically downward posture constraint, the distance calculation unit 106 first defines a range in which x and y satisfy the following formula (4) as a constraint control region as shown in FIG. 7 . . In FIG. 7, reference numeral 701 denotes a restraint control area. The restraint control area is determined so as to cover the place where work is performed on the workpiece, the place where the workpiece is picked up, or the place where the workpiece is placed and its surroundings. Next, the distance calculation unit 106 calculates the shortest distance from the current value or predicted value of the position of the robot arm 2 calculated by the position and attitude calculation unit 104 to the constraint control area.

Further, for example, when the direct teaching device 1 constrains a curved surface along a predetermined curved surface, first, the distance calculation unit 106 defines the curved surface itself as a constraint control region. Next, the distance calculation unit 106 calculates the shortest distance from the current value or predicted value of the position of the robot arm 2 calculated by the position and orientation calculation unit 104 to the curved surface.

In addition, for example, when the direct teaching device 1 performs both a plane constraint along a predetermined plane and a vertically downward posture constraint, first, the distance calculation unit 106 defines a posture conforming to the vertical downward orientation as constraint control area. Next, the distance calculation unit 106 calculates the angle at which the current value or the predicted value of the posture of the robot arm 2 calculated by the position and posture calculation unit 104 becomes the vertical downward angle as the distance. In this way, when the current or predicted direction of the fingertip is vertically downward, the distance is 0, and when the inclination from the vertical downward direction increases, the distance also increases.

Furthermore, the distance calculation unit 106 may define a plurality of constraint control regions. In this case, the distance calculation unit 106 preferably obtains the distances corresponding to the respective constraint control regions, and selects the smallest value among them.

As described above, the distance calculation unit 106 predefines an appropriate constraint control area for the purpose of direct teaching with constraints, and obtains the position and orientation of the robot arm 2 from the constraint control area to the position and posture calculation unit 104 The shortest distance to the current or predicted value of .

Then, the restraint control adjustment unit 107 continuously or stepwise adjusts the speed (strength) of the motion of the robot arm 2 calculated by the restraint control calculation unit 108 based on the distance calculated by the distance calculation unit 106 (step ST407 ). That is, the constraint control adjustment unit 107 determines parameters for adjusting the strength of the constraint control based on the distance calculated by the distance calculation unit 106 .

For example, the constraint control adjustment unit 107 sets the case where the distance is 0 as 100%, and sets a parameter for determining the strength of the constraint control as a relative value thereof. In this case, as the parameter approaches 0%, the strength of the restraint control decreases, and 0% means that the restraint control becomes ineffective.

This parameter changes continuously according to the distance, or changes stepwise in three or more steps. That is, in two stages, the strength of the restraint control has only two types: maximum and invalid, which is no different from the prior art (switching on and off of restraint control). Therefore, in order to change the parameters stepwise, three or more steps are required.

In addition, it is preferable that the value of the parameter determined by the constraint control adjustment unit 107 monotonically decreases with respect to the distance. Thereby, the restraint control can be gradually weakened as the restraint control area is left. If it is not set to monotonically decrease, the restraint control increases or decreases as the restraint control area is moved away, so there is a possibility that the operation feeling may be impaired.

Then, the restraint control calculation unit 108 calculates the motion (restraint control command value) of the robot arm 2 moving toward the target value calculated by the target value calculation unit 105 based on the calculation result given by the position and orientation calculation unit 104 (step ST408 ). That is, the constraint control calculation unit 108 obtains, by calculation, a constraint control command value in which the position and posture of the robot arm 2 gradually progress toward the target value of the position and posture calculated by the target value calculation unit 105 .

When the constraint control calculation unit 108 has the configuration shown in FIG. 3 , the following operations are performed.

In an operation example of the constraint control calculation unit 108 , first, the deviation calculation unit 1081 calculates the deviation between the target value calculated by the target value calculation unit 105 and the current value or predicted value of the position and posture calculated by the position and posture calculation unit 104 .

Here, the deviation of the position is obtained by subtracting the coordinate value of the current value from the coordinate value of the target value. The deviation of the attitude is obtained by calculating the rotation transformation from the attitude of the current value to the attitude of the target value. The deviation obtained by the deviation calculation unit 1081 is expressed as the following formula (5). In the formula (5), e represents the deviation obtained by the deviation calculation unit 1081, Δx, Δy, and Δz represent the X component, Y component, and Z component of the positional deviation, and Δrx, Δry, and Δrz represent the attitude deviation with respect to the X axis, respectively. , Y-axis, Z-axis rotation.

Next, the command value calculation unit 1082 calculates the constraint control command value before the restriction based on the calculation result given by the deviation calculation unit 1081 . That is, the command value calculation unit 1082 calculates a command value such that the position and attitude of the robot arm 2 gradually increases toward the target value calculated by the target value calculation unit 105 based on the deviation calculated by the deviation calculation unit 1081 .

In other words, the control performed by the command value calculation unit 1082 is such that the deviation calculated by the deviation calculation unit 1081 approaches 0. Such control can be realized, for example, by using, as a speed command value, a result obtained by multiplying the deviation by an appropriate gain (a 6×6 diagonal matrix) as in the following equation (6). This gain is usually set as a diagonal matrix. In formula (6), K _C represents the gain, v _x , v _y , and v _z represent the translational components in the X-axis, Y-axis, and Z-axis directions of the speed command value, and ω _x , ω _y , and ω _z represent the relative Rotational components on the X-axis, Y-axis, and Z-axis.

Alternatively, as in the following formula (7), a value obtained by normalizing the deviation with its norm or weighted norm and multiplying it by the gain may be used as the speed command value. In the case of this method, regardless of the magnitude of the deviation, the control is gradually made to the target value at a constant speed. In equation (6), the speed of control slows down as it progresses toward the target value. In formula (7), ‖e‖ represents the norm of the deviation.

In any case, the control calculation performed by the command value calculation unit 1082 may be any control calculation as long as the position and posture of the robot arm 2 gradually progress toward the target value calculated by the target value calculation unit 105 .

Next, the variable conversion unit 1083 converts the command value calculated by the command value calculation unit 1082 into an angle command value for each joint.

That is, since the speed command value obtained by the command value calculating unit 1082 is the speed command value in the Cartesian coordinate system, the variable converting unit 1083 converts it into an angular speed command value for each joint. The variable transformation unit 1083 multiplies the velocity command value in the Cartesian coordinate system from the left by the inverse matrix of the Jacobian matrix in the current value or the predicted value of the position and attitude, as shown in the following equation (8), thereby transforming it into each joint. angular velocity command value. In formula (8), J ^-1 represents the inverse of the Jacobian matrix.

In addition, when the command value calculation unit 1082 directly calculates the angular velocity command value of each joint, or when the constraint control command value calculated by the constraint control calculation unit 108 is a Cartesian coordinate system, the variable conversion unit 1083 may not be necessary. Therefore, the variable conversion unit 1083 is not an essential component.

Next, the restriction unit 1084 restricts the pre-restriction constraint control command value calculated by the command value calculation unit 1082 so that it falls within a predetermined range. Furthermore, when the variable is converted by the variable conversion unit 1083, the restriction unit 1084 restricts the pre-restriction constraint control command value after the conversion so that it falls within a predetermined range.

Furthermore, in Embodiment 1, the restraint control adjustment unit 107 adjusts the speed of the movement of the robot arm 2 by adjusting the control range of the restriction unit 1084 . Hereinafter, a case where the constraint control adjustment unit 107 restricts the angular velocity command value for each joint will be described as an example.

来表示第i关节的角速度指令值的最大上限(最大上限设为正值)。First, the limiting unit 1084 predetermines the maximum upper limit of the angular velocity command value for each joint (the maximum value of the upper limit value). This maximum upper limit is the maximum value that the constraint control command value calculated by the constraint control calculation unit 108 can take. Here, the maximum upper limit may be different for each joint or may be the same. Below, with

to indicate the maximum upper limit of the angular velocity command value of the i-th joint (the maximum upper limit is set to a positive value).

来表示对第i关节实际运用的角速度指令值的上限，则为下式(9)。式(9)中，α表示调整参数。Next, the limiting unit 1084 calculates the upper limit value of the angular velocity command value that is actually used. The upper limit value is determined based on the maximum upper limit and adjustment parameters. The upper limit of the restraint control command value calculated by the restraint control calculation unit 108 is adjusted by changing the adjustment parameter according to the output of the restraint control adjustment unit 107 , and as a result, the speed of the restraint control mobile manipulator 2 is adjusted. That is, if the

To express the upper limit of the angular velocity command value actually applied to the i-th joint, the following formula (9) is obtained. In formula (9), α represents an adjustment parameter.

This adjustment parameter is a value determined in the range of 0 or more and 1 or less in accordance with the value of the parameter output from the constraint control adjustment unit 107 . When the constraint control adjustment unit 107 outputs 100%, it is set to 1, and when 0% is output, it is set to 0.

FIG. 8 shows the relationship between the distance calculated by the distance calculation unit 106 and the adjustment parameter. When the distance is 0, the adjustment parameter is 1, and as the distance increases, the adjustment parameter decreases continuously or in stages, and when the distance is greater than a predetermined value, the adjustment parameter becomes 0. Thereby, the constraint control adjustment unit 107 adjusts the control operation of the constraint control calculation unit 108 . As a result, when the distance calculated by the distance calculation unit 106 is 0, the upper limit of the actually used angular velocity command value is maximized, and the speed of the restraint-controlled mobile arm 2 is maximized. This is the case where the current value or the predicted value of the position and posture of the robot arm 2 calculated by the position and posture calculating unit 104 is within the constraint control region. Then, when the distance calculated by the distance calculation unit 106 increases, that is, when the current value or predicted value of the position and attitude of the robot arm 2 leaves the restraint control area, the upper limit of the actually used angular velocity command value decreases, and the movement is restrained by restraint control. The speed of the robotic arm 2 is also limited and slowed down. When the distance calculated by the distance calculation unit 106 reaches a predetermined value or more, that is, when the current value or predicted value of the position and attitude of the robot arm 2 is sufficiently out of the constraint control area, the upper limit of the actually used angular velocity command value becomes 0, and the constraint Control becomes invalid. At this time, the operator can move the robot arm 2 freely without being restrained.

表示第i关节的角速度指令值。When the upper limit of the angular velocity command value actually used is set, the limiter 1084 limits the value so that the angular velocity command value of all joints does not exceed the upper limit. That is, the angular velocity command value of each joint is limited so that the angular velocity command value of the i-th joint satisfies the following formula (10). In formula (10),

Indicates the angular velocity command value of the i-th joint.

When the restriction unit 1084 restricts the angular velocity command value for each joint, it is preferable to restrict the movement direction of the robot arm 2 by the restriction control so that the direction of movement of the robot arm 2 does not change. An example of the order in which such an operation is performed is shown below.

First, the limiting unit 1084 obtains the ratio of the magnitude of the angular velocity command value to the upper limit of the actually used angular velocity command value for each joint as shown in the following equation (11). In formula (11), _ci represents a ratio.

Next, the limiting unit 1084 obtains the maximum value of the above-mentioned ratio.

表示限制后的角速度指令值，c表示比的最大值。Next, the restricting unit 1084 divides the angle command value of each joint angle by the maximum value of the above ratio to calculate the restricted angular velocity command value. In formula (12),

Represents the angular velocity command value after limitation, and c represents the maximum value of the ratio.

Next, the limiting unit 1084 outputs the angular acceleration command value obtained as described above as the restraint control command value.

Returning to the description of the flow shown in FIG. 4 again, the combining unit 109 combines the calculation result (slave control command value) given by the slave control calculation unit 103 and the calculation result (restraint control) given by the constraint control calculation unit 108 . command value) are combined into one command value (step ST409).

作为从动控制指令值，约束控制运算部108算出关节的角速度指令值

作为约束控制指令值。在该情况下，合成部109获得它们的和即

作为指令值。For example, the slave control calculation unit 103 calculates the angular velocity command value of the joint

As the slave control command value, the constraint control calculation unit 108 calculates the angular velocity command value of the joint

as the constraint control instruction value. In this case, the synthesis section 109 obtains their sum, namely

as the command value.

Of course, it is not limited to this. The driven control calculation unit 103 calculates the torque command value (τ _D ) as the driven control command value, and the restraint control calculation unit 108 calculates the torque command value (τ _C ) as the restraint control command value. In this case, the synthesizing section 109 obtains their sum, τ _D + τ _C , as a command value. Further, the slave control calculation unit 103 calculates the position command value (θ _D ) as the slave control command value, and the constraint control calculation unit 108 calculates the angular change amount (Δθ _C ) of the joint as the constraint control command value. In this case, the synthesis section 109 obtains their sum, θ _D + Δθ _C , as a command value. In addition, even when the physical quantities calculated by the slave control calculation unit 103 and the constraint control calculation unit 108 are different, the combination unit 109 may combine the command values by control calculation.

Then, the drive control unit 110 drives the robot arm 2 according to the synthesis result given by the synthesis unit 109 (step ST410 ). That is, the drive control unit 110 controls the robot arm 2 according to the command value. The control of this part is the usual control of the robot arm 2, so details are omitted.

Here, the direct teaching with constraints is controlled so that the robot arm 2 follows the constraint posture, constraint surface, or constraint axis through the calculation of constraint control, but the strength can be adjusted. Therefore, in the direct teaching device 1 of the first embodiment, instead of performing the constraint control or not, it is performed continuously or in stages, for example, the closer the constraint posture, constraint surface, or constraint axis is, the stronger the The restraint control and the adjustment of the restraint control are weakened as the distance from the restraint posture, restraint surface or restraint axis is increased, thereby reducing sudden motion and vibration as in the prior art. On the other hand, in the direct teaching device 1 according to the first embodiment, the restraint control is enhanced when approaching the restraint control area, and thus the problem that the accuracy of direct teaching is less likely to deteriorate as when the restraint control is simply weakened. In this way, the direct teaching device 1 of the first embodiment can achieve both the teaching accuracy and the operational feeling of the robot arm 2 .

As described above, according to the first embodiment, 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 slave control calculation unit 103 that calculates the compliance with the external force detection unit 102 The motion of the robot arm 2 by the detected external force; the position and attitude calculation part 104, which calculates at least one of the position or the attitude of the robot arm 2, that is, the current value or the predicted value of the position and attitude; the target value calculation part 105, which according to constraints The target value and the calculation result given by the position and attitude calculation unit 104 are used to calculate the target value of the restraint control; The movement of the robot arm 2 whose value is moved; the constraint control adjustment unit 107 can continuously or in stages adjust the speed of the movement of the robot arm 2 calculated by the constraint control calculation unit 108 continuously or in stages in at least three stages or more; the synthesis unit 109 , which synthesizes the calculation result given by the slave control computing part 103 and the computing result given by the constraint control computing part 108 ; As a result, the direct teaching device 1 according to the first embodiment can achieve both the teaching accuracy and the operating feeling of the robot arm 2 .

Embodiment 2.

In Embodiment 1, the case where the restriction control unit 1084 included in the restriction control calculation unit 108 is adjusted by the restriction control adjustment unit 107 to adjust the speed of the restriction control movable manipulator 2 is shown, but the method of adjusting the speed is not Not limited to this. In Embodiment 2, the case where the speed of the constraint control moving manipulator 2 is adjusted by adjusting the multiplication unit 1085 included in the constraint control calculation unit 108 by the constraint control adjustment unit 107 is shown.

FIG. 9 is a diagram showing a configuration example of the constraint control calculation unit 108 in the second embodiment. The constraint control calculation unit 108 in Embodiment 2 shown in FIG. 9 is changed from the command value calculation unit 1082 to the multiplication unit 1085 and the restriction unit is removed from the constraint control calculation unit 108 in Embodiment 1 shown in FIG. 3 . 1084. In addition, the constraint control adjustment unit 107 adjusts the multiplication unit 1085 . The other configuration of the direct teaching device 1 of Embodiment 2 is the same as that of the direct teaching device 1 of Embodiment 1 shown in FIGS. 1 and 3 , and therefore the same reference numerals are attached, and only different parts will be described.

The multiplication unit 1085 multiplies the deviation calculated by the deviation calculation unit 1081 by the adjustment gain.

Then, the constraint control adjustment unit 107 adjusts the adjustment gain multiplied by the multiplication unit 1085 to adjust the speed of the movement of the robot arm 2 .

As shown in the following equation (13), the multiplication unit 1085 multiplies the deviation calculated by the deviation calculation unit 1081 by the adjustment gain (gain and adjustment parameter). Here, the gain is a 6×6 matrix (preferably a diagonal matrix), and the adjustment parameter is a scalar.

The multiplication unit 1085 changes the value of the adjustment parameter in the range of 0 to 1 according to the output of the constraint control adjustment unit 107 . When the constraint control adjustment unit 107 outputs 100%, the multiplication unit 1085 sets the adjustment parameter to 1, and when the constraint control adjustment unit 107 outputs 0%, the multiplication unit 1085 sets the adjustment parameter to 0.

FIG. 10 shows the relationship between the distance calculated by the distance calculation unit 106 and the adjustment parameter.

When the distance is 0, the adjustment parameter is 1, and as the distance increases, the adjustment parameter decreases continuously or in stages, and when the distance is greater than a predetermined value, the adjustment parameter becomes 0. Thereby, the constraint control adjustment unit 107 adjusts the control operation of the constraint control calculation unit 108 . As a result, when the distance calculated by the distance calculation unit 106 is 0, the restraint control command value calculated by the restraint control calculation unit 108 is maximized, and the speed of the restraint control mobile arm 2 is maximized. Then, as the distance calculated by the distance calculation unit 106 increases, the constraint control command value decreases, and the speed of the constraint control moving robot arm 2 also decreases. When the distance calculated by the distance calculation unit 106 is equal to or greater than a predetermined value, the restraint control becomes invalid, and the operator can move the robot arm 2 freely without restraint.

In the direct teaching device 1 of the second embodiment, the speed of the restraint control moving robot arm 2 is adjusted according to the distance from the restraint control area as described above.

Embodiment 3.

In Embodiments 1 and 2, the restraint control adjustment unit 107 adjusts the restraint control speed based on the distance calculated by the distance calculation unit 106 , but the value used as the basis for adjusting restraint control speed is not limited to this. In Embodiment 3, the case where the speed of the motion of the robot arm 2 calculated by the constraint control computing unit 108 is adjusted in accordance with the amount (operation amount) of the robot arm 2 operated by the human hand is shown.

FIG. 11 is a diagram showing a configuration example of the direct teaching device 1 according to the third embodiment. The direct teaching apparatus 1 of Embodiment 3 shown in FIG. 11 has the distance calculation unit 106 changed to the manual operation amount calculation unit 111 as compared with the direct teaching apparatus 1 of Embodiment 1 shown in FIG. 1 . The other configuration of the direct teaching apparatus 1 of Embodiment 3 is the same as that of the direct teaching apparatus 1 of Embodiment 1, and therefore the same reference numerals are attached, and only different parts will be described.

The manual operation amount calculation unit 111 calculates the operation amount of the manipulator 2 by the operator based on the calculation result given by the slave control calculation unit 103 .

Then, the restraint control adjustment unit 107 adjusts the speed of the movement of the robot arm 2 based on the calculation result given by the manual operation amount calculation unit 111 .

As a preferable example, the slave control command value calculated by the slave control calculation unit 103 is used as the angular acceleration command value, and the manual operation amount calculation unit 111 calculates the operation amount according to the magnitude of the angular acceleration command value. This operation amount adjusts the speed of the movement of the robot arm 2 calculated by the constraint control computing unit 108 . Further, the restraint control adjustment unit 107 adjusts the movement speed of the robot arm 2 calculated by the restraint control calculation unit 108 so that the larger the operation amount is, the slower the movement speed of the robot arm 2 is. The speed of the movement is adjusted in a faster way. Accordingly, when the operator operates the robot arm 2 quickly, the restraint control is weakened, so that the operator can move the robot arm 2 freely without being restrained. In addition, when the robot arm 2 is slowly operated, the constraint control is enhanced, so that the robot arm 2 can be operated with high accuracy in compliance with the constraint, and the teaching accuracy of the teaching control can be improved.

In addition, not limited to the above example, the direct teaching device 1 may obtain the robot arm based on the angle of each joint measured by the position and posture measuring unit 101 or the current value or predicted value of the position and posture calculated by the position and posture calculating unit 104 The angular velocity of each joint of 2, and then adjust according to this value. Among them, the slave control command value calculated by the slave control calculation unit 103 is determined by the operation of the operator. On the other hand, the angular velocity calculated from these values is also affected by the calculation result of the restraint control, so there is an operation by the operator. The case is not well reflected in the adjustment operation of the restraint control adjustment unit 107 .

In addition, the manual operation amount calculation unit 111 may obtain the operation amount from not only the angular velocity, but also the speed, angular acceleration, and the like of the fingertip of the robot arm 2 from other physical quantities.

Embodiment 4.

Regarding the restraint control adjustment unit 107 , in Embodiments 1 and 2, the speed of restraint control is adjusted based on the distance calculated by the distance calculation unit 106 , and the third embodiment shows the operation calculated by the manual operation amount calculation unit 111 . Amount to adjust the speed of the constraint control. These can be considered as motions of the robot arm 2 calculated by the constraint control adjustment unit 107 based on the calculation results of one or more of the slave control calculation unit 103 , the position and orientation calculation unit 104 , and the target value calculation unit 105 to the constraint control calculation unit 108 . speed to adjust. In a sense, the speed of constraint control is not adjusted directly by a person, but is adjusted indirectly by calculating the result. On the other hand, it is not limited to this, and a method in which a human directly adjusts the speed of the motion of the robot arm 2 calculated by the constraint control calculation unit 108 is also conceivable.

FIG. 12 is a diagram showing a configuration example of the direct teaching device 1 according to the fourth embodiment. The direct teaching apparatus 1 according to the fourth embodiment shown in FIG. 12 has the distance calculation unit 106 changed to the input unit 112 as compared with the direct teaching apparatus 1 according to the first embodiment shown in FIG. 1 . The other structures in the direct teaching apparatus 1 of Embodiment 4 are the same as those of the direct teaching apparatus 1 of Embodiment 1, and therefore the same reference numerals are used to omit the description.

The input unit 112 accepts input from the operator.

Then, the constraint control adjustment unit 107 adjusts the speed of the movement of the robot arm 2 according to the reception result given by the input unit 112 .

An example of the input unit 112 includes two buttons provided on the main body of the robot arm 2 or a teaching pendant for robot operation. In this case, the restraint control adjustment unit 107 adjusts the speed of the motion of the robot arm 2 calculated by the restraint control calculation unit 108 in at least three steps, that is, when one of the two buttons is pressed. Speed up one level, and when the other side is pressed, slow down one level.

Further, another example of the input unit 112 is a slider for adjustment (such as a volume slider for volume adjustment) provided on the main body of the robot arm 2 or a teaching pendant for robot operation. In this case, the restraint control adjustment unit 107 continuously adjusts the movement speed of the robot arm 2 calculated by the restraint control calculation unit 108 when the slider is moved to one side, and the speed of the motion of the robot arm 2 calculated by the restraint control calculation unit 108 is further increased, and when the slider is moved to the other side When the slider is moved, the speed of the motion of the robot arm 2 calculated by the constraint control calculation unit 108 is further reduced. In addition, the slider for adjustment may be configured to operate a slider displayed on a screen such as a teaching pendant, instead of a physical slider.

Moreover, as another example of the input part 112, the sensor provided in the robot arm 2 and detecting the force with which the operator grips the robot arm 2 can be mentioned. In this case, the restraint control adjustment unit 107 performs adjustment based on the output of the sensor. For example, the restraint control adjustment unit 107 continuously adjusts so as to further increase the speed of the motion of the robot arm 2 calculated by the restraint control calculation unit 108 when the force for grasping the robot arm 2 is weakened, and when the force for grasping the robot arm 2 is further accelerated. When the force increases, the speed of the motion of the robot arm 2 calculated by the constraint control calculation unit 108 is further reduced.

Moreover, as another example of the input part 112, a foot pedal is mentioned. In this case, the restraint control adjustment unit 107 adjusts according to the amount by which the foot pedal is depressed. For example, the restraint control adjustment unit 107 continuously or in stages adjusts the speed of the motion of the robot arm 2 calculated by the restraint control calculation unit 108 to be the maximum in a state where the foot pedal is not stepped on at all. , when the stepped amount increases, the speed gradually decreases, and when the pedal is stepped to the bottom, the speed is set to 0, and the constraint control operation becomes essentially invalid.

In the direct teaching device 1 according to the fourth embodiment, as described above, the input unit 112 detects an input from a person, and the restraint control adjustment unit 107 controls the robot arm 2 calculated by the calculation unit 108 on the basis of the detected value. The speed of the movement can be adjusted.

In addition, in Embodiments 1 to 4, a part of the descriptions and examples are based on the premise of the 6-axis robot arm 2 , but it can be implemented in the same manner even if it is a robot arm 2 other than 6-axis.

In addition, in Embodiments 1 to 4, the control command value calculated by the slave control calculation unit 103 and the restraint control command value calculated by the restraint control calculation unit 108 are performed centered on the example of the command value of the angular velocity. , but even other physical quantities such as angular acceleration or torque can be realized.

In addition, the invention of the present application can freely combine the respective embodiments, modify any constituent elements of the respective embodiments, or omit any constituent elements in the respective embodiments within the scope of the present invention.

Symbol Description

1...Direct teaching device

2…Robot arm

3…End effector

101...Position and attitude measurement unit

102...External force detection department

103...Slave control calculation part

104...Position and attitude calculation unit

105...Target value calculation part

106...Distance calculation section

107...Restriction control adjustment section

108...Constraint control calculation unit

109…Synthesis Department

110...Drive control section

111...Manual operation amount calculation part

112...Input section

1031…Multiplication Department

1081...Difference calculation unit

1082...Command value calculation unit

1083...Variable Transformer

1084…Restriction Department

1085…Multiplication Department.

Claims (7)

1. A direct teaching device is characterized by comprising:

an external force detection unit that detects an external force applied to a robot arm of the robot;

a slave control calculation unit that calculates the movement of the robot arm in accordance with the external force detected by the external force detection unit;

a position/orientation calculation unit that calculates a current value or a predicted value of a position/orientation of at least one of a position and an orientation of the robot arm;

a target value calculation unit that calculates a target value of the restraint control based on the restraint target and the calculation result by the position and orientation calculation unit;

a constraint control calculation unit that calculates the motion of the robot arm moving toward the target value calculated by the target value calculation unit, based on the calculation result given by the position and orientation calculation unit;

a constraint control adjustment unit capable of adjusting the velocity of the motion of the robot arm calculated by the constraint control calculation unit continuously or in a stepwise manner in at least three stages;

a synthesizing unit that synthesizes the calculation result given by the slave control calculating unit and the calculation result given by the constraint control calculating unit; and

and a drive control unit that drives the robot arm according to the result of the combination given by the combination unit.

2. The direct teaching device according to claim 1,

the constraint control calculation unit includes:

a deviation calculation unit that calculates a deviation between the target value calculated by the target value calculation unit and the current value or the predicted value of the position and orientation calculated by the position and orientation calculation unit;

a command value calculation unit that calculates a constraint control command value before limitation corresponding to a calculation result of the constraint control calculation unit, based on a calculation result given by the deviation calculation unit; and

a limiting unit that limits the pre-limit constraint control command value calculated by the command value calculating unit,

the restraint control adjustment unit adjusts the speed of the movement of the robot arm by adjusting the restriction unit.

3. The direct teaching device according to claim 1,

the constraint control calculation unit includes:

a deviation calculation unit that calculates a deviation between the target value calculated by the target value calculation unit and the current value or the predicted value of the position and orientation calculated by the position and orientation calculation unit; and

a multiplication unit for multiplying the calculation result given by the deviation calculation unit by an adjustment gain,

the constraint control adjustment unit adjusts the adjustment gain multiplied by the multiplication unit, thereby adjusting the speed of the movement of the robot arm.

4. The direct teaching device according to any one of claims 1 to 3,

a distance calculation unit that calculates a distance from a current value or a predicted value of the position and orientation of the robot arm to a constraint control area based on a calculation result given by the position and orientation calculation unit,

the constraint control adjustment unit adjusts the speed of the movement of the robot arm based on the calculation result from the distance calculation unit.

5. The direct teaching device according to any one of claims 1 to 3,

the robot control device is provided with a manual operation amount calculation unit which calculates the operation amount of the robot arm by the operator,

the constraint control adjustment unit adjusts the speed of the movement of the robot arm based on the calculation result given by the manual operation amount calculation unit.

6. The direct teaching device according to any one of claims 1 to 3,

comprises an input unit for receiving an input from an operator,

the constraint control adjustment unit adjusts the speed of the movement of the robot arm in accordance with the reception result given by the input unit.

7. A direct teaching method is characterized by comprising the following steps:

an external force detection unit that detects an external force applied to a robot arm of the robot;

a slave control calculation unit for calculating the movement of the robot arm in accordance with the external force detected by the external force detection unit;

a position/posture calculation unit that calculates a current value or a predicted value of a position/posture of at least one of a position and a posture of the robot arm;

a target value calculation unit for calculating a target value of the restraint control based on the restraint target and the calculation result by the position and orientation calculation unit;

a constraint control calculation unit that calculates the motion of the robot arm moving toward the target value calculated by the target value calculation unit, based on the calculation result given by the position and orientation calculation unit;

a constraint control adjustment unit capable of adjusting the velocity of the motion of the robot arm calculated by the constraint control calculation unit continuously or in a stepwise manner for at least three stages;

a synthesizing unit that synthesizes the calculation result given by the slave control calculating unit and the calculation result given by the constraint control calculating unit; and

a drive control unit drives the robot arm in accordance with the result of the synthesis by the synthesis unit.

Images