JP5375249B2 - Moving path planning device, moving body control device, and moving body - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a moving path planning apparatus that can improve the efficiency of path planning for a mobile body. <P>SOLUTION: The moving path planning apparatus includes an imaging means for capturing an image ahead of the mobile body, an image processing means for applying predetermined image processing to the image captured by the imaging means to detect tracks present in the captured image, and a path planning means for planning a moving path of the mobile body according to the tracks detected by the image processing means. <P>COPYRIGHT: (C)2011,JPO&amp;INPIT

Description

  The present invention relates to a moving path planning device, a moving body control device, and a moving body.

  Conventionally, the following method has been adopted as a method for controlling a moving body (for example, a vehicle) on rough terrain. First, a surrounding three-dimensional shape is measured using a stereo image measurement, a laser range finder (LRF), or the like, and a large protrusion or the like is detected as an obstacle based on the measurement result. In addition, while ensuring a sufficient distance to avoid contact with obstacles, and to minimize the factors that make the operation unstable, such as deceleration factors such as undulations and curves, and fluctuations on the route. The route, speed, etc. are planned, the acceleration / deceleration of the vehicle and the steering angle are obtained according to the plan, and the steering and wheels of the vehicle are driven by each actuator.

  As a conventional mobile body control method, Patent Document 1 below discloses a method of measuring a surrounding three-dimensional shape, determining an actionable range (environment map), and planning a route within the actionable range. Has been. Patent Document 2 below discloses a method of creating a route search map by sensing with a terrain sensor and selecting the flattest route as the optimum route for traveling from among a plurality of route candidates. Patent Document 3 below discloses a specific configuration example of an autonomous traveling control system from detection of an obstacle to control of each actuator. Furthermore, the following Patent Document 4 discloses a method of sequential re-planning as an example of handling when the distance that can be seen in the future is short.

Japanese Patent Laying-Open No. 2005-092820 Japanese Patent Laid-Open No. 07-064631 JP-A-11-212640 JP-A-11-007318

 For example, when the three-dimensional shape of rough terrain as shown in FIGS. 19A and 19B is measured using the above-described stereo image measurement, LRF, or the like, and the vehicle route plan is performed based on the measurement result. Is assumed. In this case, in a place where an image as shown in FIGS. 19 (a) and 19 (b) is taken, a neighboring region (a range where sufficient accuracy can be obtained by stereo image measurement, or measurement by LRF) can be obtained accurately. The area below the middle of the image is flat in a wide range and can take any route, so a route that goes straight to the target point (solid arrow in the figure) Tend to plan.

 However, if you drive the vehicle along the planned route and there is an obstacle at a point where you have traveled far beyond a certain distance, it suddenly turns out that the front is inaccessible. It becomes necessary to re-plan the route by avoiding obstacles. Thus, with the conventional route planning method, the ideal route (dashed arrow in the figure) that can avoid an obstacle does not necessarily match the planned route, leading to a decrease in route planning efficiency. There was a problem.

  The present invention has been made in view of the above-described circumstances, and an object thereof is to provide a moving path planning apparatus, a moving body control apparatus, and a moving body that can improve the path planning efficiency.

 In order to solve the above-described problem, in the present invention, as a first solving means related to the movement path planning device, a predetermined means based on a photographing means for photographing a moving direction of a moving body and a photographed image obtained from the photographing means. Image processing means for detecting a traffic trace existing in the photographed image by performing image processing; and a path planning means for planning a movement path of the moving body based on the traffic trace detected by the image processing means. It is characterized by providing.

 Further, as a second solving means relating to the movement path planning apparatus, in the first solving means, a three-dimensional measuring means for measuring a three-dimensional shape in the traveling direction of the moving body, and 3 obtained from the three-dimensional measuring means. Map creation means for creating a map based on the measurement result of the dimensional shape, and the route planning means is based on the map created by the map creation means and the traffic trace detected by the image processing means. The moving path of the moving body is planned.

 Further, as a third solving means relating to the movement route planning apparatus, in the first or second solving means, the image processing means detects a band-like pattern having a width as the traffic trace. .

 Further, as a fourth solving means according to the movement route planning apparatus, in the first or second solving means, the image processing means has a plurality of strip-like patterns having a width and being intermittent in the middle. A parallel part in a three-dimensional space is detected as the traffic trace.

 Further, as a fifth solving means relating to the movement path planning device, in the first or second solving means, the image processing means has a band-like portion having a width and being intermittent in the middle. A band-shaped part that is found from the photographed image and is heading to the vanishing point shared by the largest number of band-shaped parts is narrowed down as the traffic trace.

 Further, as a sixth solving means according to the movement path planning apparatus, in the first or second solving means, the image processing means uses a plurality of spatial frequency filters having different direction parameters to detect the captured image. A filtering process for generating intensity distribution data in each direction for the detection target area by filtering the target area, and a pattern of each pixel in the detection target area based on the intensity distribution data generated by the filtering process By performing predetermined statistical processing based on a pattern direction specifying process for specifying a direction and a result of specifying the pattern direction by the pattern direction specifying process, a trace area estimated to have a traffic trace in the detection target area is obtained. A trace area detection process to be detected is performed.

 Further, as a seventh solving means according to the movement route planning apparatus, in the first or second solving means, the image processing means uses a plurality of spatial frequency filters having different direction parameters to detect the captured image. A filtering process for generating intensity distribution data in each direction for the detection target area by filtering the target area, and a pattern of each pixel in the detection target area based on the intensity distribution data generated by the filtering process A pattern direction specifying process for specifying a direction, and a sub-detection target area obtained by dividing the detection target area into a plurality of parts along the depth direction, based on a result of specifying the pattern direction by the pattern direction specifying process. By performing the statistical processing, it is estimated that there is a traffic trace in the sub detection target area. And performing the trace area detection process for detecting a trace area.

 Further, as an eighth solving means according to the movement route planning apparatus, in the sixth solving means, the image processing means performs each of the pixels in the detection target area as the statistical process in the trace area detecting process. Selected as a voting source pixel and included in a voting target area set along the pattern direction starting from the voting source pixel, or a pixel existing on a straight line parallel to the pattern direction from the voting source pixel A voting process for voting on a pixel to be found, a vanishing point specifying process for specifying a pixel with the largest number of votes as a vanishing point based on a voting result of the voting process, and a voting source pixel of the vanishing point The effective voting pixel extraction process to be extracted as follows, and a region having the vanishing point as a vertex in the detection target region is divided into a plurality of small regions. Ratio vote pixels and performing the trace area specifying processing for specifying a small region exceeding a predetermined threshold value as the trace area.

