JP6926882B2 - Robot control device - Google Patents

Description

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

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

Japanese Unexamined Patent Publication No. 10-100985

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 near the tip of the arm where vibration actually occurs. However, when it is applied to an actual robot, it is difficult to realize it because there are problems such as a space for attaching the acceleration sensor near the tip of the arm and wiring arrangement to the acceleration sensor.

The present invention has been made in view of the above circumstances, and an object of the present invention is to provide a robot control device capable of suppressing vibration of a robot without using an acceleration sensor.

According to the robot control device according to claim 1, a first axis perpendicular to the installation surface, a second axis perpendicular to the first axis, and a plurality of connected axes after the second axis. A robot having four or more axes having an axis in which at least one of the axes is parallel to the second axis is targeted for control. When the final target position for moving the hand of the arm is given, the speed pattern generation unit generates a speed pattern for moving the hand to the final target position. The command value calculation unit calculates the angle command value given to the servo mechanism so as to move the hand to the passing target position for each control cycle. The command value conversion unit converts the angle command value into the position command value.

For a robot having an arm with the above configuration, the entire arm is regarded as one spring, and the entire arm is regarded as one spring.
A model in which "arm inertia"-"arm spring"-"work inertia" is connected can be assumed. In this case, the "arm inertia" and the "arm spring" change according to the position of the hand. Then, when the arm accelerates to move the work held by the hand, the movement of the tip side of the arm is delayed due to the inertial force, and when decelerating, the movement of the tip side advances conversely. This causes vibration.

Therefore, the spring constant calculation unit calculates the spring constant of the arm according to the position command value. The acceleration calculation unit calculates the acceleration when the position command value changes for each control cycle. When the correction amount calculation unit calculates the correction amount of the command value from the spring constant, the mass of the work held by the arm and the hand, and the acceleration, the command value correction unit calculates the position command value based on the correction amount as the angle command value. Is converted back to and output to the servo mechanism.

As a result, when accelerating the arm, the command value is corrected so that the base side of the arm advances by the amount of delay of the tip side, and when decelerating the arm, the command value is corrected so that the base side of the arm is delayed by the amount of advance of the tip side. Therefore, vibration is suppressed. Therefore, it is possible to suppress the vibration generated when the arm moves the work held by the hand without attaching an acceleration sensor or the like to the tip of the arm.

According to the robot control device according to claim 2, the spring constant calculation unit calculates the spring constant of the entire arm. As a result, the amount of calculation can be reduced and the correction process can be executed at high speed.
According to the control device for the robot according to the third aspect, the spring constant calculation unit uses the distance from the third axis to the hand as L ₃ , the distance from the first axis to the hand as L _f , and the first axis as a reference. Assuming that the spring constant up to the third axis is K ₁₂ and the spring constant up to the hand based on the third axis is K ₃ , the spring constant K used to obtain the correction amount is 1 / K = {1. / K ₁₂ + L ₃ ² / (K ₃ L _f ² )}
Ask for. As a result, the spring constant K when the arm is integrally regarded as a spring can be quickly obtained.

According to the robot control device according to claim 4 , the command value correction unit performs low-pass filter processing on the corrected command value. Since the correction value includes the acceleration as a parameter, the torque may be saturated when a wide-area noise component is superimposed on the acceleration or when the rise becomes steep. Therefore, it is possible to prevent the torque from being saturated by performing a low-pass filter process on the corrected command value.

According to the robot control device according to claim 5 , the command value correction unit variably sets the cutoff frequency of the low-pass filter processing according to the correction coefficient obtained by dividing the mass by the spring constant. Thereby, the cutoff frequency can be appropriately set in consideration of the mass and the spring constant.

A functional block diagram of the first embodiment, which mainly shows a controller configuration in a robot system. Diagram showing the configuration of the robot system The figure explaining the model of the robot used for the vibration damping control The figure explaining the structure of the robot to which this embodiment is applied (the 1) The figure explaining the structure of the robot to which this embodiment is applied (the 2) The figure explaining the structure of the robot to which this embodiment is applied (part 3) Diagram explaining the principle of damping control The figure explaining the derivation of the formula for finding a spring constant K Flow chart showing the contents of damping control A time chart showing an angular velocity pattern to which the damping control of the comparative example and the present embodiment is applied. Time chart showing the error between the actual tip unit position and the target position in the comparative example and the embodiment

