JPH0796191B2 - Telescopic control device for articulated arm - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Industrial field of application] In the present invention, a vertically swingable first arm is provided, and a second arm having a working hand at its tip is provided with a tip of the first arm. Is pivotally supported in a vertically swingable manner with respect to the section, and with the start command given, the working hand is started to move along a linear set trajectory, and with a stop command given, Target drive amount calculation means for obtaining a target drive amount per unit time for each swing actuator of the first arm and the second arm so as to stop the movement of the work hand, and the target drive thereof. The present invention relates to an expansion and contraction control device for an articulated arm, which is provided with drive means for driving each of the swing actuators with the target drive amount obtained by the amount calculation means.

[Conventional technology]

In the expansion and contraction control device for the articulated arm of this type,
Conventionally, the target drive amount for the swing actuator is switched from the previous operation to the stop and is calculated as the stop command is given.

[Problems to be Solved by the Invention]

However, since the target drive amount of the actuator is configured to be calculated for each set time, if the target drive amount for stop is calculated after the stop command is given, the actuator will be There will be a time delay until the target drive amount for actual stop is reached, and the work hand will overshoot from the target stop point before actually stopping on the set trajectory. Therefore, it is difficult to quickly stop when the stop command is given.

The present invention has been made in view of the above circumstances, and an object thereof is to make it possible to shorten the time delay from when a stop command is given to when the working hand is stopped.

[Means for Solving the Problems]

The feature of the present invention is that, as described at the beginning, the second arm having the working hand at the tip is pivotally supported up and down with respect to the tip of the first arm, and the work is started by the start command. A target per unit time for the swinging actuators of the first arm and the second arm so as to start the movement of the working hand along a linear set trajectory and to stop the movement of the working hand by a stop command. In a device provided with target drive amount calculation means for obtaining a drive amount for each set time and drive means for driving each of the swing actuators with the target drive amount obtained by the target drive amount calculation means, In addition to the calculation of the target drive amount, the target drive amount calculation means causes each of the swing actuators to stop the movement of the work hand when the stop command is given. The stop drive amount to be applied is calculated before output to the drive means, and the stop drive amount is output to the drive means instead of the target drive amount when the stop command is given. The present invention is configured so as to perform a control operation to perform, and its operation and effect are as follows.

[Work]

That is, when the arm is operated, the target drive amount calculation means calculates in advance the target drive amount for performing this operation and the target drive amount for stopping to stop the arm at the current position. When the operation is continued, the target drive amount for performing the operation is output to the drive means, and when the stop command is given, as described above, before the stop command is given, the previously determined value is obtained. The target drive amount calculation means outputs the target drive amount for stopping to the drive means. Therefore, the arm can be stopped in a shorter time as compared with the conventional method in which the calculation of the target drive amount for stop is started after the stop command is given.

〔The invention's effect〕

Therefore, it is possible to reduce the time delay from when the stop command is given to when the working hand actually receives the stop operation, suppress the occurrence of overshoot, and easily stop the working hand at the target stop point quickly. Was obtained.

〔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. 8, a boom (2) is attached to a vehicle body (V) provided with a pair of left and right traveling wheels (1) at the front and rear so that the boom (2) can be raised and lowered and swung 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, and (6)
Is a boom swing hydraulic cylinder, and (7) is an auxiliary boom swing electric motor.

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 FIGS. 1 and 8, the articulated arm (8) swings around a vertical axis (Y) by a turning electric motor (9a) at the tip of the auxiliary boom (3). The base end portion (8a) to be driven and the base end portion base (8a) are swingably driven around a horizontal axis (X) by a swing electric motor (9b) as a swing actuator. A first arm (8b) and a second arm (8c) pivotally supported on the tip end of the first arm (8b) so as to be vertically swingable by an extension / contraction electric motor (9c) as a swing actuator.
Consists of. In addition, the working hand (H) is swingably driven in association with swinging of the second arm (8c) by the telescopic electric motor (9c).
It is attached to the tip of c).

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 work hand (H), as shown in FIG. 7, a vacuum pad (11) for adsorbing a fruit (F) as a work target is provided at a 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. The ventilation pipe (11) is connected to a suction pump (not shown) mounted on the vehicle body (V) side by a bellows type hose (15) (see FIG. 8).

The capturing part case (13) is an upper case (13b) fixed to a support member (13a) fitted to the ventilation pipe (12).
And a lower case (13c) pivotally attached to the support member (13a) so as to be swingable around a longitudinal axis, and is divided into two parts.

That is, the lower case (13c) is connected to the ventilation pipe (12).
The handle (F) having the handle held in the catching case (13) is cut by swinging it laterally with respect to
Are to be discharged to the outside.

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