Further, as a ninth solving means according to the movement route planning apparatus, in the seventh solving means, the image processing means, as the statistical process in the trace area detecting process, each pixel in the sub detection target area. Is selected as a voting source pixel, a pixel existing on a straight line parallel to the pattern direction starting from the voting source pixel, or a voting target area set along the pattern direction starting from the voting source pixel A voting process for voting on the included pixels, a vanishing point identifying process for identifying a pixel with the largest number of votes as a vanishing point based on the voting result of the voting process, and valid voting for the voting source pixel of the vanishing point The effective voting pixel extraction process for extracting as a pixel, and a region having the vanishing point as a vertex in the sub detection target region are divided into a plurality of small regions. Effective proportion of voting pixels and performing the trace area specifying processing for specifying a small region exceeding a predetermined threshold value as the trace area.
Is a two-dimensional Gabor filter.

 Further, as a tenth solving means according to the movement route planning apparatus, in the ninth solving means, the image processing means has previously focused on the sub-detection target area currently focused on in the voting process. A voting weighting area having a larger voting weight is set on the end side of the trace area specified by the trace area specifying process performed on the sub detection target area.

 Further, as an eleventh solving means according to the movement route planning apparatus, in the tenth solving means, the image processing means may be configured to perform a previous process after the trace area specifying process has been completed for the sub-detection target area currently focused on. In the sub-detection target area that has been focused on, a voting weighting area in which the voting weight is increased on the start end side of the trace area specified by the trace area specifying process performed on the currently focused sub-detection target area. The sub-detection target area that has been focused on by previously performing the voting process, the vanishing point identifying process, the effective voting pixel extraction process, and the trace area identifying process for the sub-detection target area that has been focused on before. It is characterized by correcting the trace area specifying result for.

 Further, as a twelfth solving means according to the movement route planning apparatus, in the tenth or eleventh solving means, the image processing means is configured to determine a size of the voting weighting area according to a position along the depth direction. And / or changing the weighting amount of the vote.

  On the other hand, in the present invention, as the first solving means related to the moving body control device, the moving path planning apparatus having any one of the above first to twelfth solving means, the actuator for generating the power of the moving body, And a movement control means for controlling the actuator so that the moving body moves along the movement path planned by the movement path planning device.

In addition, as a second solving means related to the moving body control device, in the first solving means, a position information obtaining means for obtaining information relating to the position of the moving body, and an attitude for obtaining information relating to the attitude of the moving body. An information acquisition means; and a position / posture estimation means for estimating the current position and posture of the mobile body based on the information acquired by the position information acquisition means and the posture information acquisition means; The actuator is controlled so that the moving body moves within a predetermined allowable range centered on the moving path based on the current position and posture of the moving body estimated by the position / posture estimating means. .
Furthermore, the present invention is characterized in that the moving body control device having the first or second solving means described above is mounted as a solving means relating to the moving body.

 According to the present invention, it is possible to detect a traffic trace (such as a hail or a fumigation trace) like a certain road even if it is a flat and gentle terrain such as rough terrain. And the traffic trace in rough terrain arises as a result of the fact that the vehicle has actually passed several times before, and it is highly possible that the extension line is an area that can continue to pass. By predicting a direction with a high possibility of passage outside the measurement area using such a traffic trace, it is possible to plan an efficient movement route including outside the measurement area.

It is a composition schematic diagram of vehicles C carrying a mobile control device concerning this embodiment. It is a block block diagram of the image processing apparatus 2 of the moving body control apparatus which concerns on this embodiment. It is an example of the input image in this embodiment, and its input image data. It is an operation | movement flowchart of the signal processing apparatus 2c in the image processing apparatus 2 which concerns on this embodiment. It is explanatory drawing regarding the filtering process of the signal processing apparatus 2c which concerns on this embodiment. It is explanatory drawing regarding the pattern direction specific process of the signal processing apparatus 2c which concerns on this embodiment. It is 1st explanatory drawing regarding the voting process in the trace area | region detection process of the signal processing apparatus 2c which concerns on this embodiment. It is 2nd explanatory drawing regarding the voting process in the trace area | region detection process of the signal processing apparatus 2c which concerns on this embodiment. It is 3rd explanatory drawing regarding the voting process in the trace area | region detection process of the signal processing apparatus 2c which concerns on this embodiment. It is explanatory drawing regarding the vanishing point specific process in the trace area | region detection process of the signal processing apparatus 2c which concerns on this embodiment. It is explanatory drawing regarding the effective voting pixel extraction process in the trace area | region detection process of the signal processing apparatus 2c which concerns on this embodiment. It is explanatory drawing regarding the trace area | region specific process in the trace area | region detection process of the signal processing apparatus 2c which concerns on this embodiment. It is 1st explanatory drawing regarding the modification of the trace area | region detection process of the signal processing apparatus 2c which concerns on this embodiment. It is 2nd explanatory drawing regarding the modification of the trace area | region detection process of the signal processing apparatus 2c which concerns on this embodiment. It is the 3rd explanatory view about the modification of the trace area detection processing of signal processing device 2c concerning this embodiment. It is an example of the map (three-dimensional map) created by the map creation apparatus 4 which concerns on this embodiment. It is an example of the movement path | route planned by the route planning apparatus 5 which concerns on this embodiment. It is an example of the moving range of the vehicle C which moves by control of the motion control apparatus 9 which concerns on this embodiment. It is explanatory drawing of the movement route planning method in the past.

Hereinafter, an embodiment of the present invention will be described with reference to the drawings.
FIG. 1 is a schematic configuration diagram of a moving body equipped with a moving body control apparatus according to the present embodiment. In the present embodiment, a vehicle C that autonomously travels as a moving body under the control of a moving body control device will be described as an example. 1A is a side view of the vehicle C, and FIG. 1B is a top view of the vehicle C. In the following, as shown in FIG. 1, the traveling direction of the vehicle C is the X axis, the width direction of the vehicle C perpendicular to the X axis is the Y axis, and the direction perpendicular to the XY plane (horizontal plane) (the height of the vehicle C A description will be given by setting an XYZ orthogonal coordinate system with the (direction) as the Z axis.

 As shown in FIG. 1, the moving body control device mounted on the vehicle C includes photographing devices 1a and 1b, an image processing device 2, a three-dimensional measurement device 3, a map creation device 4, a route planning device 5, and a GPS receiver. 6, a gyro 7, a position / posture estimation device 8, a motion control device 9, and an actuator 10. In addition, said imaging | photography apparatus 1a, 1b, the image processing apparatus 2, the three-dimensional measuring apparatus 3, the map creation apparatus 4, and the route planning apparatus 5 comprise the movement route planning apparatus in this invention.

 The photographing apparatuses 1a and 1b (photographing means) are, for example, infrared cameras, and are installed so as to be paired on the upper left and right sides of the vehicle C. Output to the image processing apparatus 2. As shown in FIG. 1, it is desirable to cover a vertical field of view (Z-axis direction) of 40 ° and a horizontal direction (X-axis direction) of 60 ° by these photographing apparatuses 1a and 1b. Further, since the sky information is unnecessary, it is desirable to install the photographing apparatuses 1a and 1b so that the horizontal line does not protrude from the upper end of the photographed image even if the vehicle body vibrates (for example, downward with respect to the Y axis). Install at an angle of 10 °, and shoot so that 30 ° below and 10 ° above the horizontal line are within the field of view).

 Although not shown in FIG. 1, an infrared projector may be installed on the upper portion of the vehicle C as necessary. Moreover, although the case where two imaging devices 1a and 1b are used is illustrated in the present embodiment, the number of imaging devices is not limited to this, and a sufficient imaging area can be secured by one imaging device. It is sufficient to install only one, and conversely, if one cannot secure a sufficient imaging area, a plurality of imaging devices may be installed as necessary. Moreover, you may use cameras other than an infrared camera as imaging device 1a, 1b.

 The image processing device 2 (image processing means) is, for example, a PC (Personal Computer) dedicated to image processing, and performs predetermined image processing based on image signals (that is, captured images) input from the imaging devices 1a and 1b. Thus, a traffic trace (such as a bag) existing in the photographed image is detected, and the traffic trace detection result is output to the route planning device 5. Hereinafter, the internal configuration of the image processing apparatus 2 will be described in detail with reference to FIG.

 FIG. 2 is a block configuration diagram of the image processing apparatus 2. As shown in FIG. 2, the image processing device 2 includes an A / D converter 2a, a storage device 2b, a signal processing device 2c, and a RAM (Random Access Memory) 2d.

 The A / D converter 2a converts the image signals input from the imaging devices 1a and 1b into digital data, and outputs the digital data to the signal processing device 2c as input image data indicating an input image (captured image). For example, assuming that the resolution of the photographing devices 1a and 1b is VGA (horizontal 640 × vertical 480 pixels), the input image data described above is luminance data indicating the luminance of each of the horizontal 640 × vertical 480 pixels constituting the input image. It becomes a set of. In addition, when the image signal converted into digital data is output from the imaging devices 1a and 1b, the A / D converter 2a may be omitted.

 The storage device 2b is, for example, an HDD (Hard Disk Drive), and previously stores an image processing program PG and a plurality (n) of two-dimensional Gabor filters (hereinafter abbreviated as Gabor filters) GF1 to GFn having different direction parameters. The image processing program PG and the Gabor filters GF1 to GFn are output to the signal processing device 2c in response to a read request from the signal processing device 2c.

 Here, the image processing program PG is an image process (filtering process, pattern direction specifying process, strong edge area extracting process, trace area detecting process) for detecting a traffic trace existing in the input image based on the input image data. ) Is executed by the signal processing device 2c.

