JP2022095273A - Self-position estimating device, mobile body, self-position estimating method, and self-position estimating program - Google Patents

Abstract

To provide a self-position estimating device capable of improving continuity of self-position estimation results and supporting traveling of an appropriate mobile body, a mobile body, a self-position estimating method, and a self-position estimating program.SOLUTION: A self-position estimating unit 26 acquires a plurality of prior estimation positions of a mobile body 50 on the basis of a plurality of data corresponding to a plurality of position information among databases, and estimates the self-location from the plurality of prior estimation positions. This allows the self-position estimating unit 26 to suppress a rapid change of estimation results due to data switching and the like by estimating the self-location using the plurality of data. For this reason, continuity of self-position estimation results is improved, thereby making it possible to support traveling of an appropriate mobile body.SELECTED DRAWING: Figure 14

Description

The present invention relates to a self-position estimation device, a moving body, a self-position estimation method, and a self-position estimation program.

As a conventional self-position estimation device, the one described in Patent Document 1 is known. This self-position estimation device estimates the self-position by using an image acquired in advance and an image acquired while the moving body is traveling. Here, since it takes a long time to estimate the position from the image, this self-position estimation device complements the estimation result of the self-position during the calculation by using a mechanical sensor (odometry). ..

Japanese Unexamined Patent Publication No. 2018-81080

Here, the estimation accuracy when the self-position is estimated from the image is not always good. In this case, if the running support of the moving body is provided using the estimation result of the self-position with low estimation accuracy, it is possible that the moving body cannot provide appropriate running support and the moving body runs off the route. There is sex. Further, a method may be adopted in which a database is prepared in advance for each position of the traveling route and the self-position is estimated by using the data in the database during traveling. However, in this method, when the self-position is estimated at a position far from the position where the data is set, the estimation accuracy may decrease. Further, at the place where the data used for estimating the self-position is switched, the estimated self-position may change suddenly and become discontinuous.

Therefore, the present invention provides a self-position estimation device, a moving body, a self-position estimation method, and a self-position estimation program that can improve the continuity of the self-position estimation result and provide appropriate running support for the moving body. The purpose is to do.

The self-position estimation device according to one aspect of the present invention is for estimating the self-position of a moving body by matching a feature extracted from an acquired image with a database in which position information and the feature are associated in advance. By matching the image acquisition unit that acquires an image, the extraction unit that extracts features from the image acquired by the image acquisition unit, the features extracted by the extraction unit, and the database, which is a self-position estimation device. The estimation unit includes an estimation unit that estimates the self-position of the moving object, and the estimation unit acquires a plurality of pre-estimated positions of the moving object based on a plurality of data corresponding to a plurality of position information in the database, and a plurality of pre-estimated positions. Estimate the self-position from the pre-estimated position.

The self-position estimation device is for estimating the self-position of a moving body by matching a feature extracted from an acquired image with a database in which position information and the feature are associated in advance. Here, for example, the image acquisition unit may acquire an image at a position away from the position when the data in the database was created. In such a case, the data used for estimating the self-position is switched to the data at another position. Therefore, when the estimation unit estimates the self-position with only one data, the estimated self-position may change suddenly. On the other hand, the estimation unit acquires a plurality of pre-estimated positions of the moving body based on a plurality of data corresponding to a plurality of position information in the database, and estimates the self-position from the plurality of pre-estimated positions. As a result, the estimation unit can suppress sudden changes in the estimation result due to data switching or the like by estimating the self-position using not only one data but also a plurality of data. Therefore, it is possible to improve the continuity of the estimation result of the self-position and to support the running of an appropriate moving object.

The estimation unit may select at least two nearest data with respect to the previously estimated self-position and estimate the self-position based on at least two selected data. As a result, the estimation unit can estimate the self-position based on the data existing near the moving object, so that it is easy to match the features of the acquired image with the data, and the estimation accuracy can be improved. ..

The self-position estimation device further includes an evaluation unit that evaluates the estimation accuracy of the self-position, and the estimation unit estimates the self-position by a plurality of pre-estimated positions when the evaluation by the evaluation unit is below the threshold value. It's okay. As a result, the estimation unit can estimate the self-position by using a plurality of data only at the required timing, so that the calculation load can be reduced.

The estimation unit calculates the matching rate with the data that is the basis of the pre-estimated position for each pre-estimated position, and estimates so that the final self-position estimation result is biased toward the pre-estimated position with a high matching rate. You may go. As a result, the estimation unit can estimate the self-position in a state where the data having a high matching rate is weighted, so that the estimation accuracy can be improved.

The moving body according to the present invention includes the above-mentioned self-position estimation device.

The self-position estimation method according to the present invention is for self-position estimation for estimating the self-position of a moving body by matching a feature extracted from an acquired image with a database in which position information and the feature are associated in advance. It is a method of moving objects by matching the image acquisition step of acquiring an image, the extraction step of extracting features from the image acquired in the image acquisition step, the features extracted in the extraction step, and the database. The estimation step includes an estimation step for estimating a self-position, and in the estimation step, a plurality of pre-estimated positions of a moving object are acquired based on a plurality of data corresponding to a plurality of position information in a database, and a plurality of pre-estimated positions are acquired. Estimate the self-position from.

