JP6497546B2 - Travel control device - Google Patents

Description

  The present invention relates to a traveling control device that controls traveling of a vehicle.

  In operations such as sowing, transplanting, setting up, padding, and material dispersal using agricultural vehicles, in order to improve the accuracy and efficiency of the operation, and for subsequent operations such as management and harvesting, each process goes straight forward. It is important to do so in parallel. Since these operations often take a long time in a wide field and cannot be performed again, there are large physical and mental burdens on the worker, and skill is required. For this reason, there is a great need for an automatic straight traveling technology that travels straight on agricultural vehicles and in parallel with the adjacent fence.

  As a technique for controlling the running of the vehicle, there are techniques as shown below. For example, Patent Literature 1 discloses a travel control device that detects a vine or cocoon in a field as a target tracking line from a captured image by image processing and performs control so as to travel along the target tracking line.

  Patent Document 2 discloses a technique in which a tractor is provided with a GPS device and a direction sensor, and a steering angle is controlled using a steering angle sensor based on the detection results to automatically travel.

  Patent Document 3 discloses a vehicle that sets a traveling lateral target route on a traveling path of the host vehicle from lanes detected by two lane recognition cameras and controls the host vehicle to travel along the traveling lateral target route. A traveling control device for a vehicle is disclosed.

JP 2013-201958 A JP 2002-186309 A JP 2013-180638 A

  In an inclined field, a skidding condition may occur as the vehicle travels. In the technique described in Patent Document 1, if the side skid of the vehicle continues during automatic traveling control that causes the vehicle to follow the target tracking line, the vehicle may move away from the target tracking line.

  The technique described in Patent Document 2 is to perform work by running a tractor on a preset route, and since the process of an adjacent kite or kite is not reflected, the position of the adjacent process changes. There is a risk that the interval between adjacent strokes and the degree of parallelism will decrease.

  The technique described in Patent Document 3 is intended for a vehicle traveling on a paved road, and does not disclose the correspondence of a state in which a tire skids on an inclined field. Further, the technique described in Patent Document 3 has a high friction between the tire and the paved road surface, so that the steering method can be reached without correcting the front target position with an arc-shaped locus by maintaining a certain steering angle. Is disclosed. On the other hand, a tractor traveling on soft soil has an unstable friction between the tire and the soil, and the direction of the tractor may change unexpectedly due to the influence of a soil mass in the soil. For this reason, as a method for steering the tractor, it is necessary to apply a method in which the direction of the tractor is directed to the target position with respect to the target position, and the head is moved straight toward the target position.

  The present invention has been made in view of such circumstances, and an object of the present invention is to provide a travel control device capable of accurately following the position of a line to be followed.

  In order to solve the above-described problem, a travel control device according to an aspect of the present invention includes an imaging unit that captures an image of the ground on which the vehicle travels and a distance from the vehicle, a captured image captured by the imaging unit, and the vehicle follows the vehicle. An image processing unit that specifies a target position and calculates a target rudder angle, and a travel control unit that controls the vehicle to travel at the target rudder angle. The image processing unit determines the vanishing point from the captured image, and compares the change in the position of the reference area set on the ground in the captured image with the position of the vanishing point to calculate the amount of movement before and after the vehicle and to the left and right A vanishing point tracking unit, a first correction unit that calculates a first correction amount based on a lateral displacement amount of the vehicle calculated by the vanishing point tracking unit, and a vehicle movement calculated by the vanishing point tracking unit Based on the amount, the current position information of the past target position that has gone out of the field of view of the captured image is estimated, and a second correction amount is calculated based on the amount of positional deviation between the vehicle and the past target position. The target tracking line is identified by combining the position information obtained by tracking the target position toward the distance using the luminance distribution information of the captured image and the positional information of the past target position, and based on the curvature of the target tracking line. A third correction unit for calculating a third correction amount and a target position To have a first correction amount and the target steering angle calculating section for calculating a target steering angle based on either the second correction amount and the third correction amount.

  According to this aspect, since it is possible to process the captured image captured by the imaging unit and control the traveling, the traveling control can be easily realized at low cost. In addition, when a side slip occurs in an inclined field or the like, the side slip of the vehicle is detected from the captured image and a correction amount is calculated.If the target follow line has a curvature, the curvature of the target follow line is calculated. By detecting from the captured image, calculating the correction amount, and calculating the target steering angle of the vehicle reflecting the correction amount, the vehicle can be moved with high accuracy toward the target position in the real space.

  ADVANTAGE OF THE INVENTION According to this invention, it can be made to carry out a follow-up driving | running | working with the target follow-up line accurately in the travel control apparatus of a vehicle.

