JP4998159B2 - Control method for articulated robot - Google Patents

Description

  The present invention relates to a method for controlling an articulated robot driven by a motor via a speed reducer.

  In recent years, there has been a demand for improved work accuracy in vertical articulated robots used for welding, handling, and the like. However, vertical articulated robots generally have a configuration in which the combined arm length is 1 m or more and the constituent axes are six axes or more. The arm tip easily vibrates due to the bending of the arm and the spring component of the speed reducer mounted on each shaft. In particular, the vibration due to the spring component of the speed reducer has a low frequency of 10 Hz or less in a large robot, and greatly affects the position control performance.

  On the other hand, as a motor for driving a reduction gear having a spring component, an AC synchronous motor having a rotor of a permanent magnet type (usually referred to as a brushless motor) is often employed because of good maintainability and controllability. . However, in this type of motor, it is considered that cogging torque is generated due to a change in magnetic energy in the open slot portion where the stator slot and the rotor magnet face each other. It occurs regularly according to.

  FIG. 1 is an example of an actual measurement value of cogging torque of a 4-pole brushless motor. 24 cycles are generated per rotation of the motor. In the case of a 4-pole motor, the electrical angle is 4 cycles per rotation, so that the cogging torque is generated 6 cycles for one electrical angle cycle.

  FIG. 2 is a schematic diagram for evaluating the vibration at the robot tip due to the cogging torque. The robot shown in FIG. 2 is a general vertical articulated robot having 6 axes, and the combined arm length is about 1.4 m. 2A, a welding torch (108) is attached to the tip of the robot, and a case is considered in which a rectangular parallelepiped work (106) installed in parallel with the surface on which the robot is installed is welded. The direction in which the robot arm retracts, that is, the direction of movement from the posture shown in FIG. 2A to the posture shown in FIG. When the workpiece (106) is operated in parallel with the workpiece (106) at a constant speed, that is, when the distance between the tip of the welding torch (108) and the workpiece (106) is kept constant. The vertical vibration (107) at the tip of the welding torch (108) is measured. In this case, actual welding is not performed, but the distance between the tip of the welding torch (108) and the workpiece (106) is measured using a distance sensor (for example, an eddy current sensor) not shown.

  When the robot moves in the direction of the negative movement direction (105) indicated by the arrow, the rotation direction (102) of the second axis (101) is clockwise, and the rotation direction (104) of the third axis (103) is counterclockwise. Turn clockwise.

  Further, from the posture of FIG. 2 (b), a reverse operation is performed in the plus operation direction (109) indicated by an arrow, and the vertical vibration (107) at the tip of the welding torch (108) at that time is also evaluated in the same manner.

  In the following description, in order to simplify the description, the second axis (101) having a large turning radius to the arm tip and a large degree of influence on the vertical vibration (107) of the tip will be described.

  For example, when the tip speed of the robot is 1.0 m / min, which is an example of a speed often used in welding applications, the rotation speed of the motor when the reduction ratio of the reduction gear of the second axis (101) is around 120. Is around 25 rpm, and the cogging torque period at that time is around 10 Hz. Due to this cogging torque, speed irregularity of the same frequency as the cogging torque occurs in the operating speed of the second axis (101), and appears in the vertical vibration (107) of the tip as vibration of around 10 Hz. And if this frequency approaches the vibration frequency by a reduction gear spring component, it will become difficult to suppress the vibration of a robot.

  On the other hand, when the operating speed of the robot is increased and the frequency of the cogging torque exceeds the vibration frequency due to the reduction gear spring component, the mechanical system including the spring component will not respond even if the force due to the cogging torque is applied. ) Has almost no effect on the vibration of the arm tip. And the low speed which does not exceed the vibration frequency by the reduction gear spring component, for example, in the robot shown in FIG. 2, the maximum rotation speed of the motor is 4000 to 5000 rpm, so the speed affected by the spring component is a low speed less than 10% of the maximum speed. Only the compensation at this low speed needs to be performed.

  Conventionally, as a method of reducing the cogging torque, there is a method of changing the structure of the motor such as a coreless motor or a method of offsetting the change of magnetic energy by applying skew magnetization to the magnet in order to eliminate the change of magnetic energy. However, although there is an effect of reducing the cogging torque, the output torque and the efficiency are reduced.

  Therefore, the compensation torque is derived according to the rotation angle θM from the table storing the rotation angle θM of the motor and the compensation torque that cancels the cogging torque at that angle, and subtracted from the current command to cancel the cogging torque and vibrate. A suppression method is also known (see, for example, Patent Document 1). In this specification, this method is called a feedforward cogging compensation method (hereinafter abbreviated as FF cogging compensation method).

  Hereinafter, the above-described content will be described with reference to the drawings.

  FIG. 3 is a model of a motor and a speed reducer in a robot. Arm 2 is a load in which a motor (2), a speed reducer (3), and a bearing (4) are fixed to an arm 1 (1) serving as a motor mounting base, and is coupled to a rotating portion of a speed reducer secondary side (7). Drive (9). The reduction gear primary side (6) is coupled to the motor rotor (5) at the motor rotation shaft (10) and rotates at the motor rotation speed ωM (11). The reduction gear (3) decelerates the motor rotation speed ωM (11) to the load rotation speed ωL (12) at the reduction ratio Rg.

  However, in the speed reducer (3), there is a spring component between the speed reducer primary side (6) and the speed reducer secondary side (7). It is only a steady state in which the elongation of is constant. FIG. 4 is a block diagram showing the model of FIG. 3 with Ks as the spring constant of the spring component.

  In FIG. 4, icom (13) is a motor current command for driving the motor (2), Kt (14) is a torque constant of the motor (2), 1 / Rg (15, 16) is the reciprocal of the reduction ratio, (17) Is the motor transfer function, (18) is the load transfer function, Ks (19) is the spring constant of the speed reducer (3), and θs (20) is between the speed reducer primary side (6) and the speed reducer secondary side (7). , (21) is an integral element, and Td (22) is an external force applied to the load (arm 2) (9).

  The motor cogging torque Tcog (53) is added as a disturbance to the motor torque output obtained by multiplying the motor current command icom (13) by the motor torque constant Kt (14).

  In the motor transfer function (17), JM is the moment of inertia around the motor rotation shaft (10) including the rotor (5) in the motor and the primary side of the speed reducer (6), and DM is the viscous friction coefficient. In the load transfer function (18), JL is the moment of inertia around the motor rotation shaft (10) that combines the load (arm 2) (9) and the reduction gear secondary side (7), and DL is the viscous friction coefficient. is there.

  FIG. 5 shows a positioning control loop using a conventional cogging correction method for the load (arm 2) (9) of FIG. In FIG. 5, the position control block (27) sets a motor rotation angle θM (24) obtained by integrating a motor rotation speed ωM (11) detected by an encoder or the like connected to the motor with an integration element (25). The motor speed command ωcom (28) is generated by subtracting from θcom (23) and multiplying by the position gain KPP (26).

  The speed control block (31) generates a motor current command icom0 (32) by the following equation based on the motor speed command ωcom (28) and the motor rotation speed ωM (11).

