JP2017074669A - Manipulator control device, manipulator drive device, and robot system - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a manipulator control device which can teach movement of a manipulator tip of a manipulator drive device as intended by a teaching person in an arbitrary position, attitude and movement locus in direct teaching performed by a human hand.SOLUTION: A manipulator control device includes an input unit 113 which inputs an operation region of a tip of a first arm 101 being the manipulator tip, and performs following assist control when a vector of external force (operation force) is detected by an external force detection unit 112. The manipulator control device assists the external force by controlling driving of a first drive unit 160 (first motor 106) and a second drive unit 170 (second motor 109) so that the tip of the first arm 101 moves within the input operation region on the basis of the detection result of the external force detection unit 112 and the operation region input from the input unit 113.SELECTED DRAWING: Figure 1

Description

  The present invention relates to a manipulator control device, a manipulator driving device including the manipulator control device, and a robot system including at least one of them.

A method called direct teaching is used as a teaching method for a manipulator that teaches (registers) the operation of a manipulator driving device (robot system) that performs work in a manufacturing factory, work site, home, etc., or assists human work. There is.
This is because the instructor has the first support part, which is the tip of the manipulator to which the support object such as the work tool is attached, and moves the first support part to any position, posture, and movement by directly applying force. This is a technology for controlling (teaching / registering) the robot to move along a locus.

For example, Patent Document 1 describes a manipulator driving device (robot) including a control device for a manipulator (robot arm) that performs direct teaching (power assist) as follows.
This control device is a directional component in which the manipulator can operate the external force detected by the external force detector based on the external force detected by the external force detector that detects the external force applied to the manipulator and the movable state of the joint of the manipulator. Is converted into a convertible external force.
By using the converted external force converted in this way, the restriction of the manipulator's operation area (operation restriction) according to the operation status of the joint, such as the restriction of the operation range of the joint of the manipulator, is realized with a simple control structure. Patent Document 1 describes that the instability of the control system can be reduced.

However, if the operation region of the manipulator is limited only in accordance with the operation state of the joint portion, an external force is applied in a direction that was not intended by the teacher (user) if the joint portion is operable. Even in such a case, the operation in that direction is taught (controlled).
For this reason, conventional direct teaching has a problem that when the instructor teaches the manipulator by applying external force, the position / posture / movement locus of the manipulator may not be taught as intended. .

  In order to solve the above-described problem, the invention described in claim 1 is a plurality of supports that support two or more joint parts and a support object that is connected in series via each joint part and attached to a manipulator tip. And a vector of an external force applied to at least one of the two or more joint units and the plurality of support units, each of which rotates the two or more joint units. In a manipulator control device that is used in a manipulator drive device including an external force detection unit and controls the drive of the two or more drive units based on a detection result of the external force detection unit, an operation region of the manipulator tip is determined. An input means for inputting, and when the external force vector is detected by the external force detection section, the detection result of the external force detection section and the operation region input from the input means Based on, the input operation area as the manipulator tip ends are moved, and controls the drive of the two or more drives.

  According to the present invention, in direct teaching performed by a human hand, control of a manipulator capable of teaching the movement of the manipulator tip of the manipulator driving device at an arbitrary position / posture / movement locus as intended by the instructor. Equipment can be provided.

1 is a schematic explanatory diagram of a manipulator device that is a configuration example of a manipulator driving device according to Embodiment 1. FIG. The control block diagram of the drive control apparatus of the manipulator apparatus which operates the 1st joint part which concerns on Example 1, and a 2nd joint part. Explanatory drawing of the input device which can be used for the manipulator apparatus of Example 1. FIG. Explanatory drawing about the force which generate | occur | produces with a manipulator. 4 is a control block diagram of assist control according to the assist control method 1. FIG. The figure which showed the motor output with respect to the external force which concerns on the assist control method 1. FIG. The figure which showed the control flow which concerns on the assist control method 1. 7 is a control block diagram of assist control according to assist control method 2. FIG. The figure which showed the control flow which concerns on the assist control method 2. Explanatory drawing using the schematic diagram of the assist control method 2. FIG. Explanatory drawing of the relationship between the detected external force which concerns on the operation force target production | generation part, the target value of operation force, and the operation force designation | designated value. FIG. 9 is a control block diagram of assist control according to the assist control method 3; The figure which showed the control flow which concerns on the assist control method 3. Explanatory drawing at the time of restrict | limiting an operation area | region to a certain 1 axis in the manipulator of 4 degrees of freedom. Explanatory drawing at the time of restrict | limiting an operation area | region to a certain plane in the manipulator of 4 degrees of freedom. Explanatory drawing at the time of restrict | limiting an operation area | region to the rotation direction of a certain 1 axis | shaft in the manipulator of 4 degrees of freedom. FIG. 6 is a schematic explanatory diagram of a manipulator device according to a second embodiment. Explanatory drawing of a tool point coordinate system and a manipulator absolute coordinate system. Explanatory drawing of the operation area | region designation | designated means with which a manipulator apparatus is equipped. Explanatory drawing of an example when designating an operation area | region (operation direction). FIG. 6 is a schematic explanatory diagram of a manipulator device according to a third embodiment. FIG. 9 is a control block diagram of a drive control device for a manipulator device that operates a first joint, a second joint, and a third joint according to a third embodiment. Explanatory drawing of the example of the contact detection part which can be provided in a manipulator apparatus. Explanatory drawing of the example of the operation mode of a manipulator apparatus using a contact detection part. Explanatory drawing of an example of the operation control sequence in the direct teaching of an Example.

  Hereinafter, an embodiment of a manipulator driving device including a manipulator control device such as a robot arm to which the present invention is applied will be described with reference to a plurality of examples.

Example 1
First, a first example of a manipulator driving device provided with the manipulator control device of the present embodiment will be described with reference to the drawings.
FIG. 1 is a schematic explanatory diagram of a manipulator device 100 which is one configuration example of a manipulator driving device according to the present embodiment, in which FIG. 1 (a) is a plan explanatory diagram, and FIG. 1 (b) is a side view of main components. It is explanatory drawing.

  As shown in FIGS. 1A and 1B, the manipulator device 100 has two degrees of freedom which is one of the basic configurations of a manipulator driving device including a manipulator such as a robot arm. is there. However, the manipulator control device according to the present embodiment is not limited to the manipulator driving device having two degrees of freedom like the manipulator device 100, and two or more joint portions are provided depending on the function. And it is applicable to the thing provided with the some support part connected in series via each joint part.

The manipulator device 100 includes a first arm 101 coupled to a most rear side (front end side) via a joint portion from a support column portion 111b which is a fixed portion fixed to a base 111a of the device body among a plurality of support portions. The second arm 102 connected to the front side is supported as follows.
As shown in FIG. 1 (b), the first arm 101 and the second arm 102 have a first drive center 103 of the first joint part 130 and a second joint part 140 of the second joint part 140, respectively, in the vicinity of the front end. The drive center 104 is rotatably supported.

The first joint unit 130 is connected to a first motor 106 as a first drive unit 160 and a first speed reducer 105 having the first drive center 103 as an output axis. The rotational driving force from is decelerated and transmitted to the first arm 101, and the first arm 101 is rotationally driven. A first motor encoder 107 is attached to the side surface of the first motor 106 opposite to the second arm 102 so that the amount of rotation of the first motor 106 can be measured. 105 and the first motor 106 are fixed to the second arm 102.
A second motor 109 and a second speed reducer 108 having the second drive center 104 as an output shaft center are connected to the second joint portion 140, and the rotational driving force from the second motor 109 is decelerated. Is transmitted to the second arm 102, and the second arm 102 is rotationally driven. A second motor encoder 110 is attached between the support column 111b and the second motor 109 so that the amount of rotation of the second motor 109 can be measured. These are fixed to the support 111b (base 111a).

The first motor encoder 107 also functions as a first arm encoder 131 that measures the rotation amount of the first arm 101 relative to the second arm 102 from the reduction ratio of the first reduction gear 105 and the measured rotation amount. . On the other hand, the second motor encoder 110 also functions as a second arm encoder 141 that measures the rotation amount of the second arm 102 with respect to the support 111b from the reduction ratio of the second reduction gear 108 and the measured rotation amount. However, the first arm encoder 131 and the second arm encoder 141 may be provided separately, and the angle formed by each arm connected to each joint portion may be measured to obtain the rotation amount of each joint portion.
Further, an external force detector 112 for detecting a vector of external force applied to the manipulator is attached near the tip of the first arm 101, and a gripping mechanism as a work tool is attached to the tip of the first arm 101. .
As will be described in detail later, each motor, each encoder for the motor, an external force detection unit 112, and an input unit 113 that is an input unit for inputting an operation region of the arm tip (hand) of the first arm 101 includes a manipulator. It is connected to a control unit 210 that functions as a control device.

  As an example of the operation, when an external force is applied to the external force detection unit 112, an operation command is transmitted to the first motor 106 and the second motor 109 by a signal from the control unit 210 to which the external force detection unit 112 is connected. To drive. Then, the arm tip of the first arm 101 moves within the operation area input by the input unit 113. Here, when each arm is driven, rotation information of each motor and rotation information of each arm can be measured from each motor encoder.

Next, a specific configuration and operation of the manipulator device 100 will be described with reference to FIG.
FIG. 2 is a control block diagram of the drive control device of the manipulator device 100 that operates the first joint portion 130 and the second joint portion 140 according to the present embodiment.
The manipulator device 100 mainly includes a manipulator body 200, a power supply unit 291 and a host controller 281. For example, when image information is required for manipulator operation, an image input device 271 may be connected. Here, in FIG. 2, the manipulator body 200, the host controller 281, the power supply unit 291, and the like are separated, but a stand-alone configuration in which all are mounted in the manipulator body 200 may be used.

The control unit 210 includes a CPU 211 that is a central processing unit, and performs operation control of the manipulator body 200.
The host controller 281 and the CPU 211 are connected via a communication network 282, and operation mode information, various parameters necessary for drive control, target movement position information of the manipulator body 200, and the like are transmitted via the communication network 282.
The CPU 211 receives signals from various sensors 220 and switches 221. Based on the signal information, the CPU 211 inputs (sets) the operation area of the manipulator, and starts and stops such as operation start and emergency stop. Done.

In each joint of the manipulator body 200, the first joint portion 130 is driven by the first motor 106 and the second joint portion 140 is driven by the second motor 109.
The drive information of the first motor 106 and the second motor 109 is input to the first drive information detector 234 and the second drive information detector 236 as signals from the first motor encoder 107 and the second motor encoder 110. . These input signals are converted into rotation information such as a movement amount, a movement speed, and a movement acceleration by the first drive information detection unit 234 and the second drive information detection unit 236, respectively. The converted rotation information and the external force applied to the manipulator body 200 detected by the external force detection unit 112 are input to the drive control unit 214 in the CPU 211 (control unit 210).

In addition, the operation region at the tip of the first arm 101 is input to the CPU 211 from the input unit 113, and the operation region and the operation direction at the tip of the first arm 101 are restricted.
The drive control unit 214 generates a target command value based on the rotation information, the external force, and the limited operation direction. Then, a control command is issued to the first motor driver 233 and the second motor driver 235 so as to drive according to the generated target command value, and the first motor 106 and the second motor 109 are driven.
Further, the CPU 211 has a storage unit 215, and this storage unit 215 functions as a storage unit that stores, for each joint unit, an angle formed by a support unit coupled to each joint unit.

Here, in the control unit 210, the angle formed by the support unit connected to each joint unit as the time from the start and the position and posture of the manipulator at regular intervals from the direct teaching start time is set for each joint unit. Registered (stored) in the storage unit 215. Thus, when the user moves the manipulator, the current manipulator position / posture and the motion trajectory up to the current position / posture are registered.
When the direct teaching operation is to be reproduced by the manipulator, the control unit 210 uses the cue of the user after direct teaching (instruction from the input unit 113) and the time interval registered in the position and orientation registered in the storage unit 215. Each joint is driven according to the above.

