JP3338540B2 - Trajectory control device for articulated work machines - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a trajectory control device for an articulated working machine having three or more arms.

[0002]

2. Description of the Related Art When performing trajectory control with an articulated working machine having three or more arms, a method is adopted in which two drive arms are selected and the remaining arms are fixed. As a method for selecting two arms to be driven, a method disclosed in JP-A-2-287804 is known. According to this method, as the drive arm, when the arm is rotated, the trajectory control target portion becomes maximum in the X (horizontal) direction and Y (vertical)
The arm that has the minimum in the direction and the arm that has the maximum in the Y direction and has the minimum in the X direction are selected. Then, it is determined whether or not each arm is at the stroke end in the control direction.

For example, when performing a trajectory control of vertical lowering, when the posture is as shown in FIG. 11A, the third arm 4 is the most suitable arm for the control in the Y-axis direction, and the first arm 1 is the X arm. It is selected as the arm most suitable for axial control. Since the working direction of the arm is the Y-axis direction, the third arm 3 rotates clockwise. The first arm 1 rotates clockwise or counterclockwise in accordance with the angle A34 in order to perform a movement for correcting a deviation from the trajectory control position caused by the rotation of the third arm 3.

Further, a method disclosed in Japanese Patent Application Laid-Open No. 1-278623 is known as a method of performing the linear trajectory control. In this prior art, the Y direction is the working direction and the X direction is the correction direction. Amount of deviation (deviation Δx) in the correction direction when controlling the trajectory of the arm tip in the working direction
Is detected, and a speed command value XV in the deviation correction direction is obtained from the shift amount and the speed command value YV in the working direction given by the speed command lever. The two arms are driven by speed command values in both directions so that the tip of the arm moves in the direction of a predetermined locus.

[0005]

However, when the above two methods are used in combination, there are the following problems. If the first arm 1 has reached the stroke end in the counterclockwise direction in the state as shown in FIG. 11B, there is a problem that the deviation feedback control cannot be performed with the combination of the third arm 3 and the first arm 1.

It is an object of the present invention to provide a trajectory control device for an articulated working machine in which an optimum pair of arms is selected in trajectory control to which deviation feedback is added, so that the trajectory control disabled state is minimized. is there.

[0007]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS Referring to FIGS. 1 and 4 showing one embodiment, the present invention comprises three or more arms rotatably connected via joints and their arms. Actuator for driving the arm, attitude detecting means 100 for detecting the attitude of each arm, shift amount calculating means 604 for calculating the shift amount of the locus target portion from the target locus, and work along the target locus of the locus control target portion Speed command means 200 for commanding the working speed in the direction, and correction direction speed command value calculating means 600 for calculating a correction direction speed command value in a correction direction orthogonal to the target trajectory based on the deviation amount and the working speed.
A constraint condition selecting means 300 (300A) for selecting two arms in accordance with an input signal and outputting a constraint condition according to the selected two arms; Arm angular speed control value calculating means 400 for calculating the rotation angular speed of the selected arm based on the speed command value and the corrected direction speed command value so that the trajectory control target moves on the target trajectory; The present invention is applied to a trajectory control device of an articulated working machine, comprising: a drive control value calculating means 600 for driving the actuator so that the arm rotates at the arm angular velocity. The object described above is to select the arm in which the movement amount in the working direction is the largest and the arm is the smallest in the correction direction, and is the largest in the correction direction and the smallest in the working direction. This is achieved by selecting an arm whose rotation direction is not the stroke end and outputting a constraint condition according to the selected two arms. In the constraint condition selecting means 300A, the arm whose movement amount in the working direction is maximum and which is the smallest in the correction direction is selected, and the arm which is largest in the correction direction and is the smallest in the working direction. If the arm rotation direction in the case where the feedback control based on the deviation amount is not performed is the same as the arm rotation direction in the case where the feedback control is performed, an arm that is not a stroke end in the rotation direction is selected. The above object can also be achieved by outputting the constraint conditions according to the two arms.

[0008]

