Robot control method and robot system

Abstract

To provide a robot control method that can effectively control vibrations and a robot system.SOLUTION: A robot system is provided with a robot which comprises a robot arm which has a base end-side joint and a tip-side joint having turning shafts which are parallel to each other and an inertial sensor, arranged closer to a tip side than the tip-side joint of the robot arm, which detects angular speed around a shaft parallel to the turning shaft and acceleration in a direction crossing the turning shaft. The robot control method includes: an inertia information obtaining step of obtaining the angular speed and the acceleration from the inertial sensor; a vibration detecting step of detecting respective vibrations of the base end-side joint and of the tip-side joint, on the basis of the angular speed and the acceleration obtained in the inertial information obtaining step; and a driving control step of controlling vibration on the basis of the vibrations detected in the vibration detecting step.SELECTED DRAWING: Figure 1

Description

Technical Field

[0001] The present invention relates to a method for controlling a robot and a robot system.

Background Art

[0002] The manipulator described in Patent Document 1 includes an arm having a plurality of joints, an actuator disposed at each joint, an end effector connected to the tip of the arm, and an acceleration sensor disposed at the end effector. In such a manipulator, the vibration of the end effector is detected by the acceleration sensor, and the vibration is controlled based on the detection result.

Prior Art Documents

Patent Documents

[0003]

Patent Document 1

Summary of the Invention

Problems to be Solved by the Invention

[0004] However, in the manipulator of Patent Document 1, it is impossible to determine which joint among the plurality of joints of the arm causes the vibration of the end effector. Therefore, it is difficult to effectively control the vibration.

Means for Solving the Problems

[0005] The method for controlling a robot according to the present invention includes a robot including a robot arm having a base-end side joint having rotation axes parallel to each other and a tip-end side joint located on the tip-end side of the base-end side joint, and an inertial sensor disposed on the tip-end side of the tip-end side joint of the robot arm, the inertial sensor detecting an angular velocity about an axis parallel to the rotation axis and an acceleration in a direction intersecting the rotation axis. In a robot system having the inertial sensor, An inertial information acquisition step of acquiring the angular velocity and the acceleration from the inertial sensor; An oscillation detection step of detecting the oscillation amount of each of the base end side joint and the tip end side joint based on the angular velocity and the acceleration acquired in the inertial information acquisition step; A drive control step of controlling the oscillation based on the oscillation amount detected in the oscillation detection step, and includes.

[0006] The robot system of the present invention includes a robot including a robot arm having a base end side joint having rotation axes parallel to each other and a tip end side joint located at the tip end side of the base end side joint; An inertial sensor disposed on the tip end side of the tip end side joint of the robot arm and detecting an angular velocity around an axis parallel to the rotation axis and an acceleration in a direction intersecting the rotation axis; A control device that detects the oscillation amount of each of the base end side joint and the tip end side joint based on the angular velocity and the acceleration detected by the inertial sensor and controls the oscillation based on the detected oscillation amount.

Brief Description of the Drawings

[0007]

Figure 1

Figure 2

Figure 3

Figure 4

Figure 5

Figure 6

Figure 7

Figure 8

Figure 9

Figure 10

Figure 11

Figure 12

Figure 13

Figure 14

Figure 15

Figure 16

Figure 17

Figure 18

Figure 19

Figure 20

Figure 21

Embodiments for Carrying Out the Invention

[0008] Hereinafter, a control method for a robot and a robot system of the present invention will be described in detail based on embodiments shown in the accompanying drawings.

[0009] <First Embodiment> FIG. 1 is an overall view of a robot system according to the first embodiment. FIG. 2 is a schematic view showing a posture in which the vibration amounts of the respective joints of the second and third joints cannot be detected only by acceleration. FIG. 3 is a schematic view showing a posture in which the vibration amounts of the respective joints of the second and third joints cannot be detected only by angular velocity. FIG. 4 is a flowchart showing a control process of the robot. FIGS. 5 to 10 are schematic views for explaining a method of detecting the vibration amount of each of the second and third joints. FIG. 11 is a block diagram showing the configuration of a control unit included in the control device. FIGS. 12 to 15 are diagrams showing examples of the arrangement of the inertial sensors, respectively.

[0010] The robot system 1 shown in FIG. 1 includes a robot 2, an inertial sensor 3 disposed on the robot 2, and a control device 4 that controls the driving of the robot 2.

[0011] The robot 2 is a six-axis vertical articulated robot having six drive axes, and includes a base 21 and a robot arm 22 rotatably connected to the base 21. The robot arm 22 has a configuration in which first, second, third, fourth, fifth, and sixth arms 221, 222, 223, 224, 225, and 226 are connected via first, second, third, fourth, fifth, and sixth joints 231, 232, 233, 234, 235, and 236.

[0012] The first arm 221 is rotatably connected to the base 21 about a first rotation axis J1 via the first joint 231. The second arm 222 is rotatably connected to the first arm 221 about a second rotation axis J2 via the second joint 232. The third arm 223 is rotatably connected to the second arm 222 about a third rotation axis J3 via the third joint 233. The fourth arm 224 is rotatably connected to the third arm 223 about a fourth rotation axis J4 via the fourth joint 234. The fifth arm 225 is rotatably connected to the fourth arm 224 about a fifth rotation axis J5 via the fifth joint 235. The sixth arm 226 is rotatably connected to the fifth arm 225 about a sixth rotation axis J6 via the sixth joint 236.

[0013] Among the joints 231, 232, 233, 234, 235, and 236, the second, third, and fifth joints 232, 233, and 235 are bending joints, respectively, and the first, fourth, and sixth joints 231, 234, and 236 are torsion joints, respectively. Also, the second rotation axis J2 is orthogonal to the first rotation axis J1, the third rotation axis J3 is parallel to the second rotation axis J2, the fourth rotation axis J4 is orthogonal to the third rotation axis J3, the fifth rotation axis J5 is orthogonal to the fourth rotation axis J4, and the sixth rotation axis J6 is orthogonal to the fifth rotation axis J5. Further, the second, third, and fifth rotation axes J2, J3, and J5 are each oriented in the horizontal direction.

