JP2015104789A - Robot - Google Patents

Abstract

PROBLEM TO BE SOLVED: To suppress vibrations of an arm of a robot.SOLUTION: A robot includes a plurality of joints including a first joint and a second joint capable of having a different rotating direction from the first joint, and a plurality of arm members including a first arm member rotatably provided on a base through the first joint. The first arm member (or a part in the first joint which rotates with the first arm member) is provided with a first inertia sensor, thereby controlling the plurality of joints on the basis of the output of the first inertia sensor. Therefore, from the output of the first inertia sensor, other joints can be controlled while considering influences which the action of the first joint adds to the plurality of other joints, so that vibrations of an arm can be suppressed.

Description

  The present invention relates to a robot.

  A robot having an arm in which a plurality of arm members are connected by joints is known. This robot can perform various operations by attaching a robot hand or the like to the tip of the arm. However, even if the robot hand or the like at the tip of the arm is moved to a target position and stopped to perform the work, the work cannot be started because the tip of the arm vibrates for a while.

  Therefore, a technique has been proposed in which an inertial sensor is mounted on the distal end side of the arm, and the arm vibration is controlled by controlling the movement of each joint of the arm so that the output of the inertial sensor is reduced ( Patent Document 1).

JP 2005-242794 A

  However, among robots with many joints, there are robots that change the direction of the rotation axis of a joint ahead of them by moving a certain joint. Even if the applied technology is applied, there is a problem that it is difficult to sufficiently suppress the vibration of the arm. This is because even when the tip of the arm vibrates in the same manner, for example, the control content (control amount and control direction) of the joint on the tip side of the arm changes depending on the rotational position of the joint on the base side. This is because the control becomes complicated. In addition, when the tip of the arm vibrates, vibration is also generated in the joint on the base side and the rotational position changes every moment, and as a result, the direction of the joint on the tip side changes every moment. Control is complicated. In addition, the effects of joint deformation and mechanical errors accumulate as the tip approaches. For this reason, when the proposed technique is applied to a robot having the above-described problems, vibration may be amplified. As described above, many problems remain to suppress vibration of a robot having a plurality of joints having different directions of the rotation axis.

  The present invention has been made in order to solve the above-described problems of the prior art, and an object of the present invention is to provide a technique capable of suppressing vibration of a robot arm.

In order to solve at least a part of the problems described above, the robot of the present invention employs the following configuration. That is,
A plurality of joints including a second joint having a rotation direction different from that of the first joint and the first joint; and a first arm member provided rotatably with respect to a base via the first joint Including a plurality of arm members;
A first angular velocity sensor provided on the first arm member or the first joint;
It is characterized by providing.

  If it carries out like this, the motion of the 1st arm member attached to the base via the 1st joint can be detected from the output of the 1st angular velocity sensor. Therefore, it is possible to control the other joints in consideration of the influence of the movement of the first joint between the first arm member and the base on the movements of the other plurality of joints. Can be suppressed. Further, since the movement of the first joint can be detected based on the output of the first angular velocity sensor, it is possible to suppress the accumulation of the influence of the error toward the distal end side of the arm. For this reason, it is possible to suppress the possibility that the vibration of the arm is amplified by controlling each joint.

  In the robot of the present invention described above, the second angular velocity sensor may be provided on an arm member different from the first arm member among the plurality of arm members.

  By doing so, in addition to the output of the first angular velocity sensor, it is possible to control using the output of the second angular velocity sensor, so it is possible to further suppress the vibration of the arm. Further, if a uniaxial output is used, the amount of calculation for control becomes small, so that the vibration of the arm can be further suppressed.

  Further, the robot of the present invention described above includes a control unit that feedback-controls the joint on the base side rather than the arm member provided with the second angular velocity sensor, based on the output of the second angular velocity sensor. Also good.

  In the output of the second angular velocity sensor, not only the movement of the arm member provided with the second angular velocity sensor but also the movement of the joint on the base side with respect to the arm member appears. Therefore, if the joint on the base side is feedback-controlled based on the output of the second angular velocity sensor, the vibration of the arm can be further suppressed. In addition, since the joint on the base side where the vibration is large can be controlled, the vibration of the arm can be more effectively suppressed.

  In the robot of the present invention described above, feedback control is performed based on the output of the second angular velocity sensor for the joint on the base side of the arm member on which the second angular velocity sensor is provided and the direction of the rotation axis coincides. It is good as well. Note that “the directions of the rotation axes match” is not limited to a perfect match, but may be a match. Therefore, the case where the directions of the rotation axes are different within a range of ± 5 ° corresponds to the case where “the directions of the rotation axes coincide” in this specification.

  Naturally, the output of the second angular velocity sensor reflects the movement of the joint on the base side of the arm member provided with the second angular velocity sensor. However, in addition to the joint, if there is a joint with the same rotational axis direction on the base side, the base side joint (the base side of the arm member provided with the second angular velocity sensor) The motion of the joint and the direction of the rotation axis are also reflected in the output of the second angular velocity sensor. Moreover, the movement of the joint on the base side is amplified and reflected. Therefore, the second angular velocity sensor is provided if the joint on the base side of the arm member provided with the second angular velocity sensor and the joint having the same rotational axis direction are feedback-controlled based on the output of the second angular velocity sensor. Since the joints other than the joints of the arm members can be controlled with high accuracy, the arm vibration can be suppressed. In addition, since the joint on the base side where the vibration is large can be controlled, the vibration of the arm can be more effectively suppressed.

The robot of the present invention can be grasped in the following manner. That is,
A plurality of joints including a first joint and a second joint having a rotation direction different from the first joint;
A plurality of arm members connected by the plurality of joints;
A first angular velocity sensor provided on one of the plurality of arm members;
A second angular velocity sensor provided on the arm member different from the arm member provided with the first angular velocity sensor;
It can also be grasped as a robot characterized by having

  In such a robot of the present invention, a plurality of joints can be controlled based on the outputs of the first angular velocity sensor and the second angular velocity sensor, so that vibration of the robot arm can be quickly suppressed. . In addition, since the vibration of the arm can be suppressed promptly in this way, it is possible to suppress the vibration of the arm even when the rigidity of the arm (the rigidity of the arm member or joint) is low.

  The robot of the present invention described above may include a control unit that controls a plurality of joints based on the output of one axis of the first angular velocity sensor and the output of one axis of the second angular velocity sensor.

  In this way, even when a plurality of angular velocity sensors are used, vibration of the robot arm can be suppressed with simple control. In addition, since the amount of calculation is suppressed by using the output of one axis, it is possible to perform high-speed control and suppress arm vibration.

  In the robot of the present invention described above, a plurality of joints may be controlled based on the output of one axis of the first angular velocity sensor and the output of three axes of the second angular velocity sensor.

  Even if the direction of the rotation axis of a joint changes due to the movement of another joint, if the second angular velocity sensor that uses a triaxial output is attached to the arm member that changes the direction of the rotation axis of the joint, multiple It is possible to suppress the vibration of the arm by controlling the joint. In addition, the calculation amount can be suppressed by using a uniaxial output for the first angular velocity sensor, and the control accuracy can be ensured by using a triaxial output for the second angular velocity sensor. As a result, it is possible to further suppress the vibration of the arm.

  In the robot of the present invention described above, a plurality of joints may be feedback-controlled as follows based on the outputs of the first angular velocity sensor and the second angular velocity sensor. First, based on the output of the first angular velocity sensor, feedback control is performed on the joint on the base side with respect to the arm member provided with the first angular velocity sensor. In addition, based on the output of the second angular velocity sensor, feedback control is performed on the joint on the base side with respect to the arm member provided with the second angular velocity sensor.

  The movement of the joint can be detected with higher sensitivity by providing an angular velocity sensor at a position farther from the base than the joint. Therefore, if the joint is feedback-controlled by the above-described method based on the output of the first angular velocity sensor and the output of the second angular velocity sensor, the joint can be controlled with high accuracy and the vibration of the arm can be suppressed. .

  In the robot of the present invention described above, the rotation axis of the joint that is feedback-controlled based on the output of the first angular velocity sensor and the joint that is feedback-controlled based on the output of the second angular velocity sensor are orthogonal to each other. It is good as a joint.

  Since the joints whose rotation axes are orthogonal to each other can be controlled independently, they can be controlled easily and accurately. As a result, it is possible to control the movements of a plurality of joints existing in the arm easily and accurately to suppress arm vibration.

  In the robot of the present invention described above, the first angular velocity sensor may be provided on the arm member closest to the base among the arm members having different rotation axes of the joints on both sides.

  By doing so, the movements of the plurality of joints can be controlled easily and accurately, so that the vibration of the arm can be suppressed.

  In the robot of the present invention described above, a vibration gyro sensor may be used as the first angular velocity sensor.

  Since the vibration type gyro sensor can be downsized as compared with other types of gyro sensors, the robot can be downsized.

  Further, in the robot of the present invention using the vibration gyro sensor as the first angular velocity sensor, a vibration gyro sensor including a piezoelectric material may be used as the first angular velocity sensor.

  Among vibration gyro sensors, a vibration gyro sensor including a piezoelectric material (a so-called piezoelectric vibration gyro sensor) can be made smaller than an electrostatic vibration gyro sensor. For this reason, it is possible to further reduce the size of the robot by using a vibration type gyro sensor including a piezoelectric material as the first angular velocity sensor.

