JP6834125B2 - Robot control device, robot control method - Google Patents

Description

The present invention relates to a robot control device for controlling a robot and a robot control method.

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 and a robot control method capable of suppressing vibrations that do 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.
That is, since the conventional robot does not consider the vibration in the non-operating direction, it does not have a means for detecting the vibration other than the rotation direction in the first place, and the vibration in the non-operating direction itself cannot be detected. 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.

Further, in claim 1 described above, when correcting the control position command value to the motor, a vibration suppression process is performed in which the vehicle is converted into a fixed Cartesian coordinate system indicating the position of the load center of gravity and resonance vibration is suppressed in the converted Cartesian coordinate system. It may be applied and a process of obtaining a corrected control position command value from a Cartesian coordinate system to which the vibration suppression process is applied may be executed.
As described above, the non-operating direction vibration occurs in a direction different from the rotation direction. Therefore, it is considered that the non-operating directional vibration is caused by the centrifugal force applied with the rotation. In this case, if the coordinates are converted at a position different from the position of the center of gravity of the load, a centrifugal force proportional to the distance from the position of the center of gravity of the load to the tip of the flange is generated. Therefore, the influence of the centrifugal force cannot be ignored, and vibration suppression processing such as a filter It is considered that the effect of the processing cannot be obtained as designed.

Therefore, the position of the center of gravity of the load is converted into the Cartesian coordinate system. As a result, terms such as centrifugal force that appear in the calculation when considered in units of each axis are included in the coordinate conversion to the Cartesian coordinate system, and after conversion to the Cartesian coordinate system, the centrifugal force is centrifugal. You don't have to consider force. The position of the center of gravity of the load is the mass including the shaft, the tool, the work, and the mechanical element connected to the twisting member such as the ball screw portion of the shaft in the case of a horizontal articulated robot. It means the position of the center of gravity.
Then, by performing the vibration suppression process in the converted Cartesian coordinate system, it is possible to obtain the corrected control position command value in which the vibration due to the influence of the centrifugal force, that is, the vibration in the non-operating direction is suppressed. That is, the robot can be operated on a trajectory in which vibration in the non-operating direction is suppressed.

Further, as in the invention of claim 11 , when correcting the control position command value to the motor, it is converted into a fixed Cartesian coordinate system indicating the position of the load center of gravity, and vibration that suppresses resonance vibration in the converted Cartesian coordinate system. The same effect as that of the robot control device according to claim 1 can also be obtained by a robot control method in which the suppression process is applied and the corrected control position command value is obtained from the Cartesian coordinate system to which the vibration suppression process is applied. ..

Further, in the invention according to claim 12 , among the state quantities handled by the controller, the internal state quantity is coordinate-converted to a coordinate system that changes according to the operation of the robot, while the external state quantity is the operation of the robot. Do not perform coordinate conversion to a coordinate system that changes according to.
For example, when the second axis (second arm) is fixed and the first axis (first arm) is rotated 180 °, the direction of the force applied to the shaft support portion by the rotation of the first axis at the start of movement and the movement. The direction of the force applied to the shaft support portion by the rotation of the first axis at the time of completion is the opposite direction when viewed in the fixed coordinate system. Therefore, it is considered that the vibration suppression effect cannot be exhibited as designed even if the vibration suppression process is applied to the fixed direction.

By the way, in the fields of motor control and inverter control, so-called vector control that converts the coordinate system of the input / output state quantity and the coordinate system of the control state quantity is widely known. Further, in the Newton-Euler method, which is a method for calculating the dynamics of a robot, when considering the interaction between the front and rear links, the calculation is performed while sequentially converting the coordinates. In addition, a technique has been proposed in which state quantities between functional models are appropriately transformed in coordinates and propagated.
If the conventional technique that simplifies the calculation by converting the coordinates of such a controlled state quantity is applied as it is, the rotation amount of the first axis is erased in calculation. Therefore, even if the vibration suppression process is performed in the coordinate system, the vibration caused by the first axis cannot be suppressed.

