JP6834123B2 - Robot control device - Google Patents

Description

The present invention relates to a robot control device that controls a robot.

In the field of robot control, it is common practice to simulate the motion of a modeled robot in advance and apply a desired simulation result, for example, a control content obtained with a desired vibration suppression effect to an actual robot. .. In addition, various studies have been made on vibration suppression control for suppressing vibration of a robot. For example, Patent Documents 1 and 2 propose a control device capable of operating a robot while suppressing vibration.

Japanese Patent No. 4038659 Japanese Patent No. 5411687

However, in the simulation, when the control content obtained with the desired vibration suppression effect is applied to an actual robot, the performance as in the simulation may not be exhibited. Specifically, vibrations that did not occur in the simulation were generated in the actual robot.
The present invention has been made in view of the above circumstances, and an object of the present invention is to provide a robot control device capable of suppressing vibration that does not appear in a simulation.

In the field of robot control, a so-called two-inertial frame simulation considering a vibration mode due to a spring element existing in a speed reducer between a motor and an arm is used. This vibration mode is vibration caused by the rigidity of the operating direction in the power transmission mechanism through which the power of the speed reducer or the like is transmitted. Moreover, the simulation of the bi-inertial system is widely used, and its effectiveness is considered to be recognized.

At this time, mechanical values such as arm rigidity are naturally included in the simulation conditions, and the actual robot is designed within the range of mechanical values that satisfy the simulation conditions. The fact that the robot vibrates in spite of this is considered to mean that there is a vibration mode different from the vibration mode considered in the conventional two inertial system.

Then, as a result of repeated investigations of the cause of the vibration, the inventors of the actual robot vibrate due to the rigidity of the operation direction of the power transmission mechanism in the vibration mode of the two inertial system (hereinafter, the vibration in the operation direction for convenience). In addition to (referred to as), it was found that there is vibration different from the operating direction of the power transmission mechanism (the above-mentioned non-operating direction vibration). In other words, the inventors have determined that the non-operating direction vibration in a direction different from the operating direction is the cause of the vibration that did not appear in the simulation, and the non-operating direction vibration causes the moving part side of the robot as a whole. As a result, it became clear that torsional vibration was generated in the support part of the shaft, and the torsional vibration appeared as vibration of the hand.

Now, if the existence of non-operating directional vibration has been recognized so far, a control method for coping with non-operating directional vibration should be considered. However, in reality, no consideration suggesting the existence of non-operating directional vibration or a control method for suppressing non-operating directional vibration has been studied. In other words, it is considered that the non-operating directional vibration has not been recognized until now. Therefore, the inventors considered why the non-operating directional vibration was not considered so far.

Compared to recent robots, the earliest industrial robots had significantly thicker and stronger arms, gears, and bearing members, but the overall weight of the arms and moving parts was relatively heavy. It was. In the case of a two-inertial frame, the resonance frequency is proportional to the square root of the reciprocal of inertia (that is, weight), so that the resonance frequency of the vibration in the operating direction becomes low. On the other hand, the members and the like constituting the arm are made to have extremely high rigidity, and it is considered that the resonance frequency is high even if the non-operating directional resonance exists.

In general, when a plurality of resonances are present, the influence of the resonance having a low resonance frequency is often dominant. That is, since the resonance frequency of the non-operating direction resonance was relatively high, it is considered that the influence on the position response and the speed response of the arm was negligibly small. In addition, as a result of the research by the inventors, it was found that the cause of the non-operating direction vibration is the existence of a force applied in the non-operating direction such as centrifugal force, but the earliest robots are recent robots. Since the operating speed was relatively slow compared to that of the above, it is considered that the influence of centrifugal force was negligible.

On the other hand, in recent years, the design of robots has changed from making the arm thick and sturdy to making it thinner and lighter. In other words, while the weight of the arm has been reduced, the members that make up the arm have lower rigidity than the earliest robots. Although the rigidity is reduced, the arm itself, which is called a flexible arm, is not in a twisted state. For example, a shaft support part such as a cross roller causes a slight vibration in a direction other than the rotation axis. It means that it is occurring.

Therefore, in the state where multiple resonances exist, the dominant resonance has been replaced with the earliest robot, or the resonances of both have similar resonance frequencies and affect each other. Conceivable. Furthermore, in the case of robots in recent years, the operating speed has been significantly increased as compared with the earliest robots, and it is considered that the influence of centrifugal force proportional to the square of the speed has become more prominent.

For example, in the case of a 4-axis horizontal articulated robot, it has been confirmed that the support portion that linearly moves the shaft in the vertical direction (Z direction) vibrates in conjunction with the movement of the second arm, for example. .. In this case, vibration different from vibration in the moving direction of the shaft (included in the operating direction vibration) such as vibration due to inter-axis interference, specifically, the circumferential direction of the second arm (operating direction of the second arm) or Vibration is generated in the radial direction of the second arm (the direction of the centrifugal force applied to the second arm). In addition, even in robots having other configurations such as a 6-axis vertical articulated robot (PUMA (Programmable Universal Manipulation Arm) type robot) and a so-called 7-axis robot, non-operation direction vibration occurs due to a phenomenon similar to this. doing.

In an industrial robot, a plurality of arms are often connected to form a robot, so that a phenomenon occurs in which one axis vibrates and another axis interferes with the vibration. For this reason, the vibration characteristics themselves are not as clear as in a simple two-inertial system model, and multiple resonance vibrations appear in the frequency specification, and the resonance frequency and the actual vibration waveform appearing in the frequency characteristics. There may be a slight difference from the resonance frequency that appears in. Therefore, even if the non-operating direction vibration is present, it is considered that it is difficult to find out that the non-operating direction vibration is the cause.

Further, although the vibration frequency is often different between the operating direction vibration and the non-operating direction vibration, when a wave gear device is used as the speed reducer, for example, its rigidity may change according to the input torque. Since there are various factors that cause modeling errors, such as those known, it is probable that no one came up with vibrations in the non-operating direction and that they were simply treated as errors.

