JP2015158417A - Self position calculation apparatus and self position calculation method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a self position calculation apparatus and a self position calculation method capable of accurately and stably estimating a vehicle moving amount irrespective of a road surface gradient.SOLUTION: A self position calculation apparatus comprises: a gradient-change calculation unit 36 calculating a change in a gradient of a road surface; an attitude-angle calculation unit 22 calculating an attitude angle of a vehicle with respect to the road surface from a position of pattern light acquired by a camera 12; an attitude-change-amount calculation unit 24 calculating an attitude change amount of the vehicle on the basis of temporal changes in a plurality of feature points on the road surface; and a self-position calculation unit 26 calculating a current position and the attitude angle of the vehicle by adding the attitude change amount to an initial position and the attitude angle of the vehicle. The self-position calculation unit 26 starts adding the attitude change amount to the current position of the vehicle and the attitude angle of the vehicle calculated by the attitude angle calculation unit 22 if the change in the gradient of the road surface is greater than a predetermined threshold.

Description

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

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

JP 2008-175717 A

  However, when the vehicle travels on a road surface with a gradient such as a hill, the positional relationship between the camera and the road surface changes, which causes a problem that it is impossible to measure the amount of movement with high accuracy. That is, when the movement amount is estimated based on the image captured by the camera, the posture angle of the camera with respect to the road surface changes due to the change in the gradient of the road surface. Therefore, if the amount of movement of the vehicle is estimated based on the image captured by this camera, a large error occurs in the estimated value of the amount of movement due to the change in the attitude angle, and high-precision measurement cannot be performed. .

  The present invention has been made to solve such a conventional problem, and an object of the present invention is to accurately and stably calculate the amount of movement of the vehicle regardless of the slope of the road surface. An object of the present invention is to provide a self-position calculation device and a self-position calculation method.

  In order to achieve the above object, the present invention obtains a vehicle attitude angle with respect to a road surface from a gradient change calculation unit that acquires a road surface gradient and calculates a change in the road surface gradient, and a pattern light position acquired by the imaging unit. A posture angle calculation unit to calculate, a posture change amount calculation unit to calculate a posture change amount of the vehicle based on time changes of a plurality of feature points on the road surface, and a posture change amount to an initial position and a posture angle of the vehicle. A self-position calculating unit that calculates the current position and the attitude angle of the vehicle by adding them. Then, when the change in the road surface gradient is larger than a predetermined threshold, the self-position calculation unit starts adding the posture change amount to the current vehicle position and the posture angle of the vehicle by the posture angle calculation unit at that time.

  According to the present invention, when the change in the road surface gradient is larger than the predetermined threshold, the addition of the posture angle change amount to the current vehicle position at that time and the posture angle of the vehicle by the posture angle calculation unit is started. . Therefore, even when the change rate of the road surface gradient is large, it is possible to prevent the calculation accuracy of the initial position and attitude angle of the vehicle from being lowered, and it is possible to calculate the movement amount of the vehicle with high accuracy and stability.

