JP4858229B2 - Calculation method of load mass and center of gravity position attached to robot - Google Patents

Description

  The present invention relates to a method of calculating the weight of a load attached to a robot driven by a motor and the position of the center of gravity of the load.

  In recent years, in a robot having a multi-axis arm driven by a motor, high accuracy of collision detection has been demanded in order to improve safety at the time of collision and to prevent loss due to destruction.

  In addition, with the aim of further improving the actual tact performance of the robot, various kinds of performance such as acceleration / deceleration motion performance and vibration suppression control performance, expansion of the field of use of the robot, deburring work, and fitting work of complex shaped parts, etc. There is a demand for the realization of flexible control aimed at satisfying the usage requirements.

  In the control method for each application for realizing such an application, the mass of the load and the position of the center of gravity attached to the arm of the robot and the tip of the arm are required. Among these, the mass and the position of the center of gravity of the arm provided in the robot can be identified in advance by a design tool such as CAD used at the time of design. However, when an arbitrary load is attached to the tip of the robot arm, it is necessary to separately know the load mass and the value of the center of gravity.

  Conventionally, as a method of knowing the mass of the load and the position of the center of gravity, there is a method in which the robot operator manually inputs these values from the robot controller.

  On the other hand, as a method without manual input, there is a method of measuring the current, torque, and angle of each axis of the motor provided in the robot and calculating the load mass and the position of the center of gravity. This method has two methods: a method in which the robot is stationary and a method in which each axis of the robot is driven.

  Of these methods, the method of making the robot stand still uses the torque calculated from the current value of each axis of the motor in the stationary state of the robot, and subtracts the torque component caused by the gravity of the robot alone and the friction torque when stationary. Only the torque resulting from the gravity of the load is extracted, and the mass and the center of gravity of the load are calculated (for example, see Patent Document 1).

  Further, in the method of driving each axis of the motor of the robot, the number of axes and the center of gravity position of the load for calculating the motor torque and arm angle of each axis when driving each axis of the robot motor was established. The mass of the load and the position of the center of gravity were calculated by substituting them into the relational expression caused by gravity (see, for example, Patent Document 2).

  However, in the conventional method of knowing the mass and center of gravity of the load described above, it is necessary for the operator of the robot to obtain the mass and center of gravity of the load in advance by calculation or measurement. There was an evil occurrence.

  In addition, as a method that does not input manually, the above-mentioned method in which the robot is stationary makes it difficult to estimate an accurate value because the friction torque when stationary varies greatly depending on the mass of the load and the posture of the robot. The load mass and the accuracy of the center of gravity position could be greatly affected.

  In the method of driving each axis of the motor of the robot, four motor torque values including the main drive axis are derived from having four independent calculation formulas for calculating the load mass and the center of gravity. It was necessary to measure the arm angle. For this reason, when the movement range of the robot is restricted or when the movement itself is restricted, there is a possibility that the load mass and the gravity center position cannot be calculated.

  As described above, the conventional method for estimating the load mass and the position of the center of gravity has a problem in that the load mass and the position of the center of gravity are accurately estimated with a small number of operations. In view of this, a method has been proposed in which the mass of the load and the position of the center of gravity are estimated by only the operation of the two wrist axes of the robot whose rotation center axes are orthogonal (see, for example, Patent Document 3).

  In this method, the motor torque when each arm rotating around two rotation axes is moved at an equal angular velocity for each axis is calculated, and the torque component due to the mass of the load is calculated from the calculated motor torque. The maximum value (amplitude) of the obtained load torque component (sine wave) and the arm joint angle at which the torque component becomes zero (phase difference from the arm reference angle) are calculated for each rotation axis. The load mass and the position of the center of gravity are calculated.

