JPH10128688A - Non-interfering control method of robot - Google Patents

Abstract

PROBLEM TO BE SOLVED: To compensate interference between each arm of a robot based on dynamics of the robot, and ensure a high follow-up property relating to a referred track even when the robot is in high speed operation. SOLUTION: Angular acceleration or an angular speed of a first arm is detected by a sensor 11, 12, angular acceleration or an angular speed of a second arm is detected by a second sensor 13, 14, in a control means, an output signal of the first sensor 11, 12 and an output signal of the second sensor 13, 14 are used, non-interfering control of disturbance relating to the first arm and non- interfering control of disturbance relating to the second arm are performed.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

The present invention relates to a decoupling control method for an articulated robot having a plurality of arms.

[0002]

2. Description of the Related Art Conventionally, in a positioning control method for an articulated robot having a plurality of arms, positioning control of each arm is performed using an independent servo controller for each joint. The servo controller that controls the positioning of each arm does not consider the dynamics (dynamics) of the robot, and treats the moment of inertia caused by the change in the posture of the robot and the interference force caused by the movement of the arm as disturbances to the robot position control system. ing.

[0003] State quantities such as angular velocity / angular acceleration of an arm necessary for robot dynamics and positioning control are values obtained by an angle detector comprising a rotary encoder coaxially mounted with a motor driving each joint. The value estimated based on is used.

For example, as shown in FIG. 9, a first arm 1 which is provided rotatably, a first joint 3 to which a root end of the first arm 1 is attached, and a first arm 1 A second joint 4 attached to the pivot side end;
Arm 2 with root end attached to joint 4
In horizontal biaxial scalar robot having bets, arms of mass m _i of the i, arm inertia moment I _i of the i, arm of the center of gravity position G _i of i-th, the length of the arms of the i Assuming that l _i , the distance between the ith joint and the center of gravity of the ith arm is s _i , the angle of the ith joint is θ _i , and the drive torque acting on the ith joint is τ _i , the motion of the arm The equation is formulated as follows.

[0005]

(Equation 1)

[0006]

(Equation 2)

[0007]

(Equation 3)

[0008]

(Equation 4)

[0009]

(Equation 5)

A state variable diagram of the equation of motion of the arm is as shown in FIG.

[0011]

The above-described method of controlling the positioning of the articulated robot does not include the dynamics of the robot. Therefore, when a high-speed operation in which a large acceleration is applied to the arm is required, a highly accurate robot is required. Following the reference trajectory cannot be expected. This is a level that cannot be ignored when the arm moves at high speed and the interference force between each arm caused by the fluctuation of the inertia moment due to the change in the posture of the robot and the high acceleration acting on the arm is treated as a disturbance. This is particularly noticeable in the case of a robot having a reduction gear with a low reduction ratio for the purpose of high-speed operation. In FIG. 10, a portion surrounded by a broken line regarding the inertial interference force, the Coriolis force, and the centrifugal force can be considered as an angular acceleration disturbance applied to each arm.

Further, the state quantity necessary for the robot positioning control such as the angular velocity / angular acceleration of the arm is based on a value estimated based on a value obtained by an angle detector composed of a rotary encoder. It is difficult to obtain accurate values due to insufficient bandwidth. This makes it difficult to accurately follow the reference trajectory during high-speed operation of the robot.

The present invention can compensate for interference between the arms of a robot based on the dynamics of the robot, and can maintain a high followability to a reference trajectory even at a high speed operation of the robot. It is an object to provide a control method.

[0014]

In order to achieve the above-mentioned object, the invention according to claim 1 is provided with a first rotatably provided first motor.
Arm, a first joint to which a root end of the first arm is attached, a second joint to be attached to a turning end of the first arm, and a second joint. A second arm having a root end attached thereto, a first motor for rotating the first joint, a second motor for rotating the second joint, the first motor and the second motor; A control means for controlling positioning of the first arm and the second arm by controlling a second motor, wherein the angular acceleration or the angular velocity of the first arm is provided. Is detected by a first sensor, the angular acceleration or angular velocity of the second arm is detected by a second sensor, and the control unit outputs an output signal of the first sensor and an output signal of the second sensor. Using the first
Of the first arm and the second sensor using the output signal of the first sensor and the output signal of the second sensor. Therefore, interference between the arms of the robot can be compensated based on the dynamics of the robot, and high followability to the reference trajectory can be ensured even during high-speed operation of the robot.

