JP6459701B2 - 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 for calculating a current position and a posture angle of a vehicle.

  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, as a technique for calculating the amount of change in vehicle attitude, the pattern light projected from the vehicle onto the road surface is imaged by the camera, and the road plane formula for the camera is calculated from the shape on the image using the principle of triangulation. There is a technique for calculating the distance and posture of the vehicle with respect to the road surface (see Patent Document 2).

JP 2008-175717 A JP 2007-278951 A

  However, in order to reduce the error between the distance and posture of the vehicle calculated from the area for detecting feature points and the area for projecting pattern light, the area for detecting feature points and the area for projecting pattern light are: It is desirable that they overlap. However, when pattern light is projected onto a region where a feature point is detected, there is a problem that the pattern light may be erroneously detected as a feature point.

  Therefore, the present invention has been made in view of the above problems, and even if a region for detecting feature points and a region for projecting pattern light are overlapped, pattern light is erroneously detected as a feature point. It is an object of the present invention to provide a self-position calculation apparatus and method that can prevent this.

  In order to solve the above-described problem, a self-position calculation apparatus and method according to an aspect of the present invention project pattern light from a projector mounted on a vehicle onto a road surface around the vehicle, and the pattern light is emitted by an imaging unit. An image is obtained by capturing an image of the road surface around the vehicle including the projected area. Then, an image in the wavelength range of the pattern light is extracted from the acquired image by the first filter, and the attitude angle of the vehicle with respect to the road surface is calculated from the position of the pattern light in the extracted image. In addition, an image that does not include the wavelength range of the pattern light is extracted from the acquired image by the second filter, and the amount of change in the posture of the vehicle is calculated based on temporal changes of a plurality of feature points on the road surface detected from the extracted image calculate. Using these results, the self-position calculation device and method according to one aspect of the present invention add the amount of change in posture to the initial position and posture angle of the vehicle, thereby obtaining the current position and posture angle of the vehicle. Is calculated.

  According to the present invention, the first and second filters extract the image of the pattern light wavelength region and the image not including the pattern light wavelength region, so that the feature point detection region and the pattern light projection region are Even if they are overlapped, it is possible to prevent erroneous detection of pattern light as a feature point. As a result, the current position of the vehicle can be estimated accurately and stably.

FIG. 1 is a block diagram showing an overall configuration of a self-position calculation apparatus according to an embodiment of the present invention. FIG. 2 is an external view showing an example of mounting a projector and a camera on a vehicle. FIG. 3 is a diagram for explaining a region where pattern light is irradiated and a region where feature points are detected. FIG. 4 is a diagram illustrating an image of pattern light obtained by performing binarization processing on an image acquired by a camera. FIG. 5 is a block diagram showing the structure of a laser pointer used in the projector. FIG. 6 is a schematic diagram for explaining a method of calculating the change amount of the distance and the posture angle. FIG. 7 is a diagram illustrating an example of frames (images) acquired at time t and time t + Δt. FIG. 8 is a timing chart for explaining the timing for setting the starting point of the integral calculation. FIG. 9 is a flowchart showing a procedure of a self-position calculation method by the self-position calculation apparatus according to an embodiment of the present invention. FIG. 10 is a flowchart showing a detailed procedure of step S11 of FIG. FIG. 11 is a diagram for explaining the effect of the self-position calculation apparatus according to an embodiment of the present invention.

  Embodiments will be described with reference to the drawings. In the description of the drawings, the same portions are denoted by the same reference numerals, and description thereof is omitted.

