JP2012122734A - Moving body position estimation device and moving body position estimation method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a moving body position estimation device and a moving body position estimation method capable of estimating the position of a moving body even when the appearances of feature points detected from different places change from one detection place to another.SOLUTION: The moving body position estimation device includes a rising line calculation unit 16 for calculating a rising line rising at a preset angle from a bent part plane in the bent part where linear light projected into the imaging range of a peripheral image imaged by an imaging unit 2 is bent, a feature point collation unit 20 for collating a bent part after the change of the imaging range with a bent part before the change of the imaging range based on the rising line calculated by the rising line calculation unit 16 and a distance from the imaging unit 2 to the feature point, and a moving body position estimation unit 22 for estimating the position of a vehicle V based on the change amount of a position between the bent part before the change of the imaging range and the bent part after the change of the imaging range collated by the feature point collation unit 20.

Description

  The present invention relates to a mobile object position estimation device and a mobile object position estimation method for estimating the position of a mobile object such as a vehicle.

As an apparatus for estimating the position (self position) of a moving body such as a vehicle, there is an apparatus described in Patent Document 1, for example.
The moving body position estimation apparatus described in Patent Document 1 acquires a luminance image of the front field of view of a moving body, and for feature points between luminance images for at least two consecutive frames of the acquired luminance images. The position of the moving body is calculated based on the displacement amount of the position.

JP 2002-048513 A

  The moving body position estimation apparatus described in Patent Literature 1 includes a procedure for detecting a feature point in a luminance image and measuring a relative distance between the detected feature point and a camera that has acquired the luminance image. However, since the appearance of the feature point depends on the detection position for detecting the feature point, when the feature point is detected from different locations, the appearance of the feature point changes at each location. Such a change in the appearance of the feature point is particularly noticeable when the feature point is a feature point on a plane arranged at an angle close to parallel to the radiation direction from the camera center. Therefore, it is difficult to detect the feature points detected on the building wall or the like on the roadside even if the location where the feature points are detected is slightly shifted.

For this reason, feature points detected from different places are inappropriate for use in position calculation, and there is a possibility that it may be difficult to estimate the position.
The present invention has been made paying attention to the above problems, and estimates the position of a moving object even when the appearance of feature points detected from different locations changes at each detection location. It is an object of the present invention to provide a movable body position estimation apparatus and a movable body position estimation method that can be used.

  In order to solve the above-described problem, the present invention changes the imaging range from the captured surrounding image in a state where linear light is projected into the imaging range of the surrounding image of the moving object continuously captured by the imaging unit. A feature point that is a point that can be followed is detected. Then, the distance from the imaging unit to the feature point is calculated, and further, the bending angle that is the bending angle of the bending portion that is the portion where the linear light detected from the feature point and its surroundings is bent is calculated. From this bend angle, a bend plane that is a plane where the bend exists is defined. In addition to this, a rising line that rises at a preset angle from the bent portion plane in the bent portion is calculated, and according to a change in the imaging range, based on the distance and the rising line between the imaging portion and the bent portion, The bent part before the imaging range is changed is compared with the bent part after the imaging range is changed. Further, the position of the moving body is estimated based on the displacement amount of the collated position of both bent portions.

  ADVANTAGE OF THE INVENTION According to this invention, it becomes possible to estimate the position of a mobile body based on the bending part plane in which a bending part exists, and the rising line rising from a bending part plane. For this reason, it is possible to estimate the position of the moving object even if the appearance of the feature point changes at each detection position due to the change in the detection position of the feature point accompanying the movement of the moving object.

It is a figure showing the schematic structure of the vehicles provided with the mobile object position estimating device of a first embodiment of the present invention. It is a figure which shows schematic structure of the moving body position estimation apparatus of 1st embodiment of this invention. It is a figure which shows the positional relationship with an imaging part, a light projection part, and a light projection plane. It is a figure which shows the relationship between a captured image and the real world. It is a figure which shows the normal line direction and the allowable range of a normal line direction. It is a flowchart which shows a part of operation | movement which a moving body position estimation apparatus performs. It is a flowchart which shows a part of operation | movement which a moving body position estimation apparatus performs.

Embodiments of the present invention will be described below with reference to the drawings.
(First embodiment)
Hereinafter, a first embodiment of the present invention (hereinafter referred to as the present embodiment) will be described with reference to the drawings.
(Constitution)
FIG. 1 is a diagram illustrating a schematic configuration of a vehicle V including a moving body position estimation apparatus 1 according to the present embodiment.
As shown in FIG. 1, the moving body position estimation apparatus 1 is provided in a vehicle V that is a moving body. In other words, in the present embodiment, a target including the moving body position estimation apparatus 1 is a vehicle (automobile).
FIG. 2 is a diagram illustrating a schematic configuration of the moving object position estimation apparatus 1.
As illustrated in FIG. 2, the moving body position estimation apparatus 1 includes an imaging unit 2, a light projecting unit 4, and an arithmetic processing unit 6.

The imaging unit 2 is formed using a camera such as an infrared camera, for example, and continuously captures surrounding images of the vehicle V (moving body) at a preset time interval t [s].
Further, the imaging unit 2 outputs an information signal including the captured surrounding image to the arithmetic processing unit 6. Specifically, an information signal including the captured surrounding image is output to a feature point detection unit 8, a distance calculation unit 10, a feature point matching unit 20, and a moving body position estimation unit 22 described later.