When selecting a correction direction side arm whose movement amount is maximum in the correction direction and minimum in the work direction, the constraint condition selection means 300 selects the correction direction side arm whose rotation direction is not the stroke end. The selected constraint condition is input to each speed calculating means 400. The correction direction speed command value calculating means 600 calculates a correction direction speed command value in accordance with the deviation between the trajectory control target portion and the target trajectory, and calculates the correction direction speed command value and the work direction speed command value as angular speed commands. It is input to the value calculation means 400. The calculation result is input to the control means 500, and the pair of arms are driven at the calculated angular velocity. The constraint condition selecting means 300A includes:
When selecting a correction direction side arm in which the movement amount is maximum in the correction direction and minimum in the work direction, when the feedback control based on the arm rotation direction and the deviation amount is performed when the feedback control based on the deviation amount is not performed. If the arm rotation direction is the same, an arm that does not have a stroke end in that direction is selected.

In the means and means for solving the above problems which explain the constitution of the present invention, the drawings of the embodiments are used to make the present invention easy to understand. However, the present invention is not limited to this.

[0010]

【Example】

First Embodiment A case where the present invention is applied to the four-arm working machine shown in FIG. 7 will be described with reference to FIGS. In FIG.
An upper revolving unit US is provided rotatably on the lower traveling unit LT, and these constitute a working machine body CM. A first arm 1 is rotatably provided on the upper revolving superstructure US, a second arm 2 is rotatably provided at the tip thereof, and a third arm 3 is rotatably provided at the tip thereof. A fourth arm 4 is rotatably provided, and the arms 1 to 4 are driven by hydraulic cylinders 5 to 8, respectively. A working attachment, for example, an earth auger excavating unit 9 is pin-connected to the tip of the fourth arm 4. As shown in FIG. 10, a work attachment such as a digging bucket may be attached instead of the fourth arm 4, and the work attachment may be rotatable by the hydraulic cylinder 8. In this case, constant attitude angle control of the work attachment AT is performed.

The coordinates of the working machine are defined as shown in FIG. 2A, and the following description will follow these coordinates. As shown in FIG. 2 (a), the point O is a pivot point of the first arm 1, the lengths of the first to fourth arms 1-4 are L1 to L4, and the relative angles are T1 to L4.
T4, the coordinates of the tip of each arm are (X1, Y1), (X2, Y
2), (X3, Y3), (X4, Y4), each coordinate is

[0012]

X1 = L1cosT1 (1) Y1 = L1sinT1 (2)

X2 = X1 + L2cos (T1-T2) (3) Y2 = Y1 + L2sin (T1-T2) (4)

X3 = X2 + L3cos (T1-T2-T3) (5) Y3 = Y2 + L3sin (T1-T2-T3) (6)

X4 = X3 + L4cos (T1-T2-T3-T4) (7) Y4 = Y3 + L4sin (T1-T2-T3-T4) (8) Therefore, the speeds X4V and Y4V of the tip of the fourth arm 4 are:

[0013]

X4V = −T1V · L1sinT1− (T1V−T2V) · L2sin (T1−T2) − (T1V−T2V−T3) ・ L3sin (T1−T2−T3) − (T1V−T2V−T3V−T4V) L4sin (T1-T2-T3-T4) (9) where T1V, T2V, T3V and T4V each represent an angular velocity.

Y4V = T1V · L1cosT1 + (T1V−T2V) · L2cos (T1−T2) + (T1V−T2V−T3V) · L3cos (T1−T2−T3) + (T1V−T2V−T3V−T4V) ・L4cos (T1-T2-T3-T4) (10)

As is well known, in a multi-joint working machine having four arms, to control the trajectory of the arm tip, two arms are driven by defining a constraint condition. There is a street. (1) Fix third and fourth arms 3 and 4 (driven by first and second arms 1 and 2) (2) Fix second and fourth arms 2 and 4 (first and third arms) (Driven by the first and fourth arms 1 and 3) (3) Fix the second and third arms 3 and 3 (driven by the first and fourth arms 1 and 4) (4) First and fourth arms 1 and 4 (Drive by the second and third arms 2 and 3) (5) Fix the first and third arms 1 and 3 (Drive by the second and fourth arms 2 and 4) (6) First , Fix the second arms 1 and 2 (driven by the third and fourth arms 3 and 4)

