JP2019124573A - Vehicle position estimation apparatus, vehicle position estimation method, and vehicle position estimation program - Google Patents

Abstract

To provide a vehicle position estimation apparatus, a vehicle position estimation method, and a vehicle position estimation program capable of estimating a position of a vehicle position more quickly and more accurately.SOLUTION: The vehicle position estimation apparatus estimates a position X and an attitude of the own vehicle on a prescribed traveling route by collating a plurality of traveling images with map information including an environmental feature point FP and a position CP of imaging means at the time when imaging a plurality of reference images in the environment along the prescribed traveling route S associated with each of the plural reference images, and includes setting means for estimating a road surface gradient difference of the traveling route and setting a detection range of the corresponding feature point corresponding to the environmental feature point FP in the traveling image from the estimated road surface gradient difference; and estimation means for selecting a reference image most similar to the traveling image from the plurality of reference images on the basis of the corresponding feature point detected in the detection range and for estimating the position and attitude of the own vehicle on the basis of the correspondence between the environmental feature point and the corresponding feature point.SELECTED DRAWING: Figure 2

Description

  The present invention relates to a vehicle position estimation apparatus, a vehicle position estimation method, and a vehicle position estimation program.

  There has been proposed a technique for estimating the position of a mobile object in a three-dimensional space based on a captured image captured by an imaging unit such as a camera mounted on the mobile object.

  Conventionally, as a technique related to the estimation of the vehicle position, for example, an automatic driving control device disclosed in Patent Document 1 is known. The automatic driving control device according to Patent Document 1 generates automatic driving information for automatically driving the vehicle based on the registration time image which is an image obtained by imaging the surrounding environment of the vehicle in the registration mode in which the vehicle is driven by the driver. Automatic driving control unit for automatically driving the vehicle based on the automatic driving information registration unit and the automatic driving image which is an image obtained by imaging the surrounding environment of the vehicle in the automatic driving mode in which the vehicle is automatically driven, and the automatic driving information , And the automatic driving information registration unit extracts candidate feature points present in the environment around the vehicle based on the registered image, and a plurality of registrations captured while the vehicle is moving. Among the candidate feature points, the candidate feature points determined to be fixedly arranged around the destination of the vehicle are selected as feature points based on the time image, and the feature points with respect to predetermined origin coordinates are selected. position An automatic driving information generation unit for generating automatic driving information which is information, and the automatic driving control unit is a vehicle position which is information of a position of the vehicle with respect to origin coordinates based on the automatic driving image and the automatic driving information It has a vehicle position calculation unit that calculates information, and an automatic driving execution control unit that automatically drives the vehicle to a target location based on the vehicle position information. That is, in the automatic driving control device according to Patent Document 1, feature points of the structure are extracted from the image when traveling in advance, the three-dimensional position thereof is estimated, and is registered in the map. The vehicle position is estimated by extracting the feature points of and comparing the feature points with the feature points of the structure registered in the map.

