JPS6365507A - Locus controller for articulated working machine - Google Patents

Abstract

PURPOSE:To obtain an articulated working machine having universal applicability, by selecting two arms driven for locus control according to an inputted restrictive condition, and supplying a control signal which drives selected two arms to a control part based on a speed command signal, and an angle signal. CONSTITUTION:The speed command signal by a command means, and the restrictive condition by the operation on an input means, are inputted to an arithmetic means, respectively. To the arithmetic means, the angle signal from an angle detecting means is also inputted. The two arms driven based on the inputted restrictive condition are selected, and based on the other two input signals, the control signal to drive these two arms is supplied to the control part. The control part is controlled by the control signal, and an actuator drives these arms based on a set restrictive condition.

Description

DETAILED DESCRIPTION OF THE INVENTION A. Industrial Field of Application The present invention is applicable to multi-joint working machines such as aerial work vehicles and concrete distributors in which three or more arms are rotatable relative to each other via each joint. This invention relates to a trajectory control device.

B. Prior Art A model of this type of multi-joint working machine is shown in FIG. The first arm 1 is connected to the main body through a joint 5, the second arm 2 is connected to the first arm 1 through a joint 6, and the third arm 3 is connected to the second arm through a joint ff157. It is connected to arm 2. Each arm is driven by three hydraulic cylinders (not shown).

By the way, when performing trajectory control with this type of work machine, the horizontal direction (X
The velocity Vx in the vertical direction (direction) and the velocity vy in the vertical direction (y direction) are input as command values, but since there are three or more movable arms, it is not possible to drive all the arms to control the trajectory. Therefore.

Conventionally, for example, as a constraint condition, the third arm 3 is fixed (θ3=constant), and the remaining first and second arms 1 and 2 are
Trajectory control is performed by driving the

C1 Problems to be Solved by the Invention However, in such a conventional device, as shown in FIG. The range of control was very narrow because it could not be moved. The same applies to multi-joint working machines having four or more arms.

SUMMARY OF THE INVENTION An object of the present invention is to provide a trajectory control device for a multi-joint work machine that drives an arm by selecting any one of a plurality of preset constraint conditions as desired.

Means for Solving the D0 Problem In the multi-joint work machine to which the present invention is applied, when a velocity in a predetermined direction of a part to be controlled by the trajectory is commanded, a control section such as a servo valve activates an actuator such as a hydraulic cylinder. This causes the arm to rotate. The present invention provides a command means for commanding the moving direction and speed of a region to be controlled by a trajectory and outputting a speed command signal, an angle detection means for detecting the relative angle of each arm and outputting an angle signal, and a control device for controlling a trajectory. An input means for inputting constraint conditions, and control for selecting two arms to be driven according to the constraint conditions input from the input means, and driving one selected two arms based on a speed command signal and an angle signal. It is composed of arithmetic means for supplying signals to the control section.

A speed command signal is input to the calculation means by operating the E0 action command means, and a constraint condition is input to the calculation means by operation of the input means. An angle signal is also input to the calculation means from the angle detection means. Two arms to be driven are selected based on the input constraint conditions, and these two arms are selected based on the other two input signals.
A control signal for driving the book arm is supplied to the controller.

The control unit is controlled by this control signal, and the actuator drives the arm under the set constraint conditions.

F. Embodiment FIGS. 1 to 4 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 xa and t9 are connected to the second and third arms 12.13, respectively, and their piston ronds are attached 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 control signal from a control device [30].

The relative angles of arms 11 to 14 are detected by angle detectors 24 to 27 such as potentiometers, and the angle signals On (n indicates the number of arms, here n=1
~4) are input to the control device @30. In addition, a speed command device 41 that commands the horizontal direction (X direction) and vertical direction (X direction) speed of the tip of the fourth arm 14 and outputs speed command signals Vx, Vy, and six constraint conditions described below Constraint condition command signal Rm (m=1 to 6)
An input device 42 is connected to the control device 30.

As is well known, in a multi-joint work machine having four arms, in order to control the trajectory of the tip of the arm, two arms are driven by setting constraint conditions, but in this example, the four arms are constrained. Set the conditions as follows.

(1) Fixing the first arm 11 and the second arm 12 (3rd and fourth arm driving) (2) Fixing the 15th arm 11 and fixing the third arm 13
(The second and fourth arms are driven for trajectory control, and the third arm is driven for attitude control.) (3) The first arm 11 is fixed, and the fourth arm 14 is fixed. Constant attitude angle (drives the second and third arms for trajectory control, and drives the fourth arm for attitude control) (4) Constant attitude angle of the second arm 12 and third arm 13 (1st and 4th arm drives for trajectory control, 2nd and 3rd arm drives for attitude control) (5) Constant attitude angle of the second arm 12 and fourth arm 14 (trajectory control 1st and 3rd arm drives for posture control, 2nd and 4th arm drives for posture control) (6) Constant posture angle of the third arm 13 and fourth arm 14 (for trajectory control 1. Second arm drive; 3. Fourth arm drive for posture control) Here, the model diagram of the four-arm multi-joint work machine shown in Fig. 1 is as shown in Fig. 2. be. now.

The lengths of the first to fourth arms 11 to 14 are Q - Q4, the relative angles are θ, - θ9, and the coordinates of the tip of each arm are (X).

y 1), (XztYz), (X3.Y3), (
x4°Y4), each coordinate can be expressed as respectively. Therefore, the speed of the tip of the fourth arm 14 is high 4. t4 is x,=-b□・Ω□sinθ, +(M,+b,)n,5in(0,+fll,)COL
"Oz" tll ll)・D, 5in(Q, +
02+03)+(b, +b, +b, +b,)・Q, 5
in(a, +θ, l, 14)...(3)'! 4= bl” Q, cosol Ca−”ox>” et cos (θ, +02)+(b1+
a, +b,)・Q3cos(01+0.+fl,)10,"Oz"ez"34)"Q4CosCf),"8z"
es''B4)...(4) It can be expressed as follows.

