Self-position identifying method and device, and three-dimensional shape measuring method and device

1. A self-position identifying method for identifying a self-position by incorporating three-dimensional shapes from the outside, comprising the steps of: (a) inputting, into a computer, coordinate values on a three-dimensional shape at a new measuring position; and (b) structuring an environment model that partitions a spatial region, in which the three-dimensional shape exists, into a plurality of voxels formed from rectangular solids, of which the boundary surfaces are mutually perpendicular, and stores the positions of the individual voxels; (c) a matching step comprising the steps of i. adding a representative point and the error distribution thereof to the inside of a voxel; and ii. setting and storing a probability value that expresses the probability of the existence of an object within the voxel; for setting and recording a representative point and an error distribution thereof, within the voxel corresponding to the coordinate value, (d) identifiying the self-position, wherein, i. when three-dimensional shape data for a previous measuring position does not exist, a new measuring position is identified as a self-position; and ii. when three dimensional shape data for a previous measuring position exists, then 1. performing a fine matching step for position matching so as to minimize a summation of the distances between adjacent error distributions by rotating and translating a new measured data and error distribution, or rotating and translating an environment model for a new measuring position, relative to an environment model for a previous measuring position; 2. performing a self-position identifying step for identifying the self-position from a rotation quantity and a translation quantity in the fine matching step; and 3. performing an outputting step for outputting the self-position to an outputting device.

2. The self-position identifying method as set forth in claim 1 , comprising, prior to the fine matching step: performing a rough matching step for position matching by (a) rotating and translating a new measured data and error distribution relative to an environment model for an earlier measuring position so as to minimize a summation of the distances from measured data and error distributions to adjacent voxels having representative points; or, (b) rotating and translating an environment model for a new measuring position relative to an environment model for an earlier measuring position to minimize the summation of the distances between voxels having representative points.

3. The self-position identifying method as set forth in claim 1 , comprising, prior to the fine matching step: performing a rough matching step for position matching by (a) rotating and translating a new measured data and error distribution relative to an environment model for an earlier measuring position to maximize a summation of the probability values of voxels having representative points adjacent to measured data and error distributions; or, (b) rotating and translating an environment model for a new measurement position relative to an environment model for an earlier measuring position to minimize the summation of differences of the probability values of adjacent voxels.

4. The self-position identifying method as set forth in claim 1 , comprising, after the inputting, into a computer, coordinate values on a three-dimensional shape at a new measuring point: limiting the scope of checking through obtaining a current measuring position through (a) inference from a change in the measuring position of the past, or from a sensor that is capable of obtaining the current measuring position; or, (b) the use of a reflective strength value in addition to the distance value of the measured data.

5. The self-position identifying method as set forth in claim 1 , comprising, in identifying the self-position: identifying a position in six-degree-of-freedom of a new measuring position from the position and orientation of a previous measuring position.

6. The self-position identifying method as set forth in claim 1 , comprising, in the fine matching step: (a) treating a case as a single measured point if error distributions intersect; and (b) calculating the distance between error distributions by multiplying the distance values of this case by a weight calculated from the a degrees of coincidence of the distributions.

7. The self-position identifying method as set forth in claim 1 , comprising, in structuring an environment model that partitions a spatial region: (a) setting a largest voxel to a size corresponding to the minimum required resolution; and, (b) when a plurality of measured points exists within a single voxel, further partitioning hierarchically the voxel into a plurality of voxels so that only a single measured point exists within a single voxel.

8. The self-position identifying method as set forth in claim 1 , comprising, after identifying the self-position: (a) updating the environment model through (i) retrieving a voxel corresponding to the coordinate value of a newly inputted measured point; and (ii) assuming no object exists between the origin and the measured point, to resetting or eliminating representative points and error distributions within a plurality of voxels positioned therebetween.

9. The self-position identifying method as set forth in claim 1 , comprising, after identifying the self-position, updating the environment model through: (a) retrieving a voxel corresponding to a coordinate value of a newly inputted measured point; and (b) when there is no representative point within the voxel, setting the coordinate value and a error distribution as a representative point coordinate value and error distribution.

10. The self-position identifying method as set forth in claim 1 , comprising, after identifying the self-position, updating the environment model through: (a) retrieving a voxel corresponding to a coordinate value of a newly inputted measured point; (b) when there is a representative point that has already been set within the voxel, comparing a newly obtained error distribution and the error distribution already set within the voxel; (c) if the newly obtained error distributions are mutually overlapping, resetting a new error distribution and a new representative point from both mutually overlapping error distributions; and (d) if the error distributions are not mutually overlapping, then further partitioning hierarchically the voxel into a plurality of voxels so that only a single representative point exists within a single voxel.