Therefore, of the state quantities handled by the controller, the internal state quantity is coordinate-converted to a coordinate system that changes according to the robot's operation, while the external state quantity, which indicates the input / output state quantity, is the robot's operation. Do not perform coordinate conversion to a coordinate system that changes according to. That is, the external state quantity is coordinate-converted to a fixed Cartesian coordinate system indicating the position of the load center of gravity as described above.
As a result, in the prior art, the external state quantity was also rotated, so the effect of the rotation of the coordinate system, that is, the centrifugal force could not be handled correctly, but by rotating the internal state quantity, the centrifugal force could not be handled correctly. It is possible to handle the input / output that includes the above, that is, to handle the centrifugal force correctly. Therefore, vibration due to the influence of centrifugal force, that is, vibration in the non-operating direction can be 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 FIG. 1 schematically showing a force applied to the tip of the shaft in the third embodiment. Figure 2 schematically showing the force applied to the tip of the shaft Figure 3 schematically showing the force applied to the tip of the shaft Figure 4 schematically showing the force applied to the tip of the shaft

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.

In addition, 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 control position command value to the motor of that shaft has been proposed, but as described above, it is general. In the conventional way of thinking, only the Rz axis direction can be rotated 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.

Now, the tip portion 7a of the shafts Doo 7 (hereinafter, the flange tip also referred to) force applied to the first arm support, the second arm support, and that a non-operating direction of the rotational force of the shaft supporting portion 6a, invention Revealed by those. Specifically, for example, when an outward force (Fx) is applied to the flange tip as shown in FIG. 5A, the force causes Ry rotation about the Ry axis with respect to the shaft support portion 6a. It is a force to be generated, that is, a rotational force in the non-operating direction. Further, for example, when a lateral force (Fy) is applied to the tip of the flange as shown in FIG. 5B, the force is a force that causes Rx rotation around the Rx axis with respect to the shaft support portion 6a. That is, it becomes a rotational force in the non-operating direction.

Further, as shown in FIG. 6A, the outward force (Fx) as shown in FIG. 5A also passes through the second arm support portion and the flange tip with respect to the second arm support portion. It is a force that causes Ry rotation around the Ry axis passing through the line Lc, and as shown in FIG. 7A, a virtual line Lc passing through the first arm support portion and the flange tip is also provided to the first support portion. It is a force that causes Ry rotation around the passing Ry axis. Similarly, the lateral force (Fy) as shown in FIG. 5 (b) becomes a force that causes Rx rotation also with respect to the second arm support portion as shown in FIG. 6 (b), and is also a force that causes Rx rotation, and is also shown in FIG. 7 (b). As shown in b), it is a force that causes Rx rotation also with respect to the first arm support portion. Further, as shown in FIGS. 8A and 8B, when an upward force (Fz) is applied to the flange tip, the force causes Ry rotation with respect to the second arm support portion and the first support portion. It will be a force to make you.
Therefore, if the force applied to the tip of the flange can be controlled so as not to include the vibration frequency in the non-operating direction, it is considered that the vibration in the non-operating direction can be suppressed.

First, the force applied to the tip of the flange will be examined. The driving force generated in the tool attached to the flange is the driving force for opening and closing the chuck, and is not so large. Therefore, the driving force generated in the tool is not considered to have a large effect on vibration.
Although there is no driving force generated after the flange tip other than the driving force generated in the tool, the flange tip and the tool are moved by motors such as the 1st and 2nd axes, and as a result, the inertia according to the acceleration. Power will appear. Then, if the inertial force is suppressed, it is considered that the vibration in the non-operating direction can be suppressed. That is, it is considered that controlling the acceleration at the tip of the flange so as not to include the vibration frequency in the non-operating direction leads to reducing the vibration in the non-operating direction. In this case, although the direction of the flange tip changes due to the movement operation accompanying the rotation of the arm, it is possible to follow the change in the direction of the flange tip by appropriately rotating the internal state quantity of the controller in the control system. Conceivable.