According to a second aspect of the present invention, in the robot decoupling control method according to the first aspect, the angular acceleration of the first arm and the second acceleration are determined by the first sensor and the second sensor. The angular acceleration of each arm is detected, and the disturbance decoupling control performs the first arm with respect to the first arm.
A disturbance torque is obtained using the output signal of the second sensor and the output signal of the second sensor, and is fed back to the input torque, thereby performing the control of decoupling the disturbance torque and performing the first arm control on the second arm. A disturbance torque is obtained using the output signal of the second sensor and the output signal of the second sensor, and is fed back to the input torque, thereby performing the control for decoupling the disturbance torque. Therefore, it is possible to compensate for interference between the arms of the robot based on the dynamics of the robot, and it is possible to ensure significantly high followability with respect to the reference trajectory even when the robot operates at high speed.

According to a third aspect of the present invention, in the robot decoupling control method according to the first aspect, the angular velocity of the first arm and the second arm are determined by the first sensor and the second sensor. And the disturbance decoupling control estimates a disturbance torque with a disturbance observer using the output signal of the first sensor and the output signal of the second sensor for the first arm. Control of disturbance torque by feeding back to the input torque by means of a disturbance observer using the output signal of the first sensor and the output signal of the second sensor for the second arm. Estimating the torque and feeding it back to the input torque performs decoupling control of the disturbance torque. Therefore, it is possible to compensate for interference between the arms of the robot based on the dynamics of the robot, to ensure high followability to the reference trajectory even during high-speed operation of the robot, and to reduce the number of sensors. Can be.

[0017]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS Many articulated robots that are in practical use are formed of links having high rigidity so that each arm does not bend greatly in order to facilitate the purpose of use and control thereof. In the articulated robot having such a highly rigid arm, the equation of motion of the arm is represented by the above-described equation (1).

In the equation (1), the first term on the left side is a moment of inertia matrix of the arm, and the value of each element greatly fluctuates due to a change in the posture of the arm. Also, J (θ)
Has off-diagonal elements and depends on the structure of the robot,
Usually, the size becomes non-negligible in proportion to the size of the diagonal element of the J matrix, and the sign can be both positive and negative. Therefore, a change in the acceleration of the arm greatly affects the movement of the other arm as an interference force. In particular, when a high acceleration speed command is given in order to realize high-speed positioning of the arm, overshoot or residual vibration occurs at the rise or fall, making it difficult to position the arm.

The second term on the left side of the equation (1) represents a Coriolis force / centrifugal force term acting on the arm, and is a nonlinear function depending on the angle and angular velocity of each joint. This term also exerts a large interference force between the arms. The third term on the left side of the equation (1) is often regarded as a viscous friction coefficient matrix, and is a diagonal matrix in the case of an arm robot having a rotating joint. The fourth term on the left side of the equation (1) represents solid friction, and is usually approximated as a nonlinear function that generates a constant counter torque when the arm is moving.

As described above, the equation of motion of the arm exhibits strong nonlinearity, and there is an off-diagonal element of the inertia matrix J (θ) and an interference force due to the Coriolis force / centrifugal force term. However, these non-linearities and interference forces except for the fourth term on the left side of the equation (1) depend on the structure of the robot, and are relatively easy to formulate. Then, given the motion state of the arm, the influence of the nonlinearity and the interference force can be uniquely obtained. Here, the terms of the equation of motion of the arm in the horizontal two-axis scalar robot are obtained as described above, but the solid friction term is assumed to be a disturbance term here.

If the interference force due to the inertia matrix term and the Coriolis force / centrifugal force term in the equation (1) is calculated and added to the torque operation amount of the arm and can be canceled, the equation (1) is Torque and its motion state (angular velocity, angular acceleration)
Can be regarded as a linear time-invariant system having no interference indicating the relationship of .times..times..times..times..times..times..times..times.

The method of decoupling between the arms based on the dynamics of the robot is based on a non-linear compensation method. The measured values of the angular velocities and angles of the joints are fed back to calculate the decoupling of the arms by inverse dynamics calculation. There are a calculated torque method for obtaining torque, and a feedforward compensation method for predicting a torque required for decoupling of an arm by inverse dynamics calculation from an angle, an angular velocity, and an angular acceleration of a target value.

In these methods, the torque required for decoupling the arm cannot be obtained unless the physical parameters of the arm are accurately known. In recent years, the kinematics calculation algorithm has been improved, but since the physical parameters of the arm change, it is necessary to accurately identify the physical parameters of the arm and obtain the torque required for decoupling the arm. Accurate identification of arm physical parameters requires accurate angles, angular velocities, angular accelerations, and driving torques of joints.