11. The self-position identifying method as set forth in claim 1 , comprising, in identifying a self-position: (a) identifying an error distribution for the self-position along with identifying the self-position; (b) prior to performing an outputting step, correcting the self-position and error distribution through Kalman filtering from a current self-position and error distribution and the self-position and error distribution that have been identified; and, (c) outputting the self-position and error distribution in the outputting step.

12. The self-position identifying method as set forth in claim 8 , comprising, in updating the environmental model: (a) comparing a newly obtained error distribution and an error distribution that has already been set within the voxel; (b) if the error distributions are mutually overlapping, when, as the result of resetting a new error distribution and a new representative point from both of the error distributions, the new representative point has moved into another voxel, then, (i) if there is no representative point in the other voxel, then setting the new error distribution and new representative point into the other voxel; and (ii) if there is a representative point that has already been set in the other voxel, then comparing the new error distribution and the error distribution that has already been set into the other voxel, then, (1) if the error distributions are mutually overlapping, resetting a new error distribution and a new representative point from both error distributions, or from both mutually overlapping error distributions and the representative point that has already been set within the voxel and a newly entered measured point coordinate value; or, (2) if the error distributions are not mutually overlapping, further partitioning hierarchically the voxel into a plurality of voxels so that only a single representative point exists within a single voxel.

13. The self-position identifying method as set forth in claim 1 , comprising, after identifying the self-position, updating the environment model through: (a) obtaining and resetting a new representative point and error distribution through a Kalman filter from a newly inputted measured point coordinate value and an error distribution thereof, and from the representative point and the error distribution thereof that have already been set in the voxel.

14. The self-position identifying method as set forth in claim 1 , comprising, in the fine matching step, the steps of: (a) position matching by rotating and translating a new measured data and error distribution; or, (b) rotating and translating an environment model for a new measurement position, relative to the environment model for the previous measuring position in order to maximize an evaluation value for a degree of coincidence established through maximum likelihood estimates based on a distance between adjacent error distributions, instead of position matching so as to minimize an evaluation value for the distance between adjacent error distributions.

15. The three-dimensional shape data matching method as set forth in claim 14 , wherein an equation for calculating the evaluation value for the degree of coincidence is expressed in the following equation (16): ( Equation ⁢ ⁢ 16 ) EM = ∏ j = 1 N ⁢ ⁢ { ω ⁡ ( j ) ⁢ EM ⁡ ( i , j ) } where, in this equation, a correlation is established between the measured points j and the representative points i in the environment model, the probability of obtaining measured data that is the measured point j is represented by EM (i, j), ω(j) is 1 if there is a representative point that corresponds to the measured point j within the environment model, and 0 otherwise.

16. A self-position identifying method for identifying a self-position by incorporating three-dimensional shapes from the outside, comprising: (a) inputting, into a computer, coordinate values on a three-dimensional shape at a new measuring position; (b) structuring an environment model that partitions a spatial region in which the three-dimensional shape exists, into a plurality of voxels formed from rectangular solids, of which the boundary surfaces are mutually perpendicular, and stores the positions of the individual voxels; and (c) a matching step comprising the steps of: i. adding a representative point and the error distribution thereof to the inside of a voxel; and ii. setting and storing a probability value that expresses the probability of the existence of an object within the voxel, for setting and recording a representative point and an error distribution thereof, within the voxel corresponding to the coordinate value; (d) identifying the self-position, wherein, i. when the three-dimensional shape data for a previous measuring position does not exist, a new measuring position is identified with a self-position; and ii. when the three-dimensional shape data for a previous measuring position exists 1. performing a fine matching step for position matching so as to minimize an evaluation value regarding the distances between adjacent error distributions by rotating and translating a new measured data and error distribution, or rotating and translating an environment model for a new measuring position, relative to an environment model for a previous measuring position; and, 2. identifying a self-position from a rotation quantity and a translation quantity in the fine matching step.

17. The self-position identifying method as set forth in claim 16 , further comprising the step of: outputting the self-position to an outputting device.

