JPS59170334A - Locus controller of working machine - Google Patents

Abstract

PURPOSE:To perform highly accurate control with a small proportional constant by using a system in which the deviation of height with a working target locus is deducted from an arm speed command signal to control the arm, or the flow rate of boom is controlled by arm speed, arm angle, and boom speed. CONSTITUTION:In a manipulator for controlling the locus of an oil-pressure shovel, etc., consisting of the first arm 2 for boom of large inertial load, the second arm 3 for arm pivotally supported on the first arm 2, and a working part 4 for bucket connected to the tip of the second arm 3, the deviation DELTAh of height with a target working locus is deducted from signal of operation speed command signal generator 6 for the second arm 3, the output signal is put in a means 7 to calculate operation speed command signal of the first arm 2, and operation speed command signal for the first arm 2 is obtained. Operations with high accuracy can be obtained without increasing the proportional constant of feedback loop.

Description

DETAILED DESCRIPTION OF THE INVENTION The present invention relates to a trajectory control device for a working machine that operates a working part of a hydraulic pump, crane, manipulator, etc. along a predetermined trajectory.

Generally, the working parts of these working machines are attached to the tips of a plurality of arms that are connected to each other, since they require fairly complex movements. The trajectory of the working part is therefore controlled by the relative movement between these arms. below,
This will be explained regarding a hydraulic excavator.

FIG. 1 is a schematic diagram of the front mechanism of a hydraulic shovel, and FIG. 2 is a diagram illustrating the movement of the front mechanism shown in FIG. 1. In the figure, 1 is the hydraulic cylinder body, 2 is a boom that rotates around a fulcrum A provided on the hydraulic cylinder body 1, and 3 is an arm that rotates around a fulcrum B provided on the poom 2. .

4 is a working part connected to the third end of the arm.

A packet is used in the working part 4 to excavate earth and sand,
It is used for work such as lifting heavy objects using hooks. The poom 2 and the arm 3 rotate on a plane composed of the Y axis and the Y axis shown in FIG. It rotates around B in the direction indicated by angle θ2. The rotation of the poom 2 and the arm 3 is achieved by driving a poom cylinder and an arm cylinder (not shown), respectively, and the driving of each of these cylinders is controlled by a flow rate control mechanism consisting of a hydraulic pump, a directional control valve, and the like. K1 is an angle detector that detects the angle between the hydraulic bell body 1 and the poom 2 and outputs a signal according to the angle, and K2 detects the relative angle between the poom 2 and the arm 3 and detects the angle. This is an angle detector that outputs a signal according to the angle. In addition, in Fig. 2, C
is a predetermined part on the working part 4, for example, the tip of the packet claw, 11
is the distance between fulcrums A and B, j2 is the distance between fulcrum B and site C, and for humans is the height of site C from the Y axis (horizontal plane).

FIG. 3 is a system diagram of the drive mechanism for the poom 2 and the arm 3. In the figure, 5 is a poom signal generator that generates a speed command signal for the poom 2 in response to the operation of the poom operation lever (-1), and 7 is a boom signal generator that generates a speed command signal a□ output from the boom signal generator 5. A flow rate control mechanism that controls the flow rate of pressure oil to the cylinder.Similarly, 6 is an arm signal generator that generates a speed command signal ω2 for the arm 3 in response to the operation of the arm control lever LA, and 8 is a speed command signal. This is a flow rate control mechanism that controls the flow rate of pressure oil to the arm cylinder according to ω2.

Now, the operator operates the control levers LB and LA, and the corresponding speed command signals ω1 and ω2 are output from the boom signal generator 5 and the arm signal generator 6, and the flow control mechanism 7.8 causes the boom 2 and the arm 3 is the speed ω
Suppose that it is rotated by 1 and ω2 and becomes angles θ1 and θ2.

In this case, the speeds ω□, ω2. The angles θ□ and θ are respectively expressed by the following equations. (The direction of the arrow in FIG. 2 is positive.) ωx=ft(ω1) θ1=ω□ ω,=f, (ω2) θ −1ω 2−”; t However, S is a differential operator.

ω,=Sω□dt=01.

Such operation of the pool 2 and the arm 3 is represented in the rear part of each flow control mechanism 7.8 in FIG. 3 as a result of the control of the flow control mechanism 7.8. In addition, 9.1
Each 0 is an integral element.

Here, a case will be considered in which the drive mechanism shown in FIG. 3 drives part C of the working part 40 parallel to the horizontal plane (Y-axis in FIG. 2) (on a horizontal trajectory). The height h of the working part 40 from the COX axis is expressed by the following formula.

A: l, capθ1-4. p&le(θ□10,)・
...(1)=1. (θ1, θ,)
(2) Since the portion C is driven in parallel, the height l becomes a constant 0, and the above equation (2) is expressed as the following equation.

