JP6547362B2 - 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 an attitude angle of a vehicle.

  There is known a technique of acquiring an image in the vicinity of a vehicle captured by a camera mounted on the vehicle and determining a movement amount of the vehicle based on a change in the image (see Patent Document 1). In Patent Document 1, in order to accurately determine the movement amount even at low speed and subtle movement of the vehicle, the feature point is detected from the image, the position of the feature point on the image is determined, and the movement direction and movement of the feature point The moving amount of the vehicle is obtained from the distance (moving amount).

  In addition, as a technique to calculate the amount of change in the vehicle attitude, the camera captures an image of pattern light projected from the vehicle onto the road surface, and calculates the plane formula of the road surface for the camera using the triangulation principle from the shape on the image. There is a technology for calculating the distance and attitude of a vehicle relative to the road surface (see Patent Document 2). When using these techniques, it is difficult to detect road surface features from images taken with a camera under dark conditions such as in a tunnel or at night, so light the road surface to detect road surface features. .

JP, 2008-175717, A JP 2007-278951 A

  However, in the above-described conventional technique for calculating the amount of movement, it is premised that the feature of the road surface and the pattern light on the camera image are on the same plane. Therefore, when there is an unevenness on the road surface to generate a shadow, calculating the movement amount of the vehicle from the pattern light irradiated to the feature or unevenness due to the unevenness, the error of movement amount estimation is enlarged and the vehicle's self position There was a problem that it was not possible to calculate with high accuracy.

  Therefore, the present invention has been proposed in view of the above-described situation, and it is an object of the present invention to provide a self position calculation apparatus and method capable of accurately calculating the self position of a vehicle even if there are irregularities on the road surface. To aim.

  In order to solve the problems described above, the self-position calculation apparatus and method according to one aspect of the present invention project at least pattern light from the projector mounted on the vehicle to the road surface around the vehicle, and the imaging unit Captures an image of the road surface around the vehicle including the lighted area. Then, the position of the shadow on the image due to the unevenness of the road surface is detected from the acquired image, the detection area is set on the image based on the position of the shadow, and the attitude angle of the vehicle to the road surface from the position of the pattern light in this detection area Calculate Further, a plurality of feature points on the road surface are detected from the set detection area, and the amount of change in posture of the vehicle is calculated based on the time change of the plurality of feature points on the detected road surface. Based on these results, the self-position calculating apparatus and method according to an aspect of the present invention add the amount of change in posture to the initial position and posture angle of the vehicle relative to the road surface, to obtain the current position of the vehicle and Calculate the attitude angle.

  According to the present invention, since the detection area is set on the image in consideration of the position of the unevenness of the road surface, the self position of the vehicle can be accurately calculated even if the unevenness is present on the road surface.

FIG. 1 is a block diagram showing an entire configuration of a self-position calculation apparatus according to a first embodiment of the present invention. FIG. 2 is an external view showing an example of mounting a light projector and a camera on a vehicle. FIG. 3 is a schematic view showing how the position on the road surface on which each spot light is irradiated and the moving direction of the camera are obtained from the arrangement of the light projector and the camera. FIG. 4 is a view showing an image of pattern light obtained by binarizing the image acquired by the camera. FIG. 5 is a view for explaining the change of the shadow by the convex portion on the road surface. FIG. 6 is a view for explaining a method of setting a detection area from the position of a shadow on an image by the self-position calculation apparatus according to the first embodiment. FIG. 7 is a schematic view for explaining a method of calculating the amount of change in distance and attitude angle. FIG. 8 is a view showing an example of a frame (image) acquired at time t and time t + Δt. FIG. 9 is a flowchart showing the procedure of the self position calculation method by the self position calculation device according to the first 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 change of the shadow by the convex portion on the road surface due to the movement of the vehicle. FIG. 12 is a diagram for explaining the change of the shadow by the concave portion on the road surface due to the movement of the vehicle. FIG. 13 is a diagram for describing a method of setting a detection area from the position of a shadow on an image by the self-position calculating apparatus according to the second embodiment. FIG. 14 is a diagram for describing a method of setting a detection area from the position of a shadow on an image by the self-position calculating device according to the second embodiment. FIG. 15 is a diagram for describing a method of setting a detection area from the position of a shadow on an image by the self-position calculating device according to the third embodiment.

  Hereinafter, embodiments will be described with reference to the drawings. In the description of the drawings, the same parts will be denoted by the same reference numerals and the description thereof will be 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 calculation device includes a pattern light projector 11, a camera 12, an engine control unit (ECU) 13, and a road surface light projector 14. The pattern light projector 11 is mounted on a vehicle and projects pattern light on the 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 a region where pattern light is projected. The ECU 13 is an example of a control unit that controls the pattern light projector 11 and the road light projector 14 and executes a series of information processing cycles for estimating the movement amount of the vehicle from the image acquired by the camera 12. The road surface light projector 14 is an example of a light source mounted on a vehicle and illuminating a road surface around the vehicle. The road surface light emitter 14 can also project the pattern light and the light that illuminates the road surface around the vehicle as a light emitter integrated with the pattern light light emitter 11.

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

  The height and direction in which the camera 12 is installed are adjusted so that the feature points (texture) on the road surface 31 in front of the vehicle 10 and the pattern light 32 b projected by the pattern light projector 11 can be captured. The focus and aperture of the lens included in 12 are automatically adjusted. The camera 12 repeatedly performs imaging at predetermined time intervals to acquire a series of image (frame) groups. The image data acquired by the camera 12 is transferred to the ECU 13 and stored in a memory included in the ECU 13.