ただし、

However,

  Further, in the cogging torque compensation block (50), cogging torque for one period of cogging torque or a period longer than that is divided by the motor torque constant Kt (14), that is, cogging in advance as motor current data corresponding to the cogging torque. This is stored in the compensation current table (34). The number of cycles to be stored is determined in consideration of the cogging torque regularity and the memory capacity of the control device.

  In the following description, in order to simplify the description, the cogging compensation current table (34) includes data for one period of cogging torque, and in the example of the cogging torque pattern shown in FIG. Is stored as an average value for 24 cycles.

  The encoder detection accuracy of the motor rotation angle θM (24) is assumed to be 2048 pulses per motor rotation. Since the pulse for one cycle of the cogging torque is 2048/24 = 85.333, the cogging compensation current table (34) has data for 85 pulses of the motor detection pulse.

  In the cogging cycle conversion (40), the motor rotation angle θM (24) is divided by the number of cogging cycles per rotation of the motor = 24, and the remainder is output as the cogging compensation current table argument θcog0 (33). The cogging compensation current table (34) outputs the cogging current compensation value icog1 (35) according to the cogging compensation current table argument θcog0 (33) and subtracts it from the motor current command icom0 (32), thereby cogging in feed forward. A motor current command icom (13) for canceling the torque is generated.

