CONTROL DEVICE AND ROBOT SYSTEM

Abstract

A control device for controlling a machine with axes having a corotational relation includes: axis position detectors; motor position detectors; a position command calculation unit calculating a position command for each of the axes based on an operation program; a position control unit outputting a speed command of each motor based on the position command and the detection position of the corresponding axis; a speed control unit controlling each motor based on the speed command; and a correction value calculation unit calculating a correction value for correcting, based on the corotational relation, the speed command of a motor with a to-be-controlled axis which is an axis rotating dependent on the corotational relation. The speed control unit corresponding to the to-be-controlled axis corrects the speed command based on the correction value so as to be applied to control of the motor with the to-be-controlled axis.

Claims

1. A controller configured to control an operation of a machine including a plurality of axes in a corotation relationship in which rotation of one of the plurality of axes causes rotation of another one of the plurality of axes, the controller comprising: an axis position detector configured to detect a position of each of the plurality of axes; a motor position detector configured to detect a position of each of a plurality of motors configured to drive the plurality of respective axes; a position command calculating unit configured to calculate a position command for each of the plurality of axes, in accordance with an operation program; a position control unit configured to output a speed command for each of the motors corresponding to the plurality of respective axes, based on the position command and detected positions of the plurality of respective axes; a speed control unit configured to control the respective motors based on the speed command; and a correction value calculating unit configured to calculate a correction value for correcting, based on the corotation relationship, the speed command for one of the motors corresponding to a control target axis that is an axis, among the plurality of axes, rotating depending on the corotation relationship, wherein the speed control unit that corresponds to the control target axis corrects, based on the correction value, the speed command for the motor corresponding to the control target axis, and applies the corrected speed command to control for the motor corresponding to the control target axis.

2. The controller of claim 1, wherein the correction value calculating unit calculates the correction value based on a relationship between the position of the control target axis that rotates depending on the corotation relationship, and a position or positions of a motor or motors, of the motors, corresponding to at least one of the axes that causes the corotation of the control target axis.

3. The controller of claim 1, wherein, in a case where the machine is a 6-axis robot, the control target axis is a sixth axis, a corotation relationship holds in which a position of the sixth axis depends on positions of motors, of the motors, corresponding to a fourth axis and a fifth axis respectively, the corotation relationship is defined as:
θ.sub.J6=αθ.sub.m4+βθ.sub.m5+Fθ.sub.m6 where θ.sub.J6 represents the position of the sixth axis, θ.sub.m4, θ.sub.m5, and θ.sub.m6 respectively represent positions of motors, of the motors, corresponding to the positions of the fourth axis to the sixth axis, and α, β, and F are coefficients, the correction value calculating unit calculates the correction value by: $\left(\mathrm{CORRECTION}\mathrm{VALUE}\right)=-\frac{\alpha }{F}.Math.\frac{d{\theta }_{m4}}{\mathrm{dt}}-\frac{\beta }{F}.Math.\frac{d{\theta }_{m5}}{\mathrm{dt}}.$

4. A robot system comprising: the controller of claim 1; and an articulated robot having a configuration of the machine.

Description

BRIEF DESCRIPTION OF DRAWINGS

[0009] FIG. 1 is a diagram illustrating a configuration of a robot system including a robot controller according to an embodiment.

[0010] FIG. 2 is a functional block diagram of the robot controller and a robot.

[0011] FIG. 3 is a block diagram of a feedback control circuit for a full-closed control scheme according the embodiment.

[0012] FIG. 4A is a block diagram of a feedback control circuit for a typical semi-closed control scheme, illustrated as Reference Example.

[0013] FIG. 4B is a block diagram of a feedback control circuit for a typical full-closed control scheme, illustrated as Reference Example.

[0014] FIG. 5 is a diagram illustrating an example of a configuration of robot arms in a corotation relationship, illustrated as Reference Example.

DESCRIPTION OF EMBODIMENTS

[0015] Next, an embodiment of the present disclosure will be described with reference to the drawings. In the drawings to be referenced, similar components or functional parts are denoted by the same reference numerals. The drawings are appropriately scaled for ease of understanding. A form illustrated in the drawings is an example for carrying out the present invention and the present invention is not limited to the illustrated form.

