JPH09149706A - Traveling control device for autonomous running vehicle - Google Patents

Abstract

PROBLEM TO BE SOLVED: To prevent the meander of a traveling locus in reciprocating travel following the operation in a working zone and to enable sure and efficient working travel. SOLUTION: A control gain in a reaping track following travel control based on data from a reaping track boundary detecting part 52 is made into a small value and steering control is carried out corresponding to difference between a target angle in the reaping track following travel control and a traveling direction detected by a terrestrial magnetism 4. Consequently, the hunting of a steering system caused by the nonuniformity weeds and lawn grass and the unevenness of geographical features is avoided, the meander of a traveling locus is prevented and the deviation from detected width of the reaping boundary is checked.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a traveling control device for an autonomous vehicle that controls work traveling in work areas such as mowing and lawn mowing.

[0002]

2. Description of the Related Art In recent years, an autonomous vehicle capable of unmanned grass cutting and lawn mowing in various fields such as golf courses, river banks, parks and the like has been developed. Some of such autonomous vehicles are equipped with a system for detecting an azimuth by using an azimuth detecting sensor such as a geomagnetic sensor and controlling autonomous traveling. For example, Japanese Patent Laid-Open No. 59-1051.
No. 12 discloses the distance between each target point and the previous target point, the reference azimuth angle that is the angle between the geomagnetic direction and the traveling direction, and the traveling azimuth angle that is the angle between the traveling direction and the direction of the next target point. A technique is disclosed in which settings are stored in advance and the vehicle travels while reading the set position during actual travel.

In an autonomous traveling vehicle, there are cases where the vehicle travels back and forth with a turn of the vehicle body in a predetermined area due to the work. In Japanese Patent Laid-Open No. 3-135608, there is a reciprocal work of a field vehicle. There is disclosed a technique of setting reference azimuths of forward, backward, and backward azimuth by teaching by manually traveling an arbitrary one stroke or a plurality of strokes for an arbitrary time.

[0004]

When performing grass / lawn mowing work or the like by reciprocating straight running in a work area, usually, a follow-up running control for detecting the previous cut mark and following the cut mark. In this case, if the control gain of the follow-up traveling control is too large, the steering system may hunt due to unevenness of grass / turf or unevenness of the terrain, and the traveling locus may meander.

Therefore, it is conceivable to detect the traveling direction of the work traveling by a geomagnetic sensor or the like to control the work traveling. However, in the resolution of the direction detection by the direction detecting sensor such as the geomagnetic sensor, the traveling traveling is completely parallel. Is difficult to achieve, and when reciprocating over a long distance, not only the uncut area and the overlap amount increase, but also the work trace may deviate.

The present invention has been made in view of the above circumstances, and prevents meandering of a traveling locus in reciprocating traveling accompanying work in a work area and deviation from a work trace,
It is an object of the present invention to provide a traveling control device for an autonomous vehicle that enables reliable and efficient work traveling.

[0007]

According to the first aspect of the present invention,
In a traveling control device for an autonomous vehicle that controls reciprocal traveling associated with work in a work area, as shown in the basic configuration diagram of FIG. 1, an output from a work mark detection sensor that detects a work mark in the work area is used. Based on this, the current running direction is set so as to follow the previous work track, and the work track following running control means for controlling the follow run to the previous work track, and the running direction set by the work track following run control means. It is characterized by further comprising: travel control means for correcting the traveling direction of the vehicle in accordance with the difference from the current traveling direction detected by the direction detection sensor and controlling straight traveling in reciprocating traveling.

According to a second aspect of the present invention, in the first aspect of the invention, the work trace follow-up traveling control means sets the follow-up traveling based on the previous work trace as continuous proportional-plus-integral control. The control gain is set to a value capable of avoiding meandering during follow-up running.

According to a third aspect of the present invention, in the first aspect of the invention, the work trace follow-up traveling control means sets the follow-up traveling based on the previous work trace as an error amount from the target position for a set distance. Based on the straight traveling based on the vehicle, the traveling control means, for each set distance, according to the difference between the traveling azimuth set by the work trace following traveling control means and the current traveling azimuth detected by the azimuth detection sensor, It is characterized by correcting the traveling direction.

That is, according to the first aspect of the present invention, the traveling direction of this time is set so as to follow the previous work trace by the work trace detection sensor, and the direction detection sensor is used when controlling the following traveling to the previous work trace. The traveling direction of the own vehicle is corrected according to the detected difference from the current traveling direction, and straight traveling in reciprocating traveling is controlled.

In this case, according to the second aspect of the invention, the follow-up run based on the previous work trace is a continuous proportional-integral control in which the control gain is a value that can avoid meandering in the follow-up run,
In the invention according to claim 3, the follow-up travel based on the previous work trace is set to straight travel based on the amount of error from the target position during the set distance, and the work-trace follow-up travel control is performed for each set distance. The traveling direction of the vehicle is corrected according to the difference between the traveling direction and the current traveling direction detected by the direction detection sensor.

[0012]

Embodiments of the present invention will be described below with reference to the drawings. 2 to 17 show a first embodiment of the present invention, FIG. 2 is a block diagram of a control device, and FIG.
-Explanatory view showing a lawn mowing work vehicle equipped with a mobile station for GPS and a fixed station for D-GPS, Fig. 4 is an explanatory view showing a configuration of a sensor unit for detecting embedded magnetic steel, and Fig. 5 is a configuration of a cut boundary sensor unit. 6 is an explanatory view of magnetic steel detection based on the output of the magnetic sensor group, FIG. 7 is an explanatory view showing the operation of the cut boundary sensor unit, FIG. 8 is an explanatory view showing the configuration of the steering control system, 9 is an explanatory view showing a travel route and a work area, FIGS. 10 and 11 are flowcharts of a main control routine, FIGS. 12 and 13 are flowcharts of a mobile travel control routine, and FIG.
Flowchart of PS / dead reckoning mutual correction value calculation routine, FIG. 15 is a flowchart of current position calculation routine,
FIG. 16 is a flowchart of the work traveling control routine, FIG.
7 is a flowchart of a D-GPS wireless communication routine.

