JP2996705B2 - Steering control device for automatic traveling work vehicle - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Industrial Application Field] The present invention relates to an ultrasonic sensor that is mounted on a working vehicle and rotates left and right, and is provided with two rows of targets arranged on the left and right sides of the traveling vehicle body. By transmitting an ultrasonic wave toward an object and measuring the time it takes to reflect and return from the target (the time until reception), the target object is located between the two target object arrays. The vehicle travels along the row, and detects a traffic sign provided at the end of the travel path between the two left and right target rows, and automatically turns (turns in the direction) at that point. The present invention relates to a control device for enabling a steering operation.

[Conventional technology]

Japanese Patent Application Laid-Open No. 60-258612 proposes using an ultrasonic sensor as a means for detecting trees in order to make a vehicle (speed sprayer) for chemical spraying work in an orchard run unmanned.

[Problems to be solved by the invention]

By the way, between the target rows arranged in a row like a tree row or a pile in an orchard or the like, when controlling the vehicle to travel along the target row, the ultrasonic sensor transmitter is used. The distance to the target can be detected by intermittently emitting ultrasonic waves toward the target and measuring the time required for the ultrasonic wave to be reflected from the target and received by the receiver.

In this case, in the ultrasonic sensor, in particular, the receiver has a directivity (a property that an angle at which the ultrasonic wave can be received is limited), and the directivity is generally sharp, so that the detection range is narrow.
From the necessity of simultaneously detecting target rows on both the left and right sides of the work vehicle, in the prior art, a pair of left and right ultrasonic sensors are fixedly provided in the front part of the work vehicle so that their directions are different, In addition, in order to detect a traffic sign indicating the end of the traveling route, one ultrasonic sensor is similarly provided fixed to the front part of the working vehicle in a forward direction (forward direction in the traveling direction), and the pair of left and right ultrasonic sensors is provided. The left and right steering is controlled based on the detection result, and the steering result is largely changed (for example, U-turned) at the end of the travel route by the detection result of the traveling sign by one ultrasonic sensor at the center. I want to run.

And in the said prior art, the said center ultrasonic sensor does not play any role in circulation control after detecting the marker for circulation once and outputting the signal of the timing. That is, since the ultrasonic sensor is provided to be fixed to the work vehicle, if the traveling traveling is started and the direction of the work vehicle is changed, it is no longer possible to detect the traffic sign.

Therefore, in the subsequent turning steering, it is necessary to execute the turning steering in a preset turning steering mode (for example, a U-turn mode or a 90-degree turn mode) or a value of the turning radius (turning radius) at that time. In addition, there is no way to correct even if there is a danger that a steering error enters and collides with a tree during that time, so that there is a problem that the control during the reciprocation is unstable.

An object of the present invention is to solve the technical problem of the prior art.

[Means for solving the problem]

In order to achieve this object, the present invention provides an ultrasonic sensor provided in an autonomous traveling work vehicle traveling between two left and right target object rows by using a rotary drive means in a wide range in the traveling direction of the work vehicle. Traverse traveling control using a signal detected by the ultrasonic sensor at the end of the traveling path provided between the two left and right target rows. In the steering control device, the direction of the ultrasonic sensor is controlled so as to be able to constantly detect the direction of the traveling sensor during the traveling traveling control. The traveling traveling control is performed by using the data of the rotation angle of the ultrasonic sensor directed to the traveling sign and the distance data between the traveling sign and the ultrasonic sensor.

[Action and Effect of the Invention]

According to the configuration of the present invention, since one ultrasonic sensor is intermittently or continuously rotated left and right for scanning like a radar, the sensing (detection) range of the ultrasonic sensor is set in front of the work vehicle. It can be expanded to the left and right, and does not generate a region in which detection of a target object row and a traffic sign is omitted.

After detecting the traffic sign located at the end of the route traveling between the target object rows, the tracking control is performed to rotate the ultrasonic sensor so that the traffic sign can always be detected. In this case, the relationship between the position and the orientation of the reference traffic sign and the ultrasonic sensor can be reliably grasped during the traveling, so that the traveling steering control can be quickly and reliably achieved. Can travel safely.

In addition, it is easy to remove a detection noise component such as another target by the tracking control, and the accuracy of the control is improved.

