WO2007069724A1 - 三次元形状データの位置合わせ方法と装置 - Google Patents

三次元形状データの位置合わせ方法と装置 Download PDF

Info

Publication number
WO2007069724A1
WO2007069724A1 PCT/JP2006/325047 JP2006325047W WO2007069724A1 WO 2007069724 A1 WO2007069724 A1 WO 2007069724A1 JP 2006325047 W JP2006325047 W JP 2006325047W WO 2007069724 A1 WO2007069724 A1 WO 2007069724A1
Authority
WO
WIPO (PCT)
Prior art keywords
error distribution
error
data
measurement
model
Prior art date
Application number
PCT/JP2006/325047
Other languages
English (en)
French (fr)
Inventor
Toshihiro Hayashi
Yukihiro Kawano
Hideo Terada
Original Assignee
Ihi Corporation
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Ihi Corporation filed Critical Ihi Corporation
Priority to US12/096,851 priority Critical patent/US8116558B2/en
Priority to CN2006800474726A priority patent/CN101331381B/zh
Priority to JP2007550241A priority patent/JP4650751B2/ja
Priority to DE112006003380T priority patent/DE112006003380T5/de
Publication of WO2007069724A1 publication Critical patent/WO2007069724A1/ja

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G01B11/24—Measuring arrangements characterised by the use of optical techniques for measuring contours or curvatures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/005—Tree description, e.g. octree, quadtree

