JP6645051B2 - Self-position estimation device, self-position estimation method and program - Google Patents

Description

  The present invention relates to a self-position estimation device, a self-position estimation method, and a program for estimating a self-position of a moving object or the like.

  The feature points in the environment and the position information of the feature points are stored in advance, and the stored feature points are compared with actual feature points extracted from an image captured by the capturing means of the mobile body, to thereby determine the position of the mobile body. A self-position estimating device for estimating a self-position is known (see Patent Document 1).

JP 2011-248441 A

  In the self-position estimation device, for example, when the viewpoint of the photographing unit for extracting the actual feature point is different from the viewpoint when the feature point is stored in advance, the self-position estimation accuracy may be reduced. is there.

  The present invention has been made to solve such a problem, and has as its main object to provide a self-position estimating device, a self-position estimating method, and a program capable of improving the self-position estimating accuracy. .

One embodiment of the present invention for achieving the above object is a storage unit that stores a plurality of images captured in an environment, each feature point of each image, and position information of each feature point in association with each other. Provided on a moving body moving in the environment, an image obtaining means for obtaining an image around the moving body, a feature point extracting means for extracting an actual feature point from the image obtained by the image obtaining means, Matching is performed to determine whether a feature point of each image stored in the storage unit matches an actual feature point of the image extracted by the feature point extraction unit, and the feature point of the storage unit and the feature point extraction are performed. Matching means for associating the extracted actual feature points with the means, and the geometrical positional relationship between the position of the actual feature point and the position of the feature point corresponding to the actual feature point stored in the storage means. Based on the self-position of the moving body And a self-position calculating unit that outputs the image. A self-position estimating device for matching with actual feature points extracted from the image obtained by the image obtaining means.
In this aspect, the storage unit may store images obtained by photographing the environment from a plurality of viewpoints.
In this aspect, the apparatus may further include a feature point removing unit that removes a dynamic feature point of the dynamic object from the actual feature points extracted by the feature point extracting unit.
In this aspect, the matching unit performs matching between each actual feature point extracted from the image acquired by the image acquisition unit and each feature point of an image group stored in the storage unit. For each image in the storage means, calculate the number of matched feature points, sort the images in the storage means in descending order of the number of matched feature points, and store the images stored in the storage means in the sorted order. An image may be selected from the group, and matching between a feature point included in the selected image and an actual feature point extracted from the image obtained by the image obtaining unit may be performed.
One embodiment of the present invention for achieving the above object includes a step of storing a plurality of images photographed in an environment, each feature point of each image, and position information of each feature point in association with each other, Provided on a moving body moving in the environment, acquiring an image around the moving body, extracting actual feature points from the acquired image, and feature points of each of the stored images. Performing matching to determine coincidence with the actual feature points of the extracted image, and associating the stored feature points with the extracted actual feature points; and Calculating the self-position of the moving object based on a geometrical positional relationship between the position of the feature point corresponding to an actual feature point, and a high similarity image with the acquired image. The feature points included in the In preference to the point, the actual feature point extracted from the acquired image, matching, it may be a self-position estimation method according to claim.
One embodiment of the present invention for achieving the above object includes a process of storing a plurality of images captured in an environment, each feature point of each image, and position information of each feature point in association with each other. A step of obtaining an image around the moving body, the processing being provided on a moving body moving in the environment, extracting actual feature points from the obtained image, and having a high degree of similarity with the obtained image. Matching for determining a match between a feature point included in the stored image and an actual feature point extracted from the acquired image by giving priority to a feature point included in the other stored image. Performing the process of associating the stored feature points with the extracted actual feature points, and the geometric position of the positions of the actual feature points and the positions of the feature points corresponding to the actual feature points Processing for calculating the self-position of the moving object based on the relationship , It may be a program for causing a computer to execute the.

  According to the present invention, it is possible to provide a self-position estimating device, a self-position estimating method, and a program that can improve the self-position estimating accuracy.

