JP2792925B2 - Work machine trajectory control device - Google Patents

Description

The present invention relates to a trajectory control device for a working machine having at least two arms whose trajectory is controlled.

B. Background of the Invention FIG. 8 (a) shows an example of an articulated foundation working machine (hereinafter referred to as a working machine) previously proposed by the present applicant. This work machine
The upper revolving unit US is rotatably provided on the lower traveling unit LT, and the first arm 1, the second arm 2, the third arm 3, and the first cylinder 5 and the second arm 2 for driving these are provided on the upper revolving unit US. It has a cylinder 6 and a third cylinder 7, and has an auger excavation unit 6 attached to the tip of the third arm 3. Here, DR is an auger drill. Further, as shown in FIG. 8 (b), there has been proposed a working machine in which a rotary excavation bucket 4 is mounted on the tip of a third arm 3 and the posture thereof is controlled by a fourth cylinder 8.

When drilling holes with such a working machine, it is necessary to operate the tip of the third arm 3 in a direction perpendicular to the ground in general vertical construction. For this reason, the present applicant has previously proposed a trajectory control device as shown in FIG.

In FIG. 9, reference numeral 200 denotes an operation lever for trajectory control. By operating this operation lever 200, the speeds in the X and Y directions of the tip of the third arm 3, which is a trajectory control target site, are commanded. 100 is an angle detection unit having angle meters 101 to 103, and the angle meter 101 is a first arm 1 for the ground.
The goniometer 102 detects the relative angle of the second arm 2 to the first arm 1, and the goniometer 103 detects the relative angle of the third arm 3 to the second arm 2. 400
Is an angular velocity control value calculation unit, which receives the velocity command values in the X and Y directions and the detection values of the angle meters 101 to 103, and
The angular velocity control values ₂ , ₃ of the third arms ₂ , ₃ are calculated. 500
Is a driving control value calculation unit, calculates a drive control value Q _2, Q ₃ is the angular velocity control value _{2, 3} are input. This drive control value Q
_2, Q ₃ by the second controls the electro-hydraulic converter valve 12, the hydraulic cylinders 6, 7 of the third arm 2 and 3 the tip of the third arm 3 is the trajectory control is scale control. For example, when it is desired to vertically move the tip of the third arm 3 while keeping the X coordinate constant,
Only the speed command value in the Y direction needs to be input to the angular speed control value calculation unit 400 by the operation lever 200.

C. Problems to be Solved by the Invention However, such a trajectory control device has the following problems.

In the working machine equipped with the auger drilling unit, the coordinates of the tip of the third arm 3 in the X direction are determined at the start of the excavation, and the trajectory control is performed as described above. Since the drill that swings and changes the posture of the arm to suspend the drill is suspended, the third drill must be
It is necessary to return the tip of the arm 3 to the position at the start of the work (referred to as the coordinate origin), and it is difficult to perform the operation of returning to the origin.

The drilling operation by the work machine equipped with the rotary excavation bucket repeatedly excavates and unloads the soil, so it is necessary to return the excavated rotary excavation bucket to the excavation position and make its posture match the previous one (called the angle origin). And the operation is complicated.

A technical object of the present invention is to easily return a locus control target portion to a coordinate origin and easily return a posture of a work attachment such as a rotary excavation bucket to an angle origin.