Before explaining the control method of the present embodiment, first, it will be examined whether or not the above-mentioned vibration in the Rx-axis direction or the Ry-axis direction can be suppressed by the prior art.
Let us consider a case where filtering is performed for each axis as has been done in the past. As an example of the prior art, a method of adding a notch filter to the control system of each axis has been proposed, considering the phase difference of the notch filter of each axis when the axes are moved synchronously. In this case, considering the rotating coordinate system, centrifugal force is generated in proportion to the square of the velocity. However, the proposed filtering presupposes that the control system is linear and does not work correctly for terms proportional to the square.

Explaining with a specific example, for example, when vibration occurs at 10 Hz, the component at 10 Hz is naturally suppressed. However, at this time, if there are frequency components of 20 Hz and 30 Hz that are not suppressed, the 10 Hz components that should have been suppressed are squared, and the following equations (49) and (50) are obtained. It appears as shown in.

That is, in the conventional filtering process for each axis, the effect is not sufficiently exhibited when a force applied in the non-operating direction, for example, a centrifugal force acts.
Therefore, in the present embodiment, as described below, the acceleration in the non-operating direction is controlled so as not to include the vibration frequency in the non-operating direction with respect to the acceleration at the tip of the flange, thereby reducing the vibration in the non-operating direction and in the control system. A control method is used that allows control to follow changes in the direction of the flange tip by appropriately rotating the internal state quantity of the controller.

First, the first arm length is L ₁ , the second arm length is L ₂ , the first axis rotation angle is θ ₁ (t), the second axis rotation angle is θ ₂ (t), and the fourth axis rotation angle is θ ₄ (t). ), The length from the tip of the flange to the center of gravity of the load is expressed by the following equation (51). At this time, if the position of the center of gravity of the load is expressed by the Cartesian coordinates shown in (52) below by forward kinematics transformation, the following equations (53) to (55) are obtained. However, a (t) represents the posture component at the center of gravity, that is, the rotation angle in the Rz axis direction. The position of the center of gravity of the load means the position of the center of gravity of the shaft 7 and the load, specifically, the position of the center of gravity of the shaft 7 and the tool when the tool is attached. Means the position of the center of gravity in a state including the weight of the shaft 7, the tool and the work when gripping the work.

Here, when the equation (52) is Laplace transformed as the following equation (56), the following equation (57) is obtained.

Then, when the acceleration and the angular acceleration are obtained by the following equation (58) by differentiating twice with the initial value as 0, the following equation (59) is obtained.

Here, assuming that the notch filter (also called a band elimination filter, band elimination filter, etc.) that suppresses the vibration frequency in the non-operating direction is F (s), the acceleration after passing through the filter represented by the following equation (60) is , (61) and (62).

From these equations, applying a notch filter to the position or attitude trajectory shown in Eq. (52) to obtain the corrected position or attitude trajectory as in Eq. (63) below is the acceleration shown in Eq. (60). Alternatively, after applying a notch filter to the angular acceleration trajectory, it can be seen that it is equivalent to obtaining the correction position or the correction trajectory as shown in the following equation (64) by double integration.

Then, from the above-mentioned corrected position or corrected posture trajectory, the corrected angle trajectory represented by the following equation (65) can be obtained from the following equations (66) to (69) by inverse kinematics conversion.

The combination of positive and negative is determined by whether the arm takes the right-handed system or the left-handed system. That is, if the angular orbit shown in Eq. (70) is a right-handed system, the modified angular orbit may be the right-handed system, and if the angular orbit is the left-handed system, the modified angular orbit may also be the left-handed system. Good.
Assuming that the load mass of the arm is M, the inertial force when following the corrected trajectory is as shown in (71) below.

