JPH0271989A - Control device for expansion of artuculated arm - Google Patents

Abstract

PURPOSE:To improve the moving speed of a working hand by setting the aiming drive quantity for the actuator of the arm of a bigger driving load side in the max. driving quantity and providing a means for correcting the aiming drive quantity based on a hand position detection value by the actuator of smaller side. CONSTITUTION:A position detection means 102 detecting the present position of the working hand located on a set locus is provided. Moreover, the aiming drive quantity for the actuator 9b or 9c of the arm of one part side whose driving load is larger of a 1st arm or 2nd arm is set by an aiming drive quantity arithmetic means 100. At the same time, the aiming drive quantity of the actuator 9c or 9b of the arm of the other side whose driving load is smaller is corrected by the arithmetic means 100 based on the detection information of a position detection means 102. As a result, the actuator 9b or 9c of the arm of a larger load side is driven in the max. drive quantity with constraining the generation of slippage to the set locus of the working hand and the moving speed of the hand can be increased.

Description

DETAILED DESCRIPTION OF THE INVENTION [Industrial Application Field] The present invention provides a first arm that can be driven vertically by a first actuator, and a second arm that has a working hand at its tip. The first actuator and the second actuator are pivotably supported by a second actuator to the tip of the first arm so as to be able to swing vertically, and move the working hand along a linear set locus. target drive amount calculation means for calculating a target drive amount per unit time for each of the first actuator and the second actinator at each set time; A drive means for driving is provided, and the target drive amount calculation means includes:
A target drive amount for the actuator of one arm of the first arm or the second arm with a large drive load is set to a predetermined value, and a target drive amount of the actuator of the other arm is determined. This invention relates to an extension/contraction control device for an articulated arm.

[Conventional technology]

In the above-mentioned type of multi-joint arm extension control device,
Since there is a limit to the maximum driving force of the actuator,
The maximum value that can be set for the target driving amount per unit time of the actuator is limited by the maximum driving force of the actuator so that the driving force of the actuator is equal to or less than the maximum driving force at the time of maximum load.

In order to increase the moving speed of the working hand, a large actuator with a large maximum driving force may be used, but this has the disadvantage that the device becomes expensive.

However, in this type of multi-jointed arm extension/contraction control device, changes in speed or fluctuations in the amount of movement per unit time of the work hand during movement are often not a problem, so By setting the target drive amount for the actuator of one arm, which has a large drive load, to a predetermined value and then finding the target drive amount of the actuator of the other arm, the movement speed of the work hand can be adjusted. I can make holes as much as possible.

To explain, for example, when the arm is extended from a state in which the entire arm is retracted and the working hand is made to protrude in a direction that resists gravity, the second arm is attached to the tip of the first arm, and Since the second arm is swung in a direction that resists gravity, the load on the actuator of the second arm is greater than the load on the actuator of the first arm. When retracting the arm and retiring the working hand, the first arm is swung in a direction that resists gravity, so the load on the actuator of the first arm is greater than the load on the actuator of the second arm. However, the load on one of the actuators is lighter than the load on the other actuator.

Therefore, by setting the target drive amount of the actuator on the side with a larger load to a predetermined value and determining the target drive amount of the actuator on the side with a smaller load, the driving force of the actuator on the side with a larger load can be minimized. Make it a hole,
This makes it possible to shorten the time required for the working hand to reach the target position 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 corresponding to the target drive amount, the target drive l of the actuator on the side where the load is larger is determined by position feedback from the maximum drive amount of the actuator. It was set to a value less than the value excluding the control margin for control.

[Problem to be solved by the invention]

Therefore, in the conventional configuration, the target drive amount of the actuator on the side with a larger load cannot be set to the maximum drive amount of the actuator, so the entire maximum drive amount of the actuator cannot be used, and an improvement has been desired.

The present invention has been made in view of the above-mentioned circumstances, and its purpose is to enable the actuator on the side with a larger load to be driven with the maximum drive amount of the actuator.

[Means to solve the problem]

The extension/contraction control device for a multi-jointed arm according to the present invention is provided with a first arm that can be vertically oscillated by a first actuator, and a second arm having a working hand at its distal end. The working no.
a target drive amount calculation means for calculating a target drive amount per unit time for each of the first actuator and the second actuator at each set time so as to move the actuator along a linear set locus; drive means for driving each of the first actuator and the second actuator with the target drive amount determined by the drive amount calculation means; The target driving amount for the actuator of one arm of the arms with a large driving load is set to a predetermined value, and the target driving amount of the actuator of the other arm is determined, Its characteristic structure is as follows.