However, in the case of a robot with a speed reducer having only an angle detector consisting of a rotary encoder directly connected to a motor for driving the joint, if the angular acceleration is obtained by two differentiating operations of the angle, the drive for driving the joint is obtained. Noise in the high frequency range caused by vibration due to slight nonlinearity or flexibility of the system is greatly amplified, and S / N deteriorates due to lack of resolution of the rotary encoder, making it difficult to calculate accurate angular acceleration. become.

The embodiments of the present invention solve these problems by detecting the angular acceleration or angular velocity of the arm with a sensor to obtain the angular acceleration, and reduce the influence of the dynamic characteristics between the arms. The interference torque including the interference can be obtained, and the decoupling control between the arms can be performed.

The horizontal 2 to which the invention according to claims 1 and 2 is applied.
In one embodiment of the shaft-type scalar robot, in order to achieve decoupling between the arms, instead of calculating the interference force between the arms by inverse dynamics calculation, the scalar robot is attached to the tip of the arm as shown in FIG. acceleration information alpha _1Shita obtained by the acceleration sensor 11 to 14 _{has, α 1 r, α 2θ,} α 2 r and the inertia of the arm from the output value theta ₂ of the angle detector consisting of a rotary encoder directly connected to the motor driving the joint The interference term and the Coriolis / centrifugal force terms are obtained to calculate the interference torque τc, and this is subjected to nonlinear compensation feedback to obtain the following (6)
By adding to the input torque (torque command) u of the arms as in the equation, decoupling control between the arms is performed.

[0027]

(Equation 6)

Here,

[0029]

(Equation 7)

Α _1θ is acceleration information obtained by a sensor 11 for measuring a tangential acceleration of the first arm, α _1r is acceleration information obtained by a sensor 12 for measuring a radial acceleration of the first arm, α _2θ is acceleration information obtained by the sensor 13 measuring the tangential acceleration of the second arm, and α _2r is acceleration information obtained by the sensor 14 measuring the radial acceleration of the second arm. In the equation (7), the first term on the left side is a matrix including a non-diagonal term which is an interference term in the inertia matrix as shown in the following equation (8) and a term of variation of the moment of inertia of the first arm. It is.

[0031]

(Equation 8)

By adding the interference torque τ _c expressed by equation (8) to the arm driving torque u, the arm motion equation of the horizontal two-axis scalar robot is

[0033]

(Equation 9)

[0034]

(Equation 10)

[0035]

[Equation 11]

Thus, non-interference between the arms is achieved as shown in FIG. The transfer function G ₁ (s) of the first arm and the transfer function G ₂ (s) of the _second arm are obtained as follows.

[0037]

(Equation 12)

[0038]

(Equation 13)

When calculating the interference torque τ _c , the angular acceleration of the arm must be separated from the acceleration information α _1θ , α _1r , α _2θ , α _2r obtained by the acceleration sensors 11 to 14. As shown in FIG. 7, the position vector P ₁ of an arbitrary point ζ ₁ on the i-th arm with respect to a coordinate system fixed to the robot pedestal is

[0040]

[Equation 14]

## EQU4 ##

Therefore, the acceleration vector with respect to the absolute coordinates at an arbitrary point q ₁ on the arm is

[0043]

(Equation 15)

Is required. Therefore, the magnitude of the acceleration vector measured in the coordinate system having the origin at the joint of the arm and rotating with the arm is given by the following equation.

[0045]

(Equation 16)

Where R (θ _k ) is a rotation transformation matrix,
l is an arbitrary direction vector indicating the acceleration measurement direction at o _i −x _i y _i of | l | = 1. Here, when the acceleration in the longitudinal direction and the acceleration in the joint angle θ direction at the tip of each arm of the horizontal two-axis scalar robot are measured, the angular acceleration can be obtained from the measured acceleration values as follows.

[0047]

[Equation 17]

[0048]

(Equation 18)

[0049]

[Equation 19]

[0050]

(Equation 20)

[0051]

(Equation 21)

From the equations (17) to (20), the terms of angular acceleration, Coriolis force and centrifugal force of each arm can be obtained as follows.

[0053]

(Equation 22)

[0054]

(Equation 23)

[0055]

(Equation 24)

Therefore, the above-mentioned interference torque is calculated by an interference torque calculating section as shown in FIG.
Acceleration information α _1θ , α _1r , α obtained from 1 to 14
It can be calculated from _2θ , α _2r and the output value θ _{2 of} an angle detector composed of a rotary encoder directly connected to a motor for driving a joint.

FIGS. 3 and 4 show an embodiment of a horizontal two-axis scalar robot to which the inventions according to claims 1 and 2 are applied. As shown in FIG. 3, the robot includes a first arm 1 that is pivotally provided, a first joint 3 to which a root end of the first arm 1 is attached, and a first arm 1.
, A second arm 4 having a root end attached to the second joint 4, and a first motor for rotating the first joint 3. And the second
And a second motor for rotating the joint 4 of the robot, and is attached to the robot pedestal.

