JPH0796190B2 - 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. Each of the first actuator and the second actuator 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 moves 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 each of the first actuator and the second actuator is driven by the target drive amount calculated by the target drive amount calculation means. The invention relates to a telescopic control device for an articulated arm provided with a drive means.

[Conventional technology]

In the expansion and contraction control device for the articulated arm of this type,
Conventionally, for example, as shown in FIG. 8, the first arm (8b) is arranged so that the target positions (W) on the set locus (L) of the working hand (H) are lined up at equal intervals. The work hand (H) moves at a constant speed so that the target drive amounts of the first and second actuators (9b) and (9c) for the second arm (8c) and the second arm (8c) are obtained for each set time. I was trying to do it. In the figure, (8a) is a base end portion base which is pivotally driven about a vertical axis, and the base end portion (8a) is provided with the base end portion of the first arm (8b) on the horizontal axis. It is swingably supported around the core. Servo motors are generally used as the actuators (9b) and (9c).

[Problems to be Solved by the Invention]

However, since there is a limit to the maximum driving force of the actuator, for example, when the arm is extended and the work hand is projected forward with the arm retracted as the initial position, the arm is extended. The more the work hand is projected forward, the more the work hand is driven against gravity, so that the load on the actuator of the second arm becomes large.

When the arm is contracted from the extended state and the work hand is returned to the initial position, the load on the actuator on the first arm side becomes large, so that the driving speed is the first.
It will be limited by the maximum drive force of the arm actuator.

That is, the maximum settable target drive amount of the actuator per unit time is limited by the maximum drive force of the actuator so that the drive force of the actuator becomes maximum at the maximum load.

Therefore, when the actuator is driven so that the work hand moves at a constant speed as in the above-mentioned conventional case, even if the load is light and the driving force of the actuator has a margin, the driving speed is the maximum driving at the maximum load. It can't be higher than the driving speed when driven by force,
There is a disadvantage that the time required for the work hand to reach the target position cannot be shortened.

Incidentally, in order to increase the moving speed of the work-requiring hand, a large-sized actuator having a large maximum driving force of the actuator may be used, but there is a disadvantage that the device becomes expensive.

An object of the present invention is to reduce the time required for the working hand to reach the target position without increasing the driving force of the arm driving actuator.

[Means for Solving the Problems]

The features of the present invention are, as described at the beginning, a first arm driven by a first actuator, a second arm driven by a second actuator and provided with a working hand at its tip, and a working hand. Target drive amount calculation means for obtaining the target drive amount per unit time for each of the first and second actuators so as to move along a linear set locus, and the target drive amount calculation means. In an expansion and contraction control device for an articulated arm, which is provided with a drive unit that drives each of the first and second actuators with a target drive amount, the target drive amount calculation unit includes a first drive unit and a second drive unit. It is configured such that target drive amounts of the first actuator and the second actuator are obtained so as to swing each at an equal angular velocity. And the effects are as follows.

[Work]

According to the above feature, the target drive amount calculation means obtains the target drive amounts of the first actuator and the second actuator so as to swing the first arm and the second arm at equal angles, and the drive means determines the target drive amount. Then, the first and second actuators are driven.

Specifically, as shown in FIG. 1, a work hand (H)
To move along the linear set locus (L),
When the first arm (8b) and the second arm (8c) are rocked at an equal angular velocity, the interval between the target positions (W) for each set time becomes larger as the rocking angle of the entire arm becomes smaller. It is evenly spaced. Therefore, in the characteristic configuration of the present invention, as shown in FIGS. 2A and 2B, the first and second actuators are respectively moved so as to swing the first arm and the second arm at an equal angular velocity. By ascertaining the target drive amount for each set time, the moving speed of the working hand is maximized in the range where the swing angle of the entire arm is small, and the moving speed of the working hand as a whole is high. Will be transformed.

It should be noted that FIG. 9A shows changes in the target drive amount of the actuator and the movement amount of the work hand and the swing of the first and second arms when the work hand is advanced to the side away from the arm base end. The relationship between the dynamic angular velocities is shown, and FIG. 6B shows the relationship between the target drive amount, the change in the moving amount, and the swing angular velocity when the vehicle is retracted. And
In the figure, Duty ₁ and Duty ₂ are target drive amounts of the first and second actuators, σ is a movement amount of the work hand, ₁ ,
₂ is the swing angle of each of the first and second arms.

However, the target drive amount is a ratio (% display) to the maximum drive amount.
It is shown as.

〔The invention's effect〕

Therefore, the moving speed of the working hand is not limited by the maximum driving force of the actuator, and the time required for the working hand to reach the target position is shortened without increasing the driving force of the actuator for driving the arm. I was able to do it.

〔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, 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 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 on the tip end of the first arm (8b) by a telescopic electric motor (9c) as a second actuator so as to be vertically swingable. 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). So as to move along a linear set locus (L) that connects the target electric powers (Duty ₁ ) and (Duty ₂ ) to the swing electric motor (9b) and the expansion / contraction electric motor (9c), respectively.
Is driven 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 linearly 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) The catching part case (13) that is fitted and supported in the front-rear direction so that it can be pulled back and forth, and the handle part of the fruit (F) taken into the catching part case (13) are vertically sandwiched. And a handle cutting device (14) for cutting in the state. 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. 7).

The catching 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).
Fruits (F) whose handle held in the catching case (13) is cut by swinging the handle 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 working hand (H) includes an actuator for moving the catching case (13) in and out, an actuator for driving the handle cutting device (14), and the like. Various actuators for harvesting operation, a sensor for detecting that the fruit (F) has been adsorbed to the Bamicuum pad (11), and a sensor for detecting the retracted position of the catching section case (13). Further, various sensors such as a sensor for detecting the driving state of the handle cutting device (14) are provided.

