JP2014188643A - Robot - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a robot capable of suppressing vibration while preventing oscillation of the vibration in a multi-axis robot including a plurality of arms.SOLUTION: A robot includes: a support part (base); a plurality of arms connected as one series from the support part; a driving source for rotating the arms respectively; an inertia sensor for detecting an angular speed of a rotation axis of the arm; a posture detection unit for detecting a posture of the arm; and a driving source control unit which controls a first driving source by performing feedback of a correction component derived from an angular speed detected by the inertia sensor and from an angular speed acquired from an angle sensor installed at the driving source, and which derives a feedback gain of the correction component based on the detection result of the posture detection unit. The driving source control unit adjusts the feedback gain according to the posture of the arm.

Description

  The present invention relates to a robot.

  A multi-axis robot that has a base and a plurality of pivotable arms and works freely in a three-dimensional space, and the problem is that the arm tends to vibrate due to arm rotation or disturbance applied to the arm. On the other hand, for the purpose of damping this vibration, there is known a robot having an acceleration sensor for detecting acceleration in the X-axis, Y-axis, and Z-axis directions on the most distal arm and having a vibration suppression control function. Yes. The robot includes, for example, a base, a first arm connected to the base and rotating about the first rotation axis, and a second rotation axis connected to the first arm and orthogonal to the first rotation axis. A second arm that pivots about the axis, a third arm that is coupled to the second arm and pivots about a third rotation axis that is orthogonal to the second rotation axis, and a third arm that is coupled to the third arm, A fourth arm that rotates about a fourth rotation axis that is orthogonal to the three rotation axes and has a longitudinal shape.

  In addition, it is a multi-axis robot that has a base and a plurality of arms, and the rotation axes of these arms are parallel to each other. There is known a robot that is controlled by calculation including a component detected by the inertial sensor and thereby suppresses vibration (see, for example, Patent Document 1).

Japanese Patent Laid-Open No. 10-100085

However, in the conventional robot, the first rotation axis of the first arm that is the most base side arm is parallel to the longitudinal axis that extends in the longitudinal direction of the fourth arm that is the fourth arm from the base side. In this case, the torsional vibration around the longitudinal axis of the fourth arm is generated due to the arm on the distal end side and the load mass, and since this torsional vibration is in the same direction as the first rotation axis, control of these axes As a correction value, a problem has been found that this torsional vibration is included and fed back. Similarly, when the second rotation axis of the second arm, which is the second arm from the base side, and the longitudinal axis extending in the longitudinal direction of the fourth arm are parallel, the torsion around the longitudinal axis of the fourth arm Since vibrations are generated and the torsional vibrations are in the same direction as the first rotating shaft, a problem has been found that the torsional vibrations are included and fed back as a correction value for the control of these axes.
An object of the present invention is to provide a robot capable of feedback control even in a torsional vibration in a fourth arm, which is the fourth arm from the base side, in a multi-axis robot having a plurality of arms.

The following application example according to the present invention can provide a robot that performs feedback control in vibration suppression control including torsional vibration in the fourth arm, which is the fourth arm from the base side, in a multi-axis robot having a plurality of arms. .
(Application example 1)
The robot according to the present invention includes a base,
A first arm connected to the base and rotating about a first rotation axis;
It is connected to the first arm and rotates about the second rotation axis, and the second rotation axis is rotated in parallel with the rotation axis orthogonal to the first rotation axis or the axis orthogonal to the first rotation axis. A second arm as a shaft;
It is connected to the second arm and rotates around the third rotation axis, and the third rotation axis is rotated in parallel with the rotation axis orthogonal to the second rotation axis or the axis orthogonal to the second rotation axis. A third arm as a shaft;
It is connected to the third arm and rotates around the fourth rotation axis, and the fourth rotation axis is rotated in parallel with the rotation axis orthogonal to the third rotation axis or the axis orthogonal to the third rotation axis. A fourth arm having an axial shape and a longitudinal shape;
A first drive source for rotating the first arm;
A first angle sensor for detecting a rotation angle of the first drive source;
A first inertial sensor that detects an angular velocity or acceleration of the first rotation axis of the first arm;
An attitude detection unit for detecting the attitude of the fourth arm;
First angular velocity of the first rotating shaft of the first arm derived from the rotational angle of the first driving source and angular velocity of the first rotating shaft of the first arm detected by the first inertial sensor. A first drive source control unit that feeds back one correction component to control the first drive source, and sets a feedback gain of the first correction component based on a detection result of the posture detection unit;
It is characterized by providing.

Thereby, the vibration in the arm of the robot can be suppressed.
The first rotating shaft of the first arm and the longitudinal axis extending in the longitudinal direction of the fourth arm are parallel, and the torsional vibration around the longitudinal axis of the fourth arm generated due to the arm on the distal end side and load mass Even if is in the same direction as the first rotation axis, changing the feedback gain setting prevents the vibration suppression control that feeds back the first correction component and controls the first drive source from becoming unstable. Stable vibration suppression is possible. As a result, even when the arm rigidity is insufficient or the load mass is large, the ability to suppress the vibration of the primary resonance frequency that greatly affects the performance of the robot can be maintained high. As a result, it is possible to provide a high-performance robot with high vibration suppression capability.

(Application example 2)
In the robot according to the present invention, the first drive source control unit may obtain a feedback gain of the first correction component when the longitudinal axis extending in the longitudinal direction of the fourth arm and the first rotation axis are parallel to each other. It is preferable to set lower than the feedback gain of the first correction component when the longitudinal axis and the first rotation axis are orthogonal to each other when viewed in plan from the same direction as the third rotation axis.
Even if the torsional vibration around the longitudinal axis of the fourth arm is in the same direction as the first rotation axis, it is possible to reduce the fact that the torsional vibration hinders vibration control by setting the feedback gain low.

(Application example 3)
In the robot according to the present invention, the feedback gain of the first correction component when the longitudinal axis extending in the longitudinal direction of the fourth arm and the first rotational axis are parallel is from the same direction as the third rotational axis. It is preferable to set the feedback gain of the first correction component to 0% or more and 70% or less when the longitudinal axis and the first rotation axis are orthogonal to each other in plan view.
Even if the torsional vibration around the longitudinal axis of the fourth arm is in the same direction as the first rotation axis, the degree to which the torsional vibration hinders vibration control can be reduced by setting the feedback gain low.

(Application example 4)
In the robot according to the present invention, a second drive source for rotating the second arm;
A second angle sensor for detecting a rotation angle of the second drive source;
A second inertial sensor for detecting an angular velocity or acceleration of the second rotation axis of the second arm;
A second guide derived from an angular velocity of the second rotary shaft of the second arm derived from a rotational angle of the second drive source and an angular velocity of the second rotary shaft of the second arm detected by the second inertia sensor. A second drive source control unit that feeds back two correction components to control the second drive source, and sets a feedback gain of the second correction component based on a detection result of the posture detection unit;
It is preferable to provide.
Even if the torsional vibration around the longitudinal axis of the fourth arm is in the same direction as the second rotation axis, the degree to which the torsional vibration hinders vibration control can be reduced by setting the feedback gain low. As a result, the vibration suppression control that feeds back the second correction component to control the second drive source is prevented from becoming unstable, and stable vibration suppression is possible, thereby ensuring more reliable vibration of the robot. Can be suppressed.

(Application example 5)
In the robot according to the present invention, the second drive source control unit may obtain a feedback gain of the second correction component when the longitudinal axis extending in the longitudinal direction of the fourth arm and the second rotation axis are parallel to each other. It is preferable to set lower than the feedback gain of the second correction component when the longitudinal axis and the second rotation axis are orthogonal to each other when viewed in plan from the same direction as the third rotation axis.
Even if the torsional vibration around the longitudinal axis of the fourth arm is in the same direction as the second rotation axis, the degree to which the torsional vibration hinders vibration control can be reduced by setting the feedback gain low.

(Application example 6)
In the robot according to the present invention, the feedback gain of the second correction component when the longitudinal axis extending in the longitudinal direction of the fourth arm and the second rotational axis are parallel is from the same direction as the third rotational axis. It is preferable to set the feedback gain of the second correction component to 0% or more and 70% or less when the longitudinal axis and the second rotation axis are orthogonal to each other in plan view.
Even if the torsional vibration around the longitudinal axis of the fourth arm is in the same direction as the second rotation axis, the degree to which the torsional vibration hinders vibration control can be reduced by setting the feedback gain low. Thereby, the vibration of the robot can be more reliably suppressed.

(Application example 7)
In the robot according to the present invention, it is preferable that the second inertial sensor is installed on the fourth arm.
Thereby, the vibration of the robot can be suppressed while reducing the number of sensors.
(Application example 8)
In the robot according to the present invention, it is preferable that the second inertial sensor is installed on the third arm.
Thereby, the vibration of the robot can be suppressed while reducing the number of sensors.

(Application example 9)
In the robot according to the present invention, it is preferable that the first inertial sensor is installed on the fourth arm.
Thereby, the vibration of the robot can be suppressed while reducing the number of sensors.
(Application Example 10)
In the robot according to the present invention, it is preferable that the first inertial sensor is installed on the third arm.
Thereby, the vibration of the robot can be suppressed while reducing the number of sensors.
(Application Example 11)
In the robot according to the present invention, the first inertial sensor is installed on the first arm,
The second inertial sensor is preferably installed on the second arm.
Thereby, the vibration of the robot can be more reliably suppressed.