That is, a position detecting means for detecting the position of the working hand on the set trajectory is provided, and the target drive amount calculating means is configured to detect the position of the working hand on the set trajectory, and the target drive amount calculating means is configured to detect the position of the working hand on the set trajectory, and the target drive amount calculating means is configured to detect the position of the working hand on the set trajectory. The target drive amount for the actuator of the arm on the other side is set to the maximum drive amount, and the target drive amount of the actuator of the other arm, which has a smaller drive load, is corrected based on the detection information of the position detection means. The point is that

[For production]

Since the movement trajectory of the work hand is determined by the operating positions of both the first arm and the second arm, the load will be small even if the operating position of the actuator on the side where the load is lower cannot be controlled by feedbank. By correcting the target operating position of the side actuator based on the current position of the working hand at each set time, it is possible to correct the deviation of the working hand from the set trajectory.

Therefore, the position of the working node on the set trajectory is detected, and based on the detected position information, the target drive amount of the actuator of the other arm, which has a smaller drive load, is corrected at each set time. It is driven while doing so.

〔Effect of the invention〕

Therefore, by rationally modifying the equipment, it is possible to drive the actuator on the side with a larger load at the maximum drive amount while suppressing the occurrence of deviation from the set locus of the working hand).
The movement speed of the working hand has now been improved.

〔Example〕

EMBODIMENT OF THE INVENTION Hereinafter, an embodiment in which the present invention is applied to a working machine for fruit harvesting will be described based on the drawings.

As shown in FIG. 7, a boom (2) is attached to a vehicle body (V) equipped with a pair of left and right running wheels (1) at the front and rear so as to be able to rise and fall as well as turn freely, and at the tip of the boom (2), An auxiliary boom (3) is attached to be able to swing freely in the horizontal direction,
A working manipulator (4) equipped with a working hand (H) for fruit harvesting is attached to the tip of the auxiliary boom (3), thereby forming a working machine for fruit harvesting.

In the figure, (5) is the hydraulic cylinder for lifting the boom, (6)
(7) is a hydraulic cylinder for boom rotation, and (7) is an electric motor for swinging the auxiliary boom.

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

As shown in FIGS. 1 and 7, the multi-jointed arm (
8) is a base end base (8a) that is driven to rotate around the vertical axis (Y) by a rotating electric motor (9a) at the tip of the auxiliary boom (3), and 8a
), the first arm (8b) is driven to swing around the horizontal axis (X) by the swinging electric motor (9b) as a swinging actuator, and the tip of the first arm (8b) is On the other hand, the second motor is pivotally supported by an electric motor for expansion and contraction (9c) serving as a swinging actuator so as to be able to swing vertically.
It consists of an arm (8c). In addition, the above-mentioned work node (
l() is attached to the tip of the second arm (8c) so as to be driven to swing in conjunction with the swinging of the second arm (8c) by the telescopic electric motor (9c). There is.

Then, with a state in which the first arm (8b) and the second arm (8c) substantially overlap in side view as an initial position, the work hunter (H) moves between the initial position and the fruit to be harvested (F). The target driving amount (Dut
y+), (The arm (8) is extended and contracted by driving the arm (8) while determining the opening (uty2) at every set time (Δt).

However, the first arm (8b) and the second arm (8c)
) is formed to have the same length (A), and the first arm (8
b) is directly driven to swing by the swinging electric motor (9b). On the other hand, the second arm (8c) is connected to the base end of the first arm (8b) by a chain (10a) interlockingly connected to the telescopic electric motor (9c) attached to the base end side of the first arm (8b). is configured to rotate at the tip of the first arm (8b). And the working hand (H)
The second arm (8c) is moved in conjunction with the swinging of the second arm (8c) by the electric motor for expansion and contraction (9c).
The second arm (
The pivot points are interlocked and connected by a chain (10b) so that the swinging force is swung in a direction opposite to the swinging force direction of 8c).

Therefore, the working hand (H) is moved along the set trajectory (L).
In order to move along the first arm (8
The swing angle (θ2) of the second arm (8c) with respect to b)
The angle obtained by subtracting the swing angle (θ1) of the first arm (8b) with respect to the vertical direction from half of the set trajectory (
In order to match the elevation angle (η) with respect to the horizontal direction of
A target drive amount per unit time (Dut
y+) and (Duty2) are set.

A control configuration for controlling the driving of the swing electric motor (9b) and the telescopic electric motor (9c) will be described later.

To explain the working hand (I), as shown in FIG.
A vacuum pad (11) for adsorbing F) is provided,
Fruit (F) adsorbed to the vacuum pad (11)
A trapping part case (13) is externally supported to be movable in the front and rear directions with respect to the ventilation pipe (12) of the vacuum pad (11) so as to cover the vent pipe (12), and a trapping part case (13) that is taken into the trapping part case (13) It is equipped with a delivery cutoff device (14) that cuts the fruit (F) while holding the handle part in the vertical direction.

The ventilation pipe (11) is a bellows type hose (15).
(See FIG. 7), the piping is connected to a suction pump (not shown) mounted on the vehicle body (V) side.

The trapping part case (13) includes an upper case (
13b), and a lower case (13c) which is pivotally connected to the support member (13a) so as to be swingable around the vertical axis.
It is divided and formed.

That is, the lower case (13C) is
By swinging the fruit (F) in the lateral direction with respect to 2), the fruit (F) whose handle has been cut and which is held within the trapping part case (13) can be discharged to the outside.