As shown in FIG. 5, a sensor 11 for measuring the tangential acceleration of the first arm 1 and a sensor 12 for measuring the radial acceleration of the first arm 1 are provided at the tip of the first arm 1. And have been fixed. A sensor 13 for measuring the tangential acceleration of the second arm 2 and a sensor 14 for measuring the radial acceleration of the second arm 2 are fixed to the tip of the second arm 2 as shown in FIG. Have been. These sensors 11 to 14 are, for example, those manufactured by Tokimec Corporation.

Further, a first rotary encoder as a first angle detector is directly connected to the first motor, and a second rotary encoder as a second angle detector is directly connected to the second motor. You. As the first motor and the second motor, servo motors with harmonic drive reducers are used. The reduction ratio of the speed reducer is 1/60 for the first arm 1 and 1/51 × 21/22 for the second arm 2, and this robot belongs to a relatively high speed category among industrial robots. Each of the arms 1 and 2 is made of a robust aluminum alloy casting, and the problem of deflection due to flexibility can be ignored.

Further, as shown in FIG. 4, the robot controls a first motor and a second motor to control the positioning of the first arm 1 and the second arm 2 from a computer as control means. Servo controller 17
And an extended I / O unit 18, a four-channel low-pass filter 19, and a servo amplifier 21. Extension I
The / O unit 18 includes an A / D converter 22 and a D / A
It has a converter 23, a 24-bit / 4-channel encoder counter 24, and a parallel I / O port 25. The computer 17 is, for example, a P
C9801BX is used, and the first consists of a servo amplifier 21 and a servomotor with a harmonic drive reducer.
And the second motor constitute a drive system for driving the joints 3 and 4.

The acceleration information α _1θ , α _1r , α _2θ , α _2r obtained by the acceleration sensors 11 to 14 are transmitted to the computer 1 via the low-pass filter 19 and the A / D converter 23.
7 is input. The encoder counter 24 counts the output pulse signals of the first rotary encoder and the second rotary encoder, and outputs a signal indicating the angle of each of the arms 1 and 2 to the computer 17.

The computer 17 has an encoder counter 2
The robot follows the reference trajectory by controlling the first motor and the second motor via the D / A converter 23 and the servo amplifier 21 using the output signal of No. 4 to control the positioning of each arm 1 and 2. Let it. Computer 1
7, the angular accelerations of the arms 1 and 2 are obtained by separating the angular accelerations of the arms 1 and ₂ from the acceleration information α _1θ , α _1r , α _2θ and α _2r obtained by the acceleration sensors 11 to 14 as described above. And the output signal of the encoder counter 24, the inertial interference term and the Coriolis force / centrifugal force term of each of the arms 1 and 2 are obtained as described above to calculate each interference torque τ _c , and these are subjected to non-linear compensation feedback. Arm 1,
By adding to the input torque (torque command) of 2,
Decoupling control between the arms 1 and 2 is performed.

As described above, this embodiment is an embodiment of the horizontal two-axis scalar robot to which the invention according to claim 1 is applied, and the first arm 1 that is rotatably provided.
A first joint 3 to which a root end of the first arm 1 is attached; a second joint 4 to be attached to a turning end of the first arm 1; A second arm 2 having a root end attached to a joint 4, a first motor for rotating the first joint 3, and a second joint 4
And a computer 17 as control means for controlling the first motor and the second motor to perform positioning control of the first arm and the second arm. In the robot, the angular acceleration of the first arm 1 is detected by first sensors 11 and 12, and the angular acceleration of the second arm 2 is detected by a second sensor 1.
3, 14 and the control means 17 detects the first
Output signals of the sensors 11 and 12 and the second sensor 1
Using the output signals of the first and the third sensors, the first arm 1 is controlled to make the first arm 1 non-interference-free.
Since the control for decoupling the disturbance to the second arm 2 is performed by using the output signals of the first and second sensors 12 and the output signals of the second sensors 13 and 14, the respective arms of the robot are controlled based on the dynamics of the robot. Interference can be compensated, and high followability to the reference trajectory can be ensured even during high-speed operation of the robot.