Since the inertial force is proportional to the acceleration, it can be seen that applying a notch filter to the acceleration trajectory is equivalent to applying a notch filter to the inertial force. It should be noted that applying a notch filter to the acceleration trajectory can be realized by applying a notch filter to the position trajectory as described above.
At this time, terms such as centrifugal force that appear when considered in units of each axis are included in the coordinate conversion to the position of the center of gravity of the load. Therefore, it is not necessary to consider it after converting to the Cartesian coordinate system.
In addition, when considering a position other than the load center of gravity position such as the flange tip as in the prior art, a centrifugal force proportional to the distance between the load center of gravity position and the flange tip is generated, so that the influence of the centrifugal force is exerted. Cannot be ignored. As a result, the effect of the vibration suppression filter does not work as designed. Although the description is simply referred to as the load mass and the load center of gravity here, they include mechanical elements that connect the load to the twisting member, for example, the ball screw portion of the shaft 7 in the case of a horizontal articulated robot. The mass of is also included.

By the way, as an example of the prior art, there is a method in which the difference between the command value and the sensor detection value is taken after the forward conversion, and the reverse conversion is performed and used for controlling the joint angle. However, in that method, the coordinate space of the state quantity to be controlled and the angular space of the robot that actually moves are different, and only conversion is performed to absorb the difference, and forward conversion is performed as in the present embodiment. No consideration was given to the fact that it is not necessary to consider centrifugal force or the like by applying the resonance vibration suppression method (corresponding to the vibration suppression process) later.
In addition, after converting to the Cartesian coordinate system by forward conversion, the vibration suppression trajectory is derived by the matrix filter and returned to each axis coordinate system by the inverse conversion, or the position of the hand part is changed from each axis coordinate system to the Cartesian coordinate system. After conversion, there is a method of performing predetermined input shaping and returning to each axis coordinate system again. Even in such a method, centrifugal force is obtained by converting to a Cartesian coordinate system at the load center of gravity position as in the present embodiment. No consideration was given to the fact that it is no longer necessary to consider the effects of such factors.

By the way, the above-mentioned orthogonal coordinate system is a global coordinate system, that is, an absolute coordinate system with the base 4 as the origin, and is not a coordinate system corresponding to the twisting direction of each support portion. For example, consider fixing the second axis and rotating the first axis by 180 °. At this time, the direction of the force applied to the shaft support portion 6a by the rotation of the first axis at the start of movement and the direction of the force applied to the shaft support portion 6a by the rotation of the first axis at the completion of movement are coordinate systems. In other words, it goes in the opposite direction. Therefore, it is considered that the effect of suppressing vibration was not exhibited as designed in the prior art.

For example, in the prior art, in the fields of motor control and inverter control, a control method (vector control) for converting a coordinate system of an input / output state quantity and a coordinate system of a control state quantity is widely and generally known. .. Further, in the Newton-Euler method, which is a method for calculating the dynamics of a robot, when considering the interaction between the front and rear links, the calculation is performed while sequentially converting the coordinates. Then, a technique has been proposed in which the state quantities between the functional models are appropriately transformed in coordinates and propagated. In this way, various methods have been conventionally proposed to simplify the calculation by appropriately transforming the coordinates of the controlled state quantity.

Therefore, let us consider a case where the concept of this conventional technique is applied as it is. When the orthogonal coordinate system is coordinate-converted according to the coordinate system of the shaft support portion 6a, the following equation (72) is obtained.

In this equation (72), the amount of rotation of the first axis has been erased. Therefore, even if vibration suppression is performed in this coordinate system, vibration caused by the first axis cannot be dealt with. That is, it is clear that even if the concept of the prior art is applied as it is, the effect of suppressing vibration cannot be fully exerted.

Therefore, in the present embodiment, by correcting the control position command value to the motor, control is performed so that vibration in the Rx-axis direction or the Ry-axis direction is less likely to occur. The basic idea at this time is to convert only the internal state amount of the controller into a coordinate system that changes according to the robot operation, and the external state amount (input / output state amount) changes according to the robot operation. The coordinates are converted to a fixed Cartesian coordinate system that indicates the position of the load center of gravity without performing coordinate conversion to the coordinate system. Hereinafter, a specific description will be given.