[0016] FIG. 1 is a diagram illustrating a configuration of a robot system 100 including a robot controller 20 according to an embodiment. FIG. 2 is a functional block diagram of the robot controller 20 and a robot 10. As will be described in detail below, the robot controller 20 executes appropriate control while taking into account a case where articulated axes of the robot are in a corotation relationship, through feedback control under a full-closed control scheme.

[0017] As illustrated in FIG. 1, the robot system 100 includes the robot 10 and the robot controller 20. The robot 10 is a 6-axis vertical articulated robot in the present embodiment. Another type of robot may be used as the robot 10. The robot controller 20 controls the operation of the robot 10, according to an operation program. The robot controller 20 may have a configuration as a general computer including a CPU, a ROM, a RAM, a storage device, an operation section, a display section, an input/output interface, a network interface, and the like. A teaching device (not illustrated) may be connected to the robot controller 20.

[0018] As illustrated in FIG. 1, the robot 10 is a 6-axis robot and includes six articulated axes (hereinafter, also simply referred to as axes) that are J1 to J6 axes. The J1 to J6 axes can each rotate as indicated by a corresponding one of the arrows in FIG. 1. The robot 10 includes an encoder that is provided to each axis to serve as a detector that detects the position (angle) of the axis. With this configuration, the robot 10 can perform feedback control under the full-closed control scheme based on the detected position (detected angle) of each axis.

[0019] In a robot including a plurality of axes, due to the arm structure, some of the plurality of axes may be in a corotation relationship, meaning that rotation one of such axes causes rotation of another one of such axes. In this specification, the corotation relationship is assumed to include any of the cases where rotation of at least one of a plurality of axes causes rotation of at least one of the other axes. An example of a configuration of robot arms in the corotation relationship will be described as Reference Example with reference to FIG. 5. The robot arms illustrated in FIG. 5 include a first arm 331 and a second arm 332. A mechanism of such robot arms is used as a mechanism for an arm tip portion of an articulated robot for example. A servomotor 301 drives a B axis (corotation causing axis), and a servomotor 302 drives an A axis (corotated axis). When the servomotor 301 rotates, the first arm 331 rotates via gears 311a and 311b. Thus, the rotation of the B axis is controlled by the rotation of the servomotor 301. When the servomotor 302 rotates, an axis 321 rotates via gears 312a and 312b, and an axis 323 rotates via bevel gears 322a and 322b, resulting in rotation of the second arm 332. Thus, the rotation of the A axis is controlled by the rotation of the servomotor 302.

[0020] With this mechanism, the first arm 331 and the second arm 332 are connected to each other, meaning that the rotation of the first arm 331 about the B axis causes the rotation of the bevel gear 322b, causing the rotation of the second arm 332 about the A axis (i.e., corotation). Thus, the rotation of the servomotor 301 causes the rotation of the B axis involving the rotation of the first arm 331, causing the corotation of the second arm 332 about the A axis.

[0021] For convenience of understanding of the control on the axes under the full-closed control scheme taking into account the corotation according to the present embodiment, a Reference Example of a semi-closed control scheme using an encoder that detects the position (rotation angle)/speed of the motor and a typical full-closed control scheme using an encoder that detects the position of an arm (each axis) will be described with reference to FIG. 4A and FIG. 4B.

[0022] FIG. 4A illustrates a block diagram of a feedback control circuit for the semi-closed control scheme. As illustrated in FIG. 4A, a motor position command is input as a command to the feedback control circuit of the semi-closed control scheme. The motor position command is converted into a motor speed command by a position control unit 41 (Gp(s)). A speed control unit 42 (Gv(s)) outputs a command (such as a current command) for controlling a motor 43 according to the speed command. Note that in FIG. 4A (and also in other block diagrams), a robot mechanism is described as a “torsion system”. In the feedback control circuit of FIG. 4A, the motor speed is controlled by the feedback control of a minor loop, and the position control unit 41 executes the control by using a difference between a feedback signal of the motor position and the motor position command.

