JP2018051650A - Control device for robot - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a control device for a robot which can suppress vibration of the robot and can be easily applied in an actual robot.SOLUTION: A control device 50 for controlling a robot 10 comprises: a command value calculating portion 51 that calculates a command value for driving a driving portion 32 so that a moving portion 22 is moved to a closest target position; an acceleration/deceleration calculating portion 52 that calculates acceleration/deceleration when the command value varies; a centroid distance calculating portion 52 that calculates, on the basis of a centroid of the prescribed shaft 23 and an object W as a whole and a position of the prescribed shaft in a vertical direction, a centroid distance, a distance from a support portion where the moving portion supports the prescribed shaft to the centroid; a modified command value calculating portion 52 that calculates a modified command value determined by modifying the command value on the basis of the command value, spring constant of the supporting portion relative to bending of the prescribed shaft, entire mass, the acceleration/deceleration and the centroid distance so as to make a position of the centroid projected on a track approach the target position; and a driving control portion 53 that controls the driving portion on the basis of the modified command value.SELECTED DRAWING: Figure 1

Description

  The present invention relates to a control device that controls the operation of a robot.

  Conventionally, an acceleration sensor that detects the acceleration amount is attached near the tip of the arm of the robot, and based on the acceleration amount detected by the acceleration sensor, the drive unit of each joint unit is controlled so as to suppress vibration generated at the tip of the arm. There is one that compensates a command value to be controlled (see Patent Document 1).

Japanese Patent Laid-Open No. 10-100085

  By the way, in the method using the acceleration amount detected by the acceleration sensor in this way, it is necessary to attach the acceleration sensor in the vicinity of the tip of the arm that actually generates vibration. However, when applied to an actual robot, the space for mounting the acceleration sensor in the vicinity of the tip of the arm, the wiring of the wiring to the acceleration sensor, and the like are problematic and difficult to realize.

  The present invention has been made in view of such circumstances, and a main object of the present invention is to provide a robot control apparatus that can suppress vibration of the robot and can be easily applied to an actual robot. is there.