With the configuration described above, it is possible to suppress the occurrence of vibration due to motor cogging torque.
JP 62-163591 A

  Since the cogging torque is as small as several percent of the rated torque, the influence of the friction of the motor bearing cannot be ignored during measurement, and it is difficult to measure accurately. For example, as described in Japanese Patent Laid-Open No. 5-149803, cogging torque is measured under static conditions in which the motor is not energized and is rotated at a constant low speed by external power. Is done.

  Further, the cogging phase of the motor and the reference position (origin position) of the encoder must always be the same regardless of the individual. However, it is described as a specification that there is an error of about ± 2 ° (± 2 × 24 = ± 48 ° in the cogging torque cycle) in the motor mechanical angle due to an attachment error of the encoder to the motor.

  For these reasons, when using cogging data that includes measurement errors and individual differences, and when torque is generated by applying current to your motor, the conventional FF cogging compensation method feeds back vibration and adjusts the optimum phase. There is no confirmation that a sufficient vibration suppressing effect can be obtained.

  Therefore, the vibration suppression effect when the posture of the robot changes, the motor torque necessary to support the torque due to gravity increases and decreases, and the motor command current icom0 (32) increases and decreases was evaluated.

  6 and 7 show examples of vibration evaluation postures at positions where the second axis (101) of the robot shown in FIG. 2 is rotated 60 ° and 120 ° clockwise. At this time, the gravity applied to the second axis (101) approaches the minus operation direction in FIG. 2, approaches 0 in FIG. 6, and reverses in the plus operation direction in FIG.

  The method shown in FIG. 5 has a vibration suppressing effect in the vicinity of the posture shown in FIG. 6, but a gravity torque compensation current for supporting gravity in the posture shown in FIGS. 2 and 7 flows to the motor of the second shaft (101). If this is the case, the vibration suppressing effect is reduced as described later.

  Therefore, as shown in FIG. 8, a method of adding the phase shift θoff3 (62) to the cogging compensation current table argument θcog0 (33) by the cogging phase shift means (60) is added. In FIG. 8, the cogging phase shift means (60) changes the phase shift θoff3 (62) in the range of about ± 5 ° (cogging torque ± 1/3 period, encoder pulse ± 28 pulses) in the motor mechanical angle. FIG. 9 shows the relationship between the phase shift θoff3 (62) and the vertical vibration (107) of the tip in the posture of the robot shown in FIG.

  In FIG. 9, the vertical axis represents the improvement rate when the FF cogging compensation is performed for the average amplitude of the vertical vibration (107) of the tip without adding the cogging current compensation value icog1 (35), that is, without the FF cogging compensation. It is. The optimum value of the phase shift θoff3 (62) at which the improvement rate is the largest is not about 0 °, but is about −2.2 ° in the negative operation direction (105) and about −2.6 ° in the positive operation direction (109). Yes, it does not match the phase measured and measured under static conditions, and there is a difference in the optimum phase shift in the operating direction.

  Furthermore, it can be seen from FIG. 9 that if the phase shift amount deviates by about ± 2.5 ° from the optimum phase shift amount, the vibration suppressing effect is almost lost, and if it deviates more than that, there is a possibility of amplifying the vibration.

  Therefore, by changing the angle of the second axis (101) and changing the gravitational torque applied to the second axis (101) as in the robot posture shown in FIGS. This is carried out by changing the gravity compensation current flowing through the motor of (101). FIG. 10 shows the relationship between the gravity compensation current and the optimum phase shift amount for cogging compensation at that time.

  In FIG. 10, the approximate curves of the data in the negative operation direction (105) and the positive operation direction (109) are (70) and (71), respectively, and the optimum phase shift amount is ± 2 depending on the gravity compensation current. It was found to vary by about °.

  If there is an encoder mounting error on the motor (± 2 ° in motor mechanical angle), the cogging compensation optimum phase, combined with the fluctuation due to the gravity compensation current, generates an error of up to ± 4 °, which is discussed in FIG. Thus, since the range of ± 2.5 ° is exceeded, there is a risk that the vibration will be worsened.

  In other words, in the conventional FF cogging compensation method shown in FIG. 5 using the cogging compensation current table (34) which is measured under a static condition and is tabulated, the optimum phase change due to the gravity compensation current and the optimum due to encoder mounting error are obtained. It was found that the phase shift corresponding to the phase error was not performed, and sufficient vibration suppression effect could not be exhibited.

  The present invention solves the above-described problem, and can suppress vibration caused by cogging torque even when there is a change in the optimal phase of cogging compensation due to gravity compensation current or an optimal phase error due to encoder attachment error. The purpose is to realize a robot control method.

In order to solve the above-described problem, a control method for an articulated robot according to the present invention includes a plurality of arms that rotate around a rotation shaft that is driven by a motor, and the motor based on current and rotational position information of the motor. A control method for an articulated robot that controls the operation of the motor including a cogging compensation block that adds a cogging torque compensation value of the motor to the feedback control loop, the motor current The cogging compensation obtained from the rotation angle of the motor is obtained from the gravity compensation current phase compensation angle obtained from the gravity compensation current obtained by processing the low-pass filter and the rotation direction correction angle obtained from the rotation direction of the motor. by adding the phase angle, what determines the optimum cogging compensation phase angle corresponding to a change in posture of the articulated robot That.

The articulated robot control method of the present invention includes a plurality of arms that rotate around a rotation shaft driven by a motor, and configures a feedback control loop for the motor based on the current and rotational position information of the motor. An articulated robot control method for controlling the operation of the motor having a cogging compensation block for adding a cogging torque compensation value of the motor to the feedback control loop, wherein a gravitational torque applied to the rotating shaft is calculated. The gravity compensation current phase compensation angle obtained from the gravity compensation current obtained in this manner and the rotation direction correction angle obtained from the rotation direction of the motor are added to the cogging compensation phase angle obtained from the rotation angle of the motor. Thus, the optimum cogging compensation phase angle corresponding to the posture change of the articulated robot is determined.

  In addition to the above, the articulated robot control method of the present invention adds means for multiplying the cogging torque compensation value by the cogging compensation gain, and processes the motor current when the cogging compensation gain is 0 with a high-pass filter. The cogging phase offset amount caused by the mounting error between the motor and the motor rotational position detector is detected from the obtained value, and the cogging compensation gain is set to 1 after adding the offset amount to the correction amount of the cogging torque phase. .

  In addition to the above, the articulated robot control method of the present invention adds means for multiplying the cogging torque compensation value by the cogging compensation gain, and processes the motor current when the cogging compensation gain is 0 with a high-pass filter. From this value, the offset amount of the cogging phase caused by the attachment error between the motor and the motor rotational position detector is detected. If the absolute value of the offset amount is within a predetermined value, the cogging compensation gain is set to 1. .

  As described above, according to the control method of the articulated robot of the present invention, even if there is a change in the optimal phase of cogging compensation due to gravity compensation current or an optimal phase error due to encoder attachment error, vibration caused by cogging torque Can be suppressed.