(First embodiment)
(Hardware configuration)
First, the hardware configuration of the self-position calculation apparatus according to the first embodiment will be described with reference to FIG. The self-position calculating device includes a projector 11, a camera 12, and an engine control unit (ECU) 13. The projector 11 is mounted on the vehicle and projects pattern light onto a road surface around the vehicle. The camera 12 is an example of an imaging unit that 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 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.

  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 camera 12 may use an RGB camera.

  The height and direction in which the camera 12 is installed are adjusted so that the feature point (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 lens focus and aperture of the lens are automatically adjusted. The camera 12 repeatedly captures images at a predetermined time interval and acquires a series of images (frames). The image data acquired by the camera 12 includes an R signal (Red: red), a G signal (Green: green), and a B signal (Blue: blue). The image data is transferred to the ECU 13 and stored in a memory included in the ECU 13. Is done.

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. 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. Pattern by diffracting the laser beam emitted from the laser pointer in the diffraction grating, the light projector 11, as shown in FIGS. 2 to 4, comprising a lattice image, or a matrix a plurality of which are arranged in the spot light S _p Light (32b, 32a) is generated. In the example shown in FIGS. 3 and 4, and generates a pattern light 32a consisting spotlight S _p of 5 × 7.

  FIG. 3A shows a feature point detected from another region 33 different from the region irradiated with the pattern light 32a in the road surface 31 of the imaging range of the camera 12, and the camera is detected from the time variation of the detected feature point. 12 schematically shows how 12 movement directions 34 are obtained. FIG. 3B schematically shows a state in which the pattern light 32a is irradiated into the region 33 where the feature points are detected. As described above, the projector 11 may match or make the region 33 for detecting the feature point coincide with the region for projecting the pattern light 32 a in the road surface 31 within the imaging range of the camera 12. However, it is possible to reduce the error due to the road surface change by matching the region 33 for detecting the feature points with the region for projecting the pattern light 32a.

  Here, with reference to FIG. 5, the structure of the laser pointer which comprises the light projector 11 is demonstrated. As shown in FIG. 5A, the red laser pointer 51 includes a control board 53 and a semiconductor laser 55. The control board 53 is an electrical protection circuit that stabilizes the output at 1 mW or less, and the semiconductor laser 55 includes a lens that outputs a red laser of 650 nm and optically corrects the output beam to parallel light. .

  Further, the projector 11 may be composed of a green laser pointer 57. The green laser pointer 57 includes the same control substrate 53 and semiconductor laser 55 as the red laser pointer 51, and further includes a nonlinear crystal (YAG) 59, a crystal (KTP) 61, and a laser output sensing unit 63. The nonlinear crystal (YAG) 59 converts the red laser into a long wavelength (1064 nm) such as a double wave or a second harmonic. The crystal (KTP) 61 converts the output wave from the nonlinear crystal (YAG) 59 into a half wavelength in two stages and outputs a laser having a green wavelength. The laser output sensing unit 63 senses the output green laser and feeds it back to the control board 53 to control the output of the green laser. Since the green laser has a wavelength in the infrared region, it generates heat, but this heat makes wavelength conversion in the two nonlinear crystals unstable, and the output of the green laser becomes unstable. Therefore, the laser output sensing unit 63 performs control to suppress output instability by feeding back the laser output.

  As described above, either a red laser pointer or a green laser pointer can be adopted as the laser pointer constituting the projector 11. In this embodiment, a case where a red laser pointer having a simple structure is adopted will be described. The green laser pointer not only has a complicated structure, but also has a short lifetime, so if the red laser pointer is used, the reliability of the system can be improved.

  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 by executing a computer program installed in advance. The ECU 13 repeatedly executes a series of information processing cycles for calculating the current position 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 first filter 15, a second filter 17, a pattern light extraction unit 21, an attitude angle calculation unit 22, an attitude change amount calculation unit 24, a self-position calculation unit 26, a pattern And a light control unit 27. The posture change amount calculation unit 24 includes a feature point detection unit 23.

  The first filter 15 extracts an image in the wavelength range of the pattern light from the image acquired by the camera 12 and stores it in the memory. In the present embodiment, since the pattern light is emitted by the red laser pointer, the first filter 15 extracts an image in the red wavelength region. For example, a filter having a transmittance of 80% or more in a red wavelength region of 600 to 650 nm is used. When an RGB camera is used, a camera having a transmittance of 80% or more in a wavelength region of 600 to 650 nm is used, and only an image of the R signal is used for pattern light extraction.

  The second filter 17 extracts an image not including the wavelength range of the pattern light from the image acquired by the camera 12 and stores it in the memory. In the present embodiment, since the pattern light is irradiated with the red laser pointer, the second filter 17 extracts an image of at least one wavelength region of blue or green. For example, a filter having a transmittance of 70% or more in a blue wavelength region of 450 to 570 nm, a filter having a transmittance of 70% or more in a green wavelength region of 520 to 560 nm, or both are used. When using an RGB camera, the B signal has a transmittance of 70% or more in the wavelength region of 450 to 570 nm, and the G signal has a transmittance of 70% or more in the wavelength region of 520 to 560 nm. Is used. Then, either the B signal or the G signal, or both the B signal and the G signal are used for feature point detection.

The pattern light extraction unit 21 reads an image extracted by the first filter 15 among the images acquired by the camera 12 from the memory, and extracts the position of the pattern light from this image. As shown in FIG. 3, for example, the projector 11 projects pattern light 32 a made up of a plurality of spot lights arranged in a matrix toward the road surface 31, and the pattern light 32 a reflected by the road surface 31 is captured by the camera 12. To detect. Pattern light extractor 21, by performing binarization processing on the image extracted by the first filter 15, as shown in FIGS. 4 (a) and 4 (b), the spot light S _p image Extract only. Pattern light extractor 21, as shown in FIG. 4 (c), the position _{H e} of the center of gravity of the spot light _{S p,} i.e. be calculated on the image of the spot light _{S p} coordinates _{(U j, V} j) Thus, the position of the pattern light 32a is extracted. Coordinates, the pixels of the image sensor of the camera 12 as a unit, in the case of 5 × 7 of the spot light S _p, "j" is a natural number of 35 or less 1 or more. Coordinates on the image of the spot light _{S p (U j, V j} ) is stored in a memory as data indicating the position of the pattern light 32a.

The posture angle calculation unit 22 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 extracted by the first filter 15. . For example, as shown in FIG. 3, from the base line length Lb between the projector 11 and the camera 12 and the coordinates (U _j , V _j ) of each spot light on the image, each spot is calculated using the principle of triangulation. The position on the road surface 31 irradiated with 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 (baseline length Lb) between the camera 12 and the projector 11 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 (U _j , V _j ) of each spot light on the camera 12. It can be obtained as a relative position (X _j , Y _j , Z _j ).

Note that the relative positions (X _j , Y _j , Z _j ) of each spot light with respect to the camera 12 often do 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 by using the least square method.

  The feature point detection unit 23 reads an image extracted by the second filter 17 out of images 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. At this time, as shown in FIG. 3B, when the area 33 for detecting feature points is the same as the area for projecting pattern light 32a, the feature point detector 23 is extracted by the second filter 17. The feature point is detected from the area where the pattern light on the image is projected. The feature point detector 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 (U _i , V _i ) on the image of each feature point is stored in a memory in the ECU 13. FIGS. 7 (a) and 7 (b) shows an example of a detected feature point T _e from the image acquired by the camera 12. The position (U _i , V _i ) of each feature point on the image is stored in the memory.

  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. Further, 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 reads, from the memory, the positions (U _i , V _i ) on the image of a plurality of feature points included in the previous frame among frames that are captured every certain information processing cycle. Further, the positions (U _i , V _i ) on the image of a plurality of feature points included in the current frame are read from the memory. Then, the amount of change in the attitude of the vehicle is obtained based on the change in position of the plurality of feature points on the image. Here, the feature points of each frame are detected from the image extracted by the second filter 17. In addition, as shown in FIG. 3B, when the area 33 for detecting feature points is the same as the area for projecting the pattern light 32a, the feature points of each frame are extracted by the second filter 17. The pattern light on the image is detected from the projected area. The “vehicle attitude change amount” includes both the “distance and attitude angle” change amount relative to the road surface 31 and the “vehicle (camera 12) movement amount” on the road surface. Hereinafter, a method of calculating the change amount of the distance and the posture angle and the movement amount of the vehicle will be described.

FIG. 7A shows an example of the first frame (image) 38 acquired at time t. As shown in FIG. 6 or FIG. 7A, in the first frame 38, for example, the relative positions (X _i , Y _i , Z _i ) of three feature points T _e1 , T _e2 , T _e3 are respectively calculated. Think if you are. In this case, the plane G specified by the feature points T _e1 , T _e2 , and T _e3 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 (X _i , Y _i , Z _i ). Further, the posture change amount calculation unit 24, by known camera model, the distance between each feature point _{T e1, T e2, T e3} (l 1, l 2, l 3) and feature point of each _T e1, T _The angle formed by the straight line connecting _e2 and _Te3 can be obtained. The camera 12 in FIG. 6 shows the position of the camera in the first frame.

It should be noted that as the three-dimensional coordinates (X _i , Y _i , Z _i ) indicating the relative position with respect to the camera 12, the imaging direction of the camera 12 is set to the Z axis, the imaging direction is normal, 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. 7B shows a second frame acquired at time (t + Δt) when time Δt has elapsed from time t. The camera 12 ′ in FIG. 6 indicates the position of the camera when the second frame 38 ′ is imaged. As shown in FIG. 6 or 7B, in the second frame 38 ′, the camera 12 ′ captures the feature points T _e1 , T _e2 , and T _e3 , and the feature point detection unit 23 performs the feature points T _e1 , T _e2 and _Te3 are detected. In this case, the posture change amount calculation unit 24 calculates the relative position (X _i , Y _i , Z _i ) of each feature point T _e1 , T _e2 , T _e3 at time t and the second frame 38 ′ of each feature point. From the position P ₁ (U _i , V _i ) and the camera model of the camera 12, not only the movement amount (ΔL) of the camera 12 at the time Δt but also the amount of change in the distance and posture angle can be calculated. For example, the posture change amount calculation unit 24 calculates the movement amount (ΔL) of the camera 12 (vehicle) and the change amounts of the distance and posture angle by solving simultaneous equations including the following equations (1) to (4). Can be calculated. 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.

FIG. 3A shows a feature point detected from another region 33 different from the region irradiated with the pattern light 32a in the road surface 31 of the imaging range of the camera 12, and the camera is detected from the time variation of the detected feature point. The manner in which twelve moving directions 34 are obtained is schematically shown. Further, in FIGS. 7 (a) and 7 (b) shows superimposes the vector D _te showing change direction and the change amount of the position of each feature point T _e in the image. The posture change amount calculation unit 24 can simultaneously calculate not only the movement amount (ΔL) of the camera 12 at time Δt but also the change amounts of the distance and the posture angle. Therefore, the posture change amount calculation unit 24 can accurately calculate the movement amount (ΔL) with six degrees of freedom in consideration of the change amount of the distance and the posture angle. That is, even if the distance and the posture angle change due to the roll motion or the pitch motion caused by turning or acceleration / deceleration of the vehicle 10, the estimation error of the movement amount (ΔL) can be suppressed.

Note that the posture change amount calculation unit 24 may select optimal feature points based on the positional relationship between the feature points, instead of using all the feature points whose relative positions are calculated. As selection procedures, for example, the epipolar geometry (epipolar geometry, RI Hartley:. "A linear method for reconstruction from lines and points," Proc 5 th International Conference on Computer Vision, Cambridge, Massachusetts, pp.882-887 ( 1995)) can be used.

  In order to associate 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.

As described above, when the feature points T _e1 , T _e2 , and T _e3 whose relative positions (X _i , Y _i , Z _i ) are calculated are also detected from the image 38 ′ acquired at a later timing, The posture change amount calculation unit 24 can calculate a “vehicle posture change amount” based on temporal changes of a plurality of feature points on the road surface.

  The self position calculation unit 26 calculates the distance and posture angle from the “distance and change amount of posture angle” calculated by the posture change amount calculation unit 24. Further, the current position of the vehicle is calculated from the “movement amount of the vehicle” calculated by the posture change amount calculation unit 24.

  Specifically, when the distance and posture angle calculated by the posture angle calculation unit 22 (see FIG. 1) are set as starting points, the posture change amount calculation unit 24 performs the setting with respect to the starting point (distance and posture angle). The calculated distance and posture angle change amount for each frame is sequentially added (integral calculation), thereby updating the distance and posture angle to the latest numerical values. In addition, 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 moving amount of the vehicle is sequentially added from this initial position (integral calculation). Thus, the current position of the vehicle 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.

  In this way, if it is possible to continue to detect three or more feature points that can be correlated between the preceding and following frames, the pattern light 32a can be obtained by continuing the process of adding the distance and the change amount of the posture angle (integral calculation). The distance and posture angle can be continuously updated to the latest values without using. However, in the first information processing cycle, a distance and posture angle calculated using the pattern light 32a, or a predetermined initial distance and initial 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 initial posture angle are a distance and posture angle that take into account 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 the initial 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 the present embodiment, the self-position calculation unit 26 calculates the distance and posture angle calculated by the posture angle calculation unit 22 and the vehicle position at that time as a starting point of a new integration operation (vehicle posture angle and initial position for each predetermined frame. ) And the addition of the vehicle attitude change amount is started from the starting point. For example, in this embodiment, a new base point is set every 10 frames, and when a new base point is set, the starting point of the integration calculation is maintained until the next 10th frame. Hereinafter, with reference to FIG. 8, the control in the case of setting the base point every 10 frames will be described. In FIG. 8, the case where pattern light is projected with a red laser pointer will be described.

  As shown in FIG. 8, first, when the flag for projecting pattern light rises to ON at time t1, the flag using the R signal image rises to ON at the same time. Thereby, the pattern light extraction unit 21 extracts the position of the pattern light from the image extracted by the first filter 15, and the posture angle calculation unit 22 determines the distance and posture angle of the vehicle with respect to the road surface from the position of the extracted pattern light, and The vehicle position at that time is calculated. Then, the self-position calculation unit 26 sets these as starting points for new integration calculations.

  Thereafter, at time t2, the flag for projecting pattern light and the flag using the R signal image fall from ON to OFF, and the flag using the B signal or G signal image rises from OFF to ON. Thereby, the feature point detection unit 23 detects the feature point from the image extracted by the second filter 17, and the posture change amount calculation unit 24 calculates the change amount of the distance and the posture angle from the detected feature point. Then, the self-position calculation unit 26 adds the change amount of the distance and the posture angle to the set starting point of the integral calculation, and updates it to the latest distance and posture angle. Thereafter, the distance and the posture angle are continuously updated by sequentially adding the distance and the posture angle change amount for each frame up to the next tenth frame. At time t11, a new starting point for the integration operation is set. The In this way, the self-position calculating device according to the present embodiment can continuously calculate the self-position of the vehicle.

  In the embodiment, the distance and posture angle change amount is calculated, and the distance and posture angle change amount are sequentially added to update the distance and posture angle to the latest numerical values. However, only the attitude angle of the camera 12 with respect to the road surface 31 may be a target for calculating and updating the change amount. In this case, the distance of the camera 12 relative to the road surface 31 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 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 is activated, the pattern light control unit 27 starts projecting the pattern light 32a. Thereafter, the pattern light control unit 27 continuously projects the pattern light 32a until the self-position calculating device stops. Alternatively, the on / off of the light projection may be repeated at a predetermined time interval.

(Information processing cycle)
Next, as an example of a self-position calculation method for estimating the movement amount of the vehicle 10 from the image 38 acquired by the camera 12, an information processing cycle repeatedly executed by the ECU 13 will be described with reference to FIGS. . The information processing cycle shown in the flowchart of FIG. 9 is repeatedly executed until the ignition switch of the vehicle 10 is turned on, the self-position calculation device is started, and the self-position calculation device is stopped.

  In FIG.9 S01, the pattern light control part 27 controls the light projector 11, and projects the pattern light 32a on the road surface 31 around a vehicle. In this embodiment, a red laser is used as the pattern light.

  In step S03, the ECU 13 controls the camera 12 to capture the road surface 31 around the vehicle including the area where the pattern light 32a is projected, and obtain an image 38. The ECU 13 stores the image data acquired by the camera 12 in a memory.

  The ECU 13 can automatically control the aperture of the camera 12. The diaphragm of the camera 12 may be feedback controlled so that the average luminance of the image 38 acquired in the previous information processing cycle becomes an intermediate value between the maximum value and the minimum value of the luminance value. Moreover, since the brightness | luminance value is high in the area | region where the pattern light 32a is projected, you may obtain | require an average luminance value from the area | region except the part which extracted the pattern light 32a.

  In step S05, the first filter 15 and the second filter 17 extract an image. Specifically, the first filter 15 extracts an image in the wavelength region of the pattern light 32 a from the image acquired by the camera 12. In the present embodiment, since the pattern light 32a is a red laser, an image in the red (600 to 650 nm) wavelength region is extracted. The second filter 17 extracts an image that does not include the wavelength range of the pattern light 32 a from the image acquired by the camera 12. In the present embodiment, since the pattern light 32a is a red laser, an image in one of the blue (450 to 570 nm) and green (520 to 560 nm) wavelength ranges not including the red wavelength range, or both blue and green wavelengths. Extract a region image. The images extracted by the first filter 15 and the second filter 17 are stored in the memory.

In step S07, the pattern light extraction unit 21 first reads an image extracted by the first filter 15 out of the image 38 acquired by the camera 12, from the memory, and as shown in FIG. 4C, the pattern light 32a. Extract the position of. Pattern light extraction unit 21 stores the coordinates (U _{j, V j)} on the image of the spot light S _p calculated as data indicating the position of the pattern light 32a in the memory.

  In step S07, the posture angle calculation unit 22 reads data indicating the position of the pattern light 32a in the image extracted by the first filter 15 from the memory, calculates the distance and the posture angle from the position of the pattern light 32a, and stores the memory. To remember.

Proceeding to step S09, the ECU 13 detects a feature point from the image extracted by the second filter 17 in the image 38, and extracts a feature point that can be correlated between the preceding and following information processing cycles. Then, the ECU 13 calculates the distance and the change amount of the attitude angle and the movement amount of the vehicle from the position (U _i , V _i ) on the image of the feature point.

Specifically, first, the feature point detection unit 23 reads an image extracted by the second filter 17 from the image 38 obtained by the camera 12 from the memory. Then, feature points on the road surface 31 are detected from the read image, and the positions (U _i , V _i ) of the feature points on the image are stored in the memory. The posture change amount calculation unit 24 reads the position (U _i , V _i ) of each feature point on the image from the memory, and based on the distance and posture angle and the position (U _i , V _i ) of the feature point on the image. Then, the relative position (X _i , Y _i , Z _i ) of the feature point with respect to the camera 12 is calculated. The posture change amount calculation unit 24 uses the distance and posture angle set in step S11 of the previous information processing cycle. The posture change amount calculation unit 24 stores the relative positions (X _i , Y _i , Z _i ) of the feature points with respect to the camera 12 in the memory.

The posture change amount calculation unit 24 then compares the position of the feature point on the image (U _i , V _i ) and the relative position of the feature point calculated in step S09 of the previous information processing cycle (X _i , Y _i , Z _i ) is read from memory. The posture change amount calculation unit 24 uses the relative position (X _i , Y _i , Z _i ) of the feature point and the position (U _i , V _i ) on the image that can be correlated between the previous and subsequent information processing cycles. The amount of change in distance and posture angle is calculated. Furthermore, the relative positions of feature points in the previous processing cycle _{(X i, Y i, Z} i) and the relative positions of feature points in the current processing cycle _{(X i, Y i, Z} i) from the, vehicle The amount of movement is calculated. The “amount of change in distance and attitude angle” and “amount of movement of the vehicle” calculated in step S09 are used in the process of step S13.

  Proceeding to step S11, the ECU 13 sets the starting point of the integral calculation for each predetermined frame. Details will be described later with reference to FIG.

  Proceeding to step S13, the self-position calculating unit 26 calculates the current position of the vehicle from the starting point of the integral calculation set in the process of step S11 and the movement amount of the vehicle calculated in the process of step S09.

  In this way, the self-position calculation device according to the present embodiment can calculate the current position of the vehicle 10 by repeatedly executing the series of information processing cycles described above and integrating the movement amount of the vehicle 10.

  Next, the detailed procedure of step S11 in FIG. 9 will be described with reference to the flowchart in FIG. In step S901, the ECU 13 determines whether or not the current information processing cycle is the first time. If it is the first time, that is, if there is no data in the previous information processing cycle, the process proceeds to step S905. If not, the process proceeds to step S903.

  In step S903, the self-position calculation unit 26 determines whether or not the current frame is a frame for newly setting the starting point of the integration calculation. As described with reference to FIG. 8, in this embodiment, for example, since the starting point of the integration calculation is newly set every 10 frames, it is determined whether or not the current frame corresponds to the 10th frame. If the current frame is a frame for newly setting the starting point of integration calculation (YES in step S903), the process proceeds to step S905. If the current frame is a frame for which the starting point of integration calculation is not set (NO in step S903), step is performed. The process proceeds to S907.

  In step S907, the ECU 13 maintains the currently set starting point of the integral calculation.

  In step S905, the ECU 13 sets the current position of the vehicle as the starting point, and further sets the distance and posture angle calculated in step S07 of the same information processing cycle as the starting point of the integration calculation. A new integration operation is started from this distance and posture angle as the starting point. Also, the integration calculation of the movement amount of the vehicle is newly started from the current position of the vehicle. Thereafter, the process proceeds to step S13 in FIG.

(Effect of embodiment)
Hereinafter, effects of the self-position calculation apparatus according to the present embodiment will be described. On the road surface, there are road surface changes such as unevenness and steps due to aging of cantes and asphalt for drainage. At this time, if the area where the feature points are detected differs from the area where the pattern light is projected, an error occurs in the calculation result of the distance and posture angle of the vehicle with respect to the road surface due to the change in the road surface. Therefore, it is desirable that the area where pattern light is projected and the area where feature points are detected overlap.

  However, if the area where pattern light is projected and the area where feature points are detected overlap, it is difficult to discriminate between pattern light and feature points (textures), so pattern light is erroneously detected as a feature point. There is a case. For example, when pattern light is projected onto a region for detecting feature points, the pattern light shown in FIG. 11B is extracted from the captured image shown in FIG. 11A, and the feature points shown in FIG. 11C are detected. Is done. As shown in FIG. 11C, it can be seen that pattern light is erroneously detected as a feature point. Pattern light that is erroneously detected as a road surface feature point does not move on the captured image, and this is a factor that causes an error in the detection result of the moving distance of the vehicle with respect to the road surface that is running.

  In contrast, in the self-position calculation apparatus according to the present embodiment, the first filter 15 extracts an image in the wavelength range of the pattern light, and the second filter 17 extracts an image not including the wavelength range of the pattern light. Then, the attitude angle of the vehicle with respect to the road surface is calculated from the position of the pattern light in the image extracted by the first filter 15, and based on the time change of the feature point detected from the image extracted by the second filter 17. Calculate the amount of posture change. As a result, even if the region where the feature point is detected overlaps the region where the pattern light is projected, it is possible to prevent the pattern light from being erroneously detected as a feature point, and the current position of the vehicle can be accurately and stably maintained. Can be estimated.

  Further, in the self-position calculation device according to the present embodiment, a feature point is detected from the region where the pattern light on the image extracted by the second filter 17 is projected, and based on the time change of the detected feature point. To calculate the attitude change amount of the vehicle. Thereby, since the area | region which detects a feature point and the area | region which projects pattern light can be overlapped, the error which arises by road surface changes, such as an unevenness | corrugation on a road surface and a level | step difference, can be reduced.

  In the self-position calculation device according to the present embodiment, the projector 11 projects pattern light in the red wavelength region, and the first filter 15 extracts an image in the red wavelength region. Thereby, since a red laser can be used as a light source for pattern light, the structure of the projector 11 can be simplified, and the reliability of the system can be improved.

  In the self-position calculation apparatus according to the present embodiment, the projector 11 projects pattern light in the red wavelength range, and the second filter 17 extracts an image in at least one wavelength range of blue or green. Thereby, since a red laser can be used as a light source for pattern light, the structure of the projector 11 can be simplified, and the reliability of the system can be improved.

  Although the embodiments of the present invention have been described as described above, it should not be understood that the descriptions and drawings constituting a part of this disclosure limit the present invention. From this disclosure, various alternative embodiments, examples and operational techniques will be apparent to those skilled in the art.

  Although FIG. 2 shows an example in which the camera 12 and the projector 11 are attached to the front surface of the vehicle 10, the camera 12 and the projector 11 may be installed sideways, rearward, or directly below the vehicle 10. Further, in the present embodiment, a four-wheeled passenger car is shown as an example of the vehicle 10 in FIG. 2, but a feature point on a road surface or wall surface of a road such as a motorcycle, a freight car, or a special vehicle carrying a construction machine, for example. The present invention can be applied to all moving bodies (vehicles) capable of imaging

10 vehicle 11 projector 12 camera (imaging part)
DESCRIPTION OF SYMBOLS 15 1st filter 17 2nd filter 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 31 Road surface 32a, 32b Pattern light 51 Red laser pointer 53 Control board 55 Semiconductor Laser 57 Green laser pointer 59 Nonlinear crystal (YAG)
61 Crystal (KTP)
63 Laser output sensor Te Features

Claims (5)

A projector that projects pattern light onto the road surface around the vehicle;
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 first filter that extracts an image in the wavelength range of the pattern light from the image acquired by the imaging unit;
A second filter for extracting an image that does not include the wavelength range of the 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 in the image extracted by the first filter;
An attitude change amount calculating unit that calculates an attitude change amount of the vehicle based on temporal changes of a plurality of feature points on the road surface detected from the image extracted by the second filter;
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 self-position calculating device comprising: The self-position calculating device according to claim 1,
The posture change amount calculation unit is configured to change the vehicle based on time changes of a plurality of feature points on the road surface detected from an area where the pattern light on the image extracted by the second filter is projected. A self-position calculation device that calculates the amount of change in posture. The self-position calculating device according to claim 1 or 2,
The self-position calculating device, wherein the projector projects pattern light in a red wavelength range, and the first filter extracts an image in a red wavelength range. The self-position calculating device according to claim 1 or 2,
The self-position calculation device according to claim 1, wherein the projector projects pattern light in a red wavelength range, and the second filter extracts an image in at least one wavelength range of blue or green. A procedure for projecting pattern light from a projector mounted on a vehicle onto a road surface around the vehicle,
A procedure for obtaining 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 procedure in which the control unit of the vehicle extracts an image of the wavelength range of the pattern light from the acquired image with a first filter;
A procedure for the controller to extract an image that does not include the wavelength range of the pattern light from the acquired image with a second filter;
A step of calculating a posture angle of the vehicle with respect to the road surface from the position of the pattern light in the image extracted by the first filter;
A step of calculating a posture change amount of the vehicle based on temporal changes of a plurality of feature points on the road surface detected from the image extracted by the second filter;
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;
A self-position calculation method comprising: