JP2022114472A - Direct teaching device and direct teaching method for robot - Google Patents

Abstract

To achieve both teaching accuracy and operational feeling of arms.SOLUTION: A direct teaching device for a robot includes: a slave control operation section 103 for calculating a slave control command value; a restraint control operation section 108 for calculating a restraint control command value; a restraint control adjustment section 107 capable of adjusting the speed of the motions of arms 2 calculated by the restraint control operation section 108 continuously or gradually in at least three or more steps; a synthesis section 109 for synthesizing a calculation result by the slave control operation section 103 and a calculation result by the restraint control operation section 108; and a drive control section 110 for driving the arms 2 based on a synthetic result by the synthesis section 109.SELECTED DRAWING: Figure 1

Description

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

In industrial robots, work called teaching is carried out in advance in order to cause the robot to perform work. One method of teaching the robot 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. This direct teaching device is characterized in that direct teaching with a constraint and direct teaching without a constraint can be switched.

The purpose of direct teaching with constraints is to improve teaching accuracy. While a normal direct teaching device can intuitively operate and teach a robot, it is difficult to improve the teaching accuracy because the position and orientation of the robot are easily changed by an external force. On the other hand, in direct teaching with restraints, the hand is constrained to face vertically downwards, and the hand is operated and taught; Constraining the robot so that it is positioned on the axis, or allowing the robot to be manipulated and taught only within a predetermined constraint range. By doing this, it is possible to teach the robot so that the posture is precisely vertically downward, and the position is precisely on a predetermined plane, curved surface, or on an axis. is doing.

On the other hand, direct teaching with constraints is not always necessary. For this reason, it is desirable that direct teaching without restraints is used when unnecessary, and that it is possible to operate and teach freely without restraints. Therefore, in the direct teaching device of Patent Document 1, when the posture or position to be restrained is approached, direct teaching with restraint is performed, and otherwise, direct teaching without restraint is automatically switched. .

FIG. 13 shows a schematic configuration diagram of the direct teaching device 1b of Patent Document 1. As shown in FIG.
In the direct teaching device 1b, first, the position/orientation measurement unit 101b measures parameters relating to the position and orientation of the arm 2 of the robot. Note that parameters relating to the position and orientation of the arm 2 include the position of the arm 2, the orientation of the arm 2, the joint angle of the arm 2, and the like.
Also, the external force detection unit 102b detects an external force applied to the arm 2 by the operator.
Next, the driven control calculation unit 103b calculates the movement (driven control command value) of the arm 2 according to the external force detected by the external force detection unit 102b based on the measurement result of the position/orientation measurement unit 101b.

Next, the position/orientation calculation unit 104b calculates the position/orientation of the arm 2 based on the calculation result by the driven control calculation unit 103b. The position and orientation of the arm 2 means at least one of the position and orientation of the arm 2 .
Next, the proximity determination unit 105b determines whether the position/orientation of the arm 2 is approaching the constraint target based on the calculation result by the position/orientation calculation unit 104b.
Next, switching section 106b switches the operation of restraint control calculation section 108b according to the determination result of proximity determination section 105b. That is, when the proximity determination unit 105b determines that the position and orientation of the arm 2 are approaching the constraint target, the switching unit 106b enables the processing by the constraint control calculation unit 108b. On the other hand, when the proximity determination unit 105b determines that the position/orientation of the arm 2 is not close to the constraint target, the switching unit 106b disables the processing by the constraint control calculation unit 108b.

Next, the target value calculation unit 107b calculates a target value for constraint control based on the result of calculation by the constraint target and position/orientation calculation unit 104b. For example, the target value calculation unit 107b uses a predetermined posture (restraint posture), a predetermined curved surface or plane (restraint surface), or the position and posture of the arm 2 along a predetermined axis or curve (restraint axis) as the target value. calculate.
Next, the constraint control calculation unit 108b calculates the movement (constraint control command value) of the arm 2 moving to the target value calculated by the target value calculation unit 107b, based on the calculation result of the position/orientation calculation unit 104b.
Next, the constraint control limiter 109b limits the calculation result by the constraint control calculator 108b. That is, the constraint control limiter 109b imposes a limit on the calculation result by the constraint control calculator 108b so that the calculation result by the driven control calculator 103b has priority over the calculation result by the constraint control calculator 108b.

Next, the synthesizer 110b synthesizes the calculation result (driven control command value) by the driven control calculation unit 103b and the limit result (constraint control command value) by the constraint control limiter 109b into one command value.
Next, the drive control section 111b drives the arm 2 based on the result of synthesis by the synthesis section 110b.

In the direct teaching device 1b shown in FIG. 13, the series of operations described above controls the movement of the arm 2 in accordance with an instruction given by a person's force. After that, the direct teaching device 1b records the joint angle or the position and posture at that time as a teaching point when the position and posture of the arm 2 become the states intended by the operator. The recorded teaching points are used when the robot works. In the direct teaching device 1b, the switching unit 106b switches the operation of the constraint control calculation unit 108b, so that direct teaching can be performed while switching between direct teaching with constraint and direct teaching without constraint.

JP 2019-202383 A

However, since the direct teaching device of Patent Document 1 has a choice of whether or not to perform the constraint control calculation, the arm may suddenly move or vibrate when the constraint control calculation is switched on and off. rice field. For this reason, the operational feeling of the operator may be impaired.
This problem can be alleviated by weakening the force that causes the arm to follow the constraint posture, constraint surface, or constraint axis. performance deteriorates. Therefore, it has been difficult to achieve both teaching accuracy and operational feeling.

SUMMARY OF THE INVENTION It is an object of the present invention to provide a direct teaching device capable of achieving both an arm teaching accuracy and an operational feeling.