JP, 2017-138664, A

  By the way, in order to estimate the vehicle position while flexibly dealing with the moving speed of a moving object such as a vehicle, it is required to perform the estimation process in a short time as much as possible. However, since the automatic driving control device according to Patent Document 1 detects a feature point from the entire image, it is difficult to detect the feature point in a short time. Furthermore, because the number of detected feature points increases, it takes time to extract feature points of the structure. As a result, there is a problem that it is difficult to estimate the vehicle position in a short time.

  Further, in the case of estimating the vehicle position based on the feature points extracted from the photographed image, it is also necessary to consider the distribution of the feature points in the entire image. That is, when the distribution of feature points in the entire image is biased, it is wasteful to perform feature point extraction processing for the entire image, and as a result, there is a limit to shortening the processing time for estimating the vehicle position. Patent Document 1 does not discuss such a problem.

  The present invention has been made to solve the problems described above, and is capable of estimating the position of the vehicle more quickly and more accurately, the method of estimating the position of the vehicle, the method of estimating the position of the vehicle, and the position of the vehicle. The purpose is to provide an estimation program.

  In order to achieve the above object, the vehicle position estimation apparatus according to claim 1 comprises a plurality of traveling images taken by the photographing means on a predetermined traveling route, and a plurality of positions along the predetermined traveling route. A map including environmental feature points in an environment along the predetermined traveling route associated with each of the plurality of reference images taken by the imaging means, and a position and an attitude of the imaging means at the time of taking the plurality of reference images A vehicle position estimation device for estimating the position and posture of a vehicle on the predetermined traveling route by collating information with each other, wherein the road surface gradient difference estimated as well as the road surface gradient difference of the traveling route is estimated. Setting means for setting a detection range of a corresponding feature point corresponding to the environment feature point in the traveling image from a difference, and based on the corresponding feature point detected in the detection range Estimating means for selecting the reference image most approximate to the traveling image from the plurality of reference images and estimating the position and attitude of the vehicle based on the correspondence between the environmental feature points and the corresponding feature points; It is included.

  The invention according to claim 2 is the invention according to claim 1, wherein the map information includes position coordinates in the map information of the environmental feature points, and the estimation means is associated with the map information. The position and orientation of the photographing means are estimated based on the position coordinates of the environment feature point and the position of the corresponding feature point on the traveling image, and the estimated position and attitude of the photographing means is used as a representative point of the vehicle. In order to estimate the position and attitude of the vehicle.

  In the invention according to claim 3, in the invention according to claim 2, the map information includes feature quantities indicating features of each of the environment feature points, and the estimation means matches the previous time. The reference image including the largest number of selected feature points is selected as the most approximate reference image, and the feature amount of the corresponding feature point is compared with the feature amount of the environment feature point of the most approximate reference image to compare the environment A feature point is associated with the corresponding feature point.

  The invention according to claim 4 relates to the invention according to any one of claims 1 to 3, wherein the photographing means takes an image of a predetermined area in front of the photographing means. The photographing means fixed to a predetermined position of a car, the setting means selects a reference image closest to the predetermined area from the plurality of reference images and is associated with the selected reference image The road surface gradient difference is estimated from the difference between the road surface gradient according to the position and posture of the vehicle and the road surface gradient according to the position and posture of the photographing means estimated for the previously photographed traveling image.

  Also, in the invention according to claim 5, in the invention according to any one of claims 1 to 4, the setting means sets the detection range to the upper and lower sides of the traveling image according to the road surface gradient difference. It is moved in the direction.

  In the sixth aspect of the present invention, in the fifth aspect of the present invention, the setting means may increase the road surface gradient of the vehicle position as compared to the predetermined area, in the traveling image. The position of the detection range is set at the upper part.

  In order to achieve the above object, according to the vehicle position estimation method of a seventh aspect of the present invention, a plurality of traveling images taken by the photographing means on a predetermined traveling route and a plurality of positions along the predetermined traveling route A map including environmental feature points in an environment along the predetermined traveling route associated with each of the plurality of reference images taken by the imaging means, and a position and an attitude of the imaging means at the time of taking the plurality of reference images A vehicle position estimation method by a vehicle position estimation apparatus for estimating a position and a posture of a vehicle on the predetermined traveling route by collating information with a road surface gradient difference of the traveling route by setting means. And the detection range of the corresponding feature point corresponding to the environment feature point in the traveling image is set from the road surface gradient difference estimated and the estimation means Based on the corresponding feature points detected in the detection range, a reference image closest to the traveling image is selected from the plurality of reference images, and the vehicle is selected based on the correspondence between the environment feature points and the corresponding feature points. Estimate the position and attitude of

  In order to achieve the above object, the vehicle position estimation program according to claim 8 is characterized in that a plurality of traveling images taken by the photographing means on a predetermined traveling route, and a plurality of positions along the predetermined traveling route Map information including environmental feature points in an environment along the predetermined traveling route associated with each of the plurality of reference images taken by the imaging means, and the position and posture of the imaging means when the plurality of reference images are taken And a program for controlling the vehicle position estimation device for estimating the position and attitude of the vehicle on the predetermined traveling route by collating with and the computer estimates the road surface gradient difference of the traveling route and estimates the difference. Setting means for setting a detection range of a corresponding feature point corresponding to the environmental feature point in the traveling image from the road surface gradient difference; Based on the corresponding feature points detected in the range, a reference image most similar to the traveling image is selected from the plurality of reference images and the own vehicle is selected based on the correspondence between the environmental feature points and the corresponding feature points. It functions as an estimation means for estimating the position and the attitude.

  According to the present invention, it is possible to provide an own-vehicle position estimation apparatus, an own-vehicle position estimation method, and an own-vehicle position estimation program capable of estimating the own-vehicle position more quickly and more accurately. .

