JPH09212203A - Robot controller - Google Patents

Abstract

PROBLEM TO BE SOLVED: To suppress the vibration of a robot-side driving part and speed up the positioning of this driving part by providing a torsion angle and disturbance estimating means which estimates the angle of torsion and disturbance at the same time. SOLUTION: This controller is equipped with a torsion angle and disturbance observer 7, feedforward part 8, and a control part 9. Then the observer 7 estimates the angle and angular velocity of the 2nd arm of a robot to be controlled and the angular velocity and disturbance torque of the servomotor by constituting a minimum-dimensional observer for the robot. Consequently, the vibration of the robot-side 2nd arm is suppressed and the positioning of the robot-side 2nd arm is speeded up by performing control which is tolerant of disturbance over the 2-inertia robot which is low in rigidity. Therefore, the operation efficiency of the assembly, etc., of electronic components by the robot can be improved by this robot controller.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a robot controller suitable for use in a robot requiring high-speed positioning, such as an assembly robot.

[0002]

2. Description of the Related Art In a conventional robot controller, a control system called PID control has been used.
PID control is based on the proportionality (Proportion) and integration (Integrat) of the difference between the angle of the servomotor and the target angle.
ion) and time derivative (Differentiatio)
n).

As shown in FIG. 7, this robot control device controls a robot 122. In this robot 122, for example, as shown in FIG. 8, for example, an arm 124 as a drive unit, a servo motor 123 for driving this drive unit, and a coupling unit 125 such as a reduction gear or a belt.
One can be mentioned that is configured by being connected via. Although not shown, this robot 122
Is provided with an amplifier for generating a desired torque in the servo motor 123 and an angle detector for detecting the angle of the servo motor 123. Detection signal of the angle theta _m of the servo motor 123 detected by the angle detector is fed back to the robot controller 120. In addition,
In the drawing of FIG. 8, θ _a is the angle of the arm 124.

[0004] The robot controller 120 has a controller 121 as a main component. This control unit 12
The 1, signals indicating the target angle theta _r of the servo motor 123 is supplied via the terminal 101, In addition, the robot 1
Signal indicating the angle theta _m of the servo motor 123 is detected at 22 of the angle detector is also supplied. The control unit 121
Calculates the command torque τ applied to the amplifier from the angle θ _m of the servo motor 123 and the target angle θ _r of the servo motor 123 by the following equation (25).

[0005]

[Equation 17]

In the equation (25), k _p is a proportional parameter, k _i is an integral parameter, and k _d is a time derivative parameter.

The control unit 121 gives the command torque τ obtained by the above calculation to the amplifier, thereby
2 is controlled.

[0008]

By the way, in the robot 122, as shown in FIG. 8, the servo motor 123 and the arm 124 are connected to each other through a connecting portion 125 such as a speed reducer or a belt. It is what The joint 125 functions as a spring element because of its low rigidity, and the structure of FIG. 8 is modeled by replacing the joint 125 with a spring. Therefore, the robot control device 120 controls a two-inertia system in which the servomotor 123 and the arm 124 are connected via the spring 125.

On the other hand, the PID control used in the conventional robot control device 120 effectively controls a control object having high rigidity in which the function as the spring 125 element of the connecting portion can be ignored. Therefore, the PID control cannot effectively control a control target of the two-inertia system in which the rigidity of the joint is low.

That is, in the PID control, there is a problem in that the tip of the arm 124 vibrates when the robot 122 having a low rigidity at the joint portion is positioned at a higher speed. As a means for solving the problem that the tip of the arm 124 vibrates, for example, increasing the acceleration / deceleration time of the arm 124 can be considered.
However, this makes it difficult to speed up the operation of the arm 124. As means for solving such a problem that the acceleration / deceleration time of the arm 124 is prolonged, for example, an angular acceleration detector for detecting the angle of the arm 124 and the angular acceleration of the arm 124 is provided. It is conceivable that the arm 124 is attached to the tip, the position of the tip of the arm 124 is directly detected by the angular acceleration detector, and control is performed using the detected output. However, in this case, an increase in the cost for the angular acceleration detector and a deterioration in the maintenance performance are caused.

As a means for solving the above-mentioned problem, there is a "method of controlling a servo loop of an industrial robot" described in Japanese Patent Application Laid-Open No. 1-29301. The disturbance cannot be suppressed because the estimated disturbance is not used as a feedback element, and there is no freedom in the arrangement of zeros because there is no feedforward element. I can't get it.

Therefore, in order to solve the above-mentioned problems, the present invention aims to provide a robot control device for suppressing the vibration of a drive unit such as an arm and for accelerating the positioning of the drive unit. One purpose.

Further, generally, the servo motor and the amplifier are
There is a maximum torque, which is the limit that each can generate. The conventional robot control device 120 suppresses the target angular acceleration so that the command torque from the control unit 121 does not exceed the maximum torque. However, if the command torque given to the robot 122 is smaller than the maximum torque, it becomes difficult to increase the speed of the arm 124.

Further, although there is a case where a function of controlling torque is usually provided in the amplifier, in this case, the command torque calculated from the control unit 121 and the actually generated torque match. And no good control result can be obtained. Further, even if the command torque is simply limited, the calculated command torque and the actually generated torque do not match as described above, and a good control result cannot be obtained.

Therefore, in order to solve the above-mentioned problems, the present invention realizes a good control result while limiting the command torque, and at the same time, effectively uses the capabilities of the servomotor and the amplifier to drive the motor. Another object of the present invention is to provide a robot control device that enables high-speed positioning of parts.

[0016]

A robot controller according to the present invention detects a drive unit such as an arm, a servo motor for driving the drive unit, an amplifier for generating torque in the servo motor, and an angle of the servo motor. The control target is a robot provided with the angle detection means. The robot controller
Based on the command torque given to the amplifier and the angle of the servo motor detected by the angle detection means, the angle of the robot drive unit, the angular velocity of the robot drive unit, which is the time derivative of the robot drive unit angle, and the servo motor And a torsion angle / disturbance estimating means for estimating the angular velocity of the servo motor, which is a time differential value of the angle, and the disturbance torque applied to the robot as a disturbance. The robot controller also includes a feedforward unit that calculates the target angular velocity of the servomotor based on the target angle of the servomotor. Further, the robot controller is configured such that the target angle of the servo motor, the angle of the servo motor detected by the angle detection means, the target angular velocity of the servo motor calculated by the feedforward means, and the robot estimated by the twist angle / disturbance estimation means. And the control means for calculating the command torque to be given to the amplifier based on the estimated angle of the drive unit, the estimated angular velocity of the robot drive unit, the estimated angular velocity of the servo motor, and the estimated disturbance torque. .

That is, according to the robot controller of the present invention having the above-described structure, the feedforward means calculates the target angular velocity of the servomotor based on the target angle of the servomotor and outputs it to the control means. To do. The torsion angle / disturbance estimation means, based on the command torque and the angle of the servo motor, the estimated angle of the robot drive unit, the estimated angular velocity of the robot drive unit, the estimated angular velocity of the servo motor,
The estimated disturbance torque is calculated and output to the control means. The control means is the target angle of the servo motor, the angle of the servo motor,
Based on the target angular velocity of the servo motor, the estimated angle of the robot drive unit, the estimated angular velocity of the robot drive unit, the estimated angular velocity of the servo motor, and the estimated disturbance torque, the command torque given to the amplifier is calculated and output to the robot amplifier. .
The robot control device controls the robot based on the command torque calculated by the control means.