(Application Example 12)
The robot according to the present invention includes a base,
A first arm connected to the base and rotating about a first rotation axis;
It is connected to the first arm and rotates about the second rotation axis, and the second rotation axis is rotated in parallel with the rotation axis orthogonal to the first rotation axis or the axis orthogonal to the first rotation axis. A second arm as a shaft;
It is connected to the second arm and rotates around the third rotation axis, and the third rotation axis is rotated in parallel with the rotation axis orthogonal to the second rotation axis or the axis orthogonal to the second rotation axis. A third arm as a shaft;
It is connected to the third arm and rotates around the fourth rotation axis, and the fourth rotation axis is rotated in parallel with the rotation axis orthogonal to the third rotation axis or the axis orthogonal to the third rotation axis. A fourth arm having an axial shape and a longitudinal shape;
A second drive source for rotating the second arm;
A second angle sensor for detecting a rotation angle of the second drive source;
A second inertial sensor for detecting an angular velocity or acceleration of the second rotation axis of the second arm;
An attitude detection unit for detecting the attitude of the fourth arm;
Correction derived from the angular velocity of the second rotating shaft of the second arm derived from the rotational angle of the second drive source and the angular velocity of the second rotating shaft of the second arm detected by the second inertia sensor. A second drive source control unit configured to feed back a component to control the second drive source and set a feedback gain of the correction component based on a detection result of the posture detection unit.

Thereby, the vibration of the robot can be suppressed.
The second rotation axis of the second arm and the longitudinal axis extending in the longitudinal direction of the fourth arm are parallel to each other, and the torsional vibration around the longitudinal axis of the fourth arm generated due to the arm on the distal end side or load mass is generated. Even in the same direction as the second rotation axis, feedback control can be performed even if this torsional vibration is included in the correction value for the control of these axes. In other words, the vibration suppression control that feeds back the correction component to control the second drive source is prevented from becoming unstable, and stable vibration suppression is possible. As a result, even when the arm rigidity is insufficient or the load mass is large, the ability to suppress the vibration of the primary resonance frequency that greatly affects the performance of the robot can be maintained high. As a result, it is possible to provide a high-performance robot with high vibration suppression capability.

(Application Example 13)
In the drive source control unit of the robot according to the present invention, the feedback gain of the correction component when the longitudinal axis extending in the longitudinal direction of the fourth arm and the second rotational axis are parallel to the third rotational axis. It is preferable to set it lower than the feedback gain of the correction component when the longitudinal axis and the second rotation axis are orthogonal when viewed in plan from the same direction.
Even if the torsional vibration around the longitudinal axis of the fourth arm is in the same direction as the second rotation axis, the degree to which the torsional vibration hinders vibration control can be reduced by setting the feedback gain low.

(Application Example 14)
In the robot according to the present invention, the feedback gain of the correction component when the longitudinal axis and the second rotation axis are parallel is a plan view from the same direction as the third rotation axis. It is preferably set to 0% or more and 70% or less of the feedback gain of the correction component when the two rotation axes are orthogonal.
Even if the torsional vibration around the longitudinal axis of the fourth arm is in the same direction as the second rotation axis, the degree to which the torsional vibration hinders vibration control can be reduced by setting the feedback gain low.

(Application Example 15)
In the robot according to the present invention, it is preferable that the second inertial sensor is installed on the fourth arm.
Thereby, the vibration of the robot can be suppressed while reducing the number of sensors.
(Application Example 16)
In the robot according to the aspect of the invention, it is preferable that the second inertial sensor is installed on the third arm.
Thereby, the vibration of the robot can be suppressed while reducing the number of sensors.

(Application Example 17)
In the robot according to the present invention, the robot is connected to the fourth arm and rotates about the fifth rotation axis, and the fifth rotation axis is a rotation axis orthogonal to the fourth rotation axis or the fourth rotation axis. A fifth arm having a rotation axis parallel to the axis orthogonal to
It is connected to the fifth arm and rotates about the sixth rotation axis, and the sixth rotation axis is rotated in parallel with the rotation axis orthogonal to the fifth rotation axis or the axis orthogonal to the fifth rotation axis. A sixth arm as a shaft;
It is connected to the sixth arm and rotates about the seventh rotation axis, and the seventh rotation axis is rotated in parallel with the rotation axis orthogonal to the sixth rotation axis or the axis orthogonal to the sixth rotation axis. A seventh arm as a shaft;
It is preferable to have.
Accordingly, the robot can suppress vibrations and execute more various operations.

It is a front view showing typically a 1st embodiment of a robot of the present invention. It is the schematic of the arm coupling body of the robot shown in FIG. It is the schematic of the arm coupling body of the robot shown in FIG. It is a side view of the arm coupling body of the robot shown in FIG. It is a top view of the arm coupling body of the robot shown in FIG. It is a block diagram of the principal part of the robot shown in FIG. It is a block diagram of the principal part of the robot shown in FIG. It is a block diagram of the principal part of the robot shown in FIG. It is a block diagram of the principal part of the robot shown in FIG. It is a block diagram of the principal part of the robot shown in FIG. It is a block diagram of the principal part of the robot shown in FIG. It is a block diagram of the principal part of the robot shown in FIG. It is a block diagram of the principal part of the robot shown in FIG. It is a flowchart which shows the control action of the robot shown in FIG. It is the schematic which shows the arm coupling body in 2nd Embodiment of the robot of this invention.

Hereinafter, the robot of the present invention will be described in detail based on preferred embodiments shown in the accompanying drawings.
Note that the robot of the present invention can be applied to a robot having one or more arm coupling bodies formed by pivotally coupling a plurality of arms. However, in the following embodiments, the present invention is representatively described. A case where a dual-arm robot having two arm connecting bodies is applied will be described.
<First Embodiment>
FIG. 1 is a front view schematically showing a first embodiment of the robot of the present invention. FIG. 2 is a schematic view of an arm coupling body of the robot shown in FIG. FIG. 3 is a schematic view of the arm coupling body of the robot shown in FIG. FIG. 4 is a side view of the arm connection body of the robot shown in FIG. FIG. 5 is a plan view of the arm coupling body of the robot shown in FIG. 6 to 13 are block diagrams of main parts of the robot shown in FIG. FIG. 14 is a flowchart showing a control operation of the robot shown in FIG.

  In the following, for convenience of explanation, the upper side in FIGS. 1 to 5 is referred to as “upper” or “upper”, and the lower side is referred to as “lower” or “lower”. Moreover, the support part side in FIGS. 1-5 is called a "base end", and the opposite side is called a "tip". Moreover, in FIG. 1, each arm coupling body 8 is each schematically shown, and the number, shape, posture, etc. of the arms are different from the actual ones shown in FIGS. . Further, in FIG. 4, the second rotation axis O2 is exaggerated, and in FIG. 5, the third rotation axis O3 is exaggerated. In FIGS. 2, 4, and 5, the inertial sensor 31 is illustrated outside the fourth arm 15 in order to clarify its presence. 4 and 5, the first arm 12 to the fourth arm 15 of the arm coupling body 8 are illustrated, and the fifth arm 16 and the wrist 17 are not illustrated.

  A robot (industrial robot) 1 shown in FIG. 1 to FIG. 5 is a double-arm robot having two arm connecting members each having seven arms rotatably connected. The robot 1 can be used in a manufacturing process for manufacturing a precision device such as a wristwatch, for example, and includes a robot body 10 and a control unit 20 (see FIG. 6) that controls the operation of the robot body 10. Yes. The robot body 10 and the control unit 20 are electrically connected. The control unit 20 can be configured by, for example, a personal computer (PC) with a built-in CPU (Central Processing Unit). The control unit 20 will be described in detail later.