Further, when the attitude of the fourth arm 4 is controlled to be constant, there are the following three types. (7) Fix third arm 3 (trajectory control with first and second arms 1 and 2) (8) Fix second arm 2 (trajectory control with first and third arms 1 and 3) (9) Fix first arm 1 (trajectory control with second and third arms 2 and 3)

The above nine conditions are expressed by the following equations. B) In the case of (1): T3V = 0, T4V = 0 b) In the case of (2): T2V = 0, T4V = 0 c) In the case of (3): T2V = 0, T3V = 0 d) (4) Case: T1V = 0, T4V = 0 e] (5): T1V = 0, T3V = 0 f) (6): T1V = 0, T2V = 0 to] (7): T3V = 0, T4V = T1V-T2Vh] (8): T2V = 0, T4V = T1V-T3V]] (9): T1V = 0, T4V =-(T2V + T3V)

Here, X4V and Y4V in the equations (9) and (10) are used as velocity command signals XV and YV, and the above conditions are substituted into the equations (9) and (10) for each constraint condition, and the trajectory control is performed. The angular velocity TnV of the two arms driven for

TnV = fn (XV, YV, T1, T2, T3, T4) (11) However, in the constraint conditions (7) to (9), the trajectory control target portion is the tip of the third arm,

L4 = 0 (12) The detailed equation is shown in FIG. Note that FIG.
Medium speeds are indicated as “X dots” or “Y dots”.

Next, the control device will be described with reference to FIG.
The control device includes an angle detection unit 100 and a speed command unit 200
And the constraint condition selection unit 300 and the angular velocity control value calculation unit 40
0, a drive control value calculation unit 500, and a correction direction speed command value calculation unit 600 are connected as shown in the figure.

Angle detector 1 constituting angle detector 100
Reference numerals 01 to 104 are attached to the pivots of the first to fourth arms 1 to 4, and the relative angles T1 to T4 of the respective arms are detected by a well-known lever mechanism and a potentiometer. T4 is input to the constraint condition selector 300, the angular velocity control value calculator 400, the drive control value calculator 500, and the correction direction speed command value calculator 600.

The speed command section 200 is constituted by an operating lever 202 in the trajectory control direction, and outputs a signal corresponding to the lever operating angle by a lever mechanism and a potentiometer, for example, and outputs this signal to the fourth arm 4 or the third arm. The speed command values XV and YV in the horizontal and vertical directions at the tip of the arm 3 are also input to the angular speed control value calculation unit 400, the encoder 204, and the correction direction speed command value calculation unit 600.

The modified speed command value calculating section 600 is configured as shown in FIG.
座標 T4 to calculate the coordinates and output the X coordinate of the locus control target portion. The coordinate storage unit 602 detects that the trajectory control has been started from the speed command value YV, stores the X coordinate at that time, and holds the value until the trajectory control ends. The X coordinate at that time is the starting point of the trajectory control target, in other words,
The position X0 of the target trajectory is set. Therefore, the actual position X output from the coordinate calculation unit 601 and the coordinate storage unit 6
The deviation ΔX from the target trajectory position X0 output from the target trajectory 02 is calculated at the addition point 604 and input to the correction speed calculation unit 603. The modified speed calculation unit 603 calculates the following equation (1
From 3), a speed command value XV is calculated from the deviation ΔX and the speed command value YV, and this is input to the angular speed control value calculation unit 400.

XV = k · ΔX · | YV | (13) where k is a constant

The constraint condition selector 300 determines the angles T1 to T4,
Based on the sign of the velocity command value in the trajectory control direction output from the encoder 204 and the output of the mode setting unit 301, the constraint conditions (1) to (9) are selectively output according to the processing procedure described later, and are output as angular velocity control values. It is input to the arithmetic unit 400.

Here, the mode setting unit 301 selects a mode in which the attitude angle constant control is automatically performed when the relative angle of the fourth arm 4 reaches a predetermined value by the operation of the operator. When the posture angle constant control mode is selected, when the posture angle of the fourth arm 4 becomes a predetermined angle, for example, -90 °,
A constraint condition including constant attitude angle control is preferentially selected from the nine constraint conditions.