Further, the initial conditions for the six types of constraint conditions described above are expressed by the symbols shown in FIG. 2 as follows. Note that θ,
. , θ2°, θ,. ,θ,. indicates initial values of arm angles 01 to θ at the start of trajectory control.

-('] (1) (7) Case: fj, = constant, ez
= 'l, b, = 0, b2 = 0 mouth] In the case of (2): θ
□=constant, Miyo=0, θ3=01°+02°10,. −
(θ1+02)-83=-θ2 box (3): θ□
= constant, a x ” O1θ4 = θ, 102゜10,
. Ten,. -(θ1+θ2+θ,), o, =-CO2+
03) 2] In the case of (4): θ, = constant, es”O−e*=e1
0”'2O-e1, -01 e] In the case of (5): 02=θ, .+02゜-〇0.b2
=-b1θ, =03°10. -〇1.64=-b.

] In the case of (6): θ, = 01° + 020 〇aa-(
L+02), (13=-(θ, 10□), θ4; constant,
b4=0Here, statement 4 of equations (3) and (4). When the angular velocity bn of the two arms driven for trajectory control is determined by setting the above initial conditions for each constraint condition as the speed command signals Vx and vy, bn=fn(VX+Vy*θ1.θ8. θ3.θ4)
''(s). The angular velocity ?I n of the arm driven for trajectory control can be easily determined from the initial conditions. For example, under the constraint condition (3), the angular velocity ?I n of the fourth arm 140 is , b4=(04b-04)/Δt...(
5') However, θ, b are the second. The target arm angle of the arm 14 accompanying the movement of the third arm, Δt, is the data sampling time.

It can be expressed as.

Next, the control device 30 will be explained based on FIG. 2.

The control device 30 includes an angular velocity calculation section 31 and a servo control section 3.
2, a link correction section 33, and a current calculation section 34 are connected as shown in the figure.

The angular velocity calculation unit 31 is composed of, for example, a microcomputer, and receives velocity command signals VX, V! TF, constraint condition command signal Rm, and angle signal θn are input, and an angular velocity calculation signal θna is outputted according to the processing procedure shown in FIG. That is, in step P1, the input signal x, vy, R11, on,
is read, and in step P2, the constraint condition is determined based on the constraint condition signal Rm. According to the determination result, step P3~
Proceed to P8, set the above initial conditions A) to B, and complete step P.
Proceed to step 9. In step P9L, the angular velocity calculation signal for the two arms driven for trajectory control is calculated from equation (5) above. ) If there is a constraint that the attitude angle is constant, the angular velocity calculation signal ``ona'' of the arm that is driven to keep the attitude angle constant can be obtained from a formula such as equation (5'),
These '13 na are output in procedure PLO.

This angular velocity calculation signal bna is input to the link correction section 33. This link correction section 33 includes angle detectors 24 to 27.
The actual angle on is also input, and the input signal ? according to that actual angle? J na is link corrected. That is, for the input signal bna, according to the actual angle on, ``t, n=21nafn(on)
(6) Here, fn(on) is the link correction gain given to each arm according to the relative angle on.

is calculated and multiplied by a predetermined gain to output the cylinder speed Zn. A current calculation section 34 follows the link correction section 33 .

The current calculation unit 34 calculates the input cylinder speed "In", where k is the servo valve flow coefficient and An is the pressure receiving area of the cylinder.

The control signal in1 of the servo valve is output by performing the calculation.

The servo control unit 32 has a target value calculation unit 321, and calculates each relative arm angle at the start of trajectory control, that is, the initial arm angle θn. and angular velocity calculation signal? From the time integral value of Jna, set the target value θnb as follows: θnb=on, +f? J nadt
=' Output as (8).

The servo control unit 32 further includes a deviation generator 322 that takes a deviation Δθnb between the target value θnb and the actual arm angle on;
and a gain setter 323 that multiplies the deviation Δθnb by a predetermined feedback gain kn.

The output signal of this gain setter 323 is 1nt (=kn・Δ
θnb) and the output signal in□ of the current calculation unit 34, and outputs a control signal in for driving the hydraulic device 23 such as a servo valve.

In the components of the above embodiment, the hydraulic cylinders 16 to
19 is an actuator, a hydraulic device 23 is a control unit, a speed command device 41 is a command means, angle detectors 24 to 27 are an angle detection means, an input device 42 is an input means, and a control device 30 is a calculation means. Configure.

The target region of the trajectory is the tip of the fourth arm 14.

The operation of this embodiment configured in this way will be explained.

A signal Rm indicating a constraint condition is sent from the input device 42, and speed command signals Vx and Vy are sent from the speed command device 41 to the angle detector 2.
When the angle signal On is input to the control device 30 from signals 4 to 27, initial conditions are set from the constraint condition input by the signal Rm based on the processing procedure shown in FIG. The angular velocity calculation unit 31 calculates the angular velocity 2+ of the two arms used for trajectory control and the other arm used for attitude control from the velocity command signal V x HV y + angle signal θn and the initial conditions.
Find na.

That angular velocity calculation signal? Jna is input to the link correction section 33, link correction is performed based on equation (6) according to each arm angle On at that time, and the cylinder speed in is output. This cylinder speed n is input to the current calculation section 34, and a control signal in is outputted based on equation (7).

On the other hand, in the target value calculation section 321, the input angular velocity calculation signal? A target value Onb is determined based on J na and the initial arm angle θn0, and a deviation Δθnb from the current arm angle θn is determined by the deviation generator 322. Further, this deviation Δθnb is multiplied by a predetermined feedback gain kn in a gain setter 323 to obtain a control signal in2. Then, the adder 324 inputs the plane control signal in2.
and inl are added, and as a result, a control signal in is output, thereby driving the hydraulic device 23. Therefore, a predetermined amount of pressure oil flows from the servo valve constituting the hydraulic device 23 to a predetermined hydraulic cylinder, and the arm is driven.

Among the six types of constraint conditions exemplified above, when constraint condition (1) is used, the trajectory control calculation is simpler than the other conditions, and operation can be performed at relatively high speed and with low fuel consumption. Furthermore, by using the constraint condition (3), calculations for trajectory control are relatively simple, and the fourth arm 14 can be moved into a limited space.
It is possible to control the trajectory while inserting the

Note that the constraint conditions are not limited to the six mentioned above, and other conditions may be used. Also, I explained about hydraulic work equipment,
A pneumatic type may also be used. Further, although the arm is driven by a cylinder, it may be driven by a hydraulic, pneumatic motor, or electric motor. Furthermore, the number of arms is not limited to four.

G6 Effects of the Invention According to the present invention, it is possible to provide a versatile multi-joint working machine that can arbitrarily select restraint conditions suitable for the work from among predetermined restraint conditions. In addition, if the actuator cannot be driven under certain constraint conditions, for example if the cylinder has reached the stroke end, the arm can be further driven by inputting other constraint conditions, and the working radius is also greater than before. It becomes wider.

[Brief explanation of drawings]

Fig. 1 is a diagram showing an example of a multi-joint working machine to which the present invention is applied and an embodiment of the present invention, Fig. 2 is a model diagram of a four-arm model, and Fig. 3 is a detailed example of its control device. The block diagram, FIG. 4 is a flowchart showing an example of the processing procedure in the angular velocity calculating section, and FIG. 5 is a model diagram of three arms. 11-14: Arm 16'-19: Hydraulic cylinder 23: Hydraulic device 24-27: Angle detector 30:
Control device 31: Angular velocity calculation section 32: Servo control section 33: Link correction section 34: Current calculation section
41: Speed command device 42: Input device

Claims (1)

[Claims] 1) Three or more arms rotatably connected via joints, actuators that drive those arms, and the amount and direction of movement of the arms by the actuators in accordance with input signals. A trajectory control device for an articulated working machine having a control unit for controlling a trajectory control unit, comprising: a command means for commanding a velocity in a predetermined direction of a region to be controlled by the trajectory and outputting a speed command signal; and a command means for detecting the relative angle of each arm. An angle detection means for outputting an angle signal; an input means for inputting constraint conditions during trajectory control; and two arms to be driven for trajectory control according to the constraint conditions input from the input means; A trajectory control device for an articulated work machine, comprising: arithmetic means for supplying a control signal for driving two selected arms to the control section based on a speed command signal and an angle signal.