Further, the robot controller according to the present invention is
A control target is a robot including a drive unit such as an arm, a servo motor that drives the drive unit, an amplifier that generates torque in the servo motor, and an angle detection unit that detects an angle of the servo motor. The robot control device drives the robot based on the command torque given to the amplifier and the angle of the servo motor detected by the angle detection means, which is the time differential value of the angle of the robot drive unit and the robot drive unit. And a torsion angle / disturbance estimating means for estimating the angular velocity of the portion, the angular velocity of the servo motor, which is the time differential value of the angle of the servo motor, and the disturbance torque applied to the robot as a disturbance. The robot controller also includes a feedforward unit that calculates the target angular velocity of the servomotor based on the target angle of the servomotor. Further, the robot control device is configured such that the target angle of the servo motor, the angle of the servo motor detected by the angle detection means, the target angular velocity of the servo motor calculated by the feed forward means, and the robot angle estimated by the twist angle / disturbance estimation means. A control unit is provided that calculates a calculation torque based on the estimated angle of the drive unit, the estimated angular velocity of the drive unit of the robot, the estimated angular velocity of the servo motor, and the estimated disturbance torque. Further, the robot control device includes torque limit means for calculating a command torque to be given to the amplifier limited by a preset torque limit value based on the calculated torque calculated by the control means. To solve.

That is, according to the robot controller of the present invention having the above-described structure, the feedforward means calculates the target angular velocity of the servomotor based on the target angle of the servomotor and outputs it to the control means. To do. The torsion angle / disturbance estimation means, based on the command torque and the angle of the servo motor, the estimated angle of the robot drive unit, the estimated angular velocity of the robot drive unit, the estimated angular velocity of the servo motor,
The estimated disturbance torque is calculated and output to the control means. The control means is the target angle of the servo motor, the angle of the servo motor,
A calculated torque is calculated based on the target angular velocity of the servo motor, the estimated angle of the robot drive unit, the estimated angular velocity of the robot drive unit, the estimated angular velocity of the servo motor, and the estimated disturbance torque, and outputs the calculated torque to the torque limit unit. The torque limit means outputs the command torque calculated by the calculated torque and the torque limit value to the amplifier of the robot. The robot control device controls the robot based on the command torque calculated by the torque limit means.

[0020]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS Specific embodiments of the present invention will be described below in detail with reference to the drawings of FIGS.

First, as shown in FIG. 1, a robot control apparatus 1 shown as a first embodiment of the present invention controls an assembling robot 2 used for mounting components on a board, for example. .

The first arm drive unit 21 of the robot 2 shown in FIG.
Reference numeral 3 denotes a drive motor for driving the first arm 203 and a speed reduction mechanism connected to an output shaft of the drive motor. The first arm drive unit 213 is fixed to the base 205 via the amplifier unit 5 described later, and transmits the rotational force to the rotation shaft, thereby enabling the entire first arm 203 to rotate. The first arm drive section 213 and the amplifier section 5 are covered by a cover 206.

The second arm drive unit 207 of the robot 2 is
The servo motor 4 drives the second arm 3 via a drive belt 200 made of, for example, a steel belt, and a speed reduction mechanism 215 connected to the motor output shaft. The second arm drive unit 207 includes the first arm drive unit 21
Similarly to 3, the base 205 is fixed in the rotation direction. Further, the support portion 204 is provided with the cover 20 so that the servomotor 4 itself of the second arm driving portion 207 does not rotate.
6 for fixing to the base 205. The driving force of the servo motor 4 is controlled by the speed reduction mechanism 21.
5, the rotation is transmitted to the belt drive pulley 202 via the rotation shaft 208, and the pulley 202 is rotated. The first arm drive unit 2
13 and the second arm drive unit 207 are arranged corresponding to one end of the first arm 203.

The drive belt 200 is suspended between the belt drive pulley 202 and the second shaft drive pulley 201,
The driving force generated by the second arm driving unit 207 is transmitted to the second shaft driving pulley 201. The belt driving pulley 202, the driving belt 200, and the second shaft driving pulley 201
3 are arranged inside.

The second shaft drive pulley 201 is arranged corresponding to the other end of the first arm 203, and has a rotation shaft 21.
4 is rotatable around the center of rotation.

The central portion of the second arm 3 is fixed to the second shaft drive pulley 201. Therefore, the second
When the shaft drive pulley 201 is rotated by the second arm drive unit 207 via the drive belt 200, the second drive
The arm 3 is driven with the rotation of the second shaft drive pulley. Further, at one end of the second arm 3, an operation unit 210 for loading components on the above-described substrate or the like is disposed via an operation shaft 211. The operation shaft 211 is a so-called tool mounting shaft, and is capable of moving in a vertical direction (Z-axis direction) and rotating in a direction around the axis (R-axis direction). Therefore, the operating part 210 disposed at the tip of the working shaft 211 is
The components are driven freely in the rotational direction, so that components can be freely loaded on the substrate.

As described above, the robot 2 has the first arm 203 and the second arm 3, and has a two-inertia system configuration in which the second arm 3 is driven via the drive belt 200. That is, the robot 2 mechanically cancels the interference force such as the reaction force, Coriolis, and centrifugal force generated in the conventional robot 120 described above, and the low-rigidity two-inertia system in the robot controller 1. Is controlled.

In addition to the structure described above, the robot 2 is provided with the amplifier section 5 for generating a desired torque with respect to the servomotor 4 for driving the second arm 3, and further the servomotor 4 is provided. An angle detector 6 including, for example, a rotary encoder for detecting the angle is also provided. A signal indicating the command torque τ calculated by the robot controller 1 is transmitted and supplied to the amplifier unit 5 of the robot 2 according to the embodiment of the present invention via the code 212. Generates the desired torque when driving the servomotor 4 based on the value of the command torque τ.

Here, the robot 2 has a command torque τ, an angle θ _{m of} the servo motor 4, an angular velocity θ _m ^′ of the servo motor 4, and a second arm 3 as shown in the following equation (1).
Θ _a , the angular velocity θ _a ^′ of the second arm 3, and the disturbance torque d applied to the motor shaft of the servomotor 4 are represented by state equations. Note that J _m in the equation (1) is the inertia of the servo motor 4, D
_m is the viscous friction of the servomotor 4, J _a is the inertia of the second arm 3, D _a is the viscous friction of the second arm 3, K is the spring constant, and N is the gear ratio.

[0030]

(Equation 18)

That is, in the robot 2, the above equation (1)
The servo motor 4 drives the second arm 3 based on the dynamics represented by.

Here, as shown in FIG. 2, the robot control device 1 of the first embodiment has a twist angle / disturbance estimation observer 7, a feedforward unit 8, and a control unit 9 as main components. A signal indicating a desired target angle θ _r of the servo motor 4 is input to the terminal 20 from, for example, a system controller that controls the whole.