The angular velocity control value calculation section 400 selects any one of the calculation blocks from the calculation blocks 401 to 409 according to the constraint condition, and calculates the angle T1 to T4 and the speed command values XV, YV according to the constraint condition. The angular velocity control values TnV of the selected arm are calculated, and these are input to the drive control value calculation unit 500. Note that each operation block 4
The outputs 01 to 409 are added and output at the addition points 410 to 422 of each arm for each control value of the same arm.

The drive control value calculation unit 500 calculates the flow control values Q1 to Q4 of the cylinders 5 to 8 from the angular velocity control value TnV and the angles T1 to T4 based on the following equations, To 14.

Qn = TnV ・ gn (Tn) ・ An (14) where gn (Tn) is a link correction coefficient An is a cylinder pressure receiving area

A pressure oil is guided to these electro-hydraulic conversion valves 11 to 14 from a hydraulic pressure source.
Pressure oil is supplied to the cylinders 5 to 8 for the first to fourth arms 1 to 4 at a flow rate and a direction corresponding to Q4 to perform trajectory control.

In the constraint condition selecting section 300, the selection of the arm to be controlled is performed according to the processing procedure shown in FIG.
In step S20, the output of the mode setting unit 301 is read, and it is determined whether or not the fourth arm 4 is in the constant attitude angle control mode. If the constant attitude angle control mode has not been selected, the process proceeds to step S2, and if it has been selected, the process proceeds to step S1. In step S1, it is determined whether or not the relative angle of the fourth arm 4 is a predetermined angle. If the angle has not reached the predetermined angle, the process proceeds to step S2. If the angle has reached the predetermined angle, the process proceeds to step S3. In step S2, angles (called control angles) A14, A24, A34, and A44 formed by the line segment connecting the rotation fulcrum of each arm and the tip of the fourth arm 4 shown in FIG.

Using the coordinates of the tip of each arm described above,

A14 = | tan ⁻¹ (Y4 / X4) | (15)

A24 = | tan ^-1 {(Y4-Y1) / (X4-X1)} | (16)

A34 = | tan ^-1 {(Y4-Y2) / (X4-X2)} | (17)

A44 = | tan ^-1 {(Y4-Y3) / (X4-X3)} | (18)

Similarly, in step S3, a line connecting the pivot point of each arm and the tip of the third arm 3 shown in FIG.
The respective (control angles) A13, A23, and A33 that form the axis are calculated.

A13 = | tan ⁻¹ (Y3 / X3) | (19)

A23 = | tan ^-1 {(Y3-Y2) / (X3-X2)} | (20)

A33 = | tan ^-1 {(Y3-Y2) / (X3-X2)} | (21)

Next, in step S4, the control angles of the respective arms are compared, and the arm having the control angle closest to 0 ° is selected. The arm selected in this manner is an arm in which the amount of movement of the trajectory control target portion caused by its rotation is the smallest in the X-axis direction and the largest in the Y-axis direction, in other words, the arm most suitable for control in the Y-axis direction. Arm. In step S5, it is determined whether the arm selected in step S4 has reached the stroke end in the control direction. As shown in FIG. 6, for example, in the case of the first arm, when the speed command value YV is negative, it is determined whether the relative angle of the arm is smaller than a predetermined minimum value (min), or YV is positive. In this case, it is determined whether the value is larger than a predetermined maximum value (max).

In the case of the stroke end, the process proceeds to step S6, and it is determined whether or not the number of unselected arms is one. In the case of one arm, it is impossible to select two arms, so the process proceeds to step S13, and control is disabled. That is, no constraint condition is output. If the number of unselected arms is two or more, the process proceeds to step S7, and the arm whose control angle is closest to 0 ° is selected from among the arms excluding the arm at the stroke end in step S5, and the stroke end is similarly determined. Then, the process returns to step S5.

If it is determined in step S5 that the stroke is not at the end of the stroke, the flow advances to step S8 to compare the control angles of the respective arms, and select an arm having a control angle closest to 90 °. The arm selected here is an arm in which the amount of movement of the trajectory control target portion caused by its rotation is maximum in the X-axis direction and minimum in the Y-axis direction, in other words, the arm most suitable for control in the X-axis direction. Arm.