The first means for solving the above problems is as follows.
A driving unit, a moving unit that is driven by the driving unit and reciprocates on a predetermined track along a horizontal plane, and a predetermined axis that is supported by the moving unit so as to reciprocate along a vertical direction and has an object attached to a lower portion thereof. A robot control device for controlling a robot comprising:
A command value calculating unit for calculating a command value for driving the driving unit so as to move the moving unit to the nearest target position;
An acceleration / deceleration calculation unit for calculating an acceleration / deceleration when the command value calculated by the command value calculation unit changes;
Based on the predetermined axis and the center of gravity of the entire object and the position of the predetermined axis in the vertical direction, the moving unit calculates a center-of-gravity distance that is a distance from the support unit that supports the predetermined axis to the center of gravity. A center-of-gravity distance calculation unit;
The command value calculated by the command value calculation unit, the spring constant of the support unit with respect to bending of the predetermined axis, the overall mass, the acceleration / deceleration calculated by the acceleration / deceleration calculation unit, and the gravity center distance calculation unit Based on the center-of-gravity distance calculated by the step, the corrected command value obtained by correcting the command value calculated by the command value calculation unit is calculated so that the position of the center of gravity projected onto the trajectory approaches the target position. A correction command value calculation unit to
A drive control unit that controls the drive unit based on the correction command value calculated by the correction command value calculation unit;
Is provided.

  According to the above configuration, the robot includes the driving unit, the moving unit, and the predetermined axis. The moving unit is driven by the driving unit and reciprocates on a predetermined track along a horizontal plane. The predetermined axis is supported by the moving unit so as to be able to reciprocate along the vertical direction, and an object is attached to the lower part.

  A command value for driving the drive unit is calculated by the command value calculation unit so as to move the moving unit to the nearest target position. The acceleration / deceleration calculation unit calculates the acceleration / deceleration when the command value calculated by the command value calculation unit changes. The acceleration / deceleration at the time when the command value changes correlates with the acceleration / deceleration that moves the moving unit to the nearest target position.

  When the moving unit accelerates or decelerates, the moving unit is the same as the case where the entire mass is at the position of the center of gravity distance that is the distance from the support unit that supports the predetermined axis to the predetermined axis and the entire center of gravity of the object. Inertial force acts. Here, since the predetermined axis is supported so as to reciprocate along the vertical direction, the center-of-gravity distance changes according to the operation state of the predetermined axis, and the height at which the inertial force changes changes. At this point, the center-of-gravity distance calculation unit calculates the center-of-gravity distance based on the overall center of gravity and the position of the predetermined axis in the vertical direction.

  Due to the inertial force, the position of the center of gravity projected onto the predetermined trajectory is displaced from the target position in accordance with the spring constant of the support portion with respect to the bending of the predetermined axis. When the position of the center of gravity projected onto a predetermined trajectory is shifted from the target position, vibration is generated on a predetermined axis when the shift is eliminated. This inertial force can be calculated based on the overall mass and the acceleration / deceleration when the command value correlated with the acceleration / deceleration of the moving unit changes. Furthermore, the amount by which the position of the center of gravity projected onto a predetermined trajectory is displaced from the target position can be calculated based on the inertial force, the center of gravity distance, and the spring constant.

  Therefore, the correction command value calculation unit calculates the center of gravity projected onto a predetermined trajectory based on the command value, the spring constant, the overall mass, the acceleration / deceleration when the command value changes, and the center of gravity distance. A corrected command value obtained by correcting the command value calculated by the command value calculation unit is calculated so as to bring the position closer to the target position. That is, a corrected command value is calculated by correcting the command value so as to reduce the deviation between the position of the center of gravity projected onto a predetermined trajectory and the target position. The drive control unit controls the drive unit based on the calculated correction command value. For this reason, when the deviation between the position of the center of gravity projected onto the predetermined trajectory and the target position is eliminated, it is possible to suppress the occurrence of vibration on the predetermined axis. Furthermore, since it is not necessary to attach an acceleration sensor to a portion where vibration actually occurs, the correction command value can be calculated based on the command value, the known value, the acceleration / deceleration of the command value, and the center of gravity distance. It can be easily applied to robots.

  In the second means, the correction command value calculation unit calculates the correction command value so that the moving unit is advanced from the target position when the moving unit is accelerated.

  When the moving unit accelerates, an inertial force in a direction opposite to the moving direction of the moving unit acts on the predetermined axis and the entire object. For this reason, the entire center of gravity projected onto the predetermined trajectory moves later than the moving unit. In this regard, according to the above configuration, the correction command value calculation unit calculates the correction command value so that the moving unit is advanced from the target position when the moving unit is accelerated. Therefore, the correction command value can be calculated so as to reduce the deviation between the position of the center of gravity projected onto the predetermined trajectory and the target position.

  In the third means, the correction command value calculation unit calculates the correction command value so that the moving unit is delayed from the target position when the moving unit is decelerated.

  When the moving unit decelerates, an inertial force in the same direction as the moving direction of the moving unit acts on the predetermined axis and the entire object. For this reason, the above-mentioned center of gravity projected onto a predetermined trajectory moves ahead of the moving unit. In this regard, according to the above configuration, the correction command value calculation unit calculates the correction command value so that the moving unit is delayed from the target position when the moving unit is decelerated. Therefore, the correction command value can be calculated so as to reduce the deviation between the position of the center of gravity projected onto the predetermined trajectory and the target position.

  In a fourth means, the correction command value calculation unit calculates the correction command value so that the position of the center of gravity projected onto the trajectory matches the target position.

  According to the above configuration, the correction command value calculation unit calculates the correction command value so that the position of the center of gravity projected onto the predetermined trajectory matches the target position. For this reason, the deviation between the position of the center of gravity projected onto the predetermined trajectory and the target position can be minimized, and the occurrence of vibration on the predetermined axis can be further suppressed.

  In the fifth means, the correction command value calculation unit changes the correction command value to a command value obtained by calculating the calculated correction command value.

  If the acceleration / deceleration of the command value changes abruptly, the corrected command value calculated based on the acceleration / deceleration of the command value may change abruptly. In this case, there is a possibility that a torque larger than the upper limit torque that can be generated by the drive unit is required for the drive unit controlled based on the correction command value. In this regard, according to the above-described configuration, the correction command value calculation unit changes the correction command value to a command value obtained by calculating the calculated correction command value. Therefore, it is possible to suppress a sudden change in the correction command value, and it is possible to suppress a request for torque larger than the upper limit torque from the drive unit.

  In the sixth means, the moving unit reciprocates on a predetermined arc orbit along a horizontal plane around a predetermined center axis, and the spring constant is K, the overall mass is M, and the target position is corresponded. The target angular position of the moving unit centered on the central axis is θ, the angular acceleration / deceleration at which the target angular position θ changes is a, the center of gravity distance is L, and the correction amount of the target angular position θ is Δθ. The command value calculation unit calculates the target angular position θ as the command value, and the correction command value calculation unit calculates the correction amount Δθ calculated by the equation: Δθ = M · a · L ^ 2 / K In addition to the target angular position θ, the correction command value is calculated.

  According to the said structure, a moving part reciprocates on a predetermined | prescribed circular arc track along a horizontal surface centering | focusing on a predetermined | prescribed center axis line. In this case, the target angular position θ of the moving unit around the central axis corresponding to the target position can be adopted as a command value for driving the driving unit so as to move the moving unit to the nearest target position. . Then, the correction command value calculation unit can calculate the correction command value by adding the correction amount Δθ calculated by the equation of Δθ = M · a · L ^ 2 / K to the target angular position θ. Therefore, the correction command value can be easily calculated and can be easily applied to an actual robot.

  In a seventh means, the robot includes: a second moving unit as the moving unit; and a first moving unit that reciprocates on a predetermined first arc trajectory along a horizontal plane about a predetermined central axis. The second moving unit is supported by the first moving unit so as to be movable, and reciprocates on a predetermined second circular arc track as the predetermined circular arc track along a horizontal plane with the first moving unit as a center. And processing by the command value calculation unit, the acceleration / deceleration calculation unit, the center of gravity distance calculation unit, the correction command value calculation unit, and the drive control unit for the first moving unit and the second moving unit, respectively Execute.

  According to the above configuration, the robot includes the second moving unit as the moving unit, and the first moving unit that reciprocates on the predetermined first circular arc trajectory along a horizontal plane about the predetermined central axis. ing. The second moving unit is movably supported by the first moving unit, and reciprocates on a predetermined second arc track as a predetermined arc track along the horizontal plane with the first moving unit as a center. Therefore, as a robot, a first axis that rotates along the horizontal plane, a second axis that is rotatably supported by the first axis along the horizontal plane, and a reciprocating motion along the vertical direction at the tip of the second axis It is possible to employ a horizontal articulated robot having a third axis.

  Then, the command value calculation unit, the acceleration / deceleration calculation unit, the gravity center distance calculation unit, the correction command value calculation unit, and the drive control unit are executed for the first moving unit and the second moving unit, respectively. Therefore, in the horizontal articulated robot, the vibration of the third axis as the predetermined axis can be suppressed.