Due to these circumstances, the vibration that has the greatest influence among the vibrations that appear in the simulation is treated as the operating direction vibration, and the others are treated as errors such as interference from other axes. It is probable that no consideration has been given to. In other words, the non-moving direction vibration is not so remarkable in the earliest robots, and the non-moving direction vibration is treated as an error in recent robots, which is the reason why the non-moving direction vibration was not recognized. Was speculated.

The existence of such non-operating direction vibration is an extension of the two-inertial frame model and the two-inertial frame model including interference, which are the preconditions of the conventional simulation, for example, the control of the four-inertia model referred to in Patent Document 1. model shows that did not completely originally expressed the vibration characteristics of the actual robot is in, we have a very serious technical significance.

Now, if the existence of non-operating directional vibration is found, if control is performed to suppress the vibration caused by the non-operating directional vibration, the vibration that appears in the hand that did not appear in the simulation can be suppressed. Is thought to be possible.
For example, in the case of a horizontal articulated robot, torsional vibration is generated at the support part (shaft support part) of the Z-axis linear motion part (shaft) in conjunction with the movements of the first and second axes, which are the rotation axes. It was. Then, this torsional vibration occurs not only in the rotation of the rotation axis and the Rz axis (see FIG. 1 described later) of the first and second axes, but also in the Rx axis direction and the Ry axis direction. Therefore, if the vibration is detected, it is considered that the vibration appearing at the hand can be suppressed.

However, the conventional robot is not provided with a sensor for detecting the vibration in the Rx-axis direction and the Ry-axis direction, and cannot detect the vibration in the Rx-axis direction and the Ry-axis direction itself. Therefore, it was not possible to detect the occurrence of vibration and control the suppression of the vibration . This means that the conventional robot, since the non-operating direction vibration is not considered, not provided originally a means for detecting the vibration of the non-rotational direction, since no can detect itself inoperative direction vibration , it could not even be controlled to converge the inoperative direction vibration.

Moreover, it is more difficult than generally imagined to provide a sensor at the hand of a robot. For example, when a wired sensor is installed at the hand of a robot, a protective member such as a so-called cable reel is required to protect the cable from the rotation of each arm, the rotation of the shaft, and the vertical movement, and the wiring structure is complicated. To become. However, if a wireless sensor is provided at hand, it is not easy to supply power or charge the sensor while the robot is operating, and there is a risk that wireless communication will be interrupted due to noise.

Therefore, in the invention according to claim 1, the robot control device obtains the torque applied to the shaft support portion that supports the shaft from the detection results of a plurality of sensors provided on the first arm and the second arm of the horizontal articulated robot. It is estimated, and based on the estimated torque, the twist angle at the shaft support portion that correlates with the vibration of the tip portion of the shaft is obtained.

In other words, the control device correlates with the torsional vibration generated in the shaft from the detection results of the sensors provided on each arm, not the robot's hand (the tip of the shaft), and the robot's hand, that is, the shaft. The twist angle at the shaft support, which is also correlated with the vibration of the tip, is calculated.

In this case, the specific calculation method is as follows, assuming that the torque applied to the shaft support is N3, the twist angle at the shaft support is φ3, the rigidity of the shaft support is K3, and the detection values of the multiple sensors are S1 to SN. It can be described by a mathematical formula as follows.
φ3 = φ3est (N3)
N3 = N3est (S1-SN)
However, φ3est is a function for obtaining the twist angle at the shaft support portion from the torque applied to the shaft support portion, and N3est is a function for obtaining the torque applied to the shaft support portion from the detected values of a plurality of sensors. Similarly in the following, the function with est at the end is a function that estimates the target value from the sensor detection value or another estimated value.

In this case, with respect to the first arm, the torque applied to the first arm support portion can be obtained from the detection result of the sensor provided on the first arm. In addition, since each arm is linked in the robot, the second arm is viewed only with the second arm from the detection result of the sensor provided on the first arm and the detection result of the sensor provided on the second arm. The value of the case can be obtained, and the torque applied to the second arm support portion can be obtained from the obtained value. Similarly, with respect to the shaft, the torque applied to the shaft support portion can be obtained from the detection results of each sensor.

Then, if the torque applied to the shaft support portion is obtained, the twist angle of the shaft can be obtained based on the rigidity of the shaft.
In this case, the specific calculation method can be described by a mathematical formula as follows, assuming that the rigidity of the shaft support portion is K3.
φ3 = φ3est (N3) = N3 ÷ K3
In this way, by utilizing a plurality of detection results from the sensors provided on each arm, even if the sensor is not provided on the robot's hand, the twist at the shaft support portion that causes vibration of the robot's hand Vibration can be calculated.
Further, since the sensor is provided on each arm, the wiring structure thereof can be simplified as compared with the case where the sensor is provided on the hand.

More specifically, the robot controller calculates the inertial forces and inertial torques in each arm from the detection result of the sensor provided on each arm, the obtained inertial force and estimating the torque applied to the shaft supporting portion from the inertial torque Then, the twist angle at the shaft support is obtained based on the estimated torque.
In this case, the specific calculation method is as follows, assuming that the inertial force of the first arm is FG1, the inertial torque of the first arm is NG1, the inertial force of the second arm is FG2, and the inertial torque of the second arm is NG2. Can be described.

FG1 = FG1est (S1-SN)
FG2 = FG2est (S1-SN)
NG1 = NG1est (S1-SN)
NG2 = NG2est (S1-SN)
N3 = N3est (FG1, FG2, NG1, NG2)
The torque applied to the shaft support can be estimated by such a procedure and a mathematical formula.