The robot body 10 of the robot 1 includes a trunk portion 7, a pair of arm coupling bodies 8 coupled to the trunk portion 7, and a base portion 100 that supports the trunk portion 7. In this case, the support part (base) 11 of each arm connection body 8 is being fixed to the trunk | drum 7, respectively.
In addition, since the structure of the two arm connection bodies 8 is the same, below, typically, one arm connection body 8 is demonstrated and description about the other arm connection body 8 is abbreviate | omitted.

  The arm connection body 8 includes a support portion (base) 11, five arms (links) 12, 13, 14, 15, 16, a wrist (link) 17, and seven drive sources 401, 402, 403, 404, 405, 406, and 407. This arm connection body 8 is a multi-joint (7-axis) robot arm in which a support portion 11, arms 12, 13, 14, 15, 16 and a wrist 17 are connected in this order from the proximal end side to the distal end side. It is. The support portion 11, the arms 12 to 16, and the list 17 can be collectively referred to as “arm”. The arm 12 is “first arm”, the arm 13 is “second arm”, and the arm 14 is It can be said that “third arm”, arm 15 is “fourth arm”, arm 16 is “fifth arm”, and list 17 is “sixth arm, seventh arm”. An end effector or the like can be attached to the wrist 17.

  As shown in FIGS. 2 to 5, the arms 12 to 16 and the wrist 17 are supported so as to be independently displaceable with respect to the support portion 11. The lengths of the arms 12 to 16 and the wrist 17 are not particularly limited. In the illustrated configuration, the lengths of the arms 14 and 15 are set longer than those of the other arms 12, 13, 16 and the wrist 17. ing. That is, the arms 14 and 15 each have a longitudinal shape.

  The support portion 11 and the first arm 12 are connected via a joint (joint) 171. The joint 171 has a mechanism that supports one of the support portion 11 and the first arm 12 connected to each other so as to be rotatable with respect to the other. In this case, the first arm 12 is rotatable about the first rotation axis O1 with respect to the support portion 11 with the first rotation axis O1 parallel to the horizontal direction as the rotation center (axis center). The first rotation axis O1 is orthogonal to the normal line of the upper surface of the floor 101 (see FIG. 1), which is the installation surface of the robot 1. The rotation about the first rotation axis O <b> 1 is performed by driving the first drive source 401. The first drive source 401 is driven by a motor 401M and a cable (not shown), and the motor 401M is controlled by the control unit 20 via an electrically connected motor driver 301 (see FIG. 6). The first drive source 401 may be configured to transmit drive from the motor 401M by a speed reducer (not shown) provided together with the motor 401M, or the speed reducer may be omitted. Note that, for example, a motor 401M and motor drivers 301 to 307 are accommodated in the body 7 of the robot body 10.

  The first arm 12 and the second arm 13 are connected via a joint (joint) 172. The joint 172 has a mechanism that supports one of the first arm 12 and the second arm 13 connected to each other so as to be rotatable with respect to the other. In this case, the second arm 13 is rotatable with respect to the first arm 12 about the second rotation axis O2 with the second rotation axis O2 as the rotation center. The second rotation axis O2 is orthogonal to the first rotation axis O1. The rotation around the second rotation axis O <b> 2 is performed by driving the second drive source 402. The second drive source 402 is driven by a motor 402M and a cable (not shown), and the motor 402M is controlled by the control unit 20 through an electrically connected motor driver 302 (see FIG. 6). . The second drive source 402 may be configured to transmit drive from the motor 402M by a speed reducer (not shown) provided in addition to the motor 402M, and the speed reducer may be omitted. The second rotation axis O2 may be parallel to an axis orthogonal to the first rotation axis O1. The first arm 12 houses a motor 402M, for example.

  The second arm 13 and the third arm 14 are connected via a joint 173. The joint 173 has a mechanism that supports one of the second arm 13 and the third arm 14 connected to each other so as to be rotatable with respect to the other. In this case, the third arm 14 is rotatable about the third rotation axis O3 with respect to the second arm 13 about the third rotation axis O3. The third rotation axis O3 is orthogonal to the second rotation axis O2. In the present embodiment, the longitudinal axis (center axis) extending in the longitudinal direction of the third arm 14 coincides with the third rotation axis O3. The rotation about the third rotation axis O <b> 3 is performed by driving the third drive source 403. The third drive source 403 is driven by a motor 403M and a cable (not shown), and the motor 403M is controlled by the control unit 20 through an electrically connected motor driver 303 (see FIG. 6). . The third drive source 403 may be configured to transmit drive from the motor 403M by a speed reducer (not shown) provided in addition to the motor 403M, and the speed reducer may be omitted. Note that the third rotation axis O3 may be parallel to an axis orthogonal to the second rotation axis O2. Note that the second arm 13 houses, for example, a motor 403M.

  The third arm 14 and the fourth arm 15 are connected via a joint (joint) 174. The joint 174 has a mechanism for supporting one of the third arm 14 and the fourth arm 15 connected to each other so as to be rotatable with respect to the other. In this case, the fourth arm 15 is rotatable with respect to the third arm 14 about the fourth rotation axis O4 with the fourth rotation axis O4 as the rotation center. The fourth rotation axis O4 is orthogonal to the third rotation axis O3. In the present embodiment, the longitudinal axis (center axis) 151 (see FIGS. 2B and 2C) extending in the longitudinal direction of the fourth arm 15 and the fourth rotation axis O4 are orthogonal to each other. Yes. The rotation about the fourth rotation axis O4 is performed by driving the fourth drive source 404. The fourth drive source 404 is driven by a motor 404M and a cable (not shown), and the motor 404M is controlled by the control unit 20 via an electrically connected motor driver 304 (see FIG. 6). . The fourth drive source 404 may be configured to transmit drive from the motor 404M by a speed reducer (not shown) provided together with the motor 404M, or the speed reducer may be omitted. The fourth rotation axis O4 may be parallel to an axis orthogonal to the third rotation axis O3. The third arm 14 houses a motor 404M, for example.

  The fourth arm 15 and the fifth arm 16 are connected via a joint (joint) 175. The joint 175 has a mechanism that supports one of the fourth arm 15 and the fifth arm 16 connected to each other so as to be rotatable with respect to the other. In this case, the fifth arm 16 is rotatable with respect to the fourth arm 15 about the fifth rotation axis O5 with the fifth rotation axis O5 as the rotation center. The fifth rotation axis O5 is orthogonal to the fourth rotation axis O4. The rotation about the fifth rotation axis O5 is performed by driving the fifth drive source 405. The fifth drive source 405 is driven by a motor 405M and a cable (not shown), and the motor 405M is controlled by the control unit 20 through an electrically connected motor driver 305 (see FIG. 6). . The fifth drive source 405 may be configured to transmit the drive from the motor 405M by a speed reducer (not shown) provided together with the motor 405M, or the speed reducer may be omitted. The fifth rotation axis O5 may be parallel to an axis orthogonal to the fourth rotation axis O4. The fourth arm 15 houses a motor 405M, for example.

  The list 17 has a sixth arm 18 and a seventh arm 19. The wrist 17 is detachably mounted with a manipulator 9 that holds a precision device such as a wristwatch, for example, as an end effector. In addition, it does not specifically limit as the manipulator 9, For example, the thing of the structure which has a several finger part (finger) is mentioned. And this robot 1 can convey the said precision apparatus by controlling operation | movement of arms 12-16, wrist | wrist 17, etc., holding the precision apparatus with the manipulator 9. FIG.

  The fifth arm 16 and the sixth arm of the wrist 17 are connected via a joint (joint) 176. The joint 176 has a mechanism that supports one of the fifth arm 16 and the sixth arm 18 of the wrist 17 that are connected to each other so as to be rotatable with respect to the other. In this case, the sixth arm 18 of the wrist 17 is rotatable with respect to the fifth arm 16 about the sixth rotation axis O6 with the sixth rotation axis O6 as the rotation center. The sixth rotation axis O6 is orthogonal to the fifth rotation axis O5. The rotation about the sixth rotation axis O <b> 6 is performed by driving the sixth drive source 406. The sixth drive source 406 is driven by a motor 406M and a cable (not shown), and the motor 406M is controlled by the control unit 20 via an electrically connected motor driver 306 (see FIG. 6). . The sixth drive source 406 may receive drive from the motor 406M by a speed reducer (not shown) provided together with the motor 406M, and the speed reducer may be omitted.

  Further, the sixth arm and the seventh arm 19 of the wrist 17 are connected via a joint (joint) 177. The joint 177 has a mechanism that supports one of the sixth arm 18 and the seventh arm 19 of the wrist 17 connected to each other so as to be rotatable with respect to the other. In this case, the seventh arm 19 of the wrist 17 is rotatable with respect to the sixth arm 18 around the seventh rotation axis O7 with the seventh rotation axis O7 as the rotation center. The rotation axis O7 is orthogonal to the rotation axis O6. The rotation about the seventh rotation axis O7 is performed by driving the seventh drive source 407. The drive of the seventh drive source 407 is driven by a motor 407M and a cable (not shown), and the motor 407M is controlled by the control unit 20 via an electrically connected motor driver 307 (FIG. 6). reference). The seventh drive source 407 may be configured to transmit the drive from the motor 407M by a speed reducer (not shown) provided together with the motor 407M, and the speed reducer may be omitted. Note that the sixth rotation axis O6 may be parallel to an axis orthogonal to the fifth rotation axis O5, and the seventh rotation axis O7 may be parallel to an axis orthogonal to the sixth rotation axis O6. Good. The fifth arm 16 houses, for example, motors 406M and 407M.

In addition, an inertial sensor 31 is installed in the fourth arm 15. In the present embodiment, as the inertial sensor 31, an angular velocity sensor that detects angular velocities around three detection axes that are orthogonal to each other, the x axis, the y axis, and the z axis (all not shown). Specifically, for example, a gyro sensor or the like can be used.
Further, the posture of the inertial sensor 31 is not particularly limited. For example, as shown in FIG. 2A, the posture of the arm connection body 8 is an axis parallel to the first rotation axis O1 and the third rotation axis O3. Of the inertial sensor 31 when the first rotation axis O1 and the fourth rotation axis O4 are parallel, and the first rotation axis O1 and the longitudinal axis 151 of the fourth arm 15 are parallel. The two detection axes may be in a posture that is parallel to the first rotation axis O1, the second rotation axis O2, and the third rotation axis O3, or may be in a tilted posture.

  The inertial sensor 31 detects an angular velocity around the x-axis, an angular velocity around the y-axis, and an angular velocity around the z-axis at the location where the inertial sensor 31 is installed. Then, the control unit 20 determines the angular velocity around the first rotation axis O1 of the first arm 12 and the second rotation of the second arm 13 based on the detection result and the attitude of the inertial sensor 31 (fourth arm 15). An angular velocity around the axis O2 and an angular velocity around the third rotation axis O3 of the third arm 14 are obtained. In order to obtain each angular velocity, for example, a known calculation may be performed, and the description thereof is omitted. In the claims of the present application, the angular velocity around the rotation axis of the arm is referred to as the angular velocity of the rotation axis of the arm.

  Here, in the robot 1, in order to suppress vibration of the first arm 12, the second arm 13, and the third arm 14, the inertial sensor 31 is installed on the fourth arm 15 as described above, and the inertial sensor 31 is provided. The operation of the drive sources 401, 402, and 403 is controlled based on the detection result. Thereby, the vibration of the 1st arm 12, the 2nd arm 13, and the 3rd arm 14 can be suppressed reliably, and, thereby, the vibration of the robot 1 whole can be suppressed.

  The drive sources 401 to 407 include a first angle sensor 411, a second angle sensor 412, a third angle sensor 413, a fourth angle sensor 414, a fifth angle sensor 415, and a sixth angle sensor. 416 and a seventh angle sensor 417 are provided. As these angle sensors, for example, an encoder, a rotary encoder, or the like can be used. These angle sensors 411 to 417 detect the rotation angles of the rotation shafts of the motors of the drive sources 401 to 407 or the reduction gears, respectively. The motors of the drive sources 401 to 407 are not particularly limited, and for example, a servo motor such as an AC servo motor or a DC servo motor is preferably used.