(Embodiment 1)
Hereinafter, embodiments of the present invention will be described mainly with reference to FIGS. In addition, the same code | symbol is attached | subjected about the location similar to FIGS. 1-10, and detailed description is abbreviate | omitted.

  FIG. 11 is a diagram showing a control block in the present embodiment.

  In FIG. 10 showing the relationship between the gravity compensation current and the cogging compensation optimum phase shift amount, (70) and (71), which are approximate curves of the minus operation direction (105) and the plus operation direction (109), are shown as a whole. Although there is a typical offset, the change in the optimum phase shift amount with respect to the gravity compensation current is equivalent. Therefore, the optimum phase shift amount is calculated from the gravity compensation current and the operation direction, and the cogging compensation table argument θcog0 (33) is corrected when the FF cogging compensation is performed. If the optimum phase correction angle is Δθgd (54), it can be expressed by the following linear function.

ただし、Δθｇ：重力補償電流補正角、Δθｄ：回転方向補正角、
ｉｇｆ：重力トルク補償電流推定値

Where Δθg: gravity compensation current correction angle, Δθd: rotation direction correction angle,
igf: gravity torque compensation current estimated value

  However, ag1, ag2, gth1, gth2, -ght1, -gth2, bg2, -bg2, bg3, -bg3, [Delta] [theta] dp, [Delta] [theta] dm so as to coincide with the two approximate curves (70) and (71) of FIG. Stipulated in

  FIG. 12 is a graph of Expression (6) in (Equation 4). Igf in equation (6) of (Equation 4) is the gravitational torque compensation current estimated value igf (37) shown in FIG. 11, and is calculated by the gravity compensation current calculation block (36) from the motor current command icom (13). .

  FIG. 13A shows an arm rotation angle (= θM / reduction ratio, reduction ratio = 121) and motor current command icom when the second axis (101) of the robot rotates at a constant angular speed clockwise (102). The relationship with (13) is shown, the horizontal axis is the arm rotation angle, and the vertical axis is the motor current command.

  13 (b-1) and 13 (b-2) respectively show the motor rotation angle θM (24) and the motor current when the arm rotation angle of FIG. 13 (a) is enlarged in the angular direction around 0 ° and 120 °. The relationship of the command icom (13) is shown. The horizontal axis represents the motor rotation angle, and the vertical axis represents the motor current command. In the gravity compensation current calculation block (36) of FIG. 11, the motor current command icom (13) is processed by a low-pass filter, and the gravity torque compensation current estimated value igf (shown in FIGS. 13 (c-1) and (c-2) is displayed. 37) is extracted. The reason why the low-pass filter is used is to remove a cogging component not related to gravity. In FIGS. 13C-1 and 13C-2, the horizontal axis represents the motor rotation angle, and the vertical axis represents the gravity compensation current. In FIG. 13D, the horizontal axis represents the arm rotation angle, and the vertical axis represents the gravity compensation current.

  The gravity compensation current phase compensation (38) in FIG. 11 outputs the gravity compensation current phase compensation angle Δθg (39) from the gravity compensation current estimated value igf (37) according to the equation (6) of (Equation 4).

  The motion direction phase compensation block (42) in FIG. 11 outputs the rotational direction correction angle Δθd (43) according to the equation (7) in (Equation 4). Then, according to Equation (5) of (Equation 4), the optimum phase correction angle Δθgd (54) is calculated by adding the gravity compensation current phase compensation angle Δθg (39).

  By adding the optimum phase correction angle Δθgd (54) to the cogging compensation current table argument θcog0 (33), the cogging compensation current table correction argument θcog1 (44) is obtained.

  By inputting the cogging compensation current table correction argument θcog1 (44) into the cogging compensation current table (34), the cogging current compensation value icog1 (35) subjected to phase compensation corresponding to the gravity compensation current and the operation direction is output. Irrespective of the gravity compensation current and the operation direction, the optimum FF cogging compensation is performed and vibration can be suppressed. The motor current command icom (13) is obtained from the motor command current icom0 (32) and the cogging current compensation value icog1 (35).

(Embodiment 2)
This embodiment will be described with reference to FIG. FIG. 14 shows a control block diagram of the present embodiment. In addition, about the location similar to Embodiment 1, the same code | symbol is attached | subjected and detailed description is abbreviate | omitted.

  In the first embodiment described with reference to FIG. 11, the gravitational torque compensation current estimated value igf (37) is calculated by removing the cogging frequency component from the motor current icom (13) using a low-pass filter. In this method, the processing is simple, but if the motor rotational speed ωM (11) changes, the frequencies of the cogging and the gravity compensation current also fluctuate proportionally, and therefore it is necessary to adjust the cutoff frequency selection of the low-pass filter. .

  Further, it is difficult to completely remove the cogging component, and a phase delay due to the filter is also generated, so that the effect of cogging compensation may be reduced.

  In FIG. 14, the self-axis motor rotation angle θM (24) and the other-axis motor rotation angle θM (41) are applied to the own axis by the gravity compensation current calculation block (36) using the dynamics calculation of the robot. The gravitational torque compensation value estimated value igf (37) is obtained.

  In this method, the processing time spent for the dynamics calculation increases, but the problems in the case of using the filter can be solved, and more stable performance can be exhibited.