〔Example〕

Next, an embodiment in which the present invention is applied to an automatic traveling type chemical sprayer (speed sprayer) will be described. A work vehicle 1 which is a speed sprayer has a driving operation unit having a steering wheel 3 at a front portion of a vehicle body 2. A chemical solution tank 4 is provided at the rear of the vehicle body 2 and a spray unit 5 is provided at the rear.

The spray unit 5 is provided with a large number of spray nozzles 6 facing radially outward at appropriate intervals on the outer peripheral surface except the lower surface of the vehicle body 2 and a blower (not shown) for sending wind outward in the radial direction. The spray nozzle 6 is capable of controlling spraying so as to execute a spraying operation for each of three sections with respect to the left, right, and upper surfaces of the vehicle body 2 or two sections on the left and right sides.

Reference numerals 7 and 7 denote left and right front wheels, and reference numerals 8 and 8 denote left and right rear wheels. These four wheels are so-called four-wheel drive types in which power from an engine 13 is transmitted via a traveling speed change mechanism 14 and driven. The front and rear four wheels are of a so-called four-wheel steering type in which the directions of the wheels can be changed right and left by a front steering device 9 and a rear steering device 10, respectively.

The front steering device 9 and the steering wheel 3 are connected via a conventionally known steering mechanism. This steering mechanism is a mechanism including a mechanical or hydraulic system.

The front steering device 9 can change the directions of the left and right front wheels 7, 7 by operating a double-acting hydraulic cylinder 15 attached to the steering mechanism.

Similarly, in the rear steering device 10, the directions of the left and right front wheels 8, 8 can be changed by operating a pair of left and right hydraulic cylinders 16 to which the steering mechanism is attached.

A hydraulic circuit 17 shown in FIG. 2 is a hydraulic cylinder in the front steering device 9 from a hydraulic pump 18 via an electromagnetic control valve 19.
15 and hydraulic fluid to the hydraulic cylinder 16 of the rear steering device 10 via the electromagnetic control valve 20.
21 is a steering angle sensor that can detect the steering angle of the front wheel 7,
Reference numeral 22 denotes a steering angle sensor capable of detecting the steering angle of the rear wheel 8. Both steering angle sensors are composed of a potentiometer or the like, and their detection signals can be used for feedback control. In this case, the average of the direction angles of the left and right wheels may be obtained and detected.

The steering control is performed by the front steering device 9 or the rear steering device.
The vehicle body (working vehicle) provided with only one of them may be used.

Reference numeral 23 denotes an ultrasonic sensor provided at a front portion of the vehicle body 2,
The transmitter 23a and the receiver 23b are connected to a stepping motor 24, and are provided on both left and right sides in the traveling direction of the work vehicle 1 so as to swing by a scanning area (rotation scanning angle θo) of the same angle. .

In the embodiment, at the time of detecting the M rows of the target, -π / 2 ≦ θo
The value sensor 23 can rotate 360 degrees in a section in which the vehicle enters the region of the circling steering control after detecting the circling marker 30 to be described later with ≦ π / 2.

Here, the directional angle α of the receiver 23b of the ultrasonic sensor 23
The duty (pulse width) of the ultrasonic pulse transmitted by the transmitter 23a is τ (second), and the pulse repetition interval is To
(Second), and the time interval from the fall of the transmission pulse to the fall of the reception pulse is T (<To−τ) (second). In reality, the pulse width of the ultrasonic wave is on the order of milliseconds.

Then, the installation height of the ultrasonic sensor 23 is set so that the ultrasonic wave is transmitted toward the trunk Mb or the branch Ma of the tree M as the target. Note that the target M located on the right side of the work vehicle 1 has an odd subscript (M1, M3, M5, etc.).
, And those on the left have even subscripts (M2, M4, M6, etc.).

Further, the step motor 24 is provided with the ultrasonic sensor
23 is a driving means for rotationally driving the transmission (reception) direction by 23 so as to smoothly move to the right or left direction at a constant angular velocity. The rotational scanning angular velocity is ω (radian / second),
The maximum distance Lo required for steering control is set to be Lo. When the distance from the ultrasonic sensor to the target M is equal to or longer than the maximum distance Lo, the detection result is ignored. Reference numeral 25 denotes a direction sensor of a potentiometer for detecting the rotation angle of the step motor 24 or the rotation angle of the ultrasonic sensor 23 to detect the direction (azimuth) of the ultrasonic sensor 23, particularly the direction of the receiver 23b. is there.

