JPH0790478B2 - Expansion and contraction control device for articulated arm of harvesting machine - Google Patents

Description

Description: TECHNICAL FIELD The present invention relates to an expansion and contraction control device for an articulated arm of a harvesting machine for harvesting fruits and the like. The first arm is mounted on the moving body, and the second arm having a working hand at its tip is pivotally supported by the second actuator so as to be vertically swingable with respect to the tip of the first arm. Target drive amount calculation means for obtaining a target drive amount per unit time for each of the first actuator and the second actuator so as to move the work hand along a linear set trajectory, and a target thereof. Drive means for driving each of the first actuator and the second actuator with the target drive amount obtained by the drive amount calculation means, and the target drive amount The calculating means sets a target drive amount for the actuator of one arm of the first arm or the second arm, which has a large drive load, to a predetermined value, and sets a target drive amount of the actuator for the other arm. The present invention relates to a telescopic control device for an articulated arm of a harvesting machine configured to be required.

[Conventional technology]

The expansion / contraction control device for the articulated arm of the harvesting machine of this type is mainly used for harvesting outdoors in an orchard, and therefore, the fruits to be harvested are affected by wind or the like. For example, the harvesting work is performed in an environment where the arm of the moving body may sway. Therefore, it is desirable to activate the arm as quickly as possible and reduce the influence of wind to perform the harvesting work. It is a thing.

By the way, since there is a limit to the maximum driving force of the actuator, the settable maximum value of the target driving amount per unit time of the actuator is such that the driving force of the actuator becomes less than the maximum driving force at the maximum load. It will be limited by the maximum drive force of the actuator.

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

However, in the expansion / contraction control device for the multi-joint type arm of this type of harvesting machine, the change in speed during movement of the work hand and the change in the amount of movement per unit time often do not pose a problem. Alternatively, by setting the target drive amount for the actuator of the one arm of the second arm having a large drive load to a predetermined value and obtaining the target drive amount of the actuator for the other arm, the work hand Can move as fast as possible.

In addition, for example, when the arm is extended and the working hand is projected in a direction against gravity, the second arm is attached to the tip of the first arm, and Since the second arm is swung in the direction against gravity, the load on the actuator of the second arm becomes larger than the load on the actuator of the first arm, while the entire arm is extended. When retracting the working hand by retracting the arm, the load on the actuator of the first arm is greater than the load on the actuator of the second arm because the first arm is swung in the direction against gravity. However, the load on one of the actuators will be lighter than the load on the other actuator. .

Therefore, by setting the target drive amount of the actuator on the high load side to a predetermined value and obtaining the target drive amount of the actuator on the low load side, the drive force of the actuator on the high load side is maximized. To be big,
The time required for the working hand to reach the target position can be shortened without increasing the driving force of the actuator for driving the arm.

However, in the past, in order to maintain the operating position of the actuator at a value according to the target drive amount, the target drive amount of the actuator on the side where the load is large is fed back from the maximum drive amount of the actuator in position feedback. It was set below the value excluding the control margin for control.

[Problems to be Solved by the Invention]

Therefore, in the conventional configuration, the target drive amount of the actuator on the side where the load is large cannot be set to the maximum drive amount of the actuator, so that the harvest work cannot be adequately performed quickly, and the harvest work efficiency can be further improved. Improvement was desired. Further, conventionally, the correction when the actual position of the work hand is displaced with respect to the predetermined movement locus is performed by both the actuator on the side with a large load and the actuator on the side with a small load. However, the feedback control for the correction becomes complicated, and the inertial load is generally large on the side where the load is large, so that the correction easily overshoots and it takes time to make accurate correction. .

The present invention has been made in view of the above circumstances, and an object thereof is to enable an actuator on the side with a large load to be driven with a maximum drive amount of the actuator,
The purpose is to accurately and promptly correct the deviation with respect to a predetermined movement trajectory.

[Means for Solving the Problems]

In the expansion / contraction control device for an articulated arm of a harvesting machine according to the present invention, a first arm that is vertically swingable by a first actuator is provided, and a second arm having a working hand at its tip is Second with respect to the tip of the first arm
It is pivotally supported by an actuator so that it can be swung vertically.
Target drive amount calculation means for obtaining a target drive amount per unit time for each of the first actuator and the second actuator so as to move the work hand along a linear set trajectory, and Drive means for driving each of the first actuator and the second actuator with the target drive amount obtained by the target drive amount calculation means, and the target drive amount calculation means includes the first arm or the first arm. The target drive amount for the actuator of the one arm having the larger drive load of the two arms is set to a predetermined value, and the target drive amount of the actuator for the other arm is obtained. The characteristic configuration is as follows.