(First Embodiment)
Hereinafter, the first embodiment will be described with reference to FIGS. 1 to 11. As shown in FIG. 2, the robot system 1 includes a vertically articulated robot 2, a controller 3 that controls the robot 2, and a pendant 4 connected to the controller 3. This robot system 1 is used for general industrial use.

The robot 2 has a well-known configuration as a so-called 6-axis vertical articulated robot, and the shoulder 6 rotates horizontally on the base 5 via a first axis (J1) having an axis in the Z direction. It is connected as possible. A lower end portion of a lower arm 7 extending upward via a second axis (J2) having an axial center in the Y direction is rotatably connected to the shoulder 6. The first upper arm 8 is rotatably connected to the tip of the lower arm 7 via a third axis (J3) having an axial center in the Y direction.

A second upper arm 9 is rotatably connected to the tip of the first upper arm 8 via a fourth axis (J4) having an axis in the X direction. A wrist 10 is rotatably connected to the tip of the second upper arm 9 via a fifth axis (J5) having an axis in the Y direction. A flange 11 is twistably and rotatably connected to the wrist 10 via a sixth axis (J6) having an axis in the X direction.

The base 5, the shoulder 6, the lower arm 7, the first upper arm 8, the second upper arm 9, the wrist 10 and the flange 11 function as the arm of the robot 2, and the flange 11 which is the tip of the arm is shown in FIG. As shown, each axis (J1 to J6) provided on the robot 2 to which a tool (not shown) for holding the work 13 is attached is provided with a motor (not shown) as a drive source corresponding to each axis. ..

The controller 3 is a control device for the robot 2, and controls the robot 2 by executing a computer program in a control means including a computer composed of a CPU, a ROM, a RAM, and the like (not shown). Specifically, the controller 3 includes a drive unit composed of an inverter circuit or the like, and each of them is controlled by feedback, for example, based on the rotation position of the motor detected by the encoder provided corresponding to each motor. Drive the motor.

The controller 3 includes a CPU, a ROM, a RAM, a drive circuit, a position detection circuit, and the like. The ROM stores a system program, an operation program, and the like of the robot 2. The RAM stores parameter values and the like when executing these programs. The detection signals of each encoder (not shown) provided at each joint of the robot 2 are input to the position detection circuit. The position detection circuit detects the rotation angle position of the motor provided in each joint based on the detection signal of each encoder. As shown in FIG. 1, in such a controller 3, a trajectory generation unit 21, a command value correction unit 22, and a servo mechanism 23 are configured.

The controller 3 calculates the target stop position of the hand, which is the tip of the sixth axis, based on the operation program of the robot 2. Then, the controller 3 calculates the target stop angle position of each axis based on the target stop position. The trajectory generation unit 21 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 21 calculates the command value θn, which is the nearest target angle position of each axis, based on the angular velocity pattern of each axis. The servo mechanism 23 corresponding to the drive control unit sets each motor to each drive torque so that the angle of each axis is set to the command value θn based on the angle θen of each axis and the command value θn detected by each encoder. It is controlled by Tn. The command value correction unit 22 outputs the command value θn'corrected to the command value θn to the servo mechanism 23. The details of the correction will be described later.

In the present embodiment, the controller 3 regards the robot 2 as an operation model shown in FIG. 3 and executes vibration damping control for suppressing the vibration of the hand. As shown in the figure, in the 6-axis robot 2, the 6-axis arm is connected to the 1-axis motor at the base, and the work 13 is held at the tip of the arm. In this case, if the 6-axis arm is viewed as one spring, it can be viewed as a model in which the inertia of the arm and the inertia of the load, which is the work 13, are connected to the spring.

Next, the arm structure of the robot to which this model can be applied will be described with reference to FIGS. 4 to 6. FIG. 4 corresponds to the vertical 6-axis robot shown in FIG. 2, the first axis perpendicular to the installation surface, the second axis perpendicular to the first axis, and the second and subsequent axes. A robot having four or more axes having a third axis whose at least one is parallel to the second axis among a plurality of axes connected to the robot. FIG. 5 is a vertical 5-axis robot having a fifth axis corresponding to the sixth axis, excluding the fifth axis having the configuration shown in FIG. Further, FIG. 6 is a vertical 7-axis robot in which a 7th axis orthogonal to the 2nd axis is added between the 2nd axis and the 3rd axis having the configuration shown in FIG. The model shown in FIG. 2 can be similarly applied to these.

