JP6369897B2 - Self-position calculation device and self-position calculation method - Google Patents

Description

  The present invention relates to a self-position calculation device and a self-position calculation method, and more particularly to a technique for improving the accuracy of estimating a movement amount.

  2. Description of the Related Art Conventionally, a technique is known in which an image in the vicinity of a vehicle captured by a camera mounted on the vehicle is acquired and the amount of movement of the vehicle is obtained based on a change in the image (see Patent Document 1). In Patent Document 1, in order to obtain the amount of movement with high precision even when the vehicle is moving at low speed and delicately, the feature point is detected from the image, the position of the feature point on the image is obtained, the moving direction and the movement of the feature point The movement amount of the vehicle is obtained from the distance (movement amount).

  In addition, a technique for performing three-dimensional measurement using a laser projector that projects a lattice pattern (pattern light) is known (see Patent Document 2). In Patent Document 2, a light projection area of pattern light is imaged by a camera, pattern light is extracted from the captured image, and the distance and posture angle of the vehicle with respect to the road surface are obtained from the position of the pattern light.

  If these two techniques are used, the distance and posture angle with respect to the road surface can be calculated from the pattern light, and the movement amount of the vehicle can be calculated with high accuracy by measuring the movement amount of the road surface.

JP 2008-175717 A JP 2007-278951 A

  However, it is difficult to stably extract the pattern light described in Patent Document 2 from a captured image due to the influence of sunlight in the daytime under fine weather when the road surface illuminance is high. Therefore, there has been a problem that it is difficult to obtain the distance and the attitude angle with respect to the road surface of the vehicle.

  The present invention has been made in view of the above problems, and an object of the present invention is to provide a self-position calculating device capable of stably and highly accurately estimating the amount of movement of a vehicle even when the illuminance on the road surface is high. It is to provide a self-position calculation method.

  In order to achieve the above object, a self-position calculation apparatus and method according to one aspect of the present invention project pattern light onto a road surface around a vehicle and control the pattern light to change periodically. Then, the road surface around the vehicle including the area where the pattern light is projected is captured to acquire an image. And the pattern light which changes periodically from the acquired image is extracted, and the attitude angle of the vehicle with respect to the road surface is calculated from the position of the extracted pattern light. Further, a plurality of feature points on the road surface are detected from the acquired image, and the amount of change in the posture of the vehicle is calculated based on the time change of each feature point. As a result, the self-position calculation device and method calculate the current position and posture angle of the vehicle by adding the amount of posture change to the initial position and posture angle of the vehicle. Here, the self-position calculating device and method detect the vehicle speed of the vehicle, and if the vehicle speed is equal to or less than a predetermined threshold, the current position and posture angle of the vehicle at that time are set to the initial position and posture angle of the vehicle. Start adding posture change.

  According to the present invention, even when the illuminance on the road surface is high, the amount of movement of the vehicle can be calculated stably and with high accuracy.

It is a block diagram which shows the structure of the self-position calculation apparatus which concerns on one Embodiment of this invention. It is explanatory drawing which shows the light projection pattern irradiated with the light projector mounted in the vehicle, and the imaging range by a camera. (A) is a figure which shows a mode that the position on the road surface irradiated with each spot light is calculated from the base line length Lb between a projector and a camera, and the coordinate (Uj, Vj) on the image of each spot light. (B) is an explanatory view showing a state in which the moving direction of the camera is obtained from the temporal change of the feature points detected from another area different from the area irradiated with the pattern light. (A) is the figure which shows the image of the pattern light which binarized the image acquired with the camera, (b) is the enlarged view, (c) is each spot light extracted by the pattern light extraction part It is a figure which shows the position of the gravity center. It is a schematic diagram for demonstrating the method of calculating the variation | change_quantity of a distance and a posture angle. (A) shows an example of an image acquired at time t, and (b) shows an example of an image acquired at time (t + Δt) when time Δt has elapsed from time t. It is a flowchart which shows the process sequence of the self-position calculation process by the self-position calculation apparatus which concerns on one Embodiment of this invention. It is a timing chart which shows the luminance change by the self position calculation device concerning one embodiment of the present invention.

  Hereinafter, an embodiment of the present invention will be described with reference to the drawings.

