JP6299319B2 - 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.

  Conventionally, a technique has been proposed in which a camera is mounted on a vehicle, an image of the vicinity of the vehicle is captured by the camera, and the amount of movement of the vehicle is obtained based on a change in the captured image (see Patent Document 1). In Patent Document 1, in order to obtain the amount of movement with high accuracy even when the vehicle is moving slowly and delicately, a feature point included in the image is detected, the position of the feature point on the image is obtained, and the feature point is moved. The moving amount of the vehicle is obtained from the direction and the moving distance (moving amount).

JP 2008-175717 A

  However, since the camera mounted on the vehicle changes the positional relationship with the road surface, that is, the distance to the road surface and the attitude angle with respect to the road surface, as the vehicle travels, the vehicle movement amount can be obtained with high accuracy. I can't.

  The present invention has been made to solve such a conventional problem, and an object of the present invention is to provide a self-position calculating device and a self-computing device capable of calculating the amount of movement of the vehicle accurately and stably. It is to provide a position calculation method.

In order to achieve the above object, the present invention provides a pattern light control unit for controlling the brightness of pattern light projected from a projector to periodically change to a threshold brightness or lower, and pattern light from an image captured by the imaging unit. And a posture angle calculation unit for calculating the posture angle of the vehicle with respect to the road surface from the position of the pattern light extracted by the pattern light extraction unit. Also, a posture change amount calculation unit that detects a plurality of feature points on the road surface from an image picked up by the image pickup unit, and calculates a posture change amount of the vehicle based on a temporal change of each feature point, and an initial vehicle It has a self-position calculator that calculates the current position and posture angle of the vehicle by adding the posture change amount to the position and posture angle. Then, the posture change amount calculation unit detects a feature point when the luminance of the pattern light is equal to or lower than the threshold luminance.

  In the present invention, since the brightness of the pattern light is periodically changed and the feature point is detected when the brightness of the pattern light is equal to or less than the threshold brightness, the detection accuracy of the feature point can be improved, and 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 1st, 2nd embodiment of this invention. It is explanatory drawing which shows the light projection pattern irradiated with the light projector mounted in a 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 apparatus which concerns on 1st Embodiment of this invention. It is a timing chart which shows the change of a brightness | luminance and the change of a feature point detection flag of the self-position calculation apparatus which concerns on 1st Embodiment of this invention. It is explanatory drawing which shows the pattern light and the feature point of the self-position calculation apparatus which concerns on 1st Embodiment of this invention. It is a flowchart which shows the process sequence of the self-position calculation apparatus which concerns on 2nd Embodiment of this invention. It is a timing chart which shows the change of a brightness | luminance and the change of a feature point detection flag of the self-position calculation apparatus which concerns on 2nd Embodiment of this invention. It is a block diagram which shows the structure of the self-position calculation apparatus which concerns on 3rd Embodiment of this invention. It is a flowchart which shows the process sequence of the self-position calculation apparatus which concerns on 3rd Embodiment of this invention. It is a timing chart which shows the change of a brightness | luminance and the change of a feature point detection flag of the self-position calculation apparatus which concerns on 3rd Embodiment of this invention. It is a flowchart which shows the process sequence of the self-position calculation apparatus which concerns on the 1st modification of 3rd Embodiment of this invention. It is a timing chart which shows the change of a brightness | luminance and the change of a feature point detection flag of the self-position calculation apparatus concerning the 1st modification of 3rd Embodiment of this invention. It is a block diagram which shows the structure of the self-position calculation apparatus which concerns on the 2nd modification of 3rd Embodiment of this invention.

  Embodiments of the present invention will be described below with reference to the drawings.

