JP5380792B2 - Object recognition method and apparatus - Google Patents

Description

  The present invention relates to a method and an apparatus for recognizing the shape and the position and orientation of an object to be measured obtained from a measurement point.

Conventionally, the two-dimensional features of the object are extracted from the first to n images by photographing with a plurality of vision sensors, and the three-dimensional information of the two-dimensional features of the object is added by the association, and the three-dimensional position and orientation of the object are determined. It is estimated (see, for example, Patent Document 1).
However, since the reliability evaluation value of the position / orientation depends on the edge extraction accuracy of the two-dimensional image processing, high accuracy cannot be obtained. In addition, it is necessary to set the feature amount used for reliability evaluation in advance.

Also, from the distance data obtained by the distance sensor, the rough position of the characteristic part of the object (for example, a cylinder or an ellipse) is obtained, and the vision sensor is directly opposed to the object according to this, and then the characteristic is obtained by two-dimensional processing. Some of them detect parts and determine the precise position and orientation of an object.
However, in this case, the position / orientation of the object cannot be specified only by the distance data, and costs and time are required because it is a two-stage structure. In addition, there are great restrictions on the shape of the detectable object. Furthermore, the accuracy is insufficient for application to shape inspection.

In addition, there is a monocular vision sensor that estimates the position / orientation of an object from the current appearance by sufficiently teaching the appearance of the object in advance.
However, it is difficult to cope with a case where an object is partially hidden. Moreover, since a vision sensor is used, it depends on illumination conditions. Furthermore, it is necessary to teach a large amount of images in advance. In addition, the accuracy is insufficient for application to shape inspection.
Japanese Patent No. 3525896

  When recognizing the position / orientation of a workpiece (object to be measured) using a vision sensor, it is easily affected by changes in illumination conditions such as background information of the workpiece and external light. Therefore, it is necessary to prepare a measurement environment by unifying the background and stabilizing the lighting conditions, but it is not always possible to prepare these environments. On the other hand, a laser scanner or the like that projects distance data by projecting laser light onto a workpiece is not affected by the background and is robust against changes in lighting conditions. Can be measured. However, a disadvantage of using a laser scanner is that the measurement result may be deficient or error when the incident angle of the laser beam on the workpiece surface is shallow or when the laser beam is directly reflected. . In addition, laser scanners can be broadly divided into triangulation methods and TOF (Time of Flight) methods, but triangulation laser scanners have a narrow range of measurable distances. There is a problem that only a part can be measured. In addition, the TOF laser scanner generally has a problem that the distance measurement accuracy is low although the measurement range is wider than the triangulation method.

  When a distance sensor such as a three-dimensional laser radar is used, the measured points on the three-dimensional shape to be measured are point groups that are discrete in the horizontal direction and the vertical direction. For example, when the distance from the measurement point is 50 m, for example, the distance between the point groups reaches about 315 mm in the horizontal direction and about 525 mm in the vertical direction.

Also, when measuring a stationary three-dimensional shape from a plurality of measurement positions, the position of the measurement point is usually different for each measurement position in a distance sensor such as a three-dimensional laser radar.
Further, such a distance sensor generally has an error of about 20 cm, for example, in the measurement distance.
Therefore, when using a distance sensor such as a three-dimensional laser radar, there are the following constraints A to C.
Condition A: The measurement data has a small number of points (for example, 1 frame = 166 × 50 points)
Condition B: The measurement data includes an error (for example, the measurement distance is about 20 cm)
Condition C: The measurement data does not always measure the same measurement point.
That is, the obtained distance data is a group of points that are discrete in the horizontal direction and the vertical direction, and since the position is different for each measurement, there is no corresponding point, and the measurement distance includes a relatively large error.

When such distance data is processed by the “ICP (Iterative Closest Point) algorithm”, there are the following problems.
The ICP algorithm is one of the alignment means when the corresponding points are not known. The nearest point of the subsequent measurement data with respect to the previous measurement data is obtained, and the sum of the distances is minimized while performing rotation / translation. The solution is obtained so that the state becomes the matching state.
(1) Accumulation of error The ICP algorithm is a means for superimposing two distance data, and 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. Will accumulate.
(2) Large amount of calculation Since the ICP algorithm is iterative calculation, the amount of calculation becomes enormous. That is, the ICP algorithm needs to search for model data corresponding to each data point of the measurement data, so that the amount of calculation increases as the number of model data points and the number of measurement data points increase. Specifically, when the number of model data is M and the number of measurement data is N, for example, the calculation order at the time of full search is O (M × N).
(3) Cannot handle the case where the number of measurement points is small Since the ICP algorithm is intended for dense distance data, it is a discrete point cloud, and if it is spatially sparse, it converges to an incorrect result.

Therefore, it is necessary to satisfy the following requirements for position identification using such a distance sensor.
(1) Data structure for ambient environment with good memory efficiency The method of storing all the measurement data obtained sequentially requires an infinite amount of memory. Therefore, a data structure for efficiently storing the result of measuring the surrounding environment is necessary.
(2) Insufficient number of points in measurement data and stability against errors Even when the number of points in measurement data is small and includes errors, position identification accuracy should not be reduced as much as possible.
(3) Efficient calculation for position identification Self-position identification is performed by comparing measured information with environmental information obtained by measuring the surrounding environment, but this comparison process requires a lot of calculations.

  The present invention has been developed to solve the above-described problems. In other words, object recognition that allows measurement of workpieces with unknown positions and orientations from multiple viewpoints using a distance sensor such as a laser scanner, and filling in the missing parts of the measurement results with data from different viewpoints while still integrating with high accuracy. Methods and apparatus are provided. The shortcoming of the triangulation method, which has a narrow measurement range, can be solved by combining data from multiple viewpoints. Further, the disadvantage of the TOF method that the distance measurement is low accuracy can be solved by integrating a plurality of measurements by a statistical method.

A first feature of the present invention is an object recognition method for recognizing the position and orientation of an object,
(1) a data input step of inputting coordinate values on a three-dimensional shape obtained by measurement from a new measurement position to a computer;
(2) A model construction step of constructing an environment model that divides the spatial region in which the three-dimensional shape exists into a plurality of voxels made of rectangular parallelepipeds whose boundary surfaces are orthogonal to each other, and stores each voxel position;
(3) performing a first matching step of setting and storing a representative point and its error distribution inside the voxel corresponding to the coordinate value;
(4-1) When there is no data of the three-dimensional shape obtained by measuring from the previous measurement position, the new measurement position is identified as the self-position,
(4-2) When there is data of the three-dimensional shape obtained by measuring from the previous measurement position,
(4-2-1) With respect to the environmental model for the previous measurement position, (A) rotate and translate the new measurement data and error distribution, or (B) rotate and translate the environment model for the new measurement position, A precision alignment step for aligning the evaluation values regarding the distance between adjacent error distributions to a minimum;
(4-2-2) performing a self-position identification step of identifying the self-position from the rotation amount and the translation amount in the precision alignment step;
(5) Further, an integration step for integrating a plurality of measurement data and a plurality of error distributions measured from a plurality of measurement positions;
(6) with respect to the measurement data and the error distribution obtained by integrating the rotation and to translate the measurement object represented by the model data, constituting the model data and meter measurement data obtained by pre-SL Integration And performing a second matching step for positioning so that an evaluation value related to the distance to the element is minimized.
Examples of model data expression methods include polyhedrons, free-form surfaces, voxels, representative points, and a set of error distributions.
The evaluation value related to the distance may be a sum of the distances, an average value of the distances, a sum of squares of the distances, a maximum value of the distances, or another appropriate evaluation value.
“Voxel corresponding to a coordinate value” means a voxel in which a point specified by the coordinate value exists.
The “representative point” is the center of error distribution, maximum likelihood estimated value, or the like obtained as a result of statistical processing.
A method for setting the “representative point and its error distribution” and a method for obtaining the “distance between error distributions” will be described later.

In the second matching step, the object may be inspected by determining the difference between the measurement data and the model data using the distance between the measurement data and the elements constituting the model data.
Further, in the first matching step, in addition to the representative point and its error distribution inside the voxel, a probability value representing the existence probability of the object may be set and stored in the voxel.

Before the precision alignment step, for the environmental model for the previous measurement position,
(1) Rotate and translate the new measurement data and error distribution so that the sum of the distance between the measurement data and the error distribution and the voxels having the representative points that are close to each other is minimized, or (2) New measurement position Rotate and translate the environment model for, so that the sum of the distances between voxels with representative points is minimized.
It is good also as having the rough alignment step which aligns.

Before the precision alignment step, for the environmental model for the previous measurement position,
(1) Rotate and translate new measurement data and error distribution so that the sum of the probability values of voxels having representative points close to the measurement data and error distribution is maximized, or (2) for a new measurement position Rotate and translate the environment model so that the sum of the difference in probability values of neighboring voxels is minimized.
It is good also as having the rough alignment step which aligns.

