JP3238307B2 - Guidance control device for mobile vehicles - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a guidance control device for a mobile vehicle that automatically travels along a planned traveling route.

[0002]

2. Description of the Related Art In the above-mentioned mobile vehicle guidance control device, for example, a work vehicle such as a rice planting plant as a mobile vehicle is moved along a plurality of work processes as a scheduled traveling route juxtaposed in a rectangular work site. While detecting the position of the work vehicle by receiving the projected guidance light beam, guidance control is performed so that the vehicle automatically travels in an appropriate steering state along each work process.

[0003]

However, in the above prior art, since the guiding light beam is projected along the planned traveling route, for example, when the planned traveling route is composed of a plurality of traveling routes, There is a problem that the ground-side guidance equipment becomes complicated and expensive, such as installing a large number of dedicated projection devices for each traveling route, or moving one projection device to each route position. Was.

The present invention has been made in view of the above-mentioned circumstances, and an object of the present invention is to simplify the ground-side guidance equipment in order to solve the above-mentioned disadvantages of the prior art.
An object of the present invention is to make a mobile vehicle automatically travel along a planned traveling route in an appropriate steering state.

[0005]

According to the first aspect of the present invention, a fixed station installed at a reference position on the ground receives a carrier signal from a GPS satellite, and receives a carrier signal from the fixed station. The phase information is transmitted to the mobile vehicle. on the other hand,
In the mobile vehicle, the carrier signal from the GPS satellite is received and the transmission information from the fixed station is received, and the double phase difference obtained from the carrier phase information at the mobile station and the carrier phase information at the fixed station is obtained. Based on the information, the position of the moving vehicle with respect to the reference position is obtained as time-series GPS position data at predetermined time intervals, and the amount of position change of the moving vehicle is obtained as time-series inertial navigation position data at predetermined time intervals. Can be Then, the vehicle body position at the current time is obtained from the GPS position data set time before the current time and the inertial navigation position data at the current time, and the vehicle position at the current time is determined. The steering control is performed based on the preset traveling control information set in advance so that the moving vehicle travels along the planned traveling route. Add a description
And the inertial navigation position data with respect to the GPS position data.
The vehicle position of the moving vehicle at the current time by interpolating the data
Is to be asked.

Therefore, according to the first aspect of the present invention, it is only necessary to install a fixed station for receiving a carrier signal from a GPS satellite and communication means on the ground side. The guidance equipment on the ground side is much simpler than providing a large number of light projection devices for projecting the beam light along a plurality of traveling paths. At the same time, while accurately detecting the position of the working vehicle based on the double phase difference information of the carrier signal from the GPS satellite received on both the ground side and the mobile vehicle side, the carrier phase measurement at the fixed station and the mobile station. The weak point that has a delay in the position detection by the GPS reception signal due to the time required for information communication between the two stations and the calculation of the double phase difference, etc., is compensated by the instantaneous inertial navigation data to move in real time. By detecting the position of the vehicle, the moving vehicle can be automatically driven in an appropriate steering state along the planned traveling route.

According to a second aspect of the present invention, in addition to the first aspect, in addition to the vehicle body position information and the scheduled traveling control information, the vehicle moves based on detection information of the body direction with respect to the scheduled traveling route. The steering control is performed so that the vehicle travels along the planned traveling route.

[0008] Therefore, as compared with, for example, performing steering control based on the vehicle body position information and the scheduled travel control information with respect to the scheduled travel route of the mobile vehicle, the mobile vehicle is moved along the scheduled travel route in a more appropriate steering state. The vehicle can be automatically driven, so that the preferable means of the first characteristic configuration can be obtained.

[0009]

BEST MODE FOR CARRYING OUT THE INVENTION Hereinafter, an embodiment of the present invention will be described in which a work vehicle V for planting rice as a moving vehicle is automatically moved along a plurality of work processes L as scheduled traveling routes juxtaposed in a field. The case where the vehicle is guided to run will be described with reference to the drawings.

As shown in FIGS. 1 and 2, for example, a local horizontal coordinate system E (eastward), N (northward), H
It is installed at a reference position on the ground side whose position is known with high accuracy (in the height direction from the center of the earth) and has at least 4
GPS receiving antenna 19a for the fixed station R for receiving the spread spectrum modulated radio waves (carrier signals) from the two GPS satellites 2, and a GPS for processing the signals received by the GPS receiving antenna 19a to obtain carrier phase information. Receiver 1
9 and data transmission as a ground-side communication means having a data transmission antenna 20a for transmitting carrier phase information (GPS fixed station data) at a fixed station output from the GPS receiver 19 toward the work vehicle V side. Machine 20 is provided.