The pattern light projector 11 projects the pattern light 32b having a predetermined shape including a square or rectangular grid image toward the road surface 31 within the imaging range of the camera 12, as shown in FIG. The camera 12 images the pattern light irradiated to the road surface 31. The pattern light projector 11 includes, for example, a laser pointer and a diffraction grating. By diffracting the laser beam emitted from the laser pointer in the diffraction grating, the pattern light projector 11, as shown in FIGS. 2 to 4, the grating image, or a matrix a plurality of which are arranged in the spot light S _p Pattern 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. Further, the pattern light projector 11 may be used not only for projecting the pattern light but also as a light source for illuminating the road surface within the imaging range of the camera 12 instead of the road light projector 14.

  The road surface light projector 14 is a light source which illuminates the road surface 31 within the imaging range of the camera 12, and may be a vehicle light such as a headlight or a fog lamp, or may be separately mounted on the vehicle.

  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 calculation apparatus 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 be used also as an ECU used for other control related to the vehicle 10.

  The plurality of information processing units include a detection area setting unit 20, 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, and a pattern light control unit 27. Is included. The posture change amount calculation unit 24 includes a feature point detection unit 23.

  The detection area setting unit 20 detects the position of the shadow on the image due to the unevenness of the road surface created by the light source of the road light projector 14 from the image acquired by the camera 12 and detects the detection area on the image based on the position of the shadow. Set In particular, in the present embodiment, the detection area is set by excluding the area in the light source direction of the road surface light projector 14 from the position of the shadow on the image.

  Specifically, as shown in FIG. 5, if there is a small convex portion 51 projecting only a part as shown in FIG. There is also a convex portion 53 protruding long in the light source direction as shown in b). However, on the image captured by the camera 12, if the height and width of the convex portion 51 and the convex portion 53 are the same, as shown in FIG. 5C, the shadows 55 have the same shape. Therefore, it is difficult to determine the shapes of the convex portions 51 and 53 on the road surface on the image captured by the camera 12.

  Further, the detection of feature points by the feature point detection unit 23 described later and the calculation of the distance and attitude angle of the vehicle by the attitude angle calculation unit 22 are premised on the flat road surface. Therefore, it is not preferable to detect the feature point from the image of the convex portion or to calculate the distance and the attitude angle from the pattern light projected to the convex portion.

  Therefore, in the present embodiment, the detection area is set excluding the area where the convex portion may exist on the road surface, and the detection of the feature point and the calculation of the distance and the attitude angle are performed in the set detection area. Thereby, even if the road surface is uneven, the vehicle's own position can be accurately calculated.

  Next, a method of setting a detection area by the detection area setting unit 20 will be described. First, the detection area setting unit 20 reads the image acquired by the camera 12 from the memory, and detects the position of the shadow on the image due to the unevenness of the road surface from this image. In the present embodiment, the luminance value of the image captured by the camera 12 is binarized with the threshold Bth to generate a binarized image, and the darker part is determined as a shadow area. The threshold value Bth changes according to the strength of the light source and the distance and angle of the camera and the light source with respect to the road surface, and the installation position. Therefore, the threshold value Bth may be obtained in advance by experiment etc.

  Then, when the position of the shadow is detected on the image, the detection area setting unit 20 sets a detection area on the image based on the position of the shadow. Specifically, in the present embodiment, the detection area is set by excluding the area in the light source direction of the road surface light projector 14 from the position of the shadow on the image.

  For example, when the shadows 61a to 61e are detected as shown in FIG. 6A, a region surrounded by a dotted line from the position of the shadow to the light source direction of the road surface light projector 14 as shown in FIG. Are set as exclusion regions 63a to 63e, and detection regions are set excluding the exclusion regions 63a to 63e. The width of the excluded areas 63a to 63e is set corresponding to the maximum width in the lateral direction (vehicle width direction) of the shadows 61a to 61e, and the pattern light projector 11 or road surface serving as a light source from the position of the shadows 61a to 61e Excluded areas 63a to 63e are set in the direction of the projector 14 up to the end of the image. Then, the detection area setting unit 20 sets a detection area excluding the exclusion areas 63a to 63e from the image of FIG. 6B, and stores the detection area in the memory. As a result, the detection area can be set except for the area where the convex portions 51 and 53 may exist as shown in FIGS. 5A and 5B. The self position can be calculated accurately.