The self-position estimation program according to the present invention is a self-position estimation for estimating the self-position of a moving body by matching a feature extracted from an acquired image with a database in which position information and the feature are associated in advance. It is a program, and it is a moving body by matching the image acquisition step to acquire the image, the extraction step to extract the features from the image acquired in the image acquisition step, the features extracted in the extraction step, and the database. An estimation step for estimating a self-position is performed by a computer system, and in the estimation step, a plurality of pre-estimated positions of a moving object are acquired based on a plurality of data corresponding to a plurality of position information in a database, and a plurality of pre-estimated positions are acquired. Estimate the self-position from the pre-estimated position of.

According to these moving objects, a self-position estimation method, and a self-position estimation program, the same effect as the above-mentioned self-position estimation device can be obtained.

INDUSTRIAL APPLICABILITY According to the present invention, a self-position estimation device, a moving body, a self-position estimation method, and a self-position estimation program capable of improving the continuity of the self-position estimation result and performing appropriate running support for a moving body are provided. can do.

It is a schematic diagram which shows the self-position estimation system provided with the self-position estimation apparatus which concerns on embodiment of this invention. It is a block block composition diagram which shows the block structure of the moving body which includes the self-position estimation apparatus which concerns on embodiment of this invention. It is a schematic diagram which shows the state of the workplace at the time of creating a database. It is a flowchart which shows the creation method of a database. It is a figure for demonstrating the method of extracting a feature. It is a figure which shows the image acquired by a camera, and the extracted feature. It is a conceptual diagram which shows the method of acquiring the 3D coordinates of a feature. It is a schematic diagram which shows the state when a moving body performs automatic traveling. FIG. 9 is a flowchart showing a method of estimating the self-position of the moving body. It is a graph which shows the relationship between the distance from the position corresponding to the position information registered in a database, and the estimation accuracy of a self-position. It is a conceptual diagram which shows the state at the time of evaluating a feature point. It is a conceptual diagram which shows the running support situation of a moving body in a running path. It is a flowchart which shows the evaluation of the estimation accuracy of self-position, and the method of running support of a moving body using the evaluation result. It is a conceptual diagram for demonstrating the content of self-position estimation by a self-position estimation part. It is a flowchart which shows the content of the self-position estimation by the self-position estimation unit. It is a figure which shows the experimental result which evaluated the estimation result of self-position.

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

FIG. 1 is a schematic view showing a self-position estimation system 100 including a self-position estimation device 1 according to the present embodiment. As shown in FIG. 1, the self-position estimation system 100 includes a self-position estimation device 1 provided for each of a plurality of mobile bodies 50, and a management unit 2.

In this embodiment, a forklift is exemplified as the moving body 50. FIG. 1 shows a state in which a forklift as a moving body 50 is carrying out cargo loading / unloading work at a work place E (predetermined area) such as a warehouse or a factory. In the workplace E, a plurality of shelves 60 are arranged. Further, a passage 61 for the moving body 50 to pass through is formed between the shelves 60 and the shelves 60. The self-position estimation device 1 is a device that estimates the self-position of the moving body 50 in such a workplace E. The self-position estimation device 1 estimates the self-position of the moving body 50 by matching the features extracted from the acquired image with the database in which the position information and the features are associated in advance. The moving body 50 can automatically travel in the work place E by using the self-position estimated by the self-position estimation device 1. The detailed configuration of the self-position estimation device 1 will be described later. The management unit 2 is a server that manages a plurality of mobile bodies 50 in the workplace E. The management unit 2 receives predetermined information from the plurality of moving bodies 50 as necessary, and transmits predetermined information to the plurality of moving bodies 50.

FIG. 2 is a block configuration diagram showing a block configuration of a mobile body 50 including the self-position estimation device 1 according to the present embodiment. As shown in FIG. 2, the moving body 50 includes a traveling unit 11, a camera 12, and a control unit 20. The traveling unit 11 is a drive system such as a motor that generates a driving force for traveling the moving body 50. The camera 12 (image acquisition unit) is a device that acquires an image around the moving body 50. The camera 12 transmits the acquired image to the self-position estimation unit 26.

The control unit 20 includes an ECU [Electronic Control Unit] that collectively manages the moving body 50. The ECU is an electronic control unit having a CPU [Central Processing Unit], a ROM [Read Only Memory], a RAM [Random Access Memory], a CAN [Controller Area Network] communication circuit, and the like. In the ECU, for example, various functions are realized by loading the program stored in the ROM into the RAM and executing the program loaded in the RAM in the CPU. The control unit 20 includes a route planning unit 21, a command speed calculation unit 22, a communication unit 23, a storage unit 24, a self-position estimation unit 26 (extraction unit, estimation unit), an odometry calculation unit 27, and a self-position. A determination unit 28, an evaluation unit 31, and a switching unit 32 are provided. Of these, the self-position estimation device 1 is configured by the storage unit 24, the self-position estimation unit 26, the odometry calculation unit 27, the self-position determination unit 28, the evaluation unit 31, the switching unit 32, and the camera 12.

The route planning unit 21 plans a route for the moving body 50 to move. The route planning unit 21 sets the departure position and the destination position in the work place E, and plans the route to the destination position. The route planning unit 21 transmits information on the planned route to the command speed calculation unit 22. The command speed calculation unit 22 calculates the command speed for the traveling unit 11, that is, the command rotation speed for the motor. The command speed calculation unit 22 calculates the command rotation speed based on the route transmitted from the route planning unit 21 and the self-position transmitted from the self-positioning unit 28. The communication unit 23 communicates with the traveling unit 11. The communication unit 23 transmits a control signal necessary for traveling to the traveling unit 11. The communication unit 23 acquires an encoder value from an encoder (not shown) and transmits the encoder value to the odometry calculation unit 27.

