JP3266714B2 - Manipulator attitude control device - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a manipulator for operating a working machine in response to an operation of a control lever, and more particularly to a device for controlling a posture angle of the working machine.

[0002]

2. Description of the Related Art In a master-slave type manipulator in which a control lever similar in shape to a work machine is used as a master and the work machine is used as a slave, a slave target attitude corresponding to the operation angle of the control lever is used. The posture angle of the work implement is controlled such that the deviation between the angle and the current posture angle of the work implement becomes zero.

In this case, it is necessary to detect the posture angle of the working machine, that is, the rotation angle of each arm constituting the working machine, using an angle sensor such as a potentiometer.

[0004] By the way, a conventional manipulator working machine, particularly a working machine of a self-contained movable manipulator for performing outdoor civil works, etc., has a configuration as shown in FIG.

That is, a boom (first arm) of a working machine
An arm (second arm) c is attached to the tip of a, and a cylinder d for driving the arm c is also attached to the boom a. Therefore, when the boom a is driven by the boom cylinder b, the posture of the arm c changes in a large arc even though the arm cylinder d is not operated. For this reason, the inertial mass load applied to the boom cylinder b becomes extremely large, and it has been difficult to achieve good controllability.

Further, in the working machine having such a configuration, the potentiometer for detecting the rotation angle of the boom c cannot be mounted at a place other than the base of the boom c. However, the base of the boom c is located inside the revolving frame, so that the space is not easy to maintain and the adjustment work is difficult. Furthermore, a dedicated revo frame must be designed so that interference between the potentiometer and other components does not occur, which inevitably leads to high costs.

[0007] The first invention of the present invention has been made in view of such a situation, and reduces the inertial mass load applied to the actuator for driving the first arm, so that the control of the attitude angle of the working machine can be performed with high accuracy. An object of the present invention is to make it possible to detect the posture angle of a working machine with good maintainability and at low cost.

[0008]

[0009]

[0010]

[0011]

[0012]

[0013]

[0014]

[0015]

[0016]

[0017]

[0018]

[0019]

SUMMARY OF THE INVENTION Therefore, the first aspect of the present invention has been described.
In the main invention of the invention, in a posture angle control device of a manipulator that controls the posture angle of the work machine based on the operation angle of the control lever and the current posture angle of the work machine,
The first arm of the work machine is a four-node link mechanism, and two nodes at one end of the first arm are rotatably disposed on a base, and two nodes at the other end of the first arm are connected to a bracket. A work machine configured to be rotatably disposed, and further configured so that one end of a second arm of the work machine and one end of an actuator for driving the second arm are rotatably arranged on the bracket; A first rotation angle sensor for detecting a rotation angle of the first arm is provided at a first arm attachment portion of the bracket, and a rotation angle of the second arm is detected at a second arm attachment portion of the bracket. A second rotation angle sensor for controlling the posture angle of the work implement based on each rotation angle detected by the first and second rotation angle sensors and the operation angle of the control lever. I have to.

[0020]

[0021]

According to the structure of the first aspect of the present invention, as shown in FIG. 2, the first arm of the work machine 21 is a four-node link mechanism 7, and two nodes at one end of the first arm 7 of the four-node link. 7
c and 7d are rotatably disposed on the base 2a, and two nodes 7a and 7b at the other end of the first arm 7 are rotatably disposed on the bracket 5; One end 12a, 12b of the second arm 12 and the second arm 12
One end 9a of the actuator 9 for driving the
It is arranged rotatably.

A first rotation angle sensor 22 for detecting the rotation angle of the first arm 7 and a second rotation angle sensor 2 for detecting the rotation angle of the second arm 12 are provided on the bracket 5.
3 are disposed. Thus, the rotation angle sensor is not provided at the root of the first arm 7. Here, the first
Even if the arm 7 rotates, the posture of the bracket 5 does not change because of the four-node link mechanism. Therefore,
The relative rotation angle β of the first arm 7 with respect to the bracket 5 can be detected by the angle sensor 22 disposed on the bracket 5.

The attitude angle of the work implement 21 is controlled based on the rotation angles detected by the first and second rotation angle sensors 22 and 23 and the operation angle of the control lever.

