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

Description

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

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

JP 2008-175717 A

  However, when the vehicle behavior amount, for example, the acceleration in the front-rear direction of the vehicle is large, the positional relationship between the camera and the road surface changes, which causes a problem that it is impossible to measure the movement amount with high accuracy. In other words, when estimating the amount of movement based on the image captured by the camera, if the amount of behavior of the vehicle is large, such as sudden start or stop, the posture angle of the camera with respect to the road surface changes due to the change in the posture of the vehicle. To do. 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 conventional problems, and the object of the present invention is to accurately and stably calculate the amount of movement of the vehicle even when the amount of behavior of the vehicle is large. It is an object of the present invention to provide a self-position calculation device and a self-position calculation method that can be performed.

To achieve the above object, the present invention includes a behavior amount detection unit that detects a vehicle behavior amount, and a posture angle calculation unit that calculates a posture angle of the vehicle with respect to a road surface from the position of pattern light acquired by the imaging unit. , A posture change amount calculation unit for calculating a posture change amount of the vehicle based on a temporal change of a plurality of feature points on the road surface, and an initial position of the vehicle, and detected by each of the posture calculation units at the initial position of the vehicle the attitude angle that is, that slide into adding continuously the posture variation, and a self-position calculating unit that calculates a current position and attitude angle of the vehicle. Then, when the behavior amount of the vehicle is greater than the threshold behavior amount, 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 behavior amount of the vehicle is larger than the threshold behavior amount, the addition of the posture angle change amount to the vehicle current posture and the posture angle calculation unit by the posture angle calculation unit at that time is started. . Therefore, even when the amount of behavior of the vehicle is large, it is possible to prevent the calculation accuracy of the initial position and attitude angle of the vehicle from being lowered, and the amount of movement of the vehicle can be calculated accurately and stably.

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 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 timing of the acceleration of a vehicle, and 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 timing of the acceleration of a vehicle, and 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, And a behavior amount detector 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 behavior amount of the vehicle 10 detected by the behavior amount detection unit 36 (for example, acceleration in the front-rear direction of the vehicle 10) increases.

  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 32 a is projected, that is, the distance and posture angle of the camera 12 with respect to the road surface 31 from the relative position of each spot light with respect to the camera 12. . 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 calculation method of “the amount of change in the distance and posture angle” and “the amount of movement of the vehicle” will be described.