In the present embodiment, the configuration of the imaging unit 2 is configured to continuously capture images in front of the vehicle V among the surrounding images of the vehicle V. Therefore, in the present embodiment, a case will be described in which the imaging unit 2 is disposed at a position where an image of the vehicle V in the front-rear direction of the vehicle V can be captured, for example, near the front window in the vehicle interior of the vehicle V.
The light projecting unit 4 is formed by using a device capable of projecting linear light, such as a laser irradiator, and projects linear light (line-shaped light) within the imaging range of the imaging unit 2. To do.

Further, the light projecting unit 4 outputs an information signal including a state where linear light is projected to the arithmetic processing unit 6. Specifically, an information signal including a state where linear light is projected is output to the bent portion detection unit 12 described later.
In the present embodiment, the configuration of the imaging unit 2 is configured to continuously capture images in front of the vehicle V. Accordingly, the configuration of the light projecting unit 4 is changed to linear light in front of the vehicle V. It is assumed that the light is projected. Therefore, in the present embodiment, the light projecting unit 4 is placed at a position where linear light can be projected forward in the vehicle front-rear direction of the vehicle V, such as in the vicinity of the imaging unit 2, for example, near the front window in the vehicle interior of the vehicle V. The case where it arrange | positions is demonstrated.

Here, when linear light is projected from the light projecting unit 4, the region through which the linear light passes becomes a plane (hereinafter, sometimes referred to as “light projection plane”).
In this embodiment, as shown in FIGS. 2 and 3, the positional relationship between the imaging unit 2 and the light projection plane is set to a positional relationship that is separated by a preset distance d1. As described above, in the present embodiment, the configuration of the imaging unit 2 and the light projecting unit 4 is configured to continuously capture images in front of the vehicle V, and accordingly, the imaging unit 2 and the light projecting unit. The distance d1 to 4 is the distance in the vehicle width direction. FIG. 3 is a diagram illustrating a positional relationship among the imaging unit 2, the light projecting unit 4, and the light projecting plane.

In the present embodiment, the attachment angle θ of the imaging unit 2 is taken as an example, and the distance from the imaging unit 2 to the object existing in front of the vehicle V is within the range of 2 to 10 [m], and the error 3 When it is required to measure in [%], it is set within the range of -70 to -130 [°].
This is because a condition in which a 3% change in distance relative to the distance from the imaging unit 2 to an object in front of the vehicle V appears as a change in an image of 1 [pixel] or more on the projection plane. This is because the distance from the projection plane is 1.0 [m] or more. This condition is applied when the positions of 2 [m] and 10 [m] are imaged from the imaging unit 2.

The arithmetic processing unit 6 includes a feature point detection unit 8, a distance calculation unit 10, a bending part detection unit 12, a bending angle calculation unit 14, a rising line calculation unit 16, a map storage unit 18, a feature point A collation unit 20 and a moving body position estimation unit 22 are provided.
The feature point detection unit 8 detects a feature point from a surrounding image (an image ahead of the vehicle V) captured by the imaging unit 2. Here, the feature point is a point that can be followed when the imaging range is moved with the movement of the vehicle V, for example. Further, the feature point detection unit 8 outputs an information signal including the detected feature point to the bent portion detection unit 12.

Here, the feature point is generally a point having a luminance difference in the vertical direction and the horizontal direction in the surrounding image captured by the imaging unit 2 as compared with other points. As a feature point detection method, a known SURF (Speeded Up Robust Features), SIFT (Scale-Invariant Feature Transform), or the like can be used.
In addition, the feature point generation factor greatly affects the change in the appearance of the feature point when the observation position of the feature point changes with the movement of the vehicle V or the like. For this reason, in the present embodiment, a feature point when the observation position changes is observed, and the observed feature point is stored in the map storage unit 18.
The feature points include a texture feature point where a texture on a plane is observed and a structure feature point generated at the boundary of an object existing at a different distance.

When determining whether the detected feature point is a texture feature point or a structure feature point, the feature point and the surrounding geometric structure are detected. Here, the geometric structure is an element that determines the generation factor of the feature point observed from the observation position (imaging unit 2).
The method of detecting the geometric structure of the feature point is as follows. First, the height of the linear light projected from the light projecting unit 4 in the surrounding image captured by the image capturing unit 2 is measured. Measure the distance from the target to the imaging target. Then, by confirming the continuity of the straight line connecting the imaging unit 2 and the imaging target, the feature point and the surrounding geometric structure are confirmed.

In this case, if the continuity of the straight line connecting the imaging unit 2 and the imaging target is maintained, the feature point and the periphery thereof are continuous planes. Therefore, when the continuity of the straight line connecting the imaging unit 2 and the imaging target is maintained, the feature point is a texture feature point on a plane.
On the other hand, the continuity of the straight line connecting the imaging unit 2 and the imaging target is not maintained, and when it is bent or cut, the feature point and its periphery are not a continuous plane. Therefore, when the continuity of the straight line connecting the imaging unit 2 and the imaging target is not maintained, the feature point is a structural feature point formed at the boundary of an object existing at a different distance.