The schematic diagram which shows a horizontal articulated robot and a robot controller. The schematic diagram which shows the vibration state of a 3rd axis | shaft. The schematic diagram which shows the operation | movement model of a horizontal articulated robot. The schematic diagram which shows the relationship between the target position and gravity center position in a comparative example and this embodiment at the time of operation | movement of a 2nd axis | shaft. The front view which shows the gravity center distance. The top view which shows the length and angular position of a 1st axis | shaft and a 2nd axis | shaft. The flowchart which shows the procedure of damping control. The time chart which shows the angular velocity pattern in a comparative example and this embodiment. The time chart which shows the shift | offset | difference with the target stop position and the projection position of a gravity center in a comparative example and this embodiment.

  Hereinafter, an embodiment will be described with reference to the drawings. This embodiment is embodied in an industrial robot used in an assembly line such as a machine assembly factory.

  As shown in FIG. 1, the horizontal articulated robot 10 includes a base 20, a first axis 21, a second axis 22, a third axis 23, and the like.

  The base 20 is fixed to the floor. A first shaft 21 is connected to the base 20 through a bearing (bearing) or the like so as to be horizontally rotatable. The first shaft 21 is horizontally rotatable around an axis C1 (corresponding to a predetermined center axis). A first moving part (moving part) is configured by the tip of the first shaft 21. The first moving unit reciprocates on a predetermined first circular arc track along the horizontal plane with the axis C1 as the center.

  A second shaft 22 is connected to the tip of the first shaft 21 through a bearing or the like so as to be horizontally rotatable. The second shaft 22 is horizontally rotatable around the axis C2. The second moving part is configured by the tip of the second shaft 22. The second moving portion is supported by the tip end portion (first moving portion) of the first shaft 21 so as to be movable, and a predetermined second arc trajectory (predetermined arc trajectory) along the horizontal plane with the first moving portion as a center. , Corresponding to a predetermined trajectory).

  A third shaft 23 is connected to the tip of the second shaft 22 so as to be movable up and down. The third shaft 23 (corresponding to a predetermined axis) is supported by the tip end portion (second moving portion) of the second shaft 22 so as to reciprocate along the axis C3 (vertical direction). Further, the third shaft 23 is horizontally rotatable around the axis C3. An object W corresponding to a tool or a work (part) that performs a predetermined operation is attached to the lower portion of the third shaft 23.

  A first drive unit 31 that horizontally rotates the first shaft 21 is fixed to the base 20. The first drive unit 31 (corresponding to the drive unit) includes a motor, a transmission mechanism, a speed reduction mechanism, and the like. The output shaft of the motor is connected to the base end portion of the first shaft 21 via a transmission mechanism and a speed reduction mechanism. The speed reduction mechanism is fixed to the base 20.

  A second drive unit 32 that horizontally rotates the second shaft 22 is fixed to the first shaft 21. The second drive unit 32 (corresponding to the drive unit) includes a motor, a transmission mechanism, a speed reduction mechanism, and the like. The output shaft of the motor is connected to the base end portion of the second shaft 22 via a transmission mechanism and a speed reduction mechanism.

  Fixed to the second shaft 22 is a third drive unit 33 that vertically reciprocates the third shaft 23. The third drive unit 33 includes a motor, a transmission mechanism, a speed reduction mechanism, and the like. The output shaft of the motor is connected to the third shaft 23 via a transmission mechanism and a speed reduction mechanism. The transmission mechanism and the speed reduction mechanism include a belt, a pulley, a ball screw nut, and the like.

  A fourth drive unit that horizontally rotates the third shaft 23 is fixed to the second shaft 22. The fourth drive unit (not shown) includes a motor, a transmission mechanism, a speed reduction mechanism, and the like. The output shaft of the motor is connected to the third shaft 23 via a transmission mechanism and a speed reduction mechanism.

  According to such a configuration, the first drive unit 31 drives the first shaft 21 to rotate horizontally. The first shaft 21 is supported by a bearing or the like and rotated horizontally. The second shaft 22 is driven to rotate horizontally by the second drive unit 32. The second shaft 22 is supported by a bearing or the like and rotated horizontally. The third driving unit 33 drives the third shaft 23 to reciprocate vertically. The third shaft 23 is driven to rotate horizontally by the fourth drive unit.

  The operation of the robot 10 is controlled by the robot controller 50. The controller 50 (corresponding to a control device) includes a CPU, a ROM, a RAM, a drive circuit, a position detection circuit, and the like. The ROM stores system programs and operation programs for the robot 10. The RAM stores parameter values and the like when executing these programs. Detection signals from encoders (not shown) provided at each joint of the robot 10 are input to the position detection circuit. The position detection circuit detects the rotation angle position of the motor provided at each joint based on the detection signal of each encoder. The controller 50 constitutes a trajectory generation unit 51, a correction processing unit 52, and a servo mechanism 53.

  The controller 50 calculates the target stop position of the tip of the second shaft 22 based on the operation program of the robot 10 and the like. Then, the controller 50 calculates the target stop angle position of each axis based on the target stop position. The trajectory generation unit 51 generates an angular velocity pattern of each axis when changing the angular position of each axis to the target stop angle position based on the target stop angle position of each axis. The trajectory generation unit 51 calculates a command value θan that is the nearest target angular position of each axis based on the angular velocity pattern of each axis. The servo mechanism 53 (corresponding to a drive control unit) is configured so that each drive unit has an angular position of each axis as a command value θan based on the angular position θn of each axis detected by each encoder and the command value θan. 31 to 33 are controlled by each driving torque Tn. The correction processing unit 52 will be described later.

  Incidentally, the tip of the third shaft 23 may vibrate when the first shaft 21 is driven to rotate horizontally or the second shaft 22 is driven to rotate horizontally. FIG. 2 is a schematic diagram showing a vibration state of the third shaft 23.

  The third shaft 23 is supported by the support portion 22a of the second shaft 22 so as to reciprocate along the axis C3. The support portion 22a (corresponding to the second moving portion and the moving portion) is configured by a transmission mechanism, a speed reduction mechanism, and the like of the third driving portion 33, and includes a frame (driven portion) of the second shaft 22 and a third shaft 23. The rigidity is low. For this reason, the support portion 22a acts as a spring element having a spring constant K with respect to a force in a direction in which the third shaft 23 is bent. Therefore, when the distal end portion of the second shaft 22 accelerates or decelerates, the distal end of the third shaft 23 vibrates around the support portion 22a due to the inertial force acting on the third shaft 23 and the object W. In particular, the greater the mass of the object W, the greater the vibration from the support portion 22a to the object W.

  Therefore, in the present embodiment, the controller 50 regards the robot 10 as an operation model shown in FIG. 3 and executes vibration suppression control that suppresses vibration of the third shaft 23. As shown in the figure, the first axis 21 and the second axis 22 that are horizontal rotation axes of the robot 10 are fixed models in which the state of supporting the axes 21 and 22 does not change. On the other hand, the third axis 23 which is the vertical reciprocating axis of the robot 10 is a variable model in which the position where the third axis 23 is supported by the support portion 22a changes. Further, the third axis 23 and the object W are added together to obtain the entire axis WA of the third axis 23 and the object W. Then, it is assumed that the mass M of the entire WA exists at the center of gravity of the entire WA.

  FIG. 4 is a schematic diagram showing the relationship between the target position and the center of gravity position in the comparative example and this embodiment during the operation of the second shaft 22. FIG. 4A shows a comparative example in which control is performed so that the position of the support portion 22a (tip portion) of the second shaft 22 is aligned with the target position indicated by a broken line corresponding to the command value θa of the second shaft 22. . FIG. 4B shows the present embodiment in which control is performed so that the position of the center of gravity of the entire WA projected on the trajectory R is aligned with the target position indicated by a broken line corresponding to the command value θa of the second axis 22.

  As shown in FIG. 4A, in the comparative example, when the support portion 22a is accelerated at an acceleration a, an inertial force -Ma in the direction opposite to the moving direction of the support portion 22a acts on the entire WA. For this reason, the support portion 22a is elastically deformed, and the center of gravity of the entire WA projected onto the track R moves later than the support portion 22a. And when the support part 22a transfers to the time of constant speed, inertia force -Ma becomes small and the elastic deformation of the support part 22a returns. When the elastic deformation of the support portion 22a is restored, vibration is generated in the third shaft 23.

  Thereafter, when the support portion 22a decelerates at the deceleration −a, an inertia force Ma in the same direction as the movement direction of the support portion 22a acts on the entire WA. For this reason, the support portion 22a is elastically deformed, and the center of gravity of the entire WA projected onto the track R moves ahead of the support portion 22a. When the movement of the support portion 22a is completed, the inertial force Ma is reduced, and the elastic deformation of the support portion 22a is restored. When the elastic deformation of the support portion 22a is restored, vibration is generated in the third shaft 23.

  As shown in FIG. 4B, also in the present embodiment, when the support portion 22a accelerates with the acceleration a, the inertia force -Ma in the direction opposite to the moving direction of the support portion 22a acts on the entire WA. Here, in this embodiment, at the time of acceleration of the support part 22a, the support part 22a is advanced rather than the target position of a broken line. Specifically, in consideration of the amount of elastic deformation of the support portion 22a, the position of the center of gravity of the entire WA projected on the trajectory R is aligned with the broken target position. Then, when the support portion 22a shifts to the constant speed, the amount of advance of the support portion 22a with respect to the target position is decreased as the inertial force -Ma becomes smaller and the elastic deformation of the support portion 22a returns. . For this reason, when the elastic deformation of the support portion 22a returns to the original, the occurrence of vibration in the third shaft 23 is suppressed.

  Thereafter, when the support portion 22a decelerates at the deceleration −a, an inertia force Ma in the same direction as the movement direction of the support portion 22a acts on the entire WA. Here, in the present embodiment, when the support portion 22a is decelerated, the support portion 22a is delayed from the target position indicated by the broken line. Specifically, in consideration of the amount of elastic deformation of the support portion 22a, the position of the center of gravity of the entire WA projected on the trajectory R is aligned with the broken target position. Then, when the movement of the support portion 22a is finished, the inertial force Ma is reduced, and the delay amount of the support portion 22a with respect to the target position is reduced as the elastic deformation of the support portion 22a returns. For this reason, when the elastic deformation of the support portion 22a returns to the original, the occurrence of vibration in the third shaft 23 is suppressed.

  As shown in FIG. 5, when the tip end portion of the second shaft 22 accelerates or decelerates, the center of gravity of the entire shaft WA of the third shaft 23 and the object W from the support portion 22 a where the second shaft 22 supports the third shaft 23. An inertial force similar to that in the case where there is the mass M of the entire WA is applied at the position of the center of gravity distance L3 that is the distance to Cw. Here, since the third shaft 23 is supported so as to be able to reciprocate along the vertical direction, the center-of-gravity distance L3 changes according to the operating state of the third shaft 23, and the height at which the inertial force changes changes. Will be. Therefore, in the present embodiment, the correction processing unit 52 (corresponding to the centroid distance calculation unit) calculates the centroid distance L3 based on the centroid of the entire WA and the position of the third axis 23 in the vertical direction. The position of the third shaft 23 in the vertical direction can be calculated based on a command value for driving the third shaft 23 and a detection signal of the encoder. The center of gravity of the entire WA can be calculated based on the length and mass of the third shaft 23 and the height width and mass of the object W.

  FIG. 6 is a plan view showing the lengths and angular positions of the first shaft 21 and the second shaft 22. As shown in the figure, the length from the axis C1 that is the rotation center of the first shaft 21 to the axis C2 that is the rotation center of the second shaft 22 is L1. The length from the axis C2 to the axis C3 that is the central axis of the third shaft 23 is L2. With the front direction of the base 20 as the x axis, the angle formed by the longitudinal direction of the first shaft 21 with respect to the x axis is θ1. The angle formed by the longitudinal direction of the second shaft 22 with respect to the longitudinal direction of the first shaft 21 is θ2.

  Here, when the first shaft 21 and the second shaft 22 rotate horizontally, the acceleration at the tip end portion (the position of the axis C3) of the second shaft 22 is a, and the angular acceleration / deceleration of the rotation of the second shaft 22 is aω. And The length from the axis C1 to the axis C3 is Lr. In that case, the following equation (1) holds.