D. Means for Solving the Problems The present invention will be described with reference to the drawings showing one embodiment. As shown in FIG. 1, the invention of claim 1 includes two or more rotatably connected via joints. Arms 2 and 3 and work attachment DR interchangeably attached to the ends of arms 2 and 3
And actuators 6 and 7 that drive those arms 2 and 3
And a control means 500 for controlling the amount and direction of movement of the arms 2 and 3 by the actuators 6 and 7 in accordance with an input signal along a predetermined trajectory. An angle detecting means 100 for detecting an angle of each of the arms 2 and 3 and outputting an angle signal, and an origin setting means for setting a predetermined coordinate origin of the work attachment DR
320, first speed command means 200 operated to command the speed of the work attachment DR in a predetermined direction and outputting a first speed command signal, and origin setting means 320
Generates a velocity vector heading toward the coordinate origin of the work attachment DR set by, and when moving the work attachment DR toward the coordinate origin with the velocity vector, the speed at which the work attachment DR is reduced as approaching the coordinate origin. A second speed command means 30 for calculating and outputting a second speed command signal for moving according to characteristics
0, selecting means 330 for selecting the first or second speed command signal, and calculating the angular speed control value of the trajectory controlled arms 2, 3 based on the selected speed command signal and the detected angle. The technical problem is solved by providing an arithmetic means 400 for supplying the angular velocity control value to the control means 500 as the input signal.

As in the second aspect of the present invention, it is preferable that the origin setting means 320 can be newly set when the work attachment DR is replaced.

As shown in FIG. 5, the invention according to claim 3 includes the arms 2, 3, the actuators 6, 7, and the control means.
In addition to 500, the work attachment 4 and its actuator 8 which are exchangeably mounted at the tip of the arm 3 and the arm actuators 6 and 7 are controlled in accordance with the first input signal, and the work attachment 4 is controlled in accordance with the second input signal. The present invention is applied to a trajectory control device of a working machine having a control means 550 for controlling the attachment actuator 8. Then, an angle detecting means 100 for detecting an angle of each of the arms 2 and 3 and the work attachment 4 and outputting an angle signal, and outputting a speed command signal by being operated to instruct a speed of the work attachment 4 in a predetermined direction. Speed command means 200, angle origin setting means 320 for setting a predetermined posture angle of work attachment 4, trajectory control mode for controlling work attachment DR in the predetermined direction set in advance, and work attachment 4 are stored in advance. Selecting means 330 for selecting a return control mode for returning to the attitude angle; and, when the trajectory control mode is selected, the angular velocity control values of the arms 2, 3 whose trajectory is controlled based on the velocity command signal and the detected angle. And the first calculating means 400 for inputting the first input signal to the control means 500, and when the return control mode is selected, the angle When rotating the work attachment 4 toward a predetermined posture angle of the work attachment 4 preset by the setting means 320, the work attachment 4 is rotated by a speed characteristic that is decreased as approaching the posture angle. The above technical problem is solved by providing a second calculating means 450 for calculating an angular velocity control value and inputting it to the control means 550 as a second input signal.

An origin setting means 320 for setting a predetermined coordinate origin of the work attachment DR and an attitude angle of the work attachment 4, and a trajectory control for controlling the work attachment DR in the predetermined direction. Selecting means 330 for selecting a mode and a return control mode for returning the work attachment 4 to the coordinate origin in which the work attachment DR is stored in advance and the work attachment 4 to a preset posture angle, and a return speed characteristic and a posture angle to the coordinate origin. It is preferable to set the return speed characteristic to the characteristic such that the speed is reduced as approaching the origin.

E. Operation-Claim 1-When the first speed command signal is selected, the arithmetic means 400
Calculates an angular velocity control value for controlling the trajectory of the tip of the arm 3 in the direction according to the instruction of the first velocity instruction means 200. This angular velocity control value is input to the control means 500, and drives the actuators 6, 7 so as to control the trajectory of the tip of the arm 3. The second speed command means 300 calculates a second speed command signal for moving the tip of the arm 3 toward the stored coordinate origin. When the second speed command signal is selected by the selection means 330, the angular velocity control value calculation means 400
Calculates an angular velocity control value that moves the tip of the arm 3 toward the coordinate origin. This angular velocity control value is stored in the control means 50.
The actuator 7 is driven so as to make the tip of the arm 3 coincide with the coordinate origin. The second speed command signal calculates a speed vector for returning the current position of the tip of the arm 3 to the coordinate origin on a linear trajectory and further returning the speed vector as the speed approaches the coordinate origin. Can be

