TW201644182A - Robot apparatus and stepping motor control device - Google Patents

Abstract

In order to reduce the operator load and to improve operator safety in direct teaching, this robot device is provided with an arm mechanism having joints (J1-J6) with a stepping motor as the actuator, and a control unit (100) which controls direct teaching by the operator. The control unit (100) is provided with: a torque calculation unit (104) which, on the basis of joint variables of the joints (J1-J6) and the center of mass of the arms configuring the arm mechanism, calculates the static torque in the opposite direction of and equivalent to the load torque from the weight of the joint itself; a current value calculation unit (105) which calculates the excitation current value required to generate the static torque in the stepping motors; an output unit (107) which outputs the excitation current value together with a static stop command to the drivers of the stepping motor; and a system control unit (100) which controls each unit such that said calculation processing and said output processing of the excitation current value and the static stop command are repeated over the period of manual operation of the arm by the operator.

Description

Robot device and stepper motor control device

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

The stepping motor is proportional to the number of pulse signals, so that there is basically no need for a feedback circuit, and the open loop control can be performed, which is advantageous compared with the AC motor and the DC motor, but conversely, if applied If the overload or the pulse frequency is too high, there is a disadvantage that the synchronous offset causes a so-called "offset" of the control disorder. Due to this disadvantage, a stepping motor is generally not used as the actuator of the robot apparatus.

On the other hand, in the robot apparatus, the operator has to teach the robot apparatus the operation point of the robot arm to be moved, the operation point of the operation point by the point and the work point, and the operator actually moves the arm to teach the above-mentioned action scale. For teaching.

In general, in teaching, there is also a situation in which an operator cannot move due to the weight of the robot arm. Typically, there are various countermeasures to offset the weight of the robot arm by the balance weight, and the motor assist function, the operator The actuator is utilized in the moving direction of the arm, and the control reduces the force of the operator to move the arm.

In the former countermeasure, the weight of the robot arm itself is increased. The latter does not eliminate the fear of colliding with the operator due to the movement of the driven arm.

The object of the present invention is to reduce the operator's teaching burden and improve the safety of the operator by utilizing the stepping motor.

In the present embodiment, the teaching control device including the robot device having the arm of the joint portion using the stepping motor as the actuator is based on the joint variable of the joint portion and the mass of the center of gravity of the arm, and is applied to the torque calculating unit. The load torque due to the self-weight of the joint is equivalent to the static torque in the reverse direction. The value of the field current required to generate the static torque by the stepping motor is calculated by the current value calculating unit. The output unit outputs the excitation current value together with the stationary command to the driver of the stepping motor. The control unit controls the torque calculation unit, the current value calculation unit, and the output unit, and repeats the calculation process of the static torque, the calculation process of the excitation current value, and the excitation current value during the manual operation of the arm by the operator. Output processing of stationary instructions.

50‧‧‧Operation Department

100‧‧‧Teaching control device

101‧‧‧System Control Department

102‧‧‧Operating Department I/F

103‧‧‧ Position ‧ Posture Memory

104‧‧‧Dynamic Computing Department

105‧‧‧Standing excitation current determination unit

106‧‧‧Drive unit I/F

107‧‧‧Output Department

109‧‧‧Control/data bus

210~260‧‧‧Drive unit

211‧‧‧Control Department

212‧‧‧Power circuit

213‧‧‧ Pulse Signal Generation Department

215‧‧‧Encoder

217‧‧‧ counter

310‧‧‧Stepper motor

1 is an appearance of a mechanical arm mechanism of a robot apparatus of the embodiment. Stereo picture.

Fig. 2 is a side view showing the internal structure of the mechanical arm mechanism of Fig. 1.

Fig. 3 is a view showing the configuration of the mechanical arm mechanism of Fig. 1 by the symbol representation.

Fig. 4 is a block diagram showing the configuration of the robot apparatus of the embodiment.

Fig. 5 is a flow chart for explaining the arm holding control sequence when the teaching control device of Fig. 4 performs teaching.

Fig. 6 is a supplementary explanatory view of the sequence of Fig. 5, and is a view showing three postures of the arm mechanism.

Fig. 7 is a supplementary diagram for explaining the sequence of Fig. 5, and is a view showing temporal changes in the value of the stationary excitation current of the stepping motor to the joint portion J2 corresponding to the posture change of Fig. 6.

Hereinafter, a robot apparatus according to the present embodiment or a teaching control apparatus equipped in the robot apparatus will be described with reference to the drawings. The teaching control device of the present embodiment supports the operator in performing a so-called teacher, that is, the support robot directly moves the robot device, for example, the robot hand to move, and the robot hand performs the work position and the work position to the next job. The robot device is stored in the end track such as the position via the position. The robot apparatus includes a robot arm mechanism having a joint portion that uses a stepping motor as an actuator. As the robot apparatus, a vertical multi-joint arm mechanism will be described as an example. In particular, it is described that one of the plurality of joint portions is a linear motion expansion Vertical multi-joint arm mechanism of the joint. In the following description, components having substantially the same functions and configurations are denoted by the same reference numerals, and the description will be repeated only when necessary.