(A) is a figure which shows the vehicle carrying a camera which concerns on embodiment, (b) is a block diagram which shows an example of a structure of an own vehicle position estimation apparatus. (A) is a figure which shows the feature point in a parking path | route, (b) is a figure which shows the camera position of a key frame, and the position of a feature point. (A) illustrates the difference in camera pitch angle due to road surface gradient, (b) illustrates the feature point detection range, (c) illustrates the relationship between the upper end and the lower end of the feature point detection range, and the road surface gradient It is a figure to do. (A) is a figure which shows the first feature point detection range, (b) is a figure which shows the feature point detection range in the case of road surface gradient difference> 0. It is a flowchart which shows the flow of a process of the own vehicle position estimation process program which concerns on embodiment. It is a figure explaining a projection error.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. In the following description, a vehicle is taken as an example of a moving body, a surrounding environment when the vehicle estimates the position of the vehicle is taken as a parking lot, and a route from outside the parking lot to a parking point is taken as a traveling route. explain.

  An own vehicle position estimation apparatus, an own vehicle position estimation method, and an own vehicle position estimation program according to the present embodiment will be described with reference to FIGS. 1 to 6.

  FIG. 1A shows a vehicle 50 equipped with a vehicle position estimation device 10 according to the present embodiment. As shown in FIG. 1 (a), the vehicle 50 is provided with a camera 14 as a photographing means, which is used in the process of estimating the vehicle position according to the present embodiment (hereinafter, "vehicle position estimation process"). There is. The camera 14 according to the present embodiment is provided in a trunk or the like at the rear of the vehicle 50, and images the rear of the vehicle. The camera 14 is provided, for example, near a substantially central portion in the vehicle width direction, and is disposed such that the optical axis of the camera 14 is slightly lower than the horizontal direction. In the present embodiment, the camera 14 is described as being provided at the rear of the vehicle 50. However, the present invention is not limited to this, and may be provided, for example, at the front according to the environment. Further, in the present embodiment, a single-eye camera is described as an example of the camera 14, but the present invention is not limited to this and may be a camera of another form, such as a stereo camera.

  As shown in FIG. 1 (b), the vehicle position estimation apparatus 10 according to the present embodiment is configured to include a control unit 12, a camera 14, a storage unit 16, and a display unit 18.

  The control unit 12 is a part that performs calculations and the like for estimating the vehicle position. The control unit 12 includes, for example, a central processing unit (CPU) 12A, a read only memory (ROM) 12B, a random access memory (RAM) 12C, and an I / O 12D as an input / output interface. The CPU 12A, the ROM 12B, the RAM 12C, and the I / O 12D are connected to one another by a bus 12E.

  The CPU 12A supervises and controls the entire vehicle position estimation apparatus 10, and the ROM 12B is storage means for storing in advance a control program of the vehicle position estimation apparatus 10, an own vehicle position estimation processing program to be described later, etc. Is a storage means used as a work area or the like when executing a program such as a control program. The control unit 12 corresponds to the “setting means” and the “estimating means” according to the present invention.

  The ROM 12B stores a map generation program for generating a map used in the present embodiment and various programs, data and the like including an own vehicle position estimation processing program for estimating an own vehicle position. The program stored in the ROM 12B is expanded on the RAM 12C and executed by the CPU 12A, whereby generation of a map and estimation of the position of the vehicle are performed.

  The camera 14, the storage unit 16, and the display unit 18 are connected to the control unit 12 via the I / O 12D. The image taken by the camera 14 is taken into the control unit 12 via the I / O 12D.

  In addition, the storage unit 16 stores a map or the like, which will be described later, used in the vehicle position estimation process according to the present embodiment. The form of the storage unit 16 is not particularly limited, but as an example, a hard disk drive (HDD), a solid state drive (SSD), a flash memory, or the like can be used. The storage unit 16 may be provided in the control unit 12 or may be externally connectable. In addition to the map generated under the control of the control unit 12, the storage unit 16 may store a map generation processing program and a vehicle position estimation processing program.

  The display unit 18 is a part that displays an image or the like captured by the camera 14. The form of the display unit 18 is not limited, but, for example, a liquid crystal monitor, a CRT (Cathode Ray Tube) monitor, an FPD (Flat Panel Display) monitor or the like is used.

  FIG. 2 (a) is a view showing a state of the parking lot assumed in the present embodiment (hereinafter, "environment"). In the present embodiment, as a traveling route of the vehicle 50, it is assumed from the position of “start” (departure point) shown in FIG. 2A to the position of “goal” (parking point). The code "FP" shown in FIG. 2A indicates the position of the feature point described below. In order to estimate the vehicle position of the vehicle 50, it is necessary to determine in advance the position within the vehicle 50 having a predetermined size (hereinafter, "vehicle representative point X"). As shown, in the present embodiment, this vehicle representative point X is taken as the center point of the rear wheel. However, the position of the representative point is not limited to this, and may be, for example, the position of the center of gravity of the vehicle 50.

  FIG. 2 (b) is a plan view of the environment shown in FIG. 2 (a). As shown in FIG. 2 (b), the vehicle 50 mounted with the camera 14 travels from the start point SP to the goal point GP. In the vehicle position estimation process according to the present embodiment, the environment is photographed by the camera 14 before actual traveling, and predetermined accompanying data associated with the photographed image is generated as a map in advance. The code “S” indicates the traveling locus of the vehicle 50 at the time of the generation. In actual parking, the vehicle travels basically along the traveling path S. Hereinafter, in distinction from the traveling locus S, a route traveling at the time of actual parking (during traveling) may be referred to as a “traveling route”.

  The small circle shown in FIG. 2B indicates the position of the feature point FP. In the present embodiment, the “feature point” refers to a point such as a shadow based on unevenness of a building, a pattern of a wall surface, or the like, in which a gradation difference of luminance is larger than a predetermined value in a photographed image. Therefore, as shown in FIG. 2 (b), the feature points often appear on the wall surface WS of the building 30 in the environment or at the corners of the building 30, and a plurality of feature points FP are selected in one map. Ru. Furthermore, in the present embodiment, in order to distinguish each of the plurality of feature points FP, “feature amount” is associated with each feature point FP. The “feature amount” according to the present embodiment refers to, for example, a pattern of brightness difference in the vicinity of a feature point.

  The code “CP” in FIG. 2 (b) indicates a plurality of (12 in the example of FIG. 2 (b)) shooting points at which the camera 14 performed shooting when generating a map. In the present embodiment, images taken at the plurality of shooting points CP are referred to as “key frames”, and data at the time of generating a map together with the feature points FP and information associated with the feature points FP are configured. That is, the key frame is a plurality of images captured at a plurality of shooting points CP in advance, and information on feature points is associated with the key frame. In the following description, a shooting point at the time of generating a map is referred to as a "map shooting point CP", and a shooting point at the time of actual parking is referred to as a "running shooting point". In addition, an image captured at a traveling shooting point is referred to as a “traveling image”. The "key frame" corresponds to the "reference image" according to the present invention, and the feature point FP associated with the key frame corresponds to the "environment feature" according to the present invention. On the other hand, the feature point FP associated with the traveling image corresponds to the "corresponding feature point" according to the present invention.