On the other hand, the work vehicle V has a GPS for the mobile station I that receives a radio wave (carrier signal) from the GPS satellite 2.
Receiving antenna 17a and its GPS receiving antenna 17a
And a data receiving antenna 18a for receiving transmission information (carrier phase information at the fixed station R) from the terrestrial-side transmitter 20. And a data receiver 18 as a mobile vehicle-side communication means provided. The two GPS receiving antennas 17a are installed at predetermined intervals in the front-rear direction of the vehicle body. Normally, one of them is used. Used to do.

As shown in FIG. 2, the GPS receiver 17 is used to obtain the carrier phase information at the mobile station I and the carrier phase information at the fixed station R received by the data receiver 18. The GPS position data calculating means 102 for obtaining the position of the mobile station I, that is, the work vehicle V with respect to the reference position, that is, the fixed station R, as time-series position data at predetermined time intervals (one second intervals) based on the double phase difference information. It is configured.

That is, the position detection of the work vehicle V based on the GPS reception data requires time (for example, about 2 seconds) for measuring the carrier phase at each receiving station, communicating the phase information, and calculating the double phase difference. ), It is not possible to detect the position of the work vehicle V at the current time only from the GPS reception data. Therefore, as described above, the position of the work vehicle V is determined in a time series at a predetermined time interval (one second interval). It is obtained as GPS position data, that is, position data with a measurement time label.
Accordingly, the above-mentioned time-series GPS position data at one-second intervals is data corresponding to the position of the work vehicle V two seconds before, and this position data and the GPS clock in one-second units are output from the GPS receiver 17. Is done.

The one-second-unit GPS clock is input to a synchronization signal generator 21. As shown in FIG. 3, the synchronization signal generator 21 has the same interval as the output of the GPS position data synchronized with the GPS clock. A clock for capturing INS position data, which will be described later, including a clock and a clock with a shorter time interval (for example, an interval of 0.1 second) is generated and output.

The outline of the double phase difference information will now be described. Each carrier signal from two different satellites 2 is
Received by each of the two receiving stations (fixed station R and mobile station I),
The two phase differences corresponding to each satellite 2 are obtained, and the difference between the phase differences obtained by combining these two phase differences is called a double phase difference, whereby the influence of the phase disturbance of the transmission signal of each satellite 2 is reduced. At the same time, the influence of the synchronization deviation of the clock for measuring the phase of each receiving station is removed, and finally, accurate phase difference information with less influence of errors on the satellite side and the receiving station side can be obtained. . In order to obtain a position vector r described later, actually, three independent double phase differences are obtained based on carrier signals from four different satellites 2.

The detection of the position of the work vehicle V based on the three pieces of double phase difference information by the GPS position data calculation means 102 will be specifically described. First, the work vehicle V is positioned at a position whose position is known with high precision in the local horizontal coordinate system E, N, H, and each GP on the mobile station side and the fixed station side is set.
The three double phase differences are calculated from the information received by the S receivers 17 and 19, and since the relative position between the fixed station R and the work vehicle V is known, the carrier wavelength included in the double phase difference information is determined. The uncertainty (integer value bias) of the integral multiple is decided.
Next, as shown in FIG. 5, a position vector r from the fixed station R to the work vehicle V is obtained from three pieces of double phase difference information when the work vehicle V is moved to an unknown point, and the fixed station R The position of the work vehicle V is determined from the reference position and the obtained position vector r.

