JPH0726577A - Working speed control device for articulated work machine - Google Patents

Abstract

PURPOSE:To improve the working accuracy of an articulated work machine by allowing the operation of an actuator to be controlled properly, according to a target locus within a maximum working speed range. CONSTITUTION:An operator inputs the proper minimum working speed Vcmin and maximum working speed Vcmax of each cylinder C to an actuator maximum working speed setting section 5. In order to set the working direction of the forward end 34 of a bucket L3, the operator inputs an operation angle beta to a working direction setting section 8. Also, values detected with each of joint angle sensors 41, 42 and 43 are inputted to a joint angle detecting section 4. The current coordinates (x, y) of the forward end 34 of the bucket L3 are obtained, and the minimum and maximum angular velocity values of each joint section, that is, (dtheta1/dt)min and (dtheta1/dt)max are calculated. This process is undertaken in a joint angular speed maximum value calculation section 6, and inputted to a target work calculation section 7. Furthermore, a target speed Vm and target coordinates (xr, yr) after a control cycle (t) are calculated in a target speed calculation section 71 and a target coordinate calculation section 72. From the target coordinates (xr, yr), the target values (thetar1, thetar2 and thetar3) of joint angles are calculated in a target joint angle calculation section 9.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a control device for controlling the operating speed of an articulated work machine such as a hydraulic excavator.

[0002]

2. Description of the Related Art Conventionally, in a multi-joint work machine such as a hydraulic excavator, a plurality of booms and arms are rotatably joined to each other around an axis, and an actuator composed of hydraulic equipment is used to make a rotative movement. Is configured. A work end such as a bucket for performing a predetermined work is provided at the tip of such an arm, and the work end is configured to perform the predetermined work.

Conventionally, in order to perform a predetermined work on the work end, a worker operates the actuator while silently watching, but in recent years, a certain work is performed by a control device. In many cases, the working end is automatically configured to perform work under the control of the work.

Regarding automatic control of such a working machine,
The one disclosed in Japanese Patent Laid-Open No. 2-101228 is known. The control device for a working machine disclosed in this publication is a working machine in which a boom, an arm and a bucket are connected in series, and is provided with an angle detecting means for detecting a boom angle, an arm angle and a bucket angle. The position of the bucket is calculated one by one from each of the above angles detected by the detection means, and it is calculated whether or not the calculated current position of the bucket and the preset target trajectory of the bucket match ( Specifically, determine the deviation between the above two), and if they do not match (when the deviation is larger than a preset value), the amount of fluid supplied to each actuator that operates the boom, arm, and bucket. Is adjusted so as to match the actual position of the bucket with the target trajectory, so-called trajectory control is performed.

And, by constructing in this way,
It is described that the position of the blade edge of the bucket can be made to accurately follow the target trajectory, and the operator can perform highly accurate automatic excavation without touching the operation lever.

[0006]

By the way, in the control device for a working machine disclosed in the above-mentioned Japanese Patent Laid-Open No. 2-101228, only the intention is to make the bucket follow a preset target locus, Since the operating speed of the actuators at each location and the structural characteristics of each drive unit are not taken into consideration, the operating speed of each actuator that operates the boom, arm, and bucket may not catch up depending on the posture of the link structure formed at each location. In the end, the bucket has a problem that it substantially deviates from the preset target trajectory.

The present invention has been made in order to solve the above-mentioned problems, and the operating speed of an articulated work machine capable of properly executing trajectory control within the range of the maximum operating speed of the actuator. The purpose is to provide a control device.

[0008]

According to a first aspect of the present invention, there is provided an operating speed control apparatus for an articulated work machine, wherein a plurality of arms are connected in series via joints and each arm is jointed. In an operation speed control device for an articulated work machine that controls the operation of the most distal arm of an articulated work machine that is provided with a plurality of actuators that rotate around the support shaft of the section, the operation speed control device is The actuator is configured to calculate the limit operating speed of the actuator according to the joint angle of each joint and to send an operation signal to each actuator so that the actuator operates within a range not exceeding the limit operating speed. It is characterized by.