Next, the “map” according to the present embodiment will be described. The map contains the following information:
(1) Coordinates (three-dimensional position coordinates (Xp, Yp, Zp)) displayed in three dimensions of each feature point FP.
(2) Three-dimensional position coordinates (Xc, Yc, Zc) and posture (roll angle, pitch angle, yaw angle) of the camera in each key frame.
(3) Each feature amount in each key frame.
In the generation of the map, a person drives in advance and travels the traveling locus S, and the coordinates (three-dimensional position) of the feature point and the position of the camera 14 in the key frame are estimated from the captured image by the camera 14 during traveling. These estimations can be performed using, for example, Visual SLAM (Simultaneous Localization and Mapping). That is, the three-dimensional position of the feature point and the camera position of the key frame are estimated from the captured image using Visual SLAM, and these estimated values and feature amounts of the feature point in the key frame are registered in the map.

More specifically, for example, ORB-SLAM can be applied as a feature point extraction method. ORB-SLAM detects a corner as a feature point and uses FAST (Features from Accelerated Segment Test) as a detection method. Also, ORB-SLAM uses ORB (Oriented FAST and Rotated BRIEF) to describe the feature value. The ORB is developed on the basis of Binary Robust Independent Elementary Features (BRIEF) to have scale invariance and rotation invariance. The ORB-SLAM is disclosed in the following reference 1 and thus the detailed description is omitted.
[Reference 1] Raul Mur-Artal, JMM Montiel and Juan D. Tardos. ORB-SLAM: A Versatile and Accurate Monocular SLAM System. IEEE Transactions on Robotics, vol. 31, no. Since the restriction on the calculation time is not severe at the time of map generation, the feature point detection range may be set to the entire photographed image without being limited as described later.

  In summary, coordinates of feature points at the time of capturing a key frame that is a plurality of images captured at the map capturing point CP at the time of map generation, feature amounts, position and orientation of the camera 14 are associated and stored as map information It is stored in the storage unit such as the unit 16 or the like. Further, the map stored in the storage unit 16 or the like is referred to in a vehicle position estimation process described later.

  Here, setting of the feature point detection range according to the present embodiment will be described with reference to FIGS. 3 and 4. As described above, the distribution of feature points FP in an image (key frame, traveling image) captured by the camera 14 as the vehicle 50 travels may change due to factors such as the state of the road surface on which the vehicle 50 travels. is there. In the present embodiment, as a factor affecting the distribution of feature points in the entire photographed image, the gradient of the road surface on which the vehicle 50 which is the target of the vehicle position estimation travels is assumed. That is, when there is a high probability that the distribution of feature points in the entire photographed image is biased due to the road surface gradient, the detection range of the feature points is changed.

  FIG. 3A shows a vehicle 50 traveling on a sloped road surface R. FIG. The example shown in FIG. 3A is an example in which the front of the host vehicle is an uphill, but the concept is the same as in the case where the front of the host vehicle is a downhill. In FIG. 3A, the position P0 indicates a reference position, the position P1 indicates a position on the way up, and the vehicle 50 is located at each position. Here, the pitch angle of the camera 14 of the vehicle 50 at the position P0 is θf, and the pitch angle of the camera 14 of the vehicle at the position P1 is θ1. The pitch angle θ according to the present embodiment is an inclination angle of the optical axis of the camera 14 measured from the horizontal direction, and a direction approaching the ground from the horizontal direction is defined as a positive direction. Further, the road surface gradient difference Δθ is defined by (θ1−θf) (Δθ = (θ1−θf)). Under this definition, the road surface slope difference Δθ is equal to the angle shown in FIG. 3 (a). As described above, in the present embodiment, the optical axis of the camera 14 is disposed to be slightly lower than the horizontal direction. However, for ease of understanding, the camera 14 on the flat road surface R is described below. It is assumed that the direction of the optical axis of is horizontal.