After the data input step,
(1) The current measurement position is estimated from a change in the past measurement position, or (2) the current measurement position is acquired by a sensor capable of acquiring, or (3) the reflection intensity value as well as the distance value of the measurement data Use
It is good also as having the search range limitation step which limits the range to collate.

  In the self-position identification step, a 6-degree-of-freedom position of a new measurement position may be identified from the position and orientation at the previous measurement position.

In the fine matching step, the case where the error distributions intersect may be set as the same measurement point, and the distance between the error distributions may be calculated by multiplying the distance value in that case by the weight obtained from the degree of coincidence of the distributions.
A method for obtaining the weight from the degree of coincidence of the distribution will be described later.

In the model building step, when a maximum voxel is set to a size corresponding to the minimum necessary resolution and there are a plurality of measurement points in a single voxel, a single voxel is single. The voxel may be further divided into a plurality of voxels hierarchically so that only the measured point exists.
The “size corresponding to the necessary minimum resolution” is a size that can include the measurement object.

A model update step of updating the environmental model after the self-position identification step, and in the model update step, search for a voxel corresponding to a coordinate value of a newly input measurement point;
Assuming that no object exists between the origin and the point to be measured, the representative point and the error distribution in the voxel located between them may be reset or deleted.

A model update step of updating the environmental model after the self-position identification step, and in the model update step, search for a voxel corresponding to a coordinate value of a newly input measurement point;
When there is no representative point in the voxel, the coordinate value and the error distribution may be set as the coordinate value and the error distribution of the representative point.

A model update step of updating the environmental model after the self-position identification step, and in the model update step, search for a voxel corresponding to a coordinate value of a newly input measurement point;
When there is a representative point already set in the voxel, the newly acquired error distribution is compared with the already set error distribution in the voxel,
If the error distributions overlap each other, reset a new error distribution and a new representative point from both error distributions,
When the error distributions do not overlap with each other, the voxel may be further divided into a plurality of voxels hierarchically so that only a single representative point exists within a single voxel.
A method of resetting a new error distribution and a new representative point from the two error distributions will be described later.

In the self-position identification step, self-position error distribution is identified together with self-position identification, and the self-position and error distribution are corrected by a Kalman filter from the current self-position and error distribution and the identified self-position and error distribution. , Or as good.
A method for identifying the self-position error distribution will be described later.

After the previous SL self-localization step, the have a model updating step for updating the environment model, in 該Mo del update step, compared with the error distribution of the newly acquired error distribution within the voxel already set When the error distributions overlap each other, as a result of resetting a new error distribution and a new representative point from both error distributions, when the new representative point moves into another voxel,
If there is no representative point in the other voxel, the new error distribution and the new representative point are set inside the other voxel,
When there is a representative point already set in the other voxel, the new error distribution is compared with the already set error distribution in the other voxel, and (A) when the error distributions overlap each other, From both error distributions, or from both error distributions and the coordinate values of the representative points already set in the voxel and the newly inputted measurement point, a new error distribution and a new representative point are reset, (B) When the error distributions do not overlap with each other, the voxel may be further divided into a plurality of voxels hierarchically so that only a single representative point exists within a single voxel.

A model update step of updating the environmental model after the self-location identification step;
In the model updating step, a new representative point and error distribution are obtained by a Kalman filter from the newly inputted coordinate value of the measured point and its error distribution, and the representative point in the already set voxel and its error distribution. You can reset it.

In the precise alignment step, instead of performing alignment so that the evaluation value related to the distance between the adjacent error distributions is minimized, the evaluation value related to the degree of coincidence determined by the maximum likelihood estimation value based on the adjacent error distributions is obtained. To maximize the environmental model for the previous measurement position,
(1) The new measurement data and error distribution may be rotated and translated, or (2) the environmental model for the new measurement position may be rotated and translated for alignment.

  The formula for calculating the evaluation value related to the degree of coincidence is expressed by the formula [Equation 1].

In the equation [Equation 1], it is assumed that the measurement point j is associated with the representative point i on the environmental model, and the probability that the measurement data corresponding to the measurement point j is obtained is EM (i, j). ω (j) is 1 when there is a representative point associated with the measurement point j in the environmental model, and 0 otherwise.
“The measurement point j is associated with the representative point i on the environmental model” means a case where a representative point already exists in the voxel corresponding to the measurement point j.

In the integration step, search for the voxel corresponding to the coordinate value of the newly input measurement point,
When there is a representative point already set in the voxel, the newly acquired error distribution is compared with the already set error distribution in the voxel,
(1) When error distributions overlap each other, a new error distribution and a new representative point are reset from both error distributions,
(2) When the error distributions do not overlap each other, it is preferable to further divide the voxel into hierarchical voxels so that only a single representative point exists within a single voxel. .

In the integration step, as a result of resetting a new error distribution and a new representative point from both error distributions, when a new representative point moves into another voxel,
(1) If there is no representative point in the other voxel, the new error distribution and the new representative point are set inside the other voxel,
(2) When there is a representative point already set in the other voxel, the new error distribution is compared with the error distribution in the already set other voxel, and (A) the error distributions overlap each other. In this case, a new error distribution and a new representative point are reset from both error distributions, or from the coordinate values of the representative points already set in both error distributions and voxels and the newly input measurement point, (B) When the error distributions do not overlap with each other, it is preferable to further divide the voxel into a plurality of hierarchical voxels so that only a single representative point exists in a single voxel. .

  The evaluation value is preferably a sum of distances between the measurement data and each plane close to the measurement data.

According to the method and apparatus of the present invention, a spatial region in which a three-dimensional shape exists is divided into a plurality of voxels and each voxel position is stored, so even if the measurement object is large, the amount of data can be reduced. It can be suppressed to a small data size proportional to the number of voxels.
Further, since the representative point and its error distribution are set and stored in the voxel corresponding to the coordinate value, information exceeding the resolution of the voxel can be expressed.
Therefore, measurement data of a plurality of viewpoints can be integrated into a fixed size by the data structure of the present invention.

Moreover, even if the error distribution is wider than the voxel to which the representative point belongs by setting and saving a probability value representing the existence probability of the object inside the voxel, the representative point indicates whether or not the object exists in each voxel. Since it is possible to easily determine only the probability value of the relevant voxel without finding the voxel to which it belongs and recalculating it from its error distribution, the search time can be reduced.
In addition, 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.

In addition, in the rough alignment step, new measurement data and error distribution are rotated and translated with respect to the environmental model for the previous measurement position, and the evaluation value related to the distance between the measurement data and the error distribution and the distance between the voxels having representative points close to each other. (E.g., the sum of the distances) is minimized, or the environment model for the new measurement position is rotated and translated, and the evaluation value (e.g., the sum of the distances) regarding the distance between voxels having representative points is minimized. So that
Alternatively, rotate and translate the new measurement data and error distribution so that the evaluation value related to the probability value of the voxel having a representative point close to the measurement data and error distribution (for example, the sum of the probability values) is maximized, or Rotate and translate the environment model for the new measurement position and align it so that the evaluation value (for example, the sum of the difference of the probability values) of adjacent voxels has the minimum value. Voxels having representative points can be aligned in a short time while preventing accumulation.

  In addition, the environmental model for the new measurement position is rotated and translated with respect to the previous environment model for the measurement position, and the evaluation value (for example, the sum of the difference of the probability values) regarding the difference in probability values of adjacent voxels is minimized. As described above, in the case of positioning, since the positioning is performed in consideration of information in which no object exists, the accuracy can be improved.

Next, in the precision alignment step, 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, and the error distribution between adjacent error distributions is changed. Since the alignment is performed so that the evaluation value related to the distance (for example, the sum of the distances) is minimized, accurate alignment between the error distributions can be performed in a short time.
Therefore, the integration processing of data from a plurality of viewpoints according to the present invention enables highly accurate shape acquisition while preventing error accumulation.

In addition, since the data structure of the present invention has an expanded voxel structure, the data size can be made smaller than that of the point group. That is, since the data structure proposed in the present invention stores one representative point in the voxel, the calculation order for searching for the model point corresponding to the measurement point can be set to 1, so that the total calculation order is O (N) can be reduced.
Accordingly, the amount of data to be searched for when positioning the surrounding environment and measurement data (ICP algorithm) is reduced, so that the calculation can be made more efficient.

  Furthermore, the conventional ICP algorithm outputs incorrect results for sparse data, but the environment model of the present invention has a representative point and error distribution in the voxel, so alignment corresponding to sparse data is possible. It is.

  Further, by correcting the self position and the error distribution by the Kalman filter from the current self position and the error distribution and the identified self position and the error distribution, the self position accuracy can be further improved.

  Therefore, according to the method and apparatus of the present invention, it has a function of correcting data including errors into accurate information, and converges with high accuracy for long-time measurement. In addition, the position identification process is a process for updating the representative point corresponding to each voxel and its error distribution at the measurement point, so the calculation amount is small, and the calculation is closed in the voxel without affecting the surrounding voxels. Yes. Therefore, high-speed processing is possible. Moreover, the measurement data can be sequentially integrated into the voxels, and the memory size of the surrounding environment information obtained as a result does not exceed the fixed size.