The torsion angle / disturbance estimation observer 7 is
As shown in FIG. 2, by configuring a minimum-dimensional observer for the robot 2 to be controlled, the angle θ _a of the second arm 3 of the robot 2, the angular velocity θ _a ^′ of the second arm 3 of the robot 2, the servo, The angular velocity θ _m ^' of the motor 4 and the disturbance torque d are estimated.

That is, the torsion angle / disturbance estimation observer 7 receives the command torque τ and the actual angle θ _m of the servo motor 4 detected by the angle detector 6, and formulas (2) and (3) are given. , The estimated angle θ _a of the second arm 3 of the robot 2, the estimated angular velocity θ _a ^′ of the second arm 3,
And the estimated angular velocity θ _m ^′ of the servomotor 4 and the estimated disturbance torque as a value added to the robot 2 as a disturbance.
Calculate d . The estimated angle θ _a of the second arm 3, the estimated angular velocity θ _a ^′ of the second arm 3, the estimated angular velocity θ _m ^′ of the servo motor 4, and the estimated disturbance torque d are sent to the control unit 9.

[0035]

[Equation 19]

Here, z in equations (2) and (3)
₁ , z ₂ , z ₃ and z ₄ are state variables of the minimum dimensional observer, and k ₁ , k ₂ , k ₃ and k ₄ are parameters that determine the estimated speed. Further, the estimated angular velocity θ _a ^′ of the second arm 3 of the robot 2 may be obtained by time differentiation of the estimated angle θ _a of the second arm 3. Further, the estimated angular velocity θ _m ^′ of the servo motor 4 of the robot 2 may be obtained by time differentiation of the estimated angle θ _m of the servo motor 4 of the robot 2.

In the embodiment of the present invention, the inertia J _m of the servomotor 4 is 1.7 × 10 ⁻⁴ [kgm.
² ], the viscous friction D _m of the servomotor 4 is 1.1 × 10 ⁻⁴
[Nm / rad / s], inertia J _{a of} the second arm 3
The 0.76 [kgm ^2], the viscous friction D _a of the second arm 3 5 [Nm / rad / s ], the spring constant K 8300
[Nm / rad], and the gear ratio N is 50.

Next, as shown in FIG. 2, the feedforward section 8 is inputted with a desired target angle θ _r of the servomotor 4 supplied to the terminal 20, and the target angle of the servomotor 4 is inputted. Servo motor 4 by differentiating θ _r with respect to time
The target angular velocity θ _r ^'of is calculated and output. Here, the target angular velocity θ _r ^′ of the servo motor 4 may be obtained by the difference between the angular velocity θ _m ^′ of the servo motor 4 and the desired target angle θ _r of the servo motor 4. The target angular velocity θ _r ^′ of the servo motor 4 may be calculated at the same time when the target angle θ _r of the servo motor 4 is calculated.
When the feed forward unit 8 obtains the target angular velocity θ _r ^′ of the servo motor 4 by the difference from the desired target angle θ _r of the servo motor 4, From the twist angle / disturbance estimation observer 7 to the angular velocity θ of the servo motor 4
supply _m ^' . The value of the target angular velocity θ _r ^′ of the servomotor 4 output from the feedforward unit 8 is sent to the calculation unit 21 provided in the subsequent stage of the control unit 9.

As shown in FIG. 2, the control unit 9 has terminals 2
The value of the target angle θ _r of the servo motor 4 from 0, the estimated angle θ _a of the second arm 3 from the torsion angle / disturbance estimation observer 7, the estimated angular velocity θ _a ^′ of the second arm 3, and the estimation of the servo motor 4 The angular velocity θ _m ^′ , the estimated disturbance torque d , and the value of the angle θ _m of the servo motor 4 detected by the angle detector 6 are input.

In the control unit 9 and the arithmetic unit 21 in the subsequent stage, the servo motor 4 is used by using the supplied values.
Angle theta _m and the difference between the estimated angle theta _a second arm 3, the difference between the ^'estimated angular velocity theta _a and the second arm ^3' estimated angular velocity theta _m of the servo motor 4, the servo motor 4 target angular velocity theta _r obtaining ^'and the estimated angular velocity theta _m of the servo motor ^4' difference between, the servo motor 4 target angle theta _r and a difference between the angle theta _m of the servo motor 4, respectively.

Then, in the control unit 9 and the arithmetic unit 21 in the subsequent stage, the angle θ _m of the servo motor 4 and the estimated angle of the second arm 3 obtained as described above are calculated by the following equation (4).
difference between theta _a, the difference between the ^'estimated angular velocity theta _a and the second arm ^3' estimated angular velocity theta _m of the servo motor 4, the servomotor 4
Difference between the target angular velocity θ _r ^' and the estimated angular velocity θ _m ^' of the servo motor 4, the difference between the target angle θ _r of the servo motor 4 and the angle θ _m of the servo motor 4, the target angular velocity θ of the servo motor 4
_A command torque τ is calculated and output from _r ^′ and the estimated disturbance torque d .

[0042]

(Equation 20)

Note that f ₁ , f ₂ , and
f ₃ and f ₄ are control parameters that determine the control characteristics.

The command torque τ obtained as described above is supplied to the robot 2 to be controlled.

Here, the part of the robot 2 in FIG. 2 can be expressed in more detail as shown in FIG. That is, this FIG. 3 shows the operation of each part of the robot 2 as a block diagram, and the same reference numerals are given to the components corresponding to those in FIG.

In FIGS. 3 and 2, the value of the command torque τ from the robot controller 1 is transmitted to the terminal 31. The value of the command torque τ transmitted to the terminal 31 is input to the arithmetic unit 33 as an added value. Further, the disturbance torque d is transmitted from the terminal 30 to the arithmetic unit 33 as a subtraction value. That is, the computing unit 33 subtracts the amount of influence of the disturbance torque d on the command torque τ. The output value of the calculation unit 33 is input to the calculation unit 34 as an addition value.

The calculation unit 34 includes a calculation unit 24 which will be described later.
Is output as a subtraction value, and the arithmetic unit 34 obtains a value obtained by subtracting the output value of the arithmetic unit 24 from the output of the arithmetic unit 33. The output of the operation unit 34 is
To enter. Note that the calculation units 33 and 34 correspond to the calculation unit 22 in FIG.

The arithmetic unit 23 is composed of arithmetic units 35 and 36 and corresponds to the servomotor 4. In the arithmetic unit 23, from the inertia J _m and viscous friction D _m of the servo motor 4 of the servo motor 4 by the computing unit 35, 1 /
The calculation represented by (J _m S + D _m ) is performed.
In step 6, a calculation for integrating the output value of the calculation unit 35 is performed.
The value obtained by these operations {1 / (J _m S + D _m ) S} is output. That is, in the arithmetic unit 23, the output value of the arithmetic unit 35 is the angular velocity θ _m ^{′ of the} servo motor 4.
Corresponding to the output value of the calculation unit 36 corresponds to the speed theta _m of the servo motor 4. Note that S in the above equation is a Laplace transform operator.