[Configuration of self-position calculation device]
FIG. 1 is a block diagram showing a configuration of a self-position calculation apparatus 100 according to an embodiment of the present invention. As shown in FIG. 1, the self-position calculating device 100 includes a projector 11, a camera 12 (imaging unit), an engine control unit (ECU) 13, and a vehicle speed sensor 14. The projector 11 is mounted on a vehicle and projects pattern light whose luminance periodically changes on a road surface around the vehicle. Details of the pattern light will be described later. The camera 12 is mounted on a vehicle and captures an image of a road surface around the vehicle including an area where pattern light is projected. The ECU 13 is an example of a control unit that controls the operation of the projector 11 and executes a series of information processing cycles for estimating the amount of movement of the vehicle from the image acquired by the camera 12.

  As shown in FIG. 2, the projector 11 projects pattern light 32 b having a predetermined shape including a square or rectangular lattice image toward the road surface 31 within the imaging range of the camera 12. At this time, the luminance of the pattern light 32b is controlled by the pattern light control unit 27 so as to periodically change.

  The camera 12 is a digital camera using a solid-state imaging device, for example, a CCD and a CMOS, and acquires a digital image that can be processed. The imaging target of the camera 12 is a road surface around the vehicle, and the road surface around the vehicle includes the road surface of the front, rear, side, and bottom of the vehicle. For example, as shown in FIG. 2, the camera 12 can be mounted on the front portion of the vehicle 10, specifically on the front bumper.

  The height and direction at which the camera 12 is installed are adjusted so that the feature point (also referred to as “texture”) on the road surface 31 in front of the vehicle 10 and the pattern light 32b projected by the projector 11 can be imaged. The focus and aperture of the lens provided in the camera 12 are automatically adjusted. Here, the “feature point” is an uneven portion present on the asphalt.

  The camera 12 repeatedly captures images at a predetermined time interval and acquires a series of images (frames). For example, when the cycle for changing the luminance of the pattern light 32b is 50 [Hz], imaging is performed 600 times per second. As a result, it is possible to periodically capture images at the time when the luminance of the pattern light 32b is the highest and the time at which the pattern light 32b is the lowest. The image data acquired by the camera 12 is transferred to the ECU 13 for each imaging cycle and stored in a memory (not shown) provided in the ECU 13.

  The camera 12 images the pattern light irradiated on the road surface 31. The projector 11 includes, for example, a laser pointer and a diffraction grating. As shown in FIGS. 2 to 4, the projector 11 diffracts the laser light emitted from the laser pointer with a diffraction grating, and as shown in FIG. 2 to FIG. 4, pattern light (pattern light consisting of a plurality of spot lights arranged in a matrix form) 32b, 32a). In the example shown in FIGS. 3 and 4, the pattern light 32a composed of 5 × 7 spot light is generated.

  Returning to FIG. 1, the ECU 13 includes a microcontroller including a CPU, a memory, and an input / output unit, and configures a plurality of information processing units included in the self-position calculating device 100 by executing a computer program installed in advance. . The ECU 13 repeatedly executes a series of information processing cycles for estimating the amount of movement of the vehicle from the image acquired by the camera 12 for each image (frame). The ECU 13 may also be used as an ECU used for other controls related to the vehicle 10.

  The vehicle speed sensor 14 calculates the current vehicle speed of the vehicle 10 from the output of a rotary encoder or the like attached to a wheel or a differential gear.

  The plurality of information processing units include a pattern light extraction unit 21, an attitude angle calculation unit 22, a feature point detection unit 23, an attitude change amount calculation unit 24, a self position calculation unit 26, and a pattern light control unit 27. The vehicle speed detection unit 28 is included. The posture change amount calculation unit 24 includes a feature point detection unit 23.

  The pattern light control unit 27 controls the projection of the pattern light 32 a by the projector 11. For example, when the ignition switch of the vehicle 10 is turned on and the self-position calculating device 100 is activated, the pattern light control unit 27 starts projecting the pattern light 32a whose luminance changes periodically. Thereafter, the pattern light control unit 27 continuously projects the pattern light 32a until the self-position calculating device stops. Alternatively, the pattern light 32a may be projected as necessary. In the present embodiment, as an example of the luminance change, the power supplied to the projector 11 is controlled so that the luminance of the light projection pattern changes in a sine wave shape with a predetermined frequency. For example, as shown in FIG. 8 to be described later, the luminance is controlled to change in a sine wave shape.

  The pattern light extraction unit 21 reads the image acquired by the camera 12 from the memory, and further performs the synchronous detection process at the above-described predetermined frequency, thereby extracting the pattern light 32a included in the image. The synchronous detection process will be described later. Then, the position of the pattern light 32a is extracted from the acquired pattern light 32a. As shown in FIG. 3A, for example, the projector 11 projects pattern light 32a composed of a plurality of spot lights arranged in a matrix toward the road surface 31, and the pattern light 32a reflected by the road surface 31 is emitted. Detection is performed by synchronous detection processing or difference calculation processing.

  Further, by performing binarization processing on the extracted pattern light image, only the image of the spot light Sp is extracted as shown in FIG. 4A and an enlarged view of FIG. 4B. . As shown in FIG. 4C, the pattern light extraction unit 21 calculates the position He of the center of gravity of each spot light Sp, that is, the coordinates (Uj, Vj) on the image of the spot light Sp. The position 32a is extracted. The coordinates are in units of pixels of the image sensor of the camera 12, and in the case of 5 × 7 spot light Sp, “j” is an integer from 1 to 35. The coordinates (Uj, Vj) on the image of the spot light Sp are stored in the memory as data indicating the position of the pattern light 32a.

  Next, the synchronous detection process will be described. If the measurement signal included in the image captured by the camera 12 is sin (ω0 + α), the measurement signal includes sunlight and artificial light having various frequency components in addition to the projection pattern. Therefore, when the reference signal sin (ωr + β) whose frequency is the modulation frequency ωr is multiplied by the measurement signal sin (ω0 + α), the result is cos (ω0−ωr + α−β) / 2-cos (ω0 + ωr + α + β) / 2.

  When this measurement signal is passed through a low-pass filter, a signal of ω0 ≠ ωr, that is, sunlight and artificial light other than the projection pattern whose frequency is not ωr is removed. On the other hand, a signal of ω0 = ωr, that is, a light projection pattern having a frequency of ωr can be extracted as cos (α−β) / 2. As described above, by using the synchronous detection processing, it is possible to obtain an image in which only the projection pattern is extracted from the measurement signal including various frequency components.

  That is, in the present embodiment, the pattern light control unit 27 modulates the luminance of the pattern light 32a with a predetermined modulation frequency ωr set in advance. Accordingly, a light projection pattern whose luminance is modulated at the frequency ωr is projected onto the road surface. And the pattern light extraction part 21 can extract only a light projection pattern by multiplying the image (measurement signal) imaged with the camera 12 by the modulation frequency (omega) r.

  The posture angle calculation unit 22 shown in FIG. 1 reads data indicating the position of the pattern light 32a from the memory, and calculates the distance and posture angle of the vehicle 10 with respect to the road surface 31 from the position of the pattern light 32a in the image acquired by the camera 12. calculate. For example, as shown in FIG. 3A, from the baseline length Lb between the projector 11 and the camera 12 and the coordinates (Uj, Vj) on the image of each spot light, The position on the road surface 31 irradiated with the spot light is calculated as a relative position with respect to the camera 12. Then, the attitude angle calculation unit 22 calculates the plane type of the road surface 31 on which the pattern light 32a is projected, that is, the distance and the attitude angle of the camera 12 with respect to the road surface 31 (normal vector) from the relative position of each spot light with respect to the camera 12. ) Is calculated. In addition, since the mounting position and imaging direction of the camera 12 with respect to the vehicle 10 are known, in the embodiment, the distance and posture angle of the camera 12 with respect to the road surface 31 are calculated as an example of the distance and posture angle of the vehicle 10 with respect to the road surface 31. To do. Hereinafter, the distance and posture angle of the camera 12 with respect to the road surface 31 are abbreviated as “distance and posture angle”. The distance and the posture angle calculated by the posture angle calculation unit 22 are stored in the memory.

  Specifically, since the camera 12 and the projector 11 are respectively fixed to the vehicle 10, the irradiation direction of the pattern light 32a and the distance between the camera 12 and the projector 11 (baseline length Lb in FIG. 3) are known. Therefore, the attitude angle calculation unit 22 uses the principle of triangulation to determine the position on the road surface 31 irradiated with each spot light from the coordinates (Uj, Vj) of each spot light on the image relative to the camera 12. (Xj, Yj, Zj).

  The relative position (Xj, Yj, Zj) of each spot light with respect to the camera 12 often does not exist on the same plane. This is because the relative position of each spot light changes according to the asphalt unevenness exposed on the road surface 31. Therefore, a plane formula that minimizes the sum of squares of distance errors with respect to each spot light may be obtained using the least square method.

  The feature point detection unit 23 reads an image acquired by the camera 12 from the memory, and detects a feature point on the road surface 31 from the image read from the memory. Specifically, the feature point detection flag is set to “1” in a time zone in which the luminance of the pattern light whose luminance changes sinusoidally at a predetermined frequency is equal to or lower than a preset threshold luminance, and the feature point in this time zone Is detected.

  Further, the feature point detection unit 23 detects, for example, “DG Lowe,“ Distinctive Image Features from Scale-Invariant Keypoints, ”Int. J. Comput. Vis., Vol. , no.2, pp. 91-110, Nov. 200, or “Kanazawa Satoshi, Kanaya Kenichi,“ Image Feature Extraction for Computer Vision, ”IEICE Journal, vol.87, no.12, pp.1043-1048, Dec. 2004 "can be used.

  Specifically, the feature point detection unit 23 uses, for example, a Harris operator or a SUSAN operator to detect, as a feature point, a point whose luminance value changes greatly as compared to the surroundings, such as a vertex of an object. Alternatively, the feature point detection unit 23 may detect, as a feature point, a point where the luminance value changes under certain regularity using a SIFT (Scale-Invariant Feature Transform) feature amount. . Then, the feature point detection unit 23 counts the total number N of feature points detected from one image, and assigns an identification number (i (1 ≦ i ≦ N)) to each feature point. The position (Ui, Vi) on the image of each feature point is stored in a memory in the ECU 13. FIGS. 6A and 6B show examples of feature points Te detected from an image acquired by the camera 12. Furthermore, the change direction and change amount of each feature point Te are shown as a vector Dte.

  In the embodiment, the feature points on the road surface 31 are mainly assumed to be asphalt mixture grains having a size of 1 cm to 2 cm. In order to detect this feature point, the resolution of the camera 12 is VGA (approximately 300,000 pixels). Moreover, the distance of the camera 12 with respect to the road surface 31 is about 70 cm. Furthermore, the imaging direction of the camera 12 is inclined toward the road surface 31 by about 45 degrees from the horizontal plane. In addition, the luminance value when the image acquired by the camera 12 is transferred to the ECU 13 is in the range of 0 to 255 (0: darkest, 255: brightest).

  The posture change amount calculation unit 24 includes the position coordinates (Ui, Vi) on the image of a plurality of feature points included in the image of the previous (time t) frame among the images of each frame imaged for each fixed information processing cycle. ) From memory. Furthermore, the position coordinates (Ui, Vi) on the image of a plurality of feature points included in the image of the current (time t + Δt) frame are read from the memory. And the attitude | position change amount of the vehicle 10 is calculated | required based on the position change on the image of a some feature point. Here, the “vehicle attitude change amount” includes both the “distance and attitude angle change amount” of the vehicle 10 relative to the road surface and the “vehicle movement amount” on the road surface. Hereinafter, a calculation method of “the amount of change in the distance and posture angle” and “the amount of movement of the vehicle” will be described.

  The amount of change in the distance and the posture angle can be obtained as follows, for example. FIG. 6A shows an example of the first frame image 38 acquired at time t. As shown in FIGS. 5 and 6A, a case is considered in which, for example, relative positions (xi, yi, zi) of three feature points Te1, Te2, Te3 are calculated in the image 38, respectively. In this case, the plane G (see FIG. 6A) specified by the feature points Te1, Te2, Te3 can be regarded as a road surface. Therefore, the posture change amount calculation unit 24 can obtain the distance and posture angle (normal vector) of the camera 12 with respect to the road surface (plane G) from the relative position (xi, yi, zi) of each feature point. Furthermore, the posture change amount calculation unit 24 forms distances (11, 12, 13) between the feature points Te1, Te2, Te3 and straight lines connecting the feature points Te1, Te2, Te3 by a known camera model. The angle can be determined. The camera 12 in FIG. 5 shows the position of the camera in the first frame (time t).

  It should be noted that as the three-dimensional coordinates (xi, yi, zi) indicating the relative position of the feature point with respect to the camera 12, the imaging direction of the camera 12 is set to the Z axis, the imaging direction is a normal line, and the plane including the camera 12 is included. , X axis and Y axis orthogonal to each other are set. On the other hand, as the coordinates on the image 38, the horizontal direction and the vertical direction are set to the V axis and the U axis, respectively.

  FIG. 6B shows a second frame image 38 ′ acquired at time (t + Δt) when time Δt has elapsed from time t. The camera 12 ′ in FIG. 5 indicates the position of the camera 12 when the second frame image 38 ′ is captured. As shown in FIGS. 5 and 6B, in the image 38 ′, the camera 12 ′ picks up the feature points Te1, Te2, Te3, and the feature point detection unit 23 detects the feature points Te1, Te2, Te3. In this case, the posture change amount calculation unit 24 calculates the relative positions (xi, yi, zi) of the feature points Te1, Te2, Te3 at time t and the position P1 (Ui, Vi) of the feature points on the image 38 ′. From the camera model of the camera 12, the movement amount ΔL (see FIG. 5) of the camera 12 at the time Δt can be calculated. As a result, the movement amount of the vehicle 10 can be calculated. Furthermore, the amount of change in distance and posture angle can also be calculated. For example, by solving the simultaneous equations consisting of the following formula (1) with respect to R and T, the posture change amount calculation unit 24 can change the movement amount (ΔL) of the camera 12 (vehicle) and the change amounts of the distance and posture angle. Can be calculated. Here, R is a three-dimensional rotation matrix that represents the amount of change in the posture angle of the camera, and T is a three-dimensional translation vector that represents the moving direction of the camera. The following equation (1) is modeled as an ideal pinhole camera in which the camera 12 has no distortion or optical axis deviation, λi is a constant, and f is a focal length. The camera model parameters may be calibrated in advance.

  Note that the posture change amount calculation unit 24 may select optimal feature points based on the positional relationship between feature points, instead of using all the feature points detected at time t and time t + Δt. As a selection method, for example, epipolar geometry (Epi polar geometry, RI Hartley: “A linear method for reconstruction from lines and points,” Proc. 5th International Conference on Computer Vision, Cambridge, Massachusetts, pp.882-887 (1995) )) Can be used.

  As described above, the posture change amount calculation unit 24 also uses the feature points Te1, Te2, and Te3 whose relative positions (xi, yi, zi) are calculated in the frame image 38 at time t from the frame image 38 ′ at time t + Δt. If detected, the attitude change amount and the movement amount of the vehicle can be calculated. That is, the posture change amount calculation unit 24 calculates “the change in vehicle posture” from the time change of the relative position (xi, yi, zi) of the plurality of feature points on the road surface and the position (Ui, Vi) of the feature points on the image. Amount "can be calculated. Furthermore, the amount of movement of the vehicle 10 can be calculated.

  Therefore, if it is possible to continue detecting three or more feature points that can be correlated between the previous frame and the current frame, by continuing the process of adding the distance and the change amount of the posture angle (that is, the integral calculation). The distance and the posture angle can be continuously updated without using the pattern light 32a. However, in the first information processing cycle, a distance and posture angle calculated using the pattern light 32a or a predetermined initial distance and posture angle may be used. That is, the distance and the attitude angle that are the starting points of the integration calculation may be calculated using the pattern light 32a, or may use predetermined initial values. It is desirable that the predetermined initial distance and posture angle be a distance and posture angle in consideration of at least an occupant and a load on the vehicle 10. For example, when the ignition switch of the vehicle 10 is turned on and the shift position moves from parking to another position, the pattern light 32a is projected, and the distance and posture angle calculated from the pattern light 32a are set to a predetermined initial value. What is necessary is just to use as a general distance and a posture angle. Thereby, the distance and the posture angle when the roll motion or the pitch motion due to the turning or acceleration / deceleration of the vehicle 10 is not generated can be obtained.

  In order to associate the feature points between the preceding and following frames, for example, an image of a small area around the detected feature points may be recorded in a memory and determined from the similarity of luminance and color information. Specifically, the ECU 13 records an image for 5 × 5 (horizontal × vertical) pixels centered on the detected feature point in the memory. For example, if the luminance information is 20 pixels or more and the error is within 1% or less, the posture change amount calculation unit 24 determines that the feature point is a feature point that can be correlated between the previous and next frames. Then, the posture change amount acquired by the above processing is used when the self-position calculating unit 26 in the subsequent stage calculates the self-position of the vehicle 10.

  The self-position calculator 26 shown in FIG. 1 calculates the distance and posture angle of the vehicle 10 from the “distance and change amount of posture angle” calculated by the posture change amount calculator 24. Further, the self position of the vehicle 10 is calculated from the “movement amount of the vehicle” calculated by the posture change amount calculation unit 24.

  As a specific example, a case where the distance and posture angle calculated by the posture angle calculation unit 22 (the distance and posture angle calculated using the pattern light) are set as the starting points will be described. In this case, the self-position calculation unit 26 sequentially adds the distance and the change amount of the posture angle for each frame calculated by the posture change amount calculation unit 24 with respect to the starting point (distance and posture angle). Update the distance and posture angle to the latest values. Further, the vehicle position when the distance and posture angle are calculated by the posture angle calculation unit 22 is set as a starting point (initial position of the vehicle), and the amount of movement of the vehicle 10 from this initial position is sequentially added (integral calculation is performed). The self-position of the vehicle 10 is calculated. For example, the current position of the vehicle on the map can be sequentially calculated by setting the starting point (the initial position of the vehicle) that is collated with the position on the map.

  Therefore, the posture change amount calculation unit 24 can calculate the self-position of the vehicle 10 by obtaining the movement amount (ΔL) of the camera 12 during the time Δt. Furthermore, since the change amount of the distance and the posture angle can be calculated at the same time, the posture change amount calculation unit 24 considers the change amount of the distance and the posture angle of the vehicle 10 and has six degrees of freedom (front and rear, left and right, The amount of movement (ΔL) of up and down, yaw, pitch, and roll) can be calculated with high accuracy. That is, even if the distance and the posture angle change due to roll motion or pitch motion caused by turning or acceleration / deceleration of the vehicle 10, an estimation error of the movement amount (ΔL) can be suppressed.

  In the embodiment, the movement amount (ΔL) of the camera 12 is calculated by calculating the change amount of the distance and the posture angle and updating the distance and the posture angle. However, only the attitude angle of the camera 12 with respect to the road surface 31 may be the target of the change amount calculation and update. In this case, the distance between the road surface 31 and the camera 12 may be assumed to be constant. Accordingly, it is possible to reduce the calculation load of the ECU 13 and improve the calculation speed while suppressing the estimation error of the movement amount (ΔL) in consideration of the change amount of the posture angle.

  The vehicle speed detection unit 28 detects the vehicle speed of the vehicle 10 calculated by the vehicle speed sensor 14 and determines whether or not the vehicle speed is equal to or less than a predetermined threshold value Vth [m / s]. The determination result is output to the pattern light control unit 27 and the self-position calculation unit 26.