As shown in FIG. 6, the robot body 10 is electrically connected to the control unit 20. That is, the driving sources 401 to 407, the angle sensors 411 to 417, and the inertial sensor 31 are electrically connected to the control unit 20, respectively.
The control unit 20 can operate the arms 12 to 16 and the wrist 17 independently, that is, can control the drive sources 401 to 407 independently via the motor drivers 301 to 307, respectively. . In this case, the control unit 20 performs detection using the angle sensors 411 to 417 and the inertial sensor 31, and controls driving of the drive sources 401 to 407, for example, angular velocity, rotation angle, and the like based on the detection results. This control program is stored in advance in a recording medium built in the control unit 20.

Next, the configuration of the control unit 20 will be described with reference to FIGS.
As shown in FIGS. 6 to 13, the control unit 20 controls the operation of the first drive source control unit (first angular velocity command) 201 that controls the operation of the first drive source 401 and the second drive source 402. The second drive source control unit (second angular velocity command) 202, the third drive source control unit (third angular velocity command) 203 that controls the operation of the third drive source 403, and the operation of the fourth drive source 404 are controlled. The fourth drive source control unit (fourth angular velocity command) 204, the fifth drive source control unit (fifth angular velocity command) 205 that controls the operation of the fifth drive source 405, and the operation of the sixth drive source 406 are controlled. A sixth drive source control unit (sixth angular velocity command) 206 and a seventh drive source control unit (seventh angular velocity command) 207 for controlling the operation of the seventh drive source 407 are provided.

  As shown in FIG. 7, the first drive source control unit 201 includes a subtractor 511, a position control unit 521, a subtractor 531, an angular velocity control unit 541, a rotation angle calculation unit 551, and an angular velocity calculation unit 561. , A subtractor 571, a conversion unit 581, a correction value calculation unit 591, an adder 601, a feedback gain adjustment unit 611, and an angular velocity calculation unit 621. The angular velocity calculation unit 621 and the inertial sensor 32 constitute a first angular velocity detection unit that detects an angular velocity around the first rotation axis O1 of the first arm 12.

  As shown in FIG. 8, the second drive source control unit 202 includes a subtractor 512, a position control unit 522, a subtractor 532, an angular velocity control unit 542, a rotation angle calculation unit 552, and an angular velocity calculation unit 562. , A subtractor 572, a conversion unit 582, a correction value calculation unit 592, an adder 602, a feedback gain adjustment unit 612, and an angular velocity calculation unit 622. The angular velocity calculation unit 622 and the inertial sensor 31 constitute a second angular velocity detection unit that detects an angular velocity around the second rotation axis O2 of the second arm 13.

As shown in FIG. 9, the third drive source control unit 203 includes a subtracter 513, a position control unit 523, a subtractor 533, an angular velocity control unit 543, a rotation angle calculation unit 553, and an angular velocity calculation unit 563. , A subtractor 573, a conversion unit 583, a correction value calculation unit 593, an adder 603, and an angular velocity calculation unit 623.
As shown in FIG. 10, the fourth drive source control unit 204 includes a subtractor 514, a position control unit 524, a subtractor 534, an angular velocity control unit 544, a rotation angle calculation unit 554, and an angular velocity calculation unit 564. have.

As shown in FIG. 11, the fifth drive source control unit 205 includes a subtracter 515, a position control unit 525, a subtractor 535, an angular velocity control unit 545, a rotation angle calculation unit 555, and an angular velocity calculation unit 565. have.
As shown in FIG. 12, the sixth drive source control unit 206 includes a subtracter 516, a position control unit 526, a subtractor 536, an angular velocity control unit 546, a rotation angle calculation unit 556, and an angular velocity calculation unit 566. have.
As shown in FIG. 13, the seventh drive source control unit 207 includes a subtracter 517, a position control unit 527, a subtractor 537, an angular velocity control unit 547, a rotation angle calculation unit 557, and an angular velocity calculation unit 567. have.

  Here, the control unit 20 calculates the target position of the list 17 based on the content of the process performed by the robot 1 and generates a trajectory for moving the list 17 to the target position. And the control part 20 measures the rotation angle of each drive source 401-407 for every predetermined | prescribed control period so that the list | wrist 17 may move along the produced | generated track | orbit, and the value calculated based on this measurement result Are output to the drive source control units 201 to 207 as position commands Pc of the drive sources 401 to 407, respectively (see FIGS. 7 to 13). In the above and the following, “value is input and output” and the like are described, which means “a signal corresponding to the value is input and output”.

  As shown in FIG. 7, the first drive source control unit 201 receives detection signals from the first angle sensor 411 and the inertial sensor 31 in addition to the position command Pc of the first drive source 401. In the first drive source control unit 201, the rotation angle (position feedback value Pfb) of the first drive source calculated from the detection signal of the first angle sensor 411 becomes the position command Pc, and an angular velocity feedback value ωfb described later is obtained. The first drive source 401 is driven by feedback control using each detection signal so that an angular velocity command ωc described later is obtained.

  That is, the position command Pc is input to the subtracter 511 of the first drive source control unit 201, and the position feedback value Pfb described later is input from the rotation angle calculation unit 551. The rotation angle calculation unit 551 counts the number of pulses input from the first angle sensor 411 and outputs the rotation angle of the first drive source 401 corresponding to the count value to the subtracter 511 as the position feedback value Pfb. The The subtractor 511 outputs a deviation between the position command Pc and the position feedback value Pfb (a value obtained by subtracting the position feedback value Pfb from the target value of the rotation angle of the first drive source 401) to the position control unit 521.

The position control unit 521 performs a predetermined calculation process using the deviation input from the subtractor 511 and a proportional gain that is a predetermined coefficient, and the angular velocity of the first drive source 401 corresponding to the deviation. The target value of is calculated. The position control unit 521 outputs a signal indicating the target value (command value) of the angular velocity of the first drive source 401 to the subtracter 531 as an angular velocity command (first angular velocity command) ωc. Here, in this embodiment, proportional control (P control) is performed as feedback control, but the present invention is not limited to this.
The subtractor 531 receives an angular velocity command ωc and an angular velocity feedback value ωfb described later. The subtractor 531 outputs a deviation between the angular velocity command ωc and the angular velocity feedback value ωfb (a value obtained by subtracting the angular velocity feedback value ωfb from the target value of the angular velocity of the first drive source 401) to the angular velocity control unit 541.

The angular velocity control unit 541 uses a deviation input from the subtractor 531 and a proportional gain, an integral gain, etc., which are predetermined coefficients, and performs predetermined calculation processing including integration, so that a first response corresponding to the deviation is obtained. A drive signal (proportional to the drive current) of one drive source 401 is generated and supplied to the motor 401M via the motor driver 301. In this embodiment, PI control is performed as feedback control, but the present invention is not limited to this.
In this way, feedback control is performed so that the position feedback value Pfb is as equal as possible to the position command Pc, and the angular velocity feedback value ωfb is as equal as possible to the angular velocity command ωc. The drive current 401 is controlled.

Next, the angular velocity feedback value ωfb in the first drive source control unit 201 will be described.
The angular velocity calculation unit 561 calculates the angular velocity ωm1 of the first drive source 401 based on the frequency of the pulse signal input from the first angle sensor 411, and the angular velocity ωm1 is output to the adder 601.

Further, the angular velocity calculation unit 561 calculates the angular velocity ωA1m around the rotation axis O1 of the first arm 12 based on the frequency of the pulse signal input from the first angle sensor 411, and the angular velocity ωA1m is subtracted from the subtractor 571. Is output. The angular velocity ωA1m is a value obtained by dividing the angular velocity ωm1 by the reduction ratio between the motor 401M of the first drive source 401 and the first arm 12, that is, at the joint 171.
Further, the inertial sensor 31 detects an angular velocity around each detection axis, and the detected value is output to the angular velocity calculator 621. The angular velocity calculation unit 621 calculates an angular velocity ωA1 around the first rotation axis O1 of the first arm 12 based on the detected value, and the angular velocity ωA1 is output to the subtractor 571.

The subtractor 571 receives the angular velocity ωA1 and the angular velocity ωA1m, and the subtractor 571 outputs a value ωA1s (= ωA1−ωA1m) obtained by subtracting the angular velocity ωA1m from the angular velocity ωA1 to the conversion unit 581. This value ωA1s corresponds to the vibration component (vibration angular velocity) of the angular velocity around the rotation axis O1 of the first arm 12. Hereinafter, ωA1s is referred to as a vibration angular velocity. In the present embodiment, feedback control is performed in which the vibration angular velocity ωA1s (specifically, the angular velocity ωm1s in the motor 401M, which is a value generated based on the vibration angular velocity ωA1s) is multiplied by a gain Ka described later and returned to the input side of the drive source 401. Do. Specifically, feedback control is performed on the drive source 401 so that the vibration angular velocity ωA1s is as zero as possible. Thereby, the vibration of the robot 1 can be suppressed. In this feedback control, the angular velocity of the drive source 401 is controlled.
The converter 581 converts the vibration angular velocity ωA1s into the angular velocity ωm1s in the first drive source 401, and outputs the angular velocity ωm1s to the correction value calculator 591. This conversion can be obtained by multiplying the vibration angular velocity ωA1s by the reduction ratio between the motor 401M of the first drive source 401 and the first arm 12, that is, at the joint 171.

