Robot and robot control method

Abstract

To provide a robot and a robot control method capable of suppressing shaft vibration. [Solution] The robot comprises a robot arm, a shaft that moves linearly along the central axis relative to the robot arm, a hand attached to the shaft, a first inertia sensor that measures the inertia in the direction along the central axis of the shaft, a second inertia sensor that measures the inertia in the direction perpendicular to the central axis of the shaft, a first drive unit that moves the shaft along the central axis, and a control unit that controls the drive of the first drive unit. The control unit also performs feedback control on the first drive unit based on the output of the first inertia sensor, determines whether the vibration of the shaft is normal or abnormal based on the output of the second inertia sensor, and sets different conditions for feedback control depending on whether the vibration is abnormal or normal.

Description

Technical Field

[0001] The present invention relates to a robot and a robot control method.

Background Art

[0002] The robot described in Patent Document 1 has a multi-functional arm including a first arm, a second arm rotatably connected via a first connecting portion having a first motor at the tip of the first arm, and an operating tool rotatably connected via a second connecting portion having a second motor at the tip of the second arm. Further, a first vibration sensor is disposed on the first arm, and a second vibration sensor is disposed on the second arm. Then, vibration control is performed to suppress the occurrence of vibration generated in the multi-functional arm by controlling the driving of the first and second motors based on the outputs of the first and second vibration sensors.

Prior Art Documents

Patent Documents

[0003]

Patent Document 1

Summary of the Invention

Problems to be Solved by the Invention

[0004] However, in the configuration of Patent Document 1, the vibration at the tip of the multi-functional arm may not be ideally suppressed. Further, in the configuration of Patent Document 1, it is impossible to determine whether the vibration at the tip of the multi-functional arm is suppressed by vibration control.

Means for Solving the Problems

[0005] The robot of the present invention includes a robot arm, a shaft supported by the robot arm and linearly moving along a central axis with respect to the robot arm, a hand attached to the shaft, A first inertial sensor for measuring the inertia of the shaft in the direction along the central axis, A second inertial sensor that measures the inertia of the shaft in a direction perpendicular to the central axis, A first drive unit moves the shaft along the central axis relative to the robot arm, The system includes a control unit that controls the drive of the preceding first drive unit, The control unit, Based on the output of the first inertial sensor, feedback control is performed on the first drive unit. Based on the output of the second inertial sensor, it is determined whether the vibration of the shaft is normal or abnormal, and the conditions for the feedback control are made different depending on whether the vibration is abnormal or normal.

[0006] The robot control method of the present invention comprises a robot arm and a robot arm. A shaft supported by the robot arm and moving linearly along the central axis relative to the robot arm, A hand attached to the aforementioned shaft, A first inertial sensor for measuring the inertia of the shaft in the direction along the central axis, A second inertial sensor that measures the inertia of the shaft in a direction perpendicular to the central axis, A robot control method for controlling a robot body having a first drive unit that moves the shaft along the central axis relative to the robot arm, Based on the output of the first inertial sensor, feedback control is performed on the first drive unit. Based on the output of the second inertial sensor, it is determined whether the vibration of the shaft is normal or abnormal, and the conditions for the feedback control are made different depending on whether the vibration is abnormal or normal. [Brief explanation of the drawing]

[0007] [Figure 1] This is an overall view of the robot according to the first embodiment. [Figure 2] This is a cross-sectional view showing the work head located on the second arm. [Figure 3] This is a cross-section of the robot. [Figure 4] This is a cross-section of the robot. [Figure 5] This is a block diagram showing how to control the robot body using a control device. [Figure 6] This flowchart shows how the robot body is controlled by the control device. [Figure 7] This is a cross-sectional view showing the tip of the robot body according to the third embodiment. [Figure 8] This is a block diagram showing how to control the robot body using a control device. [Figure 9] This is a block diagram showing how to control the robot body using a control device. [Figure 10] This is a block diagram showing how to control the robot body using a control device. [Figure 11] This flowchart shows how the robot body is controlled by the control device. [Modes for carrying out the invention]

[0008] The robot and robot control method of the present invention will be described in detail below based on embodiments shown in the accompanying drawings.

[0009] Figure 1 is an overall view of the robot according to the first embodiment. Figure 2 is a cross-sectional view showing the work head located on the second arm. Figures 3 and 4 are cross-sectional views of the robot, respectively. Figure 5 is a block diagram showing the method of controlling the robot body by the control device. Figure 6 is a flowchart showing the method of controlling the robot body by the control device.

[0010] In addition, the vertical direction in FIG. 1 coincides with the vertical direction, and the upper side in FIG. 1 is also referred to as "upper" and the lower side as "lower". Further, in this specification, "vertical" means not only when it coincides with the vertical, but also when it is inclined with respect to the vertical within the range where the effects of the present invention can be exhibited. Also, in this specification, "parallel" means not only when two objects coincide with parallel, but also when they are inclined from parallel within the range where the effects of the present invention can be exhibited. Further, in this specification, "orthogonal" means not only when two objects coincide with orthogonal, but also when they are inclined from orthogonal within the range where the effects of the present invention can be exhibited.

[0011] The robot 1 shown in FIG. 1 includes a robot body 2 and a control device 10 as a control unit that controls the drive of the robot body 2.

[0012] The robot body 2 is a horizontal articulated robot, that is, a scalar robot. However, the robot body 2 may be a robot provided with a shaft other than a scalar robot.