a = aω · Lr (1)
The inertial force F acting on the third shaft 23 and the entire WA of the object W is expressed by the following equation (2), where M is the mass of the entire WA.

F = M · a (2)
The torque T acting on the support portion 22a of the second shaft 22 is expressed by the following equation (3), where the center-of-gravity distance is L3.

T = F · L3 (3)
The displacement amount Δd along the trajectory of the center of gravity of the entire WA due to the inertia force F is expressed by the following equation (4), where K is the spring constant of the support portion 22a with respect to the bending of the third shaft 23.

Δd = L3 · T / K (4)
A correction amount Δθ, which is a rotation angle amount of the support portion 22a for canceling out the displacement amount Δd, is expressed by the following equation (5).

Δθ = Δd / Lr (5)
When the above formulas (1) to (4) are applied to the formula (5) and rearranged, the correction amount Δθ is expressed by the following formula (6). L3 ^ 2 is the square of L3.

Δθ = M · L3 ^ 2 · aω / K (6)
That is, the correction amount Δθ can be calculated from the mass M and the spring constant K, which are known constants, the angular acceleration / deceleration speed aω of the rotation of the second shaft 22, and the centroid distance L3.

  FIG. 7 is a flowchart showing a procedure of vibration suppression control in the present embodiment. This series of processing is executed by the controller 50 for each of the 10 horizontal rotation axes of the robot. Here, the horizontal rotation axis to be controlled is the nth axis.

  First, the trajectory generating unit 51 acquires the target stop angle position of the nth axis (S10). Subsequently, the trajectory generating unit 51 generates an angular velocity pattern of the nth axis based on the target stop angle position of the nth axis (S11). For example, a trapezoidal angular velocity pattern is generated like a commanded angular velocity indicated by a broken line in FIG. Subsequently, the trajectory generation unit 51 calculates a command value θa for the angular position of the n-th axis based on the generated angular velocity pattern (S12). The command value θa corresponds to the target angular position of the nth axis in the current control cycle. Note that, by controlling each horizontal rotation axis to the command value θa, the tip of the second shaft 22 is controlled to the target position (nearest target position) in the current control cycle.

  Subsequently, the correction processing unit 52 calculates the angular acceleration / deceleration speed aω of the n-th axis rotation based on the angular speed ω of the n-th axis rotation (S13). Specifically, the difference between the current angular velocity ω (k) and the previous angular velocity ω (k−1) is defined as an angular acceleration / deceleration aω. That is, the angular acceleration / deceleration aω corresponds to the acceleration / deceleration when the command value θa changes. Subsequently, the correction processing unit 52 calculates the center-of-gravity distance L3 as described above (S14). The correction processing unit 52 calculates the correction amount Δθ by the above equation (6) (S15). Then, the correction processing unit 52 calculates the correction command value θb by adding the correction amount Δθ to the command value θa (S16).

  Subsequently, the correction processing unit 52 applies a low-pass filter to the correction command value θb (S17), and transfers the correction command value θb after the filter processing to the servo mechanism 53 (S18). That is, the correction command value θb is changed to the command value obtained by correcting the calculated correction command value θb and transferred to the servo mechanism 53.

  Subsequently, the correction processing unit 52 determines whether the angular position of the nth axis has reached the target stop angular position based on the detection signal of the encoder provided on the nth axis (S19). In this determination, when it is determined that the angular position of the n-th axis has not reached the target stop angular position (S19: NO), the process is executed again from S12. On the other hand, in this determination, when it is determined that the angular position of the n-th axis has reached the target stop angular position (S19: YES), this series of processing ends (END).

  The process of S12 corresponds to the process as the command value calculation unit, the process of S13 corresponds to the process as the acceleration / deceleration calculation unit, the process of S14 corresponds to the process as the gravity center distance calculation unit, and S15 to S18. This process corresponds to the process as the correction command value calculation unit.

  FIG. 8 is a time chart showing angular velocity patterns in the comparative example and this embodiment. The broken line corresponds to the angular velocity pattern generated by the process of S11 in FIG. 7, and indicates the commanded angular velocity. The alternate long and short dash line indicates the angular velocity (angular velocity pattern) realized when the command value θa calculated in the process of S12 based on the generated angular velocity pattern is transferred to the servo mechanism 53 as it is. A solid line indicates an angular velocity (angular velocity pattern) realized when the vibration suppression control of this embodiment shown in FIG. 7 is executed.

  As shown in the figure, at the start of acceleration from the stop state, the timing of the angular velocity rising in this embodiment is earlier than that in the comparative example. Thereby, in this embodiment, the position where the center of gravity of the third shaft 23 and the whole object W is projected on the trajectory is adjusted to the target position closest to the support portion 22a (tip portion) of the second shaft 22. Yes. At this time, since the low-pass filter is applied to the correction command value θb by the process of S17 in FIG. 7, a rapid change in the correction command value θb is suppressed. As a result, when the drive units 31 and 32 are controlled based on the correction command value θb, it is possible to prevent the angular velocities of the shafts 21 and 22 from rapidly changing.

  In the transition from the acceleration time to the constant speed time, the present embodiment is slower than the comparative example when the angular velocity becomes constant. As a result, in this embodiment, the acceleration decreases as the elastic deformation of the support portion 22a of the second shaft 22 returns, and the position where the center of gravity of the entire WA is projected on the trajectory is the closest to the support portion 22a. It is adjusted to the target position. Also at this time, when the drive units 31 and 32 are controlled on the basis of the corrected command value θb after the filter processing, it is suppressed that the angular velocities of the shafts 21 and 22 change suddenly.

  At the time of transition from constant speed to deceleration, the present embodiment is earlier in the time when the angular velocity is lower than the comparative example. Thereby, in this embodiment, the position which projected the gravity center of whole WA on the track | orbit is match | combined with the nearest target position of the support part 22a. Also at this time, when the drive units 31 and 32 are controlled on the basis of the corrected command value θb after the filter processing, it is suppressed that the angular velocities of the shafts 21 and 22 change suddenly.

  At the time of transition from the time of deceleration to the end of movement, the present embodiment is delayed in the time when the angular velocity is lower than the comparative example. Thereby, in this embodiment, the deceleration decreases as the elastic deformation of the support portion 22a of the second shaft 22 returns to the original position, and the position where the center of gravity of the entire WA is projected on the track is the position of the support portion 22a. It is adjusted to the nearest target position. Also at this time, when the drive units 31 and 32 are controlled on the basis of the corrected command value θb after the filter processing, it is suppressed that the angular velocities of the shafts 21 and 22 change suddenly.

  FIG. 9 is a time chart showing the deviation between the target stop position and the projected position of the center of gravity in the comparative example and this embodiment, taking as an example the movement of the tip of the second shaft 22. The comparative example is the same as that described in FIG.

  As indicated by the alternate long and short dash line in the figure, in the comparative example, the deviation between the projected position of the center of gravity of the entire WA on the trajectory and the target stop position is alternately increased on the negative side and the positive side. That is, with the support portion 22a of the second shaft 22 as a fulcrum, the third shaft 23 vibrates before and after the movement direction of the support portion 22a (tip portion) of the second shaft 22. In contrast, as indicated by the solid line, in this embodiment, the deviation between the projected position of the center of gravity of the entire WA on the trajectory and the target stop position is close to zero. That is, the vibration of the third shaft 23 using the support portion 22a of the second shaft 22 as a fulcrum is effectively suppressed.

  The embodiment described in detail above has the following advantages.

  Since the third shaft 23 is supported so as to be able to reciprocate along the vertical direction, the center-of-gravity distance L3 changes according to the operating state of the third shaft 23, and the height at which the inertial force changes changes. It becomes. At this point, the center-of-gravity distance calculation unit (the process of S14) calculates the center-of-gravity distance L3 based on the center of gravity of the third shaft 23 and the whole WA of the object W and the position of the third axis 23 in the vertical direction. Based on the command value θa, the spring constant K, the mass of the entire WA, the angular acceleration / deceleration aω at which the command value θa changes, and the center of gravity distance L3, the corrected command value calculation unit (the processing of S15 and S16) A corrected command value θb obtained by correcting the command value θa calculated by the command value calculation unit (the process of S12) is calculated so that the position of the center of gravity of the entire WA projected onto the closest target position is calculated. Then, the servo mechanism 53 controls the drive units 31 and 32 based on the calculated correction command value θb. For this reason, when the deviation between the position of the center of gravity projected onto the predetermined trajectory and the target position is eliminated, it is possible to suppress the occurrence of vibration on the third shaft 23. Further, it is not necessary to attach an acceleration sensor to a portion where vibration actually occurs, and the corrected command value θb is calculated based on the command value θa, the known value, the angular acceleration / deceleration aω of the command value θa, and the center of gravity distance L3. Therefore, it can be easily applied to the actual robot 10.

  When the support portion 22a (tip portion) of the second shaft 22 accelerates, an inertial force in the direction opposite to the moving direction of the support portion 22a of the second shaft 22 acts on the entire shaft WA of the third shaft 23 and the object W. To do. For this reason, the center of gravity of the entire WA projected onto the predetermined trajectory moves with a delay from the support portion 22a of the second shaft 22. In this regard, the correction command value calculation unit calculates the correction command value θb so that the support portion 22a of the second shaft 22 is advanced from the target position when the support portion 22a of the second shaft 22 is accelerated. Therefore, the correction command value θb can be calculated so as to reduce the deviation between the position of the center of gravity projected onto the predetermined trajectory and the target position.

  When the support portion 22a of the second shaft 22 decelerates, an inertial force in the same direction as the moving direction of the support portion 22a of the second shaft 22 acts on the entire third WA of the third shaft 23 and the object W. For this reason, the center of gravity of the entire WA projected onto a predetermined trajectory moves more forward than the support portion 22a of the second shaft 22. In this regard, the correction command value calculation unit calculates the correction command value θb so that the support portion 22a of the second shaft 22 is delayed from the target position when the support portion 22a of the second shaft 22 is decelerated. Therefore, the correction command value θb can be calculated so as to reduce the deviation between the position of the center of gravity projected onto the predetermined trajectory and the target position.

  The correction command value calculation unit calculates the correction command value θb so that the position of the center of gravity projected onto a predetermined trajectory matches the target position. For this reason, the deviation between the position of the center of gravity projected onto the predetermined trajectory and the target position can be minimized, and the occurrence of vibration on the third shaft 23 can be further suppressed.

  If the angular acceleration / deceleration aω of the command value θa changes abruptly, the corrected command value θb calculated based on the angular acceleration / deceleration aω of the command value θa may change abruptly. In that case, there is a possibility that a larger torque than the upper limit torque that can be generated by the drive units 31 and 32 may be required for the drive units 31 and 32 controlled based on the respective correction command values θb. In this regard, the correction command value calculation unit changes the correction command value θb to a command value obtained by making the calculated correction command value θb. Therefore, it is possible to suppress a sudden change in the correction command value θb, and it is possible to suppress the driving units 31 and 32 from requesting a torque larger than the upper limit torque.

  The support portion 22a of the second shaft 22 reciprocates on a predetermined arc track along the horizontal plane with the axis C2 as the center. In this case, as a command value θa for driving the second drive unit 32 so as to move the support unit 22a of the second shaft 22 to the nearest target position, the support unit 22a centered on the axis C2 corresponding to the target position. The target angular position θ can be adopted. Then, the correction command value calculation unit can calculate the correction command value θb by adding the correction amount Δθ calculated by the equation of Δθ = M · a · L ^ 2 / K to the command value θa. Therefore, the correction command value θb can be easily calculated and can be easily applied to the actual robot 10.

  As the robot 10, the first axis 21 that rotates along the horizontal plane, the second axis 22 that is rotatably supported by the first axis 21 along the horizontal plane, and the vertical direction at the tip of the second axis 22. A horizontal articulated robot 10 having a third axis that reciprocates is adopted. And the process by the command value calculation part, the acceleration / deceleration calculation part, the gravity center distance calculation part, the correction command value calculation part, and the servo mechanism 53 is performed with respect to the 1st axis | shaft 21 and the 2nd axis | shaft 22, respectively. Therefore, in the horizontal articulated robot 10, the vibration of the third axis 23 can be suppressed.

  In addition, the said embodiment can also be changed and implemented as follows.

  In the flowchart of FIG. 7, the process of applying a low pass filter to the correction command value θb in S17 can be omitted.

  In the above embodiment, the correction command value calculation unit calculates the correction command value θb so that the position of the center of gravity of the third axis 23 projected onto the predetermined trajectory and the whole WA of the object W matches the target position. On the other hand, the correction command value calculation unit calculates the correction command value θb so that the position of the center of gravity coincides with an intermediate position between the position of the center of gravity of the entire WA projected onto a predetermined trajectory and the target position. You can also That is, if the corrected command value θb obtained by correcting the command value θa is calculated so that the position of the center of gravity of the entire WA projected onto the predetermined trajectory is brought closer to the nearest target position, vibration is generated on the third shaft 23. Can be suppressed.

  The object W may not be attached to the tip of the third shaft 23. In that case, the third shaft 23 may be the entire WA.

  Not only the horizontal articulated robot 10, but also a first moving unit that reciprocates on a predetermined first trajectory along an X-direction rail on a horizontal plane, and a first moving unit that is movably supported by the first moving unit. The vibration suppression control can also be applied to an XR robot including a second moving unit that reciprocates on a predetermined second circular arc trajectory along a horizontal plane as a center. In that case, vibration suppression control may be executed for the first moving unit by replacing the angular velocity with the speed and the angular position with the position.

  Not only the horizontal articulated robot 10, but also a first moving unit that reciprocates on a predetermined first trajectory along an X-direction rail on the horizontal plane, and a Y-direction on the horizontal plane that is supported by the first moving unit so as to be movable. The vibration suppression control can also be applied to an XY robot that includes a second moving unit that reciprocates on a predetermined second track along a rail. In that case, it is only necessary to execute vibration suppression control for the first moving unit and the second moving unit by changing the angular velocity to the speed and the angular position to the position.

  The vibration suppression control may be applied to a robot that does not include the first moving unit but includes the second moving unit and the third shaft 23.

  DESCRIPTION OF SYMBOLS 10 ... Horizontal articulated robot (robot), 21 ... 1st axis | shaft, 22 ... 2nd axis | shaft, 22a ... Support part (moving part, 2nd moving part), 23 ... 3rd axis | shaft, 50 ... Robot controller (control apparatus) 51: Trajectory generation unit (command value calculation unit) 52: Correction processing unit (acceleration / deceleration calculation unit, center of gravity distance calculation unit, correction command value calculation unit) 53: Servo mechanism (drive control unit)