First, the notch filter F (s) for suppressing vibration is discretized and expressed as a discrete time state space model as the following equations (73) and (74). Although there is a degree of freedom in designing how to take the state quantity, it is preferable to set it to a value that is constantly zero, such as a position difference value. This is because, in the case of a value that is not constantly zero, the offset amount is affected by the rotation of the state quantity.

By the way, during the period from the control sample cycle i-1 to the control sample cycle i, that is, the sample time Ts, the coordinate system of the shaft support portion 6a rotates by the amount shown in the following equation (75).

Therefore, the rotation matrix RΔ [i] is defined as the following equation (76). However, the number of X-axis and Y-axis directions of the state quantity is N, the number of state quantities rotating of which N _R, the number of state variables is not rotated with N _C. Further, the _{I R} and _{N R} order unit matrix, the IC and _{N C} following matrix.

At this time, since the state quantity is also rotating in the same manner, the filter is constructed as the following equations (77) and (78). Considering the coordinate space of the control sample period i + 1, the coordinate space of the control sample period i is rotated by RΔ [i]. Further, at this time, regarding the state quantity, the one to be rotated and the one not to be rotated are separated as in the following equations (79) and (80).

As described above, in the method of the present embodiment, when correcting the control position command value to the motor, instead of simply converting to the Cartesian coordinate system and then correcting the position signal, the Cartesian coordinate system indicating the position of the center of gravity of the load is used. The position signal is being corrected.

According to the embodiment described above, the following effects can be obtained.
In the robot control method of the present embodiment, when correcting the control position command value to the motor, it is converted into a fixed Cartesian coordinate system indicating the position of the load center of gravity, and vibration suppression processing for suppressing resonance vibration in the converted Cartesian coordinate system is performed. It includes a process of obtaining the corrected control position command value from the Cartesian coordinate system to which the vibration suppression process is applied.

As described above, the non-operating direction vibration occurs in a direction different from the rotation direction. Therefore, it is considered that the non-operating directional vibration is caused by the centrifugal force applied with the rotation. In this case, if the coordinates are converted at a position different from the position of the center of gravity of the load, a centrifugal force proportional to the distance from the position of the center of gravity of the load to the tip of the flange is generated. Therefore, the influence of the centrifugal force cannot be ignored, and vibration suppression processing such as a filter It is considered that the effect of the processing cannot be obtained as designed.

Therefore, the position of the center of gravity of the load is converted into the Cartesian coordinate system. As a result, terms such as centrifugal force that appear in the calculation when considered in units of each axis are included in the coordinate conversion to the Cartesian coordinate system, and after conversion to the Cartesian coordinate system, the centrifugal force is centrifugal. You don't have to consider force. The position of the center of gravity of the load is the mass including the shaft, the tool, the work, and the mechanical element connected to the twisting member such as the ball screw portion of the shaft in the case of a horizontal articulated robot. It means the position of the center of gravity.

Then, by performing the vibration suppression process in the converted Cartesian coordinate system, it is possible to obtain the corrected control position command value in which the vibration due to the influence of the centrifugal force, that is, the vibration in the non-operating direction is suppressed. That is, the robot can be operated on a trajectory in which vibration in the non-operating direction is suppressed.
Further, as described above, if the conventional technique for simplifying the calculation by converting the control state quantity into coordinates is applied as it is, the rotation amount of a certain axis is erased in calculation. Therefore, even if the vibration suppression process is performed in the coordinate system, the vibration caused by the axis cannot be suppressed.

On the other hand, among the state quantities handled by the controller, the internal state quantity is subjected to coordinate conversion to a coordinate system that changes according to the movement of the robot, while the external state quantity indicating the input / output state quantity is performed. By not performing coordinate conversion to a coordinate system that changes according to the movement of the robot, it is possible to handle the input / output including the centrifugal force, that is, to handle the centrifugal force correctly. Therefore, vibration due to the influence of centrifugal force, that is, vibration in the non-operating direction can be suppressed.
In this case, the global coordinate system may be used as it is as in the embodiment, but for example, a coordinate system rotated by 90 ° from the global coordinate system with the Z axis as the rotation center, or a coordinate system in which the origin is moved by 1 m in the X direction. And so on. In any case, if the vibration suppression method of the present embodiment is applied to the distance from the coordinate origin to the position of the center of gravity of the load, vibration in the non-operating direction can be suppressed.