Specifically, the sensor provided on each arm is composed of any combination of an inertial sensor, a rotation angle sensor, and a position sensor that detects the vertical position of the shaft, as in the invention of claim 3. ing. For example, the velocity and acceleration can be obtained by calculation if either one is detected. However, in that case, the calculation time is required. In other words, if the calculation is unnecessary, the processing speed can be improved by that amount. Then, the inertial sensor can directly detect the inertial force, the rotation angle sensor can directly detect the rotation angle, and the position sensor can directly detect the vertical position of the shaft.
In this way, the processing speed can be improved by using a sensor that can directly detect various physical quantities required for estimating the torque applied to the shaft support portion.

More specifically, as in the invention of claim 4, by using an angular velocity sensor that detects angular velocity or an acceleration sensor that detects acceleration as an inertial sensor, a calculation for obtaining torque at each arm support portion can be performed. It can be omitted or simplified.
As the angular velocity sensor, as in the invention of claim 5, an angular velocity sensor that detects angular velocities in a plurality of directions can be used.
As the acceleration sensor, as in the invention according to claim 6, an acceleration sensor that detects acceleration in a plurality of directions can be used.

As the rotation angle sensor, as in the invention of claim 7, a motor encoder provided in a motor that rotationally drives the first arm with respect to the base can be used.
Further, as the rotation angle sensor, as in the invention of claim 8, a motor encoder provided in a motor for rotationally driving the second arm with respect to the first arm can be used.
Further, as the rotation angle sensor, as in the invention of claim 9, a motor encoder provided in a motor that rotationally drives the shaft with respect to the second arm can be used.
Further, as the position sensor, a motor encoder provided in a motor that drives the shaft up and down can be used as in the invention of claim 10.
By using a motor encoder generally provided for a motor that drives an arm or a shaft in this way, it is possible to acquire a rotation angle and a vertical position of the shaft without requiring an additional member.

In the invention according to claim 11, for a state space model including a resonance characteristic that is defined in advance by an equation of motion with respect to the position of the center of gravity of the shaft that changes according to the vertical position of the shaft and causes a twist angle of the shaft support portion. Design the control system. That is, the state space model is defined in consideration of the change in the vertical position of the shaft.
The vibration at the tip of the shaft, that is, the hand of the robot, is considered to be relatively large when the shaft is located below and the center of gravity is located below, while the shaft is located above and the center of gravity is relatively located. If it is above, it is considered to be relatively small. That is, it is considered that the vibration of the tip of the shaft changes depending on the position of the center of gravity of the shaft. The position of the center of gravity of the shaft is considered to change depending on the vertical position of the shaft.
Therefore, by defining the state space model by the equation of motion with respect to the position of the center of gravity of the shaft, it is possible to design the control system in a state in which the influence of the change in the position of the center of gravity of the shaft is added to the state space model. Then, by performing vibration suppression control on the control system, it is possible to effectively suppress the vibration of the robot's hand.

In this case, vibration suppression control can be performed by feedback control by constructing a state feedback and a state observer for the state space model as in the invention according to claim 12. In addition, by constructing a state observer, it is possible to estimate the vibration of the hand whose physical quantity cannot be obtained directly.
Further, as in the invention of claim 13, by including the moment of inertia in the direction orthogonal to the operating direction at the position of the center of gravity of the shaft in the state equation, a state in which the moment of inertia in the non-operating direction different from the operating direction is added. Vibration can be suppressed, and vibration in the non-operating direction can be effectively suppressed.

The figure which shows typically the structure of the robot and the robot control device by 1st Embodiment. The figure which shows typically the Ry rotation which occurs in the first arm The figure which shows the sensor position schematically The figure which shows typically the sensor position in 2nd Embodiment Block diagram showing the feedback system according to the third embodiment

Hereinafter, a plurality of embodiments of the present invention will be described with reference to the drawings. The parts that are substantially common in each embodiment are designated by the same reference numerals, and detailed description thereof will be omitted.
(First Embodiment)
The first embodiment of the present invention will be described with reference to FIGS. 1 to 3.

As shown in FIG. 1, the robot system 1 includes a robot 2 and a control device 3 (corresponding to a robot control device) that controls the robot 2. The control device 3 includes a control unit 3a and controls the robot 2. Although the details will be described later, the control unit 3a estimates the torque applied to the shaft support unit 6a (see FIG. 3) that supports the shaft 7 based on the detection results of the sensors 8, 9, and 10 (see FIG. 3). Various processes described in the embodiments are performed, including obtaining the twist angle at the shaft support portion 6a based on the estimated torque. The twist angle at the shaft support portion 6a correlates with the torsional vibration, and also correlates with the vibration of the hand of the robot 2, that is, the tip portion 7a of the shaft 7.

The robot 2 of the present embodiment has a base 4, a first arm 5 attached to the base 4 (supported by the base 4), and a first arm 5 and a first arm 5 that rotate about the first axis (J1) with respect to the base 4. Attached to (supported by the first arm 5) and rotated about the second axis (J2) with respect to the first arm 5, and attached to the second arm 6 (supported by the second arm 6). A horizontal articulated robot having a shaft 7 that moves up and down and rotates with respect to the second arm 6 is assumed.

In this embodiment, the shaft 7 assumes a ball screw spline. In this case, the hand of the robot 2 corresponds to the lower end side of the shaft 7, that is, a portion that moves relative to the second arm 6 in the vertical direction and rotates relative to the second arm 6. The shaft 7 is designed to have extremely high rigidity, and it is unlikely that the shaft 7 itself will bend or deform. However, even if the rigidity of the shaft 7 is low and the shaft 7 itself is bent or deformed, the same can be achieved by equivalent conversion assuming that the bending or deformation occurs in the shaft support portion 6a. The effect of can be obtained.

Hereinafter, as shown in FIG. 1, the shaft support portion 6a is the origin, the vertical direction coaxial with the shaft 7 is the Z axis (Z direction), the direction orthogonal to the Z axis is orthogonal to the Z axis, and the direction along the second arm 6 is the X axis ( The X direction), and the directions orthogonal to the Z axis and the X axis will be described as the Y axis (Y direction). Further, the rotation around the Z axis shown in FIG. 1 is referred to as Rz rotation, the rotation around the X axis is referred to as Rx rotation, and the rotation around the Y axis is referred to as Ry rotation.