Next, each component of the self-position estimation device 1 will be described. The storage unit 24 stores a database necessary for self-position estimation. The database is a group of information in which position information is associated with features extracted from an image acquired at the position in advance. The database has a plurality of data in which certain location information and features are associated with each other. The storage unit 24 transmits the database to the self-position estimation unit 26.

The self-position estimation unit 26 extracts features from the image acquired by the camera 12. Further, the self-position estimation unit 26 estimates the self-position of the moving body 50 by matching the features extracted from the image acquired by the camera 12 with the database transmitted from the storage unit 24. The self-position estimation unit 26 transmits the estimated self-position to the self-position determination unit 28.

The odometry calculation unit 27 calculates the self-position by the odometry based on the encoder value acquired from the communication unit 23. The odometry calculation unit 27 can acquire the self-position by a simple calculation without using the image of the camera 12. The odometry calculation unit 27 transmits the self-position by the odometry to the self-positioning unit 28. The self-positioning unit 28 comprehensively determines the self-position from the self-position estimation unit 26 and the self-position from the odometry calculation unit 27, and determines the self-position of the moving body 50. The self-positioning unit 28 corrects the self-position by correcting the odometry error by using the self-position estimation result from the self-position estimation unit 26 using an image while using the self-position from the odometry calculation unit 27 as a base. decide. The self-position determination unit 28 transmits the determined self-position to the command speed calculation unit 22.

Here, a method of creating a database will be described with reference to FIGS. 3 to 6. The database is created in advance before the moving body 50 actually performs the work in the workplace E while estimating its own position. The database is created by acquiring images at points important for driving in the workplace E, calculating information necessary for position estimation, creating a database, and linking it to the map of the workplace E. An important point is, for example, a position numbered from "1" to "12" in FIG. In the following description, the position of the number "1" shall be referred to as the "first important point". Places with other numbers are also referred to as "nth important points". Here, it is assumed that the mobile body 50 shown in FIG. 2 is used for creating the database. As will be described later, since the self-position estimation unit 26 has a function of extracting features from images and a function of matching images with each other, it is assumed that the self-position estimation unit 26 performs various processes for creating a database. .. However, the device used for creating the database is not particularly limited, and any device that can execute the subsequent processing may be adopted.

FIG. 4 is a flowchart showing a method of creating a database. First, the camera 12 acquires one image to be stored in the database and one image (s) around the image (step S10: image acquisition step). For example, when the image at the first important point is used as a database, the camera 12 acquires one image at the first important point and another image from the vicinity of the first important point. Next, the self-position estimation unit 26 performs image preprocessing so that features can be extracted (step S20). For example, the camera 12 may have a fisheye lens so that it can capture a wide range. At this time, since the object in the image is distorted, preprocessing is performed to adjust the image to be close to the actual appearance.

Next, the self-position estimation unit 26 extracts features from each of the two images (step S30: extraction step). Here, a method of extracting features in an image will be described with reference to FIG. FIG. 5 is a diagram for explaining a method of extracting features. As shown in FIG. 5A, the self-position estimation unit 26 compares the brightness of the determination pixel X and the surrounding pixels (here, 16 pixels) into three patterns of “bright”, “dark”, and “same”. Stratify. For example, the self-position estimation unit 26 determines that the peripheral pixel for which the relationship "determination pixel (X) -peripheral pixel (i)> light / dark threshold value" is established is "bright", and "determination pixel (X) -peripheral pixel (i). ) Peripheral pixels that hold the relationship <brightness threshold "are determined to be" dark ", and peripheral pixels that hold the relationship" (absolute value of (judgment pixel (X) -peripheral pixel (i)) <brightness threshold" are "same". Is determined.

Here, the self-position estimation unit 26 extracts the determination pixel X as a feature in the image when the number of consecutive "bright" or "dark" peripheral pixels is equal to or greater than the corner threshold value. For example, when the corner threshold is set to "12", the determination pixel X in FIG. 5B is extracted as a feature because the peripheral pixels of "bright" are 12 consecutive or more. Since the determination pixel X in FIG. 5C has only 11 consecutive "bright" peripheral pixels, the determination pixel X is discarded as a feature. As a result, as shown in FIG. 6, the corners of structures such as shelves, floors, ceilings, and walls in the image PC are extracted as features (feature points FP). In addition, when luggage, installation objects, moving objects, etc. are shown in the image, the corners of those objects are also extracted as features.

In the present specification, the pixel extracted as a feature may be referred to as "feature point FP". The feature in the image used for self-position estimation is not only a point but also a line or a predetermined shape. That is, the feature is limited as long as it is a part that can be extracted by image processing as a characteristic part in the image and can be matched with the part extracted by another image. Not done.

Returning to FIG. 4, the self-position estimation unit 26 matches the features of the two images (step S40). Then, the self-position estimation unit 26 performs three-dimensional restoration of the feature and registers it as a database. In the database, the position information of the important point (the position where the image is acquired), the image coordinates of the feature in the image, and the three-dimensional coordinates of the feature are registered in an associated form. The position information also includes the posture of the camera 12 at the time of shooting. For example, as shown in FIG. 7, feature points G, R, and B are extracted from the image PC1 at important points, and their image coordinates are specified. In addition, the position information of the place where the image PC1 is acquired is also specified. Feature points G, R, and B are also extracted from the image PC2 acquired around the important points, and their image coordinates are specified. In addition, the position information of the place where the image PC 2 is acquired is also specified. The positional relationship between the surrounding shooting position and the important point is grasped by a method such as associating an odometry value or estimating from an image. The self-position estimation unit 26 matches the features of the image PC 1 and the image PC 2 with each other. As a result, the self-position estimation unit 26 can acquire the three-dimensional coordinates of the feature points G, R, and B in the same manner as triangulation.