Further, in the model update step, a new representative point and error distribution are obtained by a Kalman filter from the newly inputted coordinate value of the measured point and its error distribution, and the representative point and its error distribution in the already set voxel. Since it is acquired and reset, a shape closer to the true value can be obtained.
In particular, by repeating the model update step using the Kalman filter, a highly accurate shape converged to a true value can be obtained by the effect of the Kalman filter even for data including errors.

  Further, in the fine alignment step, maximum likelihood estimation based on the adjacent error distributions is performed instead of performing alignment so that an evaluation value (for example, the sum of the distances) between the adjacent error distributions is minimized. Rotate and translate new measurement data and error distribution with respect to the environmental model for the previous measurement position so that the evaluation value (for example, the total power of the coincidence) determined by the value is maximized, or By aligning the environment model with respect to the new measurement position by rotating and translating, it is possible to perform the alignment in consideration of errors in both the environment model and the measurement data.

  Further, in the outputting step, when outputting the position of the representative point of the voxel to the output device as a measured value of a three-dimensional shape, an index indicating the reliability or accuracy of the measured value is used as an error distribution inside the voxel. Therefore, when using the measuring device, the user can select a measurement value with low reliability according to the contents of the application.

  In the output step, when the position of the representative point of the voxel is output to the output device as a three-dimensional shape measurement value, the error distribution inside the voxel is larger than a predetermined reference value. Since the measurement value of the voxel is not output to the output device on the assumption that the reliability or accuracy of the measurement value is lower than a predetermined reference, the measurement device is highly reliable in the first place. Since only values can be handled, the amount of data to be handled is reduced and reliability is improved.

  Further, in the data input step, using a distance sensor, coordinate values on a three-dimensional shape are sequentially acquired while moving the origin as distance data having an arbitrary measurement position as the origin, and distance data is obtained particularly from different directions. This makes it possible to integrate distance data of error distributions having different distribution shapes, and improve accuracy.

  In addition, after the matching step, there is a model update step for updating the environmental model, and in the model update step, a voxel corresponding to the coordinate value of the newly input measurement point is searched, and the inside of the voxel Only in the case where at least one of the representative point and the error distribution is newly set or reset, or when the voxel is further divided and divided into a plurality of voxels hierarchically, in the output step, the voxel By outputting at least one of the representative point position, error distribution, and voxel position to the output device as a three-dimensional shape measurement value, the inside of the voxel affected by the measured point newly obtained from the distance sensor A value such as a representative point is output. For this reason, the user can use it as if the original measurement value obtained by the distance sensor was replaced with a measurement value with higher accuracy while assuming the same operation as the conventional one. In this way, more accurate three-dimensional shape measurement is possible.

In the output step, the position of the representative point of the voxel in the environmental model within the range in which the distance sensor can measure the position from the position of the distance sensor is output to the output device as a three-dimensional shape measurement value. Even when the resolution of the measurement value of the sensor is rough, it can be used as if it were a distance sensor with high accuracy and high resolution. In this way, more accurate three-dimensional shape measurement is possible.
Other objects and advantageous features of the present invention will become apparent from the following description with reference to the accompanying drawings.

  Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. In each figure, common portions are denoted by the same reference numerals, and redundant description is omitted.

FIG. 1 is an overall configuration diagram of an object recognition apparatus according to the present invention. As shown in FIG. 1, an object recognition apparatus 40 of the present invention includes a 3D sensor 41 that measures a distance to a measurement target (work), an object recognition processing unit 42 that executes object recognition processing, and a tertiary of the measurement target. A model database unit 43 that stores data representing the original shape (for example, CAD data) is provided. The position and orientation of the object recognized by the object recognition processing unit 42 are output to the robot control unit 45, a control signal is output from the robot control unit 45 to the robot 46, and the measurement object is gripped by moving the hand 47. can do.
In the present invention, the self-position means a measurement position, for example, a position and orientation of 6 degrees of freedom in the outside world of the 3D sensor 41.
Although not shown, in the present invention, an odometer, a camera, a GPS, and an attitude sensor other than the distance sensor (3D sensor) may be optionally used as necessary. Hereinafter, an example using a distance sensor will be described.

  FIG. 2 is a configuration diagram of a three-dimensional laser radar as an example of a distance sensor. As shown in this figure, the three-dimensional laser radar 10 includes a radar head 12 and a controller 20. The pulsed laser beam 1 oscillated from the laser diode 13 is shaped into parallel light 2 by the light projecting lens 14, scanned in a two-dimensional direction by the mirrors 18a and 18b and the polygon mirror 15 that rotates and oscillates, and is applied to the measurement object. Irradiated. The pulsed laser light 3 reflected from the measurement object is collected by the light receiving lens 16 via 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 rotation angle θ of the polygon mirror, and the swing angle φ as polar coordinate data (r, θ, φ).
r is a distance with the measurement position (radar head installation position) as the origin, and is obtained by the equation r = c × t / 2. Here, c is the speed of light.
The determination processing unit 23 performs detection processing by converting polar coordinate data from the signal processing board into three-dimensional space data (x, y, z) with the radar head installation position as an origin. In this figure, reference numeral 24 denotes 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 about 20 cm, for example.
Further, when displaying the measurement data as a distance image having a distance value in the depth direction for each pixel, if the number of measurement points in one frame is 166 points in the horizontal direction and 50 points in the scan direction, 166 × 50 per frame. = 8300 points are displayed. In this case, the frame rate is, for example, about 2 frames / second.

The points to be measured on the three-dimensional shape measured by the three-dimensional laser radar 10 are a group of discrete points, Δθ × r in the horizontal direction and Δφ × r in the vertical direction. For example, when Δθ = 60/166 × π / 180 = 6.3 × 10 ⁻³ radians, Δφ = 30/50 × π / 180 = 10.5 × 10 ⁻³ radians, r = 50 m, the closest case However, the distance between the measurement points is about 315 mm in the horizontal direction and about 525 mm in the vertical direction.

  In the present invention, for example, the above-described three-dimensional laser radar 10 is used as the distance sensor. However, the distance sensor is not limited to this, and a distance sensor using parallax or other known distance sensors can be used.

3 and 4 are diagrams illustrating the relationship between polar coordinate data measured by the distance sensor and errors.
As shown in FIG. 3, 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 normally exists in the measurement result by the distance sensor.
This error distribution is normalized in the directions of measurement axes r, θ, and φ when the existence probability of the error distribution at r _s , θ _s , and φ _s is P (r _s , θ _s , φ _s ). For example, it can be expressed by the equation [Equation 2]. Here, r, θ, and φ are measured values from the sensor, σ _r , σ _θ , and σ _φ are standard deviations, and A is a normalization constant.
As shown in FIG. 4, the error distribution is a distribution that is normally included in a truncated cone shape (left figure) that is long in the r direction, but the difference between a and b is small in the distance. Therefore, this error distribution can be approximated to the safe side as an ellipsoid included in a rectangular parallelepiped.

  FIG. 5 shows an example of the configuration of the object recognition processing unit. As shown in this figure, the object recognition processing unit 42 includes an internal storage device 34 and a central processing unit 35. The object recognition processing unit 42 receives data from the data input device 32 and the external storage device 33 and sends the data to the robot control unit 45.

The data input device 32 includes the above-described distance sensor, and inputs a coordinate value on a three-dimensional shape to the object recognition processing unit 42. In addition, for example, a goniometer, an odometer, or the like may be used together to input the position / posture and movement distance of the distance sensor. 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. When the size of the environment model is large and the external storage device 33 cannot hold the coordinate values on the three-dimensional shape, the voxel position, the representative point, and the entire error distribution inputted to the internal storage device 34, which will be described later, For storing a coordinate value, a voxel position, and a representative point and a part or all of an error distribution of an input three-dimensional shape with respect to a partial range or the entire range of a model, and for executing the method of the present invention Memorize the program. The external storage device 33 stores CAD model data described later.
The internal storage device 34 is, for example, a RAM, a ROM, or the like, and the input coordinate value on the three-dimensional shape, the voxel position, and the representative point and a part of its error distribution or the partial range or the entire range of the environmental model Store the whole and store the calculation information.
The central processing unit 35 (CPU) functions as a model construction device, a matching device, an alignment device for rough alignment and fine alignment, and a model update device. Run the program. The model building device is a device that performs a model building step described later, and the matching device is a device that performs a matching step between an environmental model and a point cloud described later and a matching step between measurement data and CAD model data. The apparatus is an apparatus that performs a rough alignment step and a fine alignment step, which will be described later, and the model update apparatus is an apparatus that performs a model update step, which will be described later.
The robot control unit 45 controls the movement of the robot arm and hand based on the position and orientation of the measurement object received from the object recognition processing unit 42.

  The above-described apparatus of the present invention may be a combination of the above-described distance sensor and a normal PC (computer), or may be an apparatus in which the entirety is integrated. Moreover, you may integrate in the apparatus which can be self-propelled.