The robot of the present invention can be grasped in the following manner. That is,
A plurality of joints including a first joint and a second joint having a rotation direction different from the first joint;
A plurality of arm members connected by the plurality of joints;
An angular velocity sensor provided on one of the plurality of arm members;
The direction of the rotation axis coincides with the joint on the base side of the arm member provided with the angular velocity sensor, and the joint provided on the base side is based on the output of the angular velocity sensor. A control unit for controlling
It can be grasped as a robot characterized by comprising.

  In such a robot of the present invention, the joint on the base side of the arm member provided with the angular velocity sensor is coincident with the direction of the rotation axis, and the joint provided on the base side with respect to the joint is provided. Since it can control based on the output of an angular velocity sensor, it becomes possible to suppress the vibration of an arm using an angular velocity sensor fewer than the number of joints. In addition, since the joint on the base side where the vibration is large can be controlled, it is possible to further suppress the vibration of the arm.

The robot of the present invention can be grasped in the following manner. That is,
An arm including a first joint and a second joint having a rotation direction different from that of the first joint;
An inertial sensor provided on the arm;
A controller that controls the first joint by sampling the output of the inertial sensor at a cycle of 100 Hz or more;
It can also be grasped as a robot characterized by having

  If the period of sampling the output of the inertial sensor is too slow, it may be easier to vibrate by controlling the arm. Therefore, if the period for sampling the output of the inertial sensor is set to 100 Hz or more, it is possible to reliably suppress arm vibration.

The robot of the present invention can be grasped in the following manner. That is,
An arm including a first joint and a second joint having a rotation direction different from that of the first joint;
An inertial sensor provided on the arm;
A force sensor provided closer to the tip of the arm than the inertial sensor;
It can also be grasped as a robot characterized by having

  In such a robot of the present invention, a plurality of joints can be controlled using not only the output of the inertial sensor but also the output of the force sensor, so that the vibration of the arm can be reliably suppressed.

The robot of the present invention can be grasped in the following manner. That is,
An arm including a first joint provided on a base and a second joint having a rotation direction different from that of the first joint;
An inertial sensor provided on the arm,
When the angular acceleration of the first joint is rotated 90 ° at 2200 ° / s ² , the time until the vibration at the tip of the arm changes from −0.05 mm to +0.05 mm is 1 second or less. It can also be understood as a characteristic robot.

  If the time until the vibration at the tip of the arm changes from −0.05 mm to +0.05 mm is set to such a time, it is possible to avoid a practical problem caused by the vibration of the arm. .

The robot of the present invention can be grasped in the following manner. That is,
An arm including a first joint provided on a base and a second joint having a rotation direction different from that of the first joint;
An inertial sensor provided on the arm,
When the first joint is rotated by 90 ° with a maximum angular acceleration, the time until the vibration at the tip of the arm changes from −0.05 mm to +0.05 mm is 1 second or less. It can also be grasped.

Alternatively, the robot of the present invention can be grasped in the following manner. That is,
An arm including a first joint provided on a base and a second joint having a rotation direction different from that of the first joint;
An inertial sensor provided on the arm,
When the angular acceleration of the second joint is rotated 90 ° at 2200 ° / s2, the time until the vibration at the tip of the arm changes from −0.1 mm to +0.1 mm is 0.5 seconds or less. It can also be grasped as a robot characterized by.

Alternatively, the robot of the present invention can be grasped in the following manner. That is,
An arm including a first joint provided on a base and a second joint having a rotation direction different from that of the first joint;
An inertial sensor provided on the arm,
When the first joint is rotated by 90 ° with a maximum angular acceleration, the time until the vibration at the tip of the arm changes from −0.05 mm to +0.05 mm is 1 second or less. It can also be grasped.

  In the robot of the present invention grasped in these modes, it is possible to avoid a detrimental effect due to the vibration of the arm.

Further, the present invention described above can be grasped in the form of a control device that controls a robot. That is, the control device of the present invention
A plurality of joints including a second joint having a rotation direction different from that of the first joint and the first joint; and a first arm member provided rotatably with respect to a base via the first joint A control device for a robot comprising: a plurality of arm members including: a first angular velocity sensor provided in the first arm member or the first joint;
Based on the output of the second angular velocity sensor, a control unit that feedback-controls the joint on the base side relative to the arm member provided with the second angular velocity sensor is provided. be able to.

Or
A plurality of joints including a first joint and a second joint having a rotation direction different from the first joint, a plurality of arm members connected by the plurality of joints, and one of the plurality of arm members A first angular velocity sensor provided on the arm member, and a second angular velocity sensor provided on the arm member different from the arm member provided with the first angular velocity sensor. And
A control device comprising a control unit that controls the plurality of joints based on a uniaxial output of the first angular velocity sensor and a uniaxial or triaxial output of the second angular velocity sensor. it can.

Or
A plurality of joints including a first joint and a second joint having a rotation direction different from the first joint, a plurality of arm members connected by the plurality of joints, and one of the plurality of arm members An angular velocity sensor provided on the arm member, and a robot control device comprising:
The direction of the rotation axis coincides with the joint on the base side of the arm member provided with the angular velocity sensor, and the joint provided on the base side is based on the output of the angular velocity sensor. It can be grasped as a control device characterized by comprising a control unit for controlling the device.

Or
An arm including a first joint and a second joint having a rotation direction different from that of the first joint; an inertial sensor provided on the arm; and an angle sensor for detecting an angle of the first joint. A robot control device comprising:
By sampling the outputs of the inertial sensor and the angle sensor at a cycle of 100 Hz or more, a control unit that controls the first joint can be grasped.

Or
An arm including a first joint and a second joint having a rotation direction different from each other; an inertial sensor provided on the arm; and provided on a distal end side of the arm with respect to the inertial sensor. A robot control device comprising a force sensor,
A control unit that controls the plurality of joints based on the output of the inertial sensor, the output of the force sensor, and the output of the force sensor may be provided.

Furthermore, the present invention described above can be grasped in the form of a robot system. That is, the robot system of the present invention is
A robot system comprising a robot and a control device for controlling the robot,
The robot is
A plurality of joints including a first joint and a second joint having a rotation direction different from the first joint;
A plurality of arm members including a first arm member rotatably provided to the base via the first joint;
A first angular velocity sensor provided on the first arm member or the first joint;
With
Based on the output of the second angular velocity sensor, the control device feedback-controls the joint on the base side with respect to the arm member provided with the second angular velocity sensor. can do.

Or
A robot system comprising a robot and a control device for controlling the robot,
The robot is
A plurality of joints including a first joint and a second joint having a rotation direction different from that of the first joint; and a plurality of arm members coupled by the plurality of joints;
A first angular velocity sensor provided on one of the plurality of arm members;
A second angular velocity sensor provided on the arm member different from the arm member provided with the first angular velocity sensor;
With
The controller is configured to control the plurality of joints based on a uniaxial output of the first angular velocity sensor and a uniaxial or triaxial output of the second angular velocity sensor. Can do.

Or
A robot system comprising a robot and a control device for controlling the robot,
The robot is
A plurality of joints including a first joint and a second joint having a rotation direction different from the first joint;
A plurality of arm members connected by the plurality of joints;
An angular velocity sensor provided on one of the plurality of arm members;
With
The control device is configured such that a direction of a rotation axis coincides with the joint on the base side of the arm member provided with the angular velocity sensor, and the joint provided on the base side is changed to the angular velocity. It can be understood as a robot system characterized by control based on the output of the sensor.