It is a side view of the traveling control device concerning an embodiment. It is a top view of a travel control device. It is a figure which shows the function structure of ECU which concerns on embodiment. It is a figure which shows the function structure of the image process part which concerns on embodiment. FIG. 5A is a diagram illustrating a captured image at a past time point when the reference area is set, and FIG. 5B is a captured image at a position where the tractor is advanced after the reference area 104 is set. 60 (c) is a partially enlarged view of the captured image 60 shown in FIG. 5 (b). FIGS. 6A to 6C are diagrams illustrating the captured images illustrated in FIGS. 5A to 5C with photographs. It is a figure for demonstrating the method to calculate the height distribution of a ground from a captured image. It is a figure for demonstrating the method of calculating several height distribution simultaneously by continuous image processing. It is a figure which shows the captured image shown in FIG. 8 with a photograph. It is a figure for demonstrating the method to detect a groove | channel from the detected unevenness | corrugation. It is a figure which shows the captured image shown in FIG. 10 with a photograph. It is a figure for demonstrating the method of calculating the movement locus | trajectory of a vehicle by tracking a reference | standard area | region. It is a figure for demonstrating calculation of the correction amount by a 1st correction | amendment part. It is a figure for demonstrating calculation of the correction amount by a 2nd correction | amendment part, and is a top view which looked down at the ground. It is a figure for demonstrating the method to detect a distant step part. It is a figure which shows the captured image shown in FIG. 15 with a photograph. It is a figure for demonstrating the curvature of a target driving | running | working line, and shows the imaging range, the detected target position, and the position of a reference step part area | region in the top view which projected the captured image demonstrated in FIG.15 and FIG.16 on the ground FIG. It is a top view on the ground for demonstrating the correction method of steering in case the target driving | running | working line is curving. It is a figure for demonstrating the correction | amendment of a 3rd correction | amendment part.

  The work vehicle may be superior in traction force and turning force as compared with a normal general vehicle, but may have low straight-ahead performance. In addition, the driver is forced to perform a straight-ahead operation while looking back, in order to confirm the working state of the work machine provided at the rear of the work vehicle. Some fields have a length of 500 m or more, and it is burdensome for the driver to go straight through this distance. Therefore, the travel control device of the embodiment calculates a target position that should be followed by traveling such as wrinkles and wrinkles from an image obtained by capturing the traveling direction, and causes the vehicle to travel toward the target position. Reduce the burden. Further, since the vehicle can slip while traveling in an inclined farm field, the steering angle toward the target position is corrected based on information obtained from the captured image.

  FIG. 1 is a side view of a travel control device 20 according to the embodiment. FIG. 2 is a top view of the travel control device 20. In the description of the drawings, the same elements are denoted by the same reference numerals, and repeated descriptions are omitted as appropriate. The vehicle 1 includes a vehicle cockpit 3, a left front wheel 8a and a right front wheel 8b (hereinafter collectively referred to as “front wheel 8”), a left rear wheel 10a and a right rear wheel 10b (hereinafter collectively referred to as “rear wheel 10”). The working machine 16. The travel control device 20 is mounted on the vehicle 1, and includes a left brake mechanism 6a and a right brake mechanism (not shown) (hereinafter collectively referred to as “brake mechanism 6”), a camera 11, an ECU (Electronic control unit) 12, A steering drive mechanism 14 and a steering mechanism 15 are provided.

  The vehicle 1 may be a work vehicle such as an agricultural tractor or a civil engineering work vehicle, and a work machine 16 is attached to the rear of the vehicle 1. The work machine 16 performs operations such as sowing and setting in the farm field 4, for example.

  The steering mechanism 15 includes a steering wheel, a steering shaft, and a gear device that converts the movement of the steering shaft into the movement of the front wheels. The steering mechanism 15 converts the rotation of the steering wheel as a handle into the turning motion of the front wheels 8. The steering wheel is provided in the vehicle cockpit 3 and is turned by the driver. The steering shaft has one end connected to the steering wheel so as to rotate together with the steering wheel, and functions as a rotating shaft that transmits the rotation of the steering wheel to the gear device.

  The steering drive mechanism 14 includes a steering angle sensor (not shown) and a motor (not shown). The steering drive mechanism 14 drives a motor and gives a steering force to the steering mechanism 15. The steering angle sensor is provided on the steering shaft, the front wheel 8, and the like, and detects the steering angle and steering direction of the steering wheel. The steering drive mechanism 14 is connected to the ECU 12. The detection value of the steering angle sensor is output to the ECU 12. The steering drive mechanism 14 may be a hydraulic type, and may drive a hydraulic pump to give a steering force to the steering mechanism 15.

  The brake mechanism 6 applies a braking force to the rear wheel 10 in accordance with an operation amount of a driver's brake pedal (not shown). The brake pedal includes a left rear wheel brake pedal and a right rear wheel brake pedal. When the driver operates each brake pedal, a braking force can be individually applied to the left rear wheel 10a and the right rear wheel 10b. For example, when turning the vehicle 1 in the right direction, the driver depresses the brake pedal for the right rear wheel to apply a braking force to the right rear wheel 10b, thereby turning the vehicle 1 around the right rear wheel 10b. Can do. The brake mechanism 6 is connected to the ECU 12, and the braking force applied to the rear wheel 10 by the ECU 12 can be controlled. The left or right brake mechanism 6 is operated by the ECU 12 and the vehicle 1 can be turned. The front wheel 8 of the vehicle 1 may not have a brake mechanism.

  A monocular camera 11 is disposed at the upper part of the vehicle cockpit 3. The camera 11 functions as an imaging unit and images the ground and the horizon in the traveling direction, which are targets for vehicle travel. The camera 11 outputs the captured image to the ECU 12.

  In FIG. 2, the step 18 in the first stroke, the step 19 in the second stroke, the step 22 in the third stroke, and the step 24 in the fourth stroke are provided in the farm field 4, and the vehicle 1 is in the third stroke. The vehicle is traveling on the step 22. Each step portion is, for example, a ridge formed in advance in the field 4, a work trace or a tilling boundary of a previous stroke in a cultivating operation, or a groove on a line formed as a traveling target. In the example of the vehicle 1 shown in FIG. 2, the step portion 22 is detected by the camera 11, the step portion 22 is set as a target travel line, a travel target position 26 is set on the line, and the vehicle travels toward the target position 26. To be controlled. The step portion 22 is a line-shaped groove formed as a target for traveling, for example.

  The position of the vehicle 1 when the driver turns on an automatic travel switch (not shown) connected to the ECU 12 is referred to as an initial position of the vehicle 1 and refers to a position at which travel control by the travel control device 20 is started. After the driver completes one stroke, the driver turns off the automatic travel switch, steers the vehicle 1 to change direction, moves the vehicle 1 to a position for traveling the next stroke, and turns on the automatic travel switch. Then, the traveling control device 20 starts automatic traveling and starts traveling in the next stroke.

  The traveling control by the traveling control device 20 is executed / stopped by turning on / off the automatic traveling switch by the driver. For example, when the driver turns on the automatic travel switch, the ECU 12 detects the step 22 from the captured image output from the camera 11 as a target travel line, and travel control that follows the target travel line is executed. The step control which detects the step part 18, the step part 19, and the step part 24 as a target tracking line, and tracks this target tracking line may be performed. The target travel line is included in the target follow-up line, and is a line that is a travel target with the target position 26 set almost at the front of the vehicle. On the other hand, when the target follow-up line is located on the side of the vehicle at the work track or tilling boundary in the previous stroke, the travel target line is set by adding an offset thereto. In FIG. 2, the step portion 22 is a target travel line and a target follow-up line. Further, if the step is formed by the work, the step 18, the step 19 and the step 24 formed earlier may be the target follow-up line.

  The ECU 12 functions as an image processing unit that processes a captured image, and also functions as a travel control unit that controls the steering angle of the vehicle 1. The ECU 12 is a nonvolatile memory such as a CPU that executes various arithmetic processes, a ROM that stores various control programs, a RAM that is used as a work area for data storage and program execution, and a backup RAM that can retain stored contents even when the engine is stopped. An A / D converter for converting an analog signal input from a memory, an input / output interface, various sensors, and the like into a digital signal and taking it in, a timer for timing, and the like are provided. Note that the image processing unit and the travel control unit may be separate units that are an image processing device and a travel control device, respectively.

  FIG. 3 shows a functional configuration of the ECU 12 according to the embodiment. The ECU 12 includes an image acquisition unit 30, an image processing unit 32, a storage unit 34, and a travel control unit 36. The image acquisition unit 30 acquires a captured image captured by the camera 11 at a predetermined imaging cycle.

  The image processing unit 32 processes the captured image received from the image acquisition unit 30. The image processing unit 32 derives the target position 26 on the step unit 22 from the captured image, derives a correction amount for correcting the target position, and targets the steering for the vehicle to travel toward the corrected target position. The angle is calculated and supplied to the traveling control unit 36. The storage unit 34 stores captured image information.

  The travel control unit 36 includes a steering control unit that controls steering via the steering drive mechanism 14 and a brake control unit that generates a turning force by controlling a braking force via the brake mechanism 6. The brake control unit independently controls the braking force of the brake mechanism 6 provided on the left and right rear wheels 10 to generate a turning force. The steering control unit drives the steering drive mechanism 14 and controls the steering of the front wheels 8.

  The travel control unit 36 controls the vehicle 1 to travel based on the target steering angle received from the image processing unit 32. When the target rudder angle has not been calculated at the initial position 1 and the target rudder angle has not been received, the travel control unit 36 moves the vehicle 1 straight. Even if the travel control unit 36 is being driven, the steering person can steer. When the target rudder angle is calculated, the travel control unit 36 steers the front wheel 8 at the target rudder angle and controls the rudder angle so as to go to the target position 26.

  FIG. 4 shows a functional configuration of the image processing unit 32 according to the embodiment. The image processing unit 32 includes a horizon detection unit 38, a reference region setting unit 40, a vanishing point tracking unit 41, a template generation unit 42, a reference region tracking unit 44, a target correction unit 46, an unevenness detection unit 48, a target position detection unit 50, and A target rudder angle calculation unit 51 is included.

  The horizon detection unit 38 sets a horizon on the captured image. For example, the horizon detection unit 38 obtains the horizontal angle of the camera 11 at the time of imaging from the detection result of the sensor that detects the horizontal angle, and calculates the horizontal height on the captured image based on the horizontal angle at the time of imaging. Then, a line segment element extending in the horizontal direction near the height is detected, and this is set as the horizon. When a line segment element extending in the horizontal direction is not detected, a horizon is set at a position on the captured image calculated based on the acquired horizontal angle. The horizon appears, for example, with a luminance difference in a straight line on the captured image.

  The reference area setting unit 40 sets a plurality of reference areas composed of a plurality of pixel values of the captured image. The plurality of reference regions set in the reference region setting unit 40 is a set arranged in a straight line in a horizontal row, and information such as pixel values and positions is stored in the storage unit 34. When it is determined that there is a space for setting a new reference area for a new captured image, the set of linear reference areas is set to a plurality of columns. A set of newly set reference areas may be set with an area overlapping with a set of reference areas adjacent in the vertical direction. In addition, the reference area setting unit 40 discards the reference area when a predetermined condition is satisfied. As a predetermined condition, for example, when the enlargement ratio of the template with respect to the reference area exceeds a predetermined value or approaches the lower end of the captured image, the reference area setting unit 40 discards the reference area and ends the tracking of the set of reference areas. .

  The reference area includes a relatively large first reference area and a relatively small second reference area. When the first reference area and the second reference area are not particularly distinguished, they are simply referred to as “reference areas”. The second reference area is obtained by dividing one first reference area into a plurality of pieces, and is a set arranged linearly in a horizontal row within the first reference area. For example, one first reference area is an area having 10 pixels in the vertical direction and 40 pixels in the horizontal direction, and the second reference area is an area having 10 pixels in the vertical direction and 5 pixels in the horizontal direction. Each second reference region is included in the first reference region, but may overlap with the adjacent second reference region. Pixel values and position information of the first reference area and the second reference area are stored in the storage unit 34.

  The vanishing point tracking unit 41 sets and tracks a distant area that includes the vanishing point on the captured image and is far in the real space. The far region is far from the target position in real space, and includes a vanishing point (infinite point) set as an intersection of the horizon on the captured image captured at the initial position of the vehicle 1 and the front direction of the vehicle 1. It may be an area. The vanishing point tracking unit 41 can detect that the front direction of the vehicle 1 has deviated from the initial position by setting and tracking a far region including the vanishing point. The vanishing point tracking unit 41 may reset the far region when a predetermined setting condition is satisfied. For example, the vanishing point tracking unit 41 may reset the far region every time the vehicle 1 moves by a predetermined distance. When resetting the vanishing point, the amount of change in the position of the vanishing point is updated by reflecting the information on the position of each reference area and reference area stored in the storage unit 34. The reference area refers to the same area as the reference area in real space in a captured image different from the captured image in which the reference area is set.

  The template generation unit 42 performs a process such as enlargement or deformation on the stored pixel value of the reference area to generate a template. This is because, in a newly captured image, the position where the image is captured moves forward as the vehicle 1 moves forward, so the region that is the same in the real space as the reference region moves to the lower side of the captured image. At the same time, it is for enlargement and deformation on the captured image.

  The template generation unit 42 performs enlargement, deformation, and interpolation in accordance with the amount of displacement on the captured image when it is determined that the same region is based on the reference region stored first, and the template of the reference region Is generated and stored in the storage unit 34. Further, when the difference between the generated template and the original reference area becomes larger than a predetermined value, the template generation unit 42 may discard the reference area and the template and end the tracking.

  Each time a new captured image is acquired, the reference area tracking unit 44 searches and tracks an area having a pixel value distribution similar to the set reference area template. In the new captured image, it is estimated that the same area in the real space as the reference area moves on a radial straight line connecting the infinity point and the reference area, and therefore the estimated position of the template is set on the line. The reference area tracking unit 44 searches the area around the estimated position for an area having a luminance distribution similar to the template.

  The reference area tracking unit 44 uses a technique such as a normalized correlation method to determine the degree of coincidence between the window-like area extracted from the vicinity of the estimated position on the new captured image and the pixel value of the set template. An area that is equal to or higher than the level and has the highest degree of coincidence is determined as the same area in the real space as the template and is set as a reference area. While the reference area is an area set in a straight line, the reference area may be displaced with respect to the straight line due to unevenness of the ground, etc., both of which are almost the same in real space Judged to be an area. The first reference area corresponds to the first reference area, and the second reference area corresponds to the second reference area.

  After tracking the first reference area, the reference area tracking unit 44 searches the captured image for the position of the second reference area, which is the same area in real space as the second reference area. In other words, the reference area tracking unit 44 performs rough tracking in the first reference area, and then detects minute position fluctuations using a plurality of smaller second reference areas set in the first reference area. As a result, the position of the second reference area is detected more stably than when the second reference area is tracked from the beginning, and the position is determined in smaller pixel units, for example, 1/4 pixel or 1/16 pixel units. The deviation is calculated, and as a result, the unevenness of the ground can be efficiently calculated with higher accuracy. Note that tracking the reference area means searching for the same area in the real space as the reference area on the captured image and detecting the pixel value and position information of the area.

  The concavo-convex detection unit 48 is based on the position information of the reference area and the plurality of pieces of position information of the reference area that is the same area as the reference area in the new captured image. The height distribution of is calculated. The height distribution represents the unevenness of the ground in the real space, and represents the unevenness with respect to the reference region set linearly on the captured image. Specifically, the unevenness detection unit 48 detects the unevenness of the ground in the real space based on the position information of the second reference region corresponding to the second reference region. A detailed unevenness calculation method will be described later. Since the reference area setting unit 40 sets a plurality of sets of linear second reference areas, the height distribution is calculated by the number corresponding to the number of columns in the set. Since the unevenness of the ground is calculated based on the height distribution of the ground, the unevenness of the ground can be accurately calculated without being affected by the color and shadow of the soil surface due to conditions such as weather and sunlight.

  The unevenness detector 48 detects characteristic ground unevenness (steps) from the calculated height distribution. For example, the concavo-convex detection unit 48 analyzes the height data string, extracts a part having a height difference of a predetermined value or more as a stepped part, or searches for a part that matches a pre-stored concavo-convex pattern. . The stepped portion may be a concave portion or a convex portion. Since the second reference region is set to be linear at first, the step portion can be detected with high accuracy. The unevenness detection unit 48 stores the detected position information and shape of the stepped portion in the storage unit 34.

  When the plurality of second reference regions are set up and down in parallel on the captured image, the unevenness detection unit 48 is based on the information on the shape of the step detected by the height distribution of a certain second reference region. A portion having the same step shape is detected from the height distribution of another second reference region using a pattern matching method. Many of the detected stepped portions are steps and grooves formed when a ridge is made in the previous process, and thus are continuous in the vertical direction on the captured image. By connecting the step portions detected from the plurality of height distributions, the line of the step portion extending in the vertical direction on the captured image is detected.

  The target position detection unit 50 calculates a target position that is a target for vehicle travel based on the detected stepped portion. Specifically, the target position detection unit 50 generates a target travel line by connecting a plurality of height distribution steps determined to be the same type of steps in real space. Next, the target position detection unit 50 sets a position on the target travel line that is ahead of the vehicle 1 by a predetermined interval as a target position. Alternatively, a plurality of height distribution step positions are weighted and averaged into one position, which is set as a target position.

  The target correction unit 46 calculates a correction amount of the target position for accurately guiding the vehicle to the set target position, and sends the corrected target position to the target steering angle calculation unit 51. The target correction unit 46 compares the first correction unit 52 that calculates the first correction amount by detecting the lateral shift of the vehicle, and the second correction that calculates the second correction amount by comparing the past target position and the vehicle position. And a third correction unit 56 that calculates a third correction amount based on the curvature of the target travel line.

  First, the first correction unit 52 compares the position on the captured image when the first reference area is set with the position of the vanishing point, and calculates the position of the ground corresponding to the first reference area in real space. Subsequently, the position of the first reference area on the newly captured image is compared with the position of the vanishing point, and the position of the ground corresponding to the first reference area in the real space is calculated. These ground positions are relative positions from the camera 11 mounted on the vehicle 1, and the ground corresponding to the first reference area and the ground corresponding to the first reference area are the same ground in real space. The amount of movement of the vehicle 1 in the front-rear direction and the left-right direction is calculated from the difference between the two relative positions. Further, the turning amount of the vehicle 1 is calculated from the amount of movement of the vanishing point in the left-right direction on the captured image, and the amount of lateral deviation of the vehicle 1 is calculated from the amount of movement and turning of the vehicle 1 in the front-rear direction and the left-right direction. Calculated. The lateral displacement is a situation in which the vehicle moves in the left-right direction due to the wheels sliding in the lateral direction as the vehicle travels in the front-rear direction.

  Since the occurrence of the lateral shift reduces the accuracy with which the vehicle 1 follows the target position, the first correction unit 52 calculates the first correction amount so as to cancel the lateral shift. The first correction unit 52 tracks the same area in the real space on the captured image, and calculates the first correction amount based on the positional deviation of these areas. The first correction amount is corrected by moving the target position about 10 cm to the right side and correcting if the lateral displacement of about 10 cm occurs on the left side while the vehicle 1 advances the distance to the target position, for example. The steering angle is calculated so as to go to the target position. With this correction, the vehicle 1 can accurately follow the target position even if a lateral shift occurs.

  The second correction unit 54 calculates a second correction amount for correcting the target position based on the positional deviation amount between the past target position and the vehicle position. The past target position is a target position on the target travel line detected by the target position detection unit 50, and is stored in the storage unit 34 as position information in the virtual two-dimensional space. The position information in the virtual two-dimensional space corresponds to the distances in the front, rear, left and right of the real space, and the vehicle 1 at each time may be set as the origin. The past target position and the relative position of the vehicle 1 are calculated in the virtual two-dimensional space based on the movement amount and the turning amount of the vehicle 1 before and after and calculated by the first correction unit 52. Thereby, the position of the target travel line in the vicinity immediately below the vehicle 1 that is not shown in the captured image is calculated, and the amount of deviation between the position of the vehicle 1 and the target travel line is grasped. By correcting the current target position according to the position shift amount, it is possible to improve the tracking accuracy to the target travel line.

  Specifically, the second correction unit 54 calculates the position of the past target position in the virtual two-dimensional space based on the movement amount of the vehicle 1 calculated from the captured image for each imaging. For example, if it is calculated that the vehicle 1 has moved 30 cm forward and 5 cm to the left, the past target position is moved 30 cm forward and 5 cm to the right to move relative to the vehicle 1. The position is estimated. Further, when the past target position has reached a position immediately below the vehicle 1, for example, if the past target position is located about 10 cm to the left of the vehicle 1, the target position is moved about 10 cm to the right and corrected. Then, the steering angle is calculated so as to go to the corrected target position. By this correction, the vehicle 1 can accurately follow the target position. The position on the vehicle side for determining the deviation from the past target position may be set at the center of the vehicle, the center of the work machine connected to the rear part of the vehicle, or the like.

  The third correction unit 56 calculates a third correction amount for correcting the target position based on the curvature of the target tracking line when the target tracking line is curved. If the target position is set on the target travel line when the target travel line is curved, and the steering method for directing the direction of the vehicle 1 to the target position is performed, the vehicle 1 always keeps the inner diameter side of the target travel line. There is a possibility that the vehicle travels and a deviation occurs from the target travel line. Therefore, when the target travel line is curved, a third correction amount for correcting the target position radially outward of the target travel line is calculated. Thereby, the follow-up accuracy to the target travel line can be increased.

  Specifically, the third correction unit 56 calculates the third correction amount based on the curvature of the target tracking line. The third correction unit 56 calculates a third correction amount in which the vehicle 1 advances to the left side from the target position when the target tracking line is bent in the right direction of the vehicle 1. Moreover, the 3rd correction | amendment part 56 calculates the 3rd correction amount which the vehicle 1 advances to the right side from a target position, when the target tracking line is turning in the left direction of the vehicle 1. FIG. As described above, when the target tracking line is bent, the steering angle of the vehicle is corrected toward the outside of the target tracking line.

  Further, the third correction unit 56 searches for the position of the target tracking line farther from the target position based on the distribution pattern of the target position and the surrounding pixel values, and calculates the position in the virtual two-dimensional space. Further, by adding position information of the past target position calculated in the second correction unit 54 in the virtual two-dimensional space, the position of the target follow-up line is detected in the front-rear direction, and the two-dimensional shape is detected. Calculate the curvature of the curve.

  Here, a steering method of the vehicle 1 will be described. In a car such as a passenger car traveling on a paved road, the friction between the tire and the road surface is usually large and slip hardly occurs. Therefore, when trying to move to a certain target position, the vehicle turns with a certain rudder angle. However, a steering method that reaches the target position is adopted. On the other hand, in agricultural vehicles that run on soft soil, tire friction is small, and the relationship between the steering angle and the turning radius becomes unstable. Therefore, as a method for controlling the travel of the vehicle, a steering method is adopted in which a target position is determined and the direction of the vehicle is controlled toward the target position.

  The target correction unit 46 sends the calculated correction amounts of the target position individually or in total to the target rudder angle calculation unit 51. The target correction unit 46 sends one of the first correction amount, the second correction amount, and the third correction amount to the target rudder angle calculation unit 51, or adds any two to calculate the target rudder angle. You may send to the part 51. Further, the target correction unit 46 may set a threshold value for each of the first correction amount, the second correction amount, and the third correction amount, and may not use a correction amount equal to or less than the threshold value. Thereby, it is possible to prevent the steering from fluctuating due to sensitive correction. The target correction unit 46 may calculate a steering angle correction amount according to the correction amount of the target position, and send it to the target steering angle calculation unit 51 as the steering angle correction amount. Further, the target correction unit 46 may calculate a correction amount integrated by multiplying each correction amount by the respective coefficient.

  The characteristics of the first correction amount, the second correction amount, and the third correction amount calculated by the target correction unit 46 will be described. The second correction amount based on the past target position has an effect of correcting an error with respect to the target line caused by a lateral shift or a curve of the vehicle, but if the past target position does not move to just below the vehicle 1, Since an error from the target line is not detected, there is a drawback that correction is delayed. On the other hand, the first correction amount based on the lateral deviation of the vehicle can also detect the occurrence of lateral deviation immediately after the vehicle starts running, so the correction is made before the error from the target line increases significantly. It becomes possible to add. Further, the third correction amount resulting from the curve of the target line can be corrected by detecting the curve before the vehicle enters the curve. In addition, when the effects of the first correction amount and the third correction amount are insufficient and an error from the target line occurs, it is expected that additional correction is performed by the second correction amount. By adding a plurality of corrections in this way, the accuracy of following the target line can be reliably increased.

  The target rudder angle calculation unit 51 corrects the target position by adding the correction amount calculated by the target correction unit 46 to the target position detected by the target position detection unit 50, and moves the vehicle toward the corrected target position. A target rudder angle for controlling the direction is calculated. As a general form, the camera 11 is installed on the center line of the vehicle 1, and the front direction of the camera 11 is adjusted so as to coincide with the front direction of the vehicle 1. The steering angle of the vehicle 1 is calculated so that the corrected target position overlaps the center line of the captured image. Thus, the target steering angle of the vehicle 1 can be calculated by processing the captured image by the image processing unit 32. Image processing will be described in detail with reference to the captured image.

  FIG. 5A is a diagram illustrating a captured image 59 at a past time point when the reference region 104 is set. FIG. 5B shows a captured image 60 at a position where the tractor is advanced after setting the reference region 104, and FIG. 5C is a partially enlarged view of the captured image 60 shown in FIG. 5B. is there. Moreover, Fig.6 (a)-FIG.6 (c) are figures which show the picked-up image shown to Fig.5 (a)-FIG.5 (c) with a photograph. In the captured image 59, the horizon 62, the first step portion 64, and the second step portion 66 are picked up.

  The vanishing point tracking unit 41 sets a vanishing point 70 at the intersection of the horizon 62 and the front direction 63 of the vehicle on the captured image with respect to the captured image of FIG. A far field 71 is set in the image area. The far region 71 is composed of a plurality of pixel values, and is tracked as a region having a similar distribution of pixel values on the captured image for each imaging, and the captured image is based on the change in the vertical and horizontal positions of the far region 71. The position of the upper vanishing point 70 is specified for each imaging. One reference area 104 shown in FIG. 5A is an area composed of a plurality of pixels, and a plurality of reference areas 104 set in the reference area setting unit 40 constitutes a set 102 arranged in a straight line. To do.

  In FIG. 5B, the set reference area 104 is shown as a linear set 102. When the vehicle 1 moves forward along the first step portion 64, the imaging position moves, and the reference area of the set 108 that is the same as the reference area of the set 102 in real space moves downward in the captured image. FIG. 5B, which is the image after movement, shows the set 102 at the same position as the reference area 104 set in FIG. 5A, and refers to the same ground in real space as the reference area of the set 102. Shown as region 110 and its collection 108. The set 108 is tracked by searching the image for a luminance distribution pattern similar to the set 102.

  As shown in FIG. 5C, the three reference regions 104 are set as a linear set 102, and a plurality of second reference regions 106 are set as a linear set in one reference region 104. Is set. Hereinafter, the reference area 104 is referred to as a first reference area 104. The first reference area 104 and the second reference area 106 are set parallel to the horizon 62. The second reference area 106 has pixels having the same vertical width as the first reference area 104, but the horizontal width is narrow. For example, the first reference area 104 has a horizontal width of 40 pixels, whereas the second reference area 106 has a second width. The width of the reference area 106 is 5 pixels, and is set at an interval of 3 to 4 pixels while overlapping in the horizontal direction. FIG. 5B shows the center point of each second reference region 106. The set pixel value and position information of each reference area are stored in the storage unit 34.

  The first reference area 110 shown in FIG. 5C is an area determined to be the same area (ground) in the real space as the first reference area 104, and the second reference area 112 is the second reference area 112. This is an area determined to be the same area (ground) as the area 106.

  Here, the ij coordinates on the image will be described. As shown in FIG. 5B, the lower left corner of the image is set as the origin, and the j coordinate is set in the upward direction and the i coordinate is set in the right direction. The unit of ij coordinates is a pixel, and the photographic image illustrated in FIG. 6 has a size of 640 pixels wide and 480 pixels high. As shown in FIG. 5B, the ordinate of the horizon 62 is jv, and all the second reference areas 106 of the set 102 are ordinate ja.

  The cross-sectional shape of the ground of the first step portion 64 is, for example, a V-shaped groove as shown in FIG. 6C, and each portion has a height difference. The ordinate of the second reference region at the high portion of the first step 64 is jbh, while the ordinate of the second reference region at the lower portion such as the bottom of the groove is jbl. The amount of movement in the vertical direction of the lower reference portion of the second reference area becomes smaller than the ordinate ja in which the second reference area 106 is set. This is because, with respect to the same movement amount in the real space, the lower part has a longer distance from the camera 11 than the higher part, and thus the movement amount on the image becomes smaller. As described above, in the image of FIG. 5A, a set of reference areas that existed on one straight line is shown on the straight line in a new captured image as shown in FIG. Change occurs from position.

  The reference area tracking unit 44 tracks the same area in the real space as the first reference area 104 and searches for an area having a luminance distribution most similar to the first reference area 104. The reference area tracking unit 44 determines that the area having the highest degree of similarity with the first reference area 104 is the same area in the real space, and stores the pixel value and position information as the first reference area 110. Stored in the unit 34.

  After the reference area tracking unit 44 tracks the first reference area 104 and searches for the first reference area 110 and satisfies a predetermined condition, the reference area tracking unit 44 sets the second reference area 106 in the first reference area 104, A search around the first reference area 110 is performed. The predetermined condition is, for example, after setting the first reference area, the vehicle moves forward, the first reference area moves downward on the captured image, and the amount of movement of the ordinate becomes a predetermined value or more. Or when the amount of movement of the vehicle forward is greater than or equal to a predetermined value. The reference area tracking unit 44 determines that the area having the highest similarity with the second reference area 106 around the first reference area 110 is the same area in the real space, and detects the second reference area 112. The plurality of second reference regions 112 are displaced in the vertical direction from the horizontal line, and the unevenness detection unit 48 detects the unevenness of the ground in the real space based on the displacement.

  FIG. 7 is a diagram for explaining a method of calculating the height distribution of the ground from the captured image. Assume that the camera position at which the captured image 59 in which the first reference area 104 is set is captured is Ca, and the camera position at which the new captured image 60 is captured is Cb. On the other hand, the height CAH from the ground at each position is known. The direction in which the camera 11 faces is the horizontal direction for simplicity of explanation.

  The distance Z0 that the vehicle 1 has advanced between the captured image 59 and the captured image 60 can be calculated from the time difference and the vehicle speed. Further, the distance Z0 is calculated from the difference between the angle θbh corresponding to the ordinate jbh of the first reference region 110 or the angle θbl corresponding to the ordinate jbl and the angle θa corresponding to the ordinate ja of the first reference region 104. It can also be calculated.