In this case, the Rx rotation corresponds to a state in which the second arm 6 rotates around the shaft support portion 6a in the state shown in FIG. 1, or more simply, a state in which the second arm 6 is twisted. Further, the Ry rotation is a state in which the second arm 6 rotates about the Y axis around the shaft support portion 6a in the state shown in FIG. 1, taking the second arm 6 as an example, or more simply, the second arm 6 This corresponds to a state in which the tip side swings in the Z direction.

As shown in FIG. 3, the robot 2 includes a plurality of sensors 8, 9, and 10. Of these, the sensors 8 and 9 are inertial sensors provided on the first arm 5 and the second arm 6 to detect the inertia of each arm, and the sensor 10 is provided on the second arm 6 to detect the vertical position of the shaft 7. It is a position sensor. The arrangement of the sensors 8 and 9 can be basically arbitrarily set.

More specifically, in the present embodiment, the sensor 8 is provided on the first arm 5, and includes a biaxial angular velocity sensor that detects the angular velocity of Rx rotation and the angular velocity of Ry rotation when the first arm 5 operates. The angular velocity generated in a direction different from the operating direction (non-operating direction) of the first arm 5 shown in FIG. 2 is detected. Further, the sensor 9 is provided on the second arm 6 and is composed of a biaxial angular velocity sensor that detects the angular velocity of Rx rotation and the angular velocity of Ry rotation when the second arm 6 operates, and is composed of an operating direction in the second arm 6. Detects the angular velocity that occurs in a direction different from (non-operating direction).

Further, the sensor 10 is provided on the shaft support portion 6a in the second arm 6 and is a motor encoder that detects the rotation angle of the motor that drives the shaft 7 up and down. Although not shown, each of the sensors 8, 9 and 10 uses a wired type sensor connected by a cable. Although not shown, motor encoders are also provided in the motor that drives the first arm 5 and the motor that rotationally drives the second arm 6.

Next, the operation of the above configuration will be described.
The vibration frequency of the torsional vibration at the shaft support portion 6a, which is a problem, is often sufficiently lower than the vibration frequency of the resonance vibration in the rotation direction of the reduction mechanism. As an example, for example, the vibration frequency of the torsional vibration in the support portion of the linear motion portion of the shaft 7 is about 10 Hz or less, while the vibration frequency of the resonance vibration in the rotation direction in the reduction mechanism is about 50 Hz. When the shaft 7 is a ball screw spline or the like, its resonance vibration frequency may exceed 100 Hz.

When the difference in vibration frequency is large as described above, there is no problem in using the value of the motor encoder as the Rz axis rotation angle of the arm or the Z axis position (shaft position). If resonance vibration in the rotation direction becomes a problem, an encoder (linear encoder, etc. in the case of shaft 7) may be provided on the output side of the speed reducer, an angular velocity sensor or acceleration sensor may be added, or a state observer may be used. By estimating the resonance vibration, the influence of the resonance vibration can be excluded.

Therefore, first, various parameters and variables in each arm are defined as follows. In the following, various parameters and variables are collectively referred to as parameters. Further, due to the characters that can be used, for example, the twist angle of the first arm support portion is indicated as "φ" in the specification.
<< Parameters related to the arm >>

<< Parameters related to the arm >>

<< Parameters related to shaft 7 >>

Of these parameters, the following parameters can be obtained by calculating the values detected by the sensors 8 to 10 or the detected values.

The angular velocity of the first arm 5 and the second arm 6 at the center of gravity is the same as the angular velocity at the center of gravity regardless of which point in the first arm 5 and the second arm 6 is measured.
In addition, each parameter has the following relationship.

By the way, before describing the torque estimation method, the simplification of the calculation described later will be described.
For rotational vibration, let the axis of rotation be the unit vector v and the amount of rotation be φ. However, the unit vector v is the following equation (3). At this time, the rotation matrix R is represented by the following equation (4) using Rodrigues' Rotation Formula. The matrix V in the rotation matrix R is defined by the unit vector v as shown in the following equation (5). Then, the unit vector v and the rotation amount φ can be obtained as in the following equations (6) and (7).

Here, for example, assuming that the total arm length of the first arm 5 and the second arm 6 is 1000 mm and the vibration appearing on the hand is about 1 mm, the rotation amount φ is 0.001 (rad). At this time, since φ << 1, the rotation matrix R can be expressed as the following equation (8) by approximating sinφ = φ and cosφ = 1.

Here, since v is a unit vector, it can be seen that the absolute value of the off-diagonal component of the rotation matrix R is φ or less. This indicates that the value of another axis is superimposed as an error by φ at the maximum with respect to the value of one axis.
By the way, the degree of influence from another axis orthogonal to the axis to be measured is generally called the other axis sensitivity, but it is because the rotation matrix is limited to Rz rotation by using the values approximated as described above. The effect on the sensitivity of the other axis is 0.1%.

On the other hand, the other axis sensitivity of the inertial sensors such as the sensors 8 and 9 is generally about several percent (for example, about 4%). Therefore, the error due to limiting the rotation matrix to Rz rotation is one digit or more smaller than the error due to the sensitivity of the other axis of the inertial sensor itself. Therefore, even if only the rotation in the Rz axis direction is considered and the influence of the twist angle is ignored, it does not pose a big problem in practical use, that is, in actually controlling the robot 2. Therefore, in the present embodiment, when obtaining the torque applied to the shaft support portion 6a as described below, only the rotation in the Rz axis direction is considered for the rotation matrix, and the influence of the twist angle is ignored. Will be able to be simplified.
By the way, when the calculation is simplified in this way, each rotation matrix can be expressed as the following equations (9) to (11).