[0014] In the present specification, "parallel" means not only the case where the axes of each other are parallel, but also a deviation that can be regarded as parallel in terms of common technical knowledge with respect to parallel, for example, a deviation that can occur due to the dimensional accuracy, assembly accuracy, etc. of the robot 2. Also, "orthogonal" means not only the case where the axes of each other are orthogonal, but also a deviation that can be regarded as orthogonal in terms of common technical knowledge with respect to orthogonal, for example, a deviation that can occur due to the dimensional accuracy, assembly accuracy, etc. of the robot 2.

[0015] In such a robot 2, the second joint 232 is the "base-end side joint" of the present application, and the third joint 233 is the "tip-end side joint" of the present application.

[0016] Each of the joints 231, 232, 233, 234, 235, and 236 includes a motor M, a speed reducer (not shown) that reduces the rotational speed of the motor M and outputs it, and an encoder E that detects the rotation amount of the motor M. The control device 4 controls the rotation amount of each of the joints 231, 232, 233, 234, 235, and 236 by driving the motor M by servo control that feeds back the output of the encoder E.

[0017] Also, an end effector 24 is attached to the tip of the robot arm 22, that is, the sixth arm 226. The end effector 24 is detachable from the sixth arm 226, and an appropriate one according to the work content to be performed by the robot 2 is attached as appropriate.

[0018] Also, as shown in FIG. 1, the inertial sensor 3 is disposed on the third arm 223. The inertial sensor 3 includes an angular velocity detection element 31a that detects the angular velocity ωs about an axis in the same direction as the second and third rotation axes J2 and J3, and an acceleration detection element 32a that detects the acceleration Azs in a direction intersecting, particularly orthogonal to, the second and third rotation axes J2 and J3. The acceleration Azs is, in detail, the vertical acceleration generated by the turning motion of the third arm 223 caused by the driving of at least one of the second and third joints 232 and 232. By disposing the inertial sensor 3 on the third arm 223, the detection axis of the angular velocity detection element 31a is maintained parallel to the second and third rotation axes J2 and J3 in any posture, so that the angular velocity ωs and the acceleration Azs can be detected more reliably.

[0019] Also, as shown in FIG. 1, the inertial sensor 3 is disposed on the third arm 223. The inertial sensor 3 includes an angular velocity detection element 31a that detects the angular velocity ωs about an axis in the same direction as the second and third rotation axes J2 and J3, and an acceleration detection element 32a that detects the acceleration Azs in a direction intersecting, particularly orthogonal to, the second and third rotation axes J2 and J3. The acceleration Azs is, in detail, the vertical acceleration generated by the turning motion of the third arm 223 caused by the driving of at least one of the second and third joints 232 and 232. By disposing the inertial sensor 3 on the third arm 223, the detection axis of the angular velocity detection element 31a is maintained parallel to the second and third rotation axes J2 and J3, and the detection axis of the acceleration detection element 32a is maintained orthogonal to the second and third rotation axes J2 and J3 in any posture, so that the angular velocity ωs and the acceleration Azs can be detected more reliably.

[0020] As described above, since the inertial sensor 3 is disposed on the third arm 223, it can be said that the inertial sensor 3 is disposed on the arm located between the third joint 233, which is the distal-side joint, and the sixth joint 236, which is located on the most distal side. In other words, it can be said that the inertial sensor 6 is not disposed on the sixth arm 226. Thus, by disposing the inertial sensor 3 while avoiding the arm located at the very tip, the number of joints intervening between the inertial sensor 3 and the third joint 233 can be suppressed to be as small as possible. Therefore, it becomes easier to detect the angular velocity ωs and the acceleration Azs without being affected by the posture of the robot arm 22.

[0021] In the present embodiment, one sensor unit including the angular velocity detection element 31a and the acceleration detection element 32a is used as the inertial sensor 3, but the present invention is not limited to this, and a configuration in which an angular velocity sensor including the angular velocity detection element 31a and an acceleration sensor including the acceleration detection element 32a are separately disposed may be used.

[0022] The control device 4 controls the driving of the robot 2. The control device 4 is composed of, for example, a computer and has a processor (CPU) that processes information, a memory communicably connected to the processor, and an external interface that connects to an external device. Various programs executable by the processor are stored in the memory, and the processor can read and execute the programs and the like stored in the memory.

[0023] The configuration of the robot system 1 has been described above. For example, when the inertial sensor 3 can only detect the acceleration Azs, as shown in FIG. 2, in the posture where the second and third arms 222 and 223 both extend in the horizontal direction, the angular velocity ω2 due to the vibration of the second joint 232 and the angular velocity ω3 due to the vibration of the third joint 233 cannot be distinguished. Also, for example, when the inertial sensor 3 can only detect the angular velocity ωs, as shown in FIG. 3, in the posture where the second arm 222 faces the vertical direction and the third arm 223 faces the horizontal direction and they are orthogonal, the angular velocity ω2 due to the vibration of the second joint 232 and the angular velocity ω3 due to the vibration of the third joint 233 cannot be distinguished. In contrast, in the robot system 1, the inertial sensor 3 can detect the angular velocity ωs and the acceleration Azs, and based on the relationship between the angular velocity ωs and the acceleration Azs, the vibration of the second joint 232 and the vibration of the third joint 233 can be distinguished. The method will be described below.

[0024] As shown in FIG. 4, the control method of the robot 2 includes an inertial information acquisition step S1 of acquiring the angular velocity ωs and the acceleration Azs from the inertial sensor 3, a vibration detection step S2 of detecting the vibration amount of each of the second joint 232 and the third joint 233 based on the angular velocity ωs and the acceleration Azs acquired in the inertial information acquisition step S1, and a drive control step S3 of controlling the vibration of the vibrating joint based on the detection result of the vibration detection step S2. Each of these steps S1 to S3 will be described in detail below.

[0025] - Inertial information acquisition step S1 - In the inertial information acquisition step S1, the control device 4 acquires the angular velocity ωs and the acceleration Azs from the inertial sensor 3.