18. The self-position identifying method as set forth in claim 16 , comprising, prior to the fine matching step, the step of: (a) a rough matching step for position matching, wherein the position matching is done by i. rotating and translating a new measured data and error distribution relative to an environment model for an earlier measuring position so as to minimize an evaluation value for the distances from measured data and error distributions to adjacent voxels having representative points; or, ii. rotating and translating an environment model for a new measuring position relative to an environment model for an earlier measuring position to minimize an evaluation value for the distances between voxels having representative points.

19. The self-position identifying method as set forth in claim 16 , comprising, prior to the fine matching step, the step of: (a) a rough matching step for position matching, wherein the position matching is done by i. rotating and translating a new measured data and error distribution relative to an environment model for an earlier measuring position to maximize an evaluation value for the probability values of voxels having representative points adjacent to measured data and error distributions; or, ii. rotating and translating an environment model for a new measuring position relative to an environment model for an earlier measuring position to minimize an evaluation value for the differences of the probability values of adjacent voxels.

20. A three-dimensional shape measuring method for reproducing a three-dimensional shape from coordinate values of measured points on an external three-dimensional shape and for outputting three-dimensional shape data, comprising the steps of: (a) inputting, into a computer, coordinate values on a three-dimensional shape at a new measuring position; (b) structuring an environment model that partitions a spatial region in which the three-dimensional shape exists, into a plurality of voxels formed from rectangular solids, of which a plurality of boundary surfaces are mutually perpendicular, and stores the positions of individual voxels; (c) a matching step for setting and recording a representative point and an error distribution thereof, within a voxel corresponding to the coordinate value; wherein, i. when three-dimensional shape data for a previous measuring position does not exist, a new measuring position is identified with a self-position; and ii. when three dimensional shape data for a previous measuring position exists, then, 1. a fine matching step for position matching is performed so as to minimize an evaluation value regarding the distances between adjacent error distributions by rotating and translating a new measured data and error distribution; or, 2. rotating and translating an environment model for a new measuring position, relative to an environment model for a previous measuring position; (d) identifying the self-position from a rotation quantity and a translation quantity in the fine matching step; and (e) outputting, to an outputting device, at least one of the self-position, a voxel position, a representative point and a error distribution based on the self-position, wherein, (i) at least one of the position of the voxel, the position of a representative point and the position of an error distribution is outputted to the outputting device as a three-dimensional shape measurement value; and (ii) an index indicating the reliability or accuracy of a measurement value based on the magnitude of the error distribution within the voxel is outputted to the outputting device.

21. The three-dimensional shape measuring method as set forth in claim 20 , wherein: in outputting, when at least one of the position of the voxel, the position of the representative point, and the position of the error distribution is outputted to the outputting device as the three-dimensional shape measurement value, if the magnitude of the error distribution within the voxel is larger than a specific reference value, then the reliability or accuracy of the measurement value is regarded to be less than a specific reference, and the measurement value for the voxel is not outputted to the outputting device.

22. The three-dimensional shape measuring method as set forth in claim 20 , comprising, after the matching step, a step of: (a) updating the environment model through i. retrieving a voxel corresponding to the coordinate value of a newly inputted measured point; and ii. when there is no representative point within the voxel, setting the coordinate value and the error distribution as the representative point coordinate value and error distribution.

23. The three-dimensional shape measuring method as set forth in claim 20 , comprising, after the matching step, a step of: (a) a model updating step for updating the environment model through i. retrieving a voxel corresponding to a coordinate value of a newly inputted measured point; ii. when there is a representative point that has already been set within the voxel, comparing a newly obtained error distribution and an error distribution already set within the voxel; iii. if the error distributions are mutually overlapping, resetting a new error distribution and a new representative point from both of the mutually overlapping error distributions, or from both of the mutually overlapping error distributions and the representative point already set within the voxel and the coordinate values of the measured point newly inputted; and iv. if the error distributions are not mutually overlapping, then further partitioning hierarchically the voxel into a plurality of voxels so that only a single representative point exists within a single voxel.

24. The three-dimensional shape measuring method as set forth in claim 20 , comprising, after the matching step, the step of: (a) updating the environment model through i. retrieving a voxel corresponding to the coordinate value of a newly inputted measured point; and ii. if at least one of a representative point and an error distribution within that voxel is newly set, or reset, or that voxel is further partitioned hierarchically into a plurality of voxels, then, outputting the position of the representative point of that voxel to the outputting device as a three-dimensional shape measurement value.

25. The three-dimensional shape measuring method as set forth in claim 20 , wherein, in outputting to the outputting device: the position of a representative point of a voxel in the environment model in a scope for which the position can be measured from a position of a range sensor is outputted to the outputting device as a three-dimensional shape measurement value.