The pattern light extraction unit 21 reads the image of the detection area from the memory among the images acquired by the camera 12 and extracts the position of the pattern light from the image. As shown in FIG. 3 (a), for example, the pattern light projector 11 projects pattern light 32a composed of a plurality of spot lights arranged in a matrix toward the road surface 31, and the pattern light reflected by the road surface 31 32a is detected by the camera 12. Pattern light extractor 21, by performing binarization processing on the acquired image by the camera 12, as shown in FIGS. 4 (a) and 4 (b), only the image of the spot light S _p Extract. 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 attitude angle calculation unit 22 reads data indicating the position of the pattern light 32 a from the memory, and calculates the distance and attitude angle of the vehicle 10 with respect to the road surface 31 from the position of the pattern light 32 a in the image acquired by the camera 12. For example, as shown in FIG. 3A, the principle of triangulation is based on the base length Lb between the pattern light projector 11 and the camera 12 and the coordinates (U _j , V _j ) on the image of each spot light. The position on the road surface 31 to which each spot light is irradiated 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 to which the pattern light 32a is projected from the relative position of each spot light with respect to the camera 12, that is, the distance and attitude angle (normal vector Calculate). 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 attitude angle of the camera 12 with respect to the road surface 31 are calculated as an example of the distance and attitude angle of the vehicle 10 with respect to the road surface 31. Do. Hereinafter, the distance and attitude angle of the camera 12 with respect to the road surface 31 will be abbreviated as “distance and attitude angle”. The distance and attitude angle calculated by the attitude angle calculation unit 22 are stored in the memory.

Specifically, since the camera 12 and the pattern light projector 11 are respectively fixed to the vehicle 10, the irradiation direction of the pattern light 32a and the distance (base length Lb) between the camera 12 and the pattern light projector 11 are known. . Therefore, the attitude angle calculation unit 22 sets the position on the road surface 31 at which each spot light is irradiated from the coordinates (U _j , V _j ) on the image of each spot light to the camera 12 using the principle of triangulation. It can be determined as the relative position (X _j , Y _j , Z _j ).

The relative positions (X _j , Y _j , Z _j ) of the spot lights 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 unevenness of the asphalt exposed on the road surface 31. Therefore, a least squares method may be used to obtain a plane equation that minimizes the sum of squares of the distance error with each spot light.

  The feature point detection unit 23 reads an image of a detection area among the 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. The feature point detection unit 23 detects, for example, feature points on the road surface 31 by, for example, “DG Lowe,“ Distinctive Image Features from Scale-Invariant Keypoints ”, Int. J. Comput. Vis., Vol. 60, no .2, pp. 91-110, Nov. 200 "or" Akira Kanazawa, Ken-ichi Kanaya, "Image feature point extraction for computer vision," Journal of Information Science, vol. 87, no. 12, pp. The methods described in "1043-1048, Dec. 2004" can be used.

Specifically, the feature point detection unit 23 detects, as a feature point, a point at which the luminance value largely changes as compared with the surroundings, such as a vertex of an object, using, for example, a Harris operator or a SUSAN operator. Alternatively, the feature point detection unit 23 may use a scale-invariant feature transform (SIFT) feature amount to detect, as a feature point, a point whose luminance value changes under a certain regularity. . 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. 8A and 8B show examples of the feature point T _e detected from the image acquired by the camera 12. The position (U _i , V _i ) on the image of each feature point is stored in the memory.

  In the embodiment, characteristic points on the road surface 31 mainly assume particles of asphalt mixture having a size of 1 cm or more and 2 cm or less. In order to detect this feature point, the resolution of the camera 12 is VGA (about 300,000 pixels). Further, the distance of the camera 12 to the road surface 31 is approximately 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 brightness numerical values when transferring the image acquired by the camera 12 to the ECU 13 are 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 the frames captured in each predetermined information processing cycle. Furthermore, the positions (U _i , V _i ) on the image of the plurality of feature points included in the current frame are read from the memory. Then, the amount of change in posture of the vehicle is obtained based on the change in position of the plurality of feature points on the image. Here, “the amount of change in posture of the vehicle” includes both the amount of change in “distance and attitude angle” with respect to the road surface 31 and “the amount of movement of the vehicle (camera 12) on the road surface”. Hereinafter, a method of calculating the change amount of the distance and the attitude angle and the movement amount of the vehicle will be described.

FIG. 8A shows an example of the first frame (image) 38 acquired at time t. As shown in FIG. 7 or 8A, for example, relative positions (X _i , Y _i , Z _i ) of three feature points T _e1 , T _e2 , T _e3 are calculated in the first frame 38, respectively. Consider the case. In this case, the plane G specified by the feature points T _e1 , T _e2 , T _e3 can be regarded as a road surface. Therefore, the posture change amount calculating section 24 can be determined relative position _{(X i, Y i, Z} i) from the distance and the attitude angle of the camera 12 relative to the road surface (plane G) a (normal vector). 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 determined. The camera 12 of FIG. 7 shows the position of the camera in the first frame.

The three-dimensional coordinates indicating the relative position with respect to the camera _{12 (X i, Y i,} Z i) as, the imaging direction of the camera 12 is set to the Z axis, the plane containing the to and camera 12 of the capturing direction and the normal line , X axis and Y axis orthogonal to each other are set. On the other hand, as coordinates on the image 38, the horizontal direction and the vertical direction are respectively set to the V axis and the U axis.