From the above, the blocks from the arithmetic unit 22 to the arithmetic unit 23 drive the actual servomotor 4 based on the command torque τ in the robot 2 to be controlled, and drive the servomotor 4. Corresponding to the angle detection operation of the angle detector 6 according to. The angle θ of the servo motor 4 detected by the angle detector 6
_m is fed back to the robot controller 1 via the terminal 39.

The output value of the arithmetic unit 23 is transmitted to the arithmetic unit 25 and is multiplied by 1 / N in the arithmetic unit 25. That is, N is the gear ratio, and the arithmetic unit 25
Corresponds to the speed reduction mechanism 215 connected to the motor output shaft of the servo motor 4. The output value of the arithmetic unit 25 corresponds to the angle theta _a second arm 3, this value is transmitted as an addition value to the arithmetic unit 26.

The configuration from the arithmetic unit 26 to the arithmetic unit 28 corresponds to the second arm 3 of the robot 2. To the arithmetic unit 26, the actual angle theta _a second arm 3 of the robot 2 that is transmitted from the arithmetic unit 28 to be described later, is supplied as a subtraction value. That is, in the arithmetic unit 26, the actual angle θ of the second arm 3 transmitted from the arithmetic unit 28
_The angle change amount when changing _a to the angle θa transmitted from the arithmetic unit 25 is obtained.

The output value of the arithmetic unit 26 is transmitted to the arithmetic unit 27, where it is multiplied by the spring constant K, and is further multiplied by 1 / N in the arithmetic unit 24 to obtain the arithmetic unit 34. Is transmitted as a subtracted value to. That is, the arithmetic units 27 and 24 convert the value of the angle of the servomotor 4 into the value of the torque of the servomotor 4. As a result, the arithmetic unit 34 obtains a value obtained by subtracting the current torque value of the servo motor 4 from the torque value obtained by the arithmetic unit 33.

The output value of the arithmetic unit 27 is also transmitted to the arithmetic unit 28. The operation unit 28 includes operation units 37 and 38. In the arithmetic unit 28, from the viscous friction D _a of inertia J _a and the second arm 3 of the second arm 3 by the computing unit 37 performs computation expressed by _{1 / (J a S + D} a), further operation In the section 38, calculations for integrating the output value of the calculation section 37 are performed, and these calculations ｛1 / (J _a S + D _a )
The value obtained by S} is output. That is, in the arithmetic unit 28, the output value of the arithmetic unit 37 corresponds to the angular velocity θ _a ^′ of the second arm 3, and the output value of the arithmetic unit 38 corresponds to the angle θ _a of the second arm 3. .

The actual angle θ _a of the second arm 3 obtained by the calculation unit 28 is taken out as a force acting from the terminal 29 to the outside.

As described above, the robot controller 1 according to the first embodiment of the present invention is provided with the torsion angle / disturbance estimating means for simultaneously estimating the torsion angle and the disturbance. Since the robot 2 is strongly controlled against disturbance, the vibration of the second arm 3 on the robot 2 side can be suppressed and the positioning speed of the second arm 3 on the robot 2 side can be increased. Therefore, according to the robot control device 1, work efficiency such as assembling of electronic parts by the robot 2 can be improved.

In the example of FIG. 2, in order to facilitate the explanation, the robot controller 1 is provided with a twist angle
A disturbance estimation observer 7, a feedforward unit 8,
Although it is divided into main operation means with the control unit 9 for convenience, these operation means do not necessarily have to be divided as described above, and may be common operation means or other operation means not belonging to these. May be included. In addition, the robot control apparatus 1 according to the embodiment of the present invention may use a dedicated calculation unit instead of each calculation unit, and may use the entire calculation unit using a calculation device such as a personal computer.

Next, FIG. 4 shows a robot controller 1 according to a second embodiment of the present invention. As shown in FIG. 4, the robot control device 1 of the second embodiment has the same basic configuration as the robot control device of the first embodiment, but the torsion angle / disturbance estimation observer 11 And the calculation means of the feed-forward unit 12 and the control unit 13 are different. In the following description, the same members and the same components as those of the first embodiment device described above will be denoted by the same reference numerals and will not be described.

In the robot controller 1 according to the second embodiment shown in FIG. 4, the torsion angle / disturbance estimation observer 11 constitutes the minimum dimension observer 11 for the robot 2 to be controlled, and the above-mentioned state equation is applied to the observer 11. Based on
Based on the command torque τ given to the amplifier unit 5 on the robot 2 side and the angle θ _m of the servo motor 4 detected by the angle detector 6 on the robot 2 side, the twist angle θ _t , the twist angular velocity θ _t ^′ , the servo The angular velocity θ _m ^' of the motor 4 and the disturbance torque d are estimated.

That is, the torsion angle / disturbance estimation observer 11 receives the command torque τ and the angle θ _m of the servo motor 4 detected by the angle detector 6. The torsion angle / disturbance estimation observer 11 calculates the estimated angle θ _a of the second arm 3 of the robot 2 and the estimated angular velocity of the arm 3 on the robot 2 side according to the following equations (6) and (7).
θ _a ^' and the estimated angular velocity θ _m ^' of the servo motor 4 are calculated,
Further, the estimated disturbance torque d is calculated as a value applied to the robot 2 as a disturbance. Further, the torsion angle / disturbance estimation observer 11 uses the following equation (8) to calculate the servo motor 4
Of the difference between the estimated angle theta _a ^'of the arm 3 of the angle theta _m and the robot 2 side calculates a twist estimated angle theta _t, also,
Using the following formula (9), the estimated angle θ _{m of the} servo motor 4
And the estimated angular velocity θ _a ^′ of the arm 3 on the side of the robot 2 is calculated to calculate the twist estimated angular velocity θ _t ^′ .

That is, the torsion angle / disturbance estimation observer 11 uses the following equations (6), (7), (8) and (9) to indicate the command torque τ and the angle θ of the servomotor 4. By substituting _m , the estimated twist angle θ _t , the estimated twist angular velocity θ _t ^′ , the estimated angular velocity θ _m ^′ of the servomotor 4 and the estimated disturbance torque d are calculated and output.

[0061]

(Equation 21)

In the above equations (6) and (7), z
₁ , z ₂ , z ₃ and z ₄ are state variables of the minimum dimension observer 11, and k ₁ , k ₂ , k ₃ and k ₄ are parameters that determine the estimated speed.

Here, the twist estimated angular velocity θ _t ^' may be calculated by the time derivative of the twist angle θ _t . Also, the estimated angular velocity θ _a ^' of the second arm 3 of the robot 2
May be obtained by time differentiation of the estimated angle θ _a of the second arm 3 of the robot 2. Further, the estimated angular velocity θ _m ^′ of the servo motor 4 of the robot 2 may be obtained by time differentiation of the estimated angle θ _m of the servo motor 4 of the robot 2.

Next, as shown in FIG. 4, the desired target angle θ _r of the servomotor 4 is input to the feedforward unit 12 via the terminal 40. The feed forward unit 12 time-differentiates the target angular velocity θ _r ^′ of the servo motor 4 to obtain the target angular acceleration θ _r ^″ of the servo motor 4. Then, the feed forward unit 12 uses the following equation (10). On the other hand, the target angular velocity θ _r ^′ of the servo motor 4 and the target angular acceleration θ _r ^“ of the servo motor 4 are substituted, and the feedforward value ff is calculated and output.