FIG. 3B schematically shows the feature point detection range As in the captured image G.
The feature point detection range As according to the present embodiment refers to an area designated as a search range of the feature point FP in the image G. That is, the detection of the feature point FP is not performed on the entire image G, but is performed in a designated and limited area. As shown in FIG. 3B, in the present embodiment, the feature point detection range As is specified by specifying the upper end Gu and the lower end Gd at the upper and lower positions of the image G. The method of specifying the feature point detection range As is not limited to this, and may be specified by, for example, a midpoint Gc between the upper end Gu and the lower end Gd and the image width Ws. FIG. 3B shows the feature point detection range As0 when the vehicle 50 is at the reference position of the substantially flat road surface, with the upper end being Gu0 and the lower end being Gd0.

  FIG. 3C shows the relationship between the road surface gradient difference Δθ and the feature point detection range As. FIG. 3C shows a feature point detection range in which the road surface gradient difference Δθ is in the range of Δθ1 to Δθ2 when the front of the vehicle is an upward slope. The point of the road surface gradient difference Δθ = 0 indicates a position where the road surface gradient difference is substantially zero and the road surface R is flat. That is, the positive and negative of the road surface gradient difference Δθ are switched at the boundary of Δθ = 0 (that is, Δθ1 <0, Δθ2> 0). The feature point detection range As0 at the road surface gradient difference Δθ = 0 is a range between the upper end Gu0 and the lower end Gd0. Although FIG. 3C exemplifies the case where the width of the feature point detection range As is constant when the road surface gradient difference Δθ is in the range of Δθ1 to Δθ2, it is needless to say that the width of the feature point detection range As May be changed in accordance with the road surface gradient difference Δθ.

  FIG. 4 shows an example of the feature point detection range As in an actual image. FIG. 4A is an example of the feature point detection range in the case where the vehicle 50 is located on a flat road surface R where the road surface gradient difference Δθ is substantially zero, for example. As shown in FIG. 4A, in the feature point detection range As0 in this case, the upper end Gu0 is a position of a predetermined length from the upper end of the image G, and the lower end Gd0 is predetermined from the lower end of the image G It is considered to be a position of a certain length. In other words, in the feature point detection range As0, detection of the feature point FP is not performed in a predetermined area at the upper end of the image G and a predetermined area at the lower end.

  On the other hand, FIG. 4B is an example of the feature point detection range As1 when the vehicle 50 is located on the road surface R where the road surface gradient difference Δθ is Δθ> 0. As shown in FIG. 4B, in the feature point detection range As1 in this case, the upper end Gu1 is substantially at the upper end of the image G, and the lower end Gd1 is raised to near the center position in the vertical direction of the image G. In other words, in the feature point detection range As1, detection of the feature point FP is not performed in the range from the upper end of the image G to the upper end Gu1 and in the range from the lower end Gd1 to the lower end of the image G. The relationship between the feature point detection range As0 and As1 will be described using FIG. 3A. The feature point detection range when the vehicle 50 is at the position P0 is the feature point detection range As0, and the vehicle 50 is at the position It can be considered that the feature point detection range in the case of P1 is the feature point detection range As1.

  Next, with reference to FIG. 5, a method of estimating the vehicle position according to the present embodiment will be described. FIG. 4 is a flowchart showing a flow of a vehicle position estimation processing program for estimating the vehicle position. When an instruction to start execution of the vehicle position estimation processing program shown in FIG. 5 is issued, the CPU 12A of the control unit 12 reads the present vehicle position estimation processing program from the storage unit such as the ROM 12B and develops it in the storage unit such as the RAM 12C. And run. In the present embodiment, it is assumed that a map is generated in advance and stored in the storage unit 16. Further, as the person drives the vehicle 50 from the state of stopping at the start point SP shown in FIG. 2B toward the goal point GP, the vehicle position estimation process is executed. However, the present invention is not limited to this, and the present vehicle position estimation processing program may be applied, for example, when the vehicle 50 is driven automatically from the start point SP to the goal point GP.

  In the present embodiment, although the present vehicle position estimation processing program is stored in advance in the ROM 12B or the like, the present invention is not limited to this. For example, a form in which the present vehicle position estimation processing program is provided in a state of being stored in a computer readable portable storage medium, a form in which it is distributed via communication means such as a network interface not shown May be

  In step S100, shooting is performed by the camera 14 to acquire a traveling image. The acquisition of the traveling image is performed every predetermined time after the vehicle 50 departs from the start point SP, for example, every 33 ms (milliseconds) which is a general video rate.

  In step S102, it is determined whether acquisition of a driving | running | working image is the first (1st time). In the vehicle position estimation process according to the present embodiment, the feature point detection range setting method and the key frame search method in the subsequent processes are changed depending on whether it is the first time or the second time or later. When step S102 becomes affirmation determination, it transfers to step S104, and when it becomes negative determination, it transfers to step S112.