As shown in FIG. 2, the work vehicle V has a geomagnetic compass S4 as an azimuth detecting means for detecting the body direction of the work vehicle V, and a three-dimensional body (front and rear,
A gyro device S5 for detecting angular velocities around each axis of the lateral width and the vertical direction, and an accelerometer S for detecting the acceleration of the work vehicle V in three dimensions (vehicle longitudinal, lateral width and vertical directions)
6 are provided.

Then, the GPS position data with the measurement time from the GPS receiver 17, the clock for taking in the INS data from the synchronization signal generator 21, the azimuth sensor S4, the gyro device S5, and the accelerometer S6. The detection information is input to the position data calculator 22. On the other hand, a switching signal for switching the two GPS receiving antennas 17a is output from the position data calculator 22.

Here, the inertial navigation for obtaining the position change amount of the work vehicle V as time-series inertial navigation data at predetermined time intervals using the geomagnetic compass S4, the gyro device S5, the acceleration sensor S6, and the position data calculator 22. System INS
Is configured. Specifically, the inertial navigation data (hereinafter referred to as INS position data) indicates that the position change amount of the work vehicle V, that is, the position change amount within a predetermined measurement time interval (for example, 0.1 second) is equal to a predetermined time interval ( (For example, 0.1 second).

Then, as shown in FIG. 2, the position data calculator 22 is used to set time (1) from the current time obtained by the GPS position data calculating means 102.
Vehicle position detecting means for obtaining the vehicle body position of the work vehicle V at the current time from the previous GPS position data and the inertial navigation position data (INS position data) at the current time obtained by the inertial navigation system INS. 103 is configured.

That is, the vehicle body position of the work vehicle at the current time is obtained by interpolating the INS position data having the immediateness with respect to the GPS position data having the detection delay. More specifically, the position data calculator 22 determines the GPS status based on the timing state of the position detection (the clock signal shown in FIG. 3).
The time when the position data was measured (1 second interval) or the time of a finer time interval in between (1 second interval)
The work vehicle V is positioned in real time while distinguishing between them. Hereinafter, description will be given based on FIG.

First, FIG. 3 will be supplementarily described. The upper line in the figure represents the time of the GPS clock at one second intervals, and t
0 is the current time, t-4, t-3, t-2, t-1, t +
1 indicates 4 seconds before, 3 seconds before, 2 seconds before, 1 second before and 1 second after t0, respectively. The lower line represents the time at which the INS position data is captured at 0.1 second intervals obtained by dividing the upper GPS clock into ten equal parts. As described above, there is a delay of about 2 seconds from when the satellite signal is received until the GPS position data is obtained. Therefore, the latest GPS position data at the current time t0 is at the time t-2. Since this is data based on the received satellite signal, this is called, for example, (t-2) GPS position data.

First, the calculation of the current position at the time when the GPS position data is measured (for example, t0) will be described. At time t0 when the GPS position data is measured, the GPS position data at (t-3) and the G at (t-2)
The gyro angle data at each time of the PS position data is fetched, and when the difference between the two gyro angle data is within the required measurement accuracy (that is, it can be regarded as a linear motion), (t−
The sum of the INS position data from the time point 3) to the time point (t-2) is added to the GPS position data of the time point (t-3) to obtain the position data of the time point (t-2). From the difference between the position data of -2) and the GPS position data of (t-2), the bias of the INS acceleration is obtained,
An INS speed error is output from the time (t-3) to the time (t-2). Then, the speed error of the INS is calculated as (t
The speed is corrected by adding or subtracting the speed at the time of -2).
The travel distance by the INS from the time of 2) to the current time t0 is recalculated, and the travel distance within this time is integrated (t−
The current position of the work vehicle V at the present time t0 is added to the GPS position data of 2).

On the other hand, when the difference between the rotation angles based on the gyro angle data is equal to or greater than the required measurement accuracy (that is, when the vehicle body is rotating), the speed is not corrected, and the current time is changed from the time (t-2). The travel distance by INS up to t0 is integrated, and this is added to the (t-2) GPS position data,
Assume that the current position of the work vehicle V at the current time t0.

Next, when the current time is a time at a finer time interval between the measurement times of the GPS position data, the integrated value of the travel distance by the INS from the time of the latest GPS position data to the current time is used. Is added to the latest GPS position data to obtain the current position of the work vehicle V.

As shown in FIG. 5, the work vehicle V has a plurality of work steps L arranged side by side in the rectangular field F along the length of the field in the transverse direction at the rightmost end of the figure. Starting from the start position St of the first work process L
The vehicle travels straight along each work process L to the end of the work process, turns 180 degrees from the end to the beginning of the adjacent work process L, and turns, and then reverses the work process L. The reciprocation traveling in the direction is repeated, and the vehicle is guided to travel over the entire range of the field F.

The structure of the steering control of the work vehicle V will now be described. As shown in FIGS. 1 and 4, the vehicle is driven by a rear portion of a vehicle body 5 having a pair of left and right front wheels 3 and rear wheels 4 at a lowered position. A seedling planting device 6 that can be switched between a ground working state and a non-working state other than the above is provided so as to be able to move up and down and stop driving. The front and rear wheels 3 and 4 are configured as a steering device that can be steered separately as a pair of left and right wheels. The steering hydraulic cylinders 7 and 8 and the electromagnetically operated control valves 9 and 10 are provided.
Are provided. That is, a two-wheel steering system in which only one of the front wheels 3 or the rear wheels 4 is steered, a four-wheel steering system in which the front and rear wheels 3, 4 are steered in opposite phases and at the same angle,
It is possible to select and use three types of steering systems, ie, a parallel steering system in which the front and rear wheels 3 and 4 are steered in the same phase and at the same angle. Note that, when the vehicle travels straight along each work process, a two-wheel steering system in which only the front wheels 3 are steered is performed.