-Claim 3-When the trajectory control mode is selected by the selection means 330, the trajectory control similar to that of Claim 1 is performed, and when the return control mode is selected, a return operation to the following attitude angle is performed. Done. The second calculating means 450 calculates an angular velocity control value for controlling the attitude of the work attachment 4 to the stored arbitrary attitude angle. The angular velocity control value is input to the control means 550, and upon receiving this signal, the control means 550 drives the actuator 8 so that the work attachment 4 returns to the set posture angle. The angular velocity control value is
This is given as a characteristic such that the angular velocity decreases as the posture angle approaches.

F. Embodiment -First Embodiment- Figs. 1 to 4 show an embodiment in which the present invention is applied to the working machine shown in Fig. 8 (a).

The coordinates of the work machine are defined as shown in FIG. 3, and the following description will follow these coordinates. As shown in FIG.
The length of the _{first to third} arms 1 to ₃ is L _{1 to} L ₃ , the angle of the arm 1 to ground is α, the second arm 2
Are the relative angles of the first arm 1 with respect to the first arm 1 and the third arm 3 with respect to the second arm 2, T ₂ and T ₃ , the coordinates of the tip of each arm are (X ₁ , Y ₁ ), (X ₂ , Y ₂ ) When (X ₃ , Y ₃ ), each coordinate is It can be expressed as. Therefore, the third arm 3 tip speed of _{3, 3, = - · L 1 sin α} - (- 2) · L 2 sin (α-T 2) - (- 2 - 3) · L 3 sin (α- _{T 2 -T 3) ... (3} ) = - · L 1 cos α + (- 2) · L 2 cos (α-T 2) + (- 2 - 3) · L 3 cos (α-T 2 -T ₃ ) ... (4)

As is well known, in an articulated working machine having three arms, to control the trajectory of the arm tip, two arms are driven by defining a constraint condition. In the embodiment, the first arm is fixed and the second arm is driven. 2. The third arm is driven.

 That is, the constraint condition is expressed by the following equation.

= 0, where ₃ and ₃ in the equations (3) and (4) are speed command signals, and the above conditions are substituted into the equations (3) and (4) to drive the two arms driven for trajectory control. When the angular velocities ₂ and ₃ are obtained, Next, the control device will be described with reference to FIG.

The control device includes an angle detection unit 100 and a first speed command unit 200
, A second speed command unit 300, an angular velocity control value calculation unit (calculation means) 400, and a drive control value calculation unit (control means) 500, as shown in the figure.

The angle detector 100 includes angle detectors 101 to 103.
The angle detector 101 is attached near the pivot point of the first arm 1, detects the ground angle α of the first arm with a well-known pendulum mechanism and a potentiometer, and the angle detectors 102 and 103
Attached to the pivots of the second and third arms 2 and 3, the relative angle T ₂ between the first arm 1 and the second arm ₂ , the second arm 2 and the third arm 3 are respectively known by a well-known lever mechanism and a potentiometer.
Detecting a relative angle T ₃ of the arm 3. These angles are the second
Are input to the speed command unit 300, the angular velocity control value calculation unit 400, and the drive control value calculation unit 500.

The first speed command unit 200 commands the speed command values ₁ , ₁ in the horizontal (X) and vertical (Y) directions at the tip of the third arm 3 and, for example, uses a lever mechanism and a potentiometer to adjust the lever operation angle. Output the corresponding signal.

The second speed command unit 300 computes and outputs speed command values ₂ , _2, which move the tip coordinates of the third arm 3 from the current position to the coordinate origin stored in advance. The coordinate origin to be returned is determined by the second speed command unit 300 when the switch 320 is operated.
Is stored. Any one of the first speed command values ₁ , ₁ and the second speed command values ₂ , ₂ is selected by the switch 330 (selecting means) and input to the angular speed control value calculation unit 400 as a speed command value. .