g□(θ1, θri=constant) (3) That is, the poom 2 and the arm 3 must be operated while always maintaining the relationship in equation (3) above.

However, due to the fact that there is a delay in the hydraulic system in driving the poom 2 and the arm 3, and the load on the working part 4 is not constant, manual operation of the operating lever LB%LA is required. It is almost impossible to drive C in parallel. Therefore.

Automatic control for driving part C along an arbitrary trajectory such as parallel driving has been proposed.

FIG. 4 is a block diagram of a conventional trajectory control device for a hydraulic excavator. In the figure, the same parts as those shown in FIG. 3 are given the same reference numerals. 11 is a calculation unit that inputs the angle signals θ1, θ of K2, and the speed ω of arm 2 to the angle detector and performs the calculations described below; 12 inputs the angle signals θ1, θ2, and calculates the equation (1). A calculation unit 13 calculates angle signals θ1 and θ.

A calculation unit %14 which inputs and outputs a signal of height h according to a predetermined locus is a subtracter which obtains the deviation between the output of the calculation unit 12 and the output of the calculation unit 13; 16 is a coefficient unit that multiplies the output of the subtracter 14 by a predetermined proportionality constant. 17 is a subtracter that obtains the deviation between the coefficient units 15 and 16.

The operation when driving the parts C of one working part 40 in parallel will be described below. As is clear from the figure.

The operator of the hydraulic shovel operates only the arm operating lever LA, and the boom 2 is automatically driven in conjunction with the operation of the arm 3 (5).

The reason why the arm control lever LA° is selected as the operator's control lever is that while the operation of the boom control lever LB moves the boom 2 up and down, the operation of the arm control lever LA moves the arm 3. This is to allow the operator to operate the work unit 4 in a manner that is very similar to that of directly driving the work unit 4.

When the arm operating lever LA is operated, the arm signal generator 6 outputs an arm speed command signal; 2, and the flow rate control mechanism 8 drives the arm 3 in response to this signal. The speed ω of the arm 3 during driving and the angle signal θ detected by the angle meter are input to the calculation unit 11. Here, the calculation by the calculation unit 11 will be described. If the above equation (1) is rearranged by differentiating both sides under the condition that Le = 50 (constant), then the following is obtained. Here, if (4) is set to
The height h of the part C of the working part 4 is h=h0. In other words, it will be driven while remaining constant. The calculation unit 11 performs the above calculation and outputs the speed command signal ω□ for driving the boom 3 at the boom speed 5θ''/cat.In the end, the speed command signal ω1 is expressed by the following equation.

ω1 = Chicken (θ1.θ2) ・ω2 The inputs θ1 and θ of the calculation unit 11 are obtained from the angle detector 1, K, and the speed ω of the arm 3 is obtained from the operational amplifier,
Obtained by a differential circuit (analog circuit) consisting of a resistor and a capacitor. The calculation unit 11 itself can be easily configured using a microcomputer. That is, the boom angle θ4, arm angle θ8, and arm speed ω2 are sequentially taken in via a multiplexer and A/D converter, while the ROM (read-only memory) stores - for the possible angles θ1 and θ2. (θ1, θ2), store the calculation results in advance, read the calculation results from the ROM according to the input angles θ1, θ2, and multiply them by the speed ω, then the boom speed command Signal 7
is obtained. Note that the speed ω2 can also be obtained by calculating the difference of the angle θ2 input to the microcomputer at fixed time intervals without separately providing a differentiating circuit as described above.

In the calculation path of the speed command signal ω1.

The height h of the part C of the working part 4 is not equally involved, and no feedback is provided to maintain the height ^ at a predetermined height ho, and the signal a? ' is exclusively the speed of arm 3 and boom 2,
This can only be achieved with the arm 30 angle. Therefore, this calculation path is called an open loop.

The calculation unit 12 calculates the actual height of the portion C. This calculation is performed based on equations (1) and (2) by inputting the driving boom angle θ1 and arm angle θ2 detected at 1eK* into the angle detector. In addition, the calculation unit 13
In this case, input the values θ and θ2° (initial values) of the boom angle θ1 and arm angle θ at the time when the action of pushing out part C horizontally is started, and the height ho at that point is calculated using the formula (
1), (2) Calculated based on K.

Le. =9. (θ□., θ, .)=constant, that is, in this example, since the movement of part C is horizontal movement, the signal (target height value) output from the calculation unit 13 is constant.