Although not shown, the work hand (H) is provided with an actuator for moving the catch case (13) in and out and an actuator for driving the handle cutting device (14). Various actuators for operating, a sensor for detecting that the fruit (F) has been adsorbed to the vacuum pad (11), a sensor for detecting the withdrawal position of the trap case (13), and Various sensors such as a sensor for detecting the drive state of the handle cutting device (14) will be provided.

In the figure, (S ₁ ) is a color image sensor as an image pickup means for picking up an image of a fruit (F) located in the direction in which the working hand (H) is facing, and (S ₂ ) is the working hand ( H) is a proximity sensor that uses infrared light to detect that a target fruit is approaching within a set distance. Both of these sensors (S ₁ ) and (S ₂ ) interfere with approach and fruit harvesting. It is provided inside the vacuum pad (11) so as not to become Further, (16) is an illuminating device for illuminating the front side of the working hand (H) with a set light amount in synchronization with the image pickup process of the image sensor (S ₁ ).

Then, while the working hand (H) is directed 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 (16),
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 work hand (H) expands and contracts in the determined approach direction, and As the proximity sensor (S ₂ ) operates to detect, the vacuum pad (11) is suctioned to adsorb the fruit (F) to be harvested, and the fruit (F) is trapped in the trap case (13). After taking in, the handle will be cut and harvested.

However, when there are a plurality of fruits in the field of view of the image sensor (S ₁ ), which image is to be approached first is determined by the image sensor (S ₁ ) brightness information of the captured image. Based on this, the approach order is automatically determined.

Next, a control configuration for detecting the direction in which the fruit (F) is located, that is, the approach direction and determining the approach order based on the imaged image information from the image sensor (S ₁ ) will be described.

As shown in FIG. 4, image pickup image information obtained by 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 determine each approach for determining the approach order. The working hand (H) is used for the harvest target fruit (F) in the configured and determined approach order.
So that the boom lift hydraulic cylinder (5), the boom swing hydraulic cylinder (6), the auxiliary boom swing motor (7), and the arm (8) drive electric motors ( 9a), (9b), (9c) control device for controlling each operation of the microcomputer (17)
And a servo controller (18) for driving each of the electric motors (9a), (9b), (9c) for driving the arm (8) based on a command from the control device (17). ing.

That is, using the control device (17), the swing electric motor (9b) and the telescopic electric motor are moved so as to move the work hand (H) along a linear set locus (L). The target drive amount calculation means (100) for obtaining the target drive amount per unit time of each of the motors (9c) is configured, and the servo controller (18) calculates the target drive amount. Driving means (101) for driving each of the swing electric motor (9b) and the extension electric motor (9c) by the target drive amount obtained by the means (100).
Will correspond to.

Although not described in detail, the electric motors (9a), (9
An encoder is attached to each of b) and (9c), and the servo controller (18) feedback-controls the drive amount based on the detection information of the encoder.

However, each of the electric motors (9a), (9c), (9c) is configured to adjust the drive amount by PWM-modulating the drive supply voltage, and the target drive amount (Dut
y ₁ ) and (Duty ₂ ) are set as the value of the duty ratio in PWM modulation. That is, the servo controller (18) includes the target drive amount calculation means (100).
Target drive amount ((Duty ₁ ), (Duty ₂ )
The supply voltage is PWM-modulated so that the electric motors (9a), (9c), (9c) are driven.

The horizontal direction of the working hand (H) is changed by the electric motor (9a) for pivoting the arm to expand and contract the arm (8). However, the arm turning electric motor (9a) has the arm (8) with the vertical axis (Y).
It is only turned around, and the load fluctuations therefor are less than those of the swing electric motor (9b) and the extension electric motor (9c). Therefore, unlike the swinging electric motor (9b) and the expansion / contraction electric motor (9c), the driving speed thereof does not greatly affect the work efficiency, and the target amount of the arm turning electric motor (9a) is obtained. The description of the configuration for this 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 based on the torque (τ) and the angular velocity (), It can be obtained from the following equation (i).

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

The torque (τ) and the angular velocity () are determined based on the inertial force (J) and gravity (G) applied to the pivot points of the first and second arms (8b) and (8c). Therefore, the target drive amounts (Duty ₁ ) and (Duty ₂ ) of the electric motors (9b) and (9c) are as follows (ii) and (iii), respectively.
It can be rewritten as shown in the formula.

_{Duty 1 = J 11 · 1 +} J 12 · 2 + C 1 · 1 + G 1 ...... (i
_{i) Duty 2 = J 21 ·} 1 + J 22 · 2 + C 2 · 2 + G 2 ...... (ii
i) where _i is acceleration, _i is angular velocity, and Ci is a value obtained by adding the viscous resistance and the back electromotive force coefficient of the motor.

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 expression (v) twice by differentiating the expression twice, the acceleration ( ₁ ) is ₁ = ₂ /
It can be 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).