In FIG. 4, E is the engine and 11 is the engine E
A continuously variable transmission that simultaneously drives each of the front and rear wheels 3 and 4 by shifting the output from the motor, 12 is an electric motor for the shift operation, 13 is a hydraulic cylinder for raising and lowering the planting device 6,
14 is a control valve thereof, 15 is an electromagnetically operated planting clutch for intermittently driving the planting device 6 by the engine E,
Reference numeral 16 denotes a control device using a microcomputer for controlling the running of the work vehicle V, the operation of the planting device 6, and the like, based on detection information by various sensors described later and data on a work process and the like stored in advance. The speed change motor 12, the control valves 9, 10, and 14, and the planting clutch 15 are each controlled.

The sensors mounted on the working vehicle V will be described. As shown in FIG. 4, steering angle detection sensors R1 and R2 using potentiometers for detecting the steering angles of the front and rear wheels 3 and 4 respectively. A vehicle speed sensor R3 using a potentiometer for indirectly detecting the forward and backward traveling state and the vehicle speed based on the shift state of the transmission 11, and an encoder S3 for counting the number of revolutions of the output shaft of the transmission 11 and detecting the traveling distance. Is provided.

Further, the control device 16 is used to control the vehicle position information based on the vehicle position information of the vehicle position detecting means 103 and the predetermined traveling control information set in advance corresponding to the predetermined traveling route (working stroke L). And the work vehicle V is in each of the work processes L
Control means 10 for performing steering control so as to travel along the road
The steering control means 101 further controls the steering so that the vehicle body direction of the work vehicle V follows the direction of the work path L based on the information of the geomagnetic direction sensor S4. It is configured.

Specifically, the steering control means 101 includes information on the vehicle body position (lateral position x) with respect to the proper steering position in each work stroke detected by the vehicle body position detecting means 103, and Body orientation φ for stroke L
And the steering angle θ of the front wheel 3 which is a steering device of the work vehicle V (this is detected by the steering angle detection sensor R1). Steering angle θ
The steering of the front wheel 3 is controlled by f. Note that k1, k2, and k3 are predetermined gain coefficients.

[0032]

Equation 1 θf = k1 · x + k2 · φ + k3 · θ

When the work vehicle V reaches the end of each work process L, the control device 16 turns the work vehicle V from the end of each work process toward the start of the next adjacent work process. It is configured to perform a directional operation. That is, in order to make the work vehicle V turn 180 degrees at the end of the stroke, as shown in FIG. 5, the vehicle travels straight a predetermined distance a from the end of the stroke, that is, the starting point e of the turning operation, and then turns 180 degrees. Is started, and an end point f of the turning operation is performed through a predetermined turning section g.
Is set so that the paths e to f reaching the desired turning trajectory are set.

Next, the operation of the control device 16 will be described with reference to the flowcharts shown in FIGS. In the main flow (FIG. 6), when the work vehicle V starts automatic traveling of the first work process L, time-series GPS position data and IN
The steering control is performed based on the position detection information based on the S position data, and when the planting start position is reached, the planting device 6 is lowered and the drive is started to start the planting operation.

When the vehicle further travels and reaches the point e at the end of the stroke, the planting device 6 is stopped and raised. Here, when it is determined that the work is completed based on the number of turns, the traveling is stopped and the entire process is terminated. If the work is not completed, the direction of the vehicle body at the turning start point is detected by the direction sensor S4, and after traveling straight for a predetermined distance from that point, the steering is switched from two wheels to four wheels, and the start of the next stroke is started. Of 180 degrees is performed. When you finish turning,
Switching the steering from four wheels to two wheels, the GPS
While performing steering control based on the position detection information based on the received data and the INS position data, traveling along the next work process L is started.

In the steering control process (FIG. 7), time-series GPS position data and INS position data are fetched, respectively, and the position x of the work vehicle V at the current time is calculated from both data. In S4, the body direction φ is detected,
Further, the steering angle θ of the front wheel 3 is detected. Then, the target steering angle θf is set, and the front wheels 3 are steered.