[0026] - Vibration detection step S2 - In the vibration detection step S2, the control device 4 detects the vibration amount of each of the second joint 232 and the third joint 233 based on the angular velocity ωs and the acceleration Azs acquired in the inertial information acquisition step S1.

[0027] As an example, as shown in FIG. 5, a detection method in a posture where the second arm 222 and the third arm 223 extend straight in the horizontal direction will be described. As described above, in this posture, only the acceleration Azs cannot detect the vibration amount of each joint of the second joint 232 and the third joint 233. Also, as shown in the figure, let the distance between the second rotation axis J2 and the third rotation axis J3 be L2, and the distance between the third rotation axis J3 and the inertial sensor 3 be L3. In the case of FIG. 5, the translational velocity Vzs in the vertical direction of the inertial sensor 3 is represented by Vzs = L×ω. The translational velocity Vzs is calculated by integrating the acceleration Azs detected by the inertial sensor 3. Here, L is the distance between the rotation axis of the joint where vibration occurs and the inertial sensor 3, and ω is the angular velocity around the rotation axis of the joint where vibration occurs.

[0028] As shown in FIG. 6, when the second joint 232 vibrates at an angular velocity ω2, Vzs = (L2 + L3)×ω2 and ω = ω2. On the other hand, as shown in FIG. 7, when the third joint 233 vibrates at an angular velocity ω3, Vzs = L3×ω3 and ω = ω3. Thus, a difference occurs in the relationship between the translational velocity Vzs and the angular velocity ω between the case where the second joint 232 vibrates and the case where the third joint 233 vibrates. Specifically, Vzs / ω when the second joint 232 vibrates is larger than Vzs / ω when the third joint 233 vibrates. Therefore, based on the relationship between the translational velocity Vzs and the angular velocity ω, the vibration amount of each joint of the second joint 232 and the third joint 233 can be discriminated.

[0029] From the above relationships, the following equations (1) and (2) using the Jacobian matrix hold. Therefore, based on the previously measured L2, L3, the angular velocity ωs detected by the inertial sensor 3, and the translational velocity Vzs calculated from the acceleration Azs detected by the inertial sensor 3, the angular velocities ω2 and ω3 can be obtained. Then, from the obtained values of the angular velocities ω2 and ω3, the vibration amount of each joint of the second joint 232 and the third joint 233 can be discriminated.

[0030]

Equation

[0031]

Number

[0032] Also, as another example, as shown in FIG. 8, the second arm 222 faces the vertical direction and the third arm 223 faces the horizontal direction, and the posture in which the second and third arms 222 and 223 are orthogonal will be described. At this time, let the vertical separation distance from the center of the third arm 222 to the detection center of the acceleration detected by the inertial sensor 3 be Lsz. As described above, in this posture, only with the angular velocity ωs, the vibration amounts of the respective joints of the second joint 232 and the third joint 233 cannot be detected.

[0033] As shown in FIG. 9, when the second joint 232 vibrates at an angular velocity ω2, Vzs = L3 × ω2 and ω = ω2. On the other hand, as shown in FIG. 10, when the third joint 233 vibrates at an angular velocity ω3, Vzs = √(L3 2 + Lsz 2 ) × ω3 and ω = ω3. Thus, a difference occurs in the relationship between the translational velocity Vzs and the angular velocity ω between the case where the second joint 232 vibrates and the case where the third joint 233 vibrates. Therefore, based on the relationship between the translational velocity Vzs and the angular velocity ω, the vibration amounts of the respective joints of the second joint 232 and the third joint 233 can be discriminated.

[0034] From the above relationship, the following equations (3) and (4) using the Jacobian matrix hold. Therefore, based on the previously measured L2, L3, Lsz, the angular velocity ωs detected by the inertial sensor 3, and the translational velocity Vzs calculated from the acceleration Azs detected by the inertial sensor 3, the angular velocities ω2 and ω3 can be obtained. Then, from the obtained values of the angular velocities ω2 and ω3, the vibration amounts of the respective joints of the second joint 232 and the third joint 233 can be discriminated.

[0035]

Number

[0036]

Number

[0037] As described above, for two representative postures where it is impossible to determine which of the second joint 232 and the third joint 233 is vibrating with only one of the angular velocity ωs and the acceleration Azs, it is needless to say that even for postures other than these, based on the relationship between the angular velocity ωs and the acceleration Azs, the vibration amounts of the respective joints of the second joint 232 and the third joint 233 can be determined.

[0038] In the robot 2, the following equations (5) and (6) using the Jacobian matrix J hold for any posture. The Jacobian matrix J indicates the position of the inertial sensor 3 and has different values according to the rotation amounts of the second and third joints 232 and 233. The position of the inertial sensor 3 is detected based on, for example, the outputs of the encoders E provided in the second and third joints 232 and 233. Therefore, the angular velocities ω2 and ω3 can be obtained based on the previously measured separation distances L2, L3, Lsz, the angular velocity ωs detected by the inertial sensor 3, and the translational velocity Vzs calculated from the acceleration Azs detected by the inertial sensor 3. Then, based on the obtained values of the angular velocities ω2 and ω3, the vibration amounts of the respective joints of the second joint 232 and the third joint 233 can be determined.

[0039]

Number

[0040]

Number

[0041] According to the method as described above, it is possible to determine the amount of vibration of each of the second joint 232 and the third joint 233. In particular, based on the pre-measured separation distances L2, L3, LSZ, the angular velocity ωs detected by the inertial sensor 3, and the translational velocity Vzs calculated from the acceleration Azs detected by the inertial sensor 3, it is possible to determine the amount of vibration of each of the second joint 232 and the third joint 233, so that the detection can be easily performed.

[0042] -Drive control step S3- In the drive control step S3, the control device 4 controls the drive of the robot arm 22 based on the detection result of the vibration detection step S2. Specifically, the control device 4 controls the vibration generated in the second joint 232 and the third joint 233 based on the amount of vibration of each joint determined in the vibration detection step S2. In this way, by controlling the vibration based on the amount of vibration of each joint, more appropriate control can be performed, and the vibration can be reduced more reliably and effectively.