The correction value calculation unit 591 multiplies the angular velocity ωm1s by a gain (feedback gain) Ka that is a predetermined coefficient to obtain a correction value (first correction component) Ka · ωm1s, and adds the correction value Ka · ωm1s to the adder. To 601. The gain Ka is adjusted by the feedback gain adjustment unit 611 according to the posture of the fourth arm 15 with respect to the first rotation axis O1. The adjustment of the gain Ka will be described later in detail.
The adder 601 receives an angular velocity ωm1 and a correction value Ka · ωm1s. The adder 601 outputs the addition value of the angular velocity ωm1 and the correction value Ka · ωm1s to the subtractor 531 as the angular velocity feedback value ωfb. The subsequent operation is as described above.

  As shown in FIG. 8, detection signals are input to the second drive source control unit 202 from the second angle sensor 412 and the inertial sensor 31 in addition to the position command Pc of the second drive source 402. In the second drive source control unit 202, the rotation angle (position feedback value Pfb) of the second drive source 402 calculated from the detection signal of the second angle sensor 412 becomes the position command Pc, and an angular velocity feedback value ωfb described later. The second drive source 402 is driven by feedback control using each detection signal so that becomes an angular velocity command ωc described later.

  That is, the position command Pc is input to the subtracter 512 of the second drive source control unit 202, and the position feedback value Pfb described later is input from the rotation angle calculation unit 552. The rotation angle calculation unit 552 counts the number of pulses input from the second angle sensor 412 and outputs the rotation angle of the second drive source 402 corresponding to the count value to the subtracter 512 as the position feedback value Pfb. The The subtractor 512 outputs a deviation between the position command Pc and the position feedback value Pfb (a value obtained by subtracting the position feedback value Pfb from the target value of the rotation angle of the second drive source 402) to the position controller 522.

The position control unit 522 performs a predetermined calculation process using the deviation input from the subtractor 512 and a proportional gain, which is a predetermined coefficient, so that the angular velocity of the second drive source 402 according to the deviation is obtained. The target value of is calculated. The position controller 522 outputs a signal indicating the target value (command value) of the angular velocity of the second drive source 402 to the subtracter 532 as an angular velocity command (second angular velocity command) ωc. Here, in this embodiment, proportional control (P control) is performed as feedback control, but the present invention is not limited to this.
The subtractor 532 receives an angular velocity command ωc and an angular velocity feedback value ωfb described later. The subtractor 532 outputs a deviation between the angular velocity command ωc and the angular velocity feedback value ωfb (a value obtained by subtracting the angular velocity feedback value ωfb from the target angular velocity value of the second drive source 402) to the angular velocity control unit 542.

  The angular velocity control unit 542 uses a deviation input from the subtractor 532 and a predetermined coefficient, such as a proportional gain and an integral gain, to perform predetermined calculation processing including integration, and thereby perform a first calculation corresponding to the deviation. A drive signal (proportional to the drive current) of the two drive sources 402 is generated and supplied to the motor 402M via the motor driver 302. Here, in this embodiment, PI control is performed as feedback control, but the present invention is not limited to this.

  In this way, feedback control is performed so that the position feedback value Pfb is as equal as possible to the position command Pc, and the angular velocity feedback value ωfb is as equal as possible to the angular velocity command ωc, so that the second drive source The driving current 402 is controlled. Since the rotation axis O2 is orthogonal to the rotation axis O1, the rotation axis O2 is not affected by the operation or vibration of the first arm 12, and the second drive source 402 is independent of the first drive source 401. The operation can be controlled.

Next, the angular velocity feedback value ωfb in the second drive source control unit 202 will be described.
In the angular velocity calculation unit 562, the angular velocity ωm2 of the second drive source 402 is calculated based on the frequency of the pulse signal input from the second angle sensor 412, and the angular velocity ωm2 is output to the adder 602.

Further, the angular velocity calculation unit 562 calculates an angular velocity ωA2m around the rotation axis O2 of the second arm 13 based on the frequency of the pulse signal input from the second angle sensor 412, and the angular velocity ωA2m is subtracted from the subtractor 572. Is output. The angular velocity ωA2m is a value obtained by dividing the angular velocity ωm2 by the reduction ratio between the motor 402M of the second drive source 402 and the second arm 13, that is, at the joint 172.
Further, the inertial sensor 31 detects an angular velocity around each detection axis, and the detected value is output to the angular velocity calculator 621. The angular velocity calculation unit 622 calculates an angular velocity ωA2 around the second rotation axis O2 of the second arm 13 based on the detection value, and the angular velocity ωA2 is output to the subtractor 572.

  The subtractor 572 receives the angular velocity ωA2 and the angular velocity ωA2m, and the subtractor 572 outputs a value ωA2s (= ωA2−ωA2m) obtained by subtracting the angular velocity ωA2m from the angular velocity ωA2 to the conversion unit 582. This value ωA2s corresponds to the vibration component (vibration angular velocity) of the angular velocity around the rotation axis O2 of the second arm 13. Hereinafter, ωA2s is referred to as a vibration angular velocity. In this embodiment, the vibration angular velocity ωA2s (specifically, the angular velocity ωm2s in the motor 402M, which is a value generated based on the vibration angular velocity ωA2s) is multiplied by a gain Ka described later and fed back to the input side of the second drive source 402. Take control. Specifically, feedback control is performed on the second drive source 402 so that the vibration angular velocity ωA2s is as zero as possible. Thereby, the vibration of the robot 1 can be suppressed. In this feedback control, the angular velocity of the second drive source 402 is controlled.

The converter 582 converts the vibration angular velocity ωA2s into the angular velocity ωm2s in the second drive source 402, and outputs the angular velocity ωm2s to the correction value calculator 592. This conversion can be obtained by multiplying the vibration angular velocity ωA2s by the reduction ratio between the motor 402M of the second drive source 402 and the second arm 13, that is, the joint 172.
The correction value calculation unit 592 multiplies the angular velocity ωm2s by a gain (feedback gain) Ka that is a predetermined coefficient to obtain a correction value (second correction component) Ka · ωm2s, and adds the correction value Ka · ωm2s to the adder. To 602. The gain Ka is adjusted by the feedback gain adjustment unit 612 according to the posture of the fourth arm 15 with respect to the second rotation axis O2. The adjustment of the gain Ka will be described later in detail. The gain Ka in the second drive source control unit 202 and the gain Ka in the first drive source control unit 201 may be the same or different.
The adder 602 receives an angular velocity ωm2 and a correction value Ka · ωm2s. The adder 602 outputs the addition value of the angular velocity ωm2 and the correction value Ka · ωm2s to the subtractor 532 as the angular velocity feedback value ωfb. The subsequent operation is as described above.

  As shown in FIG. 9, the third drive source control unit 203 receives detection signals from the third angle sensor 413 and the inertial sensor 31 in addition to the position command Pc of the third drive source 403. The third drive source control unit 203 uses the rotation angle (position feedback value Pfb) of the third drive source 403 calculated from the detection signal of the third angle sensor 413 as the position command Pc, and an angular velocity feedback value ωfb described later. The third drive source 403 is driven by feedback control using each detection signal so that becomes an angular velocity command ωc described later.

  That is, the position command Pc is input to the subtracter 513 of the third drive source control unit 203, and the position feedback value Pfb described later is input from the rotation angle calculation unit 553. In the rotation angle calculation unit 553, the number of pulses input from the third angle sensor 413 is counted, and the rotation angle of the second drive source 403 corresponding to the count value is output to the subtracter 513 as the position feedback value Pfb. The The subtractor 513 outputs a deviation between the position command Pc and the position feedback value Pfb (a value obtained by subtracting the position feedback value Pfb from the target value of the rotation angle of the third drive source 403) to the position control unit 523.

The position control unit 523 performs a predetermined calculation process using the deviation input from the subtractor 513 and a proportional gain, which is a predetermined coefficient, so that the angular velocity of the third drive source 403 corresponding to the deviation is obtained. The target value of is calculated. The position controller 523 outputs a signal indicating the target value (command value) of the angular velocity of the third drive source 403 to the subtracter 533 as an angular velocity command (second angular velocity command) ωc. Here, in this embodiment, proportional control (P control) is performed as feedback control, but the present invention is not limited to this.
The subtractor 533 receives an angular velocity command ωc and an angular velocity feedback value ωfb described later. The subtractor 533 outputs a deviation between the angular velocity command ωc and the angular velocity feedback value ωfb (a value obtained by subtracting the angular velocity feedback value ωfb from the target value of the angular velocity of the third drive source 403) to the angular velocity control unit 543.

The angular velocity control unit 543 uses a deviation input from the subtracter 533 and a predetermined gain, such as a proportional gain and an integral gain, to perform predetermined calculation processing including integration, thereby performing a first calculation corresponding to the deviation. A drive signal (proportional to the drive current) of the three drive sources 403 is generated and supplied to the motor 403M via the motor driver 303. Here, in this embodiment, PI control is performed as feedback control, but the present invention is not limited to this.
In this way, feedback control is performed so that the position feedback value Pfb is as equal as possible to the position command Pc, and the angular velocity feedback value ωfb is as equal as possible to the angular velocity command ωc. The drive current of 403 is controlled.

Next, the angular velocity feedback value ωfb in the third drive source control unit 203 will be described.
The angular velocity calculation unit 563 calculates the angular velocity ωm3 of the third drive source 403 based on the frequency of the pulse signal input from the third angle sensor 413, and the angular velocity ωm3 is output to the adder 603.