The Gabor filter is a spatial frequency filter using a two-dimensional Gabor function obtained by multiplying a two-dimensional Gaussian function and a sine wave function propagating in one direction on a two-dimensional plane. It is a form. Assuming a two-dimensional coordinate system in which the horizontal direction is the X axis and the vertical direction is the Y axis, the two-dimensional Gabor function GB (x, y) is expressed by the following equation (1). In the following equation (1), u ₀ is a parameter indicating the angular frequency of the wave, and σ is a standard deviation (Gauss window width) of the Gauss function. The two-dimensional Gabor function GB (x, y) has a declination φ of an angular frequency u ₀ as a parameter (direction parameter) indicating a wave direction.

By filtering the input image data using a Gabor filter using the two-dimensional Gabor function GB (x, y) as described above, the frequency characteristics and texture (pattern) direction characteristics of the input image can be extracted. . Specifically, in the input image, a region having a texture direction parallel to the direction parameter (deflection angle φ) and having frequency characteristics close to the frequency parameter (angular frequency u ₀ ) is extracted with high sensitivity.

In the present embodiment, as described above, n Gabor filters GF1 to GFn having different directional parameters (deviation angle φ of the angular frequency u ₀ ) are prepared in advance and stored in the storage device 2b. Here, the Gabor filters GF1 to GFn are prepared so that the deflection angle φ is different at equal angular intervals. For example, when n = 72, the deflection angle φ is different by π / 72 (rad) = 2.5 °. That is, the direction parameter φ1 of the Gabor filter GF1 is set to 0 (rad), the direction parameter φ2 of the Gabor filter GF2 is set to π / 72 (rad) = 2.5 °, and the direction parameter φ3 of the Gabor filter GF3 is set to 2π. / 72 (rad) = 5 °, and similarly, the direction parameter φn of the Gabor filter GFn is set to 71π / 72 (rad) = 177.5 °.
Note that the value of n may be appropriately changed according to the resolution of the input image and the processing capability of the signal processing device 2c, but may be set in the range of 32 to 96 that is a multiple of 8, for example, considering digital processing. desirable.

On the other hand, the frequency parameter (angular frequency u ₀ ) and the attenuation parameter (standard deviation σ) of each Gabor filter GF1 to GFn are set to a predetermined value common to each filter. Here, the frequency parameter is set according to the width of the traffic trace to be detected. For example, the angular frequency u ₀ is calculated and set so that the wave wavelength (period) is 1.5 degrees to 4 degrees in the field of view. When the shooting area of the passage is an area of 10 m to 20 m in the depth direction, if the angular frequency u ₀ is set so as to correspond to a field of view of 3 degrees, the distance from the wave peak to the valley on the shooting target is 25 cm to 50 cm, a size slightly larger than the wheel, and a size close to a wheel mark formed as a result of the wheel passing a plurality of times. In other words, by setting the frequency parameter in this way, soot like a wheel trace can be easily extracted as a passage with high sensitivity. The attenuation parameter σ may be set to about 1 to 0.5 wavelength.

 The signal processing device 2c is, for example, a CPU (Central Processing Unit), and performs predetermined image processing (filtering processing, pattern direction specifying processing, strong edge) based on input image data in accordance with the image processing program PG read from the storage device 2b. By performing region extraction processing and trace region detection processing), traffic traces such as wrinkles present in the input image are detected, and the traffic trace detection results are output to the route planning device 5.

 Here, the filtering process is performed by reading out the Gabor filters GF1 to GFn from the storage device 2b and filtering the detection target areas in the input image using the Gabor filters GF1 to GFn. The process of generating intensity distribution data of φ1 to φn). The pattern direction specifying process refers to a process of specifying the texture (pattern) direction of each pixel in the detection target region based on the intensity distribution data generated by the filtering process.

 Further, the strong edge region extraction processing refers to processing for extracting a strong edge region existing in the detection target region. In addition, the trace area detection processing is performed by performing predetermined statistical processing on the basis of the result of specifying the texture direction of each pixel by the pattern direction specifying process and the result of extracting the strong edge area by the strong edge area extraction process. This refers to a process of detecting a trace area where a traffic trace is estimated to exist in the area. Details of these various image processes will be described later.

 The RAM 2d temporarily stores data necessary for the signal processing device 2c to execute various types of image processing (intensity distribution data, texture direction identification results, strong edge region extraction results, statistical data, etc.) and the above input image data. It is a rewritable volatile memory used for storing data.

The above is the description of the image processing apparatus 2, and the description will be continued below by returning to FIG.
The three-dimensional measuring device 3 (three-dimensional measuring means) is a laser range finder (LRF) that measures a three-dimensional shape in the traveling direction of the vehicle C, and is installed at a predetermined angle inclined downward in the upper center of the vehicle C. ing. Such an LFR has a swinging mechanism in the X-axis direction, irradiates the laser beam L so as to scan the scanning plane SP shown in FIG. The distance data d1, d2,..., Dn and the swing angle data α1, α2,..., Αn to the irradiation points P1, P2,. 4 is output.

 In addition, since the measurable range of commercially available LRF is about 30 m under adverse conditions, the LFR used as the three-dimensional measuring device 3 can also measure the installation angle and the installation height so that a three-dimensional shape 30 m ahead can be measured. It is desirable to set the size. Further, the three-dimensional shape may be measured by scanning the laser light L while swinging the LFR in the Z-axis direction. Further, as the three-dimensional measuring device 3, a device other than the LRF, such as a stereo camera, may be used.