Returning to FIG. 4, when the process of step S50 is completed, the database creation of one image is completed. The moving body 50 moves to the next important point, acquires an image with the camera 12 at the important point, and performs the process of FIG. 4 again. In this way, a database of images is created at all important points in the workplace E. The created database is stored in the storage unit 24.

Next, with reference to FIGS. 8 and 9, a method of automatic traveling of the moving body 50 and a self-position estimation method by the self-position estimation unit 26 for automatic traveling will be described. For example, it is assumed that the moving body 50 automatically travels from the current position ST shown in FIG. 8 to the eighth important point which is the target position GL. At this time, the self-position estimation unit 26 estimates where the moving body 50 currently exists. As a result, the self-positioning unit 28 can determine that the current position ST is "2 m east of the tenth important point". Then, the route planning unit 21 plans a route of "48 m east to the 11th important point and 16 m north to the 8th important point". As a result, the moving body automatically travels. The self-position estimation unit 26 is not limited in how many data are used when estimating the self-position, and as described later, the self-position estimation unit 26 estimates the self-position using the data of a plurality of positions. However, in order to promote understanding of the contents, a case of self-position estimation using one data will be described here as an example. The method of estimating the self-position using a plurality of data will be described later.

FIG. 9 is a flowchart showing a method of estimating the self-position of the moving body 50. The self-position estimation unit 26 executes the process shown in FIG. 9 in order to estimate the self-position of the moving body 50. As shown in FIG. 9, the self-position estimation unit 26 acquires a moving image of the moving body 50 from the camera 12 (step S110: image acquisition step). For example, the camera 12 acquires an image at the current position ST near the tenth important point (see FIG. 8). Next, the self-position estimation unit 26 performs image preprocessing so that features can be extracted (step S120). Next, the self-position estimation unit 26 extracts features from the moving image (step S130: extraction step). In steps S120 and S130, processing to the same effect as in steps S20 and S30 of FIG. 4 is performed. As a result, when the image PC as shown in FIG. 6 is acquired, the feature point FP is acquired.

Next, the self-position estimation unit 26 matches the features extracted in step S130 with the features of the image in the database (step S140: estimation step). Then, the self-position estimation unit 26 estimates the self-position of the moving body 50 (step S150: estimation step).

For example, when an image similar to the image PC shown in FIG. 6 is obtained while the moving body 50 is traveling, the self-position estimation unit 26 may extract a plurality of feature point FPs similar to those in FIG. can. Then, the self-position estimation unit 26 compares the feature points with the feature points in the image of the database. When the image PC shown in FIG. 6 is obtained at the tenth important point at the time of database creation and the plurality of feature point FPs shown in FIG. 6 are extracted, the self-position estimation unit 26 together with the feature points extracted in step S130. , Can be matched with the feature point FP shown in FIG. As a method of matching feature points, for example, a method of calculating a feature amount descriptor of a feature point and matching the feature point having the shortest distance from the feature points in the database can be mentioned, but the method is particularly suitable. A known method may be used without limitation.

Here, if the location taken during driving deviates from the tenth important point and the posture taken during driving deviates from the posture taken during database creation, the image taken during driving The image coordinates of the feature points and the feature points are slightly deviated from the image coordinates of the image PC and the feature point FP in FIG. Therefore, in step S150, the self-position estimation unit 26 links the coordinates of any three feature points from the database to the three feature points of the moving image. Then, the self-position estimation unit 26 estimates the position and posture of the moving body 50 by using a known three-point method technique. The three feature points used in step S150 are randomly selected from a large number of feature points existing in the image.

Next, the evaluation unit 31 and the switching unit 32 shown in FIG. 2 will be described in detail. The evaluation unit 31 evaluates the estimation accuracy of the self-position estimation unit 26. Here, with reference to FIG. 10, the relationship between the distance from the position corresponding to the position information registered in the database and the estimation accuracy of the self-position will be described. The dots plotted on the straight line indicated as "running" on the upper side of the graph in FIG. 10 indicate the positions corresponding to the position information registered in the database. On the lower side of FIG. 10, a graph showing the distance from the position corresponding to the position information registered in the database at each position in the traveling range of the moving body 50 and the error of the estimation result of the self-position are shown. There is. The graph shows the results of prior experiments. The position corresponding to the position information registered in the database is the position of the above-mentioned "1st to 12th important points", and is the position where the image was acquired in advance for creating the database. The farther the shooting position (that is, the position for self-position estimation) during driving is from each important point, the greater the change in the appearance of the image from what is registered in the database. Therefore, as shown in FIG. 10, as the distance from each important point increases, the estimation accuracy of the self-position estimation unit 26 decreases.

Specifically, the evaluation unit 31 evaluates the estimation accuracy based on the matching between the feature and the database. The evaluation unit 31 matches the feature point FP extracted by the self-position estimation unit 26 with the feature point FP of the image registered in the database. The evaluation unit 31 evaluates that the estimation accuracy of the self-position estimation unit 26 is high when the number of successful matchings (or the matching success rate) at this time is high, and evaluates that the estimation accuracy of the self-position estimation unit 26 is low when it is low. do. The evaluation unit 31 sets a threshold value in advance for the number of successful matchings (matching success rate), and evaluates that the estimation accuracy is high when the threshold value is equal to or higher than the threshold value. The threshold value may be a fixed value or a value estimated from an image acquired in advance.