According to a second aspect of the present invention, there is provided an operating speed control device for an articulated work machine according to the first aspect, wherein the operating speed control device is a joint for each joint. An angular velocity maximum value calculation unit that calculates the maximum value of the joint angular velocity from the joint angle input from the joint angle sensor that detects the angle and the maximum operating velocity of the actuator that is input in advance, and the joint input from the joint angle sensor. Target motion calculation that calculates the target speed and target position of the tip arm part from the maximum value of the joint angular velocity input from the angle and the maximum angular velocity calculation part and the movement direction of the tip arm part that is input in advance And a target joint angle calculation unit for calculating the joint angle of each joint from the target speed and the target position input from the target motion calculation unit. Calculated And it is characterized in that it is configured to transmit the operation signal to each actuator so that the section angle.

[0010]

According to the operation speed control device for an articulated work machine described in claim 1, the operation speed control device calculates the limit operation speed of the actuator according to the joint angle of each joint portion, and each actuator is Since an operation signal is sent to each actuator so that it operates within the range where this limit operation speed is not exceeded, each actuator that receives this signal can operate in a comfortable state, and as a result, The tip arm can be controlled so as to surely perform an operation along a preset target trajectory.

According to the operating speed control device for an articulated work machine described in claim 2, first, the maximum angular velocity calculation unit of the operating speed control device is input from a joint angle sensor for detecting the joint angle of each joint. The maximum value of the joint angular velocity is calculated from the joint angle and the maximum operating speed of the actuator that is input in advance, and the calculation result is transmitted to the target motion calculating unit.

The target motion calculation unit to which the maximum value of the joint angular velocity is input is arranged to match the joint angle input from the joint angle sensor with the motion direction of the front end arm body which has been input in advance. The target velocity and the target position of the body are calculated, and the calculation result is transmitted to the target joint angle calculation unit.

Then, the target joint angle calculation unit to which the target velocity and the target position of the most distal arm are input calculates the joint angle of each joint, and each joint calculates the above based on the calculation result. An operation signal is transmitted to each actuator so that the joint angle is set.

Therefore, each actuator that receives this operation signal operates so as to reach the joint angle calculated by the target joint angle calculation unit, and this operation is performed by the maximum angular velocity calculation unit previously calculated by the maximum angular velocity calculation unit. Since it can be set as follows, the operation of each actuator does not exceed its maximum operating speed, and the most advanced arm moves on the preset target trajectory in a reasonable state. To do.

[0015]

DESCRIPTION OF THE PREFERRED EMBODIMENTS FIG. 1 is a side view showing an example of an articulated work machine to which an operation speed control device according to the present invention is applied. As shown in this figure, in this embodiment, the hydraulic excavator 1 is applied as an articulated work machine. Therefore, the linear excavation work, which is very difficult for the operator to operate by the hydraulic excavator 1, will be performed in a very short range, and an arbitrary locus will be accumulated as a collection of the linear excavation work in the short range. An operation speed control device for excavating will be specifically described.

The hydraulic excavator 1 includes a caterpillar device 2 and
A driving operation room 3 provided above the caterpillar device 2
And a multi-joint arm L provided so as to project forward from the front part of the driving operation room 3. The multi-joint arm L is a boom L1 as a first arm that is directly rotatably connected to the operation room 3 about a first horizontal axis 301, and a second horizontal axis at the tip of the boom L1. 302
An arm L2 as a second arm body that is pivotally supported around, and a third horizontal shaft 3 at the tip of the arm L2.
It is configured by a bucket L3 as a third arm body (the most distal arm body) rotatably provided around 03.

The boom L1, the arm L2 and the bucket L3 are configured to rotate about each horizontal axis by actuators 30 which are hydraulically operated and are provided on the articulated arm L. Specifically, the boom actuator 3 is provided between the operation room 3 and the boom L1.
1 is attached, an arm actuator 32 is attached between the boom L1 and the arm L2, and a bucket actuator 33 is provided between the arm L2 and the bucket L3. By causing each actuator 30 to perform a predetermined operation, each articulated arm L performs a predetermined operation, and the tip end portion 34 of the bucket L3 makes a linear motion.