That is, position detection means for detecting the position of the work hand on the set trajectory is provided, and the target drive amount calculation means is one side of the first arm or the second arm where the drive load is large. The target drive amount of the arm of the actuator is set to the maximum drive amount, and the target drive amount of the actuator of the other arm having a smaller drive load is corrected based on the detection information of the position detecting means. There is a point.

[Action]

Since the movement trajectory of the work hand is determined by the operating positions of both the first arm and the second arm, even if the operating position for the actuator on the side with a large load cannot be feedback-controlled, the side with a small load can be used. If the target operating position of the actuator is corrected based on the current position of the work hand for each set time, the deviation of the work hand from the set trajectory can be corrected.

Therefore, the position of the working hand on the set locus is detected, and based on the detected position information, the target drive amount of the actuator of the other arm having the smaller drive load is corrected while being driven at each set time. To do.

〔The invention's effect〕

Therefore, by rationally modifying the device, it is possible to drive the actuator on the side with a large load with the maximum drive amount while suppressing the occurrence of deviation from the set trajectory of the work hand, and to improve the moving speed of the work hand. Since the arm on the side where the driving load is large does not perform the position correction for the movement locus, the control for performing the correction can be simplified, and
Since the correction control is performed only by the actuator on the side where the load is small, that is, the side that is easy to control, the correction can be performed with high accuracy,
It has become possible to more reliably and quickly harvest fruits and the like.

〔Example〕

 Embodiments of the present invention will be described below with reference to the drawings.

As shown in FIG. 7, a boom (2) is attached to a vehicle body (V) as a moving body 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.
An auxiliary boom (3) is horizontally swingably attached to the tip of the auxiliary boom (3), and a work manipulator (4) having a work hand (H) for fruit harvesting is provided in the auxiliary boom (3). It is attached to the tip of the and thus constitutes a working machine for fruit harvesting.

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

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, (16) 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 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 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.

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. 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.
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 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 (18) drives each of the swing electric motor (9b) and the extension electric motor (9c) with the target drive amount obtained by the target drive amount calculation means (100). It corresponds to the means (101).

Note that each electric motor (9a), (9c), each the husband (9c) s arm (8b), the encoder (e ₀₎ for detecting the swing angle of _{(8c), (e 1)} , (E ₂ ) is attached, and the servo controller (18) basically uses the detection information of these encoders (e ₀ ), (e ₁ ), and (e ₂ ) to set each arm (8 b). , (8c) swing angle (θ ₀ ),
The drive amount is feedback-controlled so that (θ ₁ ) and (θ ₂ ) are at positions corresponding to the target drive amount.

However, each of the electric motors (9a), (9c), (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 (18) controls the target drive amount (Dut) calculated by the target drive amount calculation means (100).
y ₀ ), (Duty ₁ ), (Duty ₂ ), the supply voltage (V _B ) is PWM-modulated so that each of the electric motors (9a), (9
c) and (9c) will be driven.

The arm turning electric motor (9a), after approaching the horizontal direction of the working hand (H) at the approach start position to the direction in which the fruit to be harvested is located,
Since it is stopped, it is not necessary to control the drive amount unlike the swing motor (9b) and the expansion / contraction electric motor (9c). Therefore, the description of the configuration for obtaining the target drive amount will be given. Omit it.

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 an armature resistance of the motor, and R is a reduction ratio.

The torque (τ) and the angular velocity () can be 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 formula the by differentiating 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 (
i) and angular acceleration (i) respectively, the following (v
It can be calculated by equation (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 the current position of the arm, that is, the current swing angle (θ _{1 (} ( ₁ )) of each of the first arm (8b) and the second arm (8c). _n) ),
The value of (θ _{2 (n)} ) and the swing angle (θ) after the set time (Δt)
It can be obtained from the difference between _{1 (n + 1)} ) and (θ _{2 (n + 1)} ) using the above equations (i) to (vi).

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, the target drive amount of the electric motor on the side where the load is large, that is, the value of the duty ratio in the PWM modulation is set to a constant value of 100% corresponding to the maximum drive amount,
The target drive amount for the electric motor having the smaller load is calculated for each set time (Δt) while being corrected based on the current position of the arm for each set time (Δt).

However, 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. Every time
The sizes are compared, and the side having the larger value is set to the maximum value (100%).

A stop command is given when the proximity sensor (S ₂ ) is turned on when the band moves forward, and a stop command is given when the proximity sensor (S ₂ ) returns to the initial position when the band moves backward. 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),
The value ( _-i0 /
Δt) is set so that the target drive amounts (Duty ₁ ) and (Dut) for the operation are calculated by using the equations (i) to (vi).
It can be obtained in the same manner as y ₂ ).