Specifically, as shown in FIG. 11A, when the self-position estimation is performed by the three-point method using the feature points R, G, B, the evaluation unit 31 performs the three-point feature points R, G. , B The success or failure of matching is automatically determined for the feature point FP other than B. As shown in FIG. 11B, when determining the success or failure of matching of the feature points FP other than the three feature points R, G, and B, the feature point FP to be determined is in the image coordinates of the image PC1 of the database. Is indicated by "feature point FP1", and the feature point FP to be determined is indicated by "feature point FP2" in the image coordinates of the moving image PC2. The evaluation unit 31 reprojects the three-dimensional coordinate point of the feature point FP1 onto the image PC2. The reprojected feature point FP1 is indicated as "feature point FP2a" in the image coordinates of the image PC2. The evaluation unit 31 acquires the magnitude of the error between the feature point FP2a and the feature point FP1 and determines whether or not the error is equal to or less than the threshold value. When the error is equal to or less than the threshold value, the evaluation unit 31 determines that the matching of the feature point FPs related to the determination target is successful, and increments the count of the number of successful matchings by one. On the other hand, when the error is larger than the threshold value, the evaluation unit 31 determines that the matching of the feature point FP related to the determination target has failed and does not count.

The switching unit 32 switches whether or not to use the self-position estimation result by the self-position estimation unit 26 for driving support based on the evaluation result by the evaluation unit 31. That is, in the case of an estimation result that is highly evaluated by the evaluation unit 31, the switching unit 32 makes the estimation result by the self-position estimation unit 26 used for driving support. Specifically, the switching unit 32 is set so that the estimation result of the self-position estimation unit 26 is used in the self-position determination unit 28. As a result, the self-positioning unit 28 determines the self-position by correcting the odometry error using the self-position estimation result from the self-position estimation unit 26 using the image. On the other hand, when the evaluation result by the evaluation unit 31 is low, the switching unit 32 prevents the estimation result by the self-position estimation unit 26 from being used in the traveling support. That is, when the evaluation result by the evaluation unit 31 is low, the switching unit 32 switches to the traveling support based only on the traveling information from the traveling unit 11 of the moving body 50. The traveling information here is odometry based on the encoder value from the traveling unit 11. Specifically, the switching unit 32 switches the setting so that the estimation result of the self-position estimation unit 26 is not used in the self-position determination unit 28. As a result, the self-positioning unit 28 determines the self-position without correcting the odometry error without using the self-position estimation result from the self-position estimation unit 26 using the image.

FIG. 12 is a conceptual diagram showing a traveling support situation of the moving body 50 on the traveling route. The dots plotted on the straight line indicated as "running" in FIG. 12 indicate the positions (each important point) corresponding to the position information registered in the database. In the region E1 near the important point, the moving body 50 travels by traveling support by correcting the odometry error by using the self-position estimation result from the self-position estimation unit 26. On the other hand, the moving body 50 travels in the region E2 far from the important point by the traveling support based only on the odometry.

Next, with reference to FIG. 13, an evaluation of the estimation accuracy of the self-position and a method of supporting the traveling of the moving body using the evaluation result will be described. FIG. 13 is a flowchart showing an evaluation of the estimation accuracy of the self-position and a method of supporting the traveling of the moving body using the evaluation result. This process is executed after the self-position estimation unit 26 estimates the self-position at a certain position on the travel path.

First, the evaluation unit 31 evaluates the estimation accuracy of the self-position estimation by the self-position estimation unit 26 (step S210: evaluation step). The evaluation unit 31 evaluates based on the number of successful matchings (success rate) of the features extracted by the self-position estimation unit 26.

Next, the evaluation unit 31 determines whether or not the evaluation result in step S210 is high (step S220: evaluation step). If it is determined in step S220 that the evaluation is high, the switching unit 32 sets the estimation result by the self-position estimation unit 26 to be used in the traveling support (step S230: switching step). On the other hand, when it is determined in step S220 that the evaluation is low, the switching unit 32 prevents the estimation result by the self-position estimation unit 26 from being used in the driving support, and the driving support is performed based only on the odometry. Set (step S240: switching step). As a result, the process shown in FIG. 13 is completed.

Next, with reference to FIGS. 14 to 16, a configuration for the self-position estimation unit 26 to further estimate an appropriate self-position will be described. FIG. 14 is a conceptual diagram for explaining the content of self-position estimation by the self-position estimation unit 26. FIG. 15 is a flowchart showing the contents of self-position estimation by the self-position estimation unit 26. FIG. 16 is a diagram showing an experimental result in which the estimation result of the self-position was evaluated.

In the above-described embodiment, the self-position estimation unit 26 estimates the self-position based on one of the nearest data. On the other hand, the self-position estimation unit 26 may estimate the self-position using a plurality of data. That is, the self-position estimation unit 26 acquires a plurality of pre-estimated positions of the moving body 50 based on a plurality of data corresponding to a plurality of position information in the database. Further, the self-position estimation unit 26 estimates the self-position from a plurality of pre-estimated positions. The self-position estimation unit 26 selects at least two nearest data with respect to the previously estimated self-position, and estimates the self-position based on at least two selected data.