FIG. 2 is a model diagram for analyzing the operation of the working machine drawn corresponding to FIG. In this figure, the x-axis is set on the horizontal axis to represent the horizontal distance, and the y-axis is set to the vertical axis to represent the vertical distance. The intersection of both axes is the origin of the xy coordinates, and corresponds to the first horizontal axis 301 on the side of the operation room on which the boom L1 of FIG. 1 is pivotally supported. The effective length of the boom L1, that is, the distance between the first horizontal axis 301 and the second horizontal axis 302 is l ₁ , and the effective length of the arm L2, that is, the second horizontal axis 30.
The distance between the second horizontal axis 303 and the third horizontal axis 303 is set to l ₂ , and the effective length of the bucket L3, that is, the distance between the third horizontal axis 303 and the tip 34 of the bucket L3 is set to l _3. There is.

At a certain point in time, as shown in FIG. 2, the interior angle formed between the boom L1 and the x-axis is θ ₁ ,
It is assumed that the outer angle formed between the extension line of the boom L1 and the arm L2 is θ ₂ , and the outer angle formed between the extension line of the arm L2 and the bucket L3 is θ. In such a state, it is assumed that the operating direction of the tip end portion 34 of the bucket L3 in a very short range is inclined by an angle β (hereinafter referred to as an operation angle) with respect to the x-axis, and the operating speed thereof is V.

Therefore, first, the angular velocities of the joints in such a state (the first joint angle θ ₁ and the second joint angle θ _{2 on} the first horizontal axis 301, the second horizontal axis 302, and the third horizontal axis 303, The relationship between the angular velocity of the third joint angle θ ₃ ) and the operating speed of the tip portion 34 will be obtained.

Assuming that the coordinates of the tip end portion 34 of the bucket L3 in FIG. 2 are (x, y),

[0022]

[Formula 1] x = l ₃ · cos (θ ₁ + θ ₂ + θ ₃ ) + l ₂ · c
os (θ ₁ + θ ₂ ) + L ₁ · cos θ ₁

[0023]

[Equation 2] y = l ₃ · sin (θ ₁ + θ ₂ + θ ₃ ) + l ₂ · s
It can be expressed as in (θ ₁ + θ ₂ ) + L ₁ · sin θ ₁ .

Then, it is premised that the tip portion 34 is controlled so as to perform linear excavation in a minute range where θ ₁ , θ ₂ and θ ₃ may be any angles, and the angle β is constant. Because

[0025]

[Formula 3] α = θ ₁ + θ ₂ + θ ₃ = constant relation holds.

By differentiating each of the equations of "Equation 1", "Equation 2" and "Equation 3" with respect to time t,

[0027]

(4) (dx / dt) = (-l ₂ · sin (θ ₁ + θ ₂ ) -sin θ ₁ )
・ (Dθ ₁ / dt) -l ₂・ sin (θ ₁ + θ ₂ ) (dθ ₂ / dt)

[0028]

[Equation 5] (dy / dt) = (− l ₂ · cos (θ ₁ + θ ₂ ) −cos θ ₁ ).
・ (Dθ ₁ / dt) -l ₂・ cos (θ ₁ + θ ₂ ) (dθ ₂ / dt)

[0029]

(6) (dα / dt) = (dθ ₁ / dt) + (dθ ₂ / dt) + (dθ ₃ / dt) = 0, while (dx / dt) and (dy / dt) are Bucket tip 3
4 is the relationship between the angle β of the actuation direction with respect to the x-axis and its operating speed V:

[0030]

(7) (dx / dt) = Vcosβ

[0031]

(8) (dy / dt) = Vsinβ can be set. Therefore, by substituting the right sides of the equations 7 and 8 into the left sides of the equations 3 and 4, (dθ ₁ / dt) and (dθ ₂ / dt) are unknowns. The simultaneous equations are obtained, and by solving them, (dθ ₁ /
dt) and (dθ ₂ / dt) can be expressed by V, θ ₁ , θ ₂ and β as follows.

[0032]