Next, an input device 114 that is an example of the input unit 113 that can be used in the manipulator device 100 according to the present embodiment will be described with reference to FIG.
FIG. 3 is an explanatory diagram of the input device 114 that can be used in the manipulator device 100 of the present embodiment.
The input device 114 includes a touch panel 114a for receiving an operation display and an operation, a numeric keypad 114b for inputting an input value, a selector number, and the like, and executing (determining) the processing after input or selection. An execution button 114c is provided. A power ON button 114d and an emergency stop button 114e are also provided.
Then, operations such as direct teaching start, end, and reproduction execution are performed on the touch panel 114a and the like.

By using such an input device 114, a user (hereinafter referred to as a user) using the manipulator device 100 can operate the manipulator according to the display on the touch panel 114a. Further, after the operation profile is created by the touch panel 114a or the numeric keypad 114b, the operation of the manipulator can be started by pressing the execution button 114c. Further, the operation direction of the manipulator can be limited by the touch panel 114a or the numeric keypad 114b.
For example, the current position / posture of the manipulator is displayed on the touch panel 114a as a 3D model, and one axis with the coordinate axis of the arm tip (hand) is selected with the numeric keypad. The selected input signal is input to the control unit 210 and used when generating an operation command value.

Here, the force generated by the manipulator will be described with reference to FIG.
FIG. 4 is an explanatory diagram of the force generated by the manipulator, FIG. 4 (a) is an explanatory diagram of a model of a two-joint manipulator, and FIG. 4 (b) is generated at each part of the model shown in FIG. 4 (a). It is explanatory drawing of force.
In the two-joint manipulator model shown in FIG. 4A, a first joint axis (rotary axis) 301 is rotatably provided near the one end of the first link 301 at the origin of the XY coordinate system. In the vicinity of the other end, a second joint shaft 304 with the second link 302 is provided.

By considering the equation of motion when such a two-joint manipulator is operating in the XY plane, the torque generated at each joint (rotary axis) is obtained. The symbols in FIG. 4B indicate the following. In this example, since there are two joints, i = 1, 2.
Ii: Moment of inertia Li: Link length between axes mi: Mass θi: Angle τi: Torque of rotating shaft ri: Distance between rotating shaft and center of mass g: Gravitational acceleration (y-axis negative direction)

ここで、上記式１に記載のｑは一般化座標を示し、次の式２のように表される。

また、上記式１の第一項は慣性力、第二項は遠心力及びコリオリ力、第三項は重力項を示す。
ここで、上述した説明は、２関節マニピュレータのモデルについてのものであるが、関節軸が３以上の複数軸であっても、上記式１、及び式２で記述（表す）することができる。
そして、図４に上記運動方程式を当てはめると次の式３のように表すことができ、動作中に各回転軸にかかるトルクを求める（推定する）ことができる。

実際には、機械構成や減速機構、剛性、粘性摩擦などを考慮することで精度良くトルクを求める（推定する）ことができる。 In general, the equation of motion is given by the following equation 1.

Here, q described in the above equation 1 represents a generalized coordinate and is expressed as the following equation 2.

Further, the first term of the above formula 1 represents inertial force, the second term represents centrifugal force and Coriolis force, and the third term represents gravity term.
Here, the above description is about a model of a two-joint manipulator, but even if the joint axis is a plurality of three or more axes, it can be described (represented) by the above formulas 1 and 2.
Then, when the above equation of motion is applied to FIG. 4, it can be expressed as the following expression 3, and the torque applied to each rotating shaft during operation can be obtained (estimated).

Actually, the torque can be obtained (estimated) with high accuracy by taking into consideration the machine configuration, the speed reduction mechanism, rigidity, viscous friction, and the like.

ここで、Ｊはヤコビ行列を表す。
力を求めるためには、次の式５に示すようにして求めればよい。

Further, the force F desired to be generated at the tip (hand) of the second link 302 and the torque required for each joint have the relationship of the following equation (4).

Here, J represents a Jacobian matrix.
In order to obtain the force, it may be obtained as shown in Equation 5 below.

これを式５にあてはめることで、次の式７に示すように発生する力を求めることができる。

以上、ここでは２関節マニピュレータを例に挙げたが、関節軸はいくつであっても良い。 In the example of the two-joint manipulator, the Jacobian matrix can be expressed as the following Expression 6.

By applying this to Expression 5, the force generated as shown in the following Expression 7 can be obtained.

As mentioned above, although the 2 joint manipulator was mentioned here as an example, how many joint axes may be sufficient.

Next, a method of power assist control (hereinafter referred to as assist control) performed by direct teaching of a manipulator applicable to the manipulator driving device of the present embodiment will be described with a plurality of examples.
Here, in each example to be described later of the assist control method of the manipulator, a method using the external force detected (measured) by the external force detection unit such as the external force detection unit 112 for detecting the external force like the manipulator device 100, and the motion A method of using an external force estimated using an equation or the like will be described.

(Assist control method 1)
First, the manipulator assist control method 1 will be described with reference to the drawings.
FIG. 5 is a control block diagram of assist control according to the assist control method 1. FIG. 5 (a) shows a case where an external force detected (measured) by the external force detection unit is used, and FIG. The case where the external force estimated using is used is shown. FIG. 6 is a diagram illustrating a motor output with respect to an external force according to the assist control method 1, and FIG. 7 is a diagram illustrating a control flow according to the assist control method 1.

In the assist control method 1, as shown in the control block diagrams of FIGS. 5A and 5B, according to the detection results of the external force (operation force) detection 406 and the external force (operation force) detection 407, respectively. Assist control is performed to change the output of the motor 404 so that a large force is applied in the same direction as the external force.
In any control block diagram, when an external force applied to the manipulator 420 is detected, a calculation is performed by the PID unit 401 (drive control unit 214), and a current is supplied to the motor 404 by the driver 402. The input torque to the motor is obtained by multiplying the current by the torque constant 403. When the motor 404 is driven, the link mechanism 405 is operated.

As shown in FIG. 5A, the external force is detected when an external force (operation force) detection 406 is performed by attaching a sensor such as an axial force sensor, as shown in FIG. In some cases, external force (operation force) detection 407, that is, estimation is performed based on the position and speed of the motor 404 and the like. When the external force is estimated, it can be obtained from the equation of motion described with reference to FIG. 4 and the external force detection with higher accuracy can be performed by estimating the friction viscosity and the mechanical rigidity.
In the assist control method 1, the external force F1 and the motor output nF1 are controlled to be proportional as shown in FIG.
Here, a more specific control flow is performed as follows.

For example, as in the control flow shown in FIG. 7, when assist control is started (started), external force (operation force) is detected (S101), and when external force is detected and it is determined that there is an operation (in S102) YES), the motor (torque) output value is calculated (S103). On the other hand, when it is determined that there is no operation without detecting an external force (NO in S102), the process returns to the external force (operation force) detection (S101), and is repeated until it is determined that there is an operation by detecting the external force.
When the motor (torque) output value is calculated (S103), the motor is driven with the calculated output value (S104), and then external force (operation force) is detected again (S105). When it is determined that there is an operation by detecting an external force (YES in S106), the process returns to the calculation of the motor (torque) output value (S103). On the other hand, when external force is not detected and it is determined that there is no operation (NO in S106), the assist control is terminated.

(Assist control method 2)
Next, the assist control method 2 of the manipulator will be described with reference to the drawings.
FIG. 8 is a control block diagram of assist control according to the assist control method 2. FIG. 8A shows a case where an external force detected (measured) by an external force detection unit is used, and FIG. The case where the external force estimated using is used is shown. FIG. 9 is a diagram illustrating a control flow according to the assist control method 2. FIG. 10 is an explanatory diagram using a schematic diagram of the assist control method 2. FIG. 10 (a) is a schematic diagram when the manipulator 420 is operated, and FIG. 10 (b) sets the target value and performs assist control. It is a figure explaining the difference in the operation force when performing and not performing assist control. FIG. 11 is an explanatory diagram of the relationship between the detected external force related to the operating force target generator, the target value of the operating force, and the specified operating force value. FIG. 11A shows the dead zone when the external force is insensitive to the target value. FIG. 11B shows a profile that the external force has no dead zone with respect to the target value.

In the assist control method 2, as shown in the control block diagrams of FIGS. 8 (a) and 8 (b),
The only difference from the assist control method 1 is that the operation force target generator 409 sets the target value of the operation force to perform the assist control that enables the operation with a constant external force. The other control blocks are the same as those shown in FIGS. 5A and 5B used in the description of the assist control method 1.
In any of the control block diagrams, the assist control is started by judging the presence / absence of an operation from the change in the position (joint angle) / speed of the joint of the manipulator 420 and the change in the external force, and the operation force is set by comparing with the external force. Assist control is performed to change the output of the motor 404 so as to be a value.

  For example, as in the control flow shown in FIG. 9, when the assist control is started (started), the target value of the operating force is set (S201), and then the position of the joint (joint angle) of the manipulator 420 Detection (S202) and external force (operation force) detection (S203) are performed. It is determined whether or not there is an operation from these detection results (S204), and when it is determined that there is an operation (YES in S204), an error between the set target value of the operating force and the detected external force is detected (S205). ). On the other hand, if it is determined that there is no operation (NO in S204), the process returns to the detection of the position (joint angle) of the joint part of the manipulator 420 (S202) and repeats until it is determined that there is an operation.

When an error between the target value of the operating force and the detected external force is detected (S205), a motor (torque) output value is calculated based on the target value and the detected error (S206), and the motor at the calculated output value is calculated. Driving is performed (S207).
Thereafter, the position (joint angle) detection (S208) and the external force (operation force) detection (S209) of the joint portion of the manipulator 420 after the motor drive are performed again. Then, it is determined whether or not there is an operation from these detection results (S210). If it is determined that there is an operation (YES in S210), an error between the set target value of the operating force and the detected external force is returned to detection. (S205). On the other hand, when it is determined that there is no operation (NO in S210), the assist control is terminated.

As described above, for example, the following effects can be achieved by performing the assist control.
When operating the manipulator 420 as shown in FIG. 10A, when assisting by setting a target value of the operating force with respect to the operating force without assist as shown in FIG. Therefore, it is possible to operate with a substantially constant light operating force with little fluctuation.
Here, in the setting of the operating force, that is, the setting of the operating force command value, a profile as shown in FIGS. 11A and 11B can be used. In any profile, after the detected external force (operation force) value becomes equal to or greater than the value F corresponding to the target value Ft, it is common in that the operation force command value is set to be the target value Ft. doing. However, the profile shown in FIG. 11A is shown in FIG. 11B in which the operating force command value is changed in proportion to the change in the external force until the value of the external force becomes equal to or greater than F corresponding to the target value Ft. Unlike the profile, the profile is such that the external force has a dead zone with respect to the target value: Ft.

Specifically, the operating force command value is not changed up to a value smaller than F, which is the value of the external force corresponding to the target value: Ft, for example, F1 (operating force command value = 0), and the external force value is F1 or more. Then, the operation force command value is set so as to be the target value: Ft.
This occurs when the profile shown in FIG. 11B is used by using the profile shown in FIG. 11A, that is, by using a profile in which the external force has a dead zone with respect to the standard value Ft. It is possible to suppress the occurrence of the following problems that may be feared.
When an external force is applied to the manipulator 420, the manipulator 420 suddenly operates or oscillates.

