JPH0796193B2 - Telescopic control device for articulated arm - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Industrial field of use] The present invention provides a first arm that is vertically swingable by a first actuator, and a second arm having a working hand at its tip. The first actuator and the second actuator are pivotally supported by a second actuator so as to be vertically swingable with respect to the tip end of the first arm, and move the working hand along a linear set trajectory. Target drive amount calculation means for obtaining the target drive amount per unit time for each set time, and the first actuator and the second actuator based on the information of the target drive amount obtained by the target drive amount calculation means And a drive means for driving each of the above.

[Conventional technology]

The expansion / contraction control device for an articulated arm of this type controls the feedforward control of the arm drive so that the working hand provided at the tip of the articulated arm moves along a linear set trajectory. It is the one.

By the way, when the arm drive is feed-forward controlled, the operating characteristics of the actuator, which is a control parameter, in particular, the fluctuation of the viscous resistance information greatly affects the control accuracy.

Therefore, conventionally, the drive unit is configured to perform feedback control based on the target drive amount given from the target drive amount calculation unit and the drive result, and the control error in the feedforward control is corrected. (For example, refer to Japanese Patent Application No. Sho 63-180601 previously proposed by the applicant).

[Problems to be Solved by the Invention]

In the conventional configuration, the control accuracy error due to the fluctuation of the viscous resistance information is corrected by the feedback control, so it is difficult to set the target drive amount to the maximum drive amount capable of driving the actuator. It was not possible to move the work hand at the maximum speed that could drive the.

By the way, it is possible to start the actual work after tuning the arm by driving the arm to correct the error due to the viscous resistance, but it will cause an operation unrelated to the actual work, There is a disadvantage that work efficiency decreases.

The present invention has been made in view of the above circumstances, and an object thereof is to enable a work hand to move at high speed while suppressing a decrease in work efficiency.

[Means for Solving the Problems]

In order to achieve the above-described object, the articulation type arm extension / contraction control device according to the present invention is provided with a first arm that is vertically swingable by a first actuator, and a second arm provided with a working hand at its tip. An arm is pivotally supported by a second actuator so as to be vertically swingable with respect to a tip end portion of the first arm, and the first actuator and the first actuator are arranged so as to move the working hand along a linear set trajectory. Target drive amount calculation means for obtaining a target drive amount per unit time for each of the second actuators for each set time, and the first actuator based on the information of the target drive amount obtained by the target drive amount calculation means. The second
Driving means for driving each of the actuators are provided, and the characteristic configuration thereof is as follows.

That is, the target drive amount calculating means sets the initial drive amount of the first actuator and the second actuator, which has a large drive load, as the reference drive amount, which is a preset drive amount lower than the maximum drive amount. , The actual drive amount of the gutter with a large drive load driven by the reference target drive amount is detected, and the target drive amount of the gutter with a small drive load per unit time is corrected based on the detection result It is configured to correct the stored viscous resistance information for obtaining the target drive amount in the case of a large load on the basis of the drive result at each set time, and each time the movement along the set trajectory is performed, It is configured to gradually increase the reference target drive amount stored to obtain the target drive amount.

[Work]

That is, since the error of the feedforward control due to the fluctuation of the viscous resistance can be corrected by the feedback control of the driving means, even if the work hand is moved while tuning, the error with respect to the target movement locus can be reduced.

Therefore, of the first actuator and the second actuator, the actual drive amount of the blade actuator having a large drive load is detected, and the target drive per unit time of the blade actuator having a small drive load is detected based on the detection result. Each time the working hand is actually moved while performing the tuning to correct the amount and the viscous resistance information, the moving speed of the working hand is increased every time the work hand is moved along the set trajectory. , The reference target drive amount is gradually increased.

〔The invention's effect〕

Therefore, since the target drive amount can be gradually increased so that the actuator can be tuned while performing work and the actuator is driven at the maximum speed that can be driven, the work efficiency can be prevented from lowering and the work hand can be maintained. It became possible to move at high speed.