Of course, without simplification, it is possible to estimate the torque by the following method even when using a rotation matrix that takes into account Rx rotation and Ry rotation , including the effect of the twist angle at each support. ..
The acceleration, inertial force, and inertial torque at the center of gravity of the first arm 5 and the second arm 6 are as shown in the following equations (12) to (17).

From these, the forces and torques of the first arm support portion and the second arm support portion are as shown in the following equations (18) to (21).

Further, the torques of the first arm support portion and the second arm support portion can be obtained by using the rigidity of the support portion as in the following equations (22) and (23). However, the torque in the Rz axis direction may not be obtained strictly by using the torsional rigidity, but the motor torque may be converted into the torque in the Rz axis direction of each arm support portion by the gear ratio and used.

Then, the force of the second arm support portion is obtained from the torque of the first arm support portion. However, the force of the second arm support portion obtained here is not uniquely obtained at this point, and is in the direction of the link length vector of the first arm 5, for example, λL1 in equation (25) and λR2L1 in equation (26). It is obtained with a horizontal unknown term included. Then, the force of the shaft support portion 6a is obtained from the obtained force of the second arm support portion as shown in the following equation (27). Note that NF21 and F20 in the following equations are defined to replace the equations.

Subsequently, the force of the shaft support portion 6a is eliminated from the torque of the second arm support portion as in the following equations (28) and (29).

From this equation (28), it was clarified that the unknown term is represented by the outer product of the vector obtained by rotating the first arm link length by Rz by the second arm rotation angle and the link length of the second arm 6. Although these link length vectors have large values only in the X-axis direction, they have small values in the Y-axis direction or the Z-axis direction.
Therefore, when the rotation angle of the second arm is 0 °, the unknown term becomes a very small value, so that it can be ignored. Further, when the rotation angle of the second arm is 90 °, the unknown term becomes a vector in the Rz axis direction, and the influence in the Rx axis direction or the Ry axis direction can be ignored.

That is, although the above equation contains an unknown term, its influence may have a large effect in the Rz axis direction, but its influence can be ignored in the Rx axis direction or the Ry axis direction. If this is expressed as an equation, if the link length vector has a value only in the X-axis component, the unknown term expresses the influence only in the Rz-axis direction. Therefore, the following equations (30) to (32) Can be shown as However, "x" in the equation (32) indicates the outer product.

Next, the torque of the shaft support portion 6a is obtained from the torque of the second arm support portion by the following equations (33) to (35). Then, from the torque and rigidity of the shaft support portion 6a, the twist angle of the shaft support portion can be obtained as shown in the following equation (36). However, the torque of the shaft support portion 6a is also affected by the unknown term in the Rz axis direction described above, and due to the influence of this unknown term, the twist angle of the shaft support portion is only the twist angle in the Rx axis direction and the Ry axis direction. Is derived as valid.

As for the twist angle of the shaft support portion, the torque in the Rz axis direction is obtained by converting the motor torque of the fourth axis by the gear ratio, as in the case of the first arm support portion or the second arm support portion, and the torque in the Rz axis direction is used to obtain the torque in the Rz axis direction. The twist angle of the shaft support may be obtained.
As described above, the control device 3 of the present embodiment estimates the torque of the shaft support portion 6a based on the detection results of a plurality of sensors provided on each arm instead of the hand, and obtains the estimated torque of the shaft support portion 6a. Based on this, the torsional angle of the shaft support portion, that is, the torsional vibration at the shaft support portion 6a, which causes the vibration of the hand, is obtained.

According to the embodiment described above, the following effects can be obtained.
The robot control device (control device 3) supports the shaft 7 from the detection results of a plurality of sensors 8, 9, and 10 provided on the first arm 5 and the second arm 6 of the horizontal articulated robot 2. The torque (N3) applied to the portion 6a is estimated, and the twist angle at the shaft support portion 6a that correlates with the vibration of the tip portion 7a of the shaft 7 is obtained based on the estimated torque (N3) and the rigidity of the shaft 7. ing.

That is, the control device 3 correlates with the torsional vibration generated in the shaft 7 from the detection results of the sensors 8, 9 and 10 provided on each arm, not the hand of the robot 2 (the tip portion 7a of the shaft 7). At the same time, the twist angle at the shaft support portion 6a, which correlates with the vibration of the hand of the robot 2, that is, the tip portion 7a of the shaft 7, is obtained.

From the detection result of the sensor 8 provided on the first arm 5, the torque applied to the first arm support portion can be obtained. Further, since each arm of the robot 2 is linked, the robot 2 is provided on the detection result of the sensor 8 provided on the first arm 5 and the second arm 6 which can move relative to the first arm 5. From the detection result of the sensor 9, the value when viewed only by the second arm 6 can be obtained, and the torque applied to the second arm support portion can be obtained from the obtained value. Similarly, the torque applied to the shaft support portion 6a can be obtained from the detection results of the sensors 8 to 10.
Then, if the torque applied to the shaft support portion 6a is obtained, the twist angle of the shaft 7 can be obtained based on the rigidity of the shaft 7.
As a result, the torsional vibration at the shaft support portion 6a, which causes the vibration of the hand of the robot 2, can be obtained without providing the sensor at the hand of the robot 2.

At this time, since the sensors 8, 9, and 10 are provided on each arm, the wiring structure thereof can be simplified as compared with the case where the sensors 8, 9, and 10 are provided on the hand. Further, since it is a wired type, problems such as power supply do not occur, and communication is not interrupted due to noise unlike a wireless type, so that reliability can be improved.

More specifically, the control device 3 obtains the inertial force in each arm from the detection results of the sensors 8, 9 and 10 provided on each arm, and each arm support portion that supports each arm from the obtained inertial force. Find the torque at. Then, the control device 3 estimates the torque applied to the shaft support portion 6a from the obtained torque at each arm support portion, and obtains the twist angle at the shaft support portion 6a based on the estimated torque.
The plurality of sensors 8, 9, and 10 are composed of any combination of an inertial sensor, a rotation angle sensor, and a position sensor that detects the vertical position of the shaft 7.