The amount of change in the distance and the posture angle can be obtained as follows, for example. FIG. 6A shows an example of the first frame image 38 acquired at time t. As shown in FIGS. 5 and 6A, a case is considered in which, for example, relative positions (xi, yi, zi) of three feature points Te1, Te2, Te3 are calculated in the image 38, respectively. In this case, the plane G (see FIG. 6A) specified by the feature points Te1, Te2, Te3 can be regarded as a road surface. Therefore, the posture change amount calculation unit 24 can obtain the distance and posture angle of the camera 12 with respect to the road surface (plane G) from the relative positions (xi, yi, zi) of the feature points. 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 ′. From the camera model of the camera 12, the movement amount ΔL (see FIG. 5) of the camera 12 at the time Δt can be calculated, and the movement amount of the vehicle 10 can be calculated. Furthermore, the amount of change in distance and posture angle can also be calculated. For example, by solving simultaneous equations consisting of the following equations (1) to (4), the posture change amount calculation unit 24 can change the movement amount (ΔL) of the camera 12 (vehicle) and the change amounts of the distance and posture angle. Can be calculated. The following equation (1) is modeled as an ideal pinhole camera in which the camera 12 has no distortion or optical axis deviation, λi is a constant, and f is a focal length. The camera model parameters may be calibrated in advance.

  Note that the posture change amount calculation unit 24 does not use all of the feature points whose relative positions are calculated in the images detected at time t and time t + Δt, but is optimal based on the positional relationship between the feature points. A feature point may be selected. 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. The self-position calculation unit 26 uses the vehicle position when the distance and posture angle are calculated by the posture angle calculation unit 22 as a starting point (the initial position of the vehicle), and sequentially determines the movement amount of the vehicle 10 from the initial position. Addition (integral calculation) is performed to calculate the self-position of the vehicle 10. 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.

  Further, the behavior amount detection unit 36 illustrated in FIG. 1 acquires the behavior amount of the vehicle 10 from various sensors mounted on the vehicle 10. For example, the traveling speed of the vehicle 10 is detected by a vehicle speed sensor, and the traveling speed is differentially calculated to detect the longitudinal acceleration α1 (forward acceleration, backward acceleration) of the vehicle 10. As another behavior amount, it is also possible to acquire the moving speed in the left-right direction of the vehicle 10 detected by the yaw rate sensor and use this as the behavior amount. Alternatively, it is also possible to detect the moving speed in the vertical direction (vertical direction) of the vehicle 10 detected by the stroke sensor and use this as the behavior amount. When the acceleration α1 exceeds the preset threshold acceleration αth, it is determined that the behavior amount is excessive.

  For example, assuming that the calculation accuracy of the movement distance of the vehicle 10 is such that an error in calculating the movement amount of the vehicle 10 due to a change in the behavior of the vehicle 10 is 5% or less, the calculated value of the pitch angle or roll angle of the vehicle 10 is actually It is necessary to prevent the pitch angle from changing more than 0.05 [rad]. The pitch angle and the roll angle can be obtained by integrating the detection values of the on-vehicle gyro sensor.

  As an example, when the longitudinal acceleration of the vehicle 10 is used, the longitudinal acceleration when the pitch angle of the vehicle 10 changes by 0.05 [rad] compared to the pitch angle when the vehicle is stopped is obtained in advance. This acceleration is set as the threshold acceleration αth. The calculated value of the roll angle of the vehicle 10 is prevented from changing more than 0.05 [rad] relative to the roll angle when the vehicle is actually stopped.

  As another example, when the lateral speed of the vehicle 10 is used, the vehicle 10 when the roll angle of the vehicle 10 changes by 0.05 [rad] compared to the state in which the vehicle 10 is stopped. The lateral acceleration Yg [m / sec2] is calculated in advance, and the yaw rate γ [rad / sec] corresponding to the vehicle speed V [m / sec] as the acceleration can be set as a threshold value.

  As yet another example, when the vertical speed of the vehicle 10 is used, the vertical speed when the distance to the road surface changes more than 5% per second can be used as the threshold value. In this case, since the frequency of the change in vehicle behavior is assumed to be 1 [Hz] at the highest speed, if the height of the camera 12 from the road surface is 1.0 m, 0.05 [m / sec] is the vertical direction. Threshold speed.

  When it is determined that the behavior amount of the vehicle 10 is excessive, the posture angle calculation unit 22 calculates the distance and posture angle of the vehicle based on the pattern light extracted by the pattern light extraction unit 21. I do.