Further, the angular velocity calculation unit 563 calculates the angular velocity ωA3m around the rotation axis O3 of the third arm 14 based on the frequency of the pulse signal input from the third angle sensor 413, and the angular velocity ωA3m is subtracted by the subtractor 573. Is output. The angular velocity ωA3m is a value obtained by dividing the angular velocity ωm3 by the reduction ratio between the motor 403M of the third drive source 403 and the third arm 14, that is, at the joint 173.
Further, the inertial sensor 31 detects an angular velocity around each detection axis, and the detected value is output to the angular velocity calculator 623. The angular velocity calculation unit 623 calculates an angular velocity ωA3 around the third rotation axis O3 of the third arm 14 based on the detection value, and the angular velocity ωA3 is output to the subtractor 573.

  The subtractor 572 receives the angular velocity ωA3 and the angular velocity ωA3m, and the subtractor 573 outputs a value ωA3s (= ωA3−ωA3m) obtained by subtracting the angular velocity ωA3m from the angular velocity ωA3 to the conversion unit 583. This value ωA3s corresponds to the vibration component (vibration angular velocity) of the angular velocity around the rotation axis O3 of the third arm 14. Hereinafter, ωA3s is referred to as a vibration angular velocity. In the present embodiment, the vibration angular velocity ωA3s (specifically, the angular velocity ωm3s in the motor 403M, which is a value generated based on the vibration angular velocity ωA3s) is multiplied by a gain Ka described later and fed back to the input side of the third drive source 403. Take control. Specifically, feedback control is performed on the third drive source 403 so that the vibration angular velocity ωA3s becomes 0 as much as possible. Thereby, the vibration of the robot 1 can be suppressed. In this feedback control, the angular velocity of the third drive source 403 is controlled.

The converter 583 converts the vibration angular velocity ωA3s into the angular velocity ωm3s in the third drive source 403, and outputs the angular velocity ωm3s to the correction value calculator 593. This conversion can be obtained by multiplying the vibration angular velocity ωA3s by the reduction ratio between the motor 403M of the third drive source 403 and the third arm 14, that is, at the joint 173.
The correction value calculation unit 593 multiplies the angular velocity ωm3s by a gain (feedback gain) Ka that is a predetermined coefficient to obtain a correction value (third correction component) Ka · ωm2s, and adds the correction value Ka · ωm3s to the adder. To 603. The gain Ka in the third drive source control unit 203 and the gain Ka in the first drive source control unit 201 may be the same or different. Further, the gain Ka in the third drive source control unit 203 and the gain Ka in the second drive source control unit 202 may be the same or different.
The adder 603 receives an angular velocity ωm3 and a correction value Ka · ωm3s. The adder 603 outputs the addition value of the angular velocity ωm3 and the correction value Ka · ωm3s to the subtracter 533 as the angular velocity feedback value ωfb. The subsequent operation is as described above.

  As shown in FIG. 10, the fourth drive source controller 204 receives a detection signal from the fourth angle sensor 414 in addition to the position command Pc of the fourth drive source 404. In the fourth drive source control unit 204, the rotation angle (position feedback value Pfb) of the fourth drive source 404 calculated from the detection signal of the fourth angle sensor 414 becomes the position command Pc, and an angular velocity feedback value ωfb described later. The fourth drive source 404 is driven by feedback control using each detection signal so that becomes an angular velocity command ωc described later.

  That is, the position command Pc is input to the subtracter 514 of the fourth drive source control unit 204, and the position feedback value Pfb described later is input from the rotation angle calculation unit 554. The rotation angle calculation unit 554 counts the number of pulses input from the fourth angle sensor 414 and outputs the rotation angle of the fourth drive source 404 corresponding to the count value to the subtracter 514 as the position feedback value Pfb. The The subtractor 514 outputs a deviation between the position command Pc and the position feedback value Pfb (a value obtained by subtracting the position feedback value Pfb from the target value of the rotation angle of the fourth drive source 404) to the position control unit 524.

  The position control unit 524 performs a predetermined calculation process using the deviation input from the subtracter 514 and a proportional gain, which is a predetermined coefficient, so that the angular velocity of the fourth drive source 404 corresponding to the deviation is obtained. The target value of is calculated. The position control unit 524 outputs a signal indicating the target value (command value) of the angular velocity of the fourth drive source 404 to the subtracter 534 as the angular velocity command ωc. Here, in this embodiment, proportional control (P control) is performed as feedback control, but the present invention is not limited to this.

Further, the angular velocity calculation unit 564 calculates the angular velocity of the fourth driving source 404 based on the frequency of the pulse signal input from the fourth angle sensor 414, and outputs the angular velocity as the angular velocity feedback value ωfb. .
The subtractor 534 receives an angular velocity command ωc and an angular velocity feedback value ωfb. The subtractor 534 outputs a deviation between the angular velocity command ωc and the angular velocity feedback value ωfb (a value obtained by subtracting the angular velocity feedback value ωfb from the target value of the angular velocity of the fourth driving source 404) to the angular velocity control unit 544.

The angular velocity control unit 544 uses a deviation input from the subtractor 534 and a predetermined gain, such as a proportional gain, an integral gain, and the like, and performs predetermined calculation processing including integration so that a first response corresponding to the deviation is obtained. A drive signal (proportional to the drive current) of the four drive sources 404 is generated and supplied to the motor 404M via the motor driver 304. Here, in this embodiment, PI control is performed as feedback control, but the present invention is not limited to this.
In this way, feedback control is performed so that the position feedback value Pfb is as equal as possible to the position command Pc, and the angular velocity feedback value ωfb is as equal as possible to the angular velocity command ωc. The drive current 404 is controlled.
Note that the drive source control units 205 to 207 are the same as the fourth drive source control unit 204, and a description thereof will be omitted.

  In the robot 1, feedback gain adjustment units 611 and 612 are configured to gain (feedback) according to the posture of the fourth arm 15 with respect to the first rotation axis O1 and the posture of the fourth arm 15 with respect to the second rotation axis O2, respectively. Gain) Ka is adjusted. That is, the gain Ka is configured to be set to either a first value that is a standard value (normal value) or a second value that is lower than the standard value. Note that the attitude of the fourth arm 15 with respect to the first rotation axis O1 is obtained by the feedback gain adjustment unit 611 based on the detection results of the angle sensors 411 to 414 provided in the drive sources 401 to 404, respectively. Similarly, the attitude of the fourth arm 15 with respect to the second rotation axis O2 is obtained by the feedback gain adjustment unit 612 based on the detection results of the angle sensors 411 to 414 provided in the drive sources 401 to 404, respectively. Therefore, the feedback gain adjustment unit 611 constitutes a posture detection unit that detects the posture of the fourth arm 15 with respect to the first rotation axis O1. Similarly, the feedback gain adjustment unit 612 constitutes a posture detection unit that detects the posture of the fourth arm 15 with respect to the second rotation axis O2.

  Specifically, when the longitudinal axis (center axis) 151 extending in the longitudinal direction of the fourth arm 15 and the first rotation axis O1 of the first arm 12 are parallel or close to parallel, the length of the fourth arm 15 is increased. When the longitudinal axis 151 extending in the direction and the second rotation axis O2 of the second arm 13 are parallel or close to parallel, the gain Ka is reduced below the first value which is a standard value (normal value). That is, it is configured to set the second value lower than the first value. When the longitudinal axis 151 of the fourth arm 15 and the first rotation axis O1 of the first arm 12 are parallel or close to parallel, the gain Ka is set to the second value to thereby reduce the vibration of the robot 1. Oscillation can be prevented. Similarly, when the longitudinal axis 151 of the fourth arm 15 and the second rotation axis O2 of the second arm 13 are parallel or close to parallel, the robot 1 is set by setting the gain Ka to the second value. The oscillation of the vibration can be prevented.

The second value of the gain Ka is preferably set to a gain Ka for other postures, that is, set to 0% or more and 70% or less of the first value, and set to 30% or more and 60% or less. More preferably. Thereby, it is possible to prevent the vibration of the robot 1 while preventing the vibration of the robot 1 more reliably.
In the control of the first drive source 401, the other posture is other than when the longitudinal axis 151 of the fourth arm 15 and the first rotation axis O1 of the first arm 12 are parallel or nearly parallel. As an example (typical example), assuming a first virtual axis 152 that is parallel to the longitudinal axis 151 of the fourth arm 15 and can intersect the first rotation axis O1, the first virtual axis 152 and the first virtual axis 152 This is when the rotation axis O1 is orthogonal. That is, the one example is when the longitudinal axis 151 and the first rotation axis O1 are orthogonal to each other when viewed from the same direction as the third rotation axis O3.

  Similarly, in the control of the second drive source 402, the other posture is other than when the longitudinal axis 151 of the fourth arm 15 and the second rotation axis O2 of the second arm 13 are parallel or nearly parallel. As one example (typical example), assuming a second virtual axis 153 that is parallel to the longitudinal axis 151 of the fourth arm 15 and can intersect the second rotation axis O2, the second virtual axis 153 This is when the second rotation axis O2 is orthogonal. That is, the one example is when the longitudinal axis 151 and the second rotation axis O2 are orthogonal to each other when viewed in plan from the same direction as the third rotation axis O3.

The second value of the gain Ka is not particularly limited and is appropriately set according to various conditions, but is preferably 0 or more and 0.6 or less, preferably 0.1 or more, 0.0. More preferably, it is 4 or less. The vibration can be prevented more reliably while preventing the vibration of the robot 1 from oscillating.
Further, the first value of the gain Ka is not particularly limited, and is appropriately set according to various conditions, but is preferably 0.2 or more and 0.8 or less, preferably 0.3 or more, More preferably, it is 0.6 or less. The vibration can be prevented more reliably while preventing the vibration of the robot 1 from oscillating.

  Here, as described above, in the control of the first drive source 401, not only when the longitudinal axis 151 of the fourth arm 15 and the first rotation axis O1 of the first arm 12 are parallel, but when they are close to parallel. That is, an angle θ1 (see FIG. 2B) formed by the first virtual axis 152 that is an axis parallel to the longitudinal axis 151 of the fourth arm 15 and the first rotation axis O1 of the first arm 12 is a predetermined angle. If it is within the range, the gain Ka is set to the second value. That is, the gain Ka is also set to the second value when the angle θ1 formed by the first virtual axis 152 and the first rotation axis O1 of the first arm 12 is not less than θmin and not more than θmax.