The map creation device 4 (map creation means) is, for example, a 3D map generation device (PC (Personal Computer) dedicated to 3D map creation), and a 3D shape measurement result (distance data d1) obtained from the 3D measurement device 3. , D2,..., Dn, swing angle data α1, α2,..., Αn), a map of the traveling direction of the vehicle C (three-dimensional map) is created, and the three-dimensional map data is used as a route planning device. 5 is output. Since a method for creating a three-dimensional map from distance data and swing angle data obtained from LFR is already known, detailed description is omitted in this embodiment.
In addition to the three-dimensional map generation device, the map creation device 4 can also use a roughness / undulation map creation device. In this case, a value obtained by differentiating the height of the ground from which the amount of undulation is measured is obtained. Then, create a map by plotting the coordinates in bird's eye view as seen from above.

  The route planning device 5 (route planning means) is, for example, a PC (Personal Computer) dedicated to route planning. The route trace detection result obtained from the image processing device 2 and the three-dimensional map data obtained from the map creation device 4 are obtained. Based on this, the moving route of the vehicle C is planned, and the route planning data is output to the motion control device 9. The details of the method for planning the movement route will be described later.

  The GPS receiver 6 (position information acquisition means) is installed in the upper part of the vehicle C, and uses longitude (GPS) (Global Positioning System) to obtain longitude, latitude, and altitude as information related to the position of the vehicle C. The obtained result is output to the position / posture estimation device 8 as vehicle position data. The gyro 7 (attitude information acquisition means) is a three-axis gyro sensor installed at a predetermined position of the vehicle C, and the rotation angle and angular velocity of the vehicle C around the X axis (pitch) and the rotation around the Y axis (roll). The angle and angular velocity and the rotation angle and angular velocity around the Z axis (yaw) are acquired as posture information of the vehicle C, and the acquisition result is output to the position / posture estimation device 8 as vehicle posture data.

  The position / posture estimation device 8 (position / posture estimation means) is, for example, a PC (Personal Computer) dedicated to position / posture estimation, and includes vehicle position data obtained from the GPS receiver 6 and vehicle posture data obtained from the gyro 7. And the current position and orientation of the vehicle C are estimated based on feedback data relating to the movement amount of the vehicle C obtained from the actuator 10 described later, and the estimation result is output to the motion control device 9 as current position and orientation data. .

  The motion control device 9 (motion control means) is, for example, a PC (Personal Computer) dedicated to motion control of the vehicle C, and the route planning data obtained from the route planning device 5 and the current position obtained from the position / posture estimation device 8. Based on the attitude data, the actuator 10 is controlled so that the vehicle C moves along the planned movement route and moves within a predetermined allowable range centered on the movement route.

  The actuator 10 is a motor for driving the wheels of the vehicle C, a motor for driving the steering, a motor for driving the brake, and a sensor for measuring the driving state of these motors (for example, measuring the amount of rotation of the steering). A potentiometer, a rotary encoder, a rotary encoder that measures the amount of rotation of the wheel), and a motor amplifier that supplies a driving voltage or current to various motors under the control of the motion control device 9 while forming a feedback loop with them. ing. Further, data measured by various sensors is fed back to the position / posture estimation device 8 as feedback data relating to the movement amount of the vehicle C.

 In the present embodiment, the case where the image processing device 2, the map creation device 4, the route planning device 5, the position / posture estimation device 8, and the motion control device 9 are configured by independent PCs is exemplified. It is good also as a structure which concentrates a function on one PC.

 Next, the operation of the vehicle C (particularly the moving body control device) configured as described above will be described in detail.

(Traffic trace detection operation)
First, the traffic trace detection operation by the image processing apparatus 2 will be described. During the operation of the moving body control device, the photographing devices 1 a and 1 b photograph the traveling direction of the vehicle C at a predetermined camera rate, and output a signal (image signal) indicating the photographed image to the image processing device 2. The image signal input to the image processing device 2 is converted into input image data by the A / D converter 2a and then transferred to the signal processing device 2c.

  Here, it is assumed that the vehicle C is traveling in Kawahara, and a photographed image in which a soot as shown in FIG. 3A is captured from the photographing apparatuses 1a and 1b as a traffic trace is obtained. In the following, as shown in FIG. 3B, the horizontal direction of the captured image is the X-axis direction and the vertical direction is the Y-axis direction (this XY axis is irrelevant to the XYZ orthogonal coordinate system shown in FIG. 1). ), Luminance data corresponding to each pixel of the coordinates (X, Y) = (0, 0) to (639, 479) of the captured image is expressed as B (0, 0) to B (639, 479). That is, the input image data is a set of the luminance data B (0, 0) to B (639, 479). Such input image data is temporarily stored in the RAM 2d by the signal processing device 2c.

 FIG. 4 is an operation flowchart of the signal processing device 2c. As shown in FIG. 4, first, the signal processing device 2c reads input image data corresponding to a detection target area in the input image (captured image) from the RAM 2c, and reads the Gabor filters GF1 to GFn from the storage device 2b. By filtering the input image data using each Gabor filter GF1 to GFn, intensity distribution data in each direction is generated (step S1: filtering process).

 In the present embodiment, a case where the entire area of the input image is set as the detection target area will be described as an example. That is, input image data corresponding to the entire area of the input image is read from the RAM 2d. When a horizontal line can be extracted from the input image, an area below the horizontal line (that is, a ground area) may be set as a detection target area. As described above, by setting only a region where it is presumed that a traffic trace is surely present as a detection target region, it is possible to reduce the processing load of various image processing described below, and to shorten the processing time.

  Hereinafter, step S1 will be described in detail with reference to FIG. As shown in FIG. 5A, the signal processing device 2c performs the filtering process using the Gabor filters GF1 to GFn as the luminance data of each pixel included in the convolution region having a certain pixel as the central pixel in the input image. Then, a convolution calculation with a Gabor filter, that is, a two-dimensional Gabor function GB (x, y) is performed, and the calculation result is obtained as the intensity of the central pixel corresponding to the direction parameter φ of the Gabor filter.

 That is, the signal processing device 2c first performs a convolution operation between the luminance data of each pixel included in the convolution region having a certain pixel as a central pixel and the Gabor filter GF1, and the calculation result is used as the direction parameter φ1 of the Gabor filter GF1. Acquired as the intensity of the corresponding center pixel. Here, by creating each Gabor filter GF1 to GFn separately for the real part filter and the imaginary part filter, the intensity obtained by using the real part filter and the imaginary part filter are used. The final intensity can be obtained by squaring and adding the obtained intensities and obtaining the square root of the added value.

 Subsequently, the signal processing device 2c switches to the Gabor filter GF2 and similarly performs a convolution operation, and acquires the operation result as the intensity of the central pixel corresponding to the direction parameter φ2 of the Gabor filter GF2. Hereinafter, similarly, the signal processing device 2c performs the convolution calculation while sequentially switching the Gabor filters, and acquires the calculation result as the intensity of the central pixel corresponding to the direction parameters φ1 to φn of the Gabor filters GF1 to GFn. .

 The signal processing device 2c performs the filtering process as described above for each pixel of the coordinates (0, 0) to (639, 479) of the input image, so that each direction as shown in FIG. Intensity distribution data is generated and stored in the RAM 2d. In FIG. 5B, intensity data obtained with each pixel at coordinates (0, 0) to (639, 479) as the central pixel is I (0, 0) to I (639, 479). It is written.

 As can be seen from FIG. 5A, when the pixel close to the end of the input image is used as the central pixel, the convolution region protrudes from the input image. In this case, preset brightness data (for example, white Data, etc.) may be used as the luminance data of the convolution region protruding from the input image. In addition, the size of the convolution region may be changed as appropriate according to the size of the traffic trace to be detected. For example, when a heel corresponding to a wheel trace having a width of 25 cm to 50 cm is to be detected, the width It is desirable that the size be equal to or larger than the number of pixels corresponding to (32 pixels), for example, 50 × 50 pixels.

 Next, when step S1 as described above is completed, the signal processing device 2c reads the intensity distribution data in each direction from the RAM 2d, and specifies the texture (pattern) direction of each pixel in the input image based on the intensity distribution data. (Step S2: Pattern direction specifying process). Specifically, for example, when focusing on a pixel at coordinates (0, 0), a search is made for the largest intensity data I (0, 0) from the intensity distribution data in each direction, and the maximum intensity data I is obtained. The direction corresponding to the intensity distribution data including (0, 0) is specified as the texture direction of the pixel at coordinates (0, 0). For example, when the intensity distribution data corresponding to the direction φ3 includes the maximum intensity data I (0, 0), the direction φ3 is the texture direction of the pixel at the coordinates (0, 0).

 The signal processing device 2c identifies the texture direction of each pixel by performing the texture direction identification process as described above for each pixel of coordinates (0, 0) to (639, 479), and obtains the identification result. Store in RAM 2d. FIG. 6A shows the result of specifying the texture direction of each pixel, and FIG. 6B shows the input image superimposed with the result of specifying the texture direction. In FIG. 6, for the sake of convenience of explanation, the texture direction identification result is represented by a vector line indicating the texture direction. However, in actuality, the texture direction identification result is a value representing the direction (for example, the texture direction is If φ1, the value of φ1).

 As shown in Fig. 3, the traces of traffic, such as a kite that exists on uneven ground, remain as a result of the vehicle or the grass being stomped or solidified by driving the vehicle multiple times. In addition, the trace is observed as a discontinuous and unclear band-like or streak-like sparse region on the input image. Therefore, by performing step S1 and step S2 having the function of extracting the wave component of the input image described above, the interval between the patterns forming the traffic trace is indefinite and gentle, but parallel to the traveling direction of the traffic trace. It can be captured as a component of a wave.

 Next, when step S2 as described above ends, the signal processing device 2c performs a predetermined edge detection process based on the input image data to extract a strong edge region existing in the input image, and the extraction result Is stored in the RAM 2d (step S3: strong edge region detection processing). Here, a known technique can be adopted as the edge detection process. For example, by applying a Sobel filter in the X-axis direction and a Sobel filter in the Y-axis direction, a pixel whose sum of squares of the intensity exceeds a certain threshold is searched, and a pixel within a certain distance from the pixel exceeding the threshold is searched. Extract the region as a strong edge region.

 Next, when step S3 as described above ends, the signal processing device 2c performs predetermined processing based on the result of specifying the texture direction of each pixel by the pattern direction specifying process and the result of extracting the strong edge area by the strong edge area extracting process. By performing this statistical processing, a trace region in which a passage trace is estimated to exist in the input image is detected (step S4: trace region detection process). Specifically, the signal processing device 2c performs the voting process (step S4a), the vanishing point specifying process (step S4b), the valid voting pixel extracting process (step S4c), and the trace area specifying process (step) as the statistical process in step S4. S4d) is performed. Hereinafter, each process will be described in detail.