Or
A robot system comprising a robot and a control device for controlling the robot,
The robot is
An arm including a first joint and a second joint having a rotation direction different from that of the first joint;
An inertial sensor provided on the arm;
An angle sensor for detecting an angle of the first joint;
With
The control device can be grasped as a robot system that controls the first joint by sampling outputs of the inertial sensor and the angle sensor at a cycle of 100 Hz or more.

Or
A robot system comprising a robot and a control device for controlling the robot,
The robot is
An arm including a first joint and a second joint having a rotation direction different from that of the first joint;
An inertial sensor provided on the arm;
A force sensor provided closer to the tip of the arm than the inertial sensor;
With
The control device may be understood as a robot system that controls the plurality of joints based on an output of the inertial sensor, an output of the force sensor, and an output of the force sensor.

  The above-described control device and robot system of the present invention can also suppress the vibration of the arm and improve the working efficiency of the robot.

It is explanatory drawing which shows the whole structure of the robot 1 of a present Example. 3 is a block diagram illustrating an internal configuration of a control unit 50. FIG. It is the block diagram which showed operation | movement of the control part 50 of a present Example. FIG. 5 is a block diagram showing an operation of a first motor control unit 51. FIG. 5 is a block diagram showing operations of a second motor control unit 52 and a third motor control unit 53. It is explanatory drawing which showed rotation angle (theta) 3 at the time of setting the gain coefficient Ka of the 2nd motor control part 52. FIG. 5 is a flowchart of processing for setting a gain coefficient Ka of the second motor control unit 52. It is explanatory drawing about the confirmation conditions of the vibration suppression effect using the robot 1 of a present Example. It is explanatory drawing which illustrated the actual measurement result of the vibration suppression effect using the robot 1 of a present Example. It is explanatory drawing which illustrated the confirmation result of the vibration suppression effect using the robot 1 of a present Example. It is explanatory drawing about the confirmation conditions of the vibration suppression effect using the robot 1 of a present Example. It is explanatory drawing which illustrated the confirmation result of the vibration suppression effect using the robot 1 of a present Example. It is explanatory drawing which showed the structure of the robot 2 of a 1st modification. It is explanatory drawing which showed the structure of the robot 3 of a 2nd modification. FIG. 11 is a block diagram showing operations of a second motor control unit 52 and a third motor control unit 53 in the robot 3 of another aspect of the second modified example.

  FIG. 1 is an explanatory diagram showing the overall structure of the robot 1 of this embodiment. FIG. 1A shows a rough outer shape of the robot 1 of the present embodiment. As shown in the figure, the robot 1 of this embodiment includes a base 10 installed on the ground, an arm 20 rotatably attached to the base 10, and a robot 1 mounted on the base 10. The control part 50 which controls the whole operation | movement of this is provided.

  The arm 20 includes six arm members 21 to 26 and five joints 42 to 46. The arm member 21 in this is rotatably attached to the base 10 by a joint 41. The arm member 22 is attached to the arm member 21 so as to be bent by a joint 42, and the arm member 23 is attached to the arm member 22 so as to be bent by a joint 43. Further, the arm member 24 is rotatably attached to the arm member 23 by a joint 44, the arm member 25 is attached to the arm member 24 to be bendable by a joint 45, and the arm member 26 is rotatable by a joint 46. It is attached to the arm member 25. Note that a robot hand (so-called hand portion) and various jigs (so-called end effectors) (not shown) such as a welding jig are attached to the tip of the arm member 26 via a force sensor 27. The force sensor 27 can detect the weight of the robot hand or the end effector, the weight of the gripped work, and the like. The robot 1 of the present embodiment is a robot (so-called vertical articulated robot) in which the direction of the rotation axis of a joint ahead of it changes when a certain joint is moved.

  Further, a first motor 41 m for driving the joint 41 is mounted on the joint 41. Similarly, a second motor 42m for driving the joint 42 is mounted on the joint 42, a third motor 43m for driving the joint 43 is mounted on the joint 43, and a second motor 42m is mounted on the joint 44. A fourth motor 44m, a fifth motor 45m is mounted on the joint 45, and a sixth motor 46m is mounted on the joint 46.

  The gyro sensor 30 is attached to the arm member 21 closest to the base 10 among the six arm members 21 to 26. A gyro sensor 31 is attached to the arm member 23. Here, the gyro sensors 30 and 31 are sensors that can output angular velocities (or inertial forces) with predetermined three orthogonal axes (X axis, Y axis, and Z axis) as rotation axes. The gyro sensor 30 is attached so that the rotation axis of the joint 41 coincides with the Z axis of the gyro sensor 30. The gyro sensor 31 is attached in a direction in which the rotation axis of the joint 43 coincides with any axis of the gyro sensor 31. In this embodiment, the angular velocity is detected as the inertial force. However, instead of the gyro sensors 30 and 31, an acceleration sensor may be used. In this embodiment, the gyro sensor 30 corresponds to the “first angular velocity sensor” in the present invention, and the gyro sensor 31 corresponds to the “second angular velocity sensor” in the present invention. The gyro sensors 30 and 31 also correspond to the “inertia sensor” in the present invention. Furthermore, the arm member 21 corresponds to the “first arm member” in the present invention.

  As the gyro sensor, an optical gyro sensor, a vibration type gyro sensor, and the like are known. However, the gyro sensors 30 and 31 are vibration types formed using a thin film of a piezoelectric material because they can be miniaturized. A gyro sensor is used. That is, the optical gyro sensor is difficult to reduce in size for the reason of utilizing the Sagnac effect, but the vibration gyro sensor can be reduced in size because the angular velocity is detected using the Coriolis force. Piezoelectric and electrostatic methods are also known for vibration-type gyrosensors, but piezoelectric-type vibration-type gyrosensors that detect angular velocity using a thin film of quartz or a piezoelectric material other than quartz are small. And can be manufactured at low cost. The gyro sensors 30 and 31 that detect angular velocity are superior to the acceleration sensor that detects acceleration in the following points. First, since the acceleration sensor needs to consider the influence of gravity, this is an error factor. On the other hand, since the gyro sensors 30 and 31 for detecting the angular velocity detect the angular velocity, it is not necessary to consider the influence of gravity. In addition, the size of the acceleration sensor increases when the acceleration detection sensitivity is increased. On the other hand, since the gyro sensors 30 and 31 for detecting the angular velocity detect the angular velocity, it is possible to increase the detection sensitivity even if it is small. For this reason, the freedom degree of installation of the gyro sensors 30 and 31 can be made high.

  FIG. 1B schematically shows the positional relationship between the arm members 21 to 26, the joints 41 to 46, and the gyro sensors 30 and 31 included in the robot 1 of the present embodiment. In the following, the angle of the joint 41 is represented by the angle θ1, the angle of the joint 42 is the angle θ2, the angle of the joint 43 is the angle θ3, the angle of the joint 44 is the angle θ4, the angle of the joint 45 is the angle θ5, and the angle of the joint 46 is The angle is represented by θ6.

  FIG. 2 is a block diagram showing an internal configuration of the control unit 50. As shown in the figure, the control unit 50 controls the first motor control unit 51 for controlling the first motor 41m, the second motor control unit 52 for controlling the second motor 42m, and the third motor 43m. A third motor control unit 53 for controlling, a fourth motor control unit 54 for controlling the fourth motor 44m, a fifth motor control unit 55 for controlling the fifth motor 45m, and a sixth motor 46m. The sixth motor control unit 56, an overall control unit 50a for controlling the entire operation of the robot 1, a memory 50m storing a program executed by the overall control unit 50a, and the like are mounted.

  An angle sensor 41s that detects the rotation angle θ1 of the first motor 41m is mounted on the first motor 41m. Similarly, an angle sensor 42s for detecting the rotation angle θ2 of the second motor 42m is mounted on the second motor 42m, and an angle sensor for detecting the rotation angle θ3 of the third motor 43m is mounted on the third motor 43m. 43s, the fourth motor 44m has an angle sensor 44s for detecting the rotation angle θ4 of the fourth motor 44m, and the fifth motor 45m has an angle sensor 45s for detecting the rotation angle θ5 of the fifth motor 45m. The sixth motor 46m is equipped with an angle sensor 46s for detecting the rotation angle θ6 of the sixth motor 46m. And each motor control part controls operation | movement of each motor based on the output from the angle sensor mounted in the motor made into a control object. That is, for example, the sixth motor control unit 56 controls the operation of the sixth motor 46m based on the rotation angle θ6 detected by the angle sensor 46s. Similarly, the third to fifth motor control units 53 to 55 control the operations of the third to fifth motors 43m to 45m based on the rotation angles θ3 to θ5 detected by the angle sensors 43s to 45s. However, for the second motor control unit 52, not only the rotation angle θ2 detected by the angle sensor 42s but also the output of the gyro sensor 31 and information from the third motor control unit 53 are used to operate the second motor 42m. To control. Details of the control performed by the second motor control unit 52 will be described later.

  The first motor control unit 51 controls the operation of the first motor 41m using not only the rotation angle θ1 detected by the angle sensor 41s but also the output of the gyro sensor 30. Details of the control performed by the first motor control unit 51 will also be described later. Further, as shown in FIG. 2, the output of the force sensor 27 is also supplied to the control unit 50. And the control part 50 samples the output of the angle sensors 41s-46s, the gyro sensors 30 and 31, and the force sensor 27 with a period of 100 Hz or more.

  FIG. 3A is a block diagram illustrating an operation in which the fourth motor control unit 54 controls the fourth motor 44m. FIG. 3B is a block diagram showing an operation in which the fifth motor control unit 55 controls the fifth motor 45m. FIG. 3C shows a sixth motor control unit 56 in which the sixth motor 46m is controlled. It is the block diagram which showed the operation | movement which controls. As shown in the figure, the fourth motor control unit 54, the fifth motor control unit 55, and the sixth motor control unit 56 are controlled only by the control object being different from the fourth motor 44m, the fifth motor 45m, and the sixth motor 46m. The contents are exactly the same. Therefore, the fourth motor control unit 54 shown in FIG. 3A will be described as an example.

  The fourth motor control unit 54 receives the target position Pc from the overall control unit 50a shown in FIG. 2, and performs control such that the rotation angle θ4 of the angle sensor 44s becomes the target position Pc. Therefore, basically, the control structure is such that a large loop that feedback-controls the rotation angle θ4 detected by the angle sensor 44s and a small loop that feedback-controls the angular velocity inside. That is, as shown in FIG. 3A, the target position Pc from the overall control unit 50a is input to the position control unit 54a. At this time, the rotation angle θ4 detected by the angle sensor 44s is converted into the position feedback value Pfb by the rotation angle calculation unit 54d, and then subtracted from the target position Pc and input to the position control unit 54a.

  In the position controller 54a, a target angular velocity ωc corresponding to the deviation between the target position Pc and the position feedback value Pfb is generated and input to the angular velocity controller 54b. At this time, the rotation angle θ4 detected by the angle sensor 44s is converted into the angular velocity feedback value ωfb by the angular velocity calculation unit 54c, and then subtracted from the target angular velocity ωc and input to the angular velocity control unit 54b. Then, the angular velocity control unit 54b controls the fourth motor 44m in accordance with the deviation between the target angular velocity ωc and the angular velocity feedback value ωfb. The result is reflected in the rotation angle θ4 detected by the angle sensor 44s, position feedback control is performed via the rotation angle calculation unit 54d, and angular velocity feedback control is performed via the angular velocity calculation unit 54c.

  FIG. 4 is a block diagram illustrating an operation in which the first motor control unit 51 controls the first motor 41m. The first motor control unit 51 also performs control using the rotation angle θ1 detected by the angle sensor 41s. This point is the same as the control performed by the fourth motor control unit 54, the fifth motor control unit 55, and the sixth motor control unit 56 described above. However, the first motor control unit 51 also controls the operation of the first motor 41m using the output of the gyro sensor 30. Correspondingly, the control performed by the first motor control unit 51 is the angular velocity performed using the output of the gyro sensor 30 for the position feedback control and the angular velocity feedback control performed using the rotation angle θ1 detected by the angle sensor 41s. It is a combination of feedback control. In FIG. 4, position feedback control and angular velocity feedback control performed using the rotation angle θ <b> 1 are represented by solid arrows, and angular velocity feedback control performed using the output of the gyro sensor 30 is represented by dashed arrows. Hereinafter, a description will be given with reference to FIG.

  Also in the first motor control unit 51, the target position Pc is input from the overall control unit 50a to the position control unit 51a. At this time, the rotation angle θ1 detected by the angle sensor 41s is converted into the position feedback value Pfb by the rotation angle calculation unit 51d, and then subtracted from the target position Pc and input to the position control unit 51a.

  The position control unit 51a generates a target angular velocity ωc corresponding to the deviation between the target position Pc and the position feedback value Pfb and inputs it to the angular velocity control unit 51b. At this time, the angular velocity feedback value ωfb generated based on the output from the angle sensor 41s and the output from the gyro sensor 30 is subtracted from the target angular velocity ωc and input to the angular velocity controller 51b. A method of generating the angular velocity feedback value ωfb based on the output from the angle sensor 41s and the output from the gyro sensor 30 will be described later.

  Then, the angular velocity control unit 51b controls the first motor 41m according to the deviation between the target angular velocity ωc and the angular velocity feedback value ωfb. As a result, the rotation angle θ1 detected by the angle sensor 41s changes. Further, as a result of the arm member 21 being rotated by the first motor 41m, the gyro sensor 30 outputs an angular velocity ωA1. Among these, the rotation angle θ1 detected by the angle sensor 41s is converted into a position feedback value Pfb by the rotation angle calculation unit 51d. Position feedback control is performed by feeding back the position feedback value Pfb thus obtained to the target position Pc.

  On the other hand, the angular velocity feedback value ωfb is generated as follows. First, the rotation angle θ1 detected by the angle sensor 41s is supplied to the angular velocity calculation unit 51c. Then, the angular velocity calculation unit 51c generates the angular velocity ωm1 of the first motor 41m and the angular velocity ωA1m of the arm member 21 from the rotation angle θ1. Here, the angular velocity ωA1m of the arm member 21 is a value obtained by dividing the angular velocity ωm1 of the first motor 41m by the reduction ratio of the joint 41 between the first motor 41m and the arm member 21. The angular velocity ωA1m of the arm member 21 should be essentially the same as the angular velocity ωA1 obtained from the gyro sensor 30. Therefore, these deviations represent vibration components of the arm member 21 around the rotation axis of the joint 41.

  Therefore, the vibration acceleration ωA1s corresponding to the vibration component is generated by subtracting the angular velocity ωA1m of the arm member 21 from the angular velocity ωA1 obtained from the gyro sensor 30. Thereafter, the vibration acceleration ωA1s of the arm member 21 is converted into the vibration acceleration ωm1s of the first motor 41m by multiplying the reduction ratio of the joint 41 by the conversion unit 51e. The correction value Ka · ωm1s is calculated by multiplying the vibration acceleration ωm1s by the gain coefficient Ka by the correction value calculation unit 51f. The angular velocity feedback value ωfb is calculated by adding the correction value Ka · ωm1s thus obtained and the angular velocity ωm1 obtained by the angular velocity calculator 51c. The first motor control unit 51 performs angular velocity feedback control using the angular velocity feedback value ωfb obtained from the outputs of the angle sensor 41 s and the gyro sensor 30 in this way. In order to suppress the vibration of the arm 20 during the operation of the robot 1, it is important to set the gain coefficient Ka appropriately. The robot 1 of this embodiment is also characterized by the setting of the gain coefficient Ka, which will be described in detail later.

  FIG. 5 is a block diagram illustrating an operation in which the second motor control unit 52 controls the second motor 42m and an operation in which the third motor control unit 53 controls the third motor 43m. The control of the second motor control unit 52 and the control of the third motor control unit 53 are collectively shown when the second motor control unit 52 controls the second motor 42m as shown in FIG. In addition, the information from the third motor control unit 53 is used. At the same time, the gyro sensor 31 that uses the output when the second motor control unit 52 controls the second motor 42m is mounted on the arm member 23 driven by the third motor 43m.

  However, if attention is paid only to the third motor control unit 53, the control performed by the third motor control unit 53 is performed by the fourth motor control unit 54, the fifth motor control unit 55, and the sixth motor control unit 56 described above. It is the same as the control. First, the control performed by the third motor control unit 53 will be briefly described.

  In the third motor control unit 53, the target position Pc received from the overall control unit 50a shown in FIG. 2 is input to the position control unit 53a. At this time, the rotation angle θ3 detected by the angle sensor 43s is converted into a position feedback value Pfb by the rotation angle calculation unit 53d, and then subtracted from the target position Pc and input to the position control unit 53a. In the position controller 53a, a target angular velocity ωc corresponding to the deviation between the target position Pc and the position feedback value Pfb is generated and input to the angular velocity controller 53b. At this time, the rotation angle θ3 detected by the angle sensor 43s is converted into the angular velocity feedback value ωfb by the angular velocity calculation unit 53c, and then subtracted from the target angular velocity ωc and input to the angular velocity control unit 53b. Then, the angular velocity control unit 53b controls the third motor 43m according to the deviation between the target angular velocity ωc and the angular velocity feedback value ωfb. The result is reflected in the rotation angle θ3 detected by the angle sensor 43s, position feedback control is performed via the rotation angle calculation unit 53d, and angular velocity feedback control is performed via the angular velocity calculation unit 53c.

  As a result of the third motor 43m being driven, the arm member 23 is rotated, so that the angular velocity ωA3 is output from the gyro sensor 31 mounted on the arm member 23. The second motor control unit 52 uses the output of the gyro sensor 31 for angular velocity feedback control. The basic method of using the output of the gyro sensor 31 for the angular velocity feedback control is the same as the method of using the output of the gyro sensor 30 by the first motor control unit 51. The second motor control unit 52 also uses the angular velocity ωA3m obtained by the angular velocity calculation unit 53c of the third motor control unit 53 for the angular velocity feedback control. In FIG. 5, a portion where the second motor control unit 52 performs the angular velocity feedback control using the output of the gyro sensor 31 and the angular velocity ωA3m obtained by the angular velocity calculation unit 53 c of the third motor control unit 53 is indicated by a dashed arrow. Represents.

  Also in the second motor control unit 52, the target position Pc is input from the overall control unit 50a to the position control unit 52a. At this time, the rotation angle θ2 detected by the angle sensor 42s is converted into the position feedback value Pfb by the rotation angle calculation unit 52d, and then subtracted from the target position Pc and input to the position control unit 52a. In the position controller 52a, a target angular velocity ωc corresponding to the deviation between the target position Pc and the position feedback value Pfb is generated and input to the angular velocity controller 52b. At this time, the output from the angle sensor 42s, the output from the gyro sensor 31, and the angular velocity feedback value ωfb generated by using the angular velocity ωA3m obtained by the angular velocity calculator 53c of the third motor control unit 53 are the target. Subtracted from the angular velocity ωc and input to the angular velocity controller 52b. A method of generating the angular velocity feedback value ωfb based on the output from the angle sensor 42s, the output from the gyro sensor 31, and the angular velocity ωA3m obtained by the angular velocity calculator 53c of the third motor controller 53 will be described later.

  Then, the angular velocity control unit 52b controls the second motor 42m in accordance with the deviation between the target angular velocity ωc and the angular velocity feedback value ωfb. As a result, the rotation angle θ2 detected by the angle sensor 42s is converted into a position feedback value Pfb by the rotation angle calculation unit 52d. Position feedback control is performed by feeding back the position feedback value Pfb thus obtained to the target position Pc.

  Further, the arm member 22 is rotated by driving the second motor 42m. As a result, the arm member 23 is rotated and the gyro sensor 31 outputs the angular velocity ωA3. The angular velocity ωA3 is used for the angular velocity feedback control of the second motor 42m via the angular velocity feedback value ωfb of the second motor control unit 52.

  The angular velocity feedback value ωfb of the second motor control unit 52 is generated as follows. First, the rotation angle θ2 detected by the angle sensor 42s is supplied to the angular velocity calculation unit 52c. Then, the angular velocity calculation unit 52c generates an angular velocity ωm2 of the second motor 42m and an angular velocity ωA2m of the arm member 22 from the rotation angle θ2. The angular velocity ωA2m of the arm member 22 can be calculated by dividing the angular velocity ωm2 of the second motor 42m by the reduction ratio of the joint 42 between the second motor 42m and the arm member 22.

  If the third motor 43m is fixed, it can be considered that the arm member 22 and the arm member 23 are integral with each other. Therefore, the angular velocity ωA2m of the arm member 22 is essentially the angular velocity ωA3 obtained from the gyro sensor 31. It should be consistent with Therefore, these deviations represent the vibration components of the arm member 22 when the third motor 43m is fixed. However, actually, since the third motor 43m is not fixed, the joint 43 also rotates. Accordingly, by subtracting the angular velocity ωA2m of the arm member 22 and the angular velocity ωA3m of the arm member 23 from the angular velocity ωA3 obtained from the gyro sensor 31, the vibration acceleration ωA2s corresponding to the vibration component at the joint 42 is calculated. Can do.

  Thereafter, the vibration acceleration ωA2s of the arm member 22 obtained in this way is converted into the vibration acceleration ωm2s of the second motor 42m by multiplying the reduction ratio of the joint 42 by the conversion unit 52e. The correction value Ka · ωm2s is calculated by multiplying the vibration acceleration ωm2s by the gain coefficient Ka by the correction value calculation unit 52f. The gain coefficient Ka may be a value different from the gain coefficient Ka used in the correction value calculation unit 51f of the first motor control unit 51. The angular velocity feedback value ωfb is calculated by adding the correction value Ka · ωm2s thus obtained and the angular velocity ωm2 obtained by the angular velocity calculator 52c. The second motor control unit 52 performs the angular velocity feedback control in this way. In the second motor control unit 52 as well, it is important to set the gain coefficient Ka appropriately in order to suppress the vibration of the arm 20.

  The gain coefficient Ka of the second motor control unit 52 is set based on the rotation angle θ3 of the arm member 23 with respect to the arm member 22. The gain coefficient Ka of the first motor control unit 51 is set based on the rotation angle θ2 of the arm member 22 with respect to the arm member 21. Since the gain coefficient Ka of the first motor control unit 51 is substantially the same as the gain coefficient Ka of the second motor control unit 52, the gain coefficient Ka of the second motor control unit 52 will be described first.

  FIG. 6 is an explanatory diagram showing the rotation angle θ3 when the gain coefficient Ka of the second motor control unit 52 is set. That is, considering a straight line connecting the joints 42 and 43 on both sides of the arm member 22 and a straight line connecting the joints 43 and 44 on both sides of the arm member 23, the angle formed by these straight lines is used as the rotation angle θ3. . The arm member 22 and the arm member 23 are extended as the rotation angle θ3 approaches 180 °, and the arm member 22 and the arm member 23 are folded as the rotation angle θ3 approaches 0 °. In this embodiment, the gain coefficient Ka of the second motor control unit 52 is set to a larger value as the rotation angle θ3 approaches 180 °, and the gain of the second motor control unit 52 increases as the rotation angle θ3 approaches 0 °. The coefficient Ka is set to a small value. This is due to the following reason.

  In the state where the arm member 22 and the arm member 23 are extended (the rotation angle θ3 is close to 180 °), the moment of inertia around the rotation axis of the joint 42 increases, so that a large vibration is generated with the operation. . As a result, in order to enhance the vibration suppression effect, it is necessary for the second motor 42m to generate a large torque, and it is desirable to increase the gain coefficient Ka of the second motor control unit 52. In addition, when the arm member 22 and the arm member 23 are extended, the control of the arm 20 is not likely to be unstable. For this reason, the vibration of the arm 20 can be quickly suppressed by setting the gain coefficient Ka of the second motor control unit 52 to a large value.

  Further, when the arm member 22 and the arm member 23 are folded (the rotation angle θ3 is 0 ° or close to 360 °), the moment of inertia around the rotation axis of the joint 42 is small, so that a large torque is generated. Therefore, the vibration can be suppressed without increasing the gain coefficient Ka of the second motor control unit 52. Further, when the arm member 22 and the arm member 23 are folded, the control of the arm 20 is likely to be unstable and vibration is likely to oscillate. Therefore, the gain coefficient Ka of the second motor control unit 52 is set small. It is preferable to prevent the arm 20 from oscillating and to stabilize the control.

  FIG. 7 is a flowchart of processing for setting the gain coefficient Ka of the second motor control unit 52. When setting the gain coefficient Ka of the second motor control unit 52, first, the rotation angle θ3 is detected based on the output of the angle sensor 43s (step S101). As described above with reference to FIG. 6, the rotation angle θ <b> 3 is an angle formed by the arm member 22 and the arm member 23.

  Subsequently, it is determined whether or not the detected rotation angle θ3 is less than the first threshold angle θth1 (step S102). Here, the first threshold angle θth1 is set to an appropriate angle (typically 90 °) selected from the range of 0 ° to 110 ° (more preferably, the range of 45 ° to 90 °). As a result, when the rotation angle θ3 is less than the first threshold angle θth1 (step S102: yes), the gain coefficient Ka of the second motor control unit 52 is set to Ka1 (step S103). Here, Ka1 is set to an appropriate value selected from the range of 0 to 0.3 (more preferably, the range of 0 to 0.2).

  On the other hand, if the rotation angle θ3 is not less than the first threshold angle θth1 (step S102: no), whether or not the rotation angle θ3 is equal to or greater than the first threshold angle θth1 and less than the second threshold angle θth2. Is determined (step S104). The second threshold angle θth2 is set to an appropriate angle (typically 135 °) selected from the range of 60 ° to 150 ° (more preferably, the range of 80 ° to 140 °). As a result, when the rotation angle θ3 is not less than the first threshold angle θth1 and less than the second threshold angle θth2 (step S104: yes), the gain coefficient Ka of the second motor control unit 52 is set to Ka2 (step S105). ). Ka2 is set to an appropriate value selected from the range of 0 to 0.5 (more preferably, the range of 0.1 to 0.4).

  If the rotation angle θ3 is not less than the first threshold angle θth1 and less than the second threshold angle θth2 (step S104: no), is the rotation angle θ3 greater than or equal to the second threshold angle θth2 and less than the third threshold angle θth3? It is determined whether or not (step S106). The third threshold angle θth3 is set to an appropriate angle (typically 225 °) selected from a range of 210 ° to 300 ° (more preferably a range of 220 ° to 280 °). As a result, when the rotation angle θ3 is equal to or larger than the second threshold angle θth2 and smaller than the third threshold angle θth3 (step S106: yes), the gain coefficient Ka of the second motor control unit 52 is set to Ka3 (step S107). ). Ka3 is set to an appropriate value selected from the range of 0.1 to 0.8 (more preferably, the range of 0.2 to 0.5).

  If the rotation angle θ3 is greater than or equal to the second threshold angle θth2 and not less than the third threshold angle θth3 (step S106: no), is the rotation angle θ3 greater than or equal to the third threshold angle θth3 and less than the fourth threshold angle θth4? It is determined whether or not (step S108). The fourth threshold angle θth4 is set to an appropriate angle (typically 270 °) selected from the range of 250 ° to 360 ° (more preferably, the range of 270 ° to 315 °). As a result, when the rotation angle θ3 is not less than the third threshold angle θth3 and less than the fourth threshold angle θth4 (step S108: yes), the gain coefficient Ka of the second motor control unit 52 is set to Ka4 (step S109). ). Ka4 is set to an appropriate value selected from the range of 0 to 0.5 (more preferably, the range of 0.1 to 0.4).

  On the other hand, when the rotation angle θ3 is not less than the third threshold angle θth3 and less than the fourth threshold angle θth4 (step S108: no), the gain coefficient Ka of the second motor control unit 52 is set to Ka5 ( Step S110). Ka5 is set to an appropriate value selected from the range of 0 to 0.3 (more preferably, the range of 0 to 0.2). Incidentally, Ka1, Ka2, Ka3, Ka4, Ka5 set to the gain coefficient Ka of the second motor control unit 52 are set to satisfy Ka1 <Ka2 <Ka3> Ka4> Ka5. Here, the same value can be used for Ka1 and Ka5, and different values can also be used. Similarly, the same value can be used for Ka2 and Ka4, or different values can be used.

  In the above-described embodiment, the rotation angle θ3 of the arm member 23 with respect to the arm member 22 is divided into five ranges, and the gain coefficient Ka of the second motor control unit 52 is appropriately set for each range. did. However, it is not necessarily limited to dividing into five ranges, and may be divided into two, three, four, or six or more ranges, for example. Further, instead of changing the gain coefficient Ka of the second motor control unit 52 stepwise, the gain coefficient Ka may be changed continuously according to the rotation angle θ3.

  The process for setting the gain coefficient Ka of the second motor control unit 52 according to the rotation angle θ3 of the arm member 23 with respect to the arm member 22 has been described above. The gain coefficient Ka of the first motor control unit 51 can be performed in substantially the same manner. That is, the rotation angle θ2 of the arm member 22 relative to the arm member 21 is detected (corresponding to step S101 in FIG. 7), and the rotation angle θ2 is compared with a preset threshold angle (step S102 in FIG. 7). , S104, S106, S108, S110), and the value of the gain coefficient Ka of the first motor control unit 51 may be set according to the magnitude relationship (in steps S103, S105, S107, S109, S111 in FIG. 7). Equivalent). Further, the number of threshold values to be compared at this time is not limited to four, and may be less than four or more than four.

  As described above, the gain coefficient Ka of the second motor control unit 52 is appropriately set according to the rotation angle θ3, and the gain coefficient Ka of the first motor control unit 51 is appropriately set according to the rotation angle θ2. Therefore, the vibration of the arm 20 can be quickly damped. Hereinafter, the gain coefficient Ka of the second motor control unit 52 and the gain coefficient Ka of the first motor control unit 51 may be collectively referred to as a gain coefficient Ka.

  Further, when the vibration acceleration ωA2s is in the opposite phase, the vibration can be suppressed more quickly and the control can be stabilized by setting the gain coefficient Ka to a negative value. In this case, Ka1, Ka2, Ka3, Ka4, and Ka5 of the gain coefficient Ka of the second motor control unit 52 are set to the following values.

  First, Ka1 is set to an appropriate value selected from the range of -0.2 to 0.3 (more preferably, the range of -0.1 to 0.1). Ka2 is an appropriate value selected from the range of 0.1 to 0.4 (more preferably, the range of 0.1 to 0.3). Ka3 is an appropriate value selected from the range of 0.3 to 0.7 (more preferably, the range of 0.3 to 0.6). Ka4 is an appropriate value selected from the range of 0.1 to 0.4 (more preferably, the range of 0.1 to 0.3). Furthermore, Ka5 can be an appropriate value selected from the range of -0.2 to 0.3 (more preferably, the range of -0.1 to 0.1). By selecting and setting Ka1, Ka2, Ka3, Ka4, and Ka5 from such a range, it is possible to suppress the vibration of the arm 20 and realize stabilization of control.

  In the robot 1 of the present embodiment described above, the vibration of the arm 20 can be easily and reliably suppressed. In addition, an enormous calculation such as solving the inverse Jacobian from the distal end side of the arm 20 becomes unnecessary. For this reason, the response speed in the control of the robot 1 can be increased, and the configuration of the control unit 50 can be simplified. In addition, since there is no computation that includes a singular point as in the case of solving the inverse Jacobian, not only the computation is simplified, but there is no condition that disables the control, so the robot 1 is reliably controlled. can do.

  In addition, the robot 1 of this embodiment also uses the output of the gyro sensor 31 attached to the arm member 23 in order to control the second motor 42m that drives the arm member 22. Since the arm member 23 exists on the tip side of the arm member 22, the output of the gyro sensor 31 includes the vibration of the arm member 22 in addition to the vibration of the arm member 23. Therefore, if the output of the gyro sensor 31 is used for the control of the second motor 42m, the vibrations of the arm member 23 and the arm member 22 can be controlled together, and the vibration suppressing effect can be enhanced. In addition, since the joint on the base side with larger vibration can be controlled, the vibration of the arm member 22 can be further effectively suppressed. Further, since the vibration can be suppressed by one gyro sensor 31 for the arm member 23 and the arm member 22 having the same direction of the rotation axis, the vibration of the arm can be suppressed by using an angular velocity sensor smaller than the number of joints. It becomes possible to suppress.

  The gyro sensor 30 is attached to the arm member 21, and the gyro sensor 31 is attached to the arm member 23 connected to the arm member 21 through the joint 42, the arm member 22, and the joint 43. The rotation axis of the joint 41 that rotates the arm member 21 and the rotation axes of the joint 42 and the joint 43 are orthogonal to each other. For this reason, the angular velocity of the arm member 21 (with the gyro sensor 30 attached) and the angular velocity of the arm member 23 (with the gyro sensor 31 attached) can be detected as simple rotation components that are not mixed with each other. As a result, the vibration of the arm 20 can be suppressed more easily, accurately and reliably. Furthermore, since the angular velocity of the arm member 21 and the angular velocity of the arm member 23 can be detected as a simple rotational component, the vibration of the arm 20 can be more reliably controlled by performing feedback control for each component. It becomes possible to suppress.

  In addition, by setting the gain coefficient Ka according to the rotation angle θ3 of the arm member 23 with respect to the arm member 22, the vibration of the arm 20 can be reliably suppressed while stabilizing the control.

  In addition, when the load applied to the end effector or the robot hand attached to the tip of the arm 20 is detected using the force sensor 27 as in the robot 1 of the present embodiment, the load detection sensitivity is increased. For this reason, the rigidity of the portion of the force sensor 27 is lowered, and the rigidity of the entire arm 20 is lowered. If the rigidity of the entire arm 20 is reduced, resonances of various orders are likely to occur, so that it is difficult to suppress the vibration of the arm 20. Therefore, conventionally, instead of keeping the load detection sensitivity low, it is possible to suppress the vibration of the arm 20 as quickly as possible by keeping the rigidity of the entire arm 20 high, or to suppress the vibration of the arm 20 quickly. Instead of giving up, it has been forced to choose between the detection of a load with high sensitivity, for example, to make it possible to grip a fragile workpiece. However, in the robot 1 of this embodiment, even if the rigidity of the entire arm 20 is reduced, the vibration of the arm 20 can be quickly suppressed by feedback control of the outputs of the gyro sensors 30 and 31. For this reason, it is possible to achieve both suppression of vibration of the arm 20 promptly and realization of delicate control such as gripping a fragile workpiece.

