JPH02141808A - Angular velocity control method - Google Patents

Abstract

PURPOSE:To shorten the movement time for the operation of less load by controlling the angular velocity at each time of operation based on inputted positioning data and the load applied to a front end arm so that the torque applied to a motor is maximum. CONSTITUTION:A robot main body consists of a column 1, a first arm 11, a second arm 12, motors 13 and 14 which drive respective arms, a driving circuit part which drives respective motors, and a numerical controller 3. The numerical controller 3 consists of an arithmetic unit 31, a storage device 32, and a speed command device 33, and the angular speed of motors for slope up and slope down is determined based on positioning data stored in the storage device 32 at each time of operation, and a speed command signal is inputted to the driving circuit part 2 through the speed command device 33. This signal is amplified in the driving circuit part 2 to drive first and second motors 13 and 14.

Description

DETAILED DESCRIPTION OF THE INVENTION Field of the Invention The present invention relates to a method for controlling the angular acceleration of a first arm and a second arm in an articulated robot.

Conventional technology The torque 7 applied to the motor of the arm when the horizontal articulated robot is stretched up and down is determined by the posture of the second arm at that time, the motor rotation speed ratio of the first arm and the second arm required for movement, and the It differs depending on the rotation direction of the first arm and the second arm, etc. Therefore, conventional angular acceleration control calculates the optimal slow-up and slow-down times to maximize the torque applied to at least one of the motors within an allowable range based on the positioning data of the two points Pi, P, and +1. We are trying to shorten the time.

Problems to be Solved by the Invention However, since the above-mentioned method only handles position data at two points, the load applied to the tip arm must always be set at a maximum value for safety reasons. This means that the slow-up and slow-down operations under no load require the same amount of time as the slow-up and slow-down operations under the maximum load, which is a major obstacle when reducing travel time. Ta.

Means for Solving the Problems The present invention provides the load amount W that affects the torque of the motor.
i can be determined by setting a predetermined load amount using a command or sensor device before issuing an operation command in the numerical control device, or by setting the Z-axis at the tip of the second arm that supports the load amount Wi. ! Torque curve (I) showing the relationship between the current 2 of the moving motor and the torque T2, and the torque T2 applied to this Z-axis motor.
and the load curve WCTl indicating the relationship between the load amount Wi and the load amount Wi; 1) reading the current 2 flowing through the two-axis motor; 2) calculating Z from the current 2 and the torque curve T(I);
This step consists of the steps of determining the torque T22 applied to the shaft motor; (2) determining the load amount W from the torque T2 and the load curve WCTl;

Effects According to the present invention, with the above-described configuration, the angular acceleration can be controlled to maximize the capabilities of the motor and drive circuit and always move in the shortest time, taking into consideration the weight loaded on the tip of the arm.

EXAMPLE An example of the present invention will be described below with reference to FIG.

In the figure, 1 is a support of the robot body, 11 is a first arm connected to the support 1, 12 is a second arm, and 13 and 14 are motors that drive the first arm 11 and the second arm 12. Reference numeral 2 denotes a drive circuit section for driving the motors 13 and 14. 3 is a numerical control device, and an arithmetic device 31. Storage device 32. A device consisting of a speed command device 33 determines the angular acceleration of the motor during slow-up and slow-down for each operation from the stored positioning data, and inputs a speed command signal to the drive circuit section 2 through the speed command device 33. It is something to do.

Next, a method of acceleration control will be explained based on FIG. 2.

Even if the slow-up time is the same, the tono reflex during slow-up differs greatly depending on the attitude (θ2) of the second arm 12, the motor rotation speed ratio of the first arm 11 and the second arm 12 necessary for movement, and the rotation direction.

When the tip of the second arm moves from a certain point P to a certain point P, +1, the movement angle of the first arm 11 and second arm 12 is Δ
If the first arm 11 and the second arm 12 start and stop simultaneously at θ1 and Δθ2, the angular velocity ω1 of each motor in the steady state.

ω2 is expressed as: ■ When Δθ1×Town≦Δθ2×-; ■ When Δθ1×Town>Δθ2xX'n2. Here, ml, m2 are the first arm, and deceleration ωM of the second arm is the maximum angular velocity of the motor.

By the way, according to the laws of mechanics, the torques T1(t), T,(t) applied to the motors of the first arm 11 and the second arm 12 are expressed, for example, as follows.