(Embodiment 3)
This embodiment will be described with reference to FIGS. In addition, about the location similar to Embodiment 1 and Embodiment 2, the same code | symbol is attached | subjected and detailed description is abbreviate | omitted.

  FIG. 15 shows a control block diagram of the present embodiment. Based on the method described in the first embodiment with reference to FIG. 11, an encoder mounting error Δθoff (described later with reference to FIG. 18) 55) is added to automatically correct the block.

  In FIG. 15, the initial value of the cogging compensation gain Kcog (51) is set to zero. Therefore, the FF cogging compensation current icog2 (52) becomes 0, and the FF cogging compensation is not performed.

  In this state, there is no FF cogging compensation, but normal feedback control is applied. Therefore, the vibration caused by the motor cogging torque Tcog (53) appears in the actual motor rotation angle θM (24), and in order to eliminate the difference from the motor position command θcom (23), as a result, the motor current command icom ( The component 13) includes the cogging torque FB compensation current component icogf (46) having the same frequency as the cogging torque.

  FIG. 16A shows an arm rotation angle (= θM / reduction ratio, reduction ratio = 121) and motor current command icom (13) when the second axis (101) rotates clockwise (102) at a constant angular velocity. ).

  FIG. 16B-1 is an enlarged view of the arm rotation angle near 0 ° in the angle direction, and shows the relationship between the motor rotation angle θM (24) and the motor current command icom (13). In the cogging component extraction block (45) shown in FIG. 15, in order to extract a cogging component having a higher frequency than the gravity compensation current component, high-pass filtering is performed on the current command icom (13) to reverse the polarity. The cogging torque FB compensation current component icogf (46) shown in (c-1) is extracted.

  The cogging torque FB compensation current component icogf (46) has a difference between the actual motor rotation angle θM (24) and the motor position command θcom (23), and then a motor current command icom ( 13) is caused by the change, the phase thereof is a feedback phase lag (hereinafter referred to as the phase correction angle Δθgd (54) when the cogging torque current compensation value icog1 (35) described with reference to FIG. 11 is applied). It will be delayed by Δθdly (abbreviated as FB phase delay).

  FIG. 17 shows the relationship between the cogging torque current compensation value icog1 (35) in the block diagram shown in FIG. 14 and the cogging torque FB compensation current component icogf (46) in the block diagram shown in FIG.

  Here, let us consider a case where there is an individual difference between motors, that is, there is an attachment error Δθoff (55) of the encoder to the motor.

  FIG. 18 is obtained by adding a correspondence when the encoder mounting error Δθoff (55) exists to the method described with reference to FIG. 11, and the cogging correction value θcog2 (49) calculated by the following equation. Is an argument of the cogging compensation current table (34).

  FIG. 19 shows the relationship between the cogging torque current compensation value icog1 (35) in FIG. 18 and the cogging torque FB compensation current component icogf (46) in FIG. The θdly shown in FIG. 19 represents a phase delay due to feedback control, and the delay is caused by the response of the control loop, so that it can be measured in advance without being substantially affected by individual motor differences.

  In this case, since the attachment error Δθoff (55) of the encoder to the motor is different for each individual, when the FF cogging torque compensation is performed in FIG. It is necessary to measure the attachment error Δθoff (55) to be used as a parameter.

  This not only increases the man-hours in the manufacturing stage, but also requires pairing of the robot and the control device having the control parameters, which is disadvantageous in terms of compatibility.

  Therefore, in the cogging phase detection block (47) shown in FIG. 15, by measuring the phase of the cogging torque FB compensation current component icogf (46), the encoder mounting error Δθoff (55) of the encoder is automatically set. An estimated value Δθoffd (48) of the attachment error to the motor is obtained.

  In FIG. 19, the individual difference between the optimum phase correction angle Δθgd (54) and the FB phase delay Δθdly is so small that it can be ignored compared to the error Δθoff (55) of the encoder attached to the motor. Therefore, while always correcting the phase of (Δθdly−Δθgd) with respect to the cogging torque FB compensation current component icogf (46), average it at the cogging cycle (= every motor rotation angle of 15 °) and measure the zero cross point of the waveform. For example, the estimated value Δθoffd (48) of the attachment error of the encoder to the motor can be obtained.

  FIG. 16 and FIG. 20 specifically explain this content. FIG. 16 shows the case of Δθoff = 0 °, and FIG. 20 shows the case of Δθoff = 1.8 °.

  Here, in FIGS. 16A and 20A, the horizontal axis represents the arm rotation angle, and the vertical axis represents the motor current command. In FIGS. 16B-1, 16B-2, 20B-1 and 20B-2, the horizontal axis represents the motor rotation angle, and the vertical axis represents the motor current command. It is. 16 (c-1), FIG. 16 (c-2), FIG. 20 (c-1), and FIG. 20 (c-2), the horizontal axis represents the motor rotation angle, and the vertical axis represents the cogging torque FB. This is the compensation current icogf. 16 (d-1), FIG. 16 (d-2), FIG. 20 (d-1), and FIG. 20 (d-2), the horizontal axis represents the motor rotation angle, and the vertical axis represents the cogging torque FB. The compensation current is icogf2. In FIGS. 16 (e) and 20 (e), the horizontal axis represents the motor rotation angle, and the vertical axis represents the cogging torque FB compensation current average value.

  FIG. 16B-2 is an enlarged view of the arm rotation angle around 120 ° in the angular direction, and shows the relationship between the motor rotation angle θM (24) and the motor current command icom (13). In the cogging component extraction block (45) in FIG. 15, the motor current command icom (13) is subjected to a high-pass filter and inverted to extract the cogging torque FB compensation current component icogf (46) shown in FIG. .

  However, since there is much noise and it is difficult to obtain the zero cross point as it is, it is considered that the cogging torque FB compensation current component icogf (46) is averaged at the cogging cycle (= every motor rotation angle of 15 °).

  In FIGS. 16C-1 and 16C-2, since the gravitational current compensation is different, the optimum phase correction angle θgd (54) is different. If averaging is performed as it is, it becomes a sum of sine waves whose phases are shifted, and the meaning of averaging is lost. Accordingly, the value obtained by correcting the phase of (Δθdly−Δθgd) of the cogging torque FB compensation current component icogf (46) is icogf2, and the values shown in FIGS. 16 (d-1) and (d-2) are averaged. To do.

  When the phase correction of (Δθdly−Δθgd) is performed on the cogging torque FB compensation current component icogf (46) at any angle of the arm rotation angle, the phase is the same as in FIGS. 16 (d-1) and (d-2). Therefore, it is possible to continuously average while the arm angle moves from 0 to 120 °.

  FIG. 16E shows an average of icogf2 in a cogging cycle (= every motor rotation angle of 15 °). If the zero cross point in FIG. 16 (e) is θ0, the midpoint of the cogging cycle is 15/2 = 7.5 °, and therefore the estimated value Δθoffd (48) of the encoder mounting error to the motor is given by the following equation. Desired.

  In FIG. 16E, since the zero cross point θ0 = 7.5 °, Δθoffd = 0.0 can be obtained.

  FIG. 20 shows a case where Δθoff = 1.8 °, and averages icogf2 between the operations of FIGS. 20D-1 to 20D-2 in which the phase correction of (Δθdly−Δθgd) is performed. Then, as shown in FIG. 20 (e), the zero cross point θ0 = 9.3 °, and Δθoffd = 1.8 ° is calculated from the equation (10).

  It has been shown that the above method can be estimated from the motor current command icom (13) without examining the encoder mounting error Δθoff (55) of the encoder in advance.

  Here, for the sake of simplicity, the case where the second axis (101) rotates clockwise (102) at a constant angular velocity is shown. However, the cogging torque FB compensation current component icogf (46) is always (Δθdly). Since the phase correction of -Δθgd) is performed, estimation is possible even if the posture and speed are not constant.

  However, when the motor rotation speed increases and the vibration due to cogging becomes higher than the resonance frequency of the mechanism and the phenomenon as vibration does not appear, the cogging torque FB having the same frequency as the cogging torque is included in the component of the motor current command icom (13). Since the compensation current component icogf (46) is not included, estimation is impossible.

  However, since vibration suppression is originally performed when the vibration due to cogging is lower than the resonance frequency of the mechanism, this estimation itself is also performed within that range.

  After obtaining the estimated value Δθoffd (48) of the attachment error of the encoder to the motor, in FIG. 15, the cogging phase detection block (47) sends an ON signal Kon (56) to the cogging compensation gain Kcog (51). , Kcog (51) is set to 1, and the cogging correction value θcog2 (49) calculated by the following equation is input to the cogging compensation current table (34), and FF considering up to Δθoff (55) indicating individual differences The cogging compensation current icog2 (52) is added to the current command icom0 (32).

  However, if Kcog (51) is suddenly switched from 0 to 1, the motor current command icom (13) becomes discontinuous, which may induce vibration. Therefore, after receiving the ON signal Kon (56), it is desirable that Kcog (51) gradually increases from 0 to reach 1.