FIG. 7 describes the principle of vibration damping control of the present embodiment, and shows the control and the comparative example of the present embodiment. In the figure, the upper point corresponds to the base side of the arm and the motor side of the first axis, and the lower point corresponds to the tip side and the load side, and the arm is simplified to only one link. A case where the base side of this model moves linearly to the right in the figure will be described. The upper figure is a comparative example when the base side is simply moved, and the lower figure is a case where the control principle of the present embodiment is applied.

In the comparative example, when the base starts moving from the stopped state and accelerates to the right, an inertial force acts on the tip, so that the movement is delayed by that amount. On the contrary, when the base is decelerated to stop from the moving state, the moving position advances by the amount of inertial force acting on the tip. Then, when the base portion is stopped, the position of the tip portion returns to the left from the position where the tip portion has advanced. Such movement causes the arm to vibrate.

When the control principle of the present embodiment is applied to the movement of this comparative example, when the base is accelerated, the base is accelerated by the amount of delay in the movement of the tip. Further, when the base portion is decelerated, the base portion is decelerated so as to be delayed by the amount that the position of the tip portion is advanced. Then, when the base portion is stopped, the base portion is moved according to the restoring force that the tip portion tends to return to the left. By controlling in this way, the vibration of the arm is suppressed.

Next, a method of obtaining the spring constant K used for actual control will be described with reference to FIG. The point on the right side of the figure indicates the position of the first axis and the second axis (J1, J2) of the 6-axis robot 2, the point on the left side of the figure indicates the position of the hand, and the point between them is the third. The position of the axis (J3) is shown.

In the XZ plane when the arm is viewed from the side, the distance from the 1st axis and the 2nd axis to the 3rd axis is L ₁ , the distance from the 3rd axis to the hand is L ₃ , the 1st axis and the 2nd axis. _Let L f be the distance from the hand to the hand. Further, in the XY plane when the arm is viewed from above, the angular deviation of the third axis with respect to the X axis in the Y-axis direction is Δθ ₁₂ , and the hand is in the Y-axis direction with reference to the X-axis. Let the angular deviation be Δθ ₃ .

When the inertial force F (= M · a) due to the mass M and the acceleration a acts on the hand, the torque τ _{12 acting on the first and second axes and the torque τ 3} acting on the third axis are respectively. τ ₁₂ = L _f × F, τ ₃ = L ₃ × F… (1)
Will be.

The spring coefficient of the hand to the first axis and the second axis relative to the K _12, when the spring coefficient of the hand relative to the third axis and K _3, the angle deviation [Delta] [theta] _12, [Delta] [theta] ₃ is
Δθ ₁₂ = τ ₁₂ / K ₁₂ , Δθ ₃ = τ ₃ / K ₃ … (2)
_{The position displacements ΔY 12} and ΔY ₃ in the Y-axis direction are
ΔY ₁₂ = L _f × τ ₁₂ / K ₁₂ = L _f ² × F / K ₁₂
ΔY ₃ = L ₃ × τ ₃ / K ₃ = L ₃ ² × F / K ₃ … (3)
Will be.

The total position displacement ΔY _f is
ΔY _f = ΔY ₁₂ + ΔY ₃ = L _f ² × F / K ₁₂ + L ₃ ² × F / K ₃
= (L _f ² / K ₁₂ + L ₃ ² / K ₃ ) F ... (4)
Is. And since the inside of the parentheses in Eq. (4) is the reciprocal of the spring constant K,
1 / K = (1 / K ₁₂ + 1 / K ₃ x L ₃ ² / L _f ² )… (5)
Is. In this way, the spring constant K is a function that depends on _{the rigidity of each axis, that is, the reciprocals of the spring constants K 12} and K ₃ and the hand distance L _f. For the spring constants K ₁₂ and K ₃ , the rigidity values of the cross roller bearings used for the 2nd and 3rd axes are used, or the values actually measured in advance are used. In the present embodiment, the spring constant K represented by the equation (5) is used for vibration damping control.