For example, it is assumed that a ridge having a height dH exists on a flat ground having a height G1. If this height dH is obtained, the height distribution can be calculated. The height distribution represents the unevenness of the ground in the real space, and represents the unevenness with respect to the reference region set linearly on the captured image. The captured image 59 captured from the camera position Ca includes a region Ph above the ridge in the direction of the angle θa and a region Pl above the flat ground. In the captured image, the region Ph and the region Pl are on the same ordinate ja, but the region Ph and the region Pl have different heights in the real space. Horizontal distances Zah and Zal in real space from the camera position Ca to the region Ph and the region Pl are calculated by the following equations.
Zal = CAH / tan (θa) (1)
Zah = (CAH−dH) / tan (θa) (2)
Further, the relationship with the image is calculated by Expression (3). Here, PWV is the tan value of the viewing angle in the vertical direction of one pixel.
tan (θa) = PWV (jv−ja) (3)

When the region Ph and the region Pl in the real space are viewed from the position of the point Cb where the vehicle has advanced by a distance Z0, the shape looks down from above, and the direction is different. For the region Ph and the region Pl, the horizontal distances Zbh and Zbl in the real space using the angles θbh and θbl and the ordinates on the captured image jbh and jbl are the same as in the equations (1) to (3) below. It becomes an expression relationship.
Zbl = CAH / tan (θbl) (4)
Zbh = (CAH−dH) / tan (θbh) (5)
tan (θbl) = PWV (jv−jbl) (6)
tan (θbh) = PWV (jv−jbh) (7)