[0013] As shown in FIG. 1, the robot body 2 includes a base 21 and a robot arm 22 connected to the base 21. Further, the robot arm 22 includes a first arm 221 whose base end is connected to the base 21 and rotates around a first rotation axis J1 along the vertical direction with respect to the base 21, and a second arm 222 whose base end is connected to the tip end of the first arm 221 and rotates around a second rotation axis J2 along the vertical direction with respect to the first arm 221.

[0014] Furthermore, a work head 23 is provided at the tip of the second arm 222. As shown in Figure 2, the work head 23 has a spline nut 231 and a ball screw nut 232 arranged coaxially at the tip of the second arm 222, and a spline shaft 233, which is the main shaft axis, inserted through the spline nut 231 and the ball screw nut 232. In such a work head 23, the rotation of the spline nut 231 causes the spline shaft 233 to rotate around a third pivot axis J3, which is its central axis and runs vertically, and also moves linearly (up and down) along the third pivot axis J3. The rotation of the ball screw nut 232 causes the spline shaft 233 to move linearly along the third pivot axis J3, and the rotation of both the spline nut 231 and the ball screw nut 232 causes the spline shaft 233 to rotate around the third pivot axis J3.

[0015] Furthermore, as shown in Figure 1, a hand 4 (end effector) is attached to the lower end of the spline shaft 233. The hand 4 is detachable from the spline shaft 233, and the appropriate one can be selected for the intended work.

[0016] Furthermore, as shown in Figure 1, the robot body 2 includes a first joint drive unit 251 that rotates the first arm 221 around the first pivot axis J1 relative to the base 21, a second joint drive unit 252 that rotates the second arm 222 around the second pivot axis J2 relative to the first arm 221, a first drive mechanism 253 that rotates the spline nut 231, and a second drive mechanism 254 that rotates the ball screw nut 232.

[0017] Furthermore, as shown in Figure 3, the first joint drive unit 251 includes a reduction gear 251a that rotatably connects the base 21 and the first arm 221, and a motor 251b that is located inside the base 21 and connected to the input side of the reduction gear 251a. As a result, the rotation of the motor 251b is reduced by the reduction gear 251a and transmitted to the first arm 221, causing the first arm 221 to rotate around the first pivot axis J1 at a predetermined speed relative to the base 21. The motor 251b also has a built-in encoder 251c for detecting the amount of rotation of the motor 251b.

[0018] Similarly, as shown in Figure 4, the second joint drive unit 252 includes a reduction gear 252a that rotatably connects the first arm 221 and the second arm 222, and a motor 252b located inside the second arm 222 and connected to the input side of the reduction gear 252a. Therefore, the rotation of the motor 252b is reduced by the reduction gear 252a and transmitted to the first arm 221, causing the second arm 222 to rotate around the second pivot axis J2 at a predetermined speed relative to the first arm 221. The motor 252b also has a built-in encoder 252c for detecting the amount of rotation of the motor 252b.

[0019] As shown in Figure 4, the first drive mechanism 253 includes a motor 253b and a belt 253a connecting the spline nut 231 and the motor 253b. Therefore, the rotation of the motor 253b is transmitted to the spline nut 231 via the belt 253a, causing the spline nut 231 to rotate around the third rotation axis J3 at a predetermined speed relative to the second arm 222. The motor 253b has a built-in encoder 253c for detecting the amount of rotation of the motor 253b. Similarly, the second drive mechanism 254 includes a motor 254b and a belt 254a connecting the ball screw nut 232 and the motor 254b. Therefore, the rotation of the motor 254b is transmitted to the ball screw nut 232 via the belt 254a, causing the ball screw nut 232 to rotate around the third rotation axis J3 at a predetermined speed relative to the second arm 222. Furthermore, the motor 254b has a built-in encoder 254c for detecting the amount of rotation of the motor 254b.

[0020] Furthermore, as shown in Figure 1, the robot body 2 has a reinforcing mechanism 3 that suppresses vibration and torsion of the spline shaft 233 caused by the rotational movement of the second arm 222, the weight of the workpiece gripped by the hand 4, etc. The reinforcing mechanism 3 suppresses vibration and torsion of the spline shaft 233 by increasing the rigidity of the spline shaft 233. Note that the vibration and torsion of the spline shaft 233 become more pronounced as the load applied to the spline shaft 233 increases, that is, as the robot body has a heavier payload capacity.

[0021] As shown in Figure 1, the reinforcing mechanism 3 includes an upper support plate 31 fixed to the second arm 222 through which a spline shaft 233 is inserted, sub-bearings 331 and 332 positioned on the upper support plate 31, and sub-shaft shafts 321 and 322 inserted through the sub-bearings 331 and 332 and pivotally supported relative to the upper support plate 31 so as to be able to move vertically (up and down). The sub-shaft shafts 321 and 322 are each parallel to the spline shaft 233 and are arranged along the vertical direction. Furthermore, the sub-shaft shafts 321 and 322 are located on opposite sides of the second arm 222.

[0022] Furthermore, the reinforcement mechanism 3 has a lower support plate 34 located below the upper support plate 31 and above the hand 4. The lower ends of the sub-shafts 321 and 322 are fixed to the lower support plate 34, respectively. In other words, the upper support plate 31, lower support plate 34, and hand 4 are arranged in that order. Also, a third main bearing 35 through which the spline shaft 233 is inserted is located in the center of the lower support plate 34. The spline shaft 233 is pivotally supported by the third main bearing 35 so as to be rotatable around the third pivot axis J3 relative to the lower support plate 34. Therefore, the lower support plate 34 supports and connects the spline shaft 233 and the sub-shafts 321 and 322 so as to maintain a distance between them. With this reinforcement mechanism 3, the rigidity of the spline shaft 233 can be increased, and vibration and torsion of the spline shaft 233 can be effectively suppressed and reduced.

