JPH07106551B2 - Arm controller - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Industrial field of application] The present invention provides a telescopic arm at the tip of a boom, and a working hand at the tip of the arm. An actuator that drives the arm to move with respect to the boom, and a high-speed movement of the working hand to move the working hand closer to the target object, and the working hand moves toward the target object as the target object approaches. The present invention relates to an arm control device provided with control means for controlling the actuator so as to move at a low speed.

[Conventional technology]

The arm control device as described above is used, for example, in a fruit harvesting device.

That is, in order to improve the work efficiency, it is desirable that the work hand can move as fast as possible. However, when the target object is captured, it is necessary to stop the work hand appropriately at a position where the target object is located so that the work hand does not collide with the target object and damage the target object.

Therefore, for example, by providing a proximity sensor or the like that detects that the work hand has approached the target object, the work hand is moved at high speed until the target object is sensed, the target object is sensed, and then decelerated to a low speed. I was trying to move it.

Conventionally, the proximity sensor or the like is designed to decelerate immediately after sensing the target object.

[Problems to be Solved by the Invention]

In the above-mentioned conventional technique, when the actuator is decelerated as it approaches the target object, the vibration of the work hand increases, which may cause a problem in the work such as capturing the target object.

In addition, decelerating the actuator can be considered as applying an impulse-like excitation force to the boom that is freely vibrating.

That is, when the movement direction of the work hand does not intersect with the swing axis of the boom, a torsion moment about the axis of the boom with respect to the boom occurs when the actuator is accelerated or decelerated, and the boom moves around the axis of the boom. A vibrating force is generated to vibrate. Further, after the exciting force ceases to act on the boom that once started to vibrate, it will freely vibrate at the frequency unique to the boom vibrating system.

By the way, when applying a vibration force to a boom that is freely vibrating, if the direction of vibration and the direction of vibration force are the same, the energy of vibration becomes large, while the direction of vibration and the direction of vibration force When is opposite, the energy of vibration is small.

Therefore, when decelerating immediately after the proximity sensor or the like senses the target object as in the prior art, the vibration after deceleration may have a larger amplitude than the vibration before deceleration.

An object of the present invention is to eliminate the above-mentioned conventional drawbacks and reduce vibration of the working hand when approaching a target.

[Means for Solving the Problems]

In order to achieve this object, the arm controller according to the present invention is characterized in that phase detecting means for detecting the phase of vibration centered on the base end portion of the boom is provided, and the control means is the phase detecting means. It is configured to change the timing of shifting the actuator from the high speed movement to the low speed movement in order to reduce the vibration based on the detection information.

[Work]

The direction of vibration of the boom can be detected by detecting the phase.

When the direction of the vibration is opposite to the direction of the exciting force due to the deceleration, the deceleration can reduce the vibration energy. When the direction of the vibration and the direction of the exciting force are the same, the deceleration may be performed after the directions are reversed.

By the way, the energy of vibration is the sum of kinetic energy and potential energy. Generally maximum kinetic energy,
That is, when the potential energy is zero, it is considered that applying an exciting force by deceleration is most effective in reducing the energy of vibration and is desirable.

〔The invention's effect〕

The vibration of the work band can be reduced when the work hand is moved toward the target at a low speed. As a result, it is possible to obtain an arm control device with excellent workability that can smoothly perform work such as capturing a target object.

〔Example〕

Hereinafter, an example in the case where the present invention is applied to a working machine for fruit harvesting will be described with reference to the drawings.

As shown in FIG. 4, a boom (2) is movably attached to a vehicle body (V) having a pair of left and right traveling wheels (1) at the front and rear, and an auxiliary boom is attached to the tip of the boom (2). (3) is swingably mounted in the horizontal direction, and
A working manipulator (4) equipped with a working hand (H) for fruit harvesting is attached to the tip of the auxiliary boom (3), thus constituting a working machine for fruit harvesting. In the figure, (5) is a boom lifting hydraulic cylinder,
(7) is an auxiliary boom swing hydraulic cylinder.

The working manipulator (4) comprises a telescopic articulated arm (8) and a working hand (H) attached to the tip of the arm (8).