[0023] FIG. 4B illustrates a feedback control circuit for a typical full-closed control scheme. In the feedback control circuit of FIG. 4B, an arm (axis) position command is provided as a command, and a position control unit 51 (Gp(s)) converts the arm position command into a motor speed command. A speed control unit 52 (Gv(s)) outputs a command (such as a current command) to control the motor 53, according to the motor speed command. In the feedback control circuit of FIG. 4B, the motor speed is controlled by the feedback control of a minor loop, and the position control unit 51 executes the control by using a difference between a feedback signal of the arm (axis) position and the arm (axis) position command. It should be understood that the typical full-closed control scheme in FIG. 4B relates to a configuration in which the control is performed based only on the motor speed of a single motor 53 provided to the control target axis, meaning that the corotation relationship in which the position of the control target axis also depends on other axes is not taken into consideration.

[0024] The full-closed control scheme implemented by the robot controller 20 according to the present embodiment while taking into account the corotation will be described below with reference to FIG. 2 and FIG. 3. As illustrated in FIG. 2, the robot controller 20 includes a storage unit 32 configured to store an operation program and other various types of information related to the control on the robot 10, a position command calculating unit 27 configured to calculate a position (angle) command for each axis for controlling the operation of the robot 10, through kinematic calculations according to the operation program, and feedback control units 21 to 26 configured to execute the feedback control under the full-closed control scheme respectively for the J1 to J6 axes.

[0025] The robot controller 20 further includes a correction value calculating unit 28 configured to calculate a correction value for the speed command for the corotated axis, based on the corotation relationship. The functional blocks of the robot controller 20 such as the position command calculating unit 27, the feedback control units 21 to 26, and the correction value calculating unit 28 may be implemented by a CPU 31 of the robot controller 20 executing various types of software, or may be implemented by a configuration mainly including hardware such as an Application Specific Integrated Circuit (ASIC).

[0026] The robot 10 includes a J1 axis 111, a J2 axis 112, a J3 axis 113, a J4 axis 114, a J5 axis 115, and a J6 axis 116, respectively provided with encoders 121 to 126 serving as axis position detectors that detect the axis position (angle). The encoders 121 to 126 may be, for example, optical rotary encoders. The axis position information is fed back from the encoders 121 to 126 to the feedback control units 21 to 26 respectively. The J1 axis to the J6 axis are respectively provided with motors 211 to 216 that are servomotors. The motors 211 to 216 are respectively provided with encoders 221 to 226 as motor position detectors that detect the rotation positions. The encoders 221 to 226 may be optical rotary encoders for example. The position information and the speed information on the motors are fed back from the encoders 221 to 226 to the feedback control units 21 to 26 respectively.

[0027] A description will be given on how the full-closed control scheme is implemented while taking into account the corotation relationship when the axes of the robot 10 are in the corotation relationship. In the 6-axis robot, the arm (axis) position and the motor position are respectively defined as θ.sub.Jn(n=1, . . . , 6) and θ.sub.mn(n=1, . . . , 6). With this definition, an arm position θ.sub.Jn without the corotation is expressed as in Formula (1) below. Note that, in Formula (1), A, B, C, D, E, and F each represent a coefficient for converting the motor position θ.sub.mn(n=1, . . . , 6) into the arm position θ.sub.Jn(n=1, . . . , 6).

[00001]$\begin{array}{cc}\left[\mathrm{Math}.1\right]& \\ \left[\begin{array}{c}{\theta }_{J1}\\ 0_{J2}\\ {\theta }_{J3}\\ {\theta }_{J4}\\ {\theta }_{J5}\\ {\theta }_{J6}\end{array}\right]=\left[\begin{array}{cccccc}A& & & & & \\ & B& & & 0& \\ & & C& & & \\ & & & D& & \\ & 0& & & E& \\ & & & & & F\end{array}\right]\left[\begin{array}{c}{\theta }_{m1}\\ {\theta }_{m2}\\ {\theta }_{m3}\\ {\theta }_{m4}\\ {\theta }_{m5}\\ {\theta }_{m6}\end{array}\right]& \left(1\right)\end{array}$

[0028] In the above Formula (1), only the diagonal component in the right-side matrix is of a non-zero value, and anything other than the diagonal component is zero. Thus, the position of each of the axes depends only on the position of the motor for the axis. Thus, Formula (1) expresses a case where the axes are not in the corotation relationship.

[0029] The relationship between the arm position θ.sub.Jn and the motor position θ.sub.mn when there is the corotation relationship can be expressed as in Formula (2) below. In the right-side matrix of Formula (2), since there is a non-zero component other than the diagonal component, the position of a certain axis depends on the position of the motor provided to the axis, as well as the position of the motor provided to another axis. Note that if at least one of components other than the diagonal component is a non-zero component in the right-side matrix in Formula (2), the corotation relationship holds.