In FIG. 3 (a), reference numeral 1 indicates an autonomous traveling work vehicle which is self-propelled and is unmanned. In the present embodiment, it is a lawn mowing work vehicle for performing grass / lawn mowing work on a golf course or the like. This lawnmower vehicle is driven by the engine, and the steering angles of the front and rear wheels are independently controlled to enable highly accurate autonomous traveling. Due to this highly accurate autonomous traveling, radio waves from satellites are received. Satellite radio receiver to measure its own position by measuring the current position, dead reckoning sensor to measure the current position based on the driving history, sensor to detect running obstacles, cut boundary in the grass / lawn cutting work area. Equipped with a sensor for detecting the boundary of cuts for performing work traveling along (work trace), and in addition, in order to handle precise positioning when there are many obstacles or rough terrain, The sensor etc. which detect the magnetic field from the magnetic guidance path which generate | occur | produces the magnetic field for running guidance are mounted.

In the present embodiment, the satellite radio receiver is a global positioning satellite system (Global Positioning System).
em; hereinafter abbreviated as GPS) is a GPS receiver for receiving radio waves from GPS satellites and measuring its own position. Position information is measured by a fixed station located at a known point and correction information ( So-called differential GP which feeds back (differential information) to the mobile station.
A mobile station GPS receiver for S (hereinafter abbreviated as D-GPS).

As is well known, the factors of positioning errors by GPS include errors of satellite and receiver clocks, errors of satellite orbits, delay of radio waves by the ionosphere, delay of radio waves by the atmosphere, multipath, etc. In addition, the largest error factor is selectable availability (S / A)
There is a deliberate deterioration in accuracy called by the operator. Of the errors due to these factors, the in-phase error can be removed by using the correction information corresponding to each satellite captured by the fixed station at the known point, and the positioning accuracy at the mobile station is about several meters. Can be dramatically improved.

Therefore, the lawnmower vehicle 1 has an antenna 2 for a mobile station GPS receiver and an antenna 3 for a wireless communication device for receiving differential information from a fixed station.
Are installed upright, and at a known point outside the vehicle, as shown in FIG. 3B, the antenna 3 of the fixed station GPS receiver is installed.
1 and a fixed station 30 equipped with an antenna 32 of a wireless communication device for transmitting differential information to a mobile station GPS receiver.

As the dead reckoning sensor, a geomagnetic sensor 4 as a direction detecting sensor and a wheel encoder 5 are provided in the lawn mower 1 and an ultrasonic sensor or an optical sensor is used as the obstacle detecting sensor. Contactless sensors 6a and 6b such as sensors are attached to the front and rear parts of the lawn mowing work vehicle 1, and contact type sensors 7a and 7b using micro switches and the like are attached to the front and rear ends of the lawn mowing work vehicle 1. .

Further, as a sensor for detecting the magnetic field from the magnetic induction path, a buried magnetic steel detecting sensor section (magnetic field) in which a plurality of magnetic sensors are connected in series at a portion located at the rear side of the contact type sensor 7a at the front end of the vehicle body. A sensor unit) 8 is provided.
In the present embodiment, as shown in FIG. 4, a magnetized steel material (hereinafter referred to as magnetic steel) is embedded as a magnet, and is made of a horizontally long non-magnetic material installed in the width direction of the vehicle body. Nine magnetic sensors 8b on the mount base 8a
# 0 to 8b # 8 are attached with the sensitivity direction facing the ground, and the magnetic sensor unit 8 is configured.

On the other hand, a cutting blade mechanism 9 such as a mower for performing grass / lawn mowing work is provided at the lower part of the vehicle body of the lawn mowing work vehicle 1, and further, in order to detect a cut mark boundary of the grass / lawn mowing work. The cut mark boundary sensor unit 10 is provided as a work mark detection sensor that detects a work mark in the work area.

The cut boundary sensor section 10 is constructed by arranging two sets of grass / turf height detecting mechanisms in parallel on the left and right sides of the vehicle body in parallel. The front view of FIG. As shown in the side view of FIG. 2b), each mechanism is provided on the lower end of each rocking member 12a, 12b rotatably supported by the vehicle body 1a of the lawnmower working vehicle 1 via shafts 11a, 11b. Sled-shaped plates 13a and 13b that move up and down according to the length of the grass are rotatably suspended. The rocking members 12a, 12
b is fixed to the left and right shafts 11a and 11b, respectively, and the respective rotation angles are detected by the rotation angle sensors 14a and 14b attached to the left and right shafts 11a and 11b, respectively. .

In this case, the rocking members 12a and 12b described above are used.
Each of the sled-like plates 13 suspended from the vehicle body 1a via the
The a and 13b are light enough not to crush grass and turf, and when the lawn mowing vehicle 1 moves, they move up and down according to the height of the grass and turf, so that the rocking members 12a and 12b rotate. The rotation angle of each rotation angle sensor 14a, 14
b.

The lawnmower work vehicle 1 having the above-described structure is shown in FIG.
As shown in FIG. 5, a control device 50 including a microcomputer is mounted, and the control device 50 is connected with the above-mentioned sensors and actuators described later to perform the work trace following traveling control according to the present invention. The functions of the driving means, the traveling control means, and other control means are realized.