FIG. 8B shows a second frame acquired at time (t + Δt) when time Δt has elapsed from time t. The camera 12 'of FIG. 7 shows the position of the camera when imaging the second frame 38'. As shown in FIG. 7 or FIG. 8B, in the second frame 38 ′, the camera 12 ′ captures feature points T _e1 , T _e2 and T _e3 , and the feature point detection unit 23 detects the feature points T _e1 and T _e2 and _Te3 are detected. In this case, the posture change amount calculation unit 24 calculates the relative positions (X _i , Y _i , Z _i ) of the feature points 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 ) of and the camera model of the camera 12, it is possible to calculate not only the moving amount (ΔL) of the camera 12 at time Δt but also the amount of change in distance and attitude angle. For example, the posture change amount calculation unit 24 solves the movement amount (ΔL) of the camera 12 (vehicle) and the change amounts of the distance and the posture angle by solving a simultaneous equation consisting of the following equations (1) to (4). It 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 parameters of the camera model may be calibrated in advance.

In FIG. 3B, the moving direction 34 of the camera 12 is obtained from the time change of the feature point detected from another area 33 different from the area irradiated with the pattern light 32 a in the imaging range of the camera 12 Is schematically shown. 8 (a) and 8 (b), a vector D _te indicating the change direction and the change amount of the position of each feature point T _e is shown superimposed on 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) of the six degrees of freedom in consideration of the change amounts of the distance and the posture angle. That is, even if the distance or the attitude angle changes due to the roll motion or the pitch motion due to the turning or acceleration / deceleration of the vehicle 10, the estimation error of the movement amount (ΔL) can be suppressed.

The posture change amount calculation unit 24 may select an optimal feature point based on the positional relationship between feature points instead of using all the feature points whose relative positions are calculated. As a selection method, for example, epipolar geometry (epipolar line 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 preceding and following frames, for example, an image of a small area around a detected feature point may be recorded in a memory and determined from the similarity of luminance and color information. Specifically, the ECU 13 records an image of 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 1% or less, the posture change amount calculation unit 24 determines that the feature points can be correlated between the preceding and succeeding frames.

Thus, when the feature points T _e1 , T _e2 , T _e3 for which the 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 the “vehicle posture change amount” based on the time change of the plurality of feature points on the road surface.

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

  Specifically, when the distance and posture angle calculated by posture angle calculation unit 22 (see FIG. 1) are set as the starting point, posture change amount calculation unit 24 with respect to this starting point (distance and posture angle) The distance and the attitude angle are updated to the latest numerical values by sequentially adding up (integrating) the change amounts of the distance and the attitude angle for each frame calculated in the above. Further, the vehicle position at which the distance and attitude angle are calculated by the attitude angle calculation unit 22 is set as the starting point (initial position of the vehicle), and the movement 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) collated with the position on the map.

  As described above, if it is possible to continue detecting three or more feature points that can obtain a correspondence between the previous and subsequent frames, the pattern light 32a is continued by continuing the process (integral operation) of adding the change amounts of distance and attitude angle. The distance and attitude can be updated to the latest values without using. However, in the first information processing cycle, the distance and attitude angle calculated using the pattern light 32a, or a predetermined initial distance and initial attitude angle may be used. That is, the distance and the attitude angle as the starting point of the integration operation may be calculated using the pattern light 32a, or may use a predetermined initial value. It is desirable that the predetermined initial distance and the initial attitude angle be at least a distance and an attitude angle in consideration of 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 attitude angle calculated from the pattern light 32a are set to It may be used as an initial distance and an initial posture angle. As a result, it is possible to obtain the distance and the attitude angle when the roll motion or the pitch motion due to the turning or acceleration / deceleration of the vehicle 10 is not occurring.

  In the embodiment, the distance and the attitude angle are updated to the latest numerical values by calculating the amounts of change in the distance and the attitude angle and sequentially adding the amounts of change in the distance and the attitude angle. However, only the attitude angle of the camera 12 with respect to the road surface 31 may be used as the target of calculation and update of the amount of change. In this case, the distance of the camera 12 to the road surface 31 may be assumed to be constant. Thus, the calculation load of the ECU 13 can be reduced and the calculation speed can be improved while suppressing the estimation error of the movement amount (ΔL) in consideration of the amount of change of the attitude angle.

  Further, the self position calculation unit 26 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, but at this time, the conditions set in advance are satisfied. If there is, the posture change amount is added while maintaining the starting point of the integral calculation. On the other hand, if the conditions set in advance are not satisfied, in the same information processing cycle, the distance and attitude angle calculated by the attitude angle calculation unit 22 and the vehicle position at that time Set initial attitude angle and initial position). Then, addition of the amount of change in posture of the vehicle is started from the starting point. As conditions for maintaining the starting point of the integration operation, conditions such as the feature point being detected normally, no error factor being included, or a predetermined time or a predetermined number of cycles has not been set are set in advance. You should keep it. Such conditions may be other conditions as long as the error of the movement amount of the vehicle 10 is set so as not to be expanded by the integration calculation.

  The pattern light control unit 27 controls the projection of the pattern light 32 a by the pattern light projector 11. For example, when the ignition switch of the vehicle 10 is turned on and the self position calculation device is activated, the pattern light control unit 27 starts to project light of the pattern light 32a. Thereafter, the pattern light control unit 27 continuously emits the pattern light 32a until the self position calculation device is stopped. Alternatively, the light emission may be repeatedly turned on / off at predetermined time intervals.

