JP6633296B2 - Robot device and direct teaching control device - Google Patents

Description

  Embodiments of the present invention relate to a robot device and a stepping motor control device.

  A stepping motor has an advantage over an AC motor and a DC motor because the rotation angle is proportional to the number of pulse signals, so there is basically no need for a feedback circuit and open-loop control is possible. If the pulse frequency is too high or the pulse frequency is too high, there is a demerit that so-called "step-out" occurs in which control is disturbed due to loss of synchronization. Due to this disadvantage, it is not common to employ a stepping motor as an actuator of a robot device.

  On the other hand, in the case of a robot device, the operator needs to teach the robot device the operations such as the work point at which the robot arm should move, the waypoint, and the details of the hand work at the work point, and the operator actually moves the arm. Teaching that operation is called direct teaching.

  Generally, in direct teaching, there is a situation in which a single operator cannot move the robot due to the weight of the robot arm. Typically, the weight of the robot arm is canceled by the counterweight, and the motor assist function is used to reduce the force by which the operator moves the robot arm by controlling the actuator to move the robot arm in the direction in which the robot arm moves. Various measures have been taken.

  The former measure increases the weight of the robot arm itself. In the latter case, it cannot be said that there is no risk of collision with the worker due to the movement of the driven robot arm.

  An object of the present invention is to utilize a step-out phenomenon of a stepping motor to reduce a burden on an operator in direct teaching and to improve an operator's safety.

The “direct teaching control device for a robot device having an arm having a joint using a stepping motor as an actuator” according to the present embodiment includes a joint torque of the joint, a load torque due to its own weight applied to the joint based on the mass of the center of gravity of the arm. Is calculated by the torque calculator in the opposite direction. An exciting current value required for causing the stepping motor to generate this static torque is calculated by a current value calculating unit. The output unit outputs the exciting current value to the driver of the stepping motor together with the stationary command. The control unit controls the torque calculation unit, the current value calculation unit, and the output unit. The arm mechanism includes a torsional rotary joint about a first axis perpendicular to the base, a bending rotary joint about a second axis orthogonal to the first axis, and a linear joint along a third axis orthogonal to the second axis. And a linearly-movable telescopic joint having dynamic elasticity. The control unit repeatedly executes the calculation processing of the static torque, the calculation processing of the excitation current value, and the output processing of the excitation current value and the stationary command over the period of manual operation of the arm mechanism by the operator with respect to the bending rotation joint, The calculation processing of the static torque, the calculation processing of the excitation current value, and the output processing of the excitation current value and the stationary command are not executed for the section and the linearly-movable joint.