[Equation 9] (dθ ₁ / dt) = ((cos (θ ₁ + θ ₂ −β) /
(L ₁ · sin θ ₁ )) · V

[0033]

(D θ ₂ / dt) = {− (l ₂ · cos (θ ₁ + θ ₂ −
β) + l ₁ · cos (θ _{1 −} β)) / (l ₁ · l ₂ · sin
θ ₁ )} · V Further, by substituting the right-hand sides of the above-mentioned “Equation 9” and “Equation 10” into the “Equation 6” and solving them, (dθ ₃ / dt) becomes V, θ ₁
And β.

[0034]

(D θ ₃ / dt) = (cos (θ ₁ −β) / (l ₂ ·
sin θ ₁ )) · V As shown in the equations (9), (10) and (11), the angular velocities of the joint angles (dθ ₁ / dt), (dθ ₂ / dt) and (dθ ₂ / dt)
θ ₃ / dt) is the operating speed V of the bucket tip 34.
Can be regarded as a function of.

By the way, regarding the angular velocities of the respective joint angles, the positive and negative maximum velocities are determined by structural restrictions, and it is impossible to move faster. Therefore, when formulating such constraints,

[0036]

(12) (dθ _i / dt) _min ≤ (dθ _i / dt) ≤ (dθ _i / dt) _max
(I = 1, 2, 3) However, in the above equation, (dθ _i / dt) _min is the minimum value of the angular velocity of each joint angle (that is, the maximum value in the negative direction), and (dθ _i / d
t) _max is the maximum value and can be specifically expressed as a specific numerical value.

[Mathematical Expression 9] is added to (dθ _i / dt) in this Expression 12.
~ By substituting the right side of the equation (11) respectively, the operating speeds of the following six bucket tip portions 34 corresponding to (dθ _i / dt) _min and (dθ _i / dt) _max of each joint angle Can be calculated.

[0038] _{V 1min, V 2min, V 3min} , V 1max, V 2max, V 3max and as shown in the following formula, the "number by picking the minimum value of the absolute values of these six operating speed 1
The target operating speed V _m of the bucket tip portion 34 can be determined from the constraint condition “2”.

[0039]

V _m = min (| V _1min |, | V _2min |, | V _3min |,
| V _1max |, | V _2max |, | V _3max |) By substituting the value of V _m obtained in this way into the above equations 9 to 11, the angular velocity of each joint angle ( dθ
₁ / dt), (dθ ₂ / dt) and (dθ ₃ / dt) are obtained. Therefore, if the angular velocity control of each joint angle is performed by a known control method so as to realize these angular velocities, the tip end portion 34 of the bucket L3 holds the state in which the operating direction is inclined at the angle β with respect to the x axis. _Will operate at a speed of V _m .

By the way, the above (dθ _i / dt) _min and (dθ _i / dt)
_For the value of _max , each actuator 31, 32, 3
It can be calculated if the speed range of 3 is known in advance. This calculation method will be described based on FIG. 3, which generally shows the joint drive mechanism. In this figure, θ is the joint angle, θ _L1 is the angle formed between the vector a _L1 and the x axis, and θ _L2 is the angle formed between the vector a _L2 and the y axis. The relationship shown in the following formula is established between them.

[0041]

[ _Equation 14] θ _L2 = θ + θ _off2

[0042]

[Equation 15] (dθ _L1 / dt) = 0

[0043]

[Mathematical formula-see original document] θ _c = arctan {(a _L1 · sin θ _L1 + a _L2 · sin θ
_L2 ) / (a _L1 · cos θ _L1 + a _L2 · cos _L2 )}

[0044]

[Equation 17] (da _c / dt) = a _L2 · sin (θ _c + θ _L2 ) · ( _d θ / dt) Then, the minimum operating speed of the cylinder C constituting the actuator (that is, the maximum speed in the negative direction) V _cmin ,
Since the velocity of the vector a _c must be included between the maximum operating velocity V _cmax and the conditional equation of “Equation 17” holds.

[0045]

[Equation 18] V _cmin ≤ (da _c / dt) ≤ V _cmax Therefore, by substituting "Equation 17" into "Equation 18" and solving, the above (dθ / dt) _min and (dθ / dt) _max The value can be specifically obtained as follows.

[0046]

(D θ / dt) _min = V _cmin / {a _L2 · sin (θ _c + θ _L2 )}

[0047]

(D θ / dt) _max = V _cmax / {a _L2 · sin (θ _c + θ _L2 )}. That is, each actuator 31, 3
By performing such a calculation with respect to Nos. 2 and 33, the values of (dθ _i / dt) _min and (dθ _i / dt) _max can be specifically obtained.