At this time, in order to accurately determine the amplitude and phase difference of the load torque component by a simple method, it is preferable that the measured value includes a portion where the torque component is a maximum value and a portion where the torque component is zero. Since the arm joint angle at which the load torque component becomes 0 can be determined as the phase difference, the load torque value at an angle shifted by 90 ° from the arm joint angle can be adopted as the amplitude.
JP-A-9-91004 Japanese Patent Laid-Open No. 11-48181 JP 2006-075931 A

  In the method described in Patent Document 3, it is necessary to rotate the axis by 90 ° or more in order to measure the maximum value and the portion where the value becomes 0. In order to narrow the motion range for estimation, the least square method is used. A method for identifying the amplitude and phase of a sine wave by using it is described.

  However, the method of least squares converges the error by iterative calculation while changing parameters, and the amount of calculation increases. In addition, since a nonlinear trigonometric function (sine wave) is handled, the amount of calculation further increases, and it is difficult to process with a CPU for real-time control such as servo control mounted on the robot controller.

  Therefore, in the present invention, the increase in the amount of calculation is minimized when the arms rotating around the two rotation axes are moved at equal angular speeds for each axis to obtain the amplitude and phase of the current due to the load mass. In addition, an object of the present invention is to provide a control method for calculating the mass of the load and the position of the center of gravity that can reduce the rotational operation angle.

In order to solve the above-described problem, the load mass and center of gravity position calculation method attached to the robot of the present invention is driven by a motor, and a straight line connecting the two rotation center axes and one rotation center axis are In a robot having a plurality of arms having at least one set of two rotation axes in which the plane formed is in a torsional relationship substantially orthogonal to the other rotation center axis, one of the two rotation axes is the most advanced of the robot A rotation axis, and the other is a rotation axis on the most advanced side next to the most advanced rotation axis, and rotates around the most advanced rotation axis, and the mass of a load mounted on the most advanced arm. a method of calculating the centroid position mass and center of gravity of the load attached to the robot for calculating the said two rotary shaft is equal angular velocity motion by a predetermined operation angle smaller than 90 degrees for each axis wherein the predetermined operation Every half two points positioned at a predetermined angle has a phase interval set multiple obtains a motor current measurements at an angle and the angle of each two points, said these angles and the motor current measurement A plurality of amplitudes and phases of torque components due to the mass of the load are obtained from the torque components due to the mass of the arm, averaged for each axis, and the averaged amplitude and phase of the torque component due to the mass of the load are obtained. Based on this, the mass and the center of gravity of the load are calculated.

In the calculation method of the mass of the load and the position of the center of gravity attached to the robot of the present invention, the predetermined operation angle is greater than 10 degrees.

Further, in the method of calculating the mass of the load and the position of the center of gravity attached to the robot of the present invention, the plurality of two points are points that are moved by a minute angle from the predetermined two points in the movement direction of each axis.

  As described above, according to the robot control method of the present invention, when determining the amplitude and phase of the load gravity torque, a plurality of pairs of load gravity torques are calculated and averaged at predetermined angular intervals. The detection accuracy can be improved without increasing the measurement operation angle and the amount of calculation.

  Hereinafter, the best mode for carrying out the present invention will be described with reference to FIGS.