〔Example〕

Hereinafter, an example in the case where the present invention is applied to a working machine for fruit harvesting will be described with reference to the drawings.

As shown in FIG. 7, a boom (2) is attached to a vehicle body (V) provided with a pair of left and right traveling wheels (1) on the front and rear sides so that the boom (2) can be raised and lowered and turned freely. The auxiliary boom (3) is attached so that it can swing horizontally.
Then, a work manipulator (4) having a work hand (H) for fruit harvesting is attached to the tip end portion of the auxiliary boom (3), and thus a working machine for fruit harvesting is configured. In the figure, (5) is a boom lifting hydraulic cylinder, (6) is a boom turning hydraulic cylinder, and (7) is an auxiliary boom swing hydraulic cylinder.

The working manipulator (4) comprises an articulated arm (8) and a working hand (H) attached to the tip of the arm (8).

As shown in FIG. 1 and FIG. 7, the articulated arm (8) swings around the vertical axis (Y) by the swing electric motor (9a) at the tip of the auxiliary boom (3). The base end base (8a) to be driven and the base end base (8a) are swingably driven around the horizontal axis (X) by the swing electric motor (9b) as the first actuator. A first arm (8b) and a second arm (8c) pivotally supported by a telescopic electric motor (9b) as a second actuator so as to be vertically swingable with respect to the tip end of the first arm (8b). Consists of. The work hand (H) is rockably driven in association with the rocking of the second arm (8c) by the electric motor (9c) for expansion and contraction.
It is attached to the tip of the.

Then, with the initial position being a state where the first arm (8b) and the second arm (8c) substantially overlap with each other in a side view, the working hand (H) sets the initial position and the fruit to be harvested (F). The target drive amounts (Duty ₁ ) and (Duty ₁ ) of the swing electric motor (9b) and the expansion / contraction electric motor (9c) so as to move along a linear set locus (L) connecting
y ₂ ) is driven while being calculated for each set time (Δt) to expand and contract the arm (8).

However, the first arm (8b) and the second arm (8c) are formed to have the same length (A), and the first arm (8b)
Is oscillated directly by the oscillating electric motor (9b). Meanwhile, the second arm (8c)
Is a chain (10a) interlockingly connected to the extension / contraction electric motor (9c) attached to the base end side of the first arm (8b), and the base end of the chain is connected to the first arm (8b).
It is configured to rotate at the tip end of the. Then, the working hand (H) is interlocked with the swing of the second arm (8c) by the telescopic electric motor (9c), and the swing angle (θ ₂ ) of the second arm (8c). The pivot points of the second arm (8c) are interlockingly connected by a chain (10b) so that the second arm (8c) can be swung in the direction opposite to the swinging direction of the second arm (8c).

Therefore, the work hand (H) is moved to the set locus (L).
In order to move the first arm (8b) with respect to the first arm (8b) from the half of the swing angle (θ ₂ ) of the second arm (8c) with respect to the vertical direction (θ ₂ ). The unit time of each of the swing electric motor (9b) and the extension electric motor (9c) is adjusted so that the angle obtained by subtracting θ ₁ ) matches the elevation angle (η) of the set locus (L) with respect to the horizontal direction. The target drive amount per hit (Duty ₁ ) and (Duty ₂ ) are set respectively.

The control configuration for controlling the drive of each of the swing electric motor (9b) and the expansion / contraction electric motor (9c) will be described later.

Explaining the working hand (H), as shown in FIG. 6, a vacuum pad (11) for adsorbing a fruit (F) as a work target is provided at the tip end thereof,
The ventilation pipe (1) of the vacuum pad (11) so as to cover the fruit (F) adsorbed to the vacuum pad (11).
2) A state in which the catch case (13) that is fitted and supported in the front-rear direction so as to be retractable in the front-rear direction and the handle part of the fruit (F) taken in the catch case (13) are vertically sandwiched. And a handle cutting device (14) for cutting with. Although not described in detail, the vacuum pad (11) is sucked by a negative pressure generating device using an ejector pump (not shown) attached to the ventilation pipe (12).

