JPS6365508A - Locus controller for articulated working machine - Google Patents

Abstract

PURPOSE:To improve accuracy in locus control, by adopting minor feedback control which controls a control part by supplying a feedback gain based on the acceleration of an arm, on a control signal obtained by a locus control arithmetic calculation. CONSTITUTION:By operating a command means, and inputting a speed command signal to an arithmetic means, the control signal is calculated to perform the locus control of the arm applying a feedback gain produced by deviation between a targeted position and an actual position, from inputted angle signal, and the speed command signal. Meanwhile, to the arithmetic means, an angular velocity signal is also inputted, and based on the signal, the feedback gain is supplied to a calculated control signal. Therefore, the control part is controlled by the control signal on which the feedback gain corresponding to the acceleration, is applied, then, an actuator is driven.

Description

DETAILED DESCRIPTION OF THE INVENTION A. Field of Industrial Application The present invention relates to a trajectory control device for multi-joint working machines such as aerial work vehicles and concrete distributors.

B. Prior Art In this type of trajectory control device, the most advanced horizontal and vertical velocities of a portion to be trajectory controlled, such as a multi-jointed arm, are commanded, and each arm is moved by a predetermined amount according to the relative angle of each arm. A control signal is calculated to move the hydraulic cylinder by a certain amount, a predetermined feedback gain is given to the deviation between the target position and the actual position, and the signal is used to control a control unit such as an electro-hydraulic servo valve. Trajectory control is performed by driving such actuators.

C1 Problems to be Solved by the Invention Generally, in order to improve trajectory accuracy, it is sufficient to increase the feedback gain described above, but in a work machine with a long reach such as an articulated work machine, oscillations in the control system may cause undesirable effects on the arm. vibration occurred, making it impossible to increase the feedback gain very much, and there was a limit to trajectory accuracy.

An object of the present invention is to provide a trajectory control device for an articulated work machine in which the control system does not oscillate even when the feedback gain is increased.

Means for Solving the D0 Problem The multi-joint working machine according to the present invention has three or more arms rotatably connected via joints, each arm being driven by an actuator and receiving an input signal. The control unit controls the actuator so that each arm moves by a predetermined amount based on the control signal. The trajectory control device of the present invention used in such a multi-joint work machine includes a command unit that commands the speed of a region to be controlled in a predetermined direction and outputs a speed command signal, and a command unit that outputs a speed command signal by detecting the relative angle of each arm. An angle detection means that outputs an acceleration signal, an acceleration detection means that detects the acceleration of each arm and outputs an acceleration signal, and detects a trajectory while applying feedback gain based on the deviation between the target position and the actual position based on the speed command signal and the angle signal. It is constituted by a calculation means that calculates a control signal to control the trajectory of the target part, gives a feedback gain to the calculated control signal based on the acceleration signal, and supplies it to the control section.

When the speed command signal is input to the calculation means by operating the E0 action command means, the arm is traced while applying a feedback gain based on the deviation between the target position and the actual position from the input angle signal and speed command signal. A control signal is calculated for controlling. On the other hand, an angular velocity signal is also input to the calculation means, and based on the signal, a feedback gain is given to the calculated control signal. Therefore, the control section is controlled by a control signal to which a feedback gain is applied in accordance with the acceleration, and the actuator is driven.

F. Embodiment FIGS. 1 to 3 (a) and (b) show an embodiment in which the present invention is applied to a multi-joint working machine having four arms.

In FIG. 1, a first arm 11 is rotatably connected to a pedestal 15 fixed to the main body via a pin.

A second arm 12 is rotatably connected to the other end of the first arm 11 via a pin, and a third arm 13 is connected to the other end of the second arm 12 via a pin. A fourth arm 14 is rotatably connected to the other end of the third arm 13 via a pin. A hydraulic cylinder 16 is interposed between the first arm 11 and the main body. Further, the base end of a hydraulic cylinder 17 is connected to the first arm 11, and the piston rod is connected to the first arm 11 and the second arm 12.
It is connected to a link mechanism 20 installed between the two. Similarly, the base ends of the hydraulic cylinders 18.19 are connected to the second and third arms 12.13, respectively, and their piston rods are mounted between the second arm 12 and the third arm 13. The link mechanism 21 and the third
and a link mechanism 22 installed between the arm 13 and the fourth arm 14, respectively. These hydraulic cylinders 16 to 19 are controlled by a hydraulic device 23, which comprises, for example, an electro-hydraulic servo valve provided for each hydraulic cylinder. This hydraulic system 23 is controlled by a signal from a control device 50.