<Voting process>
First, as the voting process in step S4a, the signal processing device 2c sequentially selects each pixel in the input image as a voting source pixel, and the pixels existing on a straight line that starts from the voting source pixel and is parallel to the texture direction. Vote for. Specifically, as shown in FIG. 7, the signal processing device 2c first sets a vote count area for counting the number of votes in the memory space of the RAM 2d to the same size as the input image (that is, 640 × 480 pixels). Secure the number of votes for several minutes).

 Then, as shown in FIG. 8A, when the signal processing device 2c selects a certain pixel as the voting source pixel, the signal processing device 2c determines the texture direction of the voting source pixel from the result of specifying the texture direction of each pixel by the pattern direction specifying process. The voting is performed on pixels (voting target pixels) existing on a straight line parallel to the texture direction starting from the voting source pixel, and the voting result is voted on the RAM 2d as shown in FIG. 8B. Reflect in the total area. That is, as can be seen from FIG. 8B, the vote value “1” is added to the storage area corresponding to the vote target pixel in the vote counting area.

 The signal processing device 2c performs the voting process as described above for each pixel, and sequentially reflects the voting results in the vote counting area on the RAM 2d. Here, when the voting process as described above is performed, the signal processing device 2c excludes pixels included in the periphery (for example, within two pixels) of the strong edge area extracted by the strong edge area extraction process from the voting source pixels. . The reason for this will be described later.

 In the voting process described above, a case has been described in which voting is performed on a voting target pixel that exists on a straight line parallel to the texture direction from the voting source pixel. Alternatively, voting may be performed on pixels (voting target pixels) included in the voting target area set along the texture direction. This voting target area may be a strip (rectangular) parallel to the texture direction as shown in FIG. 9 (a), for example, or an arc or triangle as shown in FIG. 9 (b). May be good.

 Here, when setting a belt-like voting target region as shown in FIG. 9A, as in the case of voting on the voting target pixel existing on the straight line parallel to the texture direction, What is necessary is just to vote equally with respect to each of the voting target pixels included in the voting target area. That is, in the vote total area on the RAM 2d, the vote value “1” is added equally to the storage area corresponding to each vote target pixel.

 On the other hand, when setting an arc-shaped or triangular voting target area as shown in FIG. 9B, it is desirable to change the voting value according to the distance between the voting source pixel and the voting target pixel. . Specifically, the voting value of the voting target pixel located at a distance far from the voting source pixel is reduced (for example, voting value = 1 / distance from the voting source pixel). That is, the voting value of the voting target pixel near the voting source pixel is “1” or a value close to “1”, but the voting value of the voting target pixel far from the voting source pixel is “0.5” or the like. The value after the decimal point. Furthermore, it is desirable to determine the voting value of each voting target pixel so that the total voting value of the voting target pixels located at the same distance in the voting target region is “1”. That is, for example, assuming that there are four voting target pixels located at the same distance, the voting value for each of the four voting target pixels is set to “0.25”.

 When an arc-shaped or triangular voting target area is set in this way, the reason for changing the voting value according to the distance between the voting source pixel and the voting target pixel is that the voting position is far from the voting source pixel. This is because the target pixel has a high probability of overlapping with other voting target areas and tends to be voted more easily than the voting target pixel located at a close position, and it is difficult to accurately specify a vanishing point described later. In addition, when the voting target area having an arc shape or a triangular shape is set, the central angle (the angle from the voting source pixel) is changed by the change in the direction parameter φ of the Gabor filters GF1 to GFn (that is, π / 72 (rad ) = 2.5 °).

<Disappearance point identification processing>
Subsequently, as shown in FIG. 10, the signal processing device 2c, as shown in FIG. 10, as the vanishing point specifying process in step S4b, based on the voting result (that is, the number of votes counted in the vote counting area on the RAM 40), The pixel with the largest number of votes is identified as the vanishing point in the input image. When viewed from the vehicle C, a wave pattern of a traffic trace such as a bag is not completely parallel but is presumed to share the same vanishing point. For this reason, voting is performed on the position (pixel) that is estimated to be the vanishing point by the voting process as described above, and the vanishing point is determined by utilizing the detection result having many wave patterns that are statistically parallel to the traffic trace. Can be identified.

  Here, in the input image, when high contrast objects such as trees, telephone poles, and signboards are reflected in addition to the traffic traces, these patterns have a great influence on the above-mentioned counting of the number of votes. Therefore, as described above, when performing the voting process, the pixels included in the strong edge region extracted by the strong edge region extraction process are excluded from the voting source pixels, so that the contrast of trees, utility poles, signs, etc. It is possible to prevent an extension line of a high object from being specified as a vanishing point.