(Assist control method 3)
Next, the assist control method 3 for the manipulator will be described with reference to the drawings.
12 is a control block diagram of assist control according to the assist control method 3. FIG. 12 (a) shows a case where an external force detected (measured) by an external force detector is used, and FIG. 12 (b) shows an equation of motion and the like. The case where the external force estimated using is used is shown. FIG. 13 is a diagram illustrating a control flow according to the assist control method 3. FIG. 14 is an explanatory diagram when the motion region is limited to one axis in the four-degree-of-freedom manipulator 501, and FIG. 15 is an explanatory diagram when the motion region is limited to a certain plane in the four-degree-of-freedom manipulator 501. It is. FIG. 16 is an explanatory diagram when the operation region is limited to a certain one-axis rotation direction in the manipulator 501 having four degrees of freedom.

In the assist control method 3, as shown in the control block diagrams of FIGS. 12A and 12B, in addition to setting the target value of the operating force, the assist control based on the position control is performed. Only the point is different from the assist control method 2. The other control blocks are the same as those in FIGS. 8A and 8B used in the description of the assist control method 2.
In any control block diagram, when an external force (operation force) is detected, a position speed command profile for changing the position is generated based on the difference between the external force and the target value, and the motor 404 is driven. Assist control is performed so that the operating force is constant.

  For example, as in the control flow shown in FIG. 13, when the assist control is started (started), the position / speed command value of the hand of the manipulator 501 (see FIG. 14) is set (S301). This setting is set based on the operation area of the hand of the manipulator 501 input by the user and the initial value of the moving speed of the hand, which are input from an input unit, for example, the input unit 113 shown in FIG. . After this setting, the target value of the operating force is set (S302), and the position (joint angle) of the joint part (S303) of the manipulator 501 is detected (S303) and the external force (operating force) is detected (S304). It is determined whether or not there is an operation from these detection results (S305). If it is determined that there is an operation (YES in S305), an error between the set target value of the operating force and the detected external force is detected (S306). ). On the other hand, if it is determined that there is no operation (NO in S305), the process returns to the joint position (joint angle) detection (S303) of the manipulator 501 and is repeated until it is determined that there is an operation.

When an error between the target value of the operating force and the detected external force is detected (S306), based on the target value, the detection result of the position (joint angle) detection (S306), and the detected error, the hand position / speed command The position / speed command value set in the value setting (S301) is changed (S307). Based on the target value, the detected error, and the changed position / speed command value, the motor (torque) output value is calculated (S308), and the motor is driven with the calculated output value (S309).
Thereafter, the position (joint angle) detection (S310) and the external force (operation force) detection (S311) of the joint portion of the manipulator 501 after driving the motor are performed again. Then, it is determined whether or not there is an operation from these detection results (S312), and when it is determined that there is an operation (YES in S312), an error between the set target value of the operating force and the detected external force is returned to detection. (S306). On the other hand, when it is determined that there is no operation (NO in S312), the assist control is terminated.

By performing the assist control in this manner, the operation region of the hand (arm tip) of the manipulator 501 is input in advance by input means such as the input unit 113, and driving is performed when an external force (operation force) is applied to the manipulator 501. The output of the motor 404 to be controlled can be controlled. By controlling the output in this way, when an external force (operation force) is applied to the manipulator 501, the rotation angle of each joint portion to be rotated is controlled, and the movement direction and movement locus of the hand of the manipulator 501 are controlled. The operation area can be limited to an operation area previously input by an input unit such as the input unit 113. As a result, even if an external force is applied in a direction not intended by the user, it is possible to prevent the hand of the manipulator 501 from moving in that direction. Therefore, when an external force is applied to the manipulator 501, the hand of the manipulator 501 moves in a movement direction that is not intended by the user, or moves along an operation trajectory that is not intended by the user. Can be suppressed.
Therefore, it is possible to provide a manipulator control device that can teach (register) the movement of the hand of the manipulator 501 as desired by the user at an arbitrary position, posture, and movement locus in direct teaching performed by a human hand.

Next, by using the assist control method 3, a plurality of specific examples of the operation region of the hand of the manipulator 501 that can be input (limited or set) by an input unit such as the input unit 113 and the external force component detected in the operation region are used. An example will be described.
(Specific example 1)
The manipulator 501 of Example 1 is a four-degree-of-freedom manipulator having four axes as shown in FIG. 14, and coordinates of the hand z-axis 506, y-axis 507, and x-axis 508 are set, and the hand coordinate system 504 is set. doing. This hand coordinate system 504 can be arbitrarily set by the user. Further, although the manipulator 501 is a four-axis manipulator, it can be replaced with other multi-axis manipulators.

  In the manipulator 501 of the first specific example, the x-axis 508 is set as an operation region 505 as shown in FIG. For this reason, when the external force 502 applied to the hand of the manipulator 501 is disassembled by the hand coordinate system 504, the z-axis external force 509, the y-axis external force 510, and the x-axis external force 511 become the x-axis external force 511.

(Specific example 2)
The manipulator 501 of the specific example 2 is a manipulator having four degrees of freedom similar to the manipulator of the specific example 1, and the yz plane is set as an operation region 505 as shown in FIG. For this reason, when the external force 502 applied to the hand of the manipulator 501 is disassembled by the hand coordinate system 504, the z-axis external force 509, the y-axis external force 510, and the x-axis external force 511 are obtained, and the external force 503 in the operation region is the z-axis external force 509 and the y-axis external force. The total force is 510.

(Specific example 3)
The manipulator 501 of the third specific example is a manipulator having four degrees of freedom similar to the manipulators of the first and second specific examples. As illustrated in FIG. Yes. When the external force 502 applied to the hand of the manipulator 501 is decomposed by the hand coordinate system 504, a z-axis external force 509, a y-axis external force 510, and an x-axis external force 511 are obtained. Further, it is assumed that the external force 512 in the rotation direction is applied to the x-axis 508. Then, the external force 503 in the operation region becomes an external force 512 in the rotation direction.

The external force vector in the operation region detected in the above-described specific examples 1 to 3 is sent to the position / velocity command generation unit 411 (control unit) and used to generate command values for the motor 404 (drive unit) of each joint unit.
Here, the manipulators 501 of the above-described specific examples 1 to 3 are represented by a four-axis manipulator, but may be replaced by a multi-axis manipulator.
However, up to FIG. 15 can be applied up to the 3-axis manipulator, and up to FIG. 16 can be applied after the 4-axis manipulator.

When the control and each configuration related to the assist control described above are applied to the manipulator device 100 shown in FIG. 1, for example, the configurations listed below can be made.
The control unit 210 includes an input unit 113 that inputs an operation region at the tip of the first arm 101. When an external force (operation force) vector is detected by the external force detector 112, assist control is performed as follows. Based on the detection result of the external force detection unit 112 and the operation region input from the input unit 113, the first drive unit 160 (first motor 106) so that the tip of the first arm 101 moves within the input operation region. ) Or the second drive unit 170 (second motor 109) is controlled to assist the external force.

By controlling in this way, the first drive unit 160 (first motor) that is driven when an external force (operation force) is applied to the manipulator by previously inputting an operation region at the tip of the first arm 101 with the input unit 113. 106) and the output of the second drive unit 170 (second motor 109) can be controlled. By controlling the output in this way, when an external force (operation force) is applied to the manipulator, the rotation angle of each joint portion to be rotated is controlled, and the operation direction, the operation locus, etc. of the first arm 101 tip are controlled. The operation area can be limited to an operation area previously input by the input means. As a result, even if an external force is applied in a direction not intended by the user, it is possible to prevent the manipulator tip from moving in that direction. Therefore, when an external force is applied to the manipulator, the tip of the first arm 101 moves in a movement direction that is not intended by the user, or moves along an operation trajectory that is not intended by the user. Can be suppressed.
Therefore, as a manipulator control device that can teach the movement of the tip of the first arm 101 of the manipulator device 100 at an arbitrary position / posture / movement locus as intended by the teacher in direct teaching performed by a human hand. The control unit 210 can be provided.

Further, the manipulator device 100 determines the angles formed by the first arm 101 and the second arm 102, and the second arm 102 and the support column 111b connected to the first joint part 130 and the second joint part 140, respectively. An angle detector for detecting each joint is provided. Here, as the angle detection unit, the first arm encoder 131 and the second arm encoder 141 may be provided independently, or the first motor encoder 107 and the second motor encoder 110 may be provided for each arm. It may function as an encoder. Then, the control unit 210 and the first drive unit 160 and the second drive are configured so that the tip of the first arm 101 moves within the input operation area based on at least one detection result of each arm encoder. The driving of the unit 170 is controlled to assist the external force.
Thus, the following effects can be produced by assisting the external force (operation force).
When the user applies an operating force (external force) to the manipulator, it is possible to perform an assist operation based on the detection result of at least one of the encoders for each arm. Can be increased.

Further, the control unit 210 maintains the posture of the tip of the first arm 101 so that the tip of the first drive unit 160 and the second drive unit 170 moves so that the tip of the first arm 101 moves within the input operation region. The drive is controlled to assist the external force.
By assisting the external force (operation force) in this way, for example, it is possible to perform an assist operation so as to translate the tip of the manipulator along the input movement direction.

In addition, the control unit 210 determines the angle formed by the first arm 101 and the second arm 102 and the second arm 102 and the column 111b connected to the first joint unit 130 and the second joint unit 140, respectively. It has the memory | storage part 215 memorize | stored for every joint part. Then, the input unit 113 (input device 114) can designate a trajectory to be drawn when the tip of the first arm 101 moves using the angle stored in the storage unit 215.
By being able to specify in this way, it becomes possible to perform an assist operation so as to move the tip of the manipulator along the specified trajectory.

In addition, the control unit 210 determines the angle formed by the first arm 101 and the second arm 102 and the second arm 102 and the column 111b connected to the first joint unit 130 and the second joint unit 140, respectively. It has the memory | storage part 215 memorize | stored for every joint part. Then, the input unit 113 (input device 114) can specify the plane on which the tip of the first arm 101 moves using the angle stored in the storage unit 215.
By being able to designate in this way, it is possible to perform an assist operation so that the tip of the first arm 101 is moved along the designated plane.

In addition, the control unit 210 determines the angle formed by the first arm 101 and the second arm 102 and the second arm 102 and the column 111b connected to the first joint unit 130 and the second joint unit 140, respectively. A storage unit such as a storage unit 215 for storing each joint unit is provided. Then, the input unit 113 (input device 114) can designate the position where the tip of the first arm 101 rotates using the angle stored in the storage unit 215.
By being able to designate in this way, it becomes possible to perform an assist operation so as to rotate the tip of the first arm 101 around the designated position (rotation axis).

Further, the control unit 210 performs control so that the moving speed of the manipulator tip such as the tip of the first arm 101 does not exceed a predetermined speed.
By controlling in this way, the following effects can be obtained.
In the configuration in which the drive of the first drive unit 160 and the second drive unit 170 is controlled so as to assist the external force applied to the manipulator based only on the detection result of the external force detection unit 112, it is far from the moving speed intended by the user. There is a possibility that the tip of the first arm 101 moves at a moving speed.
On the other hand, by providing a position / velocity command generation unit 411 in the control block for assist control or the like so as not to exceed a predetermined speed, the first arm 101 is moved at a moving speed far from the moving speed intended by the user. It can suppress that a tip moves.

  Moreover, the manipulator apparatus 100 which can show | play the effect similar to any one of the control parts 210 mentioned above can be provided by providing the control part 210 mentioned above as a control part with which a manipulator drive device is provided.

(Example 2)
Next, Example 2 of the manipulator driving device including the manipulator control device of the present embodiment will be described with reference to the drawings.
The manipulator driving device of the present embodiment (hereinafter, manipulator device 600) exemplifies a specific example of the manipulator driving device of the above-described first embodiment and the operation region specifying means included in the input means (input device) for inputting the operation region. The differences are mainly different. Since other functions can be provided with the same functions and configurations as those of the manipulator driving device according to the first embodiment, the description thereof will be omitted as appropriate.