The angular velocity control value calculation circuit 400 calculates the _second , _second angle from the angles α, T ₂ , T ₃ and the speed command value based on the equations (5) and (6).
Calculate the angular velocity control values ₂ and ₃ of the third arms ₂ and ₃ and input them to the first drive control value calculation section 500.

The drive control value calculation unit 500 calculates the flow control values Q ₂ , Q ₃ of the second and third arm cylinders 6, 7 from the angular velocity control values ₂ , ₃ and the angles T ₂ , T ₃ based on the following equation. , Electro-hydraulic conversion valve 12,1
Enter 3

Qn = n · gn (Tn) · An (7) where gn (Tn) is a link correction coefficient An is a cylinder pressure receiving area Pressure oil is guided to the electrohydraulic conversion valves 12 and 13 from a hydraulic source (not shown). The electro-hydraulic conversion valves 12 and 13 supply pressure oil to the cylinders 6 and 7 for the second and third arms 2 and ₃ at a flow rate and a direction corresponding to the input flow rate control values Q ₂ and Q _3. Then, trajectory control is performed.

The control lever 10 applies pilot hydraulic pressure to the control valve 11 in accordance with the amount of manual operation, and the control valve 11
The opening area and the switching direction are controlled. Control valve
Numeral 11 controls the flow rate and direction of the pressure oil sent to the cylinder 5 by the pilot oil pressure from the operation lever 10. That is, the first arm 1 can be freely rotated by the operation lever 10.

Figure 2 is a calculates the second velocity command value _{2, 2} 2
Of the speed command unit 300 of FIG.

In FIG. 2, reference numeral 301 denotes a coordinate calculation block which calculates the coordinates X, Y of the tip of the third arm 3 from the respective arm angles α, T ₂ , T ₃ .

From the coordinate definition in FIG. 3, the coordinates X, Y of the tip of the third arm 3
Is calculated as follows.

X = L ₁ · cos α + L ₂ · cos (α−T ₂ ) + L ₃ · cos (α−T ₂ −T ₃ ) (7) X = L ₁ · sin α + L ₂ · sin (α−T ₂ ) + L ₃ · sin (α−T ₂ −T ₃ )... (8)

Reference numeral 302 denotes a storage unit which stores the coordinates X, Y of the tip of the third arm 3 when the switch command S2 is input from the switch 320 to the origins X ₀ ,
It is stored as Y _0. Origin (X ₀ , Y ₀ ) and second speed command
The relationship between ₂ , and ₂ will be described with reference to FIG.

The coordinates of the tip of the third arm 3 at (X, Y) are set to the origin (X ₀ ,
When trajectory control is performed at a speed V toward Y ₀ ), the speed V is X,
Since it is the synthesis of the speed commands ₂ and _{2 in} the Y direction, Here, ΔX = X ₀ −X (11) ΔY = Y ₀ −Y (12) ΔX in the equation (11) is calculated at the addition point 303 in FIG.
2) is calculated at the addition point 304. (9), (10)
Denominator of the expression Is calculated by multipliers 305 and 306, adder 307 and square rooter 308.

311 is a function generator, Output the speed V. In the embodiment V = 0 At a certain distance or more, V = V _0, and during that time, it is changed linearly.

(9), is calculated by equation _{2, 2} dividers 309 and 310 and a divider 312, 313 (10).

 Next, the operation of the present apparatus will be described.