It is a block diagram which shows the structure of the self-position calculation apparatus which concerns on 1st Embodiment of this invention. It is explanatory drawing which shows the light projection pattern irradiated with the light projector mounted in a vehicle, and the imaging range by a camera. (A) is a figure which shows a mode that the position on the road surface irradiated with each spot light is calculated from the base line length Lb between a projector and a camera, and the coordinate (Uj, Vj) on the image of each spot light. (B) is an explanatory view showing a state in which the moving direction of the camera is obtained from the temporal change of the feature points detected from another area different from the area irradiated with the pattern light. (A) is the figure which shows the image of the pattern light which binarized the image acquired with the camera, (b) is the enlarged view, (c) is each spot light extracted by the pattern light extraction part It is a figure which shows the position of the gravity center. It is a schematic diagram for demonstrating the method of calculating the variation | change_quantity of a distance and a posture angle. (A) shows an example of an image acquired at time t, and (b) shows an example of an image acquired at time (t + Δt) when time Δt has elapsed from time t. It is explanatory drawing which shows a mode that a road surface gradient is calculated based on the inclination of a spot light, and a space | interval. It is a flowchart which shows the process sequence of the self-position calculation apparatus which concerns on embodiment of this invention. FIG. 9 is a flowchart showing a detailed processing procedure of the process shown in step S09 of FIG. 8 according to the first embodiment. It is a timing chart which shows the timing of the gradient change of a road surface, and starting point setting of the self-position calculation device concerning a 1st embodiment. It is a timing chart which shows the gradient of a road surface, and the timing of starting point setting of the self-position calculation device concerning a 2nd embodiment. FIG. 10 is a flowchart showing a detailed processing procedure of the process shown in step S09 of FIG. 8 according to the third embodiment. It is a timing chart which shows the gradient of a road surface, and the timing of starting point setting of the self-position calculation device concerning a 3rd embodiment.

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

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

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

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

  The camera 12 repeatedly captures images at a predetermined time interval and acquires a series of images (frames). The image data acquired by the camera 12 is transferred to the ECU 13 for each imaging cycle and stored in a memory (not shown) provided in the ECU 13.

  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 area of the road surface 31 that includes the pattern light. The projector 11 includes, for example, a laser pointer and a diffraction grating. As shown in FIGS. 2 to 4, the projector 11 diffracts the laser light emitted from the laser pointer with a diffraction grating, and as shown in FIG. 2 to FIG. 4, pattern light (pattern light consisting of a plurality of spot lights arranged in a matrix form) 32b, 32a). In the example shown in FIGS. 3 and 4, the pattern light 32a composed of 5 × 7 spot light is generated.

  FIG. 3A shows a state in which the road surface 31 is irradiated with the pattern light 32 a and the irradiation area is imaged by the camera 12. 3B irradiates the road surface 31 with the pattern light 32a, picks up an image of another region 33 different from the irradiated region with the camera 12, detects a feature point, and detects the feature point over time from the camera. 12 shows how 12 movement directions 34 are obtained.

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

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

  The pattern light control unit 27 controls the projection of the pattern light 32 a by the projector 11. For example, when the ignition switch of the vehicle 10 is turned on and the self-position calculating device 100 is activated, the pattern light control unit 27 starts projecting the pattern light 32a. Thereafter, the pattern light control unit 27 continuously projects the pattern light 32a until the self-position calculating device 100 stops. Alternatively, the light projection may be turned on and off repeatedly at predetermined time intervals. Alternatively, the pattern light 32a may be temporarily projected only when the change rate of the road surface gradient detected by the gradient change calculation unit 36 becomes large.

  The pattern light extraction unit 21 reads an image acquired by the camera 12 from the memory and extracts the position of the pattern light from the image. As shown in FIG. 3A, for example, a pattern light 32a composed of a plurality of spot lights in which the projectors 11 are arranged in a matrix is projected toward the road surface 31, and the pattern light 32a reflected by the road surface 31 is captured by the camera. 12 to detect. The pattern light extraction unit 21 performs a binarization process on the image acquired by the camera 12, and as shown in FIG. 4A and an enlarged view of FIG. 4B, the spot light Sp Extract only images. As illustrated in FIG. 4C, the pattern light extraction unit 21 calculates the position He of the center of gravity of each spot light Sp, that is, the coordinates (Uj, Vj) on the image of the spot light Sp, thereby calculating the pattern light. The position 32a is extracted. The coordinates are in units of pixels of the image sensor of the camera 12, and in the case of 5 × 7 spot light Sp, “j” is a natural number between 1 and 35. The coordinates (Uj, Vj) on the image of the spot light Sp are stored in the memory as data indicating the position of the pattern light 32a.

  The posture angle calculation unit 22 reads data indicating the position of the pattern light 32 a 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 32 a in the image acquired by the camera 12. For example, as shown in FIG. 3A, from the baseline length Lb between the projector 11 and the camera 12 and the coordinates (Uj, Vj) of each spot light on the image, The position on the road surface 31 irradiated with the spot light is calculated as a relative position with respect to the camera 12. Then, the posture angle calculation unit 22 calculates the plane type of the road surface 31 on 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 posture angle (normal line) Vector). 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. In other words, by calculating the distance and posture angle of the camera 12 with respect to the road surface 31, the distance between the road surface 31 and the vehicle 10 and the posture angle of the vehicle 10 with respect to the road surface 31 can be obtained. 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.

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

  The relative position (Xj, Yj, Zj) of each spot light with respect to the camera 12 often does not exist on the same plane. This is because the relative position of each spot light changes according to the asphalt unevenness exposed on the road surface 31. Therefore, a plane formula that minimizes the sum of squares of distance errors with respect to each spot light may be obtained by using the least square method. The distance and posture angle data calculated in this way are used by the self-position calculation unit 26 shown in FIG.

  The feature point detection unit 23 reads an image acquired by the camera 12 from the memory, and detects a feature point on the road surface 31 from the image read from the memory. The feature point detection unit 23 detects, 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" Kanazawa Satoshi, Kanaya Kenichi, "Extracting image feature points for computer vision," IEICE Journal, vol.87, no.12, pp. 1043-1048, Dec. 2004 "can be used.

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

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

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

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

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

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

  Note that the posture change amount calculation unit 24 does not use all of the feature points whose relative positions are calculated in the images detected at time t and time t + Δt, but is optimal based on the positional relationship between the feature points. A feature point may be selected. For example, Epipolar geometry (RI Hartley: “A linear method for reconstruction from lines and points,” Proc. 5th International Conference on Computer Vision, Cambridge, Massachusetts, pp.882-887 (1995) )) Can be used.

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

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

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

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

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

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

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

  Moreover, the gradient change calculation part 36 shown in FIG. 1 acquires the gradient data of the road surface which the vehicle 10 drive | works, and calculates the change rate of a road surface gradient from this gradient data. Here, the road surface gradient data can be acquired based on the road surface gradient data registered in the navigation system as an example and the current position data of the vehicle 10 acquired by GPS. The gradient change calculation unit 36 calculates a road gradient change rate R1 by performing differentiation on the acquired gradient data. Then, when the absolute value of the gradient change rate R1 exceeds a predetermined gradient change rate threshold Rth, it is determined that the road surface is steep. When it is determined that the slope is steep, the posture angle calculation unit 22 performs a process of calculating the position and posture angle based on the pattern light extracted by the pattern light extraction unit 21.

  In the present embodiment, the predetermined threshold Rth of the gradient change rate is set to 0.05 [rad / sec] in order to suppress the error in calculating the vehicle movement amount caused by the change in the road surface gradient to 5% or less. To do. That is, when the road gradient changes more than 0.05 [rad] while the vehicle travels for 1 second, it is determined that the gradient change rate R1 has reached a predetermined threshold Rth of the gradient change rate. The gradient change rate can be obtained from GPS data. It can also be obtained from the detection value of the on-vehicle gyro sensor.

  In addition, although the gradient change of a road surface is determined by the gradient change rate, it is not limited to this, For example, the absolute value of the gradient change within a fixed processing time may be sufficient.