This embodiment is an embodiment of a horizontal two-axis scalar robot to which the invention according to claim 2 is applied. In the method for controlling decoupling of a robot according to claim 1, the first The sensors 11 and 12 and the second sensors 13 and 14 detect the angular acceleration of the first arm 1 and the angular acceleration of the second arm 2 respectively. The output signal of the first sensor and the second sensor 13, 14
A disturbance torque is obtained by using the output signal of (1) and is fed back to the input torque to perform the control of decoupling of the disturbance torque, and the output signal of the first sensor and the second Since interference torque decoupling control is performed by obtaining the disturbance torque using the output signals of the sensors 13 and 14 and feeding it back to the input torque, the interference between the arms of the robot is compensated based on the dynamics of the robot. Thus, even when the robot operates at a high speed, it is possible to secure a significantly high followability with respect to the reference trajectory.

Next, an embodiment of a horizontal two-axis scalar robot to which the invention according to claims 1 and 3 is applied will be described. In this embodiment, the interference between the arms is compensated by the feedforward compensation method, and the interference generated between the arms is considered as the angular acceleration disturbance of each arm.
Using an observer (state observer) constructed based on a mathematical model of the arm in the servo controller, the disturbance is estimated from the input torque and angular velocity of each arm and compensated, thereby achieving decoupling of each arm. Here, the angular velocity of each arm required when estimating the disturbance with the observer is
Instead of calculating and estimating from the value of the rotary encoder that detects the position (angle) of the arm, the angle is obtained by an angular velocity detector including a vibrating gyro sensor installed on each arm.

In this embodiment, as shown in FIG. 13, without providing the acceleration sensors 11 to 14 and the low-pass filter 19 in one embodiment of the horizontal two-axis scalar robot to which the inventions according to the first and second aspects are applied. Each arm 1,
Angular velocity detector 2 consisting of a vibrating gyro sensor at the tip of 2
6 and 27 are fixed respectively, and the vibration gyro sensor 2
The angular velocities of the respective arms 1 and 2 are measured by 6 and 27. As shown in FIG. 14, the output signals of the vibration gyro sensors 26 and 27 are input to the computer 17 via the two-channel low-pass filter 20 and the A / D converter 23.

The mathematical model of the horizontal two-axis scalar robot can be represented as shown in FIG. 9, and from FIG. 9, the equation of motion of the robot arm can be formulated as shown in equation (1). In order to clarify the interference existing between the input torque and the angular velocity with respect to the arms 1 and 2 in the SCARA robot, the equation of motion of the robot arm represented by the equation (1) is represented by a state variable diagram as shown in FIG. As shown.

The input torque τ _i to each of the arms 1 and 2 (i =
It is desirable that the outputs θ _i (i = 1, 2) (arm angular velocities) with respect to 1, 2) are not influenced by each other when controlling the position and velocity of each arm 1, 2. That is, if the arm 1 and the arm 2 are robot subsystems, it is more convenient for the input / output of each subsystem to correspond to each other in a one-to-one correspondence and not to interfere with each other when forming a position / speed control system. It is desirable that there is no path from the portion surrounded by the broken line in FIG.

However, in the case of the horizontal two-axis scalar robot, as shown in FIG. 10, the input and output of the arm 1 and the arm 2 intersect each other in a complicated manner, and it can be seen that each subsystem interferes. Therefore, for example, when an input torque τ ₁ is given to cause the arm 1 to move, the input torque τ ₁ changes not only the output (angular velocity) of the arm 1 but also the output of the arm 2 due to interference between the arms. Will be lost.

That is, if the position and the speed of the arm 1 are to be changed, it means that the position and the speed of the arm 2 which are not intended are changed. This is a great inconvenience in controlling the position / speed of the tip of the robot arm. Therefore, it is necessary to compensate for these interferences by using some method so as to make the arms non-interfering, so that the outputs of the respective arms correspond one-to-one.

In FIG. 10, the interference component between the arms, which is represented by the path intersecting between the arms 1 and 2, is a disturbance to the angular acceleration input of each system. Component is angular acceleration disturbance a _Di
When (i = 1, 2) is defined and this is put together for each of the arms 1 and 2, the model of the robot arm can be represented as shown in FIG. 11 and finally as shown in FIG.

In the present embodiment, the vibration gyro sensors 26 and 27 installed on the arms 1 and 2 respectively
And the computer 17 calculates the angular acceleration disturbance a which cannot be directly obtained as an output of the system.
_Di is calculated and estimated from the measurable input torque τ _i and the angular velocities of the arms 1 and 2 measured by the vibrating gyro sensors 26 and 27 using an observer constructed based on a mathematical model of the robot arm. Of course, the angular acceleration disturbance a _Di may be directly measured. However, in order to directly obtain the angular acceleration disturbance a _Di , the angular acceleration of each of the arms 1 and 2 is measured by the acceleration sensor 11.
To 14 and therefore, it is necessary to install a number of acceleration sensors 11 to 14 on each of the arms 1 and 2.