Reference numeral 26 in the control means block diagram of the embodiment of FIG.
Is a central controller for executing various operations and programs. A read / write memory (RAM) for temporarily storing detected values, a read-only memory (ROM) for storing the programs themselves and invariant data values, and an input / output memory. An output interface or the like is connected.

A sensor controller for transmitting an ultrasonic impulse of a predetermined frequency (high frequency) from the transmitter 23a in the ultrasonic sensor 23 and receiving a reflected wave at the receiver 23b, a synchronous pulse generator, and a pulse transmitter , An amplifier, etc., and a reference position detection sensor that can detect the reference position of the ultrasonic sensor 23 (for example, the center line in the traveling direction of the work vehicle).

The detection signal synchronized with the transmitting side of the ultrasonic sensor 23 is A / D-converted, and is input as a digital signal to the central controller 26 via a multiplexer.

On the other hand, the ultrasonic wave receiving direction (azimuth) is
It can be known from the number of input pulses to 24 and the detection value of the direction sensor 25 attached to the step motor 24.

Since the propagation time of the ultrasonic wave transmitted from the transmitter 23a, reflected at the target M and reaching the receiver 23b is proportional to the distance (m) from the ultrasonic sensor 23 to the target M, The detection signal having a longer or shorter propagation time is represented by a voltage (V) and a distance (m).
And then output from the sensor controller.

In this case, at a short distance where the distance from the ultrasonic sensor to the target is equal to or less than a certain value, there is a so-called insensitive range where transmission and reception interfere with each other and the distance cannot be measured,
When the distance is equal to or longer than a certain value, there is no ultrasonic wave that is reflected back. Therefore, the voltage is set to the same value as the voltage value (Vmax) of the maximum detection distance (Lmax).

Since the propagation speed of the ultrasonic wave in the air increases in proportion to the temperature (air temperature) of the air,
It is preferable to provide a temperature correction sensor.

In the present embodiment, the rotational scanning angular velocity ω per unit time of the ultrasonic sensor 23 is changed to the scanning angle ωT which rotates while repeating the transmission and reception of the ultrasonic sensor at least once or more. This is set so as to fall within the directivity angle α of the sound wave sensor.

In other words, the rotation speed of the ultrasonic sensor 23 is set such that ωT <α, where T = 2Lo / v (where v is the sound speed (meter / second)).

This relationship will be further described. The ultrasonic wave transmitted once from the transmitter 23a of the ultrasonic sensor 23 to the target M at a position separated by the maximum distance Lo required for control is received by the receiver 23a.
The time interval until the wave is received at 23b is T = 2Lo / v, during which time the ultrasonic sensor operates at a constant rotational scanning angular velocity ω.
, It means that the direction of the receiver 23b at the time t = 0 is changed by ωT at the time t = T. That is, the scanning angle is ωT.

On the other hand, since the ultrasonic wave reflected by the target can be received only within the directivity angle α of the receiver 23b in the ultrasonic sensor, when the scanning angle ωT is within the area, the target The object M can be detected and cannot be detected outside the area. The directional angle α is determined by determining the specifications of the ultrasonic sensor.

With this configuration, if the ultrasonic sensor 23 is rotated at a predetermined rotational scanning angular velocity ω, when the distance L from the ultrasonic sensor to the target M is short (L <Lo), the scanning angle ωT is also small. When the distance L of the target M is long (L ≒ Lo), the azimuth from the ultrasonic sensor can be accurately detected. Become.

In addition, since the ultrasonic sensor is continuously rotated at the rotational scanning angular velocity ω in the above relationship, a wide range can be quickly scanned compared to the case where the ultrasonic sensor is rotated intermittently, and the detection accuracy and the steering control can be controlled. Accuracy also improves.

In this case, the rotational scanning angular velocity per unit time of the ultrasonic sensor is set such that the scanning angle that rotates while repeating the transmission and reception of the ultrasonic sensor at least once or more is within the directivity angle of the ultrasonic sensor. If it is set to fit, it is possible to always detect a target whose proximity is close to the ultrasonic sensor, in other words, a target that is close to the work vehicle and has a high danger of collision within the directional angle of the ultrasonic sensor. Can,
There is an effect that steering control can be performed safely and securely without detection omission.