The distance calculation unit 10 calculates the distance from the imaging unit 2 to the feature point, and outputs an information signal indicating the calculated distance to the feature point matching unit 20.
In the present embodiment, the distance calculation unit 10 observes where the linear light projected from the light projecting unit 4 is reflected in the image captured by the imaging unit 2, thereby causing the linear light from the imaging unit 2. The case where the distance to the place where the light is projected is measured will be described. However, the method for calculating the distance from the imaging unit 2 to the feature point is not limited to this.
Hereinafter, a specific procedure for calculating the distance from the imaging unit 2 to the feature point will be described.
As shown in FIG. 3, the position of the linear light in the captured image is relative to the light projecting unit 4 with respect to the range through which the linear light passes (light projection plane π = (0 0 1 0) ^T ). When the imaging unit 2 is installed as the distance d1, the following equation (1) changes according to the distance d1.

Here, “x” in the above equation (1) is a value represented by x = (x y w) ^T , and represents a point on the captured image in Homography coordinates.
Further, “X” in the above formula (1) is a value represented by X = (X Y Z 1) ^T , and represents a point on a three-dimensional space in Homography coordinates.
In addition, “k” in the above equation (1) is a calibration matrix and is represented by the following equation (2).

Here, “f” in the above formula (2) is a focal length on the captured image, and “ux” and “uy” are offsets of the center of the captured image.
Further, “M _L ” in the above formula (2) is a camera matrix based on the light projection plane, and is represented by the following formula (3).

Here, “R” in the above equation (3) is a rotation matrix representing the attitude of the imaging unit 2 in the world coordinate system.
Further, “C” in the above equation (3) is a value representing the position (coordinates) of the imaging unit 2.
Here, when the rotation direction of the imaging unit 2 is limited to the displacement in the Z direction with respect to the projection plane and the rotation around the x axis, the above “R” and “C” are expressed by the following equations (4) and (5). ).

  Here, since the condition in which the projected linear light is irradiated is always Z = 0, the point on the light projection plane can be expressed as X = (X Y 0 1). At this time, the position x = (x y w) on the captured image is a value represented by the following equation (6).

Here, “x” in the above equation (6) is the position of the linear light on the captured image, and “f” is the focal length on the captured image. Note that “x” and “f” in the above equation (6) are both known or observable values.
In addition, “ux” and “uy” in the above equation (6) are offsets of the captured image (the captured image generally has an upper left as the origin, and thus it is necessary to set the offset), and “d2 "Is a distance from the light projection plane to the imaging unit 2. Note that “ux”, “uy”, and “d2” in Equation (6) are all known or observable values.

Therefore, y (vertical position) of the point on the captured image is determined only by the distance Y from the camera. Therefore, linear light is emitted by measuring the position of the linear light on the captured image. It is possible to confirm the distance between the object and the imaging unit 2. Thereby, it is possible to calculate the distance from the imaging unit 2 to the feature point.
In the above description, direct measurement by light projection is described as a method for calculating the distance from the imaging unit 2 to the feature point. However, a method for calculating the distance from the imaging unit 2 to the feature point is described here. It is not limited to. That is, for example, using two or more images taken from different positions, “Motion Stereo” that identifies the position in the real space from the change in the position of the feature points in these images on the screen. You may use the method called.

As described above, in the present embodiment, as described above, the distance d1 between the imaging unit 2 and the light projecting unit 4 is set, thereby measuring the distance from the imaging unit 2 to the object existing in front of the vehicle V. It becomes possible to do.
The bent portion detection unit 12 is a portion where the linear light projected by the light projecting unit 4 is bent from the feature point detected by the feature point detection unit 8 and its surroundings in the surrounding image captured by the imaging unit 2. The bent part which is is detected. Further, the bent part detection unit 12 outputs an information signal including the detected bent part to the bending angle calculation unit 14 and the distance calculation unit 10.

Hereinafter, a specific procedure for detecting a feature point detected by the feature point detection unit 8 and a bent portion thereof will be described.
When the feature point detection unit 12 confirms the feature point detection unit 8 and the continuity of the straight line connecting the imaging unit 2 and the imaging target is not maintained, the feature point is a structural feature point. Exit.
On the other hand, the bent portion detection unit 12 maintains the continuity of the straight line connecting the imaging unit 2 and the imaging target confirmed by the feature point detection unit 8, and the feature point is a texture feature point on a plane. Performs the following processing.

Specifically, when the feature point is a texture feature point on a plane, the bent portion detection unit 12 obtains the slope of the straight line of the linear light.
Here, as shown in FIG. 4, the slope of the straight line of linear light is a bent portion existing at both ends of a line segment (straight line of linear light), and coordinates x ₁ and coordinates in the captured image it is x _2. FIG. 4 is a diagram illustrating the relationship between the captured image and the real world. Further, in FIG. 4, the captured image is indicated by a symbol “P”, and the feature point and a region representing the periphery thereof are indicated by a symbol “E”.
The bending angle calculation unit 14 calculates a bending angle that is a bending angle of the bending part detected by the bending part detection unit 12, and sends an information signal including the calculated bending angle to the rising line calculation unit 16. Output. Here, the bending angle is calculated by, for example, extracting the bent portion and its peripheral portion from the surrounding image captured by the imaging unit 2.

The rising line calculation unit 16 defines a bent portion plane, which is a plane on which the bent portion exists, from the bent angle calculated by the bent angle calculating portion 14. In addition, the rising line calculation unit 16 calculates a rising line that rises at a preset angle from the plane of the bent portion at the bent portion. Further, the rising line calculation unit 16 outputs an information signal including the calculated rising line to the feature point matching unit 20.
In the present embodiment, as an example, a case will be described in which the configuration of the rising line calculation unit 16 is configured to calculate a rising line based on a normal vector rising in the vertical direction from the bent portion plane.

