JPH02152778A - Control device for expansion/contruction of multiple articulated arm - Google Patents

Abstract

PURPOSE:To shorten the time spent until a working hand reaches a work object by providing a target driving amt. arithmetic means for setting the target driving amt. for the respective 1st, 2nd actuator to the maximum drivable driving amt. in the initial moving time of the working hand. CONSTITUTION:The target driving amt. for the respective 1st actuator 9b for rocking a 1st arm vertically and 2nd actuator 9c for rocking a 2nd arm vertically with a working hand is set to the drivable maximum driving amt. by a target driving amt. arithmetic means 100, until the working hand is moved for set distance from its initial position. As a result, the acceleration force of the working hand at the movement start time can be made max. and the working hand can reach a work object in short time.

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 pivotally supported by a second actuator to the tip of the first arm so as to be able to swing vertically, and the working hand moves along a linear set locus. A target drive amount calculating means for calculating a target drive amount per unit time for each of the actuators at each set time, and a drive means for driving each of the first actuator and the second actuator based on the target drive amount. The present invention relates to an extension/contraction control device for a multi-jointed 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 as the target drive amount is limited by the maximum drive force so that the drive force of the actuator is less than or equal to the maximum drive force when the load is maximum.

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.

Incidentally, in this type of multi-joint type arm extension/contraction control device, changes in speed and fluctuations in the amount of movement per unit time of the working hand during movement are not often a problem.

Therefore, conventionally, the drive amount of the actuator on the side where the load is larger is set as large as possible, and the drive amount of the actuator on the side where the load is smaller is controlled, thereby suppressing the deviation in the set trajectory. By effectively utilizing the limited maximum part tbi, it was possible to move as fast as possible.
(See No. 4).

[Problem to be solved by the invention]

In the above conventional configuration, the target drive amount on the side where the load is smaller is set to a value less than the maximum drive amount that can be driven minus a control margin for feedback control that can correct deviations from the set trajectory. , it is not possible to set both the target drive amounts of the first and second actuators to the maximum drive amount that can be driven, and there is room for improvement.

By the way, it is rare that there is a work object near the initial position of the work hand, and therefore there is no need to guide the work hand without deviation from the set trajectory from the time of starting movement.

Therefore, in the vicinity of the initial position, even if both the first and second actuators are driven at the maximum drive amount, if the deviation from the set trajectory can be corrected before reaching the location where the work object is located, then the work This means that the hand movement speed can be further increased.

In particular, if the amount of drive at the start of movement can be increased, the initial acceleration force will be large, which is advantageous for speeding up.

The present invention has been made in view of the above-mentioned circumstances, and its purpose is to increase the moving speed of a working hand.