[0065]

(Equation 22)

Ff ₁ and ff ₂ in the above equation (10)
Is the feedforward parameter. However, since the target angular acceleration θ _r ^" of the servomotor 4 is not used in the robot control device 1 of the second embodiment,
The feedforward parameter ff _{2 is set} to zero.

Here, the target angular velocity θ _r ^' of the servo motor 4
Is the angular velocity θ _m ^' of the servo motor 4 and the servo motor 4
It may be obtained by the difference from the desired target angle θ _r . Also, the target angular velocity θ of this servo motor 4
_{The r} ^′ may be obtained at the same time when the target angle θ _r of the servo motor 4 is obtained. In the feedforward unit 12, when the target angular velocity θ _r ^′ of the servo motor 4 is obtained by the difference from the desired target angle θ _r of the servo motor 4, The torsion angle / disturbance estimation observer 11 supplies the angular velocity θ _m ^′ of the servo motor 4.
Further, the target angular acceleration θ _r ^" of the servo motor 4 is obtained by differentiating the angular velocity θ _m ^' of the servo motor 4 with respect to time.
The angular acceleration θ _m ^" of the servo motor 4 may be obtained and the difference from the angular acceleration θ _m ^" of the servo motor 4 may be obtained. The target angular velocity θ _r ^′ of the servo motor 4 and the servo motor 4
Target angular acceleration theta _r of ^"the servo motor 4 target angle theta _r
May be requested at the same time as.

Next, as shown in FIG. 4, the control unit 13 causes the desired target angle θ _{r of} the servo motor 4 and the angle θ of the servo motor 4 detected by the angle detector 6 on the robot 2 side. _m , the feedforward value ff calculated by the feedforward unit 12, the twist estimation angle θ _t calculated by the twist angle / disturbance estimation observer 11, the twist estimation angular velocity θ _t ^′ , and the servo motor 4 estimated angular velocity θ _m ^′. , The estimated disturbance torque d are input. Based on these, the control unit 13 calculates the difference between the target angle θ _r of the servo motor 4 and the angle θ _m of the servo motor 4, the target angular velocity θ _r ^′ of the servo motor 4 and the estimated angular velocity θ _m ^′ of the servo motor 4. Find the difference between and. Then, the control unit 13 uses the following equation (11) to estimate the twist angle θ _t and the estimated twist angular velocity θ.
_t ^' , estimated disturbance torque d , target angle θ _{r of} servo motor 4
And the angle θ _m of the servo motor 4, the servo motor 4
The difference between the target angular velocity θ _r ^′ and the estimated angular velocity θ _m ^′ of the servo motor 4 is substituted to calculate the command torque τ, and this estimated torque τ is output to the amplifier unit 5.

[0069]

(Equation 23)

Note that f ₁ , f in the above equation (11)
₂ , f ₃ and f ₄ are control parameters for determining control characteristics.

The command torque τ obtained as described above is supplied to the robot 2.

The robot 2 is represented by a state equation similar to that described above. The command torque τ calculated by the control unit 13 of the robot controller 1 is used to determine the angle θ _{m of} the servo motor 4 and the servo motor 4. Angular velocity θ _m ^′ , angle θ _a of arm 3, angular velocity θ _a ^{′ of} arm 3, disturbance torque d
The servo motor 4 drives the second arm 3 on the basis of the dynamics in which is a state variable. The angle detector 6 of the robot 2 detects the angle θ _m of the servo motor 4 and outputs it to the torsion angle / disturbance estimation observer 11 and the control unit 13.

According to the robot controller 1 of the second embodiment described above, since the torsion angle / disturbance estimating means for simultaneously estimating the torsion angle and the disturbance is provided, it is possible to reduce the rigidity of the robot.
Since the inertial robot 2 is strongly controlled against disturbance,
Vibration of the second arm 3 on the robot 2 side can be suppressed, and positioning of the second arm 3 on the robot 2 side can be speeded up. Further, according to the robot control device 1, work efficiency such as assembling of electronic parts by the robot 2 can be improved.

In the example of FIG. 4, in order to facilitate the description, the robot controller 1 is provided with a twist angle
Disturbance estimation observer 11 and feedforward unit 12
, And the control unit 13 are divided into main operation means for convenience. However, these operation means do not necessarily have to be divided as described above, and may be common operation means or other operation means not belonging to them. It may have a calculation means. Further, the robot control device 1 according to the second embodiment of the present invention may use dedicated arithmetic means instead of the arithmetic means, for example, the entire arithmetic means by an arithmetic device such as a personal computer. good.

Next, a robot controller shown as a third embodiment of the present invention will be described. As shown in FIG. 5, the robot control device of the third embodiment has a configuration in which a torque limit unit 91 is added to the robot control device 1 of the first embodiment of FIG. The robot 2 is for controlling the same robot 2 as that shown in FIG. 1 for assembly used when mounting components.

As shown in FIG. 5, the robot controller 1 according to the third embodiment has a torsion angle / disturbance estimation observer 7 and a feedforward unit 8 as main components.
And a control unit 90 and a torque limit unit 91.

The torsion angle / disturbance estimation observer 7 and the feedforward unit 8 are the same as those of the robot controller 1 of the first embodiment described above, and the detailed description is omitted here.

In the control unit 90, the value of the target angle θ _r of the servo motor 4 from the terminal 20, the estimated angle θ _a of the second arm 3 from the torsion angle / disturbance estimation observer 7 and the estimation of the second arm 3 are estimated. Angular velocity θ _a ^′ , estimated angular velocity θ of servo motor 4
The respective values of _m ^′ and the estimated disturbance torque d , and the value of the angle θ _m of the servo motor 4 detected by the angle detector 6 are input.

The control unit 90 and the arithmetic unit 21 in the subsequent stage
Then, using the respective supplied values, the difference between the angle θ _m of the servo motor 4 and the estimated angle θ _a of the second arm 3,
The difference between the ^'estimated angular velocity theta _a and the second arm ^3' estimated angular velocity theta _m of the servo motor 4, the target angular speed of the servo motor 4 theta _r ^'
And the estimated angular velocity θ _m ^′ of the servo motor 4, and the difference between the target angle θ _r of the servo motor 4 and the angle θ _m of the servo motor 4 are obtained. Then, in the control unit 9 and the arithmetic unit 21 in the subsequent stage, the difference between the angle θ _m of the servo motor 4 obtained as described above and the estimated angle θ _a of the second arm 3 by the following equation (15), the difference between the ^'estimated angular velocity theta _a and the second arm ^3' estimated angular velocity theta _m of the servo motor 4, the difference between the ^'estimated angular velocity theta _m of the servo motor ^4' target angular velocity theta _r of the servo motor 4, the servomotor 4 The calculated torque τ _e is calculated from the difference between the target angle θ _r and the angle θ _m of the servo motor 4, the target angular velocity θ _r ^′ of the servo motor 4, and the estimated disturbance torque d .

[0080]

(Equation 24)

Note that f ₁ , f in the above equation (15)
₂ , f ₃ and f ₄ are control parameters for determining control characteristics.