The observer is a simulation for a kind of real system constructed on the computer 17 based on the mathematical model of the real system. Regarding the structure and design method of the observer, several methods have been proposed including Gopinas' method. Have been. The observer can estimate the internal state that cannot be directly obtained from the output of the real system based on the input and output to the real system that can be observed. Therefore, by using an observer, the number of required sensors can be reduced, and the sensor configuration of the system can be simplified.

In the case of the robot shown in FIG. 12, the influence (interference component) from the line crossing between the arms, that is, the angular acceleration disturbance a _Di is an internal state that cannot be directly obtained from the output of the system. The interference component can be estimated using an observer. In the present embodiment, the Gopinas method is used for designing an observer for estimating the angular acceleration disturbance a _Di which is an interference component.

The computer 17 feeds back the angular acceleration disturbance a _Di estimated by the observer to the input torque τ _i and adds the input torque τ _i . The present invention compensates for the influence from the existing lines, that is, the interference, and apparently eliminates or reduces the effects of these lines, so that it is possible to treat the input and output as a one-to-one correspondence as a whole. Therefore, the control performance of the computer 17 as a servo controller for controlling the position / speed of each arm can be improved.

Generally, by using a disturbance observer, decoupling between the arms can be achieved, and at the same time, the influence of parameter fluctuations of the model of the controlled object (robot arm) can be reduced. It is possible to improve the robustness of the system against parameter fluctuations. As is clear from FIG. 10, the model of the robot arm to be controlled according to the present embodiment has a fluctuation component associated with a change in the posture of the robot arm as a non-linear function in each parameter, and the control object is controlled by using a disturbance observer. It is possible to treat the system as a linear system having a nominal value parameter that does not fluctuate approximately, and it is possible to further improve the performance of the position / speed control system.

Next, the observer will be specifically described. In FIG. 10, a portion surrounded by a broken line regarding the interference force, the Coriolis force, and the centrifugal force is considered as an angular acceleration disturbance a _Di applied to each arm, and is again shown in a block diagram in FIG.
It becomes as shown in. From FIG. 15, since the arms are apparently independent of each other and have the same form, it is easy to apply one to the other if one is considered. Therefore, the following description will be made ignoring the subscripts 1 and 2.

FIG. 15 shows that x = θ (where x (0) = 0),
When r (t) = u (t) and it is represented by a differential equation, the following equation is obtained.

[0079]

(Equation 25)

Here, the following variables are divided into a nominal term having a subscript n and a variable term.

[0081]

(Equation 26)

Substituting this into equation (25) and rearranging it,
The following equation is obtained.

[0083]

[Equation 27]

Here,

[0085]

[Equation 28]

Is as follows.

Next, assuming that the angular acceleration disturbance A _Dis including the parameter fluctuation takes a constant value in a short time.
The state equation is expressed as the following equation.

[0088]

(Equation 29)

The output equation is as follows because only the angular acceleration can be detected.

[0090]

[Equation 30]

When the disturbance observer is designed using the Gopinas method from the above formulation, the following equations (31) and (32) are derived. Where ω is the state variable of the observer and L
Is a design parameter related to the fixed value of the observer, and takes a value of L <0. A ＾ _Dis (＾ is correctly attached to A) is an estimated angular acceleration disturbance including a parameter variation.

[0092]

(Equation 31)

[0093]

(Equation 32)

When this is modified and represented by a block diagram,
The result is as shown in FIG. However, the design parameter L of the observer is defined again as L> 0. In FIG. 16, (a) and (b) are equivalent, and the actual servo loop uses the format of FIG. 16 (b).

The estimated angular acceleration disturbance A ＾ _Dis obtained in this manner is obtained through a first-order lag element as is apparent from FIG. This first-order lag element can be considered as a first-order filter in the disturbance observer, and has a function of suppressing high-frequency noise generated when differentiating the angular velocity.
Here, if the time constant Q _n / L of the first-order lag element is negligibly small, it can be considered that the estimated angular acceleration disturbance A ＾ _Dis is equivalent to the actual angular acceleration disturbance A _Dis .

Therefore, feedback of the torque disturbance τ _cmp obtained from the estimated angular acceleration disturbance A ＾ _Dis via the gain 1 / _Pn to the input torque as shown in FIG. 17 is _equivalent to the feed-forward compensation of the angular acceleration. Function. As a result, the interference force, the Coriolis force, and the centrifugal force cancel each other out, and only the term having a nominal value remains in the equation (1), and as in the following equations (33) and (34), each arm is independent and nominal. It can be expressed by an equation of motion with coefficients,
Decoupling and linearization can be achieved.