FIG. 17 is a schematic explanatory diagram of the manipulator device 600 according to the present embodiment, FIG. 18 is an explanatory diagram of the tool point coordinate system 691 and the manipulator absolute coordinate system 692, and FIG. It is explanatory drawing of. FIG. 20 is an explanatory diagram of an example when an operation region (operation direction) is designated.
Here, in this embodiment, as shown in FIG. 17, a manipulator device 600 having a four-degree-of-freedom manipulator will be described as an example. It may be articulated.

  As shown in FIG. 17, the manipulator device 600 of this embodiment is installed on an installation table 620. And there is a non-moving part 605 on the base side of the manipulator, and a fourth joint 604 is arranged. The fourth joint 604 and the third joint 603 are connected by a third support portion 613. Similarly, the third joint 603 and the second joint 602 are connected by a second support portion 612, and the second joint 602 and the first joint 601 are connected by a first support portion 611. Further, an end effector 610 as a work tool is provided at the tip of the first joint 601.

Here, the first joint 601 is also called a tool point, and the user (operator) 630 operates the vicinity of the tool point (first joint 601) in order to move the posture and position of the end effector 610 during direct teaching. That is natural.
Further, when the user performs direct teaching (teaching / setting) on the manipulator device 600, it is necessary to consider the tool point (first joint 601) coordinate system 691 and the manipulator absolute coordinate system 692 as shown in FIG. There is. Here, the tool point coordinate system 691 changes its position and orientation as the fourth joint 604 rotates from the first joint 601, and the manipulator absolute coordinate system 692 does not move.

Also, if the user (operator) 630 wants to move the tool point straight along the Xtp vector, for example, it is not possible to determine the direction or move straight because the conventional direct teaching is a human operation. Is possible. Further, by providing the manipulator control device as described in the first embodiment, in the direct teaching performed by a human hand, the movement of the first support portion of the manipulator driving device can be moved to any position / posture as compared with the prior art.・ It can be taught accurately with the movement trajectory.
However, in the configuration using the input device 114 or the like held by the user as the input means, when the user applies an external force as an operation force to the manipulator, the following necessity may arise, and intuitive and simple teaching It may be insufficient as a method. It is necessary for the user to work while holding one hand or both hands away from the input device 114 or the like, or to operate while grasping and confirming the manipulator coordinate system regardless of the manipulator position and posture.

Therefore, in the manipulator device 600 of this embodiment, in order to make the direct teaching method performed by a human hand a more intuitive and simple teaching method, as a means for specifying an operation area as an input means for inputting an operation area, The operation area designating means is provided.
As shown in FIG. 19, the first joint 601 includes an Xtp motion region designation button 651 for designating a motion region in the Xtp vector in the tool point coordinate system, and a Ztp motion region designation button 652 for designating a motion region in the Ztp vector. Have.
In a configuration without these, for example, when the user (operator) wants to move straight along the Xtp vector as an intention, the operation force F applied by the user: F deviates from θtp by θdeg as shown in FIG. In this case, the assist is performed with a shift of θ deg from Xtp.
On the other hand, as shown in FIG. 19, in the configuration in which the first joint 601 includes the Xtp action area designation button 651 and the Ztp action area designation button 652, when the user 630 presses the Xtp action area designation button 651, the Ztp and Ytp directions There is a brake (regulation). Then, only the Xtp direction is assisted, and the tool point (first joint 601) can be guided as intended by the user 630.

  The Xtp operation area designation button 651 is arranged in such a direction as to clearly indicate the Xtp direction, and can be selected intuitively without the user being aware of the tool point coordinate system 691. Further, the Xtp operation area designation button 651 is provided with an LED, and when pressed, it is lit in blue so that the designation of the operation area is clearly indicated.

  In the above description, a configuration has been shown in which, when the Xtp operation area designation button 651 is pressed, the operation can be performed only in the designated direction Xtp. However, when it is desired to move the Xtp-Ytp plane freely while maintaining the Ztp direction which is the height direction, it is better to specify the Ztp direction and restrict the movement in the Ztp direction, that is, to disable the operation. Sometimes it is convenient. For this reason, when the Ztp operation area designation button 652 is pressed once, the LED is lit blue so that it can operate only in Ztp. It is configured to be released.

  In the above description, the configuration example related to the operation area designation in the tool point coordinate system 691 has been described. However, when the teaching pendant is used in combination, or when it is desired to teach the coordinates of the manipulator device and peripheral devices such as a conveyor for conveying parts, it is necessary to operate the manipulator absolute coordinate system 692 as well. Therefore, as shown in FIG. 19, the Xab operation area designation button 654, the Yab action area designation button 655 and the Zab action area designation button 653 for the manipulator absolute coordinate system 692 are connected to the immovable portion 605 to which the third support portion 613 is connected. Is provided. By these, the operation area can be designated with respect to the manipulator absolute coordinate system 692.

As described above, in the manipulator control device (control unit 210) included in the manipulator device 600 according to the present embodiment, the input unit specifies the moving direction of the first joint 601 serving as the manipulator tip, for example, the specified vector direction such as the Xtp direction. Xtp operation area designation buttons 651 and the like limited to only those.
Thereby, the following effects can be produced.
The tool point coordinate system 691 or the manipulator absolute coordinate system 692 moves in any of the X, Y, and Z directions, for example, only in the Xtp (lateral direction) direction of the tool point coordinate system 691, and moves in the horizontal direction. Intuitive teaching is possible when desired.

In the manipulator control device according to the present embodiment, the input means controls the movement direction of the first joint 601 serving as the manipulator tip, for example, only the movement in the designated vector direction such as the Ztp direction. A designation button 652 and the like are included.
Thereby, the following effects can be produced.
Only the movement of the tool point coordinate system 691 or the manipulator absolute coordinate system 692 in any of the X, Y, and Z directions, for example, the Ztp (height) direction of the tool point coordinate system 691, is restricted (fixed) to the height. Intuitive teaching is possible when it is desired to hold and move.

Moreover, in the control device for the manipulator of the present embodiment, the input means has the following LEDs. This LED is lit to display the vector direction designated by the Xtp operation area designation button 651, the Ztp action area designation button 652, the Xab action area designation button 654, the Yab action area designation button 655, the Zab action area designation button 653, and the like.
Thereby, the following effects can be produced.
Since the vector direction specified by the motion area specifying means as described above, that is, in which direction it can be moved or in which direction movement is restricted, can be transmitted to the user (operator), the user's intuitive Can help the operation.

In the control device for the manipulator of the present embodiment, the input means includes an Xtp action area designation button 651, a Ztp action area designation button 652, an Xab action area designation button 654, a Yab action area designation button 655 for designating an action area, and A controller having a Zab operation area designation button 653 and capable of finely adjusting the operation area designated by the operation area designation means can be provided.
Thereby, the following effects can be produced.
If it is desired to move the tool point coordinate system 691 or the manipulator absolute coordinate system 692 in a direction that is not any of the X, Y, and Z directions, for example, in the direction of 30 deg relative to the Xab direction of the manipulator absolute coordinate system 692, it can be adjusted by the controller.

  Moreover, in the manipulator drive device of the present embodiment, by providing any of the above-described manipulator control devices as a manipulator control device, the same effects as any of the above-described manipulator control devices can be achieved. A manipulator device 600 can be provided.

  In the manipulator device 600 according to the present embodiment, the input means for inputting the operation region of the first joint 601 is installed in the manipulator device 600 main body. And, it has operation area designation means such as an Xtp action area designation button 651, a Ztp action area designation button 652, an Xab action area designation button 654, a Yab action area designation button 655, and a Zab action area designation button 653 for designating an action area. ing.

Thereby, the following effects can be produced.
In the configuration using the input device 114 or the like held by the user as the input means, when the user applies an external force as an operation force to the manipulator, the following needs arise, which is insufficient as an intuitive and simple teaching method. It may become. It is necessary for the user to work while holding one hand or both hands away from the input device 114 or the like, or to operate while grasping and confirming the manipulator coordinate system regardless of the manipulator position and posture.
On the other hand, in the configuration in which the input means has an operation area designating means that is installed in the main body of the manipulator device 600 and designates the operation area, the operation area designating means can be arranged in the vicinity where the user applies the operation force. It is possible to suppress the occurrence of unnecessary necessity.
Therefore, it is possible to arbitrarily switch the guiding direction without leaving the main body of the manipulator device 600, and a manipulator device that can easily and intuitively perform direct teaching even if it is not a specialized worker who has acquired education and experience. 600 can be provided.

In the manipulator device 600 according to the present embodiment, the operation area designation means such as the Xtp movement area designation button 651 and the Ztp movement area designation button 652 included in the input means are arranged at the first joint 601 near the manipulator tip. ing.
As a result, it becomes possible to input (set) an operation region in the vicinity of the tool point that is one of the user's operation positions, and thus more intuitive teaching can be performed.

Further, in the manipulator device 600 according to the present embodiment, the operation area specifying means such as the Xab operation area specifying button 654, the Yab operation area specifying button 655, and the Zab operation area specifying button 653 included in the input means are provided in the non-moving portion 605. Has been placed.
Thereby, the following effects can be produced.
Since the operation region can be defined by the manipulator absolute coordinate system 692 at the root of the manipulator device 600, more practical teaching can be performed.

(Example 3)
Next, Example 3 of the manipulator driving device including the manipulator control device of the present embodiment will be described with reference to the drawings.
The manipulator driving device of the present embodiment (hereinafter referred to as manipulator device 700) is mainly different from the manipulator driving device of Embodiments 1 and 2 described above in the following configuration. This is a point that a user (operator) is provided with a contact detection means for detecting a support portion to which an operation force (external force) is applied, and a configuration in which a joint portion to be rotated as intended is provided. Since other functions can be provided with the same functions and configurations as those of the manipulator driving device according to the first embodiment, the description thereof will be omitted as appropriate.

  FIG. 21 is a schematic explanatory diagram of a manipulator device 700 according to the present embodiment, in which FIG. 21 (a) is a plan explanatory diagram, and FIG. 21 (b) is a side explanatory diagram of main components. FIG. 22 is a control block diagram of the drive control device of the manipulator device 700 that operates the first joint portion 130, the second joint portion 140, and the third joint portion 150 according to the present embodiment. 23 is an explanatory diagram of an example of a contact detection unit that can be provided in the manipulator device 700. FIG. 23 (a) is an explanatory diagram of an example using a touch sensor, and FIG. 23 (b) is an optical sensor. It is explanatory drawing of an example.

FIG. 24 is an explanatory diagram of an example of the operation mode of the manipulator device 700 using the contact detection unit. FIG. 24A is an explanatory diagram of the operation mode 1 in which the first operation mode is illustrated, and FIG. It is explanatory drawing of the operation mode 2 which is a 2nd operation mode. FIG. 24C is an explanatory diagram of the operation mode 3 that is the third operation mode, and FIG. 24D is an explanatory diagram of the operation mode 4 that is the fourth operation mode. FIG. 25 is an explanatory diagram of an example of an operation control sequence in the direct teaching of the present embodiment.
Here, in this embodiment, as shown in FIG. 21 and the like, a manipulator device 700 including a manipulator having three degrees of freedom (manipulator having three joint portions) will be described as an example. However, the gist of the present embodiment described below is not limited to such a configuration, and operation mode 1 and operation mode 2 described later have two or more degrees of freedom, and operation mode 3 and operation mode 4 have three degrees of freedom. The present invention can be applied to a manipulator driving device provided with a manipulator of a degree higher than that.

  As shown in FIG. 21, the manipulator device 700 has a first joint portion 130 at the most rear side (front end side) from the column portion 111 b which is a fixed portion fixed to the base 111 a of the device body among the plurality of support portions. The first arm 101 is connected via the first arm 101. On the front stage side, the second arm 102 connected to the first arm 101 via the first joint part 130 is provided, and on the further front side, the second arm 102 is provided via the second joint part 140. And a third arm 121 coupled to the. The third arm 121 is coupled to the support column 111b via the third joint 150.