(1) Origin return control mode Based on the angles detected by the angle detectors 101 to 103, the coordinates of the tip of the third arm 3 are calculated in the coordinate calculation block 301 of the second speed command unit 300. When the switch 320 is operated when the tip of the third arm is at the position to be set as the coordinate origin to be returned, the coordinates at that time are stored in the memory of the second speed command unit 300 as the coordinates (X ₀ , Y ₀ ) of the origin. The coordinates are stored in 302, and thereafter, the coordinates (X, Y) of the tip of the third arm 3 and the coordinates of the origin (X ₀ , Y ₀ )
From the equations (9) and (10), the second speed command value
₂ , ₂ are calculated by the second speed command unit 300. When the switch 330 is operated to close the contact point b, the home position return control mode is established, the second speed command value ₂ , ₂ is selected and input to the angular speed control value calculation unit 400, and the angular speed control value ₂ , _2
3 is calculated. Angular velocity control values of the second and third arms 2 and 3
₂ and ₃ are link-corrected by the drive control value calculation unit 500,
It is converted into the flow control values Q ₂ , Q ₃ of the second and third cylinders 6,7. These flow control values Q ₂ and Q ₃ are supplied to the electrohydraulic conversion valves 12 and 13, and pressure oil from a hydraulic source is supplied to the second and third cylinders 6 and 7 in a predetermined direction and at a predetermined flow rate. As a result, the second and third arms 2, 3 rotate, and the trajectory of the tip of the third arm 3 is controlled in the direction of the origin. As the coordinates of the tip of the third arm 3 approach the origin, the output V of the function generator 311 in the second speed command unit 300 decreases, and stops when it substantially matches the origin.

(2) Trajectory control mode When the switch 330 is switched to the contact a side, the mode is the trajectory control mode, and the first speed command values ₁ and ₁ of the operation lever 200 are selected and the angular speed control value calculation unit 400
Is input to Angular velocity control value calculation unit 400 includes a first speed command value _{2, 2} input, the angle alpha, by calculating the T _2, T ₃ from the angular velocity control value _{2, 3} and inputs to the drive control value calculation unit 500 . As a result, the hydraulic cylinders 6, 7 of the second and third arms 2, 3 are driven as described above, and the tip of the third arm 3 is controlled in trajectory. For example, if only the speed command value in the Y direction is commanded from the operation lever 200, the tip of the third arm 3 is
If the moving is performed in the Y direction while keeping the coordinates constant and only the speed command value in the X direction is commanded, the tip of the third arm 3 moves in the X direction while keeping the Y coordinates constant.

Thus, in the present embodiment, the conventional trajectory control device
Operating switches 320 and 330 and a second speed command calculating unit 300
, The home return function can be easily and inexpensively configured. Also, since the speed command for returning to the origin coordinates is set only by the coordinates of the origin and the tip of the third arm, the trajectory can always return to the origin by the shortest path. Since the equation is calculated, none of the trajectories is accumulated. Further, the first arm may be arbitrarily driven during the return, or the return may be started from anywhere.

Second Embodiment FIG. 5 shows a configuration of a control device according to a second embodiment in which the present invention is applied to the rotary excavation bucket shown in FIG. 8B.

FIG. 5 shows the embodiment of FIG. 1 in which a function of returning to the original point of the bucket angle indicated by a dotted frame is added, and the added portion will be described below.

The control device includes a bucket angle detector 104, a second angular velocity control value calculator (second calculator) 450, a second drive control value calculator 550, in addition to the components of the first embodiment. It is composed of Note that 400 is referred to as a first angular velocity control value calculation unit (first calculation unit), and 500 is referred to as a first drive control value calculation unit.

The bucket angle detector 104 is attached to a pivot point of the bucket, and has a well-known lever mechanism and a potentiometer.
Detecting a relative angle T ₄ of the third arm 3 and the bucket 4. This angle T ₄ is called a bucket angle, and is an angle between the center line of the bucket 4 and the center line of the third arm 3 as shown in FIG. The angle T ₄ is input to the second angular velocity control value calculation unit 450 and the second drive control value calculation unit 550.

The second angular velocity control value calculation unit 450 calculates and outputs an angle control value ₄ of the bucket 4 based on the angles α, T ₂ , T ₃ , and T ₄ so that the bucket angle matches the angle origin at which the bucket angle should be returned. .
When the switch 320 is operated, the angle origin to be returned
It is stored in the second angular velocity control value calculation section 450.

The home position return function of the bucket angle is selected by the operation of the switch 330, and when the switch 330 is switched to the contact b side to select the home position return, the bucket angular velocity control value ₄
Is input to the second drive control value calculation unit 550.