[00002]$\begin{array}{cc}\left[\mathrm{Math}.2\right]& \\ \left[\begin{array}{c}{\theta }_{J1}\\ 0_{J2}\\ {\theta }_{J3}\\ {\theta }_{J4}\\ {\theta }_{J5}\\ {\theta }_{J6}\end{array}\right]=\left[\begin{array}{cccccc}A& & & & & \\ & B& & & \begin{array}{c}\mathrm{NON}-\\ \mathrm{ZERO}\end{array}& \\ & & C& & & \\ & & & D& & \\ & \begin{array}{c}\mathrm{NON}-\\ \mathrm{ZERO}\end{array}& & & E& \\ & & & & & F\end{array}\right]\left[\begin{array}{c}{\theta }_{m1}\\ {\theta }_{m2}\\ {\theta }_{m3}\\ {\theta }_{m4}\\ {\theta }_{m5}\\ {\theta }_{m6}\end{array}\right]& \left(2\right)\end{array}$

[0030] As one example, a case is assumed where there is a corotation relationship in which the position of the J6 axis depends on the positions of the motors for the J4 axis and the J5 axis in the robot 10. In this case, the position of the J6 axis is expressed as in Formula (3) below. In the formula, α and β represent coefficients indicating impacts by the positions of the respective motors for the J4 axis and the J5 axis, on the position of the J6 axis.

[00003]$\begin{array}{cc}\left[\mathrm{Math}.3\right]& \\ \left[\begin{array}{c}{\theta }_{J1}\\ 0_{J2}\\ {\theta }_{J3}\\ {\theta }_{J4}\\ {\theta }_{J5}\\ {\theta }_{J6}\end{array}\right]=\left[\begin{array}{cccccc}A& & & & & \\ & B& & & \begin{array}{c}\mathrm{NON}-\\ \mathrm{ZERO}\end{array}& \\ & & C& & & \\ & & & D& & \\ & \begin{array}{c}\mathrm{NON}-\\ \mathrm{ZERO}\end{array}& & & E& \\ & & & \alpha & \beta & F\end{array}\right]\left[\begin{array}{c}{\theta }_{m1}\\ {\theta }_{m2}\\ {\theta }_{m3}\\ {\theta }_{m4}\\ {\theta }_{m5}\\ {\theta }_{m6}\end{array}\right]& \left(3\right)\end{array}$

[0031] Thus, in the case of the above example, the position of the J6 axis is expressed as in the following Formula (4).

[Math. 4]

θ.sub.J6=αθ.sub.m4+βθ.sub.m5+Fθ.sub.m6  (4)

[0032] The above Formula (4) is modified into the following Formula (5) expressing the position (angle) of the motor 216 for the J6 axis.

[00004]$\begin{array}{cc}\left[\mathrm{Math}.5\right]& \\ {\theta }_{m6}=\frac{{\theta }_{J6}}{F}-\frac{\alpha }{F}{\theta }_{m4}-\frac{\beta }{F}{\theta }_{m5}& \left(5\right)\end{array}$

[0033] By differentiating both sides of the above Formula (5), the following Formula (6) is obtained expressing the speed of the motor 216 for the J6 axis.

[00005]$\begin{array}{cc}\left[\mathrm{Math}.6\right]& \\ \frac{d{\theta }_{m6}}{\mathrm{dt}}=\frac{1}{F}.Math.\frac{d{\theta }_{J6}}{\mathrm{dt}}-\frac{\alpha }{F}.Math.\frac{d{\theta }_{m4}}{\mathrm{dt}}-\frac{\beta }{F}.Math.\frac{d{\theta }_{m5}}{\mathrm{dt}}& \left(6\right)\end{array}$

[0034] In Formula (6), the second term and the third term on the right side are correction values (Formula (7) below) to be applied to the speed command for the motor 216 for the J6 axis when the corotation relationship as indicated by Formula (4) holds.

[00006]$\begin{array}{cc}\left[\mathrm{Math}.7\right]& \\ \left(\mathrm{CORRECTION}\mathrm{VALUE}\right)=-\frac{\alpha }{F}.Math.\frac{d{\theta }_{m4}}{\mathrm{dt}}-\frac{\beta }{F}.Math.\frac{d{\theta }_{m5}}{\mathrm{dt}}& \left(7\right)\end{array}$

