Patent ID: 12249089

DESCRIPTION OF EMBODIMENTS

First, an outline of an example embodiment of the present invention will be given with reference to the drawings. It should be noted that the drawing reference signs in the outline are given to each element for convenience as an example to facilitate understanding and are not intended to limit the present invention to the illustrated aspects. Further, connection lines between blocks in the drawings referred to in the following description can be both bidirectional and unidirectional. A unidirectional arrow schematically shows the main flow of a signal (data) and does not exclude bidirectionality. A program is executed by a computer apparatus and the computer apparatus comprises, for instance, a processor, storage device, input device, communication interface, and a display device as necessary. Further, the computer apparatus is configured to be able to perform wired or wireless communication with an internal device therein or external device (including a computer) via the communication interface. Although the input/output connection points of each block in the drawings also have ports or interfaces, these are not illustrated. Further, in the following description, “A and/or B” means A or B, or A and B.

In an example embodiment, the present invention can be realized by a self-position estimation apparatus10comprising a camera part11, an environmental data storage part12, a first estimation part13, a second estimation part14, a weighting determination part15, and a self-position calculation part16, as shown inFIG.1.

More concretely, the environmental data storage part12stores information that changes according to the position of a mobile object on a travel route in association with the position information thereof. The first estimation part13estimates the position of the mobile object from sensor data containing information that changes according to the position of the mobile object and the information in the environmental data storage part. As for this sensor data, sensor data obtained by a sensor17provided in the self-position estimation apparatus10or information acquired by the camera part11may be used, depending on the information held in the environmental data storage part.

The second estimation part14estimates the self-position of the mobile object from a known object included in an image inputted from the camera part11.

The weighting determination part15determines a weighting to the position estimation results by the first estimation part13and the second estimation part14.

The self-position calculation part16calculates a self-position by linearly combining the self-positions estimated by the first estimation part13and the second estimation part14using the result from the weighting determination part15.

According to the self-position estimation apparatus configured as described above, it becomes possible to increase the accuracy of estimating a self-position. The reason for this is that the configuration, in which the weighting determination part15and the self-position calculation part16are added in such a way that the second estimation part14can compensate a decrease in the position measurement accuracy caused by environmental data, is employed.

First Example Embodiment

Next, a first example embodiment of the present invention will be described in detail with reference to the drawings.FIG.2is a drawing illustrating an example of the configuration of a self-position estimation apparatus according to the first example embodiment of the present invention. With reference toFIG.2, the self-position estimation apparatus100comprises a camera part101, an environmental data storage part102, a first estimation part103, a second estimation part104, a weighting determination part105, a self-position calculation part106, a continuity determination part107, and a marking part108. Further, the following description assumes that the self-position estimation apparatus100is installed in an automated guided vehicle (referred to as “AGV” hereinafter) capable of autonomous driving and provides position information to the automated guided vehicle.

The camera part101is configured to include a camera capable of shooting surrounding environment of the AGV.

The environmental data storage part102stores environmental data in which images shot in advance along a travel route of the AGV are associated with (linked to) the shooting positions thereof.

The first estimation part103identifies an image similar to an image inputted from the camera part101in the environmental data in the environmental data storage part102and estimates the position of the automated guided vehicle (AGV) on the basis of the identified result.

The second estimation part104estimates the position of the automated guided vehicle (AGV) from an image inputted from the camera part101using VSLAM. The second estimation part104can be configured by a function of estimating the three-dimensional self-position and self-orientation of the camera and a function of estimating the three-dimensional position of an object in a camera image. The former function (localization) is achieved by solving a PnP problem using a plurality of corresponding three-dimensional points in continuous camera images. Further, the latter function (mapping) is achieved by a function of deriving a three-dimensional position using triangulation method using corresponding points in a plurality of images in which the self-position and self-orientation of the camera are known. For instance, the second estimation part104described above can be configured by using the same configuration as that of the information processing apparatus in the mobile object of Patent Literature 2.

The weighting determination part105determines weights assigned to the position estimation results obtained by the first estimation part103and the second estimation part104using the determination result from the continuity determination part107.