Next, the coordinates (x _r , y _r ) (when the time t, which is the end of control, elapses from when the tip end portion 34 of the bucket L3 shown in FIG. 2 is at the position of coordinates (x, y). The target position) is derived and calculated by the following equations. First, the bucket L is moved to the coordinate (x _r , y _r ).
The angle β of 3 does not change. At this time, the operating speeds of the tip portion 34 of the x component and the y component, V _x and V _y are
3 "can be used to express as follows.

[0049]

V _x = V _m cos β, V _y = V _m sin θ Therefore, the target position can be expressed by the following equation.

[0050]

X _r = x + V _x · t, y _r = y + V _y · t Then, the angle of each joint angle when reaching the target position is
If the boom L1 is θ _r1 , the arm L2 is θ _r2 , and the bucket L3 is θ _r3 ,

[0051]

[Mathematical formula-see original document] x _r = l ₃ · cos (θ _r1 + θ _r2 + θ _r3 ) +1
₂・ cos (θ _r1 + θ _r2 ) ＋ l ₁・ cos θ _r1

[0052]

Y _r = l ₃ · sin (θ _r1 + θ _r2 + θ _r3 ) +1
₂ · sin (θ _r1 + θ _r2 ) + l ₁ · sin θ _r1 Then, x ′ and y ′ are expressed as in “Equation 24” and “Equation 26”,

[0053]

[Expression 25] l ₂ · cos (θ _r1 + θ _r2 ) + l ₁ · cos θ _r1 = x ′

[0054]

L ₂ · sin (θ _r1 + θ _r2 ) + l ₁ · sin θ _r1 = y ′ These “equation 25” and “equation 26” are used to calculate “equation 23”,
The following equations are obtained by variously modifying the equation 24.

[0055]

Cos (θ _r2 ) = (x ′ ² + y ′ ² −l ₁ ² −l
₂ ² ) / (2 · l ₁ · l ₂ )

[0056]

(28) sin (θ _r2 ) = ± (1-cos ² (θ _r2 ))

[0057]

Tan (θ _r2 ) = sin (θ _r2 ) / cos (θ _r2 ).

[0058]

(30) cos (θ _r1 ) = {(l ₁ + l ₂ · cos
(Θ _r2 )) + l ₂ · sin (θ _r2 ) · y ′} / {(l ₁ + l ₂
・ Cos (θ _r2 ) ² + l ₂ ²・ sin ² (θ _r2 )}

[0059]

(31) sin (θ _r1 ) = {− l ₂ · sin (θ _r2 ) × x ′
+ (L ₁ + l ₂ · cos (θ _r2 )) · y ′} / {(l ₁ + l ₂ · co
s (θ _r2 ) ² + l ₂ ² · sin ² (θ _r2 )}

[0060]

Tan (θ _r1 ) = sin (θ _r1 ) / cos (θ _r1 ).

[0061]

[Equation 33] θ _r3 = β−θ _r1 −θ _r2 By solving the equations of “Equation 27” to “Equation 33” (for example, tan (θ _r1 ) and tan (θ _r2 ) are unknowns, the equation is solved. , Tan (θ _r1 ), tan (θ _r2 ) and θ _r1 , θ _r2, and θ _r3 from Equation 33), θ _r1 , θ _r2, and θ _r3 Is obtained.

The values of θ _r1 , θ _r2 and θ _r3 obtained in this way are one control when the coordinates of the tip portion 34 of the bucket L3 are (x, y) in the graph shown in the figure. It shows each joint angle when the cycle changes to (x _r , y _r ). Conversely speaking, each actuator 30 is controlled so as to have such a joint angle (that is, a target joint angle) by the angular velocity (dθ ₁ / d) of each joint angle described above.
By operating with t), (dθ ₂ / dt) and (dθ ₃ / dt), the tip end portion 34 of the bucket L3 moves reasonably and reliably while maintaining the operation angle β, and the coordinates (x _r , y _r ) can be reached.

Therefore, the tip portion 34 has coordinates (x _r ,
y _r ), a new operation angle β is given and (dθ _r1 / dt), (dθ _r2 / dt) at this time
By calculating and (dθ _r3 / dt) and operating the boom L1, the arm L2, and the bucket L3 at the angular velocities of these new joint angles, the articulated arm L performs the operation in the next control cycle.

The control cycle is set to be short enough to have concrete validity, and the operation angle is sequentially input for each control cycle, and the angular velocity of each joint angle is calculated. By moving the body L, it is possible to surely cause the tip end portion 34 of the bucket L3 to perform a desired operation.

Basically, the following "Equation 1" to "Equation 33"
The operation control of the articulated work machine in the method represented by the formula will be described based on the block diagram showing an example of the control block according to the present invention in FIG. As shown in this figure, the motion speed control device K according to the present invention includes a joint angle detection unit 4, an actuator maximum motion speed setting unit 5, a joint angular speed maximum value calculation unit 6, a target motion calculation unit 7, Movement direction setting unit 8
And a target joint angle calculation unit 9 are basically configured.

The joint angle detector 4 is for detecting the joint angle of the multi-joint arm L and inputting the detection result to the joint angular velocity maximum value calculator 6. Therefore, the joint angle detection unit 4 uses the joint angle of the boom L1 (first joint angle θ ₁ ), the joint angle of the arm L2 (second joint angle θ ₂ ), and the joint angle of the bucket L3 (third joint angle θ 1). ₃ ) is provided with a first joint angle sensor 41, a second joint angle sensor 42, and a third joint angle sensor 43. The joint angle detector 4 converts the analog signals input from the joint angle sensors 41, 42 and 43 into digital signals and outputs the digital signals to the joint angular velocity maximum value calculator 6.