<Valid voting pixel extraction process>
Subsequently, the signal processing device 2c extracts the voting source pixel at the vanishing point as the effective voting pixel as the effective voting pixel extraction processing in step S4c. That is, a pixel having a texture direction facing the vanishing point is extracted as an effective voting pixel. FIG. 11 shows the result of extracting effective voting pixels. In FIG. 11, the white area is a collection area of effective voting pixels.

<Trace area identification processing>
Subsequently, as shown in FIG. 12, the signal processing device 2c performs, as the trace area specifying process in step S4d, an area having a vanishing point as a vertex in the input image (this area may be arc-shaped or triangular). Is divided into a plurality of small areas, and a small area in which the ratio of effective voting pixels exceeds a predetermined threshold is identified as a trace area in which a passing trace is estimated to exist. Here, the ratio of effective voting pixels can be obtained by dividing the area ratio between the small area and the effective voting pixel, that is, the number of effective voting pixels included in the small area by the total number of pixels included in the small area. .

 In the input image, it is estimated that the area in which the traffic traces such as wrinkles are reflected includes a texture facing the vanishing point at a higher ratio than the surrounding area. Therefore, by performing the effective voting pixel extraction process and the trace area specifying process as described above, it is possible to distinguish a trace area where a traffic trace exists in the input image from other areas.

  In addition, since a trace area | region is detected along traffic traces, such as a heel, the adjacent trace area | region should be connected continuously or intermittently with a certain regularity. Therefore, in the input image, a trace area isolated from a collection of trace areas having a certain regularity is regarded as noise, and such an isolated area is removed from the trace area, thereby preventing erroneous detection of a traffic trace. be able to.

  By the way, the above description assumes a case where an input image is obtained in which the traffic trace extends in a straight line toward the horizontal line as shown in FIG. However, as shown in FIG. 13, when the traffic trace in the input image is curved, the vanishing point changes in the horizontal direction for each depth of the input image. In such a case, if the entire area of the input image (the entire area of the detection target area) is processed at a time as described above, an accurate vanishing point cannot be obtained.

 Therefore, in order to deal with such a problem, the processing in step S4 (voting processing, vanishing point specifying processing, effective voting pixel extraction processing, and trace region specifying processing) is performed as shown in FIG. (Here, the entire area of the input image) may be performed for each sub-detection target area obtained by dividing the input image into a plurality of areas along the depth direction. In this way, the vanishing point is obtained for each sub-detection target area, and the effective voting pixels that match the vanishing point are obtained for each corresponding depth, so that the vanishing point and the trace area corresponding to each depth are cut out. Can do.

 Here, when a two-dimensional wavelet filter is used, a region having a strong direction on the input image is detected as a trace region such as a reed, so that a parallel of grass, trees, and artifacts growing outside the reed Features may be detected incorrectly. Therefore, as described above, when specifying the trace area for each sub-detection target area obtained by dividing into a plurality of areas along the depth direction, the detection accuracy can be improved by adding the processing described below. it can.

  As a result of experiments, it was found that the accuracy of wrinkle detection (detection of trace area) depends on the distance in the depth direction to each point, and the accuracy is higher at the front. In addition, it is considered that there is a high possibility that a region continuous with the detected wrinkle (trace region) is a wrinkle (trace region). Therefore, the input image is divided into several stages from the front side to the back side, and the wrinkle (trace area) existing in the sub detection target area on the front side is detected first, and the farther wrinkle (trace area) is determined based on that. It will be detected.

  As shown in FIG. 14, first, with respect to the sub detection target area (here, the first sub detection target area W1) in the foremost side in the depth direction, the process (voting process, disappearance) described in the above embodiment is performed. By performing point specifying processing, effective voting pixel extraction processing, and trace region specifying processing), the trace region specifying result in the first sub detection target region W1 is obtained. In FIG. 14, the identification result of the trace area is represented as a continuous straight line toward the vanishing point of the first sub detection target area W1.

  Subsequently, the process of step S4 is performed for the sub-detection target area in the next stage (here, the second sub-detection target area W2). In this voting process, the sub-detection target area currently focused on ( That is, in the second sub-detection target area W2), as shown in FIG. 14, the trace specified by the trace area specifying process performed on the sub-detection target area focused on before (that is, the first sub-detection target area W1). A voting weighting area Wa in which the voting weighting is increased is set on the end side of the area. Such a voting weighting area Wa is composed of a rectangular area of a predetermined size centered on each terminal portion on each straight line representing the identification result of the trace area. The size of each rectangular area is about 0.08 × pixel height in the vertical direction and 0.03 × pixel width in the horizontal direction.

  When the voting process in step S4 is performed on the second sub detection target area W2 in which the voting weighting area Wa is set in this way, the voting for the voting target pixel existing in the voting weighting area Wa is given a weight about three times the normal. Do. For example, when the voting value for the voting target pixel is “1” in the voting process of the first sub detection target area W1 in the first stage, the voting target pixel existing in the voting weighting area Wa of the second sub detection target area W2 The vote value is “3”.

  By performing the voting process, the vanishing point specifying process, the valid voting pixel extracting process, and the trace area specifying process in step S4 for the second sub detection target area W2, the first sub detection target area W1 in the previous stage is specified. The end of the trace area is likely to be the starting point of the trace area of the second sub detection target area W2 at the next stage. In other words, the trace area of the first sub-detection target area W1 in the previous stage and the trace area of the second sub-detection target area W2 in the next stage are easily detected as a continuous trace area, and in parallel with the wrinkles or the like in the input image Even in the presence of growing grass, trees, artifacts, etc., it is possible to accurately detect trace areas such as reeds.

  After obtaining the identification result of the trace area for the second sub detection target area W2 as described above, the same processing as the second sub target area W2 is performed for the sub detection target area of the next stage (third stage). In the third-stage sub-detection target area, the size of one rectangular area constituting the voting weighting area is half of the second-stage second sub-detection target area W2, that is, vertical: 0.04 × Pixel height, width: 0.015 × pixel width.

 When the voting process in step S4 is performed for the third sub-detection target area, the voting for the voting target pixel existing in the voting weighting area is weighted about five times as usual. For example, when the voting value for the voting target pixel is “1” in the voting process of the first sub detection target area W1 in the first stage, the voting target pixel existing in the voting weighting area of the sub detection target area in the third stage The vote value is “5”. Thus, by changing the size of the voting weighting area and the voting weighting amount in accordance with the position along the depth direction, the continuity of the trace area can be ensured with higher accuracy.

  Thereafter, by performing the same process up to the farthest sub-detection target area in the depth direction, the trace areas specified in the sub-detection target areas in each stage are accurately detected as continuous trace areas, and the vehicle such as a kite is traveling. It becomes possible to detect the trace accurately. In the fourth and subsequent sub-detection target areas, it is preferable that the size of each rectangular area constituting the voting weighting area and the voting weighting amount are the same as those of the third-stage sub-detection target area.

  FIG. 15A shows the result of specifying the trace area when the voting weighting area is not used. As shown in FIG. 15A, when the voting weighting area is not used, it can be seen that an area different from the area that should originally be the trace area is specified as the trace area. FIG. 15B shows the result of specifying the trace area when the voting weighting area is used. As shown in FIG. 15 (b), when the voting weighting area is used, it can be seen that the area that should originally be the trace area is correctly specified as the trace area.

 Note that, for example, after the trace area specifying process is completed for the currently focused sub detection target area (for example, the second sub detection target area W2), the sub detection target area (the first sub detection target area W1) that has been focused on before ), A voting weighting area having a larger voting weight is set on the start end side of the trace area specified by the trace area specifying process performed on the second sub-detection target area W2, and the first sub-detection target area W1 is set. By re-implementing the voting process, vanishing point specifying process, valid voting pixel extracting process, and trace area specifying process in step S4, the trace area specifying result for the first sub-detection target area W1 can be corrected. is there.

 As described above, the signal processing device 2c in the image processing device 2 performs the above-described image processing, thereby detecting a trace region in which an input trace is estimated to exist in the input image, and detecting the detection result as a passing trace. As a result, it is output to the route planning device 5. Note that the image processing described above need not be performed for each captured image sent from the image capturing apparatuses 1a and 1b at the camera rate, and may be performed for about 2 to 10 captured images per second.