[0024]

[0025]

[0026]

[0027]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of a manipulator attitude angle control device according to the present invention will be described below with reference to the drawings.

FIG. 1 is a view showing the configuration of a self-supporting movable manipulator for performing a predetermined operation. FIG. 1 (a) is a front view, and FIG. 1 (b) is a top view of FIG. 1 (a). FIG. As shown in FIG. 1, the manipulator includes a movable trolley 1, an upper revolving unit 2 on which a control lever 14 as a master is disposed, and a work implement 21 as a slave. ing.

That is, an upper revolving unit 2 is mounted on a crawler-type movable trolley 1 so as to be freely rotatable, and a support member 2a integrally formed with the revolving unit 2 has a boom, that is, a first arm 3 One end 7c of the first arm 3 and one end 4a of a first arm drive cylinder 4 for rotating the first arm 3 are attached so that the first arm 3 and the drive cylinder 4 can be rotated. The distal end 7a of the first arm 3 is connected to the intermediate bracket 5 so that the first arm 3 can rotate.

On the other hand, the first link 6 is mounted on the support member 2 a and the bracket 5 in the same manner as the first arm 3 so as to be parallel to the first arm 3. That is, the first
One end 7d of the link 6 is mounted on the support member 2a, and the other end 7b of the first link 6 is mounted on the bracket 5 so that the first link 6 is rotatable.

As described above, the first arm 3, the first link 6, the support member 2a, and the bracket 5 cause the four nodes 7a.
The vertical link 7 which is a parallel link mechanism of 7d to 7d is configured.

The intermediate bracket 5 has one end 12a of a second arm 8 and one end 9a of a second arm drive cylinder 9 for driving the rotation of the second arm 8, connecting the second arm 8 and the drive cylinder 9 to each other. Each is mounted so as to be freely rotatable. The distal end 12c of the second arm 8 is connected to the distal end bracket 10 so that the arm 8 can rotate.

On the other hand, the second link 11 is mounted on the intermediate bracket 5 and the front end bracket 10 in the same manner as the second arm 8 so as to be parallel to the second arm 8. That is, one end 12b of the second link 11 is mounted on the intermediate bracket 5, and the other end 12d of the second link 11 is mounted on the front bracket 10, so that the second link 11 is rotatable.

As described above, the four nodes 12 a to 12 d are also provided by the second arm 8, the second link 11, the intermediate bracket 5, and the end bracket 10, similarly to the vertical link 7.
The horizontal link 12 which is a parallel link mechanism of the above is configured.

A hand 13 is rotatably mounted on the distal end bracket 10 of the horizontal link 12, and a predetermined work is performed by gripping the work with the hand 13.

The revolving unit 2 has a control lever 1
4. Equipment necessary for traveling and operation of the working machine, such as a cockpit 15, an engine 16, a hydraulic pump 17, and the like are mounted. Although FIG. 2 assumes that the movable carriage 1 is a crawler type, it is needless to say that the movable carriage 1 may be a tire type.

Here, the operation of the work machine 21 is performed by using the control lever 14 as a master and the work machine 21 as a slave.
This is performed by a known master-slave method. And
In general, the control of the attitude angle of the work implement 21 by the master-slave method, that is, the rotation angles of the first arm 3 and the second arm 8, is performed by feeding back these rotation angles.

Therefore, in order to detect the feedback amount,
A potentiometer 22 for detecting the rotation angle β of the first arm 3 is provided on the first arm 3 attachment portion (rotation fulcrum) 7a on the outer surface 5a of the intermediate bracket 5, and the second arm 3 on the outer surface 5a is also provided. Arm 8 attachment part (rotation fulcrum) 12
a is provided with a potentiometer 23 for detecting the rotation angle δ of the second arm 8.

Now, when the first arm driving cylinder 4 is extended and contracted, the vertical link 7 is a parallel link mechanism as shown in FIG. The attitude of the bracket 5 does not change. Therefore, the second arm 8 is moved up and down while maintaining the parallel state.

