JPH03170284A - Arm controlling device - Google Patents

Abstract

PURPOSE:To reduce a vibration of a working hand at the time of moving the working hand toward a target at a low speed by providing a controlling means for changing timing of transferring actuators from high speed movement io low speed movement to reduce the vibration based on detected information of a phase detecting means. CONSTITUTION:A phase of a vibration with a foot of a boom as the center is detected by a phase detecting means 101. Then based on detected information of this phase detecting means 101, timing of transferring actuators 10a, 10b from high speed movement to low speed movement is changed by a controlling means low.

Description

[Detailed Description of the Invention] [Industrial Application Field] The wooden invention includes a telescopic arm provided at the tip of a boom, a working hand provided at the tip of the arm, and a telescopic arm provided at the tip of the arm. an actuator for driving the arm to move relative to the boom; and an actuator for moving the working hand at high speed to approach a target object; The present invention relates to an arm control device including a control means for controlling the actuator to move the actuator at a low speed toward a target.

[Conventional technology]

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

In other words, in order to improve work efficiency, it is desirable that the work hand can move as fast as possible. However, when capturing a target object, it is necessary to stop the working hand accurately at a location where the target object is located so that the working hand does not collide with the target object and damage the object.

Therefore, for example, a proximity sensor or the like is installed to detect when the work hand approaches a target object, and the work hand is moved at high speed until the target object is sensed, and then decelerated to a low speed. I was trying to move it.

Conventionally, a proximity sensor or the like has been designed to decelerate immediately after detecting a target object.

[Problem to be solved by the invention]

In the above-mentioned conventional technology, when the actuator is decelerated as it approaches the target object, the vibration of the working hand increases, which may cause problems in tasks such as capturing the target object.

To explain, decelerating the actuator can be thought of as applying an impulse-like excitation force to the freely vibrating boom.

In other words, if the moving direction of the work hand does not intersect with the boom's rotation axis, when the actuator is accelerated or decelerated, a torsion moment 1 is generated with respect to the boom around the axis, and the boom is rotated around the axis. This creates an excitation force that vibrates around the core. Furthermore, after the excitation force is no longer applied to the boom that has begun to vibrate, the boom vibrates freely at a frequency unique to the boom vibration system.

By the way, when applying an excitation force to the vibrating boo If the direction of the force is opposite, the energy of the vibration will be small.

Therefore, when decelerating immediately after a proximity sensor or the like detects a target as in the past, there is a risk that the amplitude of the vibration after deceleration will be larger than the vibration before reaching the target.

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

[Means to solve the problem]

In order to achieve this object, the arm control device according to the present invention is characterized in that a phase detection means for detecting the phase of vibration centered at the base end of the boom is provided, and the control means is configured to detect the phase of vibration centered at the base end of the boom. Based on the detection information of the means, the actuator is configured to change the quiggling for shifting the actuator from high-speed movement to low-speed movement in order to reduce the vibration.

[For production]

By detecting the phase, the direction of boom vibration can be detected.

If the direction of the vibration is opposite to the direction of the excitation force due to deceleration, the vibration energy can be reduced by decelerating. When the direction of the vibration and the direction of the excitation force are the same, it is sufficient to wait until these directions become opposite before decelerating.

By the way, the energy of vibration is the sum of kinetic energy and potential energy. Generally, the maximum kinetic energy is
That is, applying an excitation force due to deceleration when the potential energy is zero is considered to be most effective and desirable for reducing vibration energy.

(Effect of the invention) It is possible to reduce the vibration of the working hand when the working hand moves at low speed toward a target.As a result, work such as capturing the target can be performed smoothly. Thus, an arm control device with excellent workability can be obtained.

〔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. 4, 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 freely, and an auxiliary boot is attached to the tip of the boom (2). 1,
(3) is attached to a horizontally swingable bar, and a working manipreke (4) equipped with a working 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 the hydraulic cylinder for raising and lowering the boom, (7)
is a hydraulic cylinder for swinging the auxiliary boom.

The working manibrake (4) consists of a telescopic multi-jointed arm (8) and a working hunt (11) attached to the tip of the arm (8).

As shown in FIGS. 2 and 3, the multi-jointed arm (
8) is a base end heath (8a) that is driven to rotate around the vertical axis (Y2) by a rotating electric moke (9) at the tip of the auxiliary boom (3), and a base end base (8). 8a
) as an actuator for the swinging electric motor (
The first arm (8h) is driven to swing around the horizontal axis (X) at the loa), and the tip of the arm (8b) is moved vertically by an electric motor for expansion and contraction (10b) as an actuator. The second arm (8c
). That is, the approach direction (8) is determined by rotating the base end base (8a), and the working hand (11) is moved toward the target by driving the first arm (8b) and the second arm (8c). fruit as (F)
can be approached.

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