[0043] For example, taking an example of the control for the second joint 232, as shown in FIG. 11, the control device 4 has a control unit 40 that controls the drive of the second joint 232. The control unit 40 includes a position command generation unit 41, a position control unit 42, a speed control unit 43, a current control unit 44, and a vibration feedback generation unit 45.

[0044] The vibration feedback generation unit 45 multiplies the angular velocity ω obtained by discrimination in step S2 based on the angular velocity ωs detected by the inertial sensor 3 and the acceleration Azs detected by the inertial sensor 3 by the arm angular velocity scaling coefficient Kgs to obtain the motor shaft conversion arm angular velocity 912. Further, the vibration feedback generation unit 45 obtains the motor shaft angular velocity 913 by differentiating the motor shaft position 902, which is the rotation angle of the motor M detected by the encoder E, with respect to time. Next, the vibration feedback generation unit 45 subtracts the motor shaft angular velocity 913 from the motor shaft conversion arm angular velocity 912 to obtain the vibration angular velocity 914. Next, the vibration feedback generation unit 45 multiplies the vibration angular velocity 914 by the feedback gain Kgp to obtain the vibration feedback 915.

[0045] The position command generation unit 41 generates the position command 901 of the motor M based on the program created by the host computer. The position control unit 42 first obtains the position deviation 903 by subtracting the motor shaft position 902 detected by the encoder E from the position command 901. Next, the position control unit 42 multiplies the position deviation 903 by the position loop proportional gain Kpp to obtain the speed command 904.

[0046] The speed control unit 43 is composed of proportional-integral control. The speed control unit 43 first adds the speed command 904 and the vibration feedback 915 generated by the vibration feedback generation unit 45 to obtain the speed loop command 905. Next, the speed control unit 43 obtains the current command 906 by adding the integral term obtained by multiplying the integral value of the speed loop command 905 by the speed loop integral gain Kvi to the proportional term obtained by multiplying the speed loop command 905 by the speed loop proportional gain Kvp.

[0047] The current control unit 44 controls the current 907 that drives the motor M so that it matches the current command 906, that is, so that the current 907 follows the current command 906. Then, the motor M is driven by the current 907 controlled by the current control unit 44.

[0048] An example of control has been described above. Next, several examples of the arrangement of the inertial sensor 3 will be described. For example, as shown in FIG. 12, in a plan view from a direction orthogonal to the vertical direction, that is, the third rotation axis J3 and the central axis A of the third arm 223, the inertial sensor 3 is located on the central axis A of the third arm 223. In the case of the robot system 1, the central axis A is orthogonal to the third rotation axis J3 and parallel to the fourth rotation axis J4. In other words, the central axis A is parallel to the extending directions of the third arm and the fourth arm. According to such an arrangement, the arrangement of the inertial sensor 3 becomes easy. Further, the inertial sensor 3 is arranged at the end on the side of the fourth joint 234, that is, the end on the side opposite to the third joint 233. Thereby, since the inertial sensor 3 can be arranged as far as possible from the third rotation axis J3, a larger acceleration Azs can be detected.

[0049] Also, for example, as shown in FIG. 13, when a motor M as a drive source is arranged in the third arm 223, the inertial sensor 3 is arranged at a distance from the motor M. Thereby, it becomes difficult for the vibration generated by the drive of the motor M to be transmitted to the inertial sensor 3, and the angular velocity ωs and the acceleration Azs can be accurately detected by the inertial sensor 3. Note that the motor M arranged in the third arm 223 may be a motor M provided in any joint. For example, it may be a motor M provided in the third joint 233 or a motor M provided in the fourth joint 234.

[0050] Also, for example, as shown in FIG. 14, the inertial sensor 3 is disposed within the third arm 223. Thereby, the inertial sensor 3 can be protected from moisture, dust, and the like. Further, the inertial sensor 3 is disposed on the wall portion of the third arm 223. Specifically, the third arm 223 has a housing 223a connected to other arms and a cover 223b attached to the housing 223a, and the inertial sensor 3 is disposed on the wall portion of the cover 223b. Since a speed reducer and a motor M are fixed to the housing 223a, by disposing the inertial sensor 3 on the cover 223b, vibrations generated from the speed reducer and the motor M are less likely to be transmitted to the inertial sensor 3. Therefore, the angular velocity ωs and the acceleration Azs can be accurately detected by the inertial sensor 3.

[0051] Also, for example, as shown in FIG. 15, the wall portion, that is, the cover 223b has an inner wall 223c, an outer wall 223d, and a hollow portion 223e located between the inner wall 223c and the outer wall 223d, and the inertial sensor 3 is disposed in the hollow portion 223e. In this way, by accommodating the inertial sensor 3 within the cover 223b, the inertial sensor 3 can be protected from moisture, dust, and the like. Further, the inertial sensor 3 is disposed on the outer wall 223d. Thereby, vibrations generated from the speed reducer and the motor M are less likely to be transmitted to the inertial sensor 3. Therefore, the angular velocity ωs and the acceleration Azs can be accurately detected by the inertial sensor 3.

[0052] The robot system 1 has been described above. As described above, the control method of the robot 2 performed by such a robot system 1 includes a robot 2 including a robot arm 22 having a second joint 232 which is a base end side joint having rotation axes parallel to each other and a third joint 233 which is a tip end side joint located on the tip end side of the second joint 232, and an inertial sensor 3 disposed on the tip end side of the third joint 233 of the robot arm 22 and detecting an angular velocity ω about an axis parallel to the second and third rotation axes J2 and J3 which are the rotation axes of the second and third joints 232 and 233 and an acceleration Azs in a direction intersecting the second and third rotation axes J2 and J3. In the robot system 1, an inertial information acquisition step S1 of acquiring the angular velocity ωs and the acceleration Azs from the inertial sensor 3, a vibration detection step S2 of detecting the vibration amount of each joint of the second joint 232 and the third joint 233 based on the angular velocity ωs and the acceleration Azs acquired in the inertial information acquisition step S1, and a drive control step S3 of controlling vibration based on the vibration amount detected in the vibration detection step S2 are included. Thus, by controlling vibration based on the vibration amount of each joint, more appropriate control can be performed, and vibration can be reduced more reliably and effectively.