Claims (7)

A driving unit, a moving unit that is driven by the driving unit and reciprocates on a predetermined track along a horizontal plane, and a predetermined axis that is supported by the moving unit so as to reciprocate along a vertical direction and has an object attached to a lower portion thereof. A control device for controlling a robot comprising:
A command value calculating unit for calculating a command value for driving the driving unit so as to move the moving unit to the nearest target position;
An acceleration / deceleration calculation unit for calculating an acceleration / deceleration when the command value calculated by the command value calculation unit changes;
Based on the predetermined axis and the center of gravity of the entire object and the position of the predetermined axis in the vertical direction, the moving unit calculates a center-of-gravity distance that is a distance from the support unit that supports the predetermined axis to the center of gravity. A center-of-gravity distance calculation unit;
The command value calculated by the command value calculation unit, the spring constant of the support unit with respect to bending of the predetermined axis, the overall mass, the acceleration / deceleration calculated by the acceleration / deceleration calculation unit, and the gravity center distance calculation unit Based on the center-of-gravity distance calculated by the step, the corrected command value obtained by correcting the command value calculated by the command value calculation unit is calculated so that the position of the center of gravity projected onto the trajectory approaches the target position. A correction command value calculation unit to
A drive control unit that controls the drive unit based on the correction command value calculated by the correction command value calculation unit;
A robot control apparatus comprising:   The robot control apparatus according to claim 1, wherein the correction command value calculation unit calculates the correction command value so that the moving unit is advanced from the target position when the moving unit is accelerated.   The robot control device according to claim 1, wherein the correction command value calculation unit calculates the correction command value so that the moving unit is delayed from the target position when the moving unit is decelerated.   The robot control according to claim 1, wherein the correction command value calculation unit calculates the correction command value so that a position of the center of gravity projected onto the trajectory matches the target position. apparatus.   The robot control device according to claim 1, wherein the correction command value calculation unit changes the correction command value to a command value obtained by calculating the calculated correction command value. The moving unit reciprocates on a predetermined arc orbit along a horizontal plane around a predetermined central axis,
The spring constant is K, the overall mass is M, the target angular position of the moving unit around the central axis corresponding to the target position is θ, the angular acceleration / deceleration at which the target angular position θ changes is a, The center of gravity distance is L, and the correction amount of the target angular position θ is Δθ.
The command value calculation unit calculates the target angular position θ as the command value,
6. The correction command value calculation unit calculates the correction command value by adding the correction amount Δθ calculated by an equation of Δθ = M · a · L ^ 2 / K to the target angular position θ. The robot control device according to any one of the above. The robot includes a second moving unit as the moving unit, and a first moving unit that reciprocates on a predetermined first arc trajectory along a horizontal plane around a predetermined central axis,
The second moving unit is movably supported by the first moving unit, and reciprocates on a predetermined second arc track as the predetermined arc track along a horizontal plane with the first moving unit as a center,
Processes by the command value calculation unit, the acceleration / deceleration calculation unit, the gravity center distance calculation unit, the correction command value calculation unit, and the drive control unit are performed on the first moving unit and the second moving unit, respectively. The robot control device according to claim 6.

Images