FIG. 1 is an external perspective view of a robot arm mechanism of the robot device according to the present embodiment. FIG. 2 is a side view showing the internal structure of the robot arm mechanism of FIG. FIG. 3 is a diagram showing the configuration of the robot arm mechanism of FIG. 1 by using graphic symbols. FIG. 4 is a block diagram illustrating a configuration of the robot device according to the present embodiment. FIG. 5 is a flowchart for explaining an arm holding control procedure at the time of direct teaching by the direct teaching control device of FIG. FIG. 6 is an explanatory supplementary diagram of the procedure of FIG. 5 and shows three types of postures of the arm mechanism. FIG. 7 is an explanatory supplementary diagram of the procedure in FIG. 5, and is a diagram showing a temporal change in the exciting current value for stopping the stepping motor of the joint J2 according to the posture change in FIG.

  Hereinafter, a robot device according to the present embodiment or a direct teaching control device provided in the robot device will be described with reference to the drawings. The direct teaching control device according to the present embodiment allows a worker to move a hand trajectory such as a work position at which a work is performed by a robot hand or a passing position from a work position to a next work position. This is to support so-called direct teaching in which the robot is directly held by hand and moved to be stored in the robot device. The robot device includes a robot arm mechanism having a joint part using a stepping motor as an actuator. A vertical multi-joint arm mechanism will be described as an example of the robot apparatus. In particular, one of the plurality of joints will be described as a vertical multi-joint arm mechanism having a linearly-movable telescopic joint. In the following description, components having substantially the same functions and configurations are denoted by the same reference numerals, and repeated description will be made only when necessary.

  The subject of the present embodiment is to actively use the step-out phenomenon of the stepping motor to reduce the burden on the operator in direct teaching and to improve the safety of the operator. That is, a load torque is applied to the joint by the weight of the arm. The load torque is calculated based on the joint variables, the mass of the center of gravity of the arm, and the like. Equivalently, a reverse stationary torque is generated by the stepping motor. As a result, the arm stops at that position while being balanced with its own weight. If the operator moves the arm in direct teaching in that state, the stepping motor loses synchronism due to an overload exceeding the static torque. Therefore, the operator can move the arm lightly. While the arm is moving, the calculation of the static torque is repeated for each position and posture, and at that position, the arm maintains a state of being balanced with its own weight. In this way, the stepping motor is adopted as the actuator of the joint of the robot device, and the drive control is performed so that the stepping motor generates a static torque, thereby reducing the burden on the operator in direct teaching and improving the operator's safety. realizable.

  FIG. 1 is an external perspective view of the robot device according to the present embodiment. The robot arm mechanism constituting the robot device has a substantially cylindrical base 10, an arm 2 connected to the base 10, and a wrist 4 attached to a tip of the arm 2. The wrist 4 is provided with an adapter (not shown). For example, the adapter is provided on a rotating portion of a sixth rotating shaft RA6 described later. A robot hand according to the application is attached to the adapter provided on the wrist 4.

  The robot arm mechanism has a plurality of, here six, joints J1, J2, J3, J4, J5, J6. The plurality of joints J1, J2, J3, J4, J5, J6 are arranged in order from the base 10. Generally, the first, second, and third joints J1, J2, and J3 are called three root axes, and the fourth, fifth, and sixth joints J4, J5, and J6 change the posture of the robot hand. Called wrist three axis. The wrist 4 has fourth, fifth, and sixth joints J4, J5, and J6. At least one of the joints J1, J2, J3 forming the three base axes is a linearly-movable telescopic joint. Here, the third joint J3 is configured as a linearly-movable telescopic joint, particularly a joint having a relatively long telescopic distance. The arm part 2 represents a telescopic part of a linearly-movable telescopic joint part J3 (third joint part J3).

  The first joint J1 is a torsion joint centered on the first rotation axis RA1 supported, for example, perpendicularly to the base surface. The second joint portion J2 is a bending joint centered on the second rotation axis RA2 arranged perpendicular to the first rotation axis RA1. The third joint portion J3 is a joint in which the arm portion 2 expands and contracts linearly about a third axis (moving axis) RA3 arranged perpendicular to the second rotation axis RA2.

  The fourth joint J4 is a torsional joint centering on the fourth rotation axis RA4. The fourth rotation axis RA4 substantially coincides with the third movement axis RA3 when a later-described seventh joint J7 is not rotating, that is, when the entire arm 2 is in a linear shape. The fifth joint J5 is a bending joint centered on a fifth rotation axis RA5 orthogonal to the fourth rotation axis RA4. The sixth joint J6 is a bending joint centered on the sixth rotation axis RA6, which is arranged orthogonal to the fourth rotation axis RA4 and perpendicular to the fifth rotation axis RA5.

  The arm support (first support) 11a forming the base 10 has a cylindrical hollow structure formed around the first rotation axis RA1 of the first joint J1. The first joint J1 is attached to a fixed base (not shown). When the first joint J1 rotates, the arm 2 turns left and right with the rotation of the axis of the first support 11a. Note that the first support 11a may be fixed to the ground surface. In this case, the arm 2 is provided in a structure in which the arm 2 turns independently of the first support 11a. A second support 11b is connected to an upper portion of the first support 11a.

  The second support 11b has a hollow structure that is continuous with the first support 11a. One end of the second support portion 11b is attached to the rotating portion of the first joint J1. The other end of the second support portion 11b is opened, and the third support portion 11c is rotatably fitted on the second rotation axis RA2 of the second joint J2. The third support portion 11c has a hollow structure composed of a scale-like exterior communicating with the first support portion 11a and the second support portion. The rear portion of the third support portion 11c is accommodated in the second support portion 11b with the bending rotation of the second joint portion J2, and is sent out. The rear part of the arm part 2 which constitutes the direct-acting telescopic joint part J3 (third joint part J3) of the robot arm mechanism is housed in the hollow structure where the first support part 11a and the second support part 11b are continuous due to the contraction. You.

  The third support portion 11c is rotatably fitted about the second rotation axis RA2 at the lower rear end of the lower end of the second support portion 11b. Thereby, a second joint J2 as a bending joint around the second rotation axis RA2 is formed. When the second joint J2 rotates, the arm 2 vertically rotates around the second rotation axis RA2, that is, performs an up-and-down operation.

  The fourth joint J4 is a torsion joint having a fourth axis of rotation RA4 that is typically in contact with the arm center axis along the direction of extension and contraction of the arm 2, that is, the third movement axis RA3 of the third joint J3. When the fourth joint J4 rotates, the wrist 4 and the robot hand attached to the wrist 4 rotate around the fourth rotation axis RA4. The fifth joint J5 is a bending joint having a fifth rotation axis RA5 orthogonal to the fourth rotation axis RA4 of the fourth joint J4. When the fifth joint J5 rotates, it rotates vertically (vertically about the fifth rotation axis RA5) together with the robot hand from the fifth joint J5 to the tip. The sixth joint J6 is a bending joint having a sixth rotation axis RA6 perpendicular to the fourth rotation axis RA4 of the fourth joint J4 and perpendicular to the fifth rotation axis RA5 of the fifth joint J5. When the sixth joint J6 rotates, the robot hand turns left and right.

  As described above, the robot hand attached to the adapter of the wrist 4 is moved to an arbitrary position by the first, second, and third joints J1, J2, and J3, and the fourth, fifth, and sixth joints J4, It is arranged in an arbitrary posture by J5 and J6. In particular, the length of the expansion and contraction distance of the arm 2 of the third joint J3 enables the robot hand to reach a wide range of objects from the proximity position of the base 10 to the remote position. The third joint J3 is characterized by a linear extension / contraction operation realized by a linear motion extension / contraction mechanism constituting the third joint J3 and a length of the extension / contraction distance.

  FIG. 2 is a perspective view showing the internal structure of the robot arm mechanism of FIG. The linearly extending / contracting mechanism has an arm section 2 and an ejection section 30. The arm section 2 has a first connecting frame array 21 and a second connecting frame array 22. The first connected frame array 21 includes a plurality of first connected frames 23. The first connecting piece 23 is formed in a substantially flat plate shape. The front and rear first connection pieces 23 are connected to each other at end portions thereof in a bendable manner by pins. The first connecting frame array 21 can be freely bent inward or outward.

  The second connected frame array 22 includes a plurality of second connected frames 24. The second connecting piece 24 is formed as a short groove having a U-shaped cross section. The front and rear second connection pieces 24 are connected to each other in a row at the bottom end portions of each other by pins so as to be freely bent. The second connecting frame array 22 can be bent inward. Since the cross section of the second connecting piece 24 is U-shaped, the second connecting piece row 22 does not bend outward because the side plates of the adjacent second connecting pieces 24 collide with each other. The surfaces of the first and second connecting pieces 23 and 24 facing the second rotation axis RA2 are referred to as inner surfaces, and the surface on the opposite side is referred to as an outer surface. The leading first connected frame 23 in the first connected frame array 21 and the leading second connected frame 24 in the second connected frame array 22 are connected by a connecting frame 27. For example, the connection piece 27 has a shape obtained by combining the second connection piece 24 and the first connection piece 23.

  The injection unit 30 includes a plurality of upper rollers 31 and a plurality of lower rollers 32 supported by a rectangular cylindrical frame 35. For example, the plurality of upper rollers 31 are arranged along the arm central axis at intervals substantially equivalent to the length of the first connection top 23. Similarly, the plurality of lower rollers 32 are arranged along the center axis of the arm at intervals substantially equivalent to the length of the second connecting piece 24. A guide roller 40 and a drive gear 50 are provided behind the injection unit 30 so as to oppose each other with the first connection frame array 21 interposed therebetween. The drive gear 50 is connected to a stepping motor 330 via a speed reducer (not shown). A linear gear is formed on the inner surface of the first connection piece 23 along the connection direction. When the plurality of first connection pieces 23 are linearly aligned, the linear gears are connected linearly to form a long linear gear. The drive gear 50 is engaged with a linear gear. The linear gears connected linearly together with the drive gear 50 constitute a rack and pinion mechanism.

  When the arm 55 is extended and the motor 55 is driven and the drive gear 50 is rotated forward, the first connecting frame array 21 is brought into a posture parallel to the arm central axis by the guide roller 40, and the upper roller 31 and the lower roller 32 Guided between. Along with the movement of the first connecting frame array 21, the second connecting frame array 22 is guided between the upper roller 31 and the lower roller 32 of the injection unit 30 by a guide rail (not shown) arranged behind the injection unit 30. . The first and second connection frame arrays 21 and 22 guided between the upper roller 31 and the lower roller 32 are pressed against each other. As a result, a columnar body composed of the first and second connected frame rows 21 and 22 is formed. The injection unit 30 joins the first and second connecting frame arrays 21 and 22 to form a column, and supports the column vertically and horizontally. The columnar body formed by joining the first and second connection frame arrays 21 and 22 is firmly held by the injection unit 30, so that the joined state of the first and second connection frame arrays 21 and 22 is maintained. When the joined state of the first and second connected frame arrays 21 and 22 is maintained, the bending of the first and second connected frame arrays 21 and 22 is restricted to each other. Thereby, the first and second connecting frame arrays 21 and 22 form a columnar body having a certain rigidity. The columnar body refers to a columnar rod formed by joining the first connecting frame array 21 to the second connecting frame array 22. In the columnar body, the second connecting piece 24 and the first connecting piece 23 are formed into a cylindrical body having various cross-sectional shapes as a whole. The cylindrical body is defined as a shape in which the top, bottom, left and right sides are surrounded by a top plate, a bottom plate, and both side plates, and the front end and the rear end are open. The columnar body formed by joining the first and second connecting frame arrays 21 and 22 has the connecting frame 27 as a starting end, and linearly extends outward from the opening of the third support portion 11c along the third movement axis RA3. Will be sent out.

  When the arm 55 is retracted when the motor 55 is driven and the drive gear 50 is rotated in the reverse direction, the first connecting frame array 21 engaged with the drive gear 50 is pulled back into the first support 11a. The columnar body is pulled back into the third support 11c with the movement of the first connected frame row. The pulled back columnar body is separated behind the injection unit 30. For example, the first connecting frame array 21 forming the columnar body is sandwiched between the guide roller 40 and the drive gear 50, and the second connecting frame array 22 forming the columnar body is pulled downward by gravity. The frame row 22 and the first connected frame row 21 are separated from each other. The separated first and second connection frame arrays 21 and 22 return to a bendable state, respectively. During storage, the second connecting frame array 22 is bent inward and conveyed from the injection unit 30 to the storing unit inside the first support 11a (the base 10), and the first connecting frame array 21 is also connected to the second connecting frame. It is bent and transported in the same direction (inside) as the row 22. The first linked frame row 21 is stored in a state substantially parallel to the second linked frame row 22.

  FIG. 3 is a diagram showing the robot arm mechanism of FIG. In the robot arm mechanism, three positional degrees of freedom are realized by the first joint part J1, the second joint part J2, and the third joint part J3 forming the three base axes. In addition, the four joints J4, the fifth joint J5, and the sixth joint J6, which form the three axes of the wrist, realize three degrees of freedom in posture.

  The robot coordinate system Σb is a coordinate system having an origin at an arbitrary position on the first rotation axis RA1 of the first joint J1. In the robot coordinate system Σb, three orthogonal axes (Xb, Yb, Zb) are defined. The Zb axis is an axis parallel to the first rotation axis RA1. The Xb axis and the Yb axis are orthogonal to each other and orthogonal to the Zb axis. The hand coordinate system Δh is a coordinate system whose origin is an arbitrary position (hand reference point) of the robot hand 5 attached to the wrist 4. For example, when the robot hand 5 is a two-finger hand, the position of the hand reference point (hereinafter, simply referred to as the hand) is defined as the center position between the two fingers. In the hand coordinate system Σh, three orthogonal axes (Xh, Yh, Zh) are defined. The Zh axis is an axis parallel to the sixth rotation axis RA6. The Xh axis and the Yh axis are axes orthogonal to each other and orthogonal to the Zh axis. For example, the Xh axis is an axis parallel to the front-rear direction of the robot hand 5. The hand posture is defined as a rotation angle (rotation angle (yaw angle) α around the Xh axis, a rotation angle (pitch angle) β around the Yh axis) β, and a Zh axis It is given as the surrounding rotation angle (roll angle) γ.

  The first joint portion J1 is disposed between the first support portion 11a and the second support portion 11b, and is configured as a torsional joint about the rotation axis RA1. The rotation axis RA1 is arranged perpendicularly to the reference plane BP of the base on which the fixing part of the first joint J1 is installed.

  The second joint J2 is configured as a bending joint about the rotation axis RA2. The rotation axis RA2 of the second joint J2 is provided in parallel with the Xb axis on the spatial coordinate system. The rotation axis RA2 of the second joint J2 is provided in a direction perpendicular to the rotation axis RA1 of the first joint J1. Further, the second joint J2 is offset with respect to the first joint J1 in two directions: a direction of the first rotation axis RA1 (Zb axis direction) and a Yb axis direction perpendicular to the first rotation axis RA1. The second support 11b is attached to the first support 11a such that the second joint J2 is offset with respect to the first joint J1 in the two directions. The virtual arm rod portion (link portion) that connects the second joint J2 to the first joint J1 has a crank shape in which two hook-shaped bodies whose tips are bent at right angles are combined. This virtual arm rod portion is constituted by first and second supports 11a and 11b having a hollow structure.

  The third joint J3 is configured as a linearly-movable telescopic joint about the movement axis RA3. The movement axis RA3 of the third joint J3 is provided in a direction perpendicular to the rotation axis RA2 of the second joint J2. When the rotation angle of the second joint J2 is zero degrees, that is, the undulation angle of the arm 2 is zero degrees and the arm 2 is in the horizontal reference posture, the movement axis RA3 of the third joint J3 is equal to the second joint. It is provided in a direction perpendicular to the rotation axis RA1 of the first joint J1 together with the rotation axis RA2 of the section J2. On the spatial coordinate system, the movement axis RA3 of the third joint J3 is provided parallel to the Yb axis perpendicular to the Xb axis and the Zb axis. Further, the third joint J3 is offset with respect to the second joint J2 in two directions, that is, the direction of the rotation axis RA2 (Yb axis direction) and the direction of the Zb axis orthogonal to the movement axis RA3. The third support 11c is attached to the second support 11b such that the third joint J3 is offset in the two directions with respect to the second joint J2. The virtual arm rod portion (link portion) connecting the third joint J3 to the second joint J2 has a hook-like body whose tip is bent vertically. This virtual arm rod portion is constituted by the second and third supports 11b and 11c.