26. A self-position identifying device operable to identify a self-position by incorporating three-dimensional shapes from the outside, the self-position identifying device comprising: (a) a data inputting device operably connected to a computer to input coordinate values on a three-dimensional shape; (b) a model structuring device operable to structure an environment model that partitions a spatial region in which the three-dimensional shape exists, into a plurality of voxels formed from rectangular solids, of which a plurality of boundary surfaces are mutually perpendicular, and stores the positions of individual voxels; (c) a matching device operable to add a representative point and an error distribution thereof to the inside of a voxel, and setting and storing a probability value that expresses the probablility of the existence of an object within the voxel; wherein, i. at a new measuring position, when a three-dimensional shape data for a previous measuring position does not exist, the matching device identifies a new measuring position as a self-position; and ii. at the new measuring position, when the three dimensional shape data for a previous measuring position exists, the matching device rotates and translates an environment model for a new measuring position relative to an environment model for a previous measuring position to perform position matching so as to minimize a summation of distances between adjacent error distributions; and iii. the matching device identifies the self-position from the rotation quantity and translation quantity in the position matching; and (d) a data transferring device operable to outputting the self-position to an outputting device.

27. A self-position identifying device operable to identify a self-position by incorporating three-dimensional shapes from the outside, comprising: (a) a data inputting device operably connected to a computer to input coordinate values on a three-dimensional shape; (b) a model structuring device operable to structure an environment model that partitions a spatial region in which the three-dimensional shape exists, into a plurality of voxels formed from rectangular solids, of which the boundary surfaces are mutually perpendicular, and stores the positions of the individual voxels; (c) a matching device operable to add a representative point and an error distribution thereof to the inside of a voxel, and setting and storing a probability value that expresses the probablility of the existence of an object within the voxel; wherein, (i) at a new measuring position, when the three-dimensional shape data for a previous measuring position does not exist, the matching device identifies a new measuring position as a self-position; and (ii) at the new measuring position, when the three dimensional shape data for a previous measuring position exists, the matching device rotates and translates an environment model for a new measuring position relative to an environment model for a previous measuring position to perform position matching so as to minimize an evaluation value regarding the distance between adjacent error distributions; and (iii) the matching device identifies the self-position from the rotation quantity and translation quantity in the position matching; and (d) a data transferring device for outputting the self-position to an outputting device.

28. A three-dimensional shape measuring device operable to reproduce a three-dimensional shape from coordinate values of measured points on a three-dimensional shape and for outputting three-dimensional shape data, comprising: (a) a data inputting device operably connected to a computer to input coordinate values on a three-dimensional shape; (b) a model structuring device operable to structure an environment model that partitions a spatial region in which the three-dimensional shape exists, into a plurality of voxels formed from rectangular solids, of which the boundary surfaces are mutually perpendicular, and stores the positions of the individual voxels; (c) a matching device operable to add a representative point and an error distribution thereof to the inside of a voxel, and setting and storing a probability value that expresses the probablility of the existence of an object within the voxel; wherein, (i) at a new measuring position, when the three-dimensional shape data for a previous measuring position does not exist, the matching device identifies a new measuring position as a self-position; and (ii) at the new measuring position, when the three dimensional shape data for a previous measuring position exists, the matching device rotates and translates an environment model for a new measuring position relative to an environment model for a previous measuring position to perform position matching so as to minimize an evaluation value regarding the distance between adjacent error distributions; and (iii) the matching device identifies the self-position from the rotation quantity and translation quantity in the position matching; and (d) a data transferring device for outputting, to an outputting device, at least one of the self-position, a voxel position, a representative point, and an error distribution based on the self-position.

29. The self-position identifying device as set forth in claim 26 , wherein the data inputting device incorporates three-dimensional information from a range sensor.

30. The self-position identifying device as set forth in claim 29 , wherein the data inputting device optionally incorporates three-dimensional information from an odometer, a camera, a GPS and an orientation sensor.

31. The self-position identifying device as set forth in claim 26 , further comprising: (a) an internal storage device; and (b) a central processing device that i. functions as the model structuring device, the matching device, a model updating device, and the data transferring device; and ii. executes programs together with the internal storage device; and (c) an external storage device, operably connected to the central processing device.

32. The self-position identifying device as set forth in claim 31 , wherein the outputting device is a display device, a printer, or an external device able to output the program executing results and the data stored in the internal storage device and the external storage device.