FIG. 6 is a flowchart illustrating the method of the present invention. As shown in FIG. 6, a data input step S1, a data correction step S2, a search range limiting step S3, a model construction step S4, a matching step S5 between an environmental model and a point group, a self-position identification step S7, S10, and a rough matching step S8, precise alignment step S9, model update step S11, and measurement data and workpiece model data matching step S12.
In addition, S4 is implemented only when measurement data is obtained for the first time, and other S1-S3 and S5-S12 are implemented every time measurement data is obtained.

In the data input step S1, the coordinate value on the three-dimensional shape measured using the distance sensor from the new measurement position is input to the storage device of the computer. In addition, for example, a goniometer, an odometer, or the like may be used together to input the position / posture and movement distance of the distance sensor.
In this data input step S1, it is preferable to use the three-dimensional laser radar 10 to sequentially acquire coordinate values on the three-dimensional shape as distance data having an arbitrary measurement position as the origin while moving the origin. .

  When the three-dimensional laser radar 10 is used as a distance sensor, the coordinate value on the three-dimensional shape is distance data having an arbitrary measurement position as the origin, and is represented by polar coordinate values (r, θ, φ). Further, the error distribution of each coordinate value is obtained by calculation from polar coordinate values (r, θ, φ), or is input in advance by another input means (for example, a keyboard).

In the data correction step S2, distance data correction processing is performed to improve the accuracy of the distance data. Alternatively, the polar coordinate data and the odometer data may be converted into three-dimensional space data (x, y, z) having an arbitrary fixed position as the origin.
In the distance data correction processing, isolated point removal, statistical processing, and the like are performed. An isolated point is a point that is isolated from surrounding points, and the measurement data is composed of a plurality of adjacent points. Therefore, the isolated point can be removed on the assumption of an erroneous measurement. The statistical process corrects the distance by statistically processing a plurality of measurements (for example, an average value) in consideration of the error distribution included in the measurement data.
Furthermore, when the target three-dimensional shape can be linearly approximated or planarly approximated, these should be performed.

In the search range limiting step S3, the search range of the distance sensor is limited.
If the measurement data is matched with the environmental model without limiting the search range, a plurality of solutions (measurement points) may be obtained. Therefore, (1) the current sensor position is estimated from the past sensor position and the amount of change from the past sensor position, and the vicinity of the sensor position estimation result is searched. (2) The sensor position is estimated using an odometer. The search range is limited. (3) Of the distance data, not only the distance value but also the reflection intensity value is used to narrow down the search result.

FIG. 7 is a schematic diagram of model building steps when an octree is used for voxel division.
In the model construction step S4, as shown in this figure, an environment model is constructed in which a spatial region in which a three-dimensional shape exists is divided into a plurality of voxels 6 made of rectangular parallelepipeds whose boundary surfaces are orthogonal to each other and each voxel position is stored. To do.
The shape of the voxel 6 may be a cube having the same length on each side or a rectangular parallelepiped having a different length on each side.
The length of each side of the voxel 6 is preferably set to a size corresponding to the minimum necessary resolution of the maximum voxel 6. Hereinafter, the largest voxel 6 is referred to as a level 1 voxel.
In addition, when there are multiple measured points within a single voxel, for example, when an octree is selected so that only a single measured point exists within a single voxel, the voxel Is further divided into eight voxels hierarchically. Hereinafter, a spatial region in which the largest voxel 6 is divided into eight is referred to as a level 2 voxel, and a spatial region in which k is performed k times is referred to as a level k + 1 voxel.

FIG. 8 is a schematic diagram of the constructed environmental model.
In the matching step S5, as shown in this figure, the representative point 7 and its error distribution 8 are set and stored in the voxel 6 corresponding to the coordinate value on the three-dimensional shape. The terminal voxel can have only one representative point of measurement. Each voxel has a representative point of the measurement value and its error distribution, thereby representing the shape of the object. In addition, the voxel can have a probability value representing the existence probability of the object.

In the matching step S5, the absolute position of the representative point is given by the following [Equation 3]. Here, (x, y, z) is the relative coordinates of the voxels of the representative points, Sx, Sy, Sz is the side of the voxel at level 1 _{size, n x (k), n} y (k), n _z (k) is the address of the voxel at level k, and L is the level at which the representative point to be found exists.

9 and 10 are diagrams showing the data structure of voxel data in the present invention.
In this figure, FIG. 9 is a memory layout example of each voxel data. In this figure, an arrow represents a link to data, and a pointer to the data is held as a value.
FIG. 10 shows an example in which a voxel of level 2 (1, 1, 0) has a representative point. In this figure, null represents an empty set.

The environmental model of the data structure described above has the following characteristics.
(1) Contents: A space is divided into a plurality of small rectangular parallelepipeds (voxels), and a representative point of measurement points and an error distribution are held in each voxel.
(2) Accuracy: Equivalent to the representative value of the measurement points for each voxel.
(3) Presence: The presence or absence of an object can be expressed.
(4) Data amount: A memory is required in proportion to the number of voxels, but the size is fixed.
(5) Conversion from a point cloud: Suitable and less computational complexity.
(6) Access speed: Since the structure is simple, access to elements is fast.

Moreover, from this characteristic, the environmental model mentioned above satisfy | fills all the following effects AC.
Effect A: It is possible to express in consideration of errors.
Effect B: Necessary memory amount and calculation amount are below a certain amount.
Effect C: Not only the presence of an object but also the absence of an object can be expressed.

In step S6 of FIG. 6, the presence or absence of data relating to the same three-dimensional shape measured from the previous measurement position is checked. In this check, when there is no data regarding the same three-dimensional shape measured from the previous measurement position, the new measurement position is identified as the self position (sensor position).
This identification is preferably performed at a known six-degree-of-freedom position (for example, the origin of the global coordinate system) at the initial position of the moving body that sequentially moves. In this identification, it is preferable to identify a 6-degree-of-freedom position including a position (3 degrees of freedom) and a posture (3 degrees of freedom) of a new measurement position.

If there is data relating to the same three-dimensional shape measured from the previous measurement position in the check in step S6, the rough matching step S8 and the fine matching step S9 are performed after the matching step S5.
FIG. 11 is a data processing flowchart of the rough matching step S8 and the fine matching step S9, FIG. 12 is a schematic diagram of the rough matching step S8, and FIG. 13 is a schematic diagram of the fine matching step S9.

In the rough matching step S8 of FIG. 11, as shown in FIG.
(1-1) Rotate and translate the new measurement data and error distribution so that the evaluation value (for example, the sum of the distances) regarding the distance between the measurement data and the error distribution and the voxel having a representative point close to the measurement data and the error distribution is minimized. Or (1-2) rotating and translating an environmental model (a voxel created from new measurement data) for a new measurement position, and an evaluation value (for example, the distance between the voxels having adjacent representative points) (Sum) is minimized, or
(2-1) Rotate and translate the new measurement data and error distribution, and the evaluation value (for example, the sum of the probability values) regarding the probability value of the voxel having a representative point close to the measurement data and error distribution is maximized. Or (2-2) rotating and translating an environmental model (a voxel created from new measurement data and each voxel has a probability value) for a new measurement position, and the probability value of the adjacent voxel has Alignment is performed so that an evaluation value related to the difference (for example, the sum of the differences of the probability values) is minimized.

The alignment in the rough alignment step S8 is performed by expressing the environment model and measurement data on the voxel space, or by expressing the environment model on the voxel space and the measurement data by representative points and error distribution. Assuming that the current measurement data is measurement at position (x, y, z) and orientation (θ, φ, ψ), the measurement data is converted into world coordinates, and the degree of coincidence with the environment model is calculated.
For example, the shortest distance method can be used to calculate the degree of coincidence. When the shortest distance method is used, the distance between voxels is expressed as follows, assuming that the two voxel spaces are x ⁽¹⁾ and x ⁽²⁾ , the total number of voxels is I, and the value of voxels is x _i ^(n). ] Expression.
The optimum position and orientation of the measurement data can be calculated by the least square method that minimizes ε by changing the position (x, y, z) and the orientation (θ, φ, ψ).