The subject matter of the present embodiment is to reduce the imbalance of the stepping motor by actively utilizing the burden of teaching by the operator and improving the safety of the operator. That is, the load torque due to the weight of the arm is applied to the joint portion. This load torque is calculated based on the joint variable, the mass of the center of gravity of the arm, and the like. The stepper motor is caused to have an equivalent and reverse static torque. Thereby, the arm is stationary in its position in equilibrium with its own weight. In this state, when the operator moves the arm during teaching, the stepping motor is out of adjustment due to the overload exceeding the static torque. Therefore, the operator can easily move the arm. During the movement of the arm, the static torque is repeatedly calculated according to its position and posture, and the arm maintains its self-weighted state at this position. In this way, by using a stepping motor for the actuator of the joint portion of the robot apparatus, the stepping motor is driven and controlled to generate a stationary torque, which can reduce the operator's teaching burden and improve the safety of the operator.

Fig. 1 is a perspective view showing the appearance of a robot apparatus of the embodiment. The arm mechanism constituting the robot apparatus has a substantially cylindrical base portion 10, an arm portion 2 connected to the base portion 10, and a wrist portion 4 attached to the front end of the arm portion 2. An adapter (not shown) is provided on the wrist portion 4. For example, the adapter is provided in a rotating portion of the sixth rotating shaft RA6 which will be described later. For the adapter provided on the wrist 4, a robot hand corresponding to the purpose is installed.

The mechanical arm mechanism has a plurality of, here, six joint portions J1, J2, J3, J4, J5, and J6. A plurality of joint portions J1, J2, J3, J4, J5, and J6 are sequentially disposed from the base portion 10. In general, the first, second, and third joints The portions J1, J2, and J3 are referred to as the root three axes, and the fourth, fifth, and sixth joint portions J4, J5, and J6 are referred to as the three axes of the wrist that change the posture of the robot hand. The wrist portion 4 has fourth, fifth, and sixth joint portions J4, J5, and J6. At least one of the joint portions J1, J2, and J3 constituting the three axes of the root is a linear motion expansion joint. Here, the third joint portion J3 is configured as a linear motion expansion joint portion, in particular, a joint portion having a relatively long stretch distance. The arm portion 2 represents a telescopic portion of the linear motion expansion joint portion J3 (third joint portion J3).

The first joint portion J1 is, for example, a torsion joint centered on the first rotation axis RA1 that is vertically supported by the base surface. The second joint portion J2 is a curved joint centering on the second rotating shaft RA2 disposed perpendicular to the first rotating shaft RA1. The third joint portion J3 is a joint that linearly expands and contracts the arm portion 2 around the third axis (moving axis) RA3 that is disposed perpendicular to the second rotation axis RA2.

The fourth joint portion J4 is a torsion joint centering on the fourth rotation axis RA4. When the seventh joint portion J7 which will be described later does not rotate, that is, when the entire arm portion 2 is linear, the fourth rotation axis RA4 substantially coincides with the third movement axis RA3. The fifth joint portion J5 is a curved joint centering on the fifth rotation axis RA5 orthogonal to the fourth rotation axis RA4. The sixth joint portion J6 is a curved joint centering on the sixth rotation axis RA6 that is orthogonal to the fourth rotation axis RA4 and that is perpendicular to the fifth rotation axis RA5.

The arm support (first support) 11a forming the base 10 has a hollow hollow structure formed around the first rotation axis RA1 of the first joint portion J1. The first joint portion J1 is attached to a fixed table (not shown). When the first joint portion J1 is rotated, the arm portion 2 is coupled to the first support body 11a. The shaft rotates together and swings left and right. Further, the first support 11a may be fixed to the ground contact surface. In this case, the structure is separate from the first support 11a, and the arm 2 is independently rotated. The second support portion 11b is connected to the upper portion of the first support 11a.

The second support portion 11b has a hollow structure continuous with the first support portion 11a. One end of the second support portion 11b is attached to the rotating portion of the first joint portion J1. The other end of the second support portion 11b is opened, and the third support portion 11c is rotatably fitted to the second rotation axis RA2 of the second joint portion J2. The third support portion 11c has a hollow structure formed in a scaly shape that communicates with the first support portion 11a and the second support portion. The third support portion 11c is accommodated in the second support portion 11b or is delivered from the second support portion 11b in accordance with the bending of the second joint portion J2. The rear portion of the arm portion 2 of the linear motion expansion joint portion J3 (the third joint portion J3) constituting the mechanical arm mechanism is housed in a continuous hollow structure of the first support portion 11a and the second support portion 11b by contraction thereof. internal.

The third support portion 11c is rotatably fitted to the lower portion of the open end of the second support portion 11b around the second rotation axis RA2. Thereby, the second joint portion J2 which is the curved joint portion around the second rotation axis RA2 is configured. When the second joint portion J2 is rotated, the arm portion 2 is rotated in the vertical direction around the second rotation axis RA2, that is, the undulating motion.

The fourth joint portion J4 has a torsion joint with a fourth rotation axis RA4 that is typically in contact with the arm center axis of the arm portion 2 in the telescopic direction, that is, the third movement axis RA3 of the third joint portion J3. When the fourth joint portion J4 is rotated, the wrist portion 4 and the robot hand attached to the wrist portion 4 are rotated by the fourth rotation axis. RA4 is centered for rotation. The fifth joint portion J5 has a curved joint portion of the fifth rotation axis RA5 that is orthogonal to the fourth rotation axis RA4 of the fourth joint portion J4. When the fifth joint portion J5 is rotated, the fifth joint portion J5 is rotated up and down with the robot hand (the vertical direction around the fifth rotation axis RA5 in the vertical direction). The sixth joint portion J6 has a curved joint of a sixth rotation axis RA6 that is orthogonal to the fourth rotation axis RA4 of the fourth joint portion J4 and perpendicular to the fifth rotation axis RA5 of the fifth joint portion J5. When the sixth joint portion J6 rotates, the robot hand swings left and right.