First, as shown in FIG. 15, the self-position estimation unit 26 selects a plurality of data to be matched (step S310: estimation step). Here, the self-position estimation unit 26 selects two nearest data with respect to the previously estimated self-position. Specifically, as shown in FIG. 14A, the database has a plurality of data corresponding to the position information at each position of the traveling path RL of the moving body 50. Since four data are set in FIG. 14A, they are referred to as data DT1, DT2, DT3, and DT4. For example, when the self-position LP related to the previous estimation by the moving body 50 exists between the position corresponding to the data DT1 and the position corresponding to the data DT2, the self-position estimation unit 26 sets the data to be matched as the data to be matched. Select the closest data D1 and the second closest data D2.

Next, as shown in FIG. 15, the self-position estimation unit 26 performs matching with the data selected in step S310 and acquires a plurality of pre-estimated positions (step S320: estimation step). As for the specific content of matching the features extracted from the image acquired by the moving body 50 at the current position with each data, the same method as the method of estimating the self-position is adopted in the explanation with reference to FIG. To. For example, in FIG. 14A, the self-position estimation unit 26 acquires the pre-estimated position BP1 by matching with the data DT1. Further, the self-position estimation unit 26 acquires the pre-estimated position BP2 by matching with the data DT2. Since the data to be matched are different and the self-position estimated using each data is slightly different from the actual self-position, the pre-estimated position BP1 and the pre-estimated position BP2 are set to different positions. ..

Next, as shown in FIG. 15, the self-position estimation unit 26 estimates the self-position from the plurality of pre-estimated positions acquired in step S320 (step S330: estimation step). The self-position estimation unit 26 is set at an intermediate position between a plurality of pre-estimated positions. At this time, the self-position estimation unit 26 calculates the matching rate with the data that is the basis of the pre-estimated position for each pre-estimated position. The matching rate is a value indicating the degree to which the pre-estimated position matches the original data, and if the degree can be indicated, the specific calculation method is Not particularly limited. Although not particularly limited, the matching rate is, for example, the sum of the deviations at each feature point when the feature points extracted in step S130 and the feature point FP shown in FIG. 6 are matched. Calculated by applying any index. Further, the self-position estimation unit 26 estimates so that the final self-position estimation result is biased toward the pre-estimated position having a high matching rate. For example, if the matching ratios of the two pre-estimated positions are equal (1: 1 ratio), the self-position is set at the midpoint of the two pre-estimated positions. On the other hand, when the matching rate of one of the pre-estimated positions is high, the self-position is closer to the one of the pre-estimated positions than the midpoint by the amount corresponding to the ratio.

Specifically, as shown in FIG. 14B, the matching rate of the pre-estimated position BP1 with respect to the data DT1 is 90%, and the matching rate of the pre-estimated position BP2 with respect to the data DT2 is 50%. In this case, the matching rate of the pre-estimated position BP1 is 1.8 times higher than the matching rate of the pre-estimated position BP2. Therefore, the estimated self-position EP is set closer to the pre-estimated position BP1 by the magnification than the midpoint between the pre-estimated position BP1 and the pre-estimated position BP2. As shown in FIG. 14 (c), the matching rate of the pre-estimated position BP1 with respect to the data DT1 is 75%, and the matching rate of the pre-estimated position BP2 with respect to the data DT2 is 70%. In this case, there is almost no difference between the matching rate of the pre-estimated position BP1 and the matching rate of the pre-estimated position BP2. Therefore, the estimated self-position EP is set at substantially the same position as the midpoint between the pre-estimated position BP1 and the pre-estimated position BP2, although it is slightly closer to the pre-estimated position BP1. The specific calculation method for how the ratio of the matching rate is reflected in the bias of the self-position is not particularly limited, and an arbitrary calculation method such as calculating the center of gravity assuming that the high matching rate is the weight is not particularly limited. May be adopted. As a result, the process of estimating the self-position shown in FIG. 15 is completed.

As described above, the evaluation unit 31 may intervene in the estimation of the self-position using a plurality of data. For example, in a normal state, the self-position estimation unit 26 may estimate the self-position using one data, and when the estimation accuracy is lowered, execute the self-position estimation using a plurality of data. That is, the evaluation unit 31 compares the estimation accuracy of the self-position based on one data with the threshold value. Then, when the evaluation of the evaluation unit 31 exceeds the threshold value, running support using the self-position is performed. On the other hand, when the evaluation by the evaluation unit 31 is equal to or less than the threshold value, the self-position estimation unit 26 estimates the self-position by the above-mentioned plurality of pre-estimated positions. It should be noted that a threshold value for evaluation may be set for the estimation accuracy of the self-position using a plurality of data. As a result, if it is evaluated that the estimation accuracy is low even if the self-position is estimated using a plurality of data, it may be switched to the running support by odometry.

Next, the actions / effects of the self-position estimation device 1, the moving body 50, the self-position estimation method, and the self-position estimation program according to the present embodiment will be described.

The self-position estimation device 1 is for estimating the self-position of a moving body by matching a feature extracted from an acquired image with a database in which position information and the feature are associated in advance. Here, for example, the camera 12 may acquire an image at a position away from the position when the data in the database was created. In such a case, the data used for estimating the self-position is switched to the data at another position. Therefore, when the self-position estimation unit 26 estimates the self-position with only one data, the estimated self-position may change suddenly.