In step S9, it is determined whether the arm selected in step S8 has reached the stroke end in either the clockwise direction or the counterclockwise direction.
If neither of the rotation directions is at the stroke end,
This means that two arms have been selected together with the arm selected in step S4, and the flow advances to step S12 to output one of the constraint conditions (1) to (9) corresponding to the selected arm.

If it is determined in step S9 that one of the turning directions is the stroke end, the number of unselected arms is checked in step S10, and if there is no remaining arm, two arms cannot be selected, so step S13 Proceed to and control is disabled. If an unselected arm remains, the process proceeds to step S11, and an arm having a control angle closest to 90 ° is selected from among the arms excluding the arm at stroke end in step S9, and the stroke end is similarly determined. It returns to step S9.

This procedure assumes vertical trajectory control. In horizontal trajectory control, steps S4 and S7 and steps S8 and S11 may be interchanged.

Next, the operation of the present apparatus will be described. When the power switch (not shown) is turned on, the apparatus starts up, and the angles T1 to T4 detected by the angle detectors 101 to 104, the sign of the speed command value YV commanded by the trajectory control lever 202, and the setting by the mode setting device 301 are set. Based on the selected mode, the constraint condition selection unit 300 selects the two arms most suitable for the trajectory control according to the procedure of FIG. 5 and outputs the constraint condition corresponding to the selected arm. The corrected direction speed command value calculation unit 600 calculates and outputs a corrected direction speed command value XV obtained by adding a deviation correction from the speed command value YV and the angles T1 to T4.

In the angular velocity control value calculation section 400, one of the arithmetic blocks corresponding to the selected constraint condition is selected, and the angular velocity control value TnV of the selected arm is determined from the angles T1 to T4 and the velocity command values XV, YV. Is calculated. Since the angular velocity control values TnV of the non-selected arithmetic blocks are zero, the angular velocity control values of the selected pair of arms can be obtained by adding the arm angular velocity control values of the outputs of the arithmetic blocks for the same arm at the addition points 410 to 420. Switching of the value is performed.

These angular velocity control values T1V to T4V are link-corrected by the drive control value calculation section 500 and converted into flow control values Q1 to Q4 for each arm. These flow control values Q1
Q4 is supplied to the electro-hydraulic conversion valves 11 to 14, and pressure oil from a hydraulic source is supplied to the first to fourth cylinders 5 to 8 at a predetermined flow rate in a predetermined direction. As a result, the selected arm rotates and the trajectory of the tip of the fourth arm 4 is controlled in accordance with the operation of the speed command lever. Alternatively, the trajectory of the tip of the third arm 3 is controlled according to the operation of the speed command lever, and the attitude angle of the fourth arm 4 is controlled to be constant.

As described above, the stroke end judging method is different from the conventional method in which the stroke is not determined in the control direction as in step S9. By changing to the method of selecting things, FIG.
As in the case of the first arm 1 shown in (b), even in the case of an arm that rotates clockwise when there is no deviation and rotates counterclockwise depending on the deviation, both arms rotate in both directions. Are not stroke ends, the control is not disabled even when the deviation feedback control is applied. That is, in the conventional method,
Even if the control direction is not the stroke end as the correction side arm, the rotation direction for correction may be the stroke end in some cases, and in that case, control may not be possible, According to the present embodiment,
Since the arm whose rotation direction for correction is at the stroke end is removed in advance, such an uncontrollable state does not occur. Further, since the stroke end can be determined only by referring to the posture of the arm, the calculation time can be reduced.

Second Embodiment Next, another embodiment of a trajectory control device for an articulated working machine according to the present invention will be described with reference to FIGS. In the first embodiment, the arm in which both the clockwise direction and the counterclockwise direction are not at the stroke end is not selected. However, in the second embodiment, the above-described deviation feedback control is performed without performing the above-described deviation feedback control. It is determined whether or not the arm rotation direction when performing trajectory control and the arm rotation direction when performing trajectory control by performing deviation feedback control are the same. If the same, the former is the stroke end and the latter is the stroke end. The arm is not selected without checking whether the direction is the stroke end or not.