[0023] Furthermore, as shown in Figure 1, the robot body 2 includes a first inertial sensor 51 positioned on the robot arm 22 and a second inertial sensor 52 positioned on the spline shaft 233. The first inertial sensor 51 is positioned at the tip of the second arm 222, that is, near the spline shaft 233, and measures the inertia in the direction along the central axis of the spline shaft 233. In particular, in this embodiment, the first inertial sensor 51 is an angular velocity sensor that can detect the angular velocity ω1 around a detection axis perpendicular to the plane defined by the second rotation axis J2 and the third rotation axis J3, that is, a detection axis perpendicular to the central axis of the spline shaft 233. Since the robot arm 22 is cantilevered to the base 21, the spline shaft 233 is displaced in the direction S, which includes a rotational component, due to the deflection deformation of the connection between the base 21 and the first arm 221 and the connection between the first arm 221 and the second arm 222, as well as the bending and torsional deformation of the first and second arms 221 and 222, as shown in Figure 1. This displacement can be detected as angular velocity ω1. Angular velocity and acceleration are examples of inertia.

[0024] On the other hand, the second inertial sensor 52 is positioned at the lower end of the spline shaft 233 and measures the inertia in a direction perpendicular to the central axis of the spline shaft 233. In particular, in this embodiment, the second inertial sensor 52 is an angular velocity sensor capable of detecting the angular velocity ω2 around a detection axis along the vertical axis, that is, a detection axis along the central axis of the spline shaft 233.

[0025] The placement of the first inertial sensor 51 is not particularly limited, and for example, it may be placed on the first arm 221. Also, in Figure 1, for the sake of explanation, the first inertial sensor 51 is located outside the second arm 222, but it is not limited to this, and may be housed inside the second arm 222. Furthermore, the placement of the second inertial sensor 52 is not particularly limited, and for example, it may be placed on the hand 4. Also, for example, the second inertial sensor 52 may be placed on the lower support plate 34. In other words, the second inertial sensor 52 may be indirectly placed on the spline shaft 233 via another member, and in this specification, this configuration is also expressed as "placed on the spline shaft 233". In other words, "placed on the spline shaft 233" includes not only the case where it is directly placed on the spline shaft 233, but also the case where it is indirectly placed on the spline shaft 233 via another member.

[0026] The control device 10 controls the drive of the entire robot body 2 by independently controlling the drive of each motor 251b, 252b, 253b, and 254b. The control device 10 is composed of, for example, a control board and a power supply board. Such a control device 10 is composed of, for example, a computer and has a processor (CPU) that processes information, a memory that is communicatively connected to the processor, and an external interface that connects to external devices. Various programs that can be executed by the processor are stored in the memory, and the processor can read and execute various programs stored in the memory. As shown in Figure 1, in this embodiment, for the sake of explanation, the control device 10 is housed inside the base 21, but it is not limited to this and may be located outside the base 21.

[0027] The mechanical configuration of robot 1 has been described above. Next, the method of controlling the robot body 2 by the control device 10 will be described in detail.

[0028] The control device 10 independently controls the drive of the robot arm 22, that is, the drive of motors 251b, 252b, 253b, and 254b, based on the output of the first inertial sensor 51, that is, the angular velocity ω1 detected by the first inertial sensor 51. Below, the control method for motor 254b, which serves as the first drive unit for the ball screw nut 232 that moves the spline shaft 233 in a straight line along the third rotation axis J3, will be described.

[0029] As shown in Figure 5, the control device 10 has a motor control unit 100 that controls the drive of the motor 254b. The motor control unit 100 includes a position command generation unit 101, a position control unit 102, a speed control unit 103, a current control unit 104, and a vibration feedback generation unit 105.

[0030] The vibration feedback generation unit 105 removes high-frequency sensor noise from the angular velocity ω1, which is the output of the first inertial sensor 51, using an LPF (not shown), and further removes the offset component using a DC removal unit (not shown). Then, the vibration feedback generation unit 105 multiplies the filtered angular velocity ω1 by a feedback gain Kgp to obtain the vibration feedback 915.

[0031] The position command generation unit 101 generates a position command 901 for the motor 254b based on a program created by the host computer. The position control unit 102 determines a speed command 904 that matches the position command 901 with the motor shaft position 902, which is the position of the motor 254b detected by the encoder 254c.

[0032] The speed control unit 103 is composed of proportional-integral control and determines a current command 906 that matches the speed 903 obtained from the motor shaft position 902 to the speed command 904. Specifically, the speed control unit 103 first subtracts the speed 903 from the speed command 904 to obtain the speed loop command 905. Next, the speed control unit 103 obtains the current command 906 by adding the integral term obtained by multiplying the speed loop command 905 by the speed loop proportional gain Kvp to the proportional term obtained by multiplying the speed loop command 905 by the speed loop integral gain Kvi, and the vibration feedback 915. The current control unit 104 controls the current 907 that drives the motor 254b so that it matches the current command 906. Then, the motor 254b is driven by the current 907 controlled by the current control unit 104. In this way, by adding the vibration feedback 915 to the current command 906, the acceleration of the spline shaft 233 is adjusted so as to dampen the angular velocity of the spline shaft 233, thereby effectively reducing the vibration of the spline shaft 233. In the following, this type of control will also be simply referred to as "feedback control".