For example, FIG. 16A shows a travel path RL in which a self-position estimation device 1 is actually mounted on a forklift and the forklift travels while estimating the self-position. The black dots on the travel route indicate the position where the data is created. The forklift travels from the start point SP to the goal point GP. First, an experiment was conducted in which the forklift was run along the travel path RL while estimating the self-position using only one data, and the estimated self-position at each position was totaled. Of these, FIG. 16B shows the estimation result of the self-position of the section surrounded by the virtual line. In FIG. 16B, the self-position estimation locus EL drawn by the estimated self-position is shown. In the section, the data used is switched at the intermediate position CT between the data DT1 and the data DT2. That is, in the region on the data DT1 side of the intermediate position CT, the self-position is estimated based on the data DT1. On the other hand, in the region on the data DT2 side of the intermediate position CT, the self-position is estimated based on the data DT1. Further, in the vicinity of the intermediate position CT, since the distance is far from any of the data DT1 and DT2, the estimation accuracy is also lowered. As a result, in the intermediate position CT, the estimation result of the self-position suddenly changes due to the switching of the data used. As a result, the self-position estimation locus EL is cut off and becomes discontinuous.

On the other hand, the self-position estimation unit 26 acquires a plurality of pre-estimated positions of the moving body 50 based on a plurality of data corresponding to a plurality of position information in the database, and self-positions from the plurality of pre-estimated positions. To estimate. As a result, the self-position estimation unit 26 estimates the self-position using not only one data but also a plurality of data, so that the estimation result can be suppressed from suddenly changing due to data switching or the like. Therefore, it is possible to improve the continuity of the estimation result of the self-position and to support the running of an appropriate moving object.

In the case of estimating the self-position using the two nearest data, an experiment was conducted in which the estimation results of the self-position were aggregated while the forklift was traveling on the travel path RL shown in FIG. 16 (a). Of these, FIG. 16 (c) shows the estimation result of the self-position of the section surrounded by the virtual line. In FIG. 16 (c), the self-position is estimated based on the data DT1 and the data DT2 in both the area on the data DT1 side and the area on the data DT2 side of the intermediate position CT. That is, by the time the forklift travels from the position of the data DT1 to the position of the data DT2, the self-position estimation unit 26 can estimate the self-position using the data DT1 and DT2 without switching the data in the middle. .. Further, by using two data DT1 and DT2, the estimation accuracy can be improved as compared with the case of one data. Therefore, even in the intermediate position CT, a continuous locus is drawn without interruption of the self-position estimation locus EL. Here, when the forklift advances from DT2, the next data is used instead of the data DT1. That is, data switching occurs, but at a position near the data DT2, since the data DT2 exists at a close position, the self-position can be estimated with high accuracy. Therefore, even if data switching occurs, the estimation result of the self-position does not change significantly. From the above, it can be seen that the robustness can be improved and the continuity of the self-position estimation result can be improved. The evaluation value of the estimation accuracy when self-position estimation is performed using one data when traveling on the travel path RL in FIG. 16A is 28 mm, whereas the evaluation value of the estimation accuracy is 21 mm when two data are used. Was able to be reduced to. The evaluation value is a variation in error when the true value and the estimation result are compared.

The self-position estimation unit 26 may select at least two nearest data with respect to the previously estimated self-position and estimate the self-position based on at least two selected data. As a result, the self-position estimation unit 26 can estimate the self-position based on the data existing near the moving body 50, so that it is easy to match the features of the acquired image with the data and the estimation accuracy is improved. can do.

The self-position estimation device 1 further includes an evaluation unit 31 for evaluating the estimation accuracy of the self-position, and the self-position estimation unit 26 is self-determined by a plurality of pre-estimated positions when the evaluation by the evaluation unit 31 is equal to or less than the threshold value. Position estimation may be performed. As a result, the self-position estimation unit 26 can perform self-position estimation by using a plurality of data only at necessary timings, so that the calculation load can be reduced.

The self-position estimation unit 26 calculates the matching rate with the data that is the basis of the pre-estimated position for each pre-estimated position, so that the final self-position estimation result is biased to the pre-estimated position with a high matching rate. May make an estimate. As a result, the self-position estimation unit 26 can estimate the self-position in a state where the data having a high matching rate is weighted, so that the estimation accuracy can be improved.

The moving body 50 according to the present embodiment includes the above-mentioned self-position estimation device 1.

In the self-position estimation method according to the present embodiment, the self-position for estimating the self-position of the moving body by matching the feature extracted from the acquired image with the database in which the position information and the feature are associated in advance. It is an estimation method, and it is a moving object by matching the image acquisition step of acquiring an image, the extraction step of extracting features from the image acquired in the image acquisition step, the features extracted in the extraction step, and the database. In the estimation step, a plurality of pre-estimated positions of a moving body are acquired based on a plurality of data corresponding to a plurality of position information in a database, and a plurality of pre-estimations are performed. The self-position is estimated from the position.

The self-position estimation program according to the present invention is a self-position estimation for estimating the self-position of a moving body by matching a feature extracted from an acquired image with a database in which position information and the feature are associated in advance. It is a program, and it is a moving body by matching the image acquisition step to acquire the image, the extraction step to extract the features from the image acquired in the image acquisition step, the features extracted in the extraction step, and the database. An estimation step for estimating a self-position is performed by a computer system, and in the estimation step, a plurality of pre-estimated positions of a moving object are acquired based on a plurality of data corresponding to a plurality of position information in a database, and a plurality of pre-estimated positions are acquired. Estimate the self-position from the pre-estimated position of.

According to the moving body 50, the self-position estimation method, and the self-position estimation program, the same effect as the above-mentioned self-position estimation device can be obtained.

