KR20140016839A - Control device and control method for robot and the robot - Google Patents

Abstract

A robot control device and a control method suppress vibrations of a robot arm composed of an arm including a plurality of links and an operating part operating the links. The method calculates a third speed based on both a first arm speed from at least one of an angular speed and acceleration detected from a vibration blocking position suppressing the vibrations and a second arm speed at a vibration control position calculated based on the operation of an operating part. The method controls the operating part based on the third arm speed. [Reference numerals] (40) Operating part; (50) Encoder; (60) Inertia sensor; (81) First calculating part (arm actual speed); (82) Second calculating part (arm operating speed); (83) Third calculating part; (84) Fourth calculating part (corrected speed); (85) Control part; (87) Jacobi matrix calculating part; (AA) Jacobi matrix J; (BB) Arm distortion speed; (CC) Reverse Jacobi matrix J^-1

Description

CONTROL DEVICE AND CONTROL METHOD FOR ROBOT AND THE ROBOT}

The present invention relates to a robot control apparatus, a control method, and a robot.

For industrial robots, further demands for high speed, multifunction, high precision, and power saving are continuing. The articulated robots implementing them are elements in which weight reduction for high speed and power saving and high rigidity for high precision are opposed. For example, in patent document 1, the method of suppressing the vibration of the robot tip which arises when the operation | movement of a robot stops is proposed. Specifically, an acceleration sensor is provided at the tip of the robot, and the damping is eliminated based on the detected torsional angular velocity and the torsional angle by adding the compensation amount to which the torsion is eliminated to the control input of the motor driving each link. That's how. As a result, the positional accuracy at the time of stoppage and the waiting time until the vibration is stabilized can be shortened.

Japanese Patent Application Laid-Open No. 2011-136395

However, in the control method (vibration method) described in patent document 1, there existed a subject that the vibration which generate | occur | produced during the operation | movement of a robot cannot be suppressed. Specifically, since the timing of the acceleration detection is based on the premise that the robot's motion stops, for example, the robot arm cannot eliminate vibrations or motion distortions generated during the movement, so that painting or welding is performed on the robot. When the work is carried out while moving, etc., there existed a subject that the fall of the precision of those work by vibration cannot be suppressed.

This invention is made | formed in order to solve at least one part of the above-mentioned subject, and can be implement | achieved as the following application examples or forms.

[Example 1]

The control apparatus for a robot according to the present application includes a robot main body, an arm connected to the robot main body, and including a plurality of links, a driving unit for driving the plurality of links, and a driving amount of the driving unit. A control device for a robot provided with an angle sensor, comprising: an inertial sensor installed at a vibration suppression position capable of suppressing vibration of the arm, and detecting at least one of an acceleration and an angular velocity at the vibration suppression position; Calculating a driving speed from a driving amount of a first calculating part for calculating a first arm speed at the damping position from at least one of an acceleration and an angular velocity at the damping position; and a driving amount of the driving part detected by the angle sensor; A second calculating unit for calculating a second arm speed at the damping position based on the amount and the driving speed; A third calculation unit calculating a third arm speed based on the second arm speed, and a correction speed for each drive unit driving the link between the robot body and the damping position based on the third arm speed. And a control unit for controlling the drive unit for driving the link between the robot body and the vibration isolating position based on the fourth calculating unit for calculating a value, the driving amount, the driving speed, and the correction speed. It features.

According to the robot control apparatus according to this application example, the third arm speed at the damping position is calculated based on the information of the first arm speed obtained from the inertial sensor and the information of the second arm speed obtained from the angle sensor. Moreover, the correction speed for every drive part which drives the link between a robot main body and a damping position is calculated based on the calculated 3rd arm speed. By controlling each drive unit based on the correction speed for each drive unit, it is possible to suppress vibration occurring at the vibration damping position.

In more detail, for example, in the case of an ideal robot which is light in weight and high in rigidity and does not have warpage or vibration due to warpage, the actual arm speed indicates this ideal arm speed. In contrast, the first arm speed obtained from the inertial sensor is the speed detected by the actual movement of the vibration damping position where the inertial sensor is installed. In other words, it is a speed including a speed component such as vibration generated due to the actual operation due to the bending of the arm or the like that does not have ideal rigidity. Therefore, by evaluating the difference between the first arm speed and the second arm speed, it is possible to extract the speed component due to warpage or vibration. By distributing the extracted velocity component to the drive unit for driving each link as the correction speed and generating a torque for suppressing warpage and vibration, the vibration can be suppressed.

By frequently suppressing the vibration by evaluating the difference between the first arm speed and the second arm speed, not only the vibration at the time of stopping but also the vibration of the arm at the time of operation (during operation) can be suppressed. As a result, the arm position accuracy at the time of driving can be controlled higher. In addition, since the vibration at the time of stopping can be suppressed without waiting for the arm stop operation, the waiting time until the vibration is stabilized after stopping can be shortened.

As described above, according to the control apparatus of the robot according to the present application example, it is possible to achieve higher precision, higher speed, and the like in an articulated robot.

Second Application Example

In the robot control apparatus according to the application example, it is preferable that the third calculating section further includes a filter for removing low frequency components below a predetermined frequency included in the waveform of the third arm speed.