[Description of First Embodiment]
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 calculation device 100 includes a projector 11, a camera 12 (imaging unit), and an engine control unit (ECU) 13. 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 controls the operation of the projector 11 and executes a series of information processing cycles for estimating the movement amount 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 of changing the luminance of the pattern light 32b is 200 [Hz], 2400 times of imaging are performed 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 by 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 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. 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, the luminance is controlled so as to change in a sine wave shape as shown in FIG. In the second embodiment to be described later, as shown in FIG. 11A, the brightness of the light projection pattern is controlled by switching on and off at a predetermined switching frequency.

  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. In the second embodiment to be described later, the difference between the image captured when the projection pattern is projected (when on) and the image captured when the projection pattern is not projected (when off). To extract the pattern light 32a. Then, the position of the pattern light 32a is extracted from the acquired pattern light 32a. As shown in FIG. 3A, for example, the pattern light 32a composed of a plurality of spot lights arranged in a matrix in the projector 11 is projected toward the road surface 31, and the pattern light 32a reflected by the road surface 31 is synchronized. Detection is performed by 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. Then, the attitude angle calculation unit 22 can extract only the light projection pattern by multiplying the image (measurement signal) captured by the camera 12 by the modulation frequency ω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. In the second embodiment to be described later, feature points are detected in a time zone in which pattern light projection is off.

  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 the memory, and 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, and 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 simultaneous equations consisting of the following equations (1) to (4), 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. 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 does not use all of the feature points whose relative positions are calculated in the images detected at time t and time t + Δt, but is optimal based on the positional relationship between the feature points. A feature point may be selected. 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. When detected, the posture change amount calculation unit 24 calculates the relative position (xi, yi, zi) of the plurality of feature points on the road surface and the time change of the position (Ui, Vi) of the feature points on the image. The “vehicle attitude change amount” can be calculated. Furthermore, the amount of movement of the vehicle 10 can be calculated.

  That is, if it is possible to continue to detect 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 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 value. The initial distance and posture angle may be used. 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.