For example, the velocity and acceleration can be obtained by calculation if either one is detected. However, in that case, the calculation time is required. In other words, if the calculation is unnecessary, the processing speed can be improved by that amount. Then, the inertial sensor can directly detect the inertial force, the rotation angle sensor can directly detect the rotation angle, and the position sensor can directly detect the vertical position of the shaft 7.

As described above, the processing speed can be improved by using the sensor capable of directly detecting various physical quantities required for estimating the torque applied to the shaft support portion 6a.
More specifically, by using an angular velocity sensor that detects the angular velocity as the inertial sensor, it is possible to omit or simplify the calculation when obtaining the torque at each arm support portion. Further, by using a motor encoder generally provided in a motor for driving the shaft 7 as a position sensor, it is possible to acquire the vertical position of the shaft 7 without requiring an additional member.
Further, since the arrangement of the sensors 8 and 9 can be basically arbitrarily set, it is possible to prevent the wiring from becoming excessively complicated.

Further, in the case of the present embodiment, since vibration can be detected at all times, the vibration characteristics are the outside air temperature (temperature fluctuation due to day / night and season), aging deterioration, or warm-up operation state (normal time and immediately after long holidays). Considering that it fluctuates due to (difference) and the like, it is possible to exhibit the expected vibration suppression effect, unlike a configuration in which the sensor is used only during test operation and the sensor is not used during actual operation.
Further, the torque applied to the shaft support portion 6a supporting the shaft 7 is estimated from the detection results of a plurality of sensors provided on the first arm 5 and the second arm 6, and the tip portion of the shaft 7 is estimated based on the estimated torque. The same effect can be obtained by a control method for obtaining the twist angle at the shaft support portion 6a that correlates with the vibration of 7a.

(Second Embodiment)
Hereinafter, the second embodiment will be described. In the case of the second embodiment, the installation mode of the sensor is different from that of the first embodiment.
As shown in FIG. 4, in the case of the present embodiment, the first arm 5 is provided with sensors 11 and 12, which are two-axis acceleration sensors, instead of the sensor 8 (angular velocity sensor) of the first embodiment. Although not shown, the second arm 6 is also provided with two 2-axis acceleration sensors. Further, the arrangement of each sensor including the sensors 11 and 12 can be basically set arbitrarily.

First, the first arm 5 will be described. CL1 is a virtual straight line passing through the uniaxial rotation center (point C1 on the J1 axis) and the biaxial rotation center ( point on the J2 axis), and the uniaxial rotation center along CL1 (point C1 on the J1 axis). ) And the respective measurement positions of the two sensors 11 and 12, respectively, are LS11X and LS12X. Further, LS11Y the distance from C L1 in the Y direction, and LS12Y, LS11Z the distance in the Z axis direction from CL1, and LS12Z. Further, it is assumed that the measurement axes of the sensors 11 and 12 are the Y axis and the Z axis.
At this time, the acceleration detected by the sensors 11 and 12 is expressed by the following equation (37), where n is the number of sensors.

Then, from the Y-axis acceleration, the Rx- axis angular acceleration and the Rz-axis angular acceleration are derived from the following equations (38) as equations (41).

Similarly, from the Z-axis acceleration, the Rx-axis angular acceleration and the Ry-axis angular acceleration are derived from the following equations (42) as in equation (45).

Then, the angular velocities can be obtained by integrating the obtained angular accelerations. The Rx-axis angular acceleration can be obtained from any acceleration, but for example, the average value is used, the error of the angular velocity and the angular acceleration is corrected so as to be small, or the value having the larger denominator is used. The accuracy can be improved by such means.

Next, consider the case of the second arm 6. In the case of the second arm 6, by subtracting the acceleration of the center of rotation of the two axes (point on the J2 axis) from the detection result of the sensor, the X-axis, Y-axis, and Z-axis can be calculated in the same manner as in the case of the first arm 5 described above. The angular velocities of can be obtained respectively. In this case, the acceleration at the center of biaxial rotation can be obtained from the angular velocity or the angular acceleration of the first arm 5.
Then, if the angular velocity in each arm is obtained, it is substituted into the equation (1) and calculated in the same manner as in the first embodiment to obtain the twist angle of the shaft support portion, that is, the hand position when the hand vibrates. It is possible to obtain the same effect as that of the first embodiment.

At this time, the sensor position is basically arbitrary, but by devising the sensor position, the calculation can be performed efficiently. For example, in the case of the first arm 5, it may be arranged as LS11X = LS12X, LS11Y = LS12Y, LS11Z = LS12Z. Further, it is even better to set LS11Z = LS12Z = 0. Considering the second arm 6, these relationships can be expressed as equations (46) to (48) below.

LSm 1 X = LSmnX ... (46)
LSm 1 Y = L SmnY ... (47)
LSm 1 Z = LSmnZ (= 0) ... (48)
Note that n is the number of sensors, where m = 1 is the first arm 5 and m = 2 is the second arm 6. That is, if n = 1 and m = 1, for example, LS11X represents the arrangement of the sensor 11 (distance from the first axis (J1)), and if n = 2 and m = 1, for example, LS12Y is the sensor 12. Represents the arrangement (distance from the straight line (CL1)).

(Third Embodiment)
Hereinafter, the third embodiment will be described. In the third embodiment, in the robot control device shown in the first embodiment and the second embodiment described above, a method for suppressing the vibration appearing at the hand will be described. The configuration is the same as that of the first embodiment . Further, it will be described with reference to FIGS. 1 to 4.

First, as described in the first embodiment described above, in the robot 2 which is a horizontal articulated robot, the shaft 7 is moved in conjunction with the movements of the first axis (J1) and the second axis (J2) which are the rotation axes. Oite torsional vibration occurs in the support shafts supporting section 6 a. And this torsional vibration is generated not only in the rotation of the rotation axis and the Rz axis of the first axis and the second axis, but also in the Rx axis direction and the Ry axis direction.