The second drive control value calculation unit 550 calculates the flow control value Q ₄ of the bucket cylinder 8 from the angular speed control value ₄ and the angle T ₄ and inputs the calculated value to the electro-hydraulic conversion valve 14. This calculation is performed in the same manner as the link correction described above.

Hydraulic pressure is guided from a hydraulic pressure source to the electro-hydraulic conversion valve 14,
The hydraulic pressure is supplied to the bucket cylinder 8 at a flow rate and direction corresponding to the flow rate control value Q ₄ input, the bucket angle control is performed.

Although omitted in the figure, the second and third arms 2, 3
An operation lever and a control valve are provided for manual control of the bucket 4 and the pressure oil from the control valve and the pressure oil from the electro-hydraulic conversion valve are merged.

FIG. 6 shows the details of the second angular velocity control value calculation section 450 for calculating the angular velocity control value ₄ of the bucket 4.

From the coordinate definitions in FIG. 7, the bucket angle δ with respect to the X axis is calculated as follows at the addition points 451 to 453.

δ = α−T ₂ −T ₃ −T ₄ (13) A storage unit 455 stores the bucket angle δ when the switch command S ₂ is input from the switch 320 as the angle origin δ ₀ . Difference or angular deviation between the summing point 454 in the angle [delta] and [delta] ₀ is calculated, the angular velocity control value ₄ of the bucket is calculated output to be proportional to the angular deviation. That is, ₄ = K · (δ−δ ₀ ) (14) The bucket angular velocity control value ₄ is obtained by multiplying the angle deviation δ−δ ₀ by K by the coefficient unit 456.

 Next, the operation of the present apparatus will be described.

(1) Angle home position return control mode Based on the angles detected by the angle detectors 101 to 104, the bucket angle δ with respect to the X axis is calculated by the second angular velocity control value calculator 450. When the switch 320 is operated at the bucket angle to be set as the angle origin to be returned, the angle at that time is stored in the storage unit 455 of the second angular velocity control value calculation unit 450 as the angle origin δ ₀ , and thereafter, the angles δ and δ ₀ The angular velocity control value ₄ proportional to the difference between
Is calculated by equation (14).

When the switch 330 is switched to the contact b side to select the bucket angle origin return control mode, the angular velocity control value ₄ is input to the second drive control value calculation unit 550. Angular velocity control value
₄ is link-corrected based on the angle T ₄ and is converted into a flow rate control value Q ₄ for the bucket cylinder 8.

The flow rate control value Q ₄ are supplied to the electro-hydraulic conversion valve 14, pressure oil from the hydraulic source is supplied to the bucket cylinder 8 at a predetermined direction, predetermined flow rate. Thereby, the bucket 4 rotates, and the bucket angle is controlled in the direction of the origin. As the bucket angle approaches the angle origin, the angular velocity control value ₄ decreases, and stops when it substantially matches the origin.

In this embodiment, when the switch 330 is switched to the contact b side in this origin return control mode, the tip of the third arm 3 also returns to the coordinate origin as described in the first embodiment. If the switch 330 is switched to the contact a side, the above-described trajectory control mode is set.

According to the present embodiment, the trajectory target portion at the tip of the third arm 3 can return to the coordinate origin and the attitude angle of the attachment 4 can return to the angle origin only by switching to the switches 330 and b.

In applying the present invention, each component of the above embodiment may be configured as follows.

The number of arms is not limited to three.

Although the control arms are the second and third arms, other combinations may be used, and the two control arms may be automatically switched.

Although each arm is driven by a hydraulic cylinder, the invention is not limited to hydraulic pressure, and other actuators such as a hydraulic motor and a hydraulic rotary actuator can be used.

Although it has been described that it can be used for earth augers and rotary excavating buckets, it can also be used for other various work attachments.

The angle of the first arm 1 may be detected as a relative angle with respect to the upper swing body, and the inclination angle of the work implement body may be detected to correct the relative angle.