[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 first actuator and the second actuator are each pivotably supported by a second actuator to be able to swing up and down, and the working hand moves along a linear set locus. target drive amount calculation means for calculating a target drive amount per unit time for each set time, and drive means for driving each of the first actuator and the second actuator based on the target drive amount. Its characteristic structure is as follows.

That is, the target drive amount calculation means sets the target drive amount for each of the first actiniator and the second acticoator to the maximum drive possible until the work hand moves a set distance from its initial position. The point is that it is configured to set the amount.

[For production]

As mentioned above, when the work hand starts moving, it is not a problem even if the movement trajectory deviates from the set trajectory, so until the work hand moves the set distance from the initial position, each of the first actuator and the second actuator By setting the target drive amount to the maximum drive amount that can be driven, the acceleration force at the start of movement is maximized.

[Effects of the Invention] Therefore, since the acceleration force at the time of starting movement can be maximized, the time required for the working hand to reach the work object can be shortened.

Therefore, by rationally modifying the device, it has become possible to make maximum effective use of the driving force of the actuator and increase the moving speed of the working hand.

〔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. 6, 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.
An auxiliary boom (3) is attached to be able to swing freely in the horizontal direction, and a work manipulator (4) equipped with a work hand (H) for fruit harvesting is attached to the tip of the auxiliary boom (3). This constitutes a working machine for fruit harvesting. In the figure, (5) is a hydraulic cylinder for lifting the boom, (6) is a hydraulic cylinder for boom rotation, and (7) is a hydraulic cylinder for boom rotation.
is a hydraulic cylinder 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 6, the multi-jointed arm (
8) is a base end pace (8a) that is driven to rotate around the vertical axis (Y) by a swing electric motor (9a) at the tip of the auxiliary boom (3), and a base end base (8). 8a
), the first actuator is oscillated around the horizontal axis (X) by the oscillating electric motor (9b) as the first actuator.
An electric motor for extension and contraction (as a second actuator) is connected to the arm (8b) and the tip of the first arm (8b).
9c), and a second arm (8c) that is pivotably supported to be vertically swingable. The working hand (H) is driven to swing in conjunction with the swinging of the second arm (8c) by the telescopic electric motor (9c). attached to the tip.

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 working hand (H) moves between the initial position and the fruit to be harvested (F). The target driving amount (Dut
The arm (8) is extended and contracted by driving while determining yυ, (Dutyz) at every set time (Δt).

However, the first arm (8b) and the second arm (8c)
) are formed to have the same length (A), and the first arm (8b) 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).
), their pivot points are connected to the chain (10b) so that the swing angle (θ2) of are linked together.

Therefore, the working hand (l()) is moved along the set trajectory (L
), the first arm (8b
) The swing angle (θ2) of the second arm (8c) with respect to
The angle obtained by subtracting the swing angle (θ, ) of the first arm (8b) with respect to the vertical direction from the half of
), the target drive 1 (Duty
+) and (Duty 2) 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 (H), as shown in FIG.
) is provided, and the vacuum pad (11) is provided with a vacuum pad (11) that adsorbs the fruit (F), and the vacuum pad (11) is provided with a vacuum pad (11) that adsorbs the fruit (F). A trapping part case (13) that is externally supported so as to be retractable and retractable, and a handle cutting device (14) that cuts the fruit (F) taken into the trapping part case (13) while sandwiching it in the vertical direction. ). Although not detailed, the vacuum pad (11) includes:
An ejector pump (
(not shown) or the like uses a negative pressure generating device.

The catching part case (13) includes a main body part (13a) that is fitted onto the ventilation pipe (12), and a state in which the main body part (13a> is biased back toward the inside of the hand).
It also consists of a cover (13b) pivotally mounted in the vertical, horizontal, and horizontal directions, respectively.

However, although not described in detail, the cover (13b) is divided into four parts, vertically, horizontally, and horizontally, and an opening is formed on the front side of the hand through which the fruit absorbed by the vacuum pad (11) passes. . That is, when the fruit is taken into the trapping part case (13) through the opening, if the diameter of the fruit is larger than the opening, each of the four-divided covers opens toward the outside of the hand. This makes it possible to take in fruits that are larger in diameter than the opening. Of the four divided covers (13b), the lower one is configured to be opened downward by an actuator (not shown), and is opened inside the trap case (13). The fruit (F), whose handle has been cut and which is held in the container, can be discharged to the outside.

The handle cutting device (14) includes a clipper-shaped blade (14a) for cutting the handle of the fruit (F), and a blade (1).
4a) is provided with a handle support member (14b) that presses and supports the handle of the fruit (F) from below.

However, although not detailed, the working hand (H) includes the following:
Various actuators for harvesting operation, such as an actuator for moving the trapping part case (13) in and out, and an actuator for driving the handle cutting device (14);
a sensor for detecting that the fruit (F) has been adsorbed to the vacuum pad (11);
), and various sensors such as a sensor for detecting the driving state of the handle cutting device (14) are provided.