Hereinafter, the procedure for calculating the normal direction of the bent portion plane will be described.
When the feature point confirmed by the feature point detection unit 8 is a texture feature point on a plane, the rising line calculation unit 16 obtains the normal direction of the plane (folded plane) where the feature point exists.
Here, for example, as shown in FIG. 5, if the range β for detecting feature points is 80 [°], the allowable range α with respect to the normal direction of the plane of the bent portion is 5 [°]. Up to an error. FIG. 5 is a diagram illustrating the normal direction and the allowable range of the normal direction. Further, in FIG. 5, as in FIG. 4, the captured image is indicated by the symbol “P”, and the feature point and the area representing the periphery thereof are indicated by the symbol “E”.

First, Y ₁ and Y ₂ that are the depths to the points X ₁ and X ₂ at both ends of the line segment in the real world on the captured image are calculated using the above equation (6). Incidentally, the depth Y ₁ and Y ₂ are the points of the bent portion _{x 1 = (x 1 y 1} w), and the position y ₁ of the longitudinal x _1, x ₂ = a (x ₂ y ₂ 1), Calculation is performed from the vertical position y ₁ and y ₂ of x ₂ using the above equation (6).
Further, the points X ₁ and X ₂ are obtained using Y ₁ and Y ₂ calculated using the above equation (6) and the equation in the _first row of the above equation (6).

Further, since the points X ₁ and X ₂ are points on the light projecting plane, and the light projecting plane π is a plane extending to Z = 0 (see FIG. 3), Z ₁ (see FIG. 3) corresponding to the point X ₁ (Not shown) is “0”, and Z ₂ (not shown) corresponding to the point X ₂ is “0”. Further, if it is assumed that the plane on which the feature point exists is parallel to the Z axis, the point X ₃ = (X1 Y1 a 1) in the real world can be obtained, although not particularly illustrated. Here, “a” is an arbitrary value.
As described above, the normal direction D _c of the plane where the feature point exists is the normal direction of the plane defined by the three points described above, that is, X1, X2, and X3. It is possible to calculate.

  The map storage unit 18 uses the feature point detected by the feature point detection unit 8, the distance between the imaging unit 2 and the feature point calculated by the distance calculation unit 10, and the bending unit detected by the bending unit detection unit 12. To form a feature point map, and store the formed feature point map. Further, the map storage unit 18 stores the feature point map formed as described above in the map storage unit 18 and outputs an information signal including the feature point map to the feature point matching unit 20.

Hereinafter, a specific procedure for forming the feature point map will be described.
The distance between the imaging unit 2 and the feature point calculated by the distance calculation unit 10 and the normal vector (rising line) calculated by the rising line calculation unit 16 are based on the camera coordinate system. However, since the position of the camera (imaging unit 2) always changes when the vehicle V travels, if a value based on the camera coordinate system is used, when the camera (imaging unit 2) moves, a feature point or the like The position becomes indefinite.

Therefore, in order to store the feature point map in the map storage unit 18, the distance between the imaging unit 2 and the feature point calculated by the distance calculation unit 10 and the normal vector calculated by the rising line calculation unit 16 are used as the camera. It is necessary to convert from the coordinate system to the world coordinate system and save it.
In order to convert the distance and normal vector between the imaging unit 2 and the feature point described above from the camera coordinate system to the world coordinate system, the position of the imaging unit 2 in the world coordinate system is calculated simultaneously with the generation of the feature point map. There is a need to.

Further, in order to convert the distance between the imaging unit 2 and the feature point and the normal vector from the camera coordinate system to the world coordinate system, it is necessary to calculate a conversion matrix to the camera coordinate system.
As a technique for simultaneously generating the feature point map and calculating the position of the imaging unit 2 and calculating the transformation matrix, for example, a well-known SLAM (Simultaneous Localization and Mapping) can be used. Here, examples of the SLAM using the imaging unit 2 (camera) include Mono-SLAM and PTAM.

As a method for calculating the position of the imaging unit 2, in addition to the above-described SLAM, for example, a method of integrating the movement amount of the vehicle V using a wheel speed pulse detected by a known wheel speed sensor. It may be used.
Then, after calculating the above transformation matrix, the distance Y _W between the imaging unit 2 and the feature point is set using the following equation (8) using the inverse of the transformation matrix from the world coordinate system to the camera coordinate system. To do.

Further, the feature point and the surrounding normal vector D _w are set using the following equation (9).

After setting the distance Y _W between the imaging unit 2 and the feature point, the feature point and the normal vector D _w around it, the feature point map is formed by combining the set value and the texture image _If of the feature point. Then, the formed feature point map is stored in the map storage unit 18.
The feature point matching unit 20 includes a rising line calculated by the rising line calculation unit 16 according to a change in the imaging range, and a bending unit before the imaging range changes based on the distance calculated by the distance calculation unit 10. The folded portion after the imaging range is changed is collated. Further, the feature point matching unit 20 outputs an information signal including a result obtained by matching the bent part before the imaging range is changed and the bent part after the imaging range is changed to the moving body position estimating unit 22. .