[0035] When the corotation relationship as indicated by Formula (4) holds, the correction value calculating unit 28 calculates the correction value as in Formula (7), and applies the calculated correction value to the feedback control unit (the feedback control unit 26 in this case) for the control target axis. Also in a case of a corotation relationship other than that in the example described above, the correction value calculating unit 28 calculates the correction value based on the correspondence relationship (Formula (4) in the above example) between the position of the control target axis that rotates depending on the corotation relationship and the position of the motor corresponding to at least one axis that makes the control target axis corotate, as in the example described above.

[0036] FIG. 3 is a block diagram illustrating feedback control under the full-closed control scheme for the J6 axis. The full-closed control scheme in FIG. 3 similarly applies to the other axes. As in the configuration illustrated in FIG. 4B, the feedback control circuit in FIG. 3 includes a position control unit 261 that calculates the speed command for the motor 216 based on the arm (axis) position command, and a speed control unit 262 that outputs a drive signal for driving the motor 216 according to the speed command. The position control unit 261 and the speed control unit 262 in FIG. 3 correspond to the feedback control unit 26 for the J6 axis in FIG. 2.

[0037] In the feedback control circuit in FIG. 3, the motor speed is controlled by the feedback control of a minor loop, and the position control unit 261 executes the control by using a difference between the arm position command and the axis position fed back. When the J6 axis is in the corotation relationship as expressed by the above Formula (4), the motor speed correction value as a correction value (Formula (7)) calculated by the correction value calculating unit 28 is added to the speed command value output from the position control unit 261, whereby the speed command is corrected. Thus, the speed control is implemented for the J6 axis while taking into account the corotation relationship. With this configuration, the feedback control unit 26 for the J6 axis can perform the speed control based on the speed command corrected based on the correction value calculated by the correction value calculating unit 28.

[0038] As described above, according to the present embodiment, even when axes of a robot are in the corotation relationship, the control taking into account the corotation relationship can be applied to the control for the control target axis, whereby the performance of the robot arm position control can be improved. In particular, the dynamic performance is improved for a case where a robot arm is operated with control performed on a plurality of axes including axes in the corotation relationship.

[0039] While the present invention has been described above by using typical embodiments, it is to be understood that those skilled in the art can make changes, various other modifications, omissions, and additions to each of the above embodiments without departing from the scope of the present invention.

[0040] The coefficients A to F, α, β, and the like between each of the axis positions and each of the motor positions described in the above embodiment may be stored in the storage unit 32 of the robot controller 20 in advance, or may be settable by the user using an operation section (not illustrated) of the robot controller 20. Alternatively, these coefficients may be input to the robot controller 20 from an external apparatus.

[0041] The configuration of the above embodiment can be applied to various machines including axes in a corotation relationship.

[0042] The functional configuration of the robot controller 20 illustrated in FIG. 2 is merely an example, and may be modified in various ways. For example, in the configuration example illustrated in FIG. 2, the robot controller 20 includes one correction value calculating unit 28, and the correction value calculating unit 28 calculates the correction value to be applied to the control on the control target axis, and the control value is applied to the feedback control unit for the control target axis. Instead of such a configuration, a configuration where each of the feedback control units 21 to 26 includes a function of the correction value calculating unit that calculates the correction value may be employed.

REFERENCE SIGNS LIST

[0043] 10 ROBOT [0044] 20 ROBOT CONTROLLER [0045] 21 TO 26 FEEDBACK CONTROL UNIT [0046] 27 POSITION COMMAND CALCULATING UNIT [0047] 28 CORRECTION VALUE CALCULATING UNIT [0048] 31 CPU [0049] 32 STORAGE UNIT [0050] 100 ROBOT SYSTEM [0051] 111 J1 AXIS [0052] 112 J2 AXIS [0053] 113 J3 AXIS [0054] 114 J4 AXIS [0055] 115 J5 AXIS [0056] 116 J6 AXIS [0057] 121 TO 126 ENCODER [0058] 211 TO 216 MOTOR [0059] 221 TO 226 ENCODER [0060] 261 POSITION CONTROL UNIT [0061] 262 SPEED CONTROL UNIT