(Embodiment 1)
In this embodiment, a robot including an arm having a rotation center axis as shown in FIG. 1 expressed in a vertical coordinate system and in FIG. 2 expressed in a horizontal coordinate expression will be described. Further, in this embodiment, as shown in FIGS. 1 and 2, the rotation center axes of the two rotation shafts 5 and 6 on the arm tip side are in a twisted position, and the straight line connecting the shortest axis is An example of a robot in which a plane formed by the rotation center axis is arranged so as to be substantially orthogonal to the other rotation center axis is shown. Although the rotation center axis in this embodiment is an example at the position of torsion, the following description is valid even in a case where the rotation center is orthogonal to this, so the description is simplified in this embodiment. For the sake of simplicity, the description will be made assuming that the rotation axis is orthogonal even when the rotation axis is in the twisted position.

  1 and 2, Xl 29, Zl 30, and Yl 31 indicate distances in three directions from the arm rotation center position of the sixth axis to the gravity center position of the load attached to the tip of the arm. In FIG. 1, Lii 46 and Lj 47 indicate the arm lengths in two directions from the rotation center of the fifth shaft 44 to the rotation center of the sixth shaft 45. Note that the arm length in the other direction is set to 0 because the fifth shaft 44 and the sixth shaft 45 in FIG. 2 have the rotation centers on the same line.

  Of these dimensions, the load center-of-gravity positions Xl, Zl, and Yl are unknown numerical values, and the arm lengths Lii and Lj of the fifth axis and the sixth axis are known numerical values in advance.

  3 corresponds to the vertical coordinate system of FIG. 1 and shows a state in which the arm 51 rotating around the fifth axis is rotated, and FIG. 4 corresponds to the horizontal coordinate system of FIG. It is a figure which shows the state which rotated the load 48 attached to the 6th axis | shaft centering on the 6th axis | shaft. In each figure, (a) shows the initial position, (b) shows the negative rotation position (c), and the positive rotation position. 3 denotes a part of the arm of the fourth shaft 43 shown in FIG.

  Further, L526 in FIG. 3 and L634 in FIG. 4 indicate the length in three directions to the load center of gravity when the rotation center of the fifth axis is the origin, as shown in FIG. 3 and FIG. Are the combined lengths of the arm lengths Lii, Lj and the center of gravity positions Xl, Zl, Yl of the load.

  Next, a method for calculating the mass and the position of the center of gravity of the load provided at the arm tip as described above will be described.

  First, the arms rotating around the fifth axis 44 and the sixth axis 45 shown in FIGS. 1 and 2 are moved at an equal angular velocity for each axis, and the torque (sinusoidal wave) caused by the load mass applied to each axis An example in which the phase is obtained from the amplitude and the arm reference angle will be described.

  FIG. 5 is a block diagram for carrying out the control method in the present embodiment. In addition, although the block diagram similar to FIG. 5 exists for a plurality of axes for the entire robot, FIG. 5 is representatively shown for the control of one motor axis.

  In FIG. 5, 6 is a position control block, which generates a speed loop command ωcom7 from the difference value between the motor position command θcom3 and the motor position feedback θm2 calculated by multiplying the robot joint angle command θrc1 by the reduction ratio Rg37.

  Reference numeral 10 in FIG. 5 denotes a speed control block, which generates a motor current command Im11 from a difference value between the speed loop command ωcom7 and the motor speed feedback ωm4.

  Reference numeral 18 in FIG. 5 is a block showing the motor and external force. τm13 is a motor-generated torque, and assuming that the speed reducer is a rigid body, it is expressed by Expression (1) when viewed from the motor drive side, and is expressed by Expression (Expression 2) when viewed from the load side.

    However, the symbols in the equations (Equation 1) and (Equation 2) are as follows.

Kt (12): Motor torque constant Im (11): Motor current αm: Motor angular acceleration (differential value of ωm)
ωm (4): Motor angular velocity Jm: Motor inertia (converted to rotor + reducer primary side, reducer secondary side)
D: Coefficient of viscous friction τμ (14): Dynamic friction torque τdyn (16): Dynamic torque (the sum of gravity torque, inertial force, centrifugal force, and Coriolis force)
Rg: Reduction gear reduction ratio The dynamic friction torque τμ in the equation (Equation 2) can be calculated by the following equation.

ただし、
Ｋμ：動摩擦の大きさ

However,
Kμ: Size of dynamic friction

  The dynamic torque τdyn in equation (2) can be approximated as a gravitational torque component because inertial force, centrifugal force, and Coriolis force due to angular velocity and angular acceleration can be ignored during constant angular velocity operation at low speed. ).