In step S104, the feature point detection range As is set to a predetermined initial feature point detection range. This is because the road surface gradient difference Δθ has not been estimated in the first acquisition of the traveling image.
There is no limitation on the method of setting the initial feature point detection range, but as an example, the feature point detection range As0 disposed at the center of the upper and lower positions of the image G shown in FIG. As described above, the feature point detection range As0 has a limited detection range in the image G. By limiting the initial feature point detection range in this way, it is possible to suppress detection of feature points that are difficult to distinguish from each other on a road surface or the like, and to detect more easily distinguishable feature points from each other on a building or the like. it can.

  In step S106, detection of feature points and calculation of feature quantities of the detected feature points are executed. The above-described FAST may be used to detect feature points, and the ORB may be used to calculate feature values.

  In step S108, a key frame KF0 most similar to the traveling image is selected from the feature amounts calculated in step S106, using, for example, a Bag of Visual Words. The Bag of Visual Words is a vector quantization of many local features in an image into a histogram, and is a tool for determining the degree of similarity between images. Although extraction of key frames by the Bag of Visual Words takes a certain amount of time, this processing is acquisition of the first traveling image when the vehicle is stopped at the start point SP. You can use Visual Words.

  In step S110, the feature amounts are compared between the key frame KF0 and the traveling image to associate feature points (pairing). Since the feature points detected from the feature point detection range As0 include many feature points that can be easily distinguished from each other, they can be associated more accurately.

  In the next step S124, the position and orientation of the camera such that the projection error is minimized from the three-dimensional position (position on the map) of the feature point associated with the traveling image and the key frame KF0 and the position on the traveling image. Estimate Here, the projection error according to the present embodiment will be described with reference to FIG. 6, a, b, c shown in FIG. 6 are positions of feature points on the traveling image, A, B, C are positions of feature points in the map coordinate system CM associated with the map, a ′, b ′, c ′ These show the position which projected position A, B, C of the feature point matched on the map on the driving | running | working image. At this time, the projection error εa for the position A is the difference between the position a and the position a '. The same applies to the positions B and C. According to the correspondence of the feature points according to the present embodiment, the position and orientation of the camera can be estimated more accurately because there is less erroneous correspondence.

  In the next step S126, the position and orientation of the camera estimated in step S124 are converted into the position and orientation of the vehicle. The position of the own vehicle means the position on the map of the vehicle representative point X. In the present embodiment, since the relative positions of the vehicle representative point X and the camera 14 are known, the position and attitude of the camera are converted to the position of the vehicle representative point X based on the relative positional relationship. Since the conversion is a simple operation, the position of the host vehicle while traveling can be estimated in a short time, and the delay time of the vehicle motion control can be reduced. As a result, since the vehicle can be guided to the target route with high accuracy, for example, the vehicle can be parked in a narrow place, and the parking space can be saved.

  In step S128, it is determined whether acquisition of images has ended at all traveling shooting points. In the case of the first time, this determination is a negative determination, so the process returns to step S100, and acquisition of the traveling image is continued.

  On the other hand, in step S112, a key frame KFf closest to a position ahead of the previously estimated position of the camera 14 by a predetermined distance, for example, 5 m, is searched and selected. More specifically, since the distance to the road surface R appearing at the center of the image is known in advance from the mounting angle of the camera 14, the key frame closest to that position is selected.

  In step S114, the deviation (θ1-θf) between the camera pitch angle θf of the key frame KFf and the previously estimated camera pitch angle θ1 is taken as the road surface gradient difference Δθf.