Further, the delivery deadline cutting device (14) includes a clipper-shaped blade (14a) for cutting the handle of the fruit (F), and a clipper-shaped blade (14a) that presses the handle of the fruit (F) from below. A supporting member (14b) for supporting the handle is provided.

However, although not shown, the working hand (H) includes an actuator for moving the catching part case (13) in and out, an actuator for driving the delivery deadline device (14), etc. There are various actuators for actuation and fruit (F) on the vacuum pad (11).
), a sensor for detecting the egress/egress position of the trapping part case (13), a sensor for detecting the driving state of the delivery deadline interrupting device (14), etc. Various sensors will be provided.

In FIG. 6, (Sl) is a color image sensor serving as an imaging means for capturing an image of the fruit (F) located in the direction in which the working hand (H) is facing, and (S2) is a color image sensor for photographing the fruit (F) located in the direction in which the working hand (H) is facing. (H) is a proximity sensor using infrared light that detects when the fruit approaches the fruit to be harvested within a set distance,
Both sensors (s+) and (S2) are provided inside the vacuum pad 11) so as not to interfere with approach and fruit harvesting. Further, (16) is an illumination device for illuminating the front side of the working hand (H) with a set light amount in synchronization with the imaging process of the image sensor (Sl).

Then, with the work hand (H) facing the direction in which the fruit to be harvested is located, and while being illuminated by the illumination device (16), 1. the image sensor (Sl) performs an imaging process and images the image. Based on the image information, the approach direction of the working hand (H), that is, the working hand (H)
The direction of the set locus (L) for moving is determined.

After determining the approach direction, the work hand (H
) so that each of the electric motors (9a) expands and contracts in the determined approach direction.

(9b) and (9c) are activated, and as the proximity sensor (S2) detects the approach to the fruit, the vacuum pad (11) is activated to suck the fruit (F) to be harvested. , and the trapping part case (13)
After the fruit (P) is taken inside, the stalk is cut and harvested.

However, if there are multiple fruits within the imaging field of the image sensor (Sl), which fruit should be approached first is determined based on brightness information of the image captured by the image sensor (Sl). , so that 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 image information captured by the image sensor (Sl) will be described.

As shown in FIG. 2, each means is configured to process the image information captured by the image sensor (Sl) to determine the direction in which the fruit to be harvested (F) is located, and to determine the approach order. Then, in the determined approach order, move the working hand (H) to the fruit to be harvested (F
), the hydraulic cylinder for lifting and lowering the boom (5), the hydraulic cylinder for swinging the boom (6), the auxiliary boom swinging motor (7), and each electric motor for driving the arm (8). (9a), (9b), (9c)
A control device (17) using a microcomputer controls the operation of each of the electric motors (9a>) for driving the arm (8) based on instructions from the control device (17).
.

A servo controller (18) is provided to drive each of (9b) and (9c).