θmax is not particularly limited and is appropriately set according to various conditions, but is preferably 10 ° or more and 45 ° or less, and more preferably 15 ° or more and 35 ° or less.
Θmin is not particularly limited and is appropriately set according to various conditions, but is preferably −45 ° or more and −10 ° or less, and is −35 ° or more and −15 ° or less. Is more preferable.

Thereby, the oscillation of the vibration of the robot 1 can be prevented more reliably.
Note that θ1 is obtained by the feedback gain adjustment unit 611 based on detection results of the angle sensors 411 to 414 provided in the drive sources 401 to 404, respectively.
Similarly, as described above, in the control of the second drive source 402, not only when the longitudinal axis 151 of the fourth arm 15 and the second rotation axis O2 of the second arm 13 are parallel, but when they are close to parallel. That is, an angle θ2 (see FIG. 2C) formed by the second virtual axis 153 that is an axis parallel to the longitudinal axis 151 of the fourth arm 15 and the second rotation axis O2 of the second arm 13 is a predetermined angle. If it is within the range, the gain Ka is set to the second value. That is, the gain Ka is also set to the second value when the angle θ2 formed by the second virtual axis 153 and the second rotation axis O2 of the second arm 13 is not less than θmin and not more than θmax.
θmax is not particularly limited and is appropriately set according to various conditions, but is preferably 10 ° or more and 45 ° or less, and more preferably 15 ° or more and 35 ° or less.

Θmin is not particularly limited and is appropriately set according to various conditions, but is preferably −45 ° or more and −10 ° or less, and is −35 ° or more and −15 ° or less. Is more preferable.
Thereby, the oscillation of the vibration of the robot 1 can be prevented more reliably.
Note that θ2 is obtained by the feedback gain adjustment unit 612 based on the detection results of the angle sensors 411 to 414 provided in the drive sources 401 to 404, respectively.

Next, the control operation of the control unit 20 when adjusting the gain Ka of the robot 1 will be described.
As shown in FIG. 14, first, an angle range that is a reference for switching the gain Ka in each of the control of the first drive source 401 and the control of the second drive source 402 is set (step S101). In the present embodiment, for example, an angle θ1 formed by the first virtual axis 152 and the first rotation axis O1 of the first arm 12, and an angle θ2 formed by the second virtual axis 153 and the second rotation axis O2 of the second arm 13 are used. Are respectively set to “−30 ° or more and 30 ° or less”.

Next, an angle θ1 formed by the first virtual axis 152 and the first rotation axis O1 of the first arm 12 is obtained (step S102).
Next, it is determined whether or not the angle θ1 formed by the first virtual axis 152 and the first rotation axis O1 of the first arm 12 is −30 ° or more and 30 ° or less (step S102), and θ1 is − When the angle is not less than 30 ° and not less than 30 °, the gain Ka in the control of the first drive source 401 is set to the first value (step S103). In the control of the first drive source 401, if θ1 is not −30 ° or more and 30 ° or less, the vibration of the robot 1 is difficult to oscillate. Therefore, even if the gain Ka is set to the first value, the vibration of the robot 1 Does not oscillate, and vibration can be more reliably suppressed by setting the gain Ka to the first value.

  In step S102, if θ1 is not less than −30 ° and not more than 30 °, the gain Ka in the control of the first drive source 401 is set to the second value (step S104). When θ1 is not less than −30 ° and not more than 30 °, the vibration of the robot 1 is likely to oscillate. Therefore, by setting the gain Ka to the second value, the oscillation of the robot 1 is prevented from oscillating. However, vibration can be suppressed.

Next, an angle θ2 formed by the second virtual axis 153 and the second rotation axis O2 of the second arm 13 is obtained (step S106).
Next, it is determined whether or not the angle θ2 formed by the second virtual axis 153 and the second rotation axis O2 of the second arm 13 is −30 ° or more and 30 ° or less (step S105), and θ2 is − When the angle is not less than 30 ° and not less than 30 °, the gain Ka in the control of the second drive source 402 is set to the first value (step S106). In the control of the second drive source 402, if θ2 is not −30 ° or more and 30 ° or less, the vibration of the robot 1 is difficult to oscillate. Therefore, even if the gain Ka is set to the first value, the vibration of the robot 1 Does not oscillate, and vibration can be more reliably suppressed by setting the gain Ka to the first value.

  In step S105, when θ2 is not less than −30 ° and not more than 30 °, the gain Ka in the control of the second drive source 402 is set to the second value (step S107). When θ2 is not less than −30 ° and not more than 30 °, the vibration of the robot 1 is likely to oscillate. Therefore, setting the gain Ka to the second value prevents the oscillation of the robot 1 from oscillating. However, vibration can be suppressed.

Then, after steps S106 and S107, the process returns to step S102, and step S102 and subsequent steps are executed again.
In this way, the gain Ka in the control of the first drive source 401 and the gain Ka in the control of the second drive source 402 are either the first value or the second value according to θ1 and θ2, respectively. Set to. Thereby, it is possible to suppress the vibration while preventing the vibration of the robot 1 from oscillating.

As described above, according to this robot 1, vibration of the robot 1 can be easily and reliably suppressed.
In particular, according to the posture of the robot 1, that is, the posture of the fourth arm 15 with respect to the first rotation axis O1 and the second rotation axis O2, the gain Ka in the control of the first drive source 401 and the control of the second drive source 402, respectively. Since the gain Ka is adjusted, the vibration of the robot 1 can be prevented from oscillating, and the vibration of the robot 1 can be more reliably suppressed.

In this embodiment, the inertial sensor 31 is installed on the fourth arm 15, but the present invention is not limited to this, and the inertial sensor 31 may be installed on the third arm 14. Also in this case, the vibration of the robot 1 can be suppressed while preventing the vibration of the robot 1 from oscillating.
The inertial sensor 31 is not limited to the angular velocity sensor having the three detection axes. For example, three angular velocity sensors having one detection axis may be installed on the third arm 14 or the fourth arm 15. Good. In this case, the angular velocity sensors are installed so that the detection axes are orthogonal to each other.

Second Embodiment
FIG. 15 is a schematic view showing an arm coupling body in the second embodiment of the robot of the present invention.
In the following, for convenience of explanation, the support side in FIG. 15 is referred to as “base end” and the opposite side is referred to as “tip”. In FIG. 15, the first inertia sensor 32, the second inertia sensor 33, and the third inertia sensor 34 are respectively illustrated outside the corresponding arms 15 in order to clarify their existence.

Hereinafter, the second embodiment will be described with a focus on the differences from the first embodiment described above, and the description of the same matters will be omitted.
As shown in FIG. 15, in the robot 1 of the second embodiment, the first inertia sensor 32 is installed in the first arm 12, the second inertia sensor 33 is installed in the second arm 13, and the third A third inertia sensor 34 is installed in the arm 14.

As the first inertia sensor 32, the second inertia sensor 33, and the third inertia sensor 34, an angular velocity sensor or an acceleration sensor having one detection axis is used. In this embodiment, an angular velocity sensor is used. Specifically, for example, a gyro sensor or the like can be used.
Moreover, the installation position of the first inertial sensor 32 in the first arm 12 is not particularly limited, but the tip of the first arm 12 is preferable. Since the vibration of the first arm 12 is maximized at the tip portion thereof, the vibration of the robot 1 can be more reliably suppressed.

Similarly, the installation position of the second inertial sensor 33 in the second arm 13 is not particularly limited, but the tip of the second arm 13 is preferable. Since the vibration of the second arm 13 is maximized at the tip thereof, the vibration of the robot 1 can be more reliably suppressed.
Similarly, the installation position of the third inertial sensor 33 in the third arm 14 is not particularly limited, but the tip of the third arm 14 is preferable. Since the vibration of the 3rd arm 14 becomes the maximum in the front-end | tip part, this can suppress the vibration of the robot 1 more reliably.

  The first inertia sensor 32 detects the angular velocity of the first arm 12 around the first rotation axis O1. Further, the second inertia sensor 33 detects the angular velocity of the second arm 13 around the second rotation axis O2. Further, the angular velocity of the third arm 14 is detected by the third inertia sensor 34. Since the angular velocity of the third arm 14 detected by the third inertia sensor 34 includes a component due to the rotation of the first arm 12, the control unit 20 determines the first value from the detection value of the third inertia sensor 34. The detection value of the one inertia sensor 32 is subtracted, and the value is set as an angular velocity around the third rotation axis O3 of the third arm 14.

According to this robot 1, the same effects as those of the first embodiment described above can be obtained.
And in this robot 1, the calculation in the control part 20 can be simplified. That is, since the first rotation axis O1 and the second rotation axis O2 are orthogonal to each other, and the second rotation axis O2 and the third rotation axis O3 are orthogonal to each other, the first inertia sensor 32 causes the first arm 12 to Only the angular velocity around the rotation axis O <b> 1 is detected, and only the angular velocity around the second rotation axis O <b> 2 of the second arm 13 is detected by the second inertial sensor 33. As a result, no special calculation is required, and the circuit configuration of the control unit 20 can be simplified.

The robot of the present invention has been described based on the illustrated embodiment. However, the present invention is not limited to this, and the configuration of each unit may be replaced with an arbitrary configuration having the same function. Can do. In addition, any other component may be added to the present invention.
Further, the present invention may be a combination of any two or more configurations (features) of the above embodiments.
In addition, as a motor of a drive source, for example, a stepping motor or the like can be cited in addition to the servo motor.

  Further, in the embodiment, as the angle sensor, various other sensors for detecting the rotation angle of the motor rotor such as an encoder, a resolver, and a potentiometer can be used, and the rotation speed of the motor rotor such as a tachometer generator is used. Various sensors for detecting the above may be used. When a stepping motor is used as the motor, for example, the rotation angle or rotation speed of the rotor of the motor may be detected by measuring the number of drive pulses input to the stepping motor.

  In the above embodiment, a gyro sensor can be used as the inertial sensor. However, the present invention is not limited to this. For example, other various angular velocity sensors that detect the angular velocity of the arm may be used. Various acceleration sensors that detect acceleration may be used. In the case where an acceleration sensor is used, the angular velocity is calculated using the detection value of the acceleration sensor.