As in this application example, by further including a filter for removing the low frequency component of the third arm speed, it is possible to more accurately extract the vibration component by evaluating the difference between the first arm speed and the second arm speed. Specifically, by removing the low frequency component of the third arm speed, it is possible to reduce the influence of the correction that suppresses the original operation of the robot arm, the influence of the offset error of the inertial sensor, and the like.

Third Application Example

A control method of a robot according to the present application is a control method of a robot having an arm including a plurality of linked links and a drive unit for driving the plurality of links, the control method of which a vibration is suppressed at a vibration suppressing position of the arm. A third arm based on a difference between detecting the first arm speed and the second arm speed which is the speed at which the damping position is driven based on the calculated first arm speed and the driving amount of the drive unit; Compensation control of the drive unit is performed on the basis of calculating the speed and the calculated third arm speed.

According to the control method of the robot according to the present application example, the method includes calculating the third arm speed based on the first arm speed and the second arm speed, and adjusting and controlling the driving unit based on the calculated third arm speed. The vibration during the operation of the robot can be suppressed. As a result, the arm position accuracy at the time of driving (in operation) can be controlled higher. In addition, since the vibration at the time of stopping can be suppressed without waiting for the arm stop operation, the waiting time until the vibration is stabilized after stopping can be shortened. That is, according to the control method of the robot which concerns on this application example, high precision, high speed, etc. can be aimed at in a multi articulated robot.

[Example 4]

A control method of a robot according to the present application includes detecting a robot body, an arm connected to the robot body, and including a plurality of links, a driver for driving the plurality of links, and a driving amount of the driver. A control method of a robot having an angle sensor, the method comprising: detecting at least one of an acceleration and an angular velocity of the vibration damping position by an inertial sensor provided at a vibration damping position for suppressing vibration of the arm, between the vibration damping position and the robot body; Detecting a driving amount of the drive unit for driving the link with an angle sensor, calculating a first arm speed of the vibration damping position from at least one of an acceleration and an angular velocity of the vibration damping position detected by the inertial sensor; The driving speed is calculated from the driving amount of the driving unit detected by the angle sensor, and the driving amount and the sphere Calculating a second arm speed of the damping position based on the same speed, calculating a third arm speed of the damping position based on the first arm speed and the second arm speed, and the third arm speed Calculating a correction speed for each of the driving units for driving the link between the robot body and the vibration isolating position, based on the driving amount, the driving speed, and the correction speed. And controlling said drive unit for driving said link between said damping position.

According to the robot control method according to this application example, the third arm speed at the damping position is calculated based on the information of the first arm speed obtained from the inertial sensor and the information of the second arm speed obtained from the angle sensor. Moreover, the correction speed for every drive part which drives the link between a robot main body and a damping position is calculated based on the calculated 3rd arm speed. By controlling each drive unit based on the correction speed for each drive unit, it is possible to suppress vibration occurring at the vibration damping position.

In more detail, for example, in the case of an ideal robot which is light in weight, high in rigidity and free from warpage or vibration due to warpage, the actual arm speed represents the ideal arm drive speed. In contrast, the first arm speed obtained from the inertial sensor is the speed detected by the actual movement of the vibration damping position where the inertial sensor is installed. In other words, it is a speed including a speed component such as vibration generated due to the actual operation due to the bending of the arm or the like that does not have ideal rigidity. Therefore, by evaluating the difference between the first arm speed and the second arm speed, the speed component due to the vibration can be extracted. It is possible to suppress the vibration by distributing the extracted speed component of the vibration to the drive unit for driving each link as the correction speed and generating a torque that suppresses the vibration.

By frequently suppressing the vibration by evaluating the difference between the first arm speed and the second arm speed, not only the vibration at the time of stopping but also the vibration of the arm at the time of operation (during operation) can be suppressed. As a result, the arm position accuracy at the time of driving can be controlled higher. In addition, since the vibration at the time of stopping can be suppressed without waiting for the arm stop operation, the waiting time until the vibration is stabilized after stopping can be shortened.

As mentioned above, according to the control method of the robot which concerns on this application example, high precision, high speed, etc. can be aimed at in a multi articulated robot.

[Example 5]

In the control method of the robot according to the application example, in order to calculate the third arm speed, the method further includes removing low frequency components below a predetermined frequency included in the waveform with the calculated third arm speed. desirable.

As in this application example, by further including removing the low frequency component of the third arm speed, it is possible to more accurately extract the vibration component by evaluating the difference between the first arm speed and the second arm speed. Specifically, by removing the low frequency component of the third arm speed, it is possible to reduce the influence of the correction that suppresses the original operation of the robot arm, the influence of the offset error of the inertial sensor, and the like.

[Example 6]

In the robot control method according to the application example, the driving unit is configured to calculate a correction speed for each of the driving units for driving the link between the robot body and the vibration damping position based on the third arm speed. It is preferable to carry out the preselected drive unit among them.

According to the control method of the robot according to the present application example, for example, in the case of knowing a driving unit that does not need control to suppress vibration in advance, or when there is a driving unit which does not perform the control intentionally, By excluding from the object, each process can be simplified, and a process can be performed more efficiently, for example, to raise a process speed.