[Operation of First Embodiment]
Next, as an example of a calculation method for calculating the self-position of the vehicle 10 from the image 38 (see FIG. 5) acquired by the camera 12, that is, the “vehicle distance and attitude angle” and the “vehicle movement amount”. An information processing cycle repeatedly executed by the ECU 13 will be described with reference to flowcharts shown in FIGS. The information processing cycle shown in FIG. 8 is repeated at the same time as the ignition switch of the vehicle 10 is turned on, the self-position calculation device 100 is started, and the self-position calculation device 100 is stopped.

  First, in step S01 of FIG. 8, the pattern light control unit 27 controls the projector 11 to project the pattern light 32a onto the road surface 31 around the vehicle. In FIG. 8, an example in which the pattern light 32a is continuously projected will be described. As described above, the pattern light 32a may be projected as necessary.

  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 aperture 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 pattern light extraction unit 21 reads the image 38 acquired by the camera 12 from the memory, and extracts the position of the pattern light 32a from the image 38 as shown in FIG. The pattern light extraction unit 21 stores the coordinates (Uj, Vj) on the image of the spot light Sp calculated as data indicating the position of the pattern light 32a in the memory. Further, in step S05, the posture angle calculation unit 22 reads data indicating the position of the pattern light 32a from the memory, calculates the distance and the posture angle from the position of the pattern light 32a, and stores them in the memory. The method for calculating the distance and the posture angle is as described above. Note that the distance and posture angle calculated by the posture angle calculation unit 22 are a new starting point when executing the integral calculation when the information processing cycle is the first time and when the conditions described later are satisfied, as will be described later. Set.

  Proceeding to step S07, the ECU 13 detects feature points (for example, uneven portions present on the asphalt) from the image 38, and extracts feature points that can be correlated between the previous information processing cycle and the current information processing cycle. Then, the amount of change in distance and posture angle is calculated from the position (Ui, Vi) of the feature point on the image. Furthermore, the movement amount of the vehicle 10 is calculated.

  Specifically, the feature point detection unit 23 first reads an image 38 acquired by the camera 12 from the memory, detects a feature point on the road surface 31 from the image 38, and determines the position (Ui, Vi) is stored in memory.

  The posture change amount calculation unit 24 reads the position (Ui, Vi) of each feature point on the image from the memory, and uses the distance and posture angle and the position (Ui, Vi) of the feature point on the image to the camera 12. The relative position (xi, yi, zi) of the feature point is calculated. Note that the posture change amount calculation unit 24 uses the distance and posture angle selected in step S09 (described later) of the previous information processing cycle. The posture change amount calculation unit 24 stores the relative position (xi, yi, zi) of the feature point with respect to the camera 12 in the memory.

  The posture change amount calculation unit 24 stores the position (Ui, Vi) of the feature point on the image and the relative position (xi, yi, zi) of the feature point calculated in step S07 of the previous information processing cycle. Read from. The posture change amount calculation unit 24 uses the relative position (xi, yi, zi) of the feature point and the position (Ui, Vi) on the image that can be correlated between the current information processing cycle and the current information processing cycle. Thus, the change amount of the distance and the posture angle is calculated. Furthermore, the movement amount of the vehicle 10 is calculated from the previous relative position (xi, yi, zi) of the feature point and the current relative position (xi, yi, zi) of the feature point. The “distance and change amount of posture angle” and “vehicle movement amount” calculated in this process are used in the process of step S11.

  Proceeding to step S09, the ECU 13 sets a starting point for calculating the distance and the attitude angle by integral calculation according to the change rate of the road surface gradient on which the vehicle 10 travels. The detailed processing procedure will be described later with reference to the flowchart shown in FIG.

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

  Thus, the self-position calculation apparatus 100 according to the present embodiment can calculate the self-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, a detailed procedure of step S09 in FIG. 8 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 of the previous information processing cycle, the process proceeds to step S909, and if not the first time, the process proceeds to step S903.

  In step S903, the gradient change calculation unit 36 detects the gradient change rate R1 of the road surface based on GPS information included in the navigation system mounted on the vehicle 10. In this process, the road surface gradient data obtained from the GPS is differentiated.

  In step S905, the gradient change calculation unit 36 determines whether or not the absolute value of the gradient change rate R1 of the road surface is greater than a predetermined threshold Rth of the preset gradient change rate. If the gradient change rate is larger than the predetermined threshold Rth (YES in step S905), the process proceeds to step S907. On the other hand, if the gradient change rate is equal to or less than the predetermined threshold Rth (NO in step S905), the process proceeds to step S913. Here, in the present embodiment, in order to suppress the error in calculating the movement amount of the vehicle 10 due to the influence of the road surface change to 5% or less, it is characterized when the road surface slope change rate becomes 0.05 [rad / sec]. Set the initial position of the point. That is, the predetermined threshold value of the gradient change rate is set to 0.05 [rad / sec].

  In step S907, the posture angle calculation unit 22 determines whether the distance and the posture angle are calculated based on the pattern light 32a extracted by the pattern light extraction unit 21. As a specific example, since the distance and the posture angle can be calculated when three or more spot lights are detected among the 35 spot lights irradiated from the projector 11, the three or more spot lights are detected. It is determined whether or not it has been detected. If the distance and the posture angle are calculated (YES in step S907), the process proceeds to step S909. On the other hand, when the distance and the posture angle are not calculated (NO in step S907), the process proceeds to step S911.

  In step S909, the ECU 13 starts from the current position of the vehicle 10, and further calculates the distance and posture angle calculated in step S05 (see FIG. 8) of the same information processing cycle, that is, the distance calculated from the position of the pattern light. And the attitude angle is set as the starting point of the integral calculation. Accordingly, the distance and posture angle are set to the distance and posture angle calculated from the pattern light 32a. A new integration operation is started from this distance and posture angle as the starting point. In addition, a starting point for calculating the movement amount of the vehicle 10 is newly set. That is, the integral calculation of the movement amount of the vehicle 10 is newly started from this position.

  In step S911, the ECU 13 sets the current position of the vehicle 10 as a starting point, and further sets the distance and the posture angle employed in the previous information processing cycle as starting points for integration calculation. That is, when the posture angle calculation unit 22 cannot calculate the posture angle of the vehicle from the pattern light, the posture angle calculated immediately before is used as the starting point of the integration calculation. Further, instead of the posture angle calculated immediately before, an initial value of a preset posture angle may be used. A new integration operation is started from this distance and posture angle as the starting point. In addition, a starting point for calculating the movement amount of the vehicle 10 is newly set. That is, the integral calculation of the movement amount of the vehicle 10 is newly started from this position. Thereafter, the process proceeds to step S11 in FIG.

  That is, when the road surface gradient change rate R1 is larger than a predetermined threshold value Rth of the gradient change rate (YES in step S905), the distance and posture angle of the vehicle 10 vary greatly, and accordingly, the camera 12 Since the distance and the posture angle fluctuate greatly, there is a high possibility that the position coordinates of the feature points cannot be obtained accurately. Therefore, in such a case, the posture change amount calculation unit 24 does not calculate the amount of change in the distance and posture angle, but the distance and posture angle calculated from the pattern light (step S909) or in the previous information processing cycle. The adopted distance and posture angle (step S911) are set as starting points. That is, the initial position, distance, and posture angle of the vehicle 10 are set to the current position, distance, and posture angle, and addition of the amount of change in the distance and posture angle is started. By doing so, it is possible to prevent a large error in the distance and the posture angle, and to prevent the calculation accuracy of the movement amount of the vehicle 10 from being lowered.

  On the other hand, in step S <b> 913 in FIG. 9, the ECU 13 maintains the current set starting point. When the slope change rate R1 of the road surface is equal to or less than a predetermined threshold Rth of the slope change rate (NO in step S905), a large error occurs in calculating the movement amount of the vehicle 10 even if the current starting point is maintained. Since it is judged not to do so, the starting point is maintained. Thereafter, the process proceeds to step S11 in FIG.

  The above processing is summarized as follows. When an image including a feature point existing on the road surface is captured using the camera 12 and the movement amount of the vehicle 10 (movement amount of six degrees of freedom) is calculated from the temporal change of the feature point, The condition is that the positional relationship with the road surface is constant. However, since the camera 12 is actually mounted on the vehicle 10, the relative position with respect to the road surface changes from moment to moment according to the traveling state of the vehicle 10 or the road surface environment. Accordingly, it is necessary to update the positional relationship between the camera 12 and the road surface, that is, the distance and the posture angle with the passage of time.

  Here, as a method of measuring the distance and the posture angle, there is a method using the pattern light 32a projected from the projector 11 (processing of S05 in FIG. 8). Therefore, if the distance and posture angle are always calculated using the pattern light 32a for each information processing cycle, and the temporal change of the feature point is detected, the movement amount of the vehicle 10 can be calculated. Self-position can be calculated. However, this method is effective if the road surface is flat and the pattern light 32a is reflected well, such as when the road surface is concrete, but the actual road surface is often an asphalt road surface with many irregularities. . In this case, rather than using the pattern light 32a, the amount of change in the distance and posture angle is calculated based on the temporal change of the feature point, and the change with respect to the distance and posture angle set as the starting point is calculated. It is possible to obtain the distance and the posture angle with higher accuracy by executing the process of sequentially adding the quantities (integral calculation).

  Therefore, in the present embodiment, the distance and posture angle are calculated using the pattern light 32a for the first time, and this is used as the starting point. Thereafter, the amount of change in the distance and posture angle based on the temporal change of the feature points is calculated. By calculating and sequentially adding the calculation results, the distance and the posture angle are updated to the latest ones. Further, when the change rate R1 of the road surface gradient of the road surface increases, the detection accuracy of the feature points decreases, and thus the accuracy of the distance and posture angle change amount calculated based on the feature points decreases. Therefore, under such conditions, that is, when the absolute value of the road surface slope change rate R1 exceeds a predetermined threshold value Rth of the slope change rate, the distance and posture angle using the pattern light 32a are used. By calculating the movement amount, the calculation accuracy of the self-position of the vehicle 10 is improved.

  Next, with reference to the timing chart shown in FIG. 10, the relationship between the change in the road surface gradient and the timing for setting a new starting point will be described. FIG. 10A is a diagram illustrating timings for setting the starting points of the distance and the attitude angle, FIG. 10B is a diagram illustrating a road surface gradient, and FIG. 10C is a diagram illustrating a road surface gradient change rate R1. The horizontal axis indicates time, and one scale indicates one information processing cycle.

  Now, as shown in FIG. 10B, a case is assumed in which the road changes from a flat road to an uphill at time t1, and then the road changes from an uphill to a flat road at time t6. Since the road surface gradient gradually increases at time t1, the gradient change rate R1 increases as shown in FIG. Thereafter, the peak is reached at time t2, and the gradient change rate R1 falls and becomes substantially zero at time t3. In other words, the slope is a certain slope. Then, the gradient change rate R1 rises in the negative direction at time t4, reaches a peak at time t5, and the gradient change rate R1 becomes substantially zero at time t6. That is, it becomes a flat road.

  In addition, a predetermined threshold value Rth of the gradient change rate is set for the gradient change rate R1, and when the absolute value of the gradient change rate R1 exceeds the predetermined threshold value Rth of the gradient change rate, that is, “R1> When “Rth” or “R1 <−Rth”, processing for setting the starting point of the distance and the posture angle is performed. That is, in each information processing cycle in the time periods T1 and T2 shown in FIG. 10A, the distance and posture angle by the pattern light are calculated and set as the starting points.

  As described above, the self-position calculating apparatus 100 according to the first embodiment of the present invention provides the following operational effects. That is, when the vehicle travels on a slope, the absolute value of the slope change rate R1 of the road surface increases, and when the distance and posture angle of the vehicle 10 change greatly due to this, the distance based on the time change of the feature point. And the integration calculation of the attitude angle is stopped. When the distance and posture angle using the pattern light are obtained, the distance and posture angle are set as a new starting point. That is, when the change rate of the road surface gradient is larger than a predetermined threshold value of the gradient change rate, the self-position calculation unit 26 determines the current position, distance, and attitude angle of the vehicle 10 at that time, the initial position of the vehicle 10, the distance, Set the posture angle, and start adding the distance and the change amount of the posture angle. If the distance and posture angle using the pattern light are not obtained, the distance and posture angle updated in the previous cycle are set as a new starting point. As a result, it is possible to prevent a large error from occurring in the calculation results of the distance and the posture angle. For this reason, the estimation accuracy of the moving distance of the vehicle 10 can be remarkably improved.

  Further, since the road surface gradient is acquired from the road surface gradient data registered in the map information obtained from the navigation system or the like, the change in the road surface gradient can be calculated with high accuracy and speed, and the amount of movement of the vehicle 10 Measurement can be performed with high accuracy.

  In the first embodiment, the case where GPS information is used as an example of road surface gradient detection used in the gradient change calculation unit 36 has been described. However, the present invention is not limited to this, and a vehicle-mounted stroke sensor or gyro sensor is used. It is also possible to calculate the road surface height based on the detection result of this and to determine the change in the road surface gradient from the amount of change in the road surface height.

  In this case, the distance to the road surface has changed by 5% or more compared to the state in which the vehicle 10 is stationary in order to suppress the error in calculating the movement amount of the vehicle 10 due to the influence of the vehicle behavior change to 5% or less. Sometimes the initial position of the feature point may be set. For example, when the height from the road surface to the camera 12 is 1.0 m, 0.05 m is the threshold value. The change in road surface height can be obtained by a stroke sensor or the like. When a gyro sensor or a stroke sensor is used, the road surface gradient can be detected without acquiring GPS information. Therefore, the road surface gradient not included in the GPS information can be detected, and the movement amount of the vehicle 10 can be measured with higher accuracy. It becomes possible.

  It is also possible to measure the gradient of the road surface using the inclination angle of the line segment connecting each spot light to the matrix included in the pattern light projected onto the road surface. As a method using pattern light, when pattern light as shown in FIG. 7 is obtained, a line segment connecting spot lights arranged in the row direction (Y-axis direction) and a line segment connecting spot lights arranged in the column direction. The inclination of (Q1 direction) (inclination angle when the X-axis direction is 0 deg) is calculated, and this inclination is larger than a predetermined angle (for example, 5 deg) as compared with the case of traveling on a flat road surface. In this case, it can be determined that the slope of the road surface has increased. That is, as the road surface gradient increases, the slope of the line segment Q1 with respect to the X-axis increases. Therefore, the road surface gradient can be estimated from the slope of the line segment Q1. Then, by calculating the change in the road surface gradient per unit time, it is possible to obtain the gradient change rate.

  As described above, the inclination angle of the line segment connecting the spot lights arranged in the column direction with respect to the line segment connecting the spot lights arranged in the row direction is calculated, and the road surface gradient is obtained based on the inclination angle, thereby obtaining higher accuracy. It is possible to calculate the rate of change of the road surface gradient.

  Further, when the spot light interval q1 changes, for example, when it changes by 5% or more as compared to when traveling on a flat road surface, it can be determined that the road surface gradient has increased. . That is, as the road surface gradient increases, the amount of change in the interval q1 increases, so the road surface gradient can be estimated from this interval q1.

  In the first embodiment, when the posture angle cannot be calculated by the posture angle calculation unit 22, the posture angle calculated immediately before is used, so that the position of the vehicle can be calculated without including a large error. . Furthermore, even if a preset initial value of posture angle (posture angle initial value) is used, the position of the vehicle can be calculated without including a large error.