In other words, by using the control device (17), the swinging electric motor (9a) and the swinging electric a motor (9b), and the telescopic electric motor (
9c) A target drive amount calculation means (100) is configured to calculate the target drive amount per unit time for each set time, and the servo controller (18) is configured to calculate the target drive amount calculation means ( drive means (101) for driving each of the swing electric motor (9b) and the telescopic electric motor (9c) with the target drive amount determined in step 100);
).

In addition, each of the electric motors (9a), (9c), (9c
) are equipped with encoders (eo), (e,), for detecting the swing angle of each arm (8b), (8c), respectively.
(e2) is attached, and the servo controller (18)
are basically those encoders (eo>, (e,
), (e=), each arm (
8b), (8c) swing angle (θ0), (θ1), (
The drive amount is feedback-controlled so that θ2) becomes a position corresponding to the target drive amount.

However, each of the electric motors (9a), (9c), (9
c) is configured such that the drive amount is adjusted by PWM modulating the supply voltage for drive, and each target drive 1t(tlutya), (Duty+), (D
uty2) is set as a duty ratio value in PWM modulation. In other words, the servo controller (18) has the target drive amount calculation means (
Target drive ff1 (Duty
o).

(Duty+), (Duty2), the supply voltage (v8) is PWM-modulated to each electric motor (9a).
), (9c).

(9c) will be driven.

The arm turning electric motor (9a) is stopped after the working hand (H) is horizontally directed in the direction in which the fruit to be harvested is located at the approach start position. Therefore, unlike the swing motor (9b) and the telescoping electric motor (9c), there is no need to control the drive amount, so a description of the configuration for determining the target drive amount will be omitted.

Next, target drive amounts (Outy+) of the swing electric motor (9b) and the telescopic electric motor (9c).

(Duty 2) Target drive amount calculation means (1,00
) will be explained.

Target drive amount of each of the electric motors (9b) and (9c),
That is, the duty ratio (Duty) for PWM modulating the supply voltage (v8) is calculated from the following equation (i) based on the torque (τ) and the angular velocity (θ). However, K
T is the torque constant, r is the armature resistance of the motor, and R is the reduction ratio.

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), respectively. , can be determined, so each of the electric motors (9b), (9c)
Each target drive amount (Duty+), (DljjY2)
can be rewritten as shown in formulas (ii) and (in) below.

Duty I =, L l'θ++J+2・θ2+cl
-θ, +G,...group (i+)Duty2=J2+”θ
1+J2□・θ2+C2・θ2+G2. Old - (iii
) However, θ1 is the acceleration, vl is the angular velocity, and C1 is the sum of the viscous resistance and the back electromotive force coefficient of the motor.

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

(7=2A-sin(θ2/2)-'-,B-・-
1iv) η=θ2/2-θ1 ・・・・・・(
v) However, since the change in the elevation angle (η) is small,
By differentiating the above equation (v) twice, the acceleration (
θ, ) can be set as θ1=θ2/2.

Therefore, the first and second arms (
The target swing angle (θI) of 8a) and (8c) is the angular velocity (
θ1) and angular acceleration (vl) using the following equation (vi).