The calculated torque τ _e obtained by the control unit 90 as described above is sent to the torque limit unit 91.

The torque limit unit 91 uses the following equation (16) to calculate the torque τ _e calculated by the control unit 90 and the preset torque limit value τ _l based on the amplifier unit of the robot 2. A command torque τ similar to the above given to 5 is calculated.

[0084]

(Equation 25)

The command torque τ calculated by the torque limit unit 91 is transmitted to the robot 2. The part of the robot 2 in FIG. 5 is described in more detail in the same manner as in FIG. 3 described above, and a detailed description thereof will be omitted.

According to the robot control apparatus 1 of the third embodiment described above, the provision of the torque limit section 91 allows the servo motor 4 and the amplifier section 5 to comply with the torque limitation by the servo motor 4 and the amplifier section 5. It is possible to sufficiently bring out the capability of No. 5 and thereby speed up the positioning of the second arm 3 on the robot 2 side. Therefore, according to the robot control device 1, work efficiency such as assembling of electronic parts by the robot 2 can be improved.

In the example of FIG. 5 as well, in order to facilitate the explanation, the robot controller 1 is provided with a twist angle
A disturbance estimation observer 7, a feedforward unit 8,
Although the control unit 90 and the torque limit unit 91 are divided into main arithmetic means for convenience, these arithmetic means do not necessarily have to be divided as described above, and may be a common arithmetic means or further divided into them. You may have the other arithmetic means which does not belong. In addition, the robot control apparatus 1 according to the embodiment of the present invention may use a dedicated calculation unit instead of each calculation unit, and may use the entire calculation unit using a calculation device such as a personal computer.

Next, FIG. 6 shows a robot controller 1 according to a fourth embodiment of the present invention. As shown in FIG. 6, the robot control device of the fourth embodiment has a configuration in which a torque limit unit 92 is added to the robot control device 1 of the second embodiment shown in FIG. The robot 2 controls the same robot 2 as that shown in FIG. 1 for assembly used when mounting components on a board. That is, as shown in FIG. 6, the robot control device 1 according to the fourth embodiment has the same basic configuration as the robot control device according to the second embodiment, except that the control unit 93 and the torque are controlled. The limit part 92 is characterized in that it is different. In the following description, the same members and the same components as those of the above-described second embodiment device will be denoted by the same reference numerals and will not be described.

In the robot controller 1 according to the third embodiment, the twist angle / disturbance estimation observer 11 is used.
Since the feed-forward unit 12 and the feed-forward unit 12 are the same as those of the robot control device 1 according to the second embodiment described above, detailed description thereof will be omitted.

In the control unit 93, the desired target angle θ _{r of} the servo motor 4, the angle θ _m of the servo motor 4 detected by the angle detector 6 on the robot 2 side, and the feed forward unit 12 are calculated. The feedforward value ff, the twist estimation angle θ _t calculated by the twist / disturbance estimation observer 11, the twist estimation angular velocity θ _t ^′ , the servo motor 4 estimated angular velocity θ _m ^′ , and the estimated disturbance torque d are input. In the control unit 93, based on these, the servo motor 4
Difference between the target angle θ _r and the angle θ _m of the servo motor 4,
The difference between the target angular velocity θ _r ^′ of the servo motor 4 and the estimated angular velocity θ _m ^′ of the servo motor 4 is calculated. And the control unit 13
Is the estimated value of these twist angles with respect to the following equation (23).
θ _t , estimated torsional angular velocity θ _t ^′ , estimated disturbance torque d , target angle θ _r of servo motor 4 and angle θ _{m of} servo motor 4
And the target angular velocity θ _r ^′ of the servo motor 4 and the estimated angular velocity θ _m ^′ of the servo motor 4 are substituted to calculate the calculated torque τ _e , and the calculated torque τ _e is stored in the torque limit unit 92. To transmit.

[0091]

(Equation 26)

Note that f ₁ , f in the above equation (23)
₂ , f ₃ and f ₄ are control parameters for determining control characteristics.

The calculated torque τ _e obtained by the control unit 93 as described above is sent to the torque limit unit 92.

The torque limit section 92 uses the following equation (24) to calculate the torque τ _e calculated by the control section 93 and the preset torque limit value τ _l based on the amplifier section of the robot 2. A command torque τ similar to the above given to 5 is calculated.

[0095]

[Equation 27]

The command torque τ calculated by the torque limit unit 91 is transmitted to the robot 2. The part of the robot 2 in FIG. 6 is described in more detail in the same manner as in FIG. 3 described above, and a detailed description thereof will be omitted.

According to the robot controller 1 of the fourth embodiment described above, the provision of the torque limit unit 92 allows the servo motor 4 and the amplifier unit 5 to comply with the torque limitation of the servo motor 4 and the amplifier unit 5. It is possible to sufficiently bring out the capability of No. 5 and thereby speed up the positioning of the second arm 3 on the robot 2 side. Therefore, according to the robot control device 1, work efficiency such as assembling of electronic parts by the robot 2 can be improved.

In the example of FIG. 6 as well, in order to facilitate the explanation, the robot controller 1 is provided with a twist angle
Disturbance estimation observer 11 and feedforward unit 12
, The control unit 93 and the torque limit unit 92 are divided into respective main calculation means for convenience, but these calculation means do not necessarily have to be divided as described above, and common calculation means may be used. It is also possible to have other computing means not belonging to these. In addition, the robot control apparatus 1 according to the embodiment of the present invention may use a dedicated calculation unit instead of each calculation unit, and may use the entire calculation unit using a calculation device such as a personal computer.

[0099]

As described above, according to the robot control apparatus of the present invention, since the torsion angle / disturbance estimating means for simultaneously estimating the torsion angle and the disturbance is provided, the vibration of the drive unit on the robot side can be suppressed. In addition to being suppressed, positioning of the drive unit can be speeded up. Further, according to this robot control device, it is possible to improve work efficiency such as assembling of electronic parts by the robot.

Further, according to the robot control apparatus of the present invention, by providing the torque limit means, it is possible to obtain a good control result while limiting the command torque, and to make the servomotor and the amplifier effective. It is possible to speed up the positioning of the drive unit by utilizing the above.

[Brief description of drawings]

FIG. 1 is a perspective view showing a robot controller according to a first embodiment of the present invention and a robot to be controlled by the robot controller.

FIG. 2 is a block diagram shown for explaining a state in which the robot control device according to the first embodiment of the present invention controls a robot.

FIG. 3 is a block diagram shown for explaining a state in which an arm of a robot is driven.

FIG. 4 is a block diagram shown for explaining a state in which a robot control device according to a second embodiment of the present invention controls a robot.

FIG. 5 is a block diagram shown for explaining a state in which a robot controller according to a third embodiment of the present invention controls a robot.

FIG. 6 is a block diagram shown for explaining a state in which a robot control device according to a fourth embodiment of the present invention controls a robot.

FIG. 7 is a block diagram shown for explaining a state in which a conventional robot control device controls a robot.

FIG. 8 is a diagram schematically showing a robot to be controlled by a conventional robot control device.

[Explanation of symbols]