The first arm 101, the second arm 102, and the third arm 121 are supported as follows. As shown in FIG. 21 (b), the first arm 101 and the second arm 102 have a first drive center 103 of the first joint part 130 and a second joint part 140 of the second joint part 140, respectively, in the vicinity of the front end. The drive center 104 is rotatably supported. The third arm 121 is also rotatably supported by the third drive center 122 of the third joint 150 in the vicinity of the front end.
Here, the first drive center 103 of the first joint part 130, the second drive center 104 of the second joint part 140, and the third drive center 122 of the third joint part 150 are provided in parallel.

The first joint unit 130 is connected to a first motor 106 as a first drive unit 160 and a first speed reducer 105 having the first drive center 103 as an output axis. The rotational driving force from is decelerated and transmitted to the first arm 101, and the first arm 101 is rotationally driven. A first motor encoder 107 is attached to the side surface of the first motor 106 opposite to the second arm 102 so that the amount of rotation of the first motor 106 can be measured. 105 and the first motor 106 are fixed to the second arm 102.
A second motor 109 as a second drive unit 170 and a second speed reducer 108 having the second drive center 104 as an output axis are connected to the second joint unit 140. The rotational driving force is decelerated and transmitted to the second arm 102, and the second arm 102 is rotationally driven. A second motor encoder 110 is attached to the side surface of the second motor 109 opposite to the third arm 121 so that the rotation amount of the second motor 109 can be measured. 108 and the second motor 109 are fixed to the third arm 121.

  A third motor 124 as a third drive unit 180 and a third speed reducer 123 having the third drive center 122 as an output axis are connected to the third joint unit 150. The rotational driving force from is decelerated and transmitted to the third arm 121, and the third arm 121 is rotationally driven. A third motor encoder 125 is attached between the support 111b and the third motor 124 so that the amount of rotation of the third motor 124 can be measured. These are fixed to the support 111b (base 111a).

  The first motor encoder 107 also functions as a first arm encoder 131 that measures the rotation amount of the first arm 101 relative to the second arm 102 from the reduction ratio of the first reduction gear 105 and the measured rotation amount. The second motor encoder 110 also functions as a second arm encoder 141 that measures the rotation amount of the second arm 102 relative to the third arm 121 from the reduction ratio of the second reduction gear 108 and the measured rotation amount. . The third motor encoder 125 also functions as a third arm encoder 151 that measures the rotation amount of the third arm 121 relative to the support column 111b from the reduction ratio of the third reduction device 123 and the measured rotation amount. However, the first arm encoder 131, the second arm encoder 141, and the third arm encoder 151 are provided separately, and the angle formed by each arm connected to each joint portion is measured, and the rotation of each joint portion is measured. It is good as quantity.

  The first arm 101 has a first external force detector 132 for detecting a vector of an external force applied to the first drive center 103 via the first arm 101, and an object such as a user's hand contacts the first arm 101. A first contact detection unit 133 for detecting the occurrence of the failure is attached. A gripping mechanism that is a work tool is attached to the tip of the first arm 101. On the other hand, the second arm 102 includes a second external force detector 142 for detecting a vector of an external force applied to the second drive center 104 via the second arm 102 and an object such as a user's hand. A second contact detection unit 143 for detecting that the contact has been made is attached. The third arm 121 includes a second external force detector 142 for detecting a vector of an external force applied to the third drive center 122 via the third arm 121, and an object such as a user's hand. A third contact detection unit 153 for detecting that the contact has been made is attached.

As will be described in detail later, the following is connected to the control unit 210a that functions as a control device for the manipulator. Each motor, each motor encoder, each external force detection unit, each contact detection unit, and the drive of each drive unit that rotates the operation region of each arm tip and each joint unit are controlled to assist the external force (assist control ) An input unit 113 for inputting an operation mode is connected.
As an example of the operation, when an external force is applied to the second external force detection unit 142 and the third external force detection unit 152 and the contact of an object such as a user's hand is detected by the second contact detection unit 143, a signal from the control unit 210 is used. The operation command is transmitted to the second motor 109, and the second motor 109 is driven. Here, the second motor 109 among the first motor 106, the second motor 109, and the third motor 124 is driven according to the operation mode input from the input unit 113. Further, when any of the arms is driven, rotation information of each motor and rotation information of each arm can be measured from each motor encoder.

Next, it is a control block diagram of the drive control device of the manipulator device 700 for operating the first joint portion 130, the second joint portion 140, the third joint portion 150, and a specific example of the manipulator device 100 using FIG. The configuration and operation will be described.
The manipulator device 700 mainly includes a manipulator body 200a, a power supply unit 291 and a host controller 281 as in the case of the manipulator device according to the first embodiment described with reference to FIG. For example, when image information is required for manipulator operation, an image input device 271 may be connected. Here, in FIG. 22, the manipulator main body 200a, the host controller 281 and the power supply unit 291 are separated. However, all of them may be a stand-alone configuration mounted in the manipulator main body 200a.

The controller 210a is provided with a CPU 211, which is a central processing unit, and controls the operation of the manipulator body 200a.
The host controller 281 and the CPU 211 are connected via a communication network 282, and the communication network 282 receives operation mode information, various parameters necessary for drive control, target movement position information of the manipulator body 200a, and the like.
The CPU 211 receives signals from various sensors 220 and switches 221. Based on the signal information, the CPU 211 inputs (sets) the operation area of the manipulator, and starts and stops such as operation start and emergency stop. Done.

Each joint of the manipulator body 200 a is driven by the first joint portion 130 by the first motor 106, the second joint portion 140 by the second motor 109, and the third joint portion 150 by the third motor 124.
The drive information of the first motor 106, the second motor 109, and the third motor 124 is detected as the first drive information as signals from the first motor encoder 107, the second motor encoder 110, and the third motor encoder 125. Are input to the unit 234, the second drive information detection unit 236, and the third drive information detection unit 238. These input signals are converted into rotation information such as a movement amount, a movement speed, and a movement acceleration by the first drive information detection unit 234, the second drive information detection unit 236, and the third drive information detection unit 238, respectively. The converted rotation information and the external force applied to the manipulator body 200a detected by the external force detection unit 112 are input to the drive control unit 214 in the CPU 211 (control unit 210a).

In addition, the operation region at the tip of the first arm 101 is input to the CPU 211 from the input unit 113, and the operation region and the operation direction at the tip of the first arm 101 are restricted.
In addition to this, in this embodiment, the operation mode is input or changed from the input unit 113, and the driving force of each drive motor that rotates each joint unit is controlled to assist the external force.
The drive control unit 214 generates a target command value based on the rotation information, the external force, the limited operation direction, and the operation mode. Then, a control command is issued to the first motor driver 233, the second motor driver 235, and the third motor driver 237 so as to drive according to the generated target command value, and the first motor 106, the second motor 109, and The third motor 124 is driven.
Further, the CPU 211 has a storage unit 215, and this storage unit 215 functions as a storage unit that stores, for each joint unit, an angle formed by a support unit coupled to each joint unit.

Here, in the control unit 210a, the angle formed by the support unit connected to each joint unit as the time from the start and the position and posture of the manipulator at regular intervals from the direct teaching start time is set for each joint unit. Registered (stored) in the storage unit 215. Thus, when the user moves the manipulator, the current manipulator position / posture and the motion trajectory up to the current position / posture are registered.
When the manipulator reproduces the direct teaching operation, the control unit 210a uses the cue of the user after direct teaching (instruction from the input unit 113), and the time interval registered in the position and orientation registered in the storage unit 215. Each joint is driven according to the above.
Also, the input and change of the operation area, the operation direction, and the operation mode of the arm tip of the first arm 101 are performed by the touch panel 114a and the numeric keypad 114b of the input device 114 similar to the first embodiment described with reference to FIG. Can do. For example, the type of operation mode is displayed on the touch panel 114a, and the operation mode is selected from the touch panel 114a or the numeric keypad 114b. The selected input signal is input to the control unit 210a and used when generating an operation command value.

Next, an example of each contact detection unit that can be provided in the manipulator device 700 according to the present embodiment will be described with reference to FIGS. 23 (a) and 23 (b).
FIG. 23A illustrates an example in which a touch sensor is used for each contact detection unit that detects that a user's hand, which is an object, has touched one of the arms.
As shown in FIG. 23A, the first touch sensor 134 as the first contact detection unit 133 is provided on the side surface of the first arm 101. A second touch sensor 144 as the second contact detection unit 143 is provided on the side surface of the second arm 102, and a third touch sensor 154 as the third contact detection unit 153 is provided on the side surface of the third arm 121. Provided.
These touch sensors are connected to the drive control unit 214 of the CPU 211 (control unit 200a) described above, and a threshold value is set inside the CPU 211. When the threshold value is exceeded, it is determined that the touch is made.

FIG. 23B illustrates an example in which an optical sensor is used for each contact detection unit that detects that the user's hand, which is an object, has touched any arm.
As shown in FIG. 23 (b), a first optical sensor 135 as a first contact detection unit 133 is provided on the upper front side (first joint portion 130) of the first arm 101. A second optical sensor 145 as a second contact detection unit 143 is provided on the upper front stage side (second joint part 140) of the second arm 102, and the upper front stage side (third joint part 150) of the third arm 121 is provided. ) Is provided with a third optical sensor 155 as a third contact detector 153.
These three optical sensors are connected to the drive control unit 214 of the CPU 211 (control unit 200a) described above, and a threshold value is set inside the CPU 211. When the threshold value is exceeded, it is determined that a contact has occurred.

Next, an example of an operation mode using each contact detection unit in the manipulator device 700 of the present embodiment will be described with reference to FIG.
The manipulator device 700 of the present embodiment has four operation modes of operation mode 1 to operation mode 4 described below. The assist control of the operation mode set (input / selection / change) using the input device 114 generates a target command value based on the rotation information, the external force, the limited operation direction, and the operation mode. The assist control methods 1 to 3 described in 1 are performed in the same manner.

Here, in the manipulator device 700 including a plurality of support portions connected in series via a plurality of joint portions, an intended joint portion does not rotate or an unintended joint portion rotates. Sometimes. If the intended joints cannot be rotated in this way, the rotation angles of the joints will deviate from the intended ones, and it will be necessary to correct the rotation of each joint, and the time required for teaching will be May be longer.
Therefore, the manipulator device 700 of this embodiment is provided with a plurality of operation modes as follows.

(Operation mode 1)
In the operation mode 1 (first operation mode), for example, as shown in FIG. 24A, when the user manually applies the first operating force F71 to the second arm 102 downward, the second joint part 140 is applied. Assist control for driving the second drive unit 170 and the third drive unit 180 of the third joint unit 150 is performed.
Here, as described above, the manipulator device 700 is in contact with the first arm 101, the second arm 102, and the third arm 121, which are the support portions excluding the support column portion 111 b, among the plurality of support portions. A contact detector for detecting the occurrence of the failure is provided. And between the 2nd arm 102 which is the contact support part which provided the 2nd contact detection part 143 which detected that the object contacted, and the support | pillar part 111b, two joint parts 140 and the 3rd joint part 150 are two. There is a joint.
In such a case, in the operation mode 1, the control unit 210a assists the first operating force F71 by controlling the driving of the second driving unit 170 and the third driving unit 180 that rotate the two joint portions, respectively. .

By having such an operation mode 1, the manipulator device 700 can exhibit the following effects.
When it is desired to rotate only two joints between the second arm 102 which is the contact support portion and the support column 111b, the operation mode 1 is set, and the first operating force F71 is applied to the second arm 102. Can be added. By applying the first operating force F71 in this way, the intended joint part does not rotate or the unintended joint part rotates, so that the rotation angle of each joint part is intended. It is possible to suppress deviation from what has been done.
Therefore, it is possible to reduce the time required to correct the rotation angle of each joint, and to reduce the time required when teaching the manipulator driving device 700.