In step S116, the feature point detection range As is set based on the road surface gradient difference Δθ f set in step S114, using the diagram shown in FIG. 3 (c). That is, the upper end Gu and the lower end Gd of the feature point detection range As are set in accordance with the magnitude of the road surface gradient difference Δθf. Therefore, regardless of the road surface gradient difference Δθ, it is possible to suppress detection of feature points that are difficult to distinguish from each other on the road surface, and to detect more easily distinguishable feature points on a building or the like (FIG. 4). (B)).
Further, by limiting the feature point detection range As, the number of detection of feature points can be reduced, and therefore, the amount of calculation required to calculate feature amounts and to associate feature points can be reduced. Therefore, even in a system with limited computational resources, it is possible to estimate the vehicle position in a short time.

  In step S118, detection of feature points and calculation of feature quantities of the detected feature points are executed in the same manner as step S106.

  In step S120, a key frame KF1 including the largest number of feature points associated previously is selected.

  In step S122, the feature amounts are compared between the key frame KF1 and the traveling image to associate feature points (pairing). In the present embodiment, since many feature points that are easy to distinguish from each other are included as feature points to be compared, regardless of the road surface gradient difference Δθ, more accurate correspondence is possible.

  Next, steps S124 to S128 described above are executed. However, in step S124, the key frame KF0 is read as KF1. Thereafter, when the determination is affirmative in step S128, the present vehicle position estimation processing program is ended.

  As described above in detail, according to the vehicle position estimation device, the vehicle position estimation method, and the vehicle position estimation program according to the present embodiment, even in a system in which the computational resources are limited, the vehicle position can be shortened. It can be estimated by In the present embodiment, since the detection range of the feature point from the image is limited, the detection time can be reduced. Furthermore, since the number of detected feature points is reduced, it is possible to reduce the amount of calculation required to calculate feature amounts and to associate feature points. Therefore, even in a system with limited computational resources, it is possible to estimate the vehicle position in a short time.

  Further, according to the vehicle position estimation device, the vehicle position estimation method, and the vehicle position estimation program according to the present embodiment, even when there is a change in road surface gradient, the vehicle position can be estimated with high accuracy. In the present embodiment, by changing the feature point detection range in the traveling image according to the road surface gradient of the vehicle position compared to the front of the camera, more feature points which are easily distinguishable from each other on the building are detected. This is because the feature points can be associated more accurately between the key frame and the traveling image in order to detect feature points that are difficult to distinguish from each other, such as on the road surface, less frequently. Therefore, even if there is a change in the road surface gradient, it is possible to estimate the vehicle position with higher accuracy with the minimum number of detections.

  Furthermore, when applied to the automatic parking system, the vehicle can be guided to the target route with high accuracy, so the vehicle can be parked in a narrow space, and the parking space can be saved. In the present embodiment, since the position of the host vehicle while traveling can be estimated in a short time, the delay time of the vehicle motion control can be reduced. Therefore, since the vehicle can be guided to the target route with high accuracy, the vehicle can be parked in a narrow space, and the parking space can be saved.