A direct teaching device according to the present invention includes an external force detection unit that detects an external force applied to an arm of a robot, a driven control calculation unit that calculates the movement of the arm according to the external force detected by the external force detection unit, and an arm position detection unit. Alternatively, a position/orientation calculation unit that calculates a current value or a predicted value of the position/orientation that is at least one of the orientations, and a target value that calculates the target value of the constraint control based on the constraint target and the calculation result by the position/orientation calculation unit. a calculation unit, a constraint control calculation unit that calculates the movement of the arm moving to the target value calculated by the target value calculation unit based on the calculation result of the position/orientation calculation unit, and the arm calculated by the constraint control calculation unit A constraint control adjustment unit that can adjust the speed of movement of the body continuously or in steps of at least three steps, a synthesis unit that synthesizes the calculation result of the driven control calculation unit and the calculation result of the constraint control calculation unit, and a drive control unit that drives the arm based on the result of synthesis by the synthesis unit.

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

1 is a diagram showing a configuration example of a direct teaching device according to Embodiment 1; FIG. 4 is a diagram showing a configuration example of a driven control calculation unit according to Embodiment 1; FIG. 4 is a diagram showing a configuration example of a constraint control calculation unit according to Embodiment 1; FIG. 4 is a flowchart showing an operation example of the direct teaching device according to Embodiment 1; FIG. 10 is a diagram showing an operation example of a target value calculation unit according to Embodiment 1 (in the case of posture constraint); FIG. 10 is a diagram showing an operation example of a target value calculation unit according to Embodiment 1 (in the case of posture constraint); FIG. 4 is a diagram showing an example of a restraint control area used in the direct teaching device according to Embodiment 1; FIG. 4 is a diagram showing an operation example of a constraint control adjustment unit according to Embodiment 1; FIG. FIG. 10 is a diagram showing a configuration example of a constraint control calculation unit according to Embodiment 2; FIG. 10 is a diagram showing an operation example of a constraint control adjustment unit according to Embodiment 2; FIG. 11 is a diagram showing a configuration example of a direct teaching device according to Embodiment 3; FIG. 12 is a diagram showing a configuration example of a direct teaching device according to Embodiment 4; It is a figure which shows the structural example of the conventional direct teaching apparatus.

BEST MODE FOR CARRYING OUT THE INVENTION Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
Embodiment 1.
FIG. 1 is a diagram showing a configuration example of a direct teaching device 1 according to Embodiment 1. As shown in FIG.
A direct teaching device 1 performs direct teaching of a robot. As shown in FIG. 1, the direct teaching device 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 target value calculation unit 105, a distance calculation unit 106, and a constraint control adjustment unit. It has a unit 107 , a constraint control calculation unit 108 , a synthesizing unit 109 and a drive control unit 110 . The direct teaching device 1 is implemented 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.

A position and orientation measurement unit 101 measures parameters relating to the position and orientation of the arm 2 of the robot. The position and orientation of the arm 2 means at least one of the position and orientation of the arm 2 . The position of the arm 2 means the position of the end effector 3 (see FIG. 5 etc.) provided at the tip of the arm 2 , and the orientation of the arm 2 means the orientation of the end effector 3 . In addition, the joint angle (angle of each joint) of the arm 2 is given as a parameter related to the position and orientation of the arm 2 . This joint angle is measured using an encoder or the like attached to the rotating shaft of the motor. The position and orientation measurement unit 101 may measure the position of the arm 2 or the orientation of the arm 2 as a parameter related to the position and orientation of the arm 2 .

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

For example, the external force detection unit 102 may use a force sensor attached to the tip of the arm 2 and detect the magnitude and direction of the force measured by this force sensor as the external force. Further, for example, the external force detection unit 102 may use a torque sensor attached to the motor drive shaft of each joint of the arm 2 and detect the magnitude of the torque measured by this torque sensor as the external force. In addition, the external force detection unit 102 does not directly measure the external force using the sensor as described above, but indirectly detects the external force from the current of the motor of the arm 2 or the measured value of the joint angle of the arm 2. An external force observer may be used to detect the external force.

When a sensor is used to detect an external force, the external force detection unit 102 may detect not only the external force applied by a person but also the force or torque caused by gravity, depending on the structure of the sensor. be. Therefore, in such a case, the external force detection unit 102 should obtain only the external force component by estimating and subtracting the component caused by gravity. This is a well-known technique called gravity compensation, and is disclosed in a plurality of documents such as Patent Document 2.
JP-A-01-066715

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 . At this time, first, the driven control calculation unit 103 determines how the operator intends to move the arm 2 from the external force detected by the external force detection unit 102 . Then, the driven control calculation unit 103 calculates a command value (driven control command value) for driving the arm 2 based on the above determination result. Examples of the driven control command value include the angular velocity of each joint of the arm 2 or the amount of movement (difference between angles of each joint).

The position/orientation calculation unit 104 calculates current values or predicted values of the position/orientation of the arm 2 based on the measurement result of the position/orientation measurement unit 101 .

Note that in FIG. 1 , the position/orientation calculation unit 104 uses the measurement result of the position/orientation measurement unit 101 and does not use the calculation result of the driven control calculation unit 103 . However, not limited to this, the position/orientation calculation unit 104 uses the calculation result by the driven control calculation unit 103 to calculate the position/orientation reflecting the movement of the arm 2 according to the external force detected by the external force detection unit 102 . good too.

The target value calculation unit 105 calculates a target value for constraint control based on the result of calculation by the constraint target and position/orientation calculation unit 104 . Note that the constraint target is a constraint destination of the position and orientation of the arm 2 .
Constraint targets for the position of the arm 2 include constraint points, constraint axes, and constraint surfaces. A constraint point is a position where the tip of the arm 2 or a position determined with reference to the tip is constrained to a specific point. Types of constraint axes include straight lines and curved lines. Types of constraint surfaces include flat surfaces and curved surfaces.
A constraint target for the posture of the arm 2, that is, a constraint posture, includes a constraint direction and a constraint rotation axis. A constraint direction is a posture when the posture of the arm 2 is completely constrained in a certain direction. A constrained rotation axis is a rotation axis when a constraint is applied to allow rotation of the posture along one or two designated rotation axes. In this case, although the posture of the arm 2 can be changed along the designated rotation axis, it cannot take any posture and is in a restricted state, so this is also a kind of posture constraint.
Further, the number of restraint targets used in the direct teaching device 1 is not limited to one, and may be plural. Also, the target value of constraint control means at least one of the target position of the arm 2 and the target orientation of the arm 2 .