Note that, in the ordinary embodiment, 3 to 4
The transmission and reception of the ultrasonic sensor are repeated at least once.

As another embodiment in which the ultrasonic sensor 23 is rotated, the stepping motor 24 is intermittently rotated so that the wave transmission direction shifts to the right or left at an appropriate pitch, and the rotation stop time is determined by the transmission stop time. The time length from when the ultrasonic wave transmitted by the wave device 23a is reflected by the target M or the like and is received by the wave receiver 23b (the time from the fall of the above-mentioned transmitted pulse to the fall of the received pulse) The interval is substantially equal to T seconds).

Also, as shown in FIG. 3 and FIG.
The range of the angle 2θ1 (for example, θ1 = 15 degrees) near the center with respect to the traveling direction axis X (reference line) among the
The sign detection area K for detecting 30 is set, and the left and right regions excluding the sign detection area K are set as the detection areas of the target M rows.

In this case, the size (left and right length) of the circulation marker 30
Accordingly, the traveling marker 30 can be detected even if the rotation scanning angular velocity in the marker detection region L is increased to a degree that the marker 30 can be detected at least once or twice, and the detection accuracy is reduced. As long as the scanning position is not misleading, it is safe, and by increasing the rotational scanning angular velocity, the time interval before scanning in other target object line detection areas can be started can be shortened, and the detection accuracy is improved. It will improve.

Then, as shown in the main flow chart of the steering control shown in FIG. 5, the work vehicle 1 travels forward, and the ultrasonic sensor 23 is rotated in step 501 following the start of the automatic steering control. Is read, and it is determined in step 503 whether or not the circulation marker 30 has been recognized. When the traffic sign 30 cannot be recognized in step 503 (no), in step 504, the tracing travel control is executed between the left and right target M columns along the column,
When the traveling marker 30 is detected in step 503 (yes), the traveling steering control such as a U-turn is executed in step 505.

FIG. 6 is a subroutine flowchart for executing the traveling steering control. When the traveling marker 30 is detected in step 601, the direction θ2 of the ultrasonic sensor 23 with respect to the reference line X at that time is read from the detected value of the direction sensor 25. Further, the reference distance do from the traveling marker 30 to the ultrasonic sensor 23 is read and stored (step 602).

Next, in step 603, the work vehicle 1 is calculated by calculating a change in distance between the ultrasonic sensor 23 and the traveling marker 30 for a time Δt immediately before or immediately after the detection of the reference distance do.
The vehicle speed SV is detected and stored.

In step 604, a command signal for tracking control for setting the direction of the ultrasonic sensor 23 to be always fixed to the circling marker 30 is sent to the step motor 24.

In this state, in step 605, the vehicle starts traveling while executing the traveling steering control based on the U-turn mode or the 90-degree turn mode which is stored in advance in a program.

At step 606, the orientation angle θ3 of the work vehicle 1 with respect to the traffic sign 30 obtained by the tracking control at step 604
Then, the current position (X, Y) of the ultrasonic sensor 23 during the traveling traveling is calculated from the data of the distance d1 from the traveling marker 30 to the ultrasonic sensor 23.

In this calculation, the position of the ultrasonic sensor 23 at the reference distance do detected in the step 602 is set as the origin O (0, 0) of the XY coordinate system, and the reference line X is centered on the 30 traffic markers. The included angle θ4 with the ultrasonic sensor 23 is defined as the angle θ
From the data of 2 and θ3, the distance d2 of the current ultrasonic sensor 23 to the origin O is calculated by a trigonometric function based on the data of the distance d1 from the current ultrasonic sensor 23 during tracking to the circulating marker 30. Thus, the current position (X, Y) of the ultrasonic sensor 23 during the traveling traveling can also be easily obtained (see FIG. 3).