The details of the control device 50 will be described below. The control device 50 includes an embedded magnetic steel detection unit 51 to which the magnetic sensors 8b # 0 to 8b # 8 of the magnetic sensor unit 8 are connected, and a rotation angle sensor 14 of the cut boundary sensor unit 10.
a, 14b are connected to the cut boundary detection unit 52, the geomagnetic sensor 4 and the wheel encoder 5 are connected to the dead reckoning position detection unit 53, the mobile station GPS receiver 15 and the wireless communication device 16 are connected. The D-GPS position detector 54, the contactless sensors 6a, 6b and the obstacle detector 55 to which the contact sensors 7a, 7b are connected, and the respective detectors 51, 52, 53, 54, 55. A traveling control unit 56 connected thereto, a work data storage unit 57 in which a work data map referred to by the traveling control unit 56 is stored, and a vehicle control unit 58 for performing vehicle control according to an instruction from the traveling control unit 56. Further, the drive control unit 59 and the steering control unit 6 are provided to drive each mechanism unit of the lawnmower working vehicle 1 based on the output from the vehicle control unit 58.
0 and a cutting blade control unit 61 are provided.

The embedded magnetic steel detecting section 51 detects the magnetic lines of force from the magnetic steel embedded in the magnetic guide path and calculates the embedded position of the magnetic steel. That is, each magnetic sensor 8b # 0-8b # 8
From each output, the average value AVE and the maximum value MAX are obtained, and if the difference between the average value AVE and the maximum value MAX is equal to or more than the set value, it is determined that the magnetic steel is buried in the ground below the sensor. , Each magnetic sensor 8b installed laterally to the ground
The magnetic steel position is calculated from the positions # 0 to 8b # 8.

For example, as shown in FIG. 6, the relationship between the output of each magnetic sensor 8b # 0-8b # 8 and the sensor position is represented by the positions of # 0- # 8 corresponding to each sensor as the horizontal axis. When the output of the sensor is represented by the vertical axis, the maximum value MAX is obtained at the position of # 4 corresponding to the magnetic sensor 8b # 4 as shown in FIG. 6A, and the maximum value MAX and each magnetic sensor 8b # are obtained. 0-8b #
When the difference between the output of 8 and the average value AVE is equal to or more than the set value, # 3 adjacent to the position of # 4 showing the maximum sensor output
It is judged that the magnetic steel is buried between the positions from # 5 to # 5,
The position of magnetic steel is estimated to be # 4.5 by linear interpolation calculation.

On the other hand, as shown in FIG. 6 (b), the maximum value M of the sensor output at the position # 1 corresponding to the magnetic sensor 8b # 1.
AX is obtained, and the maximum value MAX and each magnetic sensor 8b # 0
If the difference between the sensor output of 8b # 8 and the average value AVE is smaller than the set value, it is determined that magnetic steel does not exist in the ground below the sensor.

Next, in the cut boundary detection unit 52, the rotation angle sensors 14a and 14b of the cut boundary sensor unit 10 are arranged.
The cutting angle signal corresponding to the grass / turf height is processed to calculate the grass / turf boundary position. That is, in the grass / lawn mowing work area, as shown in FIGS. 7A and 7B, the rotation angle θ1 detected by one rotation angle sensor 14a and the rotation angle θ1 detected by the other rotation angle sensor 14b. When the difference from the rotation angle θ2 is a certain value or more, that position is detected as a cut boundary between the already-cut part where grass / turf has already been cut and the uncut part where grass / turf has not yet been cut, The position data of the cut boundary is output to the traveling control unit 56.

In the dead reckoning position detecting section 53, the vehicle speed detected by the wheel encoder 5 is integrated to obtain the traveling distance, and the traveling distance is accumulated in correspondence with the change in the traveling direction detected by the geomagnetic sensor 4. By doing so, the travel history from the reference point is calculated, the current position of the vehicle is measured, and the positioning data is output to the travel control unit 56. The sensors connected to the dead reckoning position detection unit 53 include the geomagnetic sensor 4 and the wheel encoder 5.
The combination is not limited to the above, and a gyro or the like may be combined.

In the D-GPS position detector 54, a group of GPS satellites captured via the mobile station GPS receiver 15 (at least four in the case of three-dimensional positioning and at least three in the case of two-dimensional positioning). ) Navigation message from 70,
That is, satellite clock correction coefficient, orbit information, satellite calendar,
The position of the vehicle is measured with high accuracy based on positioning information such as satellite placement and the differential information received from the fixed station 30 via the wireless communication device 16, and the positioning data is output to the travel control unit 56. .

The fixed station 30 for the D-GPS position detector 54 is a D- to which a fixed station GPS receiver 33 is connected.
GPS fixed station section 34, D-GPS for transmitting the differential information from this D-GPS fixed station section 34
The information transmitter 35 includes a wireless communication device 36 connected to the D-GPS information transmitter 35.

In the D-GPS fixed station section 34, the satellite group 70 received via the fixed station GPS receiver 33.
And processing the positioning information from to create differential correction data. This differential correction data is
The D-GPS information transmitting unit 35 converts the packet data into wireless communication packet data and transmits the packet data via the wireless communication device 36.

In this embodiment, the D-GPS fixed station 30 is installed as a specific device for the mobile station of the lawnmower work vehicle 1, but it is a radio station for transmitting differential information. Existing D-GPS with
It is also possible to use a fixed station, or an existing D-GPS fixed station that transmits differential information via a communication satellite.

On the other hand, the obstacle detecting section 55 includes the non-contact type sensors 6a and 6b and the contact type sensors 7a and 7a.
An obstacle that cannot be predicted is detected by 7b, and a detection signal is output to the traveling control unit 56.

The traveling control unit 56 receives the positioning data from the buried magnetic steel detecting unit 51, the cut mark boundary detecting unit 52, the dead reckoning position detecting unit 53, and the D-GPS position detecting unit 54. By appropriately selecting and processing, the amount of error between the current position and the target position is calculated by referring to the work data of the work data storage unit 57, and the travel route and the vehicle control instruction are determined. When an obstacle is detected by the obstacle detection unit 55, an instruction to avoid the obstacle or stop the vehicle is issued.