mt) = f u(t) * o < t≦1, but TA
=(as+ae6w+(a7+aB ・W)cosθ2
)ω1+(a9+a1oIIW+(a11+a12・W
)IIcO5θ2)ω2−(a13+a14eW)−(
2ω1・ω2+ω22)・sinθ2 However, TB=(b5
+b6・W)ω2 +(b7+b8-W+(b9+b1°-W) -co
sθ2)ω1+(b11+b12・W)ω1・sinθ
2 Here, W is the load at the end of the arm, ωA is the angular velocity of the first motor, ω2 is the angular velocity of the second motor, and a1ta2,
......, a14 and bl, b2.・・・・・・、
b12 is a constant and varies depending on the length, weight, and frictional resistance of the robot arm.

Therefore, the slow-up curve is set as a smooth curve as shown in FIG. 3, and the angular velocities of the first and second motors at steady state are set to ω1. ω2 and the slow-up time is tu.

If the attitude angle of the second arm 12 at the start of operation is 028, the torque applied to each motor during slow-up is W as follows.
, θ28. ω1. It can be expressed as a function of ω2m'u.

TI(t)=cr1 (W, θ2s meeting ω1+'21tu
)T2(t)=q2(W, θ28.ω1.ω2s'u)
Therefore, when 1α1 × 1, ω1 = ωMt ω2” a””M1α
When 1>1, ω1C・ωM, ω2−ωMα, so using W, θ2s, α as parameters, the minimum slow-up time tui k required for the operation with each W, 02B, α Fig. 4 It can be calculated according to the flowchart.

The algorithm of the next flowchart (FIG. 4) will be explained.

■ Slow-up time tuTh Appropriate initial value 'u.

shall be.

■ Find the torque T 1 (t) t 'r 2 (t) of the first arm and the second arm during slow-up ■ Otherwise, find t.

■ Set tu-=max(tul, tu,) and return to ■.

However, max (tul, tu2) = where ■ T1 = maxT1 (t), T2 = mrxT2 (t)
as 1′r1−”Mll<ε1 or I”2−”M2
If +<ε2, the parameter θ28 for which the current tu is being executed
．． The optimum slow-up time 'ui is given to α.

TMl is the maximum output torque allowed for the first motor.

The recommendation is to set the second motor to the second motor.By repeating this process, the optimum slow-up time tui can be found for any W and θ2Jα.

The optimal slow-up curve for tui thus determined is determined as follows: (1) When ω1i (tui)-0M; (2) When ω2i (tui)-'M, where 0≦t t tui.

In the case of slowdown, if θ2Ee is the attitude angle of the second arm at the end point Pl+1 of the motion, it is determined by using the same method as in the case of slowup, using 02E'α as a parameter.

There is a characteristic that

The angular velocity data thus determined is input to the velocity command device 33 and outputted to the drive circuit section 2 as a velocity signal. The signal is further amplified by the drive circuit section 2, and the first motor 13 and the second motor 14 are driven.

Next, a method for calculating the load amount Wi applied to the tip of the second arm will be explained based on FIG. 8.

(1) First, it is determined whether the load amount W is set by a program instruction or a sensor device.

■ If the load amount W1 is not set, then the Z-axis motor current value z that supports this Wi at the tip of the second arm
' is read using a current transformer, A/D converter, etc. If the motor is an AC motor, you can generally calculate the current equivalent to the DC motor current by performing the following conversion: W, = W ('r2) In the above multiple example, the first motor and the second motor Although the explanation has been made assuming that the maximum angular velocities of the two are the same, this is not necessary. The above algorithm can be explained more generally using a flowchart as follows (see FIG. 6).

(2) The torque T2 of the z-axis motor is obtained from the above motor current value I, (or I,) and the torque curve "(I) already stored in the memory by the following equation.

T='r(t2) (2) The load amount Wi is determined by the following equation from the torque T2 of the two-axis motor and the load curve already stored in the memory.

However, fd(0)-1, fd(1)-〇 Enter.

Also, input the torque constant KT of the two-axis motor and the load curve showing the relationship between the torque and the load amount Wi.

■Enter the data for 2 points PP. Also, negative it+1 Enter or calculate the load amount Wi.