[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. 7 is repeatedly executed until the ignition switch of the vehicle 10 is turned on, the self-position calculating device 100 is started, and the self-position calculating device 100 is stopped.

  First, in step S01 of FIG. 7, the pattern light control unit 27 controls the projector 11 to project the pattern light 32a onto the road surface 31 around the vehicle. FIG. 7 illustrates an example in which the pattern light 32a is continuously projected. 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 behavior amount when 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, the detailed procedure of step S09 in FIG. 7 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 S <b> 903, the behavior amount detection unit 36 detects an acceleration α <b> 1 in the front-rear direction of the vehicle 10 based on travel speed data acquired from a vehicle speed sensor mounted on the vehicle 10. In this process, the traveling speed data is differentiated.

  In step S905, the behavior amount detection unit 36 determines whether or not the acceleration α1 of the vehicle 10 is greater than a preset threshold acceleration αth. If it is larger than the threshold acceleration αth (YES in step S905), the process proceeds to step S907. On the other hand, if the acceleration is equal to or less than the threshold acceleration αth (NO in step S905), the process proceeds to step S913.

  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. 7) 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. 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 acceleration α1 of the vehicle 10 is larger than the threshold acceleration αth (in the case of YES in step S905), the distance and posture angle of the vehicle 10 fluctuate greatly, and accordingly, the distance and posture angle of the camera 12 change. Since it fluctuates greatly, there is a high possibility that the position coordinates of 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 S913 in FIG. 8, the ECU 13 maintains the current set starting point. If the acceleration α1 of the vehicle 10 is equal to or less than the threshold acceleration αth (NO in step S905), it is determined that no large error occurs in the calculation of the movement amount of the vehicle 10 even if the current starting point is maintained. So keep the starting point. 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. 7). 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 acceleration α1 (behavior amount) of the vehicle 10 increases, the detection accuracy of the feature points decreases, and therefore 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 acceleration α1 of the vehicle 10 exceeds the threshold acceleration αth, the movement amount of the vehicle 10 is calculated using the distance and the attitude angle using the pattern light 32a. Thus, the calculation accuracy of the self-position of the vehicle 10 is improved.

  Next, the relationship between the change in the acceleration α1 of the vehicle 10 and the timing for setting a new starting point will be described with reference to the timing chart shown in FIG. FIG. 9A is a diagram showing the timing for setting the starting point of the distance and the posture angle, and FIG. 9B is a diagram showing the acceleration α1 of the vehicle. The horizontal axis indicates time, and one scale indicates one information processing cycle.

  Now, as shown in FIG. 9B, it is assumed that the vehicle starts to accelerate at a time t1 and reaches a certain speed, and then starts to decelerate and stops at a time t4. Since the acceleration of the vehicle 10 is started at time t1, the acceleration α1 increases as shown in FIG. 9B. Thereafter, the peak is reached at time t2, and the acceleration α1 decreases and becomes almost zero at time t3. That is, the speed is constant. Then, by starting deceleration at time t4, the acceleration α1 increases in the negative direction, reaches a peak at time t5, and the acceleration α1 becomes almost zero at time t6. That is, the vehicle 10 stops.

  When the threshold acceleration αth is set with respect to the acceleration α1 of the vehicle 10 and the acceleration α exceeds the threshold acceleration αth, that is, when “α1> αth” or “α1 <−αth”. Then, a process 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. 9A, 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 10 is accelerated or decelerated, the acceleration α1 increases, and when the distance and posture angle of the vehicle 10 change greatly due to this, the distance and posture angle based on the temporal change of the feature points. Stop the integral operation of. 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 acceleration (behavior amount) of the vehicle 10 is larger than the threshold acceleration (threshold behavior amount), the self-position calculating unit 26 determines the current position, distance, and posture angle of the vehicle 10 at that time as the initial position of the vehicle 10. And the distance and the posture angle are set, and the addition of the change amount of the distance and the posture angle is started. 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 movement amount of the vehicle 10 can be remarkably improved.

  Moreover, since the behavior amount detection unit 36 detects the acceleration in the front-rear direction of the vehicle as the behavior amount, the calculation accuracy of the distance and the posture angle can be improved. Furthermore, the estimation accuracy of the moving distance of the vehicle 10 can be remarkably improved.

  In the first embodiment, as an example of the behavior amount of the vehicle 10, the case where the longitudinal acceleration α1 is used is taken as an example. In the present invention, as another behavior amount, as described above, the lateral movement speed or the vertical movement speed can be used as the behavior amount.

  That is, when the lateral speed of the vehicle 10 is used, the lateral acceleration of the vehicle 10 when the roll angle of the vehicle 10 changes by 0.05 [rad] compared to the state where the vehicle 10 is stopped. Yg [m / sec2] is calculated in advance, and the yaw rate γ [rad / sec] corresponding to the vehicle speed V [m / sec] that becomes the acceleration can be set as a threshold value. Therefore, the behavior amount detection unit 36 can improve the calculation accuracy of the distance and the posture angle by detecting the moving speed of the vehicle in the left-right direction as the behavior amount. Furthermore, the estimation accuracy of the moving distance of the vehicle 10 can be remarkably improved.

  When the vertical movement amount is used, the vertical movement speed is obtained by differentiating the vertical movement distance data detected by the stroke sensor. At this time, in order to suppress an error in calculating the amount of movement of the vehicle 10 within 5%, a speed at which the distance from the road surface to the camera 12 (the height of the camera 12 with respect to the road surface) changes by 5% per second is a threshold value. Is desirable. For example, when the distance from the road surface to the camera 12 is 1 m, the threshold moving speed (threshold behavior amount) may be set to 0.05 [m / s].

  Thus, when the vehicle 10 moves in the vertical direction and the movement speed reaches the threshold movement speed, the distance and posture angle using the pattern light are set as new base points, and the movement amount of the vehicle 10 Thus, the estimation accuracy of the moving distance of the vehicle 10 can be remarkably improved.

  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 thereafter 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 acceleration α1 increases (when the threshold acceleration αth is exceeded).

  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 calculating apparatus 100 according to the second embodiment will be described with reference to the timing chart shown in FIG. FIG. 10A shows a timing for calculating the distance and posture angle using the pattern light (timing for resetting the distance and posture angle), and FIG. 10B shows a timing for periodically calculating the distance and posture angle. FIG. 2C is a diagram showing the acceleration α1 in the front-rear direction of the vehicle 10. The horizontal axis indicates time, and one scale indicates one information processing cycle.

  As shown in FIG. 10B, when the acceleration α1 of the vehicle 10 is small (unless the acceleration α1 changes unlike FIG. 10C), the distance and posture angle using the pattern light at a constant cycle. Is calculated. That is, the distance and posture angle calculated by the self-position calculation unit 26 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.

  As shown in FIG. 10C, when the vehicle 10 starts to accelerate at time t11, the acceleration α1 gradually increases at time t11. Thereafter, the acceleration α1 reaches a peak, and the acceleration α1 decreases and becomes almost zero at time t12. That is, the speed is almost constant. Then, at time t13, the acceleration α1 increases in the negative direction, and at time t14, the acceleration α1 becomes almost zero. That is, the vehicle 10 stops.

  When the threshold acceleration αth is set with respect to the acceleration α1 and the acceleration α1 exceeds the threshold acceleration αth, that is, when “α1> αth” or “α1 <−αth”, the distance and A process for setting the starting point of the posture angle is performed. That is, the integration calculation is stopped in the time periods T11 and T12 shown in FIG. 10A, 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. 10B), processing for setting the starting point of the distance and the posture angle is performed at times T11 and T12.

  Then, by setting the starting point of the distance and the posture angle as described above, the movement amount of the vehicle 10 can be estimated more accurately as shown in FIG. In FIG. 10 (d), a curve r1 indicates the true value of the movement amount of the vehicle 10, a curve r2 indicates the movement amount of the vehicle 10 when the second embodiment is adopted, and a curve r3 indicates 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. 10D, when the movement amount calculation according to the second embodiment is employed, 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. That is, when the acceleration (behavior amount) in the front-rear direction of the vehicle 10 is large, the starting point of the distance and the posture angle is set. Therefore, even when the behavior amount is large, the movement amount of the vehicle 10 can be calculated with high accuracy. . 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, so the movement amount of the vehicle 10 includes a large error. Can be prevented.