FIG. 1 is a block diagram illustrating a schematic system configuration of a self-position estimation device according to an embodiment of the present invention. FIG. 5 is a diagram illustrating correspondence between a feature point of a luminance image, a three-dimensional position of a sensor coordinate system, and a three-dimensional global position. FIG. 9 is a diagram illustrating an example of three-dimensional position data of a feature point in which a feature point index, an image index, and a three-dimensional global position are associated with each other. FIG. 4 is a diagram illustrating an example of mapping data stored in a memory. FIG. 7 is a diagram illustrating matching between each actual feature point of a current image and each feature point of each image of mapping data. FIG. 5 is a diagram illustrating an example of each image index of mapping data and the number of matching feature points. FIG. 9 is a diagram illustrating an example of mapping data in the order of a large number of matching feature points. FIG. 6 is a diagram for explaining matching with each actual feature point of a current image and each feature point of each image index of mapping data. It is a figure which shows each real feature point of a sensor coordinate system, and each feature point of each image of the mapping data of the corresponding global coordinate system. 5 is a flowchart illustrating a mapping processing flow of the self-position estimation device according to the embodiment of the present invention. 5 is a flowchart showing a self-position estimation processing flow of the self-position estimation device according to one embodiment of the present invention.

  Hereinafter, embodiments of the present invention will be described with reference to the drawings. The self-position estimating device according to the present embodiment is mounted on a moving object such as an autonomous mobile robot, and estimates the self-position of the moving object. The moving body moves based on the self-position estimated by the self-position estimation device.

  First, the self-position estimating device performs a mapping process of storing mapping data obtained by learning the entire scene in the environment. After that, after the mapping processing, the self-position estimating apparatus performs global self-position estimation processing by associating the acquired current image with the mapping data, and estimates the self-position of the moving object.

  FIG. 1 is a block diagram illustrating a schematic system configuration of the self-position estimation device according to the present embodiment. The self-position estimating device 1 according to the present embodiment includes an image acquisition unit 2, a feature point extraction unit 3, a correspondence unit 4, a feature point removal unit 5, a matching unit 6, a self-location calculation unit 7, A memory 8.

  The self-position estimating apparatus 1 includes, for example, a CPU (Central Processing Unit) that performs arithmetic processing and the like, a ROM (Read Only Memory) and a RAM (Random Access Memory) in which an arithmetic program executed by the CPU is stored. The hardware configuration is centered on a microcomputer including a memory, an interface unit (I / F) for inputting and outputting signals to and from the outside. The CPU, the memory, and the interface unit are mutually connected via a data bus or the like.

  The image acquisition unit 2 is a specific example of an image acquisition unit. The image acquisition unit 2 acquires an image around a moving object in the environment. The image acquisition unit 2 acquires, for example, a luminance image and a distance image around the moving object using a distance image sensor 21 such as an RGB-D sensor provided on the moving object.

  The feature point extracting unit 3 is a specific example of a feature point extracting unit. The feature point extraction unit 3 extracts a feature point from the luminance image acquired by the image acquisition unit 2. The feature points include, for example, edges, corners, and color distributions.

  The associating unit 4 associates the feature points extracted by the feature point extracting unit 3 with a three-dimensional position in a global coordinate system (hereinafter, a three-dimensional global position). The associating unit 4 associates the feature points of the images acquired from a plurality of viewpoints in the environment with the three-dimensional global position.

For example, mapping unit 4, based on the obtained distance image by the image acquisition unit 2, and calculates the three-dimensional position ^{S P} output sensor coordinate system of the feature point f of the extracted luminance image by the feature point extraction section 3 (FIG. 2). Note that the three-dimensional position ^{SP in the} sensor coordinate system is uniquely associated with the feature point f of the luminance image. Associating unit 4, based on the three-dimensional position ^{S P} output calculated sensor coordinate system, the following equation to calculate the three-dimensional global position ^{G P} corresponding to the feature point f extracted by the feature point extracting unit 3.
^G P ^{= G} _T ^{S S} P