The self-position calculation part106linearly combines the positions estimated by the first estimation part103and the second estimation part104using the weights determined by the weighting determination part105and outputs the result as the position of the automated guided vehicle (AGV).

The continuity determination part107determines whether or not the position calculated by the self-position calculation part106is continuous (i.e., whether or not spatial discontinuity occurs between the positions at consecutive points in time). This continuity determination can be performed by whether or not a position at a time of t and a position at a time of t−1 fall within a predetermined range.

The marking part108marks the environmental information held by the environmental data storage part102with a predetermined marking on the basis of the determination result by the continuity determination part107.

Next, the operation of the present example embodiment will be described in detail with reference to the drawings.FIG.3is a drawing for explaining the operation of the self-position estimation apparatus according to the first example embodiment of the present invention. An image I shot by the camera part101is inputted to the first estimation part103and the second estimation part104.

The first estimation part103identifies an image similar to the image inputted from the camera part101in the environmental data in the environmental data storage part102and estimates a position xi_1of the automated guided vehicle (AGV) using the shooting position information associated with (linked to) the similar image.

The second estimation part104estimates a position xi_1of the automated guided vehicle (AGV) using VSLAM on the basis of the image inputted from the camera part101.

The position xi_1estimated by the first estimation part103and the position xi_2estimated by the second estimation part104are provided to the self-position calculation part106. The self-position calculation part106calculates a position xi=wixi_1+(1−wi)xi_2of the automated guided vehicle (AGV) by linearly combining the positions xi_1and x1_2using weights wi, (1−wi). Here, the weight wi(where 0<=wi<=1) is a weight applied to the position xi_1estimated by the first estimation part103. If the environmental data is an image sufficient to identify a position, it is possible to set that the weight wi=1 or a value close to 1. If the environmental data is not an image sufficient to identify a position, the decrease in the position estimation accuracy can be compensated by the second estimation part104by setting the weight wito be smaller than 1.

The position xiof the automated guided vehicle (AGV) calculated by the self-position calculation part106is supplied to the continuity determination part107. The continuity determination part107verifies whether or not that there is continuity of the trajectory of the automated guided vehicle (AGV) by deriving a distance between the position xiand a position xi-1supplied at the previous timing. The example ofFIG.3shows a case where the continuity determination part107determines that there is “no continuity”. In this case, the continuity determination part107notifies the weighting determination part105of a decrease value Δwiof the weight wi. The self-position calculation part106, which has received the notification of the decrease value Δwi, recalculates a position xi′ of the automated guided vehicle (AGV) using a weight wi−Δwi. The recalculated position xi′ is supplied to the continuity determination part107and it is again verified whether or not that there is the continuity of the trajectory of the automated guided vehicle (AGV).

If it is determined that there is no continuity, the continuity determination part107instructs the marking part108to mark the corresponding environmental data. The marking part108which has received the instruction identifies the environmental data used when it was determined that continuity had been lost and marks it with a marking. This marking can be used to identify the target environmental data when the environmental data is modified or recreated.

Meanwhile, when it is determined that there is continuity, the continuity determination part107instructs the self-position calculation part106to output the calculated position xi.

As described above, according to the present example embodiment, the position xiof the automated guided vehicle (AGV) can be calculated by assigning appropriate weights to the position estimated by the first estimation part103and the position estimated by the second estimation part104, and, in addition to that, it is possible to early detect position deviation and take countermeasures. More concretely, when the continuity determination part107determines that there is no continuity, the weighting determination part105is instructed to change the weight and the marking part108is instructed to perform marking. As a result, it becomes possible to recalculate the position xiof the automated guided vehicle (AGV) after increasing the weight of the position xi_2estimated by the second estimation part104and mark the causal environmental data.