However,
θr: Robot joint angle (speed reducer output angle) = θm / Rg
τgA [θr]: Gravitational torque caused by the robot arm τgL [θr]: Gravitational torque caused by the load attached to the robot Both τgA [θr] and τgL [θr] are uniquely determined by the robot joint angle θr .

  The gravity torque τgA [θr] resulting from the robot body arm in Equation (5) is known because the mass and shape of parts such as the arm constituting the robot are known. Calculation is possible (if the angle is known).

  Therefore, the arm gravity torque calculation block 20 in FIG. 5 receives the robot joint angle θr36 of the relevant axis and the robot joint angle θr19 of the other axis in a stationary state, and parameters of the mass and shape of parts such as arms constituting the robot. And the arm gravity torque τgA [θr] 21 is calculated.

  Since the motor angular acceleration αm = 0 at low speed and constant speed operation, a formula for obtaining the gravitational torque τgL [θr] 24 caused by the load based on the formulas (Equation 1) to (Equation 5) is derived as follows. It becomes like a formula (Formula 6).

  In obtaining the gravitational torque τgL [θr], as shown in FIGS. 3 and 4, the fifth axis 44 and the sixth axis 45 are respectively positive and negative in each direction as shown in (a) to (c). Are reciprocated at a constant angular velocity by a predetermined angle Δθr125, and the motor current value Im and the robot joint angle θr in each direction are measured.

  Here, Im [θr] m is the current when operated at a constant angular velocity at -ωm0 in the negative direction (sgn = -1), and the motor current when operated at a constant angular velocity at ωm0 in the positive direction (sgn = 1). When the above formula (formula 6) is expressed as Im [θr] p, the following formulas (formula 7) and (formula 8) are obtained, respectively.

  When the equations (Equation 7) and (Equation 8) are added together, the following equation (Equation 9) is obtained.

ただし、

However,

  That is, the robot is reciprocated at a low speed of the same speed, the robot joint angle θr and the motor current value Im [θr] m at the negative direction in the negative direction, and the robot joint angle θr and the motor current at the angle in the positive direction. If the data of the value Im [θr] p is accumulated, and the average of the positive and negative directions is taken between the same joint angles θr after the reciprocating motion of the predetermined angle Δθr (addition and divided by 2), the term related to the friction force Is removed, and τgI [θr] 23 can be obtained without having to obtain the values of the viscous friction coefficient D and the dynamic friction Kμ.

  In order to actually execute the above contents, data is recorded and calculated at intervals dr obtained by dividing a predetermined operating angle Δθr by 2n.

ただし、
θ０：計測開始時のロボット関節角度
回転方向負の場合：ｋ＝１，２……，２ｎ−１，２ｎ
回転方向正の場合：ｋ＝２ｎ，２ｎ−１……，２，１