The capturing portion case (13) is a body portion (13a) fitted onto the ventilation pipe (12) and a state in which the body portion (13a) is urged to return toward the inner side of the hand. In addition, a cover (13
b) consists of.

However, although not described in detail, the cover (13b) is divided into four parts in the vertical and horizontal directions, and an opening through which the fruit to which the vacuum pad (11) is adsorbed passes is formed on the front side of the hand. . That is, when the fruit is taken into the trap case (13) through the opening, if the actual diameter is larger than the opening, each of the four-divided covers opens outward in the hand direction, and It is possible to take in fruits whose diameter is larger than the opening. The cover (13b) divided into four parts, which is located on the lower side, is constructed so as to be opened downward by an actuator (not shown), so that the inside of the capturing part case (13). The fruit (F) whose pattern is cut off can be discharged to the outside.

The handle cutting device (14) presses the handle of the fruit (F) against the blade (14a) of a clipper type for cutting the handle of the fruit (F) against the blade (14a) from below. And a handle support member (14b) for supporting.

However, although not described in detail, the working hand (H) includes
Various harvesting actuators such as an actuator for moving the trap case (13) in and out and an actuator for driving the handle cutting device (14), and a fruit (F) on the vacuum pad (11). Various types of sensors, such as a sensor for detecting that the suction part has been adsorbed, a sensor for detecting the retracted position of the trap case (13), and a sensor for detecting the drive state of the handle cutting device (14). A sensor will be provided.

Incidentally, in FIG. 6, (S ₁ ) is a color type image sensor as an image pickup means for picking up an image of the fruit (F) located in the direction in which the work hand (H) is facing, and (S ₂ ) is the work Proximity sensors that use infrared light to detect when the harvesting hand (H) has approached the harvested fruit within a set distance. Both of these sensors (S ₁ ) and (S ₂ ) are the approach and the fruit. It is provided inside the vacuum pad (11) so as not to interfere with harvesting. Further, (15) is the work hand (H) in synchronization with the image pickup processing of the image sensor (S ₁ ).
Is an illumination device for illuminating the front side of the vehicle with a set light amount.

Then, while the working hand (H) is oriented in the direction in which the fruit to be harvested is located, the image is processed by the image sensor (S ₁ ) while being illuminated by the illumination device (15),
Based on the captured image information, the working hand (H)
The approach direction, that is, the direction of the set locus (L) for moving the work hand (H) is determined.

After determining the approach direction, the electric motors (9a), (9b), (9c) are operated so that the working hand (H) expands and contracts in the determined approach direction, and When the proximity sensor (S ₂ ) detects the approach to the fruit, the vacuum pad (11) is suction-operated to adsorb the fruit (F) to be harvested, and then inside the capturing part case (13). After capturing the fruit (F),
The handle will be cut and harvested.

However, if there is a plurality of fruit in the imaging field of view of the image sensor (S _1), whether to approach first from any fruit, the brightness information of the image sensor (S ₁₎ by the captured image Based on this, the approach order is automatically determined.

If the image processing is further described, as shown in FIG. 4, the work hand (H) is subjected to image pickup processing in a state where it is set at a preset approach start position, and the image pickup information is subjected to image processing. The specific color area (F ₁ ) corresponding to the color of the fruit (F) and the specific color area (F ₁ )
Within the setting area where the brightness is above the set value (F ₂ )
And the center of gravity (F ₃ ) of the set area (F ₂ ) are extracted. Then, the work area (F ₂ ) having the largest size is extracted, and the work hand (H) is first approached toward the center of gravity (F ₃ ) of the work area (F ₃ ). ) Will be adjusted to extend the arm (8).