(3D map creation operation)
Next, a three-dimensional map creation operation by the map creation device 4 will be described. During the operation of the moving body control device, the three-dimensional measurement device 3 (LRF) emits the laser beam L so as to scan the scanning plane SP shown in FIG. 1B, and the irradiation point P1 of the laser beam L , P2,..., Pn, distance data d1, d2,..., Dn and their swing angle data α1, α2,. To do. Here, as shown in FIG. 16A, the three-dimensional shape may be measured by scanning the laser beam L while the vehicle C is traveling, or the laser beam L while swinging the LFR in the Z-axis direction. The three-dimensional shape may be measured by scanning.

  Based on the three-dimensional shape measurement results (distance data d1, d2,..., Dn, swing angle data α1, α2,..., Αn) obtained from the three-dimensional measurement device 3, the map creating device 4 is shown in FIG. As shown in (b), a three-dimensional map of the traveling direction of the vehicle C is created, and the three-dimensional map data is output to the route planning device 5. Since a method for creating a three-dimensional map from distance data and swing angle data obtained from LFR is already known, detailed description is omitted in this embodiment.

(Route planning operation)
Next, the route planning operation by the route planning device 5 will be described. The route planning device 5 is based on the detection result of the traffic trace obtained from the image processing device 2 and the three-dimensional map data obtained from the map creation device 4, along the direction along the heel (traffic trace) and within a certain distance from the heel. The optimal movement route is planned from the surrounding three-dimensional shape under the condition that the distance is maintained.

  More specifically, the route planning device 5 converts a factor that decreases the stability of travel, such as undulations on the travel route and the steering amount at each point on the travel route, into a cost value corresponding to the degree, The travel route is planned so that the cost is minimized while satisfying the constraints of the vehicle performance such as speed and turning radius. Alternatively, the maximum speed allowed by the undulation on the movement route and the steering wheel is obtained, and the acceleration / deceleration that approximates the maximum speed on the movement route is simulated, and the arrival time is minimized. A travel route may be planned.

Also, changes in the surrounding three-dimensional shape are monitored from the created three-dimensional map, and if there is no significant change, the neighborhood is not re-planned, and is far away (for example, more than 1/2 of the planning range) You may re-plan only. As a result, it is possible to prevent the movement path from becoming unstable due to minute changes in the vicinity of the trajectory in the vicinity or vibration of the trajectory. On the other hand, when the traffic trace detection result such as a three-dimensional map or a bag changes greatly, it is desirable to re-plan the entire route. Furthermore, when the plan under the condition that the vehicle can stably travel in the range along the heel is not made, the mode is changed to a mode that does not consider the heel (that is, a mode that ignores the result of detection of the traffic trace obtained from the image processing device 2). Switching, as in the prior art, it is desirable to plan an optimal travel path using only a three-dimensional map.
The route planning device 5 plans the moving route of the vehicle C as shown in FIG. 17 and outputs the route planning data to the motion control device 9 by the method described above.

(Operation for estimating the current position and posture of the vehicle C)
Next, the current position and posture estimation operation of the vehicle C by the position / posture estimation device 8 will be described. During the operation of the mobile control device, the GPS receiver 6 acquires longitude, latitude, and altitude as information relating to the position of the vehicle C, and outputs the acquisition result to the position / posture estimation device 8 as vehicle position data. Further, the gyro 7 determines the rotation angle and angular velocity around the X axis (pitch) of the vehicle C, the rotation angle and angular velocity around the Y axis (roll), and the rotation angle and angular velocity around the Z axis (yaw). C position information is acquired, and the acquired result is output to the position / posture estimation apparatus 8 as vehicle position data. Further, data measured by various sensors of the actuator 10 is fed back to the position / posture estimation device 8 as feedback data relating to the movement amount of the vehicle C.

  In addition to the vehicle position data obtained from the GPS receiver 6, the vehicle attitude data obtained from the gyro 7, and the feedback data relating to the movement amount of the vehicle C obtained from the actuator 10, the position / orientation estimation device 8 receives GPS signals. Based on the measurement error of the vehicle position obtained from the number of satellites receivable by the aircraft 6 and the measurement error obtained from the wheel slip measured in advance on rough terrain, the current position and posture of the vehicle C are estimated and the estimation is performed. The result is output to the motion control device 9 as current position / posture data. Note that the Kalman filter and the particle filter are used for this estimation, whereby the current position and posture of the vehicle C can be obtained with higher accuracy.

(Motion control operation of vehicle C)
Next, the motion control operation of the vehicle C by the motion control device 9 will be described. The motion control device 9 moves along the planned movement route based on the route planning data obtained from the route planning device 5 and the current position / posture data obtained from the position / posture estimation device 8. Thus, the actuator 10 is controlled to move within a predetermined allowable range centered on the movement path.

  Specifically, first, the deviation amount with respect to the planned movement route is added based on the current position and posture of the vehicle C so that the planned movement route and speed are obtained, and the movement route is set within a predetermined allowable range. The target turning radius of the vehicle C is determined so as to be along (the turning radius of the moving path is determined as the target turning radius if it is sufficiently close to the moving path). Then, after the target turning radius is determined, the steering angle to be executed is determined from the current speed and the target turning radius based on a table that associates the steering angle / speed and the turning radius prepared in advance. The motion control device 9 instructs the actuator 10 on the amount of steering rotation so that the steering angle determined as described above is obtained.

On the other hand, the maximum speed for the vehicle C to turn safely is obtained from the current turning radius and the most severe (small radius) value of the turning radius of the nearby (about 10 m) moving route (the physical model of the vehicle C and the sand) A smaller one of the maximum speed and the planned speed is set as the target speed of the vehicle C. The motion control device 9 determines a control value using PID control or the like based on the target speed determined as described above and the current speed, and controls a motor for driving the wheels of the actuator 10.
If the amount of displacement with respect to the movement route becomes large, the route planning device 5 is instructed to re-plan the movement route, and a smooth movement route is planned from the current position without depending on the previous movement route. It is desirable to do.

  By the motion control operation by the motion control device 9 as described above, as shown in FIG. 18, the vehicle C moves along the planned travel route and within a predetermined allowable range centered on the travel route. Will be controlled to move.

  As described above, according to the present embodiment, even if the terrain is generally flat and gentle, such as rough terrain, it is possible to detect a traffic trace (such as a hail or a fumigation trace) like a certain road. Is possible. And the traffic trace in rough terrain arises as a result of the fact that the vehicle has actually passed several times before, and it is highly possible that the extension line is an area that can continue to pass. By predicting a direction with a high possibility of passage outside the measurement area using such a traffic trace, it is possible to plan an efficient movement route including outside the measurement area.