The distance calculation unit 106 calculates the distance to the constraint control area based on the calculation result by the position/orientation calculation unit 104 . A constraint control area is an area in which constraint control is performed.

The constraint control adjustment unit 107 continuously or stepwise adjusts the movement speed (strength) of the arm 2 calculated by the constraint control calculation unit 108 based on the calculation result of the distance calculation unit 106 . When adjusting the movement speed of the arm 2 stepwise, the restraint control adjustment unit 107 performs adjustment in at least three steps.

The constraint control calculation unit 108 calculates the movement (constraint control command value) of the arm 2 moving to the target value calculated by the target value calculation unit 105 based on the calculation result of the position/orientation calculation unit 104 .

The synthesis unit 109 synthesizes the calculation result (driven control command value) by the driven control calculation unit 103 and the calculation result (constraint control command value) by the constraint control calculation unit 108 into one command value.

The drive control section 110 drives the arm 2 based on the result of synthesis by the synthesis section 109 .

Next, a configuration example of the driven control calculation unit 103 will be described with reference to FIG.
The driven control calculation unit 103 has a multiplication unit 1031 as shown in FIG. 2, for example.

Multiplying section 1031 multiplies the external force (magnitude of torque for each joint) detected by external force detection section 102 by a gain to obtain an angular velocity command value (following control command value) for each joint. It is assumed that the external force detection unit 102 has removed disturbances such as gravity components from the external force. Also, the gain is typically a diagonal matrix. Also, the gain may be different for each joint.
2, τ (hat) _ext indicates the magnitude of the torque detected by the external force detection unit 102, _KT indicates the gain, and θ (dot) _D indicates the angular velocity calculated by the driven control calculation unit 103. command value (driven control command value).

Note that the driven control calculation unit 103 can apply a limiter to the obtained angular velocity command value for each joint to limit the speed of the driven control.

In the above description, the external force detection unit 102 detects the magnitude of the torque as the external force, and the driven control calculation unit 103 calculates the angular velocity command value for each joint as the driven control command value. do not have.
For example, when the external force detection unit 102 detects the magnitude and direction of the force applied to the tip of the arm 2 (or the force applied to the tip of the end effector 3) as the external force, similar control calculations are possible (for example, Reference 1). Further, the driven control calculation unit 103 may calculate the amount of movement of the hand of the arm 2 or the driving torque of each joint as the driven control command value. Since this control operation has been published in a number of documents and is well known, and various methods have been developed, further details will be omitted.

Next, an example configuration of the constraint control calculation unit 108 will be described with reference to FIG.
The constraint control calculation unit 108 has, for example, a deviation calculation unit 1081, a command value calculation unit 1082, a variable conversion unit 1083, and a limit unit 1084, as shown in FIG. In this case, the constraint control adjuster 107 adjusts the behavior of the limiter 1084 .

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

Command value calculation section 1082 calculates a constraint control command value before limitation based on the calculation result of deviation calculation section 1081 . That is, command value calculation section 1082 calculates a command value such that the position/orientation of arm 2 asymptotically approaches the target value calculated by target value calculation section 105 based on the deviation calculated by deviation calculation section 1081 . The command value calculated by the command value calculation unit 1082 is called a restraint control command value before limitation. The command value calculated by the command value calculator 1082 is limited by the limiter 1084 to be the constraint control command value obtained by the constraint control calculator 108 .

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

Note that FIG. 3 shows the 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 component of the constraint control calculation unit 108, and the variable conversion unit 1083 may not be provided in the constraint control calculation unit 108.

Limiting section 1084 limits the pre-limiting constraint control command value calculated by command value calculating section 1082 to be within a predetermined range. When the variable is converted by the variable conversion unit 1083, the limiting unit 1084 limits the pre-limiting constraint control command value after the conversion so as to be within a predetermined range.
In Embodiment 1, the restraint control adjustment unit 107 adjusts the movement speed of the arm 2 by adjusting the limit range of the limit unit 1084 .

Next, an operation example of the direct teaching device 1 according to Embodiment 1 shown in FIG. 1 will be described with reference to FIG. Further, in the following, it is assumed that the constraint control calculation unit 108 has the configuration shown in FIG.
In the operation example of the direct teaching device 1 according to the first embodiment shown in FIG. 1, first, as shown in FIG. ST401).

Next, external force detection section 102 detects an external force applied to arm 2 by the operator (step ST402).

Next, driven control calculation section 103 calculates the movement (driven control command value) of arm 2 according to the external force detected by external force detection section 102 (step ST403).

Next, the position/orientation calculation section 104 calculates the current values or predicted values of the position/orientation of the arm 2 based on the measurement result by the position/orientation measurement section 101 (step ST404).

Here, when the direct teaching device 1 constrains the position of the arm 2, that is, when the direct teaching device 1 constrains the position of the arm 2 to a predetermined constraint point, constraint axis, or constraint surface, the position/orientation calculation unit 104 at least I need to calculate the position.
Further, when the direct teaching device 1 constrains the posture, that is, when the direct teaching device 1 constrains the posture of the arm 2 in a predetermined constraint direction, or when the direct teaching device 1 constrains only the rotation of the posture along a predetermined constraint rotation axis. In this case, the position/orientation calculator 104 needs to calculate at least the orientation of the arm 2 .
Further, when the direct teaching device 1 performs position constraint and posture constraint, the position/posture calculation unit 104 needs to calculate both the position and posture of the arm 2 .