Here, the traveling control of the lawnmower work vehicle 1 is mainly performed by positioning data from the dead reckoning position detecting section 53 and D-from the D-GPS position detecting section 54 with respect to the dead reckoning positioning data. Autonomous traveling control by D-GPS / dead reckoning that adaptively switches between data corrected by GPS positioning data (mutual correction value data), magnetic guidance traveling control based on data from the embedded magnetic steel detection unit 51, and It is divided into work traveling control in a work area in which data from the cut boundary detection unit 52 and data from the geomagnetic sensor 4 are combined.

In this case, when autonomously traveling over a wide range of normal terrain with relatively few obstacles and undulations, as will be described later, the moving distance to the target point is reduced based on D-GPS / dead reckoning. Accordingly, the positioning data by the D-GPS and the positioning data by the dead-reckoning navigation are switched to autonomously travel. However, in a terrain with many obstacles and undulations, accurate positioning is difficult only by the autonomous travel by the D-GPS / dead-reckoning navigation. . Therefore, the magnetic field from the existing magnetic induction path is detected,
Automatically switch from D-GPS / dead-reckoning autonomous driving to magnetically guided driving, and direct the steering of the front and rear wheels so that the position of the magnetic steel detected in the magnetically guided path is always in the center of the vehicle. .

Further, in the traveling control of the work traveling accompanying the grass / lawn mowing work in the work area, when the control gain is too large in the cutting trace following traveling control based on the data from the cut boundary detecting unit 52,・ Since the steering system may hunt due to turf unevenness or uneven terrain, the running trajectory may meander.Therefore, the control gain in the cut trail tracking control should be set to a small value, and the target azimuth angle in the cut trail tracking control should be The steering control is performed according to the difference from the traveling direction detected by the geomagnetic sensor 4.

In this case, the azimuth detection by the geomagnetic sensor 4 has such a resolution that it can detect a lateral displacement of 80 cm (about 1 degree) after traveling 50 m, and the traveling azimuth can be detected only by the data from the geomagnetic sensor 4. If determined, an error occurs in the parallelism of the forward and return paths, and when the round trip travels over a long distance, the uncut area and the overlap amount increase, but the cut marks based on the data from the cut boundary detection unit 52. By reducing the control gain of the follow-up running control and correcting the azimuth of the cut run with the azimuth detected by the geomagnetic sensor 4, it is possible to avoid the occurrence of meandering in the cut follow run, and to detect the cut boundary of the cut. It is possible to travel a long distance without departing from.

The work data storage unit 57 is composed of a ROM area for storing fixed data and a RAM area for storing work data under control execution. The ROM area has a work for performing grass / lawn mowing work. The terrain data of the area, the terrain data of the entire area including a plurality of work areas, the position data of the magnetic taxiway included in the terrain of the entire area, and the like are stored in advance. The RAM area stores the signals from the sensors. Processed data, positioning data by D-GPS,
Dead reckoning positioning data, mutual correction value data of D-GPS / dead reckoning described later, current position data of the vehicle calculated based on this mutual correction value data or dead reckoning positioning data, and in the magnetic taxiway Magnetic steel detection position data and the like are stored.

The vehicle control unit 58 converts the instruction from the traveling control unit 56 into a specific control instruction amount and outputs it to the drive control unit 59, the steering control unit 60, and the cutting blade control unit 61. As a result, the drive control unit 59 drives the travel control actuators 20 such as the throttle actuator, the speed change actuator, the forward / reverse switching actuator, and the brake actuator for adjusting the throttle opening to control the engine output, and the hydraulic pump. 21 is controlled to generate hydraulic pressure for driving each functional unit.

In the steering control unit 60, the front wheel steering angle sensor 25
a, steering control (steering amount feedback control) is performed via the front wheel steering hydraulic control valve 22a and the rear wheel steering hydraulic control valve 22b based on the input from the rear wheel steering angle sensor 25b. The servo control of the cutting blade mechanism 9 is performed via the cutting blade control hydraulic control valve 26.

As shown in FIG. 8, the steering system of the lawnmower working vehicle 1 includes a hydraulic pump 2 driven by an engine 19.
1, the front wheel steering hydraulic control valve 22a and the rear wheel steering hydraulic control valve 22b controlled by the steering control unit 60 are connected, and the hydraulic control valves 22a and 22b are respectively connected to
Front wheel hydraulic cylinder 23a, rear wheel hydraulic cylinder 23b
Are respectively connected to each of the hydraulic cylinders 23a, 2
3b, front wheel steering mechanism 24a, rear wheel steering mechanism 24b
Are driven independently.

When the steering angles of the front and rear wheels detected by the steering angle sensors 25a and 25b attached to the steering mechanisms 24a and 24b are input to the steering control section 60, the steering angles detected are The steering control unit 60 controls the steering mechanisms 24a and 24b via the hydraulic control valves 22a and 22b so as to eliminate the deviation from the target steering angle.

An example of unmanned grass / lawn mowing work in a plurality of divided work areas as shown in FIG. 9 will be described below. In the present embodiment, it is assumed that the lawnmower work vehicle 1 is waiting at an arbitrary preparation position 80 before starting work,
As shown by the broken line, a part of the path 81 leading from the preparation position 80 to the work area 82 and a part of the path 87 leading from the work area 85 to the return position 88 are terrain with many obstacles and undulations. Magnetic guideway 8 with magnetic steel embedded in each
1a and 87a are provided.

The movement from the preparation position 80 to the first work area 82, the grass / lawn mowing work traveling in the work area 82, the movement from the work area 82 to the next work area 85, the grass / lawn mowing work traveling in the work area 85 And return position 8
The movement to 8 is autonomously performed according to the main control routine shown in FIGS. 10 and 11 and the traveling control routine shown in FIGS. 12 and 13.

First, in the main control routine shown in FIGS. 10 and 11, in step S101, the preparation position 80, which is the current self position, is measured using the D-GPS. For this position measurement, D-GPS positioning data such as longitude and latitude (altitude data is also added if necessary) is used as the work data storage unit 5.
It is performed by converting into the data of the geodetic system stored in 7. In addition, the data conversion to this geodetic system is DG
It may be performed by the PS position detection unit 54, or may be performed by the travel control unit 56.