Since the horizontal distance Zbl between the region Ph and the region Pl does not change, the following equation is obtained.
Zbl = Zal-Zah = Zbl-Zbh (8)
Substituting the equations (1), (2), (4), and (5) into the equation (8) and arranging them by the height difference dH yields the equation (9).
... (9)

Substituting Equations (3), (6), and (7) into Equation (9) gives Equation (10), and jbh is rewritten as Equation (11) and rearranged to give Equation (12). dj is the difference between the ordinates on the captured image 60 of the region Ph and the region Pl. It can be noted that looking at equation (12), the travel distance Z0 and the value of the viewing angle PWV of the camera 11 are not required directly.
... (10)
jbh = jbl-dj (11)
(12)

Here, when CAH = 1970 mm, jv = 380 pixels, ja = 250 pixels, and jbl = 230 pixels are substituted into equation (12), equation (13) is obtained.
(13)
The height dH is a value that depends on the ordinate difference dj. Since the second reference area 106 is set in a horizontal straight line, the value of (jv-ja) is the same for the same set, and the height distribution of the ground can be calculated from the distribution of the value of dj.

  In calculating the ordinate difference dj on the picked-up image of the region Pl on the picked-up image 60 by the equation (11), the region Ph may be a region adjacent to the region Pl in the horizontal direction, or in the horizontal direction. It may be a remote area. The ordinate difference dj may be a difference between the ordinate of the region Pl and the average value of the ordinates of the second reference region 112 of the same set. Alternatively, a standard line may be calculated from the position information of the second reference area 112 in the same set by the least square method, and the ordinate difference dj may be calculated from the difference between the standard line and the ordinate of the area Pl. Such a region Ph is referred to as a region to be compared, and the comparison region is determined from the second reference region 112 of the same set as the region Pl and may be an arbitrary second reference region 112, and the same second reference region The second reference region 112 may be an averaged or standardized set of 112. Thus, based on the ordinate difference dj calculated by comparing the relative position in the vertical direction of the second reference region 112 due to the movement of the imaging position from the second reference region 106 that was originally set in a straight line. The height distribution can be calculated.