[0053] Also, as described above, the robot system 1 includes a robot 2 including a robot arm 22 having a second joint 232 which is a base end side joint having rotation axes parallel to each other and a third joint 233 which is a tip end side joint located on the tip end side of the second joint 232, an inertial sensor 3 disposed on the tip end side of the third joint 233 of the robot arm 22 and detecting an angular velocity ωs about an axis parallel to the second and third rotation axes J2 and J3 which are the rotation axes of the second and third joints 232 and 233 and an acceleration Azs in a direction intersecting the second and third rotation axes J2 and J3, and a control device 4 which detects the vibration amount of each joint of the second joint 232 and the third joint 233 based on the angular velocity ωs and the acceleration Azs detected by the inertial sensor 3 and controls vibration based on the detected vibration amount. Thus, by controlling vibration based on the vibration amount of each joint, more appropriate control can be performed, and vibration can be reduced more reliably and effectively.

[0054] Also, as described above, the control device 4 detects the amount of vibration of each of the second joint 232 and the third joint 233 based on the separation distance L2 between the second rotation axis J2 of the second joint 232 and the third rotation axis J3 of the third joint 233 and the separation distance L3 between the third rotation axis J3 of the third joint 233 and the inertial sensor 3. Thereby, the amount of vibration of each of the second joint 232 and the third joint 233 can be detected by a simple method.

[0055] Also, as described above, the robot 2 has a base 21, and the robot arm 22 is connected to the base 21 and includes a first arm 221 that rotates with respect to the base 21, a second arm 222 that is connected to the first arm 221 via a second joint 232, and a third arm 223 that is connected to the second arm 222 via a third joint 233. And the inertial sensor 3 is disposed on the third arm 223. According to such a configuration, in any posture, since the detection axis of the angular velocity detection element 31a is maintained parallel to the second and third rotation axes J2 and J3, the angular velocity ωs and the acceleration Azs can be detected more reliably.

[0056] Also, as described above, the inertial sensor 3 is located on the central axis A of the third arm 223 in a plan view from a direction orthogonal to the second and third rotation axes J2 and J3 and the central axis A of the third arm 223. According to such an arrangement, the arrangement of the inertial sensor 3 becomes easy.

[0057] Also, as described above, the inertial sensor 3 is disposed at an end of the third arm 223 on the side opposite to the third joint 233. Thereby, since the inertial sensor 3 can be disposed as far as possible from the third rotation axis J3, a larger acceleration Azs can be detected.

[0058] Also, as described above, the robot 2 has a motor M as a drive source that is disposed within the third arm 223 and drives the robot arm 22. And the inertial sensor 3 is disposed at a distance from the motor M. Thereby, vibrations generated by the driving of the motor M are less likely to be transmitted to the inertial sensor 3, and the angular velocity ωs and the acceleration Azs can be accurately detected by the inertial sensor 3.

[0059] Also, as described above, the inertial sensor 3 is disposed within the third arm 223. Thereby, the inertial sensor 3 can be protected from moisture, dust, and the like.

[0060] Also, as described above, the inertial sensor 3 is disposed on the cover 223b which is a wall portion of the third arm 223. Thereby, vibrations generated from the speed reducer and the motor M are less likely to be transmitted to the inertial sensor 3. Therefore, the angular velocity ωs and the acceleration Azs can be accurately detected by the inertial sensor 3.

[0061] Also, as described above, the cover 223b which is a wall portion has an inner wall 223c, an outer wall 223d, and a hollow portion 223e located between the inner wall 223c and the outer wall 223d. And the inertial sensor 3 is disposed within the hollow portion 223e. Thereby, the inertial sensor 3 can be protected from moisture, dust, and the like.

[0062] <Second Embodiment> FIG. 16 is a schematic diagram showing a robot according to the second embodiment. FIG. 17 is a diagram showing the arrangement of the inertial sensor.

[0063] The robot system 1 according to the present embodiment is the same as the robot system 1 of the first embodiment described above, except that the configuration and arrangement of the inertial sensor 3 are different. In the following description, regarding the robot system 1 of the present embodiment, the differences from the first embodiment described above will be mainly described, and the description of the same matters will be omitted. Also, in each drawing of the present embodiment, the same reference numerals are given to the same configurations as those of the above-described embodiment.

[0064] As shown in FIG. 16, in the robot system 1 of this embodiment, an inertial sensor 3 is disposed on the fourth arm 224. By disposing the inertial sensor 3 on the fourth arm 224 in this way, the inertial sensor 3 can be disposed at a greater distance from the second and third rotation axes J2 and J3, so that a larger acceleration Azs can be detected. Therefore, the amount of vibration of each of the second joint 232 and the third joint 233 can be determined more accurately.

[0065] In such a configuration, as the fourth joint 234 rotates, the detection axis of the inertial sensor 3 rotates with respect to the second and third rotation axes J2 and J3. Therefore, in a configuration having one angular velocity detection element 31a and one acceleration detection element 32a as in the inertial sensor 3 of the first embodiment described above, depending on the orientation of the fourth joint 234, the angular velocity ωs and the acceleration Azs cannot be detected.

[0066] Therefore, the inertial sensor 3 of this embodiment includes, in addition to the angular velocity detection element 31a, an angular velocity detection element 31b having a detection axis that intersects, particularly orthogonally, the detection axis of the angular velocity detection element 31a and the fourth rotation axis J4. Further, the inertial sensor 3 of this embodiment includes, in addition to the acceleration detection element 32a, an acceleration detection element 32b having a detection axis that intersects, particularly orthogonally, the detection axis of the acceleration detection element 32a and the fourth rotation axis J4. That is, the inertial sensor 3 can detect the angular velocity about two axes that are orthogonal to the fourth rotation axis J4 and orthogonal to each other, and the acceleration in two axial directions that are orthogonal to the fourth rotation axis J4 and orthogonal to each other. According to such a configuration, regardless of the orientation of the fourth joint 234, the angular velocity ωs can be detected based on the angular velocities detected by the angular velocity detection elements 31a and 31b, and the acceleration Azs can be detected based on the accelerations detected by the acceleration detection elements 32a and 32b.