[Self-position calculation processing procedure]
Next, as an example of a self-position calculation method for estimating the amount of movement of the vehicle 10 from an image 38 (see FIG. 5) acquired by the camera 12, an information processing cycle repeatedly executed by the ECU 13 is shown in the flowchart in FIG. This will be described with reference to the characteristic curve shown in FIG.

  FIG. 8 is a characteristic diagram showing a change in luminance of the pattern light 32 a projected from the projector 11. The information processing cycle shown in FIG. 7 is repeatedly executed until the ignition switch of the vehicle 10 is turned on, the self-position calculating device 100 is started, and the self-position calculating device 100 is stopped.

  First, in step S11 of FIG. 7, the vehicle speed detection unit 28 detects the current vehicle speed of the vehicle 10 calculated by the vehicle speed sensor 14, and whether or not the vehicle speed is equal to or lower than a predetermined threshold value Vth [m / s]. Judging. From the principle of synchronous detection, if the vehicle speed is high and the amount of movement during one cycle increases and the road surface condition changes, sunlight and artificial light other than the projection pattern change, and the assumption of synchronous detection is There is a possibility of collapse. In such a case, it is difficult to stably extract the pattern light 32a, and errors in the distance to the road surface and the posture angle calculated by the posture angle calculation unit 22 become large or cannot be calculated, thereby increasing the movement amount of the vehicle. It cannot be detected accurately. Therefore, only when the vehicle speed is equal to or less than the predetermined threshold Vth, pattern light is projected and pattern light is extracted by synchronous detection to calculate the distance and attitude angle with respect to the road surface, as described in the following steps. I have to.

  For example, the threshold value Vth may be set to 5.0 [m / s]. This is to keep the premise of synchronous detection as described above, and specifically, to suppress the movement amount in one cycle to 0.1 m or less. However, when a camera having a higher frame rate is used, the threshold value Vth can be increased. If the threshold value Vth is set to a lower vehicle speed, an inexpensive camera having a lower frame rate can be used, and the cost of the system can be reduced.

  If the vehicle speed is equal to or lower than the threshold value Vth (YES in step S11), in step S12, the pattern light control unit 27 controls the projector 11 to project pattern light onto the road surface 31 around the vehicle. At this time, the pattern light control unit 27 controls the light projection power so that the brightness of the pattern light changes into a sine wave having a predetermined period, as shown in FIG. For example, the frequency of the sine wave is 50 [Hz]. As a result, pattern light whose luminance changes in a sine wave shape with the passage of time is projected onto the road surface 31. Here, in the present embodiment, the maximum luminance of the pattern light 32a (the peak at which the luminance waveform in FIG. 8 becomes maximum) so that the pattern light 32a can be detected even under the clear sky close to the summer solstice (June) with the largest amount of sunlight. Is set.

  In step S <b> 13, the camera 12 captures the road surface 31 including the area where the pattern light is projected and acquires an image. For example, an image is captured at 600 [fps], that is, at an imaging speed of 600 images per second.

  In step S14, the pattern light extraction unit 21 extracts the pattern light 32a by the above-described synchronous detection process. Then, the coordinates (Uj, Vj) on the image of the spot light Sp are calculated as data indicating the position of the pattern light 32a and stored in the memory. Further, the posture angle calculation unit 22 reads data indicating the position of the pattern light 32a from the memory, calculates the distance and the posture angle from the position of the pattern light 32a, and stores them in the memory. The method for calculating the distance and the posture angle is as described above. Hereinafter, the process of obtaining the distance and the posture angle using the pattern light and adopting the distance and the posture angle as the distance and the posture angle of the camera 12 is expressed as “resetting the distance and the posture angle”. Thereafter, the process proceeds to step S16.

  On the other hand, when the vehicle speed is greater than the threshold value Vth (NO in step S11), in step S20, the camera 12 captures an image by capturing the road surface 31 on which pattern light is not projected. For example, the road surface 31 is imaged at 600 [fps], that is, at an imaging speed of 600 images per second.

  In step S <b> 21, the feature point detection unit 23 detects a feature point existing on the road surface 31. Further, the posture change amount calculation unit 24 calculates the change amount of the feature point with respect to the camera 12 based on the position of the feature point detected by the feature point detection unit 23 on the image.

  Here, the feature point detection unit 23 detects a feature point (for example, an uneven portion present on the asphalt) from the image 38, and a feature point that can take a correspondence between the previous information processing cycle and the current information processing cycle. To extract. Then, the posture change amount calculation unit 24 updates the distance and posture angle from the position (Ui, Vi) of the feature point on the image. Specifically, the feature point detection unit 23 reads an image 38 acquired by the camera 12 from the memory, detects a feature point on the road surface 31 from the image 38, and determines the position (Ui, Vi) of each feature point on the image. ) In the memory. The posture change amount calculation unit 24 reads the position (Ui, Vi) of each feature point on the image from the memory, and determines the position relative to the camera 12 from the distance and posture angle and the position (Ui, Vi) of the feature point on the image. The relative position (Xi, Yi, Zi) of the feature point is calculated. The posture change amount calculation unit 24 stores the relative position (Xi, Yi, Zi) of the feature point with respect to the camera 12 in the memory.

  The posture change amount calculation unit 24 stores the position (Ui, Vi) of the feature point on the image and the relative position (Xi, Yi, Zi) of the feature point calculated in step S21 of the previous information processing cycle. Read from. The posture change amount calculation unit 24 uses the relative position (Xi, Yi, Zi) of the feature point and the position (Ui, Vi) on the image that can be correlated between the previous information processing cycle and the current information processing cycle. Thus, the change amount of the distance and the posture angle is calculated. The posture change amount calculation unit 24 updates the distance and posture angle by adding the above-described change amounts of the distance and posture angle to the distance and posture angle obtained in the previous information processing cycle. Then, the updated distance and posture angle are stored in the memory. That is, the distance and posture angle are integrated by calculating the distance and posture angle change amount of the current information processing cycle with respect to the distance and posture angle set in the process of step S16 (described later) of the previous cycle. Execute the update process. Thereafter, the process proceeds to step S16.

  In step S16, the ECU 13 sets the starting point of the integral calculation. In this process, initially, the distance and the posture angle calculated from the pattern light 32a are selected and set. Further, in step S11, when the current vehicle speed of the vehicle 10 is equal to or less than the threshold value Vth [m / s] (YES in step S11), the distance and posture angle for calculating the movement amount starting point by the pattern light, that is, step The distance and posture angle calculated in S14 are reset. On the other hand, when the current vehicle speed of the vehicle 10 is larger than the threshold value Vth [m / s] in step S11 (NO in step S11), the distance and the posture angle are updated based on the position of the feature point.

  Next, in step S <b> 17, the self-position calculation unit 26 calculates the distance and posture angle of the vehicle 10 and the self-position of the vehicle 10. That is, the movement of the camera 12 with respect to the road surface 31 based on the distance and posture angle obtained in the process of step S14 or S21, the starting point of the integration calculation, and the amount of change in the position (Ui, Vi) of the feature point on the image. The amount (ΔL), that is, the amount of movement of the vehicle 10 is calculated.

  Thus, in the self-position calculation device according to the present embodiment, the position of the vehicle 10 can be calculated by repeatedly executing the above-described series of information processing cycles and integrating the movement amount of the vehicle 10.

  In this way, in the self-position calculation apparatus 100 according to the present embodiment, the projector 11 projects the pattern light 32a whose luminance changes sinusoidally at a predetermined modulation frequency onto the road surface, and displays an image including the pattern light 32a. An image is taken by the camera 12. The pattern light 32a is extracted by subjecting this image to synchronous detection processing. Therefore, the pattern light 32a projected onto the road surface can be detected with high accuracy. Thereafter, the distance and posture angle of the camera 12 are calculated based on the position of the pattern light 32a.

  However, in scenes with high illuminance on the road surface, especially in sunny weather under clear weather, it is difficult to extract pattern light from the captured image, and it is difficult to calculate the distance and posture angle with respect to the road surface from the pattern light. Even in such a case, the current position of the vehicle can be obtained by calculating the amount of attitude change from a plurality of feature points on the road surface, and adding the amount of attitude change to the initial position and attitude angle of the vehicle serving as the base point. If integration is continued, integration errors accumulate and errors become large.

  Therefore, in the self-position calculation apparatus 100 according to the present embodiment, the maximum luminance (shown in FIG. 8) of the pattern light 32a is detected so that the pattern light 32a can be detected even under the clear sky near the summer solstice (June) with the largest amount of sunlight. The peak at which the luminance waveform is maximized is set. Further, the frequency modulation frequency of the pattern light is set to 50 [Hz], and the frame rate of the camera 12 (number of images taken per second) is set to 600 [fps].

  Furthermore, in the self-position calculation device 100 according to the present embodiment, the brightness of the pattern light is periodically changed when the vehicle speed is equal to or lower than a predetermined threshold, the pattern light is extracted using the synchronous detection technique, and the vehicle as a base point is extracted. Set the initial position and attitude angle. When the vehicle speed is larger than the threshold value with respect to this base point, the amount of change in the attitude of the vehicle calculated based on the time change of each feature point is added. Thereby, even in the case of daytime under fine weather when the illuminance on the road surface becomes high, the influence of the integration error can be reduced and the amount of movement of the vehicle can be calculated stably and with high accuracy.

  Further, in the present embodiment, an example in which pattern light 32a is extracted by irradiating pattern light whose luminance is modulated at a predetermined modulation frequency and further performing synchronous detection processing on the captured image has been described. However, an image obtained by integrating the synchronized images a predetermined number of times may be generated, and the pattern light 32a may be extracted from the accumulated image. That is, in the synchronous image obtained by the one-cycle synchronous detection processing, the portion where the pattern light 32a is projected is not sufficiently emphasized and contains a lot of noise, and the binarization processing can be performed with high accuracy. It may not be possible. Therefore, by integrating the synchronous images acquired in two or more cycles and creating a synchronous detection image in which the portion where the pattern light 32a is projected is further emphasized, the influence of noise is further eliminated and the projection on the image is performed. A light pattern can be detected.

  In this case, however, the vehicle speed threshold value Vth needs to be set to a smaller value. Specifically, in the case of two cycles, the amount of movement during extraction of pattern light by synchronous detection is 0.1 [m], such as 5.0 ÷ 2 = 2.5 [m / s]. Must be set not to exceed.

  The self-position calculation device and self-position calculation method of the present invention have been described based on the illustrated embodiment. However, the present invention is not limited to this, and the configuration of each unit is an arbitrary function having the same function. It can be replaced with that of the configuration.