(Embodiment 4)
This embodiment will be described with reference to FIG. FIG. 21 shows a control block diagram of the present embodiment. In addition, the same code | symbol is attached | subjected about the part similar to Embodiment 1 or Embodiment 3, and detailed description is abbreviate | omitted.

  In the third embodiment described with reference to FIG. 15, the gravitational torque compensation current estimated value igf (37) is calculated by removing the cogging frequency component from the motor current icom (13) using a low-pass filter. , The gravity torque applied to the own axis is calculated from the motor rotation angle θM (24) of the own axis and the motor rotation angle θM (41) of the other axis by using the dynamics calculation of the robot, and the estimated compensation current value igf (37) is obtained.

  In this method, the processing time spent for the dynamics calculation increases, but the problems in the case of using the filter can be solved, and more stable performance can be exhibited.

(Embodiment 5)
This embodiment will be described with reference to FIG. FIG. 22 shows a control block diagram of this embodiment. In addition, the same code | symbol is attached | subjected about the part similar to Embodiment 1 or Embodiment 4, and detailed description is abbreviate | omitted.

  The cogging phase detection block (47) obtains the estimated value Δθoffd (48) of the attachment error of the encoder to the motor by Expression (10) of (Equation 7), and if the following condition is satisfied, the ON signal Kon ( 56) and Kcog (51) is changed from 0 to 1.