The fourth joint J4 is configured as a torsional joint about the rotation axis RA4. The rotation axis RA4 of the fourth joint J4 is arranged to substantially coincide with the movement axis RA3 of the third joint J3.
The fifth joint J5 is configured as a bending joint around the rotation axis RA5. The rotation axis RA5 of the fifth joint J5 is disposed so as to be substantially orthogonal to the movement axis RA3 of the third joint J3 and the rotation axis RA4 of the fourth joint J4.
The sixth joint J6 is configured as a torsional joint about the rotation axis RA6. The rotation axis RA6 of the sixth joint J6 is disposed substantially orthogonal to the rotation axis RA4 of the fourth joint J4 and the rotation axis RA5 of the fifth joint J5. The sixth joint J6 is provided for turning the robot hand 5 as a hand effector right and left. The sixth joint J6 may be configured as a bending joint whose rotation axis RA6 is substantially perpendicular to the rotation axis RA4 of the fourth joint J4 and the rotation axis RA5 of the fifth joint J5.

  In this manner, one of the three joint bases of the plurality of joints J1-J6 is replaced with a linearly-movable telescopic joint, and the second joint J2 is offset in two directions with respect to the first joint J1. By causing the third joint J3 to be offset in two directions with respect to the second joint J2, the robot arm mechanism of the robot apparatus according to the present embodiment structurally eliminates the singularity posture.

  FIG. 4 is a block diagram illustrating a configuration of the robot device according to the present embodiment. Stepping motors 310, 320, 330, 340, 350, and 360 are provided as actuators at the joints J1, J2, J3, J4, J5, and J6 of the robot arm mechanism of the robot apparatus according to the present embodiment, respectively. . Here, the stepping motor is a five-phase stepping motor. Driver units 210, 220, 230, 240, 250, 260 are electrically connected to the stepping motors 310, 320, 330, 340, 350, 360. Typically, the driver units 210, 220, 230, 240, 250, 260 are respectively provided in parallel with the stepping motors to be controlled. These driver units 210, 220, 230, 240, 250, and 260 have the same configuration and perform the same operation on the stepping motor to be controlled in accordance with a control signal from the direct teaching control device 100. Here, only the driver unit 210 will be described, and description of the other driver units 220, 230, 240, 250, and 260 will be omitted.

  The driver unit 210 controls driving and stopping of the stepping motor 310. The driver unit 210 includes a control unit 211, a power supply circuit 212, a pulse signal generation unit 213, a rotary encoder 215, and a counter 217. The control unit 211 controls the driver unit 210 in accordance with a command value input from the direct teaching control device 100.

  As is well known, the stepping motor 310 has a plurality of stator coils arranged around a rotor to which a drive shaft is connected. The stator coil is connected to a power supply circuit via a switching element. When these switching elements are sequentially turned on by a pulse signal, the rotor rotates sequentially at a predetermined step angle. The rotation speed can be changed by changing the frequency (pulse frequency) of the pulse signal. The stepping motor 310 can be stopped by continuing the ON state of the specific switching element and continuing the energized state of the specific stator coil. The static torque at this time can be changed by changing the exciting current supplied from the power supply circuit to the stator coil. The static torque is a torque balanced with the load torque due to the weight of the arm, and is clearly distinguished from the so-called maximum excitation static torque.

  A command code representing the exciting current value of the stepping motor 310 is input from the direct teaching control device 100 to the control unit 211. The control unit 211 outputs a control signal corresponding to the command code to the power supply circuit 212. The power supply circuit 212 is a variable current AC / DC conversion power supply circuit, and generates a current having an exciting current value specified by a command code. The generated exciting current is supplied to the stator coil of the stepping motor 310.

  In addition, a stop command signal for stopping the stepping motor 310 at the current position is input from the direct teaching control device 100 to the control unit 211 in the driver unit 210. The stepping motor 310 stops at that position by continuously supplying current to the stator coil of the phase corresponding to the current position. For this purpose, for example, a code indicating the current joint angle θ1 (t) (joint variable) of the joint J1 is given as the stationary command signal. Similarly, the current joint angles θ2 (t), θ4 (t), θ5 (t), θ6 (t) are respectively provided to the driver units 220, 240, 250, 260 corresponding to the joints J2, J4, J5, J6. Is input from the direct teaching control device 100 to the driver unit 230 corresponding to the joint J3. The stationary instruction signal including the code indicating the current extension distance (linear motion displacement) L3 (t) is input from the direct teaching control device 100. Is done. In the joints J1, J2, J4, J5, and J6, the joint angle indicates a positive or negative rotation angle from a reference position, and the joint expansion and contraction distance indicates a distance from the most contracted state in the joint J3. . The joint angle and the extension distance are collectively referred to as joint variables.

  The pulse signal generation unit 213 determines the number of pulses by dividing the difference from the joint angle after a predetermined control cycle Δt (for example, 10 ms) specified by the control unit 211 from the current joint variable by the step angle. , The control period Δt is divided by the number of pulses, and the reciprocal thereof is used to determine the pulse frequency. When stationary, the control unit 211 gives the current joint variables as the joint angles after a predetermined control cycle Δt (for example, 10 ms). As a result, the stepping pulse is output only to the phase corresponding to the current position of the stepping motor 310 among the plurality of phases (coils) of the stepping motor 310. Thus, the stepping motor 310 can stay at the current position. At this time, a stationary torque corresponding to the exciting current value is generated in the stepping motor 310.

  The rotary encoder 215 is connected to the drive shaft of the stepping motor 310, and outputs a pulse signal (encoder pulse) for each fixed rotation angle. The counter 217 calculates the count by adding or subtracting the number of encoder pulses output from the rotary encoder 215 according to the rotation direction. The counted number is reset at the reference position (origin) of the drive shaft of the stepping motor 310. The counter 217 calculates the joint angle θ1 (t) (joint variable) of the joint J1 based on the number of resets and the count.

  The direct teaching control device 100 includes a system control unit 101, an operation unit interface 102, a position / posture storage unit 103, a dynamics calculation unit 104, a stationary excitation current determination unit 105, an output unit 107, a driver unit interface 106. In the direct teaching control device 100, data on the current joint variables of each of the joints J1 to J6 calculated by the counter 217 is transmitted from the driver unit 210 via the driver unit interface 106 at a predetermined control cycle (for example, every 10 ms). ).

  The system control unit 101 has a CPU (Central Processing Unit), a semiconductor memory, and the like, and controls the direct teaching control device 100 as a whole. Each unit is connected to the system control unit 101 via a control / data bus 109.

  The operation unit 50 is connected to the direct teaching control device 100 via an operation unit interface 102. The operation unit 50 functions as an interface for an operator to register a work position, a via position, a hand work content at each work position, and the like in direct teaching. For example, the operation unit 50 includes a switch for switching the control mode of the robot apparatus from the normal mode to the direct teaching mode. The operation unit 50 includes a registration switch for the operator to register the hand trajectory. The system control unit 101 sequentially stores a set of joint variables of the joints J1 to J6 when the registration switch is pressed, in a position / posture storage unit 103 described later together with the order in which the registration switches are pressed. The input device constituting the operation unit 50 can be replaced with another device, for example, a mouse, a keyboard, a trackball, a touch panel, or the like.

  The position / posture storage unit 103 stores operation sequence data taught by an operator by direct teaching. In the operation sequence data, the starting point and the ending point of the hand reference point and the intermediate point between the starting point and the ending point are described in the robot coordinate system. These points are associated with command values such as the values of the joint variables of each of the joints J1 to J6, the movement time, and the work contents of the robot hand. The movement time and the work content of the robot hand may be registered in the direct teaching period or may be registered in other periods according to the operation of the operator via the operation unit 50.

  The dynamics calculation unit 104 stores the dynamics models respectively corresponding to the six joints J1 to J6 in its ROM. The dynamics model is a calculation formula for calculating the torque applied to each of the joints J1 to J6 based on the joint variables. The dynamics model is calculated in advance for each joint J1-J6 based on the structural characteristics of the robot arm mechanism, that is, the center of gravity position, link mass, link length, etc. of each link connecting the joints J1-J6. . For example, a load torque is generated at the joint J1 by its own weight according to the structural features of the robot arm mechanism that precede the joint J1. For example, by applying the current joint variables of each of the joints J1 to J6 to the dynamics model for the joint J1, the load torque due to the arm's own weight applied to the joint J1 is calculated. Further, a reverse torque equivalent to the load torque due to the own weight is calculated. By generating this torque at the joint J1, the arm 2 at the joint J1 stands still in a state of being balanced with the weight. Hereinafter, this torque is referred to as static torque. The other joints J2-J6 similarly apply the joint variables of all the joints J1-J6 to the corresponding dynamics models, respectively, so that the joints J2-J6 stand still in proportion to the own weight of the arm 2. The static torques T2 (t) to T6 (t) required for J5 are calculated.

  The stationary excitation current determination unit 105 determines the excitation current value Is1 (t) of the excitation current supplied to the stepping motor 310. The stationary exciting current determination unit 105 holds data of a correspondence table in which the torque of the stepping motor 310 is associated with the exciting current value. The stationary excitation current determination unit 105 determines a stationary excitation current value Is1 (t) corresponding to the static torque T1 (t) of the joint J1 calculated by the dynamics calculation unit 104 with reference to the correspondence table. The stationary excitation current determining unit 105 calculates the stationary excitation current values Is2 (t) to Is6 (t) of the joints J2-J6 in the same manner.

  Here, the stationary exciting current determining unit 105 determines the stationary exciting current value Is1 (t) corresponding to the static torque T1 (t) of the joint J1 calculated by the dynamics calculating unit 104. However, the calculation of the static torque by the dynamics calculation unit 104 includes error factors such as frictional force inside the robot arm mechanism. Therefore, in order to generate a static torque slightly larger than the static torque T1 (t) calculated by the dynamics calculation unit 104 at the joint J1, the excitation current determination unit 105 for static state calculates, for example, the calculated static torque. A stationary exciting current value corresponding to 1.1 times may be determined.

  The output unit 107 outputs command values corresponding to the joints J1 to J6 determined by the direct teaching control device 100 to the driver unit 210 under the control of the system control unit 101. Specifically, the output unit 107 outputs a command code representing the stationary excitation current value Is1 (t) determined by the stationary excitation current determination unit 105 to the current joint angle θ1 ( Output to the driver unit 210 together with a stationary command signal including a code representing t). Similarly, under the control of the system control unit 101, the output unit 107 outputs command values (excitation current values for static and current joint variables) for each of the joints J2 to J6 to the driver units 220 to 260.

  Hereinafter, an arm holding control procedure at the time of direct teaching by the direct teaching control device 100 of the robot apparatus according to the present embodiment will be described with reference to FIG. 5, and a supplementary explanation will be given with reference to FIGS. 6 and 7. FIG. 5 is a flowchart for explaining an arm holding control procedure at the time of direct teaching by the direct teaching control device 100 of FIG. FIG. 6 is an explanatory supplementary diagram of the procedure of FIG. 5, and is a diagram showing three types of postures of the robot arm mechanism. FIG. 7 is a supplementary diagram for explaining the procedure of FIG. 5, and shows a temporal change in the value of the exciting current for stationary supplied to the stepping motor of the joint J2 according to the posture change in FIG.