As the degree of coincidence, for example, in both voxels of the environmental model and the measurement data, an evaluation value (for example, the sum of the difference between the probability values) regarding the difference between the probability values of the adjacent voxels can be used. In this case, the optimum position / posture of the measurement data is changed so as to minimize the degree of coincidence.
In addition, when the environmental model is represented in the voxel space and the measurement data is expressed as a representative value and an error distribution, the representative value of the measurement data and an evaluation value related to the probability value of the voxel of the environmental model in which the error distribution is close (for example, (Sum of probability values) can be used. In this case, the optimum position / posture of the measurement data is changed so as to maximize the degree of coincidence.

In the precision alignment step S9 of FIG. 11, as shown in FIG. 13, with respect to the environmental model for the previous measurement position,
(1) Rotate and translate new measurement data and error distribution, or (2) Rotate and translate environment model for new measurement position,
Alignment is performed so that an evaluation value (for example, the sum of the distances) related to the distance between adjacent error distributions is minimized.
For precise alignment of the environmental model and measurement data in the precision alignment step S9, a technique that considers an error distribution is used for an ICP algorithm that can align the point cloud and the point cloud. For the initial value of alignment, the position / posture obtained by rough alignment is used.
In calculating the distance between error distributions used in the ICP algorithm, for example, the case where the error distributions intersect is considered as the same measurement point, and the distance value in that case is multiplied by the weight obtained from the degree of coincidence of the distributions. For example, a distance scale such as Mahalanobis distance can be used to match the distributions.
The distance between the environmental model and the measurement data in this case is p _{Mi for} the environmental model data, Σ _{Mi for} the error distribution of the environmental model data, P _{Di for} the measurement data, Σ _{Di for} the error distribution of the measurement data, and a composite function of the error distribution. If the number of environmental model data corresponding to w and measurement data is N, it can be defined by the equation [Equation 5]. Here, T represents transposition. The error distribution composition function w is a weight obtained from the distribution coincidence.
The optimum position / orientation of the measurement data is the smallest self that minimizes ε by moving P _Di by changing the position (x, y, z) and orientation (θ, φ, ψ) where the measurement data is measured. It can be calculated by multiplication.
In addition to the self-position / posture identification, the self-position error distribution is identified, and the self-position and error distribution are corrected by the Kalman filter from the current self-position and error distribution and the identified self-position and error distribution.

  Further, the model update step S11 in FIG. 6 is performed after the self-position identification step S10, and updates the environment model constructed in the model construction step S4.

FIG. 14 is a data processing flowchart in the model update step S11. As shown in this figure, a voxel corresponding to the coordinate value of the point to be measured newly input in step S21 is searched, and in step S22, there is no representative point in the corresponding voxel (the voxel is empty). Sets (newly registers) the coordinate value and error distribution of the measured point newly input in step S23 as the coordinate value and error distribution of the representative point.
In step S23, in principle, no object should exist between the new measurement position (origin) and the point to be measured. Therefore, the representative point and error distribution in the voxel located between the new measurement position (origin) and the measurement point are reset or deleted.

FIG. 15 is a schematic diagram in the case where there is a representative point already set in the corresponding voxel.
If there is a representative point already set in the corresponding voxel in step S22 of FIG. 14, the error distribution newly acquired in step S24 is compared with the error distribution in the already set voxel (that is, whether different points are the same). Judge whether it is one point).
In this comparison, if the error distributions overlap each other ((A) in FIG. 15), in step S25, both error distributions or the representative points already set in the both error distributions and voxels are newly input. A new error distribution and a new representative point are reset from the coordinate value of the measurement point (that is, the error distribution is synthesized).
In this comparison, if the error distributions do not overlap with each other (FIG. 15B), the voxels are further reduced so that only a single representative point exists in a single voxel in steps S26 and S27. Dividing into 8 voxels hierarchically and newly registering.
The criteria for division and synthesis are determined from, for example, the degree of coincidence of error distributions. For example, a distance scale such as Mahalanobis distance can be used for the degree of coincidence of error distributions. Further, based on the two error distributions, it may be determined by a statistical test whether both represent the same point.

As a result of resetting the new error distribution and the center of the new error distribution from both error distributions in step S25, when a new representative point moves into another voxel (that is, Yes in step S28), the process proceeds to step S22. Return and repeat the above process.
Note that FIG. 16 shows a new error distribution and a new error from both error distributions in step S25, or from both error distributions and the coordinate values of the representative points already set in the voxels and the newly inputted measured points. As a result of resetting the center of the distribution, a case where a new representative point moves into another voxel is shown.

  When a probability value representing the existence probability of an object is set in a voxel, new registration processing is performed after newly registering, resetting, deleting, or dividing representative points and error distributions in the voxel in the model update step S11. Accordingly, the probability value in the voxel is also newly registered, reset, erased, or newly registered after division by statistical processing.

  FIG. 17 is another schematic diagram when the error distributions overlap each other (FIG. 15A). In step S25, a Kalman filter can be used as means for combining the two representative points and the error distribution to set an error distribution for the new representative point. For example, in the case of two dimensions, as shown in this figure, the two representative points are x (1) and x ′ (2), the two error distributions are Σ (1) and Σ ′ (2), respectively. 17 is a schematic diagram for calculating the representative point x (2) and the error distribution Σ (2), where x (2) and the error distribution are Σ (2).

In the matching step S12 of FIG. 6, it is assumed that the measurement data follows a normal distribution of an average value m (i) and an error covariance matrix Sm (i).
FIG. 22 is a two-dimensional schematic diagram for explaining matching between measurement data and a CAD model. As shown in this figure, a CAD model of an object to be measured (for example, a workpiece) is represented by, for example, a triangular patch, and its vertex is represented by w (j). When the position of the CAD model is t and the posture is R, the coordinates of the vertices of the triangular patch are R · w (j) + t. Note that t is a three-element translation vector, and R is a 3 × 3 matrix.

FIG. 23 is a diagram for explaining the correspondence between the measurement data and the CAD model. The subscript j of the triangle patch w (j) is the result of selecting the nearest vertex among the vertices of all triangle patches for m (i).
FIG. 24 is a diagram for explaining the degree of coincidence between the measurement data and the CAD model. The degree of coincidence E (R, t) between the measurement data and the CAD model can be defined by the equation shown in FIG. 25 using the Mahalanobis distance. The position and orientation of the measurement object calculated from the measurement data are given by R, t that minimizes E (R, t).

Data on the calculated position t and posture R of the measurement object is output to the robot controller 45 and the like. Data relating to the position t and posture R of the measurement object output to the robot controller 45 or the like is used for the robot 46 to grasp the measurement object.
If the position t and orientation R of the object to be measured can be calculated, the position of the portion where the error distribution of the measurement data does not match the CAD model data and the amount of deviation thereof can be calculated. When the amount of deviation is large, it can be seen that there is a defect on the surface of the object to be measured. That is, the surface shape of the measurement object can be inspected.

  In the model update step S11, a voxel corresponding to the coordinate value of the newly input measurement point is searched, and at least one of the representative point and the error distribution in the voxel is newly set or reset. Alternatively, when the voxel is further divided into a plurality of hierarchically divided voxels, the position of the representative point of the voxel may be a three-dimensional shape measurement value in the matching step S12.

  In the matching step S12, when the position / posture of the distance sensor is obtained, the position of the representative point of the voxel in the environmental model in the range visible from the position may be used as the measurement value of the three-dimensional shape. The range that can be seen from the position of the distance sensor is the range in which the distance sensor can measure the position from the position of the distance sensor, the angle range (field of view) that the distance sensor can measure from the position of the distance sensor, and the position of the distance sensor Distance range in which the distance sensor can measure the position.

  The process procedure shown in FIG. 6 is repeated every time new measurement data is obtained at a new measurement position, and the result is stored in at least one of the internal storage device 34 and the external storage device 33. In order to speed up the processing, it is preferable to store the result in the internal storage device 34 as long as the capacity permits.

According to the above-described method and apparatus of the present invention, a spatial region in which a three-dimensional shape exists is divided into a plurality of voxels 6, and each voxel position is stored. Therefore, even if the measurement object is large, data The amount can be suppressed to a small data size proportional to the number of voxels.
In addition, since the representative point 7 and its error distribution 8 are set and stored in the voxel 6 corresponding to the coordinate value, information exceeding the resolution of the voxel can be expressed.

Moreover, even if the error distribution is wider than the voxel to which the representative point belongs by setting and saving a probability value representing the existence probability of the object inside the voxel, the representative point indicates whether or not the object exists in each voxel. Since it is possible to easily determine only the probability value of the relevant voxel without finding the voxel to which it belongs and recalculating it from its error distribution, the search time can be reduced.
Also, the current measurement position
(1) Estimate based on “past measurement position” and “planned amount of change from past measurement position”,
(2) The current measurement position can be acquired by a sensor that can be acquired, or (3) not only the distance value of the measurement data but also the reflection intensity is used to limit the collation range, so that the search time can be reduced.

Further, in the rough matching step S8, for the environmental model for the previous measurement position,
(1-1) Rotate and translate new measurement data and error distribution, and the evaluation value (for example, the sum of the distances) regarding the distance between the measurement data and the error distribution and a voxel having a representative point adjacent to the measurement data and error distribution is minimized. Or (1-2) rotating and translating an environmental model (a voxel created from new measurement data) for a new measurement position, and an evaluation value (for example, the distance) between voxels having adjacent representative points (2-1) New measurement data and error distribution are rotated and translated, and an evaluation value related to the probability value of a voxel having a representative point close to the measurement data and error distribution (for example, Or (2-2) an environmental model for a new measurement position (created from new measurement data). Rotate and translate voxels, each voxel has a probability value), and align so that the evaluation value (for example, the sum of the difference of the probability values) of the probability values of neighboring voxels is minimized Therefore, it is possible to align voxels having representative points in a short time while preventing error accumulation.

  In addition, the environmental model for the new measurement position is rotated and translated with respect to the previous environment model for the measurement position, and the evaluation value (for example, the sum of the difference of the probability values) regarding the difference in probability values of adjacent voxels is minimized. As described above, in the case of positioning, since the positioning is performed in consideration of information in which no object exists, the accuracy can be improved.