Further, the pattern light control unit 27 controls the light projection of the pattern light projector 11 and the lighting of the road surface light projector 14 so that the detection area setting unit 20 can detect the position of the shadow due to the unevenness. Specifically, the ignition switch of the vehicle 10 may be turned on to start light emission and lighting at the same time as the self position calculation device is activated, and light emission and lighting may be performed as necessary. .
(Information processing cycle)
Next, an information processing cycle repeatedly executed by the ECU 13 will be described with reference to FIGS. 9 and 10 as an example of a self position calculation method of estimating the movement amount of the vehicle 10 from the image 38 acquired by the camera 12. . The information processing cycle shown in the flowchart of FIG. 9 is started at the same time when the ignition switch of the vehicle 10 is turned on and the self position calculation device is activated, and is repeatedly executed until the self position calculation device is stopped.

  In step S01 of FIG. 9, the pattern light control unit 27 controls the pattern light projector 11 to project the pattern light 32a onto the road surface 31 around the vehicle. In the flowchart of FIG. 9, an example in which the pattern light 32a is continuously projected will be described. The pattern light control unit 27 emits light from the pattern light projector 11 to the road surface 31 around the vehicle or turns on the road light projector 14 when the road surface is dark or the like.

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

  The ECU 13 can automatically control the aperture of the camera 12. The aperture of the camera 12 may be feedback-controlled so as to be an intermediate value between the maximum value and the minimum value of the luminance value from the average luminance of the image 38 acquired in the previous information processing cycle. In addition, since the area where pattern light 32a is projected has a high luminance value, the average luminance value may be determined from the area excluding the portion where pattern light 32a is extracted.

  In step S05, the detection area setting unit 20 reads the image acquired by the camera 12 from the memory, detects the position of the shadow on the image due to the unevenness of the road surface, and sets the detection area on the image based on the position of the shadow Do. In the present embodiment, the detection area is set excluding the area in the light source direction from the position of the shadow on the image, and is stored in the memory.

Proceeding to step S07, first, the pattern light extraction unit 21 reads the image of the detection area from the memory 38 among the images 38 acquired by the camera 12, and the position of the pattern light 32a as shown in FIG. 4C from this image. Extract 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 attitude angle calculation unit 22 reads data indicating the position of the pattern light 32a from the memory, calculates the distance and attitude angle of the vehicle with respect to the road surface from the position of the pattern light 32a, and stores it in the memory.

Proceeding to step S09, the ECU 13 detects feature points from the image of the detection area in the image 38, extracts feature points whose correspondence can be obtained between the information processing cycles before and after, and detects the position of the feature points on the image (U _{From i} and V _i ), the amount of change in distance and attitude angle and the amount of movement of the vehicle are calculated.

Specifically, first, the feature point detection unit 23 reads the image of the detection area from the memory among the images 38 acquired by the camera 12, detects the feature points on the road surface 31 from this image, and detects the image of each feature point The upper position (U _i , V _i ) is stored in 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 calculates the distance and attitude angle and the position (U _i , V _i ) on the image of the feature point. 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 the posture angle set in step S11 of the previous information processing cycle. The posture change amount calculation unit 24 stores the relative position (X _i , Y _i , Z _i ) of the feature point with respect to the camera 12 in the memory.