In FIG. 5, (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 capturing an image of the fruit (F) located in the direction in which the working hand (H) is facing. () I) is a proximity sensor that uses infrared light to detect when the target fruit approaches within a set distance, and both sensors (S, ) and (S2) are used for approach and fruit harvesting. It is provided inside the vacuum pad (11) so as not to get in the way. Further, (15) 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 working hand (H) facing the direction in which the fruit to be harvested is located, the image sensor (Sl) performs imaging processing while illuminating with the illumination device (15), and the image is captured. Based on the information, the working hand (
The approach direction of the work hand (H), that is, the direction of the set locus (L) for moving the working hand (H) 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 (F) is taken inside, the stalk is cut and harvested.

However, if there are multiple fruits within the field of view of the image sensor (S+), which fruit should be approached first is determined based on the brightness information of the image captured by the image sensor (Sl). , so that the approach order is automatically determined.

To explain the image processing, as shown in FIG. 3, the work hand 01) is set at a preset approach start position and image processing is performed, and the image information is image processed. , a specific color area (Fυ) corresponding to the color of the fruit (F) 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). Then, in order to extract the setting area (F2) with the largest size and make the working hand (H) approach the center of gravity (F) first, the working hand (H) The direction of the arm (8) is adjusted to extend the arm (8).

In other words, as shown in FIG. 4, the brightness of the image information is inversely proportional to the distance, and the closer the image information is, the brighter it becomes. Taking advantage of the fact that the size of the area (F2) becomes larger as the fruit is located closer, the fruit (F) that is located closer to the front and easier to harvest is approached first.

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 (S+)
), (B) to binarize 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 darkness value. The original image (third image) captured by the image sensor (S,)
A binarized image (see FIG. 3(b)) is obtained by extracting a specific color area (F,) corresponding to the color of the fruit (F) from the figure (see ri).

Then, based on the value of the luminance signal (Y) among the image signals output from the image sensor (S+), a setting area (F2) where the brightness in the specific color area (Fl) is equal to or higher than a setting value. At the same time, extract the setting area (F
2) The direction in which the center of gravity (F,) is located on the image is
This will be obtained as the position information of the fruit (F) (see Fig. 4).

However, as shown in FIG. 3 (c), if multiple setting areas (F2) are extracted within one specific color area (Fl), the thickness 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.

Next, a control configuration for guiding the working hand (H) toward the fruit to be worked on based on the imaging information of the image sensor (Sυ) will be described.

゛As shown in Fig. 2, the image information captured by the image sensor (S,) is image-processed to produce the fruit (F) to be harvested.
The working hand (H) is configured to determine the direction in which the fruit is located and to determine the approach order, and in the determined approach order, moves the working hand (H) to the fruit to be harvested (
F), the boom lifting hydraulic cylinder (5), the boom turning hydraulic cylinder (6),
the auxiliary boom swinging hydraulic cylinder (7), each electric motor (9a), (9b) for driving the arm (8);
(9c), and a control device (16) using a micro combinator that controls the operation of each of the electric motors (9a), (9c) for driving the arm (8) based on instructions from the control device (16). A servo controller (17) is provided to drive each of 9b) and 9c.

In other words, by using the control device (16), the swinging electric motor (9a), the swinging electric motor a motor (9b), and the telescopic electric motor (
9c) 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 (17) is configured to calculate the target drive amount calculation means (100) for each unit time. 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
), the base end base (8a) and each arm (
8b).

(8c) An encoder (eo
), (e,), and (F2), and the servo controller (17) basically performs the following based on the detection information of these encoders (e,), (e,), and (e,). The base end base (8a) and each arm (8b),
The drive amount is feedback-controlled so that the swing angles (θ.), (θ, ), (θ, ) of (8c) are at positions corresponding to the target drive amount.

However, each of the electric motors (9a), (9c), (9
c) is configured such that the driving amount is adjusted by PWM modulating the supply voltage for driving, and each target driving amount (Outya), (out!/+), (Dut
y, ) are set as duty ratio values in PWM modulation. That is, the servo controller (17) operates as the target drive amount calculation means (10).
0) is the target drive amount (Duty, ).