The output signal of the calculation unit 12 and the output signal h0 of the calculation unit 13 are input to a subtracter 14, which outputs the deviation Δh of the actual height from the target value. In other words, Δ person = Lou 80 If ΔA ) o, the amount of rotation of the poom 20 is smaller than the amount of rotation of the arm 3, so
The speed command value to the flow rate control mechanism 7 of the pool 2 may be further negatively corrected, and the magnitude of the correction may be set to 1Δh1. ΔA (If 0, the correction is opposite in sign and equal in absolute value. In either case, the signal −Δ
If the flow rate control mechanism 8 of the pool 2 is controlled with a control amount proportional to , the height of part C will be a constant value. will be controlled by. The calculation units 12.13 can be configured with a microcomputer similarly to the calculation unit 11, and the ROM
The calculation result of equation (1) is stored in advance, and the angle θ1
, θ2, θ1o, and θ2° and take out the calculation results stored according to these angles, the signal ri and ho can be obtained. Note that, among the heights h that are constantly calculated by the calculation unit 12, the calculation unit 13 initializes the height h when the command to start automatic operation is received. It can also be omitted by closing it and taking it out.

The calculation path for obtaining the deviation Δk by the calculation units 12 and 13 and the subtractor 14 is to extract the actual height h and convert it to the target height A. Therefore, it constitutes a so-called feedback loop.

The speed command signal ω of the pool 2 obtained by the calculation unit 11 is multiplied by 0 by an appropriate coefficient by the coefficient unit 15, while the height deviation Δh obtained by the subtractor 14 is multiplied by an appropriate coefficient by the coefficient unit 16. It is multiplied by a coefficient Kf. Then, the output K of the coefficient unit 15.・ω1 and the output Kf・Δk of the coefficient unit 16 are subtracted by the subtractor 17, and the flow rate control mechanism 7 of the pool 2 is subtracted by the subtracter 17.
A speed command signal a1 is obtained. This can be expressed as a formula as follows.

ω = K −1 − Kf・Δl 0 In the conventional device as described above, control using feedback loose is originally a control that corrects the error only when an error occurs in the height of part C. Yes, in order to perform accurate correction, it is natural to use the coefficient multiplier 16.
It is better to select a large proportionality constant Kf. However, the proportionality constant K.

If A is set large, the inertia of the pool 2 is large, so the target height is A. In the vicinity of , the input and output of the flow control mechanism 7 of the pool 2 changes greatly between positive and negative, and as a result, the portion C moves forward while vibrating up and down. The open loop in the conventional example described above is provided to avoid such an unstable phenomenon, and since horizontal control can be performed fairly accurately with only the speed command value ω1 of the poom 2 obtained by the movement of the arm 3, feedback is required. Control with good stability and accuracy can be achieved without increasing the loose proportionality constant so much.

FIG. 5 is a block diagram of a conventional trajectory control device for a hydraulic excavator. In contrast to the conventional device shown in FIG.
The conventional device shown in the figure is different in that the speed command signal ω2 of the arm 3 is directly input to the calculation unit 110, and other parts are exactly the same as the conventional device shown in FIG. 4, and the operation and effects are almost the same. It's the same.

By the way, the flow rate control mechanism 7.8 is generally a hydraulic pump,
It consists of a directional switching valve, a flow rate control valve, etc., and its output characteristics change depending on the magnitude of the applied load. That is, the flow rate control mechanism 7 of the boom 2 is greatly influenced by the load applied via the arm 3 and the inertial load of the boom 2 itself, and the flow rate control mechanism 8 of the arm 3 is greatly influenced by the load applied to the working part 4 and the inertial load of the boom 2 itself. Also, it is greatly affected by the inertial load of the arm 3 itself. Therefore, when driving the poom 2 and when driving the arm 3, since the inertial load of the poom 2 is much larger than the inertial load of the arm 3, the control element is larger when driving the poom 2. It has a delay element and is therefore not suitable for feedback control. Therefore, in the conventional device shown in Figs. 4 and 5, the stability is improved by using feedback control and Aug-oblique control in combination, compared to the case where only feedback control (c) is used. This is not satisfactory, as instability may still occur.

The purpose of the present invention is to solve the above-mentioned conventional problems and improve accuracy (
It is an object of the present invention to provide a trajectory control device for a work machine that is capable of performing stable control.

In order to achieve this object, the present invention subtracts the operating speed command signal of the arm with a small inertial load and the deviation from the target trajectory of the working part to obtain a speed command signal of the flow rate control mechanism of the arm with a small inertial load, The arm with a small inertial load is feedback-controlled, and the speed of the arm with a small inertial load or the operating speed command signal is used to obtain the operating speed command signal of the arm with a large inertial load, thereby controlling the arm with a large inertial load. It is characterized by open loop control.

The present invention will be explained below based on the embodiments shown in FIGS. 6 and 7.

FIG. 6 is a block diagram of a trajectory control device for a hydraulic layer pel according to an embodiment of the present invention. In the figure, the same parts as in FIGS. 4 and 5 are given the same reference numerals.

A subtracter 18 inputs the arm speed command signal ω2/ from the arm signal generator 6 and the signals K, .DELTA.h from the coefficient multiplier 16 and performs subtraction. That is, the actual height h of the working part 40 portion C (FIG. 2) and the target value. Which deviation Δ
h is calculated by the subtractor 14, and the value is subtracted from the speed command signal ω2' of the arm 2 from the signal generator 6.
Thus, the arm 2 is feedback-controlled.

Therefore, the signal B2 from the subtractor 18 becomes an arm speed command signal under feedback control. The signal is expressed as follows.

ω = ω/ −K t·Δh 2 The flow rate control mechanism 8 is driven by this speed command signal 8□ and operates the arm 2. In this way, the speed ω2 of the arm 3 that operates under feedback control is the angle θ□, θ2
The door (θ8, θ2) and ω are also input to the calculation unit 11 as in the conventional device.

is calculated, and an appropriate proportionality constant is multiplied by 0 by a coefficient unit 15 to obtain a boom speed command signal. This speed command signal 8□ is directly input to the flow rate control mechanism 7 of the boom 2,
The flow control mechanism 7 appropriately drives the boom 2. In the calculation path of the speed command signal F, the height deviation Δh is not equally involved, and this calculation path is open-loose. Therefore, the control of the boom 2 is not feedback control but open-loose control. The signal f is expressed as follows.

ωt ”” K O・m (θ1, θ2)・ω2 In this way, in this embodiment, the height deviation is subtracted from the arm speed command signal and used as the input signal for the arm flow rate control mechanism. The operation to correct the deviation in height is performed by an arm with a small inertial load, and a predetermined calculation is performed based on the arm speed, arm angle, and zoom angle, and the signal obtained by this is used to control the flow rate control mechanism of the boom. By using this as an input signal, a boom with a large inertial load can be controlled only by open-loose control, so high-precision operation can be obtained without having to make the feedback-loose proportionality constant too large. With,
Stable operation can also be obtained.

FIG. 7 is a block diagram of a trajectory control device for a hydraulic excavator according to another embodiment of the present invention. In the figure, the same parts as those shown in FIG. 6 are given the same reference numerals. The difference between this embodiment and the rabbit embodiment is that in the rabbit embodiment, the actual speed ω2 of the arm 3 is input to the calculation section 11, whereas in this embodiment, the arm speed command signal ζ' is calculated. The other points are completely the same. And this speed command signal ω2! is the signal before subtracting .DELTA.h from the height deviation signal in the subtractor 18 and is 7, so the calculation path of the boom speed command signal ω1 is open-loose as in the example of the rabbit. The signal in this case is expressed by the following equation.

g 1 ”” K o ” 77L (θ1.θ2)”
;"2' Therefore, this embodiment also has the same effect as the previous embodiment.

In the above explanation, the boom and arm of a hydraulic excavator were described, but it is of course applicable to any system having a system with a large inertial load and a system with a small inertial load. Furthermore, the present invention is not limited to motion within a vertical plane, and can also be applied to motion within other planes. Furthermore, the trajectory of the working part is not limited to a horizontal trajectory, and control that draws other trajectories can also be applied.

As described above, in the present invention, the arm with a small inertial load is controlled based on the deviation from the target trajectory, and the arm with a large inertial load is not controlled based on the deviation. Since the control is performed based on the operation, the operation of the working part of the working machine can be performed with high precision and stability.

[Brief explanation of the drawing]

Figure 1 is a schematic diagram of the front mechanism of the hydraulic seal. ,
Figure 2 is a line diagram explaining the movement of the front mechanism shown in Figure 1, Figure 3 is a system diagram of the boom and arm drive mechanism,
4 and 5 are block diagrams of a conventional hydraulic cylinder trajectory control device, and FIGS. 6 and 7 are hydraulic cylinder trajectory control devices according to one embodiment and other embodiments of the present invention. FIG. 2 is a block diagram of the device. 6...Arm signal generator, 7...Boom flow rate control mechanism, 8...Arm flow rate control mechanism, 11...Coefficient unit. 1w Figure 2 Figure 3 Figure 4 Figure 5 Figure 6

Claims (1)

[Claims] 1. A first arm with a large inertial load, a second arm with a small inertial load connected to the first arm, and a working part connected to the second arm; a signal generating device that generates a motion speed command signal for the second arm; means for subtracting a deviation from a target trajectory of the working part from a signal from the signal generating device; and means for obtaining a motion speed command signal for the first arm based on a value related to the motion of the first arm. 2. A trajectory control device for a work machine according to claim 1, wherein the value related to the motion of the second arm is a motion speed of the second arm. 3. A trajectory control device for a working machine according to claim 1, wherein the value related to the motion of the second arm is a motion speed command signal of the second arm.