The posture change amount calculation unit 24 then calculates the position (U _i , V _i ) of the feature point on the image relative to the position of the feature point calculated in step S 09 of the previous information processing cycle (X _i , Y _i , Read Z _i ) from memory. The posture change amount calculation unit 24 uses the relative positions (X _i , Y _i , Z _i ) of the feature points and the positions (U _i , V _i ) on the image where correspondence can be obtained between the information processing cycles before and after. , Calculate the amount of change in distance and attitude angle. 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 Calculate the movement amount. The “amount of change in distance and attitude angle” and the “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 based on the conditions set in advance. 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.

  Thus, the self-position calculating apparatus according to the present embodiment can calculate the current 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.

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

  In step S901, the self-position calculating unit 26 determines whether the conditions required to maintain the starting point of the integration operation are satisfied. As this condition, for example, a condition that a feature point is normally detected, does not include an error factor, or a predetermined time or a predetermined number of cycles may not be set in advance. Such conditions may be other conditions as long as the error of the movement amount of the vehicle 10 is set so as not to be expanded by the integration calculation. If it is determined that the condition is satisfied (YES in step S901), the process proceeds to step S903. If it is determined that the condition is not satisfied (NO in step S901), the process proceeds to step S902.

  In step S902, the ECU 13 sets the current position of the vehicle as a starting point, and further sets the distance and attitude 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 attitude angle. Further, the integral calculation of the movement amount of the vehicle is newly started from the current position of the vehicle as a starting point.

  On the other hand, in step S903, the ECU 13 maintains the starting point of the integration operation currently set. When the starting point of the integration operation is set in this way, the process of setting the starting point of the integration operation in step S11 of FIG. 9 is ended.

  As described above in detail, in the self-position calculation apparatus according to this embodiment, the position of the shadow on the image due to the unevenness of the road surface is detected from the image acquired by the camera 12, and the position is determined based on the position of the shadow. Set the detection area. Then, the attitude angle of the vehicle with respect to the road surface is calculated from the position of the pattern light in the detection area, and the attitude change amount of the vehicle is calculated based on the time change of the feature point detected from the detection area. Thus, the detection area can be set on the image in consideration of the position of the unevenness of the road surface, so that the self position of the vehicle can be accurately calculated even if the unevenness is present on the road surface.

  Further, in the self-position calculation apparatus according to the present embodiment, the detection area is set excluding the area in the light source direction from the position of the shadow on the image. Thus, the detection area can be set excluding the area where the convex portion may exist, so that the vehicle's own position can be accurately calculated even if the road surface is uneven.

Second Embodiment
Next, a self-position calculation apparatus according to a second embodiment of the present invention will be described with reference to the drawings. The hardware configuration is the same as that of the first embodiment, and thus the detailed description is omitted.

  The detection area setting unit 20 according to the present embodiment determines the shadow by the convex portion of the road surface based on the change in the length of the shadow on the image. And it is different from 1st Embodiment that the detection area was set on the image based on the position of the shadow on the image by the convex part of a road surface.

  As shown in FIG. 11A, when the light source is present far from the convex portion 71, the shadow 73 formed by the convex portion 71 of the road surface becomes long. And as shown in FIG.11 (b), if a light source approaches the convex part 71, the shadow 73 will become short. That is, the shape of the shadow by the convex portion largely changes according to the movement of the vehicle. In particular, the change in the length in the light source direction is large.

  On the other hand, as shown in FIG. 12A, the shadow 77 produced by the recess 75 of the road surface does not extend so long even when the light source is present far from the recess 75. Then, as shown in FIG. 12B, even if the light source approaches the recess 75, the shadow 73 is only slightly shortened, and the length does not change significantly. That is, the shape of the shadow due to the recess does not change much like the shadow of the protrusion even when the vehicle moves. In particular, the change in length in the light source direction is small.

  Therefore, when the vehicle is traveling, the region of the shadow on the image is tracked, and the change in the shape of the shadow between the frames, in particular, the change in the length of the light source direction is larger than a preset threshold. Is determined to be the shadow of the convex portion, and is determined to be the shadow of the concave portion if it is less than the threshold value. Thereby, it can be determined whether the shadow on the image is a shadow by a convex portion or a shadow by a concave portion. The threshold may be set in advance by experiment in consideration of the installation positions of the pattern light projector 11, the camera 12, and the road surface light projector 14.

  When the shadow by the convex portion is thus determined, the detection area setting unit 20 sets the detection area based on the position of the shadow on the image by the convex portion of the road surface. Specifically, in the present embodiment, the detection area is set excluding the area of the shadow by the convex portion.

  For example, in the image shown in FIG. 13A, if it is determined that the shadows 61a and 61d are shadows due to the convex portion and the shadows 61b, 61c and 61e are estimated to be concave portions other than the convex portion, As shown in FIG. 13B, detection regions are set excluding the exclusion regions 81a to 81e. Since the exclusion areas 81a and 81d are shadow areas by the convex portions, they include both the shadow area by the convex portions and the area from the position of the shadow to the light source direction (the area surrounded by the broken line). On the other hand, since the excluded areas 81b, 81c, and 81e are shadow areas due to the recess, they are only the area from the shadow position to the light source direction (the area enclosed by the broken line) and do not include the shadow area due to the recess Range).

  Here, to explain the reason for excluding the shadow area due to the convex portion from the detection area, the shadow due to the convex part largely changes in shape, in particular the length in the light source direction, as the vehicle travels. Therefore, if the edge of the shadow by the convex portion is used to detect the feature point, it becomes an error factor when calculating the movement amount of the vehicle. Therefore, by excluding the shadow area due to the convex portion from the detection area, the error due to the convex portion of the road surface can be suppressed when detecting the feature point, and the self position of the vehicle can be accurately calculated.

  In addition, the detection area setting unit 20 may set the detection area based on the position of the shadow on the image due to the concave portion of the road surface. Specifically, the detection area is set excluding the area shaded by the recess.

  For example, in FIG. 13B described above, the detection area is set except for the shadow area due to the convex portion, but in FIG. 14 the detection area is further excluded from the detection area in FIG. Regions 91a to 91e are set (a range surrounded by a broken line). The excluded areas 91a and 91d are the same as the excluded areas 81a and 81d in FIG. 13B, but the excluded areas 91b, 91c and 91e are the shadows of the recessed areas in the excluded areas 81b, 81c and 81e in FIG. An area has been added. Therefore, the exclusion regions 91b, 91c, and 91e include both the shadow region by the recess and the region from the position of the shadow to the light source direction (a region surrounded by a broken line).

  Here, the reason for excluding the shadowed area due to the recess from the detection area will be described. In the technology of calculating the movement amount, it is premised that the road surface is flat also when calculating the distance and the attitude angle from the position of the pattern light. Therefore, it is not desirable to calculate the distance and the attitude angle from the pattern light irradiated to the concave portion such as a small crack or chip of asphalt. Since shadows are formed on the concave portions of the road surface, errors in the concave portions of the road surface can be suppressed when calculating the distance and the attitude angle from the pattern light if the shadow region having a high possibility of being concave portions is removed from the detection region. . Therefore, the area of the shadow due to the concave portion of the road surface is excluded from the detection area.

  In the present embodiment, the convex portion of the road surface is determined, and first, the region of the shadow by the convex portion is excluded from the detection region, and thereafter, the region of the shadow presumed to be a concave portion other than the convex portion is excluded. However, all shadow areas may be excluded from the detection area without determining the convex portion of the road surface.

  As described above in detail, in the self-position calculation apparatus according to the present embodiment, the shadow by the convex portion of the road surface is determined based on the change in the length of the shadow on the image. Thereby, since it is possible to separately exclude from the detection region the shadow region estimated by the convex portion and the shadow region estimated to be the concave portion other than the convex portion as necessary, it is possible to calculate the vehicle's own position with high accuracy.

  Further, in the self position calculation device according to the present embodiment, the detection area is set on the image based on the position of the shadow on the image by the convex portion of the road surface. Thereby, since the area of the shadow by the convex part can be excluded from the detection area, an error due to the convex part of the road surface can be suppressed when detecting the feature point, and the self position of the vehicle can be accurately calculated.

  Furthermore, in the self-position calculation apparatus according to the present embodiment, the detection area is set on the image based on the position of the shadow on the image due to the concave portion of the road surface. Thereby, since the area of the shadow by the recess can be excluded from the detection region, when calculating the distance and the attitude angle from the pattern light, the error due to the recess of the road surface can be suppressed, and the vehicle's own position can be accurately calculated.