[Operation of First Embodiment]
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. 8 and the explanatory diagram shown in FIG.

  FIG. 8A is a characteristic diagram showing a change in luminance of the pattern light 32a projected from the projector 11, and FIG. 8B is a characteristic diagram showing a change in the feature point detection flag. Moreover, FIG. 9 is explanatory drawing which shows the pattern light projected on the road surface, FIG. 9 (a), (c) is the case where the brightness | luminance of the pattern light 32a is high, FIG.9 (c) shows the pattern light 32a. The state when the luminance is low is shown. 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 S <b> 11 of FIG. 7, 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, as shown in FIG. 8A, the pattern light control unit 27 controls the light projection power so that the luminance B1 of the pattern light changes in a sine wave shape with a predetermined period. For example, the frequency of the sine wave is 200 [Hz]. As a result, the pattern light whose luminance B1 changes in a sine wave shape with the passage of time is projected onto the road surface 31.

  In step S12, the camera 12 captures an image of the road surface 31 including the area where the pattern light is projected. In step S13, the feature point detector 23 determines whether or not the luminance of the pattern light 32a projected from the projector 11 is equal to or lower than a preset threshold luminance Bth.

  If it is larger than the threshold luminance Bth (NO in step S13), in step S14, the pattern light extraction unit 21 extracts the pattern light 32a by the above-described synchronous detection process. For example, as shown in FIGS. 9A and 9C, pattern light 32a is obtained by synchronous detection processing from an image taken at time t1 or time t3 (time when luminance B1 is larger than threshold luminance Bth) shown in FIG. To extract. 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. In the following description, a process for obtaining the distance and posture angle of the vehicle 10 with respect to the road surface 31 by using the pattern light by the posture angle calculation unit 22 and resetting the distance and the posture angle as a starting point of the integration calculation is referred to as “distance and posture”. "Reset the posture angle". Thereafter, the process proceeds to step S16.

  On the other hand, if it is equal to or lower than the threshold luminance Bth (YES in step S13), the feature point detector 23 detects a feature point existing on the road surface 31 in step S15. Specifically, when the luminance B1 of the pattern light 32a changes in a sine wave shape as shown in FIG. 8A, the feature point is detected in a time zone in which the luminance B1 is equal to or lower than the threshold luminance Bth. That is, as shown in FIG. 8B, the feature point detection flag is set to “1” in a time zone where the brightness B1 is equal to or less than the threshold brightness Bth, and the feature point is detected in this time zone. For example, as shown in FIG. 9B, feature points are detected from an image taken at time t2 (time when luminance B1 is equal to or lower than threshold luminance Bth) shown in FIG. 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. At this time, the region for detecting the feature points is a region that is entirely or partially overlapped with the region that projects the pattern light 32a.

  In step S15, the feature point detection unit 23 detects feature points (for example, uneven portions present on the asphalt) from the image 38, and there is a correspondence between the previous information processing cycle and the current information processing cycle. The feature points that can be taken are extracted, and the distance and posture angle are updated from the positions (Ui, Vi) of the feature points on the image.

  Specifically, when the feature point detection flag is “1”, the feature point detection unit 23 reads the image 38 acquired by the camera 12 from the memory, detects the feature point on the road surface 31 from the image 38, and The position (Ui, Vi) of the feature point on the image is stored 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 of the feature point on the image (Ui, Vi) and the relative position (Xi, Yi, Zi) of the feature point calculated in step S15 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 current 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 calculated by integrating the distance and posture angle change amount for each information processing cycle with respect to the distance and posture angle set in step S16 (described later) of the previous cycle. Execute the process to update. Thereafter, the process proceeds to step S16.

  In step S16, the ECU 13 selects the starting point of the integral calculation. In this process, in the first information processing cycle, the distance and posture angle calculated from the pattern light 32a are selected and set. Further, when a preset condition is satisfied, for example, when the feature point detection state in the feature point detection unit 23 is lowered, and a plurality of feature points cannot be detected at the timing when the feature point flag is “1”. The distance and posture angle calculated from the pattern light, that is, the distance and posture angle calculated in step S14 are reset. On the other hand, when the feature point is normally detected by the feature point detection unit 23, the distance and the posture angle are updated based on the position of the feature point.

  In other words, if the feature point is not normally detected by the feature point detection unit 23, the distance and posture angle of the camera 12 cannot be set with high accuracy, and the low-accuracy distance and posture angle are adopted to the vehicle. If the amount of movement is calculated, the amount of movement of the vehicle cannot be detected with high accuracy. Accordingly, in such a case, the starting point of the movement amount calculation is reset to the distance and posture angle obtained from the position of the pattern light. By doing so, it is possible to prevent a large error in the distance and the posture angle.

  Next, in step S17, the self-position calculation unit 26 changes the distance and posture angle obtained in the process of step S14 or S15, the starting point of the integration calculation, and the position (Ui, Vi) of the feature point on the image. The amount of movement of the camera 12 relative to the road surface 31 (ΔL), that is, the amount of movement of the vehicle 10 is calculated from the amount.

  Thus, in the self-position calculation device according to the first 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.

  Thus, in the self-position calculation apparatus 100 according to the first embodiment, the pattern light 32a whose luminance changes sinusoidally at a predetermined modulation frequency (for example, 200 Hz) is projected from the projector 11 onto the road surface. An image including the light 32 a is captured 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. At this time, the calculation of the distance and the attitude angle may be performed every cycle set by the predetermined modulation cycle described above, or may be performed every plural cycles (for example, every 10 cycles). Therefore, the distance and the posture angle based on the pattern light 32a are calculated every predetermined period or every plural periods.

  In addition, feature points existing on the road surface are detected in a time zone in which the luminance of the pattern light 32a is equal to or lower than the threshold luminance Bth. At this time, since the brightness of the pattern light 32a is low, it can be prevented from being influenced by the pattern light 32a when detecting the feature points, and the feature points can be detected with high accuracy. Based on the detected feature points, the amount of movement of the vehicle can be calculated by adopting the method described above. As a result, the amount of movement of the vehicle can be calculated with high accuracy.

  Here, the setting of the luminance of the pattern light 32a will be described. In the present embodiment, the maximum brightness of the pattern light 32a (above the brightness B1 shown in FIG. 8A) is detected 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. Peak). Further, at night when the influence of the pattern light is greatest, the minimum brightness of the pattern light (below the brightness B1 shown in FIG. Set the peak.

  Further, the frequency of luminance modulation of the pattern light is, for example, 200 [Hz], and the frame rate of the camera 12 (number of images taken per second) is 2400 [fps]. This is because in this embodiment, when the maximum speed of the vehicle is assumed to be 72 km / h (≈20 m / s), the movement amount in one cycle is suppressed to 0.1 m or less. This is because, based on the principle of this embodiment, it is desirable that the area where the pattern light 32a is projected and the area where the feature points are detected be as close as possible or the same. In addition, from the principle of synchronous detection, if the amount of movement during one cycle increases and the road surface condition changes, sunlight and artificial light other than the light projection pattern change, and the assumption of synchronous detection may be lost. This is avoided by minimizing the amount of movement in one cycle. Therefore, further improvement in performance can be expected by using a faster camera and further shortening this fixed period.

  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 performing synchronous detection processing on the captured image has been described. An image obtained by integrating the number of times may be generated, and the pattern light 32a may be extracted from the integrated 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.

  Further, the region for detecting the feature points overlaps with the region for projecting the pattern light 32a. Therefore, the imaging range by the camera 12 can be narrowed, and the calculation load required for image processing can be reduced.

[Description of Second Embodiment]
Next, a second embodiment of the self-position calculation apparatus 100 according to the present invention will be described. The apparatus configuration is the same as that of FIG. In the first embodiment described above, an example in which the pattern light 32a is detected by changing the luminance of the pattern light projected from the projector 11 in a sine wave shape and performing synchronous detection processing on the image captured by the camera 12 has been described. . On the other hand, in the second embodiment, the projection of the pattern light 32a is controlled to be turned on and off at a predetermined switching period, and the image captured when the pattern light 32a is projected (on) and the pattern light 32a The pattern light 32a is obtained by calculating the difference between the images captured when the light is not projected (off).

  That is, as shown in FIG. 11A, the pattern light control unit 27 switches on / off of the pattern light 32a at a predetermined switching cycle. Then, as shown in FIG. 11B, the feature point detection unit 23 detects the feature point by setting the feature point detection flag to “1” when the pattern light 32a is off.

  Hereinafter, the processing procedure of the self-position calculation apparatus 100 according to the second embodiment will be described with reference to the flowchart shown in FIG.

  First, in step S31 of FIG. 10, 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, as shown in FIG. 11A, the pattern light control unit 27 controls the projection power so that the pattern light is switched on and off at a predetermined switching cycle.

  In step S <b> 32, the camera 12 captures an image by capturing the road surface 31 including the region where the pattern light is projected, and stores the image in the memory. In step S33, the feature point detector 23 determines whether or not the pattern light 32a projected from the projector 11 is off.

  When the pattern light 32a is on (NO in step S33), in step S34, the pattern light extraction unit 21 stores the pattern light on-image stored in the memory and the pattern light previously captured. A difference image from the off-time image is generated, and pattern light is extracted from the difference image. That is, the pattern light 32a can be extracted by calculating the difference between the image captured when the pattern light 32a is projected and the image captured when the pattern light 32a is not projected. At this time, the vehicle 10 is moving, and the relative position between the vehicle 10 and the road surface changes. However, since the frame rate of the camera 12 is large, the moving distance of the vehicle 10 can be ignored.

  As in the first embodiment described above, 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. Thereafter, the process proceeds to step S36.

  On the other hand, when the pattern light is off (the brightness of the pattern light is equal to or lower than the threshold brightness) (YES in step S33), in step S35, the feature point detection unit 23 detects the feature points existing on the road surface 31. To detect. That is, the feature point is detected using the image captured by the camera 12 in the time zone in which the feature point detection flag shown in FIG. 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. The area for detecting the feature points is the same area as the area where the pattern light 32a is projected. The method of calculating the change amount of the distance and posture angle of the camera 12 from the position of the feature point is the same as in the first embodiment described above. Thereafter, the process proceeds to steps S36 and S37. The processing in steps S36 and S37 is the same as the processing in steps S16 and S17 shown in FIG.

  Thus, the self-position calculation device 100 according to the second embodiment can calculate the position of the vehicle 10 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 device 100 according to the second embodiment, the projector 11 projects the pattern light 32a that is switched on and off at a predetermined switching cycle onto the road surface, and the image including the pattern light 32a is projected. An image is taken by the camera 12. Then, the pattern light 32a is extracted by calculating the difference between the image when the pattern light 32a is turned on and the image when the pattern light 32a is turned off. Therefore, the pattern light 32a projected onto the road surface can be detected with high accuracy. Since the distance and posture angle of the camera 12 are calculated based on the position of the pattern light 32a, the distance and posture angle can be calculated with high accuracy.

  Also, feature points present on the road surface are detected during the time zone when the pattern light 32a is off. At this time, since the pattern light 32a is not projected, it is possible to detect feature points with high accuracy without being affected by the pattern light 32a in detecting feature points. Then, by adopting the method described above, the amount of movement of the vehicle can be calculated. As a result, the amount of movement of the vehicle can be calculated with high accuracy.

  In the self-position calculation apparatus 100 according to the second embodiment, as in the first embodiment described above, the switching frequency when switching on and off the pattern light is set to 200 [Hz], for example, and the camera 12 It is desirable to set the frame rate to 2400 [fps].

[Description of Third Embodiment]
Next, a third embodiment of the present invention will be described. FIG. 12 is a block diagram illustrating a configuration of a self-position calculation apparatus 100a according to the third embodiment. The third embodiment is different from the self-position calculation device 100 shown in FIG. 1 in that a feature point moving speed calculation unit 37 is provided.

  The feature point movement speed calculation unit 37 calculates the movement speed of the feature point based on the change in the position coordinates of the feature point detected by the feature point detection unit 23. Since the period for acquiring the feature points is known, the movement speed of the feature points can be calculated by obtaining the movement amount of the feature points. The moving speed data of the feature point is output to the posture change amount calculation unit 24. Then, the posture change amount calculation unit 24 changes the posture change amount calculation cycle based on the magnitude of the moving speed data of the feature points. As an example, a threshold speed is set in advance, and when the feature point movement speed acquired from the feature point movement speed calculation unit 37 is higher than the threshold speed, the posture change amount calculation cycle is set to be short. To do.

  Since the configuration other than the feature point moving speed calculation unit 37 in FIG. 12 is the same as that in FIG. 1 described above, the same reference numerals are given and description of the configuration is omitted.

  Next, the operation of the self-position calculating apparatus 100a according to the third embodiment will be described with reference to the flowchart shown in FIG. 13 and the timing chart shown in FIG. The flowchart shown in FIG. 13 is different from the flowchart of FIG. 7 described above in that steps S131, S132, and S133 are added. The other steps are the same as in FIG. 7 and are therefore given the same step numbers. After steps S11 and S12, if it is determined in step S13 that the luminance of the pattern light 32a is equal to or lower than the threshold luminance Bth, the posture change amount calculation unit 24 calculates the feature point moving speed in step S131. It is determined whether the feature point moving speed acquired by the unit 37 is greater than a preset threshold speed.

  If the speed is equal to or lower than the threshold speed (NO in step S131), in step S132, the posture change amount calculation unit 24 calculates the posture change amount of the vehicle 10 in the first calculation cycle. For example, the posture change amount is calculated at a period τ2 shown in FIG. On the other hand, if it is greater than the threshold speed (YES in step S131), in step S133, the posture change amount calculation unit 24 calculates the posture change amount of the vehicle 10 in a second calculation cycle that is shorter than the first calculation cycle. To do. For example, the posture change amount is calculated at a period τ1 (τ1 <τ2) shown in FIG. Then, the process of step S15-S17 is performed. FIG. 14 shows an example in which twice τ1 is τ2 as an example, but the present invention is not limited to this. Further, as an example, an example in which the feature point detection cycle is switched according to the threshold speed is shown, but the feature point detection cycle may be sequentially changed according to the feature point moving speed.

  In order to track the feature points existing on the road surface, in principle, the amount of movement of the vehicle between the frames before and after the feature points are detected needs to be within the imaging range. Further, the movement amount should be as small as possible in order to prevent an error in the association of the feature points in the front and rear frames. However, if the time between the previous and next frames is too short, the feature points on the road surface hardly change, and it becomes difficult to calculate the movement amount. That is, when the movement amount is calculated by tracking the feature point, the feature point needs to move to some extent between the previous and next frames.

  In the third embodiment, for example, the feature point detection cycle is set so that the feature points existing on the road surface are in the range of 0.05 to 0.1 [m] in one cycle. Therefore, when the maximum frequency when projecting the pattern light 32a is 200 [Hz], the moving speed of the feature point [20 m / s] (≈vehicle speed 72 [Km / h]) in this embodiment is When the control is performed at the maximum frequency of 200 [Hz], the target range is 0.1 m. On the other hand, when the moving speed of the feature point is 1 [m / s] (≈vehicle speed 3.6 [Km / h]), the frequency is set to 0.05 [m] within the target range by setting the frequency to 20 [Hz]. Can do. According to this calculation method, detection of road surface texture is stopped while the vehicle is stopped, so the frequency is set to 5 [Hz] at the minimum.

  As described above, the self-position calculation apparatus 100a according to the third embodiment can achieve the same effects as those of the first embodiment described above. Furthermore, since the period for calculating the posture change amount is changed according to the moving speed of the feature point, the posture changing amount suitable for the moving speed of the feature point can be calculated, and the detection of the moving amount of the vehicle 10 can be further enhanced. Can be done with precision.

[Description of First Modification of Third Embodiment]
Next, a first modification of the third embodiment will be described. The apparatus configuration is the same as that shown in FIG. Hereinafter, the operation of the self-position calculating apparatus 100a according to the first modification will be described with reference to the flowchart shown in FIG. 15 and the timing chart shown in FIG. In the flowchart shown in FIG. 5, steps S301 to S303 are added to the flowchart shown in FIG. 10 described above, and other configurations are the same as those in FIG. doing.

  First, in step S301, the feature point moving speed calculation unit 37 calculates the moving speed of the feature point and determines whether or not the moving speed of the feature point is higher than a preset threshold speed. If it is equal to or less than the threshold speed (NO in step S301), the process proceeds to step S302. If it is greater than the threshold speed (YES in step S301), the process proceeds to step S303.

  In step S302, the posture change amount calculation unit 24 sets the on / off cycle of the pattern light to the first switching cycle. Specifically, as shown in FIG. 16A, the cycle is set so that the pattern light is projected for the time τ11, and the pattern light is projected again after the time τ12 has elapsed. Therefore, as shown in FIG. 16B, feature points are detected during time τ12.

  In step S303, the posture change amount calculation unit 24 sets the on / off cycle of the pattern light to the second switching cycle. Specifically, as shown in FIG. 16C, the cycle is set so that the pattern light is projected for the time τ13 and the pattern light is projected again after the time τ14. Therefore, as shown in FIG. 16D, feature points are detected during time τ14. At this time, (τ11 + τ12) is the first switching period, and (τ13 + τ14) is the second switching period. And (τ11 + τ12)> (τ13 + τ14). That is, the second switching cycle is set to be shorter than the first switching cycle.

  Next, in step S31 of FIG. 15, the pattern light control unit 27 controls the projector 11 to project pattern light onto the road surface 31 around the vehicle at the first switching period or the second switching period. Since the process after step S32 is the same as FIG. 10 mentioned above, description is abbreviate | omitted.

  As described above, in the self-position calculation apparatus 100a according to the first modification of the third embodiment, the period for calculating the posture change amount is changed according to the moving speed of the feature point, as in the third embodiment described above. Thus, it is possible to calculate the amount of change in posture suitable for the moving speed of the feature point, and the movement amount of the vehicle 10 can be detected with higher accuracy.

[Description of Second Modification of Third Embodiment]
Next, a second modification of the third embodiment will be described. In the third embodiment described above, the example in which the feature point moving speed calculating unit 37 calculates the moving speed of the feature point based on the moving amount of the feature point detected by the feature point detecting unit 23 has been described. In the second modification, instead of the moving speed of the feature point, the vehicle speed is acquired from the detection signal of the vehicle speed sensor mounted on the vehicle, and the period for calculating the attitude change amount is changed according to the vehicle speed. That is, as shown in FIG. 17, a vehicle speed acquisition unit 36 that acquires vehicle speed data output from the vehicle speed sensor is provided, and the vehicle speed data acquired by the vehicle speed acquisition unit 36 is output to the attitude angle calculation unit 24 to change the attitude change. You may make it change the period which calculates quantity. Even in such a configuration, it is possible to achieve the same effect as that of the third embodiment described above.

  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 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 32a, 32b Pattern light 36 Vehicle speed acquisition part 37 Feature point movement speed calculation part 100,100a , 100b Self-position calculation device

Claims (8)

A projector that projects pattern light onto the road surface around the vehicle;
A pattern light control unit that controls the brightness of the pattern light to periodically change to a threshold brightness or less ;
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 from the 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;
With
The posture change amount calculation unit detects the feature point when the luminance of the pattern light is equal to or lower than a threshold luminance.   The pattern light control unit modulates the brightness of the pattern light at a predetermined modulation frequency, and the pattern light extraction unit synchronously detects an image captured by the imaging unit at the predetermined modulation frequency, The self-position calculating apparatus according to claim 1, wherein:   The attitude angle calculation unit calculates an attitude angle of the vehicle with respect to the road surface based on pattern light obtained by synchronous detection of images of two or more periods captured by the imaging unit. The self-position calculation device described.   The pattern light control unit controls on / off of the pattern light at a predetermined switching frequency, and the pattern light extraction unit captures an image captured when the pattern light is on and an image captured when the pattern light is off. The self-position calculating apparatus according to claim 1, wherein the pattern light is extracted by calculating a difference between images.   5. The posture change amount calculation unit obtains a moving speed of the plurality of feature points, and sets the posture change amount calculation cycle to be shorter as the moving speed is higher. The self-position calculation device according to item.   The vehicle further includes a vehicle speed acquisition unit that acquires vehicle speed data detected by a vehicle speed sensor mounted on the vehicle, and the posture change amount calculation unit sets the posture change amount calculation cycle to be shorter as the vehicle speed is higher. The self-position calculating device according to any one of claims 2 to 4.   The self-position according to any one of claims 1 to 6, wherein a region for detecting each feature point on the road surface overlaps with a region to which the pattern light is projected in whole or in part. Calculation device. Projection procedure of projecting pattern light onto the road surface around the vehicle,
A pattern light control procedure for controlling the brightness of the pattern light to periodically change below a threshold brightness ;
An imaging procedure for acquiring an image by imaging a road surface around the vehicle including an area mounted on the vehicle and projected with the pattern light;
A pattern light extraction procedure for extracting pattern light from the 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 in the pattern light extraction procedure, and detecting a plurality of feature points on the road surface from the image acquired by the imaging procedure And a posture change amount calculating procedure for calculating a posture change amount of the vehicle based on a time change of each feature point;
A self-position calculation procedure for calculating the current position and attitude angle of the vehicle by adding the attitude change amount to the initial position and attitude angle of the vehicle;
With
The self-position calculation method, wherein the feature point is detected when the brightness of the pattern light is equal to or lower than a threshold brightness.