As shown in FIGS. 2 and 3, the articulated arm (8) is rotated around the longitudinal axis (Y ₂ ) by the turning electric motor (9) at the tip of the auxiliary boom (3). A base portion base (8a) that is driven to rotate, and a swing electric motor (10) as an actuator for the base portion base (8a).
In a), the first arm (8b) is rockably driven around the horizontal axis (X), and the tip end of the arm (8b) is vertically rocked by the electric motor (10b) for expansion and contraction. It is composed of a second arm (8c) pivotably supported. That is, the approach direction (A) is determined by turning the base end base (8a), and the working hand (H) is moved by driving the first arm (8b) and the second arm (8c). The fruit (F) can be approached.

Here, the angles in FIGS. 2 and 3 will be described.

In FIG. 2, (θ ₂ ) is the swing angle of the first arm (8b) with respect to the vertical direction (hereinafter referred to as the first angle), and (θ ₃ ) is the first arm (8b) of the second arm (8c). (Η) is an elevation angle with respect to the horizontal direction in the approach direction (A), and (θ ₁ ) in FIG. 3 is an auxiliary boom (3) of the base (8a) of the base end portion. () Is the angle between the longitudinal direction of the boom (2) and the approach direction (A) in the horizontal plane.

When the working hand (H) approaches the fruit (F), the sum of the first angle (θ ₂ ) and the elevation angle (η) should be half of the second angle (θ ₃ ). In addition, both electric motors (10a) and (10b) are driven respectively. However, in the following description, it is assumed that the first angle (θ ₂ ) is half the second angle (θ ₃ ) ignoring the change in the elevation angle (η).

Further, in FIG. 3, (Y ₁ ) shows the vertical axis of the base end of the boom (2).

That is, as will be described later, a boom (2) excited by an exciting force generated by accelerating or decelerating the moving speed of the working hand (H) when the working hand (H) approaches the fruit (F). Of the vertical axis of the above (Y ₁ )
The displacement of the surrounding vibration is φ, and its phase is δ (see FIG. 1). That is, the phase (8) corresponds to the phase of vibration around the base end of the boom (2).

Explaining the work hand (H), as shown in FIG. 9, a vacuum pad (11) for adsorbing a fruit (F) as a work target is provided at a tip end thereof,
The ventilation pipe (1) of the vacuum pad (11) so as to cover the fruit (F) adsorbed to the vacuum pad (11).
2) A state in which the catch case (13) that is fitted and supported in the front-rear direction so as to be retractable in the front-rear direction and the handle part of the fruit (F) taken in the catch case (13) are vertically sandwiched. And a handle cutting device (14) for cutting with. Although not described in detail, the vacuum pad (11) is sucked by a negative pressure generating device (19) using an ejector pump (18) attached to the ventilation pipe (12) ( (See FIG. 5).

The capturing portion case (13) is a body portion (13a) fitted onto the ventilation pipe (12) and a state in which the body portion (13a) is urged to return toward the inner side of the hand. In addition, a cover (13
b) consists of.

However, although not described in detail, the cover (13b) is vertically and horizontally divided into four parts, and an opening through which the fruit adsorbed to the vacuum pad (11) passes is formed on the front side of the hand. . That is, when the fruit is taken into the catching 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 outer side of the hand. It is possible to take in fruits whose diameter is larger than the opening. The cover (13b) divided into four parts, which is located on the lower side, is constructed so as to be opened downward by an actuator (not shown), so that the inside of the capturing part case (13). The fruit (F) whose pattern is cut off can be discharged to the outside.

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

Further, as shown in FIG. 5, the working hand (H) is
A negative pressure sensor (S ₃ ) is provided to detect that the fruit (F) is adsorbed on the vacuum pad (11).

In FIG. 5, (S ₁ ) is a color type image sensor for picking up the fruit (F) located in the direction in which the working hand (H) is facing, and (S ₂ ) is the working hand (H). ) Is a proximity sensor that uses infrared light to detect that a target fruit is within a set distance. Both sensors (S ₁ ) and (S ₂ ) interfere with approach and fruit harvesting. It is provided inside the vacuum pad (11) so as not to be damaged. Further, (15) is an illumination device for illuminating the front side of the work hand (H) with a set light amount in synchronization with the image pickup processing of the image sensor (S ₁ ).