In addition, the external state quantity (input / output state quantity) is not subjected to coordinate conversion into a coordinate system that changes according to the robot operation. For example, the current vector control has a feature that by appropriately rotating the external state quantity (input / output state quantity), the external state quantity that should originally be an AC voltage and a current can be treated as a DC voltage and a current. However, if the external state quantity is rotated to match the coordinate system of the shaft support portion 6a based on such an idea, the influence of the rotation of the coordinate system, that is, the centrifugal force cannot be handled correctly. Therefore, by rotating the internal state quantity as in the present embodiment, it is possible to obtain an effect that cannot be realized by a conventional concept such as current vector control, that is, input / output including centrifugal force can be handled.

In this embodiment, the state space model is used as the model of the controller, in order to show the internal state quantity in an easy-to-understand manner for the sake of explanation. Therefore, the method of the present embodiment can be applied without any problem even if the transfer function model is used as it is instead of the state space model. Further, even when the transfer function model is used, it is the same that an internal state quantity is required to implement the method of the present embodiment.
Further, in the present embodiment, the non-rotating state quantity can be included in the calculation. As a result, for example, when the speed is obtained from the difference between the position one sampling before and the current position, the filter is effectively operated by rotating the speed after obtaining the speed without rotating in the dimension of the position. Can be done. Even when there is no non-rotating state quantity, the method of the present embodiment can be used.

Although an example of a discrete-time system is shown in this embodiment, the method of this embodiment is by converting to a continuous-time system state-space model in consideration of the limit in which the control sample time of the discrete-time system state-space model is set to 0. , Can also be applied in a continuous time system. In that case, since it is necessary to consider a discrete-time system in order to implement the control system in the control device 3, considering the prospect of equation expansion, the rotation of the internal state quantity converts the control system into a discrete-time system. It is better to do it after that.
In the present embodiment, the notch filter is exemplified as the vibration suppressing means, but other vibration suppressing means may be used. For example, the technical idea of rotating the internal state quantity can be used as it is even if it is a vibration suppressing means for superimposing a signal that cancels or attenuates the resonance vibration by moving it in the opposite phase to the resonance vibration. ..

Further, instead of applying the method of this embodiment to all axes, different methods may be used in the X-axis direction, the Y-axis direction, and the Rz-axis rotation direction. However, in the case of a horizontal articulated robot, it is usually considered that the X-axis direction and the Y-axis direction have the same rigidity, so it is desirable to use the same control system in order to avoid complication of the control system. However, when applied to a three-dimensional space as described later or a vertical articulated robot such as a so-called 6-axis robot, the rigidity may differ in each axial direction, so a different vibration suppression method is used in each direction. Is expected to be effective. In addition, regarding the Rz axis rotation direction, if vibration in the rotation direction does not matter, or if vibration is detected by an angle sensor such as a motor encoder in the rotation direction and vibration control is sufficient, this It is possible to avoid applying the method of the embodiment.

Further, although the example in the two-dimensional space of the XY coordinates is shown in the embodiment, the method of the present embodiment can be applied to the three-dimensional space of the XYZ coordinates with the same idea.
Further, the method of the present embodiment can be applied as it is to any robot including a rotating joint axis such as a vertical articulated robot other than the horizontal articulated robot. That is, in the case of the horizontal articulated robot 2 of the embodiment, since the rotation axis direction in the Rz axis rotation direction does not change, an example in which only the state quantity of the position component is rotated is shown, but the rotation of a vertical articulated robot or the like is shown. When the axial direction changes, the effect of the filter applied in the rotation axis direction such as vibration suppression can be obtained as designed by applying the method of the present embodiment in which the state quantity is rotated also with respect to the posture component. Will be.