(Operation mode 2)
In the operation mode 2 (second operation mode), for example, as shown in FIG. 24B, when the user manually applies the second operating force F72 to the second arm 102 downward, the second joint part 140 is applied. Assist control for driving the second drive unit 170 is performed.
Here, as described above, the manipulator device 700 is in contact with the first arm 101, the second arm 102, and the third arm 121, which are the support portions excluding the support column portion 111 b, among the plurality of support portions. A contact detector for detecting the occurrence of the failure is provided. And between the 2nd arm 102 which is the contact support part which provided the 2nd contact detection part 143 which detected that the object contacted, and the support | pillar part 111b, two of the 2nd joint part 140 and the 3rd joint part 150 are provided. There are (two or more) joints.
In such a case, in the operation mode 2, the control unit 210a rotates the second joint unit 140 to which the second arm 102 serving as the contact support unit is coupled among the two (two or more) joint units. The driving of the second driving unit 170 is controlled to assist the second operating force F72.

By having such an operation mode 2, the manipulator device 700 can exhibit the following effects.
When it is desired to rotate only one joint part on the column part 111b side to which the second arm 102 as the contact support part is connected, the operation mode 2 is set, and the second operating force F72 is applied to the second arm 102. Can be added. By applying the second operating force F72 in this way, the intended joint part does not rotate or the unintended joint part rotates, so that the rotation angle of each joint part is intended. It is possible to suppress deviation from what has been done.
Therefore, it is possible to reduce the time required to correct the rotation angle of each joint, and to reduce the time required when teaching the manipulator driving device 700.

(Operation mode 3)
In the operation mode 3 (third operation mode), for example, as shown in FIG. 24C, when the user manually applies the third operating force F73 downward to the first arm 101, the following assistance is provided. Take control. Assist control for driving the first drive unit 160 of the first joint unit 130, the second drive unit 170 of the second joint unit 140, and the third drive unit 180 of the third joint unit 150 is performed.
Here, as described above, the manipulator device 700 is in contact with the first arm 101, the second arm 102, and the third arm 121, which are the support portions excluding the support column portion 111 b, among the plurality of support portions. A contact detector for detecting the occurrence of the failure is provided. And between the 1st arm 101 which is the contact support part which provided the 1st contact detection part 133 which detected that the object contacted, and the support | pillar part 111b, the 1st joint part 130, the 2nd joint part 140, and the 1st There are three (three or more) joints of the three joints 150.
In such a case, in the operation mode 3, the control unit 210a rotates the three joint portions (three of the three or more joint portions on the side close to the first arm 101 that is the contact support portion). The first driving unit 160, the second driving unit 170, and the third driving unit 180 are controlled to assist the first operating force F71.

By having such an operation mode 3, the manipulator device 700 can achieve the following effects.
If you want to rotate only the three joints (on the side close to the first arm 101) between the first arm 101 and the support column 111b, which are contact support parts, set the operation mode 3, A third operating force F73 can be applied to the first arm 101. By applying the third operating force F73 in this way, the intended joint part does not rotate or the unintended joint part rotates, so that the rotation angle of each joint part is intended. It is possible to suppress deviation from what has been done.
Therefore, it is possible to reduce the time required to correct the rotation angle of each joint, and to reduce the time required when teaching the manipulator driving device 700.

(Operation mode 4)
In the operation mode 4 (fourth operation mode), for example, as shown in FIG. 24D, when the user manually applies the fourth operating force F74 to the first arm 101 downward, the first joint portion 130 is provided. Assist control for driving the first drive unit 160 and the second drive unit 170 of the second joint unit 140 is performed.
Here, as described above, the manipulator device 700 is in contact with the first arm 101, the second arm 102, and the third arm 121, which are the support portions excluding the support column portion 111 b, among the plurality of support portions. A contact detector for detecting the occurrence of the failure is provided. And between the 1st arm 101 which is the contact support part which provided the 1st contact detection part 133 which detected that the object contacted, and the support | pillar part 111b, the 1st joint part 130, the 2nd joint part 140, and the 1st There are three (three or more) joints of the three joints 150. Further, the rotation axes of the three joint portions (on the side close to the first arm 101 which is the contact support portion) are provided in parallel.
In such a case, in the operation mode 4, the control unit 210a rotates the two joint portions on the side close to the first arm 101 that is the contact support portion among the three (three or more) joint portions. The drive of the one drive part 160 and the 2nd drive part 170 is controlled, and the 1st operating force F71 is assisted.

By having such an operation mode 4, the manipulator device 700 can achieve the following effects.
When it is desired to rotate only the two joint portions between the first arm 101 which is the contact support portion and the support column portion 111b, the rotation shafts on the side close to the first arm 101 are parallel, operation mode 4 The fourth operating force F74 can be applied to the first arm 101. By applying the fourth operating force F74 in this way, the intended joint part does not rotate or the unintended joint part rotates, and the rotation angle of each joint part is intended. It is possible to suppress deviation from what has been done.
Therefore, it is possible to reduce the time required to correct the rotation angle of each joint, and to reduce the time required when teaching the manipulator driving device 700.

Next, an example of an operation control sequence of assist control in the manipulator device 700 of the present embodiment will be described with reference to FIG.
As shown in FIG. 25, when direct teaching (assist control) is started, first, external force (operation force) is detected, and it is determined whether any external force is detected by any of the external force detection units (S401). If it is determined in this determination that any of the external force detection units has detected an external force (YES in S401), contact position detection for specifying (detecting) which contact detection unit has detected the contact is performed ( S402). On the other hand, when it is determined that no external force is detected by any of the external force detection units (NO in S401), the external force detection is repeated until the external force is detected by any of the external force detection units. Further, even when the contact position detection cannot determine which contact detection unit detects contact (NO in S402), the process returns to the external force detection, and the external force is detected by any one of the external force detection units. The external force detection and the contact position detection are repeated until it can be determined whether or not contact has been detected.

  If it is possible to identify which contact detection unit has detected the contact by detecting the contact position (YES in S402), the set (selected) operation mode is determined (S403 to S406). Then, assist control for driving the drive unit based on the determined operation mode and the contact position specified by the contact position detection is performed (S407 to S410), and the operation of the manipulator is stored (registered) (S411), and the process ends. . However, if it is determined whether or not the operation mode is 4 (S406) and it is determined that the operation mode is not 4 (NO in S406), the operation is terminated without storing the operation of the manipulator.

More specifically, assist control is performed as follows.
It is determined whether or not the operation mode is the operation mode 1 (S403). If it is determined that the operation mode is the operation mode 1 (YES in S403), for example, the second contact detection unit 143 is identified as the second contact. The drive unit 170 and the third drive unit 180 are driven to perform assist control (S407). Then, the operation of the manipulator is stored (registered) (S411), and the process ends.
On the other hand, when it is determined that the operation mode is the operation mode 1 or not (NO in S403), the process proceeds to the determination of whether or not the operation mode is 2 (S404).

When it is determined whether or not the operation mode 2 is selected (YES in S404), for example, when the contact at the second contact detection unit 143 is specified, the second drive unit 170 is driven. Assist control is performed (S408). Here, when the contact at the first contact detection unit 133 is specified, the first drive unit 160 is driven. When the contact at the third contact detection unit 153 is specified, the third drive unit 180 is driven to perform assist control. Then, the operation of the manipulator is stored (registered) (S411), and the process ends.
On the other hand, if it is determined whether the operation mode is the operation mode 2 or not (NO in S404), the process proceeds to the determination of whether or not the operation mode is 3 (S405).

If it is determined whether or not the operation mode 3 is selected (YES in S405), the first drive unit 160 and the second drive unit 170 are identified when the contact in the first contact detection unit 133 is specified. Then, the third drive unit 180 is driven to perform assist control (S409). Then, the operation of the manipulator is stored (registered) (S411), and the process ends.
On the other hand, if it is determined whether the operation mode is the operation mode 3 or not (NO in S405), the process proceeds to the determination of whether or not the operation mode is 4 (S406).
If it is determined whether or not the operation mode 4 is in the operation mode 4 (YES in S406), the first drive unit 160 and the second drive unit 170 are identified after the contact in the first contact detection unit 133 is specified. To perform assist control (S410). Here, in determining whether or not the operation mode is 4, it is also a condition that the rotation axes of the adjacent drive units to be driven are parallel. And it complete | finishes, without memorize | storing operation | movement of a manipulator.

One feature of the operation control sequence of the assist control described above is that the assist control is not performed if the contact position cannot be detected even if the external force is detected by any of the external force detection units.
By configuring in this way and selecting an operation mode and applying an operating force (external force) to the contact support part, the intended joint part does not rotate or the unintended joint part rotates. And it can suppress reliably that the rotation angle of each joint part deviates from what was intended.
Therefore, it is possible to reduce the time required to correct the rotation angle of each joint, and to reduce the time required when teaching the manipulator driving device 700.

As described above, the present embodiment has been described with reference to the drawings. However, the specific configuration is not limited to the configuration provided with the above-described manipulator control device of the present embodiment, and the scope of the present invention is not deviated. Design changes may be made.
For example, in a robot system provided with a manipulator drive device, the present invention can also be applied to a robot system provided with at least one of the above-described manipulator control device or any manipulator drive device. By configuring the robot system including the manipulator driving device in this way, it is possible to provide a robot system that can achieve the same effects as any of the manipulator control devices described above or any of the manipulator driving devices.

What was demonstrated above is an example, and there exists an effect peculiar for every following aspect.
(Aspect A)
Two or more joint parts such as the first joint part 130 and the second joint part 140 are connected in series via each joint part, and are supported such as a gripping mechanism attached to a manipulator tip such as the tip of the first arm 101. A plurality of support parts such as a first arm 101, a second arm 102, and a column part 111b, and a first drive part 160 and a second drive part 170 that rotate the two or more joint parts, respectively. Such as the external force detection unit 112 that detects a vector of external force applied from the outside to at least one of the above-described drive unit, the two or more joint units, and the plurality of support units, and the tip of the first arm 101, etc. And a manipulator driving device such as a manipulator device 100 including an external force detecting unit, and controls driving of the two or more driving units based on a detection result of the external force detecting unit. The manipulator control device such as the control unit 210 has input means such as an input unit 113 (input device 114) for inputting an operation region of the manipulator tip, and the external force vector is detected by the external force detection unit. And driving the two or more drive units so that the tip of the manipulator moves within the input operation region based on the detection result of the external force detection unit and the operation region input from the input means. It is characterized by controlling.

According to this, as described in the present embodiment, the operation area of the tip portion of the manipulator is input in advance by the input means, and the output of each driving unit that is driven when an external force (operation force) is applied to the manipulator is output. Can be controlled. By controlling the output in this way, when an external force (operation force) is applied to the manipulator, the rotation angle of each joint portion to be rotated is controlled, and the operation area such as the operation direction and the operation locus of the manipulator tip Can be limited to the operation region previously input by the input means. As a result, even if an external force is applied in a direction not intended by the user, it is possible to prevent the manipulator tip from moving in that direction. Therefore, when an external force is applied to the manipulator, the manipulator tip can be prevented from moving in a movement direction not intended by the user or moving along an operation trajectory not intended by the user. .
Therefore, it is possible to provide a manipulator control device capable of teaching the movement of the manipulator tip of the manipulator driving device at an arbitrary position, posture, and movement locus as intended by a teacher in direct teaching performed by a human hand. .