[0067] Also, as shown in FIG. 17, the inertial sensor 3 is disposed on the fourth rotation axis J4 of the fourth arm 224. Therefore, regardless of the orientation of the fourth arm 224, the positional relationship between the second and third rotation axes J2, J3 and the inertial sensor 3 is kept constant. Thus, the calculation of the angular velocity ωs and the acceleration Azs becomes easy.

[0068] As described above, the robot 2 of this embodiment has a base 21, and the robot arm 22 is connected to the base 21 and includes a first arm 221 that rotates with respect to the base 21, a second arm 222 that is connected to the first arm 221 via a second joint 232, a third arm 223 that is connected to the second arm 222 via a third joint 233, and a fourth arm 224 that is connected to the third arm 223 and rotates about a fourth rotation axis J4 that intersects the third rotation axis J3 with respect to the third arm 223. An inertial sensor 3 is disposed on the fourth arm 224, and the inertial sensor 3 detects the angular velocity about two axes that intersect the fourth rotation axis J4 and intersect each other, and the acceleration in the directions of two axes that intersect the fourth rotation axis J4 and intersect each other. According to such a configuration, regardless of the orientation of the fourth joint 234, the angular velocity ωs and the acceleration Azs can be detected. Further, since the inertial sensor 3 can be disposed at a greater distance from the second and third rotation axes J2, J3, a greater acceleration Azs can be detected. Therefore, the amount of vibration of each of the joints of the second joint 232 and the third joint 233 can be determined more accurately.

[0069] Also, as described above, the inertial sensor 3 is located on the fourth rotation axis J4 that is the rotation axis of the fourth arm 224. Therefore, regardless of the orientation of the fourth arm 224, the positional relationship between the second and third rotation axes J2, J3 and the inertial sensor 3 is kept constant. Thus, the calculation of the angular velocity ωs and the acceleration Azs becomes easy.

[0070] Also by such a second embodiment, the same effects as those of the first embodiment described above can be exhibited.

[0071] <Third Embodiment> FIG. 18 is a schematic diagram showing a robot according to the third embodiment.

[0072] The robot system 1 according to the present embodiment is the same as the robot system 1 of the first embodiment described above, except that the configuration of the inertial sensor 3 is different. In the following description, regarding the robot system 1 of the present embodiment, the differences from the first embodiment described above will be mainly described, and the description of the same matters will be omitted. Also, in the drawings of the present embodiment, the same reference numerals are given to the same configurations as those in the above-described embodiments.

[0073] For example, in the case of the posture where the second and third arms 222 and 223 are orthogonal as shown in FIG. 9, the acceleration Azs generated by the vibration of the second joint 232 is small, and the translational speed Vzs is also small. Therefore, there is a possibility that the vibration amounts of the respective joints of the second joint 232 and the third joint 233 cannot be accurately determined.

[0074] Therefore, as shown in FIG. 18, the inertial sensor 3 of the present embodiment further includes an acceleration detection element 32b having a detection axis that intersects, particularly orthogonally, the detection axis of the acceleration detection element 32a and the second and third rotation axes J2 and J3 in addition to the acceleration detection element 32a. According to such a configuration, the detection axis of the acceleration detection element 32a substantially coincides with the direction of the acceleration generated by the vibration of the second joint 232, and a larger acceleration can be detected by the acceleration detection element 32b. Therefore, the vibration amounts of the respective joints of the second joint 232 and the third joint 233 can be accurately determined.

[0075] In the case of FIG. 18, the translational speed Vzs in the vertical direction of the inertial sensor 3 is Vzs = L2×ω2 + √(L3 2 + Lsz 2 )×ω3, and the translational speed Vxs in the horizontal direction is represented by Vxs = L2×ω2. Therefore, the following formula (7) holds. And from the values of the angular velocities ω2 and ω3 obtained by a pseudo-inverse matrix or the like based on formula (7), the vibration amounts of the respective joints of the second joint 232 and the third joint 233 can be determined.

[0076] [Number]

[0077] Also by such a third embodiment, the same effects as those of the first embodiment described above can be exhibited.

[0078] <Fourth Embodiment> FIG. 19 is a schematic diagram showing a robot according to the fourth embodiment.

[0079] The robot system 1 according to the present embodiment is the same as the robot system 1 of the first embodiment described above, except that the configuration of the inertial sensor 3 is different. In the following description, regarding the robot system 1 of the present embodiment, the differences from the first embodiment described above will be mainly described, and the description of the same matters will be omitted. Also, in the drawings of the present embodiment, the same components as those of the above-described embodiments are denoted by the same reference numerals.

[0080] In the robot system 1 of the present embodiment, the inertial sensor 3 is configured to be able to detect vibrations around the first rotation axis J1 of the first joint 231. Specifically, as shown in FIG. 19, the inertial sensor 3 of the present embodiment further includes angular velocity detection elements 31c and 31d having detection axes that intersect, particularly orthogonal to, the detection axis of the angular velocity detection element 31a and intersect each other, particularly orthogonally, in addition to the angular velocity detection element 31a. According to such a configuration, vibrations around the first rotation axis J1 of the first joint 231 can be detected based on the angular velocities detected by the angular velocity detection elements 31c and 31d regardless of the orientations of the second and third joints 232 and 233. For convenience of explanation, the acceleration detection element 32a is not shown.

[0081] Also by such a fourth embodiment, the same effects as those of the first embodiment described above can be exhibited.

[0082] <Fifth Embodiment> FIG. 20 is a schematic diagram showing a robot according to the fifth embodiment.

[0083] The robot system 1 according to this embodiment is the same as the robot system 1 of the first embodiment described above, except that the configuration of the robot 2 and the arrangement of the inertial sensor 3 corresponding thereto are different. In the following description, regarding the robot system 1 of this embodiment, the differences from the first embodiment described above will be mainly described, and the description of the same matters will be omitted. Also, in the drawings of this embodiment, the same reference numerals are given to the configurations that are the same as those of the above-described embodiments.