The robot hand attached to the adapter of the wrist 4 as described above is moved to an arbitrary position by the first, second, and third joint portions J1, J2, and J3, and by the fourth, fifth, and sixth The joint portions J4, J5, and J6 are arranged in an arbitrary posture. In particular, the length of the telescopic distance of the arm portion 2 of the third joint portion J3 allows the robot hand to reach a wide range of objects from the proximal position of the base portion 10 to the remote position. The third joint portion J3 is characterized by a linear expansion and contraction operation and a length of a telescopic distance thereof by a linear motion expansion mechanism.

Fig. 2 is a perspective view showing the internal structure of the mechanical arm mechanism of Fig. 1. The linear motion expansion mechanism has an arm portion 2 and an injection portion 30. The arm portion 2 has a first connecting link row 21 and a second connecting link row 22. The first connecting link row 21 includes a plurality of first connecting links 23 . The first connecting link 23 is formed in a substantially flat plate shape. The first connecting links 23 in the front and rear are connected to each other at their end portions, and are bent and connected in a row by a pin. The first connecting link row 21 can be freely bent toward the inside or the outside.

The second connecting link row 22 includes a plurality of second connecting links 24 . The second connecting link 24 is configured as a cross section Short groove shaped body. The front and rear second connecting links 24 are connected to each other at the end portions of the bottom surfaces thereof so as to be bent and connected in a row by pins. The second connecting link row 22 can be bent inward. The cross section of the second connecting link 24 is The shape of the word is such that the second connecting link row 22 and the side plates of the adjacent second connecting link 24 collide with each other and are not bent outward. Further, the surface of the first and second connecting links 23 and 24 facing the second rotating shaft RA2 is defined as an inner surface, and the surface on the opposite side is referred to as an outer surface. The first connecting link 23 at the foremost end of the first connecting link row 21 and the second connecting link 24 at the foremost end of the second connecting link row 22 are connected by a joint link 27. For example, the joint link 27 has a shape in which the second joint link 24 and the first joint link 23 are combined.

The injection unit 30 is formed by supporting a plurality of upper rolls 31 and a plurality of lower rolls 32 in a frame 35 having a rectangular tube shape. For example, the plurality of upper rollers 31 are arranged along the central axis of the arm at intervals substantially equivalent to the length of the first connecting link 23. Similarly, a plurality of lower rollers 32 are arranged along the central axis of the arm at intervals substantially equivalent to the length of the second connecting link 24. After the injection portion 30, the guide roller 40 and the drive gear 50 are disposed to face each other across the first connecting link row 21. The drive gear 50 is connected to the stepping motor 330 via a speed reducer (not shown). A linear gear is formed on the inner surface of the first connecting link 23 along the connecting direction. When the plurality of first connecting links 23 are linearly aligned, the linear gears are linearly connected to each other to form a long linear gear. The drive gear 50 is meshed with a linear linear gear. The linearly connected linear gear train and the drive gear 50 together constitute a rack and pinion mechanism.

When the arm is extended, the motor 55 is driven and the drive gear 50 is rotated in the forward direction. Then, the first connecting link row 21 is guided between the upper roller 31 and the lower roller 32 by the guide roller 40 being in a posture parallel to the central axis of the arm. As the first connecting link row 21 moves, the second connecting link row 22 is guided between the upper roller 31 and the lower roller 32 of the emitting portion 30 by a guide rail (not shown) disposed behind the emitting portion 30. The first and second connecting link rows 21 and 22 guided between the upper roller 31 and the lower roller 32 are pressed against each other. Thereby, the columnar bodies are constituted by the first and second connecting link rows 21 and 22. The injection unit 30 joins the first and second connecting link rows 21 and 22 to form a columnar body, and supports the columnar body up, down, left, and right. The columnar body in which the first and second connecting link rows 21 and 22 are joined is held by the emitting portion 30 to prevent the separation, thereby maintaining the joined state of the first and second connecting link rows 21 and 22. When the joined state of the first and second connecting link rows 21 and 22 is maintained, the bending of the first and second connecting link rows 21 and 22 is mutually restrained. Thereby, the first and second connecting link rows 21 and 22 constitute a columnar body having a certain rigidity. The columnar system refers to a columnar rod in which the first connecting link row 21 is joined to the second connecting link row 22. The columnar system second connecting link 24 and the first connecting link 23 are integrally formed into a tubular body having various cross-sectional shapes. The cylindrical system is defined as a shape in which the front end portion and the rear end portion are opened by the top, bottom, and side plates surrounded by the top, bottom, left, and right sides. The columnar system in which the first and second connecting link rows 21 and 22 are joined is a starting end of the joining link 27, and is linearly fed out from the opening of the third supporting portion 11c along the third moving axis RA3.