Each reference area is calculated not only in the height direction (ordinate), but also in the three-dimensional position of the position Z in the front-rear direction and the position X in the left-right direction (abscissa).
Z = (CAH−dH) / (PWV · (jv−jbh)) (14)
X = Z · PWH (ibh-iv) (15)
Here, PWH is the tan value of the horizontal viewing angle of one pixel, ibh is the abscissa of the second reference region 112 of the captured image 60, and iv is the abscissa of the front front of the vehicle on the captured image. Thereby, the three-dimensional position of the target tracking line can also be calculated. Here, the XYZ coordinates in the three-dimensional space are set as follows. First, the origin directly on the ground directly below the camera 11 is the Y coordinate or height H in the vertical upward direction, the front front direction of the vehicle is the Z coordinate, and the left and right direction is the X coordinate.

  As described above, the unevenness detection unit 48 calculates the height distribution of the ground in the real space where the set 102 is set based on the ordinate of the second reference region 112, and the first indented from the calculated height distribution. The step 64 and the step are detected.

  FIG. 8 is a diagram for explaining a method of simultaneously calculating a plurality of height distributions by continuous image processing. Moreover, FIG. 9 is a figure which shows the captured image shown in FIG. 8 with a photograph. FIG. 8C is a partially enlarged view of the captured image 72 shown in FIG. FIG. 8A shows that the tracking of the reference region shown in FIG.

  As shown in FIG. 8A, the reference region tracking unit 44 sets the first reference region 104 at a position several tens of pixels above the lower end of the captured image, and continuously captures images at regular time intervals. A reference area that is the same area in the real space as the first reference area 104 is tracked for the captured image. As the vehicle moves forward, the reference area moves downward on the captured image, and if the reference area moves downward by about 10 pixels, a new reference area is set in an open portion on the captured image. When the reference area approaches the lower end of the captured image, the reference area and the reference area are discarded, and information such as pixel values and positions are deleted.

  In FIG. 8A, the first reference area 104 newly set for the captured image 72 and the first reference area 104 in the previous captured image are substantially at the same position as the first reference area 104, but the vehicle 1 moves downward as the vehicle 1 advances The moved first reference area 113, and the first reference area 114, the first reference area 116, and the first reference area 118 in which the first reference area is set in the previous captured image are shown.

  8B and 8C, after tracking the first reference area 114, the first reference area 116, and the first reference area 118, the second reference area is tracked in each area, and the unevenness detecting unit 48 The height distribution in the real space calculated based on the position of the second reference area is shown. The first height distribution 120, the second height distribution 122, and the third height distribution 124 correspond to the first reference region 114, the first reference region 116, and the first reference region 118, respectively.

  In calculating the height distribution, if the amount of movement of the position in the image vertical direction between the reference region and the reference region is small, the error in the calculation result tends to increase. Therefore, the unevenness detection unit 48 calculates the height distribution when the amount of vertical movement of the first reference region is equal to or greater than the threshold value. In FIG. 8B, since the movement amount does not reach the threshold value in the second reference area 113 that is the second from the top, the height distribution is not calculated, and the movement amount is not calculated in the third and subsequent first reference areas from the top. Since the threshold is reached, the height distribution is calculated. Thereby, the error of the calculation result of height distribution can be suppressed, and the 1st step part 64 can be detected accurately.

  FIG. 10 is a diagram for explaining a method of detecting a groove from the detected unevenness. FIG. 11 is a diagram showing the captured image 74 shown in FIG. 10 as a photograph. FIG. 10 shows the first height distribution 120, the second height distribution 122, and the third height distribution 124 detected in FIG.

  Here, the rough shape of the groove formed in the field as the target travel line is known by using a preset groove forming device. In this way, the shape that can be estimated in advance is registered in the storage unit 34, the pattern matching method is applied to the detected height distribution, and the degree of coincidence is a predetermined level or higher and the highest part. May be detected by determining that they are in the same region. Thereby, it is possible to easily detect the unevenness of the ground to be tracked.

  The storage unit 34 holds a registered shape 126 corresponding to, for example, a V-shaped groove. The target position detection unit 50 performs pattern matching with the V-shaped registered shape 126 on the first height distribution 120, the second height distribution 122, and the third height distribution 124 to identify similar shapes, A stepped portion that is continuous in the vertical direction on the captured image is detected. FIG. 10 illustrates a V-shaped shape at a position where the degree of coincidence with the registered shape 126 is the highest for each height distribution.

  The target position detection unit 50 detects the position information of the bottom portion of the registered shape 126 at the position having the highest degree of coincidence with the height distribution as a stepped portion. The target position detection unit 50 detects the position information of the step portion for each of the first height distribution 120, the second height distribution 122, and the third height distribution 124, and averages the positions to thereby obtain the target position. 26 is calculated. Thereby, the step part of the groove | channel formed in the agricultural field can be made into a target position. By performing pattern matching using a pre-stored registered shape, it is possible to easily detect the stepped portion and improve the detection speed of the stepped portion.

  FIG. 12 is a diagram for explaining a method of calculating the movement trajectory of the vehicle 1 by tracking the reference area. FIG. 12A shows the positional relationship between the vehicle 1 and the first reference area 104 at time t1. The first reference area 104 is a state set on an extension line of the center line 128 of the vehicle 1. Since the camera 11 is installed on the center line 128 of the vehicle 1, the center line of the captured image matches the center line 128 of the vehicle 1. The center line 128 passes through the center of the vehicle 1 and extends along the front direction of the vehicle 1.

  FIG. 12B shows the positional relationship between the vehicle 1 and the first reference area 110 at time t2. The first reference area 110 is the same area in the real space as the first reference area 104. At time t <b> 2, the first reference area 110 approaches the vehicle 1 and moves to the right of the center line 128 of the vehicle 1 due to the forward movement of the vehicle 1.

  As shown in FIG. 12C, when the first reference region 110 is disposed so as to be in the center of the left and right in FIG. 12C, the vehicle 1 is slightly turned leftward. Subsequently, since the first reference area 104 and the first reference area 110 are the same area in the real space, when they are overlapped, the result becomes as shown in FIG. 12D, and between time t1 and time t2. The movement locus 129 of the vehicle 1, that is, the movement amount and the turning amount can be known. By tracking the reference area on the captured image in this way, the movement locus of the vehicle 1 can be calculated. Further, the reference area tracking unit 44 can simultaneously calculate the movement locus of the vehicle 1 by tracking a plurality of reference areas as shown in FIG. 8A and averaging the calculation results.

  FIG. 13 is a diagram for explaining calculation of the correction amount by the first correction unit 52. For example, in a situation where the farm field is tilted to the left and the vehicle 1 causes a lateral shift in the left direction as the vehicle 1 moves forward, the vehicle 1 travels toward the target position 26a as shown in FIG. Even if it controls, the vehicle 1 will be in the state which produces the lateral shift 77 continuously with respect to the target travel line 76a.

  When a lateral deviation 77 of the vehicle 1 is detected by tracking a plurality of reference areas, as shown in FIG. 13B, the lateral deviation 77 is added to the target position 26a on the target travel line 76a. The corrected target position 26c is set. Subsequently, by calculating the steering angle so that the direction of the vehicle 1 is controlled toward the target position 26c, the follow-up accuracy of the vehicle 1 to the target travel line 76a can be increased. In other words, even in a situation where the lateral displacement 77 of the vehicle 1 occurs on a slope or the like, the vehicle 1 can travel on the target travel line 76a by correcting the target position 26a based on the lateral displacement 77.

  FIG. 14 is a diagram for explaining the calculation of the correction amount by the second correction unit 54, and is a plan view overlooking the ground. The range on the ground that falls within the field of view of the camera 11 is an imaging range 80. The target travel line located in front of the imaging range 80 is out of the field of view of the camera 11 and cannot be imaged. Therefore, the position of the step 82 on the target travel line detected near the boundary in the imaging range 80 is stored. Subsequently, as shown in FIG. 12, since the movement trajectory of the vehicle 1 for each imaging can be calculated from the captured image, if the movement trajectory is reflected in the position information of the vehicle 1, the stepped portion outside the imaging range 80 Position 82a is estimated. By continuously performing this calculation process for each imaging, it is possible to continuously track the position 82a of the past stepped portion. By continuing to track the position 82a directly below the vehicle 1, A positional deviation amount 78 of the target travel line is calculated. A correction amount 79 is calculated by the second correction unit 54 in accordance with the positional deviation amount 78, and a correction target position 83 obtained by correcting the position of the stepped portion 82 is calculated.

  FIG. 14 shows a state where the past stepped portion position 82 a approaches the vehicle 1 as the vehicle 1 moves forward. The storage unit 34 may hold vertical and horizontal deviations between the position 1a of the vehicle 1 and the position 82a of the previous step as position information, and the position 1a of the vehicle 1 is always set as the origin in the virtual two-dimensional space. The position information of the past step position 82a may be changed. In any case, the storage unit 34 holds position information indicating the positional relationship between the position 82a of the previous stepped portion and the position 1a of the vehicle 1. The position of the new step 82 is stored when, for example, a certain reference area approaches the lower end of the captured image and is discarded, and when the position 82a reaches the rear of the vehicle 1, the stored information is stored. It can be discarded. In this way, the positions of a plurality of steps are stored at the same time, and the positions of the steps are tracked in parallel.

  A positional deviation amount 78 from the position 1a of the vehicle 1 is calculated with respect to the closest position 82a of the step 1a that is closest to the vehicle 1 among the tracked positions 82a of the plurality of steps. . The positional deviation amount 78 can be estimated as the positional deviation of the target travel line immediately below the vehicle 1 in real space. The second correction unit 54 calculates the second correction amount so as to correct the target position by adding the positional deviation amount 78 to the target position on the target travel line detected within the imaging range. By reflecting the deviation between the vehicle position and the past target position in steering in this way, it is possible to accurately correct a small positional deviation like feedback control, and to improve the tracking accuracy to the target travel line. Can do.

  FIG. 15 is a diagram for explaining a method of detecting a distant stepped portion. FIG. 16 is a view showing the captured image 84 shown in FIG. 15 as a photograph. 10 and 11, once the stepped portion on the target travel line 65 and the target position 26 are detected, the stepped region 132 is set at the position of the target position 26 as shown in FIG. The position information and pixel value of the stepped region 132 are stored in the storage unit 34. The step region 132 may be composed of a plurality of linear pixel values in the same manner as the reference region. The third correction unit 56 sets the reference step region 130, which is a region having a distribution of pixel values similar to that of the step region 132, at a position above the step region 132 in the captured image 84 and a distant position in the real space. Explore.

  The third correction unit 56 searches for a reference step region 130 having a luminance distribution having the highest degree of coincidence with the step region 132 by a pattern matching method. FIG. 16 shows a situation in which 10 positions are searched upward at intervals of 20 pixels on the captured image 84. First, with the detected target position 26 as the center, the step region 132 is set with the same width as the reference region, and the distribution of the pixel values is stored. Subsequently, the reference step region 130 is set at a preset interval, that is, at a position approximately 20 pixels above in FIG. Since the image of the object becomes smaller as the position is higher on the captured image 84, the stepped region 132 is reduced in proportion to the height difference from the horizon 62 to generate an image pattern. The width of the reference step region 130 is a range obtained by adding a certain search range to the left and right to the width of the generated image pattern. In the reference step region 130, pattern matching is performed with the generated image pattern, and a position having the highest similarity is extracted as a position 130a. Subsequently, a reference step region 130 is set at a position 20 pixels above, and image pattern generation and pattern matching processing are similarly performed to extract the position 130a. This process is repeated to extract the positions 130a at the upper 10 locations and store them in the storage unit 34. It can be estimated that there is a stepped portion in real space at the position connecting the target position 26 and the ten positions 130a above it, and this is the target travel line.

  FIG. 17 is a diagram for explaining the curvature of the target travel line. In the plan view of the captured image 84 described in FIGS. 15 and 16 projected on the ground, the imaging range 86 and the detected target position 26 are referred. The position 130a of the stepped region 130 is shown. The positions 130a of the reference step region 130 are constant on the captured image, but on the projected ground, the distance between the positions 130a increases as the distance increases.

  FIG. 17 also shows a plurality of past step positions 134 that appear before the imaging range 86. When the target position 26 shown in FIG. 17 and the point sequences of the plurality of positions 130a and 134 are connected, the target travel line 88 on the plan view is continuously calculated. The third correction unit 56 calculates the curvature of the target travel line 88. The least square method or the like is applied to these point sequences and applied to a quadratic equation, and the curvature of the target travel line is calculated as an approximate value from the coefficient of the square term.

  FIG. 18 is a plan view on the ground for explaining a steering correction method when the target travel line 90 is bent. For example, in an agricultural field on the mountainside, it may be curved along the shape of the mountain, and the target travel line 90 is also bent. In FIG. 18, the target travel line 90 is a circular arc having a center C and a radius R1. FIG. 18 shows a movement locus 94 of the traveling vehicle 1, and shows a state in which target positions 92 a, 92 b, and 92 c (referred to as “target position 92” when these are not distinguished) are detected at each position of the vehicle 1. .

  Each target position 92 is detected on a target travel line 90 that is separated from the vehicle 1 by a predetermined distance D. The distance D is the distance from the rear wheel axle of the vehicle 1 to the target position 92. First, the vehicle 1 is steered so that the front direction faces the target position 92a. Subsequently, as the vehicle 1 moves forward, the target position also moves forward with a distance D on the target travel line 90, and then moves forward from the target position 92a to the target positions 92b and 92c. When such traveling control is performed, the traveling locus of the vehicle 1 passes through the locus 94 inside the target traveling line 90.