■ Torque “ul(
t) −”u2(j) for tEco, tu) l Tul(t) l≦Yu1 t l Tu2(t)
l≦Yu2, and for t(2(0, tu), l T ul (t) l =7u 1 or l Tu
tu which makes 2(t) l =〒u2 is determined by the optimization method shown in FIG. Here Tul s”u2
is the maximum torque allowed for the first and second motors during slow-up.

■ The optimal slow-up time obtained in step ■ is now u.
Set it as 1.

■ The angular velocity of the first motor and second moke is ω14(t)*
ω2. (1), the optimal slow-up curve is found as follows.

■ When ω1i (tui) is 01M ■ When ω2i (tui) is 02M From then on, steps 2 to 2 are repeated until Pi+1 reaches the final point.

Moreover, the optimal slowdown curve can be considered in exactly the same way.

Next, another example will be described.

According to the explanation of Example 1, if the attitude angle θ28 of the second arm at point P, the rotation speed ratio α of the motor, and the rotation direction of the motor (arm) are known from the load amount W, the data at point PiP, +1, then It is possible to find the optimal slow-up time tui at that time.

Therefore, by using W, θ28, and α as discrete value parameters, their optimal slow-up times can be calculated in advance and expressed as follows (here, for simplicity, the maximum rotation speeds of the first and second motors are assumed to be the same). ).

Here l, m, n are l = 1.2,...
・-, km=1.2. ......, n=1.2. ......, ni'', and θ7*''mtβ. , Wn is 0≦θl-1<θ1<180'-1≦am-1<α□≦1 -1≦β. −1〈β. ≦1 0≦Wn-1〈Wn≦”wax (maximum allowable load).

Since these tu and tu can all be stored in advance in the storage device 32, they can be approximately determined according to the program.

Once the slow-up time 'ui is found, as in Example 1,
The optimal slow-up curve at that time can be found.

The above is expressed in flowchart F as shown in FIG.

Furthermore, it is clear that the same concept can be applied to slowdown.

Furthermore, even when the maximum angular velocities of the first motor and the second motor are different, the same concept can be applied as shown in the first embodiment.

In this embodiment, θ2, α, and the rotational direction of the motor are used as elements that affect the torque of the motor (Vt-), but other elements may be used instead. The point is, no matter how you express it, Wl
It is sufficient that all elements that affect the motor torque according to and PLv Pi +1 are included.

In the first embodiment, it is possible to obtain optimal slow-up and slow-down curves. In the second embodiment, the parameter θ2. This is a sub-optimal slow-up/slow-down curve in the sense that α and W are obtained by converting them into discrete values, but in general, the slow-up is performed after reading the data of Wi, points PL, P, +1.

A feature of this method is that the processing time required to obtain the slowdown curve can be significantly shortened compared to the former method.

Effects of the Invention As described above, the present invention controls the motor of at least one of the first arm or the second arm during slow-up or slow-down, based on the input positioning data and the load applied to the tip arm for each operation. The angular acceleration is controlled so that the torque applied to the shaft is maximized within the allowable range. Therefore, compared to the conventional case where the load applied to the distal end arm is fixed at the maximum allowable amount and the angular acceleration is controlled, there is an effect that the movement time can be shortened for operations with a small load.

[Brief explanation of the drawing]

FIG. 1 is a schematic configuration diagram of a horizontal articulated robot according to an embodiment of the present invention, FIG. 2 is a diagram showing the relationship between the angular velocity curves of the first arm and the second arm and the torque curve applied to the motor at that time, and FIG. The figure shows a standard slow-up curve, Figure 4 is a flowchart for determining the optimal slow-up time, Figure 6 is a flowchart for determining the optimal slow-up and slow-down curves of the first embodiment, and Figure 6 is a program. A flowchart for searching the optimum slow-up time from a table based on data; FIG. 7 is a flowchart for determining the optimum slow-up curve of the second embodiment;
FIG. 8 is a flowchart for determining the load amount Wl. 1... Support of the robot body, 11...
First arm, 12... Second arm, 13...
...First motor, 14...Second motor, 2.
...Motor drive circuit section, 3 ... Numerical control device, 31 ... Arithmetic device, 32 ... Memory device, 33 ... Speed command Device. Name of agent: Patent attorney Shigetaka Awano and 1 other person rs
-Zmari 31・-Hamakagi Rifle 1 31-1・Sign Otsu+A Village, 1 Quantity Figure 4 Ward C9 Law Figure

Claims (2)