FIG.4is a drawing illustrating movement trajectories of a mobile object shown as a reference example. The solid line inFIG.4shows a movement trajectory of the mobile object when the position thereof is estimated by using only the environmental data. In areas indicated by arrows, the movement trajectory of the mobile object deviates greatly from a prescribed route indicated by the dotted line. One of the causes of this position deviation is that the environmental data includes noise whereby the position estimation accuracy is degraded. According to the present example embodiment, when spatial discontinuity, such as the areas indicated by the arrows inFIG.4, occurs in time-series data of the position of the mobile object, this can be addressed by correcting the position by position estimation using VSLAM and marking the causal environmental data.

Second Example Embodiment

The following describes a second example embodiment which is configured in such a way that data measured using LiDAR is used as environmental data and weights are changed using the similarity in the first estimation part. Note that LiDAR stands for Light Detection and Ranging or Laser Imaging Detection and Ranging.

FIG.5is a drawing illustrating an example of the configuration of a self-position estimation apparatus according to the second example embodiment of the present invention. Since the basic configuration thereof is common to that of the first example embodiment, the differences will be mainly described below.

An environmental data storage part102astores environmental data in which visible light images shot using LiDAR in advance along a travel route of the AGV are associated with the shooting positions where the images were captured.

A first estimation part103acalculates the similarity between the environmental data in the environmental data storage part102aand an image inputted from the camera part101, identifies an image having the maximum similarity therebetween, and estimates that the identified image is the position of the automated guided vehicle (AGV). In the present example embodiment, an index called the SSIM (Structural SIMilarity) index is used as the similarity. Note that although, in the following description of the example embodiment, it is assumed that the index called the SSIM index is used as the similarity, a different similarity index, for example, such as a histogram comparison result may be used as the similarity.

Using an SSIM calculated by the first estimation part103a, a weighting determination part105acalculates a weight wiapplied to a position xi_1estimated by the first estimation part and notifies the self-position calculation part106thereof. Note that, as a calculation method of the weight wi, it is possible to use a method of calculating the weight wiby simply multiplying the SIMM by a predetermined constant. As a result, it becomes possible to set a weight according to the reliability of the result estimated by the first estimation part103a.

Since the operations of the second estimation part104and the self-position calculation part106are the same as those in the first example embodiment, the description thereof will be omitted.

A continuity determination part107adetermines whether or not there is spatial discontinuity by converting a position xicalculated by the self-position calculation part106into a self-position vector tiand comparing the difference series D(ti)=ti−ti−1 from a self-position vector ti−1 calculated previously with a threshold th. If it is determined that spatial discontinuity occurs at the position xicalculated by the self-position calculation part106as a result of the comparison, the continuity determination part107ainstructs a marking part108ato mark the environmental data. Further, the continuity determination part107ainstructs the weighting determination part105ato perform recalculation with the weight wiset to 0. As a result of this, the self-position calculation part106outputs a position xi=xi_2. In other words, the position xi_2estimated by the second estimation part104using VSLAM is calculated as the position xiof the automated guided vehicle (AGV).

Further, in the present example embodiment, if the environmental data identified by the first estimation part103aaccording to similarity has been already marked environmental data, this environmental data may be deleted. As a result, it is possible to avoid using environmental data estimated to have a lot of noise for identifying a position in position estimation thereafter.

As described above, according to the present example embodiment, it becomes possible to increase the position estimation accuracy, in addition to the effects of the first example embodiment. The reason for this is that it is configured that the weight is set dynamically according to the reliability of the result estimated by the first estimation part103a. Further, by avoiding using marked environmental data estimated to have a lot of noise, the degradation in the position estimation caused by this environmental data can be suppressed.

Third Example Embodiment

The following describes a third example embodiment in which depth data measured by a depth sensor is also used as the environmental data and the method for adjusting the weight wiwhen spatial discontinuity is detected is modified.

FIG.6is a drawing illustrating an example of the configuration of a self-position estimation apparatus according to the third example embodiment of the present invention. Since the basic configuration thereof is common to that of the first example embodiment, the differences will be mainly described below.

An environmental data storage part102bstores environmental data in which visible light images shot in advance along a travel route of the AGV as well as depth images are associated with the shooting positions.