[0033] However, with the vibration control described above, it may not be possible to ideally suppress the vibration of the spline shaft 233. Furthermore, it is not possible to determine whether the vibration of the spline shaft 233 is being suppressed by the vibration control. As a result, the vibration control may actually worsen the vibration of the spline shaft 233. Therefore, the control device 10 is configured such that the vibration feedback generation unit 105 further determines whether the vibration of the spline shaft 233 is being suppressed, and changes the vibration feedback 915 based on the determination result, thereby suppressing the worsening of the vibration of the spline shaft 233 and effectively suppressing the vibration of the spline shaft 233. This will be explained in detail below.

[0034] While the motor 254b is driven by the feedback control described above, the vibration feedback generation unit 105 monitors the vibration state of the spline shaft 233 based on the angular velocity ω2 measured by the second inertial sensor 52. If the magnitude of the vibration of the spline shaft 233 is less than a preset threshold SH, it is estimated that the vibration of the spline shaft 233 is sufficiently reduced by the current feedback control. Therefore, the vibration feedback generation unit 105 determines that the vibration of the spline shaft 233 is "normal". On the other hand, if the magnitude of the vibration of the spline shaft 233 is greater than or equal to the threshold SH, it is estimated that the vibration of the spline shaft 233 is not sufficiently reduced by the current feedback control, but there is no confirmation of this. Therefore, the vibration feedback generation unit 105 initially determines that the vibration of the spline shaft 233 is in an "unknown state" where it cannot distinguish whether the vibration is normal or abnormal. This method allows for accurate determination of the vibration state of the spline shaft 233. The threshold SH is not particularly limited and can be set appropriately depending on the required accuracy, cycle time, etc., for example, of the robot 1.

[0035] If the vibration of the spline shaft 233 is "normal," it is presumed that the vibration suppression effect of the feedback control described above is being fully realized, and therefore the vibration feedback generation unit 105 continues the feedback control described above without changing the vibration feedback 915.

[0036] On the other hand, if the vibration of the spline shaft 233 is in an "unknown state," the vibration suppression effect by the feedback control described above is unknown, so the vibration feedback generation unit 105 performs a verification operation to confirm the vibration suppression effect by the feedback control. Specifically, the robot 1 is made to perform the same operation with and without the vibration feedback 915, i.e., with the feedback gain Kgp set to 0 (zero), and the magnitude of the vibration of the spline shaft 233 is compared based on the angular velocity ω2 measured by the second inertial sensor 52 at that time. The same operation performed by the robot 1 is also referred to as the "test operation" below. In other words, the first vibration, which is the vibration of the spline shaft 233 measured by the second inertial sensor 52 when the test operation is performed with the vibration feedback 915 being fed back, is compared with the second vibration, which is the vibration of the spline shaft 233 measured by the second inertial sensor 52 when the test operation is performed without the vibration feedback 915 being fed back. The first and second vibrations may, for example, be the average or maximum value of the vibration magnitude measured during the test operation.

[0037] Furthermore, if the first vibration is less than the second vibration, that is, if the vibration of the spline shaft 233 with feedback is smaller than the vibration of the spline shaft 233 without feedback, it is presumed that the vibration suppression effect of the feedback control is being fully realized. Therefore, the vibration feedback generation unit 105 determines that the vibration of the spline shaft 233 is "normal". On the other hand, if the first vibration is greater than or equal to the second vibration, that is, if the vibration of the spline shaft 233 with feedback is equal to or greater than the vibration of the spline shaft 233 without feedback, it is presumed that the vibration suppression effect of the feedback control is not being realized, or that the feedback control is worsening the vibration of the spline shaft 233. Therefore, the vibration feedback generation unit 105 determines that the vibration of the spline shaft 233 is "abnormal". With this configuration, the vibration state of the spline shaft 233 can be determined with high accuracy.

[0038] If the vibration of the spline shaft 233 is "normal," it is presumed that the vibration suppression effect of the feedback control described above is being fully realized, and therefore the vibration feedback generation unit 105 continues the feedback control described above without changing the vibration feedback 915.

[0039] On the other hand, if the vibration of the spline shaft 233 is "abnormal," continuing the feedback control as is may worsen the vibration of the spline shaft 233. Therefore, if the vibration of the spline shaft 233 is "abnormal," the vibration feedback generation unit 105 changes the vibration feedback 915 from the vibration feedback 915 in the "normal" state. Specifically, the vibration feedback 915 is made smaller than in the "normal" state by making the feedback gain Kgp smaller than in the "normal" state. In particular, in this embodiment, the vibration feedback generation unit 105 stops the feedback control based on the output of the first inertial sensor 51 by setting the feedback gain Kgp to 0 (zero). This effectively suppresses the worsening of the vibration of the spline shaft 233 due to feedback control. In the case of "abnormality," for example, the feedback gain Kgp may be gradually reduced from the value in the "normal" state to 0 (zero) until the vibration of the spline shaft 233 returns to "normal."