However,
θ0: Robot joint angle at the start of measurement When rotation direction is negative: k = 1, 2,..., 2n−1, 2n
When the rotation direction is positive: k = 2n, 2n−1..., 2, 1

  In this embodiment, it is not operated 90 ° or more, and only a predetermined angle Δθr is operated, so τgI [θr] is a part of a sine wave. The predetermined angle Δθr is preferably smaller than 90 ° and larger than 10 °, and more preferably 30 °. FIG. 6 shows an example of waveforms of τgI [θr], Kg · Rg · Im [θr], and Kg · Rg · Im [θr] p at this time, and Δθr = 30 °, dr = 1. ° (n = 15).

  The above processing is executed in the data accumulation / friction torque removal block 22 shown in FIG. 5 to obtain τgI [θr] 23, and the arm gravity torque τgA [θr] obtained in the arm gravity torque calculation block 20 from this τgI [θr] 23. 21 is subtracted to calculate the gravity torque τgL (θr) 24 caused by the load. That is, the equation (Equation 9) is calculated at the interval dr, and the data is stored in the amplitude / phase calculation means 25 shown in FIG.

  Since the arm gravity torque τgA [θr] is also an operation of only the axis, it becomes a part of the sine wave, and the load gravity torque τgL [θr [k]] calculated by the following equation (Equation 13) is also a sine wave. Become part. If the amplitude is A5 [k] and the phase φ5 [k], it can be expressed by the following equation (Formula 14).

  Next, a method for obtaining the amplitude A5 (26) and the phase φ5 (27) of the load gravity torque τgL (θr [k]) 24 will be described.

  In the amplitude / phase calculation means 25 shown in FIG. 5, 2n load gravity torques τgL (θr [k]) stored at an interval dr are separated by n (Δθr / 2 apart). Pair with an angle of.

  FIG. 7 shows an example of pairing when k = 3 and k = 12.

    When k = 3, the value is indicated by the arrow of τgL [θr1 [3]] 58 and the value indicated by the arrow of τgL [θr2 [3]] 59. Further, when k = 12, the value indicated by the arrow of τgL [θr1 [12]] 60 and the value indicated by τgL [θr2 [12]] 61 are obtained.

  Assuming that the paired data satisfies the relationship of the above equation (Equation 14), the following equation (Equation 16) is obtained.

  When the equation (Equation 16) is solved for φ5 [k] and A5 [k] using the cosine theorem, the following equation (Equation 17) is obtained.

  In the equation (Equation 17), if one set of θr1 [k] and θr2 [k] having a predetermined interval Δθr / 2 is calculated, the phase φ5 [k] can be obtained. It should be noted that multiple sets may be used instead of one set, and n is calculated from k = 1 to n, and n sets are averaged as shown in the following equation (Equation 18) to improve estimation accuracy. I can do it.

  By substituting the n sets of average values φ5 into the equation (Equation 16), the amplitude A5 [k] can be obtained as shown in the equation (Equation 19).

  Alternatively, by obtaining a plurality of φ5 [k], obtaining a plurality of A5 [k] from the plurality of φ5 [k], and taking the average of n sets as shown in the following equation (Equation 20) in the same manner as the phase φ5. The accuracy of the amplitude A5 can be improved.

  The same processing as described above is performed for the sixth axis. As with the fifth axis, the load gravity torque τgL [θr [k]] is a part of a sine wave. If the amplitude is A6 [k] and the phase φ6 [k], it can be expressed by the following equation (Equation 21).

  Unlike the cos in the equation (Equation 14) in the equation (Equation 21), it is expressed by sin, because when the joint angle θr = 0 °, the sixth axis arm is held in a posture close to the vertical direction. This is because τgL [θr [k]] is considered to be close to the minimum value. On the other hand, on the fifth axis, when the joint angle θL = 0 °, the arm to be driven is held in a posture close to the horizontal direction, so τgL [θr [k]] is also close to the maximum value. Therefore, it is displayed in cos. In either case, there is no particular problem because the phase is only expressed with a 90 ° shift, but it is better to select the one that can be expressed with an acute angle. Becomes easy.

  The equation corresponding to the equation (Equation 16) is as the following equation (Equation 22).

  When the equation (Equation 22) is solved for φ6 and A6 using the sine theorem, the following equation is obtained.

  As described above, the arms rotating around the rotation axes of the fifth shaft 44 and the sixth shaft 45 shown in FIGS. 1 and 2 are moved at a constant angular velocity for each axis, and the torque (sinusoidal force) applied to each axis is applied. The example of obtaining the phase from the amplitude of the wave) and the arm reference angle is shown.

  In the present embodiment, the amplitude and phase of the torque (sinusoidal wave) caused by the load mass can be obtained without rotating the shaft by 90 ° or more, and iterative calculation as in the case of using the least square method can be obtained. There is no need, and an increase in calculation amount can be suppressed.

  Hereinafter, a method for obtaining the load mass M and the gravity center positions Xl, Zl, Yl of the load mass using the amplitude and phase of the torque (sinusoidal wave) caused by the load mass will be described. It is the same as that.

  By the way, the amplitude A5 and the amplitude A6 calculated by the equations (Equation 20) and (Equation 26) are the maximum values of the torque component TgL. Therefore, as shown in FIGS. When acceleration is g, the torque component Tg takes the maximum value when the center of gravity of the load is in the horizontal direction of the center of rotation. Therefore, the combined lengths L5 and L6 from the center of rotation to the position of the center of gravity of the load and the load When the force of is applied, it is expressed by the following equations (Equation 27) and (Equation 28).

  As described above, the combined length L5 is obtained by combining the load center-of-gravity positions Xl, Zl, Yl and the arm lengths Lii and Lj of the fifth shaft 44 and the sixth shaft 45 as shown in the following equation (Equation 29). expressed.

  In the equation (Equation 28), L6 is the combined length of the vertical component Zl of the load centroid position and the horizontal component Yl of the load centroid location, and is represented by the following equation (Equation 30).

  The phase differences φ5 and φ6 are expressed by the following equations (Equation 31) and Equation (Equation 32) from the arm angle of the fifth shaft 44 and the sixth shaft 45 and the condition of the arm arrangement, respectively.

  Equation (Equation 31) is when the load position shown in FIG. 3A is the joint angle θr = 0 in Equation (Equation 14), and the rotation center of the fifth axis and the load gravity center position at this time are expressed as follows. This is derived from the fact that the angle between the connecting line and the horizontal axis is φ5. Equation (32) is when the load position shown in FIG. 4A is the joint angle θr = 0 in Equation (21), and the rotation center of the sixth axis and the load gravity center position at this time are expressed as follows. This is derived from the fact that the angle between the connecting line and the vertical axis is φ6.

  Further, by substituting Expression (Expression 27) and Expression (Expression 28) into Expression (Expression 31) and Expression (Expression 32), the following Expressions (Expression 33) and (Expression 34) are derived.

  By the way, since Equation (Equation 33) and Equation (Equation 34) are calculated for the center of gravity position of the same load, naturally, Zl in Equation (Equation 33) and Equation (Equation 34) are equal. Therefore, the following equation (Equation 35) is obtained.

  Furthermore, it can derive | lead-out to the relational expression which calculates the mass M of a load from Formula (Formula 35), The following formula | equation (Formula 36) is obtained.

  Furthermore, Zl can be calculated by substituting the mass M calculated by the equation (Equation 36) into the equation (Equation 33) or the equation (Equation 35).

  Also, the calculated Zl and robot arm lengths Lii and Lj are substituted into equations (Equation 29) and (Equation 30), and Xl and Yl are expressed as in Equation (Equation 37) and Equation (Equation 38). Desired.

  As already described, in this embodiment, the torque component caused by the mass of the arm and load of the robot is monitored, and the amplitude value and phase difference obtained from the two-axis motor torque with different rotation center axes are used. In the process of deriving simultaneous equations using the characteristics of the rotation center axis of the motor of the robot, and calculating the mass and center of gravity of the tip load from the simultaneous equations, when determining the amplitude and phase of the load gravity torque, the measurement angle A method of calculating a plurality of pairs of load gravity torques at intervals of 1/2 was shown. Conventionally, in order to increase the detection accuracy, it has been necessary to increase the measurement operation angle or increase the amount of calculation by the least square method. By adopting this method, the measurement operation angle and the amount of calculation are increased. Therefore, the detection accuracy can be improved.

  The present invention provides a control method for quickly and easily calculating the weight of a load of a robot driven by a motor and the position of the center of gravity of the load by a simple measurement method and calculation method. It can be used for various purposes such as expanding the field of use of robots, deburring work, and fitting work of complex shaped parts.