In order to confirm that the robot 1 of this embodiment described above can suppress vibrations by feedback control of the outputs of the gyro sensors 30 and 31, the following confirmation test was performed. First, as shown in FIG. 8A, the joints 42 to 46 are fixed so that the arm members 22 to 26 are extended horizontally. Then, the joint 41 is rotated, and the entire arm 20 is rotated by 90 ° and stopped as shown in FIG. A laser displacement meter was provided at a position where the tip of the arm 20 stopped, and the displacement (vibration) of the tip of the arm 20 was measured. Further, when rotating the arm 20, the joint 41 was driven at a maximum angular acceleration of 2200 ° / s ² and a maximum angular velocity of 275 ° / s. A 3 kg member to be measured was attached to the tip of the arm 20. The laser of the laser displacement meter is applied to the surface of the member to be measured to measure the displacement of the tip of the arm 20.

  FIG. 9 is an explanatory diagram showing actual measurement results. The vertical axis in the figure is the displacement (unit: mm) of the member to be measured at the tip of the arm 20 measured by a laser displacement meter, and the horizontal axis is the time when the arm 20 is rotated by 90 ° (the tip of the arm 20) This is the elapsed time (in seconds) from the moment when the measurement member first passes the target position. Incidentally, the control cycle of the robot 1 was 8 kHz, and the displacement of the tip of the arm 20 was measured at a sampling interval of 1 msec.

  A broken line shown in FIG. 9 represents an actual measurement result when the outputs of the gyro sensors 30 and 31 are not feedback-controlled (no gyro control). On the other hand, the solid line shown in FIG. 9 represents the actual measurement result when the outputs of the gyro sensors 30 and 31 are feedback-controlled (with gyro control). As is apparent from the figure, it can be confirmed that the vibration of the tip of the arm 20 can be significantly suppressed by feedback control of the outputs of the gyro sensors 30 and 31. The gain coefficient Ka during gyro control is set to an appropriate value.

  FIG. 10 is an explanatory diagram showing the measurement result of the settling time in each case. Here, the settling time is required until the displacement amplitude becomes ± 0.05 mm or less after the tip of the arm 20 first passes the stop position (in this case, the position where the joint 41 is rotated by 90 °). It's time. In the figure, for reference, a case where a member to be measured of 1 kg is attached to the tip of the arm 20 is also indicated by a broken line.

  As shown in the figure, when the member to be measured at the tip of the arm 20 is 3 kg, the settling time without gyro control is 1.8 seconds, whereas when the gyro control is performed, the settling time is zero. Reduced to 6 seconds. Further, when the measured member at the tip of the arm 20 is 1 kg, the settling time without gyro control is 1.3 seconds, but when the gyro control is performed, the settling time is reduced to 0.5 seconds. It was. Of course, even when the weight of the member to be measured attached to the tip of the arm 20 is other than 1 kg or 3 kg, the settling time can be greatly shortened by performing the gyro control. Thus, it becomes possible to obtain a large vibration suppression effect by feedback-controlling the outputs of the gyro sensors 30 and 31 as the condition that makes it difficult to suppress the vibration of the arm 20.