Next, based on the flowcharts shown in FIGS. 3A and 3B, the target drive amount (Du
ty ₁ or Duty ₂ ) is set to the maximum value (100%), and the processing for obtaining the target drive amount of the electric motor on the side where the load is small will be described.

However, it is assumed that the load of the second arm (8c) is larger than that of the first arm (8b) both when the band is moving forward and when the band is moving backward, and the target drive amount () Duty ₂ ) is set to 100%, and the target drive amount (Duty ₁ ) of the first arm (8b) is obtained for each set time (Δt).

First, based on the flowchart shown in FIG.
The target drive amount calculation processing during hand advance will be described.

That is, the control device (17) controls the set time (Δ
For each t), the detection information of the encoder (e ₁ ) for the arm swing motor (9b) input via the servo controller (18) and the encoder (e ₂ for the arm extension motor (9c). ), The current value of the swing angle of the arms (8b) and (8c) corresponding to the current position (n) of the working hand (H) (θ _{1 (n )} ), (Θ _{2 (n)} ), and then the inertial force (J _(n) ) and gravity (G _(n) ) are calculated based on these values.

That is, based on the detection information of the encoders (e ₁ ) and (e ₂ ), the current value of the swing angle of the arms (8b) and (8c) (θ
_The process of obtaining _{1 (n)} ) and (θ _{2 (n)} ) corresponds to the position detecting means (102) for detecting the position of the working hand (H) on the set trajectory.

Next, the above (iv) wherein the target angular speed of the at the next target position the second arm ^{_{(8c) (2 * (n}} + 1)) and a target swing angle of ^{_{(θ 2 * (n + 1}} )) Each of them is calculated, and the target swing angle (θ ₂
^* _{(N + 1)} (target oscillating angle ^{_{8b) (θ 1 * (n}} + 1) the first arm from the value of)) is determined.

Then, both the target swing angles (θ ₁ ^* _{(n + 1)} ),
Based on (θ ₂ ^* _{(n + 1)} ), the inertial force ((J _{(n + 1)} ) after the set time (Δt) and the gravity (G _{(n + 1)} ) are obtained,
From these values, determining the respective target angular velocity at the next target position ^{_{(n + 1) (2 *}} (n + 2)) and the target swing angle ^{_{(θ 2 * (n + 2}} )).

Next, based on the obtained value of the target swing angle (θ ₂ ^* _{(n + 2)} ), the target swing angle (θ ₀ ^* _{(n +} ) around the vertical axis (Y) of the first arm (8b) is obtained. ₂₎ ) and the target swing angle (θ ₁ ^* _{(n + 2)} ) around the horizontal axis of the first arm (8b), and based on these values, the arm rotation electric motor ( 9a) target drive amount (Duty ₀ ) and arm swing electric motor (9b) target drive amount (Duty ₁ ), respectively, and the arm extend / retract electric motor (9c) target drive amount (Duty ₂₎ = 100%), and the servo controller (1) for passing to the servo controller (18) and calculating the next target drive amount.
8) to the current value of the swing angle (θ
_{After each of 1 (n + 1)} ) and (θ _{2 (n + 1)} ) is received, “1” is added to the value of the variable (n) for setting the target position.

The target drive amount calculation processing when the hand moves backward will be described. As shown in FIG. 3B, the target swing of the first arm (8b) at the next target position is performed in the same manner as when moving the hand forward. Moving angle (θ ₁ ^* _{(n + 1)} ) and the second arm (8
c) Each target swing angle (θ ₂ ^* _{(n + 1)} ) is calculated, and
From the obtained target swing angles (θ ₁ ^* _{(n + 1)} ) and (θ ₂ ^* _{(n + 1)} ), the inertial force ((J _{(n + 1)} ) and gravity (G _{(n +) 1)} Ask each of the).

Next, the inertial force ((J
_{(n + 1)} ) and gravity (G _{(n + 1)} ), the second arm (8
c) Target angular velocity ( ₂ ^* _{(n + 2)} ) and target swing angle (θ ₂ ^*
_{(n + 2)} ) and the target swing angle (θ ₂
^* _{(n + 2)} ) from the target turning angle (θ ₀ ^* _{(n + 2)} ) around the vertical axis (Y) and the target swing angle (θ ₁ around the horizontal axis (X)). ^* _{(n + 2)} ) for each.