The figure which shows an example of arrangement | positioning in the vertical coordinate system of the drive shaft which comprises the robot in embodiment of this invention The figure which shows an example of arrangement | positioning in the horizontal coordinate system of the drive shaft which comprises the robot in embodiment of this invention The figure which shows an example in which one drive shaft in embodiment of this invention performs equal acceleration movement of a positive / negative direction The figure which shows an example which the other drive shaft in embodiment of this invention performs the equal acceleration motion of a positive / negative rotation direction Block diagram for implementing the control method in the embodiment of the present invention The figure which shows an example of the motor generation torque at the time of performing the constant velocity motion of FIG. 3 in embodiment of this invention The figure which shows an example of the characteristic of the torque component which originates in load at the time of performing constant velocity motion of FIG. 3 in embodiment of this invention

Explanation of symbols

1 Robot joint angle command θrc
2 Motor position feedback θm
3 Motor position command θcom
4 Motor speed feedback (motor angular speed) ωm
5 Position proportional gain KPP
6 Position control block 7 Speed loop command ωcom
8 Speed proportional gain KP
9 Speed integral gain KI
10 Speed control block 11 Motor current command Im
12 Motor torque constant Kt
13 Motor generated torque τm
14 Dynamic friction torque τμ
15 Differential element 16 Dynamic torque τdyn
17 Motor transfer function 18 Block showing motor and external force 19 Robot joint angle of other axis θr
20 Arm gravity torque calculation block 21 Arm gravity torque τgA [θr]
22 Data accumulation / friction torque removal block 23 τgI [θr] (motor generated torque gravity component)
24 Gravity torque due to load τgL [θr]
25 Amplitude / phase calculation means 26 Amplitude A5
27 Phase φ5
28 Center of gravity position / mass calculation means 29 Xl (load offset X component)
30 Zl (load offset Z component)
31 Yl (load offset Y component)
32 Load mass M
33 Load mass / offset estimation block 34 Inverse reduction ratio 1 / Rg
35 Product of torque constant and reduction ratio 36 Robot joint angle θr
37 Reduction ratio Rg
40 1st axis 41 2nd axis 42 3rd axis 43 4th axis 44 5th axis 45 6th axis 46 Arm length 47 Arm length 48 Load 50 Part of arm of 4th axis 23 52 Measurement operating angle Δθr (5th axis)
53 Measurement operating angle Δθr (6th axis)
54 Positive motor generated torque Kt · Rg · Im [θr] p
55 Negative direction motor generated torque Kt ・ Rg ・ Im [θr] m
56 Data measurement interval dr
57 Data pairing interval Δθr / 2