That is, as shown in FIG. 5, the brightness of the image information becomes brighter as it is inversely proportional to the distance, so that the brightness becomes equal to or higher than the set value in the specific color region (F ₁ ). By taking advantage of the fact that the size of the set area (F ₂ ) becomes larger at closer distances, the easily harvestable fruit (F) located on the front side is approached first. .

The image processing for extracting the specific color region (F ₁ ) will be described. The three primary color signals (R), (G), among the image signals output from the image sensor (S ₁ ),
(B) is used to binarize a signal obtained by subtracting the other color signal (G or B) from the red signal (R) containing the color component of the fruit (F) based on the set threshold value, thereby captured original image by the image sensor (S ₁₎ (FIG. 4 (b) refer) from the binarized image obtained by extracting a specific color area (F ₁₎ corresponding to the color of the fruit (F) (No. 4 (see (b)).

Then, based on the value of the luminance signal (Y) of the image signal output from the image sensor (S ₁ ), the brightness within the specific color area (F ₁ ) is set to a setting area ( F ₂ ) is extracted, and the direction in which the center of gravity (F ₃ ) of the set area (F ₂ ) is located on the image is obtained as the position information of the fruit (F). (See FIG. 5).

However, as also shown in FIG. 4 (c), when a plurality of one specific color area (F ₁₎ the set area (F ₂₎ is extracted, the setting region (F ₂₎ Based on the size of, the approach order will be determined so that the larger size is approached first.

Next, a control configuration for guiding the work hand (H) toward the work target fruit based on the imaging information of the image sensor (S ₁ ) will be described.

As shown in FIG. 2, image pickup image information from the image sensor (S ₁ ) is subjected to image processing to determine the direction in which the fruit (F) to be harvested is located, and to measure each means for determining the approach order. The working hand (H) is used for the harvest target fruit (F) in the configured and determined approach order.
Electric motors for driving the boom lifting hydraulic cylinder (5), the boom turning hydraulic cylinder (6), the auxiliary boom swinging hydraulic cylinder (7), and the arm (8) so as to be guided toward Based on a control device (16) using a microcomputer for controlling the operation of each of (9a), (9b) and (9c), and a command from the control device (16),
Electric motors (9a), (9) for driving the arm (8)
Servo controller (1) that drives each of b) and (9c)
7) and are provided.

That is, using the control device (16), the turning electric motor (9a) and the swinging electric motor are moved so as to move the work hand (H) along a linear set locus (L). A target drive amount calculation means (100) for obtaining a target drive amount per unit time of each of the motor (9b) and the expansion / contraction electric motor (9c) is configured,
Then, the servo controller (17) drives each of the swing electric motor (9b) and the expansion / contraction electric motor (9c) with the target drive amount obtained by the target drive amount calculation means (100). It corresponds to the means (101).

In addition, each of the electric motors (9a), (9b), (9c) has the base end portion base (8a) and the arms (8b), (8c).
Encoder (e ₀ ) for detecting the swing angle of
(E ₁ ) and (e ₂ ) are attached, and the servo controller (17) basically includes the encoders (e ₀ ),
Based on the detection information of (e ₁ ) and (e ₂ ), the swing angles (θ ₀ ), (θ ₁ ), (θ) of the base end base (8a) and the arms (8b) and (8c) _The drive amount is feedback-controlled so that ₂ ) is located at a position corresponding to the target drive amount.

However, each of the electric motors (9a), (9b), (9c) is configured to adjust the drive amount by PWM-modulating the drive supply voltage, and each target drive amount (Duty
Each of ₀ ), (Duty ₁ ), and (Duty ₂ ) is set as the value of the duty ratio in PWM modulation. That is, the servo controller (17) controls the target drive amount (Dut) calculated by the target drive amount calculation means (100).
The supply voltage (Y _B ) is PWM-modulated so that y ₀ ), (Duty ₁ ), and (Duty ₂ ), and the electric motors (9a), (9
b) and (9c) will be driven.

The arm turning electric motor (9a) for changing the horizontal direction of the working hand (H) is similar to the swinging motor (9b) and the extension / contraction electric motor (9c). It suffices that the driving load does not change significantly, and that the direction of the working hand (H) can be adjusted to the direction in which the harvest target fruit is located by the time it reaches the harvest target fruit. Therefore, the swing motor (9b)
Also, since it is not necessary to drive at high speed like the expansion / contraction electric motor (9c), description of the configuration for obtaining the target drive amount is omitted.

Next, the target drive amount calculating means (100) for obtaining the target drive amounts (Duty ₁ ) and (Duty ₂ ) of the swing electric motor (9b) and the expansion / contraction electric motor (9c) will be described.

The target drive amount of each of the electric motors (9b) and (9c), that is, the duty ratio (Duty) for PWM-modulating the supply voltage (V _B ) is, for example, an electric power that drives the second arm (8c). If the target drive amount (Duty ₂ ) of the motor (9c) is taken as an example, it can be obtained from the following formula (i) based on the torque (τ) and the angular velocity ( ₂ ).

However, K _T is a torque constant, r is an armature resistance of the motor, and R is a reduction ratio.

Further, the torque (τ) and the angular velocity (i) are based on the inertial force (J) and the gravity (G) applied to the pivot points of the first and second arms (8b) and (8c), respectively. Since it can be determined, the torque (τ) of the electric motor (9c) can be rewritten as shown in the following formula (ii).

_{τ = J 21 · 1 + J} 22 · 2 + G 2 + V 2 · 2 ...... (ii) where, V ₂ is the viscous resistance coefficient in the second arm (8c).

By the way, in order to move the working hand (H) at a high speed, it suffices to reduce the reduction ratio (R). However, if the reduction ratio (R) is reduced, the above (i),
As is clear from the equation (ii), the influence of the inertial force (J) and the gravity (G) becomes large with respect to the error of the target drive amount, that is, the movement locus of the work hand (H).

Therefore, as will be described later in detail, when the target drive amount (Duty) is obtained for each set time based on a reference value (M) described later while actually driving the telescopic arm (8), the viscous resistance is set. The coefficient (V ₂ ) is corrected based on the driving result, and the working hand (H) is guided while being tuned so that the deviation from the set locus (L) is reduced.

On the other hand, the moving distance (σ) from the approach start position of the work hand (H) and the elevation angle (η) are the first,
The length (A) of each of the second arms (8b) and (8c), the length (B) of the working hand (H), and the length of the first arm (8).
b) the vertical swing angle (θ ₁ ) and the second arm (8c) swing angle (θ ₁ ) with respect to the first arm (8b).
₂ ) and can be obtained based on the following equations (iv) and (v).