[0040] The control method for the robot body 2 by the control device 10 has been described above. With this control method, the normal / abnormal vibration of the spline shaft 233 is determined, and furthermore, the vibration feedback 915 is changed based on the determination result, thereby preventing the deterioration of the vibration of the spline shaft 233 due to feedback control and effectively suppressing the vibration of the spline shaft 233.

[0041] The robot 1 has been described above. As previously mentioned, the robot 1 includes a robot arm 22, a spline shaft 233 which is supported by the robot arm 22 and moves linearly along the central axis relative to the robot arm 22, a hand 4 attached to the spline shaft 233, a first inertia sensor 51 which measures the inertia of the spline shaft 233 in the direction along the central axis, a second inertia sensor 52 which measures the inertia of the spline shaft 233 in the direction perpendicular to the central axis, a motor 254b which is a first drive unit that moves the spline shaft 233 along the central axis relative to the robot arm 22, and a control device 10 which is a control unit that controls the drive of the motor 254b. The control device 10 performs feedback control to the motor 254b based on the output of the first inertia sensor 51, and determines whether the vibration of the spline shaft 233 is "normal" or "abnormal" based on the output of the second inertia sensor 52, and makes the conditions for feedback control different depending on whether it is "abnormal" or "normal". With this configuration, different feedback control can be performed depending on whether the vibration of the spline shaft 233 is "normal" or "abnormal." This prevents the vibration of the spline shaft 233 from worsening due to feedback control and effectively suppresses the vibration of the spline shaft 233.

[0042] Furthermore, as mentioned above, the control device 10 modifies the vibration feedback 915 generated based on the output of the first inertial sensor 51 depending on whether the vibration is "abnormal" or "normal". With this configuration, vibration of the spline shaft 233 can be effectively suppressed with simple control.

[0043] Furthermore, as mentioned above, the control device 10 determines that the vibration magnitude is above the threshold SH, resulting in an "unknown state" where it cannot distinguish between normal and abnormal, and determines that the vibration magnitude is below the threshold SH, resulting in a "normal" state. With this configuration, the vibration state of the spline shaft 233 can be determined with high accuracy.

[0044] Furthermore, as mentioned above, when the vibration is in an "unknown state," the control device 10 compares the first vibration, which is the vibration of the spline shaft 233 when feedback control based on the output of the first inertial sensor 51 is performed on the motor 254b, with the second vibration, which is the vibration of the spline shaft 233 when feedback control based on the output of the first inertial sensor 51 is not performed on the motor 254b. If the first vibration is less than the second vibration, it is determined to be "normal," and if the first vibration is equal to or greater than the second vibration, it is determined to be "abnormal." With this configuration, the vibration state of the spline shaft 233 can be determined with high accuracy.

[0045] Furthermore, as mentioned above, the control device 10 sets the vibration feedback 915 to 0 (zero) if the vibration is "abnormal". With this configuration, the deterioration of vibration of the spline shaft 233 can be effectively suppressed.

[0046] Next, an example of the robot control method of the present invention will be described using the flowchart shown in Figure 6. The following description will begin after the operation program has been read and executed.

[0047] First, in step S11, the control device 10 acquires the output of the first inertial sensor 51.

[0048] Next, in step S12, the vibration feedback generation unit 105 determines the vibration feedback 915 based on the angular velocity ω1, which is the output of the first inertial sensor 51. Then, the vibration feedback 915 is used to perform feedback control of the motor 254b.

[0049] Next, in step S13, the vibration feedback generation unit 105 determines whether the vibration of the spline shaft 233 is "normal". More specifically, while the motor 254b is driven by feedback control, the second inertial sensor 52 measures the angular velocity ω2, and based on the measured angular velocity ω2, it determines whether the vibration of the spline shaft 233 is "normal".

[0050] In step S13, if the vibration of the spline shaft 233 is "normal" (step S13 / YES), the process proceeds to step S14. In step S14, the vibration feedback generation unit 105 continues the feedback control described above without changing the vibration feedback 915.

[0051] On the other hand, if in step S13 the vibration of the spline shaft 233 cannot be determined to be "normal" and is determined to be in an "unknown state" (step S13 / NO), the process proceeds to step S15. In step S15, the vibration feedback generation unit 105 causes the robot 1 to perform a test operation, which is a verification operation to confirm the vibration suppression effect by feedback control.

[0052] Next, in step S16, it is determined whether the vibration measured during the test operation is less than the first vibration.

[0053] In step S16, if the first vibration is less than the second vibration (step S16 / YES), it is presumed that the vibration suppression effect by feedback control is sufficiently achieved, and the process proceeds to step S14.

[0054] On the other hand, if in step S16 the first vibration is not less than the second vibration (step S16 / NO), that is, if the vibration of the spline shaft 233 is "abnormal", the process proceeds to step S17. In step S17, the vibration feedback generation unit 105 changes the vibration feedback 915 from the vibration feedback 915 in the "normal" state.

[0055] This control method allows for different feedback control depending on whether the spline shaft 233 is vibrating "normally" or "abnormally." This prevents the vibration of the spline shaft 233 from worsening due to feedback control and effectively suppresses the vibration of the spline shaft 233.