In addition, this invention is not limited to the said embodiment, The following modifications can be considered.
(1) In the above embodiment, the case where the frequency parameter (angular frequency u ₀ ) of each of the Gabor filters GF1 to GFn is a fixed value is exemplified. This is because, when the shooting area of the passage is an area of 10 m to 20 m in the depth direction, if the angular frequency u ₀ is set to correspond to 3 degrees in the field of view, the wave peaks and valleys on the shooting target This is because the distance is 25 cm to 50 cm, which is slightly larger than the wheel, and is close to a wheel mark that can be obtained as a result of passing the wheel multiple times. However, as can be seen from the input image in FIG. 3, the width of the traffic trace such as the heel varies depending on the position in the depth direction. Therefore, it is desirable to change the frequency parameters of the Gabor filters GF1 to GFn according to the position in the depth direction in the input image.

 Specifically, a Gabor filter having a frequency parameter corresponding to the position in the depth direction in the input image and having a different direction parameter is stored in advance in the storage device 2b, and the detection target from the storage device 2b is stored in the signal processing device 2c. What is necessary is just to have a function of reading and filtering a Gabor filter having a frequency parameter corresponding to the filtering target position in the region (that is, the position of the central pixel in FIG. 5).

(2) In the above embodiment, the case where a two-dimensional Gabor filter is used as the spatial frequency filter is exemplified. However, the present invention is not limited to this, and other two-dimensional wavelet filters, filters using a two-dimensional windowed Fourier transform, and the like can be used. It may be used as a frequency filter.

(3) In the above embodiment, the case where the spatial frequency filter is used as the image processing technique for detecting the traffic trace existing in the input image has been described as an example. However, the present invention is not limited to this, and the traffic trace is detected. Other techniques may be adopted as long as they are possible image processing techniques. That is, a method of detecting a band-shaped pattern having a width from a photographed image as a passing trace, or a plurality of band-shaped patterns having a width and intermittent in the middle are parallel in a three-dimensional space. To detect a moving part as a moving trace, or to find a band-like part that has a width and is intermittent in the middle of the captured image, and is moving toward the vanishing point shared by the largest number of belt-like parts Other techniques may be used as long as the technique narrows down the belt-like portion as a passing trace.

(4) In the above-described embodiment, the vehicle C is illustrated and described as the moving body on which the moving body control device is mounted. However, the moving body is not limited thereto, and may be a moving body that walks on two legs, such as a robot. The moving body control device according to the present invention can be applied.

  C ... Vehicle, 1a, 1b ... Imaging device, 2 ... Image processing device, 3 ... Three-dimensional measuring device, 4 ... Three-dimensional map creation device, 5 ... Path planning device, 6 ... GPS receiver, 7 ... Gyro, 8 ... Position / attitude estimation device, 9 ... motion control device, 10 ... actuator, 2a ... A / D converter, 2b ... storage device, 2c ... signal processing device, 2d ... RAM (Random Access Memory), PG ... passage detection program, GF1 ~ GFn ... Gabor filter

Claims (11)

Photographing means for photographing the traveling direction of the moving body;
Image processing means for detecting traffic traces present in the photographed image by performing predetermined image processing based on the photographed image obtained from the photographing means;
Route planning means for planning a moving route of the moving body based on the traffic trace detected by the image processing means;
Three-dimensional measuring means for measuring the three-dimensional shape of the moving body in the traveling direction;
A map creation means for creating a map based on the measurement result of the three-dimensional shape obtained from the three-dimensional measurement means,
The route planning unit plans the moving route of the moving body based on the map created by the map creating unit and the traffic trace detected by the image processing unit . The movement path planning apparatus according to claim 1 , wherein the image processing unit detects a band-like pattern having a width as the traffic trace . The image processing means detects a plurality of strip-like patterns having a width and being intermittent in the middle as a part that is parallel in a three-dimensional space as the traffic trace. 1. The movement path planning device according to 1. The image processing means finds a band-like portion having a width and being intermittent in the middle from the photographed image, and the band-like portion facing the vanishing point shared by the most numerous belt-like portions is taken as the traffic trace. The movement path planning apparatus according to claim 1 , wherein the movement path planning apparatus is narrowed down as follows . The image processing means includes
A filtering process for generating intensity distribution data in each direction for the detection target region by filtering the detection target region in the captured image using a plurality of spatial frequency filters having different direction parameters;
A pattern direction specifying process for specifying a pattern direction of each pixel in the detection target region based on the intensity distribution data generated by the filtering process;
And performing a predetermined statistical process based on the pattern direction identification result of the pattern direction identification process, thereby performing a trace area detection process for detecting a trace area that is estimated to have a traffic trace in the detection target area.
The movement path planning apparatus according to claim 1 . The image processing means includes
A filtering process for generating intensity distribution data in each direction for the detection target region by filtering the detection target region in the captured image using a plurality of spatial frequency filters having different direction parameters;
A pattern direction specifying process for specifying a pattern direction of each pixel in the detection target region based on the intensity distribution data generated by the filtering process;
For each sub-detection target region obtained by dividing the detection target region into a plurality of portions along the depth direction, predetermined sub-statistic processing is performed based on the pattern direction identification result by the pattern direction identification processing, thereby And a trace area detection process for detecting a trace area that is estimated to have a traffic trace in the detection target area.
The movement path planning apparatus according to claim 1 . The image processing means, as the statistical processing in the trace area detection processing,
Each pixel in the detection target area is selected as a voting source pixel, and the pixel existing on a straight line parallel to the pattern direction starting from the voting source pixel, or along the pattern direction starting from the voting source pixel A voting process for voting on pixels included in the voting target area set as described above,
A vanishing point identifying process that identifies a pixel with the largest number of votes as a vanishing point based on the voting result of the voting process;
An effective voting pixel extraction process for extracting the voting source pixel of the vanishing point as an effective voting pixel;
In the detection target area, an area having the vanishing point as a vertex is divided into a plurality of small areas, and a small area in which the ratio of the effective voting pixels exceeds a predetermined threshold is specified as the trace area. Trace area identification processing
The movement path planning apparatus according to claim 5, wherein: The image processing means, as the statistical processing in the trace area detection processing,
Each pixel in the sub-detection target region is selected as a voting source pixel, and the pixel existing on a straight line parallel to the pattern direction from the voting source pixel, or the voting source pixel as a starting point in the pattern direction A voting process for voting on the pixels included in the voting target area set along
A vanishing point identifying process that identifies a pixel with the largest number of votes as a vanishing point based on the voting result of the voting process;
An effective voting pixel extraction process for extracting the voting source pixel of the vanishing point as an effective voting pixel;
In the sub detection target area, an area having the vanishing point as a vertex is divided into a plurality of small areas, and a small area in which the ratio of the effective voting pixels exceeds a predetermined threshold is specified as the trace area. Trace area identification processing to be performed
The movement path planning apparatus according to claim 6 . The movement path planning device according to any one of claims 1 to 8,
An actuator for generating power of the moving body;
Motion control means for controlling the actuator so that the moving body moves along the movement path planned by the movement path planning device;
A moving body control apparatus comprising: Position information acquisition means for acquiring information relating to the position of the moving body;
Attitude information acquisition means for acquiring information relating to the attitude of the moving body;
A position / posture estimation unit that estimates the current position and posture of the moving body based on the information acquired by the position information acquisition unit and the posture information acquisition unit;
The motion control means controls the actuator so that the moving body moves within a predetermined allowable range centering on the moving path based on the current position and posture of the moving body estimated by the position / posture estimating means. The mobile body control device according to claim 9, wherein the mobile body control device is controlled. A moving body equipped with the moving body control device according to claim 9.