The present invention relates to a robot control method.
Conventionally, an articulated robot including a robot arm having a plurality of joints is known (see, for example, PTL 1).
In such an articulated robot, the arm may be flexurally deformed by its own weight or a load applied to the tip of the arm. This deformation of the arm occurs particularly prominently at a joint with a rotary shaft. That is, when a member forming a joint having a movable is elastically deformed, the joint may be displaced in the movable direction from a target position.
PTL 1 discloses a configuration in which the actual angle of a joint is obtained for each arm, a displacement amount that is the difference between the obtained actual angle and a command angle for control from a control device is obtained, and the command angle applied from the control device to the joint is compensated on the basis of the displacement amount.
Meanwhile, according to the invention disclosed in PTL 1, only the angle of a joint having the rotary shaft that rotates in the gravity direction is compensated, and no consideration is given to a joint that does not rotate in the gravity direction.
Specifically, even a joint having no rotary shaft that rotates in the gravity direction is deflected in the tilting direction of the rotary shaft itself due to the elastic deformation of a speed reducer or a bearing in the joint. Accordingly, the arm tip cannot be accurately positioned, and hence the accuracy of machining performed using the robot is reduced, resulting in a problem.
The present invention has been made in view of such a point and has its object to suppress the positional displacement of the tip of a robot arm caused by the deflection of a joint of the robot arm.
The present invention provides the following means for solving the problem for a method of controlling a robot that controls the operation of a robot arm including at least a first joint, a second joint, a third joint, and a fourth joint.
That is, according to a first invention, a first joint, a second joint, a third joint, and a fourth joint each have a rotary shaft,
According to a first aspect of the present invention, the determination value calculated based on the distance from the work point of the tip of a robot arm to a virtual straight line passing through the axis of the rotary shaft of a second joint and the axis of the rotary shaft of a third joint is compared with a predetermined threshold.
When the determination value is larger than the threshold, a deflection compensation amount for the second joint is calculated based on the deflection angles of the first joint, the second joint, and the fourth joint. A deflection compensation amount for the third joint is calculated based on the deflection angles of the first joint, the third joint, and the fourth joint.
On the other hand, when the determination value is smaller than the threshold, a deflection compensation amount for the second joint is calculated based on the deflection angles of the first joint, the second joint, and the fourth joint, and a threshold value. A deflection compensation amount for the third joint is calculated based on the deflection angles of the first joint, the third joint, and the fourth joint, and a threshold value.
The second joint and the third joint are caused to pivot based on the calculated deflection compensation amounts.
As described above, the positional displacement of the tip of the robot arm can be suppressed by compensating not only the deflection angles of the second joint and the third joint using the second joint and the third joint that are pivotable in the gravity direction but also the tilts of the rotary shafts of the first joint and the fourth joint.
In this case, the deflection angle is an angle deviated from a target set angle.
Note that the method of calculating deflection compensation amounts for the second joint and the third joint is changed based on the comparison result between the determination value and the threshold for the following reasons.
The inventor of the present application has found that there is a specific position in which the deflection compensation amount rapidly increases when the robot arm is in a predetermined position. It was then found that such a specific position occurs when the distance between the work point of the tip of the robot arm and the virtual straight line approaches “0”.
Accordingly, in the present invention, a threshold for determining that the robot arm approaches a specific position is set, and when the determination value is smaller than the threshold, a deflection compensation amount is calculated using a calculation formula capable of suppressing a rapid increase in deflection compensation amount.
This makes it possible to continuously correct the operation of robot arm and suppress unstable control.
According to a second aspect of the present invention, in the first aspect, the determination value is the value obtained by integrating the distance between the axes of the second joint and the third joint and the distance from the work point of the tip of the robot arm to the virtual straight line.
The second aspect of the present invention uses, as the determination value, the value obtained by integrating the distance between the axes of the rotary shafts of the second joint and the third joint and the distance from the work point of the tip of the robot arm to the virtual straight line.
According to the present invention, it is possible to suppress the positional displacement of the tip of the robot arm caused by deflection of the joint of the robot arm.
An exemplary embodiment of the present invention will be described below with reference to the accompanying drawings. Note that the following description of the preferable exemplary embodiment is essentially merely an example, and is not intended to limit the present invention, and application or usage of the present invention.
As illustrated in
Robot arm 10 includes base 11, shoulder 12, lower arm 13, first upper arm 14, second upper arm 15, wrist 16, and attachment 17. Robot arm 10 further includes first joint J1, second joint J2, third joint J3, fourth joint J4, fifth joint J5, and sixth joint J6. Each of first joint J1, second joint J2, third joint J3, fourth joint J4, fifth joint J5, and sixth joint J6 has a rotary shaft.
Shoulder 12 is supported on base 11 so as to be turnable in the horizontal direction about the rotary shaft of first joint J1. Lower arm 13 is supported on shoulder 12 so as to be pivotable in the gravity direction about the rotary shaft of second joint J2.
First upper arm 14 is supported on lower arm 13 so as to be pivotable in the gravity direction about the rotary shaft of third joint J3. Second upper arm 15 is supported on first upper arm 14 so as to be torsionally rotatable about the rotary shaft of fourth joint J4.
Wrist 16 is supported on second upper arm 15 so as to be pivotable in the gravity direction about the rotary shaft of fifth joint J5. Attachment 17 is supported on wrist 16 so as to be torsionally rotatable about the rotary shaft of sixth joint J6. Tool 18 such as a welding torch is attached to attachment 17.
An actuator (not illustrated) is built in each of first joint J1 to sixth joint J6. Controller 20 controls the driving of the actuators of first joint J1 to sixth joint J6 on the basis of an operation program input in advance by teaching or the like such that first joint J1 to sixth joint J6 reach target positions (command angles), respectively.
Note that the deflection generated at fifth joint J5 and sixth joint J6 has a small influence on the positional displacement of the work point of tool 18 at the tip of robot arm 10 and can be ignored, and hence no consideration is given to fifth joint J5 and sixth joint J6.
Second joint J2 and third joint J3 are pivotable in the gravity direction. As shown in
More specifically, the gravitational torque acts on second joint J2 and third joint J3 in the clockwise direction in
However, as indicated by the dotted arrows in
Therefore, in the present exemplary embodiment, second joint J2 and third joint J3 are rotated in consideration of the static elastic deflection generated in first joint J1 and fourth joint J4, so that the positional displacement of the work point of tool 18 can be eliminated.
More specifically, as illustrated in
Accordingly, second joint J2 and third joint J3 are caused to pivot in the counterclockwise direction in
This makes it possible to obtain a compensation amount (illustrated by the black arrow) for canceling the deflection amount (illustrated by the white arrow) of the work point of tool 18 and to eliminate the positional displacement of the work point of tool 18.
A method of calculating compensation amounts for second joint J2 and third joint J3 will be described below. As illustrated in
The mass of robot arm 10 and tool 18 is m [kg], the barycentric position of robot arm 10 and tool 18 is xg, and the length of robot arm 10 and tool 18 is L [m].
Gravitational torque τi [Nm] applied in the pivoting direction of the i-th joint can be expressed by a function of τi=f1(m, xg, L, θi). Furthermore, gravitational torque Wi[Nm] applied in the tilting direction of the rotary shaft of the i-th joint can be expressed by a function of Wi=f2 (m, xg, L, θi).
Horizontal distance ri between the work point of tool 18 and the i-th joint can be expressed by a function of ri=f3(L, θi). Vertical distance zi, between the work point of tool 18 and the i-th joint can be expressed by a function of zi=f4(L, θi).
Next, the deflection angle due to the gravitational torque applied in the pivoting direction of second joint J2 and third joint J3 is calculated. More specifically, the deflection angle of second joint J2 can be expressed by τ2/k2. The deflection angle of third joint J3 can be expressed by τ3/k3. Note that these calculations are performed by controller 20.
Next, the displacement given to the work point of tool 18 by the tilt of the rotary shafts of first joint J1 and fourth joint J4, that is, the deflection amount, is calculated. The compensation amount can be calculated based on the deflection amounts in the horizontal direction and the vertical direction.
More specifically, total deflection amount Σr in the horizontal direction can be calculated by equation (1) given below.
Σr=(W1/M1)z1+(W4/M4)z4 (1)
Total deflection amount Ez in the vertical direction can be calculated by equation (2) given below.
Σr=(W1/M1)r1+(W4/M4)r4 (2)
Assuming that the compensation amount for the i-th joint is Δθ1 [rad], compensation amount Δθ2 for second joint J2 can be calculated by equation (3) given below.
Δθ2=(τ2/k2)+(z3−Σz−r3−Σr)/(z3r2−r3z2)4 (3)
That is, compensation amount Δθ2 of second joint J2 is calculated based on the deflection angles of first joint J1, second joint J2, and fourth joint J4.
Compensation amount Δθ3 for third joint J3 can be calculated by equation (4) given below.
Δθ3=(τ3/k3)+(r2−Σr−z2−Σz)/(z3·r2−r3·z2) (4)
That is, compensation amount Δθ3 of third joint J3 is calculated based on the deflection angles of first joint J1, third joint J3, and fourth joint J4. Controller 20 then drives second joint J2 based on compensation amount Δθ2 of second joint J2 and drives third joint J3 based on compensation amount Δθ3 of third joint J3. This makes it possible to eliminate the deflection generated in first joint J1 to fourth joint J4.
The relationship between joint angle θ2 of second joint J2 and an error with respect to the target position of the work point of tool 18 will be described below.
First, in a state in which lower arm 13, first upper arm 14, and second upper arm 15 of robot arm 10 are linearly extended, as illustrated in
Next, as illustrated in
As illustrated in
In this case, when robot arm 10 sequentially changes joint angle θ2 of second joint J2 so as to take the position illustrated in
With reference to
With reference to
As illustrated in
Accordingly, in calculating a compensation amount, a compensation amount can be calculated in consideration of an actual deflection angle as compared with the conventional deflection compensation method, and the positional displacement of the work point of tool 18 can be suppressed.
The following will describe the relationship between joint angle θ3 of third joint J3 and an error with respect to the target position of the work point of tool 18.
First, as illustrated in
Next, as illustrated in
As illustrated in
In this case, when robot arm 10 sequentially changes joint angle θ3 of third joint J3 so as to take the position illustrated in
With reference to
As illustrated in
Accordingly, in calculating a compensation amount, a compensation amount can be calculated in consideration of an actual deflection angle as compared with the conventional deflection compensation method, and the positional displacement of the work point of tool 18 can be suppressed.
As described above, the method of controlling robot 1 according to the present exemplary embodiment calculates compensation amounts Δθ2 and Δθ3 for second joint J2 and third joint J3 using not only the deflection angles of second joint J2 and third joint J3, pivotable in the gravity direction, in the pivoting direction of their rotary shafts but also the deflection angles of first joint J1 and fourth joint J4 in the tilting direction of their rotary shafts. Compensation amounts Δθ2 and Δθ3 thus calculated reduce the positional displacement of the tip of robot arm 10.
<Specific Position>
The inventor of the present application has found that there is a specific position in which the deflection compensation amount rapidly increases when robot arm 10 is in a predetermined position (see
More specifically, equation (3) described above derives compensation amount Δθ2 of second joint J2. Equation (4) described above derives compensation amount Δθ3 of third joint J3. Equations (3) and (4) each include (z3 r2−r3 z2) as a variable of the denominator of the second term.
In this case, the variable (z3r2−r3 z2) is equivalent to the value obtained by integrating distance D1 and distance D2. Distance D1 is the distance between the axes of the rotary shafts of second joint J2 and third joint J3. Distance D2 is the distance from the work point of the tip of robot arm 10 to virtual straight line 30 passing through the axis of second joint J2 and the axis of third joint J3.
When the variable (z3r2−r3z2) approaches “0”, the deflection compensation amount, that is, the correction angle diverges infinitely and rapidly increases. In the following description, the variable (z3r2−r3z2) is set as a determination value for determining a specific position.
As illustrated in
Therefore, when a deflection compensation amount is calculated using equations (3) and (4) described above even though robot arm 10 is in a specific position, the correction angle rapidly changes. Therefore, the safety function of robot arm 10 may work and the apparatus may stop.
Therefore, in the present exemplary embodiment, when the absolute value (|z3 r2−r3z2|) of the determination value is smaller than predetermined threshold value Dth, the calculation formula for a deflection compensation amount is changed such that the correction angle continuously changes with emphasis on stability.
More specifically, in the case illustrated in
On the other hand, when the absolute value of the determination value is smaller than the threshold (|z3r2− r3 z2|<Dth), the determination value is not included in the denominator of the second term, and a deflection compensation amount is calculated using a calculation formula capable of suppressing a rapid increase in the deflection compensation amount.
For example, in the hatched range (from −0.1 to +0.1) in
Within this range, the change in correction angle may be linearly complemented as illustrated in
A calculation formula for calculating a deflection compensation amount when the determination value is smaller than the threshold value will be described below. Compensation amount Δθ2 of second joint J2 can be calculated by equation (5) given below.
Δθ2=(τ2/k2)+(z3Σr−r3Σz)(z3r2−r3z2)/Dth2 (5)
That is, compensation amount Δθ2 of second joint J2 is calculated based on the deflection angles of first joint J1, second joint J2, and fourth joint J4, and threshold value Dth.
Compensation amount Δθ3 for third joint J3 can be calculated by equation (6) given below.
Δθ3=(τ3/k3)+(r2·Σz−z2·Σr)(z3r2−r3z2)/Dth2 (6)
That is, compensation amount Δθ3 of third joint J3 is calculated based on the deflection angles of first joint J1, third joint J3, and fourth joint J4, and threshold value Dth.
In the region where the determination value is smaller than −0.1 and the region where the determination value is larger than 0.1, compensation amount Δθ2 of second joint J2 is calculated using equation (3) described above. On the other hand, within the hatched threshold range (from −0.1 to +0.1) in
In the region where the determination value is smaller than −0.1 and the region where the determination value is larger than 0.1, compensation amount Δθ3 of third joint J3 is calculated using equation (4) described above. On the other hand, within the hatched threshold range (from −0.1 to +0.1) in
As described above, in the case illustrated in
In the above exemplary embodiment, robot arm 10 includes first joint J1, second joint J2, third joint J3, and fourth joint J4 in this order. The present invention is also applicable to a robot arm including first joint J1 to fourth joint J4 in an order different from that in the above exemplary embodiment. For example, the robot arm may include first joint J1, fourth joint J4, second joint J2, and third joint J3 in this order. Similarly to the exemplary embodiment, first joint J1 and fourth joint J4 are configured such that the tilt of each rotary shaft affects the positional displacement of the tip of robot arm 10. Similarly to the exemplary embodiment, second joint J2 and third joint J3 are configured such that each rotary shaft rotates in the gravity direction.
As described above, the present invention is extremely useful and has high industrial applicability because it is possible to obtain a highly practical effect that the positional displacement of the tip of the robot arm due to the deflections of the joint of the robot arm can be suppressed.
Number | Date | Country | Kind |
---|---|---|---|
2019-104609 | Jun 2019 | JP | national |
Number | Date | Country |
---|---|---|
9-174466 | Jul 1997 | JP |
2010-058256 | Mar 2010 | JP |
2013-094947 | May 2013 | JP |
2014-180726 | Sep 2014 | JP |
2019-188500 | Oct 2019 | JP |
Entry |
---|
Office Action dated Jul. 13, 2023 in corresponding Indian Patent Application No. 202147055601. |
International Search Report of PCT application No. PCT/JP2020/017624 dated Jul. 14, 2020. |
Number | Date | Country | |
---|---|---|---|
20220072702 A1 | Mar 2022 | US |
Number | Date | Country | |
---|---|---|---|
Parent | PCT/JP2020/017624 | Apr 2020 | US |
Child | 17531267 | US |