Next, in the precision alignment step S7, new measurement data and error distribution are rotated and translated with respect to the environment model for the previous measurement position, or the environment model for the new measurement position is rotated and translated, and the error distribution between adjacent error distributions is calculated. Since the alignment is performed so that the evaluation value (for example, the total sum of the distances) regarding the distance between the error distributions is minimized, accurate alignment between the error distributions can be performed in a short time.
Therefore, the integration processing of data from a plurality of viewpoints according to the present invention enables highly accurate shape acquisition while preventing error accumulation.

  In the present invention, since the voxel structure is expanded, the data size can be made smaller than that of the point group. Accordingly, the amount of data to be searched for when positioning the surrounding environment and measurement data (ICP algorithm) is reduced, so that the calculation can be made more efficient.

  Furthermore, the conventional ICP algorithm outputs incorrect results for sparse data, but the environment model of the present invention has a representative point and error distribution in the voxel, so alignment corresponding to sparse data is possible. It is.

  In the model construction step S4, when the maximum voxel 6 is set to a size corresponding to the necessary minimum resolution, and there are a plurality of measurement points in the single voxel 6, a single voxel is obtained. The voxel is further divided into eight voxels hierarchically and divided into a plurality of voxels so that only a single point to be measured exists in the image. The resolution can be further increased by using the representative points.

  In particular, a plurality of coordinate values related to the same position on the 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 coordinates of the distance data are obtained. By using the measurement error of the value as the error distribution of the representative point, a plurality of measurements can be statistically integrated using the accurate coordinate value and the error distribution, and the accuracy can be further improved.

  FIG. 18 shows how the error distribution of the representative points is reduced and the accuracy of the representative points is improved by integrating the distance data having a plurality of measurement positions as the origin. Since distance data obtained using different measurement positions (that is, positions of a three-dimensional measuring instrument as a distance sensor) in this way also have different error distribution directions, these distance data are sequentially integrated via an environmental model. As a result, the error distribution of the representative point is reduced, and the position accuracy of the representative point is improved. In FIG. 18, the figure after the three-dimensional measurement is a schematic diagram showing the two-dimensional cross section of the cup, and the broken line in the figure after the three-dimensional measurement shows the actual surface of the cup.

  In addition to the self-position identification, the self-position error distribution is identified, and the self-position and error distribution are corrected by the Kalman filter from the current self-position and error distribution and the identified self-position and error distribution. Accuracy can be improved.

  Also, assuming that there is no object between the origin and the point to be measured, the influence of erroneous measurement data can be removed by resetting or deleting the representative point and error distribution in the voxel located between them. it can.

  Also, a voxel corresponding to the coordinate value of the newly input measurement point is searched, and when there is no representative point in the voxel, the coordinate value and the error distribution are set as the coordinate value and the error distribution of the representative point. Thus, the coordinate value and error distribution of the representative point can be easily set.

Further, when there is a representative point already set in the voxel, the newly acquired error distribution is compared with the already set error distribution in the voxel,
If the error distributions overlap each other, either a new error distribution and a new representative from both error distributions, or from the coordinate values of both the error distribution and the representative point already set in the voxel and the newly input measured point. Reset the point,
When the error distributions do not overlap each other, the voxel is further divided into eight voxels and divided into a plurality of voxels hierarchically so that only a single representative point exists in a single voxel. It is possible to converge to a highly accurate shape while avoiding accumulation.

  Therefore, according to the method and apparatus of the present invention, it has a function of correcting data including an error into accurate information and converges with high accuracy for a plurality of measurements. In addition, the position identification process is a process for updating the representative point corresponding to each voxel and its error distribution at the measurement point, so the calculation amount is small, and the calculation is closed in the voxel without affecting the surrounding voxels. Yes. Therefore, high-speed processing is possible. Moreover, the measurement data can be sequentially integrated into the voxels, and the memory size of the surrounding environment information obtained as a result does not exceed the fixed size. Specifically, when the number of voxels of level 1 is N and the maximum division level is Lmax, the memory size is a value obtained by multiplying the memory size required for each voxel by the formula shown in FIG. This is no more than a fixed size.

  The model update step using the Kalman filter will be described in detail.

  In the case of the model update step using the Kalman filter, a new representative point is obtained by the Kalman filter from the newly input coordinate value of the measurement point and its error distribution, and the representative point in the already set voxel and its error distribution. And get the error distribution and reset.

  The model is expressed by the following [Equation 6] based on the position m (i) of each model point group as the state quantity and based on the position of the measurement point of the distance sensor. In this embodiment, m (i) is a representative point inside the voxel (the same applies hereinafter).

In [Equation 6],
L (j) is a measurement position by the distance sensor. For example, L (j) is a position L (j) = (x _L (j) of a measurement point j (j = 1,..., N) of a three-dimensional LRF (laser range finder) in the sensor coordinate system of the distance sensor. ), Y _L (j), z _L (j)) ^t . Here, t represents a transposed matrix (the same applies hereinafter).
h _m (R _r , _tr , m (i)) is an observation system model for L (j).
R _r is a rotation matrix R _r = R (θx, θy, θz) representing the attitude of a mobile body (for example, a mobile robot) equipped with a distance sensor with respect to the world coordinate system. Note that θx, θy, and θz indicate rotation angles around the x-axis, y-axis, and z-axis, respectively (hereinafter the same).
_tr is a translation vector _tr = (x, y, z) representing the position of the moving body with respect to the world coordinate system.
v _L (i) is observation noise added to the measurement value L (j) of the distance sensor.
R _s is a rotation matrix Rs = R (θx, θy, θz) with respect to the moving body coordinate system of the sensor coordinate system.
t _s is a translation vector t _s = (x, y, z) representing the position of the sensor coordinate system with respect to the moving object coordinate system.

  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 equation [7]. It should be noted that only the representative point m (i) on the model point group associated with the measurement point group by the distance sensor may be updated by the following equation (7).

In the formula of [Formula 7],
The subscript k represents a value at a discrete time k.
For m _k (i), m ′ _k (i) indicates the updated value (post-mortem value) of m _k (i), and m _{k, k−1} (i) is based on m ′ _k−1 (i). In addition, a predicted value (preliminary estimated value) of m _k (i) is shown. Since the environment (measurement object) is stationary, m _{k, k−1} (i) = m ′ _k−1 (i).
Σ _mk (i) is an error covariance matrix (that is, the above-described error distribution) of the representative point m _k (i) inside the voxel. Further, for Σ _mk (i), Σ ′ _mk (i) indicates an updated value (post-mortem estimate) of Σ _mk (i), and Σ _{mk, k−1} (i) indicates Σ ′ _mk−1 (i). The predicted value (preliminary estimated value) of Σ _mk (i) based on In the sensor coordinate system, the position of the measurement point j (j = 1,..., N) of the three-dimensional LRF is represented by L (j), and its error covariance matrix is represented by Σ _L (j). Here, N is the total number of measurement points obtained by the three-dimensional LRF. Assuming a constant normal distribution regardless of the measurement distance as an error model for 3D LRF. The error covariance matrix in the case of irradiating a laser beam to the x-axis direction of the sensor coordinate system and sigma _S. The error distribution also changes its posture according to the laser irradiation direction. Σ _L (j) is expressed as Σ _L (j) = R _L (j) Σ _S R _L ^t (j) using the rotation matrix R _L (j) with respect to the reference direction. The position z (j) of the measurement point j in the world coordinate system and its error covariance matrix Σ _z (j) are z (j) = R _r (R _s L (j) + t _s ) + t _r , Σ _{z (j) =} it can be represented as _{R r R s Σ L (j} ) R s t R r t.
K _mk (i) is a Kalman gain for m _k (i).
h _mk (R _rk , t _rk , m _{k, k−1} (i)) is an observation system model for L _k (j) and i = p _k (j). i = p _k (j) is a point on the environment map (ie, environment model) associated with the measurement point j.
H _mk is the Jacobian matrix of the observation system model for L _k (j) and i = p _k (j), and is expressed by the following [Equation 8].

When the Kalman filter is updated, the position of each point (representative point of voxel) of the model point cloud of the environmental map and the updated values m ′ _k (i) and Σ ′ _mk (i) of the error covariance matrix are obtained. The environmental model is updated according to the following procedure.
(1) These updated values m ′ _k (i) and Σ ′ _mk (i) are reset as new representative points and error distributions.
(2) As a result of the above (1), when the position of the representative point moves into another voxel, if the destination voxel does not hold the representative point, the moved representative point and its error covariance matrix Is held in the destination voxel, and representative points and the like are removed from the source voxel. When the destination voxel already holds a representative point, it is determined whether these two error distributions overlap at the two representative points (similar to the determination in S24 described above). The subsequent processing may be the same as the processing after S24 in FIG.
(3) For a measurement point by a distance sensor that has not been associated with the representative point m (i) on the model point group, if the voxel including the measurement point does not have a representative point, the measurement point and its error The distribution is added and retained as a representative point of the voxel and an error distribution. If a representative point already exists in the voxel, the existing representative point and each measurement point are all included in different voxels, including other measurement points that were not associated in the voxel. As shown, the voxel is divided, and the divided voxels are made to inherit the representative points and the like.