_{σ = 2A · sin (θ 2} /2) + B ...... (iv) η = θ 2/2-θ 1 ...... (v) where the elevation angle (eta), because the change is small, (v) above by differentiating the equation twice, the acceleration ₍₁₎ may be a _{1 = 2/2.}

Therefore, the target swing angle (θi) of the first and second arms (8a) and (8c) after the set time (Δt) is equal to the angular velocity (
It can be calculated by the following equation (vi) based on each of i) and angular acceleration ().

θi = θ _i0 + (i + _i0 ) Δt / 2 (vi) Therefore, the predicted angular velocity ( ₂ ) corresponding to the target drive amount (Duty ₂ ) can be calculated based on the following formula (vii). .

However, D ₂ = G ₂ −Duty ₂ C ₂ = V ₂ + motor back electromotive force coefficient ₂₀ = initial value.

That is, each of the target drive amounts (Duty ₁ ) and (Duty ₂ ) is the current position of the arm, that is, the current swing angles (θ _{1 (} θ _{1 (} 8 1) of the first arm (8b) and the second arm (8c)). _n) ), (θ _{2 (n)} ) and the set time (Δ
swing angle (θ _{1 (n + 1)} ) after t),
From the difference with (θ _{2 (n + 1)} ), the above (i) to (vi
It can be obtained using the equation i).