8 and 9, the same parts as those in FIGS. 1 and 5 are denoted by the same reference numerals, and the differences will be mainly described. The correction direction speed command value XV calculated by the correction direction speed command value calculation unit 600 is input to the constraint condition selection unit 300A. The constraint condition selection unit 300A selects an arm to be controlled according to the flowchart of FIG.

In the flowchart of FIG. 9, after selecting the arm having the control angle closest to 90 ° in step S8,
In step S21, the corrected direction speed command value XV is read.
Next, in step S22, it is determined whether or not the correction direction and the control direction are the same. If they are different, the process proceeds to step S10. Here, in step S22, it is determined whether or not the arm control rotation direction in the case of performing the trajectory control without performing the deviation feedback coincides with the correction rotation direction known from the read correction direction speed command value XV. In step S23, it is determined whether or not the control rotation direction is the stroke end. If it is the stroke end, the process proceeds to step S10, and if not, the process proceeds to step S12.

In step S10, the number of unselected arms is checked. If there is no remaining arm, two arms cannot be selected, so the flow advances to step S13 to disable control. If an unselected arm remains, the process proceeds to step S11, where the arm not in the same direction in step S22 and step S23
Then, an arm whose control angle is closest to 90 ° is selected from among the arms excluding the arm at the stroke end, and the process returns to step S22.

In the first embodiment described above, the arm whose one of the arm rotation directions is the stroke end is not selected. In other words, only the arms that were not stroke end in both directions of rotation were selected. But,
In the second embodiment, when the control rotation direction and the correction rotation direction are the same, the arm is selected if the rotation direction is not the stroke end, so that the arm can be used effectively.

Although the constraint condition selecting section 300 is configured in the form of software, constraint conditions for various combinations of angles T1 to T4, signs of speed command values XV and YV, and mode setting signals are calculated in advance. , The constraint conditions for these combinations may be tabulated. In this case, the constraint condition is immediately obtained by referring to the table with the input data, and the processing time in FIGS. 5 and 9 can be reduced. Further, since it is only necessary to select the calculation block corresponding to the constraint condition and calculate the angular velocity control value, it is not necessary to calculate the angular velocity control value for all arm combinations, and the processing time is longer even if the number of arms increases. does not change. Further, by providing the mode setting device 301, it is possible to easily switch to the attitude angle constant control.

Further, since the angular velocity control values of the respective operation blocks are added for each arm, the constraint condition is automatically switched. At the time of this switching, the old data can be gradually reduced and the new data can be gradually increased to alleviate the shock at the time of switching. Furthermore, if the attitude control mode is selected, when the fourth arm 4 reaches a predetermined value, for example, -90 °, the attitude angle control is automatically performed. When the attachment is moved, contact between the hole and the tip arm can be prevented.

In applying the present invention, each component of the above embodiment may be configured as follows. The number of arms is not limited to four. Although each arm is driven by a hydraulic cylinder, it 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 a vibro hammer, earth auger, excavating bucket, etc., it can also be used for various other work attachments. Although the angle of the first arm 1 is detected as a relative angle with respect to the upper swing body, the angle of the first arm with respect to the ground may be detected, or the inclination angle of the work implement body may be detected to correct the relative angle. good. 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.

[0048]

According to the present invention, the following effects can be obtained. In other words, when a pair of drive arms are selected to perform trajectory control driving of a trajectory control target site, a trajectory control device that feedback-controls a deviation amount of the trajectory control target site from a target trajectory to an angular velocity command value of the arm, In the conventional method of excluding the arm whose control direction is the stroke end when selecting, when the control direction is not the stroke end but the selected arm is also the stroke end in the correction direction, deviation feedback cannot be performed, Although the robot has fallen into an uncontrollable state, as in the present invention, when selecting a pair of arms driven for trajectory control, an arm that does not have a stroke end in both rotation directions of the arm is selected. Therefore, control is not lost during the deviation feedback control. Therefore, even in the case of performing the trajectory control using the above-described deviation feedback control method, it is possible to automatically select a set of two arms most suitable for the trajectory control regardless of the number of arms and the posture of the work implement. Also, it is determined whether the arm rotation direction when not performing the feedback control based on the deviation amount is the same as the arm rotation direction when performing the feedback control based on the deviation amount, and if the same, only the control direction is performed at the stroke end. The same effect as described above can be obtained with the method of selecting an arm by judging that there is, and the arm can be used effectively.