What the position/orientation calculation unit 104 obtains may be a current value based only on the measured values, or may be a future predicted value that also incorporates the calculation result by the driven control calculation unit 103 .
Here, the position/orientation calculation unit 104 can calculate the current values of the position/orientation by using the forward kinematics calculation from the angle of each joint measured by the position/orientation measurement unit 101 . In addition to the above, the position/orientation calculation unit 104 uses the driven control command values obtained by the driven control calculation unit 103 to calculate future predicted values of the position/orientation assuming that there is no constraint control. can do.

Next, target value calculation section 105 calculates a target value for constraint control based on the result of calculation by constraint target and position/orientation calculation section 104 (step ST405).

For example, the purpose of 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/posture calculation unit 104 is Let it be [Xp, Yp, Zp]. In this case, the target value calculation unit 105 sets the target value of constraint control to [Xp, Yp, 0.1] (in this case, the posture is not changed, so the description is omitted).

Ｊｏｈｎ Ｊ． Ｃｒａｉｇ （三浦・下山訳）：ロボティクス，共立出版（１９９１） Further, for example, as shown in FIG. 5, the purpose of restraint control is to set the Z-axis of the direction of the end effector 3 in the downward direction (the direction indicated by reference numeral 502) and set the X-axis to the X-axis of the robot coordinate system. Suppose that it is to match. In FIG. 5, reference numeral 501 indicates the upward direction (reference direction). In this case, the desired orientation, which is the desired value of the constraint control, is represented by the rotation matrix expression as shown in the following equation (1) (in this case, since the position is not changed, the description is omitted). Note that the representation of the orientation by the rotation matrix is disclosed, for example, in Non-Patent Document 1, etc., and the details thereof will be omitted.

JohnJ. Craig (translated by Miura and Shimoyama): Robotics, Kyoritsu Shuppan (1991)

Note that the target value calculation unit 105 may calculate only one target value as the target value of the constraint control, or may calculate a plurality of target values and select one target value from among them. .
For example, if the purpose of constraint control is to fix the Z-axis coordinate of the tip of the end effector 3 to the closest position in increments of 0.1 [m], the target value calculation unit 105 sets the target value of constraint control to [ Xp, Yp, 0.1n] (where n is an integer), the one closest to the current position is selected.

In addition, as shown in FIG. 6, the purpose of restraint control is to set the Z-axis of the orientation of the end effector 3 directly downward, and to rotate the end effector 3 in increments of π/2 (in increments of 90 degrees) about the Z-axis as the rotation axis. In the case of fixing, the target value calculation unit 105 selects the target value closest to the current posture as the target value for constraint control. In this case, the desired orientation, which is the desired value of the constraint control, is represented by the rotation matrix representation as shown in the following equation (2).

As shown in FIG. 6, this formula (2) has four target postures depending on n, and the target value calculation unit 105 selects one of them that is closest to the current posture. The target value calculation unit 105 can determine whether or not the current posture is close to the current posture based on the size of the rotation angle when rotated from the current posture to each target posture.

Next, distance calculation section 106 calculates the distance from the current value or the predicted value of the position and orientation of arm 2 to the constraint control area based on the calculation result by position and orientation calculation section 104 (step ST406).

For example, when the direct teaching device 1 performs both the axis constraint in the Z-axis direction and the vertically downward posture constraint, the distance calculation unit 106 first defines the axis of the axis constraint as the constraint control area. Then, the distance calculation unit 106 calculates the distance from the current values or predicted values of the position and orientation of the arm 2 to the above axis, as shown in the following equation (3). In equation (3), l represents the distance to the axis of the axis constraint, x and y represent the X and Y coordinates of the current or predicted position of the arm 2, and x _a and y _a represent the axis The X and Y coordinates of the axis of constraint are shown.
l=√{(x−x _a ) ² +(y−y _a ) ² } (3)

Further, for example, when the direct teaching device 1 constrains the posture to face vertically downward, the distance calculation unit 106 first sets the range where x and y satisfy the following expression (4) as the constraint control region, as shown in FIG. predefined. In FIG. 7, reference numeral 701 indicates a constraint control area. This restraint control area is determined to cover the place where work is performed on the work, the place where the work is picked up or the place where the work is placed, and their surroundings. Then, the distance calculation unit 106 calculates the shortest distance to this constraint control area from the current value or predicted value of the position of the arm 2 calculated by the position/orientation calculation unit 104 .
X _min ≤ x ≤ X _max
Y _min ≤ y ≤ Y _max (4)

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

Further, 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 the posture corresponding to the vertically downward direction as a constraint control region. . Then, the distance calculation unit 106 calculates, as a distance, the angle formed by the current value or predicted value of the posture of the arm 2 calculated by the position/posture calculation unit 104 and the vertically downward direction. In this way, the distance becomes 0 when the current or predicted direction of the hand is vertically downward, and the distance increases as the inclination from the vertically downward direction increases.

Note that the distance calculation unit 106 may define a plurality of constraint control regions. In this case, the distance calculation unit 106 should obtain the distance to each constraint control area and select the smallest distance among them.

As described above, the distance calculation unit 106 predefines an appropriate constraint control region for the purpose of performing direct teaching with constraints, and the position/orientation of the arm 2 calculated by the position/orientation calculation unit 104 from this constraint control region. Find the shortest distance to the current or predicted value of

Next, the constraint control adjustment unit 107 continuously or stepwise adjusts the movement speed (strength) of the arm 2 calculated by the constraint 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 constraint control based on the distance calculated by the distance calculation unit 106 .

For example, the constraint control adjustment unit 107 determines a parameter that determines the strength of constraint control as a relative value, with the case where the distance is 0 as 100%. In this case, the strength of the constraint control becomes weaker as the parameter approaches 0%, and 0% means that the constraint control is disabled.

This parameter is changed continuously according to the distance, or is changed stepwise in three or more steps. That is, in two stages, there are only two strengths of constraint control, maximum and ineffective, which is the same as the conventional technology (on/off switching of constraint control). Therefore, if the parameters are to be changed stepwise, it is necessary to change the parameters in three steps or more.