A first estimation part103bcalculates the similarity between the environmental data in the environmental data storage part102band an image inputted from the camera part101and to identify an image having the maximum similarity therebetween. Then, the first estimation part103bestimates that the shooting position associated with the identified image is the position of the automated guided vehicle (AGV).

Using an SSIM calculated by the first estimation part103b, a weighting determination part105bcalculates a weight wiapplied to a position estimated by the first estimation part and notifies the self-position calculation part106of the calculation result. Note that, in the present example embodiment, as calculation method of the weight wi, it is possible to use a method for calculating the weight wiby simply multiplying the SIMM by a predetermined constant. As a result, it becomes possible to set a weight according to reliability of the results estimated by the first estimation part103busing both the visible light images and the depth images.

Since the operations of the second estimation part104and the self-position calculation part106are the same as those in the first example embodiment, the description thereof will be omitted.

The continuity determination part107bconverts a position xicalculated by the self-position calculation part106into a self-position vector ti. Then, the continuity determination part107bdetermines whether or not spatial discontinuity is occurring by comparing the difference series D(ti)=ti−ti-1between the self-position vector tiand a self-position vector ti-1at the time of the previous calculation with a first threshold th1. The continuity determination part107bfurther determines whether or not spatial discontinuity is occurring by comparing the difference series D(Ri)=Ri−Ri-1between a rotation matrix Riindicating the orientation of the automated guided vehicle (AGV) and a rotation matrix Ri-1at the time of the previous calculation with a second threshold th2.

If at least one of D(ti) and D(Ri) exceeds the threshold and it is determined that spatial discontinuity is occurring at the position xicalculated by the self-position calculation part106as a result of this comparison, the continuity determination part107binstructs the weighting determination part105bto perform recalculation after the weight wiis reduced to half. As a result, the self-position calculation part106outputs a position xi=wi÷2*xi_1+(1−wi÷2)*xi_2. In this expression, “÷” denotes division and “*” multiplication (the same applies in the present application hereinafter). In other words, the self-position is calculated so as to put emphasis on the position estimated by the second estimation part104using VSLAM. Further, the continuity determination part107binstructs a marking part108bto mark the environmental data and change the position information of the corresponding environmental data to the current recalculated position xi. As a result, from the next time onward, the position estimated by the first estimation part103bat the same position will be changed from xi_1to xi.

In the present example embodiment, for marked environmental data, the PnP (Perspective-n-Point) problem may be solved again after excluding the corresponding points used to calculate the shooting position of such environmental data, and update may be performed in such a way that the calculation result may be associated with the marked environmental data stored in the environmental data storage part102bas the shooting location thereof.

As described above, according to the present example embodiment, it becomes possible to increase the position estimation accuracy, in addition to the effects of the first example embodiment. The reason for this is that it is configured that the weight is set according to the reliability of the estimation results from both the visible light images and the depth images and that the position information of environmental data estimated to have a lot of noise is corrected. Further, by avoiding using marked environmental data estimated to have a lot of noise, the degradation in the position estimation caused by this environmental data can be suppressed.

Fourth Example Embodiment

The following describes a fourth example embodiment in which data created using SfM-MVS is used as the environmental data and the method for adjusting the weight wiwhen spatial discontinuity is detected is modified. Note that SfM-MVS stands for Structure from Motion/Multi-View Stereo and is a technology that creates a 3D model of the shot area using a plurality of photos shot by a camera.

FIG.7is a drawing illustrating an example of the configuration of a self-position estimation apparatus according to the fourth example embodiment of the present invention. Since the basic configuration thereof is common to that of the first example embodiment, the differences will be mainly described below.

An environmental data storage part102cstores environmental data in which visible light images shot in advance along a travel route of the AGV as well as SfM-MVS depth images are associated with the shooting positions.

A first estimation part103ccalculates the similarity between the environmental data in the environmental data storage part102cand an image inputted from the camera part101to identify an image having the maximum similarity therebetween and estimates that the identified image is the position of the automated guided vehicle (AGV).