Then, the target turning angular velocity of the first arm (8b) ( ₀ ^*
_{(n + 2)} ), target swing angular velocity ( ₁ ^* _{(n + 2)} ), target swing angular acceleration ( ₁ ^* _{(n + 2)} ), and target swing angular acceleration of the second arm (8c) From ( ₂ ^* _{(n + 2)} ) respectively, the target drive amount (Duty ₀ ), (Duty ₁ ) of each of the electric motors (9a), (9b)
Of the second arm (8c) and the target drive amount (Duty ₂ = 100%) of the second arm (8c) together with the servo controller (18).
And for calculating the next target drive amount, the present values (θ _{1 (n + 1)} ), (θ _{2 (n +} ) from the servo controller (18) after the set time of the swing angle are calculated. After receiving each of ₁₎ ), "1" is added to the value of the variable (n) for setting the target position.

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). _{In 1} ), the setting area (F ₂ ) where the brightness is equal to or higher than the setting value and the center of gravity (F ₃ ) of the setting area (F ₂ ) are obtained (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 activated to detect the approach to the harvest target fruit (F) within the set distance, as described above. In addition, the target drive amount (Du) for operation of the swing electric motor (9b) and the expansion / contraction electric motor (9c) per unit time is
ty ₁ ) and (Duty ₂ ) are calculated for each set time (Δt), and the servo motors are driven so as to drive the electric motors (9b) and (9c) with the calculated driving amounts. A command is given to the controller (18) to linearly move the work hand (H) toward the harvest target fruit (F).

The proximity sensor (S ₂ ) is activated and the fruit to be harvested (F)
With the detection of approaching within the set distance, the suction operation of the vacuum pad (10) is started and the harvesting operation is started.

However, even if the arm (8) is extended to reach a preset distance, if the proximity sensor (S ₂ ) is not ON, it is determined that the harvesting is impossible, 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
By switching from y ₁ ) and (Duty ₂ ) to the target drive amount for stopping, 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 ₁ ),
(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 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, based on the value of the luminance signal (Y) in the image signal output from the image sensor (S ₁ ), the brightness in 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 shown in the FIG. 5 (c), when a plurality of one specific color area (F ₁₎ the set area (F ₂₎ is extracted, the setting area (F ₂ ) Based on
The approach order is determined such that the larger the size, the more the approach is made first.

[Another embodiment]

In the above embodiment, the case where the center of gravity (F ₃ ) of the setting area (F ₂ ) where the brightness in the specific color area (F ₁ ) is higher than the setting is used as the position information of the fruit (F) is shown. but the and regions brightness in the specific color region (F ₁₎ is maximum, the specific color area (F ₁₎ can also be used centroid itself.

Further, in the above-mentioned embodiment, the case where the present invention is applied to the working machine for fruit harvesting is exemplified, but the specific configuration of each part necessary for carrying out the present invention can be variously changed.

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 expansion / contraction control device for an articulated arm of a harvesting machine according to the present invention. FIG. 1 is a schematic side view of the articulated arm, FIG. 2 is a block diagram of a control configuration, and FIG. (A) is a flow chart of the target drive amount calculation processing when the hand is moving forward, (B) is a flow chart of the target drive amount calculation processing when the hand is moving backward, FIG. 4 is a flow chart of control operation, and FIG. (C) is an explanatory view 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. (H) …… Working hand, (8b) …… First arm, (8
c) ... second arm, (9b), (9c) ... oscillation actuator, (100) ... target drive amount computing means, (101) ...
... Driving means, (102) ... Position detecting means.

Claims (1)

[Claims] 1. A second arm (8c) having a first arm (8b) which is vertically swingable and driven by a first actuator (9b) mounted on a moving body and having a working hand (H) at its tip. The second arm with respect to the tip of the first arm (8b).
The first actuator (9b) is pivotally supported by an actuator (9c) so as to be swingable up and down, and moves the working hand (H) along a linear set trajectory.
And a target drive amount calculation means (100) for obtaining a target drive amount per unit time for each of the second actuators (9c), and a target drive amount obtained by the target drive amount calculation means (100). And the first actuator (9
b) and a drive means (101) for driving each of the second actuator (9c), and the target drive amount calculation means (100) includes the first arm (8b) or the second arm (8c). ), The target drive amount for the actuator (9b or 9c) of the one arm having the larger drive load is set to a predetermined value, and the target drive amount of the actuator (9c or 9b) of the other arm is obtained. In the expansion and contraction control device for an articulated arm of a harvesting work machine, the position detection means (102) for detecting the position of the work hand (H) on the set trajectory is provided, and the target is provided. The drive amount calculation means (100) drives the target drive amount for the actuator (9b or 9c) of the one arm of the first arm (8b) or the second arm (8c) having a large drive load to the maximum drive amount. Set the amount and drive load Expansion and contraction of the articulated arm of the harvesting machine configured to correct the target drive amount of the actuator (9c or 9b) of the smaller arm on the other side based on the detection information of the position detection means (102). Control device.