In the present embodiment, as an example, the configuration of the feature point matching unit 20 is configured such that the folding unit before the imaging range is changed and the folding after the imaging range is changed by the matching process on the surrounding image captured by the imaging unit 2. The case where it is set as the structure which collates with a part is demonstrated.
Hereinafter, a process performed by the feature point matching unit 20 when the bent part before the imaging range is changed and the bent part after the imaging range is changed will be described.

The feature point matching unit 20 first performs a process of estimating the prior position of the vehicle V before traveling (preliminary position estimation) as a process of calculating a bent part before the imaging range changes.
Here, at the time of pre-position estimation, the feature points in the feature point map held in the map storage unit 18 and the feature points detected in the image captured by the imaging unit 2 are collated.
Specifically, prior position estimation is performed using prior knowledge (position estimation result in the previous position estimation process, position where the stopped vehicle V starts traveling, position measurement results by various sensors, and the like). . The above-mentioned “various sensors” are, for example, GPS (Global Positioning System).

Then, after detecting the position in the feature point map and the attitude of the imaging unit 2 in the world coordinate system by the prior position estimation, the feature point predicted to be reflected in the image captured by the imaging unit 2 is Select from the feature point map. Here, the feature point predicted to be reflected in the image captured by the imaging unit 2 is obtained by using the position of the feature point in the feature point map and the attitude of the imaging unit 2 in the world coordinate system. Select from the feature points stored in the map.
When selecting a feature point predicted to appear in an image captured by the imaging unit 2 from the feature point map, the surrounding image by the imaging unit 2 is selected with respect to the position of the feature point in the feature point map. The above position is calculated using the following equation (10).

Here, “X _p ” in the above equation (10) is the position of the feature point stored in the feature point map in the world coordinate system.
In addition, “M _W ” in the above equation (10) is the position of the imaging unit 2 with respect to the world coordinate system, and is a prior estimated value M _W according to the position of the vehicle V. This pre-estimated value M _W gives an initial solution for performing detailed pre-position estimation of the vehicle V. This is performed in order to simplify the prior position estimation by giving an initial solution because the calculation load accompanying the estimation is large.

Here, if “x” in the above equation (10) is within the imaging range of the surrounding image, it is determined that there is a high possibility that the feature point in the feature point map will appear in the image captured by the imaging unit 2. And hold that place.
Then, after selecting the feature points that are predicted to be captured in the surrounding image captured by the imaging unit 2, the position of the selected feature points in the surrounding image is estimated, and the estimated position is the center, The actual position of the feature point is searched in the surrounding image. Thereby, the bent part before the imaging range based on the position in the surrounding image assumed from the pre-estimated value is changed, and the bent part after the imaging range based on the actually detected image position is changed. And match.

At this time, in the case where the position where the feature point is detected by the feature point detection unit 8 and the current position of the vehicle V are greatly different, the way the feature points are viewed is greatly different. For this reason, in the above matching processing between feature points, the actual position of the feature points may not be searched.
On the other hand, in the present embodiment, as described above, by calculating the rising line based on the normal vector rising in the vertical direction from the bent portion plane, which is the plane where the bent portion exists, the feature point is calculated. Estimate how it looks. Based on the estimated appearance of the feature point, the actual position of the feature point is searched.
Here, when the feature point is a texture feature point and the normal direction thereof is stored, the normal direction D is aligned with the coordinates (position) C ′ of the imaging unit 2 and the following equation (11) is obtained. Use to convert.

As a method used when searching for the actual position of the feature point in the surrounding image, the method described below can be used.
That is, in the method used when searching for the actual position of the feature point, first, a rectangular range having a certain size is set as a search area around the position (x) estimated as described above. Then, in the set search area, the image matching degree calculated by SSD (Source Skin Distance), SAD (Source Axis Distance), or the like is changed for each [pixel]. Furthermore, a place with the highest image matching degree is searched.

At this time, when the feature point is a texture feature point and the normal direction of the texture plane is stored, the texture is converted in accordance with the appearance from the position of the imaging unit 2.
When converting a texture, the texture exists in a plane, so a two-dimensional Homography conversion is performed. Here, assuming that each pixel position of the stored texture image _If is Pi = (Ix Iy 1), the position Pt ′ = (I′x I′y 1) of the feature point after the texture is transformed is Is expressed as the following equation (12).

  In the above equation (12), “H” is a 3 × 3 transformation matrix, and is calculated from the normal vector and the position of the imaging unit 2 using the following equation (13).

Here, “R” in the above equation (13) images the bent part after the imaging range has changed from the position C of the imaging unit 2 when the bent part before the imaging range has changed is imaged. The rotation matrix to the position C ′ of the imaging unit 2 at the time.
In addition, “t” in the above formula (13) is an image of the bent part after the imaging range is changed from the position C of the imaging unit 2 when the bent part before the imaging range is changed. The movement vector to the position C ′ of the imaging unit 2 at the time.
Further, “Dc ′” in the above equation (13) is a normal vector of the texture feature point when viewed from the imaging unit 2.

Further, “d3” in the above equation (13) is a distance from the imaging unit 2 to the feature point.
The moving body position estimation unit 22 uses the bent part before the imaging range changed and the bent part after the imaging range changed, which are collated by the feature point matching unit 20, to calculate the displacement amount of both positions. calculate. Based on the calculated displacement, the position of the vehicle V is estimated (hereinafter may be referred to as “post-mortem estimation”).