[Description of Second Embodiment]
Next, a second embodiment of the self-position calculation apparatus 100 according to the present invention will be described. In the first embodiment described above, the distance and posture angle are calculated using the pattern light at the first time of the information processing cycle, and then the distance and posture angle are updated using the feature points. It has been described that the starting point of the distance and the posture angle using the pattern light is set when the change rate R1 becomes large (when the gradient change rate exceeds a predetermined threshold Rth).

  On the other hand, in the second embodiment, the distance and posture angle using the pattern light are calculated periodically in a preset cycle (first cycle), and the starting point is set to the distance and posture angle. This is different from the first embodiment. Specifically, for example, a distance and posture angle using pattern light is calculated every 10 information processing cycles, and processing for setting the starting point of the distance and posture angle is performed. Since the apparatus configuration is the same as that of FIG. 1 shown in the first embodiment, the description thereof is omitted.

  Hereinafter, the operation of the self-position calculation apparatus 100 according to the second embodiment will be described with reference to the timing chart shown in FIG. FIG. 11A is a diagram illustrating timing for calculating the distance and posture angle using the pattern light (timing for resetting the distance and posture angle), and FIG. 11B is a timing for periodically calculating the distance and posture angle. The figure shown, (c) is a figure which shows the gradient change rate R1 of the road surface. The horizontal axis indicates time, and one scale indicates one information processing cycle.

  As shown in FIG. 11B, when the gradient change rate R1 of the road surface is low (when the gradient does not change as shown in FIG. 11C), the distance and posture angle using the pattern light at a constant period. Is calculated. That is, the distance and posture angle calculated by the posture change amount calculation unit 24 shown in FIG. 1 are reset at a constant cycle, and the starting point is set by the distance and posture angle using the pattern light.

  Further, as shown in FIG. 11C, when the road changes from a flat road to an uphill at time t11, the gradient of the road surface gradually increases at time t11, so that the gradient change rate R1 increases. . Thereafter, the gradient change rate R1 reaches a peak, and the gradient change rate R1 decreases and becomes substantially zero at time t12. In other words, the slope is a certain slope. Then, the gradient change rate R1 rises in the negative direction at time t13, and the gradient change rate R1 becomes substantially zero at time t14. That is, it becomes a flat road.

  A predetermined threshold value Rth of the gradient change rate is set for the gradient change rate R1, and when the absolute value of the gradient change rate R1 exceeds the predetermined threshold value Rth of the gradient change rate, that is, “R1> When “Rth” or “R1 <−Rth”, processing for setting the starting point of the distance and the posture angle is performed. That is, the integration calculation is stopped in the time periods T11 and T12 shown in FIG. 11A, and the starting point of the distance and posture angle by the pattern light is set. Therefore, in addition to the periodically set time (timing shown in FIG. 11B), processing for setting the starting point of the distance and posture angle is performed at times T11 and T12.

  Then, by setting the starting point of the distance and the attitude angle as described above, it is possible to estimate the movement amount of the vehicle 10 more accurately as shown in FIG. In FIG.11 (d), the curve r1 shows the true numerical value of the moving amount of the vehicle 10, the curve r2 shows the moving amount of the vehicle 10 when the second embodiment is adopted, and the curve r3 is the second embodiment. The movement amount of the vehicle 10 when only periodic starting point setting is used without adopting the above is shown.

  As understood from FIG. 11D, when the movement amount calculation according to the second embodiment is adopted, the movement amount of the vehicle 10 is closer to a true value.

  In this way, the self-position calculating apparatus 100 according to the second embodiment can achieve the same effects as those of the first embodiment described above. Furthermore, in the second embodiment, the distance and posture angle are calculated using the pattern light in the first period, and the starting point is set based on the distance and posture angle. It is possible to estimate the movement distance with accuracy. Furthermore, even if an error occurs in the distance and the posture angle due to other factors, since the starting point is set in the first cycle, it is possible to avoid a large error in the moving distance of the vehicle 10.