(Aspect B)
In (Aspect A), the manipulator driving device such as the manipulator device 100 includes a first arm 101 and a second arm connected to each of the two or more joint portions such as the first joint portion 130 and the second joint portion 140. 102 or the second arm 102 and two or more such as a first arm encoder 131 and a second arm encoder 141 that detect the angle formed by the support portion such as the support column portion 111b or the support target for each of the two or more joint portions. An angle detector, and based on at least one detection result of the two or more angle detectors, the tip of the manipulator such as the tip of the first arm 101 moves within the input operation region. The external force is assisted by controlling driving of the two or more driving units such as the first driving unit 160 and the second driving unit 170.

According to this, as described in the present embodiment, the following effects can be achieved.
When the user applies an operating force (external force) to the manipulator, it is possible to perform an assist operation based on at least one detection result of two or more angle detection units, and the accuracy of the assist operation of the manipulator tip is Can be increased.

(Aspect C)
In (Aspect A) or (Aspect B), the manipulator driving device such as the manipulator device 100 maintains the posture of the tip of the manipulator such as the first arm 101 in the input operation region. The external force is assisted by controlling the driving of the two or more driving units so that the tip part moves.
According to this, as described in the present embodiment, for example, it is possible to perform an assist operation so as to translate the tip of the manipulator along the input movement direction.

(Aspect D)
In any one of (Aspect A) to (Aspect C), the first arm 101, the second arm 102, and the second arm connected to each of the two or more joint parts such as the first joint part 130 and the second joint part 140. A storage unit such as a storage unit 215 that stores, for each of the two or more joints, a support unit such as the two arms 102 and the support column unit 111b or the angle to be supported is provided, and an input unit 113 (input device 114) The input means can specify a trajectory to be drawn when the tip of the manipulator such as the tip of the first arm 101 moves, using the angle stored in the previous storage means.
According to this, as described in the present embodiment, it is possible to perform an assist operation so that the tip of the manipulator is moved along the designated trajectory.

(Aspect E)
In any one of (Aspect A) to (Aspect C), the first arm 101, the second arm 102, and the second arm connected to each of the two or more joint parts such as the first joint part 130 and the second joint part 140. A storage unit such as a storage unit 215 that stores, for each of the two or more joints, a support unit such as the two arms 102 and the support column unit 111b or the angle to be supported is provided, and an input unit 113 (input device 114) The input means can specify a plane on which the tip of the manipulator such as the tip of the first arm 101 moves using the angle stored in the pre-storage means.
According to this, as described in the present embodiment, it is possible to perform an assist operation so as to move the manipulator tip along a designated plane.

(Aspect F)
In any one of (Aspect A) to (Aspect C), the first arm 101, the second arm 102, and the second arm connected to each of the two or more joint parts such as the first joint part 130 and the second joint part 140. A storage unit such as a storage unit 215 that stores, for each of the two or more joints, a support unit such as the two arms 102 and the support column unit 111b or the angle to be supported is provided, and an input unit 113 (input device 114) or the like The input means can specify a position where the tip of the manipulator such as the tip of the first arm 101 rotates using the angle stored in the pre-storage means.
According to this, as described in the present embodiment, the assist operation can be performed so as to rotate the manipulator tip portion around the designated position (rotation axis).

(Aspect G)
In any one of (Aspect A) to (Aspect E), the input means specifies an Xtp operation region that limits the moving direction of the tip of the manipulator such as the first joint 601 to only a specified vector direction such as the Xtp direction. It has an operation area designating means such as a button 651.
According to this, as described in the present embodiment, the following effects can be achieved.
The tool point coordinate system 691 or the manipulator absolute coordinate system 692 moves in any of the X, Y, and Z directions, for example, only in the Xtp (lateral direction) direction of the tool point coordinate system 691, and moves in the horizontal direction. Intuitive teaching is possible when desired.

(Aspect H)
In any one of (Aspect A) to (Aspect F), the input means regulates the movement direction of the manipulator tip such as the first joint 601 only in the designated vector direction such as the Ztp direction. It has an operation area specifying means such as an operation area specifying button 652.

According to this, as described in the present embodiment, the following effects can be achieved.
Only the movement of the tool point coordinate system 691 or the manipulator absolute coordinate system 692 in any of the X, Y, and Z directions, for example, the Ztp (height) direction of the tool point coordinate system 691, is restricted (fixed) to the height. Intuitive teaching is possible when it is desired to hold and move.

(Aspect I)
In (Aspect G) or (Aspect H), the input means includes an Xtp action area designation button 651, a Ztp action area designation button 652, an Xab action area designation button 654, a Yab action area designation button 655, and a Zab action area designation button. It has LED which displays the vector direction designated by the said operation area | region designation | designated means, such as 653, by lighting.

According to this, as described in the present embodiment, the following effects can be achieved.
It can convey to the user (operator) the vector direction specified by the motion area specification means, that is, in which direction it can be moved, or in which direction movement is restricted, so that it helps the user's intuitive operation. Can do.

(Aspect J)
In any one of (Aspect A) to (Aspect I), the input means includes an Xtp action area designation button 651, a Ztp action area designation button 652, an Xab action area designation button 654, and a Yab action area designation button for designating an action area. 655 and a Zab operation area designation button 653 and the like, and a controller capable of finely adjusting the operation area designated by the operation area designation means is also provided.

According to this, as described in the present embodiment, the following effects can be achieved.
If it is desired to move the tool point coordinate system 691 or the manipulator absolute coordinate system 692 in a direction that is not any of the X, Y, and Z directions, for example, in the direction of 30 deg relative to the Xab direction of the manipulator absolute coordinate system 692, it can be adjusted by the controller.

(Aspect K)
In any one of (Aspect A) to (Aspect J), the moving speed of the manipulator tip such as the tip of the first arm 101 is controlled so as not to exceed a predetermined speed.
According to this, as described in the present embodiment, the following effects can be achieved.
In the configuration in which the drive of two or more drive units is controlled so as to assist the external force applied to the manipulator based only on the detection result of the external force detection unit such as the external force detection unit 112, it is far from the moving speed intended by the user. The manipulator tip may move at the moving speed.
On the other hand, by providing a position speed command generation unit 411 in the control block of the assist control and controlling so as not to exceed a predetermined speed, the first support unit is moved at a moving speed far from the moving speed intended by the user. It can suppress that the front-end | tip part moves.

(Aspect L)
Two or more joint parts such as the first joint part 130 and the second joint part 140 are connected in series via each joint part, and are supported such as a gripping mechanism attached to a manipulator tip such as the tip of the first arm 101. A plurality of support parts such as a first arm 101, a second arm 102, and a column part 111b, and a first drive part 160 and a second drive part 170 that rotate the two or more joint parts, respectively. Such as the external force detection unit 112 that detects a vector of external force applied from the outside to at least one of the above-described drive unit, the two or more joint units, and the plurality of support units, and the tip of the first arm 101, etc. A manipulator such as a manipulator device 600 including an external force detection unit and a control unit such as the control unit 210 that controls driving of the two or more drive units based on the detection result of the external force detection unit. In Regulator driving device, as the control unit, characterized in that it comprises a control unit of the manipulator according to any one of claims 1 to 11.

  According to this, as described in the present embodiment, it is possible to provide a manipulator driving device that can achieve the same effect as the manipulator control device of any one of (Aspect A) to (Aspect K).

(Aspect M)
In (Aspect L), the input means for inputting the operation region of the tip of the manipulator such as the first joint 601 is installed in the manipulator driving device body, and the Xtp operation region specification button 651 for specifying the operation region, the Ztp operation region specification It is characterized by having operation area specifying means such as a button 652, an Xab operation area specifying button 654, a Yab operation area specifying button 655, and a Zab operation area specifying button 653.

According to this, as described in the present embodiment, the following effects can be achieved.
In the configuration using the input device 114 or the like held by the user as the input means, when the user applies an external force as an operation force to the manipulator, the following needs arise, which is insufficient as an intuitive and simple teaching method. It may become. It is necessary for the user to work while holding one hand or both hands away from the input device 114 or the like, or to operate while grasping and checking the manipulator coordinate system regardless of the manipulator position and posture.
On the other hand, in the configuration having the operation area designating means that is installed in the manipulator driving device main body and designates the operation area as the input means, the operation area designating means can be arranged in the vicinity where the user applies the operation force. It is possible to suppress the occurrence of unnecessary necessity.
Therefore, it is possible to switch the direction of guidance without leaving the main body of the manipulator drive device, and manipulator drive that enables direct teaching intuitively and easily even if it is not a specialized worker who has been trained or experienced. Equipment can be provided.

(Aspect N)
In (Aspect M), the operation region specifying means such as the Xtp operation region specifying button 651 and the Ztp operation region specifying button 652 provided in the input means are arranged in the vicinity of the manipulator tip such as the first joint 601. It is characterized by that.
According to this, as described in the present embodiment, it becomes possible to input (set) an operation area in the vicinity of a tool point that is one of the user's operation positions, so that more intuitive teaching can be performed.

(Aspect O)
In (Aspect M), the operation area specifying means such as the Xab operation area specifying button 654, the Yab operation area specifying button 655, and the Zab operation area specifying button 653 included in the input means such as the input unit 113 (input device 114). Is arranged in a non-moving part such as the non-moving part 605.
According to this, as described in the present embodiment, the following effects can be achieved.
Since the operation region can be defined by the absolute coordinates of the manipulator driving device such as the manipulator absolute coordinate system 692 at the root of the manipulator driving device such as the manipulator device 600, more practical teaching can be performed.

(Aspect P)
(Aspect L) In any one of Aspects O, the support parts such as the first arm 101, the second arm 102, and the third arm 121 excluding the non-moving part such as the support pillar part 111b among the plurality of support parts. Are provided with contact detection units such as a first contact detection unit 133, a second contact detection unit 143, and a third contact detection unit 153 that detect that an object such as a user's hand has come into contact with the object. The second joint part 140, the third joint part 150, etc. between the contact support part such as the second arm 102 provided with a contact detection part such as the second contact detection part 143 that has detected contact with the stationary part. When there are two joint parts, the control device of the manipulator such as the control part 210a drives two drive parts such as the second drive part 170 and the third drive part 180 that rotate the two joint parts, respectively. Control Characterized in that it has a to first operation mode, such as operation mode 1 to assist the external force, such as a first operating force F71.

According to this, as described in the present embodiment, the following effects can be achieved.
If you want to rotate only the two joints near the contact support part between the contact support part and the non-moving part, you can set the first operation mode and apply external force to the contact support part. it can. By applying external force in this way, the intended joint part does not rotate or the unintended joint part rotates, and the rotation angle of each joint part deviates from the intended one. Can be suppressed.
Therefore, it is possible to reduce the time required to correct the rotation angle of each joint, and to shorten the time required when teaching the manipulator driving device.

(Aspect Q)
(Aspect L) In any one of Aspects O, the support parts such as the first arm 101, the second arm 102, and the third arm 121 excluding the non-moving part such as the support pillar part 111b among the plurality of support parts. Are provided with contact detection units such as a first contact detection unit 133, a second contact detection unit 143, and a third contact detection unit 153 that detect that an object such as a user's hand has come into contact with the object. The second joint part 140, the third joint part 150, etc. between the contact support part such as the second arm 102 provided with a contact detection part such as the second contact detection part 143 that has detected contact with the stationary part. When there are two or more joint parts, the control device of the manipulator such as the control part 210a rotates the joint part such as the second joint part 140 to which the contact support part is coupled, of the two or more joint parts. Second wheel to move Characterized in that it has a second operating mode, such as operation mode 2 to assist the external force, such as the second operating force F72 controls the drive of the driving unit, such as section 170.

According to this, as described in the present embodiment, the following effects can be achieved.
When it is desired to rotate only one joint portion on the non-moving portion side to which the contact support portion is connected, the second operation mode can be set and an external force can be applied to the contact support portion. By applying external force in this way, the intended joint part does not rotate or the unintended joint part rotates, and the rotation angle of each joint part deviates from the intended one. Can be suppressed.
Therefore, it is possible to reduce the time required to correct the rotation angle of each joint, and to shorten the time required when teaching the manipulator driving device.