[0084] As shown in FIG. 20, the robot 2 of this embodiment is a 7-axis vertical articulated robot having seven drive axes. Further, the robot arm 22 has a configuration in which the first, second, third, fourth, fifth, sixth, and seventh arms 221, 222, 223, 224, 225, 226, and 227 are connected via the first, second, third, fourth, fifth, sixth, and seventh joints 231, 232, 233, 234, 235, 236, and 237.

[0085] Specifically, the first arm 221 is rotatably connected to the base 21 via the first joint 231 about the first rotation axis J1. Further, the second arm 222 is rotatably connected to the first arm 221 via the second joint 232 about the second rotation axis J2. Further, the third arm 223 is rotatably connected to the second arm 222 via the third joint 233 about the third rotation axis J3. Further, the fourth arm 224 is rotatably connected to the third arm 223 via the fourth joint 234 about the fourth rotation axis J4. Further, the fifth arm 225 is rotatably connected to the fourth arm 224 via the fifth joint 235 about the fifth rotation axis J5. Further, the sixth arm 226 is rotatably connected to the fifth arm 225 via the sixth joint 236 about the sixth rotation axis J6. Further, the seventh arm 227 is rotatably connected to the sixth arm 226 via the seventh joint 237 about the seventh rotation axis J7.

[0086] Among the first, second, third, fourth, fifth, sixth, and seventh joints 231, 232, 233, 234, 235, 236, and 237, the second, fourth, and sixth joints 232, 234, and 236 are bending joints, respectively, and the first, third, fifth, and seventh joints 231, 233, 235, and 237 are torsion joints, respectively. Also, the second rotation axis J2 is orthogonal to the first rotation axis J1, the third rotation axis J3 is orthogonal to the second rotation axis J2, the fourth rotation axis J4 is orthogonal to the third rotation axis J3, the fifth rotation axis J5 is orthogonal to the fourth rotation axis J4, the sixth rotation axis J6 is orthogonal to the fifth rotation axis J5, and the seventh rotation axis J7 is orthogonal to the sixth rotation axis J6.

[0087] Also, an inertial sensor 3 is disposed on the third arm 223. The inertial sensor 3 can detect the angular velocity ωs about an axis parallel to the third rotation axis J3 and the acceleration Axs in a direction orthogonal to the third rotation axis J3.

[0088] In such a robot 2, the first joint 231 is the "base-end side joint" of the present application, and the third joint 233 is the "tip-end side joint" of the present application. That is, in the robot 2, the second joint 232 is located between the base-end side joint and the tip-end side joint. In the robot 2, as shown in FIG. 20, when the second joint 232 is in a predetermined orientation, the first rotation axis J1 of the first joint 231, which is the base-end side joint, and the third rotation axis J3 of the third joint 233, which is the tip-end side joint, are parallel. Therefore, in the same manner as in the first embodiment described above, the vibration amounts of the respective joints of the first joint 231 and the third joint 233 can be detected.

[0089] Also, such a fifth embodiment can exhibit the same effects as those of the first embodiment described above.

[0090] <Sixth Embodiment> FIG. 21 is a diagram showing a robot according to the sixth embodiment.

[0091] The robot system 1 according to this embodiment is the same as the robot system 1 of the first embodiment described above, except that the configuration of the robot 5 and the arrangement of the inertial sensor 3 corresponding thereto are different. In the following description, regarding the robot system 1 of this embodiment, the description will focus on the differences from the first embodiment described above, and the description of the same matters will be omitted. Also, in the drawings of this embodiment, the same reference numerals are given to the same configurations as those of the above-described embodiments.

[0092] As shown in FIG. 21, the robot 5 of this embodiment is a horizontal articulated robot (scalar robot). Such a robot 5 has a base 51 and a robot arm 52 connected to the base 51. Further, the robot arm 52 has a configuration in which first and second arms 521 and 522 are connected via first and second joints 541 and 542.

[0093] The first arm 521 is rotatably connected to the base 21 about a first rotation axis J11 via the first joint 541. Further, the second arm 522 is rotatably connected to the first arm 521 about a second rotation axis J12 via the second joint 542. Also, these first and second joints 541 and 542 are torsion joints, respectively, and the first and second rotation axes J11 and J12 are parallel and along the vertical direction with respect to each other.

[0094] Further, a working head 53 is provided at the tip of the second arm 522. The working head 53 includes a spline nut 531 and a ball screw nut 532 coaxially arranged at the tip of the second arm 522, and a spline shaft 533 which is a main shaft inserted through the spline nut 531 and the ball screw nut 532. The spline shaft 533 is rotatable about a third rotation axis J13 which is its central axis and extends along the vertical direction with respect to the second arm 522, and is movable (vertically) along the third rotation axis J13. Further, a mounting portion 533a for mounting an end effector (not shown) is provided at the lower end of the spline shaft 533. The end effector is detachable from the mounting portion 533a, and an appropriate one suitable for the intended work is selected as appropriate. The third rotation axis J13 extends along the vertical direction and is parallel to the first and second rotation axes J11 and J12.

[0095] In such a robot 5, the first joint 541 is the "base end side joint" of the present application, and the second joint 542 is the "tip end side joint" of the present application.

[0096] Each of the joints 541 and 542 also includes a motor M, a speed reducer (not shown) that reduces the rotational speed of the motor M and outputs it, and an encoder E that detects the rotation amount of the motor M.

[0097] Also, as shown in FIG. 21, the inertial sensor 3 is arranged on the second arm 522. The inertial sensor 3 also includes an angular velocity detection element 31a that detects the angular velocity ωs about an axis in the same direction as the first and second rotation axes J11 and J12, and an acceleration detection element 32a that detects the acceleration Axs in a direction orthogonal to the first and second rotation axes J11 and J12.

[0098] Even in the robot system 1 configured as described above, the vibration amounts of the respective joints of the first joint 541 and the second joint 542 can be detected in the same manner as in the first embodiment described above.

[0099] Also, according to such a sixth embodiment, the same effects as those of the first embodiment described above can be exhibited.