1 robot controller, 2 robot, 3 second
Arm, 4 servo motor, 5 amplifier, 6 angle detector, 7,11 torsion angle / disturbance estimation observer, 8,12 feed forward section, 9,90,
93 control unit, 91, 92 torque limit unit, d
Disturbance torque, d estimated disturbance torque, τ command torque,
θ _a Arm angle, θ _a ^' Arm angular velocity, θ
Estimated angle of _a- arm, θ _a ^' Estimated angular velocity of arm,
θ _m Servo motor angle, θ _m ^' Servo motor angular velocity, θ _m ^' Servo motor estimated angular velocity, θ _r Servo motor target angle, θ _r ^' Servo motor target angular velocity, θ _t Torsional angle estimated value, θ _t ^' Torsion estimated angular velocity

Claims (20)

[Claims] 1. A robot to be controlled, comprising: a drive unit, a servo motor for driving the drive unit, an amplifier for generating torque in the servo motor, and an angle detection unit for detecting an angle of the servo motor. Based on the command torque given to the amplifier and the angle of the servo motor detected by the angle detection means, the angular velocity of the robot drive unit, which is the time derivative of the angle of the robot drive unit and the robot drive unit, , Torsional angle / disturbance estimation means for estimating the servomotor angular velocity, which is the time derivative of the servomotor angle, and the disturbance torque applied to the robot as a disturbance, and the servomotor target angular velocity based on the servomotor target angle Feedforward means for calculating the target angle of the servo motor, and the servo motor detected by the angle detecting means. Angle, the target angular velocity of the servomotor calculated by the feedforward means,
Control for calculating a command torque to be given to the amplifier based on the estimated angle of the robot drive unit estimated by the twist angle / disturbance estimation means, the estimated angular velocity of the robot drive unit, the estimated angular velocity of the servo motor, and the estimated disturbance torque. Means for controlling the robot based on the command torque calculated by the control means. 2. The robot controller according to claim 1, wherein the robot is controlled by the state equation of the following equation (1). [Equation 1] In the above equation (1), θ _m is the angle of the servo motor, θ _m ^' is the angular velocity of the servo motor, J _m is the inertia of the servo motor, D _m is the viscous friction of the servo motor, θ _a is the angle of the drive unit, θ _a ^' is the angular velocity of the drive unit, J _a is the inertia of the drive unit, D _a is the viscous friction of the drive unit, K is the spring constant of the gear between the servo motor shaft and the drive unit, N is the gear ratio, and d is the servo motor. Is the disturbance torque applied to the motor shaft, and τ is the command torque. 3. The torsion angle / disturbance estimating means uses the command torque τ and the angle θ _{m of the} servo motor to calculate the estimated angle θ _a of the drive unit and the drive angle by the following equations (2) and (3). Estimated angular velocity θ _a ^' and estimated servo motor angular velocity θ
_m ^'and the robot control apparatus according to claim 2, characterized in that the respectively calculated and the estimated disturbance torque d. [Equation 2] Note that z ₁ , z ₂ , and z in the above formulas (2) and (3)
₃ , z ₄ are state variables of the minimum dimensional observer, k ₁ , k
₂ , k ₃ and k ₄ are parameters that determine the estimated speed. 4. The angle θ of the servomotor is controlled by the control means.
and _m, and the estimated angle theta _a drive unit, ^'and the estimated angular velocity theta _a drive ^unit' estimated angular velocity theta _m of the servo motor and the estimated disturbance torque d, and the target angle theta _r of the servo motor, the servo motor The robot controller according to claim 3, wherein the command torque τ is calculated by the following equation (4) using the target angular velocity θ _r ^′ . (Equation 3) Note that f ₁ , f ₂ , f ₃ , and f ₄ in the above equation (4) are control parameters that determine the control characteristics. 5. A control target is a robot including a drive unit, a servo motor for driving the drive unit, an amplifier for generating torque in the servo motor, and an angle detection unit for detecting an angle of the servo motor, Based on the command torque given to the amplifier and the angle of the servo motor detected by the angle detection means, the twist angle, which is the difference between the angle of the drive unit of the robot and the angle of the servo motor, and the time derivative of the twist angle. Based on the target angle of the servomotor and the torsional angle / disturbance estimation means for estimating the servomotor angular velocity, which is the time derivative of the servomotor angle, and the disturbance torque applied to the robot as a disturbance. Feedforward means for calculating a feedforward value, a target angle of the servo motor, and detection by the angle detection means Angle of the servo motor, a feedforward value calculated by the feedforward means, a twist estimated angle estimated by the twist angle / disturbance estimation means, a twist estimated angular velocity, an estimated disturbance torque, and an estimated angular velocity of the servomotor. And a control means for calculating a command torque given to the amplifier based on the above, and controlling the robot based on the command torque calculated by the control means. 6. The robot controller according to claim 5, which controls the robot represented by a state equation of the following equation (5). (Equation 4) In the above equation (5), θ _m is the angle of the servo motor, θ _m ^' is the angular velocity of the servo motor, J _m is the inertia of the servo motor, D _m is the viscous friction of the servo motor, θ _a is the angle of the drive unit, θ _a ^' is the angular velocity of the drive unit, J _a is the inertia of the drive unit, D _a is the viscous friction of the drive unit, K is the spring constant of the gear between the servo motor shaft and the drive unit, N is the gear ratio, and d is the servo motor. Is the disturbance torque applied to the motor shaft, and τ is the command torque. 7. The twist angle / disturbance estimating means uses the command torque τ and the angle θ _{m of the} servomotor according to the following equations (6), (7), (8), and (9): The estimated angle θ _a of the drive unit, the estimated angular velocity θ _a ^′ of the drive unit, the estimated angular velocity θ _m ^′ of the servo motor, and the estimated disturbance torque d are calculated, and the estimated twist angle θ _t and the estimated twist angular velocity are calculated.
The robot controller according to claim 6, wherein θ _t ^' is calculated respectively. (Equation 5) Note that z ₁ , z ₂ , and z in the above formulas (6) and (7) are
₃ , z ₄ are state variables of the minimum dimensional observer, k ₁ , k
₂ , k ₃ and k ₄ are parameters that determine the estimated speed. 8. The feedforward means, ^'and target angular velocity theta _r of the servo ^motor' target angular velocity theta _r of the servo motor is a time differential value of the target angle theta _r of the servo motor servo motor which is a time differential value of And the target angular acceleration θ _r ^"of
The robot controller according to claim 7, wherein ff is calculated. (Equation 6) Note that ff ₁ and ff ₂ in the above equation (10) are feedforward parameters. 9. The control means uses the following equation (11) to calculate a target angle θ _r of the servo motor, a target angular velocity θ _r ^′ of the servo motor, an angle θ _m of the servo motor, and an estimated angular velocity θ of the servo motor. _m ^' , feedforward value ff,
9. The robot controller according to claim 8, wherein the command torque τ is calculated using the estimated value of twist angle θ _t , the estimated twist angular velocity θ _t ^′, and the estimated disturbance torque d . Equation 7] In addition, f ₁ in the formula (11), f
_2, f _3, f ₄ is controlled in Japanese This is a control parameter that determines the sex. 10. A robot having a drive unit, a servo motor for driving the drive unit, an amplifier for generating torque in the servo motor, and an angle detection unit for detecting an angle of the servo motor is a control target, and the amplifier is used as the control target. Based on the command torque given to the and the angle of the servo motor detected by the angle detection means, the angle of the drive unit of the robot, the angular velocity of the drive unit of the robot is a time derivative of the angle of the drive unit of the robot, The servo motor angular velocity, which is the time derivative of the servo motor angle, and the torsion angle / disturbance estimation means that estimates the disturbance torque applied to the robot as a disturbance, and the target angular velocity of the servo motor based on the target angle of the servo motor. The feed forward means for calculating, the target angle of the servo motor, and the servo motor detected by the angle detecting means. The angle, the target angular velocity of the servomotor calculated by the feedforward means,
Based on the estimated angle of the drive unit of the robot estimated by the twist angle / disturbance estimation unit, the estimated angular velocity of the drive unit of the robot, the estimated angular velocity of the servo motor, and the estimated disturbance torque, a control unit that calculates a calculated torque, and Torque limit means for calculating a command torque given to the amplifier limited within a torque limit value by a preset torque limit value based on the calculated torque calculated by the control means, and the torque limit means calculates A robot controller that controls the robot based on the commanded torque. 11. The robot according to claim 1, which is represented by a state equation of the following equation (12) is controlled.
0. The robot controller according to item 0. (Equation 8) In the above formula (12), θ _m is the angle of the servo motor, θ _m ^' is the angular velocity of the servo motor, J _m is the inertia of the servo motor, D _m is the viscous friction of the servo motor, and θ _a is the angle of the drive unit. θ _a ^' is the angular velocity of the drive unit, J _a is the inertia of the drive unit, D _a is the viscous friction of the drive unit, K is the spring constant of the gear between the servo motor shaft and the drive unit, N is the gear ratio, and d is the servo motor. Is the disturbance torque applied to the motor shaft, and τ is the command torque. 12. The torsion angle / disturbance estimation means is a command
Torque τ and servo motor angle θ_m Using
Estimated angle of the drive unit from (13) and equation (14)θ _a
And the estimated angular velocity of the driveθ _a ^' And the estimated angle of the servo motor
speedθ _m ^' And estimated disturbance torquedAnd can be calculated respectively
The robot controller according to claim 11, wherein: (Equation 9)Note that z in the above equations (13) and (14)₁ , Z
_Two , Z_Three , Z_Four Is the state variable of the minimum dimensional observer, k
₁ , K_Two , K_Three , K_Four Is a parameter that determines the estimated speed
is there. 13. The control means includes a servo motor angle θ _m , an estimated drive angle θ _a , an estimated servo motor angular velocity θ _m ^′ , an estimated drive angular velocity θ _a ^′, and an estimated disturbance torque. 13. The calculated torque τ _e is calculated by the following equation (15) using d , the target angle θ _r of the servo motor, and the target angular velocity θ _r ^′ of the servo motor. Robot controller. (Equation 10) Note that f ₁ , f ₂ , f ₃ , and f ₄ in the above formula (15) are
Is a control parameter that determines the control characteristics. 14. The torque limit means calculates a command torque τ limited within a preset torque limit value τ _l based on the calculated torque τ _e by the following equation (16). The robot controller according to claim 13. [Equation 11] 15. A robot to be controlled is provided with a drive section, a servo motor for driving the drive section, an amplifier for generating torque in the servo motor, and an angle detection means for detecting an angle of the servo motor. Based on the command torque given to the amplifier and the angle of the servo motor detected by the angle detection means, the twist angle, which is the difference between the angle of the drive unit of the robot and the angle of the servo motor, and the time derivative of the twist angle. Based on the target angle of the servomotor and the torsional angle / disturbance estimation means for estimating the servomotor angular velocity, which is the time derivative of the servomotor angle, and the disturbance torque applied to the robot as a disturbance. The feedforward means for calculating the feedforward value, the target angle of the servo motor, and the angle detection means detect the angle. The servo motor angle output, the feedforward value calculated by the feedforward means, the twist angle / twist angle estimated by the twist angle / disturbance estimation means, the twist estimated angular velocity, the estimated disturbance torque, and the servomotor estimated angular velocity. Based on the control means for calculating the calculated torque, and the calculated torque calculated by the control means, the command torque given to the amplifier limited within the torque limit value by the preset torque limit value is calculated. And a torque limit unit for controlling the robot based on the command torque calculated by the torque limit unit. 16. The robot according to claim 1, which is represented by a state equation of the following equation (17) is controlled.
5. The robot controller according to item 5. (Equation 12) In the above equation (17), θ _m is the angle of the servo motor, θ _m ^' is the angular velocity of the servo motor, J _m is the inertia of the servo motor, D _m is the viscous friction of the servo motor, and θ _a is the angle of the drive unit. θ _a ^' is the angular velocity of the drive unit, J _a is the inertia of the drive unit, D _a is the viscous friction of the drive unit, K is the spring constant of the gear between the servo motor shaft and the drive unit, N is the gear ratio, and d is the servo motor. Is the disturbance torque applied to the motor shaft, and τ is the command torque. 17. The twist angle / disturbance estimation means uses the command torque τ and the angle θ _{m of the} servomotor according to the following equation (18), equation (19), equation (20), and equation (21). Estimated drive angle θ _a and estimated drive angular velocity θ _a ^'
And the estimated angular velocity θ _m ^' of servo motor and estimated disturbance torque
17. The robot controller according to claim 16, wherein d is calculated respectively, and the twist estimated angle θ _t and the twist estimated angular velocity θ _t ^′ are calculated respectively. (Equation 13) Note that z ₁ and z in the above formulas (18) and (19)
₂ , z ₃ and z ₄ are state variables of the minimum dimensional observer, k
₁ , k ₂ , k ₃ and k ₄ are parameters that determine the estimated speed. 18. The feedforward means comprises a target angular velocity θ _r ^′ of the servo motor and a target angular velocity θ _r ^′ of the servo motor, which is a time derivative of a target angle θ _r of the servo motor.
18. The robot controller according to claim 17, wherein the feedforward value ff is calculated by the following equation (22) using the target angular acceleration θ _r ^" of the servo motor, which is the time differential value of. Number 14] Note that ff ₁ and ff ₂ in the above equation (22) are feedforward parameters. 19. The control means uses the following equation (23) to calculate a target angle θ _r of the servo motor, a target angular velocity θ _r ^′ of the servo motor, an angle θ _m of the servo motor, and an estimated angular velocity θ of the servo motor. _m ^' , feedforward value ff,
19. The robot controller according to claim 18, wherein the calculated torque τ _e is calculated using the estimated value of twist angle θ _t , the estimated twist angular velocity θ _t ^′, and the estimated disturbance torque d . [Mathematical formula-see original document] Note that f ₁ , f in the above equation (23)
₂ , f ₃ , f ₄ are control It is a control parameter that determines the characteristics. 20. The torque limit means calculates a command torque τ limited within a preset torque limit value τ _l based on the calculated torque τ _e by the following formula (24). 20. The robot controller according to claim 19. (Equation 16)