The relative angles of the arms 11-14 are detected by angle detectors 24-27 such as potentiometers, and the head pressures PH1-PH and rod pressures PR1-PR4 of each hydraulic cylinder 16-19 are detected by pressure gauges 28-35, respectively. Detected. These detection results are input to the control device 50.

Further, the speed of the tip of the third arm 13 in the horizontal direction (X direction) and vertical direction (y direction) is commanded, and a speed command signal Vx
, Vy is connected to the control device 50.

As is well known, when trajectory control is performed using four arms, two arms are driven by setting constraint conditions, but in this embodiment, (1) the first arm 11 is fixed.

■ The attitude angle of the fourth arm 14 is kept constant.

In order to adopt this constraint condition, the second arm 12 and the third arm 13 are driven, the tip of the third arm 13 is trajectory controlled by the command from the speed command device 36, and the fourth arm 14 is assumed to be constant.

The four-arm multi-joint working machine shown in FIG. 1 is now modeled as shown in FIG. 2. The coordinates (x, y) of the tip of the third arm 413 are x = Q 1 cos θ1-Q, cos (θ1+02)
+Q3cos(θ1+0210,) −
(1) y = Q 1 sin θ, -Q 2 sin (01 + 02) + Q, 5in (θ, +02 + 03)
−(2). Therefore, x
The velocity in the y force direction is.

x”LQ, 5in (θ1+oz) -Q, 5in (0
1+θ2+θa)) az+ (-a asun(θ1+
02103)) bx ・(3)y = (
-Q 2cos(θ, 10,)+Q,cos(θ1+0
200a)) az+(n, cos(θ, +θ2+θ,)
) b, ・ (4) Therefore, the angular velocities of the second arm 12 and the third arm 13 'o, b, are expressed as (5) ・...(6) becomes.

On the other hand, the attitude angle control of the fourth arm 14 is θ1+02+
θ may be controlled so that 0100=constant.

Second arm e along trajectory ff1lJ#2. Set the target value of the third arm 13 to 02b, θ, b, (θ, +02〇,
+04) initial value is θ. , when the target value of the fourth arm I4 associated with attitude angle control is 04b, the target value θ4b is.

θ4b=θ. -(θ1+02b10ib)
...(7). Here, when the data sampling time is expressed as Δt, the angular velocity of the fourth arm 14 is:

b4=(θ4b-04)/Δt...
It can be expressed as (8).

The control device 50 will be explained based on FIGS. 3(a) and 3(b).

The control device 50 includes an angular velocity calculation section 51 and a servo control section 5.
2, a link correction section 53, a current calculation section 54, an initial value calculation section 55, a target value calculation section 56, and a thrust feedback section 57 (see FIG. 3(b)) are connected as shown. It is configured.

The angular velocity calculation section 51 receives velocity command signals Vx and Vy.

The angle signals 01 to θ and the fourth
The target value θ4b of the arm 14 is input. Arithmetic unit 51
calculates equations (5), (6), and (8),
As a result of those calculations, is S3a a the angular velocity calculation signal? ) , a, a, a, m input to the next stage link correction unit 53 as S4a 8. a and S3a are also input to the target value calculating sections 521 and 525.

Angular signals θ, ~θ4 are also input to the link correction unit 53, and the input angular velocity calculation signal bmal 82a9?
For J 4a, Zz”0zaB<02) z, = b, af, (θ3) z 4 =04 a f 4 (θ4) However, f2(
θ, ), f, (θ3)-t'-(θ4) are each arm angle θ2. θ1. is the link correction gain expressed as a function of θ.

The cylinder speed 221 Z31 z4 is output to the current calculation section 54 at the next stage.

This current calculation unit 54 is configured to input cylinder speed 2□l.
Z)? For Z4, xzx"z't・A, /to 121=Z3・A, /to 141=24・A, /k (θ4) However, k is the servo valve flow coefficient, A2 to A.

is the pressure receiving area of the cylinder.

The servo valve control signal 12□y131t is calculated by
Output i41.

The servo control unit 52 has target value calculation units 521 and 525, and calculates the initial relative arm angles θ, θ, 6 of the second arm 12 and the third arm 13 at the start of trajectory control and the angular velocity calculation signal ? From J 2a, 'e, a, the target value θ2b,
0. b as θ2b=θ2゜+f? ), adt θ, b=θ,. + f b , acit to the deviation generators 522 and 526.