Then, the predicted angular velocity (i) corresponding to the target drive amount (Duty) is compared with the actual angular velocity obtained based on the detection information of the encoders (e ₁ ) and (e ₂ ), and the actual angular velocity is compared. Is faster than the predicted angular velocity (i), a value (Ci) obtained by adding the viscous resistance coefficient (Vi) and the back electromotive force coefficient of the motor (in the following description, viscous resistance information (Ci)
(Referred to as)) by a set amount (ΔVi) (see FIG. 8 (a)), and when the actual angular velocity is slower than the predicted angular velocity (i), the viscous resistance information (Ci) is set by the set amount ( ΔCi) is increased (see FIG. 8B), and the actual angular velocity is corrected so as to be the predicted angular velocity (i), and the deviation of the actual movement locus from the set locus (L) is reduced. I am doing so.

By the way, when the arm is extended and the working hand (H) is projected in a direction against gravity against the whole arm (when moving forward), the second arm (8c) is moved to the first arm ( Since the second arm (8c) attached to the tip end of 8b) swings in the direction against gravity, the load on the extension / contraction electric motor (9c) is greater than that of the extension electric motor (9c). The load on the motor (9b) is larger than that on the other hand. On the other hand, when retracting the working hand (H) by retracting the arm from the state where the entire arm is extended (when retracting), the first arm (8b) is gravitated. In order to swing the electric motor (9b) for swinging, the load applied to the electric motor (9b) for swinging is greater than that for the electric motor (9) for stretching.
It is greater than the load on b).

That is, during the expansion / contraction operation, the load on one of the electric motors becomes lighter than the load on the other electric motor, so the target drive amount for the electric motor on the side with the larger load is as large as possible. By controlling the target drive amount for the electric motor having the smaller load in this way, the drive energy can be effectively utilized, and the moving speed of the working hand (H) as a whole can be increased. Become.

Therefore, as shown in FIG. 3, the reference value (M) of the target drive amount of the electric motor on the side where the load is large, that is, the PWM
The initial value of the duty ratio in the modulation is set to 90% which is obtained by subtracting half of the control margin (± 20%) of the feedback control by the servo amplifier (18) from the maximum drive amount, and the approach is started. .

After starting the approach, at every set time (Δt), the target drive amount for the electric motor on the smaller load side is corrected while correcting the viscous resistance information (Ci) on the larger load side based on the drive result. The correction is performed based on the current position of the arm for each set time (Δt). That is, the reference value (M) of the target drive amount (Duty) on the side where the load is large is set to the maximum while driving the telescopic arm (8), and the work hand is actually used. The telescopic arm (8) is tuned while (H) is moved toward the target.

Then, every time the harvesting work is performed, that is, each time the work hand (H) is moved along the set locus (L), the reference value (M) of the target drive amount (Duty) is set to the servo. The work margin is gradually increased until the control margin of the feedback control by the controller (17) becomes zero, that is, the drive amount actually driven by the servo controller (17) reaches the maximum drive amount. ) Causes the electric motor (9b or 9c) to move at the maximum speed at which it can be driven.

However, when the actual drive amount becomes the maximum drive amount,
Since the reference value (M) of the target drive amount (Duty) needs to be set large by the control margin of the feedback control by the servo controller (17), it is actually set as the reference value (M). The maximum value of is 120% (see FIGS. 9 (a) to 9 (d)).

By the way, when the hand moves backward, the relationship between the magnitudes of the loads on the electric motors (9b) and (9c) is reversed from the middle of the movement path. Therefore, although not described in detail, each of the target drive amounts (Duty ₁ ) and (Duty ₂ ) is switched so that the electric motor on the side that sets the target drive amount to a constant value is switched from the time when the magnitude of the load reverses. Each time is calculated, the magnitudes are compared, and the drive amount on the side having the larger value is set to be larger.