[0056] As described above, the robot control method is a robot control method for controlling a robot body 2 having a robot arm 22, a spline shaft 233 which is supported by the robot arm 22 and moves linearly along the central axis relative to the robot arm 22, a hand 4 attached to the spline shaft 233, a first inertia sensor 51 which measures the inertia of the spline shaft 233 in the direction along the central axis, a second inertia sensor 52 which measures the inertia of the spline shaft 233 in the direction perpendicular to the central axis, and a motor 254b which is a first drive unit that moves the spline shaft 233 along the central axis relative to the robot arm 22. The method provides feedback control to the motor 254b based on the output of the first inertia sensor 51, and determines whether the vibration of the spline shaft 233 is "normal" or "abnormal" based on the output of the second inertia sensor 52, and sets different conditions for feedback control depending on whether it is "abnormal" or "normal". This method allows for different feedback control depending on whether the spline shaft 233 is vibrating "normally" or "abnormally." This prevents the vibration of the spline shaft 233 from worsening due to feedback control and effectively suppresses the vibration of the spline shaft 233.

[0057] <Second Embodiment> The robot 1 of this embodiment is the same as that of the first embodiment described above, except that the control method by the control device 10 is different. In the following description, this embodiment will be described mainly in terms of the differences from the first embodiment described above, and similar matters will be omitted from the description.

[0058] In the first embodiment described above, when the vibration of the spline shaft 233 is determined to be "abnormal," the deterioration of the vibration of the spline shaft 233 is suppressed by changing the vibration feedback 915. In contrast, in this embodiment, when the vibration of the spline shaft 233 is "abnormal," the deterioration of the vibration of the spline shaft 233 is suppressed by changing the driving conditions of the spline shaft 233.

[0059] Specifically, first, the control device 10 determines whether the vibration of the spline shaft 233 is normal or abnormal, in the same manner as in the first embodiment described above. If the vibration of the spline shaft 233 is "normal", the control device 10 continues the feedback control described above. On the other hand, even if the vibration of the spline shaft 233 is "abnormal", the control device 10 continues the feedback control described above without changing the vibration feedback 915. However, the control device 10 reduces at least one of the drive parameters of the spline shaft 233, such as acceleration, deceleration, and travel speed, compared to the "normal" state, so that the vibration of the spline shaft 233 returns to "normal". By reducing these drive parameters, the movement of the spline shaft 233 becomes gentler, and as a result, the vibration of the spline shaft 233 can be suppressed to a smaller extent. Therefore, the deterioration of the vibration of the spline shaft 233 can be prevented, and the vibration of the spline shaft 233 can be effectively suppressed.

[0060] The order in which the drive parameters are changed is not particularly limited, but it is preferable to first gradually decrease the deceleration from the "normal" value, and if the vibration of the spline shaft 233 still does not return to "normal", then gradually decrease the acceleration from the "normal" value, and if the vibration of the spline shaft 233 still does not return to "normal", finally gradually decrease the travel speed from the "normal" value. However, the method of change is not limited to this, and for example, two or more drive parameters selected from acceleration, deceleration, and travel speed may be changed simultaneously.

[0061] As described above, in the robot 1 of this embodiment, when vibration is "abnormal", the control device 10 reduces at least one drive parameter of acceleration, deceleration, and travel speed during the movement of the spline shaft 233 to a lower value than when vibration is "normal". With this configuration, the movement of the spline shaft 233 becomes gentler, and as a result, the vibration of the spline shaft 233 can be suppressed to a smaller extent. Therefore, it is possible to prevent the deterioration of the vibration of the spline shaft 233 and effectively suppress the vibration of the spline shaft 233.

[0062] This second embodiment can also achieve the same effects as the first embodiment described above.

[0063] <Third Embodiment> Figure 7 is a cross-sectional view showing the tip of the robot body according to the third embodiment. Figures 8 to 10 are block diagrams showing the control method of the robot body by the control device, respectively. Figure 11 is a flowchart showing the control method of the robot body by the control device.

[0064] Robot 1 of this embodiment is the same as the first embodiment described above, except that the configuration of the first and second inertial sensors 51 and 52 is different, and the control method is different as a result. In the following description, this embodiment will be described mainly in terms of the differences from the first embodiment described above, and similar matters will not be described. Also, in the figures of this embodiment, the same reference numerals are used for components that are the same as in the previously described embodiment.

[0065] In the robot 1 of this embodiment, as shown in Figure 7, the first and second inertial sensors 51 and 52 are configured by a single inertial measuring device 50 (IMU). In other words, the first and second inertial sensors 51 and 52 are integrated as an inertial measuring device 50. The inertial measuring device 50 is located on the spline shaft 233. However, the inertial measuring device 50 may also be located on the hand 4.

[0066] Furthermore, in this embodiment, the first inertial sensor 51 is an acceleration sensor that detects acceleration Ac in the direction along the central axis of the spline shaft 233, that is, in the direction along the third rotation axis J3. On the other hand, the second inertial sensor 52 is an angular velocity sensor that detects angular velocity ω2 around a detection axis along the central axis of the spline shaft 233, similar to the first embodiment described above. In this way, even if the first inertial sensor 51 is an acceleration sensor, the vibration state of the spline shaft 233 can be determined with high accuracy, similar to the first embodiment described above. In particular, by integrating the first and second inertial sensors 51 and 52 as an inertial measuring device 50, it is possible to reduce the number of parts in the robot body 2.