The present invention is not limited to the above-described embodiment.

The self-position estimation device 1 may perform additional processing in addition to the above-described embodiment. For example, the evaluation unit 31 may evaluate how far the estimation accuracy is from the position corresponding to the position information registered in the database. This makes it possible to increase the location information registered in the database for areas where the estimation accuracy tends to decrease. This process may be performed particularly at the time of creating the database shown in FIG. As a result, in an area where the estimation accuracy tends to decrease (for example, an environment where many objects exist nearby), a large number of databases can be set by setting many important points. As a result, the estimation accuracy can be improved. Further, in an area where the estimation accuracy is difficult to decrease (for example, an open environment), it is possible to set a small number of important points. As a result, the data capacity can be reduced. The process may be performed while the mobile body 50 is traveling, and if necessary, the important points may be increased later and the database may be newly registered.

Further, the evaluation unit 31 may set a position evaluated as having high estimation accuracy in the traveling range of the moving body 50 as a position for positioning the moving body 50. For example, when the moving body 50 loses its self-position, the moving body 50 performs self-position estimation for all databases and evaluates the estimation accuracy as the highest (for example, the highest number of successful matches). It is possible to return to running after grasping the self-position.

Further, the evaluation unit 31 may evaluate the database itself based on the evaluation results at each position in the traveling range of the moving body 50. For example, if the evaluation of the estimation accuracy does not improve even when the position corresponding to the position information registered in the database is approached, the database itself may have failed to be created. Therefore, when the evaluation unit 31 evaluates the database itself, it becomes possible to take measures against the situation. For example, a process such as issuing a warning to the administrator or recreating the database may be performed. The evaluation of the database itself may be performed using the images collected at the time of prior teaching, or may be performed during traveling.

Further, the evaluation unit 31 may evaluate the degree of deviation from the route of the moving body 50 based on the evaluation result at each position of the traveling range of the moving body 50. For example, if the evaluation of the estimation accuracy does not improve even when the position corresponding to the position information registered in the database is approached, the moving body 50 may be warped from the route itself. Therefore, the evaluation unit 31 evaluates the degree of deviation from the path of the moving body, so that it is possible to take measures against the situation. In this case, the moving body 50 is temporarily stopped, and the self-position estimation is repeated in the stopped state. When the variation in the iterative calculation result is small, the self-position estimation device 1 considers that the position estimation accuracy is high and performs odometry correction. When the variation of the iterative calculation result is large, the self-position estimation device 1 considers that the correction is difficult and issues a warning to the administrator.

In the above embodiment, the moving body 50 includes all the components of the self-position estimation device. Instead of this, the management unit 2 may have some functions of the self-position estimation device.

1 ... Self-position estimation device, 12 ... Camera (image acquisition unit), 26 ... Self-position estimation unit (extraction unit, estimation unit), 31 ... Evaluation unit, 50 ... Moving object.

Claims (7)

It is a self-position estimation device for estimating the self-position of a moving body by matching a feature extracted from an acquired image with a database in which position information and the feature are associated in advance.
An image acquisition unit that acquires the image and
An extraction unit that extracts the features from the image acquired by the image acquisition unit, and an extraction unit.
It is provided with an estimation unit that estimates the self-position of the moving body by matching the feature extracted by the extraction unit with the database.
The estimation unit is
A plurality of pre-estimated positions of the moving body are acquired based on a plurality of data corresponding to the plurality of the position information in the database.
A self-position estimation device that estimates the self-position from a plurality of the pre-estimated positions. The self-position estimation device according to claim 1, wherein the estimation unit selects at least two nearest data with respect to the previously estimated self-position and estimates the self-position based on at least two selected data. Further equipped with an evaluation unit for evaluating the estimation accuracy of the self-position,
The self-position estimation device according to claim 1 or 2, wherein the estimation unit estimates the self-position by a plurality of the pre-estimated positions when the evaluation by the evaluation unit becomes equal to or less than a threshold value. The estimation unit is
The matching rate with the data that is the basis of the pre-estimated position is calculated for each of the pre-estimated positions.
The self-position estimation device according to any one of claims 1 to 3, wherein the final self-position estimation result is estimated so as to be biased toward the pre-estimated position having a high matching rate. A mobile body including the self-position estimation device according to any one of claims 1 to 4. It is a self-position estimation method for estimating the self-position of a moving body by matching a feature extracted from an acquired image with a database in which position information and the feature are associated in advance.
The image acquisition step for acquiring the image and
An extraction step for extracting the feature from the image acquired in the image acquisition step, and an extraction step.
It comprises an estimation step of estimating the self-position of the moving body by matching the feature extracted in the extraction step with the database.
In the estimation step,
A plurality of pre-estimated positions of the moving body are acquired based on a plurality of data corresponding to the plurality of the position information in the database.
A self-position estimation method for estimating the self-position from a plurality of the pre-estimated positions. It is a self-position estimation program for estimating the self-position of a moving object by matching the features extracted from the acquired image with the database in which the position information and the features are associated in advance.
The image acquisition step for acquiring the image and
An extraction step for extracting the feature from the image acquired in the image acquisition step, and an extraction step.
The computer system is made to execute the estimation step of estimating the self-position of the moving object by matching the feature extracted in the extraction step with the database.
In the estimation step,
A plurality of pre-estimated positions of the moving body are acquired based on a plurality of data corresponding to the plurality of the position information in the database.
A self-position estimation program that estimates the self-position from a plurality of the pre-estimated positions.

Images