By repeating the model update step using the above-described Kalman filter, the range of the error covariance matrix (that is, error distribution) in the voxel is gradually reduced, and the voxel is easily divided. By dividing the voxel, it is possible to express a change below the size of the initial voxel.
FIG. 19 shows the result obtained by the model update step using the Kalman filter. FIG. 20 is a partially enlarged view of FIG. In these figures, the length of one side of the initial voxel is 100 cm, and the number of subdivisions is allowed up to six. In the region where the target exists, the measurement target is expressed with high accuracy as a result of repeated subdivision of the voxels. It can be seen that the voxel is not subdivided in an area where there is no target, and the environment can be expressed with a necessary and sufficient amount of data. In addition, the error distribution of the representative points in each voxel is small, and the environment map can be expressed with high accuracy. As described above, even if data includes an error, a result converged to a true value can be obtained due to the effect of the Kalman filter. Further, in this method, the standard deviation is reduced by increasing the number of measurement data, and further improvement in accuracy can be expected.

  Further, since the position / orientation of the measurement object is fixed, the update can be performed independently of the position / orientation of the measurement object. Note that only the representative point m (i) on the model point group associated with the measurement point group by the distance sensor is updated by the above-described Kalman filter, so that the calculation cost can be greatly reduced. Become.

In the fine alignment step, instead of performing alignment so that the sum of the distances between the adjacent error distributions is minimized, an evaluation value (for example, an agreement value determined by a maximum likelihood estimation value based on the adjacent error distributions (for example, , Rotate and translate new measurement data and error distribution with respect to the environment model for the previous measurement position, or rotate and translate the environment model for the new measurement position so that the matching power is the maximum) May be aligned.
This case will be described in detail.

Since an error model is taken into account for both the model point cloud that is an environmental map (environment model) and the measurement point cloud of the sensor, a calculation formula for the evaluation value related to the matching degree (for example, the sum of the matching degree) It is also possible to incorporate both errors into the evaluation function. In the case of the present embodiment, instead of simply associating the closest point, by incorporating the concept of likelihood, the error distribution on the environmental map and the error distribution of the measurement point are added to the evaluation function, and In the environmental map, the evaluation function is determined to be maximized when the appearance probability of each measurement point is maximized.
Specifically, the position of the point i = p (j) on the environment map associated with the measurement point j is the normal value of the average value (representative point) m (i) and the error covariance matrix Σ _m (i). Assuming that the distribution follows, the probability value Pr (L (j) | m (i), Σ _m (i)) from which the measurement data L (j) is obtained as a result of measurement by the three-dimensional LRF is expressed as a point i. The evaluation function EM (i, j) with respect to the point j is set, and the evaluation function is determined by the following equation [Equation 9] so that the sum of the functions is maximum. The degree of coincidence determined by the maximum likelihood estimation value based on the error distribution can be calculated by the equation [Equation 9].

However, ω (j) is 1 when there is a point associated with the measurement point j in the model point group, and 0 otherwise.
Here, Pr (L (j) | q) represents a probability value for obtaining measurement data L (j) when the point on the environment map is at the position q, and Pr (q | m (i) , Σ _m (i)) is assumed to follow the normal distribution of mean value m (i) and error covariance matrix Σ _m (i), and represents the probability value that the point of the environmental map is at the position of q. , [Equation 10] holds.

Pr (q | m (i), Σ _m (i)) is expressed by the following [Equation 11] assuming a normal distribution.

On the other hand, Pr (L (j) | q) can be approximated by the following equation [Equation 12] by replacing L (j) with z (j).

Here, z _k (j) depends on the position t _r and posture R _r of a mobile body equipped with a distance sensor, for example, a mobile robot. Actually, since the direction of q viewed from the center of the sensor coordinate system of the three-dimensional LRF and the direction of the measurement point L (j) are different as shown in FIG. 21, the error covariance matrix Σ _z (j) is also the direction of q. However, since the existence probability of q located far away from the point i on the associated environmental map is low, it can be approximated with sufficient accuracy. Therefore, Pr (L (j) | m (i), Σ _m (i)) can be expressed by the following equation [Equation 13].

  By the simple calculation, the following equation (14) is obtained.

  However, α (j) and β (j) can be expressed by the following equation (15).

Accordingly, 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 Σ _m (i). In the normal distribution of + Σ _z (j), it can be approximated to a probability value for obtaining z (j). By using this evaluation function, it is possible to associate the environment map and the measurement data in consideration of errors.

Supplementary explanation will be given for the association between the measurement points and the environment map (that is, the environment model). In the above embodiment, since a statistical evaluation function considering the error distribution is used, the corresponding point cannot be determined unless the value of the evaluation function is obtained. Therefore, candidates to be associated in the model point group on the environment map are narrowed down in advance, and corresponding points are obtained from the candidates based on the value of the evaluation function. Specifically, it can be determined as follows.
(1) The highest voxel that intersects the range of the error covariance matrix Σ _L (j) of the target measurement point j (for example, a range of three times the standard deviation) and the highest voxel adjacent to the voxel The representative points existing in these voxels including the lower-layer voxels are used as candidates for corresponding points. Since the voxels have 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 why adjacent voxels are also added to the candidates is that the range of the error covariance matrix may extend to adjacent voxels depending on the position of the representative point in the voxel.
(2) The value of the evaluation function EM (i, j) is obtained using the representative point i of the candidate voxel and the error covariance matrix.
(3) The representative point i having the largest value of the evaluation function EM (i, j) is taken as the corresponding point. However, when the value of the largest evaluation function is less than a certain threshold value, it is considered that there is no corresponding point.
In this embodiment, as an evaluation function EM (i, j) for association, an expression based on likelihood is adopted, and there is a statistically clear determination scale regarding the presence or absence of corresponding points, so there is a corresponding point. 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 corresponding to an unmeasured part so far and added to the environment map.

  Although the measurement of the three-dimensional shape has been described as an embodiment, the two-dimensional shape can be similarly measured by looking at the two-dimensional shape as a special case of the three-dimensional shape.

  In addition, this invention is not limited to embodiment mentioned above, Of course, it can change variously in the range which does not deviate from the summary of this invention.

It is a whole block diagram of the object recognition apparatus of this invention. It is a block diagram of a three-dimensional laser radar. It is a figure which shows the relationship between polar coordinate data measured with the distance sensor, and an error. The case where an error distribution is approximated as an ellipsoid included in a rectangular parallelepiped is shown. It is a figure which shows an example of a structure of an object recognition process part. 3 is a flowchart illustrating the method of the present invention. It is a schematic diagram of a model construction step. It is a schematic diagram of the constructed environmental model. It is a figure which shows the data structure of the voxel data in this invention, and has shown the memory layout example of each voxel data. It is a figure which shows the data structure of the voxel data in this invention, and has shown the example in case the voxel of level 2 (1, 1, 0) has a representative point. It is a data processing flowchart of rough alignment step S6 and fine alignment step S7. It is a schematic diagram of rough matching step S6. It is a schematic diagram of precise alignment step S7. It is a data processing flowchart in a model update step. It is a schematic diagram in case there exists a representative point already set in the corresponding voxel. In the case where the error distributions overlap each other, a new error distribution and a new error distribution center are reset from both error distributions, and as a result, a new representative point moves into another voxel. It is a schematic diagram when error distributions mutually overlap. FIG. 6 is a schematic diagram showing a state in which error distribution of representative points is reduced and accuracy of representative points is improved by integrating distance data having a plurality of measurement positions as origins. The result obtained by the model update step using the Kalman filter is shown. FIG. 20 is a partially enlarged view of FIG. 19. The correspondence in consideration of the error is shown. It is a two-dimensional schematic diagram for demonstrating matching with measurement data and a CAD model. It is a figure for demonstrating a response | compatibility with measurement data and a CAD model. It is a figure for demonstrating the coincidence of measurement data and a CAD model. This is an equation for defining the degree of coincidence E (R, t) between the measurement data and the CAD model using the Mahalanobis distance. It is a figure for demonstrating the memory size when the number of voxels of level 1 is N and the division | segmentation maximum level is set to Lmax.

Explanation of symbols

40 Object Recognition Device 41 3D Sensor 42 Object Recognition Processing Unit 43 Model Database Unit 45 Robot Control Unit 46 Robot

Claims (20)

An object recognition method for recognizing the position and orientation of an object,
A data input step for inputting a coordinate value on a three-dimensional shape obtained by measuring from a new measurement position to a computer;
A model construction step of dividing the spatial region where the three-dimensional shape exists into a plurality of voxels composed of rectangular parallelepipeds whose boundary surfaces are orthogonal to each other, and constructing an environment model that stores each voxel position;
Performing a first matching step of setting and storing a representative point and its error distribution inside the voxel corresponding to the coordinate value;
When there is no data of the three-dimensional shape obtained by measuring from the previous measurement position, the new measurement position is identified as the self-position,
When there is data of the three-dimensional shape obtained by measuring from the previous measurement position,
For the environmental model for the previous measurement position,
Rotate and translate new measurement data and error distribution, or rotate and translate environment model for new measurement position,
A precision alignment step for aligning the evaluation values regarding the distance between adjacent error distributions to a minimum;
Performing a self-position identification step of identifying a self-position from the rotation amount and the translation amount in the precision alignment step;
Furthermore, an integration step for integrating a plurality of measurement data and a plurality of error distributions measured from a plurality of measurement positions;
To the measurement data and the error distribution obtained by integrating the rotation and to translate the measurement object represented by the model data, before Symbol integrated a total measurement data obtained by the elements constituting the model data And a second matching step of performing positioning so that an evaluation value related to distance is minimized. In the second matching step, using the distance between the elements constituting the measurement data and the model data, by determining the difference between the measured data and the model data, to allow inspection of an object, characterized by object-recognition method according to claim 1.   2. The object according to claim 1, wherein in the first matching step, in addition to a representative point and its error distribution inside a voxel, a probability value representing the existence probability of the object is set and stored in the voxel. Recognition method. Before the precision alignment step, for the environmental model for the previous measurement position,
Rotate and translate the new measurement data and error distribution so that the total sum of the distance between the measurement data and the error distribution and the voxels having the representative points that are close to each other is minimized, or the environment model for the new measurement position is rotated and translated. Translate so that the sum of the distances between voxels with representative points is minimized,
The object recognition method according to claim 1, further comprising a rough alignment step of aligning. Before the precision alignment step, for the environmental model for the previous measurement position,
Rotate and translate the new measurement data and error distribution so that the sum of the probability values of voxels with representative points close to the measurement data and error distribution is maximized, or rotate and translate the environment model for the new measurement position So that the sum of the difference in probability values of neighboring voxels is minimized,
The object recognition method according to claim 1, further comprising a rough alignment step of aligning. After the data input step,
Estimate the current measurement position from past measurement position changes, or acquire the current measurement position with a sensor that can acquire it, or use the reflection intensity value as well as the distance value of the measurement data,
The object recognition method according to claim 1, further comprising a search range limiting step for limiting a range to be collated.   The object recognition method according to claim 1, wherein in the self-position identification step, a 6-degree-of-freedom position of a new measurement position is identified from the position and orientation at the previous measurement position.   In the fine alignment step, when the error distributions intersect, the same measurement point is used, and the distance between the error distributions is calculated by multiplying the distance value in that case by the weight obtained from the degree of coincidence of the distributions. The object recognition method according to claim 1.   In the model building step, when a maximum voxel is set to a size corresponding to the minimum necessary resolution and there are a plurality of measurement points in a single voxel, a single voxel is single. The object recognition method according to claim 1, wherein the voxel is further divided and divided into a plurality of voxels hierarchically so that only the measured point exists. A model update step of updating the environmental model after the self-position identification step, and in the model update step, search for a voxel corresponding to a coordinate value of a newly input measurement point;
2. The object recognition method according to claim 1, wherein an object distribution between the origin and the point to be measured is assumed to be absent, and the representative point and the error distribution in the voxel located between them are reset or deleted. A model update step of updating the environmental model after the self-position identification step, and in the model update step, search for a voxel corresponding to a coordinate value of a newly input measurement point;
2. The object recognition method according to claim 1, wherein when there is no representative point in the voxel, the coordinate value and the error distribution are set as the coordinate value and the error distribution of the representative point. A model update step of updating the environmental model after the self-position identification step, and in the model update step, search for a voxel corresponding to a coordinate value of a newly input measurement point;
When there is a representative point already set in the voxel, the newly acquired error distribution is compared with the already set error distribution in the voxel,
If the error distributions overlap each other, reset a new error distribution and a new representative point from both error distributions,
The voxel is further divided into a plurality of voxels hierarchically so that only a single representative point exists in a single voxel when error distributions do not overlap each other. Item 2. The object recognition method according to Item 1.   In the self-position identification step, self-position error distribution is identified together with self-position identification, and the self-position and error distribution are corrected by a Kalman filter from the current self-position and error distribution and the identified self-position and error distribution. The object recognition method according to claim 1, wherein: After the previous SL self-localization step, the have a model updating step for updating the environment model, in 該Mo del update step, compared with the error distribution of the newly acquired error distribution within the voxel already set When the error distributions overlap each other, as a result of resetting a new error distribution and a new representative point from both error distributions, when the new representative point moves into another voxel,
If there is no representative point in the other voxel, the new error distribution and the new representative point are set inside the other voxel,
When there is a representative point already set in the other voxel, the new error distribution is compared with the already set error distribution in the other voxel, and (A) when the error distributions overlap each other, From both error distributions, or from both error distributions and the coordinate values of the representative points already set in the voxel and the newly inputted measurement point, a new error distribution and a new representative point are reset, (B) The voxel is further divided into a plurality of voxels hierarchically so that only a single representative point exists in a single voxel when error distributions do not overlap each other. Item 2. The object recognition method according to Item 1. A model update step of updating the environmental model after the self-location identification step;
In the model updating step, a new representative point and error distribution are obtained by a Kalman filter from the newly inputted coordinate value of the measured point and its error distribution, and the representative point in the already set voxel and its error distribution. The object recognition method according to claim 1, wherein the object recognition method is reset. In the precise alignment step, instead of performing alignment so that the evaluation value related to the distance between the adjacent error distributions is minimized, the evaluation value related to the degree of coincidence determined by the maximum likelihood estimation value based on the adjacent error distributions is obtained. To maximize the environmental model for the previous measurement position,
The object recognition method according to claim 1, wherein the new measurement data and the error distribution are rotated and translated, or the environmental model for the new measurement position is rotated and translated for alignment. A formula for calculating an evaluation value related to the degree of coincidence in the precise alignment step is represented by the following equation (1):
In this equation, it is assumed that the measurement point j is associated with the representative point i on the environmental model, and the probability that measurement data corresponding to the measurement point j is obtained is EM (i, j), and ω (j) is The object recognition method according to claim 16, wherein 1 is set when a representative point associated with the measurement point j exists in the environmental model, and 0 is set otherwise. In the integration step, search for the voxel corresponding to the coordinate value of the newly input measurement point,
When there is a representative point already set in the voxel, the newly acquired error distribution is compared with the already set error distribution in the voxel,
If the error distributions overlap each other, reset a new error distribution and a new representative point from both error distributions,
The voxel is further divided into a plurality of voxels hierarchically so that only a single representative point exists in a single voxel when error distributions do not overlap each other. Item 2. The object recognition method according to Item 1. In the integration step, as a result of resetting a new error distribution and a new representative point from both error distributions, when a new representative point moves into another voxel,
If there is no representative point in the other voxel, the new error distribution and the new representative point are set inside the other voxel,
When there is a representative point already set in the other voxel, the new error distribution is compared with the already set error distribution in the other voxel, and (A) when the error distributions overlap each other, From both error distributions, or from both error distributions and the coordinate values of the representative points already set in the voxel and the newly inputted measurement point, a new error distribution and a new representative point are reset, (B) The voxel is further divided into a plurality of voxels hierarchically so that only a single representative point exists in a single voxel when error distributions do not overlap each other. Item 18. The object recognition method according to Item 18. An object recognition device for recognizing the position and orientation of an object,
A data input unit for inputting coordinate values on a three-dimensional shape obtained by measuring from a new measurement position to a computer;
A model construction unit that divides the spatial region in which the three-dimensional shape exists into a plurality of voxels made of rectangular parallelepipeds whose boundary surfaces are orthogonal to each other, and constructs an environment model that stores each voxel position;
A first matching unit that sets and stores a representative point and its error distribution inside a voxel corresponding to the coordinate value;
When there is no data of the three-dimensional shape obtained by measuring from the previous measurement position, the new measurement position is identified as the self-position,
When there is data of the three-dimensional shape obtained by measuring from the previous measurement position,
The distance between adjacent error distributions by (A) rotating and translating new measurement data and error distribution with respect to the environmental model for the previous measurement position, or (B) rotating and translating environmental model for the new measurement position. A self-position identifying unit that aligns the evaluation value with respect to the minimum and identifies the self-position from the rotation amount and the translation amount necessary for the alignment,
An integration unit that integrates a plurality of measurement data and a plurality of error distributions measured from a plurality of measurement positions;
Rotate and translate the object to be measured represented as a polyhedron by model data with respect to the measurement data and error distribution obtained by integration, and the distance between the measurement data obtained by the integration and the plane constituting the polyhedron And a second matching unit that performs positioning so that the evaluation value regarding the minimum value is minimized.