Next, in step S102, the topographical data of the first work area 82 is read out by referring to the work data storage unit 57, and from the measured preparation position 80 to the work start point, the path 81 including the magnetic guide path 81a is read. Is generated and the process proceeds to step S103. In step S103, a traveling control routine shown in FIGS. 12 and 13 which will be described later is executed to move the vehicle through the route 81 to the work start position. When the route 81 is moved, when the presence of the magnetic taxiway 81a is detected during autonomous traveling by D-GPS / dead reckoning, the vehicle is automatically switched to travel along the magnetic taxiway.

Then, the above steps S103 to S1
Proceeding to 04, when the hydraulic control valve 26 for controlling the cutting blade is opened to supply the hydraulic pressure to the cutting blade mechanism 9, the cutting blade is operated to start the grass / lawn mowing work. The grass and lawn mowing work is performed at a constant speed (for example, 3 to 6 km / h). If the traveling speed of grass / lawn mowing is too slow, the grass / lawn mowing work efficiency will deteriorate, and if it is too fast, uneven cutting will occur, so about 3 to 6 km / h is desirable.

In the following step S105, it is checked whether or not it is the first work, and if it is the first work, the process proceeds from step S105 to step S106, and the D-GPS / estimation obtained by the current position calculation routine of FIG. After the current vehicle position by navigation is read from the work data storage unit 57, the work data in the work data storage unit 57 is referred to in step S107, and the current position for the first path (one column) of the work in the work area 82 is calculated. Find the amount of error from the position.

Next, the process proceeds to step S108, and the above step
The steering amount for each target steering angle of the front and rear wheels is determined according to the error amount obtained in S107, and in step S109, the front wheel steering mechanism is operated via the front wheel steering hydraulic control valve 22a and the rear wheel steering hydraulic control valve 22b. 24a and the rear wheel steering mechanism 24b are respectively driven, and the front wheel steering angle sensor 25a and the rear wheel steering angle sensor 25b detect the front wheel steering angle and the rear wheel steering angle to perform control so as to obtain the target steering angle.

Then, in step S110, one step (one column)
It is checked whether or not the end point has been reached. If the end point has not been reached, the process returns to step S106 described above to continue the grass / lawn mowing work, and when the end point is reached, one section (current section It is determined whether or not the work of the work area of the work target) is completed.

In this case, since it is the first work, the process returns from step S116 to the above-mentioned step S105 to check again whether or not it is the first work, and when it is the second or later work, the process branches from step S105 to step S111. Then, the traveling control actuator 20 is driven to laterally shift the vehicle body by the width of the cutting blade to move it to the next working position, and in step S112 and thereafter, work traveling of the work path 83 along the cut boundary is performed.

The movement to the next work position is performed by the D-GP.
S. The dead reckoning navigation is performed based on the vehicle position. However, since the travel distance is extremely short, the dead reckoning positioning data is used directly in the present position calculation routine of FIG.

In step S112, a work traveling control routine shown in FIG. 16 which will be described later is executed, and the data from the geomagnetic sensor 4 and the data from the cut boundary sensor unit 10 are combined to travel along the cut boundary by the previous work. The azimuth is calculated to determine the steering amounts of the front and rear wheels. Next, in step S113, the front wheel steering mechanism 24a and the rear wheel steering mechanism 2 are operated via the front wheel steering hydraulic control valve 22a and the rear wheel steering hydraulic control valve 22b.
4b are respectively driven to control to obtain a target rudder angle and to control traveling while performing grass / lawn mowing work.

After that, in step S114, the current vehicle position by D-GPS / dead reckoning is read again from the work data storage unit 57. In step S115, it is checked whether or not the one-stroke end point has been reached. When the end point has not been reached, the process returns to step S112 described above and the work traveling along the cut boundary is continued, and when the end point of one stroke is reached, step S116 is performed.
Then, it is determined whether or not the work for one section (work area 82) is completed.

Then, steps S105 to S116 are repeated until the work in one section (work area 82) is completed, and when the work in one section is completed, the process proceeds from step S116 to step S117 to finish all the work. Determine whether or not. Here, since the work in the next work area 85 has not been completed yet, if the process returns to step S102 described above and the path 84 from the work area 82 to the work area 85 is generated by the same procedure,
According to the traveling control routine of FIG. 12 and FIG. 13, the robot moves to the next work area 85, and similarly, only the first one stroke DG
The grass and lawn mowing work is performed by PS / dead reckoning, and the grass and lawn mowing work is performed by the work traveling on the route 86 after the second time.

Eventually, when all the work is completed, step S1
When the process proceeds from 17 to step S118 and a path 87 including the magnetic guide path 87a from the work area 85 to the return position 88 is generated with reference to the work data storage unit 57, in step S119,
According to the traveling control routine shown in FIGS. 12 and 13, the vehicle is moved to the return position 88, the routine is terminated, and the vehicle is stopped.

Next, traveling control on the routes 81, 84, 87 by the traveling traveling control routine shown in FIGS. 12 and 13 will be described. In the main control routine described above, the routes 81, 84 and 87 are generated from the positioning data of the self position and the work data of the work data storage unit 57. However, the routes 81, 84 and 87 themselves are previously generated. It may be stored in the work data storage unit 57.

In this routine, when the target point, the target trajectory, and the preset designated traveling speed stored in the work data storage unit 57 are read in step S201,
In S202, it is determined whether or not the movement is completed. If the movement is completed, the vehicle is stopped and the routine is exited in step S203. If the movement is not completed, the process proceeds to step S204.