[0100] The control method of the robot and the robot system of the present invention have been described with respect to the illustrated embodiments. However, the present invention is not limited thereto, and the configuration and process of each part can be replaced with any configuration and process having the same function. Further, any other arbitrary configuration and process may be added to the present invention. Also, the embodiments may be combined as appropriate.

Description of Reference Numerals

[0101] 1…Robot system, 2…Robot, 21…Base, 22…Robot arm, 221…First arm, 222…Second arm, 223…Third arm, 223a…Housing, 223b…Cover, 223c…Inner wall, 223d…Outer wall, 223e…Hollow part, 224…Fourth arm, 225…Fifth arm, 226…Sixth arm, 227…Seventh arm, 231…First joint, 232…Second joint, 233…Third joint, 234…Fourth joint, 235…Fifth joint, 236…Sixth joint, 237…Seventh joint, 24…End effector, 3…Inertial sensor, 31a…Angular velocity detection element, 31b…Angular velocity detection element, 31c…Angular velocity detection element, 31d…Angular velocity detection element, 32a…Acceleration detection element, 32b…Acceleration detection element, 4…Control device, 40…Control unit, 41…Position command generation unit, 42…Position control unit, 43…Velocity control unit, 44…Current control unit, 45…Vibration feedback generation unit, 5…Robot, 51…Base, 52…Robot arm, 521…First arm, 522…Second arm, 53…Working head, 531…Spline nut, 532…Ball screw nut, 533…Spline shaft, 533a…Mounting part, 541…First joint, 542…Second joint, 901…Position command, 902…Motor shaft position, 903…Position deviation, 904…Velocity command, 905…Velocity loop command, 906…Current command, 907…Current, 912…Arm angular velocity converted to motor shaft, 913…Motor shaft angular velocity, 914…Vibration angular velocity, 915…Vibration feedback, A…Central axis, Axs…Acceleration, Azs…Acceleration, E…Encoder, J1…First rotation axis, J2…Second rotation axis, J3…Third rotation axis, J4…Fourth rotation axis, J5…Fifth rotation axis, J6…Sixth rotation axis, J7…Seventh rotation axis, J11…First rotation axis, J12…Second rotation axis, J13…Third rotation axis, Kgp…Feedback gain, Kgs…Arm angular velocity scaling coefficient, Kpp…Position loop proportional gain, Kvi…Velocity loop integral gain, Kvp…Velocity loop proportional gain, L2…Separation distance, L3…Separation distance, M…Motor, S1…Step of obtaining inertial information, S2…Step of detecting vibration, S3…Step of drive control, Vxs…Translation velocity, Vzs…Translation velocity, ω…Angular velocity, ω2…Angular velocity, ω3…Angular velocity, ωs…Angular velocity

Claims

1. A robot comprising a robot arm having a base-end joint with rotation axes parallel to each other and a tip-end joint located on the tip-end side of the base-end joint, and an inertial sensor disposed on the tip-end side of the tip-end joint of the robot arm, for detecting an angular velocity about an axis parallel to the rotation axis and an acceleration in a direction intersecting the rotation axis, in a robot system, an inertial information acquisition step of acquiring the angular velocity and the acceleration from the inertial sensor; a vibration detection step of detecting a vibration amount of each of the base-end joint and the tip-end joint based on the angular velocity and the acceleration acquired in the inertial information acquisition step; and a drive control step of controlling vibration based on the vibration amount detected in the vibration detection step. A method for controlling a robot, characterized by including these steps.

2. A robot comprising a robot arm having a base-end joint with rotation axes parallel to each other and a tip-end joint located on the tip-end side of the base-end joint, and an inertial sensor disposed on the tip-end side of the tip-end joint of the robot arm, for detecting an angular velocity about an axis parallel to the rotation axis and an acceleration in a direction intersecting the rotation axis, and a control device that detects a vibration amount of each of the base-end joint and the tip-end joint based on the angular velocity and the acceleration detected by the inertial sensor, and controls vibration based on the detected vibration amount. A robot system, characterized by including these components.

3. The robot system according to claim 2, wherein the control device detects a vibration amount of each of the base-end joint and the tip-end joint based on a distance between the rotation axis of the base-end joint and the rotation axis of the tip-end joint and a distance between the rotation axis of the tip-end joint and the inertial sensor.

4. The robot has a base, the robot arm has a first arm connected to the base and rotatable with respect to the base, a second arm connected to the first arm via the base-end joint, and a third arm connected to the second arm via the tip-end joint, and the robot system according to claim 2, wherein the inertial sensor is disposed on the third arm.

5. The robot system according to claim 4, wherein the inertial sensor is located on the central axis of the third arm in a plan view from a direction orthogonal to the rotation axis and the central axis of the third arm.

6. The robot system according to claim 4, wherein the inertial sensor is disposed at an end of the third arm opposite to the tip-side joint of the third arm.

7. The robot has a drive source disposed within the third arm for driving the robot arm, The robot system according to claim 4, wherein the inertial sensor is disposed at a distance from the drive source.

8. The robot system according to claim 4, wherein the inertial sensor is disposed within the third arm.

9. The robot system according to claim 4, wherein the inertial sensor is disposed on a wall portion of the third arm.

10. The wall portion has an inner wall, an outer wall, and a hollow portion located between the inner wall and the outer wall, The robot system according to claim 9, wherein the inertial sensor is disposed in the hollow portion.

11. The robot has a base, The robot arm includes a first arm connected to the base and rotatable with respect to the base, a second arm connected to the first arm via the base-end side joint, a third arm connected to the second arm via the tip-side joint, and a fourth arm connected to the third arm and rotatable about a rotation axis intersecting the rotation axis of the third arm. The inertial sensor is disposed on the fourth arm, The robot system according to claim 2, wherein the inertial sensor detects angular velocities about two axes that intersect the rotation axis of the fourth arm and intersect each other, and accelerations in two axial directions that intersect the rotation axis of the fourth arm and intersect each other.

12. The robot system according to claim 11, wherein the inertial sensor is located on the rotation axis of the fourth arm.

Images