In addition, the estimation result of the position of the vehicle V by the moving body position estimation unit 22 is displayed as coordinates on the map or the like on a display device (display or the like), for example, although not particularly illustrated.
In the present embodiment, post-estimation is performed by adjusting the result of prior estimation so that the position of the feature point matches the actual measurement position. This is because, when the position of the feature point detected by the feature point detection unit 8 is correct, the difference in position between the feature points verified by the feature point verification unit 20 is considered as an error of the prior estimated value. Therefore, the pre-estimation value is corrected by minimizing the position difference between the feature points collated by the feature point collating unit 20 to obtain the posterior estimation value.
In addition, as a method for minimizing the difference between the positions of the feature points collated by the feature point matching unit 20, for example, the positions of the feature points are newly determined with respect to a plurality of post-estimation candidates such as a known Monte Carlo method. A method for calculating the difference between the two may be used.

(Operation)
Hereinafter, the operation performed by the moving object position estimation apparatus 1 will be described with reference to FIGS. 1 to 5 and FIGS. 6 and 7.
FIG. 6 is a flowchart showing a part of the operation performed by the mobile object position estimation apparatus 1.
As shown in FIG. 6, when the moving object position estimation device 1 starts operation (START), first, in step S <b> 10, the imaging unit 2 displays the surrounding image of the vehicle V at a preset time interval t [s]. Are continuously imaged. At this time, the light projecting unit 4 projects linear light within the imaging range of the imaging unit 2. If the surrounding image of the vehicle V is continuously imaged in step S10, the process performed by the mobile object position estimation device 1 proceeds to step S12.

In step S <b> 12, the feature point detection unit 8 detects a feature point from the surrounding image captured by the imaging unit 2. When the feature point is detected in step S12, the process performed by the mobile object position estimation apparatus 1 proceeds to step S14.
In step S14, the distance calculation unit 10 calculates the distance from the imaging unit 2 to the feature point. When the distance from the imaging unit 2 to the feature point is calculated in step S14, the process performed by the mobile object position estimation device 1 proceeds to step S16.

In step S <b> 16, the bent portion detection unit 12 detects a bent portion from the feature point detected by the feature point detection unit 8 and its surroundings. In step S16, if a bending part is detected, the process which the mobile body position estimation apparatus 1 performs will transfer to step S18.
In step S18, the rising line calculation unit 16 defines a bent portion plane from the bent angle calculated by the bent angle calculating portion 14, and further rises at a predetermined angle from the bent portion plane at the bent portion. Calculate the line. When the rising line is calculated in step S18, the process performed by the mobile object position estimation device 1 proceeds to step S20.

In step S20, the map storage unit 18 forms a feature point map and stores the formed feature point map. At this time, the feature point detected by the feature point detection unit 8 is deleted along with the storage of the feature point map. When the feature point map is stored in step S20, the process performed by the mobile object position estimation device 1 returns to step S10.
After the feature point map is stored in the map storage unit 18 by the processing of steps S10 to S20 described above, the mobile object position estimation device 1 performs the operation shown in FIG. FIG. 7 is a flowchart showing a part of the operation performed by the mobile object position estimation apparatus 1.

As shown in FIG. 7, when the moving body position estimating apparatus 1 starts operation (START), first, the feature point matching unit 20 performs a prior position estimation with respect to the vehicle V in step S30. If prior position estimation is performed in step S30, the process performed by the mobile object position estimation apparatus 1 proceeds to step S32.
In step S32, it is determined whether or not the prior position estimation performed in step S30 has ended normally. And when it determines with the prior position estimation performed by step S30 having been complete | finished normally, the process which the mobile body position estimation apparatus 1 performs transfers to step S34.

On the other hand, when it is determined that the prior position estimation performed in step S30 has not ended normally, the process performed by the mobile object position estimation device 1 ends (END).
Here, the reason for performing the process of step S32 is that the prior position estimation performed in step S30 may fail for some reason, and in the case of failure, the parameters used for the prior position estimation are reset. This is because it is necessary to perform the prior position estimation again.

In step S34, the appearance of the feature point is estimated by calculating the rising line based on the normal vector rising in the vertical direction from the bent plane where the bent portion exists. If the appearance of the feature point is estimated in step S34, the process performed by the mobile object position estimation apparatus 1 proceeds to step S36.
In step S36, the surrounding image of the vehicle V imaged by the imaging unit 2 is acquired. Here, the surrounding image acquired in step S36 is an image in a state where the vehicle V is moving after a lapse of time from the surrounding image acquired in step S10 described above. If the surrounding image of the vehicle V is acquired in step S36, the process which the mobile body position estimation apparatus 1 performs will transfer to step S38.

In step S38, the feature point collating unit 20 collates the bent part before the imaging range changes with the bent part after the imaging range changes. In step S38, when the bent part before the imaging range is changed and the bent part after the imaging range is changed, the process performed by the mobile object position estimating apparatus 1 proceeds to step S40.
In step S <b> 40, the moving body position estimation unit 22 uses the bent part before the change of the imaging range and the bent part after the change of the imaging range, which are collated by the feature point matching unit 20. The amount of displacement is calculated. When the displacement amount is calculated in step S40, the process performed by the mobile object position estimation apparatus 1 proceeds to step S42.