Seventh Application Example

On the basis of the third arm speed of the control method of the robot according to the application example, the correction speed for each of the driving units for driving the link between the robot body and the damping position is calculated. It is preferable that calculation of the correction speed for every said drive part which drives the said link between the said damping positions is performed with weighting for every said drive part set previously.

According to the control method of the robot according to this application example, by assigning the magnitude of the correction speed for each drive unit to the weight for each preset drive unit, the robot's specifications, characteristics, or workloads to be performed on the robot (for example, In accordance with the weight of the object held by the robot hand, etc.), more effective suppression control of vibration can be performed. Specifically, for example, by setting the vibration damping torque of the drive portion closer to the robot main body to be larger, the vibration can be suppressed more effectively, or the object held by the robot hand is changed in a series of operations of the robot. In the case where the weight changes, the vibration can be suppressed more effectively.

[Example 8]

The robot according to the present application includes a robot body, an arm connected to the robot body, and including a plurality of links, a driver for driving the plurality of links, and an angle sensor for detecting a driving amount of the driver. A robot provided with an inertial sensor installed at a vibration damping position capable of suppressing vibration of the arm, and detecting at least one of an acceleration and an angular velocity at the vibration damping position, and the vibration damping position detected by the inertial sensor. A driving speed is calculated from a driving amount of a first calculating unit for calculating a first arm speed at the vibration damping position from at least one of acceleration and angular velocity, and the driving unit detected by the angle sensor; A second calculating unit for calculating a second arm speed at the damping position based on the first arm speed and the second arm speed; In addition, a third calculation unit for calculating a third arm speed and a fourth calculation unit for calculating a correction speed for each drive unit for driving the link between the robot body and the damping position based on the third arm speed. And a control unit for controlling the drive unit for driving the link between the robot body and the vibration damping position based on the drive amount, the drive speed, and the correction speed. It is done.

According to this application example, by using the above-described robot control apparatus as the robot, it is possible to more effectively suppress the vibration generated during the operation of the robot. As a result, a robot with higher precision, higher speed, and the like can be provided. For example, when making a high precision work | work while moving an arm, the fall of the precision of those work by vibration can be suppressed.

[Example 9]

In the control method of the robot according to the application example,

The said 3rd calculating part is further provided with the filter which removes the low frequency component below the predetermined frequency contained in the waveform of a said 3rd arm velocity, The robot of Claim 1 characterized by the above-mentioned.

As in this application example, by further including a filter for removing the low frequency component of the third arm speed, it is possible to more accurately extract the vibration component by evaluating the difference between the first arm speed and the second arm speed. Specifically, by removing the low frequency component of the third arm speed, it is possible to reduce the influence of the correction that suppresses the original operation of the robot arm, the influence of the offset error of the inertial sensor, and the like.

1 is a schematic diagram showing a link model of a robot according to a first embodiment.
2 is a functional block diagram of a control device for a robot according to the first embodiment.
3 is a flowchart showing a vibration suppression method as a control method for a robot.
4 is a schematic diagram showing a link model of a robot according to a second embodiment.
5 is a schematic diagram showing a link model of a robot according to a third embodiment.
6 is a schematic view of a robot according to a fourth embodiment.

EMBODIMENT OF THE INVENTION Below, embodiment which actualized this invention is described with reference to drawings. The following is one embodiment of the present invention and does not limit the present invention. In addition, in each following figure, in order to make description easy to understand, it may describe on the scale different from an actual thing.

(1st embodiment)

First, the control apparatus and control method of the robot concerning 1st Embodiment are demonstrated.

1 is a schematic diagram showing a link model of a robot 200 that mounts a control device for a robot according to the first embodiment. In addition, in FIG. 1, the control apparatus of a robot abbreviate | omits illustration.

The robot 200 is a six-axis articulated robot, an arm 20 including six linked links 10 (10a to 10f), a robot body 30 including an arm 20, and a link. It consists of six drive parts 40 (40a-40f) etc. which drive 10, respectively.

Hereinafter, "arm" is described as "a structure in which a plurality of links are connected." In addition, the connection part as a joint which connects each link is demonstrated as a drive part which has a function which drives each link.

As for the arm 20, the six links 10a-10f are connected in order through the drive parts 40a-40f, and the tip part of the link 10f which comprises one end of the arm 20 is a hand part. As a hand tool, a welding gun, a spray gun, etc. (not shown) are provided.

The other end part of the arm 20 is connected to the robot main body 30 via the drive part 40a.

The drive parts 40a, 40d, and 40f are provided with a motor for rotationally driving the shafts of the links 10a, 10d, and 10f to be connected.

The drive parts 40b, 40c, 40e are equipped with the motor which rotates a link so that the connection angle of the links 10b, 10c, 10e to connect may change.

