JP2002234700A - Attitude angle control device for manipulator - Google Patents

Abstract

PROBLEM TO BE SOLVED: To improve the operability, namely, control accuracy at a low cost by realizing 'non-similarity' without requiring complicated calculation. SOLUTION: A plurality of different operation angles α1, α2, α3 are extracted from desired operation ranges αmin-αmax of an operation lever 14. In order that a working machine 21 can work within desired working ranges βmin-βmax when the operation lever 14 is operated within the desired operation ranges αmin-αmax, the plurality of extracted operation angles α1, α2, α3 are coordinated with attitude angles β1, β2, β3 of the working machine 21 and the plurality of coordinated angles are interpolated to find a correlation L1 or L2. The correlation L1 or L2 between the operation angle of the operation lever being within the desired operation ranges αmin-αmax and the attitude angle of the working machine within the desired working ranges βmin-βmax is stored in storage means 24. An attitude angle βd of the working machine corresponding to the present operation angle α of the operation lever 14 is found in accordance with the correlation L1 or L2 stored in the storage means 24 and the attitude angle of the working machine 21 is controlled to obtain the found attitude angle βd.

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 Some manipulators have a master-slave system 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.

In this manipulator, when operating a work machine having a plurality of arms and having many degrees of freedom, a control lever (master) having a shape similar to that of the work machine (slave) is prepared. It is effective to operate the control lever within an operation range similar to the operation range of the work implement to improve operability.

[0004] However, the operation range of the control lever, which can be operated comfortably without burdening the operator, is not always similar to the operation range required for the working machine.
If the lever operation range is intentionally dissimilar, the load on the operator may be reduced, the operability may be improved, and the control accuracy may be improved.

In view of the above, the conventional "dissimilarization" for the control lever 14 and the work machine 21 having a two-degree-of-freedom consisting of the first arm 3 (3 ') and the second arm 8 (8'). An example is shown in FIG.

That is, as shown in FIG. 1, 1) the attitude angle (operation amount) of the first arm 3 'of the master 14;
α and the attitude angle (operation amount) γ of the second arm 8 ′ are detected,
These detected values α and γ are added to the angle / coordinate conversion unit 25, and the angle / coordinate conversion unit 25 performs an operation of converting the detected values into coordinate positions xm and ym at the tip of the master 14.

2) Coordinate positions xm, ym of the tip of the master 21
Is applied to the master / slave coordinate conversion unit 26, and the master / slave coordinate conversion unit 26 performs an operation of converting the coordinates into the target coordinate positions xs and ys at the tip of the slave 21.

3) Target coordinate position x at the tip of slave 21
s and ys are applied to a coordinate / angle converter 27, where the target attitude angle βd,
An operation for converting to δd is performed.

4) The attitude angle β of the first arm 3 and the attitude angle δ of the second arm 8 of the slave 21 are detected, and the detected current slave attitude angles β, δ and the target attitude angle βd,
δd are compared with each other to determine respective deviations, and a drive command value corresponding to each deviation is output to the drive system of the slave 21.

In this control device, the operation amounts α and γ of the master 14 are set within a desired operation range, and the conversion units 25 and 25 are controlled so that the slave 21 performs a desired operation within the set operation range. Conversion operations are performed at 26 and 27.

However, in the process 1), the forward kinematics problem needs to be solved, and in the process 3), the inverse kinematics problem needs to be solved. Therefore, complicated calculations are required, and the load on the computer is increased. A large amount of time is required for the calculation.
For this reason, it is inevitable to use a CPU with high computational power, which inevitably leads to high cost.

The present invention has been made in view of such a situation, and realizes “dissimilarity” without requiring complicated calculations, thereby improving operability, that is, improving control accuracy. It is intended to be realized at a cost.

[0013]

SUMMARY OF THE INVENTION Therefore, according to the main invention of the present invention, a master for performing a control for changing the attitude angle of a working machine provided on a slave side in accordance with the operation angle of a control lever provided on the master side. In the attitude angle control device of the slave type manipulator, a plurality of different operation angles are extracted from a desired operation range of the control lever, and when the control lever is operated in the desired operation range, the work machine Operates in the desired operating range,
Interpolating means for associating the extracted plurality of operation angles with the posture angle of the work implement, and interpolating between the associated plurality of angles, and the desired operation obtained by interpolation by the interpolating means. Storage means for storing a correspondence relationship between a control lever operation angle of a range and a work implement attitude angle of the desired operation range; and a work implement attitude angle corresponding to a current operation angle of the control lever is stored in the storage means. And control means for controlling the attitude angle of the work implement so as to obtain the determined attitude angle based on the obtained correspondence.

[0014]

According to the structure of the present invention, FIGS.
As shown in the figure, the desired operation range αmin of the control lever 14 is
A plurality of different operation angles α1, α2, α3 are extracted from αmax to αmax, and the control lever 14 is moved to a desired operation range αmin to αm.
When the operating machine 21 is operated in the ax
The extracted plurality of operation angles α1, α2, α3 and the posture angles β1, β2, β3 of the work implement 21 are associated with each other so as to operate at n to βmax. Are interpolated to obtain the correspondence L1 or L2.
Then, the correspondence L1 or L2 between the operating lever operation angle in the desired operation range αmin to αmax and the working machine posture angle in the desired operation range βmin to βmax is stored in the storage means 24.