θl=θ1o+(θl〇,.)・Δt/2−・・・・
・(vl) However, θ1=(θ, o+0/C)exp(-CΔt/J) −
〇/Cθi = -C/J・(θi o + D/'C)
exp (-Δt/J)D=Gi-Dutyl C=C1 J=Jt+/2+Jtz.

Note that θ1o is an initial value.

In other words, the target drive t(Duty+), (Duty
Each of 2) is the current position of the arm, that is, the value of the current swing angle (θ1(n))+(θ2(h)) of each of the first arm (8b) and the second arm (8c), Swing angle (θ1(.+1)), (θ2(,
,. 3. ), it can be determined using equations (i) to (vi) above.

By the way, when the arm is extended from the state where the entire arm is retracted and the working hand (H) is made to protrude in a direction that resists gravity (when moving forward), the second arm (8c) is moved from the first arm ( 8b), and
Since the second arm (8c) is to be swung in a direction that resists gravity, the load on the telescopic electric motor (9c) is greater than the load on the oscillating electric motor (9b), On the other hand, when retracting the arm from the state in which the entire arm is extended and retracting the working arm (H) (when retreating), the first arm (8b) is swung in a direction that resists gravity. Therefore, the load on the swing electric motor (9b) is greater than the load on the telescoping electric motor (9c).

In other words, during the expansion and contraction operation, the load on one of the electric motors is lighter than the load on the other electric motor, so the target drive amount for the electric motor with the larger load is as large as possible. By controlling the target drive for the electric motor with the smaller load so that the drive energy can be effectively used, the overall moving speed of the working node (H) can be increased. It is possible to convert

Therefore, the target drive amount of the electric motor on the side where the load is larger, that is, the value of the duty ratio in the PWM modulation, is
In addition to setting the target drive amount to a constant value of 100% corresponding to the large drive pot, at each set time (Δt), the target drive amount for the electric motor on the side with a smaller load is set to the current value of the arm at each set time (Δt). This is done by making corrections based on the position.

However, when the hand is retracted, the magnitude relationship 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, the target drive 1 (Duty+), (
Each time each of Duty2) is calculated, the magnitude is compared and the larger value is set to the maximum value (100%).

When the hand moves forward, a stop command is given as the proximity sensor (S2) turns ON, and when the hand moves backward, a stop command is given as the hand returns to its initial position. When a stop command is given, the target drive amount (Duty+), (Duty2) is switched to a value for stopping, and the working hand (1()) is stopped.

Further, although not described in detail, each of the target drive amounts for stopping is set such that the value of the angular velocity (θ1) is zero, and the value of the angular acceleration (θ1) is set to the value of the first and second arms ( 8b),
Values (-θ,
. /Δt), the above (1) to (v
i) Using the formula, the target drive amount for the operation (Duty +
), (Duty2) can be obtained in the same manner as in the case of obtaining.

Next, based on the flowcharts shown in Figures 3 (a) and (b), the target drive amount (D
A description will be added of a process for setting Uty (or Duty2) to the maximum value (100%) and determining the target drive amount of the electric motor with the smaller load.

However, both when the hand moves forward and when the hand moves backward, the load on the second arm (8c) is greater than the load on the first arm (8b).
), the target drive amount (Dutyz) at which the load is large is set to 100%, and the target drive amount (Dutyz) of the first arm (8b) is set over the set time (Dutyz). Δt).

First, based on the flowchart shown in Figure 3 (a),
The target drive amount calculation process when the hand moves forward will be explained.

That is, the control device (17) controls the set time (△
t), detection information of the encoder (el) for the arm swinging motor (9b) which is manually operated via the servo controller (1g), and detection information of the encoder (e2) for the arm extension/contraction motor (9c). Based on each of the detection information, the arm (8b), (8) has a value corresponding to the current position (n) of the working hand (H).
c) Current value of swing angle (θI (+,) ) + (θ2(
. )), and then, based on the values, inertial force (J (-) ) and gravity (G (h)) are determined.

That is, the current values (θ, (.)) and (θ2 (n)) of the swing angles of the arms (8b) and (8c) are calculated based on the detection information of the encoders (e,) and (e2). The processing is performed using the working hand (H) on the set trajectory.
This corresponds to the position detecting means (102>) for detecting the position of.

Next, from the above equation (iv), determine each of the target angular velocity (θ2′+1)) and target swing angle (θ2′+1)) of the second arm (8c) at the next target position, So.

The target swing angle (01', . . . ) of the first arm (8b) is determined from the value of the target swing angle (θ2Zn+11>).

And both target swing angles (θbi(..1)).

(θ2I(.++)), the inertia (J (gm +) ) after the set time (Δt) and the gravity (G <,
, -11), and from those values, the target angular velocity (θ2°(n.2
)) and the target swing angle (θ2'' (r++21)).

Next, from the obtained target swing angle (θ2'' [n+2)), the target swing angle (θ.(n+2)) of the first arm (8b) around the vertical axis (Y) and the 1 arm (8
Target swing angle (θ”(n+2) around the horizontal axis (X) of b)
)), and from those values, the target drive 1 (Dutyo) of the arm turning electric motor (9a) is determined.
and the target drive amount of the arm swinging electric motor (9b) (
Duty +), and calculate the electric power % for arm extension/contraction.
- Target drive amount of data (9c) (Duty 2 = 100%)
At the same time, the current value of the swing angle at the next target position (θl (r++ 1) ) + is passed to the servo controller (18) and is sent from the servo controller (18) to the next target drive amount calculation. (θ2(n+1))
After receiving each of them, "1" is added to the value of the variable (n>) for setting the target position.