Third Embodiment
Next, a self-position calculation apparatus according to a third embodiment of the present invention will be described with reference to the drawings. The hardware configuration is the same as that of the first embodiment, and thus the detailed description is omitted.

  The detection area setting unit 20 according to the present embodiment sets a first detection area on the image based on the position of the shadow on the image due to the unevenness of the road surface, and based on the position of the shadow on the image due to the convex portion on the road A first detection area is set on the image. The attitude angle calculation unit 22 calculates the distance and attitude angle of the vehicle to the road surface from the position of the pattern light in the first detection area, and the feature point detection unit 23 calculates a plurality of feature points on the road surface from the second detection area. To detect That is, in the present embodiment, the first detection area for calculating the distance and the attitude angle of the vehicle and the second detection area for detecting the feature point are set separately. It differs from the embodiment.

  For example, as shown in FIG. 15A, the first detection area for extracting the pattern light for calculating the distance and the attitude angle of the vehicle is set to an area excluding the exclusion areas 101a to 101e. When calculating the distance and attitude angle from the pattern light, it is premised that the road surface is flat. Therefore, since the excluded areas 101a and 101d are areas determined to be shadows by the convex portions, areas from the position of the shadow to the light source direction are set in order to exclude areas where the convex portions may exist. Further, since the excluded areas 101b, 101c, and 101e are areas determined to be shadows due to the concave portions, the shadow areas due to the concave portions are set in order to exclude the area where the concave portions exist. At this time, since the outer edge of the recess is brightened, it is preferable to set the exclusion areas 101b, 101c and 101e wider than the actual shadow area. Thus, when the exclusion regions 101a to 101e are set, the first detection regions are set to regions excluding the exclusion regions 101a to 101e.

  On the other hand, as shown in FIG. 15B, the second detection area for detecting the feature point is set to an area excluding the exclusion areas 103a and 103b. Since the excluded areas 103a and 103b are areas determined to be shadows by the convex portions, areas from the position of the shadow to the light source direction are set in order to exclude areas where the convex portions may exist. Furthermore, since the shadow by the convex portion is largely changed in shape due to the traveling of the vehicle, the shadow region by the convex portion is also included in the exclusion regions 103a and 103b. However, since the area determined to be a shadow due to the concave portion can be used for detection of feature points, it is not excluded and is used as a detection area. When the exclusion areas 103a and 103b are thus set, the second detection area is set to the area excluding the exclusion areas 103a and 103b.

  Thereafter, the attitude angle calculation unit 22 calculates the distance and attitude angle of the vehicle to the road surface from the position of the pattern light in the first detection area, and the feature point detection unit 23 calculates a plurality of features on the road surface from the second detection area. Detect points

  As described above in detail, in the self-position calculation apparatus according to the present embodiment, the first detection area is set on the image based on the position of the shadow on the image due to the unevenness of the road surface. A second detection area is set on the image based on the position of the shadow on the image. Then, the attitude angle of the vehicle with respect to the road surface is calculated from the position of the pattern light in the first detection area, and a plurality of feature points on the road surface are detected from the second detection area. As a result, the detection area suitable for calculating the attitude angle and the detection area suitable for detecting the feature point can be set respectively, so that both the error due to the concave and the convex due to the road surface can be suppressed. The self position can be calculated accurately. In particular, since it is possible to detect more pattern light and feature points from the image, it is possible to calculate the movement amount more robust to the road surface with few features and the unevenness of the road surface.

  Although the first to third embodiments of the present invention have been described above, it should not be understood that the statements and the drawings that form a part of this disclosure limit the present invention. Various alternative embodiments, examples and operation techniques will be apparent to those skilled in the art from this disclosure.

  In addition, although FIG. 2 showed the example which attached the camera 12 and the pattern light projector 11 to the front of the vehicle 10, you may install toward the side of the vehicle 10, back, and directly below. Furthermore, although a four-wheeled passenger vehicle is shown in FIG. 2 as an example of the vehicle 10 in the above-described embodiment, features on a road or wall of a road, such as a motorcycle, a lorry, or a special vehicle for transporting a construction machine, for example It is applicable to all the mobile bodies (vehicles) which can image a point.