In step S42, the moving body position estimation unit 22 performs a posteriori estimation on the position of the vehicle V based on the displacement amount calculated in step S40. If post-mortem estimation is performed in step S42, the process performed by the mobile object position estimation apparatus 1 proceeds to step S44.
In step S44, it is determined whether or not the posterior estimation performed in step S42 has been completed normally. And when it determines with the posterior estimation performed in step S42 having been normally complete | finished, the process which the mobile body position estimation apparatus 1 transfers to step S46.

On the other hand, when it is determined that the posterior estimation performed in step S42 is not normally completed, the process performed by the mobile object position estimation device 1 is terminated (END).
Here, the reason for performing the process of step S44 is that the posterior estimation performed in step S42 may fail for some reason, and in the case of failure, the parameters used for the posterior estimation are reset and again This is because it is necessary to perform post-mortem estimation.

In step S46, it is determined whether or not the estimation of the position of the moving object performed by the moving object position estimation unit 22 has ended normally. And when it determines with the estimation of the position of the moving body performed by the moving body position estimation part 22 having been complete | finished normally, the process which the moving body position estimation apparatus 1 performs transfers to step S30.
On the other hand, when it is determined that the estimation of the position of the moving object performed by the moving object position estimation unit 22 has not ended normally, the process performed by the moving object position estimation device 1 ends (END).

Note that, as described above, the mobile object position estimation method implemented by the operation of the mobile object position estimation apparatus 1 according to the present embodiment is the folding position before the imaging range is changed and the folding position after the imaging range is changed. This is a method for estimating the position of the vehicle V based on the amount of displacement from the position of the curved portion.
Here, the collation between the bent part before the imaging range changes and the bent part after the imaging range changes is a rising line that rises at a preset angle from the bent part plane according to the change of the imaging range. And based on the distance from the imaging unit 2 to the feature point.

  The rising line is calculated from the surrounding image captured by the imaging unit 2 in a state where linear light is projected into the imaging range of the surrounding image of the vehicle V continuously captured by the imaging unit 2. A feature point that is a point that can be followed even if the range changes is detected. And the bending part which is the part which the linear light projected in the imaging range of the surrounding image of the vehicle V continuously imaged with the imaging part 2 is bent from the detected feature point and its periphery is detected. . Accordingly, a rising line is calculated by defining a bent portion plane that is a plane on which the bent portion exists from the detected bending angle that is the bending angle of the bent portion.

  Thus, the light projecting unit 4 corresponds to the light projecting unit. The feature point detector 8 corresponds to a feature point detector. The distance calculation unit 10 corresponds to a distance calculation unit. Moreover, the bent part detection part 12 respond | corresponds to a bent part detection means. The bending angle calculation unit 14 corresponds to a bending angle calculation unit. The rising line calculation unit 16 corresponds to rising line calculation means. The feature point matching unit 20 corresponds to feature point matching means. Moreover, the moving body position estimation part 22 respond | corresponds to a moving body position estimation means.

(Effects of the first embodiment)
(1) The feature point detection unit 8 is a feature point that can be tracked even if the imaging range changes from the surrounding image captured by the imaging unit 2 in a state where the light projecting unit 4 projects linear light. The bend detection unit 12 detects a bend from the feature point detected by the feature point detection unit 8 and its surroundings. The bending angle calculation unit 14 calculates a bending angle that is a bending angle of the bending part, the rising line calculation unit 16 defines a bending part plane that is a plane on which the bending part exists, and Then, a rising line that rises at a predetermined angle from the plane of the bent portion in the bent portion is calculated.

In addition to this, the feature point matching unit 20 is based on the rising line calculated by the rising line calculation unit 16 and the distance from the imaging unit 2 to the feature point calculated by the distance calculation unit 10 according to the change of the imaging range. The folded part before the imaging range is changed is compared with the bent part after the imaging range is changed. Furthermore, the mobile body position estimation unit 22 estimates the position of the vehicle V based on the displacement amount of the position between the bent part before the imaging range changes and the bent part after the imaging range changes.
For this reason, it becomes possible to estimate the position of the vehicle V based on the bent portion plane where the bent portion exists and the rising line rising from the bent portion plane.
As a result, it is possible to estimate the position of the vehicle V even if the appearance of the feature points detected from different places changes at each detection place, and increase the conditions under which the position of the vehicle V can be estimated. It becomes possible.

(2) The rising line calculation unit 16 calculates the rising line based on the normal vector rising in the vertical direction from the bent plane where the bent portion exists.
For this reason, when performing a posteriori estimation of the position of the vehicle V, the normal vector of the plane where the feature point exists, the position of the feature point on the feature point map, and the prior estimated value of the position of the imaging unit 2 are used. It is possible to estimate the appearance of the feature point from the pre-estimated position of the imaging unit 2.
As a result, it is possible to improve the detection accuracy of a feature point whose appearance changes greatly depending on the detected angle.

(3) The feature point matching unit 20 matches the bent part before the imaging range is changed with the bent part after the imaging range is changed by the matching process on the surrounding image captured by the imaging unit 2.
As a result, using processes other than matching processing, collation can be performed with simpler processing compared to the case where the folded part before the imaging range changes and the folded part after the imaging range changes. Therefore, the process for estimating the position of the vehicle V can be simplified.