In the above formula, ^G T _S is a homogeneous transformation matrix of position and orientation of the range image sensor 21 in the global coordinate system. The position and orientation T of the distance image sensor 21 in the global coordinate system can be measured or calculated by an arbitrary method. The associating unit 4 calculates the position and orientation T of the distance image sensor 21 in the global coordinate system according to the following method, for example, according to the purpose of the moving object and the constraint conditions (such as a sensor mounted on the moving object). May be.

  For example, the associating unit 4 calculates the position and orientation T of the distance image sensor 21 in the global coordinate system based on the result of dead reckoning (Dead-Reckoning: DR) in which the starting position of the moving object is the origin of the global coordinate system. I do. The associating unit 4 calculates the position and orientation T of the range image sensor 21 in the global coordinate system based on the position estimation result of the range image sensor 21 using Simultaneous Localization and Mapping (SLAM). The associating unit 4 calculates the position and orientation T of the range image sensor 21 in the global coordinate system based on the three-dimensional position measured by an external measuring device (such as a motion capture).

Associating unit 4, a three-dimensional global position ^{G P} of the calculated feature points, it associates the characteristic points.
The associating unit 4 attaches an index i (i = 1, 2,...) To each feature point and an index j (j = 1, 2,...) To each image, for example. Then, the associating unit 4 calculates the index i of the feature point (hereinafter, feature point index), the index j of the image from which the feature point is extracted (image index) j, and the three-dimensional global position P _{ij of the} feature point. (X _ij , y _ij , z _ij ) are stored in the memory 8 in three-dimensional position data (FIG. 3) of the feature points associated with each other.

  The associating unit 4 causes the memory 8 to store, as mapping data, an image group from which the feature points have been extracted, together with the three-dimensional position data of the feature points (FIG. 4). The memory 8 is a specific example of a storage unit. In addition, the self-position estimating device 1 may be configured to have no association unit 4. In this case, the mapping data is stored in the memory 8 in advance, or the mapping data is input to the memory 8 by a user.

  The feature point removing unit 5 is a specific example of a feature point removing unit. The feature point removing unit 5 extracts the real feature points (dynamic feature points) of the dynamic object from the real feature points (feature points extracted at the time of the self-position estimation process) extracted by the feature point extraction unit 3. Remove. This process is performed, for example, at the time of the self-position estimation process, if there is a moving dynamic object in the two temporally continuous images acquired above, the dynamic object is selected from the actual feature point group. The corresponding real feature points (dynamic feature points) are removed. Thus, since the self-position estimation can be performed using only the position-invariant feature points, the self-position estimation accuracy described later can be improved. The present dynamic feature point removal processing method is already disclosed in detail in Japanese Patent Application No. 2012-244540 proposed by the inventor of the present application, and this can be referred to. The self-position estimating apparatus 1 may be configured not to include the feature point removing unit 5. In this case, the configuration of the self-position estimation device 1 is further simplified, and the processing speed is improved.

  By the way, for example, when the viewpoint of the distance image sensor for extracting the actual feature point at the time of the self-position estimation processing of the moving object is different from the viewpoint of the distance image sensor for previously storing the feature point at the time of the mapping processing. In addition, there is a possibility that the self-position estimation accuracy is reduced.

  For example, as shown in FIG. 3, the same feature point may be present in a plurality of images of different viewpoints such that the same feature point 1 exists in images 1 and 3 having different viewpoints. Therefore, even for the same feature point 1, the corresponding three-dimensional global position is different between feature point 1 of image 1 and feature point 1 of image 3. Therefore, as described later, when the self-position estimation is performed using the three-dimensional global position, the self-position estimation accuracy varies depending on the selected image. In particular, when an image having a significantly different viewpoint is selected, the corresponding three-dimensional global position is greatly shifted, and the self-position estimation accuracy may be significantly reduced.

  On the other hand, in the present embodiment, the matching unit 8 replaces the feature point included in the image of the memory 8 having a high similarity with the image acquired by the image acquisition unit 6 with the feature point included in another image of the memory 8. The actual feature points extracted from the image acquired by the image acquisition unit 6 are matched with each other, and the feature points in the memory 8 are associated with the actual feature points. That is, the matching unit 6 selects an image from an image group stored in advance in the memory 8 in the order of similarity to the current image acquired by the image acquisition unit 2, and selects a feature included in the selected image. Matching is performed to determine whether a point matches an actual feature point extracted from the current image acquired by the image acquisition unit 2, and the feature point in the memory 8 is associated with the actual feature point. Then, the self-position calculating unit 7 calculates the self-position of the moving object based on the geometrical positional relationship between the position of the real feature point and the position of the feature point corresponding to the real feature point stored in the memory 8. calculate.

  Thereby, the matching unit 6 is stored in the memory 8 in the order of similarity to the current image acquired by the image acquisition unit 2, that is, in the order close to the viewpoint of the distance image sensor 21 at the time of the self-position estimation processing. An image can be selected from the group of images, and the feature points of the image can be matched with the actual feature points extracted from the current image. That is, an image close to the viewpoint of the range image sensor 21 at the time of the self-position estimation processing is preferentially selected, and the self-position can be estimated using the three-dimensional global position of the feature point of the image. Therefore, it is possible to suppress the error of the self-position estimation accuracy caused by the difference in the viewpoint of the distance image sensor 21 between the above-described mapping processing and the self-position estimation processing, and to improve the self-position estimation accuracy.

  Further, the memory 8 stores mapping data in which feature points of images acquired from a plurality of viewpoints in the environment are associated with three-dimensional global positions. The matching unit 6 uses the mapping data. Matching. Therefore, even if the viewpoints of the distance image sensor 21 at the time of the mapping process and at the time of the self-position estimation process are significantly different, the distance image sensor 21 at the time of the self-position estimation process is selected from the images acquired from the plurality of viewpoints. Can select a plurality of images closer to the viewpoint. Therefore, even when the viewpoints are different as described above, the self-position estimation can be performed with high accuracy and reliability.

  Further, images are successively selected from a group of images at a plurality of viewpoints stored in the memory 8 in the order of similarity to the current image, and feature points of the image and actual feature points extracted from the current image are selected. And a plurality of sets of feature points and actual feature points in the memory 8 are associated with each other. Then, the self-position calculating unit 7 calculates the self-position of the moving object based on the plurality of sets of the positions of the actual characteristic points and the positions of the characteristic points corresponding to the actual characteristic points stored in the memory 8. . As described above, by using more feature points, the self-position can be estimated with higher accuracy.

  The matching unit 6 includes a real feature point group of an image (current image) from which dynamic feature points have been removed by the feature point removing unit 5 and a feature point group of an image group of mapping data stored in the memory 8. Matching is performed to determine whether they match (FIG. 5). The matching unit 6 calculates the number of matching feature points (hereinafter, the number of matching feature points) for each image index of the mapping data. FIG. 6 is a diagram showing an example of each image index (image 1, image 2,...) Of the mapping data and the number of matching feature points.

  The matching unit 6 sorts the image indices in descending order of the number of matching feature points, for example, as shown in FIG. This means that the image indexes of the mapping data are arranged in descending order of similarity with the current image. That is, the matching unit 6 selects an image from the group of images of the mapping data stored in the memory 8 in the order of similarity with the current image, and selects each feature point included in the selected image and the current image. Is matched with each actual feature point extracted from.

  Note that the matching unit 6 arranges the image indexes of the mapping data in descending order of similarity with the current image based on the number of matching feature points, but is not limited thereto. For example, the matching unit 6 may arrange the image indexes of the mapping data in descending order of similarity with the current image based on the arrangement of each feature point. More specifically, the matching unit 6 compares the actual feature point group of the current image with the feature point group of the image group of the mapping data stored in the memory 8, Each image index may be sorted.