Using an SSIM calculated by the first estimation part103c, a weighting determination part105ccalculates a weight wiapplied to a position xi_1estimated by the first estimation part and notifies the self-position calculation part106thereof. Note that, as a calculation method of the weight wi, it is possible to use a method of calculating the weight wiby simply multiplying the SIMM by a predetermined constant. As a result, it becomes possible to set a weight according to the reliability of the result estimated by the first estimation part103c.

Since the operations of the second estimation part104and the self-position calculation part106are the same as those in the first example embodiment, the description thereof will be omitted.

A continuity determination part107cof the present example embodiment determines whether or not spatial discontinuity is occurring by performing an outlier test on each of the difference series D(ti) and the difference series D(Ri). As described above, the difference series D(ti) is configured by the difference between the present self-position vector tiand the self-position vector ti-1at the time of the previous calculation. Likewise, the difference series D(Ri) is configured by the difference between the rotation matrix Riand the rotation matrix Ri-1at the time of the previous calculation.

When an abnormal value is detected from at least one of D(ti) and D(Ri) as a result of the outlier test, the continuity determination part107cdetermines that spatial discontinuity is occurring at the position xicalculated by the self-position calculation part106. In this case, the continuity determination part107cinstructs the weighting determination part105cto change the weight wiaccording to a predetermined rule and perform recalculation. As for the predetermined rule, it is possible to employ a method in which a new weight wi′ (=wi−Δwi) may be derived by subtracting a predetermined value Δwifrom the weight wi. As a result, the self-position calculation part106outputs a position xi=wi′*xi_1+(1−wi′)*xi_2. Then, the continuity determination part107cinstructs a marking part108cto mark the environmental data and change the position information of the corresponding environmental data to the position xirecalculated this time. As a result, from the next time onward, the position estimated by the first estimation part103cfor the same position will be changed from xi_1to xi.

As described above, according to the present example embodiment, it becomes possible to increase the position estimation accuracy, in addition to the effects of the first example embodiment. The reason for this is that it is configured that the weight is set according to the reliability of the result estimated by the first estimation part103cand that the position information of environmental data estimated to have a lot of noise is corrected. Further, in the present example embodiment, as described above, when it is determined that spatial discontinuity is occurring at the position xi, environmental data is updated by instructing to change the weight wito wi′=wi−Δwiaccording to a predetermined rule and to perform recalculation. As a result, it is possible to increase the position estimation accuracy thereafter in the present example embodiment.

While each example embodiment of the present invention has been described as above, it is to be understood that the present invention is not limited to the example embodiments above and that further modifications, replacements, and adjustments may be added without departing from the basic technical concept of the present invention. For instance, the configuration of each element and the expression of each mathematical formula shown in each drawing are examples to facilitate understanding of the present invention and are not limited to the configurations shown in the drawings.

For instance, it is described that visible light images and depth data, and so on, are used as the environmental data in the example embodiments described above, however, the environmental data is not limited to these examples. For instance, geomagnetic data mapping information that associates geomagnetic data measured in advance with the position information thereof may be used as the environmental data. In this case, the first estimation part13/103performs position estimation on the basis of geomagnetic data obtained by a geomagnetic sensor mounted in the mobile object. Then, the second estimation part14/104performs position estimation so as to complement the first estimation part13/103.

For instance, it is described that the self-position estimation apparatus100is mounted in an AGV in the example embodiments described above, however, the self-position estimation apparatus100may be mounted to in other mobile objects such as a drone (unmanned aerial vehicle), and so on.

Further, in the example embodiments described above, it is described that the weight wiis updated by wi−Δwiwhen the accuracy of a position when using the environmental data is problematic as an example, however, the mode of changing the weight wiis not limited thereto. For instance, it is possible to change the weight wiby multiplying the weight wiby a predetermined reduction rate α (where α is a positive value of 1 or less).

Further, although the first to the fourth example embodiments were described above, the example embodiments of the present invention are not limited thereto and the elements disclosed in the first to the fourth example embodiments can be appropriately combined to form a new modified example embodiment. For instance, one can employ a configuration in which LiDAR is used for the environmental data as in the second example embodiment and recalculation is instructed after a weight wiis reduced to half when spatial discontinuity between positions is detected as in the third example embodiment.