(4) The moving body position estimation method of the present embodiment is a point that can follow even if the imaging range changes in a state where linear light is projected into the imaging range of the surrounding image captured by the imaging unit 2. A feature point is detected, and a bent portion is detected from the detected feature point and its periphery. Further, a bending angle that is a bending angle of the bent portion is calculated, a bent portion plane that is a plane on which the bent portion exists is defined, and further, a predetermined angle is set from the bent portion plane in the bent portion. Calculate the rising line that rises.

In addition to this, in accordance with the change of the imaging range, the bent line before the imaging range changes and the bent part after the imaging range changes based on the rising line and the distance from the imaging unit 2 to the feature point And match. Furthermore, the position of the vehicle V is estimated based on the displacement amount of the position of the bent part before the imaging range changes and the bent part after the imaging range changes.
For this reason, it becomes possible to estimate the position of the vehicle V based on the bent portion plane where the bent portion exists and the rising line rising from the bent portion plane.
As a result, it is possible to estimate the position of the vehicle V even if the appearance of the feature points detected from different places changes at each detection place, and increase the conditions under which the position of the vehicle V can be estimated. It becomes possible.

(Modification)
(1) In the moving body position estimation apparatus 1 according to the present embodiment, the rising line calculation unit 16 calculates the rising line based on the normal vector rising in the vertical direction from the bent part plane where the bent part exists. The configuration of the rising line calculation unit 16 is not limited to this. That is, the configuration of the rising line calculation unit 16 may be configured to calculate the rising line based on other than the normal vector.

(2) In the moving body position estimation apparatus 1 according to the present embodiment, the feature point matching unit 20 changes the folding range and the imaging range before the imaging range is changed by the matching process on the surrounding image captured by the imaging unit 2. Check the folded part after the check. However, the configuration of the feature point matching unit 20 is not limited to this, and the bending portion and the imaging range before the imaging range changes are changed by processing other than the matching processing for the surrounding image captured by the imaging unit 2. It is good also as a structure which collates with the bent part after doing.
(3) In the mobile body position estimation apparatus 1 of the present embodiment, the object including the mobile body position estimation apparatus 1 is a vehicle. However, the present invention is not limited to this. For example, a work machine, a train, etc. It is good also as a target provided with the estimation apparatus 1. FIG.

DESCRIPTION OF SYMBOLS 1 Mobile body position estimation apparatus 2 Imaging part 4 Light projection part (light projection means)
6 arithmetic processing unit 8 feature point detection unit (feature point detection means)
10 Distance calculation unit (distance calculation means)
12 Bent part detection part (bent part detection means)
14 Bending angle calculation unit (bending angle calculation means)
16 Rising line calculation unit (rising line calculation means)
18 Map storage unit 20 Feature point matching unit (feature point matching unit)
22 Moving body position estimation unit (moving body position estimation means)
V Vehicle P Captured image E Feature point and its surrounding area

Claims (4)

An imaging unit that continuously captures surrounding images of the moving body;
A light projecting unit that projects linear light into an imaging range of a surrounding image captured by the imaging unit;
A feature point detection unit that detects a feature point that is a point that can be followed even if the imaging range changes, from a surrounding image captured by the imaging unit in a state where the light projecting unit projects linear light;
Distance calculating means for calculating a distance from the imaging unit to the feature point;
A bent portion detecting means for detecting a bent portion that is a portion where the linear light projected by the light projecting means is bent from the feature point detected by the feature point detecting means and the periphery thereof; and
A bending angle calculating means for calculating a bending angle that is a bending angle of the bent portion detected by the bent portion detecting means;
A bent portion plane, which is a plane on which the bent portion exists, is defined from the bent angle calculated by the bent angle calculating means, and the bent portion rises from the bent portion plane at a preset angle. A rising line calculating means for calculating the rising line;
Based on the rising line calculated by the rising line calculating means and the distance calculated by the distance detecting means, after the change of the imaging range and the bent portion before the imaging range changes, in accordance with the change of the imaging range A feature point matching means for matching the bent portion of
A moving body that estimates the position of the moving body based on a displacement amount of a position between the bent portion before the imaging range changed by the feature point matching means and the bent portion after the imaging range has changed. A moving body position estimating apparatus comprising: a position estimating means;   2. The moving body position estimating apparatus according to claim 1, wherein the rising line calculating means calculates the rising line based on a normal vector rising in a vertical direction from the bent portion plane.   The feature point matching unit is configured to match a bent part before the imaging range is changed with a bent part after the imaging range is changed by a matching process on a surrounding image captured by the imaging unit. The moving body position estimation apparatus according to claim 1 or 2. Project linear light into the imaging range of the surrounding image of the moving object continuously captured by the imaging unit,
In a state where the linear light is projected, a feature point that is a point that can be followed even if the imaging range changes is detected from the captured surrounding image,
Calculating a distance from the imaging unit to the feature point;
From the detected feature point and its surroundings, a bent portion that is a portion where the projected linear light is bent is detected,
Calculating a bending angle that is a bending angle of the detected bending portion;
Define a bent portion plane that is a plane in which the bent portion exists from the calculated bent angle, and further calculate a rising line that rises at a preset angle from the bent portion plane in the bent portion,
Based on the calculated rising line and the calculated distance in accordance with the change of the imaging range, the folded part before the imaging range is changed and the bent part after the imaging range is changed,
The movement is characterized in that the position of the moving body is estimated based on a displacement amount of a position of the folded portion before the collated imaging range is changed and a bent portion after the imaging range is changed. Body position estimation method.

Images