θi = _θi0 + (i + _i0 ) · Δt / 2 …… (vi) where i = ( _i0 + D / C) exp (−CΔt / J) −D / C i = −C / J · ( _i0 + D / C) exp (−Δt / J) D = Gi−Duty _i C = Ci J = J _i1 / 2 + J _i2 .

Note that _i0 is an initial value.

That is, each of the target drive amounts (Duty ₁ ) and (Duty ₂ ) is calculated using the above formulas (i) to (vi) based on the difference between the target positions before and after the set time (Δt). Can be done.

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 c).

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 used, and the moving speed of the working hand (H) as a whole can be increased. is there.

Therefore, as shown in FIGS. 2 (a) and 2 (b), the target drive amount of the electric motor on the larger load side, that is, the PWM
The value of the duty ratio in the modulation is set to a constant value of 90%, and the target drive amount for the electric motor with the smaller load is set based on the equations (i) to (vi), and the set time (Δt) I ask each time.

However, when the hand is retracted, the relationship of the magnitude of the load on the swing electric motors (9b) and (9c) is reversed from the middle of the movement path, so from the time when the magnitude of the load is reversed, In order to switch the electric motor on the side that sets the target drive amount to a constant value, the target drive amount (Duty
Every time each of ₁ ) and (Duty ₂ ) is obtained, the magnitudes are compared and the side with the larger value is set to the constant value (90%) (see Fig. 2 (b)). ).

It should be noted that the target drive amount on the heavy load side is set to 90% instead of the maximum driveable value (100%) to ensure a control margin for feedback control by the servo controller (18). This is because

Then, as shown in FIG. 3, in order to enable the working hand (H) to quickly stop in response to a stop command being given, when the target movement amount for operation is obtained, at the current position The target drive amounts for stop (Dutys ₁ ) and (Dutys ₂ ) for sudden stop are simultaneously obtained.

Although not described in detail, the target drive amount for the stop (Duty
s ₁ ) and (Dutys ₂ ) each have the value of the angular velocity (i) as zero and the angular acceleration (i) as shown in the following equation.
By setting the value of i) to a value ( _−i0 / Δt) in which the driving directions of the first and second arms (8b) and (8c) are reversed, the above (i) to (vi) are set. The target drive amounts (Duty ₁ ) and (Duty ₂ ) for the operation can be obtained using the formulas in the same manner as in the case of obtaining the target drive amounts.

Next, based on the flow chart shown in FIG. 5, while explaining the operation of the control device (17), a control configuration for guiding the working hand (H) in the direction in which the fruit to be harvested is located will be described. To do.

First, the working 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 to obtain a specific color region (F) corresponding to the fruit (F). ₁ ), a setting area (F ₂ ) in which the brightness is equal to or higher than the setting value in the specific color area (F ₁ ), and a center of gravity (F ₃ ) of the setting area (F ₂ ) are calculated (Fig. 6). reference).

Then, based on the size of the set area (F ₂ ), the work having the largest size is extracted and the work hand (H) is approached toward the center of gravity (F ₃ ). The arm (8) is extended by adjusting the direction of the hand (H).

In addition, since the brightness of the image information becomes brighter as it is inversely proportional to the distance, the brightness in the specific color area (F ₁ ) becomes a setting value (F ₂ ) in which the brightness is equal to or higher than the setting value. The size of is larger at the closer distance.

In other words, the fruit (F), which is located on the front side and is easy to harvest, is approached first.

After the extension of the arm (8) is started, until the proximity sensor (S ₂ ) is turned on and it is detected that the fruit (F) to be harvested is approaching within the set distance, as described above. ,
Target drive amount (Dut) for operation of the swing electric motor (9b) and the expansion / contraction electric motor (9c) per unit time.
y ₁ ), (Duty ₂ ) and the target drive amount for stop (D
utys ₁ ), (Dutys ₂ ) and the set time (Δ
Each time t), a command is given to the servo controller (18) to drive each of the electric motors (9b) and (9c) with the calculated drive amount, and the work hand is supplied. (H) is linearly moved toward the fruit to be harvested.

When the proximity sensor (S ₂ ) is turned on and it is detected that the fruit to be harvested is approaching within a set distance, the suction operation of the vacuum pad (10) is started to perform the harvesting operation. Become.

However, if the proximity sensor (S ₂ ) does not turn ON even when the arm (8) is extended to reach a preset set distance, it is determined that harvesting is not possible and the arm (8 ) Will be shortened and returned to the approach start position.

After starting the harvesting operation, based on the detection information of various sensors attached to the working hand (H), it is determined whether or not the harvesting has failed. The hand (H) is re-imaging processed while the set distance is further retracted from the position where the proximity sensor (S ₂ ) is turned on, and the imaging information is image-processed to obtain the working hand (H). ), The approach direction is re-determined, the expansion and contraction direction of the arm (8) is corrected, and then the approach is performed again.