It should be noted that a stop command is given as the proximity sensor (S ₂ ) is turned on when the hand is moved forward, and a stop command is given as the proximity sensor (S ₂ ) is returned to the initial position when retracted. When a stop command is given, the target drive amounts (Duty ₁ ) and (Duty ₂ ) are switched to stop values, and the working hand (H) is stopped.

Although not described in detail, the target drive amounts for stopping each have the value of the angular velocity (i) as zero and the value of the angular acceleration (i) as the first and second arms ( 8b),
A value (− _i0 /
By setting Δt), the above (i) to (vii)
Using the formula, the target drive amount for the above operation (Duty ₁ ), (Duty ₁
It can be obtained in the same manner as y ₂ ).

[Another embodiment]

In the above-described embodiment, the case where the center of gravity (F ₃ ) of the setting area (F ₂ ) in which the brightness in the specific color area (F ₁ ) is equal to or higher than the setting value is used as the position information of the fruit (F) is used. Although illustrated, the and regions brightness is maximized in the specific color area (F ₁₎ in the specific color area (F ₁₎ can also be used centroid itself.

Further, in the above embodiment, the case where the present invention is applied to the working machine for fruit harvesting is illustrated, but the present invention can be applied to various working machines other than the harvesting machine, and is directed toward the target position. It can be applied to various devices that control the expansion and contraction of the articulated arm so that the work hand moves along a linear set trajectory. Then, various changes can be made to the specific configurations of the respective units necessary for carrying out the present invention.

It should be noted that reference numerals are added to the claims for convenience of comparison with the drawings, but the present invention is not limited to the structures of the accompanying drawings by the entry.

BRIEF DESCRIPTION OF THE DRAWINGS The drawings show an embodiment of a telescopic control device for an articulated arm according to the present invention. FIG. 1 is a schematic side view of the articulated arm, and FIG. 2 is a block diagram of a control configuration. FIG. 3 is a flow chart of control operation, FIGS. 4 (a) to (c) and FIG. 5 are explanatory views of image processing, FIG. 6 is a sectional view showing a schematic configuration of a working hand, and FIG. Overall side view of the working machine, FIGS. 8A and 8B are explanatory views of tuning, and FIG. 9A.
FIGS. 6A to 6D are explanatory diagrams of increasing the moving speed of the work hand. (H) …… Working hand, (8b) …… First arm, (8
c) …… Second arm, (9b) …… First actuator,
(9c) ... second actuator, (100) ... target drive amount calculation means, (101) ... drive means.

Claims (1)

[Claims] 1. A first arm (8b), which is vertically swingable by a first actuator (9b), is provided, and a second arm (8c) having a working hand (H) at its tip portion is the first arm (8c). 1
With respect to the tip of the arm (8b), the second actuator (9
c) is pivotally supported by a vertical swing motion, and each of the first actuator (9b) and the second actuator (9c) is moved so as to move the work hand (H) along a linear set trajectory. Target drive amount calculation means (100) for obtaining a target drive amount per unit time for each set time
And the first actuator (9) based on the information on the target drive amount obtained by the target drive amount calculation means (100).
b) and a drive means (101) for driving each of the second actuators (9c), which is an expansion and contraction control device for an articulated arm, wherein the target drive amount calculation means (100)
Of the first actuator (9b) and the second actuator (9c), which has a large drive load, and has an initial drive amount that is a preset lower drive amount than the maximum drive amount as a reference target drive amount. The actual drive amount of the gutter with a large drive load driven by the reference target drive amount is detected, and the target drive amount of the gab with a small drive load is corrected based on the detection result. It is configured to correct the stored viscous resistance information for obtaining the target drive amount in the large gutter at every set time based on the drive result, and the target drive is performed every time the movement along the set trajectory is performed. An expansion / contraction control device for an articulated arm configured to gradually increase the reference target drive amount stored to obtain the amount.