Therefore, a lateral shift 95 occurs between the target travel line 90 and the track 94 of the vehicle 1. The radius R2 of the trajectory 94 of the vehicle 1 is calculated by the equation (16), and the lateral deviation 95 is calculated as a difference between the radius R2 and the radius R1 of the target travel line 90 as in the equation (17).
R2 = (R1 ² −D ² ) ^1/2 (16)
Side shift 95 = R1-R2 (17)

  FIG. 19 is a diagram for explaining the correction of the third correction unit 56. In the third correction shown in FIG. 19, according to the curvature of the target travel line 90 so that the vehicle 1 travels radially outside the target travel line 90 in a situation where the vehicle 1 turns as shown in FIG. 18. By performing the correction by the third correction unit 56, the vehicle 1 travels on the target travel line 90, and the follow-up accuracy to the target travel line 90 can be increased. When each target position 92 is detected on the target travel line 90 and the radius R1 is calculated from the curvature of the target travel line, the lateral deviation 95 is calculated by the equations (16) and (17). Subsequently, as shown in FIG. 19, target positions 96 a, 96 b, and 96 c are set by moving the respective target positions 92 to the outside in the radial direction of the target travel line 90 by the lateral deviation 95. Steering control is performed such that the front of the vehicle 1 faces the corrected target positions 96a, 96b, and 96c, so that the track of the vehicle 1 passes on the target travel line 90. The third correction unit 56 calculates a third correction amount for moving the target position to the outside of the arc of the target travel line 90 based on the curvature of the target travel line 90.