Moreover, the method of an angle sensor and an inertial sensor is not specifically limited, respectively, For example, an optical system, a magnetic system, an electromagnetic system, an electrical system etc. are mentioned.
In the embodiment, the number of rotation axes of the robot is seven. However, the present invention is not limited to this, and the number of rotation axes of the robot is four, five, six, or eight. That's all. That is, in the above embodiment, since the list has two arms, the number of robot arms is seven. However, in the present invention, the number of robot arms is not limited to this. It may be 4, 5, 6, or 8 or more.

Further, in the embodiment, the robot is a double-arm robot having two arm connecting bodies formed by rotatably connecting a plurality of arms. However, in the present invention, the robot is not limited to this. It may be a single-arm robot having one arm connection body formed by rotatably connecting arms, or a robot having three or more.
The robot of the present invention is not limited to an arm type robot (robot arm), but may be another type of robot, such as a legged walking (running) robot.

  DESCRIPTION OF SYMBOLS 1 ... Robot (industrial robot) 10 ... Robot main body 11 ... Support part 12, 13, 14, 15, 16, 18, 19 ... Arm (link) 17 ... List (link) 151 ... Longitudinal axis 152, 153... Virtual axes 171, 172, 173, 174, 175, 176, 177... Joints 31, 32, 33, 34... Inertial sensor 7. ... Manipulator 100 ... Base 20 ... Control part 101 ... Floor 201, 202, 203, 204, 205, 206, 207 ... Drive source control part 301, 302, 303, 304, 305, 306, 307 ... Motor Driver 401, 402, 403, 404, 405, 406, 406 ... Driving source 401M, 402M, 403M, 404M 405M, 406M, 407M ... Motors 411, 412, 413, 414, 415, 416, 417 ... Angle sensors 511, 512, 513, 514, 515, 516, 517 ... Subtractors 521, 522, 523, 524, 525, 526, 527 ... Position control unit 531, 532, 533, 534, 535, 536, 537 ... Subtractor 541, 542, 543, 544, 545, 546, 547 ... Angular velocity control unit 551, 552, 553 554, 555, 556, 557... Rotation angle calculation unit 561, 562, 563, 564, 565, 566, 567 ... Angular velocity calculation unit 571, 572, 573 ... Subtractor 581, 582, 583 ... Conversion unit 591, 592, 593... Correction value calculation unit 601, 602, 603. 611, 612 ...... hood back gain adjustment unit 621, 622, 623 ...... angular velocity calculating unit O1, O2, O3, O4, O5, O6, O7 ...... rotating shaft

Claims (17)

The base,
A first arm connected to the base and rotating about a first rotation axis;
It is connected to the first arm and rotates about the second rotation axis, and the second rotation axis is rotated in parallel with the rotation axis orthogonal to the first rotation axis or the axis orthogonal to the first rotation axis. A second arm as a shaft;
It is connected to the second arm and rotates around the third rotation axis, and the third rotation axis is rotated in parallel with the rotation axis orthogonal to the second rotation axis or the axis orthogonal to the second rotation axis. A third arm as a shaft;
It is connected to the third arm and rotates around the fourth rotation axis, and the fourth rotation axis is rotated in parallel with the rotation axis orthogonal to the third rotation axis or the axis orthogonal to the third rotation axis. A fourth arm having an axial shape and a longitudinal shape;
A first drive source for rotating the first arm;
A first angle sensor for detecting a rotation angle of the first drive source;
A first inertial sensor that detects an angular velocity or acceleration of the first rotation axis of the first arm;
An attitude detection unit for detecting the attitude of the fourth arm;
First angular velocity of the first rotating shaft of the first arm derived from the rotational angle of the first driving source and angular velocity of the first rotating shaft of the first arm detected by the first inertial sensor. A first drive source control unit that feeds back one correction component to control the first drive source, and sets a feedback gain of the first correction component based on a detection result of the posture detection unit;
A robot characterized by comprising:   The first drive source control unit has the same feedback gain of the first correction component as that of the third rotation axis when a longitudinal axis extending in the longitudinal direction of the fourth arm and the first rotation axis are parallel to each other. The robot according to claim 1, wherein the robot is set to be lower than a feedback gain of the first correction component when the longitudinal axis and the first rotation axis are orthogonal to each other in plan view from the direction.   The feedback gain of the first correction component when the longitudinal axis extending in the longitudinal direction of the fourth arm and the first rotation axis are parallel is the longitudinal axis in plan view from the same direction as the third rotation axis. The robot according to claim 1, wherein the robot is set to 0% or more and 70% or less of a feedback gain of the first correction component when the first rotation axis and the first rotation axis are orthogonal to each other. A second drive source for rotating the second arm;
A second angle sensor for detecting a rotation angle of the second drive source;
A second inertial sensor for detecting an angular velocity or acceleration of the second rotation axis of the second arm;
A second guide derived from an angular velocity of the second rotary shaft of the second arm derived from a rotational angle of the second drive source and an angular velocity of the second rotary shaft of the second arm detected by the second inertia sensor. A second drive source control unit that feeds back two correction components to control the second drive source, and sets a feedback gain of the second correction component based on a detection result of the posture detection unit;
The robot according to claim 1, further comprising:   The second drive source control unit has the same feedback gain of the second correction component as that of the third rotation axis when the longitudinal axis extending in the longitudinal direction of the fourth arm and the second rotation axis are parallel to each other. The robot according to claim 4, wherein the robot is set to be lower than a feedback gain of the second correction component when the longitudinal axis and the second rotation axis are orthogonal to each other in plan view from the direction.   The feedback gain of the second correction component when the longitudinal axis extending in the longitudinal direction of the fourth arm and the second rotation axis are parallel is the longitudinal axis in plan view from the same direction as the third rotation axis. 6. The robot according to claim 4, wherein the robot is set to 0% or more and 70% or less of a feedback gain of the second correction component when the second rotation axis is orthogonal to the second rotation axis.   The robot according to any one of claims 4 to 6, wherein the second inertial sensor is installed on the fourth arm.   The robot according to claim 4, wherein the second inertial sensor is installed on the third arm.   The robot according to any one of claims 1 to 7, wherein the first inertial sensor is installed on the fourth arm.   The robot according to any one of claims 1 to 7, wherein the first inertial sensor is installed on the third arm. The first inertial sensor is installed on the first arm,
The robot according to claim 1, wherein the second inertial sensor is installed on the second arm. The base,
A first arm connected to the base and rotating about a first rotation axis;
It is connected to the first arm and rotates about the second rotation axis, and the second rotation axis is rotated in parallel with the rotation axis orthogonal to the first rotation axis or the axis orthogonal to the first rotation axis. A second arm as a shaft;
It is connected to the second arm and rotates around the third rotation axis, and the third rotation axis is rotated in parallel with the rotation axis orthogonal to the second rotation axis or the axis orthogonal to the second rotation axis. A third arm as a shaft;
It is connected to the third arm and rotates around the fourth rotation axis, and the fourth rotation axis is rotated in parallel with the rotation axis orthogonal to the third rotation axis or the axis orthogonal to the third rotation axis. A fourth arm having an axial shape and a longitudinal shape;
A second drive source for rotating the second arm;
A second angle sensor for detecting a rotation angle of the second drive source;
A second inertial sensor for detecting an angular velocity or acceleration of the second rotation axis of the second arm;
An attitude detection unit for detecting the attitude of the fourth arm;
Correction derived from the angular velocity of the second rotating shaft of the second arm derived from the rotational angle of the second drive source and the angular velocity of the second rotating shaft of the second arm detected by the second inertia sensor. A second drive source control unit that feeds back a component to control the second drive source, and sets a feedback gain of the correction component based on a detection result of the attitude detection unit;
A robot characterized by comprising:   In the drive source control unit, the feedback gain of the correction component when the longitudinal axis extending in the longitudinal direction of the fourth arm and the second rotation axis are parallel is viewed in plan from the same direction as the third rotation axis. The robot according to claim 12, wherein the robot is set lower than a feedback gain of the correction component when the longitudinal axis and the second rotation axis are orthogonal to each other.   The feedback gain of the correction component when the longitudinal axis and the second rotation axis are parallel is a plan view from the same direction as the third rotation axis, and the longitudinal axis and the second rotation axis are orthogonal to each other. The robot according to claim 12 or 13, wherein the robot is set to 0% or more and 70% or less of the feedback gain of the correction component at the time.   The robot according to any one of claims 12 to 14, wherein the second inertial sensor is installed on the fourth arm.   The robot according to any one of claims 12 to 14, wherein the second inertial sensor is installed on the third arm. It is connected to the fourth arm and pivots about the fifth rotation axis, and the fifth rotation axis rotates in parallel with the rotation axis orthogonal to the fourth rotation axis or the axis orthogonal to the fourth rotation axis. A fifth arm as a shaft;
It is connected to the fifth arm and rotates about the sixth rotation axis, and the sixth rotation axis is rotated in parallel with the rotation axis orthogonal to the fifth rotation axis or the axis orthogonal to the fifth rotation axis. A sixth arm as a shaft;
It is connected to the sixth arm and rotates about the seventh rotation axis, and the seventh rotation axis is rotated in parallel with the rotation axis orthogonal to the sixth rotation axis or the axis orthogonal to the sixth rotation axis. A seventh arm as a shaft;
The robot according to claim 1, comprising:

Images