[0097]

[Equation 33]

[0098]

(Equation 34)

FIG. 17 is a block diagram showing a feedforward angular acceleration control method using the above-described disturbance observer.

In this embodiment, the computer 17
Uses an observer constructed based on a mathematical model of a robot arm based on the input torque τ _i that can be measured and the angular velocities of the arms 1 and 2 measured by the vibration gyro sensors 26 and 27 as described above. Then, the interference between the arms is compensated by feeding back and adding the angular acceleration disturbance estimated by the observer to the input torque. Thus, the new torque input and output (angular velocity of the arm) correspond one-to-one, and as shown in the block diagram on the right side of FIG.

As described above, this embodiment is an embodiment of the horizontal two-axis type scalar robot to which the invention according to claim 1 is applied, and the first arm 1 that is rotatably provided.
A first joint 3 to which a root end of the first arm 1 is attached; a second joint 4 to be attached to a turning end of the first arm 1; A second arm 2 having a root end attached to a joint 4, a first motor for rotating the first joint 3, and a second joint 4
A robot comprising: a second motor for rotating a motor; and a computer 17 as control means for controlling the first motor and the second motor to perform positioning control of the first arm and the second arm. , The angular velocity of the first arm 1 is detected by a vibration gyro sensor 26 as a first sensor, and the angular velocity of the second arm 2 is detected by a vibration gyro sensor 27 as a second sensor. The means 17 controls the interference of the disturbance to the first arm 1 using the output signal of the first sensor 26 and the output signal of the second sensor 27, and outputs the output of the first sensor 26. Signals and the output signal of the second sensor 27 are used to control the interference of the second arm 2 to be decoupling. Can compensate for interference between the arms of Tsu bets, it is possible to secure a high followability to see orbit during high-speed operation of the robot.

This embodiment is an embodiment of a horizontal two-axis scalar robot to which the invention of claim 3 is applied. In the method of controlling decoupling of a robot according to claim 1, the first The sensor 26 and the second sensor 27 detect the angular velocity of the first arm 1 and the angular velocity of the second arm 2, respectively. The disturbance observer estimates the disturbance torque using the output signal of the first sensor 26 and the output signal of the second sensor 27 and feeds it back to the input torque, thereby performing the control for decoupling the disturbance torque and the second signal. The disturbance torque is estimated by the disturbance observer using the output signal of the first sensor 26 and the output signal of the second sensor 27 for the arm 2 and the input torque is fed. Control of the disturbance torque to prevent interference between each arm of the robot based on the dynamics of the robot, thereby ensuring high followability to the reference trajectory even during high-speed operation of the robot. And the number of sensors can be reduced. The invention according to each claim is not limited to the above embodiment, and can be applied to, for example, a robot having three or more sets of arms and joints.

[0103]

As described above, according to the first aspect of the present invention, the first arm rotatably provided, the first joint to which the root end of the first arm is attached, A second joint attached to a pivot-side end of the first arm, a second arm having a root-side end attached to the second joint, and a first arm for rotating the first joint , A second motor for rotating the second joint, and control for controlling the first motor and the second motor to perform positioning control of the first arm and the second arm. Means for controlling decoupling of a robot, wherein the angular acceleration or angular velocity of the first arm is set to a first value.
And the angular acceleration or angular velocity of the second arm is detected by the second sensor, and the control unit uses the output signal of the first sensor and the output signal of the second sensor. Since disturbance control for disturbance of the first arm is performed, and control of disturbance control for the second arm is performed using the output signal of the first sensor and the output signal of the second sensor. In addition, it is possible to compensate for interference between the arms of the robot based on the dynamics of the robot, and to ensure high followability to the reference trajectory even when the robot operates at high speed.

According to a second aspect of the present invention, in the robot decoupling control method according to the first aspect, the angular acceleration of the first arm and the angular acceleration of the first arm are determined by the first sensor and the second sensor. Angular acceleration of the two arms is detected respectively,
In the disturbance decoupling control, a disturbance torque is obtained for the first arm by using an output signal of the first sensor and an output signal of the second sensor, and is fed back to an input torque to thereby reduce the disturbance torque. By performing the decoupling control and obtaining the disturbance torque for the second arm using the output signal of the first sensor and the output signal of the second sensor and feeding it back to the input torque, the disturbance torque is reduced. Since decoupling control is performed, interference between each arm of the robot can be compensated based on the dynamics of the robot, so that even when the robot is operating at high speed, it is possible to ensure significantly high followability with respect to the reference trajectory. .