When the curvature of the target travel line is small and is equal to or less than a predetermined value, the third correction unit 56 does not have to calculate the third correction amount by determining that the target travel line is substantially a straight line. As described above, the target correction unit 46 calculates the first, second, and third correction amounts for moving the position in the horizontal direction with respect to the target position based on the detection position of the stepped portion, and adds the correction amounts. The target position is corrected based on the calculated value, and the steering control angle is calculated so that the front of the vehicle faces the corrected target position.

  In another aspect, the travel control device includes an image capturing unit that captures an image of the ground on which the vehicle travels and a distance from the ground, a captured image captured by the image capturing unit, and a target position at which the vehicle follows and identifies a target rudder angle. An image processing unit to calculate, and a travel control unit that controls the vehicle to travel at the target rudder angle. The image processing unit includes a reference region setting unit that sets a reference region on the ground on the captured image, a reference region tracking unit that tracks the reference region for each imaging and identifies a reference region, and unevenness of the ground from the position of the reference region And a target position detection unit that analyzes the uneven shape of the ground and identifies the target position that the vehicle should follow, and the image processing unit identifies the position of the vanishing point of the captured image A vanishing point tracking unit that calculates the amount of change in the position of the vehicle by comparing the change in the position of the reference area and the reference area with the position of the vanishing point, and calculates the first correction amount based on the skidding of the vehicle. The calculated target position correction amount includes a first correction unit and a second correction unit that estimates a past target position from the amount of change in the position of the vehicle and calculates a second correction amount from the positional relationship with the vehicle. Target that calculates the target rudder angle based on the target position corrected by It has a corner calculation unit, the. As described above, in another aspect, the target rudder angle may be calculated with correction based on the first correction amount and the second correction amount.

  In the above, this invention was demonstrated based on the Example. This embodiment is an exemplification, and it will be understood by those skilled in the art that various modifications can be made to combinations of the respective constituent elements, and such modifications are also within the scope of the present invention.

  In the embodiment, the driving control device 20 controls the steering amount by pressing the automatic driving switch while the driver controls the driving speed of the vehicle with an accelerator pedal or the like, but is not limited to this mode. Both the steering of the vehicle 1 and the traveling speed may be controlled simultaneously.

  The traveling control device 20 may include a sensor that detects a roll angle of the vehicle at the time of imaging and a sensor that detects a pitch angle of the vehicle. When the image processing unit 32 acquires a new captured image, the image processing unit 32 may correct the new captured image based on the vehicle state (roll angle and pitch angle) at the time of capturing the captured image. The horizon detection unit 38 detects a horizontal line from the corrected captured image. Thereby, the position shift of the horizontal line for every captured image can be suppressed.

  In the embodiment, the target position is corrected with respect to the target position on the target travel line in the virtual two-dimensional space by each correction amount calculated by the target correction unit 46, and the target rudder is set so that the vehicle is directed to the corrected target position. Although the aspect which calculates a corner was shown, it is not restricted to this aspect. For example, the target rudder angle calculation unit 51 calculates the temporary target rudder angle toward the target position, the target correction unit 46 calculates the rudder angle correction amount, and the temporary target rudder angle is corrected by the rudder angle correction amount to obtain the target rudder angle. May be calculated. In any case, the correction amount calculated by the target correction unit 46 is a value that can be converted into the steering angle of the vehicle.

  DESCRIPTION OF SYMBOLS 1 Vehicle, 3 Vehicle cockpit, 4 Farm field, 6 Brake mechanism, 8 Front wheel, 10 Rear wheel, 11 Camera, 12 ECU, 14 Steering drive mechanism, 15 Steering mechanism, 16 Working machine, 20 Travel control device, 22 Step part, 26 target position, 30 image acquisition unit, 32 image processing unit, 34 storage unit, 36 travel control unit, 38 horizon detection unit, 40 reference region setting unit, 41 vanishing point tracking unit, 42 template generation unit, 44 reference region tracking unit , 46 target correction unit, 48 unevenness detection unit, 50 target position detection unit, 51 target rudder angle calculation unit, 52 first correction unit, 54 second correction unit, 56 third correction unit, 60 captured image, 62 horizon, 64 First step, 66 Second step, 70 Vanishing point.