These deviation generators 522, 526 generate a target value θ2bl
03b and the actual arm angle θ2. θ, deviation Δθ
2L Δθ,b is taken and output to gain setters 523 and 527.

Both gain setters 523 and 527 set the deviation Δθzb+Δθ
3b to a predetermined feedback gain; 2. Multiply the
control signal i. (=Δθxb X k z ) * l
3x (=Δθ, bXk,) is output. These signals 1
22713□ is added to the output signals 12□, i, and 1 from the current calculation unit 54 in adders 524 and 528, respectively, to obtain the control signal lit i3.

The angle signals θ□ to θ4 are input to the initial value calculation unit 55, and at the start of trajectory control, θ. = (θ1÷02+03+04), and the initial value 0° used for posture control of the fourth arm 14 is set to the target value calculation unit 56 and control deviation calculation unit 5 of the next stage.
Output to 29.

The target value calculation unit 56 receives the angle signal θ1. second arm 1
The target values θ1lblθ3b of the second and third arms 13 are also input, and the fourth arm 1
The target values θ and b of 4 are output to the angular velocity calculation section 51 described above. As a result, as described above, the output signal 141 is output from the current calculation section 54.

On the other hand, the control deviation calculation section 52 that constitutes the servo control section 52
The angle signals θ□ to θ are input to 9, and the deviation Δθ4 is output as Δθ,=θ,−(θ.−(θ□10110, )). This deviation Δθ. is input to the gain setter 530, a predetermined feedback gain is multiplied by 4,
Control signal Lx (=Δθ, ・k4
) is output. Then, the output signal 141 from the current calculation section 54 and this control signal 142 are added by an adder 531 to obtain a control signal i4.

Note that the servo control section 52 described above is the main feedback system.

The thrust feedback unit 57 shown in FIG. 3(b) has a head side multiplier 5 which multiplies the head pressure of the hydraulic cylinders 17 to 19 detected by each head pressure gauge by the head side pressure receiving area.
71 to 573, rond side multipliers 574 to 576 that multiply the rod pressure of the hydraulic cylinders 17 to 19 detected by each rod pressure gauge and the rod side pressure receiving area, each head side multiplier 571 to 573, and each rond side. Subtractors 577-57 that calculate the thrust of each cylinder from the difference with multipliers 574-576
9, and bypass filters 580 to 5 that extract high frequency components of thrust changes from the outputs of each of these subtractors 577 to 579.
82, and thrust feedback gains k,', k3' at the outputs of the bypass filters 580-582.

The gain setters 583 to 585 obtain the control signal i.?133?143 by multiplying by k4'', the control signal IIS13914 obtained by the servo control unit 52, and the control signal 123
? Adder 5 adds control signal 12"l 331143 and supplies control signal 12"l xi's 14" to hydraulic system 23
86 to 588.

Note that the thrust feedback section 57 described above is a minor feedback system.

In the configuration of the above embodiment, the hydraulic cylinders 16 to 19
is the actuator, the hydraulic device 23 is the control section, the speed command device 36 is the command means, the angle detectors 24 to 27 are the angle detection means, and the pressure gauges 29 to 35. Multipliers 571-576
, subtractors 577-579. Bypass filter 580~5
82 is the acceleration detection means and the control device! 50 respectively constitute calculation means.

The operation of such an embodiment will be explained.

When the velocity command device 36 commands the velocities Vx and Vy of the tip of the third arm 13, the angular velocity calculation section 51 outputs an angular velocity calculation signal? j, a, 2+, a are output, and these signals are sent to the link correction section 53 to adjust the cylinder speed Z2t z3.
The control signal 12□ is further converted into a control signal 12□ by the current calculation unit 54.

i3. is converted to Angular velocity calculation signal a, ? J
, a are also input to target value calculation units 521 and 525, and target values θ2btθ,b are obtained. Deviation generator 522, 52
6 and the actual angle θ2. The deviation from θ is input to gain setters 523, 527 and set as a feedback gain of 2. k3 is multiplied by the gain setter 5.
Control signals 12□, i, 2 are output from 23°527. The adder 524 adds the input signals 12□ and 12□ and outputs the control signal i, and the adder 528 adds the input signal xzz
and 132 are added to output a control signal i. These control signals 12+13 are input to adders 586 and 587 of the thrust feedback section 57. These adders 586 and 587 receive a control signal 123 which is the product of the change in the thrust of the hydraulic cylinder 17 and the feedback gain k2', and a control signal 123 which is the product of the change in the thrust of the hydraulic cylinder 18 and the feedback gain k2'. The adder 586 outputs i2+itz=1g', and the adder 587 outputs i3+131=i,'. These control signals ii's 13' are supplied to the servo valves of the hydraulic system 23, and the amount and direction of pressure oil discharged from the servo valves are controlled, and the pressure oil discharged from the hydraulic cylinders 17.
18 expands and contracts, and the trajectory of the tip of the third arm 13 is controlled.