Although the speed command is set in two directions of X and Y, it may be set in either one direction.
Another direction can be used as a speed command for correcting the trajectory deviation as disclosed in No. 8099. Alternatively, other directions may be used.

The angle detector is not limited to a potentiometer, such as one using a magnetoresistive element, one using a differential coil, one using an optical or magnetic rotary encoder.

The control device may be configured with only the angle home position return function of the attachment.

G. Effects of the Invention According to the first aspect of the invention, the trajectory of the work attachment in a desired direction can be controlled by the first speed command signal, and the origin return control is selected by the selection means regardless of the position of the work attachment. By doing so, the work attachment can be easily moved to the coordinate origin stored at an arbitrary position. Then, at the time of the return, the speed is reduced as approaching the coordinate origin, so that the operator can reduce the shock at the time of stopping without being conscious of the coordinate origin.

According to the third aspect of the present invention, the trajectory control can be performed, and regardless of the posture of the work attachment, if the origin return control mode is selected by the selection means, the angle stored in an arbitrary posture can be obtained. The posture angle of the work attachment to the origin can be easily controlled. Then, at the time of the return, the angular velocity is reduced as approaching the posture angle, so that the operator can reduce the shock at the time of stopping without being conscious of the set posture angle.

[Brief description of the drawings]

FIGS. 1 to 4 illustrate the first embodiment.
2 is a detailed block diagram of a second speed command unit, FIG. 3 is a diagram for defining coordinates, and FIG.
FIG. 5 is a diagram for explaining a speed command value of FIG. FIGS. 5 to 7 illustrate the second embodiment.
6 is a detailed block diagram of a second angular velocity control value calculation unit, and FIG. 7 is a diagram for explaining a bucket angle. FIG. 8 (a) is a side view of a working machine equipped with an auger excavation unit, FIG. 8 (b) is a side view of a rotary excavation bucket, and FIG. 9 is an overall configuration diagram of a conventional trajectory control device. 1-3: arm, 4: rotary drilling bucket 5-8: hydraulic cylinder, 11-14: electro-hydraulic conversion valve 100: angle detector, 101-104: angle detector 200: first speed commander, 300: Second speed command section 320, 330: selection switch 400, 450: angular velocity control value calculation section 500, 550: drive control value calculation section

──────────────────────────────────────────────────続 き Continued on the front page (58) Field surveyed (Int. Cl. ⁶ , DB name) E02F 3/43 E21B 44/00

Claims (4)