[0067] The control device 10, for example as shown in Figures 8 and 9, feeds back the vibration feedback 915 obtained by multiplying the torsional angular velocity 914 generated based on the output of the second inertial sensor 52 by a feedback gain Kgp to the drive of the second drive units, motors 251b and 252b. Here, the torsional angular velocity 914 can be calculated by subtracting the angular velocity ω3 of the first arm 221 around the first rotation axis J1 and the angular velocity ω4 of the second arm 222 around the second rotation axis J2 from the angular velocity ω2 detected by the second inertial sensor 52. In other words, when the torsional angular velocity 914 is ωN, then ωN = ω2 - (ω3 + ω4). Note that the angular velocity ω3 is obtained by differentiating the motor shaft position detected by the encoder 251c built into motor 251b with respect to time, and the angular velocity ω4 is obtained by differentiating the motor shaft position detected by the encoder 252c built into motor 252b with respect to time. This type of control suppresses vibrations of the spline shaft 233 located at the tip of the first and second arms 221 and 222, thereby suppressing vibrations of the spline shaft 233 located at the tip of the arms.

[0068] Furthermore, in conjunction with the above control, the control device 10 feeds back vibration feedback 915, which is generated based on the acceleration Ac, the output of the first inertial sensor 51, to the drive of the motor 254b, as shown in Figure 10. With this type of control, vibration of the spline shaft 233 can be suppressed.

[0069] Furthermore, when the first and second arms 221 and 222 are stopped, that is, when the motors 251b and 252b are stopped, the control device 10 can estimate that the output of the second inertial sensor 52 is vibration of the spline shaft 233. Therefore, based on the output of the second inertial sensor 52, the control device 10 determines the vibration state of the spline shaft 233 in the same manner as in the first embodiment described above. If the vibration of the spline shaft 233 is "abnormal", it is possible that the vibration in the direction perpendicular to the central axis of the spline shaft 233 (lateral vibration) has increased as a result of suppressing the vibration in the direction along the central axis of the spline shaft 233 (longitudinal vibration) by the drive control shown in Figure 10. Therefore, in controlling the motor 254b, the control device 10 gradually reduces the vibration feedback 915 from the value when it is "normal" to 0 (zero). There is no particular limit to the method of reducing the vibration feedback 915, but a simple method is to gradually reduce the feedback gain Kgp. This effectively suppresses vibrations in the spline shaft 233.

[0070] However, the control method in the case of an "abnormal" condition is not particularly limited. For example, as in the second embodiment described above, at least one drive parameter of the spline shaft 233, such as acceleration, deceleration, or travel speed, may be reduced compared to the "normal" condition so that the vibration of the spline shaft 233 returns to "normal" condition.

[0071] As described above, in the robot 1 of this embodiment, the first inertial sensor 51 measures the acceleration Ac in the direction along the third rotation axis J3, which is the central axis of the spline shaft 233, and the second inertial sensor 52 measures the angular velocity ω2 around the third rotation axis J3, which is the central axis of the spline shaft 233. With this configuration, the vibration state of the spline shaft 233 can also be determined.

[0072] Furthermore, as mentioned above, the robot 1 has an inertial measuring device 50 equipped with a first inertial sensor 51 and a second inertial sensor 52, and the inertial measuring device 50 is located on the spline shaft 233 or the hand 4. By integrating the first and second inertial sensors 51 and 52 as the inertial measuring device 50 in this way, it is possible to reduce the number of parts in the robot body 2.

[0073] As mentioned above, the robot 1 has motors 251b and 252b, which are second drive units that drive the robot arm 22. The control device 10 provides feedback control to the motors 251b and 252b based on the output of the second inertial sensor 52. When the robot arm 22 is stopped, the control device 10 determines whether the vibration of the spline shaft 233 is "normal" or "abnormal" based on the output of the second inertial sensor 52. If it is "abnormal", the control device 10 reduces the vibration feedback 915 compared to the "normal" case. This effectively suppresses the vibration of the spline shaft 233.

[0074] Next, an example of the robot control method of the present invention will be described using the flowchart shown in Figure 11. The following description will begin after the operation program has been read and executed.

[0075] First, in step S21, the control device 10 acquires the output of the first inertial sensor 51. Next, in step S22, the vibration feedback generation unit 105 determines the vibration feedback 915 based on the angular velocity ω1, which is the output of the first inertial sensor 51. Then, the vibration feedback 915 is used to perform feedback control of the motor 254b.

[0076] Next, in step S23, the control device 10 determines whether the first and second arms 221 and 222 are stopped. If the first and second arms 221 and 222 are not stopped in step S23 (step S23 / NO), the process proceeds to step S24. In step S24, the vibration feedback generation unit 105 continues the feedback control described above without changing the vibration feedback 915. On the other hand, if the first and second arms 221 and 222 are stopped in step S23 (step S23 / YES), the process proceeds to step S25.

[0077] In step S25, the vibration feedback generation unit 105 determines whether the vibration of the spline shaft 233 is "normal". If the vibration of the spline shaft 233 is "normal" in step S25 (step S25 / YES), the process proceeds to step S24. On the other hand, if the vibration of the spline shaft 233 cannot be determined to be "normal" in step S25 and is determined to be in an "unknown state" (step S25 / NO), the process proceeds to step S26.

[0078] In step S26, the vibration feedback generation unit 105 has the robot 1 perform a test operation, which is a verification operation to confirm the vibration suppression effect by feedback control. Next, in step S27, it is determined whether the vibration measured during the test operation is first vibration < second vibration. In step S27, if first vibration < second vibration (step S27 / YES), it is presumed that the vibration suppression effect by feedback control is sufficiently demonstrated, and the process proceeds to step S24. On the other hand, in step S27, if first vibration < second vibration is not true (step S27 / NO), the process proceeds to step S28. In step S28, the vibration feedback generation unit 105 changes the vibration feedback 915 from the vibration feedback 915 in the "normal" state.