FIG. 9 is a flowchart showing the procedure of vibration damping control in the present embodiment. When the trajectory generation unit 21 acquires the angular position of the hand, which is the target position of each axis, when the hand is stopped (S1), the trajectory generation unit 21 generates an angular velocity pattern of each axis based on the target position of each axis (S2). For example, a trapezoidal angular velocity pattern is generated like the command angular velocity shown by the broken line in FIG. 10 described later. The "velocity" shown in each figure is an "angular velocity". Further, the target position in step S1 corresponds to the final target position.

Subsequently, the trajectory generation unit 21 calculates the target angle θ (n = 1 to 6) of each axis, which is a command value, based on the generated angular velocity pattern (S3). The target angle θn here corresponds to the target angle of the nth axis in the current control cycle, that is, the cycle of sampling the angle θen detected by each axis encoder. Further, the trajectory generation unit 21 corresponds to a velocity pattern generation unit and a command value generation unit.

Next, when the command value correction unit 22 obtains the target position P of the hand from the target angle of each axis (S4), the spring constant K corresponding to the target position P is obtained by the equation (4). Further, the mass M of the work 13 including the arm is acquired from a memory stored in advance (S5). _{When finding the target position P, the distances L 3} and L _f, which are the calculation parameters of the equation (4), are also found. The target position P corresponds to the passing target position. Subsequently, from the velocity pattern of each axis, the angular acceleration a while each axis reaches the target angle is calculated (S6).

Next, the command value correction unit 22 calculates the position correction amount (Ma / K) (S7). Then, when the correction command value P'by adding the correction amount (Ma / K) to the position command value P of the hand is calculated (S8), the low-pass filter processing is performed on the correction command value P'(S9). When the command value θ'corresponding to the angle is calculated from the correction command value P'for each axis (10), the command value θ'is transferred to the servo mechanism 23 (S11). Then, the process returns to step S3. The command value correction unit 22 corresponds to a command value conversion unit, a spring constant calculation unit, an acceleration calculation unit, and a correction amount calculation unit.

The low-pass filter processing is performed in step S9 for the following reasons. Since the angular acceleration a is included as a parameter in the correction amount, the torque may be saturated when a wide-area noise component is superimposed on the angular acceleration or when the rising edge becomes steep. Therefore, it is possible to prevent the torque from being saturated by performing a low-pass filter process on the corrected command value. The cutoff frequency is, for example, about several Hz to several several Hz.

FIG. 10 is a time chart showing a comparative example of normal control and an angular velocity pattern according to the present embodiment. The broken line corresponds to the angular velocity pattern generated in step S2 and indicates the commanded angular velocity. The alternate long and short dash line indicates the angular velocity when the command value θ calculated in step S3 based on the generated angular velocity pattern is transferred to the servo mechanism 23 as it is. The solid line shows the angular velocity when the vibration damping control of this embodiment is executed.

As shown in the figure, at the start of acceleration from the stopped state, in the control of the present embodiment, the timing at which the angular velocity increases is earlier than that in the comparative example. This corresponds to (2) the movement during acceleration shown in FIG. Further, at this time, since the low-pass filter processing is performed before the correction command value θ'is inversely transformed in step S9, a sudden change in the correction command value θ'is suppressed. As a result, when the motors of each axis are controlled based on the correction command value θ', the angular velocity of each axis is suppressed from suddenly changing.

Further, at the time of transition from the acceleration to the constant velocity, the timing at which the angular velocity becomes constant is later than that in the comparative example in the control of the present embodiment. This corresponds to (3) the movement during deceleration shown in FIG.

Even at the time of transition from the next constant velocity to deceleration, the timing at which the angular velocity decreases is earlier than that of the comparative example in the control of the present embodiment, and even at the transition from the next deceleration to the end of movement, the present In the embodiment control, the timing at which the angular velocity decreases is later than that in the comparative example. This corresponds to the movements shown in FIG. 7 during (3) deceleration and (4) stop.