Claims (3)

A plurality of at least one set of two rotation axes driven by a motor and having a twisted relationship in which a plane formed by a straight line connecting the two rotation center axes and one rotation center axis is substantially orthogonal to the other rotation center axis One of the two rotation axes is the most advanced rotation axis of the robot, and the other is the most recent rotation axis next to the most advanced rotation axis. A method for calculating the mass and the position of the center of gravity of a load attached to a robot that rotates around the rotation axis on the tip side and calculates the mass and the position of the center of gravity of the load attached to the most advanced arm,
Wherein the two rotational axes two points is equal angular velocity motion by a predetermined operation angle smaller than 90 degrees, located half a predetermined angle has a phase interval of said predetermined operation angle plurality set for each axis obtains a motor current measurements at an angle and the angle of each 2-point,
A plurality of amplitudes and phases of the torque component due to the mass of the load are obtained from the angle, the motor current measurement value, and the torque component due to the mass of the arm, and averaged for each axis.
A method for calculating a mass and a center of gravity of a load attached to a robot for calculating a mass and a position of a center of gravity of the load based on an amplitude and a phase of a torque component caused by the average of the mass of the load. The method for calculating a mass and a gravity center position of a load attached to a robot according to claim 1, wherein the predetermined operation angle is greater than 10 degrees. The method for calculating the mass and center of gravity position of a load attached to a robot according to claim 1 or 2 , wherein the plurality of two points are points that are moved by a minute angle in a moving direction of each axis from two predetermined points.

Images