[Claims] (1) An angular acceleration control method in which a speed command is given to a motor to actuate a second arm connected to a rotatable first arm, in which the tip of the second arm moves from point P_i to point P_i.
When moving to ___+_1, a standard slow-up curve f_u (t/t_u), 0≦t≦t_u and a standard slow-down curve f_d (t/t_d), 0<t ≦t_d However, f_u(0)=f_d(1)=0 is stored, and [1] Step of reading data of two points P_i and P_i_+_1. [2] A step of reading data on the amount of load W_i applied to the tip of the arm. [3] In the slow-up curve used when moving from point P_i to point P_i_+_1, the optimal slow-up time t that maximizes at least one of the torques applied to the motor of the first arm or the second arm within the allowable range.
Step of calculating _u_i, [4] The motor angular velocities of the first arm and the second arm are ω_1_i(t), ω_2_i(
t) and the optimal slow-up curve as {ω_1_i(t)
=C_1_i・f_u(t/■_u_i), 0≦t≦■
＿u_iω_2_i(t)=C_2_i・f_u(t/
■＿u_i), 0≦t≦■＿u_i}However, C_1_i,
C_2_i is the step of finding a constant; [5] In the slowdown curve used when moving from point P_i to point P_i_+_1, the optimum value is determined to maximize at least one of the torques applied to the motor of the first arm or the second arm within the allowable range. Slowdown time■＿
Step of calculating d_i, [6] Optimum slowdown curve {ω_1_i(t)=C_1_i・f_d[(t-t_
s)/■_d_i], 0≦t−t_s≦t_d_iω_
2_i(t)=C_2_i・f_d[(t-t_s)/
■_d_i], 0≦t−t_s≦■＿d_i}However, t_
In an angular acceleration control method configured by repeating the step of determining s as a slowdown start time, as a method for determining the load amount W_i that affects the torque of the motor, a predetermined load amount is determined in a numerical control device before an operation command is issued. A method of setting by command or sensor device,
Alternatively, in the Z-axis that is at the tip of the second arm and supports the load amount W_i, the current value I of the motor that drives this Z-axis
Torque curve T_(_I
_) and a load curve W_(_T_) showing the relationship between the torque T_Z applied to the Z-axis motor and the load amount W_i, [1] reading the current value I_Z flowing through the Z-axis motor, [2] the above step; The current value I_Z and the torque curve T_(_I_
) and calculating the torque T_Z applied to the Z-axis motor from [3] the torque T_Z and the load curve W_(_T_);
An angular acceleration control method comprising: calculating a load amount W_i from. (2) An angular acceleration control method in which a speed command is given to a motor to operate a rotatable first arm and a second arm connected to the first arm, the tip of the second arm being
When moving from point P_i to point P_i_+_1, a method of giving slow-up and slow-down angular velocity commands is as follows:
The possible values of the load amount W_i that affect the motor torque are converted into discrete values, and they are expressed as @W@_(_j_), 1≦j
≦k_0, which also affects the motor torque, P
The element determined from the data of _i, P_i_+_1 is a_1
, ......, a_n, and convert each possible value into a discrete value, and convert it into ▲ mathematical formulas, chemical formulas, tables, etc. ▼ However, j and k_0, k_1, k_2, ......
k_n is expressed as an integer, and these @W@_(_j_)
, a_1(j), a_2(j), ......, a_n
The optimum slow-up time and optimum slow-down time that maximize the torque applied to at least one of the motors of the first arm and the second arm within the allowable range in all slow-up curves and slow-down curves created by combining (j). are calculated in advance, stored in memory, and given any W_i, the nearest @W@_(_j
), and given arbitrary P_i and P_i_+_1, element a is calculated from the data of P_i and P_i_+_1.
Calculate _1, a_2, ......, a_n, and find the closest a_1(j), a_2(j), ......
a_n(j), and read the positioning data of the loads W_i, P_i, and P_i_+_1 by calling the slow-up time and slow-down time for them from the memory, and then calculating the optimal slow-up curve and the optimal slow-down curve. In the angular acceleration control method for reducing time, the method of determining the load amount W_i that affects the torque of the motor is a method of setting a predetermined load amount by a command or a sensor device before an operation command in a numerical control device, or In the Z-axis located at the tip of the second arm and supporting the load W_i, a torque curve T_(_I_) showing the relationship between the current value I_z of the motor driving this Z-axis and the torque T_Z, and the torque T_Z applied to this Z-axis motor. Load curve W showing the relationship with the load amount W_i
[1] Reading the current value I_Z flowing through the Z-axis motor; [2] The step of memorizing the current value I_Z and the torque curve T_(_I_
) and calculating the torque T_Z applied to the Z-axis motor from [3] the torque T_Z and the load curve W_(_T_);
An angular acceleration control method comprising: calculating a load amount W_i from.