As described above, since only the attitude of the first arm 3 changes without changing the attitude of the bracket 5, the potentiometer 22 disposed on the bracket 5
The rotation angle β of the first arm 3 relative to the bracket 5 can be detected. On the other hand, the potentiometer 23 provided on the bracket 5 also
Relative rotation angle δ of the second arm 3 is detected.

According to the working machine 21 having the above structure,
Since it is not necessary to dispose the potentiometer at the root of the vertical link 7, maintenance is improved, adjustment can be performed easily, and a dedicated revo frame is prepared to prevent interference with other parts. Since a general-purpose frame can be used as it is, the cost can be reduced.

The potentiometers 22, 2
Based on the deviation between each of the rotation angles β and δ detected in 3 and the target posture angle corresponding to the operation angle of the control lever 14, the posture angle of the work implement 21 is controlled as described later.

The trajectories of the center of gravity W1 of the first arm 3 and the center of gravity W2 of the second arm 8 are as shown by the alternate long and short dash lines in FIG. 2, and the moment of inertia J acting on the first arm drive cylinder 4 at this time. J = (W1 / g) .r * 2 + (W2 / g) .R * 2 (1) Note that “* 2” is defined as meaning “square” (the same applies hereinafter), while the center of gravity W1 ′ of the boom (first arm) a of the working machine having the conventional structure and the arm The trajectory of the center of gravity W2 'of the (second arm) c is shown by the dashed line in FIG. 7, and the moment of inertia J' acting on the boom cylinder (first arm drive cylinder) b at this time is J ' = (W1 '/ g) .r * 2 + (W2' / g). (R1) * 2 (2)

When these expressions (1) and (2) are compared, since R <r1, usually R * 2 << (r1) * 2, and therefore J << J ′, and when the first arm 3 rotates, The moment of inertia acting on the first arm drive cylinder 4 is much smaller than that of a working machine having a conventional structure. Therefore, the inertia load is reduced, and the control accuracy is improved. As described above, according to this embodiment, it is possible to reduce the cost and to control the attitude angle with high accuracy.

FIG. 3 is a view showing another embodiment, and shows only the components of the working machine 21 '. In addition,
1 have the same functions as those in FIG. That is, in the configuration shown in FIG. 3, only the portion corresponding to the first arm 3 is configured by the four-node parallel link 7, and the portion corresponding to the second arm 8 is not a link configuration but has a general configuration. A tip bracket drive cylinder 20 for rotating the bracket 10 is connected to the second arm 8.

In the embodiment described above, the potentiometers 22 and 23 are connected to the same surface 5 on one side of the intermediate bracket 5.
a, but the mounting surface may be different.

In the embodiment described above, the working machine 2
In operation 1, the control lever 14 is used as a master and
Is assumed to be performed by a known master-slave method.
That is, "a handling instrument, device or machine having a gripper or work head, such that tangible movements and changes in direction are possible in space, and such movements and changes are optionally controlled by means remote from the head"
However, the present invention is not necessarily limited to a master-slave type manipulator in which both the control unit and the controlled unit make corresponding spatial movements.

Next, the control of the attitude angle of the manipulator having the working machine having the above-described structure will be described with reference to FIGS.

Here, the control lever 14, which is the master,
As shown in FIG. 4, a first arm 3 ′ and a second arm 8 ′ having shapes similar to the first arm 3 and the second arm 8 of the work machine 21.
have. Therefore, the first arm 3 of the control lever 14
Is rotated by an operation amount (posture angle) α, the first arm 3 of the work implement 21 is rotated by an angle β corresponding to the operation angle α, and the posture angle is changed. Similarly, when the second arm 8 ′ of the control lever 14 is turned by the operation amount (posture angle) γ, the second arm 8 of the work implement 21 is turned by the angle δ corresponding to the operation angle γ. Then, the attitude angle is changed. Since the control contents of the first arm 3 and the second arm 8 are the same, only the first arm 3 will be described below.

Before starting the control, the following processing is performed as preprocessing.

That is, first, a desired operation range αmin to αmax for the first arm 3 ′ of the control lever 14 is determined. The criterion for this determination is that the operator can operate without difficulty. FIG.
As shown separately in (a) and (b), the operation range can be changed according to the type of work performed by the work implement 21.

Next, the desired operation range αmin to αmax
, A plurality of different operation angles α1, α2, α3 are extracted (see FIG. 5). On the other hand, the desired operation range βmin to βmax of the first arm 3 of the work machine 21 is determined.
Also in this case, as shown separately in FIGS. 6A and 6B,
The operation range can be changed according to the type of work performed by the work machine 21.

Therefore, the first arm 3 'of the control lever 14
Is operated in the desired operation range αmin to αmax, the first arm 3 of the work machine 21 moves to the desired operation range βmin to βm.
ax, so that each of the extracted operation angles α1,
Correlation between α2, α3 and posture angles β1, β2, β3 of work implement 21 is performed as shown by the broken lines in FIG. 5 or FIG.

Then, an operation for interpolating between the plurality of associated angles is performed, and FIG. 6 (a) or (b)
The predetermined correspondence L1 or L2 is obtained as shown in FIG. Here, the method of interpolation can be made different depending on the type of work.

FIG. 6A shows an interpolation method in which the fine operability is emphasized in the vicinity where the operation angle of the control lever 14 is α1, while FIG. 6B shows the vicinity of the operation angles α1 and α3. Is a method of interpolation for lowering the speed of the work machine, and correspondences L1 and L2 having different characteristics are obtained according to the work content.

As described above, the desired operation range αmin
αmax control lever operation angle and desired operation range βmin to βma
When the correspondence L1 or L2 between x and the working machine posture angle is obtained, these correspondences L1 and L2 are stored in the table 24 in association with the work types, for example, work mode 1 and work mode 2, respectively. .

The pre-processing is completed as described above.

Hereinafter, with reference to the control block diagram shown in FIG. 4, the control contents of the attitude angle after the completion of the preprocessing will be described.

First, the operator operates the selection switch (not shown) to select a work mode suitable for the current work content. Now, as the work mode, the correspondence L1
It is assumed that the work mode 1 corresponding to is selected.

On the other hand, the current operation angle α of the first arm 3 ′ is detected by a potentiometer (not shown) attached to the control lever 14, and the first arm of the working machine 21 corresponding to the current operation angle α is detected. The target attitude angle βd of 3 is
It is obtained based on the correspondence stored in the table 24. In this case, since the work mode 1 is currently selected, the correspondence L1 corresponding to the selected work mode 1 is read from the table 24, and the target posture angle βd is obtained based on the correspondence L1. Can be

Next, the obtained target attitude angle βd
Is obtained, the posture angle of the first arm 3 of the work implement 21 is controlled. That is, the current rotation angle β of the first arm 3 is detected by the potentiometer 22, the detected value β is used as a feedback amount, and a deviation between the target value βd and the feedback amount β is obtained. It is added to the gain multiplication unit 25. Gain multiplication unit 2
5, the deviation βd−β is multiplied by a predetermined gain K, and a drive command value corresponding to the deviation βd−β multiplied by the predetermined gain K is
It is added to a drive control unit (not shown). In this drive control unit, the drive cylinder 4 of the work machine 21 is set to the above-mentioned deviation βd−β
Is driven to be zero.

As described above, according to this embodiment, the correspondences L1 and L2 are obtained in advance, these are stored in the table 24, and it is only necessary to read them out, and control is performed without requiring complicated calculations. You. Therefore, since the load on the computer is reduced, it is not necessary to prepare a CPU having a high calculation capability, and the cost is dramatically reduced.

Further, the time required for the arithmetic processing can be greatly reduced.

The operation range of the control lever 14 is set to a range where the operator can operate the control lever 14 without difficulty.
The operability is improved, and the accuracy of the attitude angle control can be improved.

Further, since the operation is performed according to the operation characteristics suitable for the type of work, the operability is improved, and the accuracy of the attitude angle control is improved.

Although the control of the first arm 3 has been described as a representative in this embodiment, the control of the second arm 8 can be performed in the same manner as described above.

The correspondences L1 and L2 may be stored in the table 24 in the form of a mathematical expression such as a function, or may be stored as a numerical table. However,
The method of storing as a numerical table and directly reading out βd from the input value α is stored in the form of a mathematical expression such as a function, and βd (function value) is obtained from the input value α (variable) by calculation. The effect that the calculation time can be shortened more than the method is obtained.

In this embodiment, a manipulator having a working machine 21 having the structure shown in FIG.
The case where the control of the attitude angle shown in FIG. 4 is performed has been described. However, the configuration of the working machine in the case of performing the control of the attitude angle in FIG. The configuration shown in FIG. 7 may be used.

[0069]

As described above, according to the present invention, since the first arm of the working machine of the manipulator has a four-node link configuration, the arm rotation angle sensor is provided at the tip of the arm, not at the base of the arm. can do. As a result, the load on the actuator that drives the arm is reduced, control can be performed with high accuracy, and a dedicated revolving frame is required as in the case where a sensor is provided at the base, which reduces maintenance. As a result, the maintainability is dramatically improved and the cost is dramatically reduced.

[0070]

[Brief description of the drawings]

FIGS. 1A and 1B are a front view and a top view showing a configuration of a self-supporting manipulator which is an embodiment of a manipulator attitude angle control device according to the present invention.

FIG. 2 is a view showing a configuration of a working machine of the manipulator shown in FIG. 1, and is a view for explaining the movement thereof.

FIG. 3 is a diagram showing another configuration example of the work machine shown in FIG. 1;

FIG. 4 is a control block diagram showing a configuration of a control device which is an embodiment of a master-slave type manipulator attitude angle control device according to the present invention.

FIG. 5 is a diagram for explaining contents stored in a table shown in FIG. 4, and is a diagram showing a correspondence between a master operation angle and a slave posture angle by an arm posture.

6A and 6B are diagrams for explaining contents stored in a table shown in FIG. 4, and are graphs showing correspondence between a master operation angle and a slave attitude angle; It is.

FIG. 7 is a diagram showing a configuration of a conventional working machine.

[Explanation of symbols]

 3 1st arm 4 1st arm drive cylinder 5 middle bracket 6 1st link 7 vertical link 8 2nd arm 9 2nd arm drive cylinder 14 control lever 21 work implement 22 potentiometer 23 potentiometer 24 table

──────────────────────────────────────────────────続 き Continuing on the front page (72) Inventor Kazuhiko Otsubo 1200 Manda, Hiratsuka-shi, Kanagawa Prefecture Komatsu Manufacturing Co., Ltd. Central Research Laboratory (72) Inventor Minoru 23 23 Satsutsucho, Komatsu-shi, Komatsu-shi, Ishikawa Awazu Factory, Ltd. (72) Inventor Ikuo Kita 23 Tsutsucho-cho, Komatsu City, Ishikawa Prefecture Inside Awazu Plant, Komatsu Manufacturing Co., Ltd. (72) Inventor Shinji Takeuchi 9-406 Imae-cho, Komatsu City, Komatsu City, Ishikawa Prefecture (56) References JP Showa 56 -157980 (JP, A) (58) Fields investigated (Int. Cl. ⁷ , DB name) B25J 3/00 B25J 19/02 B25J 9/06

Claims (2)

(57) [Claims] 1. An attitude angle control device for a manipulator for controlling an attitude angle of a work implement based on an operation angle of a control lever and a current attitude angle of the work implement, wherein a first arm of the work implement is connected to a four-node link. A mechanism, wherein two nodes at one end of the first arm are rotatably disposed on a base, and two nodes at the other end of the first arm are rotatably disposed on a bracket. A work machine configured to rotatably dispose one end of a second arm of the machine and one end of an actuator that drives the second arm to the bracket; A first rotation angle sensor for detecting the rotation angle of one arm is provided, and the second arm mounting portion of the bracket is provided with the second rotation angle sensor.
A second rotation angle sensor for detecting a rotation angle of the arm is provided, and the work implement is controlled based on each rotation angle detected by the first and second rotation angle sensors and an operation angle of the control lever. An attitude angle control device for a manipulator that controls an attitude angle. 2. The manipulator attitude angle control device according to claim 1, wherein said first and second rotation angle sensors are arranged on the same outer surface of said bracket.