In step S204, the moving speed of the lawnmower working vehicle 1 detected by the wheel encoder 5 is the above-mentioned step S2.
The output of the engine 19 is controlled via the throttle actuator that constitutes one of the traveling control actuators 20 so that the specified traveling speed read out at 01 is reached, and the vehicle travels at the specified traveling speed. Whether or not it is permitted, that is, the starting point of the magnetic guidance path (in this embodiment, the magnetic guidance path 81a included in the path 81 or the magnetic guidance path 87a included in the path 87) is the target point for the time being, and the magnetic guidance Check whether preparations for switching to driving are permitted.

When the magnetically guided traveling is permitted, the routine proceeds from step S205 to step S206 and thereafter to perform traveling control based on the detected position of the buried magnetic steel. When the magnetically guided traveling is not permitted, the above step is performed. The process proceeds from step S205 to step S211 and thereafter, and travel control is performed by D-GPS and dead reckoning.

First, a case will be described in which the magnetically guided traveling is permitted and the process proceeds from step S205 to step S206. In step S206, the magnetic sensors 8b # 0-8b
The output from # 8 is checked to determine whether the embedded magnetic steel is detected. When the embedded magnetic steel is not detected, it is determined that the magnetic induction path (magnetic induction path 81a or magnetic induction path 87a) has not been reached. In order to execute self-position detection by D-GPS and dead reckoning, the process proceeds to step S211 and subsequent steps, and when the embedded magnetic steel is detected, it is determined that the magnetic guideway is reached and the process proceeds to step S207.

At step S207, the magnetic sensors 8b # 0-8 #
Calculate the embedding position of the magnetic steel from the information of b # 8, and step S208
When the steering amounts of the front and rear wheels are calculated so that the magnetic steel detection position becomes the center of the vehicle body in step S209, the front wheel steering mechanism 24a, the rear wheel steering mechanism 24a, the rear wheel steering hydraulic control valve 22b, the rear wheel steering hydraulic control valve 22b in step S209. Each of the wheel steering mechanisms 24b is driven and controlled so as to obtain the target steering angle.

After that, the process proceeds to step S210, and it is checked whether or not the magnetic steel to be the end mark indicating the end point of the magnetic guideway is detected. For example, when the magnetic induction path is made of magnetic steel of N pole, magnetic steel of S polarity is detected by embedding the magnetic steel of S pole as an end mark at the end point of the magnetic induction path. At this time, it can be determined that the end point of the magnetic guide path has been reached.

When the magnetic steel of the end mark is not detected, the process returns to step S207 and the magnetic guide path (the magnetic guide path 81a or the magnetic guide path 87 due to the detection of the embedded magnetic steel).
When the traveling control according to a) is continued and the magnetic steel of the end mark is detected, the process returns to step S201, and the process for a new target point that autonomously travels by D-GPS / dead reckoning is performed.

As a result, when an terrain with many obstacles and undulations is reached during autonomous driving by D-GPS / dead reckoning,
Since the magnetic field from the pre-established magnetic guidance path is detected and automatically switched to magnetic guidance travel, accurate positioning is possible regardless of obstacles and terrain ups and downs, and the target is maintained without departing from the travel route. You can definitely reach the point. Moreover, since the vehicle normally runs autonomously by D-GPS and dead reckoning, and guides the vehicle by the magnetic taxiway only when precise positioning is required, the installation cost of the magnetic taxiway is kept to the minimum necessary. You can

On the other hand, in the traveling control by D-GPS and dead reckoning after step S211, the current position of the own vehicle obtained by the current position calculation routine of FIG. 15 which will be described later is read from the work data storage unit 57 in step S211. Then, in step S212, the target point is compared with the current position,
The target traveling azimuth angle is calculated.

Next, in step S213, the current advancing azimuth detected by the geomagnetic sensor 4 is read out, and in step S214, the error amount of the advancing azimuth is calculated from the target advancing azimuth and the current advancing azimuth. Then, the steering amounts of the front and rear wheels are determined according to the error amount, the process proceeds to step S215, and the front wheel steering hydraulic control valve 22 is determined according to the determined front and rear wheel steering amount.
The front wheel steering mechanism 24a and the rear wheel steering mechanism 24b are respectively driven via a and the rear wheel steering hydraulic control valve 22b to perform control so as to obtain the target steering angle.

Then, in step S216, the current position is compared with the target position, and the process proceeds to step S217 to determine whether or not the target position has been reached. As a result, when the target position has not been reached, the process returns to step S212, the target traveling azimuth angle is calculated again, the traveling is continued, and when the target position is reached,
Returning to step S201, the target point, the target trajectory, and the designated traveling speed are read out, and the same processing is repeated.

Now, the measurement of the current position of the vehicle by D-GPS / dead reckoning will be described.

In the self-position measurement by D-GPS, much better accuracy can be obtained than in the case of a single GPS, but it is necessary for autonomous traveling control depending on the satellite acquisition state, the radio wave reception state, and the like. The accuracy required for timing may not be obtained. Therefore, the D-GP of FIG.
In the S / dead reckoning mutual correction value calculation routine, regardless of whether the vehicle 1 is running or stopped, the D-GPS is always used.
Mutual position correction values (mutual correction values) including both dead reckoning and dead reckoning are calculated, and when the moving distance to the target point exceeds a preset value by the current position calculation routine of FIG. The current position is calculated by correcting the dead reckoning positioning data with a value obtained by averaging the difference between the D-GPS positioning data and the dead reckoning positioning data, and when the moving distance to the target point is less than or equal to the above set value, the dead reckoning navigation is performed. Calculate current position using positioning data directly.

The D-GPS / dead reckoning mutual correction value calculation routine will be described below. This D-GPS / dead-reckoning mutual correction value calculation routine is processed in the background by time sharing or the like so that the latest correction value can always be referred to. In step S301, D- in FIG. Waiting for the input of the positioning result Pg by the differential calculation from the GPS wireless communication routine,
-When the GPS positioning result Pg is input, the process proceeds to step S302 to read the dead reckoning position data Ps at the time corresponding to the D-GPS positioning time.