Then, the current operation angle α of the control lever 14
The attitude angle βd of the working machine corresponding to
The attitude angle of the work implement 21 is controlled based on the correspondence L1 or L2 stored in the storage device 21 so that the determined attitude angle βd is obtained.

As described above, according to the present invention, it is only necessary to determine and store the correspondences L1 and L2 in advance and read them out, and no complicated calculation is required.

[0017]

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 structure 2 is mounted on a crawler type movable cart 1 so as to be freely rotatable, and a support member 2a integral with the revolving structure 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 four nodes 7a are formed by the first arm 3, the first link 6, the support member 2a, and the bracket 5.
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 rotating the second arm 8, and connects 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 distal 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 12a to 12d are also provided by the second arm 8, the second link 11, the intermediate bracket 5, and the tip bracket 10 as in the case of 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 a workpiece 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.

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). On the other hand, 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)

Comparing these expressions (1) and (2), 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, in which only the components of the working machine 21 'are shown. 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 of 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 arithmetic 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 interpolation method in 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 selects a work mode suitable for the current work content by operating a selection switch (not shown). 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 work 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 they are simply read out, and the 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 easily operate the control lever 14.
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.
Although the description has been given of the case where the attitude angle control shown in FIG. 4 is performed, the configuration of the work implement when performing the attitude angle control of FIG. 4 is arbitrary, and for example, the configuration shown in FIG.

[0059]

As described above, according to the present invention, the operation range of the control lever can be set to a desired range, and the calculation at the time of control becomes unnecessary. As a result, the control accuracy is improved by the improvement of the operability, and the cost is drastically reduced because a computer having a high computing ability is not required.
In addition, the arithmetic processing ability is dramatically improved.

[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.

FIGS. 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 control block diagram showing a configuration of a conventional control device.

[Explanation of symbols]

 3 1st arm 4 1st arm drive cylinder 5 intermediate 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

Continued on the front page (51) Int.Cl. ⁷ Identification symbol FI Theme coat II (reference) // E02F 3/43 E02F 3/43 A (72) Inventor Kenji Okamura 1200 Manda, Hiratsuka-shi, Kanagawa Komatsu Ltd. Inside the Central Research Laboratory (72) Inventor Kazuhiko Otsubo 1200 Manda, Hiratsuka-shi, Kanagawa Prefecture Inside the Komatsu Seisakusho Central Research Laboratory (72) Inventor Minoru Ohura 23 Satsutsucho, Komatsu-shi, Komatsu-shi, Ishikawa Prefecture Komatsu Manufacturing Awazu Plant ( 72) Inventor Ikuo Kita 23 Satsutsu-cho, Komatsu City, Ishikawa Prefecture Inside the Awazu Plant of Komatsu Manufacturing Co., Ltd. (72) Inventor Shinji Takeuchi 9-406 Imae-cho, Komatsu City, Ishikawa Prefecture F-term (reference) BA02 BB08 DB04 FA02 3C007 AS21 BS10 BS22 CS08 CT05 CV08 CW08 HT12 JT06 WA17 3F333 AA01 AE21 BB07 CA21 FA22 FA23 FD07 FD08 FE04 FE09

Claims (4)

[Claims] 1. A posture angle control device for a master-slave type manipulator for controlling a posture angle of a working machine provided on a slave side in accordance with an operation angle of a control lever provided on a master side. A plurality of different operation angles are extracted from a desired operation range of the lever, and the extracted operation angles are extracted so that the work implement operates in a desired operation range when the control lever is operated in the desired operation range. Interpolating means for correlating the plurality of operation angles with the posture angle of the work machine, and interpolating between the plurality of correlated angles, and manipulating the desired operation range obtained by interpolation by the interpolating means. Storage means for storing a correspondence relationship between a lever operation angle and a work machine attitude angle in the desired operation range; and a work machine corresponding to a current operation angle of the control lever And a control means for controlling the attitude angle of the work implement so as to obtain the determined attitude angle based on the correspondence stored in the storage means, and to obtain the determined attitude angle.
Attitude control device for slave type manipulator. 2. The interpolator performs interpolation according to the type of work performed by the work machine, and the storage unit stores the correspondence in association with the type of work. The control means, when the work type is selected, reads out the correspondence corresponding to the selected work type from the storage content of the storage means, and performs the work based on the read correspondence. The attitude angle control device for a master / slave type manipulator according to claim 1, wherein the attitude angle control apparatus controls the attitude angle of the machine. 3. The working machine includes a plurality of arms having different degrees of freedom, and the control lever operates the plurality of arms individually. 2. The attitude angle control device for a master-slave type manipulator according to claim 1, wherein the attitude angle is obtained for each arm and stored in the storage means in correspondence with each arm. 4. The work machine has a first arm and a second arm, the first arm being a four-node link mechanism, and two nodes at one end of the first arm being rotatable on a base. And two nodes at the other end of the first arm are rotatably disposed on a bracket, and one end of the second arm and one end of an actuator for driving the second arm are also turned on the bracket. 4. The attitude angle control device for a master-slave type manipulator according to claim 3, wherein the attitude angle control device is configured to be movably disposed.