[Alternative Embodiment] The configuration of the inertial navigation system INS is not limited to the above embodiment. For example, instead of the acceleration sensor S6, a wheel tachometer for detecting the number of rotations of the wheels of the moving vehicle may be used, and the number of rotations of the wheels may be counted according to the time interval of position measurement to determine the moving amount of the moving vehicle. It may be.

Vehicle position detecting means 10 for determining the vehicle position of a moving vehicle based on GPS position data and INS position data.
The configuration 3 is not limited to the configuration described in the above embodiment, and various configurations are possible in consideration of conditions such as errors in each data.

In the above embodiment, the steering control means 101
Although the steering control is performed based on both the body position with respect to the proper steering position of the planned traveling route (working stroke L) and the body direction with respect to the planned traveling route, the steering control may be performed based only on the body position information. Good.

In the above embodiment, the present invention is applied to the rice transplanting work vehicle V as a mobile vehicle. However, the present invention is also applied to agricultural work vehicles other than rice transplantation vehicles and various types of mobile vehicles other than agricultural work vehicles. The specific configuration of each part at that time is appropriately changed according to the purpose of the mobile vehicle, the working conditions, and the like.

In the above embodiment, the planned traveling route is
Although the work route L is configured to travel while working, the travel route may simply move without working.

In the claims, reference numerals are provided for convenience of comparison with the drawings, but the present invention is not limited to the configuration shown in the attached drawings.

[Brief description of the drawings]

FIG. 1 is a schematic side view showing a mobile vehicle and a fixed station.

FIG. 2 is a block diagram showing a control configuration for detecting the position of a moving vehicle.

FIG. 3 is a time chart showing timing of position detection of a moving vehicle;

FIG. 4 is a block diagram showing a control configuration for steering control of a mobile vehicle.

FIG. 5 is a schematic plan view showing a planned traveling route.

FIG. 6 is a flowchart of a control operation.

FIG. 7 is a flowchart of a control operation.

[Explanation of symbols]

 R Fixed station V Mobile vehicle 20 Ground side communication means I Mobile station 18 Mobile vehicle side communication means 102 GPS position data calculation means INS inertial navigation system 103 Body position detection means 101 Steering control means S4 Direction detection means

──────────────────────────────────────────────────続 き Continued on the front page (72) Inventor Hiroshi Saito 1230 Mukaiyama, Nakamachi, Naka-gun, Naka-gun, Ibaraki Pref. (58) Fields surveyed (Int. Cl. ⁷ , DB name) G05D 1/00-1/12 G01S 5/00-5/14

Claims (2)

(57) [Claims] 1. A guidance control device for a mobile vehicle that automatically travels along a scheduled travel route, comprising: a fixed station (R) installed at a reference position on the ground and receiving a carrier signal from a GPS satellite; And ground-side communication means (20) for transmitting the carrier phase information at the fixed station (R) to the mobile vehicle (V). The mobile vehicle (V) receives a carrier signal from a GPS satellite. Mobile station (I)
A mobile-vehicle-side communication means (18) for receiving transmission information of the ground-side communication means (20); and a carrier-wave phase information at the mobile station (I) and the mobile-vehicle-side communication means (18). GPS position for obtaining the position of the mobile vehicle (V) with respect to the reference position as time-series GPS position data at predetermined time intervals based on double phase difference information obtained from carrier phase information at the fixed station (R). Data calculation means (10
2), an inertial navigation system (INS) for obtaining the amount of change in the position of the moving vehicle (V) as time-series inertial navigation position data at predetermined time intervals, and the GPS position data calculating means (102). GPS position data before a set time before the current time, and the inertial navigation system (IN
Body position detecting means (103) for obtaining the vehicle position of the moving vehicle (V) at the current time from the inertial navigation position data at the current time obtained in S), and the vehicle body of the vehicle position detecting means (103) Steering control for performing steering control so that the moving vehicle (V) travels along the scheduled traveling route based on position information and scheduled traveling control information set in advance corresponding to the scheduled traveling route. It means a (101) is provided, et al is, the vehicle body position detecting means (103), the GPS position de
Data by interpolating the inertial navigation position data
To find the body position of the moving vehicle (V) at the current time
A guidance control device for a mobile vehicle that is configured . 2. The vehicle (V) is provided with an azimuth detecting means (S4) for detecting an azimuth of the vehicle body with respect to the planned traveling route, and the steering control means (101) is provided with the azimuth detecting means (S).
The guidance control device for a mobile vehicle according to claim 1, wherein steering control is performed based on the information of (4) so that the vehicle body direction of the mobile vehicle (V) follows the direction of the planned traveling route. .