(Aspect R)
(Aspect L) In any one of Aspects O, the support parts such as the first arm 101, the second arm 102, and the third arm 121 excluding the non-moving part such as the support pillar part 111b among the plurality of support parts. Are provided with contact detection units such as a first contact detection unit 133, a second contact detection unit 143, and a third contact detection unit 153 that detect that an object such as a user's hand has come into contact with the object. The first joint portion 130, the second joint portion 140, between the contact support portion such as the first arm 101 provided with a contact detection portion such as the first contact detection portion 133 that detects that the contact is made and the stationary portion, When there are three or more joint parts such as the third joint part 150, the control device for the manipulator such as the control part 210a may be configured such that the first joint part closer to the contact support part among the three or more joint parts. 130, second joint 140, and the third drive unit such as the third joint unit 150 are rotated to control the driving of the three drive units such as the first drive unit 160, the second drive unit 170, and the third drive unit 180, respectively. A third operation mode such as an operation mode 3 for assisting the external force such as a three operation force F73 is provided.

According to this, as described in the present embodiment, the following effects can be achieved.
If you want to rotate only the three joints close to the contact support part between the contact support part and the non-moving part, you can set the third operation mode and apply external force to the contact support part. it can. By applying external force in this way, the intended joint part does not rotate or the unintended joint part rotates, and the rotation angle of each joint part deviates from the intended one. Can be suppressed.
Therefore, it is possible to reduce the time required to correct the rotation angle of each joint, and to shorten the time required when teaching the manipulator driving device.

(Aspect S)
(Aspect L) In any one of Aspects O, the support parts such as the first arm 101, the second arm 102, and the third arm 121 excluding the non-moving part such as the support pillar part 111b among the plurality of support parts. Are provided with contact detection units such as a first contact detection unit 133, a second contact detection unit 143, and a third contact detection unit 153 that detect that an object such as a user's hand has come into contact with the object. The first joint portion 130, the second joint portion 140, between the contact support portion such as the first arm 101 provided with a contact detection portion such as the first contact detection portion 133 that detects that the contact is made and the stationary portion, And the first drive center 103 of the three joint parts such as the first joint part 130 and the second joint part 140 on the side close to the contact support part. Second drive center 104, and second The manipulator control device such as the control unit 210a has parallel rotation axes such as the drive center 122, and the control unit 210a includes a first joint unit 130 and a second joint unit closer to the contact support unit among the three or more joint units. Operation for assisting the external force such as the fourth operating force F74 by controlling the driving of the two driving units such as the first driving unit 160 and the second driving unit 170 that respectively rotate the two joint units such as the joint unit 140. It has a fourth operation mode such as mode 4.

According to this, as described in the present embodiment, the following effects can be achieved.
If you want to rotate only two joints between the contact support part and the non-moving part, the rotation axis on the side close to the contact support part is parallel, set to the fourth operation mode, contact support part External force can be applied. By applying external force in this way, the intended joint part does not rotate or the unintended joint part rotates, and the rotation angle of each joint part deviates from the intended one. Can be suppressed.
Therefore, it is possible to reduce the time required to correct the rotation angle of each joint, and to shorten the time required when teaching the manipulator driving device.

(Aspect T)
In a robot system including manipulator driving devices such as the manipulator device 100 and the manipulator device 600, a manipulator control device such as the control unit 210 of any one of (Aspect A) to (Aspect K), or (Aspect L) to (Aspect) A manipulator driving device such as any one of the manipulator devices 600 of S) is provided.
According to this, as described in the present embodiment, the following effects can be achieved.
A robot system capable of producing the same effect as at least one of the manipulator control device according to any one of (Aspect A) to (Aspect K) or the manipulator driving device according to any one of (Aspect L) to (Aspect S). Can provide.

100 Manipulator device (Example 1)
101 First Arm 102 Second Arm 103 First Drive Center 104 Second Drive Center 105 First Reducer 106 First Motor 107 First Motor Encoder 108 Second Reducer 109 Second Motor 110 Second Motor Encoder 111a Foundation 111b Post unit 112 External force detection unit 113 Input unit 114 Input device 121 Third arm 122 Third drive center 123 Third reduction gear 124 Third motor 125 Third motor encoder 130 First joint unit 131 First arm encoder 132 First One external force detection unit 133 First contact detection unit 134 First touch sensor 135 First optical sensor 140 Second joint unit 141 Second arm encoder 142 Second external force detection unit 143 Second contact detection unit 144 Second touch sensor 145 First Two-optical sensor 150 Third joint 151 Third arm encoder The third external force detection unit 153 The third contact detection unit 154 The third touch sensor 155 The third optical sensor 160 The first drive unit 170 The second drive unit 180 The third drive unit 200 The manipulator body 200a The manipulator body (Example 3)
210 Control Unit 210a Control Unit (Example 3)
214 Drive control unit 215 Storage unit 233 First motor driver 234 First drive information detection unit 235 Second motor driver 236 Second drive information detection unit 237 Third motor driver 238 Third drive information detection unit 409 Operating force target generation unit 411 Position / velocity command generation unit 504 Hand coordinate system 505 Operation area 600 Manipulator device (Example 2)
601 1st joint 602 2nd joint 603 3rd joint 604 4th joint 605 Non-moving part 610 End effector 611 1st support part 612 2nd support part 613 3rd support part 620 Installation stand 630 User (operator)
651 Xtp action area designation button 652 Ztp action area designation button 653 Zab action area designation button 654 Xab action area designation button 655 Yab action area designation button 691 Tool point coordinate system 692 Manipulator absolute coordinate system 700 Manipulator device (Example 3)

Patent No. 4896276

Claims (20)

Two or more joint parts, a plurality of support parts that are connected in series via each joint part, and support a support target attached to the tip of the manipulator, and two or more drives that respectively rotate the two or more joint parts And an external force detection unit that detects a vector of an external force applied to at least one of the two or more joint portions and the plurality of support portions from the outside, and the external force detection In the control device of the manipulator that controls the driving of the two or more driving units based on the detection result of the unit,
An input unit that inputs an operation region of the manipulator tip, and when the external force vector is detected by the external force detection unit, a detection result of the external force detection unit and an operation region input from the input unit; On the basis of,
The manipulator control device, wherein the drive of the two or more drive units is controlled so that the tip of the manipulator moves within the input operation region. In the manipulator control device according to claim 1,
The manipulator driving device includes two or more angle detection units that detect, for each of the two or more joint parts, a support part connected to each of the two or more joint parts or an angle formed by the support target.
Based on the detection result of at least one of the two or more angle detectors,
A manipulator control device that assists the external force by controlling driving of the two or more drive units so that the manipulator tip moves within the input operation region. The manipulator control device according to claim 1 or 2,
The external force is assisted by controlling the driving of the two or more drive units so that the manipulator tip moves within the input operation region while maintaining the posture of the manipulator tip. A manipulator control device. In the control device of the manipulator according to any one of claims 1 to 3,
A storage unit that stores, for each of the two or more joints, a support unit connected to each of the two or more joints or an angle formed by the support target;
The manipulator control device characterized in that the input means can designate a trajectory to be drawn when the tip of the manipulator moves using the angle stored in the pre-storage means. In the control device of the manipulator according to any one of claims 1 to 3,
A storage unit that stores, for each of the two or more joints, a support unit connected to each of the two or more joints or an angle formed by the support target;
The manipulator control device characterized in that the input means can designate a plane on which the tip of the manipulator moves using the angle stored in the pre-storage means. In the control device of the manipulator according to any one of claims 1 to 3,
A storage unit that stores, for each of the two or more joints, a support unit connected to each of the two or more joints or an angle formed by the support target;
The manipulator control device characterized in that the input means can designate a position where the tip of the manipulator rotates using the angle stored in the pre-storage means. In the control device of the manipulator according to any one of claims 1 to 5,
The manipulator control device characterized in that the input means includes an operation area specifying means for limiting a moving direction of the tip portion of the manipulator to only a specified vector direction. In the control device of the manipulator according to any one of claims 1 to 6,
The manipulator control device according to claim 1, wherein the input unit includes an operation region designating unit that regulates a movement direction of the manipulator tip only in a designated vector direction. The control device for a manipulator according to claim 7 or 8,
The manipulator control device characterized in that the input means has an LED for displaying the vector direction designated by the operation area designating means by lighting. In the control device of the manipulator according to any one of claims 1 to 9,
The manipulator control device characterized in that the input means includes an operation area specifying means for specifying an operation area, and a controller capable of finely adjusting the operation area specified by the operation area specifying means. In the control device of the manipulator according to any one of claims 1 to 10,
A control device for a manipulator, characterized in that control is performed so that the moving speed of the tip of the manipulator does not exceed a predetermined speed. Two or more joint parts, a plurality of support parts that are connected in series via each joint part, and support a support target attached to the tip of the manipulator, and two or more drives that respectively rotate the two or more joint parts An external force detection unit that detects a vector of an external force applied to at least one of the two or more joint units and the plurality of support units, and the two or more based on a detection result of the external force detection unit A manipulator driving device comprising: a control unit that controls driving of the driving unit;
A manipulator driving device comprising: the manipulator control device according to claim 1 as the control unit. The manipulator drive device according to claim 12,
The manipulator driving device characterized in that the input means for inputting the operation region of the tip portion of the manipulator has an operation region specifying device which is installed in the manipulator driving device body and specifies the operation region. The manipulator drive device according to claim 13,
The manipulator driving device characterized in that the operation area designating means included in the input means is disposed in the vicinity of the manipulator tip. The manipulator drive device according to claim 13,
The manipulator driving device according to claim 1, wherein the operation area designating unit included in the input unit is disposed in a non-moving part. In the manipulator drive device according to any one of claims 12 to 15,
Of the plurality of support parts, the support part excluding the non-moving part is provided with a contact detection part that detects that an object has contacted,
When there are two joint parts between the contact support part provided with a contact detection part that detects that the object has contacted and the stationary part,
The control device of the manipulator is
A manipulator driving device having a first operation mode for assisting the external force by controlling driving of two driving units for rotating the two joints. In the manipulator drive device according to any one of claims 12 to 15,
Of the plurality of support parts, the support part excluding the non-moving part is provided with a contact detection part that detects that an object has contacted,
When there are two or more joint parts between the contact support part provided with a contact detection part that detects that the object has contacted and the stationary part,
The control device of the manipulator is
A second operation mode for assisting the external force by controlling driving of a drive unit that rotates a joint unit to which the contact support unit is coupled among the two or more joint units is provided. Manipulator drive device. In the manipulator drive device according to any one of claims 12 to 15,
Of the plurality of support parts, the support part excluding the non-moving part is provided with a contact detection part that detects that an object has contacted,
When there are three or more joint parts between the contact support part provided with a contact detection part that detects that the object has contacted and the stationary part,
The control device of the manipulator is
A third operation mode for assisting the external force by controlling the driving of the three driving sections that respectively rotate the three joint sections closer to the contact support section among the three or more joint sections. A manipulator driving device characterized by that. In the manipulator drive device according to any one of claims 12 to 15,
Of the plurality of support parts, the support part excluding the non-moving part is provided with a contact detection part that detects that an object has contacted,
There are three or more joint parts between the contact support part provided with a contact detection part that detects that the object is in contact and the stationary part, and rotation of the three joint parts on the side close to the contact support part. The axes of movement are parallel,
The control device of the manipulator is
Of the three or more joint portions, a fourth operation mode for assisting the external force by controlling driving of two drive portions that respectively rotate two joint portions closer to the contact support portion. A manipulator driving device characterized by that. In a robot system equipped with a manipulator driving device,
A robot system comprising at least one of the manipulator control device according to any one of claims 1 to 11 and the manipulator driving device according to any one of claims 12 to 19.

Images