Moreover, the confirmation test of the vibration suppression effect by gyro control was performed also when the arm 20 was rotated in the direction different from the direction mentioned above. That is, as shown in FIG. 11, from the state where the arm members 22 to 26 extend horizontally, the joint 42 is rotated by 90 ° to be stopped. As a result, the arm members 22 to 26 on the distal end side relative to the joint 42 operate so as to be swung up. A laser displacement meter was provided at a position where the tip of the arm 20 stopped, and the displacement (vibration) of the tip of the arm 20 was measured. Further, when rotating the arm 20, the joint 42 was driven at a maximum angular acceleration of 2200 ° / s ² and a maximum angular velocity of 275 ° / s. A 3 kg member to be measured was attached to the tip of the arm 20. The laser of the laser displacement meter is applied to the surface of the member to be measured to measure the displacement of the tip of the arm 20.

  FIG. 12 is an explanatory diagram showing the measurement result of the settling time obtained by the confirmation test when the arm 20 is swung up. Here, the settling time when the arm 20 is swung up (the same applies when swinging down) is such that the tip of the arm 20 first passes the stop position (in this case, the position where the joint 42 is rotated by 90 °), It was defined as the time required for the displacement amplitude to be within ± 0.1 mm. Further, in the drawing, for reference, a case where a member to be measured of 5 kg is attached to the tip of the arm 20 is also indicated by a broken line.

  Under the condition that the member to be measured at the tip of the arm 20 is 3 kg, as shown by the solid line in the figure, the settling time without gyro control is 1.3 seconds, whereas the settling time is when gyro control is performed. Was reduced to 0.4 seconds. Also, under the condition that the member to be measured at the tip of the arm 20 is 5 kg, the settling time without gyro control is 2.0 seconds as shown by the broken line in the figure, whereas when the gyro control is performed, Settling time was reduced to 0.6 seconds. Of course, even when the weight of the member to be measured attached to the tip of the arm 20 is other than 3 kg or 5 kg, the settling time can be greatly shortened by performing the gyro control. As described above, even when the arm 20 is swung up (or swung down), the output of the gyro sensors 30 and 31 is feedback-controlled so that the vibration of the arm 20 is difficult to suppress. It can be confirmed that it is obtained.

  There are several variations of the robot 1 of the present embodiment described above. Hereinafter, these modified examples will be briefly described with a focus on differences from the present embodiment. In the description of the modification, the same components as those in the present embodiment are denoted by the same reference numerals, and a description thereof is omitted.

  FIG. 13 is an explanatory diagram showing the configuration of the robot 2 of the first modification. In the embodiment and the modification described above, the gyro sensor 30 of the arm member 21 and the gyro sensor 31 of the arm member 23 are mounted. However, the gyro sensor 32 may be mounted on the arm member 22 instead of the gyro sensor 31 mounted on the arm member 23. In this case, the control of the second motor 42m that drives the arm member 22 on which the gyro sensor 32 is mounted is the same as the control of the first motor 41m that drives the arm member 21 on which the gyro sensor 30 is mounted. Therefore, the control content of the second motor control unit 52 in the robot 2 of the first modification is the same as the control content of the first motor control unit 51 described above with reference to FIG.

  In addition, since the gyro sensor 31 is not mounted on the arm member 23 of the robot 2 of the first modification, the third motor 43m for driving the arm member 23 is controlled by the fourth motor 44m for driving the arm member 24 or the like. This is the same as the control of the fifth motor 45m for driving the arm member 25 and the sixth motor 46m for driving the arm member 26. Therefore, the control content of the third motor control unit 53 in the robot 2 of the first modification is the same as that of the fourth motor control unit 54, the fifth motor control unit 55, and the sixth motor control unit 56 described above with reference to FIG. The control contents are the same.

  FIG. 14 is an explanatory diagram showing the configuration of the robot 3 of the second modification. In the embodiment and the first modification described above, the gyro sensor 30 of the arm member 21 and the gyro sensor 31 of the arm member 23 are mounted. However, the gyro sensor 32 may also be mounted on the arm member 22. In this case, the control of the second motor 42m for driving the arm member 22 on which the gyro sensor 32 is mounted and the control of the third motor 43m for driving the arm member 23 on which the gyro sensor 31 is mounted are mounted on the gyro sensor 30. This is the same as the control of the first motor 41m that drives the arm member 21 that has been made. Therefore, the control content of the second motor control unit 52 and the control content of the third motor control unit 53 in the robot 3 of the second modification are the same as the control content of the first motor control unit 51 described above with reference to FIG. And it is sufficient.

  In particular, as shown in FIG. 14, in the robot 3 of the second modified example, the rotation shaft of the second motor 42m that drives the arm member 22 on which the gyro sensor 32 is mounted, and the arm member 23 on which the gyro sensor 31 is mounted. Is parallel to the rotation axis of the third motor 43m for driving the motor. For this reason, the control of the second motor 42m using the gyro sensor 32 and the control of the third motor 43m using the gyro sensor 31 are not performed independently, but the output of the gyro sensor 32 is used. May be corrected.

  FIG. 15 shows the second motor control unit 52 and the third motor control unit 53 when the output of the gyro sensor 31 is corrected using the output of the gyro sensor 32 in the robot 3 of the second modification example. It is explanatory drawing which illustrated the control content. As described above, the control content performed by the second motor control unit 52 and the third motor control unit 53 is the same as the control content of the first motor control unit 51 described above with reference to FIG. However, in the second modified example shown in FIG. 15, ωA3 detected by the gyro sensor 31 is not input to the third motor control unit 53 as it is, but after the angular velocity ωA2 detected by the gyro sensor 32 is subtracted. Input to the third motor control unit 53.

  This is due to the following reason. First, since the rotation axis of the second motor 42m and the rotation axis of the third motor 43m are parallel, the angular velocity ωA3 detected by the gyro sensor 31 is not only due to the third motor 43m driving the arm member 23. The second motor 42m is driven by the arm member 22. Therefore, by subtracting the angular velocity ωA2 detected by the gyro sensor 32 from the angular velocity ωA3 detected by the gyro sensor 31, it becomes possible to accurately detect the angular velocity due to the third motor 43m driving the arm member 23. Because. As a result, the vibration acceleration ωA3s of the arm member 23 can be detected with higher accuracy and fed back to the control of the third motor 43m, so that the vibration of the arm 20 can be quickly suppressed.

  While the present embodiment and various modifications have been described above, the present invention is not limited to the above-described embodiment and modification, and can be implemented in various modes without departing from the spirit of the present invention.

    DESCRIPTION OF SYMBOLS 1 ... Robot, 2 ... Robot, 3 ... Robot, 10 ... Base, 20 ... Arm, 21-26 ... Arm member, 27 ... Force sensor, 30-32 ... Gyro sensor, 41-46 ... Joint, 41m ... 1st Motor, 41s ... Angle sensor, 42m ... Second motor, 42s ... Angle sensor, 43m ... Third motor, 43s ... Angle sensor, 44m ... Fourth motor, 44s ... Angle sensor, 45m ... Fifth motor, 45s ... Angle sensor 46m ... 6th motor, 46s ... angle sensor, 50 ... control unit, 50a ... overall control unit, 50m ... memory, 51 ... first motor control unit, 51a ... position control unit, 51b ... angular velocity control unit, 51c ... angular velocity A calculation unit, 51d, a rotation angle calculation unit, DESCRIPTION OF SYMBOLS 1e ... Conversion part, 51f ... Correction value calculation part, 52 ... 2nd motor control part, 52a ... Position control part, 52b ... Angular velocity control part, 52c ... Angular velocity calculation part, 52d ... Rotation angle calculation part, 52e ... Conversion part, 52f ... Correction value calculation unit 53 ... Third motor control unit 53a ... Position control unit 53b ... Angular velocity control unit 53c ... Angular velocity calculation unit 53d ... Rotation angle calculation unit 54 ... Fourth motor control unit 54a ... Position control unit, 54b ... angular velocity control unit, 54c ... angular velocity calculation unit, 54d ... rotation angle calculation unit, 55 ... fifth motor control unit, 56 ... sixth motor control unit.