When the arm is contracted, the motor 55 is driven, and the drive gear 50 is rotated in the reverse direction, and the first connecting link row 21 engaged with the drive gear 50 is pulled back into the first support 11a. As the first link chain moves, the column is pulled Returning to the third support 11c. The columnar body that is pulled back is separated behind the injection portion 30. For example, the first connecting link row 21 constituting the columnar body is sandwiched by the guide roller 40 and the drive gear 50, and the second connecting link row 22 constituting the columnar body is pulled downward by gravity, whereby the second link The chain link row 22 and the first connecting link row 21 are apart from each other. The first and second connecting link rows 21 and 22 that have moved away from each other are restored to a bendable state. At the time of storage, the second connecting link row 22 is conveyed to the inside of the first support body 11a (base portion 10) in a curved manner from the inside of the projecting portion 30, and the first connecting link row 21 is also facing the second. The connecting link rows 22 are bent in the same direction (inside) and transported. The first connecting link row 21 is in a state of being substantially parallel to the second connecting link row 22 .

Figure 3 is a diagram showing the mechanical arm mechanism of Figure 1 by representation of the symbols. In the arm mechanism, three positional degrees of freedom are realized by the first joint portion J1, the second joint portion J2, and the third joint portion J3 which constitute the three axes of the root portion. Further, three posture degrees of freedom are realized by the fourth joint portion J4, the fifth joint portion J5, and the sixth joint portion J6 which constitute the three axes of the wrist.