(Step S1) Switch to the direct teaching mode
The direct teaching program is started by the system control unit 101 according to the ON operation of the direct teaching by the changeover switch of the operation unit 50.

(Step S2) Input of joint variables of joints
The direct teaching program is started, and first, the driver unit 210 sends the direct teaching control device 100 the current joint variables θ1 (t), θ2 (t), L3 (t), L4 (t) corresponding to the joints J1-J6, respectively. ), Θ5 (t) and θ6 (t) are input.

(Step S3) Calculation process of static torque of joints J1-J6
Based on the current joint variables θ1 (t) to θ6 (t) of each of the joints J1 to J6, the dynamics calculation unit 104 uses a dynamics model to reverse the load torque equivalent to the load due to the weight of the joints J1 to J6. The orientation static torques T1 (t) to T6 (t) are calculated.

(Step S4) Calculation of excitation current value for stationary
By referring to the correspondence table between the torque and the exciting current value by the stationary exciting current determining unit 105, the static torques T1 (t) to T6 (t) calculated by the dynamics calculating unit 104 in step S3 are converted to the stepping motor. Excitation current values Is1 (t) to Is6 (t) for quiescence required for generating 310 to 360 are determined.

(Step S5) Output to driver unit
As a command value from the output unit 107 to the control unit 211 of each of the driver units 210 to 260 of the joints J1 to J6, a code representing the excitation current values Is1 (t) to Is6 (t) determined in step S4 is set to the current value. It is output together with a stationary command signal including codes representing joint variables θ1 (t) to θ6 (t).