The maximum operating speed setting unit 5 of the actuator is one of the input devices for the operating speed control device K by the operator. Then, in the maximum operating speed setting unit 5, the minimum operating speed V of the cylinder C of each actuator 30 (boom actuator 31, arm actuator 32, bucket actuator 33) is set.
_{The cmin} and the maximum operating speed V _cmax are input to the joint angular velocity maximum value calculation unit 6.

The joint angular velocity maximum value calculation unit 6 calculates each joint angle θ _i (θ ₁ , θ) input from the motion velocity control device K.
₂ , θ ₃ ) and the minimum operating speed V of the cylinder C input from the actuator maximum operating speed setting unit 5
Each of (dθ _i / Equation 20) to (Equation 20) is programmed in advance from _cmin and the maximum operating speed V _cmax.
dt) _min and (dθ _i / dt) _max values (where i = 1, 2,
3) is calculated.

The target motion calculation unit 7 calculates the target motion speed for reaching the target position after the control cycle t has elapsed from the coordinates (x, y) of the tip portion 34 of the current bucket L3 shown in FIG. The target speed calculation unit 71 for calculating and the target coordinate (x
_r , y _r ) and a target position calculation unit 72.

Then, in the target speed calculation unit 71, each joint angle θ input from the joint angle detection unit 4 is inputted.
_{Based on the value of i} (θ ₁ , θ ₂ , θ ₃ ) and the operation angle β input from the movement direction setting unit 8, “Equation 12” to “Equation 13”
The target operating speed V _m is calculated based on the equation. The operation direction setting unit 8 is another one of the input devices by the operator for the operation speed control device K.

Further, in the target position calculating section 72, each joint angle θ _i (θ ₁ ,
θ ₂ , θ ₃ ), the value of the operation angle β input from the operation direction setting unit 8, and the value of the target operation speed V _m input from the target speed calculation unit 71. The target coordinates (x _r , y _r ) after the control cycle t has elapsed are calculated based on the expressions (21) and (22).

The target joint angle computing unit 9 uses the current joint angles θ to reach the target coordinates (x _r , y _r ).
_i (θ ₁ , θ ₂ , θ ₃ ) is a calculation unit that calculates what angle should be changed, and the target coordinate (input from the target position calculation unit 72 of the target operation calculation unit 7) x _r ,
y _r ) is calculated based on the equations (27) to (33), and the target joint angles θ _ri (θ _r1 , θ _r2 ,
θ _r3 ) can be obtained. The value of the target joint angle θ _ri thus obtained is obtained from the target joint angle calculation unit 9 for each actuator 30 (boom actuator 31,
The actuator 32 for the arm and the actuator 33 for the bucket) are transmitted to a controller (not shown), and the controller which receives the control controls each actuator 30 to perform a predetermined operation.