The robot coordinate system Σb is a coordinate system in which an arbitrary position on the first rotation axis RA1 of the first joint portion J1 is the origin. In the robot coordinate system Σb, orthogonal three 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 end coordinate system Σh is a coordinate system in which the robot hand 5 attached to the wrist 4 is at an arbitrary position (end reference point). For example, when the robot hand 5 is a 2-finger hand, the position of the end reference point (hereinafter simply referred to as the end) is defined at the center position between the two fingertips. End coordinate system In Σh, three orthogonal axes (Xh, Yh, and Zh) are defined. The Zh axis is an axis parallel to the sixth rotation axis RA6. The Xh axis and the Yh axis are orthogonal to each other and orthogonal to the Zh axis. For example, the Xh axis is an axis parallel to the front and rear directions of the robot hand 5. The end posture is set as the rotation angle of each of the orthogonal coordinate axes of the end coordinate system Σh with respect to the robot coordinate system (b (rotation angle (roll angle) α around the Xh axis, rotation angle around the Yh axis (pitch angle) β, the angle of rotation around the Zh axis (swing 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 torsion joint centering on the rotation axis RA1. The rotation axis RA1 is disposed perpendicular to the reference surface BP of the base on which the fixing portion of the first joint portion J1 is provided.

The second joint portion J2 is configured as a curved joint centering on the rotation axis RA2. The rotation axis RA2 of the second joint portion J2 is provided in parallel with the Xb axis on the space coordinate system. The rotation axis RA2 of the second joint portion J2 is oriented perpendicular to the rotation axis RA1 of the first joint portion J1. Further, the second joint portion J2 is offset from the first joint portion J1 in the direction of the first rotation axis RA1 (Zb axis direction) and the direction of the Yb axis direction perpendicular to the first rotation axis RA1. The second support 11b is attached to the first support 11a so that the second joint portion J2 is displaced in the above-described two directions with respect to the first joint portion J1. The dummy arm portion (link portion) that connects the second joint portion J2 to the first joint portion J1 has a crank shape in which two hook-shaped bodies that are vertically bent at the tip end are combined. The dummy arm portion is composed of first and second supports 11a and 11b having a hollow structure.

The third joint portion J3 is configured to be a direct motion centering on the moving shaft RA3. Telescopic joints. The movement axis RA3 of the third joint portion J3 is oriented perpendicular to the rotation axis RA2 of the second joint portion J2. When the rotation angle of the second joint portion J2 is zero degrees, that is, the undulation angle of the arm portion 2 is zero and the arm portion 2 is horizontal, the movement axis RA3 of the third joint portion J3 is set to be the second joint portion J2. The rotation axis RA2 and the rotation axis RA1 of the first joint portion J1 are perpendicular to each other. In the space coordinate system, the movement axis RA3 of the third joint portion J3 is provided in parallel with the Yb axis perpendicular to the Xb axis and the Zb axis. Further, the third joint portion J3 is offset from the second joint portion J2 in 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 portion J3 is offset from the second joint portion J2 in the above two directions. The dummy arm portion (link portion) that connects the third joint portion J3 to the second joint portion J2 has a hook-shaped body whose front end is vertically curved. The dummy arm portion is composed of the second and third supports 11b and 11c.

The fourth joint portion J4 is configured as a torsion joint centering on the rotation axis RA4. The rotation axis RA4 of the fourth joint portion J4 is disposed so as to substantially coincide with the movement axis RA3 of the third joint portion J3.

The fifth joint portion J5 is configured as a curved joint centering on the rotation axis RA5. The rotation axis RA5 of the fifth joint portion J5 is disposed so as to be substantially orthogonal to the movement axis RA3 of the third joint portion J3 and the rotation axis RA4 of the fourth joint portion J4.

The sixth joint portion J6 is configured as a torsion joint centering on the rotation axis RA6. The rotation axis RA6 of the sixth joint portion J6 is the same as the fourth joint portion J4 The rotation axis RA4 and the rotation axis RA5 of the fifth joint portion J5 are arranged substantially orthogonal to each other. The sixth joint portion J6 is provided to rotate the robot hand 5 as an end effector to the right and left. Further, the sixth joint portion J6 may constitute a curved joint whose rotation axis RA6 is substantially orthogonal to the rotation axis RA4 of the fourth joint portion J4 and the rotation axis RA5 of the fifth joint portion J5.

In this manner, one of the three axial axes of the plurality of joint portions J1 - J6 is replaced with a linear motion joint portion, and the second joint portion J2 is offset with respect to the first joint portion J1 in two directions, and The third joint portion J3 is offset with respect to the second joint portion J2 in the two directions, whereby the mechanical arm mechanism of the robot apparatus of the present embodiment structurally eliminates the critical point posture.

Fig. 4 is a block diagram showing the configuration of the robot apparatus of the embodiment. In the joint portions J1, J2, J3, J4, J5, and J6 of the robot arm mechanism of the robot apparatus of the present embodiment, stepping motors 310, 320, 330, 340, 350, and 360 are provided as actuators, respectively. Here, the stepping motor is a five-phase stepping motor. The stepper motors 310, 320, 330, 340, 350, 360 are electrically connected to the driver units 210, 220, 230, 240, 250, 260. Typically, the driver units 210, 220, 230, 240, 250, 260 are each associated with a stepper motor that controls the object. The 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 the control signal from the teaching control device 100. Here, only the driver unit 210 will be described, and the description of the other driver units 220, 230, 240, 250, 260 will be omitted.

The driver unit 210 controls the driving and stopping of the stepping motor 310. drive The actuator 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 collectively controls the driver unit 210 in accordance with the command value input from the teaching control device 100.

As is well known, the stepping motor 310 is formed by arranging a plurality of stator coils around a rotor to which a drive shaft is coupled. The stator coil is connected to the power supply circuit via a switching element. The switching elements are sequentially turned on by a pulse signal, and the rotors are sequentially rotated at a specific step angle. The rotation speed can be changed by changing the frequency (pulse frequency) of the pulse signal. The stepper motor 310 can be made stationary by continuing the on state of the particular switching element and continuing the energization state of the particular 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. In addition, the torque at which the static torque is equal to the load torque due to the self-weight of the arm is clearly distinguished from the so-called maximum static torque.

The control unit 211 inputs a command code indicating the field current value of the stepping motor 310 from the teaching control device 100. 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 current-variable AC/DC conversion power supply circuit that generates a current of a field current value specified by a command code. The generated exciting current is supplied to the stator coil of the stepping motor 310.

Further, the control unit 211 in the driver unit 210 receives a stationary command signal for causing the stepping motor 310 to be at the current position from the teaching control device 100. The stepping motor 310 is stationary at its position by continuously supplying a current to the stator coil of the phase corresponding to the current position. Therefore, as static The command signal provides, for example, a code indicating the current joint angle θ1(t) (joint variable) of the joint portion J1. Similarly, the self-teaching control device 100 inputs, to the driver units 220, 240, 250, and 260 corresponding to the joint portions J2, J4, J5, and J6, respectively, the current joint angles θ2(t) and θ4(t). The stationary command signal encoded by θ5(t) and θ6(t) is input to the driver unit 230 corresponding to the joint portion J3 with a stationary command including the code indicating the current stretch distance (linear translation) L3(t). signal. Further, in the joint portions J1, J2, J4, J5, and J6, the joint angle refers to the positive and negative rotation angles from the reference position, and in the joint portion J3, the telescopic distance refers to the distance from the most contracted state. Joint angle and elongation distance are collectively referred to as joint variables.

The pulse signal generation unit 213 determines the number of pulses by dividing the difference between the joint angles from the current joint variable and the specific control period Δt (for example, 10 ms) indicated by the control unit 211 by the step angle, and uses the control period. Δt is divided by the number of pulses, and the pulse frequency is determined by its reciprocal. At the time of rest, the self-control unit 211 gives the current joint variable a joint angle after a specific control period Δt (for example, 10 ms). Thereby, the step pulse system outputs only the phase corresponding to the current position of the stepping motor 310 among the plurality of phases (coils) of the stepping motor 310. Thereby, the stepping motor 310 can stay at the current position. At this time, the stepping motor 310 generates a stationary torque corresponding to the value of the exciting current.

The rotary encoder 215 is connected to the drive shaft of the stepping motor 310, and outputs a pulse signal (encoder pulse) at a constant rotation angle. The counter 217 adds and subtracts the number of encoder pulses output from the rotary encoder 215 in accordance with the direction of rotation, thereby calculating the number of counts. The count is tied to The reference position (origin) of the drive shaft of the stepping motor 310 is reset. The counter 217 calculates the joint angle θ1(t) (joint variable) of the joint portion J1 based on the number of resets and the number of counts.

The teaching control device 100 includes a system control unit 101, an operation unit interface 102, a position and posture storage unit 103, a dynamic calculation unit 104, a stationary excitation current determination unit 105, an output unit 107, and a driver unit interface 106. In the teaching control device 100, the self-driver unit 210 is input with the current joint variable of each of the joint portions J1-J6 calculated by the counter 217 via the driver unit interface 106 every predetermined control period (for example, 10 ms). Information.

The system control unit 101 includes a CPU (Central Processing Unit), a semiconductor memory, and the like, and integrally controls the teaching control device 100. The system control unit 101 is connected to each other via the control/data bus 96.

The operation unit 50 is connected to the teaching control device 100 via the operation unit interface 102. The operation unit 50 functions as an interface for the operator to register the work position, the position, the end work content of each work position, and the like during teaching. For example, the operation unit 50 includes a changeover switch for switching the control mode of the robot device from the normal mode to the teaching mode. Further, the operation unit 50 is provided with a registration switch for the operator to register the end track. The system control unit 101 sequentially stores the joint variables of the joint portions J1 - J6 when the registration switch is pressed, in the order in which the registration switch is pressed, in the position ‧ posture memory portion 103 to be described later. The input device constituting the operation unit 50 may be replaced by other devices such as a mouse, a keyboard, a trackball, a touch panel, and the like.

The position ‧ posture memory unit 103 memorizes the action sequence data taught by the operator by teaching. In the motion sequence data, the robot coordinates are used to describe the start point and end point of the end reference point and the middle point between the start point and the end point. Command values such as the value of the joint variable of each of the joint portions J1 - J6, the movement time, and the work contents of the robot hand are associated with each of these points. The movement time and the work content of the robot hand can be registered during the teaching period according to the operation of the operator via the operation unit 50, and can be registered in other periods.

The dynamic calculation unit 104 stores a dynamic model corresponding to each of the six joint portions J1 - J6 in the ROM. The dynamic model is used to calculate a calculation formula of the torque applied to each joint portion J1-J6 based on the joint variable. The dynamic model is calculated in advance for each of the joint portions J1 - J6 based on the structural features of the mechanical arm mechanism, that is, the position of the center of gravity of each of the links connecting the joint portions J1 - J6 , the link quality, the link length, and the like. For example, in the joint portion J1, the load torque due to the self-weight corresponding to the above-described structural feature is generated from the joint portion J1 in the mechanical arm mechanism. For example, the current joint variable for each of the joint portions J1 - J6 is applied to the dynamic model for the joint portion J1, thereby calculating the load torque due to the weight of the arm applied to the joint portion J1. Further, the torque equivalent to the load torque due to the self-weight and the reverse torque is calculated. By generating the torque in the joint portion J1, the arm portion 2 is stationary in the state of being balanced with the weight in the joint portion J1. Hereinafter, this torque is referred to as a stationary torque. Similarly, in the other joint portions J2-J6, the joint variables of all the joint portions J1-J6 at that time are applied to the respective dynamic models, thereby calculating the rest for the self-weight balance with the arm portion 2, and the joint portion J2-J5 is required. The static torque T2(t)~T6(t).

The stationary excitation current determining unit 105 determines the supply to the stepping motor 310. The excitation current value of the excitation current is Is1(t). The stationary excitation current determining unit 105 holds data of a correspondence table in which the torque of the stepping motor 310 is correlated with the exciting current value. The stationary excitation current determination unit 105 refers to the correspondence table and determines the stationary excitation current value Is1(t) corresponding to the stationary torque T1(t) of the joint portion J1 calculated by the dynamic calculation unit 104. The stationary excitation current determining unit 105 calculates the excitation current values Is2(t) to Is6(t) for the stationary portions of the joint portions J2-J6 by the same method.

In addition, the stationary excitation current determination unit 105 determines the stationary excitation current value Is1(t) corresponding to the stationary torque T1(t) of the joint portion J1 calculated by the dynamic calculation unit 104. However, when the dynamic calculation unit 104 calculates the stationary torque, it includes an error factor such as the frictional force inside the mechanical arm mechanism. Therefore, the stationary excitation current determining unit 105 may determine a stationary torque that is slightly larger than the static torque T1(t) calculated by the dynamic calculating unit 104, for example, and may determine a stationary state corresponding to 1.1 times the calculated static torque. The value of the excitation current used.

The output unit 107 outputs a command value corresponding to the joint portions J1 to J6 determined by the teaching control device 100 to the driver unit 210 in accordance with the control of the system control unit 101. Specifically, the output unit 107 sets the command code indicating the stationary field current value Is1(t) determined by the stationary field current determining unit 105 and the current joint angle θ1 (t) indicating the input from the driver unit 210. The encoded stationary command signal is output to the driver unit 210. Similarly, the output unit 107 outputs the command values (excitation current values for stationary and current joint variables) of the joint portions J2-J6 to the driver units 220 to 260 in accordance with the control of the system control unit 101.

Hereinafter, the arm holding control sequence at the time of teaching of the 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 flow chart for explaining the arm holding control sequence at the time of teaching of the teaching control device 100 of Fig. 4. Fig. 6 is a supplementary explanatory view of the sequence of Fig. 5, and is a view showing three postures of the mechanical arm mechanism. Fig. 7 is a supplementary explanatory view of the sequence of Fig. 5, and is a view showing temporal changes in the value of the exciting current for the stationary stepping motor supplied to the joint portion J2 in accordance with the posture change of Fig. 6.

(Step S1) Move to teaching mode

The system control unit 101 activates the tutorial mode in accordance with the ON operation of the teaching of the changeover switch of the operation unit 50.

(Step S2) Input of joint variables of the joint portion

Starting the tutorial mode, first, the current joint variables θ1(t), θ2(t), L3(t), θ4(t) corresponding to the joint portions J1-J6 are input from the driver unit 210 to the teaching control device 100, Θ5(t), θ6(t).

(Step S3) Calculation processing of the static torque of the joint portions J1 - J6

The dynamic calculation unit 104 calculates the load torque due to the self-weight applied to each of the joint portions J1-J6 by the dynamic model based on the current joint variables θ1(t) to θ6(t) of the joint portions J1-J6. The equivalent and reverse static torque T1(t)~T6(t).

(Step S4) Calculation processing of the excitation current value for stationary

The stationary excitation current determination unit 105 refers to the correspondence table between the torque and the excitation current value, and determines the stationary torque T1(t) to T6(t) calculated by the dynamic calculation unit 104 in step S3 to allow the stepping motor. 310~360 The excitation current value Is1(t)~Is6(t) for static generation necessary for torque generation.

(Step S5) output to the driver unit

The control unit 211 from the output unit 107 to each of the driver units 210 to 260 of the joint portions J1 to J6 sets the code indicating the field current values Is1(t) to Is6(t) determined in step S4, and includes the current The stationary command signals encoded by the joint variables θ1(t) to θ6(t) are output as a command value.

(Step S6) Determination processing of the end of teaching

The system control unit 101 controls the respective units in order to repeat the steps from step S2 to step S5 in a specific cycle throughout the teaching period and at least during the manual operation of the operator's opponent or arm. The system control unit 101 ends the tutorial mode in accordance with the OFF operation taught by the switch of the operation unit 50.

Next, a supplementary explanation of FIG. 5 will be made with reference to FIGS. 6 and 7. Fig. 6 is a supplementary explanatory view of the sequence of Fig. 5, and is a view showing three postures of the mechanical arm mechanism. Fig. 7 is a supplementary explanatory view showing the order of Fig. 5, and is a view showing temporal changes in the value of the stationary excitation current of the stepping motor 320 to the joint portion J2 corresponding to the posture change of Fig. 6. For convenience of explanation, the stepping motor 320 assumes 5 phases (A phase to E phase), that is, a pair of stator coil systems. 5 sets. 6(a), 6(b), and 6(c) show the postures of the arm mechanisms at times t0, t1, and t2, respectively. Here, teaching is started at time t0, and the arm mechanism is manually operated at time t1 and time t2.

At the start of the teaching (time t0), the stepping motor 320 of the joint portion J2 is supplied with the A phase (coil) corresponding to the current position of the joint portion J2 of the posture of the mechanical arm mechanism of Fig. 6(a). Excitation current of the excitation current value It0 for stationary. Thereby, the stepping motor 320 generates the stationary torque corresponding to the exciting current value It0, and the joint portion J2 is stationary at the current position. This static torque is inversely equivalent to the load torque applied to the joint portion J2 by its own weight in the arm mechanism of the posture shown in Fig. 6(a). Therefore, the stepping motor 320 is in a state of being immediately detuned even if a small load is applied. Therefore, the operator applies a small force to the arm mechanism, and the stepping motor 320 generates an offset.

Therefore, in the manual operation of the robot arm mechanism by the operator at time t1, the operator can move the robot arm mechanism from the posture shown in FIG. 6(a) to FIG. 6 (b) without using the force. ) The posture shown. During the movement of the arm mechanism, the command value (current joint variable and field current value) is output from the teaching control device 100 to the driver units 210 to 260 of the joint portions J1 - J6 every control cycle. Therefore, after the operator manually moves the arm mechanism from the posture shown in FIG. 6(a) to the posture shown in FIG. 6(b), even if the hand leaves the arm mechanism, the arm mechanism remains as the operator's hand leaves. The posture at the time (Fig. 6(b)). The reason is that the current position of the stepping motor 320 of the joint portion J2 has been corresponded to when the operator's hand leaves the robot arm mechanism. The B phase (coil) is supplied with an exciting current having a field current value It1 for stationary. Thereby, the stepping motor 320 generates the stationary torque corresponding to the exciting current value It1, and the joint portion J2 is at the current position. This static torque is reversed and equivalent to the load torque applied to the joint portion J2 by its own weight in the arm mechanism of the posture shown in FIG. 6(b). Therefore, the stepping motor 320 is in a state of being immediately detuned even if a small load is applied. Therefore, in the manual operation of the robot arm mechanism by the operator at time t2, the operator can move the arm mechanism from the posture shown in FIG. 6(b) to FIG. 6 by an easy manual operation without using the force. (c) The posture shown. When the operator manually moves the arm mechanism from the posture shown in FIG. 6(b) to the posture shown in FIG. 6(c), even if the hand leaves the arm mechanism, the arm mechanism remains at the time when the operator's hand leaves. Position (Fig. 6(c)). At this time, the stepping motor 320 of the joint portion J2 supplies the excitation current value for stationary to the E phase (coil) corresponding to the current position of the joint portion J2 of the posture of the mechanical arm mechanism of Fig. 6(c). The excitation current of It1. Further, the joint portion J2 of the posture of the mechanical arm mechanism of FIGS. 6(b) and 6(c) has a load parallel to the gravity direction applied by its own weight, and is compared with the horizontal posture of the mechanical arm mechanism of FIG. 6(a). It is smaller. Therefore, the field current values It1, It2 are smaller than the field current value It0.

Further, in the robot arm mechanism of the robot apparatus of the present embodiment, for example, the base portion 10 is provided perpendicular to the base surface (ground surface), and the first joint portion J1 is formed by the first rotation axis RA1 perpendicular to the base surface. The center of the torsion joint. Therefore, in the teaching, the joint portion J1 may be a so-called idle state in which the exciting current is not supplied to the stepping motor 310 of the joint portion J1. Therefore, The control unit 101 may exclude the driver unit 210 corresponding to the joint unit J1 from the control target of the arm holding control in the teaching, and may not perform the output processing of the control signal on the driver unit 210 corresponding to the joint unit J1. Similarly, in the linear motion expansion mechanism constituting the third joint portion J3, the arm portion 2 is supported by the injection portion 30. Therefore, in the arm holding control in the teaching, the joint portion J3 may be in a so-called idle state in which the exciting current is not supplied to the stepping motor 330 of the joint portion J3. Therefore, the system control unit 101 can exclude the driver unit 230 corresponding to the joint portion J3 from the control target of the arm holding control in the teaching, and does not perform the output processing of the control signal on the driver unit 230 corresponding to the joint portion J3.

According to the arm holding control of the robot apparatus of the present embodiment described above, the static torque generated by the joint portions J1 - J6 can be dynamically changed in accordance with the posture change of the arm mechanism. At this time, each of the joint portions J1 - J6 generates a minimum static torque required for the current position to continue to be static. The static torque has a value that is substantially equivalent to or slightly larger than the load applied to the joint portions J1 - J6 by the weight of the arm mechanism. Therefore, the stepping motor of each of the joint portions J1 - J6 is in a state of being immediately detuned even if a small load is applied. Therefore, if the operator manually operates the arm mechanism, the stepping motor is out of adjustment by applying a load equal to or higher than the static torque, whereby the operator can easily perform the manual operation of the arm mechanism without using the force. Further, when the operator's hand leaves the arm mechanism, the joint portions J1 - J6 are applied to the minimum stationary torque required for the position to remain stationary, so that the operator can keep the hand away from the arm mechanism. The posture of the mechanical arm mechanism.

In the teaching, the operator repeats the manual operation of the robot arm mechanism and the registration of the position of the arm mechanism in order to teach the motion sequence information to the robot device. During the manual operation of the arm during at least the operator's arm, the arm holding control allows the operator to manually operate the arm mechanism without using the force, so that the arm mechanism can be held at the position where the hand is away from the arm mechanism. The posture. Therefore, the robot apparatus of the present embodiment can utilize the offset phenomenon of the stepping motor, reduce the burden of teaching by the operator, and improve the safety of the operator.

The embodiments of the present invention have been described, but the embodiments are presented as examples and are not intended to limit the scope of the invention. The embodiments can be implemented in various other forms, and various omissions, substitutions and changes can be made without departing from the scope of the invention. The invention and its modifications are intended to be included within the scope of the invention and the scope of the invention.

50‧‧‧Operation Department

100‧‧‧Teaching control device

101‧‧‧System Control Department

102‧‧‧Operating Department I/F

103‧‧‧ Position ‧ Posture Memory

104‧‧‧Dynamic Computing Department

105‧‧‧Standing excitation current determination unit

106‧‧‧Drive unit I/F

107‧‧‧Output Department

109‧‧‧Control/data bus

210, 220, 260‧‧‧ drive unit

211‧‧‧Control Department

212‧‧‧Power circuit

213‧‧‧ Pulse Signal Generation Department

215‧‧‧Encoder

217‧‧‧ counter

310, 320, 360‧‧‧ stepper motor

Claims (4)

A teaching control device including a robot control device having a robot having an arm portion of a stepping motor as an actuator, and a torque calculation unit based on a joint variable of the joint portion The gravity center mass of the arm is calculated as a static torque equivalent to the load torque applied to the self-weight of the joint portion, and the current value calculation unit calculates a field current value required to generate the static torque by the stepping motor. And an output unit that outputs the excitation current value together with the stationary command to the driver of the stepping motor; and a control unit that repeats the calculation of the static torque for the manual operation period of the arm over the operator And the calculation process of the excitation current value, the excitation current value, and the output processing of the stationary command, and controlling the torque calculation unit, the current value calculation unit, and the output unit. The teaching control device of claim 1, wherein the arm mechanism has a base portion, a torsion rotation joint portion around a first axis of a substantially center line of the base portion, and a bending rotation about a second axis orthogonal to the first axis a joint portion and a linear motion expansion joint portion having a linear motion linearity along a third axis orthogonal to the second axis, and the control unit does not perform the static torque calculation process and the excitation on the torsion joint portion The calculation of the current value, the excitation current value, and the output processing of the stationary command. The teaching control device of claim 2, wherein the control unit does not perform the calculation of the static torque, the calculation process of the excitation current value, and the excitation current for the torsion joint portion and the linear motion joint portion. The value and the output processing of the above stationary command. A robot apparatus including: an arm mechanism including an arm having a joint portion using a stepping motor as an actuator; and a teaching control unit that controls an operator to teach the arm; and the teaching The control unit includes a torque calculation unit that calculates a static torque equivalent to the load torque applied to the self-weight of the joint portion based on the joint variable of the joint portion and the mass of the center of gravity of the arm, and a current value calculation unit. Calculating a field current value necessary for the stepping motor to generate the static torque; and an output unit that outputs the field current value together with a stationary command to a driver of the stepping motor; and a control unit for The operator repeats the calculation process of the static torque, the calculation process of the excitation current value, the excitation current value, and the output processing of the stationary command during the manual operation of the arm, and controls the torque calculation unit and the current. a value calculation unit and the output unit.