(57) [Claims] At least two arms rotatably connected via joints, a work attachment exchangeably attached to a tip of the arms, an actuator for driving those arms, and an input signal And a control means for controlling the amount and direction of movement of the arm by the actuator along a predetermined trajectory, wherein the angle detection for detecting the angle of each arm and outputting an angle signal Means, origin setting means for setting a predetermined coordinate origin of the work attachment, and a first speed which is operated to command a speed of the work attachment in a predetermined direction and outputs a first speed command signal. Instructing means, and generating a velocity vector heading toward the coordinate origin of the work attachment set by the origin setting means. And calculating a second speed command signal for moving the work attachment with a speed characteristic reduced as approaching the coordinate origin when the work attachment is moved toward the coordinate origin with the speed vector. Speed command means for outputting the selected speed command signal, selecting means for selecting the first or second speed command signal, and trajectory control based on the selected speed command signal and the detected angle. A trajectory control device for a working machine, comprising: an arithmetic unit that calculates an angular velocity control value of the arm and supplies the angular velocity control value to the control unit as the input signal. 2. An arm that is rotatably connected via a joint, at least two arms, a work attachment that is exchangeably attached to a tip of the arm, an actuator that drives those arms, And a control means for controlling the amount and direction of movement of the arm by the actuator along a predetermined trajectory, wherein the angle detection for detecting the angle of each arm and outputting an angle signal Means, origin setting means for setting a predetermined coordinate origin of the exchanged work attachment, and a first speed command signal which is operated to command a speed of the work attachment in a predetermined direction and outputs a first speed command signal. 1 speed command means, and a speed vector toward the coordinate origin of the work attachment set by the origin setting means. And a second speed command for moving the work attachment with a speed characteristic that decreases as the work attachment approaches the coordinate origin when the work attachment is moved toward the coordinate origin at the speed vector while generating the torque. Second speed command means for calculating and outputting a signal; selecting means for selecting the first or second speed command signal; trajectory based on the selected speed command signal and the detected angle A trajectory control device for a working machine, comprising: an arithmetic unit that calculates an angular velocity control value of the arm to be controlled and supplies the angular velocity control value to the control unit as the input signal. 3. An arm that is rotatably connected via a joint, at least two arms, a work attachment that is exchangeably attached to a tip of the arm, an arm actuator that drives those arms, and the work. Attachment actuator for driving the attachment, Control means for controlling the movement of the arm by the arm actuator in response to a first input signal, and controlling the movement of the work attachment by the attachment actuator in accordance with a second input signal. A trajectory control device for a working machine, comprising: an angle detection unit that detects an angle of each arm and a work attachment and outputs an angle signal; and a speed command signal operated to command a speed of the work attachment in a predetermined direction. Speed command means for outputting Angle origin setting means for setting a predetermined attitude angle of the attachment; a trajectory control mode for controlling the trajectory of the work attachment in the predetermined direction; and a return control mode for returning to the attitude angle set by the angle origin setting means. And selecting the first input. When the trajectory control mode is selected, an angular velocity control value of an arm whose trajectory is controlled is calculated based on the speed command signal and the detected angle, and the first input A first calculating means for inputting a signal as a signal to the control means, and when the return control mode is selected, a work attachment directed to a predetermined posture angle of the work attachment set in advance by the angle origin setting means. When turning, the turning speed is reduced by the speed characteristic which is reduced as approaching the posture angle. Because of calculating the angular velocity control value, the second working machine of the trajectory control device according to claim as an input signal that includes a second calculating means for inputting to said control means. 4. An arm that is rotatably connected via a joint, at least two arms, a work attachment that is exchangeably attached to a tip of the arm, an arm actuator that drives those arms, and the work. Attachment actuator for driving the attachment, Control means for controlling the movement of the arm by the arm actuator in response to a first input signal, and controlling the movement of the work attachment by the attachment actuator in accordance with a second input signal. In the trajectory control apparatus for a working machine having: an angle detecting means for detecting an angle of each arm and a work attachment and outputting an angle signal; and an origin setting means for setting a predetermined coordinate origin and a predetermined posture angle of the work attachment. And the work attachment Selection means for selecting a trajectory control mode for performing trajectory control in a set direction and a return control mode for returning to the coordinate origin and posture angle set by the origin setting means; and a command for a speed of the work attachment in a predetermined direction. A first speed command means that is operated to output a first speed command signal to generate a speed vector heading toward the coordinate origin of the work attachment set by the origin setting means; A second speed command signal for calculating and outputting a second speed command signal for moving the work attachment with a speed characteristic reduced as approaching the coordinate origin when the work attachment is moved toward the coordinate origin; Speed command means, and when the trajectory control mode is selected, the first speed command Calculating the angular velocity control value of the arm whose trajectory is controlled based on the detected angle and inputting it to the control means as the first input signal, while the return control mode is selected, First calculating means for calculating an angular velocity control value of an arm whose trajectory is controlled based on a second speed command signal and the detected angle, and inputting the angular velocity control value to the control means as the first input signal; When the control mode is selected, when rotating the work attachment toward a predetermined posture angle of the work attachment set in advance by the origin setting means, the rotation speed is reduced as the work attachment approaches the posture angle. And a second calculating means for calculating an angular velocity control value for turning according to the speed characteristic to be applied and inputting the angular velocity control value to the control means as the second input signal. Trajectory control device for a working machine, characterized and.