The supply voltage (V, ) is PWM modulated so that (mouth), <mouth, ) is applied to each electric motor (9a).
), (9c).

(9c) will be driven.

The arm rotation electric motor (9a) for changing the horizontal direction of the work hand (H) is connected to the swing motor (9b) and the telescopic electric motor (9).
The driving load does not change greatly as in c), and by the time the working hand (H) reaches the fruit to be harvested,
It is sufficient if the direction can be adjusted to the direction in which the fruit to be harvested is located. Therefore, there is no need to drive them at high speed like the swing motor (9b) and the telescopic electric motor (9c), so a description of the structure for determining the target drive Tom will be omitted.

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

(Dut, yz)
) 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 (V,) is, for example, the second arm (
Target drive fl(
Taking the example of torque (τ) and
Based on the angular velocity (θ, ), from equation (i) below, K
T is the torque constant, r is the armature resistance of the motor, and R is the reduction ratio.

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

r=J2+” θI+J22・θ2+G 2+V
2- θ2 -- (ii) However, v2 is the viscous drag coefficient in the second arm (8c).

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

a = 2A-sin (θ2/2) +8 ”...(i
v) η=θ2/2-θ, ......(v)
However, since the change in the elevation angle (η) is small, the acceleration (θ,
) can be set as θ1=θ2/2.

Therefore, the first and second (θl) after the set time (Δt)
, and the angular acceleration (vi), the following (v
i) It can be obtained using the formula.

θi=θlo + (θi〇to)・Δt/2−・・
...(vi) Therefore, the previous target drive amount (Dutyz)
The predicted angular velocity (θ, ) corresponding to can be determined based on the following equation (vii).

・・・・・・(vii) However, D, = G2-Dutyz C2=V2+motor back electromotive force coefficient θ2゜=initial value It is.

In other words, the target drive fl(Duty+), (Dut
y, ) is the current position of the arm, that is, the 17th-
The values of the current swing angle (θ+ (nt) + (θ2 (n)) of each arm (8b) and the second arm (8c) and the swing angle (θ1(.) after the set time (Δt)) ,)),(
From the difference from θ2(1°+)), the above (i) to (vii
) can be obtained using the formula.

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 fully extended state to retire the working hand (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 amount for the electric motor on the side where the load is smaller, drive energy can be used effectively, so the overall movement speed of the working hand (H) can be increased. Become.

Therefore, the target drive amount of the electric motor on the side with a larger load, that is, the duty ratio in the PWM modulation, is set to the maximum value corresponding to the maximum drive amount that can be driven, and the target drive amount of the electric motor on the side with a smaller load is set to the maximum value corresponding to the maximum drive amount that can be driven. The target drive amount is controlled based on the drive result.

However, since there is no work target while the working hand (H) moves the set distance (200+11 [11) from its initial position, the movement trajectory during that time may deviate somewhat from the set trajectory (L). This will not affect the guidance accuracy.

Therefore, while the working hand (H) moves a set distance (200 mm) from its initial position, both the electric motors (9b) and (9c) are driven at the target level. (Duty+).

(Duty 2) is set to the maximum driveable value so that the acceleration can be maximized.

Incidentally, in reality, the servo controller (17) controls each of the encoders (e,
), (e=), feedback control is performed to correct the deviation from the set trajectory (L), so each electric motor (9b), (9c ) can be driven (1
00%), the target drive ff1(D
The maximum value of ut)'+), (Duty2) is set to a value (for example, 120%) that is the sum of the maximum drive amount (100%) and the control margin of the feedback control.

In the case where both the electric motors (9b) and (9c) are driven at the maximum driving amount, especially when the working hand ()l) moves forward, the first arm (8b)
) is driven to swing downward under the action of gravity, so the gravity acting on the first arm (8b) can also be effectively used to drive at high speed, thereby increasing the movement speed of the working hand (H). (See Figure 7).

After the working hand (H) has moved the set distance (200 mm) from its initial position, as described above, the target drive amount on the side with the larger load is set to the maximum value, and the set time is (Δt), the target drive amount for the electric motor on the side where the load is smaller is determined while being corrected based on the current position of the arm for each set time (Δt).

By the way, when the hand is retracting, 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 ff1 (Duty+), (outy
Each time z) is determined, the magnitude is compared, and the drive amount with the larger value is set to be larger.

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, each of the target drive amounts (Duty+) and (Dutyz) is switched to a value for stopping, thereby stopping the work hand (H).

Further, although not described in detail, each of the target driving amounts for stopping is set such that the value of the angular velocity (Gi) is zero, and the value of the angular acceleration (Gi) is set to the value of the eleventh second arm (8b). ),
By setting the driving direction for each of (8c) to a value (-〇5./Δt), the above (i) to (v
ii) Using the formula, the target part iJ+1t(D
duty+) and (Dutyz).