It is preferable that the parameter values determined by the constraint control adjustment unit 107 monotonously decrease with respect to the distance. By doing so, it is possible to gradually weaken the constraint control as the distance from the constraint control region increases. If it is not monotonically decreasing, the constraint control becomes stronger and weaker as it moves away from the constraint control area, which may impair the feeling of operation.

Next, the constraint control calculation unit 108 calculates the movement (constraint control command value) of the arm 2 moving to the target value calculated by the target value calculation unit 105 based on the calculation result of the position/orientation calculation unit 104 (step ST408). That is, the constraint control calculation unit 108 calculates a constraint control command value that causes the position and orientation of the arm 2 to asymptotically approach the target values of the position and orientation calculated by the target value calculation unit 105 .

When this constraint control calculation unit 108 has the configuration shown in FIG. 3, it performs the following operations.
In the 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 position/orientation current value or predicted value calculated by the position/orientation calculation unit 104 . Calculate

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 attitude deviation is obtained by calculating the rotational transformation from the current attitude to the target attitude. The deviation obtained by the deviation calculator 1081 is represented by the following equation (5). In equation (5), e indicates the deviation obtained by the deviation calculator 1081; The amounts of rotation about the X-axis, Y-axis, and Z-axis are shown, respectively.

Next, command value calculation section 1082 calculates a constraint control command value before limitation based on the calculation result of deviation calculation section 1081 . That is, command value calculation section 1082 calculates a command value such that the position/orientation of arm 2 asymptotically approaches the target value calculated by target value calculation section 105 based on the deviation calculated by deviation calculation section 1081 .

The control performed by the command value calculation unit 1082 is, in other words, such control that the deviation calculated by the deviation calculation unit 1081 approaches zero. Such control is possible by, for example, using the result of multiplying the deviation by an appropriate gain (6×6 diagonal matrix) as the speed command value, as in the following equation (6). This gain is usually a diagonal matrix. In equation (6), K _C represents a gain, v _x , v _y , v _z represent translational components in the X-axis direction, Y-axis direction, and Z-axis direction of the velocity command value, and ω _x , ω _y , ω _z indicates rotational components about the X-axis, Y-axis, and Z-axis, respectively.

Alternatively, as in the following equation (7), the speed command value can be obtained by multiplying the value normalized by the norm or weighted norm of the deviation by the gain. In the case of this method, the control is such that the target value is asymptotically approached at a constant speed regardless of the magnitude of the deviation. In equation (6), the speed of control decreases as the target value is asymptotically approached. In Equation (7), |e| indicates the norm of the deviation.

In any case, the control calculation performed by the command value calculation unit 1082 may be any control calculation that causes the position and orientation of the arm 2 to asymptotically approach the target value calculated by the target value calculation unit 105 .

Next, the variable conversion section 1083 converts the command value calculated by the command value calculation section 1082 into an angle command value for each joint.
That is, since the speed command value obtained by the command value calculation unit 1082 is a speed command value in the Cartesian coordinate system, the variable conversion unit 1083 converts it into an angular speed command value for each joint. The variable conversion 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 orientation, as in the following equation (8), to obtain the angular velocity of each joint. Convert to command value. In equation (8), J ⁻¹ indicates the inverse matrix of the Jacobian matrix.

If the command value calculation unit 1082 directly calculates the angular velocity command value of each joint, or if the constraint control command value calculated by the constraint control calculation unit 108 is in the Cartesian coordinate system, the variable conversion unit 1083 is unnecessary. sometimes. Therefore, the variable conversion unit 1083 is not an essential component.

Next, the limiting unit 1084 limits the pre-limiting constraint control command value calculated by the command value calculating unit 1082 to be within a predetermined range. When the variable is converted by the variable conversion unit 1083, the limiting unit 1084 limits the pre-limiting constraint control command value after the conversion so as to be within a predetermined range.
In Embodiment 1, the restraint control adjustment section 107 adjusts the speed of movement of the arm 2 by adjusting the control range of the restriction section 1084 . A case where the constraint control adjustment unit 107 limits the angular velocity command value for each joint will be described below as an example.

First, the limiting unit 1084 determines in advance the maximum upper limit (maximum value of the upper limit) of the angular velocity command value for each joint. 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. Hereinafter, the maximum upper limit of the angular velocity command value of the i-th joint is represented by θ (dot) _i,MAX (the maximum upper limit is assumed to be a positive value).

Next, the limiting unit 1084 calculates the upper limit of the actually applied angular velocity command value. This upper limit is determined based on the maximum upper limit and adjustment parameters. By changing this adjustment parameter according to the output of the constraint control adjustment unit 107, the upper limit of the constraint control command value calculated by the constraint control calculation unit 108 is adjusted. to adjust. That is, when the upper limit of the angular velocity command value actually applied to the i-th joint is expressed by θ (dot) _i,limit , the following formula (9) is obtained. In Equation (9), α indicates an adjustment parameter.

The value of this adjustment parameter is determined within a range of 0 to 1 based on the parameter value output from the constraint control adjustment unit 107 . When the constraint control adjustment unit 107 outputs 100%, the value is set to 1, and when 0% is output, the value is set to 0.

FIG. 8 shows the relationship between the distance calculated by the distance calculator 106 and the adjustment parameters. When the distance is 0, the adjustment parameter is 1, and as the distance increases from there, the adjustment parameter decreases continuously or stepwise, and when the distance exceeds a predetermined value, the adjustment parameter becomes 0. . By doing so, 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 applied angular velocity command value becomes maximum, and the speed at which the restraint control moves the arm 2 becomes maximum. This is the case where the current value or predicted value of the position and orientation of the arm 2 calculated by the position and orientation calculation 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 orientation of the arm 2 moves away from the constraint control region, the upper limit of the actually applied angular velocity command value decreases, and the constraint The speed at which the control moves arm 2 is also limited and slowed down. When the distance calculated by the distance calculation unit 106 becomes a predetermined value or more, that is, when the current value or predicted value of the position and orientation of the arm 2 is sufficiently separated from the restraint control area, the upper limit of the actually applied angular velocity command value becomes 0, and constraint control is disabled. In this way, the operator can freely move the arm 2 regardless of restraint.