The matching unit 6 determines, for each image index, whether or not each actual feature point matches each feature point of each image index of the mapping data in the sorted order.
For example, as shown in FIG. 8, the matching unit 6 determines whether or not the actual feature point 1 of the current image matches each feature point of the image 2 of the mapping data (the image index with the largest number of matching feature points). (A). The matching unit 6 determines that the actual feature point 1 of the current image does not match each feature point of the image 2 of the mapping data. It is determined whether or not each feature point matches the second most common image index (b). Here, the matching unit 6 determines that the actual feature point 1 of the current image matches the feature point of the image 3 of the mapping data, extracts the feature point of the image 3 and associates the extracted feature point with the actual feature point 1 of the current image. . At this point, the matching unit 6 ends the process of determining the actual feature point 1 of the current image. Similarly, the matching unit 6 repeats the process of determining the actual feature points 2, 3, 4,... Of the current image, and associates each actual feature point with each feature point of each image of the mapping data.

  The matching unit 6 is configured based on each of the feature points of each image of the mapping data corresponding to each of the calculated actual feature points of the current image and the mapping data including the three-dimensional global position of the feature point stored in the memory 8. Then, the three-dimensional global position of each feature point corresponding to each actual feature point of the current image is calculated.

  The self-position calculator 7 is a specific example of a self-position calculator. The self-position calculator 7 calculates the distance image based on the global coordinate system based on the geometrical positional relationship between the position of each actual feature point of the current image obtained above and the position of each feature point of each image of the corresponding mapping data. The position of the sensor 21, that is, the self-position of the moving body is calculated.

  As shown in FIG. 9, the actual feature points 1, 2,... Obtained by the distance image sensor 21 have their positions measured on the basis of the sensor coordinate system, and the respective feature points of each image of the corresponding mapping data. .. Are represented on a global coordinate system basis.

  The self-position calculating unit 7 performs the alignment by associating each actual feature point of the obtained current image with each feature point of each image of the associated mapping data, thereby performing position adjustment. Is calculated, and the self-position is estimated. The self-position calculating unit 7 calculates, for example, a conversion parameter for adjusting the sensor coordinate system position of the actual feature point of the current image to the global coordinate system position of the feature point of the image of the associated mapping data. Then, the self-position calculation unit 7 calculates the position of the range image sensor 21 on the global coordinate system basis using the calculated conversion parameters.

  The self-position calculating unit 7 calculates the conversion parameters using, for example, RANSAC (Random Sample Consensus) or an ICP (Iterative Closest Point) algorithm. The self-position calculating unit 7 may calculate the conversion parameters by the following method, for example, according to the purpose of the moving object and the constraint conditions (such as a sensor mounted on the moving object). The method of calculating the self-position is an example, and the present invention is not limited to this, and a calculation method based on an arbitrary geometric positional relationship can be used.

  Next, the mapping processing flow and the self-position estimation processing flow of the above-described self-position estimation device will be described. FIG. 10 is a flowchart illustrating a mapping processing flow of the self-position estimation device according to the present embodiment.