Then, while the working hand (H) is oriented 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 (15),
Based on the captured image information, the working hand (H)
The approach direction (A) is determined.

After determining the approach direction (A), both electric motors (10a) and (10b) are operated so that the work hand (H) expands and contracts in the determined approach direction (A). As 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
After the fruit (F) is taken into the catching case (13), the handle is 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.

If the image processing is further explained, FIG.
As shown in (b), the work 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 the color of the fruit (F). specific color area corresponding to the (F ₁₎ and, the set area where specific color area (F ₁₎ brightness within of its greater than or equal to a set value (F _2), the center of gravity (F ₃ of the set area (F ₂₎ ) And are extracted. And the setting area (F ₂ )
The center of gravity (F ₃ )
The arm (8) is extended by adjusting the orientation of the working hand (H) so that the working hand (H) approaches first.

That is, as shown in FIG. 8, the brightness of the image information becomes brighter as it is inversely proportional to the distance, so that the brightness becomes equal to or higher than the set value in the specific color area (F ₁ ). By taking advantage of the fact that the size of the set area (F ₂ ) becomes larger at closer distances, the easily harvestable fruit (F) located on the front side is approached first. .

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 captured original image by the image sensor (S ₁₎ (FIG. 7 (b) refer) from the binarized image obtained by extracting a specific color area (F ₁₎ corresponding to the color of the fruit (F) (No. 7 (see FIG. 7B).

Then, based on the value of the luminance signal (Y) of the image signal output from the image sensor (S ₁ ), the brightness within the specific color area (F ₁ ) is set to a setting area ( F ₂ ) is extracted, and the direction in which the center of gravity (F ₃ ) of the set area (F ₂ ) is located on the image is determined as the position information of the fruit (F) (see FIG. 8).

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

Next, a control configuration for approaching the work target fruit with the work hand (H) will be described.

As shown in FIG. 5, the imaged image information from the image sensor (S ₁ ) is image-processed to determine the direction in which the fruit (F) to be harvested is located, the order of approach is determined, and the determination is made. In order of the approach order, the boom lifting hydraulic cylinder (5), the auxiliary boom swinging hydraulic cylinder (7), and the arm are guided so as to guide the work hand (H) toward the fruit (F) to be harvested. A control device using a microcomputer (1) for controlling the operation of each of the driving electric motors (9), (10a), (10b) of (8)
6) and the electric motors (9), (10a), (1) for driving the arm (8) based on the command from the control device (16).
0b) and a servo controller (17) for driving each of them.

That is, using the control device (16), the working hand (H) is moved at high speed so as to approach the fruit (F) to be harvested, and the working hand (H) is operated as the fruit (F) is approached. A control means (100) for controlling the electric motors (10a), (10b) so as to move the hand (H) toward the fruit (F) at a low speed, and the vertical axis (Y) of the boom (2). ₁ ) Each of the phase detecting means (101) for detecting the phase (δ) of the vibration in the surroundings is configured.

Next, the procedure for harvesting the fruit (F) will be briefly described with reference to FIG.

First, the work hand (H) is imaged in a state where it is set at a preset approach start position, and the picked-up image information is subjected to image processing to obtain the work hand (H).
Determine the approach direction (A). Then, in order to extend the arm (8) in that direction (A), the proximal end base (8a) is swung by the turning electric motor (9), and the speed is set by the working hand (H) in advance. ( _H )
Both electric motors (10a), (10
b) will be driven. Then, the proximity sensor (S
Based on the information in ₂ ), it is determined whether the work hand (F) has approached the fruit (F) to be harvested to the front side of the set distance, and as the work hand (F) approaches the set distance, the vacuum pad While starting the absorbing operation of (11), the electric motors (10a) and (10b) are decelerated so that the working hand (H) moves at a low speed at a preset speed ( _L ).

However, the timing of decelerating from the high speed movement to the low speed movement is changed based on the detection information of the phase detecting means (101) as described later.

After the deceleration, when the negative pressure sensor (S ₃ ) is turned on and fruit adsorption is detected, the telescopic arm (8) is stopped and the harvesting operation is started.

After harvesting the fruits, the work hand (H) is returned to the approach start position in a state where the fruits whose patterns are cut are captured in the capturing part case (13), and the fruits (F) are discharged. .

After discharging the harvested fruits, it is determined whether or not the work is finished, and if the work is not finished, the approach direction (A) to the remaining fruits (F) is determined, and the determined approach direction (A ), Each process of guiding the work hand (H) is repeated.

In addition, in the above description, the description of the process and the like when the harvest fails is omitted.

In addition, the speed of the working hand (H) in the above description (
_H ) and ( _L ) are the first angle (θ ₂ ) or the second angle (θ
The first-order time derivative of ₃ ) can be used.

A method for detecting the vibration displacement (φ) and its phase (δ) around the vertical axis (Y ₁ ) of the boom (2) will be described.

When the vertical axis (Y ₁₎ moment of inertia about the boom (2) J, and F function of excitation force acting on the boom (2) to the longitudinal axis (Y ₁₎ around the vibration The equation of motion of can be expressed as J + K ₁ φ = F (i). Where both electric motors (10
Excitation force (F) when shifting a) and (10b) becomes F = K ₂ ·· cos θ ₂ · sin γ (ii) (see Figures 2 and 3).

Note that K ₁ and K ₂ are constants, and is a second derivative of the first angle (θ ₂ ) or the second angle (θ ₃ ). Further, the attenuation is neglected in the equation (i).

By solving the equation (i), φ = F (1-cosωt) / ω ² + φ ₀ cosωt + ₀ sinωt / ω (iii) = Fsinωt / ω−φ ₀ ωsinωt + ₀ cosωt (iv) is obtained.

Here, φ ₀ and ₀ are initial conditions, and ω is the natural angular frequency of the vibration system.

By the way, the displacement (φ) and its first-order derivative () are always calculated every time the set time elapses, and the numerical value obtained last time is used as the initial condition. That is, φ ₀ and
₀ indicates the value obtained last time. Therefore, the phase (δ) of vibration of the boom (2) is constantly monitored from the start of work to the end of work.

The phase (δ) is calculated from the following equation.

δ = tan ⁻¹ (φ · ω /) Therefore, the phase (δp) of the vibration at the time when the proximity sensor (S ₂ ) is turned on can be obtained.

Next, a method of changing the timing at which the two actuators (10a) and (10b) are moved from the high speed movement to the low speed movement in order to reduce the vibration based on the phase (δp) will be described.

As shown in FIG. 1, the phase (δp) is 0 ≦ δp <2.
It shall be in the range of π. Further, when is positive, it is assumed that the approach direction (A) and the boom (2) vibrate in the same direction. Therefore, when is positive and maximum, that is,
When the phase in the figure is 2π, the maximum effect can be obtained in reducing the vibration energy by decelerating to a low speed movement. That is, it can be said that (R) in the figure is an ideal deceleration point.

However, when the time (t ₁ ) from when the proximity sensor (S ₂ ) is turned on to when it reaches the ideal point (R) is long and deceleration is performed at the ideal point, the work hand (H) is There is a risk of colliding with the fruit while moving at high speed.

The time (t ₁ ) is calculated by the following formula.

t ₁ = (2π−δp) · T / 2π where T is the period of vibration.

Therefore, the allowable range (Δθ) in which the proximity sensor (S ₂ ) is allowed to move in the high-speed moving state after the ON operation is set in advance,
Both electric motors (10a) and (10b) are controlled so as not to move at a high speed beyond the range (Δθ).

Hereinafter, when the phase (δp) is π / 2 ≦ δp <2π,
Description will be given separately for the case of 0 ≦ δp <π / 2.

(1) In the case of π / 2 ≦ δp <2π First, from the speed ( _H ) during high-speed movement to the ideal point (R)
At the time of deceleration at a constant acceleration so that the speed ( _L ) during low speed movement is reached, the amount of movement from when the proximity sensor (S ₂ ) is turned on until the ideal point (R) is reached falls within the above range (Δθ). Determine if it exceeds. Then, the deceleration procedure is changed based on the result.

(I) When Δθ ≧ ( _H + _L ) · t _1/2 holds, the speed () is adjusted until the phase (δ) of vibration reaches the ideal point (R). Decelerate at. Then, at the ideal point (R), (p +
From t ₁ ) to low speed ( _L ), decelerate at once. Therefore, the vibration energy (E) is reduced.

_{(Ii) Δθ <(H +} L) · t 1/2 once when you satisfied, decelerates the speed () to zero, thereafter the state immediately on the velocity () in [Delta] [theta] / t ₁ until ideal point (R) Move forward and decelerate to _L at the ideal point (R).

(2) Case of 0 ≦ δp <π / 2 In this case, since it is far from the ideal point (R), it is not possible to decelerate at the ideal point (R). However, as can be seen from FIG. 1, the direction of vibration and the direction of the excitation force due to deceleration are opposite, so if the proximity sensor (S ₂ ) is turned on and deceleration is immediately performed, the vibration energy (E) will be released. Can be small. Therefore, the acceleration (E) that minimizes the vibration energy (E)
_E ) is obtained and decelerated by the acceleration ( _E ).

That is, the vibration of the energy (E) can be obtained ^{d (φ 2 ω 2 + 2} ) / d = 0 the acceleration from (v) _(E) proportional to φ ² ω ² + ^2.

When solving the above equation (v), the equation (ii),
Equations (iii) and (iv) will be substituted. However, _E <
0 and> 0.

[Another embodiment]

Although the working hand (H) is made to approach the target object (F) by swinging the articulated arm (8) in the above-described embodiment, the target object (F) is moved by sliding the arm. Alternatively, the arm (8) may be expanded and contracted in various ways.

In the above embodiment, the present invention is applied to the fruit harvesting working machine, but it can be applied to various industrial working machines such as an assembly robot.

It should be noted that although symbols are added to the claims for convenience of comparison with the drawings, the present invention is not limited to the structure of the accompanying drawings by the entry.

[Brief description of drawings]

The drawings show an embodiment of the arm control device according to the present invention, FIG. 1 is an explanatory view showing the relationship between displacement and phase of vibration, FIG. 2 is a schematic side view of the arm, and FIG. Fig. 4, Fig. 4 is a side view of the same, Fig. 5 is a block diagram of a control configuration, and Fig. 6
FIG. 7 is a flow chart of control operation, FIGS. 7 (a) to 7 (c) and 8 are explanatory views of image processing, and FIG. 9 is a sectional view showing a schematic configuration of a work hand. (2) …… Boom, (8) …… Arm, (10a), (10
b) …… actuator, (100) …… control means, (10
1) ... phase detection means, (H) ... work hand,
(F) …… Target, (δ) …… Phase.

 ─────────────────────────────────────────────────── ─── Continuation of the front page (72) Inventor Hiroshi Suzuki 64, Ishizukita-cho, Sakai City, Osaka Prefecture Kubota Iron Works Co., Ltd. Sakai Works (56) References JP 59-3506 (JP, A) JP 1 -218514 (JP, A)

Claims (1)

[Claims] 1. A boom (2) is provided with an extendable arm (8) at the tip thereof, and a working hand (H) is provided at the tip of the arm (8), and the working hand (H) is provided. The actuators (10a), (10b) for driving the arm (8) to move the robot with respect to the boom (2) and the working hand (H) at high speed so as to approach the target (F). And the actuators (10a), (10) so that the working hand (H) is moved at a low speed toward the target object (F) as the target object (F) is approached.
An arm control device provided with a control means (100) for controlling b), the phase detection means (101) for detecting a phase (δ) of vibration centered on the base end of the boom (2). The control means (100) shifts the actuators (10a) and (10b) from high speed movement to low speed movement based on the detection information of the phase detection means (101) in order to reduce the vibration. An arm controller configured to change timing.