Then, the steering operation amount S to be controlled in the revolving steering mode is a function using the coordinates X, Y of the current position of the ultrasonic sensor and the angle θ4 as variables, and data in which this functional relationship is stored as a table in advance. In step 607, it is determined whether or not the current position (X, Y) of the ultrasonic sensor 23 has escaped from the circling area. When it is within the traveling area (yes), the process returns to step 601 to execute traveling steering control, and when it is determined that the vehicle is out of traveling (no), scanning control along the previous target M rows (step 50
The process moves to 4) (step 608).

As shown in FIG. 4, a region overlapping with the marker detection region K or a region slightly wider on both the left and right sides than the region K is defined as a direction deviation detection region Ho, and the region of θ5 at both ends is detected as a left and right position deviation detection. By setting the area Io, the value of the minimum distance (L) data among the targets that have entered the left and right displacement detection areas Io, Io is averaged over a certain period of time,
The left distance data dl and the right distance data dr are obtained, and the difference ρ =
Calculate dr-dl, and determine the left / right position deviation and the magnitude of the left / right position deviation amount based on the sign of ρ and the magnitude of the value.

On the other hand, in the direction shift detection area Ho, the area data of the fan-shaped area (area surrounded by points A, B, C, and D) surrounded by the unmeasurable distance (minimum distance Ln) to the maximum distance Lo is calculated. At the same time, the fan-shaped centroid coordinates G (θx, θy = by rectangular coordinates) or (θG, θr = by cylindrical coordinates) are obtained by calculation, and the deviation angle with respect to the traveling direction axis X (reference line) is calculated as , The direction shift amount θ is obtained.

The steering operation instruction amount S may be determined based on the ρ and θ to correct the steering.

If the steering control is performed in this manner, the work vehicle 1 can travel along the tree row without laterally shifting laterally at substantially the center of the row of target objects (trees) on both the left and right sides.

If the user wants to move forward while approaching one of the right and left trees, the steering control instruction amount may be determined by executing correction such as adding a correction value of ± to the value of θG. is there.

Further, the ultrasonic sensor 23 and the vehicle 2
When the central controller 26 determines that the distance to the front end of the vehicle is within a predetermined distance, an alarm device such as a buzzer mounted on the vehicle body 2 or provided on a remote control device (not shown) is activated. In addition to this, the brake driving means and / or the engine stop device may be operated to stop the work vehicle 1 urgently.

As described above, the ultrasonic sensor is rotated intermittently or continuously, a region near the left and right center portion of the rotation scanning region is set as a marker detection region, and a region near the left and right sides is a target object row. If only one ultrasonic sensor with a sharp directivity, in other words, a narrow detection area is used, it is possible to obtain many information simultaneously without omission of detection of a sign. As a result, the detection accuracy is improved, and thus the accuracy of the steering control is improved.

In that case, the control system configuration (hardware configuration)
It can be simplified in proportion to the small number of ultrasonic sensors.

[Brief description of the drawings]

The drawings show an embodiment of the present invention, FIG. 1 is a perspective view of a work vehicle, FIG. 2 is a diagram of a steering control mechanism, a hydraulic circuit and a control means block, and FIG. FIG. 4 is an explanatory plan view showing a positional relationship with a traffic sign, FIG. 4 is a diagram showing a relationship between a distance to a target by an ultrasonic sensor and a detected value, FIG. 5 is a main flowchart, and FIG. It is. 1 ... work vehicle, 2 ... body, 3 ... handle, 5 ...
Spraying unit, 7 Front wheel, 8 Rear wheel, 9 Front steering device, 10 Rear steering device, 23 Ultrasonic sensor, 24
… Step motor, 25… bearing sensor, 26… central controller, 30… circulating sign.

Claims (1)

(57) [Claims] An ultrasonic sensor provided on an automatic traveling work vehicle traveling between two left and right target object rows is rotated left and right in a wide range with respect to the traveling direction of the work vehicle by a rotation driving means. The ultrasonic traveling sensor is configured to perform traveling traveling control using a signal obtained by detecting a traveling sign provided at an end of a traveling path between the two left and right target rows. In the steering control device, during the traveling traveling control, the direction of the ultrasonic sensor is controlled to rotate so that the traveling sign can be always detected, and is directed toward the traveling sign with respect to the reference line of the work vehicle. The rotation angle data of the ultrasonic sensor
A steering control device for an automatic traveling work vehicle, wherein the traveling control is performed using distance data between a traveling marker and an ultrasonic sensor.