The image acquisition unit 2 acquires a luminance image and a distance image in the environment from a plurality of viewpoints using the distance image sensor 21 (Step S101).
The feature point extraction unit 3 extracts a feature point from the luminance image acquired by the image acquisition unit 2 (Step S102). Here, the feature point removing unit 5 may remove the dynamic feature points from the feature point group extracted by the feature point extracting unit 3.

The associating unit 4 includes a feature point index i of each feature point extracted by the feature point extraction unit 3, an image index j of each image from which each feature point is extracted, and a three-dimensional global position of each feature point. The memory 8 stores the three-dimensional position data of the feature point in which P _ij = (x _ij , y _ij , z _ij ) is associated (step S103).
Finally, the associating unit 4 causes the memory 8 to store, as mapping data, the image group from which the feature points have been extracted, together with the three-dimensional position data of the feature points (step S104).

FIG. 11 is a flowchart illustrating a self-position estimation processing flow of the self-position estimation device according to the present embodiment.
The matching unit 6 reads the mapping data from the memory 8 (Step S201).
The image acquisition unit 2 acquires a luminance image and a distance image in the environment using the distance image sensor 21 (Step S202).

The feature point extraction unit 3 extracts actual feature points from the luminance image acquired by the image acquisition unit 2 (Step S203).
The feature point removing unit 5 removes dynamic feature points from the actual feature point group extracted by the feature point extracting unit 3 (Step S204).