Next, in step S303, the D-GPS is used.
If the difference between the positioning result Pg by P and the position data Ps by dead reckoning is calculated as the mutual correction value K (K = Pg−P
s), in step S304, the moving average value Ka of the past n points including the currently calculated mutual correction value K is calculated, and the moving average value K stored in the RAM area of the work data storage unit 57 is calculated.
a is updated, the process returns to step S301, and the next D-GPS
Wait for input of positioning result Pg. It should be noted that the value of n is determined in advance as an optimum value depending on the precision of dead reckoning and the magnitude of accumulated error.

The latest averaged mutual correction value Ka calculated in the background as described above is read into the current position calculation routine of FIG. 15, and when the moving distance to the target point is relatively long, dead reckoning navigation is performed. The current position is calculated by applying the mutual correction value Ka to the positioning data according to, and in the case of movement of an extremely short distance, the current position is calculated based on the dead reckoning positioning data.

First, in step S401, the current position data Psn by dead-reckoning is read out, and in step S402, the moving distance S to the target point is compared with a set value Df set in advance in consideration of controllability of the system. If S> Df, the process proceeds to step S403, and the latest averaged mutual correction value Ka calculated by the D-GPS / dead-reckoning mutual correction value calculation routine is added to the current position data Psn by dead-reckoning navigation. Is added to obtain the current position P, this data is stored in the work data storage unit 57, and the routine exits.

That is, since the difference between the D-GPS positioning data and the dead reckoning positioning data is always obtained as a mutual correction value and accumulated and the moving average is calculated, it is temporarily changed depending on the satellite acquisition state or the radio wave reception state. Even when the positioning accuracy of the D-GPS decreases, the dead reckoning positioning data is corrected by the latest moving average mutual correction value, so that the vehicle 1 is stopped at a certain point for a predetermined time. It is possible to always accurately calculate the current position without the need to take measures such as accumulating the positioning data of (1) to improve the accuracy.

On the other hand, in step S402, S ≦ Df
If the moving distance to the target point is extremely short, the process branches from step S402 to step S404 and the dead reckoning current position data Ps read in step S401.
Since n is directly passed as the current position P, it is stored in the work data storage unit 57, and the routine is exited.