[Description of Third Embodiment]
Next, a third embodiment of the present invention will be described. In the second embodiment described above, the distance and posture angle are calculated using the pattern light at a predetermined cycle, and the starting point of the distance and posture angle is set. Furthermore, the example in which the starting point is set based on the distance and posture angle using the pattern light when the road surface slope change rate R1 exceeds a predetermined threshold value Rth of the slope change rate has been described.

  On the other hand, in the third embodiment, the distance and posture angle are calculated using the pattern light in the first period in the normal time (when R1 <Rth), and the absolute gradient change rate R1 of the road surface is calculated. When the value exceeds a predetermined threshold Rth of the gradient change rate, the distance and posture angle are calculated using the pattern light in the second period shorter than the first period. Since the apparatus configuration is the same as that of FIG. 1 described in the first embodiment, description thereof is omitted.

  The self-position calculation apparatus 100 according to the third embodiment is executed by the process shown in FIG. Since the process of step S09 in FIG. 8 is different, the movement amount calculation starting point setting process will be described below with reference to the flowchart shown in FIG.

  The flowchart shown in FIG. 12 differs from FIG. 9 in that the processing of steps S906a and S906b is added. This will be described in detail below.

  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 of the previous information processing cycle, the process proceeds to step S909, and if not the first time, the process proceeds to step S903.

  In step S903, the gradient change calculation unit 36 detects the gradient change rate R1 of the road surface based on GPS information included in the navigation system mounted on the vehicle 10. In this process, the road surface gradient data obtained from the GPS is differentiated.

  In step S905, the gradient change calculation unit 36 determines whether or not the absolute value of the road gradient change rate R1 is greater than a predetermined gradient change rate threshold Rth. If the gradient change rate is greater than the predetermined threshold Rth (YES in step S905), the process proceeds to step S906b. On the other hand, if the gradient change rate is equal to or less than the predetermined threshold Rth (NO in step S905), the process proceeds to step S906b.

  In step S906a, the ECU 13 sets the distance and orientation calculation cycle using the pattern light to the first cycle. For example, 10 information processing cycles are set as the first cycle. Thereafter, the process proceeds to step S906c.

  In step S906b, the ECU 13 sets the distance and orientation calculation cycle using the pattern light to a second cycle shorter than the first cycle. For example, two information processing cycles are set as the second period. Thereafter, the process proceeds to step S906c.

  In step S906c, the ECU 13 determines whether it is a detection timing. If it is the detection timing, the process proceeds to step S907. If it is not the detection timing, the process proceeds to step S913. Since the process after step S907 is the same as that of FIG. 9, description is abbreviate | omitted.

  Next, with reference to the timing chart shown in FIG. 13, the relationship between the change in the road surface gradient and the setting of the distance and the posture angle will be described. FIG. 13A shows the timing of processing for setting the starting point of the distance and posture angle by changing the cycle (as the second cycle), and FIG. 13B shows the first cycle without changing the cycle. FIG. 4C is a diagram showing timing of processing for setting a starting point of a distance and a posture angle, and FIG. 5C is a diagram showing a slope change rate R1 of a road surface. The horizontal axis indicates time, and one scale indicates one information processing cycle.

  If the road surface gradient change rate R1 does not change greatly, as shown in FIG. 13B, the distance and posture angle are reset with the first period, for example, 10 information processing cycles as one period. Processing is performed. Specifically, the starting point setting process of the distance and the posture angle is performed at timings t21, t23, t25, and t27.

  Now, as shown in FIG. 13 (c), when the slope change rate R1 of the road surface increases and the absolute value thereof exceeds a predetermined threshold value Rth of the slope change rate, the starting point setting period is changed from the first period to the second period. (The processing of S906b in FIG. 12). As an example, the distance and posture angle starting point setting process is performed with two cycles of the information processing cycle as one cycle. That is, in the time zone in which R1 exceeds Rth between times t22 and t23 shown in FIG. 13C, the starting point setting period is changed to the second period as shown in FIG. 13A. .

  Similarly, since the absolute value of the gradient change rate R1 exceeds a predetermined threshold Rth of the gradient change rate between times t26 and t27 shown in FIG. 13C, the reset process cycle is the second in this time zone. The cycle is changed.

  Thus, according to the third embodiment, when the slope change rate R1 of the road surface does not increase, the starting point of the distance and the posture angle is set in the first cycle, and the absolute value of the slope change rate R1 is When the gradient change rate exceeds a predetermined threshold value Rth, the starting point of the distance and the posture angle is set in a second cycle shorter than the first cycle. Therefore, in a situation where a large error is expected to occur in the measurement of the moving distance of the vehicle 10 in which the gradient change rate R1 is large, the distance and posture angle calculation cycle using the pattern light is shortened. That is, the cycle for setting the starting point of the distance and the calculated angle is shortened. Accordingly, it is possible to prevent a large error from occurring in the calculation results of the distance and the posture angle, and it is possible to significantly improve the estimation accuracy of the moving distance of the vehicle 10.

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

  For example, FIG. 2 shows an example in which the camera 12 and the projector 11 are attached to the front surface of the vehicle 10, but the present invention is not limited to this, and the camera 12 and the projector 11 may be installed sideways, rearward, or directly below. . In each of the above-described embodiments, the case where it is mounted on the vehicle 10 has been described. However, the present invention can be applied to all moving bodies capable of imaging feature points on road surfaces or wall surfaces of roads such as motorcycles and construction machines.

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