Once the upper limit of the actually applied angular velocity command value is determined, the limiting unit 1084 limits the value so that the angular velocity command values of all joints do not exceed this upper limit. That is, the angular velocity command value for each joint is limited so that the angular velocity command value for the i-th joint satisfies the following equation (10). In Equation (10), θ (dot) _i indicates the angular velocity command value of the i-th joint.

When the limiter 1084 limits the angular velocity command value for each joint, it is preferable to limit the movement direction of the arm 2 by restraint control. An example of a procedure for performing such calculations is shown below.

First, the limiting unit 1084 obtains the ratio between the magnitude of the angular velocity command value and the upper limit of the actually applied angular velocity command value for each joint, as shown in the following equation (11). In equation (11), c _i indicates the ratio.

Next, limiting section 1084 obtains the maximum value of the above ratio.
Next, the limiter 1084 divides the angle command value of each joint angle by the maximum value of the ratio to calculate the angular velocity command value after the limit. In equation (12), θ (dot) ^* _i indicates the angular velocity command value after limitation, and c indicates the maximum value of the ratio.

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

Returning again to the description of the flow shown in FIG. are combined into one command value (step ST409).

For example, the driven control calculation unit 103 calculates the joint angular velocity command value (θ (dot) _D ) as the driven control command value, and the constraint control calculation unit 108 calculates the joint angular velocity command value (θ (dot) D ) as the constraint control command value. _C ) is calculated. In this case, the synthesizing unit 109 obtains the sum θ (dot) _D + θ (dot) _C 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 constraint control calculation unit 108 calculates the torque command value (τ _C ) as the constraint control command value. Suppose we calculated In this case, synthesizing section 109 obtains the sum, τ _D +τ _C , as the command value. It is also assumed that the driven control calculation unit 103 calculates the position command value (θ _D ) as the driven control command value, and the constraint control calculation unit 108 calculates the joint angle change amount (Δθ _C ) as the constraint control command value. In this case, the synthesizing unit 109 obtains the sum, θ _D +Δθ _C , as the command value. In addition, even when the physical quantities calculated by the driven control calculation unit 103 and the constraint control calculation unit 108 are different, the synthesizing unit 109 can synthesize command values through control calculation.

Next, drive control section 110 drives arm 2 based on the result of synthesis by synthesis section 109 (step ST410). That is, the drive control section 110 controls the arm 2 according to the command value. Since the control of this part is the normal control of the arm 2, the details will be omitted.

Here, in the direct teaching with constraint, the arm 2 is controlled to follow the constraint posture, the constraint surface, or the constraint axis by calculation of constraint control, but this strength can be adjusted. Therefore, in the direct teaching device 1 according to the first embodiment, instead of choosing whether or not to perform constraint control, for example, adjustment is made such that the closer the constraint posture, the constraint surface, or the constraint axis, the stronger the constraint control, and the further away, the weaker the constraint control. Continuous or step-by-step, it is possible to reduce sudden movements and vibrations as in the prior art. On the other hand, in the direct teaching device 1 according to the first embodiment, by increasing the constraint control when the region is close to the constraint control region, there is no problem that the accuracy of the direct teaching deteriorates like when the constraint control is simply weakened. It becomes difficult to get up. Thus, in the direct teaching device 1 according to Embodiment 1, it is possible to achieve both teaching accuracy and operational feeling of the arm 2 .

As described above, according to the first embodiment, the direct teaching device 1 includes the external force detection unit 102 that detects an 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. 2, a position/posture calculation unit 104 that calculates the current value or predicted value of the position/posture, which is at least one of the position and/or posture of the arm 2, and the constraint target and position/posture calculation. A target value calculation unit 105 that calculates a target value for constraint control based on the calculation result of the unit 104, and a movement to the target value calculated by the target value calculation unit 105 based on the calculation result of the position/orientation calculation unit 104. a constraint control calculation unit 108 that calculates the movement of the arm 2, and a constraint control adjustment that enables the speed of the movement of the arm 2 calculated by the constraint control calculation unit 108 to be adjusted continuously or in at least three steps or more. 107, a synthesizing unit 109 that synthesizes the calculation result by the driven control arithmetic unit 103 and the calculation result by the restraint control arithmetic unit 108, and the drive control unit 110 that drives the arm 2 based on the synthesis result by the synthesizing unit 109. prepared. As a result, the direct teaching device 1 according to the first embodiment can achieve both teaching accuracy and operational feeling of the arm 2 .

Embodiment 2.
In the first embodiment, the constraint control adjustment unit 107 adjusts the limit unit 1084 of the constraint control calculation unit 108 to adjust the speed at which the constraint control moves the arm 2. The adjustment method is not limited to this. Embodiment 2 shows a case where the constraint control adjuster 107 adjusts the speed at which the constraint control moves the arm 2 by adjusting the multiplier 1085 of the constraint control calculator 108 .

FIG. 9 is a diagram showing a configuration example of the constraint control calculation unit 108 according to the second embodiment. Constraint control calculation section 108 in Embodiment 2 shown in FIG. 9 is different from constraint control calculation section 108 in Embodiment 1 shown in FIG. are removing. Also, the constraint control adjusting section 107 adjusts the multiplying section 1085 . Other configurations of the direct teaching device 1 according to the second embodiment are the same as those of the direct teaching device 1 according to the first embodiment shown in FIGS. conduct.

The multiplier 1085 multiplies the deviation calculated by the deviation calculator 1081 by the adjustment gain.
Then, the constraint control adjusting section 107 adjusts the speed of movement of the arm 2 by adjusting the adjustment gain multiplied by the multiplying section 1085 .

The multiplier 1085 multiplies the deviation calculated by the deviation calculator 1081 by the adjustment gain (gain and adjustment parameter) as shown in the following equation (13). where the gain is a 6×6 matrix (preferably a diagonal matrix) and the adjustment parameter is a scalar.