10 vehicles 11 pattern light projectors (projectors)
12 Camera (imaging unit)
14 Road Surface Emitter (Sender)
Reference Signs List 20 detection area setting unit 21 pattern light extraction unit 22 attitude angle calculation unit 23 feature point detection unit 24 attitude change amount calculation unit 26 self position calculation unit 31 road surface 32a, 32b pattern light 51, 53, 71 convex portion 55, 61a, 61b , 61c, 61d, 61e, 73, 77 shadow areas 63a to 63e, 81a to 81e, 91a to 91e, 101a to 101e, 103a to 103e excluded area 75 concave portion Te feature point

Claims (7)

A projector for projecting at least pattern light onto the road surface around the vehicle;
An imaging unit which 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;
The position of the shadow on the image due to the unevenness of the road surface is detected from the image acquired by the imaging unit, and the image excluding the area where the unevenness may exist on the road based on the position of the shadow A detection area setting unit that sets a detection area on the upper side;
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 detection area of the image acquired by the imaging unit;
A feature point detection unit that detects a plurality of feature points on the road surface from the detection area of the image acquired by the imaging unit;
A posture change amount calculation unit that calculates a posture change amount of the vehicle based on time changes of a plurality of feature points on the road surface detected by the feature point detection unit;
A self position calculation unit that calculates the current position and attitude angle of the vehicle by adding the amount of attitude change to the initial position and attitude angle of the vehicle;
The self-position calculation apparatus characterized by having. The self position calculation apparatus according to claim 1, wherein
The detection position setting unit sets the detection position by excluding the region in the direction of the light projector from the position of the shadow on the image. It is a self-position calculation apparatus of Claim 1 or 2, Comprising:
The detection position setting unit sets the detection area on the image based on the position of the shadow on the image by the convex portion of the road surface. The self-position calculation apparatus according to claim 3,
The detection position setting unit sets a detection area on the image based on a position of a shadow on the image due to a concave portion of the road surface. The self-position calculation apparatus according to claim 3 or 4, wherein
The detection area setting unit sets a first detection area on the image based on the position of the shadow on the image due to the unevenness of the road surface, and based on the position of the shadow on the image due to the convex portion on the road Setting a second detection area on the image;
The attitude angle calculation unit calculates an attitude angle of the vehicle with respect to the road surface from the position of the pattern light in the first detection area,
The feature point detection unit detects a plurality of feature points on the road surface from the second detection region. It is a self-position calculation apparatus of Claim 1 or 2, Comprising:
The detection position setting unit determines a shadow by a convex portion of the road surface based on a change in length of a shadow on the image. A procedure for projecting at least pattern light from the projector mounted on the vehicle to the road surface around the vehicle;
A procedure for capturing an image by capturing an image of a road surface around the vehicle including an area where the pattern light is projected by an imaging unit mounted on the vehicle;
The control unit of the vehicle detects the position of the shadow on the image due to the unevenness of the road surface from the image acquired by the imaging unit, and the unevenness may exist on the road surface based on the position of the shadow Setting a detection area on the image excluding an area;
A procedure in which the control unit calculates an attitude angle of the vehicle with respect to the road surface from the position of the pattern light in the detection area of the image acquired by the imaging unit;
A procedure in which the control unit detects a plurality of feature points on the road surface from the detection area of the image acquired by the imaging unit;
A procedure in which the control unit calculates the amount of change in posture of the vehicle based on the time change of a plurality of feature points detected on the road surface;
A procedure in which the control unit calculates the current position and attitude angle of the vehicle by adding the amount of change in attitude to the initial position and attitude angle of the vehicle;
A self position calculation method characterized by including.