Claims (2)

An imaging unit that captures an image of the ground on which the work vehicle travels in the field and its distance;
An image processing unit that processes a captured image captured by the imaging unit to identify a target position where the work vehicle follows and calculates a target steering angle;
A travel control unit for controlling the work vehicle so as to travel at the target rudder angle,
The image processing unit
The vanishing point is calculated based on the horizon calculated from the captured image, and the change in the position of the reference area set on the ground in the captured image is compared with the position of the vanishing point, before and after the work vehicle. A vanishing point tracking unit that calculates the movement amount of
A first correction unit that calculates a first correction amount based on a lateral displacement amount of the work vehicle calculated by the vanishing point tracking unit;
Based on the movement amount of the work vehicle calculated by the vanishing point tracking unit, the current work vehicle position information of the past target position outside the field of view of the captured image is estimated, and the work vehicle , the past target position, and A second correction unit that calculates a second correction amount based on the amount of positional deviation;
Based on the curvature of the target tracking line, the target tracking line is identified by combining the positional information obtained by tracking the target position in the distance using the luminance distribution information of the captured image and the positional information of the past target position. A third correction unit that calculates a third correction amount;
A target for calculating the target rudder angle by correcting the target position or a temporary target rudder angle based on the target position based on at least one of the first correction amount, the second correction amount, and the third correction amount. And a steering angle calculation unit. The image processing unit
A target position detection unit that detects a target position that is a target of following traveling of the work vehicle from the captured image;
A reference area setting unit that sets a plurality of reference areas composed of a plurality of pixel values on the ground in the captured image, arranged in a line in a straight line; and
In a captured image newly captured during vehicle travel, a reference area tracking unit that calculates a position information by tracking a reference area that is the same area as the reference area on the captured image;
A field deviation in a real space is calculated based on a plurality of pieces of positional information of the reference area, and a position deviation from the straight line of the reference area caused by a change in the imaging position due to the movement of the work vehicle is calculated. An unevenness detecting unit for detecting unevenness on the ground such as steps and grooves formed in
The reference area setting unit sets, as the reference area, a first reference area and a plurality of second reference areas composed of a small number of pixel values by subdividing the first reference area,
The reference area tracking unit calculates position information on the captured image of the second reference area in the first reference area after tracking the first reference area;
The unevenness detecting unit detects unevenness of the ground in real space based on position information of the second reference region,
The travel control device according to claim 1, wherein the target position detection unit detects the target position based on the shape of the unevenness.