Multiplication section 1085 changes the value of the adjustment parameter within a range from 0 to 1 according to the output of constraint control adjustment section 107 . The multiplication unit 1085 sets the adjustment parameter to 1 when the constraint control adjustment unit 107 outputs 100%, and to 0 when the constraint control adjustment unit 107 outputs 0%.

FIG. 10 shows the relationship between the distance calculated by the distance calculator 106 and the adjustment parameters.
When the distance is 0, the adjustment parameter is 1, and as the distance increases from there, the adjustment parameter decreases continuously or stepwise, and when the distance exceeds a predetermined value, the adjustment parameter becomes 0. . By doing so, 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 calculator 106 is 0, the constraint control command value calculated by the constraint control calculator 108 is maximized, and the speed at which the constraint control moves the arm 2 is maximized. As the distance calculated by the distance calculator 106 increases, the constraint control command value decreases, and the speed at which the constraint control moves the arm 2 also decreases. When the distance calculated by the distance calculator 106 becomes equal to or greater than a predetermined value, the restraint control becomes invalid and the operator can freely move the arm 2 regardless of restraint.

In the direct teaching device 1 according to the second embodiment, as described above, the speed at which the constraint control moves the arm 2 is adjusted based on the distance from the constraint control area.

Embodiment 3.
In the first and second embodiments, the constraint control adjustment unit 107 adjusts the speed of constraint control based on the distance calculated by the distance calculation unit 106. However, the speed of constraint control is adjusted. The original value is not limited to this. Embodiment 3 shows a case where the movement speed of the arm 2 calculated by the constraint control calculation unit 108 is adjusted based on the amount of manual manipulation of the arm 2 by a person (manipulation amount).

FIG. 11 is a diagram showing a configuration example of the direct teaching device 1 according to the third embodiment. A direct teaching device 1 according to Embodiment 3 shown in FIG. 11 is different from the direct teaching device 1 according to Embodiment 1 shown in FIG. The rest of the configuration of the direct teaching device 1 according to Embodiment 3 is the same as that of the direct teaching device 1 according to Embodiment 1, and the same reference numerals are given and only different parts will be described.

The manual operation amount calculation unit 111 calculates the operation amount of the arm 2 by the operator based on the calculation result by the driven control calculation unit 103 .
Then, the restraint control adjustment section 107 adjusts the speed of movement of the arm 2 based on the calculation result by the manual operation amount calculation section 111 .

As a preferred example, the driven control command value calculated by the driven control calculation unit 103 is used as the angular acceleration command value, and the manual operation amount calculation unit 111 calculates the operation amount based on the magnitude of the angular acceleration command value, The movement speed of the arm 2 calculated by the restraint control calculation unit 108 is adjusted by this operation amount. Then, the constraint control adjustment unit 107 adjusts the movement speed of the arm 2 calculated by the constraint control calculation unit 108 so that the larger the operation amount is, the slower the movement speed of the arm 2 is. Adjust so that it is faster. By doing so, if the operator operates the arm 2 quickly, the restraint control becomes weak, so the operator can freely move the arm 2 regardless of the restraint. In addition, since the constraint control becomes stronger when the arm 2 is operated slowly, the arm 2 can be operated accurately along with the constraint, and the teaching control can be enhanced.

Further, the direct teaching device 1 is not limited to the above example, and the direct teaching device 1 calculates the arm angle based on the angle of each joint measured by the position/orientation measurement unit 101 or the current value or predicted value of the position/orientation calculated by the position/orientation calculation unit 104. It is also possible to obtain angular velocities for each of the two joints and make adjustments based on these values. However, while the driven control command values calculated by the driven control calculation unit 103 are determined by the operator's operation, the angular velocities calculated from these values are affected by the calculation result of the restraint control, so that the operator's operation is restrained. The adjustment operation of the control adjustment unit 107 may not be well reflected.

Note that the manual operation amount calculation unit 111 may obtain the operation amount not only from the angular velocity, but also from other physical quantities such as the velocity or angular acceleration of the hand of the arm 2 .

Embodiment 4.
The constraint control adjustment unit 107 adjusts the constraint control based on the distance calculated by the distance calculation unit 106 in the first and second embodiments, and based on the operation amount calculated by the manual operation amount calculation unit 111 in the third embodiment. The case of adjusting the speed of Based on the calculation result of one or more of the driven control calculation unit 103 , the position/orientation calculation unit 104 and the target value calculation unit 105 , the constraint control adjustment unit 107 calculates the arm 2 position calculated by the constraint control calculation unit 108 . It is thought that it adjusts the speed of movement of the In a sense, the speed of restraint control is not adjusted directly by humans, but indirectly through the calculation results. On the other hand, without being limited to this, a method in which a person directly adjusts the movement speed of the arm 2 calculated by the restraint 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 device 1 according to the fourth embodiment shown in FIG. 12 is different from the direct teaching device 1 according to the first embodiment shown in FIG. The rest of the configuration of the direct teaching device 1 according to Embodiment 4 is the same as that of the direct teaching device 1 according to Embodiment 1, so the same reference numerals are given and the description thereof is omitted.

The input unit 112 receives input from the operator.
Then, the restraint control adjustment unit 107 adjusts the movement speed of the arm 2 based on the result received by the input unit 112 .

An example of the input unit 112 is two buttons provided on the main body of the arm 2 or a teaching pendant for operating the robot. In this case, the constraint control adjustment unit 107 increases the movement speed of the arm 2 calculated by the constraint control calculation unit 108 by one step when one of the two buttons is pressed, and increases the speed by one step when the other button is pressed. Adjust to at least 3 steps or more to slow down the steps.

Another example of the input unit 112 is an adjustment slider (such as a volume slider for volume adjustment) provided on the main body of the arm 2 or a teaching pendant for operating the robot. In this case, the constraint control adjustment unit 107 increases the movement speed of the arm 2 calculated by the constraint control calculation unit 108 when the slider is moved in one direction, and slows it when the slider is moved in the other direction. adjust accordingly. It should be noted that the slider for adjustment may not be a physical one, but may be a mechanism for operating something displayed on the screen, such as a teaching pendant.