Further, the procedures described in the first to the fourth example embodiments above can be implemented by a program causing a computer (9000inFIG.8) that functions as the self-position estimation apparatus to realize the functions as the self-position estimation apparatus.FIG.8illustrates, as an example, such a computer configured to comprise a CPU (Central Processing Unit)9010, a communication interface9020, a memory9030, and an auxiliary storage device9040. In other words, the CPU9010inFIG.8executes a position estimation program and a self-position calculation program to perform updating each computation parameter held by the auxiliary storage device9040.

That is to say, each part (each processing means or function) of the self-position estimation apparatus described in each of the first to the fourth example embodiments above can be realized by a computer program causing a processor installed in the self-position estimation apparatus to execute each of the processes described above using the hardware thereof.

Finally, preferred modes of the present invention will be summarized.

Mode 1

Refer to the Self-Position Estimation Apparatus According to the First Aspect

Mode 2

The weighting determination part of the self-position estimation apparatus described above may be configured to calculate a weight w to the position estimation result by the first estimation part on the basis of the similarity between the information in the environmental data storage part and the sensor data and set a weight to the position estimation result by the second estimation part to (1−w).

Mode 3

The self-position estimation apparatus described above may be configured to further comprise a continuity determination part that determines whether or not the self-position calculated by the self-position calculation part is continuous and a marking part that marks the information held by the environmental data storage part with a predetermined marking on the basis of the result by the continuity determination part.

Mode 4

The weighting determination part of the self-position estimation apparatus described above may be configured to change the weighting to reduce the weight with respect to the first estimation part relating to the marked information,the self-position calculation part may be configured to recalculate a self-position using the changed weighting, andthe marking part may be configured to rewrite the position information of the information associated with the marked information to the recalculated position.

Mode 5

The self-position estimation apparatus described above may be configured to exclude the marked information from targets of the linear combining by the self-position calculation part when it is determined that there is no continuity at a self-position calculated using a position calculated using the marked information.

Mode 6

The self-position estimation apparatus described above may be configured in such a way that at least one type of data out of data obtained by using LiDAR, data obtained from a depth sensor, and SfM-MVS data is/are used as the information data that changes according to the position of the mobile object on a travel route.

Mode 7

Refer to the Self-Position Estimation Method According to the Second Aspect

Mode 8

Refer to the Computer Program According to the Third Aspect

Further, like Mode 1, Modes 7 and 8 can be developed into Modes 2 to 6.

Further, the disclosure of each Patent Literature cited above is incorporated herein in its entirety by reference thereto and can be used as a basis or a part of the present invention as needed. It is to be noted that it is possible to modify or adjust the example embodiments or examples within the scope of the whole disclosure of the present invention (including the Claims) and based on the basic technical concept thereof. Further, it is possible to variously combine or select (or partially remove) a wide variety of the disclosed elements (including the individual elements of the individual claims, the individual elements of the individual example embodiments or examples, and the individual elements of the individual figures) within the scope of the disclosure of the present invention. That is, the present invention of course includes any types of variations and modifications which would be done by those skilled in the art according to the whole disclosure including the Claims, and the technical concept of the present invention. Particularly, any numerical ranges disclosed herein should be interpreted that any values or subranges falling within the disclosed ranges are also concretely disclosed even without specific recital thereof. In addition, each disclosure of above cited documents and also using a part or all thereof by combining with the disclosure of the present application are regarded as being included in the disclosure of the present application, as necessary, in accordance with the intent of the present invention, as a part of the disclosure of the present invention.

REFERENCE SIGNS LIST

10,100self-position estimation apparatus11,101camera part12,102,102a,102b,102cenvironmental data storage part13,103,103a,103b,103cfirst estimation part14,104second estimation part15,105,105a,105b,105cweighting determination part16,106self-position calculation part17sensor107,107a,107b,107ccontinuity determination part108,108a,108b,108cmarking part9000computer9010CPU9020communication interface9030memory9040auxiliary storage device