By the way, if the proximity sensor (S ₂ ) is not turned on while the arm (8) is extended by the preset distance even if the approach is re-applied based on the re-imaging processed image information, It is judged that it is not possible to harvest, and it will be returned to the approach start position.

When the fruit (F) can be harvested properly, it is returned to the approach start position and the harvested fruit is discharged.

After discharging the harvested fruits, it is judged whether the work is finished or not, based on the number of the specific color area (F ₁ ) extracted from the image information taken at the approach start position, and the work is not finished. In the case, the working hand (H)
Is set at the approach start position and each process after the process of capturing an image is repeated.

Although not described in detail, the harvest may fail even when the approach is performed again, but in that case, the approach is returned to the approach start position without performing the re-imaging process.

That is, when the proximity sensor (S ₂ ) is turned on while the extension distance of the arm (8) reaches the set distance, or even when the arm (8) is extended until the set distance is reached, the proximity sensor (S ₂ ) When S ₂ ) does not turn ON, or when a harvest failure is detected, the controller (17) gives a stop command to the servo controller (18).

That is, the servo controller (18) causes each of the electric motors (9b), (9c) to follow the stop command.
Drive amount to the target drive amount (Dut
y ₁ ), (Duty ₂ ), the target drive amount for the stop (Dutys
By switching between ₁ ) and (Dutys ₂ ), the expansion / contraction operation of the arm (8) is stopped.

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 ₁ ),
By using (B), the signal obtained by subtracting the other color signal ((G or B) from the red signal (R) containing the color component of the fruit (F) is binarized based on the set threshold value, A binarized image in which a specific color area (F ₁ ) corresponding to the color of the fruit (F) is extracted from the current image (see FIG. 6 (A)) taken by the image sensor (S ₁ ) ( (See FIG. 6B).

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

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

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

In the above embodiment, the target drive amount of the actuator on the large load side is set to a constant value, and only the target drive amount of the actuator on the small load side is obtained. You may make it obtain | require the target drive amount of each actuator.

Further, in the above embodiment, the case where the present invention is applied to the working machine for fruit harvesting is illustrated, but it is also applicable to various working machines other than the harvesting machine, and the working hand is aimed toward the target position. It can be applied to various devices that control the expansion and contraction of the articulated arm so that the articulated arm is moved along a linear set trajectory.

By the way, the target drive amount of each swing actuator is
As shown in the above embodiment, the swing angles of the first and second arms are obtained as control parameters, and, for example, the target positions for each set time on the set trajectory of the working hand are set at equal intervals. Then, the work hand may be moved at a constant speed.

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 drawings]

The drawings show an embodiment of an extension / contraction control device for an articulated arm according to the present invention. FIG. 1 is a schematic side view of the articulated arm, and FIGS. 2 (a) and 2 (b) are target drive amounts of actuators. Drawing which shows the change of the moving amount of a working hand, and the relationship of each swing angular velocity of an arm, FIG. 3 is a flowchart of a target drive amount calculation process, FIG. 4 is a block diagram of a control structure, 5th.
FIG. 6 is a flow chart of control operation, FIGS. 6A to 6C are explanatory views of image processing, FIG. 7 is a cross-sectional view showing a schematic configuration of a work hand, and FIG. 8 is an overall side view of a work machine. . (H) …… Working hand, (8b) …… First arm, (8
c) ... second arm, (9b), (9c) ... oscillation actuator, (100) ... target drive amount computing means, (101) ...
… Driving means.

Claims (1)

[Claims] 1. A first arm (8b) swingable up and down is provided, and a second arm (8c) having a working hand (H) at its tip is a tip of the first arm (8b). It is pivotally supported up and down with respect to, and as the start command is given,
The first arm is configured to start the movement of the working hand (H) along a linear set locus and to stop the movement of the working hand (H) in response to a stop command given. (8b) and target drive amount calculating means (100) for obtaining the target drive amount per unit time for the swing actuators (9b) and (9c) of the second arm (8c) and the target drive thereof. With the target drive amount obtained by the amount calculation means (100), the swing actuator (9b),
(9c) is a multi-joint arm expansion and contraction control device provided with a drive means (101) for driving each, wherein the target drive amount calculation means (100) is Separately, when the stop command is given, calculation of the stop drive amount to be output to each of the swing actuators (9b) and (9c) to stop the movement of the work hand (H) is performed by the drive. Before the output to the means (101), the stop drive amount is replaced with the stop drive amount instead of the target drive amount when the stop command is given.
An expansion and contraction control device for an articulated arm that is configured to perform the control operation output to 1).