In Fig. 2, (θ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 swing angle of the first arm (8b) of the second arm (8c). The opposite swing angle (hereinafter referred to as the second angle), (η) is the elevation angle with respect to the horizontal direction of the approach direction (A), (θ1) in Fig. 3
is the swing angle of the proximal end base (8a) with respect to the auxiliary boom (3), and (r) is the angle between the longitudinal force direction of the boom (2) and the approach direction (8) in the horizontal plane.

When the work hand (11) approaches the fruit (F), the work hand (11) should be moved so that the sum of the first angle (θ2) and the elevation angle (η) is half of the second angle (θ3). It is adapted to drive electric motors (10a) and (10b), respectively. However, in the following description, it is assumed that the first angle (θ2) is half of the second angle (θ3), ignoring the change in the elevation angle (η).

In FIG. 3, (y,) indicates the vertical axis of the base end of the boom (2).

That is, as described below, the work hand (11) is used to hold the fruit (
When approaching F), use the working hand (I+)
The two lower axes of the boom (2) are excited by the excitation force generated by accelerating or decelerating the moving speed
Y1) Let the displacement of the surrounding vibration be φ and its phase be δ (
(See Figure 1). In other words, the phase (8) is the boom (2).
corresponds to the phase of vibration centered at the proximal end of.

To explain the working hand (H), as shown in FIG.
) is provided, and the ventilation pipe (
12), which is externally supported so as to be freely retractable in the front and back direction, and the handle of the fruit (F) taken into the catch case 13) is vertically clamped. It is equipped with a handle cutting device (14) that cuts the handle in the state. Although not described in detail, the vacuum bath l'' (11) is suctioned by a negative pressure generator (19) using an ejector pump (18) attached to the ventilation pipe (12). (See Figure 5).

The trapping part gauge (13) includes a main body part (13a) that is fitted onto the ventilation pipe (12), and a main body part (1.3a) thereof.
It consists of a cover (13b) which is pivoted so as to be biased back toward the inside of the hand and can be opened and closed vertically, horizontally, and horizontally, respectively.