However, in the case of a conventional general robot, although a motor encoder is attached as an angle sensor to the fourth axis which is the rotation axis of the shaft 7 in the Rz axis direction, it is in the Rx axis direction and the Ry axis direction. There was no sensor capable of detecting the operation. Therefore, with respect to the vibrations in the Rx-axis direction and the Ry-axis direction, the torsional vibration itself cannot be observed, and it is difficult to detect the occurrence of the torsional vibration and suppress the torsional vibration.

Then, a method of suppressing the twist due to the interference force from another shaft received in the operation direction on a certain rotating shaft by correcting the motor control position command value of that shaft has been proposed, but as described above, it is general. The idea is that only the Rz axis direction can rotate in each axis, and the rotation cannot be performed in the Rx axis direction or the Ry axis direction. Therefore, the twist in the Rx axis direction and the Ry axis direction cannot be compensated. In addition, various methods for compensating for the influence from other axes have been proposed, but none have been examined for the non-operating direction other than the operating direction.

In addition, although a method of correcting the control position command value in consideration of the elasticity of each joint including the non-operating direction has been proposed, no consideration is given to the resonance characteristics, and the resonance vibration (non-operating direction resonance). ) Was not effective. In addition, although it is expected that the accuracy of the position of the tip of the hand will be improved by such a method, the resonance vibration is excited by the resonance characteristic consisting of inertia, elasticity and rigidity (spring constant) in the Rx axis direction or the Ry axis direction. Therefore, there is a possibility that the posture of the tip of the hand, particularly the posture with respect to the Rx axis direction or the Ry axis direction, becomes vibrating. In this case, at first glance, it seemed that moving in the opposite phase would be effective in suppressing vibration, but the inertial force due to the hand acceleration acts directly in the non-moving direction during operation, and the hand acceleration increases after the operation ends. Since it becomes zero and no inertial force is generated, neither the effect of suppressing the generation of resonance vibration nor the effect of dampening the resonance vibration can be obtained.

Further, conventionally, in order to attenuate the resonance vibration, the resonance characteristic is appropriately attenuated and the vibration is quickly converged by a method such as state feedback control in the feedback control of the resonance system. Further, when the state quantity of the resonance characteristic cannot be measured directly, the value estimated by a method such as a state observer is used instead of the measured value.
However, in a horizontal articulated robot, when vibration suppression control is performed by such state feedback and a state observer, there is a problem that the vibration suppression effect differs depending on the operating conditions. Then, when the feedback gain was changed according to the moment of inertia in the rotation direction of the arm for verification, the result that the vibration suppression effect was not improved so much was obtained.

Therefore, as a result of further detailed investigation by the inventors, the resonance vibration caused by the shaft support portion 6a depends on the moment of inertia in the direction orthogonal to the rotation direction of the arm, that is, the Z axis of the shaft 7. It has been clarified that the resonance frequency and the antiresonance frequency fluctuate depending on the position in the direction (hereinafter referred to as the Z-axis height). This means that the moment of inertia is not obtained from the torque and angular acceleration by considering only the operating direction as in the past, but if it is not considered in the three-dimensional space including the non-operating direction, an effective vibration suppression effect can be obtained. It shows that it is not enough to get.

Therefore, in the present embodiment, feedback control capable of suppressing vibration in the non-operating direction is realized by using the state feedback control as described below. This feedback control is executed by the control unit 3a, and also indicates a control method for suppressing vibration in the non-operating direction.
First, the motor angle seen from the arm side of the first axis is θ _M1 , the motor inertia is J _M1 , the motor viscosity is D _M1 , the motor torque is τ _M1 , the gear rigidity is K _S1 , the arm angle is θ _A1 , and the arm inertia is Let _JA1 and arm viscosity be _DA1 . Further, the horizontal distance from the first arm support portion to the shaft supporting portion 6a L _XY1, the rigidity of the shaft support portion 6a K _B, the viscosity of the shaft supporting portion 6a D _B, the twist angle (the twist angle of the shaft supporting portion 6a Corresponding) is θ _B. The non-operating direction of the center of gravity position of the shaft 7, where the moment of inertia J _BC to a direction perpendicular to the operating _direction, weight M _B, the distance from the shaft support portion 6a to the center of gravity and L _BZ. The position of the center of gravity of the shaft 7 here means the position of the center of gravity when a load such as a tool or a work is attached to the shaft 7 and the shaft 7, the tool and the work are regarded as one. There is.
At this time, the equation of motion is as follows.

The equation of state at this time is as follows.

From this equation, the matrix E includes the distance L _BZ from the shaft support portion 6a to the center of gravity, it is apparent that the resonance characteristics varies according to the Z-axis height of the shaft 7.
Then, when the state feedback and the state observer are constructed for the system represented by the above equation of motion, it becomes as follows. However, w is an observer state quantity. A block diagram of this system is shown in FIG.

At this time, the feedback gain K and the observer gain F can be designed by a pole arrangement method or the like. Further, each gain can be designed to be variable according to the distance LBZ from the shaft support portion 6a to the center of gravity.
By the way , in the above example, the rigidity KB uses the rigidity of the shaft support portion 6a, but if the torsion angle θB is sufficiently small, it can be approximated as sin (θ) = θ and cos (θ) = 1. , The rigidity of the shaft 7 itself can be considered to be equivalently the rigidity of the shaft support portion 6a.

Therefore, the stiffness K _B, may include the stiffness of the shaft 7 itself. In this case, the rigidity of the shaft support portion 6a _{K B0,} the rigidity of the shaft 7 itself _{K B1,} when the force applied to the center of gravity to is f, the twist angle theta _B, is as follows (63) below. Further, the stiffness K _B when including rigidity of the shaft 7 itself, by obtaining the amount of displacement when a force f applied to the center of gravity has changed the position of the center of gravity, is as follows (64) below.