By the way, in order to obtain the target drive amount (Duty), the swing angle (Gi) and gravity (Gi) of each arm are determined based on the information of the encoders (eo), (e+), (e,).
) needs to be calculated based on the following equations (vii) and (ix), for example.

θ1=Ki xCNTi −−・−(vii
i) Gi =Mix g xsin(Gi) ・-
--(ix) However, Ki is a constant, Mi is the mass of each arm, and g is gravity.

However, the information output from each encoder is usually a real value. Therefore, in order to obtain the value of the gravity (Gi), calculations including trigonometric functions are performed, which increases the calculation time and increases the target drive amount (Duty).
) may become longer than the set time interval (Δt).

If the interval between the set times (Δt) is long, the operation of the telescoping arm (8) cannot be controlled at real time intervals (
As a result, there is a risk that control accuracy may deteriorate.

In order to perform high-speed calculations, it is conceivable to install a dedicated numerical calculation processor, for example, but this has the disadvantage of making the device configuration complicated and expensive.

Therefore, although it will not be described in detail, the trigonometric function value (sin(G
i)) is calculated and made into a table, and the control device (16)
Then, the output value of each encoder is stored using the value obtained by adding the start address value of the table and 1/2" of the output value (CNTi) of each encoder as the address value for reading the table. The value (sin(Gi)) of the trigonometric function can be directly read from (CNTi).

[Another example]

In the above example, the case where the center of gravity (F3) 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) is exemplified. It is also possible to use the area where the brightness is maximum within the specific color area (Fl) or the center of gravity of the specific color area (Fl) itself.

Further, in the above embodiment, the present invention is applied to a fruit harvesting machine, but the present invention can also be applied to various types of machines other than fruit 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 expansion/contraction control device for a multi-joint arm according to the present invention, with FIG. 1 being a schematic side view of the multi-joint arm, FIG. 2 being a block diagram of the control configuration, and FIGS. (c)
4 is an explanatory diagram of image processing, FIG. 5 is a sectional view showing the schematic configuration of the working hand, FIG. 6 is an overall side view of the working machine, and FIG. 7 is an explanatory diagram of the movement trajectory of the working hand. It is. (H)...Working hand, (8b)...
・First arm, (8c)...Second arm, (9
b)...First actuator, (9c)...
...Second actuator, (100)...
Target drive amount calculation means (101)... Drive means.

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 connected to the first arm (8b). The second actuator (9c
) to each of the first actuator (9b) and the second actuator (9c) so that the working hand (H) moves along a linear set locus. Target drive amount calculation means (100
) and a drive means (101) for driving each of the first actuator (9b) and the second actuator (9c) based on the target drive amount. The target drive amount calculating means (100) controls each of the first actuator (9b) and the second actuator (9c) until the working hand (H) moves a set distance from its initial position. An extension/contraction control device for an articulated arm configured to set a target drive amount to a maximum drive amount.