On the other hand, at the start of trajectory control, the angle signals θ, ~θ, from the angle detectors 24 to 27 are taken into the initial value calculation unit 55 and set to the initial value θ. is calculated. This initial value θ. is input to the target value calculating section 56, and the target value 04b of the fourth arm 14 is calculated. In the angular velocity calculation section 51, this target value θ
4b and the angle signal θ at a certain point. The angular velocity calculation signal b4a of the fourth arm 14 is calculated by dividing the difference between the two by the data sampling time Δt, and the control signal i4 is obtained from the current calculation unit 54 in the same manner as other signals.

Initial value θ. From the angle signals 01 to 04, the arm angle θ
, the deviation Δθ. is determined by the control deviation calculation unit 529. By multiplying this deviation by 4 by the feedback gain in the gain setter 530, the control signal 142 is obtained,
The adder 531 adds the control signal 141 and i4Z to obtain the control signal i4. This control signal i4 is input to the adder 588 of the thrust feedback section 57. A control signal 143 which is the product of the change in thrust of the hydraulic cylinder 19 and the feedback gain by 4' is input to the adder 588.

Outputs i4+143=:44'. These control signals i4' are supplied to the servo valve of the hydraulic bag [23, the amount and direction of oil discharge from the servo valve are controlled, the hydraulic cylinder 19 expands and contracts, and the attitude angle of the fourth arm 14 is adjusted. Constantly controlled.

Although the actuator has been described above as a hydraulic cylinder, it may also be a pneumatic cylinder. Alternatively, a hydraulic, pneumatic motor, or electric motor may be used, and in this case, rotational motion may be directly transmitted to the arm or may be converted into linear motion.

Also, the number of arms may be any number as long as it is 3 or more,
Furthermore, the constraint conditions during trajectory control are not limited to this embodiment;
For example, the first arm 11 and the second l-arm 12 may be fixed, and the tip of the fourth arm 14 may be trajectory-controlled by the third arm 13 and the fourth arm 14. Yet again. Instead of determining the acceleration of each arm as a variation in thrust, each arm may be provided with an accelerometer to directly detect the acceleration.

G1 Effects of the Invention According to the present invention, minor feedback control is adopted in which the control unit is controlled by giving a feedback gain to the control signal obtained by the trajectory control calculation based on the arm acceleration, thereby increasing the feedback of the main feedback system. Even if set to , the main feedback system will not oscillate and trajectory accuracy can be improved.

[Brief explanation of the drawing]

Figures 1 to 3 (a) and (b) show an embodiment of the present invention; Figure 1 is an overall configuration diagram, Figure 2 is a model diagram of four arms, Figure 3 (a), (b) is a detailed diagram of the control device. 11-14: Arm 16-19: Hydraulic cylinder 23: Hydraulic device 24-27: Angle detector 28-3
5: Pressure gauge 36: Speed command device 50: Control device 51: Angular velocity calculation section 52: Servo control section 53: Link correction section 54; Current calculation section 5
5: Initial value calculation unit 56: Target value calculation unit 57: Thrust feedback unit Patent author: Hitachi Construction Machinery Co., Ltd. Patent attorney Fuyuki Nagai Figure 3 (1)) - 1 foot

Claims (1)

[Claims] Three or more arms rotatably connected via joints, a plurality of actuators that drive these arms, and an input control signal to control the amount of movement of the arms by each actuator. In a trajectory control device for an articulated working machine, the trajectory control device includes a control unit for controlling a trajectory control target portion in a predetermined direction and outputting a speed command signal; An angle detection means for outputting an output, an acceleration detection means for detecting acceleration of each arm and outputting an acceleration signal, and a trajectory while applying feedback gain based on the deviation between the target position and the actual position based on the speed command signal and the angle signal. A multi-joint device comprising: calculation means for calculating a control signal to control the trajectory of a target part, giving a feedback gain to the calculated control signal based on the acceleration signal, and supplying the calculated control signal to a control unit. Trajectory control device for work equipment.