In the embodiment, the center of rotation exists in the shaft support portion 6a or the like, that is, the rigidity of the shaft support portion 6a or the like is relatively low, but for example, the rigidity of the shaft 7 itself is relatively low and the shaft 7 itself does not operate. It is possible that the vibration in the direction has a higher degree of influence. Even in such a case, since the rigidity of the shaft 7 can be equivalently approximated to that in the shaft support portion 6a, the method of the present embodiment can be applied as it is.
Further, in the embodiment, an example of correcting the position trajectory as feedforward control is shown, but the present embodiment is not limited to this, and the internal state quantity is rotated even in the feedback control based on the difference between the observed current position and the command position. The form method can be used as it is.
Further, the same effect can be obtained by a robot control device provided with a control unit 3a that executes the robot control method of the present embodiment described above.

(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 (Z-axis height acquisition unit, distance acquisition unit, suppression frequency acquisition unit, update unit, calculation unit), 4 is a base, and 5 Is the first arm, 6 is the second arm, 6a is the shaft support, 7 is the shaft, and 8, 9, 10, 11 and 12 are sensors (inertial sensor, angular velocity sensor, acceleration sensor, rotation angle sensor, position sensor).

Claims (12)

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 control unit is provided that performs a process of estimating and obtaining a twist angle at the shaft support portion that correlates with the vibration of the tip portion of the shaft based on the estimated torque and the rigidity of the shaft .
When correcting the control position command value to the motor, the control unit converts the load center of gravity position into a fixed Cartesian coordinate system, applies vibration suppression processing that suppresses resonance vibration in the converted Cartesian coordinate system, and applies vibration suppression processing to the vibration. A robot control device that executes a process of obtaining the corrected control position command value from a Cartesian coordinate system to which a suppression process is applied. 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. A plurality of said sensor, an inertial sensor, the rotation angle sensor and the robot control device請Motomeko 1 or 2, wherein that consists of any combination of the position sensor for detecting the vertical position of the shaft. The inertial sensor, an angular velocity sensor or the robot control apparatus請Motomeko 3 wherein Ru acceleration sensor der for detecting acceleration, to detect the angular velocity. The angular velocity sensor, the robot control device請Motomeko 4 wherein Ru angular velocity sensor der for detecting the angular velocity of the plurality of directions. The acceleration sensor, the robot controller of the acceleration sensor der Ru請Motomeko 4 or 5, wherein detecting the acceleration in a plurality of directions. The rotational angle sensor, the robot control apparatus according to any one claim of請Motomeko 3-6 Ru Ah in motor encoder provided in the motor for rotating the first arm relative to said base. The rotational angle sensor, the robot control apparatus according to any one claim of said second arm from請Motomeko 3 Ru Ah in motor encoder provided in the motor which rotates relative to the first arm 7. The rotational angle sensor, the robot control apparatus according to any one claim of請Motomeko 3 to 8 Ru Ah in motor encoder provided in the motor for rotating the shaft relative to the second arm. Said position sensor, the robot control apparatus according to any one claim from請Motomeko 3 Ru Ah in motor encoder provided in the motor for vertically driving the shaft 9. 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 method for controlling a horizontal articulated robot having a shaft attached to the second arm and moving and rotating up and down with respect to the second arm and a motor for driving the arm.
The inertial force and inertial torque in 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. Estimated, and based on the estimated torque and the rigidity of the shaft, the twist angle at the shaft support portion that correlates with the vibration of the tip portion of the shaft is obtained, and the twist angle is obtained.
When correcting the control position command value to the motor, the load center of gravity position is converted to a fixed Cartesian coordinate system, a vibration suppression process that suppresses resonance vibration is applied in the converted Cartesian coordinate system, and the vibration suppression process is applied. A robot control method for obtaining the corrected control position command value from the Cartesian coordinate system. In the vibration suppression process, of the state quantities handled by the controller, the internal state quantity is coordinate-converted to a coordinate system that changes according to the movement of the robot, while the external state quantity is based on the movement of the robot. The robot control method according to claim 11, wherein the coordinate conversion to the changing coordinate system is not performed.

Images