Claims (7)

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 gradient change calculating unit for obtaining a road gradient and calculating a change in the road gradient;
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 acquired by the imaging unit;
An attitude change amount calculation unit that calculates an attitude change amount of the vehicle based on temporal changes of a plurality of feature points on the road surface of the image acquired by the imaging unit;
A self-position calculator that calculates the current position and posture angle of the vehicle by adding the amount of posture change to the initial position and posture angle of the vehicle;
With
When the change in the road surface gradient is larger than a predetermined threshold, the self-position calculation unit adds the posture change amount to the current position of the vehicle at that time and the posture angle of the vehicle by the posture angle calculation unit. A self-position calculation device characterized by starting. When the change in road surface gradient is equal to or less than a predetermined threshold, the self-position calculation unit is configured to calculate the current position of the vehicle at that time and the posture angle of the vehicle by the posture angle calculation unit in the first period. Start adding posture change amount,
When the change in the road surface gradient is larger than a predetermined threshold, the current position of the vehicle at that time and the attitude angle of the vehicle by the attitude angle calculation unit at a second period shorter than the first period. The self-position calculation apparatus according to claim 1, wherein addition of the posture change amount is started.   The self-position calculation device according to claim 1, wherein the gradient change calculation unit obtains a change in the gradient of the road surface from the amount of change in the road surface height.   The pattern light is composed of light arranged in a matrix direction, and the gradient change calculating unit obtains a change in the gradient of the road surface from the degree of inclination of the line segments of light arranged in the column direction with respect to the line segments of light arranged in the row direction. The self-position calculating device according to claim 1, wherein:   The self-position calculation unit uses a preset attitude angle initial value when the attitude angle calculation unit cannot calculate the attitude angle of the vehicle from the pattern light. The self-position calculation device according to item 1.   The self-position calculation unit uses the posture angle calculated immediately before when the posture angle calculation unit cannot calculate the posture angle of the vehicle from the pattern light. The self-position calculation device according to item 1. Projection procedure of projecting pattern light onto the road surface around the vehicle,
A procedure for obtaining an image by imaging a road surface around a vehicle including an area where the pattern light is projected;
A gradient change calculation procedure for obtaining a road gradient and calculating a change in the road gradient,
An attitude angle calculation procedure for calculating an attitude angle of the vehicle with respect to the road surface from the position of the pattern light in the image;
An attitude change amount calculating procedure for calculating an attitude change amount of the vehicle based on temporal changes of a plurality of feature points included in the image;
A self-position calculation procedure for calculating the current position and attitude angle of the vehicle by adding the attitude change amount to the initial position and attitude angle of the vehicle;
With
In the self-position calculation procedure, when the change in the road surface gradient is larger than a predetermined threshold, the attitude change with respect to the attitude angle of the vehicle calculated from the current position of the vehicle and the position of the pattern light at that time A self-position calculation method characterized by starting addition of a quantity.