10 Vehicle position estimation device 12 control unit 12A CPU
12B ROM
12C RAM
12D I / O
12E bus 14 camera 16 memory unit 18 display unit 30 building 50 vehicle CP map shooting point FP feature point G image road surface S travel locus SP start point GP goal point WS wall surface X vehicle representative point θf, θ pitch angle Δθ road slope difference

Claims (8)

The predetermined traveling associated with each of a plurality of traveling images photographed by the photographing means on a predetermined traveling path and a plurality of reference images photographed by the photographing means at a plurality of positions along the predetermined traveling path The position and posture of the vehicle on the predetermined traveling route by collating the environmental feature points in the environment along the route and the map information including the position and posture of the photographing means at the time of photographing the plurality of reference images Vehicle position estimation device for estimating
Setting means for estimating a road surface gradient difference of the traveling route and setting a detection range of a corresponding feature point corresponding to the environmental feature point in the traveling image from the road surface gradient difference estimated;
Based on the corresponding feature points detected in the detection range, a reference image most similar to the traveling image is selected from the plurality of reference images, and the self image is selected based on the correspondence between the environment feature points and the corresponding feature points. An estimation unit for estimating the position and attitude of a vehicle. The map information includes position coordinates of the environmental feature point in the map information,
The estimation unit estimates the position and orientation of the imaging unit on the basis of the position coordinates of the environment feature point associated with each other and the position of the corresponding feature point on the traveling image, and the imaging unit estimated The vehicle position estimation apparatus according to claim 1, wherein the vehicle position and posture are estimated by converting the vehicle position and posture into a representative point of the vehicle. The map information includes feature quantities indicating features of each of the environment feature points,
The estimation means selects a reference image including the largest number of previously associated feature points as the closest approximation image, and a feature of the environmental feature points of the approximation closest image to the feature amount of the corresponding feature point. The vehicle position estimation device according to claim 2, wherein the environment feature point and the corresponding feature point are associated by comparing the amount with the amount. The photographing means is fixed at a predetermined position of the vehicle so as to photograph a predetermined area in front of the photographing means.
The setting means selects a reference image most approximate to the predetermined area from the plurality of reference images, and previously photographed the road surface gradient according to the position and attitude of the photographing means associated with the selected reference image. The vehicle position estimation apparatus according to any one of claims 1 to 3, wherein the road surface gradient difference is estimated from a difference between the road surface gradient and the position and attitude of the photographing means estimated for the traveling image. The own vehicle position estimation device according to any one of claims 1 to 4, wherein the setting means moves the detection range in the vertical direction of the traveling image according to the road surface gradient difference. The vehicle position estimation method according to claim 5, wherein the setting unit sets the position of the detection range on the traveling image at a higher position as the road surface gradient of the vehicle position is larger than the predetermined region. apparatus. The predetermined traveling associated with each of a plurality of traveling images photographed by the photographing means on a predetermined traveling path and a plurality of reference images photographed by the photographing means at a plurality of positions along the predetermined traveling path The position and posture of the vehicle on the predetermined traveling route by collating the environmental feature points in the environment along the route and the map information including the position and posture of the photographing means at the time of photographing the plurality of reference images A vehicle position estimation method using a vehicle position estimation device for estimating
Setting means for estimating a road surface gradient difference of the traveling route and setting a detection range of a corresponding feature point corresponding to the environmental feature point in the traveling image from the road surface gradient difference estimated;
Based on the corresponding feature points detected in the detection range, the estimation means selects a reference image closest to the traveling image from the plurality of reference images, and associates the environment feature points with the corresponding feature points Vehicle position estimation method that estimates the position and attitude of the vehicle based on. The predetermined traveling associated with each of a plurality of traveling images photographed by the photographing means on a predetermined traveling path and a plurality of reference images photographed by the photographing means at a plurality of positions along the predetermined traveling path The position and posture of the vehicle on the predetermined traveling route by collating the environmental feature points in the environment along the route and the map information including the position and posture of the photographing means at the time of photographing the plurality of reference images A program for controlling a vehicle position estimation device that estimates
Setting means for estimating the road surface gradient difference of the traveling route on a computer and setting the detection range of the corresponding feature point corresponding to the environmental feature point in the traveling image from the road surface gradient difference estimated;
Based on the corresponding feature points detected in the detection range, a reference image most similar to the traveling image is selected from the plurality of reference images, and the self image is selected based on the correspondence between the environment feature points and the corresponding feature points. A vehicle position estimation program that functions as an estimation unit that estimates the position and attitude of a vehicle.