ただし、
θｔｈ：コギング補償有効判定 位相閾値

However,
θth: Cogging compensation valid judgment Phase threshold

  In the third embodiment, cogging compensation is performed by correcting the phase with the estimated value Δθoffd (48) of the attachment error of the encoder to the motor, whereas in this embodiment, Δθoffd (48). Is within a predetermined value θth, cogging compensation is performed without correcting the phase. If this method is adopted, the ability to cope with individual differences in encoder attachment error Δθoff (55) of the encoder is weakened, but the estimated error magnitude of the estimated error θoffd (48) of attachment of the encoder to the motor is small. Since it is not directly reflected on the cogging compensation phase, more stable operation can be obtained.

(Embodiment 6)
This embodiment will be described with reference to FIG. FIG. 23 shows a control block diagram of the present embodiment. In addition, the same code | symbol is attached | subjected about the part similar to Embodiment 1 or Embodiment 5, and detailed description is abbreviate | omitted.

  In the fifth embodiment described with reference to FIG. 22, the gravitational torque compensation current estimated value igf (37) is calculated by removing the cogging frequency component from the motor current icom (13) using a low-pass filter. , The gravity applied to the own axis from the own-axis motor rotation angle θM (24) and the other-axis motor rotation angle θM (41) using the dynamics calculation of the robot by the gravity compensation current calculation block (36). The torque is calculated to obtain the compensation current estimated value igf (37).

  In this method, the processing time spent for the dynamics calculation increases, but the problems in the case of using the filter can be solved, and more stable performance can be exhibited.

  The control method of the articulated robot of the present invention is the FF cogging compensation method, in which the vibration caused by the cogging torque is detected even when the optimum phase change of the cogging compensation due to the gravity compensation current or the optimum phase error due to the encoder mounting error exists. Since it can be suppressed, it is industrially useful as a robot control method.

Diagram showing an example of motor cogging torque Reference diagram to explain the evaluation method of robot tip vibration caused by cogging torque Schematic configuration diagram showing the spring component of the reducer of the robot A block diagram modeling the spring component of a robot reducer Block diagram showing a position control loop including conventional cogging compensation Reference diagram to explain the evaluation method of robot tip vibration caused by cogging torque Reference diagram to explain the evaluation method of robot tip vibration caused by cogging torque Block diagram showing a position control loop that evaluates the relationship between cogging compensation phase angle and vibration suppression effect Diagram showing the relationship between cogging compensation phase shift and vibration suppression improvement rate The figure which shows the relationship between the gravity torque compensation current and the cogging compensation optimum phase in the second shaft motor FIG. 3 is a block diagram showing a position control loop including cogging compensation in Embodiment 1 of the present invention. Graphical representation of the formula for calculating the gravity compensation current correction angle (A) Diagram showing relationship between arm rotation angle and motor current command (b-1) Diagram showing relationship between motor rotation angle and motor current command (b-2) Diagram showing relationship between motor rotation angle and motor current command ( c-1) Diagram showing relationship between motor rotation angle and motor current command (c-2) Diagram showing relationship between motor rotation angle and motor current command (d) Diagram showing relationship between arm rotation angle and motor current command Block diagram showing a position control loop including cogging compensation in Embodiment 2 of the present invention Block diagram showing a position control loop including cogging compensation in Embodiment 3 of the present invention (A) Diagram showing relationship between arm rotation angle and motor current command (b-1) Diagram showing relationship between motor rotation angle and motor current command (b-2) Diagram showing relationship between motor rotation angle and motor current command ( c-1) Diagram showing relationship between motor rotation angle and cogging torque FB compensation current (c-2) Diagram showing relationship between motor rotation angle and cogging torque FB compensation current (d-1) Motor rotation angle and cogging torque FB compensation (D-2) A diagram showing the relationship between currents (d-2) A diagram showing the relationship between motor rotation angle and cogging torque FB compensation current (e) A diagram showing the relationship between motor rotation angle and cogging torque FB compensation current average value 14 is a diagram showing the relationship between the cogging torque current compensation value of FIG. 14 and the cogging torque FB compensation current component of FIG. 15 with respect to the motor rotation angle. The block diagram which added the correspondence when the attachment error to the motor of an encoder exists in the block diagram of FIG. 18 is a diagram showing the relationship between the cogging torque current compensation value and the cogging torque FB compensation current component of FIG. 18 with respect to the motor rotation angle. (A) Diagram showing relationship between arm rotation angle and motor current command (b-1) Diagram showing relationship between motor rotation angle and motor current command (b-2) Diagram showing relationship between motor rotation angle and motor current command ( c-1) Diagram showing relationship between motor rotation angle and cogging torque FB compensation current (c-2) Diagram showing relationship between motor rotation angle and cogging torque FB compensation current (d-1) Motor rotation angle and cogging torque FB compensation (D-2) A diagram showing the relationship between currents (d-2) A diagram showing the relationship between motor rotation angle and cogging torque FB compensation current (e) A diagram showing the relationship between motor rotation angle and cogging torque FB compensation current average value Block diagram showing a position control loop including cogging compensation in the fourth embodiment of the present invention Block diagram showing a position control loop including cogging compensation in the fifth embodiment of the present invention Block diagram showing a position control loop including cogging compensation in the sixth embodiment of the present invention