Since the robot 200 having the above-described configuration has a cantilever structure in which one end of the arm 20 is supported by the robot body 30, the arm 20 has a rigidity and an operating speed of the arm 20 as a structure. , Bending due to heavy load or the like of the hand portion, or vibration due to bending occurs. In order to improve the operation position accuracy of the arm 20, to speed up the operation, etc., the position accuracy can be improved and the position control of the high speed operation can be performed by programming in consideration of this deflection amount and vibration in advance. However, in this case, the programming load is large, or a large difficulty such as an operation analysis is involved. Therefore, it is more simple and preferable to perform control to suppress them while detecting warpage and vibration occurring during operation.

The control device for robots according to the present embodiment has this control function. The function will be described below.

2 is a functional block diagram of the vibration suppression apparatus 100 as a control device for the robot according to the first embodiment.

The vibration damping apparatus 100 is the bending or vibration which generate | occur | produces in the arm 20 in the robot 200 during operation | movement or at the time (henceforth "bending and vibration", including a curvature "is called" vibration "). As a control device of the robot which suppresses this, it is comprised by the encoder 50 (50a-50f) as an angle sensor, the inertial sensor 60, the operation control part 80, etc.

First, the encoder 50 and the inertial sensor 60 will be described with reference to FIG. 1. Encoder 50 (50a-50f) detects each rotation angle (theta) _1- (theta) ₆ of the motor with which the drive part 40a-40f is equipped, ie, the drive amount of the drive part 40. FIG. The inertial sensor 60 is a six-axis sensor and detects the acceleration and the angular velocity of the installation position. That is, the inertial sensor 60 has accelerations (x ", y", z ") in the X, Y, and Z axes in FIG. 1 and angular velocities (Rx ', Ry', Rz around the X, Y, and Z axes). ') Is detected.

In the vibration damping apparatus 100, the inertial sensor 60 is installed in the damping position as a target position which suppresses the vibration of the arm 20. As shown in FIG. The robot 200 has shown the example which installed the inertial sensor 60 in the link 10f (namely, the link of the front-end | tip of the arm 20).

Next, returning to FIG. 2, the calculation control unit 80 will be described.

The calculation control part 80 is comprised from the 1st calculation parts 81 thru | or 4th calculation part 84, the control part 85, the Jacobian matrix calculation part 87, etc.

The 1st calculating part 81 calculates the 1st arm speed of a damping position from the acceleration and the angular velocity of the damping position which the inertial sensor 60 detects.

The second calculating unit 82 calculates the driving speed from the driving amount of the driving unit 40 detected by the encoder 50, and based on the Jacobian matrix J corresponding to the driving speed and the damping position, the arm driving speed of the vibration damping position. Calculate The Jacobian matrix J is calculated in the Jacobian matrix calculation unit 87 based on the driving amount information detected by the encoder 50 and the like.

The third calculation unit 83 calculates the third arm speed at the damping position based on the first arm speed calculated by the first calculation unit 81 and the second arm speed calculated by the second calculation unit 82. Moreover, the 3rd calculating part 83 is equipped with the filter 86, and removes the low frequency component below the predetermined frequency contained in the calculated waveform of the 3rd arm speed | rate. In this embodiment, 1 Hz or less low frequency component is removed as a predetermined frequency. The filter 86 is comprised by HPF (High Pass Filter).

The fourth calculating unit 84 determines the link 10 between the robot body 30 and the damping position by the inverse Jacobian matrix J ^-1 based on the third arm velocity calculated by the third calculating unit 83. The correction speed for each drive unit 40 to drive is calculated. The inverse Jacobian matrix J- ¹ is calculated in the Jacobian matrix calculation unit 87 based on the driving amount information detected by the encoder 50 and the like.

The control part 85 controls the drive part 40 which drives the link 10 between the robot main body 30 and the damping position based on the correction speed which the 4th calculating part 84 calculated.

3 is a flowchart showing a vibration suppression method by the vibration suppression apparatus 100 as a control method of the robot of the present embodiment.

With reference to the flowchart of FIG. 3, the vibration damping method by the calculation control part 80 and the calculation control part 80 is demonstrated concretely with respect to the link model of the robot 200 shown in FIG.

First, with the operation of the robot 200, the configuration specifications (distance between the links of each link, etc.) of the robot 200, the information of the vibration damping position in the robot 200 provided with the inertial sensor 60, and the encoder ( Based on the rotation angles θ ₁ to θ ₆ and the like detected by 50) (50a to 50f), the Jacobian matrix J corresponding to the damping position is obtained (step S1).

Next, the inverse matrix of the Jacobian matrix J is calculated to find the inverse Jacobian matrix J- ¹ (step S2).

Jacobian matrix J and inverse Jacobian matrix J- ¹ satisfy the following relational expression.

Next, in the 1st calculating part 81, the acceleration and angular velocity of the damping position which the inertial sensor 60 detects (step S3).

(x ₁ ", y ₁ ", z ₁ ", Rx ₁ ', Ry ₁ ', Rz ₁ ')

Arm speed of the damping position from the

(x ₁ ', y ₁ ', z ₁ ', Rx ₁ ', Ry ₁ ', Rz ₁ ')

Is calculated (step S4).

Next, in the second calculating section 82, the driving speed θ ′ is obtained from the driving amount (rotation angles θ ₁ to θ ₆ ) of the driving units 40a to 40f that the encoder 50 detects (step S5).

(θ ' ₁ , θ' ₂ , θ ' ₃ , θ' ₄ , θ ' ₅ , θ' ₆ )

(Step S6), the second arm speed of the vibration damping position based on the Jacobian matrix J corresponding to the driving speed and the vibration damping position.

(x ₂ ', y ₂ ', z ₂ ', Rx ₂ ', Ry ₂ ', Rz ₂ ')

Is calculated (step S7).

Next, in the third calculating section 83, the difference between the first arm speed calculated by the first calculating section 81 and the second arm speed calculated by the second calculating section 82 is calculated, and the damping position is calculated. 3 arm speed

(x ₁ '-x ₂ ', y ₁ '-y ₂ ', z ₁ '-z ₂ ', Rx ₁ '-Rx ₂ ', Ry ₁ '-Ry ₂ ', Rz ₁ '-Rz ₂ ')

(Step S8), the low frequency component below the predetermined frequency contained in the calculated waveform of the third arm velocity is removed by the filter 86 included in the third calculating section 83 (step S9). Specifically, in this embodiment, 1 Pa or less is removed by HPF.

Next, in the 4th calculating part 84, the link between the robot body 30 and the damping position by the inverse Jacobian matrix J ^-1 based on the 3rd arm velocity calculated by the 3rd calculating part 83 is next. The correction speed θ'r (θ'r ₁ to θ'r ₆ ) for each drive unit 40 for driving (10) is calculated (step S10). The correction speed θ'r (θ'r ₁ to θ'r ₆ ) satisfies the relational expression shown below.

Next, in the control unit, the link 10 between the robot body 30 and the vibration isolating position is based on the driving amount (rotation angles θ ₁ to θ ₆ ), the driving speed θ 'and the correction speed θ'r. The feedback control is performed with respect to the drive unit 40 for driving (step S11). Specifically, each of the driving units 40a to 40f generates a torque in a direction to cancel the correction speeds θ'r (θ'r ₁ to θ'r ₆ ) distributed to each of the driving units 40a to 40f. Control the motor to be provided.

Next, it is determined whether or not control is to be terminated (step S12). When the control of the robot is continued, the process returns to step S1.

As mentioned above, according to the control apparatus and control method of the robot which concern on this embodiment, the following effects can be acquired.

The third arm speed at the damping position is calculated based on the information of the first arm speed obtained from the inertial sensor 60 and the information of the second arm speed obtained from the encoder 50. Further, based on the calculated third arm speed, the correction speed for each drive unit 40 for driving the link 10 between the robot body 30 and the vibration damping position is calculated. By controlling each of the driving units 40 based on the correction speed for each of the driving units 40, it is possible to suppress the vibration occurring at the vibration damping position.

In more detail, the arm drive speed of the damping position obtained from the encoder 50 is computed from the drive amount controlled to follow the instruction (for example, the preprogrammed instruction) which controls the some drive part 40. Since it is a speed | rate, the speed component by the vibration etc. which generate | occur | produce in the arm 20 with operation | movement is the speed with very little. That is, in the case of an ideal robot which is light in weight and high in rigidity and does not have warpage or vibration due to warpage, the actual arm 20 exhibits the ideal arm driving speed.

In contrast, the first arm speed obtained from the inertial sensor 60 is a speed detected by the actual movement of the damping position where the inertial sensor 60 is installed. That is, it is the speed including the speed component, such as the vibration which arises with an actual operation | movement by the bending of the arm 20 etc. which do not have ideal rigidity. Therefore, by evaluating the difference between the first arm speed and the second arm speed, the speed component due to the vibration can be extracted. It is possible to suppress the vibration by distributing the extracted speed component of the vibration to the drive unit 40 driving the respective links 10 as the correction speed and generating a torque that suppresses the vibration.

By frequently suppressing the vibration by evaluating the difference between the first arm speed and the second arm speed, not only the vibration at the stop but also the vibration of the arm 20 at the time of driving (in operation) can be suppressed. . As a result, the arm position accuracy at the time of driving can be controlled higher. In addition, since the vibration at the time of stopping can be suppressed without waiting for the stop operation of the arm 20, the waiting time until the vibration is stabilized after stopping can be shortened.

Further, by further comprising a filter 86 for removing the low frequency components below the predetermined frequency included in the waveform of the third arm speed, the extraction of the vibration component by evaluating the difference between the first arm speed and the second arm speed is performed. This can be done more accurately. Specifically, by removing the low frequency component of the third arm speed, the correction that suppresses the original operation of the arm 20 is erroneously performed, and the influence of the offset error of the inertial sensor 60 can be reduced. .

As described above, according to the control apparatus and control method of the robot according to the present embodiment, it is possible to achieve higher precision, higher speed, and the like in the articulated robot.

In addition, although it was demonstrated that the 3rd calculating part 83 is equipped with the filter 86 and removes the low frequency component of 1 Hz or less as the low frequency component below the predetermined frequency contained in the calculated waveform of the 3rd arm speed | rate, It does not necessarily have to be less than 1 ms. It is preferable to determine suitably according to the damping situation of a damping position. In addition, the filter 86 does not necessarily need to be provided. For example, the filter 86 can be omitted in the case where there is no low frequency component to be removed in the calculated third arm speed or when it can be ignored.

In addition, although the Jacobian matrix J and the inverse Jacobian matrix J- ¹ were computed by the Jacobian matrix calculation part 87 based on the drive amount information detected by the encoder 50 etc., it is not necessarily the case of the robot 200. It does not need to be calculated while operating. For example, the timing of vibration damping is determined in advance, and the Jacobian matrix J and the reverse Jacobian matrix J ^-1 prepared according to the timing are prepared in advance as a table, and the corresponding Jacobian matrix J and the reverse are adjusted according to the timing of vibration damping. The method of selecting the Jacobian J- ¹ may be used. In this case, it is not necessary to include the Jacobian calculation unit 87 in the calculation control unit 80.

(Second Embodiment)

Next, the control apparatus and control method of the robot concerning 2nd Embodiment are demonstrated. In addition, in description, about the same component part as embodiment mentioned above, the same code | symbol is used and the overlapping description is abbreviate | omitted.

4 is a schematic diagram showing a link model of the robot 201 that mounts the control device for robots according to the second embodiment.

The robot 201 is a six-axis articulated robot similar to the robot 200 on which the vibration damping device 101 (not shown) is mounted.

The vibration damping apparatus 101 is the vibration damping apparatus 100 except for using the gyro sensor (hereinafter, referred to as an inertial sensor 61) that detects three angular velocities Rx ', Ry', and Rz 'as an inertial sensor. Is the same as In addition, in this embodiment, the inertial sensor 61 is provided in the link 10d. That is, in the first embodiment, the inertial sensor 60 is provided in the link 10f, and the vibration of the tip portion of the arm 20 is suppressed. It is installed in the link 10d, and the intermediate | middle degree of the arm 20 is made into the damping position. In addition, in the first embodiment, the inertial sensor 60 was a six-axis acceleration and angular velocity sensor, whereas in the present embodiment, a gyro sensor that detects the three-axis angular velocity Rx ', Ry', and Rz 'is used. have. 2nd Embodiment is the same as that of 1st Embodiment except these points.

The feedback control for suppressing the vibration in this embodiment is performed with respect to the drive part located in the robot main body 30 side from the damping position in which the inertial sensor 61 is provided. That is, based on the three-axis angular velocity information, control is performed on four driving units of the driving units 40a to 40d having one degree of freedom. In this way, in the case of redundant control, the control is distributed by the weighted pseudo inverse matrix.

The Jacobian matrix J ₁ and the weighted pseudo inverse matrix J ₁ ^# corresponding to the present embodiment are shown below.

In the above equation, the weighted pseudo-inverse matrix J ₁ ^# is given by the following relational expression.

J ₁ ^#- ＝ W ^-1 J ₁ ^T (J ₁ ^T W ^-1 J ₁ ^T ) ^-1

J ₁ ^# is a matrix whose weighted sum of squares of the correction speeds θ'r (θ'r ₁ to θ'r ₄ ) distributed to each of the drive units 40a to 40d obtained thereby is minimized. That is, with respect to the degree of freedom, the movement for suppressing the vibration is made the minimum value.

W is a 4 × 4 weighting matrix. That is, the weight is given to the value of the correction speed | rate (theta) 'r distributed to each of the drive parts 40a-40d.

The vibration damping apparatus 101 and the vibration damping method are the same as in the case of the first embodiment except that the weighted pseudo inverse matrix J ₁ ^# is used instead of the inverse Jacobian matrix J ⁻¹ .

Similarly to this embodiment, even with a robot having the same specification, vibration can be similarly suppressed by changing the position where the inertial sensor is installed in accordance with the position of the object to be damped.

In addition, by distributing the magnitude of the correction speed for each of the driving units 40 by the weight for each of the driving units 40 set in advance, the specifications and characteristics of the robot 201 or the workload for the robot 201 to be performed (for example, For example, the vibration suppression control can be performed more effectively in accordance with the weight of the object held by the robot hand. Specifically, for example, by setting the vibration damping torque of the drive unit 40 near the robot body 30 to a larger value, the vibration can be suppressed more effectively, or in a series of operations of the robot 201, When the object held by the robot hand changes and the weight changes, the vibration can be suppressed more effectively.

(Third Embodiment)

Next, the control apparatus and control method of the robot concerning 3rd Embodiment are demonstrated. In addition, in description, about the same component part as embodiment mentioned above, the same code | symbol is used and the overlapping description is abbreviate | omitted.

5 is a schematic diagram showing a link model of the robot 202 on which the control device for robots according to the third embodiment is mounted.

The robot 202 is a two-armed articulated robot on which the vibration damping device 102 (not shown) is mounted. The two arms 21, the turning arm 22, the robot body 30, and the vibration damping which form a seven-axis both arms are provided. The device 102 and the like.

In the arm 21, seven links 11b-11h are connected in order through the drive parts 41b-41h. The front end of the link 11h constituting one end of the arm 21 is provided with a hand tool, a welding gun, a spray gun, or the like (not shown) as the hand portion.

The swing arm 22 is comprised from the link 11a, the drive part 41a, etc., One end of the link 11a is connected to the robot main body 30 via the drive part 41a, and the other The end part supports two arms 21 connected to each drive part 41b via two drive parts 41b. That is, the other end part of the arm 21 is connected to the turning arm 22 via the drive part 41b.

The drive parts 41a, 41b, 41d, 41f, and 41h are provided with a motor for rotationally driving the shafts of the links 11a, 11b, 11d, 11f, and 11h to be connected.

The drive parts 41c, 41e, 41g are equipped with the motor which rotates a link so that the connection angle of the links 11c, 11e, 11g to connect may change.

Each drive part 41a-41h is provided with the encoder 50a-50h which detects the drive amount (rotation angle of a motor).

Encoder 50 (50a-50h) detects each rotation angle (theta) _1-( theta) ₈ of the motor with which drive part 41a-41h is equipped.

The inertial sensor 61 is provided at each link 11e of the arm 21.

Feedback control for suppressing vibration in this embodiment is performed independently with respect to each arm 21. Therefore, the vibration damping apparatus 102 is comprised so that two vibration damping apparatus 101 may be processed in parallel.

In addition, the vibration damping device 102 does not have such a structure (a structure which comprises two vibration damping devices 101 and performs parallel processing), but time-sharing one vibration damping device 101 with respect to each arm 21. The method may be used. That is, it is good also as a structure which independently performs the process by one damping apparatus 101, switching with respect to the two arms 21. As shown in FIG.

In addition, the state feedback control for suppressing the vibration in this embodiment is not performed about the turning arm 22 common to each arm 21. That is, based on the three-axis angular velocity information detected by the inertial sensor 61 included in each arm 21, control is performed for four driving units of the driving units 41b to 41e.

The Jacobian matrix J ₂ and the weighted pseudo inverse matrix J ₃ ^# corresponding to the present embodiment are shown below.

In the above equation, since the Jacobian matrix J ₂ is a matrix in which the five axes θ ₁ to θ ₅ of the driving units 41a to 41e are reflected, it is given as a matrix of 3 × 5. On the other hand, since the driving unit to be controlled is the four axes θ ₂ to θ ₅ of the driving units 41b to 41e, the weighted pseudo inverse matrix J ₃ ^# is applied to the four axes θ ₂ to θ ₅ . It is calculated | required by the following relational expressions using the Jacobian matrix ₃ of 3x5.

J ₃ ^#- ＝ W ^-1 J ₂ ^T (J ₂ ^T W ^-1 J ₂ ^T ) ^-1

J ₃ ^# is a matrix whose weighted sum of squares of the correction speeds θ'r (θ'r ₂ to θ'r ₅ ) distributed to each of the drive units 41b to 41e obtained thereby is minimized. That is, similarly to the second embodiment, the motion for suppressing the vibration is set to the minimum value for the degree of freedom.

As in the present embodiment, the driving unit is excluded from the object of control when the driving unit does not need to control the vibration in advance, or when there is a driving unit that does not intentionally perform the control. The processing is simplified, and the processing can be performed more efficiently, such as to increase the processing speed.

(Fourth Embodiment)

Next, the robot concerning 4th Embodiment which shows the robot in embodiment mentioned above more concretely is demonstrated. In addition, in description, about the same component part as embodiment mentioned above, the same code | symbol is used and the overlapping description is abbreviate | omitted.

6 is a schematic diagram of the robot 203 according to the fourth embodiment.

The robot 203 is a two-armed articulated robot on which the vibration damping device 102 is mounted, and includes two arms 23, a swinging arm 24, a hand 70, a robot body 30, and a vibration damping device 102. It is comprised including these.

As for the arm 23, the link 13b and the link 13c are connected through the drive part 43c, and the hand 70 is provided in one end of the arm 23 via the drive part 43d. . The hand 70 has a function of holding a predetermined object.

The swinging arm 24 is comprised from the link 13a, the drive part 43a, etc., One end of the link 13a is connected to the robot main body 30 via the drive part 43a, and the other The end part supports two arms 23 connected to each drive part 43b via two drive parts 43b. That is, the other end part of the arm 23 is connected to the turning arm 24 via the drive part 43b.

The drive parts 43b and 43c are equipped with the motor which rotates the links 13b and 13c so that the connection angle of the link to connect may change.

The drive parts 43a and 43d are provided with the link 13a which each connects, and the motor which drives the shaft of the hand 70 to rotate.

Moreover, each drive part 43a-43d is provided with the encoder 50a-50d which detects the drive amount (rotation angle of a motor).

The inertial sensor 61 is provided in each link 13c of the arm 23.

The robot body 30 supports two arms 23 via the swinging arm 24 and the swinging arm 24, and is installed on the floor or the like. In addition, a robot controller 31 including a vibration suppression apparatus 102 or the like is provided inside the robot body 30. The robot controller 31 controls the entire robot 203 including control of the vibration suppression apparatus 102 and the like.

Also in the multi-joint robot of both arms of the above structure, since the vibration damping apparatus 102 is mounted, the vibration which generate | occur | produces in the arm 23 in operation can be suppressed similarly to the control method of the robot mentioned above. As a result, it is possible to provide a robot with a further enhanced function as an articulated robot capable of highly precise and high-speed operation flexibly. For example, in the case where a spray gun or the like is attached to the hand 70 and the coating object is coated, the vibration is suppressed in the case where coating nonuniformity due to vibration generated in the arm 23 tends to occur. By doing so, the coating nonuniformity can be reduced.

10: Link
20: arm
30: robot body
31: robot controller
40:
50: encoder
60: inertial sensor
70: hand
80: operation control unit
81 to 84: first to fourth calculation unit
85: control unit
86: filter
87: Jacobian matrix operation unit
100: damping device
200: Robot

Claims (9)

Robot body,
An arm connected to the robot body and further comprising a plurality of links;
A driving unit for driving a plurality of the links;
A control apparatus for a robot having an angle sensor for detecting a driving amount of the driving unit,
An inertial sensor provided at a vibration damping position capable of suppressing vibration of the arm, and detecting at least one of acceleration and angular velocity at the vibration damping position;
A first calculating unit for calculating a first arm velocity at the vibration damping position from at least one of an acceleration and an angular velocity at the vibration damping position detected by the inertial sensor;
A second calculating unit calculating a driving speed from the driving amount of the driving unit detected by the angle sensor, and calculating a second arm speed at the damping position based on the driving amount and the driving speed;
A third calculating unit calculating a third arm speed based on the first arm speed and the second arm speed;
A fourth calculating unit calculating a correction speed for each driving unit for driving the link between the robot body and the damping position based on the third arm speed;
A control unit for controlling the drive unit for driving the link between the robot body and the damping position based on the drive amount, the drive speed, and the correction speed
Robot control device having a. The method of claim 1,
And the third calculating section further includes a filter for removing low frequency components below a predetermined frequency included in the waveform of the third arm velocity. An arm including a plurality of linked links,
Drive unit for driving a plurality of the links
As a control method of a robot having:
Detecting a first arm velocity at a damping position for suppressing vibration of the arm;
Calculating a third arm speed based on a difference between the calculated first arm speed and a second arm speed that is a speed at which the vibration damping position is driven based on the driving amount of the driving unit;
Correcting and controlling the drive unit based on the calculated third arm speed
Robot control method provided with. Robot body,
An arm connected to the robot body and further comprising a plurality of links;
A driving unit for driving a plurality of the links;
A control method of a robot having an angle sensor for detecting a driving amount of the driving unit,
Detecting at least one of an acceleration and an angular velocity of the vibration damping position by an inertial sensor provided at a vibration damping position for suppressing vibration of the arm;
Detecting, by an angle sensor, an amount of driving of the drive unit for driving the link between the vibration damping position and the robot body;
Calculating a first arm velocity of the vibration damping position from at least one of the acceleration and the angular velocity of the vibration damping position detected by the inertial sensor;
Calculating a driving speed from the driving amount of the driving unit detected by the angle sensor, calculating a second arm speed of the vibration damping position based on the driving amount and the driving speed;
Calculating a third arm speed at the damping position based on the first arm speed and the second arm speed;
Calculating a correction speed for each of the driving units for driving the link between the robot body and the damping position based on the third arm speed;
Controlling the drive unit for driving the link between the robot body and the damping position based on the drive amount, the drive speed, and the correction speed.
Robot control method provided with. 5. The method of claim 4,
In order to calculate the third arm speed, the control method of the robot further comprising removing low frequency components below a predetermined frequency included in the calculated waveform of the third arm speed. The method according to claim 4 or 5,
Based on the third arm speed, calculating the correction speed for each of the driving units for driving the link between the robot body and the damping position is performed on a previously selected driving unit among the driving units. How to control the robot. 7. The method according to any one of claims 4 to 6,
Calculating the correction speed for each of the driving units for driving the link between the robot body and the damping position based on the third arm speed, wherein the link between the robot body and the damping position is driven. The control method of the robot characterized in that calculation of the correction speed for every said drive part is carried out with weighting to every said drive part set previously. Robot body,
An arm connected to the robot body and further comprising a plurality of links;
A driving unit for driving a plurality of the links;
A robot having an angle sensor for detecting the driving amount of the drive unit,
An inertial sensor provided at a vibration damping position capable of suppressing vibration of the arm, and detecting at least one of an acceleration and an angular velocity at the vibration damping position;
A first calculating unit for calculating a first arm velocity at the vibration damping position from at least one of an acceleration and an angular velocity at the vibration damping position detected by the inertial sensor;
A second calculating unit calculating a driving speed from the driving amount of the driving unit detected by the angle sensor, and calculating a second arm speed at the damping position based on the driving amount and the driving speed;
A third calculating unit calculating a third arm speed based on the first arm speed and the second arm speed;
A fourth calculating unit calculating a correction speed for each driving unit for driving the link between the robot body and the damping position based on the third arm speed;
A control unit for controlling the drive unit for driving the link between the robot body and the damping position based on the drive amount, the drive speed, and the correction speed
Robot having a control device comprising a. 9. The method of claim 8,
And the third calculating section further includes a filter for removing low frequency components below a predetermined frequency included in the waveform of the third arm speed.

Images