The matching unit 6 performs matching between the actual feature points of the current image from which the dynamic feature points have been removed by the feature point removing unit 5 and the feature points of the image group of the mapping data stored in the memory 8 (step S205).
The matching unit 6 calculates the number of matching feature points for each image of the mapping data (step S206).

The matching unit 6 sorts the image indexes of the images in descending order of the number of matching feature points (step S207).
The matching unit 6 determines, for each image index, a match between each actual feature point of the current image and each feature point of each image of the mapping data in the sorted order, and associates the matched one (step S208).

  The self-position calculating unit 7 calculates the self-position of the moving object from the geometrical positional relationship between the position of each real feature point of the current image obtained above and the three-dimensional global position of each feature point of each image of the corresponding mapping data. The position is calculated (step S209).

As described above, in the self-position estimating apparatus according to the present embodiment, the memory 8 associates a plurality of images photographed in the environment, each feature point of each image, and position information of each feature point with each other. It is remembered. Then, the matching unit 8 gives priority to the feature points included in the image of the memory 8 having a high degree of similarity to the image acquired by the image acquisition unit 6 over the feature points included in other images of the memory 8, The actual feature points extracted from the image acquired by the acquisition unit 6 are matched with each other, and the feature points in the memory 8 are associated with the actual feature points. Then, the self-position calculating unit 7 calculates the self-position of the moving object based on the geometrical positional relationship between the position of the real feature point and the position of the feature point corresponding to the real feature point stored in the memory 8. calculate.
As a result, the matching unit 6 selects an image from the image group stored in the memory 8 in the order close to the viewpoint of the range image sensor 21 at the time of the self-position estimation processing, and selects the feature points of the image and the current image. Can be matched with the actual feature points extracted from. Therefore, it is possible to suppress the error of the self-position estimation accuracy caused by the difference in the viewpoint of the range image sensor 21 between the above-described mapping processing and the self-position estimation processing, and to improve the self-position estimation accuracy.

It should be noted that the present invention is not limited to the above-described embodiment, and can be appropriately changed without departing from the gist.
In the above embodiment, the self-position estimation device 1 is configured to be mounted on a moving body, but is not limited to this. For example, only the image acquisition unit 2 is provided on the moving object, and the feature point extracting unit 3, the associating unit 4, the feature point removing unit 5, the matching unit 6, and the self-position calculating unit 7 are provided outside the moving object. There may be.

Further, the present invention can be realized, for example, by causing the CPU to execute a computer program for the processing shown in FIG. 10 or FIG.
The program may be stored using various types of non-transitory computer readable media and provided to a computer. Non-transitory computer readable media include various types of tangible storage media. Examples of non-transitory computer readable media are magnetic recording media (eg, flexible disk, magnetic tape, hard disk drive), magneto-optical recording media (eg, magneto-optical disk), CD-ROM (Read Only Memory), CD-R, CD-R / W, semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (random access memory)).

  Also, the program may be supplied to the computer by various types of transitory computer readable media. Examples of transitory computer readable media include electrical signals, optical signals, and electromagnetic waves. Transitory computer readable media can provide the program to a computer via a wired communication line such as an electric wire and an optical fiber, or a wireless communication line.

  REFERENCE SIGNS LIST 1 self-position estimation device, 2 image acquisition unit, 3 feature point extraction unit, 4 correspondence unit, 5 feature point removal unit, 6 matching unit, 7 self-location calculation unit, 21 distance image sensor

Claims (4)