Current position data Psn by this dead reckoning
In the present embodiment, the situation in which is used directly without being corrected by the positioning data by D-GPS is the movement to the next work position in the work area (step S111 in the main control routine;
(See FIG. 10), the control stability is improved by stopping the correction of the dead reckoning positioning data by the D-GPS positioning data during an extremely short distance movement that cannot be handled by the D-GPS positioning accuracy. It is possible to improve the sex.

Next, the work traveling control routine of FIG. 16 for executing the processing for determining the traveling direction and the steering angle during the traveling in the grass / lawn mowing work area (step S112 of the main control routine; see FIG. 10) will be described.

In this work traveling control routine,
In S501, the position of the cut boundary is calculated from the output of the cut boundary sensor unit 10. That is, the left and right rotation angle sensors 14a and 1b are attached to the respective swing members 12a and 12b that suspend the left and right sled-shaped plates 13a and 13b that move up and down according to the height of grass / lawn cutting.
The angle of each rocking member 12a, 12b is detected by 4b, and the data is accumulated and averaged. Then, referring to the grass / lawn cutting height data in the target work area, the data is converted into left and right grass / turf heights, and when the converted left and right grass / turf heights have a step difference of a predetermined value or more, The current position is captured as the boundary line between the already-cut part and the uncut part.

Then, the process proceeds to step S502, and the above step
The amount of error ERR between the cut boundary position calculated in S501 and the target position of the cut boundary for realizing the preset lawn mowing overlap amount is calculated, and in step S503, this error amount ERR is calculated.
Is multiplied by a proportional control gain Kp in proportional-plus-integral control to obtain a proportional control amount Ap (Ap ← Kp × ERR), and
In step S504, the integrated value SUM (ERR) of the error amount ERR
Is multiplied by the integral control gain Ki to obtain the integral control amount Ai (Ai ← Ki × SUM (ERR)). In this case, as the proportional control gain Kp and the integral control gain Ki, values preset according to the vehicle characteristics are used, and both are extremely small values.

Then, the process proceeds to step S505, and the proportional control amount Ap and the integral control amount Ai obtained in the above steps S503 and S504.
Is added to the reference advancing azimuth Aref corresponding to the target position of the cut, and the target advancing azimuth A is set (A ← Aref + Ap + A
i) In step S506, the steering angles of the front and rear wheels are proportionally controlled from the difference between the current traveling azimuth Areal obtained from the geomagnetic sensor 4 and the target traveling azimuth A, and the routine is exited.

That is, the azimuth angle based on the cut boundary calculated by the cut boundary sensor unit 10 and the error amount ERR from the target position
When proportional control is performed on the basis of the following, the proportional control gain Kp and the integral control gain Ki are both set to extremely small values to avoid the occurrence of meandering in the cut trail following traveling, and after the traveling for a comparatively long distance, The azimuth angle detected by the geomagnetic sensor 4 is corrected so that the azimuth angle detected by the geomagnetic sensor 4 is corrected so as not to deviate from the detection width. You can perform high-quality grass and lawn mowing work.

Data communication between the fixed station 30 and the mobile station in D-GPS is performed by packet data by the D-GPS wireless communication routine shown in FIG. In this data communication, the mobile station GPS receiver 15 is initialized in step S601, and the fixed station GPS receiver 33 is initialized by data transmission through the wireless communication devices 16 and 36 in step S602. Then, the differential information from the fixed station 30 is obtained by wireless data communication.

Next, in step S604, the D-GP
The S position detection unit 54 applies the differential information from the fixed station 30 to the positioning data obtained from the mobile station GPS receiver 15 and performs the differential calculation to measure the own vehicle position. Then, when the positioning information is sent to the traveling control unit 56, the process returns to step S603, and the next data processing is repeated. In this case, the differential calculation with the fixed station 30 may be performed by a function unique to the mobile station receiver 15.

FIG. 18 relates to a second embodiment of the present invention,
It is a flowchart of a work traveling control routine.

In the present embodiment, the traveling control in the work area in the above-mentioned first embodiment is continuous proportional-integral control, whereas a checkpoint is provided for each constant traveling distance, and a cut boundary is provided for each checkpoint. The collective correction is performed by referring to the position.

That is, in the work traveling control routine of this embodiment, first in step S701 of FIG. 18, it is checked whether or not the vehicle has traveled the set distance DCK from the previous check point to the current check point, and the set distance DCK is set. When it has not reached, the routine is exited, and when it reaches the set distance DCK, the process proceeds to step S702, where the cut boundary position is calculated from the output of the cut boundary sensor unit 10 in the same manner as in the first embodiment described above, and the step Proceed to S703.

In step S703, the error amount ERR between the cut boundary position calculated in step S702 and the cut boundary target position for realizing the preset lawn mowing overlap amount.
Then, in step S704, the correction amount θ for setting the error amount ERR to 0 is set while traveling the set distance DCK to the next check point (θ ← tan ⁻¹ (ERR / DC
K)).

Next, in step S705, the above step
The correction amount θ set in S704 is added to the reference traveling azimuth Aref to set the target traveling azimuth A (A ← Aref + θ + A
i) In step S706, the steering angle of the front and rear wheels is proportionally controlled based on the difference between the current traveling azimuth Areal obtained from the geomagnetic sensor 4 and the target traveling azimuth A, and the routine exits.

That is, in this embodiment, the set distance D
Between check points separated by CK, a straight traveling is performed by correcting the traveling direction detected by the geomagnetic sensor 4 so that the target traveling direction based on the amount of error from the previous cut mark boundary position is corrected. Does not occur, and there is no need to perform gain tuning according to the characteristics of the vehicle as in the first embodiment.

[0092]

As described above, according to the present invention, the traveling direction of this time is set so as to follow the previous work trace by the work trace detection sensor, and when the follow-up traveling to the previous work trace is controlled, Correct the traveling direction of the vehicle according to the difference from the current traveling direction detected by the detection sensor and control the straight traveling in the reciprocating traveling, so the control gain in the work trace tracking control is appropriately set or set. Correcting the traveling azimuth by work trace tracking control for each distance with the azimuth detected by the azimuth detection sensor, etc. to ensure parallelism in reciprocating traveling, to prevent meandering of the traveling trace and to prevent deviation from the work trace. It is possible to perform reliable and efficient work traveling.

[Brief description of the drawings]

FIG. 1 is a basic configuration diagram of the present invention.

FIG. 2 is a block diagram of a control device according to the first embodiment of the present invention.

FIG. 3 is an explanatory diagram showing a lawnmower equipped with a mobile station for D-GPS and a fixed station for D-GPS.

FIG. 4 is an explanatory diagram showing a configuration of a sensor unit for detecting embedded magnetic steel, which is the same as the above.

FIG. 5 is an explanatory diagram showing a configuration of a cut boundary sensor unit as above.

FIG. 6 is an explanatory diagram of magnetic steel detection based on the output of the magnetic sensor group.

FIG. 7 is an explanatory diagram showing the operation of the cut boundary sensor unit as above.

FIG. 8 is an explanatory diagram showing a configuration of a steering control system of the above.

FIG. 9 is an explanatory diagram showing a traveling route and a work area of the same.

FIG. 10 is a flowchart of a main control routine of the same as above.

FIG. 11 is the same as the flowchart of the main control routine (continued).

FIG. 12 is a flowchart of a mobile traveling control routine same as above.

FIG. 13 is the same as the flowchart of the traveling control routine (continued).

FIG. 14 is a flowchart of a D-GPS / dead-reckoning mutual correction value calculation routine.

FIG. 15 is a flowchart of the same position calculation routine as above.

FIG. 16 is a flowchart of a work traveling control routine same as above.

FIG. 17 is a flowchart of a D-GPS wireless communication routine same as above.

FIG. 18 is a flowchart of a work traveling control routine according to the second embodiment of the present invention.

[Explanation of symbols]

1 ... Lawn mowing work vehicle (autonomous vehicle) 4 ... Geomagnetic sensor (direction detection sensor) 10 ... Mow boundary sensor unit (work trace detection sensor) 50 ... Control device (work trace tracking travel control means, travel control means) Kp ... Proportional control gain (control gain) Ki ... Integral control gain (control gain) DCK ... Set distance

─────────────────────────────────────────────────── ─── Continuation of the front page (51) Int.Cl. ⁶ Identification code Internal reference number FI Technical display location G01S 5/14 G01S 5/14

Claims (3)

[Claims] 1. A traveling control device for an autonomous vehicle that controls reciprocal traveling associated with work in a work area, wherein a previous work is performed based on an output from a work mark detection sensor that detects a work mark in the work area. The current direction of travel is set to follow the trace, and it is detected by the work trace tracking control means that controls the following trace of the previous work trace, and the travel bearing and the direction detection sensor set by the above work trace tracking control means. A traveling control device for an autonomous traveling vehicle, comprising: traveling control means for correcting the traveling direction of the vehicle in accordance with the difference from the current traveling direction and controlling straight traveling in reciprocal traveling. 2. The work trace follow-up running control means sets the follow-up run based on the previous work trace as continuous proportional-plus-integral control,
The traveling control device for an autonomous traveling vehicle according to claim 1, wherein the control gain in the proportional-plus-integral control is set to a value capable of avoiding meandering during follow-up traveling. 3. The work trace follow-up traveling control means sets the follow-up traveling based on the previous work trace as straight traveling based on an error amount from a target position for a set distance, and the travel control means comprises the set distance. The traveling direction of the vehicle is corrected according to the difference between the traveling direction set by the work trace following traveling control means and the current traveling direction detected by the direction detecting sensor for each time. Control device for autonomous vehicle.