(Step S6) Direct teaching end determination processing
Each unit is controlled by the system control unit 101 so that the process from step S2 to step S5 is repeatedly performed at a predetermined cycle during at least a period during which the direct teaching is performed and a hand or an arm is manually operated by an operator. . The direct teaching program is ended by the system control unit 101 in accordance with the direct teaching OFF operation by the changeover switch of the operation unit 50.

  Next, a supplementary explanation of FIG. 5 will be made with reference to FIGS. FIG. 6 is an explanatory supplementary diagram of the procedure of FIG. 5, and is a diagram showing three types of postures of the robot arm mechanism. FIG. 7 is an explanatory supplementary diagram of the procedure of FIG. 5, and is a diagram showing a temporal change of the exciting current value for stopping the stepping motor 320 of the joint J2 according to the posture change of FIG. Here, for convenience of description, it is assumed that the stepping motor 320 has five phases (A phase to E phase), that is, five sets of stator coils of a pair. FIGS. 6A, 6B, and 6C show postures of the robot arm mechanism at times t0, t1, and t2, respectively. Here, it is assumed that direct teaching is started at time t0, and the robot arm mechanism is manually operated at time t1 and time t2.

  At the start of the direct teaching (time t0), the stepping motor 320 of the joint J2 is supplied with the stationary phase A (coil) corresponding to the current position of the joint J2 in the posture of the robot arm mechanism in FIG. The exciting current having the exciting current value It0 is supplied. As a result, a static torque corresponding to the exciting current value It0 is generated in the stepping motor 320, and the joint J2 is stopped at the current position. This static torque is, in the opposite direction, equivalent to the load torque applied to the joint J2 by its own weight in the robot arm mechanism having the posture shown in FIG. 6A. Therefore, the stepping motor 320 is in a state of stepping out as soon as a slight load is applied. Therefore, if the operator applies any force to the robot arm mechanism, the stepping motor 320 loses synchronism.

  Therefore, in the manual operation of the robot arm mechanism by the worker at the time t1, the worker moves the robot arm mechanism from the posture shown in FIG. 6A to the one shown in FIG. It can be moved to the position shown. While the robot arm mechanism is moving, command values (current joint variables and exciting current values) are output from the direct teaching control device 100 to the driver units 210 to 260 of the joints J1 to J6 in each control cycle. Therefore, the operator manually moves the robot arm mechanism from the posture shown in FIG. 6A to the posture shown in FIG. 6B, and then releases the hand from the robot arm mechanism. Is held in the posture (FIG. 6B) at the time when the worker releases his / her hand. This is because when the operator releases his / her hand from the robot arm mechanism, the exciting current having the stationary exciting current value It1 already exists in the B phase (coil) corresponding to the current position of the stepping motor 320 of the joint J2. This is because it is supplied. As a result, a static torque corresponding to the exciting current value It1 is generated in the stepping motor 320, and the joint J2 is stopped at the current position. This static torque is equivalent in the opposite direction to the load torque applied to the joint J2 by its own weight in the robot arm mechanism having the posture shown in FIG. 6B. Therefore, the stepping motor 320 is in a state of stepping out as soon as a slight load is applied. Therefore, even in the manual operation of the robot arm mechanism by the operator at the time t2, the operator does not use a large force and the robot arm mechanism is moved from the posture shown in FIG. Can be moved to the position shown in FIG. The operator manually moves the robot arm mechanism from the posture shown in FIG. 6B to the posture shown in FIG. 6C, and then releases the hand from the robot arm mechanism. Is held in the posture (FIG. 6C) at the time when the person releases his / her hand. At this time, the stepping motor 320 of the joint J2 has the stationary exciting current It1 in the E phase (coil) corresponding to the current position of the joint J2 in the posture of the robot arm mechanism in FIG. 6C. An exciting current is supplied. 6B and 6C, the load parallel to the direction of gravity exerted on the joint J2 by its own weight in the posture of the robot arm mechanism shown in FIG. 6B is applied when the robot arm mechanism shown in FIG. Smaller than. Therefore, the exciting current values It1 and It2 are smaller than the exciting current value It0.

  In the robot arm mechanism of the robot apparatus according to the present embodiment, for example, the base 10 is provided perpendicular to the base surface (ground), and the first joint J1 has a first rotation axis RA1 perpendicular to the base surface. It is a centered torsion joint. Therefore, in direct teaching, it is preferable that the joint J1 be in a so-called free state in which no exciting current is supplied to the stepping motor 310 of the joint J1. Therefore, the system control unit 101 excludes the driver unit 210 corresponding to the joint J1 from the control target of the arm holding control in the direct teaching, and does not execute the output processing of the control signal to the driver unit 210 corresponding to the joint J1. You may. Similarly, in the linearly extending / contracting mechanism constituting the third joint J3, the arm 2 is supported by the injection unit 30. Therefore, in the arm holding control in the direct teaching, the joint J3 may be set to a so-called free state in which the exciting current is not supplied to the stepping motor 330 of the joint J3. Therefore, the system control unit 101 excludes the driver unit 230 corresponding to the joint J3 from the control target of the arm holding control in the direct teaching, and does not execute the output processing of the control signal to the driver unit 230 corresponding to the joint J3. You may.

  According to the arm holding control of the robot device according to the present embodiment described above, the static torque generated in the joints J1 to J6 can be dynamically changed according to the change in the posture of the robot arm mechanism. At this time, a minimum resting torque required to keep standing still at the current position is generated in each of the joints J1 to J6. The static torque has a value substantially equal to or slightly larger than the load applied to the joints J1 to J6 by its own weight in the posture of the robot arm mechanism. Therefore, the stepping motor of each of the joints J1 to J6 is in a state where it loses synchronism when a small load is applied. Therefore, when the robot arm mechanism is manually operated by the operator, the stepping motor is out of step because the load more than the static torque is applied, thereby allowing the operator to perform the manual operation of the robot arm mechanism without using a large force. , Can be performed lightly. Also, at the time when the worker releases his / her hand from the robot arm mechanism, the minimum resting torque necessary to keep the joints J1-J6 stationary at that position has already been applied. In addition, the posture of the robot arm mechanism can be held in a state where the hand is released from the robot arm mechanism.

  In direct teaching, an operator repeatedly performs a manual operation of the robot arm mechanism and a registration operation of the position of the robot arm mechanism in order to teach the operation sequence data to the robot apparatus. The arm holding control enables the operator to manually operate the robot arm mechanism without using a large force during the direct teaching period, at least during the manual operation of the arm by the operator, and the position where the hand is released from the robot arm mechanism Can hold the posture of the robot arm mechanism. Therefore, the robot device according to the present embodiment can reduce the burden on the operator in direct teaching by utilizing the step-out phenomenon of the stepping motor, and can improve the safety of the operator.

  Although several embodiments of the present invention have been described, these embodiments are provided by way of example and are not intended to limit the scope of the invention. These embodiments can be implemented in other various forms, and various omissions, replacements, and changes can be made without departing from the spirit of the invention. These embodiments and their modifications are included in the scope and gist of the invention, and are also included in the invention described in the claims and equivalents thereof.

  Reference numeral 50: operation unit, 100: direct teaching control device, 101: system control unit, 102: operation unit I / F, 103: position / posture storage unit, 104: dynamics calculation unit, 105: stationary excitation current determination unit, 106 ... Driver unit I / F, 107 ... Output unit, 109 ... Control / data bus, 210-260 ... Driver unit, 211 ... Control unit, 212 ... Power supply circuit, 213 ... Pulse signal generation unit, 215 ... Encoder, 217 ... Counter , 310 ... stepping motor