Storage means for storing a plurality of images taken of the environment, each feature point of each image, and position information of each feature point in association with each other,
Image acquisition means provided in a moving body moving in the environment, to acquire an image around the moving body,
Feature point extraction means for extracting actual feature points from the image acquired by the image acquisition means,
A feature point removing unit that removes a dynamic feature point of a dynamic object from among the actual feature points extracted by the feature point extracting unit;
Matching is performed to determine whether a feature point of each image stored in the storage unit matches an actual feature point of the image removed by the feature point removal unit, and the feature point of the storage unit and the feature point removal are performed. Matching means for associating the actual feature points removed by the means,
A self-position for calculating a self-position of the moving object based on a geometrical positional relationship between the position of the real feature point and the position of the feature point corresponding to the real feature point stored in the storage unit; Calculating means;
With
The matching unit is configured to prioritize feature points included in an image having a high degree of similarity to the image acquired by the image acquisition unit over feature points included in other images, and obtain an image acquired by the image acquisition unit. Matching with the actual feature points extracted from
In the storage means, images captured from a plurality of viewpoints in the environment is stored ,
The matching means continuously selects images from a group of images of a plurality of viewpoints stored in the storage means in descending order of the degree of similarity, and selects a feature point of the image and an actual feature point. Performing matching, associating a plurality of sets of feature points and actual feature points of the storage unit,
The self-position calculating means calculates the self-position of the moving body based on the positions of the plurality of sets of real feature points and the positions of the feature points corresponding to the real feature points stored in the storage means. Do
A self-position estimating device, characterized in that: The self-position estimating device according to claim 1 ,
The matching means,
Matching was performed between each actual feature point extracted from the image acquired by the image acquisition unit and each feature point of the image group stored in the storage unit, and a match was obtained for each image in the storage unit. Calculate the number of feature points,
Sorting each image of the storage means in descending order of the number of matching feature points,
In the sorted order, an image is selected from the image group stored in the storage unit, and the feature points included in the selected image and the actual feature points extracted from the image acquired by the image acquisition unit And matching,
A self-position estimating device characterized by the above-mentioned. Storing a plurality of images captured in the environment from a plurality of viewpoints , each feature point of each image, and position information of each feature point, in association with each other;
A step of obtaining an image around the moving body, the moving body being provided in the moving body;
Extracting real feature points from the obtained image;
Removing the dynamic feature points of the dynamic object from the extracted actual feature points;
A step of associating feature points of each image the storage, the actual feature point of the removed image, of for matching determining match, the actual feature point said stored feature points and the removal ,
Calculating the self-position of the moving object based on a geometrical positional relationship between the position of the actual feature point and the position of the feature point corresponding to the actual feature point;
Including
The feature points included in the image having a high degree of similarity to the obtained image are given priority over the feature points included in other images, and are matched with the actual feature points extracted from the obtained image ,
In the descending order of the degree of similarity, images are continuously selected from the stored image group of the plurality of viewpoints, and the feature points of the image and the actual feature points are matched, and the feature points and Associating multiple pairs with actual feature points,
Based on the plurality of sets of positions of actual feature points and the positions of feature points corresponding to the stored actual feature points, calculate a self-position of the moving body,
A self-position estimation method characterized by the above-mentioned. A process of storing a plurality of images captured in the environment from a plurality of viewpoints , each feature point of each image, and position information of each feature point, in association with each other;
A process for obtaining an image around the moving body, the processing being provided on the moving body moving in the environment;
A process of extracting actual feature points from the obtained image;
A process of removing a dynamic feature point of a dynamic object from the extracted actual feature points;
Feature points included in the acquired image similar high degree of the stored image, the stored been another priority than feature points included in the image, and the actual feature point said removal, the Performing matching to determine a match, and associating the stored feature points with the removed actual feature points;
A process of calculating a self-position of the moving body based on a geometric positional relationship between the position of the actual feature point and the position of the feature point corresponding to the actual feature point;
To the computer ,
In the descending order of the degree of similarity, images are continuously selected from the stored image group of the plurality of viewpoints, and the feature points of the image and the actual feature points are matched, and the feature points and Associating multiple pairs with actual feature points,
Based on the positions of the plurality of sets and the actual feature points and the positions of the feature points corresponding to the stored actual feature points, calculate the self-position of the moving body,
A program characterized by that:

Images