To explain the target drive amount calculation process when the hand moves backward, as shown in FIG. While determining each of the swing angle (θbi(waya,)) and the target swing angle (02*(.+,)) of the second arm (8c), the obtained target swing angle (θl''(n . . .
), (θ2′(n, )), the inertial force (J.1)) and the gravity (G.1)) are each determined.

Next, using the above equation (iv), the inertial force (J
(,,41,) and the gravity (G (rl+ 11)
, the target angular velocity (θ2"(
. +2)) and target swing angle (θ2” (n+2))
, and the obtained target swing angle (02° (
n+2)), the target turning angle (θ.I(,.2)) around the vertical axis (Y) and the target swing angle (θ.2) around the horizontal axis (X). ).

degree (θ.′(,.2.), target swing angular velocity (θI(.+
2)), target swing angular acceleration (θ-i+2)), and target swing angular acceleration (θ2'(9)) of the second arm (8c)
, 2)), each of the electric motors (9a), (
9b) Target drive amount (outyo), (Duty+)
are calculated and passed to the servo controller (18) together with the target drive amount (Dutyz□100%) of the second arm (8c), and for calculation of the next target drive amount, the servo controller ( 18) to the current value of the rocking angle after the set time (θ+(n++>L(θ2(
After receiving each of n+1)), "1" is added to the value of the target position setting variable (b).

Next, based on the flowchart shown in FIG. 4, while explaining the operation of the control device (17), a control configuration for guiding the work hand (H) in the direction where the fruit to be harvested is located will be explained. do.

First, image processing is performed with the work hand (H) set at a preset approach start position, and by image processing the image information, a specific color area (Fl) corresponding to the fruit (F) is created. ) and its specific color area (Fl
) where the brightness is higher than the set value (F2
) and the center of gravity (F3) of the setting area (F2) (see FIG. 5).

Then, based on the size of the set area (F2), the work hand (H) is extracted so that the work hand (H) approaches the center of gravity (F3) of the largest size. By adjusting the direction of (H), the arm (8) will be extended.

To explain, the brightness of the image information is inversely proportional to the distance, and the closer the image information is, the brighter it becomes. The closer the objects are located, the larger they become.

In other words, the fruit (F) located on the near side, which is easier to harvest, is approached first.

After the arm (8) starts to extend, the proximity sensor (S2) is activated as described above until it detects that the fruit (F) to be harvested has approached within the set distance. Each of the target drive amounts (Duty+) and (outy2) for the operation of the swinging electric motor (9b) and the telescoping electric motor (9c) per unit time is determined for each set time (Δt), and Each of the electric motors (9b) is driven by the determined drive amount.

(9c), a command is given to the servo controller (18) to move the working hand (H) in a straight line toward the fruit to be harvested.

When the proximity sensor (S2) turns ON and detects that the fruit approaches the target fruit within a set distance, the suction operation of the vacuum pad (10) is started and the harvesting operation is performed. .

However, even if the arm (8) is extended until it reaches a preset distance, the proximity sensor (S2) will not turn ON.
If it does not operate, it is determined that harvesting is not possible, and the arm (8) is retracted and returned to the approach starting position.

After starting the harvesting operation, the working hand (It)
Based on detection information of various sensors attached to
While moving back the set distance from the N activated position,
At the same time as re-imaging processing, the imaging information is image-processed,
After re-determining the approach direction of the working hand (H) and correcting the extension/contraction direction of the arm (8), the approach is made again.

By the way, even if the re-approach is made based on the image information that has been re-imaging processed, the proximity sensor (S2) is turned on while the arm (8) is extended by a preset distance.
If it does not operate, it will be determined that harvesting is not possible and the harvest will be returned to the approach starting position.

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

After discharging the harvested fruits, it is determined whether the work is completed or not based on the number of specific color areas (Fl) extracted from the image information first captured at the approach start position, and if the work is not completed. The work hand (I
Each process after the process of setting I) at the approach start position and imaging is repeated.

Although not described in detail, harvesting may fail even when re-approaching, but in that case, the re-imaging process is not performed and the harvesting is returned to the approach starting position.

In other words, when the proximity sensor (S2) is turned ON while the extension distance of the arm (8) reaches the set distance,
When the proximity sensor (S2) does not turn ON even when the arm (8) is extended to reach a set distance, or when a harvest failure is detected, the control device (17) sends a command to the servo controller (18). A stop command will be given to the

c. When a stop command is given, each of the electric motors (9b),
(9c) By switching the drive amount for the arm (8
) will stop the expansion/contraction operation.

To explain the image processing for extracting the specific color region (F,), the three primary color signals (R), (G) of the image signals output from the image sensor (Sl) are
), (B) by binarizing the signal obtained by subtracting the other color signals (G or B) from the red signal (R) containing the color component of the fruit (F) based on the set threshold value. , the current image (fifth image) captured by the image sensor (sl)
A binarized image (see Figure 5 (B)) in which a specific color area (F, ) corresponding to the color of the fruit (F) is extracted from the
(see).