In the figure, (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 working hand (H) is facing, and (S ₂ ) is the working hand. (H) is a proximity sensor using infrared light that detects that the fruit to be harvested has approached within a set distance. Both sensors (S ₁ ) and (S ₂ ) are used for approach and fruit harvesting. It is provided inside the vacuum pad (11) so as not to get in the way. 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 working 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 captured in the capturing part case (13). ), 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.

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. 3, 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) as the first actuator is moved so as to move the work hand (H) along the linear set locus (L). And a target drive amount calculating means (100) for obtaining a target drive amount per unit time of each of the expansion / contraction electric motor (9c) as the second actuator, and the servo controller. (18) is a drive means (101) for driving 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).
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), (9b), (9c) is configured so that the drive amount is adjusted 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).
The supply voltage is PWN-modulated so as to obtain the target drive amounts (Duty ₁ ) and (Duty ₂ ) obtained by the above, and the electric motors (9a), (9b) and (9c) are driven.

The horizontal direction of the working hand (H) is changed by the arm rotating electric motor (9a) as the arm (8) is expanded and contracted. However, the arm turning electric motor (9a) only turns the arm (8) around the vertical axis (Y), and load fluctuations corresponding thereto change the swing electric motor (9b) and the expansion and contraction. It is less than the electric motor (9c) for. Therefore, unlike the swinging electric motor (9b) and the telescopic electric motor (9c), its driving speed does not greatly affect the work efficiency, and the target driving amount of the arm turning electric motor (9c). The description of the configuration for obtaining 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 (). , 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 the angular acceleration, θi is the angular velocity, and Ci is the 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 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).

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

However, as shown in FIGS. 2 (a) and 2 (b), each of the first and second arms (8b) and (8c) swings at a constant angular velocity after the start of driving until a stop command is given. In order to do so, the value of the angular velocity (i) is set to be constant, and the differences between the target positions are set to have equal angular intervals. Incidentally, FIG. 2 (i) shows the relationship between the target drive amount for each set time (Δt) during forward movement, the moving distance (σ) of the working hand (H), and the angular velocity (i). The same figure (b) shows the relationship at the time of reverse travel.

Next, based on the flow chart shown in FIG. 4, 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). ₁ ), the setting area (F ₂ ) where the brightness is higher than the setting value in the specific area (F ₁ ) and the center of gravity (F ₃ ) of the setting area (F ₂ ) are calculated (see FIG. 5). ).

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, the proximity sensor (S ₂ ) is turned on and the swinging is performed until it is detected that the fruit (F) to be harvested is within a set distance. Each of the target drive amounts (Duty ₁ ) and (Duty ₂ ) per unit time of the electric motor (9b) and the expansion / contraction electric motor (9c) is calculated for each of the set times (Δt), and the calculated driving is performed. A command is given to the servo controller (18) so as to drive each of the electric motors (9b) and (9c) with a certain amount, and the working hand (H) is directed toward the fruit to be harvested. Will be moved.

When the proximity sensor (S ₂ ) is turned on and it is detected that the target fruit is within the set distance, the suction operation of the vacuum pad (10) is started and the fruit is harvested. .

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.

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 color signal (G or B) other than the red signal (R) containing the color component of the fruit (F) from each other is binarized on the basis of the set threshold value. A binarized image (No. ₁ ) obtained by extracting a specific color region (F ₁ ) corresponding to the color of the fruit (F) from the current image (see FIG. 5A) taken by the image sensor (S ₁ ). 5 (see FIG. 5B)).

Then, in the first image processing, the brightness in the specific color region (F ₁ ) becomes the set value or more based on the value of the luminance (Y) in the image signal output from the image sensor (S ₁ ). The setting area (F ₂ ) is extracted, and the direction in which the center of gravity (F ₃ ) of the setting area (F ₂ ) is located on the image is obtained as the position information of the fruit (F).

However, as also shown in FIG. 5 (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 order of approach will be determined such that the larger size the first approach is made.

[Another embodiment]

In the above-mentioned embodiment, the case where it is applied to the working machine for fruit harvesting of the present invention is illustrated, but it is also applicable to various working machines other than the harvesting machine, and the working hand is linear toward the target position. The present invention can be applied to various devices that control expansion and contraction of an articulated arm so that the articulated arm moves along a set locus. And
Various changes can be made to the specific configurations of the respective parts 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 configurations 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. FIG. 3 is a side view showing the relationship between the change in the amount of movement of the working hand and the swing angular velocity of the arm. FIG. 3 is a block diagram of the control configuration.
FIG. 4 is a flowchart of control operation, FIGS. 5A to 5C are explanatory views of image processing, FIG. 6 is a sectional view showing a schematic configuration of a working hand, and FIG. 7 is an overall side view of a working machine. , 8th
The figure is an illustration of a conventional example. (8c) …… Second arm, (8b) …… First arm, (9b)
...... First actuator, (9c) ...... Second actuator, (100) ...... Target drive amount calculation means, (101) ...... Drive means, (H) ...... Working hand.

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). A second actuator (9c) pivotally supports the tip of one arm (8b) so that the work hand (H) can be moved along a linear set trajectory. Target drive amount calculation means (10) for obtaining target drive amount per unit time for each of the first actuator (9b) and the second actuator (9c)
0) and a drive means (10) for driving each of the first actuator (9b) and the second actuator (9c) with the target drive amount obtained by the target drive amount calculation means (100).
1) is an expansion and contraction control device for an articulated arm, wherein the target drive amount computing means (100) includes the first arm (8b) and the second arm (8c), etc. The first actuator (9
b) and an expansion and contraction control device for an articulated arm, which is configured to obtain a target drive amount of each of the second actuator (9c).