Claims (4)

In a direct teaching control device of a robot device having an arm mechanism having a plurality of joints using a stepping motor as an actuator,
A joint variable of the joint, a torque calculating unit that calculates a stationary torque in the opposite direction equivalent to a load torque due to its own weight on the joint based on a center of gravity mass of the arm mechanism ;
A current value calculation unit that calculates an excitation current value required to generate the static torque in the stepping motor,
An output unit that outputs the exciting current value to a driver of the stepping motor together with a stationary command,
Before Symbol torque calculation unit, the current value calculation unit, and a control unit for controlling the output unit,
The arm mechanism includes a torsional rotation joint around a first axis perpendicular to the base, a bending rotation joint around a second axis orthogonal to the first axis, and a third axis orthogonal to the second axis. A linearly extending and contracting joint having a linearly extending and retractable
The control unit performs a calculation process of the static torque, a calculation process of the excitation current value, and an output process of the excitation current value and the static command over a manual operation period of the arm mechanism by the operator for the bending and rotating joint unit. The process is repeatedly executed, and the calculation process of the static torque, the calculation process of the excitation current value, and the output process of the excitation current value and the static command are not performed with respect to the torsional rotation joint portion and the linear motion expansion / contraction joint portion. Direct teaching control device. A luer over arm mechanism having a plurality of joints for the stepping motor and the actuator,
A direct teaching control unit that controls direct teaching by the operator to the arm,
The direct teaching control unit,
A torque calculating unit that calculates a reverse static torque equivalent to a load torque due to its own weight based on the mass of the center of gravity of the arm based on a joint variable of the joint,
A current value calculation unit that calculates an excitation current value required to generate the static torque in the stepping motor,
An output unit that outputs the exciting current value to a driver of the stepping motor together with a stationary command,
The torque calculation unit, the current value calculation unit, a control unit that controls the output unit,
The arm mechanism includes a torsional rotary joint about a first axis perpendicular to the base, a bending rotary joint about a second axis orthogonal to the first axis, and a third axis orthogonal to the second axis. Having a linearly-movable telescopic joint having a linear-movable elasticity along,
The control unit performs a calculation process of the static torque, a calculation process of the excitation current value, and an output process of the excitation current value and the static command over the manual operation period of the arm mechanism by the operator for the bending and rotating joint unit. The process is repeatedly executed, and the calculation process of the static torque, the calculation process of the excitation current value, and the output process of the excitation current value and the static command are not performed with respect to the torsional rotation joint portion and the linearly-movable telescopic joint portion. Robotic device to do. In a direct teaching control device of a robot device having an arm mechanism having a plurality of joints using a stepping motor as an actuator,
A joint variable of the joint, a torque calculating unit that calculates a stationary torque in the opposite direction equivalent to a load torque due to its own weight on the joint based on a center of gravity mass of the arm mechanism;
A current value calculation unit that calculates an excitation current value required to generate the static torque in the stepping motor,
An output unit that outputs the exciting current value to a driver of the stepping motor together with a stationary command,
The torque calculation unit, the current value calculation unit, comprising a control unit that controls the output unit,
The arm mechanism includes a torsional rotation joint around a first axis perpendicular to the base, a bending rotation joint around a second axis orthogonal to the first axis, and a third axis orthogonal to the second axis. A linearly extending and contracting joint having a linearly extending and retractable
The control unit is configured to calculate the static torque over a manual operation period of the arm mechanism by an operator with respect to the bending and rotating joint and the linearly-movable telescopic joint, and calculate the exciting current value and the exciting current value. The output process of the stationary command is repeatedly executed, and the static torque calculation process, the excitation current value calculation process, and the excitation current value and the output process of the static command are not executed for the torsional rotation joint. Direct teaching control device. An arm mechanism having a plurality of joints using a stepping motor as an actuator,
A direct teaching control unit that controls direct teaching by the operator to the arm,
The direct teaching control unit,
A torque calculating unit that calculates a reverse static torque equivalent to a load torque due to its own weight based on the mass of the center of gravity of the arm based on a joint variable of the joint,
A current value calculation unit that calculates an excitation current value required to generate the static torque in the stepping motor,
An output unit that outputs the exciting current value to a driver of the stepping motor together with a stationary command,
The torque calculation unit, the current value calculation unit, a control unit that controls the output unit,
The arm mechanism includes a torsional rotation joint around a first axis perpendicular to the base, a bending rotation joint around a second axis orthogonal to the first axis, and a third axis orthogonal to the second axis. A linearly extending and contracting joint having a linearly extending and retractable
The control unit is configured to calculate the static torque over a manual operation period of the arm mechanism by an operator with respect to the bending and rotating joint and the linearly-movable telescopic joint, and calculate the exciting current value and the exciting current value. The output process of the stationary command is repeatedly executed, and the static torque calculation process, the excitation current value calculation process, and the excitation current value and the output process of the static command are not executed for the torsional rotation joint. Robotic device to do.

Images