FIG. 5 is a flow chart showing an example of operation speed control according to the present invention. The operation of the operating speed control device of the present invention will be described below with reference to this drawing. First, in step S1, the maximum value of the operating speed of each actuator 30 is set. This setting is performed by the operator inputting the minimum operating speed V _cmin and the maximum operating speed V _cmax unique to each cylinder C to the actuator maximum operating speed setting unit 5.

In the next step S2, the operation direction of the tip end portion 34 of the bucket L3 is set. The operation direction is set by the operator inputting the operation angle β into the operation direction setting unit 8. This operation angle β
In general, a value (β _i ) is input for each of a plurality of control cycles t _i , and i is executed from 1 to n, whereby the bucket L3 can be surely caused to perform a desired operation.

Then, in step S3, the detection values from the joint angle sensors 41, 42, 43 are input to the joint angle detecting section 4 of the motion speed control device K. Then, in step S4, the coordinates (x, y) of the current tip end portion 34 of the bucket L3 are obtained, and in step S5, the maximum angular velocity values (each (dθ _i / dt) _min , and (dθ)
The value of _i / dt) _max (where i = 1, 2, 3) is calculated.
The processes in steps S4 and S5 are performed in the joint angular velocity maximum value calculation unit 6. The calculated value of the maximum angular velocity of each joint is input to the target motion calculation unit 7.

Next, in step S6, the target speed V _m of the tip portion 34 of the bucket L3 after the control period t has elapsed.
Is calculated, and in step S7, the target coordinates (x _r , y _r ) of the tip portion 34 of the bucket L3 after the control period t has elapsed.
Is calculated. These are executed in the target speed calculation unit 71 and the target position calculation unit 72 of the target operation calculation unit 7.

Then, in step S8, the target values (θ _r1 , θ _r2 , θ _r3 ) of the joint angles are calculated from the target coordinates (x _r , y _r ) of the tip portion 34 of the bucket L3. This calculation is performed in the target joint angle calculation unit 9. This calculation result is transmitted to a controller (not shown) of each actuator 30, and the multi-joint arm L (boom L1, arm L2, bucket L3) is controlled by the tip coordinate 34 of the bucket L3 while being controlled by this controller. A predetermined operation is performed so as to aim at (x _r , y _r ), and the first horizontal shaft 301, the second horizontal shaft 302, and the third horizontal shaft 303 are rotated.

Finally, in step S9, it is determined whether or not the control is completed. If it is not completed, the process is fed back to step S3 and the processes of the above steps are repeatedly executed.

Since the operation speed control device for the articulated work machine of the present invention is configured to determine the operation of each joint portion from the maximum operation speed of each actuator 30 as described above, the bucket L3 as in the prior art. The control is not performed by paying attention to only the operation of the tip portion 34 of the above, and as a result, each actuator 30 is not forced to perform an unreasonable operation. Therefore, since there is no operation delay caused by force, the tip end portion 34 operates in a state of surely following the preset control locus, and the operation accuracy of the articulated work machine improves.

In this embodiment, the bucket L3
The maximum speed V _m is adopted as the speed V of the tip portion 34 of the above, but the speed is not particularly limited to the maximum speed V _m , and a speed that does not exceed this speed can be arbitrarily set.

Further, in this embodiment, the articulated work machine employs three joints including the boom L1, the arm L2 and the bucket L3, but the joints are not limited to three. No, it may be two or four or more.

Further, the joint drive mechanism is not limited to the one using the cylinder C as shown in FIG. 3, but may be one using an electric motor or the like.

[0083]

As described above in detail, the operation speed control device for an articulated work machine according to the present invention calculates the limit operation speed of the actuator according to the joint angle of each joint, and each of the actuators operates at this limit operation speed. Actuator signals are sent to each actuator to operate within a range that does not exceed speed, so each actuator that receives this signal can operate in a comfortable state, and as a result, the most advanced arm Can be controlled so as to reliably perform an operation along a preset target trajectory.