According to a third aspect of the present invention, in the robot decoupling control method according to the first aspect, the angular velocity of the first arm and the second angular velocity are controlled by the first sensor and the second sensor. In the disturbance decoupling control, a disturbance torque is detected by a disturbance observer using the output signal of the first sensor and the output signal of the second sensor in the disturbance decoupling control. Estimation and feedback to the input torque perform decoupling control of the disturbance torque, and a disturbance observer for the second arm using the output signal of the first sensor and the output signal of the second sensor. The disturbance torque is estimated and fed back to the input torque to perform decoupling control of the disturbance torque. To be able to compensate for interference between beam also can secure high followability to the reference trajectory at the time of high-speed operation of the robot, and can reduce the number of sensors.

[Brief description of the drawings]

FIG. 1 is a block diagram showing a decoupling control method of an embodiment of a horizontal two-axis scalar robot to which the inventions according to claims 1 and 2 are applied.

FIG. 2 is a block diagram showing an equivalent circuit of FIG. 1;

FIG. 3 is a front view showing a part of the embodiment.

FIG. 4 is a block diagram showing another part of the embodiment.

FIGS. 5A and 5B are a side view and a front view showing a state where the acceleration sensor is attached to the first arm of the embodiment. FIGS.

FIG. 6 is a side view and a front view showing a state where the acceleration sensor is attached to the second arm of the embodiment.

FIG. 7 is a diagram showing a reference coordinate system of a scalar robot and a rotating coordinate system fixed to each arm.

FIG. 8 is a functional block diagram illustrating an interference torque calculator according to the embodiment.

FIG. 9 is a diagram showing a mathematical model of a horizontal two-axis scalar robot.

FIG. 10 is a state variable diagram showing an equation of motion of an arm of the robot.

FIG. 11 is a block diagram illustrating a model of an arm of a horizontal two-axis scalar robot.

FIG. 12 is a block diagram showing an equivalent circuit of FIG. 11;

FIG. 13 is a front view showing a part of an embodiment of a horizontal two-axis scalar robot to which the inventions according to claims 1 and 3 are applied.

FIG. 14 is a block diagram showing another portion of the embodiment.

FIG. 15 is a state variable diagram showing an equation of motion of the robot arm.

FIG. 16 is a block diagram showing a disturbance observer.

FIG. 17 is a block diagram illustrating a feedforward angular acceleration control method according to the embodiment.

[Description of Signs] 1, 2 Arms 3, 4 Joints 11 to 14 Acceleration Sensor 17 Computer 18 Expansion I / O Unit 19, 20 Low Pass Filter 21 Servo Amplifier 22 A / D Converter 23 D / A Converter 24 Encoder Counter 25 Parallel I / O port 26, 27 Vibrating gyro sensor

Claims (3)

[Claims] 1. A first arm rotatably provided, a first joint to which a root end of the first arm is attached, and a first arm attached to a pivot end of the first arm. A second joint, a second arm having a root end attached to the second joint, a first motor for rotating the first joint, and a second motor for rotating the second joint. A non-interacting control method for a robot, comprising: a motor; and control means for controlling the first motor and the second motor to perform positioning control of the first arm and the second arm. The angular acceleration or angular velocity of the first arm is detected by a first sensor; the angular acceleration or angular velocity of the second arm is detected by a second sensor; Output signal and the output of the second sensor Signal to control the disturbance to the first arm using the signal, and to reduce the disturbance to the second arm by using the output signal of the first sensor and the output signal of the second sensor. A decoupling control method for a robot, comprising performing control. 2. The method for controlling decoupling of a robot according to claim 1, wherein the angular acceleration of the first arm and the angular acceleration of the second arm are respectively determined by the first sensor and the second sensor. In the disturbance non-interference control, a disturbance torque is obtained for the first arm by using the output signal of the first sensor and the output signal of the second sensor, and is fed back to the input torque. By performing disturbance torque decoupling control and obtaining a disturbance torque for the second arm using the output signal of the first sensor and the output signal of the second sensor, and feeding it back to the input torque. A non-interacting control method for a robot, which performs non-interacting control of disturbance torque. 3. The method for controlling decoupling of a robot according to claim 1, wherein the first sensor and the second sensor detect an angular velocity of the first arm and an angular velocity of the second arm, respectively. In the disturbance decoupling control, the first
A disturbance torque is estimated by a disturbance observer using the output signal of the first sensor and the output signal of the second sensor for the arm of the first arm and fed back to the input torque, thereby performing the control of decoupling the disturbance torque. With the second
A disturbance torque is estimated by a disturbance observer using the output signal of the first sensor and the output signal of the second sensor for the arm of the first arm and fed back to the input torque, thereby performing the control of decoupling the disturbance torque. A non-interacting control method for a robot.