Explanation of symbols

1 Arm 1
2 Motor 3 Reducer 4 Bearing 5 Motor rotor 6 Reducer primary side 7 Reducer secondary side 8 Spring 9 Arm 2 (Load)
10 Motor rotation shaft 11 Motor rotation speed ωM
12 Load rotation speed ωL
13 Motor current command icom
14 Motor torque constant Kt
15 Reciprocal of reduction ratio 16 Reciprocal of reduction ratio 17 Motor transfer function 18 Load transfer function 19 Spring constant Ks
20 Helix angle θs
21 integral element 22 external force Td
23 Motor position command θcom
24 Motor rotation angle θM
25 integral element 26 position gain KPP
27 Position control block 28 Motor speed command ωcom
29 Speed proportional gain KP
30 Speed integral gain KI
31 Speed control block 32 Motor current command icom0 (speed loop block output)
33 Cogging compensation current table argument θcog0
34 Cogging compensation current table 35 Cogging current compensation value icog1
36 Gravity compensation current calculation block 37 Gravity torque compensation current estimated value igf
38 Gravity compensation current phase compensation 39 Gravity compensation current phase compensation angle Δθg
40 Cogging cycle conversion 41 Motor rotation angle θM of other axis
42 Operation direction phase compensation block 43 Rotation direction correction angle Δθd
44 Cogging compensation current table correction argument θcog1
45 Cogging component extraction block 46 Cogging torque FB compensation current component icogf
47 Cogging phase detection block 48 Estimated error of encoder attachment to motor Δθoffd
49 Cogging correction value θcog2
50 Cogging torque compensation block 51 Cogging compensation gain Kcog
52 FF cogging compensation current icog2
53 Motor cogging torque Tcog
54 Optimum phase correction angle Δθgd
55 Encoder motor installation error Δθoff
56 ON signal Kon
60 Cogging phase shift means 62 Phase shift θoff3
70 Approximate curve (minus operating direction)
71 Approximate curve (plus movement direction)
75 Block indicating the motor, speed reducer and load of the robot 101 Second shaft 102 Rotating direction 103 Third shaft 104 Rotating direction 105 Negative operating direction 106 Work piece 107 Vertical vibration at the tip 108 Welding torch 109 Positive operating direction

Claims (4)

A plurality of arms that rotate about a rotating shaft driven by a motor; and a feedback control loop of the motor is configured based on the current and rotational position information of the motor, and the cogging torque compensation value of the motor is set to the feedback control loop An articulated robot control method for controlling the operation of the motor including a cogging compensation block to be added to
The gravity compensation current phase compensation angle obtained from the gravity compensation current obtained by processing the motor current with a low-pass filter and the rotation direction correction angle obtained from the rotation direction of the motor are obtained from the rotation angle of the motor. A control method for an articulated robot that determines an optimal cogging compensation phase angle corresponding to a posture change of the articulated robot by adding to the cogging compensation phase angle . A plurality of arms that rotate about a rotating shaft driven by a motor; and a feedback control loop of the motor is configured based on the current and rotational position information of the motor, and the cogging torque compensation value of the motor is set to the feedback control loop An articulated robot control method for controlling the operation of the motor including a cogging compensation block to be added to
The gravity compensation current phase compensation angle obtained from the gravity compensation current obtained by calculating the gravity torque applied to the rotation shaft, and the rotation direction correction angle obtained from the rotation direction of the motor are calculated from the rotation angle of the motor. An articulated robot control method for determining an optimum cogging compensation phase angle corresponding to a posture change of the articulated robot by adding to the obtained cogging compensation phase angle . A means for multiplying the cogging torque compensation value by the cogging compensation gain is added, and the value of the cogging phase caused by the mounting error between the motor and the motor rotational position detector is obtained from the value obtained by processing the motor current when the cogging compensation gain is 0 with a high-pass filter. The method of controlling an articulated robot according to claim 1 or 2, wherein an offset amount is detected and the cogging compensation gain is set to 1 after adding the offset amount to a correction amount of a cogging torque phase. A means for multiplying the cogging torque compensation value by the cogging compensation gain is added, and the value of the cogging phase caused by the mounting error between the motor and the motor rotational position detector is obtained from the value obtained by processing the motor current when the cogging compensation gain is 0 with a high-pass filter. 3. The articulated robot control method according to claim 1, wherein an offset amount is detected, and the cogging compensation gain is set to 1 if the absolute value of the offset amount is within a predetermined value.

Images