Claims (19)

A plurality of joints including a second joint having a rotation direction different from that of the first joint and the first joint; and a first arm member provided rotatably with respect to a base via the first joint Including a plurality of arm members;
A first angular velocity sensor provided on the first arm member or the first joint;
A robot characterized by comprising: The robot according to claim 1,
A robot comprising: a second angular velocity sensor provided on the arm member different from the first arm member among the plurality of arm members. The robot according to claim 2,
A robot comprising: a control unit that feedback-controls the joint on the base side relative to the arm member provided with the second angular velocity sensor based on an output of the second angular velocity sensor. The robot according to claim 3,
The control unit feedback-controls the joint in the same direction as the joint on the base side of the arm member provided with the second angular velocity sensor based on the output of the second angular velocity sensor. Robot characterized by. A plurality of joints including a first joint and a second joint having a rotation direction different from the first joint;
A plurality of arm members connected by the plurality of joints;
A first angular velocity sensor provided on one of the plurality of arm members;
A second angular velocity sensor provided on the arm member different from the arm member provided with the first angular velocity sensor;
A robot characterized by comprising: The robot according to claim 5,
A robot comprising: a control unit that controls the plurality of joints based on a uniaxial output of the first angular velocity sensor and a uniaxial output of the second angular velocity sensor. The robot according to claim 5,
A robot comprising: a control unit that controls the plurality of joints based on a uniaxial output of the first angular velocity sensor and a triaxial output of the second angular velocity sensor. The robot according to claim 6 or 7, wherein
The control unit feedback-controls the joint on the base side of the arm member provided with the first angular velocity sensor based on the output of the first angular velocity sensor, and the second angular velocity sensor A robot that performs feedback control of the joint on the base side with respect to the arm member provided with the second angular velocity sensor based on an output. A robot according to any one of claims 6 to 8,
The rotation axis of the joint that is feedback-controlled based on the output of the first angular velocity sensor and the rotation axis of the joint that is feedback-controlled based on the output of the second angular velocity sensor are orthogonal to each other. Robot to do. A robot according to any one of claims 6 to 9, wherein
The robot according to claim 1, wherein the first angular velocity sensor is provided on the arm member closest to the base among the arm members having different rotation axes of the joints on both sides. A robot according to any one of claims 1 to 10,
The robot according to claim 1, wherein the first angular velocity sensor is a vibration type gyro sensor. The robot according to claim 11,
The robot according to claim 1, wherein the first angular velocity sensor includes a piezoelectric material. A plurality of joints including a first joint and a second joint having a rotation direction different from the first joint;
A plurality of arm members connected by the plurality of joints;
An angular velocity sensor provided on one of the plurality of arm members;
The direction of the rotation axis coincides with the joint on the base side of the arm member provided with the angular velocity sensor, and the joint provided on the base side is based on the output of the angular velocity sensor. A control unit for controlling
A robot characterized by comprising: An arm including a first joint and a second joint having a rotation direction different from that of the first joint;
An inertial sensor provided on the arm;
A controller that controls the first joint by sampling the output of the inertial sensor at a cycle of 100 Hz or more;
A robot characterized by comprising: An arm including a first joint and a second joint having a rotation direction different from that of the first joint;
An inertial sensor provided on the arm;
A force sensor provided closer to the tip of the arm than the inertial sensor;
A robot characterized by comprising: An arm including a first joint provided on a base and a second joint having a rotation direction different from that of the first joint;
An inertial sensor provided on the arm,
When the angular acceleration of the first joint is rotated 90 ° at 2200 ° / s ² , the time until the vibration at the tip of the arm changes from −0.05 mm to +0.05 mm is 1 second or less. Characteristic robot. An arm including a first joint provided on a base and a second joint having a rotation direction different from that of the first joint;
An inertial sensor provided on the arm,
When the first joint is rotated by 90 ° with a maximum angular acceleration, the time required for the vibration at the tip of the arm to change from −0.05 mm to +0.05 mm is 1 second or less. An arm including a first joint provided on a base and a second joint having a rotation direction different from that of the first joint;
An inertial sensor provided on the arm,
When the angular acceleration of the second joint is rotated 90 ° at 2200 ° / s2, the time until the vibration at the tip of the arm changes from −0.1 mm to +0.1 mm is 0.5 seconds or less. Robot characterized by. An arm including a first joint provided on a base and a second joint having a rotation direction different from that of the first joint;
An inertial sensor provided on the arm,
When the first joint is rotated by 90 ° with a maximum angular acceleration, the time required for the vibration at the tip of the arm to change from −0.05 mm to +0.05 mm is 1 second or less.

Images