[0079] This third embodiment can also achieve the same effects as the first embodiment described above.

[0080] Although the robot and robot control method of the present invention have been described above based on the illustrated embodiments, the present invention is not limited thereto, and the configuration of each part can be replaced with any configuration or process having a similar function. Furthermore, other arbitrary configurations or processes may be added to the present invention. [Explanation of symbols]

[0081] 1...Robot, 10...Control device, 100...Motor control unit, 101...Position command generation unit, 102...Position control unit, 103...Speed ​​control unit, 104...Current control unit, 105...Vibration feedback generation unit, 2...Robot body, 21...Base, 22...Robot arm, 221...First arm, 222...Second arm, 23...Working head, 231...Spline nut, 232...Ball screw nut, 233...Spline shaft, 251...Number 1st joint drive unit, 251a...reducer, 251b...motor, 251c...encoder, 252...2nd joint drive unit, 252a...reducer, 252b...motor, 252c...encoder, 253...1st drive mechanism, 253a...belt, 253b...motor, 253c...encoder, 254...2nd drive mechanism, 254a...belt, 254b...motor, 254c...encoder, 3...reinforcement mechanism, 31...upper support plate, 321 …Sub-shaft axis, 322…Sub-shaft axis, 331…Sub-bearing, 332…Sub-bearing, 34…Lower support plate, 35…Third main bearing, 4…Hand, 50…Inertial measuring device, 51…First inertial sensor, 52…Second inertial sensor, 901…Position command, 902…Motor shaft position, 903…Speed, 904…Speed ​​command, 905…Speed ​​loop command, 906…Current command, 907…Current, 914…Torsion angular velocity, 915…Vibration feedback, Ac...acceleration, J1...first rotation axis, J2...second rotation axis, J3...third rotation axis, S11...step, S12...step, S13...step, S14...step, S15...step, S16...step, S17...step, S21...step, S22...step, S23...step, S25...step, S26...step, S27...step, S28...step, ω1...angular velocity, ω2...angular velocity, ω3...angular velocity, ω4...angular velocity

Claims

1. A robotic arm and A shaft supported by the robot arm and moving linearly along the central axis relative to the robot arm, A hand attached to the aforementioned shaft, A first inertial sensor for measuring the inertia of the shaft in the direction along the central axis, A second inertial sensor that measures the inertia of the shaft in a direction perpendicular to the central axis, A first drive unit moves the shaft along the central axis relative to the robot arm, The first drive unit has a control unit that controls the drive of the first drive unit, The control unit, Based on the output of the first inertial sensor, feedback control is performed on the first drive unit. A robot characterized by determining whether the vibration of the shaft is normal or abnormal based on the output of the second inertial sensor, and by making the conditions of the feedback control different depending on whether the vibration is abnormal or normal.

2. The robot according to claim 1, wherein the control unit modifies the vibration feedback generated based on the output of the first inertial sensor when there is an abnormality compared to when there is a normal condition.

3. The robot according to claim 2, wherein the control unit determines that the state is an unknown state in which it is not possible to distinguish between normal and abnormal when the magnitude of the vibration is greater than or equal to a threshold, and determines that the state is normal when the magnitude of the vibration is less than the threshold.

4. When the state is unknown, the control unit compares the first vibration, which is the vibration when the first drive unit performs the feedback control based on the output of the first inertial sensor, with the second vibration, which is the vibration when the first drive unit does not perform the feedback control based on the output of the first inertial sensor. If the first vibration is less than the second vibration, it is determined to be normal. The robot according to claim 3, wherein if the first vibration is greater than or equal to the second vibration, it is determined that there is an abnormality.

5. The robot according to claim 4, wherein the control unit sets the vibration feedback to 0 (zero) in the event of the abnormality.

6. The robot according to claim 1, wherein the control unit, in the event of the abnormality, reduces at least one drive parameter of acceleration, deceleration, and movement speed during the movement of the shaft to a smaller value than in the normal case.

7. The first inertial sensor measures the acceleration of the shaft in the direction along the central axis, The robot according to claim 1, wherein the second inertial sensor measures the angular velocity of the shaft about the central axis.

8. The inertial measuring device comprises the first inertial sensor and the second inertial sensor, The robot according to claim 7, wherein the inertial measuring device is arranged on the shaft or the hand.

9. It has a second drive unit that drives the robot arm, The robot according to claim 2, wherein the control unit provides feedback control to the second drive unit based on the output of the second inertial sensor, determines whether the vibration of the shaft is normal or abnormal based on the output of the second inertial sensor when the robot arm is stopped, and if it is abnormal, reduces the vibration feedback compared to the normal case.

10. A robotic arm and A shaft supported by the robot arm and moving linearly along the central axis relative to the robot arm, A hand attached to the aforementioned shaft, A first inertial sensor for measuring the inertia of the shaft in the direction along the central axis, A second inertial sensor that measures the inertia of the shaft in a direction perpendicular to the central axis, A robot control method for controlling a robot body having a first drive unit that moves the shaft along the central axis relative to the robot arm, Based on the output of the first inertial sensor, feedback control is performed on the first drive unit. A robot control method characterized by determining whether the vibration of the shaft is normal or abnormal based on the output of the second inertial sensor, and making the conditions for the feedback control different depending on whether the vibration is abnormal or normal.

Images