FIG. 11 is a time chart simulating the error between the target position and the actual hand position in the comparative example and the present embodiment at the end of the above movement. As shown by the alternate long and short dash line in the figure, in the comparative example, the error from the target position changes alternately between the negative side and the positive side, and the hand of the arm vibrates. On the other hand, as shown by the solid line, in the control of the present embodiment, the error with respect to the target position is close to zero. That is, the vibration of the hand is suppressed.

As described above, according to the present embodiment, the trajectory generating unit 21 generates a velocity pattern for moving the hand to the final target position when the final target position for moving the hand of the arm is given, and the hand is sampled. The angle command value θn given to the servo mechanism 23 is calculated so as to move to each target position. The command value correction unit 22 converts the angle command value θn into the target position of the hand and the position command value P, calculates the spring constant K of the arm according to the position command value P, and the position command value P is calculated for each sampling cycle. The acceleration a when changing to is calculated. Then, when the correction amount (Ma / K) is calculated from these values, the correction amount is added to the position command value P to obtain the correction command value P', and the correction command value P'is set to the position command value command value θn'. It is reversely converted and output to the servo mechanism 23.

As a result, when accelerating the arm, the command value is corrected so that the base side of the arm advances by the amount of delay of the tip side, and when decelerating the arm, the command value is corrected so that the base side of the arm is delayed by the amount of advance of the tip side. Therefore, vibration is suppressed. Therefore, it is possible to suppress the vibration generated when the arm moves the work 13 held by the hand without attaching an acceleration sensor or the like to the tip of the arm.

Further, since the command value correction unit 22 calculates the spring constant K of the entire arm, the calculation amount can be reduced and the correction process can be executed at high speed. Specifically, since the spring constant K is obtained by the equation (5), the spring constant K when the arm is integrally regarded as a spring can be quickly obtained. As a result, the robot 2 can be operated at high speed.

Further, the command value correction unit 22 performs a low-pass filter process on the corrected command value P'. As a result, it is possible to prevent the torque from being saturated even when a wide-area noise component is superimposed on the acceleration or the rise becomes steep.

The present invention is not limited to the embodiments described above or in the drawings, and the following modifications or extensions are possible.
The low-pass filter processing in steps S9 and S17 may be performed as needed.
The spring constant K in the first embodiment may be obtained by an equation other than the equation (5).

In the drawings, 1 is a robot system, 2 is a robot, 3 is a controller, 13 is a work, 21 is a trajectory generating unit, and 22 is a command value correction unit.

Claims (5)

At least one of a first axis perpendicular to the installation surface, a second axis perpendicular to the first axis, and a plurality of axes connected after the second axis is the second axis. An arm with four or more axes that has axes that are parallel to each other,
A robot control device that drives and controls this arm via a servo mechanism.
Given a final target position for moving the hand of the arm, a speed pattern generator that generates a speed pattern for moving the hand to the final target position.
A command value calculation unit that calculates an angle command value given to the servo mechanism so as to move the hand to a passing target position for each control cycle.
A command value conversion unit that converts the angle command value into a position command value,
A spring constant calculation unit that calculates the spring constant of the arm according to the position command value,
An acceleration calculation unit that calculates the acceleration when the position command value changes for each control cycle,
A correction amount calculation unit that calculates a correction amount of the position command value from the spring constant, the mass of the work held by the arm and the hand, and the acceleration.
A robot control device including a command value correction unit that reversely converts the corrected position command value into an angle command value and outputs the corrected position command value to the servo mechanism when the position command value is corrected based on the correction amount. The robot control device according to claim 1, wherein the spring constant calculation unit calculates the spring constant of the entire arm. The spring constant computing section, a distance from the third axis to said hand L _3,
The distance from the first axis to the hand is L _f ,
K ₁₂ a spring constant of up to the third axis relative to the first axis,
When the spring constant of up to the hand relative to the said third axis and K _3,
The spring constant K used to obtain the correction amount is calculated by the following equation 1 / K = {1 / K ₁₂ + L ₃ ² / (K ₃ L _f ² )}.
2. The robot control device according to claim 2. The robot control device according to any one of claims 1 to 3 , wherein the command value correction unit performs low-pass filter processing on the corrected command value. The robot control device according to claim 4, wherein the command value correction unit variably sets the cutoff frequency of the low-pass filter processing according to a correction coefficient obtained by dividing the mass by the spring constant.

Images