Definitions

  • the present invention relates to an alignment method and apparatus for 3D shape data for measuring a plurality of measurement position forces of a stationary 3D shape and integrating the distance data.
  • estimating the geometric positional relationship between a plurality of distance images obtained by measuring a multi-viewpoint force of a static three-dimensional shape is called “alignment”.
  • Three-dimensional shape measurement technology has become widespread, and three-dimensional shape data can be used in various application fields, such as shape measurement of machine parts and workpieces, identification of self-positions in mobile robots, measurement of topography 'structures, etc. Is used.
  • the measurement from one viewpoint can acquire surface information facing that viewpoint, but cannot measure the back of the object. Therefore, to restore the 3D shape, it is necessary to accurately measure the measurements from multiple viewpoints and the measurement results.
  • the positional relationship between the viewpoint and the measurement target In order to align data acquired from a plurality of viewpoints, the positional relationship between the viewpoint and the measurement target must be clear. There are two methods for obtaining the positional relationship between this viewpoint and the measurement target: a method in which an object is placed on the stage and rotated, and a method in which an arm with a link mechanism has a viewpoint. There is a limit. Even if accuracy is obtained, the size of the measurement object is limited.
  • the position is obtained by comparing the previously obtained measurement data with the measurement data obtained thereafter.
  • Various means for matching are proposed.
  • Non-Patent Documents 1 and 2 As a positioning means when the corresponding point is not known, matching by an invariant feature amount, Direct methods and ICP algorithms have already been proposed (for example, Non-Patent Documents 1 and 2).
  • the “direct method” is for obtaining the optical flow in a two-dimensional image for a three-dimensional image.
  • the riCP Intelligent Closest Points
  • Non-Patent Document 3 is disclosed as a technique related to the present invention.
  • Non-Patent Document 1 Ken Masuda, et al. “Shape model generation technology from multiple range images”
  • Non-Patent Document 2 Paul J. Besl, "A Method for Registration of 3 -D Shapes, IEEE Transactions of Pattern Analysis and Mechanical Intelligence, Vol. 14, No. 2, February 1992
  • Non-Patent Literature 3 Kiyohide Sekimoto, et al. “Development of 3D Laser Radar”, Ishikawajima-Harima Technical Report Vol. 4 3 No. 4 (2003-7)
  • the measurement points on the three-dimensional shape to be measured are point groups discrete in the horizontal direction and the vertical direction. For example, when the distance from the measurement point is 50 m, the distance between the measurement points reaches, for example, about 315 mm in the horizontal direction and about 525 mm in the vertical direction.
  • a distance sensor such as a three-dimensional laser radar usually has different positions of measurement points for each measurement position. Further, such a distance sensor generally has an error of about 20 cm, for example, in the measurement distance.
  • Condition B Measurement data includes an error (for example, measurement distance is around 20cm)
  • Condition C Measurement data does not always measure the same measurement point! /.
  • the obtained distance data is a group of points that are discrete in the horizontal direction and the vertical direction, and since the position differs for each measurement, the measurement distance between the corresponding points includes a relatively large error.
  • the ICP algorithm is a means of superimposing two distance data. Even if the comparison between the previous data and the subsequent data is repeated and the difference is integrated, there are almost no corresponding points, so errors accumulate. .
  • the calculation amount is enormous.
  • the ICP algorithm needs to search for model data corresponding to each data point of measurement data, the amount of calculation increases as the number of model data points and measurement data points increase. Specifically, if the number of model data points is M and the number of measurement data points is N, for example, the calculation order for a full search is O (M X N).
  • the ICP algorithm targets dense distance data, it is a discrete point cloud, and if it is spatially sparse, it converges to an incorrect result.
  • the present invention has been developed to solve the above-described problems.
  • the purpose of the present invention is a point group in which distance data obtained by measuring a plurality of measured position forces of a stationary three-dimensional shape is spatially discrete, and the corresponding points are different for each measurement. Even if a large error is included, accurate positioning is possible with a small amount of calculation that does not accumulate errors. It is an object of the present invention to provide a method and apparatus for aligning three-dimensional shape data that can be matched.
  • a model construction step for constructing an environment model that divides the spatial region where the three-dimensional shape exists into a plurality of botasels made of rectangular parallelepipeds whose boundary surfaces are orthogonal to each other, and stores the positions of the respective botasels;
  • a precision alignment step that aligns the values (eg, the sum of the distances) to a minimum
  • an alignment method of three-dimensional shape data comprising: an output step of outputting the botasel position, the representative point, and the error distribution to an output device.
  • the evaluation value related to the distance is not the total sum of the distances, but the average value of the distances, the sum of the squares of the distances, or other appropriate evaluation values that may be the maximum values of the distances. It may be.
  • a probability value representing the existence probability of the object is set in the button cell.
  • the new measurement data and the error distribution are rotated and translated with respect to the environmental model for the previous measurement position, and the botasel having a representative point close to the measurement data and the error distribution.
  • An evaluation value related to the distance between the botasels having representative points for example, the rotation of the environmental model for the new measurement position and translation
  • the evaluation value related to the distance between the two for example, the sum of the distances
  • the distance There is a rough alignment step for alignment so that the sum of the separation is minimized.
  • the above evaluation value regarding the distance between the measured data and the error distribution and the distance between the botasels having representative points adjacent to each other is not the sum of the distance, but the average value of the distance, the sum of the squares of the distance, or It may be another appropriate evaluation value that may be the maximum value of the distance.
  • the above evaluation value regarding the distance between the botasels having representative points is not the sum of the distances, but the average value of the distances, the sum of the squares of the distances, or the sum of the distances. Another appropriate evaluation value may be used instead of the maximum value of the distance.
  • the new measurement data and error distribution are rotated and translated with respect to the environmental model for the previous measurement position, and the botasel having a representative point close to the measurement data and error distribution.
  • An evaluation value related to the difference between the probability values of adjacent botasels so that the evaluation value related to the probability value (for example, the sum of the probability values) is maximized or the environmental model for the new measurement position is rotated and translated.
  • the above evaluation value for the probability value of the botasel having a representative point close to the measurement data and the error distribution is the average value of the probability value or the square of the probability value instead of the sum of the probability values. It may be the sum or other appropriate evaluation value that may be the minimum value of the probability value.
  • the above evaluation value regarding the difference in probability values of neighboring botasels is not the sum of the difference of the probability values, but the average value of the difference of the probability values, the sum of the squares of the difference of the probability values, May be another appropriate evaluation value, which may be the maximum value of the difference between the probability values.
  • the current measurement position is estimated by a change force of the past measurement position, or acquired by a sensor capable of acquiring the current measurement position, or not only the distance value of the measurement data
  • a search range limiting step for limiting the range to be collated using the reflection intensity value is provided.
  • the maximum botacell is set to a size corresponding to the necessary minimum resolution, and a plurality of measurement points exist in a single botacell.
  • the voxel is further divided into a plurality of hierarchies in a hierarchical manner so that only a single point to be measured exists within a single botagel.
  • a model update step for updating the environmental model is provided, and in the model update step, a Botacel corresponding to the coordinate value of the newly input measurement point is searched.
  • a model update step for updating the environmental model is provided, and in the model update step, a botacell corresponding to the coordinate value of the newly input measurement point is searched.
  • the coordinate value and error distribution are set as the coordinate value and error distribution of the representative point.
  • a model update step for updating the environmental model is provided, and in the model update step, a botacell corresponding to the coordinate value of the newly input measurement point is searched.
  • the newly acquired error distribution is compared with the error distribution in the already set botacell,
  • the voxel is further divided into a plurality of hierarchically so that there is only a single representative point in a single votacell.
  • an octree or a KD tree is used to divide the voxels.
  • the newly acquired error distribution is compared with the error distribution in the already set botacell. If the error distributions overlap each other, both error distribution forces new error distribution and new error distribution As a result of resetting a representative point, when a new representative point moves into another voxel,
  • the new error distribution is compared with the already set error distribution in the other button cell, and (A) the error distributions overlap each other. In addition, from both error distributions, or from both error distributions and the coordinate values of the representative points already set in the botacell and the newly input measurement point, new error distributions and new representative points are reset. (B) If the error distributions do not overlap with each other, the voxel is further divided into a plurality of hierarchically arranged botasels so that only a single representative point exists within a single votacell.
  • the method includes a model update step of updating the environment model after the fine alignment step,
  • a new representative point is obtained by a Kalman filter from the newly input coordinate value of the measured point and its error distribution, and the representative point and its error distribution in the already set button cell. And get the error distribution and reset.
  • a maximum likelihood estimation value based on the adjacent error distributions is used. Rotate and translate the new measurement data and error distribution with respect to the environmental model for the previous measurement position, or create a new one so that the evaluation value (for example, the sum of the degree of coincidence) is the maximum. Rotate and translate the environment model for the correct measurement position.
  • a stationary three-dimensional shape is also measured by measuring a plurality of measurement position forces.
  • 3D shape data alignment device for integrating and aligning the distance data of
  • a data input device for inputting coordinate values on a three-dimensional shape to a computer
  • a model construction device that divides the spatial region in which the three-dimensional shape exists into a plurality of botasels made of rectangular parallelepipeds whose boundary surfaces are orthogonal to each other, and constructs an environment model that stores the positions of the respective botasels
  • a matching device for setting and storing a representative point and its error distribution in the inside of the button cell corresponding to the coordinate value
  • a data transmission device that outputs the botacell position, representative point, and error distribution to an output device
  • An apparatus for aligning three-dimensional shape data is provided.
  • the matching device sets the representative value and its error distribution in the inside of the button cell, and sets and stores a probability value representing the existence probability of the object in the button cell.
  • the three-dimensional shape data alignment apparatus rotates and translates new measurement data and error distribution with respect to the environmental model for the previous measurement position before the alignment (the fine alignment step). Rotate and translate the environment model for the new measurement position so that the evaluation value (for example, the sum of the distances) between the measurement data and the error distribution and the distance between the botasels that have representative points close to each other is minimized. Then, the rough alignment step is performed so that the evaluation value (for example, the sum of the distances) regarding the distance between the botasels having the representative points is minimized.
  • the evaluation value for example, the sum of the distances
  • the 3D shape data alignment apparatus rotates and translates new measurement data and error distribution with respect to the environmental model for the previous measurement position, thereby measuring the data and error distribution.
  • Approximate evaluation values for example, the sum of the probability values
  • the evaluation value for example, the sum of the difference of the probability values
  • the three-dimensional shape data alignment apparatus estimates the current measurement position from a change in the past measurement position, or acquires the current measurement position using a sensor capable of acquiring the current measurement position.
  • a search range limiting step for limiting the range to be collated is performed using not only the distance value of the measurement data but also the reflection intensity value.
  • the three-dimensional shape data alignment apparatus uses the same measurement point when the error distribution intersects in the fine alignment step, and multiplies the distance value in that case by the weight obtained from the degree of coincidence of the distribution. To calculate the distance between the error distributions.
  • the model construction apparatus sets a maximum botacell to a size corresponding to a necessary minimum resolution, and a plurality of measurement points exist in a single botacell.
  • the voxel is further divided into a plurality of hierarchically so as to have only a single point to be measured.
  • the three-dimensional shape data alignment apparatus performs a model update step for updating the environmental model after the precise alignment step, and V to be measured newly input in the model update step. Search for the botacell corresponding to the coordinate value of the point,
  • the three-dimensional shape data alignment apparatus includes a model update apparatus that updates the environment model after the fine alignment step,
  • the coordinate value and error distribution are set as the coordinate value and error distribution of the representative point.
  • the three-dimensional shape data alignment apparatus includes a model update apparatus that updates the environment model after the fine alignment step,
  • the voxel is further divided into a plurality of hierarchically so that there is only a single representative point in a single votacell.
  • the model updating device compares the newly acquired error distribution with the already set error distribution in the box, and when the error distributions overlap each other, both the error distribution forces are new. As a result of resetting the error distribution and the new representative point, when the new representative point moves into another botacel,
  • the new error distribution is compared with the already set error distribution in the other button cell, and (A) the error distributions overlap each other. In addition, from both error distributions, or from both error distributions and the coordinate values of the representative points already set in the botacell and the newly input measurement point, new error distributions and new representative points are reset. (B) If the error distributions do not overlap with each other, the voxel is further divided into a plurality of hierarchically arranged botasels so that only a single representative point exists within a single votacell.
  • the three-dimensional shape data alignment apparatus includes a model update apparatus that updates the environmental model after the fine alignment step
  • the model update apparatus includes: Then, new representative points and error distributions are acquired by the Kalman filter from the newly input coordinate values of the measurement points and their error distributions, and the representative points and their error distributions already set in the botacell, and are reset.
  • the 3D shape data alignment apparatus performs alignment so that an evaluation value related to the distance between the adjacent error distributions (for example, the square of the degree of coincidence) is minimized. Instead, the evaluation value (for example, the sum of the matching degrees) determined by the maximum likelihood estimated value based on the adjacent error distribution is the largest.
  • the new measurement data and the error distribution are rotated and translated with respect to the environmental model for the previous measurement position, or the environmental model for the new measurement position is rotated and translated.
  • the formula for calculating the evaluation value related to the degree of coincidence is expressed by the above [Equation 1].
  • a spatial region where a three-dimensional shape exists is divided into a plurality of voxels and the positions of each botacel are stored. Therefore, even if the measurement object is large, the amount of data Can be suppressed to a small data proportionate to the number of botasels.
  • the representative point and its error distribution are set and stored in the inside of the button cell corresponding to the coordinate value, information exceeding the resolution of the button cell can be expressed.
  • the object in each voxel can be obtained even when the error distribution is wider than the botacell to which the representative point belongs.
  • the search time can be reduced because it can be easily determined only by the probability value of the relevant buttonel without finding the buttonel to which the representative point belongs and recalculating the error distribution force.
  • the current measurement position can be estimated from changes in the past measurement position, or acquired using a sensor that can acquire the current measurement position, or using the reflection intensity as well as the distance value of the measurement data, Because it is limited, search time can be reduced.
  • new measurement data and error distribution are rotated and translated with respect to the environmental model for the previous measurement position, and between the botasels having representative points close to the measurement data and error distribution.
  • Evaluation value for example, the total sum of the distances
  • the environment model for the new measurement position is rotated and translated to evaluate the distance between the botasels having representative points (for example, the (Sum of distances) is minimized.
  • rotate and translate the new measurement data and error distribution so that the evaluation value (for example, the sum of the probability values) of the probability values of the botasels that have representative points close to the measurement data and error distribution is maximized.
  • Rotate or translate the environment model for the new measurement position and evaluate the evaluation value for the difference in the probability values of adjacent botasels (for example, For example, since the positioning is performed so that the sum of the difference of the probability values is minimized, it is possible to align the botasels having representative points in a short time while preventing the accumulation of errors.
  • an evaluation value for example, the difference between the probability values of adjacent botasels.
  • the new measurement data and error distribution are rotated and translated with respect to the environmental model for the previous measurement position, or the environmental model for the new measurement position is rotated and translated.
  • the alignment is performed so that the sum of the distances between adjacent error distributions is minimized, precise alignment between the error distributions can be performed in a short time.
  • the number of calculations can be reduced because the number of points is reduced by having one representative point in the button cell.
  • the calculation order for searching for the model point corresponding to the measurement point can be set to 1, so the total calculation order can be reduced.
  • the accuracy of the environmental model of the present invention is maintained by performing the error distribution of representative points and the hierarchization of voxels.
  • the conventional ICP algorithm has the ability to output erroneous results for sparse data. Since the environment model of the present invention has representative points and error distributions in the boatel cell, alignment corresponding to sparse data is possible. Is possible.
  • the method and apparatus of the present invention in three-dimensional shape information restoration using measurement data from a plurality of viewpoints, highly accurate shape acquisition can be performed while preventing accumulation of errors even for sparse data. It becomes possible. In addition, since the amount of data to be compared at the time of overlay is reduced, the amount of calculation can be reduced. [0051] Further, in the model update step, a new value is obtained by a Kalman filter from the newly input coordinate value of the measured point and its error distribution, and the representative point and its error distribution in the already set button cell. Since the representative point and the error distribution are acquired and reset, the shape can be obtained closer to the true value.
  • the fine alignment step instead of performing alignment so that an evaluation value related to the distance between the adjacent error distributions is minimized, maximum likelihood estimation based on the adjacent error distributions is performed.
  • the evaluation value for example, the sum of the matching degrees
  • alignment can be performed taking into account errors in both the environmental model and measurement data.
  • FIG. 1 is a configuration diagram of a three-dimensional laser radar disclosed in Non-Patent Document 2.
  • FIG. 2A is a diagram showing a relationship between polar coordinate data measured by a distance sensor and errors.
  • FIG. 2B shows a case where the error distribution is approximated as an ellipsoid included in a rectangular parallelepiped.
  • FIG. 3 is an apparatus configuration diagram for executing the method of the present invention.
  • FIG. 4 is a flowchart showing the method of the present invention.
  • FIG. 5 is a schematic diagram of model building steps.
  • FIG. 6 A schematic diagram of the constructed environmental model.
  • FIG. 7A is a diagram showing a data structure of the button cell data in the present invention, showing an example of a memory layout of each button cell data.
  • FIG. 7B is a diagram showing the data structure of the button cell data in the present invention, and shows an example in which a level 2 (1, 1, 0) button cell has a representative point.
  • FIG. 8 is a data processing flow diagram of rough alignment step S6 and fine alignment step S7.
  • FIG. 9 is a schematic diagram of the rough matching step S6.
  • FIG. 10 is a schematic diagram of precision alignment step S7.
  • FIG. 11 is a data processing flow diagram in a model update step.
  • FIG. 12 is a schematic diagram in the case where there is a representative point already set in the corresponding button cell.
  • FIG. 14 is a schematic diagram when error distributions overlap each other.
  • FIG. 16 is a partially enlarged view of FIG.
  • FIG. 1 is a configuration diagram of a three-dimensional laser radar as an example of a distance sensor.
  • a three-dimensional laser radar is disclosed in Non-Patent Document 3, for example.
  • the three-dimensional laser radar 10 includes a radar head 12 and a controller 20.
  • the laser light 1 oscillated from the laser diode 13 is shaped into parallel light 2 by the light projection lens 14 and scanned in a two-dimensional direction by the mirrors 18a and 18b and the polygon mirror 15 which is rotated and oscillated. Is irradiated.
  • the pulse laser beam 3 reflected from the measurement object is condensed by the light receiving lens 16 through the polygon mirror 15 and converted into an electric signal by the photodetector 17.
  • the time interval counter 21 in the controller 20 measures the time interval between the start pulse 4 synchronized with the pulse oscillation timing of the laser diode 13 and the stop pulse 5 output from the photodetector 17.
  • the signal processing board 22 outputs the time interval t when the reflected light is detected, the polygon mirror rotation angle ⁇ , and the swing angle ⁇ as polar coordinate data (r, ⁇ , ⁇ ).
  • C is the speed of light.
  • the judgment processing unit 23 converts the polar coordinate data of the signal processing board power into three-dimensional spatial data (X, y, z) with the radar head installation position as the origin, and performs detection processing.
  • 24 is a drive unit.
  • the measurement range of the three-dimensional laser radar 10 described above is, for example, a horizontal field angle of 60 °, a vertical field angle of 30 °, and a maximum measurement distance of 50 m.
  • the position detection accuracy is, for example, about 20 cm.
  • the measurement point on the three-dimensional shape measured by the three-dimensional laser radar 10 is in the horizontal direction.
  • the distance sensor is not limited to this, and a distance sensor using parallax and other known distance sensors can be used.
  • FIG. 2A and FIG. 2B are diagrams showing the relationship between polar coordinate data measured by a distance sensor and errors.
  • polar coordinate values (r, ⁇ , ⁇ ) with an arbitrary measurement position as the origin are measured as measurement results.
  • An error distribution as shown in the figure usually exists in the measurement result of the distance sensor.
  • the error distribution is assumed to be normally distributed in the measurement axes r, ⁇ , and ⁇ directions, and can be expressed, for example, by equation (1).
  • r, ⁇ , and ⁇ are measured values of sensor force
  • ⁇ , ⁇ , and ⁇ are standard deviations
  • A is a standard constant.
  • the difference between a and b is small in the far field, which is the force that is normally contained in a truncated cone (long left) in the r direction. Therefore, this error distribution can be approximated to the safe side as an ellipsoid included in a rectangular parallelepiped.
  • FIG. 3 is a block diagram of an apparatus for executing the method of the present invention. As shown in this figure, this device includes a data input device 32, an external storage device 33, an internal storage device 34, a central processing device 35 and an output device 36.
  • the data input device 32 includes the above-described distance sensor, and inputs coordinate values on a three-dimensional shape to a storage device. For example, using a goometer, odometer, etc.
  • the data input device 32 may also have normal input means such as a keyboard.
  • the external storage device 33 is a hard disk, a floppy (registered trademark) disk, a magnetic tape, a compact disk, or the like.
  • the external storage device 33 is used when the size of the environmental model is large and the coordinate values on the three-dimensional shape, the botacell position, and the representative points and their error distribution that are input to the internal storage device 34 described later cannot be stored.
  • the internal storage device 34 is, for example, a RAM, a ROM, etc., and the coordinate values on the three-dimensional shape, the botacell positions, and the representative points and a part or all of the error distribution thereof inputted to a partial range or the entire range of the environmental model. And the calculation information.
  • the central processing unit 35 functions as a model building device, matching device, rough alignment and fine alignment alignment device, model update device, and data transmission device.
  • the program is executed together with the storage device 34.
  • the model building device is a device that performs the model building step described later
  • the matching device is
  • the alignment device is a device that performs a rough alignment step and a fine alignment step, which will be described later
  • the model update device is a device that performs a model update step, which will be described later.
  • the device is a device that outputs data to the output device 36.
  • the output device 36 is, for example, a display device, a printer, an external storage device, or the like, and outputs at least one of the data stored in the internal storage device 34 and the external storage device 33 and the execution result of the program. ing.
  • the interface to the external device is LAN, USB, IEEE1394, etc., and the result of adding the representative point, error distribution, botacell position, etc. within the corresponding three-dimensional shape to the input coordinate value on the three-dimensional shape, environment The entire model or part of the environmental model is output on demand.
  • the above-described device of the present invention may be a combination of the above-described distance sensor and a normal PC (computer), or may be a device integrated as a whole. Also, it may be integrated into a self-propelled device.
  • FIG. 4 is a flowchart illustrating the method of the present invention.
  • the method of the present invention is a method for aligning three-dimensional shape data for restoring a three-dimensional shape from the coordinate values of a measurement point on the three-dimensional shape, and includes a data input step Sl, a data correction step S2, and a search. It has a range limiting step S3, a model building step S4, a matching step S5, a rough matching step S6, a fine matching step S7, a model updating step S8 and an output step S9.
  • Sl, S2, S3, and S5 to S9 are performed each time measurement data is obtained, and S4 is performed only when measurement data is obtained for the first time.
  • the coordinate value on the three-dimensional shape is input to the storage device of the computer using the distance sensor.
  • the position / posture of the distance sensor and the movement distance may be input using a goometer, odometer, or the like.
  • this data input step S1 it is preferable to use the three-dimensional laser radar 10 to sequentially obtain the coordinate values on the three-dimensional shape as distance data having an arbitrary measurement position as the origin and without moving the origin.
  • the coordinate value on the three-dimensional shape is This is distance data with an arbitrary measurement position as the origin, and is represented by polar coordinate values (r, ⁇ , ⁇ ). Also, the error distribution of each coordinate value is input in advance by another input means (for example, a keyboard), which is a force obtained by calculation from the polar coordinate values (r, ⁇ , ⁇ ).
  • distance data correction processing is performed to improve the accuracy of the distance data.
  • polar coordinate data and odometer data may be converted to three-dimensional spatial data (X, y, z) with an arbitrary fixed position as the origin.
  • An isolated point is a point that exists in an isolated point force, and the measurement data is composed of a plurality of adjacent points. Therefore, the isolated point can be removed assuming that it is an erroneous measurement.
  • Statistical processing takes into account the error distribution included in the measurement data and corrects the distance by statistical processing (for example, average value) of multiple measurements.
  • the target three-dimensional shape can be approximated by a straight line or a plane, these should be performed.
  • the search range of the distance sensor is limited.
  • FIG. 5 is a schematic diagram of model building steps in the case where an octree is used for dividing the botasel.
  • the space model where the three-dimensional shape exists is divided into a plurality of botasels 6 with rectangular parallelepiped forces whose boundary surfaces are orthogonal to each other, and an environment model that stores the location of each bocell Build up.
  • the shape of the bot cell 6 may be a cube having the same length on each side or a rectangular parallelepiped having a different length on each side.
  • each side of the boatacell 6 is preferably set to a size corresponding to the minimum necessary resolution of the maximum boatacell 6.
  • the largest Botacel 6 is called Level 1 Botacell
  • the data is further divided into eight hierarchies.
  • the spatial region in which the largest division of eight botels 6 is performed once is referred to as a level 2 botcell, and the spatial region that has been performed k times is referred to as a level k + 1 botacell.
  • FIG. 6 is a schematic diagram of the constructed environmental model.
  • the representative point 7 and its error distribution 8 are set and stored inside the boat cell 6 corresponding to the coordinate value on the three-dimensional shape.
  • the terminal botasel can have only one representative point of measurement.
  • Each botacel has a representative point of the measured value and its error distribution to represent the shape of the object.
  • a probability value representing the existence probability of an object can be given.
  • FIG. 7A and FIG. 7B are diagrams showing the data structure of the botacell data in the present invention.
  • FIG. 7A is an example of the memory layout of each botacell data.
  • an arrow indicates a link to data, and a pointer to the data is held as a value.
  • FIG. 7B shows an example in which a level 2 (1, 1, 0) buttonel has a representative point.
  • null represents an empty set.
  • the environmental model of the data structure described above has the following characteristics.
  • Presence The presence or absence of an object can be expressed.
  • Effect B The required amount of memory and the amount of calculation are below a certain amount.
  • the rough matching step S6 and the fine matching step S7 are performed after the matching step S5.
  • FIG. 8 is a data processing flow diagram of the rough alignment step S6 and the fine alignment step S7
  • FIG. 9 is a schematic diagram of the rough alignment step S6
  • FIG. 10 is a schematic diagram of the fine alignment step S7.
  • the environmental model and measurement data are both It can be expressed in space, or the environment model is expressed in the Botacel space, and the measurement data is expressed as representative points and error distribution. Assuming that the current measurement data is measurement at position (X, y, z) and posture ( ⁇ ,, ⁇ ), the measurement data is converted into world coordinates and the degree of coincidence with the environment model is calculated.
  • the shortest distance method can be used for calculating the degree of coincidence.
  • the distance between the botasels is given by [Equation 4] where x (1) and x (2) are the two botasel spaces, and the total number of botasels I is X (n) . It can be defined by equation (3).
  • the optimal position of the measurement data 'orientation can be calculated by the least square method that minimizes ⁇ by changing the position (x, y, z) and orientation ( ⁇ ,, ⁇ ).
  • the degree of coincidence for example, an evaluation value (for example, the sum of the difference between the probability values) regarding the difference between the probability values of both adjacent voxels of the environmental model and the measurement data is used. be able to. In this case, the optimal position and orientation of the measurement data are changed so that the degree of coincidence is minimized.
  • an evaluation value for example, the sum of the difference between the probability values
  • the representative value of the measurement data and an evaluation value related to the probability value of the environmental model's botasel that is close to the error distribution can be used.
  • the optimal position and orientation of the measurement data are changed so as to maximize the degree of coincidence.
  • Precise alignment step For the alignment of the environmental model and measurement data in S7, use a method that considers the error distribution in the ICP algorithm that can align the point cloud and the point cloud. The For the initial value of alignment, use the position and orientation obtained by rough alignment.
  • the distance between error distributions used in the ICP algorithm for example, when the error distributions intersect, it is considered as the same measurement point, and the distance value in that case is multiplied by the weight obtained by the distribution matching force. To do.
  • a distance scale such as the Mahalanobis distance can be used to match the distributions.
  • Equation (4) the composite function of the Mi Di Di distribution is w and the number of environmental model data corresponding to the measurement data is N, it can be defined by Equation (4) in [Equation 5].
  • T represents transposition.
  • the optimal position and orientation of the measurement data is determined by changing the measurement data measurement position (X, y, z) and orientation ( ⁇ ,, ⁇ )
  • model update step S8 is performed after the fine alignment step S7, and updates the environment model constructed in the model construction step S4.
  • FIG. 11 is a data processing flowchart in the model update step S8. As shown in this figure, if a search is made for a button cell corresponding to the coordinate value of the new measurement point input in step ST1, and there is no representative point in the corresponding button cell in step ST2 (both cell is empty) In step ST3, the coordinate value and error distribution of the measured point newly input in step ST3 are set (registered newly) as the coordinate value and error distribution of the representative point.
  • step ST3 there should be essentially no object between the new measurement position (origin) and the measured point. Therefore, the representative point and error distribution in the button cell located between the new measurement position (origin) and the measured point are reset or deleted.
  • FIG. 12 is a schematic diagram in the case where there is a representative point already set in the corresponding button cell. If there is a representative point already set in the corresponding botacell in step ST2 in Fig. 11, the error distribution newly acquired in step ST4 is compared with the error distribution in the already set botacell (i.e., different points are the same). Judge whether it is one point).
  • the voxels are set so that only a single representative point exists in a single botacel in steps ST6 and ST7. Further, it is divided into eight and hierarchically divided into a plurality of botasels and newly registered.
  • the criteria for division and synthesis are determined from, for example, the degree of coincidence of error distributions.
  • a distance scale such as Mahalanobis distance can be used as the degree of coincidence of the error distribution.
  • step ST5 Both error distribution forces in step ST5 When the new error distribution and the center of the new error distribution are re-set, a new representative point moves into another botacel (ie, in step ST8, ⁇ es) Returning to step ST2, the above-described processing is repeated.
  • Fig. 13 shows new error distributions and new errors from both error distributions in step ST5, or from both error distributions and the coordinate values of the representative points already set in the botacell and the newly input measured points. As a result of resetting the center of the distribution, a new representative point moves to another botasel.
  • the representative point and error distribution in the botacell are newly registered or reset, or deleted, Or, depending on the new registration process after division, the probability value in the boat cell is also newly registered, reset, erased, or newly registered after division by statistical processing.
  • FIG. 14 is another schematic diagram when error distributions overlap each other ((A) in FIG. 12).
  • a Kalman filter can be used as means for setting a new representative point and error distribution by combining the two representative points and the error distribution.
  • the two representative points are x (l) and x '(2), and the two error distributions are ⁇ (1) and ⁇ ' (2).
  • Figure 14 shows a schematic diagram for calculating the representative point x (2) and the error distribution ⁇ (2) where X is (2) and the error distribution is ⁇ (2).
  • the botacell position, the representative points, and their error distribution are output to the output device 36.
  • the output device 36 is a display device (for example, CRT), it is preferable to perform stereoscopic display on a three-dimensional image. These data may be transferred to another device (for example, a control device or a computer) or output by a printer.
  • an output step S9 together with the self-position, the botacell position, representative point, and error distribution based on the self-position are output to an output device. Further, in the output step S9, the position of the representative point of the button cell is output to the output device 36 as a measured value of a three-dimensional shape, and an index (for example, a numerical value) indicating the reliability or accuracy of the measured value is displayed on the button cell. You may output to the output device 36 based on the magnitude
  • the measured value is larger than the reference value! If the reliability or accuracy of the measured value is lower than a predetermined reference value, the measured value of the voxel (that is, the position of the representative point of the votacel) is output. Don't output to
  • the procedure of the process shown in FIG. 4 repeats the process every time new measurement data is obtained, and at least the internal storage device 34 and the external storage device 33 are used. Also stores the result in either. In order to speed up processing, it is preferable to store the results in the internal storage device 34 as much as the capacity allows.
  • a spatial region in which a three-dimensional shape exists is divided into a plurality of botasels 6, and the positions of the respective botasels are stored in the external storage device 33. Even if is large, the amount of data can be reduced to a data size that is small in proportion to the number of botasels.
  • a probability value representing the existence probability of an object is set and stored in the inside of the boat cell. Therefore, even when the error distribution is wider than the one that the representative point belongs to, the probability value of the corresponding botacel that does not cause the voxel to which the representative point belongs to find the presence or absence of an object in each voxel and recalculate the error distribution force. This makes it possible to reduce the search time.
  • the current measurement position can be estimated from changes in the past measurement position, or acquired using a sensor that can acquire the current measurement position, or using the reflection intensity as well as the distance value of the measurement data, Because it is limited, search time can be reduced.
  • step S6 new measurement data and error distribution are rotated and translated with respect to the environmental model for the previous measurement position, and representative points close to the measurement data and error distribution are included.
  • An evaluation value related to the distance between the botacells for example, the total value of the distances
  • the environment model for the new measurement position is rotated and translated, and the evaluation value related to the distance between the botasels having representative points ( For example, the sum of the distances)
  • the new measurement data and error distribution are rotated and translated with respect to the environmental model for the previous measurement position, or the environmental model for the new measurement position is rotated and translated to approach each other. Since the alignment is performed so that the evaluation value related to the distance between the error distributions (for example, the sum of the distances) is minimized, precise alignment between the error distributions can be performed in a short time.
  • the environmental model of the present invention when used, it is possible to obtain a point by having one representative point in the boatel cell. Since the number is reduced, the amount of calculation can be reduced. In addition, despite the decrease in the number of points, the environmental model of the present invention can maintain the accuracy by performing the error distribution of representative points and the hierarchization of botasels.
  • the conventional ICP algorithm has the ability to output incorrect results for sparse data. Since the environment model of the present invention has a representative point and an error distribution in a boatel cell, alignment corresponding to sparse data is possible. Is possible.
  • the maximum botacell 9 is set to a size corresponding to the necessary minimum resolution, and there are multiple measurement points in a single botacell 9
  • the voxel is further divided into eight and divided into a plurality of hierarchically so that only a single point to be measured exists in a single botagel, the data volume can be reduced to a small data size.
  • the resolution can be further increased by using the divided botacell and representative points.
  • a plurality of coordinate values on a three-dimensional shape are acquired as distance data having a plurality of measurement positions as origins, and the coordinate value of the distance data is used as the coordinate value of the representative point, and the distance data
  • the voxel is further divided into eight and divided into a plurality of hierarchically so that only a single representative point exists in a single votacell. It is possible to converge to a highly accurate shape while avoiding accumulation.
  • the method and apparatus of the present invention has a function of correcting distance data including errors to accurate information, and by repeating this, it is highly accurate for long-time measurement. Converge to shape.
  • the method of the present invention is a process for updating the representative points 7 corresponding to the respective botasels 6 and the error distribution 8 thereof with new measurement points, so that the calculation amount is small.
  • the measurement data can be sequentially integrated into a voxel structure with the maximum resolution and the minimum resolution, and the memory size does not greatly exceed the fixed size.
  • the Kalman filter is used to calculate the coordinate value of the measured point newly input and its error distribution, the representative point in the already-configured button cell, and its error distribution. New representative points and error distribution are acquired and reset.
  • the position m (i) of each model point group is a state quantity, and the model is expressed by the following equation [Equation 6] based on the position of the distance sensor measurement point.
  • m (i) is a representative point inside the boat cell (the same applies hereinafter).
  • L (j) is a measurement position by the distance sensor.
  • t is the transposed matrix (the same applies below)
  • h (R, t, m (i)) is an observation system model for L (j).
  • the translation vector t (X, y, z) representing the position of the mobile body (for example, mobile robot) equipped with the distance sensor with respect to the world coordinate system.
  • v (i) is the observation noise added to the measured value L (j) of the distance sensor.
  • the measurement object is stationary, and the position and orientation of the measurement object! Fix ⁇ to the environment model.
  • the measurement point group by the distance sensor is associated with the point i (that is, the representative point) on the environmental model point group.
  • the point i on the model point group to which this association is performed is updated by the following equation (4). Update only the representative point m (i) on the model point cloud associated with the measurement point cloud by the distance sensor using the following [Equation 7]!
  • m and (i) indicate the updated value (post-estimate value) of m (i), and m (i) is m, k k k k, k-1 k-
  • ⁇ (i) is the error covariance matrix of the representative point m (i) inside the boata cell (i.e., the above error mk k
  • ⁇ (i) indicates the updated value (post-mortem estimate) of ⁇ (i) mk mk mk
  • ⁇ (i) indicates the predicted value (preliminary estimated value) of / (i) based on ⁇ '(i)! /.
  • N is the total number of measurement points obtained by 3D LRF.
  • K (i) is the Kalman gain for m (i).
  • the range of the error covariance matrix (that is, the error distribution) in the botacell is gradually reduced, and the botacell is easily divided.
  • the botacell is easily divided.
  • FIG. 16 is a partially enlarged view of FIG.
  • the length of one side of the initial botasel is 100 cm and the number of subdivisions is allowed up to six.
  • the measurement target is accurately represented as a result of repeated subdivision of the voxels. If the target does not exist, the sub-division of the botacell is not performed in the area, which means that the environment can be expressed with the necessary and sufficient amount of data.
  • an environment map with a small error distribution of representative points in each botasel can be expressed with high accuracy. In this way, even for data containing errors, the result of convergence to the true value can be obtained due to the effect of the Kalman filter.
  • the standard deviation is reduced by increasing the number of measurement data, and further improvement in accuracy can be expected.
  • the degree of coincidence determined by the maximum likelihood estimated value based on the adjacent error distributions. Rotate and translate the new measurement data and error distribution with respect to the environment model for the previous measurement position, or the environment for the new measurement position so that the evaluation value (for example, the total power of the degree of coincidence) is maximized. Rotate and translate the model to align! /.
  • the evaluation value for example, the sum of the matching degrees
  • the evaluation function is determined to be maximized when the appearance probability of each measurement point is maximized.
  • mean value representedative point
  • the probability value of L (j) measurement data is obtained. Pr (L (j)
  • ⁇ (j) is 1 when there is a point associated with the measurement point j in the model point group, and 0 otherwise.
  • Pr (L (j) I q) represents the probability value that measurement data L (j) can be obtained when the point on the environment map is at position q
  • Pr (q I m (i) , ⁇ (i)) is assumed to follow the normal distribution of mean value m (i) and error covariance matrix ⁇ (i), and represents the probability that the point on the environment map is at position q.
  • the number [10] holds.
  • the evaluation function EM (i, j) representing the degree of matching between the model point cloud point i and the measurement point cloud point j has an average value m (i) and an error covariance matrix ⁇ ( i) In the normal distribution of + ⁇ (j), z
  • Range of the error covariance matrix ⁇ (j) at the target measurement point j e.g., three times the standard deviation
  • the topmost botacell that intersects with the surrounding (bottom)) and the topmost botacell adjacent to the botacell are obtained, and the representative points existing in these botacells including the lower one are taken as candidates for corresponding points. Since the Botacell has a hierarchical structure, the search for this candidate point requires little computational cost. At this time, if there is no candidate representative point, it is considered that there is no corresponding point.
  • the reason for adding an adjacent buttonel to a candidate is that the range of the error covariance matrix may extend to the adjacent buttonel depending on the position of the representative point in the buttonel.
  • the formula based on likelihood is adopted as the evaluation function EM (i, j) for correspondence, and there is a statistically clear judgment scale regarding the presence or absence of corresponding points. Even in the case where it is considered not to be performed, the association is not forcibly performed. If there is no corresponding point, the target measurement point is interpreted as a point that has not been measured so far and is added to the environment map.
  • the method and apparatus for aligning three-dimensional shape data has been described. However, by viewing the two-dimensional shape as a special case of three-dimensional shape, the method for aligning two-dimensional shape data and the form of the apparatus Can also be implemented.
  • the above output step it is not necessary to output all the botacell positions, representative points, and error distributions.
  • a three-dimensional shape can be grasped or one of them can be obtained.
  • at least one of the botacell position, the representative point, and the error distribution may be output to the output device.

Abstract

　三次元形状上の座標値をコンピュータに入力するデータ入力ステップＳ１と、三次元形状の存在する空間領域を、境界表面が互いに直交する立方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築ステップＳ４と、座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチングステップＳ５とを実施する。更に、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離の総和が最小になるように位置合わせする精密合わせステップＳ７を実施し、ボクセル位置、代表点および誤差分布の少なくともいずれかを出力装置に出力する。

Description

明 細 書
三次元形状データの位置合わせ方法と装置
発明の背景
[0001] 発明の技術分野
本発明は、静止している三次元形状を複数の計測位置力 計測し、その距離デー タを統合して位置合わせするための三次元形状データの位置合わせ方法と装置に 関する。
[0002] 関連技術の説明
本発明にお 、て静的な三次元形状を多視点力 計測した複数の距離画像の幾何 学的位置関係を推定することを「位置合わせ」と呼ぶ。
[0003] 三次元形状の計測技術が普及し、様々な応用分野、例えば、機械部品 ·工作物等 の形状計測、移動ロボットにおける自己位置の同定、地形'構造物の計測等に三次 元形状データが用いられて 、る。
ある物体の三次元計測を行って三次元形状を復元する場合、 1つの視点 (計測位 置)からの計測ではその視点に面する表面情報は獲得できるが、物体の背面は計測 できない。そのため、三次元形状の復元には、複数視点からの計測およびその計測 結果を精度よく位置合わせする必要がある。
[0004] 複数の視点から取得したデータを位置合わせするためには、視点と計測対象の位 置関係が明らかでなければならない。この視点と計測対象の位置関係を取得する方 法として、物体をステージ上に配置し回転させる方法や、リンク機構を持つアームな どに視点を持たせる方法が存在するが、共に位置関係の精度に限界がある。また、 仮に精度が得られたとしても、計測物体の大きさに制限が発生してしまう。
[0005] そこで位置関係の精度の影響を受けず、かつ計測物体の大きさにも制限されない 手段として、先に得られた計測データとその後で得られた計測データとを比較しなが ら位置合わせする手段が種々提案されて 、る。
このうち、対応点が既知の場合には、回転と並進による自乗誤差の最小化問題とし て、容易に求めることができる。しかし、一般的に対応点は既知ではない。
[0006] 対応点が既知でない場合の位置合わせ手段として、不変特徴量によるマッチング、 直接法、および ICPアルゴリズムが既に提案されている（例えば、非特許文献 1、 2)。
[0007] 「不変特徴量によるマッチング」は、形状に不変な局所的な特徴点 (微分特徴など） を利用して 2つのデータのマッチングを行うものである。
「直接法」は、二次元画像におけるオプティカルフローを三次元画像について求め るものである。
riCP (Iterative Closest Points)アルゴリズム」は、先の計測データに対するそ の後の計測データの最も近傍の点を求め、回転'並進を行いながらその距離の総和 が最小になる状態を一致状態となるように解を求めるものである。
[0008] また、本発明に関連する技術として、非特許文献 3が開示されている。
[0009] 非特許文献 1：増田健、他「複数距離画像からの形状モデル生成技術」
非特許文献 2 : Paul J. Besl, "A Method for Registration of 3 -D Shap es , IEEE Transactions of Pattern Analysis and Mechanical Intellig ence, Vol. 14, No. 2, February 1992
非特許文献 3 :関本清英、他「三次元レーザレーダの開発」、石川島播磨技報 Vol. 4 3 No. 4 (2003 - 7)
発明の開示
発明が解決しょうとする課題
[0010] 三次元レーザレーダのような距離センサを用いる場合、計測される三次元形状上の 被計測点は、横方向及び垂直方向に離散した点群となる。この点群の間隔は、計測 点からの距離が例えば 50mの場合、被計測点の間隔は、例えば横方向で約 315m m、垂直方向で約 525mmに達する。
[0011] また、複数の計測位置力も静止している三次元形状を計測する場合、三次元レー ザレーダのような距離センサでは、計測位置毎に、被計測点の位置は通常異なる。 さらに、このような距離センサは、一般に測定距離に例えば約 20cm前後の誤差を 有する。
従って、三次元レーザレーダのような距離センサを用いる場合、以下の制約条件 A 〜Cがある。
条件 A：計測データに点数が少ない（例えば、 1フレーム = 166 X 50点） 条件 B：計測データに誤差を含む (例えば、測定距離に約 20cm前後） 条件 C：計測データは同じ計測点を計るとは限らな!/、。
すなわち、得られる距離データは横方向及び垂直方向に離散した点群であり、計 測毎に位置が相違するため対応点がなぐ測定距離に比較的大きな誤差を含んで いる。
[0012] このような距離データを上述した「不変特徴量によるマッチング」や「直接法」で処理 する場合、離散した点群であり計測点が少なぐ対応点がなぐ計測誤差が大きいた め、正確な位置合わせはほとんど不可能である。
[0013] また、 ICPアルゴリズムを用いる場合、点群の位置合わせは原理的には可能である 力 以下の問題点がある。
(1)誤差の蓄積
ICPアルゴリズムは、 2つの距離データの重ね合わせ手段であり、先のデータとその 後のデータとの比較を繰り返し、その差分を積分しても、対応点がほとんどないため、 誤差が蓄積してしまう。
(2)計算量が多い
ICPアルゴリズムは、繰り返し計算であるため、計算量が膨大となる。すなわち、 IC Pアルゴリズムは、計測データの各データ点に対応するモデルデータを探索する必 要があるため、モデルデータ点数および計測データ点数が増加すると計算量が増大 する。具体的には、モデルデータの点数を M、計測データの点数を Nとした場合、例 えば全探索時の計算オーダは、 O (M X N)となる。
(3)計測点が少な!/、場合を扱えな!/、
ICPアルゴリズムは、密な距離データを対象としているため、離散した点群であり、 空間的に疎である場合は、誤った結果に収束してしまう。
発明の要約
[0014] 本発明は上述した問題点を解決するために創案されたものである。すなわち、本発 明の目的は、静止している三次元形状を複数の計測位置力 計測した距離データが 空間的に離散した点群であり、計測毎に位置が相違して対応点がなぐ比較的大き な誤差を含んでいる場合でも、誤差の蓄積がなぐ少ない計算量で、正確な位置合 わせができる三次元形状データの位置合わせ方法と装置を提供することにある。
[0015] 本発明によれば、静止して 、る三次元形状を複数の計測位置力 計測し、その距 離データを統合して位置合わせするための三次元形状データの位置合わせ方法で あって、
新たな計測位置にぉ 、て、三次元形状上の座標値をコンピュータに入力するデー タ入力ステップと、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体から なる複数のボタセルに分割し、各ボタセル位置を記憶する環境モデルを構築するモ デル構築ステップと、
前記座標値に対応するボタセルの内部に代表点とその誤差分布を設定し記憶する マッチングステップとを実施し、
更に、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分 布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並 進させ、近接する誤差分布間の距離に関する評価値 (例えば、当該距離の総和）が 最小になるように位置合わせする精密合わせステップと、
前記ボタセル位置、代表点、および誤差分布を出力装置に出力する出力ステップ とを有する、ことを特徴とする三次元形状データの位置合わせ方法が提供される。 なお、上記距離に関する評価値は、当該距離の総和の代わりに、当該距離の平均 値、当該距離の 2乗の総和、または、当該距離の最大値であってもよぐ他の適切な 評価値であってもよい。
[0016] 本発明の好ま 、実施形態によれば、前記マッチングステップにお 、て、ボタセル の内部に代表点とその誤差分布に加え、ボタセル内に物体の存在確率を表す確率 値を設定し、記憶する。
[0017] また、前記精密合わせステップの前に、先の計測位置に対する環境モデルに対し 、新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分 布と近接する代表点を有するボタセル間との距離に関する評価値 (例えば、当該距 離の総和）が最小になるように、もしくは新たな計測位置に対する環境モデルを回転 及び並進させ、代表点を有するボタセル間の距離に関する評価値 (例えば、当該距 離の総和）が最小になるように、位置合わせする粗合わせステップを有する。
なお、計測データおよび誤差分布と近接する代表点を有するボタセル間との距離 に関する上記評価値は、当該距離の総和の代わりに、当該距離の平均値、当該距 離の 2乗の総和、または、当該距離の最大値であってもよぐ他の適切な評価値であ つてもよい。
また、代表点を有するボタセル間の距離に関する上記評価値は、当該距離の総和 の代わりに、当該距離の総和の代わりに、当該距離の平均値、当該距離の 2乗の総 和、または、当該距離の最大値であってもよぐ他の適切な評価値であってもよい。
[0018] または、前記精密合わせステップの前に、先の計測位置に対する環境モデルに対 し、新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差 分布と近接する代表点を有するボタセルの確率値に関する評価値 (例えば、当該確 率値の総和）が最大になるように、もしくは新たな計測位置に対する環境モデルを回 転及び並進させ、近接するボタセルが持つ確率値の差に関する評価値 (例えば、当 該確率値の差の総和）が最小になるように、位置合わせする粗合わせステップを有す る。
なお、計測データおよび誤差分布と近接する代表点を有するボタセルの確率値に 関する上記評価値は、当該確率値の総和の代わりに、当該確率値の平均値、または 、当該確率値の 2乗の総和、または、当該確率値の最小値であってもよぐ他の適切 な評価値であってもよい。
また、近接するボタセルが持つ確率値の差に関する上記評価値は、当該確率値の 差の総和の代わりに、当該確率値の差の平均値、当該確率値の差の 2乗の総和、ま たは、当該確率値の差の最大値であってもよぐ他の適切な評価値であってもよい。
[0019] また、前記データ入力ステップの後に、現在の計測位置を過去の計測位置の変化 力 推定し、もしくは現在の計測位置を取得可能なセンサにより取得し、もしくは計測 データの距離値だけでなく反射強度値を利用し、照合する範囲を限定する探索範囲 限定ステップを有する。
[0020] また、前記モデル構築ステップにお 、て、最大のボタセルを必要最小限の分解能 に相当する大きさに設定し、かつ単一のボタセル内に複数の被計測点が存在する場 合に、単一のボタセル内に単一の被計測点のみが存在するように、該ボクセルを更 に分割して階層的に複数のボタセルに分割する。
[0021] また、前記精密合わせステップの後に、前記環境モデルを更新するモデル更新ス テツプを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標 値に対応するボタセルを探索し、
原点と被計測点の間に物体が存在しな 、ものとして、その間に位置するボタセル内 の代表点と誤差分布を再設定もしくは消去する。
[0022] また、前記精密合わせステップの後に、前記環境モデルを更新するモデル更新ス テツプを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標 値に対応するボタセルを探索し、
該ボクセル内に代表点がな 、場合に、前記座標値と誤差分布を代表点の座標値と 誤差分布として設定する。
[0023] また、前記精密合わせステップの後に、前記環境モデルを更新するモデル更新ス テツプを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標 値に対応するボタセルを探索し、
前記ボタセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と 既に設定したボタセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセ ル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差 分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボタセル内に単一の代表点のみが 存在するように、該ボクセルを更に分割して階層的に複数のボタセルに分割する。ボ クセルの分割には、例えば八分木や K—D木を用いる。
[0024] 前記モデル更新ステップにおいて、前記新たに取得した誤差分布と前記既に設定 したボタセル内の誤差分布とを比較し、誤差分布が互いに重複する場合に、両誤差 分布力 新たな誤差分布と新たな代表点を再設定した結果、新たな代表点が他のボ クセル内へ移動したとき、
該他のボタセル内に代表点がな!、場合に、該新たな誤差分布と該新たな代表点を 該他のボタセルの内部に設定し、
該他のボタセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に設 定した該他のボタセル内の誤差分布とを比較し、 (A)誤差分布が互いに重複する場 合に、両誤差分布から、または、両誤差分布とボタセル内に既に設定した代表点と新 たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し 、（B)誤差分布が互いに重複しない場合に、単一のボタセル内に単一の代表点のみ が存在するように、該ボクセルを更に分割して階層的に複数のボタセルに分割する。
[0025] 本発明の別の実施形態によると、前記精密合わせステップの後に、前記環境モデ ルを更新するモデル更新ステップを有し、
該モデル更新ステップにお 、て、新たに入力された被計測点の座標値およびそ の誤差分布と、既に設定したボタセル内の代表点およびその誤差分布とから、カル マンフィルタにより新たな代表点と誤差分布を取得して再設定する。
[0026] 前記精密合わせステップにお ヽて、前記近接する誤差分布間の距離に関する評 価値が最小になるように位置合わせする代わりに、前記近接する誤差分布間に基づ く最尤推定値によって定めた一致度に関する評価値 (例えば、当該一致度の総乗） が最大となるように、先の計測位置に対する環境モデルに対し、新たな計測データお よび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを 回転及び並進させて位置合わせする。
[0027] 前記一致度に関する評価値の算出式が次の [数 1]で表わされ、
[0028] [数 1]
N
Figure imgf000009_0001
1
[0029] この式において、計測点 jと環境モデル上の代表点 iとが対応付けられているとし、 当該計測点 jなる計測データが得られる確率を EM (i、j)としており、 ω (j)は、環境モ デルの中に計測点 jと対応付けられる代表点が存在する場合は 1、それ以外の場合 は 0としている。
[0030] また、本発明によれば、静止して 、る三次元形状を複数の計測位置力も計測し、そ の距離データを統合して位置合わせするための三次元形状データの位置合わせ装 置であって、
三次元形状上の座標値をコンピュータに入力するデータ入力装置と、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体から なる複数のボタセルに分割し、各ボタセル位置を記憶する環境モデルを構築するモ デル構築装置と、
前記座標値に対応するボタセルの内部に代表点とその誤差分布を設定し記憶する マッチング装置と、
前記ボタセル位置、代表点、および誤差分布を出力装置に出力するデータ伝達装 置とを備え、
先の計測位置に対する環境モデルに対し、新たな計測位置に対する環境モデル を回転及び並進させ、近接する誤差分布間の距離に関する評価値 (例えば、当該距 離の総和）が最小になるように位置合わせする、ことを特徴とする三次元形状データ の位置合わせ装置が提供される。
[0031] 前記マッチング装置は、ボタセルの内部に代表点とその誤差分布にカ卩え、ボタセル 内に物体の存在確率を表す確率値を設定し、記憶する。
[0032] 前記三次元形状データの位置合わせ装置は、前記位置合わせ (前記精密合わせ ステップ）の前に、先の計測位置に対する環境モデルに対し、新たな計測データおよ び誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を 有するボタセル間との距離に関する評価値 (例えば、当該距離の総和）が最小になる ように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、代表点を 有するボタセル間の距離に関する評価値 (例えば、当該距離の総和）が最小になるよ うに、位置合わせする粗合わせステップを行う。
[0033] 前記三次元形状データの位置合わせ装置は、前記精密合わせステップの前に、先 の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転 及び並進させ、計測データおよび誤差分布と近接する代表点を有するボタセルの確 率値に関する評価値 (例えば、当該確率値の総和）が最大になるように、もしくは新た な計測位置に対する環境モデルを回転及び並進させ、近接するボタセルが持つ確 率値の差に関する評価値 (例えば、当該確率値の差の総和）が最小になるように、位 置合わせする粗合わせステップを行う。
[0034] 前記三次元形状データの位置合わせ装置は、 前記データ入力ステップの後に、 現在の計測位置を過去の計測位置の変化から推定し、もしくは現在の計測位置を取 得可能なセンサにより取得し、もしくは計測データの距離値だけでなく反射強度値を 利用し、照合する範囲を限定する探索範囲限定ステップを行う。
[0035] 前記三次元形状データの位置合わせ装置は、前記精密合わせステップにおいて、 誤差分布が交差する場合を同一計測点とし、その場合の距離値に分布の一致度か ら求めた重みを掛け合わせて誤差分布間の距離を算出する。
[0036] 前記モデル構築装置は、最大のボタセルを必要最小限の分解能に相当する大きさ に設定し、かつ単一のボタセル内に複数の被計測点が存在する場合に、単一のボタ セル内に単一の被計測点のみが存在するように、該ボクセルを更に分割して階層的 に複数のボタセルに分割する。
[0037] 前記三次元形状データの位置合わせ装置は、 前記精密合わせステップの後に、 前記環境モデルを更新するモデル更新ステップを行う、該モデル更新ステップにお V、て、新たに入力された被計測点の座標値に対応するボタセルを探索し、
原点と被計測点の間に物体が存在しな 、ものとして、その間に位置するボタセル内 の代表点と誤差分布を再設定もしくは消去する。
[0038] 前記三次元形状データの位置合わせ装置は、前記精密合わせステップの後に、前 記環境モデルを更新するモデル更新装置を有し、
該モデル更新装置は、
新たに入力された被計測点の座標値に対応するボタセルを探索し、
該ボクセル内に代表点がな 、場合に、前記座標値と誤差分布を代表点の座標値と 誤差分布として設定する。
[0039] 前記三次元形状データの位置合わせ装置は、前記精密合わせステップの後に、前 記環境モデルを更新するモデル更新装置を有し、
該モデル更新装置は、
新たに入力された被計測点の座標値に対応するボタセルを探索し、 前記ボタセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と 既に設定したボタセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセ ル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差 分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボタセル内に単一の代表点のみが 存在するように、該ボクセルを更に分割して階層的に複数のボタセルに分割する。
[0040] 前記モデル更新装置は、前記新たに取得した誤差分布と前記既に設定したボクセ ル内の誤差分布とを比較し、誤差分布が互いに重複する場合に、両誤差分布力ゝら新 たな誤差分布と新たな代表点を再設定した結果、新たな代表点が他のボタセル内へ 移動したとき、
該他のボタセル内に代表点がな!、場合に、該新たな誤差分布と該新たな代表点を 該他のボタセルの内部に設定し、
該他のボタセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に設 定した該他のボタセル内の誤差分布とを比較し、 (A)誤差分布が互いに重複する場 合に、両誤差分布から、または、両誤差分布とボタセル内に既に設定した代表点と新 たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し 、（B)誤差分布が互いに重複しない場合に、単一のボタセル内に単一の代表点のみ が存在するように、該ボクセルを更に分割して階層的に複数のボタセルに分割する。
[0041] 本発明の別の実施形態によると、前記三次元形状データの位置合わせ装置は、前 記精密合わせステップの後に、前記環境モデルを更新するモデル更新装置を有し、 該モデル更新装置は、新たに入力された被計測点の座標値およびその誤差分布 と、既に設定したボタセル内の代表点およびその誤差分布とから、カルマンフィルタ により新たな代表点と誤差分布を取得して再設定する。
[0042] 前記三次元形状データの位置合わせ装置は、前記精密合わせステップにおいて、 前記近接する誤差分布間の距離に関する評価値 (例えば、当該一致度の総乗)が最 小になるように位置合わせする代わりに、前記近接する誤差分布間に基づく最尤推 定値によって定めた一致度に関する評価値 (例えば、当該一致度の総乗）が最大と なるように、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤 差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及 び並進させて位置合わせする。この場合、前記一致度に関する評価値の算出式が 上記 [数 1]で表わされる。
[0043] 発明の効果
上記本発明の方法と装置によれば、三次元形状の存在する空間領域を、複数のボ クセルに分割し、各ボタセル位置を記憶するので、計測対象物が大きい場合であつ ても、データ量をボタセル数に比例する小さ 、データサイズに抑えることができる。 また、座標値に対応するボタセルの内部に代表点とその誤差分布を設定し記憶す るので、ボタセルの分解能以上の情報を表現することができる。
[0044] また、ボタセルの内部に物体の存在確率を表す確率値を設定し、保存することによ つて、誤差分布が代表点の属するボタセルよりも広がっている場合においても、各ボ クセルにおける物体の存在有無を代表点が属するボタセルを見つけ、その誤差分布 力 再計算させることなぐ当該ボタセルの確率値だけで容易に判断できるので、検 索時間を抑えることができる。
また、現在の計測位置を過去の計測位置の変化から推定し、もしくは現在の計測 位置を取得可能なセンサにより取得し、もしくは計測データの距離値だけでなく反射 強度を利用し、照合する範囲を制限するので、検索時間を抑えることができる。
[0045] また、粗合わせステップにお 、て、先の計測位置に対する環境モデルに対し、新た な計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と 近接する代表点を有するボタセル間との距離に関する評価値 (例えば、当該距離の 総和）が最小になるように、もしくは新たな計測位置に対する環境モデルを回転及び 並進させ、代表点を有するボタセル間の距離に関する評価値 (例えば、当該距離の 総和）が最小になるように、
もしくは新たな計測データおよび誤差分布を回転及び並進させ、計測データおよ び誤差分布と近接する代表点を有するボタセルの確率値に関する評価値 (例えば、 当該確率値の総和）が最大になるように、もしくは新たな計測位置に対する環境モデ ルを回転及び並進させ、近接するボタセルが持つ確率値の差に関する評価値 (例え ば、当該確率値の差の総和）が最小になるように、位置合わせするので、誤差の蓄積 を防ぎながら、短時間に代表点を有するボタセル同士の位置合わせができる。
[0046] また、先の計測位置に対する環境モデルに対し、新たな計測位置に対する環境モ デルを回転及び並進させ、近接するボタセルが持つ確率値の差に関する評価値 (例 えば、当該確率値の差の総和）が最小になるように、位置合わせする場合には、物体 が存在しない情報も加味して位置合わせするので、精度の向上が図れる。
[0047] 次 、で、精密合わせステップにお 、て、先の計測位置に対する環境モデルに対し 、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置 に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離の総和が最 小になるように位置合わせするので、誤差分布同士の精密な位置合わせを短時間に できる。
従って、逐次得られる計測データを本発明の環境モデルに統合することで得たモ デルデータを用いて、計測データが最も一致する位置姿勢を求め、かつ誤差の蓄積 を防ぐことができる。
[0048] また、本発明の環境モデルを用いると、ボタセル内に 1つの代表点を持つことで点 数を減少することになるため、計算量を低下することができる。すなわち、本発明で提 案するデータ構造はボタセル内に 1つの代表点を保存するため、計測点に対応する モデル点を探索する計算オーダを 1とすることができるため、全体での計算オーダを O (N)に減少することができる。
また、点数の減少にもかかわらず、本発明の環境モデルは代表点の誤差分布とボ クセルの階層化を行うことで精度が保たれる。
[0049] さらに、従来の ICPアルゴリズムは疎データに対して誤った結果を出力する力 本 発明の環境モデルは、ボタセル内に代表点と誤差分布を持っているため、疎データ に対応した位置合わせが可能である。
[0050] 従って、本発明の方法と装置によれば、複数視点の計測データを用いた三次元形 状情報復元において、疎なデータであっても誤差の蓄積を防ぎながら高精度な形状 取得が可能となる。また、重ね合わせ時の比較対象となるデータの量が減少するた め、計算量を減少することができる。 [0051] さらに、前記モデル更新ステップにお 、て、新たに入力された被計測点の座標値 およびその誤差分布と、既に設定したボタセル内の代表点およびその誤差分布とか ら、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定するので、より 真値に近 、形状を得ることができる。
特に、カルマンフィルタを用いたモデル更新ステップを繰り返すことで、誤差を含む データであってもカルマンフィルタの効果により真値に収束した高精度な形状が得ら れる。
[0052] また、前記精密合わせステップにお 、て、前記近接する誤差分布間の距離に関す る評価値が最小になるように位置合わせする代わりに、前記近接する誤差分布間に 基づく最尤推定値によって定めた一致度に関する評価値 (例えば、当該一致度の総 和）が最大となるように、先の計測位置に対する環境モデルに対し、新たな計測デー タおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデ ルを回転及び並進させて位置合わせすることで、環境モデル、計測データの双方の 誤差を考慮して位置合わせすることができる。
本発明のその他の目的及び有利な特徴は、添付図面を参照した以下の説明から 明らかになろう。
図面の簡単な説明
[0053] [図 1]非特許文献 2に開示された三次元レーザレーダの構成図である。
[図 2A]距離センサで計測された極座標データと誤差の関係を示す図である。
[図 2B]誤差分布を直方体に包含される楕円体として近似する場合を示している。
[図 3]本発明の方法を実行するための装置構成図である。
[図 4]本発明の方法を示すフローチャートである。
[図 5]モデル構築ステップの模式図である。
[図 6]構築された環境モデルの模式図である。
[図 7A]本発明におけるボタセルデータのデータ構造を示す図であり、各ボタセルデ ータのメモリレイアウト例を示して 、る。
[図 7B]本発明におけるボタセルデータのデータ構造を示す図であり、レベル 2 (1, 1 , 0)のボタセルが代表点を持つ場合の例を示している。 [図 8]粗合わせステップ S6と精密合わせステップ S7のデータ処理フロー図である。
[図 9]粗合わせステップ S6の模式図である。
[図 10]精密合わせステップ S7の模式図である。
[図 11]モデル更新ステップにおけるデータ処理フロー図である。
[図 12]該当するボタセル内に既に設定した代表点がある場合の模式図である。
[図 13]誤差分布が互いに重複する場合に、両誤差分布カゝら新たな誤差分布と新たな 誤差分布の中心を再設定した結果、新たな代表点が他のボタセル内へ移動する場 合を示している。
[図 14]誤差分布が互いに重複する場合の模式図である。
[図 15]カルマンフィルタを用いたモデル更新ステップにより得られた結果を示す。
[図 16]図 15の一部拡大図である。
[図 17]誤差を考慮した対応付けを示している。
好ましい実施例の説明
[0054] 以下本発明の好ましい実施形態について、図面を参照して説明する。なお、各図 において、共通する部分には同一の符号を付し、重複した説明を省略する。
[0055] 図 1は、距離センサの一例としての三次元レーザレーダの構成図である。三次元レ 一ザレーダは、例えば非特許文献 3に開示されて 、る。
この図に示すように、三次元レーザレーダ 10は、レーダヘッド 12と制御器 20から構 成される。レーザダイオード 13から発振されたノ ルスレーザ光 1は、投光レンズ 14で 平行光 2に整形され、ミラー 18a, 18bと回転 '揺動するポリゴンミラー 15で二次元方 向に走査され、測定対象物に照射される。測定対象物から反射されたパルスレーザ 光 3は、ポリゴンミラー 15を介して受光レンズ 16で集光され、光検出器 17で電気信 号に変換される。
[0056] 制御器 20内の時間間隔カウンタ 21は、レーザダイオード 13のパルス発振タイミン グと同期したスタートパルス 4と、光検出器 17から出力されたストップパルス 5の時間 間隔を計測する。信号処理ボード 22は、反射光が検出された時点の時間間隔 t、ポ リゴンミラーの回転角度 Θ、揺動角度 φを極座標データ (r, θ , φ )として出力する。 rは計測位置 (レーダヘッド設置位置)を原点とする距離であり、 r=c X t/2 の式 で求められる。ここで Cは光速である。
判定処理ユニット 23は、信号処理ボード力もの極座標データを、レーダヘッド設置 位置を原点とした三次元空間データ (X, y, z)へ変換して、検出処理を行うようになつ て 、る。なおこの図で 24はドライブユニットである。
[0057] 上述した三次元レーザレーダ 10の計測範囲は、例えば、水平画角 60° 、垂直画 角 30° 、最大測定距離 50mである。また、位置検出精度は、例えば約 20cmである また、計測データを各画素に対して奥行き方向の距離値を持った距離画像で表示 する場合、 1フレームの計測点数を、横方向 166点、スキャン方向 50点とすると、 1フ レームに 166 X 50 = 8300点力 S表示される。この場合にフレームレートは、例えば約 2フレーム Z秒である。
[0058] この三次元レーザレーダ 10で計測される三次元形状上の被計測点は、横方向に
Δ 0 X r、垂直方向に Δ φ X r、互いに離散した点群となる。例えば、 Δ Θ =60/16 6 X π /180 = 6. 3 X 10—3ラジアン、 Δ φ = 30/50 X π /180= 10. 5 X 10—3ラ ジアン、 r= 50mの場合、最も近接する場合でも、被計測点の間隔は、横方向で約 3 15mm,垂直方向で約 525mmとなる。
[0059] 本発明では、距離センサとして、例えば、上述した三次元レーザレーダ 10を用いる 。しかし、距離センサはこれに限定されず、視差を利用した距離センサ、その他の周 知の距離センサを用いることができる。
[0060] 図 2A,図 2Bは、距離センサで計測された極座標データと誤差の関係を示す図で ある。
図 2Aに示すように、任意の計測位置を原点とする極座標値 (r, θ , φ )を計測結果 として計測する。距離センサによる計測結果には、図に示すような誤差分布が通常存 在する。
この誤差分布は、誤差分布の r , θ , φ での存在確率を P (r , θ , φ )とした場合
、誤差分布は計測の軸 r, θ , φ方向に正規分布しているとし、例えば式（1)で表す ことができる。ここで、 r， θ， φはセンサ力もの計測値、 σ , σ , σ は は標準偏差
、 Aは規格ィ匕定数である。 図 2Bに示すように、誤差分布は、通常 r方向に長い切頭円錐形 (左図）に内包され る分布である力 遠方において aと bの差は小さい。従って、この誤差分布を直方体に 包含される楕円体として安全サイドに近似することができる。
[数 2] exp め2 exp iA - Φ)
A 2σ 2 2σ 2 2 2σ .(1 )
2
ムび 2
υ ψ ノ
[0062] 図 3は、本発明の方法を実行するための装置構成図である。この図に示すように、 この装置は、データ入力装置 32、外部記憶装置 33、内部記憶装置 34、中央処理装 置 35および出力装置 36を備える。
[0063] データ入力装置 32は、上述した距離センサを有し、三次元形状上の座標値を記憶 装置に入力する。また、例えばゴ-ォメータ、ォドメータ等を併用して、距
離センサの位置 ·姿勢や移動距離も入力するのがよい。なお、データ入力装置 32は 、キーボード等の通常の入力手段も有するのがよい。
外部記憶装置 33は、ハードディスク、フロッピー（登録商標）ディスク、磁気テープ、 コンパクトディスク等である。外部記憶装置 33は、環境モデルのサイズが大きく後述 する内部記憶装置 34に入力された三次元形状上の座標値、ボタセル位置、及び代 表点とその誤差分布の全体を保持できな ヽ場合には、環境モデルの一部範囲また は全体範囲に対する入力された三次元形状上の座標値、ボタセル位置、及び代表 点とその誤差分布の一部または全部を記憶し、かつ本発明の方法を実行するための プログラムを記憶する。
内部記憶装置 34は、例えば RAM, ROM等であり、環境モデルの一部範囲または 全体範囲に対する入力された三次元形状上の座標値、ボタセル位置、及び代表点と その誤差分布の一部または全部を保管し、かつ演算情報を保管する。
中央処理装置 35 (CPU)は、モデル構築装置、マッチング装置、粗合わせと精密 合わせの位置合わせ装置、モデル更新装置、データ伝達装置として機能し、演算や 入出力等を集中的に処理し、内部記憶装置 34と共に、プログラムを実行する。モデ ル構築装置は、後述のモデル構築ステップを行う装置であり、マッチング装置は、後 述のマッチングステップを行う装置であり、位置合わせ装置は、後述の粗合わステツ プと精密合わせステップを行う装置であり、モデル更新装置は、後述のモデル更新ス テツプを行う装置であり、データ伝達装置は、出力装置 36へデータを出力する装置 である。
出力装置 36は、例えば表示装置、プリンタ、外部記憶装置等であり、内部記憶装 置 34、および外部記憶装置 33の少なくとも 、ずれかに記憶したデータ及びプロダラ ムの実行結果を出力するようになっている。外部装置とのインターフェイスは、 LAN、 USB、 IEEE1394等であり、入力された三次元形状上の座標値に対して該当する ボタセル内の代表点、誤差分布、ボタセル位置などを付加した結果や、環境モデル 全体または環境モデル一部を要求に応じて出力する。
[0064] 上述した本発明の装置は、上述した距離センサと通常の PC (コンピュータ）を組み 合わせたものでもよぐ或いは、全体を一体にした装置であってもよい。また、自走可 能な装置内に一体的に組み込んでもよい。
[0065] 図 4は、本発明の方法を示すフローチャートである。
本発明の方法は、三次元形状上の被計測点の座標値から三次元形状を復元するた めの三次元形状データの位置合わせ方法であり、データ入力ステップ Sl、データ補 正ステップ S2、探索範囲限定ステップ S3、モデル構築ステップ S4、マッチングステツ プ S5、粗合わせステップ S6、精密合わせステップ S7、モデル更新ステップ S8及び 出力ステップ S9を有する。
なお、これら一連の処理のうち、 Sl、 S2、 S3、 S5〜S9は、計測データが得られる 毎に実施し、 S4は初めて計測データが得られたときにだけ実施する。
[0066] データ入力ステップ S1では、距離センサを用いて、三次元形状上の座標値をコン ピュータの記憶装置に入力する。また、例えばゴ-ォメータ、ォドメータ等を併用して 、距離センサの位置 ·姿勢や移動距離も入力するのがよい。
なおこのデータ入力ステップ S1において、三次元レーザレーダ 10を用いて、三次 元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しな 力 順次取得するのがよい。
[0067] 距離センサとして三次元レーザレーダ 10を用いた場合、三次元形状上の座標値は 、任意の計測位置を原点とする距離データであり、極座標値 (r, θ , φ )で表される。 また、各座標値の誤差分布は、極座標値 (r, θ , φ )から演算で求める力 予め別の 入力手段 (例えばキーボード)で入力する。
[0068] データ補正ステップ S2では、距離データの補正処理を行い、距離データの精度を 向上させる。また、極座標データとォドメータのデータから、任意の固定位置を原点と した三次元空間データ (X, y, z)へ変換してもよい。
[0069] 距離データの補正処理では、孤立点の除去、統計的処理、等を行う。孤立点は、 周囲の点力 孤立して存在する点であり、計測データは複数の近接する点で構成さ れることから、孤立点は誤計測と仮定して除去することができる。統計的処理は、計 測データが含む誤差分布を考慮して、複数回の計測を統計処理 (例えば平均値等） することで、距離の補正を行う。
さらに、対象とする三次元形状が、直線近似又は平面近似できる場合にはこれらを 行うのがよい。
[0070] 探索範囲限定ステップ S3では、距離センサの探索範囲を限定する。
探索範囲を限定せずに環境モデルに対する計測データのマッチング処理を行うと 、複数の解 (被計測点）が得られる可能性がある。そこで、（1)現在のセンサ位置を過 去のセンサ位置の変化力 推定し、センサ位置推定結果の近傍を探索する、（2)ォ ドメータを用いてセンサ位置を推定し、探索範囲を限定する、（3)距離データのうち、 距離値だけでなぐ反射強度値も利用して探索結果を絞り込む、等を実施する。
[0071] 図 5は、ボタセルの分割に八分木を用いた場合のモデル構築ステップの模式図で ある。
モデル構築ステップ S4では、この図に示すように、三次元形状の存在する空間領 域を、境界表面が互いに直交する直方体力 なる複数のボタセル 6に分割し、各ボタ セル位置を記憶する環境モデルを構築する。
ボタセル 6の形状は、各辺の長さが等しい立方体でも、各辺の長さが異なる直方体 でもよい。
また、ボタセル 6の各辺の長さは、最大のボタセル 6を必要最小限の分解能に相当 する大きさに設定するのがよい。以下、最大のボタセル 6をレベル 1のボタセルと呼ぶ また、単一のボタセル内に複数の被計測点が存在する場合には、単一のボタセル 内に単一の被計測点のみが存在するように、例えば八分木を選んだ場合は、ボクセ ルを更に八分割して階層的に複数のボタセルに分割する。以下、最大のボタセル 6 の八分割を 1回実施した空間領域をレベル 2のボタセル、 k回実施した空間領域をレ ベル k+ 1のボタセルと呼ぶ。
[0072] 図 6は、構築された環境モデルの模式図である。
マッチングステップ S5では、この図に示すように、三次元形状上の座標値に対応す るボタセル 6の内部に代表点 7とその誤差分布 8を設定し記憶する。末端のボタセル は計測値の代表点を 1つだけ持つことができる。各ボタセルが計測値の代表点とその 誤差分布を持つことで、物体の形状を表す。また、物体の存在確率を表す確率値を ボクセノレ〖こ持たせることちできる。
[0073] マッチングステップ S5において、代表点の絶対位置は [数 3]の式（2)で与えられる 。ここで、（X, y, z)は代表点のボタセルでの相対座標、 Sx, Sy, Szはレベル 1での ボタセルの一辺の大きさ、 n (k) , n (k) , n (k)はレベル kでのボタセルの番地、 Lは 求める代表点が存在するレベルである。
[0074] [数 3]
Figure imgf000021_0001
[0075] 図 7A,図 7Bは、本発明におけるボタセルデータのデータ構造を示す図である。
この図において、図 7Aは、各ボタセルデータのメモリレイアウト例である。この図に おいて、矢印はデータへのリンクを表し、値としてはデータへのポインタを保持する。 図 7Bは、レベル 2 (1, 1, 0)のボタセルが代表点を持つ場合の例を示している。な おこの図において、 nullは空集合を表す。
[0076] 上述したデータ構造の環境モデルは、以下の特徴を有する。
( 1)内容:空間を小直方体で分割して各ボタセルに計測点の代表点と誤差分布を保 持する。 (2)精度:ボタセル毎に持つ計測点の代表値相当である。
(3)存在：物体の存在の有無を表現できる。
(4)データ量:ボタセルの個数に比例してメモリを必要とする力 サイズ固定である。
(5)点群からの変換：適しており、計算量は少な!/、。
(6)アクセス速度：シンプルな構造をして 、るため、要素へのアクセスが高速である。
[0077] またこの特徴から、上述した環境モデルは、以下の効果 A〜Cをすベて満たしてい る。
効果 A：誤差を考慮した表現が可能である。
効果 B：必要なメモリ量と計算量が一定量以下である。
効果 C：物体の存在だけでなく、物体が存在しな!、ことを表せる。
[0078] また図 4において、粗合わせステップ S6と精密合わせステップ S7は、マッチングス テツプ S5の後に実施する。
図 8は、粗合わせステップ S6と精密合わせステップ S7のデータ処理フロー図であり 、図 9は粗合わせステップ S6の模式図、図 10は精密合わせステップ S7の模式図で ある。
[0079] 図 8において、粗合わせステップ S6では、図 9に示すように、先の計測位置に対す る環境モデルに対し、
新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分 布と近接する代表点を有するボタセル間との距離に関する評価値 (例えば、当該距 離の総和）が最小になるように、もしくは新たな計測位置に対する環境モデルを回転 及び並進させ、代表点を有するボタセル間の距離に関する評価値 (例えば、当該距 離の総和）が最小になるように、
もしくは、新たな計測データおよび誤差分布を回転及び並進させ、計測データおよ び誤差分布と近接する代表点を有するボタセルの確率値に関する評価値 (例えば、 当該確率値の総和）が最大になるように、もしくは新たな計測位置に対する環境モデ ルを回転及び並進させ、近接するボタセルが持つ確率値の差に関する評価値 (例え ば、当該確率値の差の総和）が最小になるように、位置合わせする。
[0080] 粗合わせステップ S6における位置合わせは、環境モデルと計測データをボタセル 空間上で表現すること、もしくは環境モデルはボタセル空間上で、計測データは代表 点と誤差分布表現することで実施する。現在の計測データが位置 (X, y, z)、姿勢（ θ , , φ )での計測であったとして、計測データをワールド座標に変換して環境モ デルとの一致度を算出する。
[0081] 一致度の算出には、例えば最短距離法を用いることができる。最短距離法を用い た場合のボタセル間の距離は、 2つのボタセル空間を x(1)、 x(2)、ボタセルの総数 Iを、 ボタセルの値 X (n)とすると、 [数 4]の式（3)で定義できる。
計測データの最適な位置'姿勢は、位置 (x, y, z)、姿勢（θ , , φ )を変化させる ことによって εを最小にする最小自乗法によって算出できる。
[0082] また、一致度として、例えば環境モデルと計測データの両ボクセルにぉ 、て、近接 する両ボクセルの持つ確率値の差に関する評価値 (例えば、当該確率値の差の総 和）を用いることができる。この場合は一致度を最小にするように、計測データの最適 な位置 ·姿勢を変化させる。
また、環境モデルはボタセル空間上で、計測データは代表値と誤差分布表現した 場合には、計測データの代表値、および誤差分布が近接する環境モデルのボタセル の確率値に関する評価値 (例えば、当該確率値の総和）を用いることができる。この 場合は一致度を最大にするように、計測データの最適な位置 ·姿勢を変化させる。
[0083] [数 4] s = hc(l) ~ —-x ((22)\\ = {y, (x m - x P)) y " 3)
[0084] 図 8において、精密合わせステップ S7では、図 10に示すように、先の計測位置に 対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、 もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分 布間の距離に関する評価値 (例えば、当該距離の総和）が最小になるように位置合 わせする。
精密合わせステップ S7における環境モデルと計測データの位置合わせには、点群 と点群の位置合わせが可能な ICPアルゴリズムに誤差分布を考慮した手法を利用す る。位置合わせの初期値には、粗い位置合わせにより得られた位置 ·姿勢を利用す る。
ICPアルゴリズムに利用する誤差分布間の距離の算出には、例えば誤差分布が交 差する場合を同一計測点と考え、その場合の距離値に分布の一致度力 求めた重 みを掛け合わせて算出する。分布の一致には例えばマハラノビス距離のような距離 尺度を利用できる。
この場合の環境モデルと計測データの距離は、環境モデルデータを P
Mi、環境モデ ルデータの誤差分布を∑ 、計測データを P 、計測データの誤差分布を∑ 、誤差
Mi Di Di 分布の合成関数を w、計測データに対応する環境モデルデータの個数を Nとすると 、 [数 5]の式 (4)で定義できる。ここで、 Tは転置を表す。
計測データの最適な位置 ·姿勢は、計測データを計測した位置 (X, y, z) 、姿勢（ θ , , φ )を変化させて P
Diを移動することにより εを最小にする最小自乗法によつ て算出できる。
[0085] [数 5]
Figure imgf000024_0001
[0086] さらに図 4において、モデル更新ステップ S8は、精密合わせステップ S7の後に実 施し、モデル構築ステップ S4で構築した環境モデルを更新する。
[0087] 図 11は、モデル更新ステップ S8におけるデータ処理フロー図である。この図に示 すように、ステップ ST1で新たに入力された被計測点の座標値に対応するボタセル を探索し、ステップ ST2で該当するボタセル内に代表点がない（ボタセルが空である） 場合には、ステップ ST3で新たに入力された被計測点の座標値と誤差分布を代表点 の座標値と誤差分布として設定 (新規に登録)する。
また、このステップ ST3において、新しい計測位置 (原点）と被計測点の間には、原 理的に物体が存在しないはずである。従って新しい計測位置 (原点）と被計測点の間 に位置するボタセル内の代表点と誤差分布を再設定、もしくは消去する。
[0088] 図 12は、該当するボタセル内に既に設定した代表点がある場合の模式図である。 図 11のステップ ST2で該当するボタセル内に既に設定した代表点がある場合には 、ステップ ST4で新たに取得した誤差分布と既に設定したボタセル内の誤差分布と を比較する (すなわち異なる点か同一点かを判断する)。
この比較で、誤差分布が互いに重複する場合（図 12の (A) )には、ステップ ST5で 両誤差分布から、または、両誤差分布とボタセル内に既に設定した代表点と新たに 入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定する（ すなわち誤差分布を合成する)。
またこの比較で、誤差分布が互いに重複しない場合（図 12の（B) )には、ステップ S T6、 ST7で単一のボタセル内に単一の代表点のみが存在するように、該ボクセルを 更に八分割して階層的に複数のボタセルに分割し新規に登録する。
分割と合成の基準は、例えば誤差分布の一致度から判断する。誤差分布の一致度 には例えば、マハラノビス距離のような距離尺度を利用できる。また、 2つの誤差分布 に基づき、両者が同一点を表しているかを統計的検定によって判定してもよい。
[0089] ステップ ST5で両誤差分布力 新たな誤差分布と新たな誤差分布の中心を再設定 した結果、新たな代表点が他のボタセル内へ移動したとき（即ち、ステップ ST8で、 Υ es)、ステップ ST2へ戻り、上述の処理を繰り返す。
なお、図 13は、ステップ ST5で両誤差分布から、または、両誤差分布とボタセル内 に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布 と新たな誤差分布の中心を再設定した結果、新たな代表点が他のボタセル内へ移動 する場合を示している。
[0090] ボタセル内に物体の存在確率を表す確率値を設定する場合には、モデル更新ステ ップ S8において、ボタセル内の代表点と誤差分布の新規登録、もしくは再設定、もし くは消去、もしくは分割後に新規登録の処理に応じて、ボタセル内の確率値も統計処 理によって、新規登録、もしくは再設定、もしくは消去、もしくは分割後に新規登録を 行う。
[0091] 図 14は、誤差分布が互いに重複する場合（図 12の (A) )の別の模式図である。ス テツプ ST5にお 、て、 2つの代表点と誤差分布を合成して新たな代表点と誤差分布 を設定する手段として、カルマンフィルタを用いることができる。例えば、二次元の場 合に、この図に示すように、 2つの代表点をそれぞれ x (l) , x' (2)、 2つの誤差分布 を∑ (1)、∑ ' (2)とし、これを合成した代表点を X (2)、誤差分布を∑ (2)とすると、代 表点 x (2)と誤差分布∑ (2)を算出する模式図は図 14のようになる。
[0092] 図 4において、出力ステップ S9では、ボタセル位置、及び代表点とその誤差分布を 出力装置 36に出力する。出力装置 36が表示装置 (例えば CRT)の場合、三次元画 像上に立体表示するのが好ましい。また、これらのデータを別の装置 (例えば制御装 置、コンピュータ）に転送してもよぐプリンタで出力してもよい。
出力ステップ S9において、前記自己位置とともに、前記自己位置に基づいた前記 ボタセル位置、代表点、および誤差分布を出力装置に出力する。また、出力ステップ S9において、ボタセルの代表点の位置を三次元形状の計測値として出力装置 36に 出力するとともに、該計測値の信頼性または精度を示す指標 (例えば、数値)を、該 ボタセルの内部の誤差分布の大きさに基づいて、出力装置 36に出力してもよい。さ らに、出力ステップ S9において、ボタセルの代表点の位置を三次元形状の計測値と して出力装置 36に出力するときに、該ボクセルの内部の誤差分布の大きさ（広がり） が所定の基準値よりも大き!、場合に、該計測値の信頼性または精度が所定の基準よ りも低いとして、該ボクセルの前記計測値 (即ち、このボタセルの代表点の位置）を出 力装置 36に出力しな 、ようにしてもょ 、。
[0093] 図 4に示した処理の手順は、新たな計測位置にぉ 、て、新 、計測データが得ら れる度に、処理を繰り返し行い、内部記憶装置 34および外部記憶装置 33の少なくと もいずれかに結果を格納する。処理を高速化するためには、内部記憶装置 34に容 量が許す限り結果を格納することが好ま 、。
[0094] 上述した本発明の方法と装置によれば、三次元形状の存在する空間領域を、複数 のボタセル 6に分割し、各ボタセル位置を外部記憶装置 33に記憶するので、計測対 象物が大き 、場合であっても、データ量をボタセル数に比例する小さ 、データサイズ に抑えることができる。
また、座標値に対応するボタセル 6の内部に代表点 7とその誤差分布 8を設定し記 憶するので、ボタセルの分解能以上の情報を表現することができる。
[0095] また、ボタセルの内部に物体の存在確率を表す確率値を設定し、保存することによ つて、誤差分布が代表点の属するボタセルよりも広がっている場合においても、各ボ クセルにおける物体の存在有無を代表点が属するボタセルを見つけ、その誤差分布 力 再計算させることなぐ当該ボタセルの確率値だけで容易に判断できるので、検 索時間を抑えることができる。
また、現在の計測位置を過去の計測位置の変化から推定し、もしくは現在の計測 位置を取得可能なセンサにより取得し、もしくは計測データの距離値だけでなく反射 強度を利用し、照合する範囲を制限するので、検索時間を抑えることができる。
[0096] また、粗合わせステップ S6にお 、て、先の計測位置に対する環境モデルに対し、 新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分 布と近接する代表点を有するボタセル間との距離に関する評価値 (例えば、当該距 離の総和）が最小になるように、もしくは新たな計測位置に対する環境モデルを回転 及び並進させ、代表点を有するボタセル間の距離に関する評価値 (例えば、当該距 離の総和）が最小になるように、
もしくは新たな計測データおよび誤差分布を回転及び並進させ、計測データおよ び誤差分布と近接する代表点を有するボタセルの確率値に関する評価値 (例えば、 当該確率値の総和）が最大になるように、もしくは新たな計測位置に対する環境モデ ルを回転及び並進させ、近接するボタセルが持つ確率値の差に関する評価値 (例え ば、当該確率値の差の総和）が最小になるように、位置合わせするので、誤差の蓄積 を防ぎながら、短時間に代表点を有するボタセル同士の位置合わせができる。 次いで、精密合わせステップ S 7において、先の計測位置に対する環境モデルに対 し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位 置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する 評価値 (例えば、当該距離の総和）が最小になるように位置合わせするので、誤差分 布同士の精密な位置合わせを短時間にできる。
従って、逐次得られる計測データを本発明の環境モデルに統合することで得たモ デルデータを用いて、計測データが最も一致する位置姿勢を求め、かつ誤差の蓄積 を防ぐことができる。
[0097] また、本発明の環境モデルを用いると、ボタセル内に 1つの代表点を持つことで点 数を減少することになるため、計算量を低下することができる。また、点数の減少にも かかわらず、本発明の環境モデルは代表点の誤差分布とボタセルの階層化を行うこ とで精度が保たれる。
[0098] さらに、従来の ICPアルゴリズムは疎データに対して誤った結果を出力する力 本 発明の環境モデルは、ボタセル内に代表点と誤差分布を持っているため、疎データ に対応した位置合わせが可能である。
[0099] 従って、本発明の方法と装置によれば、複数視点の計測データを用いた三次元形 状情報復元において、疎なデータであっても誤差の蓄積を防ぎながら高精度な形状 取得が可能となる。また、重ね合わせ時の比較対象となるデータの量が減少するた め、計算量を減少することができる。
[0100] また、モデル構築ステップ S4にお 、て、最大のボタセル 9を必要最小限の分解能 に相当する大きさに設定し、かつ単一のボタセル 9内に複数の被計測点が存在する 場合に、単一のボタセル内に単一の被計測点のみが存在するように、該ボクセルを 更に八分割して階層的に複数のボタセルに分割するので、データ量を小さいデータ サイズに抑えると同時に、分割後のボタセルと代表点を用いて解像度を更に高めるこ とがでさる。
[0101] 特に、三次元形状上の複数の座標値を複数の計測位置を原点とする距離データと して取得し、該距離データの座標値を、前記代表点の座標値とし、距離データの座 標値の計測誤差を代表点の誤差分布とすることにより、正確な座標値と誤差分布を 用いて複数回の計測を統計的に統合することができ、一層の精度向上が可能となる
[0102] また、原点と被計測点の間に物体が存在しないものとして、その間に位置するボタ セル内の代表点と誤差分布を再設定、もしくは消去すること〖こより、誤った計測デー タの影響を除去することができる。
[0103] また、新たに入力された被計測点の座標値に対応するボタセルを探索し、該ボクセ ル内に代表点がな 、場合に、前記座標値と誤差分布を代表点の座標値と誤差分布 として設定することにより、代表点の座標値と誤差分布を容易に設定できる。
[0104] 更に、前記ボタセル内に既に設定した代表点がある場合に、新たに取得した誤差 分布と既に設定したボタセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセ ル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差 分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボタセル内に単一の代表点のみが 存在するように、該ボクセルを更に八分割して階層的に複数のボタセルに分割する、 ことにより、誤差の蓄積を回避しながら高精度な形状に収束させることができる。
[0105] 従って、本発明の方法と装置によれば、誤差を含む距離データを正確な情報に補 正する機能を有すると共に、これを繰り返すことにより、長時間の計測に対して高精 度な形状に収束する。なおかつ、本発明の方法は、各ボタセル 6に対応する代表点 7 とその誤差分布 8を新たな計測点で更新する処理であるため計算量が小さ 、。また、 演算は周囲のボタセルへの影響を与えずボタセル内で閉じているため、高速処理が 可能である。また、計測データは最大のボタセルが必要最小限の分解能を有するボ クセル構造に逐次統合可能であり、メモリサイズは固定サイズを大きく上回ることはな い。
[0106] カルマンフィルタを用いたモデル更新ステップについて、詳しく説明する。
[0107] カルマンフィルタを用いたモデル更新ステップの場合には、新たに入力された被計 測点の座標値およびその誤差分布と、既に設定したボタセル内の代表点およびその 誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定 する。
[0108] 各モデル点群の位置 m (i)を状態量とし、距離センサの計測点の位置を基に、モデ ルを次の式 [数 6]で表現する。なお、本実施例では、 m (i)は、ボタセル内部の代表 点である（以下、同様)。
[0109] 園
L(j) = hm(Rr，tr，m(i)) + vL(j)
= Rs- Rr (i) - tr) - t + vL(j) (j = 1， ...，N)
[0110] [数 6]において、 L(j)は、距離センサによる計測位置である。例えば、 L(j)は、距離センサのセンサ 座標系において三次元 LRF (レーザレンジファインダ）の計測点; j(j = l, ... , N)の 位置 L(j) = (x (j), y (j), z (j) )tである。ここで、 tは転置行列を示す (以下、同様）
し し し h (R , t , m(i) )は、 L (j)に対する観測系モデルである。
m r r
Rは、距離センサを搭載した移動体 (例えば、移動ロボット）のワールド座標系に対 する姿勢を表す回転行列 R=R(0x, θγ, 0ζ)である。なお、 0χ, θγ, 0ζは、そ れぞれ X軸、 y軸、 z軸周りの回転角を示す (以下、同様)。
ま、距離センサを搭載した移動体 (例えば、移動ロボット）のワールド座標系に対 する位置を表す並進ベクトル t = (X, y, z)である。
v (i)は、距離センサの計測値 L(j)に加わる観測ノイズである。
し
Rは、センサ座標系の移動体座標系に対する回転行列 Rs=R(0x, θγ, 0ζ)で s
ある。
tは、センサ座標系の移動体座標系に対する位置を表す併進ベクトル t = (X, y, z s s
)である。
[0111] 測定対象物は静止しているものであり、測定対象物の位置 姿勢！^を環境モデル に対して固定する。
[0112] 距離センサによる計測点群と、環境モデル点群上の点 i (即ち、代表点)を対応づけ る。この対応付けが行われたモデル点群上の点 iに対して次式 (4)により更新を行う。 なお、距離センサによる計測点群と対応付けが行われたモデル点群上の代表点 m (i )に対してのみ次の [数 7]により更新を行ってよ!、。
[0113] [数 7]
Figure imgf000030_0001
∑Lk(j))- 1
m，k(i) = mk,k— i) +Kmk(i)(Lk(j) -！！ ！^， ，！！！ — ))）
[0114] [数 7]において、 添え字 kは、離散時刻 kでの値であることを表す。
m (i)につ 、て、 m， (i)は m (i)の更新値 (事後推定値)を示し、 m (i)は m， k k k k, k- 1 k-
(i)に基づ!ヽた m (i)の予測値 (事前推定値)を示す。なお、環境 (測定対象物）は
1 k
静止しているので、 m (i) =m， （i)である。
k, k-1 k-1
∑ (i)は、ボタセル内部の代表点 m (i)の誤差共分散行列 (即ち、上述の誤差分 mk k
布)である。また、∑ (i)につ 、て、 ∑， (i)は∑ (i)の更新値 (事後推定値)を示 mk mk mk
し、∑ (i)は∑ ' (i)に基づ!/、た∑ (i)の予測値 (事前推定値)を示す。セ mk, k— 1 mk— 1 mk
ンサ座標系にお 、て三次元 LRFの計測点 j (j=l ,… ,Ν)の位置を L (j)で表し、その誤 差共分散行列を∑ (j)で表す。ここで Nは、三次元 LRFで得られた計測点の総数で
し
ある。三次元 LRFの誤差モデルとして計測距離に関係な 、一定の正規分布を仮定 する。センサ座標系の X軸方向にレーザを照射する場合の誤差共分散行列を∑ Sと する。レーザの照射方向に応じて誤差分布も姿勢を変える。∑ (j)
し は、基準の方向 に対するレーザ照射方向を回転行列 R (j) =R (j)∑ R t(j)
し を用いて∑ (j)
し し S し と表さ れる。計測点 jのワールド座標系における位置 z (j)、およびその誤差共分散行列∑ z ( j)は、それぞれ z (j) =R (R L (j) +t ) +t 、∑ (j) =R R∑ WR ^ tと表すことがで r s s r z s し
きる。
K (i)は、 m (i)に対するカルマンゲインである。
mk
(R , t , m (i) )は、 L (j)、 i=p (j)に対する観測系モデルである。 i=p (j mk rk rk k, k— 1 k k k
)は、計測点 jに対応付けられた環境地図（即ち、環境モデル)上の点である。
H は、 L (j)、 i=p (j)に対する観測系モデルのヤコビアン行列であり、次の [数 8 mk k k
]で表わされる。
[数 8] mk(ivk,tk,mk
Figure imgf000031_0001
カルマンフィルタの更新過程によって、環境地図のモデル点群の各点（ボタセルの 代表点)の位置と誤差共分散行列の更新値 m'. (i)、∑' . (i)が得られた段階で、環
Π1Κ
境モデルの更新を以下の手順で行う。 (1)これら更新値 m' (i)、∑ ' (i)を、新たな代表点、誤差分布として再設定する。
k mk
(2)上述（1)の結果、代表点の位置が別のボタセル内に移動した場合、移動先のボ クセルが代表点を保持して 、な 、ときは、移動後の代表点とその誤差共分散行列を 移動先のボタセルに保持させ、移動元のボタセルからは代表点等を取り除く。移動先 のボタセルが既に代表点を保持しているときには、 2つの代表点において、これらの 両誤差分布が重複するかを判断する（上述の ST4における判断と同様)。その後の 処理は、図 11の ST4以降の処理と同じであってよい。
(3)モデル点群上の代表点 m (i)と対応付けが行われな力つた距離センサによる計 測点について、当該計測点が含まれるボタセルが代表点を持たない場合は、計測点 とその誤差分布をそのボタセルの代表点と誤差分布として追加し保持する。もし、ボ クセル内に既に代表点が存在する場合には、ボタセル内にある対応付けが行われな 力つた他の複数の計測点を含め、既存の代表点と各計測点とが全て異なるボタセル に含まれるように、ボタセルを分割した上で分割後のボタセルに代表点等を継承させ る。
[0117] 上述のカルマンフィルタを用いたモデル更新ステップを繰り返すことで、序々にボタ セル内の誤差共分散行列 (即ち、誤差分布)の範囲が小さくなるとともに、ボタセルが 分割され易くなる。ボタセルが分割されることによって、初期ボタセルのサイズ以下の 変化も表現することが可能となる。
図 15は、カルマンフィルタを用いたモデル更新ステップにより得られた結果を示す 。図 16は図 15の一部拡大図である。これら図において、初期のボタセルの 1辺の長 さを 100cmとし、再分割数を 6分割まで許している。対象が存在している領域では、ボ クセルの再分割を繰り返した結果、計測対象を精度良く表現している。対象が存在し な 、領域ではボタセルの再分割は行われず、必要十分なデータ量で環境を表現で きることがわ力る。また、各ボタセル内の代表点の誤差分布も小さぐ環境地図を高精 度で表現できている。このように、誤差を含むデータであってもカルマンフィルタの効 果により、真値に収束した結果が得られる。さらに、この方法では計測データ数を増 カロさせることによって標準偏差が小さくなり、精度のさらなる向上が期待できる。
[0118] また、測定対象物の位置 ·姿勢は固定しているため、更新を測定対象物の位置'姿 勢と独立して行うことができる。なお、距離センサによる計測点群と対応付けが行われ たモデル点群上の代表点 m (i)に対してのみ、上述のカルマンフィルタによる更新を 行うことで、大幅な計算コストの削減が可能になる。
[0119] 前記精密合わせステップにおいて、前記近接する誤差分布間の距離の総和が最 小になるように位置合わせする代わりに、前記近接する誤差分布間に基づく最尤推 定値によって定めた一致度に関する評価値 (例えば、当該一致度の総乗）が最大と なるように、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤 差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及 び並進させて位置合わせしてもよ!/、。
この場合につ 、て詳しく説明する。
[0120] 環境地図（環境モデル)であるモデル点群、およびセンサの計測点群双方に誤差 モデルを考慮しているため、前記一致度に関する評価値 (例えば、当該一致度の総 乗)の算出式 (以下、評価関数と言う）にも双方の誤差を取り入れることが可能である 。本実施例の場合、単に最も近い点を対応付けするのではなぐ尤度の概念を取り 入れることによって、評価関数に環境地図上の誤差分布、および計測点の誤差分布 をカ卩味し、現時点での環境地図において、各計測点の出現確率が最大となる場合に 評価関数も最大となるように定める。
具体的には、計測点 jに対応付けられた環境地図上の点 i=p (j)の位置が、平均値 (代表点) m (i)、誤差共分散行列∑ (i)の正規分布に従うと仮定した上で、三次元し RFで計測した結果、 L (j)なる計測データが得られる確率値 Pr (L (j) | m (i) , ∑ (i) m
)を点 iと点 jとの評価関数 EM (i, j)とし、その総乗が最大となるように次の [数 9]で評 価関数を定める。
[0121] [数 9]
N
EM = I ] { {j)EM{i )}
ゾ =1
N
Figure imgf000033_0001
w)))} [0122] ただし、 ω (j)は、モデル点群の中に計測点 jと対応付けられる点が存在する場合は 1、それ以外の場合は 0とする。
ここで、 Pr(L(j) I q)を環境地図の点が qの位置にある場合に L(j)なる計測データ が得られる確率値を表すものとし、 Pr(q I m(i), ∑ (i) )を平均値 m (i)、誤差共分 散行列∑ (i)の正規分布に従うと仮定した上で環境地図の点が qの位置にある確率 値を表すものとすると、数 [10]が成り立つ。
[0123] [数 10]
Pr(L(y)|m( ,∑/?i()) =
ji W) I q)'Pr(q | m(),∑m())} q
[0124] Pr(q | m(i), ∑ (i))は正規分布を仮定すると、次の [数 11]となる。
[0125] [数 11]
Pr(q|m(/),∑,„(/)) =
exp (- (q - m(i)t∑m(i)— 1 (q - m(i)))
Figure imgf000034_0001
[0126] 一方、 Pr(L(j) | q)は、 L(j)を z(j)で置き換えて、次の [数 12]で近似することがで
[数 12]
Pr(L (ブ) I q)
eXp(- (Z j)― αγΈζΟ)~] (z(j) - q))
2^ J|∑z(j) ここで、 z (j)は、距離センサを搭載した移動体の位置 t、姿勢 Rに依存している。
k r r 実際は、三次元 LRFのセンサ座標系の中心力も見た qの向きと計測点 L(j)の向きと は図 17に示すように異なるため、誤差共分散行列∑ (j)も qの向きに合わせて回転 変換する必要がある力 対応付けられた環境地図上の点 iから大きく離れたところに ある qの存在確率は低いため、十分な精度で近似できると考えられる。よって、 Pr(L( j) I m(i), ∑ (i))は、次の [数 13]で表すことができる。
[数 13]
Pr(LO')|m( ,∑w( ) =
1
X
^ (])∑ ) J{exp(-l(zO) - ∑z yl (ζθ)― q))
• x exp(-i(m(i)― q)t∑m(i)"1 (m(i)― q))}dq
[0129] 簡単な計算によって、次の [数 14]を得る。
[0130] [数 14]
Pr(L(J)|m( ,∑m( ) =
1 z し
= . _ 6 , _ X exP(-rP ')) X
2^ (j)∑m(i)| 2 exp(-i(q一 αひ)) t 一1 +∑m(i)一1 )(q - (j)))dq 2^ ^\(Z∑( )~] +∑m(i)-Y \ 〗
6 , exp(--p ))
, 2 , 1 exp (ュ β( ))
2^ I∑,G) +∑m(i)l 2
[0131] ただし、 a (j), β (j)は次の [数 15]で表すことができる。
[0132] [数 15] a( ')=(∑z G)— +∑m (/)—
Figure imgf000036_0001
zG^O^ CD+ Em W^ ^i)— zG))
[0133] 従って、モデル点群の点 iと計測点群の点 jとの対応付けの一致度を表す評価関数 EM (i, j)は、平均値 m(i)、誤差共分散行列∑ (i) +∑ (j)の正規分布において、 z
m z
(j)が得られる確率値に近似できる。この評価関数を用いることによって、環境地図、 計測データ双方の誤差を考慮した対応付けが可能となる。
[0134] 計測点と環境地図 (即ち、環境モデル)との対応付けについて補足説明をする。上 記実施例では、誤差分布を考慮した統計的な評価関数を用いるため、評価関数の 値を求めなければ対応点を定めることができない。そこで、環境地図上のモデル点 群の中で対応付けする候補を予め絞り込み、その候補の中から評価関数の値を基 に対応点を求める。具体的には、以下のように定めることができる。
(1)対象とする計測点 jの誤差共分散行列∑ (j)の範囲 (例えば標準偏差の 3倍の範
し
囲）と交わる最上位のボタセルとそのボタセルに隣接して 、る最上位のボタセルを求 め、下層のボタセルも含めこれらのボタセル内に存在する代表点を対応点の候補と する。ボタセルが階層構造となっているため、この候補点の探索には計算コストはほ とんどかからない。このとき、候補となる代表点がない場合には、対応点がないものと みなす。隣接するボタセルも候補に加える理由は、ボタセル内の代表点の位置によ つては、誤差共分散行列の範囲が隣接するボタセルまではみ出すことがあるからで ある。
(2)候補となるボタセルの代表点 iと誤差共分散行列を用いて、評価関数 EM (i, の 値を求める。
(3)評価関数 EM (i, j)の値が最も大きい代表点 iを対応点とする。ただし、評価関数 の値がある閾値未満の場合には、対応点がないものとみなす。
本実施例では、対応付けの評価関数 EM (i, j)として、尤度に基づいた式を採用し ており、対応点の有無に関して統計的に明確な判断尺度があるため、対応点が存在 しないと考えられる場合においても無理に対応付けを行うようなことはない。なお、対 応点がない場合には、対象となる計測点はこれまで未計測の部分に相当する点であ ると解釈し、環境地図に追加する。
[0135] 実施形態として、三次元形状データの位置合わせ方法と装置について説明したが 、二次元形状を三次元形状の特別な場合として見ることにより、二次元形状データの 位置合わせ方法と装置の形態としても実施できる。
また、上記出力ステップにおいて、前記ボタセル位置、代表点、および誤差分布の すべてを出力しなくてもよぐ例えば、これらすべてが無くても三次元形状が把握でき る場合や、これらのうち 1つ又は 2つが必要な場合などにおいては、前記ボタセル位 置、代表点、および誤差分布の少なくともいずれかを出力装置に出力してもよい。
[0136] なお、本発明は、上述した実施形態に限定されず、本発明の要旨を逸脱しない範 囲で種々〖こ変更することができることは勿論である。

Claims

請求の範囲
[1] 静止して!/、る三次元形状を複数の計測位置力 計測し、その距離データを統合し て位置合わせするための三次元形状データの位置合わせ方法であって、
新たな計測位置にぉ 、て、三次元形状上の座標値をコンピュータに入力するデー タ入力ステップと、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体から なる複数のボタセルに分割し、各ボタセル位置を記憶する環境モデルを構築するモ デル構築ステップと、
前記座標値に対応するボタセルの内部に代表点とその誤差分布を設定し記憶する マッチングステップとを実施し、
更に、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分 布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並 進させ、近接する誤差分布間の距離の総和が最小になるように位置合わせする精密 合わせステップと、
前記ボタセル位置、代表点、および誤差分布を出力装置に出力する出力ステップ とを有する、ことを特徴とする三次元形状データの位置合わせ方法。
[2] 前記マッチングステップにおいて、ボタセルの内部に代表点とその誤差分布に加え 、ボタセル内に物体の存在確率を表す確率値を設定し、記憶する、ことを特徴とする 請求項 1に記載の三次元形状データの位置合わせ方法。
[3] 前記精密合わせステップの前に、先の計測位置に対する環境モデルに対し、新た な計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と 近接する代表点を有するボタセル間との距離の総和が最小になるように、もしくは新 たな計測位置に対する環境モデルを回転及び並進させ、代表点を有するボタセル 間の距離の総和が最小になるように、位置合わせする粗合わせステップを有する、こ とを特徴とする請求項 1に記載の三次元形状データの位置合わせ方法。
[4] 前記精密合わせステップの前に、先の計測位置に対する環境モデルに対し、新た な計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と 近接する代表点を有するボタセルの確率値の総和が最大になるように、もしくは新た な計測位置に対する環境モデルを回転及び並進させ、近接するボタセルが持つ確 率値の差の総和が最小になるように、位置合わせする粗合わせステップを有する、こ とを特徴とする請求項 1に記載の三次元形状データの位置合わせ方法。
[5] 前記データ入力ステップの後に、現在の計測位置を過去の計測位置の変化力 推 定し、もしくは現在の計測位置を取得可能なセンサにより取得し、もしくは計測データ の距離値だけでなく反射強度値を利用し、照合する範囲を限定する探索範囲限定ス テツプを有する、ことを特徴とする請求項 1に記載の三次元形状データの位置合わせ 方法。
[6] 前記精密合わせステップにお 、て、誤差分布が交差する場合を同一計測点とし、 その場合の距離値に分布の一致度カゝら求めた重みを掛け合わせて誤差分布間の距 離を算出する、ことを特徴とする請求項 1に記載の三次元形状データの位置合わせ 方法。
[7] 前記モデル構築ステップにお 、て、最大のボタセルを必要最小限の分解能に相当 する大きさに設定し、かつ単一のボタセル内に複数の被計測点が存在する場合に、 単一のボタセル内に単一の被計測点のみが存在するように、該ボクセルを更に分割 して階層的に複数のボタセルに分割する、ことを特徴とする請求項 1に記載の三次元 形状データの位置合わせ方法。
[8] 前記精密合わせステップの後に、前記環境モデルを更新するモデル更新ステップ を有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対 応するボタセルを探索し、
原点と被計測点の間に物体が存在しな 、ものとして、その間に位置するボタセル内 の代表点と誤差分布を再設定もしくは消去する、ことを特徴とする請求項 1に記載の 三次元形状データの位置合わせ方法。
[9] 前記精密合わせステップの後に、前記環境モデルを更新するモデル更新ステップ を有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対 応するボタセルを探索し、
該ボクセル内に代表点がな 、場合に、前記座標値と誤差分布を代表点の座標値と 誤差分布として設定する、ことを特徴とする請求項 1に記載の三次元形状データの位 置合わせ方法。
[10] 前記精密合わせステップの後に、前記環境モデルを更新するモデル更新ステップ を有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対 応するボタセルを探索し、
前記ボタセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と 既に設定したボタセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から新たな誤差分布と新たな代表 点を再設定し、
誤差分布が互いに重複しない場合に、単一のボタセル内に単一の代表点のみが 存在するように、該ボクセルを更に分割して階層的に複数のボタセルに分割する、こ とを特徴とする請求項 1に記載の三次元形状データの位置合わせ方法。
[11] 前記モデル更新ステップにお 、て、前記新たに取得した誤差分布と前記既に設定 したボタセル内の誤差分布とを比較し、誤差分布が互いに重複する場合に、両誤差 分布から、または、両誤差分布とボタセル内に既に設定した代表点と新たに入力され た被計測点の座標値から、新たな誤差分布と新たな代表点を再設定した結果、新た な代表点が他のボタセル内へ移動したとき、
該他のボタセル内に代表点がな!、場合に、該新たな誤差分布と該新たな代表点を 該他のボタセルの内部に設定し、
該他のボタセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に 設定した該他のボタセル内の誤差分布とを比較し、（A)誤差分布が互いに重複する 場合に、両誤差分布から、または、両誤差分布とボタセル内に既に設定した代表点と 新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設 定し、（B)誤差分布が互いに重複しない場合に、単一のボタセル内に単一の代表点 のみが存在するように、該ボクセルを更に分割して階層的に複数のボタセルに分割 する、ことを特徴とする請求項 10に記載の三次元形状データの位置合わせ方法。
[12] 前記精密合わせステップの後に、前記環境モデルを更新するモデル更新ステップ を有し、
該モデル更新ステップにお 、て、新たに入力された被計測点の座標値および誤差 分布と、既に設定したボタセル内の代表点およびその誤差分布とから、カルマンフィ ルタにより新たな代表点と誤差分布を取得して再設定する、ことを特徴とする請求項
1に記載の三次元形状データの位置合わせ方法。
[13] 前記精密合わせステップにおいて、前記近接する誤差分布間の距離の総和が最 小になるように位置合わせする代わりに、前記近接する誤差分布間に基づく最尤推 定値によって定めた一致度に関する評価値が最大となるように、先の計測位置に対 する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、も しくは新たな計測位置に対する環境モデルを回転及び並進させて位置合わせする、 ことを特徴とする請求項 1に記載の三次元形状データの位置合わせ方法。
[14] 前記一致度に関する評価値の算出式が次の [数 16]で表わされ、
[数 16]
N
Figure imgf000041_0001
ゾ =1 この式にぉ 、て、計測点 jと環境モデル上の代表点 iとが対応付けられて 、るとし、 当該計測点 jなる計測データが得られる確率を EM (i、j)としており、 ω (j)は、環境モ デルの中に計測点 jと対応付けられる代表点が存在する場合は 1、それ以外の場合 は 0としている、ことを特徴とする請求項 13に記載の三次元形状データの位置合わ せ方法。
[15] 静止して 、る三次元形状を複数の計測位置力 計測し、その距離データを統合し て位置合わせするための三次元形状データの位置合わせ装置であって、
三次元形状上の座標値をコンピュータに入力するデータ入力装置と、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体から なる複数のボタセルに分割し、各ボタセル位置を記憶する環境モデルを構築するモ デル構築装置と、
前記座標値に対応するボタセルの内部に代表点とその誤差分布を設定し記憶する マッチング装置と、
前記ボタセル位置、代表点、および誤差分布を出力装置に出力するデータ伝達装 置とを備え、
先の計測位置に対する環境モデルに対し、新たな計測位置に対する環境モデル を回転及び並進させ、近接する誤差分布間の距離の総和が最小になるように位置合 わせする、ことを特徴とする三次元形状データの位置合わせ装置。
[16] 静止して 、る三次元形状を複数の計測位置力 計測し、その距離データを統合し て位置合わせするための三次元形状データの位置合わせ方法であって、
新たな計測位置にぉ 、て、三次元形状上の座標値をコンピュータに入力するデー タ入力ステップと、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体から なる複数のボタセルに分割し、各ボタセル位置を記憶する環境モデルを構築するモ デル構築ステップと、
前記座標値に対応するボタセルの内部に代表点とその誤差分布を設定し記憶する マッチングステップとを実施し、
更に、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分 布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並 進させ、近接する誤差分布間の距離に関する評価値が最小になるように位置合わせ する精密合わせステップと、
前記ボタセル位置、代表点および誤差分布の少なくともいずれかを出力装置に出 力する出力ステップとを有する、ことを特徴とする三次元形状データの位置合わせ方 法。
[17] 前記精密合わせステップの前に、先の計測位置に対する環境モデルに対し、新た な計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と 近接する代表点を有するボタセル間との距離に関する評価値が最小になるように、も しくは新たな計測位置に対する環境モデルを回転及び並進させ、代表点を有するボ クセル間の距離に関する評価値が最小になるように、位置合わせする粗合わせステ ップを有する、ことを特徴とする請求項 16に記載の三次元形状データの位置合わせ 方法。
[18] 前記精密合わせステップの前に、先の計測位置に対する環境モデルに対し、新た な計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と 近接する代表点を有するボタセルの確率値に関する評価値が最大になるように、もし くは新たな計測位置に対する環境モデルを回転及び並進させ、近接するボタセルが 持つ確率値の差に関する評価値が最小になるように、位置合わせする粗合わせステ ップを有する、ことを特徴とする請求項 16に記載の三次元形状データの位置合わせ 方法。
静止して 、る三次元形状を複数の計測位置力 計測し、その距離データを統合し て位置合わせするための三次元形状データの位置合わせ装置であって、
三次元形状上の座標値をコンピュータに入力するデータ入力装置と、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体から なる複数のボタセルに分割し、各ボタセル位置を記憶する環境モデルを構築するモ デル構築装置と、
前記座標値に対応するボタセルの内部に代表点とその誤差分布を設定し記憶する マッチング装置と、
前記ボタセル位置、代表点および誤差分布の少なくともいずれかを出力装置に出 力するデータ伝達装置とを備え、
先の計測位置に対する環境モデルに対し、新たな計測位置に対する環境モデル を回転及び並進させ、近接する誤差分布間の距離に関する評価値が最小になるよう に位置合わせする、ことを特徴とする三次元形状データの位置合わせ装置。
PCT/JP2006/325047 2005-12-16 2006-12-15 三次元形状データの位置合わせ方法と装置 WO2007069724A1 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
US12/096,851 US8116558B2 (en) 2005-12-16 2006-12-15 Three-dimensional shape data position matching method and device
CN2006800474726A CN101331381B (zh) 2005-12-16 2006-12-15 三维形状数据的位置对准方法和装置
JP2007550241A JP4650751B2 (ja) 2005-12-16 2006-12-15 三次元形状データの位置合わせ方法と装置
DE112006003380T DE112006003380T5 (de) 2005-12-16 2006-12-15 Verfahren und Vorrichtung für die Positionsabgleichung von dreidimensionalen Formdaten

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2005363302 2005-12-16
JP2005-363302 2005-12-16

Publications (1)

Publication Number Publication Date
WO2007069724A1 true WO2007069724A1 (ja) 2007-06-21

Family

ID=38163026

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2006/325047 WO2007069724A1 (ja) 2005-12-16 2006-12-15 三次元形状データの位置合わせ方法と装置

Country Status (5)

Country Link
US (1) US8116558B2 (ja)
JP (1) JP4650751B2 (ja)
CN (1) CN101331381B (ja)
DE (1) DE112006003380T5 (ja)
WO (1) WO2007069724A1 (ja)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2008309671A (ja) * 2007-06-15 2008-12-25 Ihi Corp 物体認識方法および装置
JPWO2007069721A1 (ja) * 2005-12-16 2009-05-28 株式会社Ihi 三次元形状データの記憶・表示方法と装置および三次元形状の計測方法と装置
JPWO2007069726A1 (ja) * 2005-12-16 2009-05-28 株式会社Ihi 自己位置同定方法と装置および三次元形状の計測方法と装置
JP2009128097A (ja) * 2007-11-21 2009-06-11 Ihi Corp バリまたは欠損認識方法と装置およびバリ取り方法と装置
JP2010176657A (ja) * 2009-01-30 2010-08-12 Nvidia Corp 区分されたドメインの重要度サンプリングのシステム、方法、及びコンピュータプログラム製品
JP2011522217A (ja) * 2008-03-28 2011-07-28 ロッキード・マーチン・コーポレーション 部品の姿勢を表す点データに３次元モデルを位置合わせするシステム、プログラム製品、および関連する方法
CN102959354A (zh) * 2009-12-22 2013-03-06 韦尔豪泽Nr公司 用于利用LiDAR数据来分析树冠层的方法和装置
WO2020209135A1 (ja) * 2019-04-09 2020-10-15 株式会社Ihi 形状導出装置および荷揚げ装置
JP2020173132A (ja) * 2019-04-09 2020-10-22 株式会社Ihi 形状導出装置
JP2020172351A (ja) * 2019-04-09 2020-10-22 株式会社Ihi 荷揚げ装置
CN113835101A (zh) * 2021-09-30 2021-12-24 同济大学 一种基于雷达点云的车辆定位方法、装置及存储介质

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8686992B1 (en) * 2009-03-30 2014-04-01 Google Inc. Methods and systems for 3D shape matching and retrieval
TWI409717B (zh) * 2009-06-22 2013-09-21 Chunghwa Picture Tubes Ltd 適用於電腦產品與影像顯示裝置的影像轉換方法
DE102010017630B4 (de) 2010-06-29 2016-06-02 Leica Microsystems Cms Gmbh Verfahren und Einrichtung zur lichtmikroskopischen Abbildung einer Probenstruktur
KR102123196B1 (ko) * 2011-09-13 2020-06-16 오에스아이 옵토일렉트로닉스 개선된 레이저 레인지파인더 센서
GB2505936A (en) 2012-09-17 2014-03-19 Materialise Dental Nv 3D modelling of scanned body
CN103335604B (zh) * 2013-07-05 2015-10-28 温州大学 一种工作状态下风轮叶片全场三维变形在线监测方法
US9529454B1 (en) 2015-06-19 2016-12-27 Microsoft Technology Licensing, Llc Three-dimensional user input
AU2016341528A1 (en) 2015-10-22 2018-05-10 Grey Orange Pte. Ltd. Method of managing resources in a warehouse
WO2017168899A1 (ja) * 2016-03-30 2017-10-05 ソニー株式会社 情報処理方法および情報処理装置
JP6976080B2 (ja) * 2017-05-22 2021-12-01 三菱パワー株式会社 状態分析装置、状態分析方法、およびプログラム
JP7051366B2 (ja) * 2017-10-18 2022-04-11 株式会社東芝 情報処理装置、学習済モデル、情報処理方法、およびプログラム
CN108088407B (zh) * 2017-12-15 2020-11-10 成都光明光电股份有限公司 光学玻璃制品形貌偏差校正方法及系统
JP7159033B2 (ja) * 2018-12-21 2022-10-24 株式会社日立製作所 ３次元位置・姿勢認識装置及び方法
CN111060006A (zh) * 2019-04-15 2020-04-24 深圳市易尚展示股份有限公司 一种基于三维模型的视点规划方法
CN116125490B (zh) * 2023-02-03 2023-07-04 中国科学院精密测量科学与技术创新研究院 面向滑坡体形变场时序监测的tls多目标优化选址方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS63186308A (ja) * 1987-01-28 1988-08-01 Hitachi Ltd 移動体の誘導方法、および装置
JPH0618221A (ja) * 1991-03-11 1994-01-25 Agency Of Ind Science & Technol 多視点距離データの統合方法
JPH09229648A (ja) * 1996-02-21 1997-09-05 Canon Inc 画像情報入出力装置及び画像情報入出力方法
JP2000113193A (ja) * 1998-10-08 2000-04-21 Minolta Co Ltd 多視点３次元データの合成方法および記録媒体
JP2001022939A (ja) * 1999-07-12 2001-01-26 Nippon Telegr & Teleph Corp <Ntt> ３次元情報抽出方法、装置、および３次元情報抽出プログラムを記録した記録媒体
JP2001236522A (ja) * 1999-12-17 2001-08-31 Canon Inc 画像処理装置
JP2003296755A (ja) * 2001-11-27 2003-10-17 Samsung Electronics Co Ltd 深さイメージに基づく３次元物体を表現するためのノード構造
JP2004521423A (ja) * 2001-03-12 2004-07-15 コーニンクレッカ　フィリップス　エレクトロニクス　エヌ　ヴィ ８分木を用いた多数の画像からの３次元表現の生成
JP2005037379A (ja) * 2003-06-30 2005-02-10 Sanyo Electric Co Ltd 三次元モデリング方法と装置

Family Cites Families (27)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
FR2625345A1 (fr) 1987-12-24 1989-06-30 Thomson Cgr Procede de visualisation en trois dimensions d'objets codes numeriquement sous forme arborescente et dispositif de mise en oeuvre
JPH03170345A (ja) 1989-11-28 1991-07-23 Asahi Glass Co Ltd サーマルヘッドの抵抗体オーバーコートガラス組成物
JP3170345B2 (ja) 1992-05-13 2001-05-28 日本電信電話株式会社 ３次元情報抽出方法
JPH06223201A (ja) 1993-01-22 1994-08-12 Matsushita Electric Ind Co Ltd 並列画像生成装置
US5724493A (en) 1994-12-13 1998-03-03 Nippon Telegraph & Telephone Corporation Method and apparatus for extracting 3D information of feature points
JPH0981788A (ja) 1995-09-12 1997-03-28 Toshiba Corp 環境モデル入力装置
US5689629A (en) 1995-12-12 1997-11-18 The Regents Of The University Of California Iterative optimizing quantization method for reconstructing three-dimensional images from a limited number of views
JPH09231370A (ja) * 1996-02-21 1997-09-05 Canon Inc 画像情報入力装置
US7486811B2 (en) * 1996-09-16 2009-02-03 The Research Foundation Of State University Of New York System and method for performing a three-dimensional virtual examination of objects, such as internal organs
US6064942A (en) 1997-05-30 2000-05-16 Rockwell Collins, Inc. Enhanced precision forward observation system and method
JPH1196374A (ja) 1997-07-23 1999-04-09 Sanyo Electric Co Ltd ３次元モデリング装置、３次元モデリング方法および３次元モデリングプログラムを記録した媒体
JP3813343B2 (ja) 1997-09-09 2006-08-23 三洋電機株式会社 ３次元モデリング装置
US6026189A (en) 1997-11-13 2000-02-15 National Research Council Of Canada Method of recognizing objects within two-dimensional and three-dimensional images
US6476803B1 (en) * 2000-01-06 2002-11-05 Microsoft Corporation Object modeling system and process employing noise elimination and robust surface extraction techniques
US6914601B2 (en) 2001-06-12 2005-07-05 Minolta Co., Ltd. Method, apparatus, and computer program for generating three-dimensional shape data or volume data
JP2003015739A (ja) 2001-07-02 2003-01-17 Yaskawa Electric Corp 外環境地図、並びに自己位置同定装置および誘導制御装置
JP2003065736A (ja) 2001-08-24 2003-03-05 Sanyo Electric Co Ltd ３次元モデリング装置
CN1218282C (zh) * 2001-11-27 2005-09-07 三星电子株式会社 用深度图像表示三维物体的节点结构
EP1510896B1 (en) 2002-05-31 2016-03-09 Fujitsu Limited Remotely-operated robot, and robot self position identifying method
KR100639139B1 (ko) * 2002-06-28 2006-10-30 후지쯔 가부시끼가이샤 3차원 이미지 비교 프로그램을 기록한 컴퓨터로 판독 가능한 기록 매체, 3차원 이미지 비교 방법 및 3차원 이미지 비교 장치
US7317456B1 (en) 2002-12-02 2008-01-08 Ngrain (Canada) Corporation Method and apparatus for transforming point cloud data to volumetric data
US7843512B2 (en) 2004-03-31 2010-11-30 Honeywell International Inc. Identifying key video frames
US20060017720A1 (en) 2004-07-15 2006-01-26 Li You F System and method for 3D measurement and surface reconstruction
WO2006046552A1 (ja) 2004-10-27 2006-05-04 Toyama Chemical Co., Ltd. 新規な含窒素複素環化合物およびその塩
US7653235B2 (en) 2005-10-27 2010-01-26 Honeywell International Inc. Surface anomaly detection system and method
CN101331380B (zh) * 2005-12-16 2011-08-03 株式会社Ihi 三维形状数据的存储/显示方法和装置以及三维形状的计测方法和装置
DE112006003363B4 (de) * 2005-12-16 2016-05-04 Ihi Corporation Verfahren und Vorrichtung zur Identifizierung der Eigenposition, und Verfahren und Vorrichtung zur Messung einer dreidimensionalen Gestalt

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS63186308A (ja) * 1987-01-28 1988-08-01 Hitachi Ltd 移動体の誘導方法、および装置
JPH0618221A (ja) * 1991-03-11 1994-01-25 Agency Of Ind Science & Technol 多視点距離データの統合方法
JPH09229648A (ja) * 1996-02-21 1997-09-05 Canon Inc 画像情報入出力装置及び画像情報入出力方法
JP2000113193A (ja) * 1998-10-08 2000-04-21 Minolta Co Ltd 多視点３次元データの合成方法および記録媒体
JP2001022939A (ja) * 1999-07-12 2001-01-26 Nippon Telegr & Teleph Corp <Ntt> ３次元情報抽出方法、装置、および３次元情報抽出プログラムを記録した記録媒体
JP2001236522A (ja) * 1999-12-17 2001-08-31 Canon Inc 画像処理装置
JP2004521423A (ja) * 2001-03-12 2004-07-15 コーニンクレッカ　フィリップス　エレクトロニクス　エヌ　ヴィ ８分木を用いた多数の画像からの３次元表現の生成
JP2003296755A (ja) * 2001-11-27 2003-10-17 Samsung Electronics Co Ltd 深さイメージに基づく３次元物体を表現するためのノード構造
JP2005037379A (ja) * 2003-06-30 2005-02-10 Sanyo Electric Co Ltd 三次元モデリング方法と装置

Cited By (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4650750B2 (ja) * 2005-12-16 2011-03-16 株式会社Ｉｈｉ 三次元形状データの記憶・表示方法と装置および三次元形状の計測方法と装置
JPWO2007069721A1 (ja) * 2005-12-16 2009-05-28 株式会社Ihi 三次元形状データの記憶・表示方法と装置および三次元形状の計測方法と装置
JPWO2007069726A1 (ja) * 2005-12-16 2009-05-28 株式会社Ihi 自己位置同定方法と装置および三次元形状の計測方法と装置
JP4650752B2 (ja) * 2005-12-16 2011-03-16 株式会社Ｉｈｉ 自己位置同定方法と装置および三次元形状の計測方法と装置
JP2008309671A (ja) * 2007-06-15 2008-12-25 Ihi Corp 物体認識方法および装置
JP2009128097A (ja) * 2007-11-21 2009-06-11 Ihi Corp バリまたは欠損認識方法と装置およびバリ取り方法と装置
JP2011522217A (ja) * 2008-03-28 2011-07-28 ロッキード・マーチン・コーポレーション 部品の姿勢を表す点データに３次元モデルを位置合わせするシステム、プログラム製品、および関連する方法
JP2010176657A (ja) * 2009-01-30 2010-08-12 Nvidia Corp 区分されたドメインの重要度サンプリングのシステム、方法、及びコンピュータプログラム製品
CN102959354A (zh) * 2009-12-22 2013-03-06 韦尔豪泽Nr公司 用于利用LiDAR数据来分析树冠层的方法和装置
WO2020209135A1 (ja) * 2019-04-09 2020-10-15 株式会社Ihi 形状導出装置および荷揚げ装置
JP2020173132A (ja) * 2019-04-09 2020-10-22 株式会社Ihi 形状導出装置
JP2020172351A (ja) * 2019-04-09 2020-10-22 株式会社Ihi 荷揚げ装置
JP7285121B2 (ja) 2019-04-09 2023-06-01 株式会社Ihi 荷揚げ装置
JP7285122B2 (ja) 2019-04-09 2023-06-01 株式会社Ihi 形状導出装置
CN113835101A (zh) * 2021-09-30 2021-12-24 同济大学 一种基于雷达点云的车辆定位方法、装置及存储介质

Also Published As

Publication number Publication date
CN101331381B (zh) 2011-08-24
CN101331381A (zh) 2008-12-24
DE112006003380T5 (de) 2008-10-16
US20090202155A1 (en) 2009-08-13
US8116558B2 (en) 2012-02-14
JP4650751B2 (ja) 2011-03-16
JPWO2007069724A1 (ja) 2009-05-28

Similar Documents

Publication Publication Date Title
WO2007069724A1 (ja) 三次元形状データの位置合わせ方法と装置
JP4650752B2 (ja) 自己位置同定方法と装置および三次元形状の計測方法と装置
JP4650750B2 (ja) 三次元形状データの記憶・表示方法と装置および三次元形状の計測方法と装置
JP5380792B2 (ja) 物体認識方法および装置
EP3280977B1 (en) Method and device for real-time mapping and localization
Folkesson et al. Vision SLAM in the measurement subspace
JP5759161B2 (ja) 物体認識装置、物体認識方法、学習装置、学習方法、プログラム、および情報処理システム
JP5627325B2 (ja) 位置姿勢計測装置、位置姿勢計測方法、およびプログラム
CN112219087A (zh) 位姿预测方法、地图构建方法、可移动平台及存储介质
CN113052908B (zh) 一种基于多传感器数据融合的移动机器人位姿估计算法
CN112444246B (zh) 高精度的数字孪生场景中的激光融合定位方法
Pitzer et al. Automatic reconstruction of textured 3D models
Li et al. Aerial-triangulation aided boresight calibration for a low-cost UAV-LiDAR system
KR102130687B1 (ko) 다중 센서 플랫폼 간 정보 융합을 위한 시스템
CN115930948A (zh) 一种果园机器人融合定位方法
Walvoord et al. Geoaccurate three-dimensional reconstruction via image-based geometry
Panahandeh et al. IMU-camera data fusion: Horizontal plane observation with explicit outlier rejection
Hu et al. Efficient Visual-Inertial navigation with point-plane map
Shokrzadeh Simultaneous Localization and Mapping for Semi-Sparse Point Clouds
Fossel Improving Light Detection and Ranging Based Simultaneous Localization and Mapping with Advanced Map Representations
CN116524014A (zh) 一种在线标定外参的方法、装置
Simakov et al. Cubesat onboard algorithm for space debris motion determination by processing stereo images
Voorhies Efficient slam for scanning lidar sensors using combined plane and point features
Mannel et al. Generating semi global elevation maps from planetary descent imagery using bundle adjustment
Stähle Lehrstuhl für Technische Elektronik

Legal Events

Date Code Title Description
WWE Wipo information: entry into national phase

Ref document number: 200680047472.6

Country of ref document: CN

121 Ep: the epo has been informed by wipo that ep was designated in this application
WWE Wipo information: entry into national phase

Ref document number: 2007550241

Country of ref document: JP

WWE Wipo information: entry into national phase

Ref document number: 12096851

Country of ref document: US

RET De translation (de og part 6b)

Ref document number: 112006003380

Country of ref document: DE

Date of ref document: 20081016

Kind code of ref document: P

WWE Wipo information: entry into national phase

Ref document number: 112006003380

Country of ref document: DE

122 Ep: pct application non-entry in european phase

Ref document number: 06834790

Country of ref document: EP

Kind code of ref document: A1