However, although not described in detail, the cahar (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 adsorbed on the honey pad (11) passes. ing. 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. The diameter of the opening is 10mm so that it can take in fruits that are larger in diameter than the opening. In addition, the cover divided into four (13b
) is located at the bottom of the actuator (
(not shown) so that the fruit (F) whose handle has been cut and which is held in the trapping part case (13) can be discharged to the outside. There is.

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

Further, as shown in FIG. 5, the working hand (I+) is equipped with a negative pressure sensor (S:I) for detecting that the fruit (1') has been adsorbed to the vacuum pad F (11). It is provided.

In FIG. 5, (S1) is a color image sensor that images the fruit (F) located in the direction that the working hand (H) is facing, and (S2) is the working hand (■1). This is a proximity sensor that uses infrared light to detect when the fruit approaches the fruit to be harvested within a set distance, and both sensors (S+) and (SZ) It is provided inside the vacuum port (11) to prevent this from occurring. or,
(l5) is a IIe light 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 (S1).

Then, with the work handle l'' (I+) facing the direction in which the fruit to be harvested is located, the illumination device i1i1 (
15) while illuminating the image sensor (S+)
The approach direction (A) of the working hand (H) is determined based on the image information obtained by processing the image.

After determining the approach direction (^), both electric motors (10a), (
10b) and the proximity sensor (S2).
detects the approach to the fruit, the vacuum pad (11) is actuated to suck the fruit (F) to be harvested, and the fruit (F) is placed inside the trapping part case (13).
After harvesting F), the stems are cut and harvested.

1? 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 brightness information of the image captured by the image sensor (S1). , the approach order is automatically determined.

To explain the image processing, Fig. 7 (a),
As shown in (I1), the working hand (I1) is
By performing imaging processing while set at a preset approach start position and processing the imaging information,
A specific color area (F1) corresponding to the color of the fruit (F), a set area (F2) where the brightness is equal to or higher than a set value within the specific color area (F1), and a set area (F2) of the set area (F2). Then, the setting area (F2) with the largest size is extracted, and the work hand (H) is moved toward the center of gravity (F3).
The working hand (H)
The direction of the arm (8) is adjusted to extend the arm (8).

13 In other words, as shown in FIG. 8, the brightness of the image information is inversely proportional to the distance, and the closer the image information is, the brighter it becomes. Therefore, the brightness within the specific color area (F1) exceeds the set value. Taking advantage of the fact that the size of the setting area (F2) increases the closer the fruit is located, the fruit that is easier to harvest (1') located on the near side is approached first. .

To explain the image processing for extracting the specific color region (F1), three primary color signals (R), (of the image signals output from the image sensor (S,))
Using G) and (B), set the signal obtained by subtracting the other color signals (G or B) from the red signal (R) that includes the color of the fruit (1'). Binary value based on the darkness value A specific color region (F1) corresponding to the color of the fruit (F) is extracted from the original image captured by the image sensor (S,) (see FIG. 7 (a)) by A valued image (see FIG. 7(H)) is obtained.

Then, based on the value of the luminance signal (Y) among the image signals output from the image sensor (S1),
The setting area (F2) where the brightness in the specific color area (Fl) is equal to or higher than the setting value is extracted, and the direction in which the center of gravity (F3) of the setting area (F2) is located on the image is determined by the fruit ( F) will be obtained as position information (see Figure 8).

However, as shown in FIG. 7(c), if a plurality of the setting areas (F2) are extracted within one specific color area (F1), the size of the setting area (F2) Based on this, the approach order is determined so that the larger the size, the earlier the approach is made.

Next, a control structure for approaching the work hand (11) to the fruit to be worked will be explained.

As shown in FIG. 5, image information captured by the image sensor (S1) is image-processed to determine the direction in which the fruit to be harvested (F) is located, and to determine the approach order. In order to guide the working hand (11) toward the fruit to be harvested (F) in the approach order, the boom lifting hydraulic cylinder (5), the auxiliary boom swinging hydraulic cylinder (7), Each electric motor (9), (10a) for driving the arm (8). (10b) A control device using a microcomputer (
16), and each electric motor (9), (10a) for driving the arm (8) based on the command from the control device (16).
) and (1.Ob) are provided.

That is, by using the control device (16), the working hand (11) is moved at high speed so as to approach the fruit (F) to be harvested, and as the working hand (11) approaches the fruit (F), the working hand both electric motors (10a) to move the hand (11) toward the fruit (F) at low speed;
(10b), a control means (100) for controlling the boom
2> The phase of vibration around the vertical axis (V,) (
Each of the phase detection means (101) for detecting δ) is configured.

Next, the procedure for harvesting the fruit (F) will be briefly explained based on FIG.

First, the working hand (1l) is set at a preset approach start position, and an image is taken.
The approach direction (8) of the working hand (H) is determined by image processing the captured image information. Then, in order to extend the arm (8) in the direction (A), the base end base (8a) is rotated by the electric rotation motor (9), and the working hand (11) is rotated at a preset speed. Both electric motors (10a) and (10b) are driven to move at high speed at (θH). Thereafter, based on the information of the proximity sensor (S2), it is determined whether the working hand (F) has approached the target fruit (F) to be harvested by the predetermined distance, and has approached the predetermined distance. Accordingly, the electric motors (10a) and (10b) are operated so that the absorption operation of the vacuum pad (11) is started and the working hand (+1) moves at a preset speed (θL) at a low speed. Activate deceleration.

However, the timing of deceleration from high-speed movement to low-speed movement is changed based on the detection information of the phase detection means (1.01), as will be described later.

After deceleration, when the negative pressure sensor (S3) turns on and detects fruit adsorption, the telescopic arms 1 and (8) are stopped and the harvesting operation is started.

After harvesting the fruit, the working hand (11) is returned to the approach start position while catching the cut fruit in the catcher case (13), and the fruit (P) is discharged. .

After discharging the harvested fruits, it is determined whether the work is completed or not. If the work is not completed, the approach direction (^) for the remaining fruits (F) is determined, and the determined approach direction (A ) will be repeated.

In addition, in the above explanation, explanation of the treatment etc. in case of failure in harvesting is omitted.

Also, the speed of the working hand (I!) in the above explanation (
θH), (θ,) are the first angle (θ2) or the second angle (
The first time derivative of θ3) can be used.

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

If the moment of inertia 1 of the boom (2) around the vertical axis (Y,) is J, and the function of the excitation force acting on the boom (2) around the vertical axis (Y1) is F, The equation of motion of vibration can be expressed as Jφ+K,φ−F (i). Here, both the electric motors (10a),
The excitation force (F) when changing the speed of (10b) is F=K.・θ' cosθ2・sitH
(ii) (see Figures 2 and 3).

Note that K and Kg are constants, and θ is the first angle (θ2)
Or it is the second differential of the second angle (θ3).

Furthermore, equation (i) ignores attenuation.

Solving equation (1), we get φ1F (1 −cosωt)/ω”+φOCOS
ωt+φ(,sinωt/ω(iii)φ=Fs
inωt/ω−φ0ωsinωt+φactsωt19 (iv) is obtained.

Here φ. and φ. is the initial condition and ω is the natural angular frequency of the vibration system.

Incidentally, the displacement (φ) and its first-order differential (φ) are always calculated every time a set time elapses, and the previously obtained values are used as the initial conditions. That is, φ0 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 obtained from the following formula.

δ−jan''(φ・ω/φ>) Thus, the phase (δp) of the vibration at the time when the proximity sensor (S2) is turned ON can be determined.

Next, based on the phase (δp), both the actuators (10a) and (10b) are activated to reduce vibration.
We will explain how to change the timing that moves from high-speed movement to low-speed movement.

As shown in FIG. 1, the phase (δp) is 0≦δp<2
It is assumed that it is in the range of π. Also, when φ is positive, it is assumed that the approach direction (8) and the vibration direction of the boom (2) match. Therefore, when φ is positive and maximum, that is,
By decelerating the movement to a low speed when the phase in the figure is 2π, the maximum effect in reducing vibration energy can be obtained. In other words, (1?) in the figure can be said to be the ideal deceleration point.

However, if the time (t1) from when the proximity sensor (S2) is turned on until it reaches the ideal point (R) is long and the deceleration is made at the ideal point, the work hand (H) will move at high speed. There is a risk of collision with the fruit.

In addition, the said time (1+) is calculated|required by the following formula.

t+1(2π-δp)・T/2π Here, T is the period of vibration.

Therefore, after the proximity sensor (S2) is turned ON, a permissible range (Δθ) within which movement is allowed in a high-speed movement state is set in advance, and the two electric motors (10a ), (10b).

2l? Below, the case where the phase (δp) is π/2≦δp<2π and the case where 0≦δp<π/2 will be explained separately.

(1) When π/2≦δp<2π First, from the speed (θ,,) during high-speed movement, the ideal point (R
), when the proximity sensor (S2) is decelerated at a constant acceleration to reach the speed (θL) at low speed
It is determined whether the amount of movement from the N operation until reaching the ideal point (11) exceeds the range (Δθ). The deceleration procedure is then changed based on the results.

(i) When Δθ≧(θ■10θL)・t,/2, the vibration phase (δ) will change from (θp10θ・1+) to low speed (θ , ). This reduces the vibrational energy (E).

22 (ii) When Δθ〈(θ,,]-θL)・t1/2 stands, the speed (θ) is decelerated to zero, and then the speed (θ) is immediately increased to Δθ/1+. The vehicle moves forward to the ideal point (R) and decelerates to 014 at the ideal point (R).

(2) When O≦δp<π/2 In this case, the ideal point (R) is far away, so it is impossible to set the speed at the ideal point (1?). However, as can be seen from Figure 1, the direction of the vibration and the direction of the excitation force due to deceleration are opposite, so if the proximity sensor (S2) is activated and the speed is quickly decelerated, the vibration energy (E) will be absorbed. Can be made small. Therefore, the acceleration (θE) at which the vibration energy (E) is minimum is determined, and deceleration is performed at that acceleration (θE).

In other words, since the energy of vibration (E) is proportional to φ2ω21φ2, the acceleration (θE) can be found from d(φ2ω2''-φ2)/dθ-0 (v).23 Furthermore, when solving (v) 2, , (v) into equation (ii)
, (iii), (iv) 2 will be substituted. However, θ, <0, and θ>0.

[Another example]

In the above embodiment, the work hand (11) was made to approach the target (F) by swinging the multi-jointed arm (8), but by moving the arm in a straight direction, the target (F) The arm (8) may be made to extend and retract, or various changes may be made.

In the above embodiment, the present invention was applied to a fruit harvesting working machine, but it can also be applied to various industrial working machines such as Assembly Robot I.

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 arm control device according to the present invention, Fig. 1 is an explanatory diagram showing the relationship between vibration displacement and phase, Fig. 2 is a schematic side view of the arm, and Fig. 3 is an illustration of the working machine. Overall plan view, Figure 4 is a side view, Figure 5 is a block diagram of the control configuration, Figure 6 is a flowchart of control operation 1.
7(a) to 8(c) and FIG. 8 are explanatory diagrams of image processing, and FIG. 9 is a sectional view showing the schematic structure of the working hand. (2) Boom, (8) Arm, (10a), (].Ob) Actuator, (100) Control means, (1
01)...Phase detection means, (11)...Working hand, (F)...Target, (δ) Crying phase.

Claims (1)

[Claims] A telescopic arm (8) is provided at the tip of the boom (2), and a working hand (H) is provided at the tip of the arm (8).
is provided, and the working hand (H) is connected to the boom (2).
actuators (10a) and (10b) that drive the arm (8) to move relative to ) and the working hand (H) at high speed to approach the target (F), and The actuators (10a) and (10b) are configured to move the working hand (H) at a low speed toward the target object (F) as it approaches the object (F).
), the arm control device is provided with a control means (100) for controlling a vibration phase (δ) centered on the base end of the boom (2).
) is provided, and the control means (100) controls the actuators (10a) and (10b) to reduce the vibration based on the detection information of the phase detection means (101). ) is configured to change the timing of transitioning from high-speed movement to low-speed movement.