Then, based on the value of the luminance signal (Y) among the image signals output from the image sensor (sl), a set area (F2) where the brightness in the specific color area (F,) is equal to or higher than a set value. ), and its setting area (F
2) The direction in which the center of gravity (F3) is located on the image is
This will be obtained as the position information of the fruit (F).

However, as shown in FIG. 5(c), if multiple setting areas (F2) are extracted within one specific color area (F,), the size of the setting area (F2) Based on this, the approach order is determined so that the larger the object, the earlier the object is approached.

[Another example]

In the above example, the case is illustrated in which the center of gravity (F,) of the setting area (F2) where the brightness in the specific color area (Fl) is equal to or higher than the setting value is used as the position information of the fruit (F). However, it is also possible to use the area where the brightness is maximum within the specific color area (F,) or the center of gravity of the specific color area (F,) itself.

Further, in the above embodiment, the present invention is applied to a working machine for harvesting fruit, but the present invention can also be applied to various working machines other than a harvesting machine, and the present invention can be applied to various working machines other than harvesting machines. The present invention can be applied to various devices that control the expansion and contraction of a multi-jointed arm so as to move a working hand along a set linear trajectory. The specific configuration of each part necessary to carry out the present invention can be changed in various ways.

Incidentally, although reference numerals are written in the claims section for convenient comparison with the drawings, the present invention is not limited to the structure shown in the accompanying drawings.

[Brief explanation of the drawing]

The drawings show an embodiment of the extension/contraction control device for a multi-joint arm according to the present invention, in which FIG. 1 is a schematic side view of the multi-joint arm, FIG. 2 is a block diagram of the control configuration, and FIG. A flowchart of the target drive amount calculation process when the hand advances, FIG. 4 is a flowchart of the target drive amount calculation process when the hand moves backward, and FIG.
FIGS. 5A to 5C are explanatory diagrams 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 the working machine. (H)...Working hand, (8b)...
・First arm, (8c)...Second arm, (9
b), (9c)... Swinging actuator,
(100)...Target drive amount calculation means, (101
)... Drive means, (102)... Position detection means. Figure 1■

Claims (1)

[Claims] A first arm (8b) is provided which can be driven to swing up and down by a first actuator (9b), and a second arm (8c) having a working hand (H) at its tip is attached to the first arm (9b). The second actuator (9c
) to each of the first actuator (9b) and the second actuator (9c) so as to move the working hand (H) along a linear set locus. Target drive amount calculation means (10
0) and drive means (101) for driving each of the first actuator (9b) and the second actuator (9c) with the target drive amount determined by the target drive amount calculation means (100). and the target drive amount calculation means (100) calculates a target drive for the actuator (9b or 9c) of the first arm (8b) or the second arm (8c), which has a larger drive load. A multi-joint arm extension/contraction control device configured to set a predetermined amount to a predetermined value and obtain a target drive amount of an actuator (9c or 9b) of the other arm, the device comprising: A position detection means (102) for detecting the position of the working hand (H) is provided, and the target drive amount calculation means (100) is configured to detect one of the first arm (8b) or the second arm (8c). Set the target drive amount for the actuator (9b or 9c) of one arm with a large drive load to the maximum drive amount, and set the target drive amount of the actuator (9c or 9b) of the other arm with a small drive load. , an extension/contraction control device for an articulated arm configured to perform correction based on detection information from the position detection means (102).