From these equations, it is clear from the mathematical formulas that even when the rigidity including the rigidity of the shaft 7 itself is used as the rigidity KB of the shaft support portion 6a, it can be treated equivalently.
At this time, if it is a general metal material, the tensile strength is 1% or less with respect to Young's modulus. In other words, if the shaft 7 itself is twisted to exceed 1%, the shaft 7 is subjected to. Since it breaks, the above-mentioned condition that the twist angle θB is sufficiently small is always satisfied.

As described above, according to the present embodiment, by designing the control system for the state space model including the resonance characteristic of the shaft support portion 6a, it is possible to carry out feedback control effective for vibration suppression. it can.
Specifically, for a state space model that is defined in advance by an equation of motion with respect to the position of the center of gravity of the shaft 7 that changes according to the vertical position of the shaft 7 and includes a resonance characteristic that causes a twist angle of the shaft support portion 6a. By designing the control system, it is possible to define a state space model in which the influence of the change in the position of the center of gravity of the shaft 7 is taken into consideration. Then, by designing the control system for the state space model, that is, by performing the vibration suppression control for the control system, the vibration of the hand of the robot 2 can be effectively suppressed.

In addition, since the state feedback and the state observer are constructed for the state space model, the vibration suppression control can be performed by the feedback control, and the vibration of the hand whose physical quantity cannot be directly obtained can be estimated by the state observer. However, vibration suppression control can be performed.
Further, by including the moment of inertia in the direction orthogonal to the operating direction at the position of the center of gravity of the shaft 7 in the state equation, vibration can be suppressed in a state where the moment of inertia in the non-operating direction different from the operating direction is added. , Non-operating direction vibration can be effectively suppressed.

In this way, for example, when the resonance frequency changes due to a change in the posture of the arm, only the change in the moment of inertia corresponding to the operating direction is considered, and no consideration is given to the change in the moment of inertia in the non-operating direction. Unlike the prior art, the vibration suppression method of the present embodiment can perform vibration suppression control corresponding to a change in the moment of inertia in the non-operating direction.
Further, since the vibration suppression control is performed based on the fact that the moment of inertia changes depending on the Z-axis height of the shaft 7, which has not been considered in the past, the vibration suppression effect can be further enhanced.

(Other embodiments)
The present invention is not limited to the embodiments described above and shown in the drawings, and various modifications and extensions can be made without departing from the gist thereof.
The numerical values shown in the embodiments are examples, and the present invention is not limited thereto.
Although the first arm 5 and the second arm 6 are each provided with a biaxial angular velocity sensor capable of detecting angular velocities in a plurality of directions, other sensor installation methods may be used, for example, an acceleration sensor and an angular velocity sensor may be combined. For example, it is conceivable that the second arm 6 having a long wiring distance is provided with an angular velocity sensor for wiring saving, and the first arm 5 is provided with two acceleration sensors as in the second embodiment.

In the drawing, 2 is a robot, 3 is a control device (robot control device), 3a is a control unit, 4 is a base, 5 is a first arm, 6 is a second arm, 6a is a shaft support portion, 7 is a shaft, 8, 9, Reference numerals 10, 11 and 12 indicate sensors (inertial sensor, angular velocity sensor, acceleration sensor, rotation angle sensor, position sensor).

Claims (13)

A base, a first arm attached to the base and rotating about the first axis with respect to the base, and a second arm attached to the first arm and rotating about the second axis with respect to the first arm. A robot control device that controls a horizontal articulated robot attached to the second arm and having a shaft that moves up and down and rotates with respect to the second arm.
The inertial force and inertial torque of each arm are obtained from the detection results of a plurality of sensors provided on the first arm and the second arm, and the torque applied to the shaft support portion that supports the shaft is calculated from the obtained inertial force and inertial torque. A robot control device including a control unit that performs a process of estimating and obtaining a twist angle at the shaft support portion that correlates with vibration of the tip portion of the shaft based on the estimated torque and the rigidity of the shaft. The robot control device according to claim 1, wherein the sensor detects vibration in a non-operating direction different from the operating direction of the second arm. The robot control device according to claim 1 or 2, wherein the plurality of sensors are a combination of an inertial sensor, a rotation angle sensor, and a position sensor that detects the vertical position of the shaft. The robot control device according to claim 3, wherein the inertial sensor is an angular velocity sensor that detects an angular velocity or an acceleration sensor that detects an acceleration. The robot control device according to claim 4 , wherein the angular velocity sensor is an angular velocity sensor that detects angular velocities in a plurality of directions. The robot control device according to claim 4 or 5 , wherein the acceleration sensor is an acceleration sensor that detects acceleration in a plurality of directions. The robot control device according to any one of claims 3 to 6, wherein the rotation angle sensor is a motor encoder provided in a motor that rotationally drives the first arm with respect to the base. The robot control device according to any one of claims 3 to 7, wherein the rotation angle sensor is a motor encoder provided in a motor that rotationally drives the second arm with respect to the first arm. The robot control device according to any one of claims 3 to 8, wherein the rotation angle sensor is a motor encoder provided in a motor that rotationally drives the shaft with respect to the second arm. The robot control device according to any one of claims 3 to 9, wherein the position sensor is a motor encoder provided in a motor that drives the shaft up and down. The control unit is defined in advance by an equation of motion with respect to the position of the center of gravity of the shaft, which changes according to the vertical position of the shaft, and controls a state space model including a resonance characteristic that causes a twist angle of the shaft support unit. The robot control device according to any one of claims 1 to 8 for designing a system. The robot control device according to claim 11, wherein the control unit designs a control system by constructing a state feedback and a state observer for the state space model. The robot control device according to claim 11 or 12, wherein the equation of state of the control system is defined to include a moment of inertia in a direction orthogonal to the operating direction at the position of the center of gravity of the shaft.

Images