An angular velocity for calculating the maximum value of the joint angular velocity from the joint angle input from the joint angle sensor for detecting the joint angle of each joint and the maximum operating velocity of the actuator which is input in advance in the operating velocity control device. The maximum value calculation unit, the joint angle and the angular velocity input from the joint angle sensor, the maximum value of the joint angular velocity input from the maximum value calculation unit, and the tip end portion that is input in advance. A target motion calculation unit that calculates a target speed and a target position of the arm, and a target joint angle calculation unit that calculates a joint angle of each joint from the target speed and the target position input from the target motion calculation unit. If the target joint angle calculation unit transmits an operation signal to each actuator so that each joint unit has the calculated joint angle, the angular velocity maximum value calculation unit first A joint angle joint angle input from the sensor for detecting the section angle, the maximum value of the joint angular velocity is calculated from the maximum operating speed of the actuator previously entered,
The calculation result is transmitted to the target motion calculation section.

In the target motion calculating section to which the maximum value of the joint angular velocity is inputted, the joint angle inputted from the joint angle sensor and the movement direction of the tip portion arm body inputted in advance are combined into the tip portion. The target speed and the target position of the arm are calculated, and the calculation result is transmitted to the target joint angle calculation unit.

Then, the joint angles of the joints are calculated by the target joint angle calculation unit to which the target velocity and the target position of the most distal arm are input, and based on the calculation result, each joint calculates the above. An operation signal is sent to each actuator so that the joint angle is set.

Therefore, each actuator that receives this operation signal operates so as to reach the joint angle calculated by the target joint angle calculation unit, and this operation is performed by the maximum angular velocity calculation unit previously calculated by the maximum angular velocity calculation unit. Since it can be set as follows, the operation of each actuator does not exceed its maximum operating speed, and the most advanced arm moves on the preset target trajectory in a reasonable state. To do.

As described above, in the present invention, the maximum value of the angular velocity of the joint of the arm is calculated one by one from the maximum operating speed of the actuator, and each arm is prevented from exceeding the maximum value based on the calculation result. Since the actuator is operated so as to change the joint angle thereof, the actuator is not always forced to perform an unreasonable operation, and can operate properly and satisfactorily.

[Brief description of drawings]

FIG. 1 is a side view showing an example of an articulated work machine to which an operation speed control device according to the present invention is applied.

FIG. 2 is a model diagram for analyzing the operation of the work machine.

FIG. 3 is an explanatory diagram showing a relationship between an operation of a cylinder of a joint portion and a joint angle.

FIG. 4 is a block diagram showing an example of a control block according to the present invention.

FIG. 5 is a flowchart showing an example of operation speed control according to the present invention.

[Explanation of reference numerals] 1 hydraulic excavator 2 caterpillar device 3 operating room 30 actuator 31 boom actuator 32 arm actuator 33 bucket actuator 4 joint angle detection unit 41 first joint angle sensor 42 second joint angle sensor 43 third joint Angle sensor 5 Actuator maximum operating speed setting unit 6 Joint angular velocity maximum value computing unit 7 Target motion computing unit 71 Target speed computing unit 72 Target position computing unit 8 Operating direction setting unit 9 Target joint angle computing unit

Claims (2)

[Claims] 1. A multi-joint working machine comprising: a plurality of arms connected in series through joints; and a plurality of actuators for rotating each arm around a support axis of the joints. In an operation speed control device for an articulated work machine that controls the operation of a distal arm, the operation speed control device calculates a limit operation speed of an actuator according to a joint angle of each joint, and each actuator described above An operating speed control device for an articulated work machine, which is configured to send an operating signal to each actuator so as to operate in a range not exceeding a limit operating speed. 2. The motion speed control device calculates the maximum value of the joint angular speed from the joint angle input from a joint angle sensor that detects the joint angle of each joint and the maximum operating speed of the actuator input in advance. Angular velocity maximum value calculation unit,
From the joint angle input from the joint angle sensor and the maximum value of the joint angular velocity input from the maximum angular velocity calculation unit and the movement direction of the tip end arm body input in advance, the target velocity of the tip end arm body and The target motion calculation unit calculates the target position, and the target joint angle calculation unit calculates the joint angle of each joint from the target speed and the target position input from the target motion calculation unit. The operation speed control of the articulated work machine according to claim 1, wherein the angle calculator is configured to transmit an operation signal to each actuator so that each joint has the calculated joint angle. apparatus.