10 vehicle 11 projector 12 camera (imaging part)
13 ECU
DESCRIPTION OF SYMBOLS 14 Vehicle speed sensor 21 Pattern light extraction part 22 Posture angle calculation part 23 Feature point detection part 24 Posture change amount calculation part 26 Self position calculation part 27 Pattern light control part 28 Vehicle speed detection part 32a, 32b Pattern light 100 Self position calculation apparatus

Claims (2)

A projector that projects pattern light onto the road surface around the vehicle;
A pattern light control unit for controlling the brightness of the pattern light to change periodically;
An imaging unit that is mounted on the vehicle and captures an image of a road surface around the vehicle including an area where the pattern light is projected; and
A pattern light extraction unit that extracts pattern light that periodically changes from an image acquired by the imaging unit;
An attitude angle calculation unit that calculates an attitude angle of the vehicle with respect to the road surface from the position of the pattern light extracted by the pattern light extraction unit;
A posture change amount calculating unit that detects a plurality of feature points on the road surface from the image acquired by the imaging unit, and calculates a posture change amount of the vehicle based on a temporal change of each feature point;
A self-position calculator that calculates the current position and posture angle of the vehicle by adding the amount of posture change to the initial position and posture angle of the vehicle;
A vehicle speed detector for detecting the vehicle speed of the vehicle,
When the vehicle speed is equal to or lower than a predetermined threshold, the self-position calculation unit sets the vehicle current position and the posture angle of the vehicle by the posture angle calculation unit at that time to the initial position and posture angle of the vehicle, A self-position calculation apparatus, which starts addition of a posture change amount. A light projecting procedure for projecting pattern light from a projector mounted on the vehicle onto a road surface around the vehicle;
A pattern light control procedure in which the control unit of the vehicle controls the brightness of the pattern light to change periodically;
An imaging procedure for acquiring an image by imaging a road surface around the vehicle including an area where the pattern light is projected by an imaging unit mounted on the vehicle;
A pattern light extraction procedure in which the control unit extracts pattern light that periodically changes from an image acquired in the imaging procedure;
An attitude angle calculation procedure for calculating an attitude angle of the vehicle with respect to the road surface from the position of the pattern light extracted by the pattern light extraction procedure;
A posture change amount calculating procedure in which the control unit detects a plurality of feature points on the road surface from the image acquired by the imaging procedure, and calculates a posture change amount of the vehicle based on a time change of each feature point; ,
A self-position calculation procedure in which the control unit calculates the current position and posture angle of the vehicle by adding the amount of posture change to the initial position and posture angle of the vehicle;
The control unit includes a vehicle speed detection procedure for detecting a vehicle speed of the vehicle,
In the self-position calculation procedure, when the vehicle speed is equal to or less than a predetermined threshold, the current position of the vehicle at that time and the posture angle of the vehicle according to the posture angle calculation procedure are set to the initial position and the posture angle of the vehicle, A self-position calculation method characterized by starting to add posture change amounts.