Another example of the input unit 112 is a sensor that is provided on the arm 2 and detects the force with which the operator grips the arm 2 . In this case, the restraint control adjustment unit 107 performs adjustment according to the output of this sensor. For example, the restraint control adjustment unit 107 increases the movement speed of the arm 2 calculated by the restraint control calculation unit 108 when the force with which the arm 2 is grasped becomes weak, and decreases it when the force with which the arm 2 is grasped becomes strong. to continuously adjust.

Another example of the input unit 112 is a foot pedal. In this case, the restraint control adjustment unit 107 performs adjustment according to the amount of depression of the foot pedal. For example, the constraint control adjustment unit 107 maximizes the movement speed of the arm 2 calculated by the constraint control calculation unit 108 when the foot pedal is not depressed at all, and gradually increases the speed as the amount of depression increases. It is adjusted continuously or step by step so that the speed is reduced to 0 when the pedal is depressed to the deepest position and the constraint control calculation is substantially invalidated.

In the direct teaching device 1 according to the fourth embodiment, as described above, the input unit 112 detects the input from the person, and the constraint control adjustment unit 107 adjusts the constraint control calculation unit according to the detected value. Adjust the speed of movement of the arm 2 calculated by 108 .

In addition, in the first to fourth embodiments, the description and examples were partly based on the assumption that the arm 2 has six axes.
Further, in the first to fourth embodiments, the example in which the driven control command value calculated by the driven control calculation unit 103 and the constraint control command value calculated by the constraint control calculation unit 108 are angular velocity command values are mainly described. Although explained, implementation is possible with other physical quantities such as angular acceleration or torque.

In addition, within the scope of the present invention, it is possible to freely combine each embodiment, modify any component of each embodiment, or omit any component in each embodiment. be.

1 direct teaching device 2 arm 3 end effector 101 position and orientation measurement unit 102 external force detection unit 103 driven control calculation unit 104 position and orientation calculation unit 105 target value calculation unit 106 distance calculation unit 107 constraint control adjustment unit 108 constraint control calculation unit 109 synthesis unit 110 Drive control unit 111 Manual operation amount calculation unit 112 Input unit 1031 Multiplication unit 1081 Deviation calculation unit 1082 Command value calculation unit 1083 Variable conversion unit 1084 Limitation unit 1085 Multiplication unit

Claims (7)

an external force detection unit that detects an external force applied to an arm of the robot;
a driven control calculation unit that calculates the movement of the arm according to the external force detected by the external force detection unit;
a position/posture calculation unit that calculates a current value or a predicted value of the position/posture, which is at least one of the position and posture of the arm;
a target value calculation unit that calculates a target value for constraint control based on the constraint target and the result of calculation by the position and orientation calculation unit;
a constraint control calculation unit that calculates the movement of the arm moving to the target value calculated by the target value calculation unit based on the calculation result of the position/orientation calculation unit;
a constraint control adjustment unit that can adjust the speed of the arm movement calculated by the constraint control calculation unit continuously or in steps of at least three steps;
a synthesizing unit that synthesizes the calculation result by the driven control arithmetic unit and the calculation result by the constraint control arithmetic unit;
A direct teaching device comprising: a drive control section that drives the arm based on a result of synthesis by the synthesis section. The constraint control calculation unit is
a deviation calculation unit that calculates a deviation between the target value calculated by the target value calculation unit and the current value or 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 the calculation result of the constraint control calculation unit based on the calculation result of the deviation calculation unit;
a limiting unit that limits the pre-limiting constraint control command value calculated by the command value calculating unit;
2. The direct teaching device according to claim 1, wherein the restraint control adjusting section adjusts the movement speed of the arm by adjusting the restricting section. The constraint control calculation unit is
a deviation calculation unit that calculates a deviation between the target value calculated by the target value calculation unit and the current value or predicted value of the position and orientation calculated by the position and orientation calculation unit;
a multiplication unit that multiplies the result calculated by the deviation calculation unit by an adjustment gain,
The direct teaching device according to claim 1, wherein the constraint control adjustment section adjusts the speed of movement of the arm by adjusting an adjustment gain multiplied by the multiplication section. a distance calculation unit that calculates a distance from a current value or a predicted value of the position and orientation of the arm to a constraint control area based on a calculation result of the position and orientation calculation unit;
4. The direct control device according to any one of claims 1 to 3, wherein the constraint control adjustment unit adjusts the speed of movement of the arm based on the calculation result of the distance calculation unit. Teaching device. A manual operation amount calculation unit that calculates an operation amount by an operator for the arm,
The restraint control adjustment section adjusts the movement speed of the arm based on the calculation result of the manual operation amount calculation section. direct teaching device. An input unit for receiving input from an operator,
4. The direct teaching according to any one of claims 1 to 3, wherein the constraint control adjusting section adjusts the speed of movement of the arm based on a result received by the input section. Device. an external force detection unit detecting an external force applied to an arm of the robot;
a step in which a driven control calculation unit calculates the movement of the arm according to the external force detected by the external force detection unit;
a position and orientation calculation unit calculating a current value or a predicted value of the position and orientation, which is at least one of the position and orientation of the arm;
a target value calculation unit calculating a target value for constraint control based on a constraint target and a calculation result by the position and orientation calculation unit;
a step of calculating, by a constraint control calculation unit, the movement of the arm moving to the target value calculated by the target value calculation unit, based on the calculation result by the position/orientation calculation unit;
a step in which the constraint control adjustment unit is capable of adjusting the speed of the arm movement calculated by the constraint control calculation unit continuously or in steps of at least three steps;
a combining unit combining a calculation result by the driven control calculation unit and a calculation result by the constraint control calculation unit;
A direct teaching method comprising: a drive control unit driving the arm based on the result of synthesis by the synthesis unit.

Images