[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 by the distance and the posture angle using the pattern light when the acceleration α1 exceeds the threshold acceleration αth has been described.

  On the other hand, in the third embodiment, calculation of the distance and posture angle using the pattern light is executed in the first period in the normal time (when α1 <αth), and the acceleration α1 exceeds the threshold acceleration αth. In such a case, the calculation of the distance and the posture angle using the pattern light is executed in the second cycle shorter than the first cycle. 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. 7 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. 11 is different from FIG. 8 in that the processes of steps S906a and S906b are 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 behavior amount detection unit 36 obtains vehicle speed data detected by a vehicle speed sensor mounted on the vehicle 10, and performs a differential operation on the vehicle speed data to obtain the longitudinal acceleration α1 of the vehicle 10. To detect.

In step S905, the behavior amount detection unit 36 determines whether or not the acceleration α1 (behavior amount) is larger than a preset threshold acceleration αth (threshold behavior amount). Then, if the threshold motion amount is larger than (in step S905 YES), the process proceeds to step S906 a. On the other hand, if the amount is equal to or less than the threshold behavior amount (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. 8, description is abbreviate | omitted.

  Next, with reference to the timing chart shown in FIG. 12, the relationship between the change in the road surface gradient and the setting of the distance and the posture angle will be described. FIG. 12A 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. 12B shows the first cycle without changing the cycle. FIG. 4C is a diagram showing the timing of processing for setting the starting point of the distance and the posture angle, and FIG. The horizontal axis indicates time, and one scale indicates one information processing cycle.

  Now, when the acceleration α1 of the vehicle 10 is small, as shown in FIG. 12B, the distance and posture angle reset processing is performed with the first period, for example, 10 information processing cycles as one period. Is called. 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. 12C, when the acceleration α1 increases and exceeds the threshold acceleration αth, the starting point setting period is changed from the first period to the second period (the process of S906b in FIG. 11). ). 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 α1 exceeds αth between times t22 and t23 shown in FIG. 12C, the starting point setting cycle is changed to the second cycle as shown in FIG. .

  Similarly, since the acceleration α1 in the negative direction is lower than the threshold acceleration −αth between times t26 and t27 shown in FIG. 12C, the reset processing cycle is changed to the second cycle in this time zone. The

  Thus, according to the third embodiment, when the longitudinal acceleration α1 of the vehicle 10 does not increase, the starting point of the distance and posture angle is set in the first period, and the acceleration α1 is set to the threshold acceleration αth. Is exceeded, the starting point of the distance and the posture angle is set in the 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 where the acceleration α1 is large, the calculation period of the distance and posture angle using the pattern light is shortened. That is, the cycle for setting the starting point of the distance and the calculated angle is shortened. As a result, 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.

  In the third embodiment, the longitudinal acceleration of the vehicle 10 is described as an example of the behavior amount of the vehicle 10, but the present invention is not limited to this, and the lateral speed of the vehicle 10 or the vehicle It is also possible to use 10 vertical movement speeds as the behavior amount. Furthermore, the behavior amount of the vehicle 10 can be obtained by combining two or more of the longitudinal acceleration, the lateral movement speed, and the vertical movement speed.

  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 Behavior amount detection part 100 Self position calculation apparatus

Claims (8)

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 behavior amount detection unit for detecting the behavior amount of the vehicle;
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;
By continuously adding the posture change amount to the initial position of the vehicle and the posture angle detected by the posture calculation unit at the initial position of the vehicle, the current position and posture angle of the vehicle A self-position calculating unit for calculating
With
When the behavior amount of the vehicle is larger than the threshold behavior amount, 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 the operation. When the vehicle behavior amount is less than or equal to the threshold behavior amount, 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 vehicle behavior amount is larger than the threshold behavior amount, the current position of the vehicle at that time and the posture angle of the vehicle by the posture angle calculation unit at a second cycle shorter than the first cycle. The self-position calculation apparatus according to claim 1, wherein addition of the posture change amount is started. The self-position calculating device according to claim 1, wherein the behavior amount detection unit detects an acceleration in the front-rear direction of the vehicle as the behavior amount . The self-position calculating apparatus according to claim 1, wherein the behavior amount detection unit detects a moving speed of a vehicle in a left-right direction as the behavior amount .   The self-position calculating device according to claim 1, wherein the behavior amount detection unit detects a moving speed of a vehicle in a vertical direction as the behavior amount.   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 vehicle behavior detection procedure for detecting a vehicle behavior amount;
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;
By continuously adding the posture change amount to the initial position of the vehicle and the posture angle detected by the posture calculation unit at the initial position of the vehicle, the current position and posture angle of the vehicle Self-position calculation procedure for calculating
With
When the vehicle behavior amount is larger than the threshold behavior amount, the self-position calculation procedure is configured to change the posture with respect to the vehicle posture angle 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.