[Brief description of the drawings]

FIG. 1 is a configuration diagram of an embodiment of the present invention.

FIG. 2 is a diagram for defining coordinates.

FIG. 3 is a diagram showing details of constraint conditions.

FIG. 4 is a detailed block diagram of a correction direction speed command value calculating means.

FIG. 5 is a diagram showing a processing procedure of a constraint condition selecting unit.

FIG. 6 is a view for explaining conditions for determining a stroke end.

FIG. 7 is a side view of an embodiment of a working machine to which the present invention is applied.

FIG. 8 is a configuration diagram of another embodiment of the present invention.

FIG. 9 is a diagram showing a processing procedure of a constraint condition selecting unit.

FIG. 10 is a side view of another example of a working machine to which the present invention is applied.

FIG. 11 is a diagram illustrating a conventional problem.

[Explanation of symbols]

 1-4: Arm 5-8: Hydraulic cylinder 11-14: Electro-hydraulic conversion valve 100: Angle detecting means 101-104: Angle detector 200: Speed command unit 202: Speed command lever 300, 300A: Constraint condition selecting unit 301: Mode setting unit 400: angular velocity control value calculation unit 401 to 409: calculation block 500: drive control value calculation unit 600: correction direction speed command value calculation means

──────────────────────────────────────────────────続 き Continued on the front page (58) Field surveyed (Int.Cl. ⁷ , DB name) G05B 19/18-19/46 E02F 3 / 43,9 / 20

Claims (2)

(57) [Claims] 1. An arm which drives three or more arms rotatably connected via a joint, an actuator for driving the arms, a posture detecting means for detecting a posture of each arm, and a target trajectory of a trajectory target portion A shift amount calculating means for calculating a shift amount from the position, a speed commanding means for commanding a work speed in a work direction along a target locus of the locus control target portion, and a target locus based on the shift amount and the work speed. A correction direction speed command value calculating means for calculating a correction direction speed command value of the correction direction to be performed; and an arm having a maximum movement amount in the working direction and a minimum in the correction direction and not a stroke end in the control direction. In addition to selecting the arm, select the arm that is the largest in the correction direction and the smallest in the working direction, and one of the rotation directions is not at the stroke end. A constraint condition selecting means for outputting a constraint condition according to the selected two arms; and a trajectory control target portion based on the constraint condition, the detected arm posture, a work speed command value, and a correction direction speed command value. Means for calculating a rotation angular velocity of the selected arm so that the arm moves on the target trajectory; and drive control for driving the actuator so that the arm rotates at the calculated arm angular velocity. A trajectory control device for an articulated working machine, comprising: a value calculating means. 2. An arm for driving three or more arms rotatably connected via a joint, an actuator for driving the arms, a posture detecting means for detecting a posture of each arm, and a target trajectory of a trajectory target portion A shift amount calculating means for calculating a shift amount from the position, a speed commanding means for commanding a work speed in a work direction along a target locus of the locus control target portion, and a target locus based on the shift amount and the work speed. A correction direction speed command value calculating means for calculating a correction direction speed command value of the correction direction to be selected; and an arm in which the movement amount in the working direction is maximum and the arm in the correction direction is minimum, The arm rotation direction when the feedback control based on the deviation amount is not performed, and the arm rotation direction when the feedback control is performed based on the displacement amount. When the moving direction is the same, an arm that is not a stroke end in that direction is selected, and a constraint condition selecting unit that outputs a constraint condition according to the selected two arms; Arm angular velocity control value computing means for computing a rotational angular velocity of the selected arm, based on the work velocity instruction value and the correction direction velocity instruction value, so that the trajectory control target moves on the target trajectory, A trajectory control device for an articulated working machine, comprising: drive control value calculation means for driving the actuator so that the arm rotates at the calculated arm angular velocity.