JP2011179909A - Device and method for measuring position and attitude, and program - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a technology for measuring a position and an attitude which improves a processing speed and suppresses memory consumption, in processing for estimating the position and the attitude by using a distance image of an object. <P>SOLUTION: This device for measuring the position and the attitude which measures the position and the attitude of the object based on the picked-up distance image of the object includes an update part which compares information showing a distance between an object model and an object area with information showing a distance between a partial area out of a partial area group which is not set as the object area, and the object model, and adds a new partial area to the object area according to the result of comparison, and an estimation part which applies the object model to a three-dimensional point group of the object area after the update, and estimates the position and the attitude of the object. <P>COPYRIGHT: (C)2011,JPO&INPIT

Description

  The present invention relates to a technique for measuring the position and orientation of a target object whose three-dimensional shape is known. In particular, the present invention relates to a position / orientation measurement apparatus, a position / orientation measurement method, and a program for calculating the position and orientation of a target object based on a distance image obtained by photographing the target object.

  In recent years, with the development of robot technology, robots are starting to perform complex tasks that have been performed by humans instead. A typical example of a complex task is assembly of an industrial product. In order for the robot to perform the assembling work, it is necessary to grip the part with an end effector such as a robot hand or to be fitted with another part. For that purpose, the relative position and orientation of the part to be gripped or the part to be fitted and the robot are measured, and the robot hand movement plan is formulated based on the measurement result, and the robot is actually It is necessary to control an actuator for driving the hand.

  The position and orientation of the robot has been measured using a camera and a distance sensor mounted on the robot, and a method using a two-dimensional image or a distance image is typical. In particular, distance images are often used when the object has only monotonous features, such as a scene in an office or factory, or a part of an industrial product, such as a straight line, a curved line, or a surface with little shading.

  Non-Patent Document 1 discloses a method for measuring the position and orientation of an object by model fitting to a distance image. In this method, the distance image is converted into three-dimensional point cloud data, and the position and orientation of the object are measured by fitting the three-dimensional shape model of the target object to the point cloud data. That is, based on the approximate values of the position and orientation, the surface of the three-dimensional shape model is searched for each point of the point cloud data, and the position and orientation of the position and orientation are minimized so as to minimize the sum of the distances between the points. The position and orientation are calculated by repeating optimization.

  Non-Patent Document 2 discloses a method for estimating changes in the position and orientation of an object between frames using the method of Non-Patent Document 1 based on a distance image obtained in time series. Non-Patent Document 3 discloses a method for estimating the position and orientation of an object by model fitting to a distance image. In this method, the distance image is converted into three-dimensional point cloud data, and the position and orientation of the object are estimated at high speed by fitting the point cloud data with the three-dimensional shape model of the target object expressed by an implicit polynomial. That is, for each point in the point cloud data, based on the approximate position and orientation values, the position is calculated so as to minimize the sum of the distance between the point approximated by the distance field defined by the implicit polynomial and the surface of the three-dimensional model. And the position and orientation are calculated by repeating the optimization of the orientation.

P. J. Besl and N. D. McKay, "A method for registration of 3-D shapes," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.14, no.2, pp.239-256, 1992. DA Simon, M. Hebert, and T. Kanade, "Real-time 3-D pose estimation using a high-speed range sensor," Proc. 1994 IEEE International Conference on Robotics and Automation (ICRA '94), pp.2235- 2241, 1994. B. Zheng, R. Ishikawa, T. Oishi, J. Takamatsu, K. Ikeuchi, "6-DOF Pose Estimation from Single Ultrasound Image Using 3D IP Models," Proc. IEEE International Workshop on Object Tracking and Classification Beyond the Visible Spectrum (OTCBVS08), pp.1-8, 2008. MM Blane, Z. Lie, H. Civi, DB Cooper, "The 3L algorithm for fitting implicit polynomial curves and surfaces to data," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.22, no.3, pp.298- 313, 2000. T. Sahin, M. Unel, "Fitting Globally Stabilized Algebraic Surfaces to Range Data," Proc. IEEE International Conference on Computer Vision, vol.2, pp.1083-1088, 2005. Z. Zhang, "A flexible new technique for camera calibration," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.22, no.11, pp.1330-1334, 2000. X. Jiang and H. Bunke, "Range image segmentation: Adaptive grouping of edges into regions," Proceedings of Asian Conference on Computer Vision 1998 (ACCV '98), pp.299-306, 1998. A. Bab-Hadiashar, N. Gheissari, "Range image segmentation using surface selection criterion," IEEE Transactions on Image Processing, Vol.15, No.7, pp.2006-2018, 2006.

  In general, a distance image obtained by capturing a target object whose position and orientation are to be estimated includes not only target object data but also data of objects other than the target object (non-target objects) around or behind the target object. To do. When estimating the position and orientation of the target object based on such a distance image, the calculation result of the shape model fitting is affected by the three-dimensional point cloud data of the non-target object. For this reason, there is a problem in that the position / orientation measurement (calculation) accuracy is lowered as compared with the case where only the target object data is included in the distance image. As a method of dealing with this problem, the above-described robust estimation can be used. Compared to the target object, objects other than the target object (non-target objects) tend to have a larger distance between the correspondences with the model, so applying robust estimation reduces the impact of non-target object data on optimization calculations Because.

  However, in the position and orientation measurement (calculation) method as described above, the processing load increases and the memory consumption increases as the number of three-dimensional point groups to be processed increases. Therefore, when estimating the position and orientation of an object, it is ideal to limit the three-dimensional point group to be processed to only the target object. However, the conventional method using robust estimation does not exclude non-target object data that is originally unnecessary from the model fitting process. In other words, the conventional method has a problem of a decrease in processing speed and an increase in memory consumption.

  An object of the present invention is to provide a high-speed processing for estimating the position and orientation of a target object using distance image data including images of a target object and a non-target object as input data, and to provide a position and orientation measurement technique that reduces memory consumption. And

A position and orientation measurement apparatus according to the present invention includes an acquisition unit that acquires a distance image of a target object imaged by an imaging unit;
A dividing unit that extracts an edge from the acquired distance image and divides the distance image into partial region groups surrounded by the edge;
From the divided partial area group, based on the distance value of the distance image, search for a partial area that is considered to be closest to the imaging means, and setting means for setting the partial area as an object area;
Estimating means for estimating the position and orientation of the target object by fitting the surface of the object model obtained by modeling the target object to the set three-dimensional point group of the object region;
Compare the information indicating the distance between the object model and the object region, and the information indicating the distance between the partial region group not set as the object region and the object model in the partial region group. According to the result, a determination means for determining whether to add a new partial area to the object area,
An update unit for adding a new partial region to the object region when the determination unit determines to add a new partial region to the object region;
When the update is performed by the update unit, the estimation unit includes a control unit that estimates the position and orientation of the target object using a three-dimensional point group of the updated object region. To do.

  According to the present invention, it is possible to provide a position / orientation measurement technique that improves processing speed and suppresses memory consumption by limiting the distance image data to be subjected to position / orientation estimation processing to the target object region. .

The figure explaining the hardware constitutions of the position and orientation measurement apparatus concerning embodiment of this invention. The figure explaining the process of the position and orientation measurement method concerning 1st Embodiment. The figure explaining the process of the area | region division | segmentation process of the distance image in 1st Embodiment. The figure explaining the process of the model fitting process in 1st Embodiment. The figure explaining the process of the object area | region update process in 1st Embodiment. The figure explaining the estimation of the position and orientation of one object from the distance image in which the several object was image | photographed illustratively. The figure explaining the flow of a process of the position and orientation measurement method concerning 2nd Embodiment. The figure explaining the flow of the object area update process concerning 2nd Embodiment.

(First embodiment)
In the present embodiment, it is assumed that the position and orientation of an object is measured in the operation of a robot that picks up and picks up objects one by one with a robot hand from a group of objects having the same shape in a piled state. For this purpose, a distance image is taken from above the object group. Although the distance values to a plurality of objects are recorded in the captured distance image, the position and orientation of the object are measured while detecting the area occupied by the object at the top of the distance image.

  The outline of the processing in this embodiment is as follows. First, the distance image is divided into partial regions estimated to belong to the same plane. From the divided areas, start with the area that has the smallest distance value, that is, the area at the top, and apply the shape model of the object while adding the area that is estimated to belong to the object to be grasped. Therefore, the position and orientation of the target object are estimated. At this time, the region to be added is selected so as to reduce the error when fitted to the model. With this selection method, it is possible to reduce the risk that data of a plurality of objects (non-target object data) is included in the selected region group.

  In the following description, “partial region” refers to each region obtained by dividing a distance image into regions estimated to belong to the same plane. The “object region” is a region estimated to belong to the target object on the distance image, and is a region composed of at least one partial region.

  A hardware configuration of a position and orientation measurement apparatus according to an embodiment of the present invention will be described with reference to FIG. The CPU 101 controls the operation of the entire apparatus. The memory 102 stores programs and data used for the operation of the CPU 101. The bus 103 manages data transfer between the component modules. The bus 103 and various devices are connected to the interface 104. The external storage device 105 stores programs and data to be read into the CPU 101. The keyboard 106 and the mouse 107 function as input devices for starting a program and designating the operation of the program. The display unit 108 is a display unit for displaying the operation result of the process. The data input / output unit 109 can transmit / receive data to / from an external device. The distance image measurement device is connected to the position / orientation measurement device via the data input / output unit 109.

  A processing flow of the position and orientation measurement method according to the first embodiment will be described with reference to FIG. This process is executed under the overall control of the CPU 101. In step S201, initialization of data used for processing is performed. Specifically, the shape model data is loaded from the external storage device 105 into the memory 102. In addition, the camera parameters (principal point position, focal length, image distortion coefficient) of the imaging unit of the range image measuring device, and the pixel value and range value conversion ratio of the range image are not shown connected to the data input / output unit 109. It inputs from the distance image measuring device.

  The shape model of the object is an implicit polynomial model, and is defined by the equation shown in (Expression 1). Here, x = (x, y, z) is the coordinates of a point in the three-dimensional space, and n is the order of the implicit polynomial. The coordinates (x, y, z) at which the expression of (Equation 1) becomes “0” represents the surface of the object represented by the implicit polynomial model. The coordinates at which the expression of (Equation 1) is greater than “0” represent the outside of the target object. The coordinates at which the formula (Equation 1) is smaller than “0” represent the inside of the target object.

  This implicit polynomial model is generated in advance using a method disclosed in Non-Patent Document 4 or Non-Patent Document 5 from a three-dimensional point group measured by a CAD model of a target object, a range finder, or the like. Non-Patent Document 4 discloses a method of constructing a three-dimensional model expressed by an implicit polynomial. It is difficult to construct an implicit polynomial model so as to appropriately represent only the target three-dimensional shape. In this method, there are three sets of points: a point on the surface of the target shape, a point that is a certain distance c from the surface of the target shape, a point that is a distance c from the surface of the target shape, and the value of the corresponding implicit polynomial The coefficient of the implicit polynomial is determined so that the square of is minimized. Non-Patent Document 5 discloses a method for more stably obtaining an implicit polynomial having a complicated shape by using ridge regression in the method disclosed in Non-Patent Document 4.

  The camera parameters are calibrated in advance by the method disclosed in Non-Patent Document 6, for example. By using the camera parameters input in step S201, the three-dimensional coordinates in the imaging unit coordinate system of the points in the three-dimensional space corresponding to the pixels are calculated from the two-dimensional coordinates and pixel values of each pixel of the distance image. Can do.

  In step S <b> 202, distance image data (hereinafter also simply referred to as “distance image”) measured by a distance image measurement device (not shown) connected to the data input / output unit 109 is acquired and stored in the memory 102. Note that any means for photographing a distance image can be used. For example, it is possible to photograph by a light cutting method or a stereo method.

  In the next step S203, the distance image is divided into partial areas. Details of the processing contents of this step will be described later. The division result is stored in the memory 102 in the form of a two-dimensional map (hereinafter referred to as “partial region map”) in which positive integer values are recorded. The size of the partial area map is equal to the number of vertical and horizontal pixels of the distance image. The positive integer value recorded in the partial area map is the same value for the same partial area, and different partial areas have different values. That is, what is recorded in the partial area map is the identification number of the partial area to which each pixel of the distance image belongs. Therefore, by referring to the value of the partial area map, it is possible to uniquely determine which partial area the pixel on the distance image corresponding to the position on the referenced map belongs.

  In step S204, the partial area group extracted in the previous step S203 is searched for a partial area that commonly includes the pixel having the smallest distance value, and is set as an object area (initial object area). As a result, the partial area existing at the uppermost position (side closer to the distance image capturing apparatus) becomes the object area. Thereafter, an object including the partial area set as the object area in this step is set as a position / orientation measurement target. The data of the object area is recorded in the memory 102 as a list of identification numbers of partial areas belonging to the object area. After the process of this step is completed, the object area data includes the identification number of at least one partial area.

  In step S205, the position and orientation of the measurement target object is estimated by fitting an implicit polynomial model representing the shape of the object to the distance measurement point group in the object region. Details of this step will be described later.

  In step S206, it is determined whether or not a new partial area is added to the set object area. If the determination result is true, the object area is updated. Details of the processing in this step will also be described later.

  In step S207, it is determined whether or not the object region data has been updated in the previous step S206. If there is an update (S207-Yes), the process returns to step S205 to estimate the position and orientation of the target object again. If there is no update of the object area data (S207-No), it is determined that the object area is confirmed and the position / orientation estimation of the target object is completed, and the entire process is terminated.

(Distance image region segmentation process)
Details of the distance image region dividing step (step S203) will be described with reference to FIG. In this step, based on the method disclosed in Non-Patent Document 7, the distance image is divided for each closed region surrounded by an edge described later. The detected area is defined as a partial area.

  When the process is started, an edge is extracted in step S301. Here, the “edge” is a “jump edge” in which the distance value changes discontinuously or a “crease edge” in which the normal direction of the object surface changes discontinuously. Specifically, first, a curve on the subject corresponding to a straight line (referred to as a “scan line”) on the distance image is divided into a plurality of quadratic curve segments. Then, for each boundary point of the curved segment, calculate the “edge strength” (the discontinuity of the depth value that represents the jump edge, the discontinuity in the normal direction of the segment that represents the crisp edge), and the edge strength. A point whose length is greater than or equal to the threshold is extracted as an edge. Since the direction of the edge varies, the scan line has four directions of vertical, horizontal, and diagonal directions (45 °, 135 °) of the distance image, and the connection having the highest edge strength in the four directions. A point is an edge. As described above, an edge map representing the position of the edge pixel on the distance image is created and recorded in the memory 102.

  Next, in step S302, the edge map created in the previous step S301 is labeled. As a result, the distance image is divided into a closed region group delimited by the connected edge pixels. The division result is recorded in the memory 102 as a label image having the same size as the distance image. The label value is uniquely set for each closed region. In the subsequent step S303, a region of area T (“T” is an area threshold of the partial region) or more is selected from the closed regions detected in the previous step S302, and a list of label values of the selected closed region (hereinafter referred to as “closed region”). This list is called “area list”) and recorded in the memory 102.

  In step S304, it is determined whether or not the number of closed areas registered in the area list is “0”. If the number of closed regions is “0” (S304—Yes), the process proceeds to step S312. On the other hand, if the number of closed regions is not “0” (S304—No), the process proceeds to step S305. In step S305, the unprocessed closed region R is deleted from the closed region group registered in the region list, and then a three-dimensional plane or a three-dimensional curved surface is fitted to the closed region R (step S306).

  In step S307, it is determined whether or not the fitting error when fitting a flat surface or curved surface in step S306 is less than a predetermined threshold value. If it is less than the threshold value (S307-Yes), it is estimated that the closed region R does not include a plurality of surfaces, so it is registered as a partial region (S308), and the process proceeds to step S312. On the other hand, if it determines with more than a threshold value (S307-No), a process will be advanced to step S309. The “fitting error” refers to the sum of squares of the distances between the three-dimensional points corresponding to the respective pixels in the closed region R and the nearest point at which the distance on the fitted plane or curved surface is the smallest. This is an average of all R pixels. If it is determined in step S307 that the fitting error is greater than or equal to the threshold value, the closed region R may include a plurality of surfaces. This is presumed to be because the edges existing at the boundaries of a plurality of surfaces in the closed region are interrupted due to the influence of noise or the like included in the distance image. In order to connect a plurality of surfaces by connecting the broken edges in this way, in step S309, the edge map is subjected to expansion processing. In step S310, the edge map is labeled, and in step S311, closed region candidates are extracted. The processing contents of steps S310 and S311 are the same as those of steps S302 and S303, respectively.

  On the other hand, when it is determined in step S307 that the fitting error is less than the threshold value, it is estimated that the closed region R includes only a single surface. Therefore, in step S308, the closed region R is registered as a partial region. Finally, in step S312, unregistered pixels such as edge pixels are integrated into the surrounding closed areas. Even in this step, a flat surface or a curved surface is fitted, and if the fitting error is less than the threshold value, they are integrated.

(Process of model fitting process)
Next, details of the model fitting step (step S205) will be described with reference to FIG. In this step, the position and orientation are calculated by repeatedly correcting the approximate values of the position and orientation of the measurement object by iterative calculation. In step S401, approximate values of the position and orientation of the target object with respect to a distance image measurement device (not shown) connected to the data input / output unit 109 are estimated. This processing can be realized by an object recognition method disclosed in Japanese Patent Application Laid-Open No. 01-009307 using distance image data of the object region as an input. However, the method for obtaining the approximate value of the position and orientation of the target object is not limited to this. For example, if the model fitting process has passed once or more, a value representing the position and orientation of the target object calculated in the process is stored in the memory 102, and the value is stored in the next model fitting process. It can be used as a value of the approximate position and orientation. Alternatively, the speed and angular velocity of the target object are estimated from past measurement values by time-series filtering such as a linear filter or Kalman filter, and the position and orientation of the target object at the current time are predicted based on the estimated speed and angular velocity. May be used. In addition, when the position and orientation of an object can be measured by other sensors, output values from the sensors may be used as approximate values of the position and orientation. The sensor may be, for example, a magnetic sensor that measures the position and orientation by detecting a magnetic field generated by the transmitter with a receiver attached to the object. Moreover, the optical sensor which measures a position and attitude | position by image | photographing the marker arrange | positioned on a target object with the camera fixed to the scene may be sufficient. In addition, any sensor can be used as long as it measures the position and orientation with six degrees of freedom. In addition, when the approximate position or posture where the target object is placed is known in advance, the value may be used as an approximate value.

  In step S402, using the approximate values of the position and orientation of the target object, the coordinates in the three-dimensional space corresponding to each pixel in the object region are converted from the imaging unit coordinate system of the distance image measurement device to the coordinate system of the object model. To do. In the following description, the point whose coordinates are calculated as a result of the coordinate conversion in this step will be referred to as a “three-dimensional point of the object region”.

  In step S403, based on the distance field constructed from the implicit polynomial model and the coordinates of the three-dimensional point group of the object region, the sum of the distances between the three-dimensional point group and the surface of the object model modeling the target object is minimized. Find the position and posture to be. Specifically, the position and orientation are optimized so that the energy function shown in (Expression 2) is minimized.

  Here, Ω is a three-dimensional point group of the object region, and x∈Ω. Further, dist is a distance calculated from an implicit polynomial model, and is defined as (Equation 3) having “0” on the model surface, a negative value inside, and a positive value outside.

Specifically, the optimization of the position and orientation is based on the steepest descent method to obtain the three-dimensional point group coordinate X ^{k + 1} from the gradient according to the equation (Equation 4) (Equation 6). Here, X is X∈R ^{N × 3} and represents the coordinates of the three-dimensional points of the N object regions.

A matrix shown in (Expression 7) is created from the original three-dimensional point group coordinates X ^k and the obtained X ^{k + 1,} and singular value decomposition is performed as A = USV ^T , and approximate values of position and orientation are obtained. Update. Here, X ⁻ represents the average of X, t is the position of the target object, and R is a matrix representing the posture.

  Finally, in step S404, a convergence determination is made. If the convergence has been achieved (S404-Yes), the entire model fitting process is terminated. If not converged, the process returns to step S402, and the same process is repeated. In the convergence determination, it is determined that the convergence has occurred when the value of the equation (Equation 4) is approximately 0, or when the difference between the square sums of the error vectors before and after the update regarding the position and orientation is approximately 0.

  In the above description, the steepest descent method is used for the energy optimization process, but the present invention is not limited to this. Convergence determination may be performed by the Gauss-Newton method or the Levenberg-Marquardt method, which have faster convergence. In addition, other nonlinear optimization calculation methods such as a conjugate gradient method may be used for convergence determination.

(Process of object area update process)
Details of the object region updating step (step S206) will be described with reference to FIG. First, in step S501, an average distance from the implicit polynomial model of the object defined by (Equation 10) is calculated for the three-dimensional point group of the object region. Here, N is the number of three-dimensional points in the object region, and Φ is a three-dimensional point group in the object region.

However, N is the number of three-dimensional points in the area on the distance image to be calculated, and e _ii is obtained by the following (Equation 11).

  Here, Φ is a three-dimensional point group in the region on the distance image to be calculated.

  Next, in step S502, a partial area candidate C to be added to the object area is selected from the partial areas not included in the object area. Specifically, the barycentric position of the three-dimensional point group corresponding to the pixel group belonging to each of the object region and each partial region not included in the object region is calculated. Then, the distance value between the gravity center position relating to the object area and the gravity center position relating to each partial area is calculated, and n partial areas are selected as partial area candidates C to be added to the object area in order of increasing distance value. Note that n is a positive integer of 1 or more.

  In step S503, one unprocessed partial area is selected from the candidates C. The partial area selected here is represented by a. In the subsequent step S504, an evaluation value for determining whether or not to add the partial area a to the object area is calculated. Specifically, for the partial area a, an average distance between the implicit polynomial model representing the object represented by (Equation 10) and the partial area is calculated. Here, N is the number of three-dimensional points in the partial region a, and Φ is a three-dimensional point group in the partial region a.

  In step S505, the average distance obtained in step S501 multiplied by a predetermined ratio (> 1.0) is compared with the average distance obtained in step S504. If the latter is less than the former (S505-Yes), the process proceeds to step S506. On the other hand, if it is determined that the latter is greater than or equal to the former (S505-No), the process proceeds to step S507.

  In step S506, the partial area a is added to the object area. That is, the identification number of the partial area a is added to the list of identification numbers representing the partial area group included in the object area. In step S507, it is determined whether or not all the partial areas included in the partial area candidate C to be added have been processed. If the processing has been completed (Yes in S507), the object area update process ends. On the other hand, if there is an unprocessed partial area (S507-No), the process returns to step S503, and the same process is repeated. In this manner, the object region is updated by repeating the process of determining whether or not to add all the additional candidate partial regions to the object region.

  With reference to FIG. 6, estimation of the position and orientation of one object from a distance image obtained by shooting a plurality of objects will be described as an example. However, the number n of object region candidates to be added to the object region is 1. FIG. 6A shows an image of a target object for photographing a distance image, and includes two objects having the same shape. FIG. 6B shows a state in which the distance image obtained by photographing these objects is divided into partial areas. In order to distinguish and display different areas, the paint pattern is shown to be different for each area. The numbers displayed in FIG. 6B are identification numbers set for each partial area. Of these, number (6) corresponds to the background area. FIGS. 6C to 6E show each step of updating the object region, and the hatched portion corresponds to the object region. FIG. 6C shows the initial object region (the state after the end of step S204), and the partial region (1) including the measurement point closest to the distance image capturing device is the object region. Has been extracted. Next, when the object region update process (step S206) is performed, the object region becomes as shown in FIG. 6D, and the partial region (2) is added to the initial object region. Similarly, after the second object region updating step (step S206), a partial region (3) is further added as shown in FIG. 6E, and the object region is divided into three partial regions (partial region (1)). To (3)). When the process further proceeds from the state of FIG. 6E to the object region update step (step S206), it is determined whether or not the partial regions (4) and (5) are added to the object region. However, since the partial areas (4) and (5) are areas on an object different from the object to which the partial areas (1) to (3) belong, the average distance from the model calculated in step S504 is a relatively large value. Take. Therefore, it is determined that the partial areas (4) and (5) are not added to the object area. Through the above process, an object including the partial area (1) to the partial area (3) is finally extracted from the distance image, and its position and orientation can be obtained.

  According to the present embodiment, by limiting the distance image data to be subjected to the position / orientation estimation process to the area of the target object, the processing speed can be improved and the memory consumption can be suppressed.

(Modification 1-1-1)
The embodiment of the present invention is not limited to the above, and various modifications exist. In the first embodiment, in step S206, a partial area having a centroid near the centroid of the three-dimensional point group in the object area is selected as a candidate for an area to be added to the object area. However, the method of selecting the partial region candidate to be added is not limited to this. For example, a partial region having a centroid in the vicinity of the centroid of the three-dimensional point group in the initial object region set in step S204 may be selected. Or you may select the partial area | region where the gravity center of the two-dimensional coordinate on a distance image is close instead of the gravity center of a three-dimensional coordinate.

(Modification 1-1-2)
In the first embodiment, a partial region having a high degree of proximity to the object region is selected as a candidate to be added to the object region. However, the method for selecting a partial region as an additional candidate is not limited to this. For example, a partial region having a large area, that is, a large number of pixels belonging to the region may be selected as a candidate. This is because areas with a larger area are less susceptible to random errors such as distance image measurement noise, so it is possible to calculate the distance from the shape model and fit the model after updating the object area with high accuracy. It is.

(Modification 1-1-3)
As another partial region candidate selection method, for example, a method using information on a region occupied by a target object on a distance image can be cited. Specifically, based on the position and orientation of the target object calculated in the model fitting process, the shape model is projected on the distance image, and the area where the projection image exists is set as the area occupied by the target object. The partial areas to be added to the object area in descending order of the overlapping ratio between the partial areas and the area occupied by the target object (that is, the value obtained by dividing the number of pixels in which both areas overlap by the number of pixels belonging to each partial area). It is also possible to select as a candidate.

(Modification 1-1-4)
Furthermore, the criterion for selecting a partial region as an additional candidate is not necessarily one, and a plurality of criteria may be combined. For example, a value obtained by weighting and adding three types of evaluation values of the proximity to the object region, the area, and the overlapping rate with the target object region on the distance image described in Modification 1-1 to Modification 1-3 It is also possible to select candidate partial regions based on

(Modification 1-2-1)
In the update process of the object region (FIG. 5), it is determined whether or not the partial region can be added to the object region by comparing the average distance between the object region and the shape model related to the partial region. However, the method for determining whether or not to add is not limited to this. For example, if a partial region with a small average distance is preferentially added, another method can be applied. For example, a partial area having a minimum average distance may be added to the object area in the partial area group of addition candidates.

(Modification 1-2-2)
There is not necessarily one criterion for determining whether or not a partial area can be added, and a plurality of criteria may be combined. For example, four types of evaluation values of the proximity to the object region, the area, the overlapping rate with the target object region on the distance image, and the average distance to the shape model described in Modification 1-1 to Modification 1-3 It is also possible to determine whether or not addition is possible based on a value obtained by adding a weight to.

(Modification 1-2-3)
Further, there is no necessity to add partial areas one by one, and a plurality of partial areas may be added to the object area at the same time. In that case, it is also possible to select from a candidate group of partial areas to be added in order to add a plurality of partial areas whose evaluation values (for example, the average distance between the three-dimensional point cloud of the partial area and the model) to the object area. is there.

(Modification 1-3-1)
In the first embodiment, the method of Non-Patent Document 7 is used to divide the distance image into partial regions. However, the method for generating the partial region is not limited to this, and may be a method based on Non-Patent Document 8, for example. This method selects a parametric surface type that approximates a partial area of a distance image based on a predetermined evaluation criterion (a mathematical expression of the surface is different for each type), and represents the distance image on the selected type of surface. It approximates a three-dimensional shape. According to this method, a region with a small approximation error can be extracted as a continuous surface.

(Modification 1-3-2)
Instead of dividing the distance image according to the distance data, it may be divided by a predetermined dividing method. For example, it may be divided for each pixel, or the distance image may be divided into a grid pattern for every 10 pixels. When the predetermined dividing method is used as described above, it is possible to omit the area dividing step (step S203) of the distance image.

(Modification 1-4-1)
In the first embodiment, the partial area including the pixel having the smallest distance value is selected as the partial area included in the initial object area. However, the method of setting the initial object area is not limited to this, and it is only necessary to select a partial area that is considered to be closest to the imaging device and may be an area on the object that is arranged at the uppermost position. . For example, the partial area with the smallest average distance value may be selected, or the partial area with the smallest average distance value of the top m pixels (m is a positive integer of 2 or more) in ascending order of the distance value. It is also possible to do.

(Modification 1-4-2)
The reason why the partial area with a small distance value is given priority as the initial object area is because it is assumed that the object is imaged from above and is gripped by the robot hand from above. If the imaging conditions are different, it is possible to adopt an initial object region setting method suitable for the imaging conditions. For example, if the position and orientation of an object of a specific color is estimated, a color image is captured using a different second imaging device in addition to the distance image. Then, an area on the distance image corresponding to the area on the color image that most closely matches the specific color is searched, and a partial area including the area on the distance image may be set as the initial object area.

(Modification 1-4-3)
If the approximate position and orientation of the target object are known in advance, the coordinates of the three-dimensional point cloud and the average distance represented by (Equation 10) are calculated for each partial area, and the partial area with the minimum average distance is determined as the initial object. It may be an area.

(Modification 1-4-4)
If the approximate position and orientation of the target object are known in advance as in Modification 1-4-3, the initial object region can be determined using information on the region occupied by the target object on the distance image. Specifically, based on the approximate values of the position and orientation of the target object, the shape model is projected on the distance image, and the area where the projection image exists is set as the area occupied by the target object. Then, select the partial area with the highest overlap ratio between each partial area and the area occupied by the target object (the number of pixels that overlap both areas divided by the number of pixels belonging to each partial area) as the initial object area. It is also possible to do.

(Modification 1-4-5)
Further, the number of partial areas included in the initial object area is not limited to one, and may be plural.

  For example, a predetermined number of partial areas are selected as object areas in order of increasing average distance value (Modification 1-4-1) and in descending order of matching with a specific color (Modification 1-4-2). It is also possible. It is also possible to select a predetermined number of partial areas as the object area in the order of decreasing average distance (Modification 1-4-3) and in order of increasing overlap ratio (Modification 1-4-4-).

(Modification 1-5)
In the first embodiment, the average distance between the implicit polynomial model defined by (Equation 10) and the three-dimensional point group is used as an evaluation value for purposes such as determining whether to add a partial region. However, the evaluation value used as the determination criterion is not limited to this, and may be a value indicating the degree of deviation between the object model and the measurement point group. For example, instead of the average distance, evaluation values represented by (Equation 12) and (Equation 13) may be used.

  Here, N is the number of three-dimensional points in the area on the distance image to be evaluated, and Φ is a three-dimensional point group in the area.

Here, N is the number of three-dimensional points in the area on the distance image to be evaluated, and Φ is a three-dimensional point group in the area. These evaluation values D _RMS ( _Equation 12) and D _average ( _Equation 13) represent the degree of coincidence of the surface positions of the shape model and the three-dimensional point group.

  Also according to the above modification, by limiting the distance image data to be subjected to the position / orientation estimation process to the area of the target object, the processing speed can be improved and the memory consumption can be suppressed.

(Second Embodiment)
In the first embodiment, before adding a partial area to the object area, it is determined whether or not the partial area is an area corresponding to the target object. The configuration for processing as part of the area has been described. In this embodiment, a configuration is described in which it is determined whether or not a partial area that has been added to the object area is a partial area corresponding to a non-target object, and if the determination result is true, the partial area is deleted from the object area. To do.

  With reference to FIG. 7, the process flow of the position and orientation measurement method according to the second embodiment will be described. This process is executed under the overall control of the CPU 101. The processing from step S701 to S704 is the same as that from step S201 to step S204, respectively.

  Step S705 is the same as step S205, but at the stage of calculating the position and orientation of the object, the average distance between the measurement point represented by (Equation 10) and the object model is calculated with respect to the object region and stored in the memory 102. . At this time, the average distance data calculated in the previous object model fitting process and stored in the memory 102 is copied to another storage area in the memory 102 without being overwritten.

  In step S706, the partial area included in the object area is updated. In the process of this step, a partial area is added to the object area, or the added partial area is excluded from the object area. This step will be described in detail later.

  In step S707, it is determined whether or not the partial area is excluded from the object area in step S706. If it is determined that the object has been excluded (S707-Yes), the entire object position / orientation estimation process is terminated. Here, the processing is terminated because it is assumed that all partial areas corresponding to the target object have been selected as object areas, and the most recently added partial area corresponds to a non-target object.

  On the other hand, if it is determined in step S707 that the partial area is not excluded (S707-No), the process returns to step S705, and the target object is obtained by fitting the object shape model to the three-dimensional point cloud of the object area. Update the position and orientation.

  The flow of the object area update process according to the second embodiment will be described with reference to FIG. When the process is started, the average distance between the three-dimensional point cloud of the object region and the object model is compared with the values calculated in the latest and previous object model fitting steps (step S705), and the average distance increases from the previous time. Whether to increase or not is determined. If the distance has increased (S801-Yes), the process proceeds to step S802. On the other hand, if it is determined that the distance has not increased (S801-No), the process proceeds to step S803.

  In step S802, the partial area added to the object area most recently is removed from the object area. That is, the identification number of the partial area to be removed is deleted from the list of identification numbers representing the partial area group included in the object area.

  In step S803, a partial area selected from the partial areas not included in the object area is added to the object area. This process is the same as the flow shown in the flowchart of FIG.

  By performing the processing described above, the validity of the partial area selected to be added to the object area, that is, the degree to which the partial area corresponds to the target object, can be confirmed by the fitting error when the object model is fitted. Can do. As a result, compared to the configuration of selecting a partial region to be added with an evaluation value for each partial region as in the first embodiment, the risk of erroneously selecting a partial region corresponding to a non-target object as an object region is reduced. It is possible to determine the object region more accurately.

  In the above description, an example is described in which the average distance represented by (Equation 10) is used as a criterion for determining whether or not to delete the added partial region. However, the present invention is not limited to this example. . For example, any value can be used as long as it is a value that increases in proportion to the fitting error of the object model and is normalized so as not to depend on the size of the object region. For example, a value obtained by averaging the size of the error vector after the position and orientation update in the process of step S403 (FIG. 4) with pixels included in the object region may be used as the reference value.

  Further, in the above description, when the partial area is deleted from the object area, the entire process is completed. However, the update of the object area may be further continued. In this case, the excluded partial region may be excluded from candidates to be added as the object region so that the excluded partial region is added to the object region again in the next object region updating step and does not fall into an infinite loop. Alternatively, a process of lowering the selection order (for example, setting the lowest position) among the partial area candidates to be added to the object area may be added.

  According to the present embodiment, by limiting the distance image data to be subjected to the position / orientation estimation process to the area of the target object, the processing speed can be improved and the memory consumption can be suppressed.

(Modification 2-1)
The embodiment of the present invention is not limited to the above, and various modifications exist. In the second embodiment, the criterion for determining whether or not to exclude a partial region from the object region is an increase or decrease in the average distance. However, the determination criterion is not limited to this, and it is only necessary to be able to evaluate a change in the accuracy of model fitting before and after adding a partial region. For example, the average distance (A) related to the partial area most recently added to the object area is compared with the average distance (B) related to the object area. If the former (A) is greater than or equal to the latter (B), the most recently added part It is also possible to determine to delete the area. Conversely, if the former (A) is less than the latter (B), the partial area is not deleted.

(Modification 2-2)
Alternatively, instead of the average distance related to the object area, the average distance related to the area excluding the partial area added most recently from the object area may be used.

(Modification 2-3)
Further, instead of the average distance, it may be determined whether or not to exclude the partial region using the evaluation values described in the modified examples 1-4-1 to 1-4-5 of the first embodiment.

(Third embodiment)
In the first and second embodiments described above, the position and orientation of an object are estimated by fitting a shape model of the object expressed by an implicit polynomial to a three-dimensional point group of the object region. However, the position and orientation estimation method is not limited to this, and any method can be used as long as it is based on the shape model of the object and the information of the three-dimensional point group of the object region.
In the present embodiment, the shape model is a patch model composed of triangular plane patch groups, a pair of corresponding points is obtained between this model and the three-dimensional point group of the object region, and the distance between the pair of corresponding points is determined. The position and orientation of the object are optimized so that the sum is minimized.

  The overall processing flow of this embodiment is the same as the flowchart of FIG. However, the shape model data input from the external storage device 105 in step S201 includes the identification number and coordinates of each vertex of the triangular patch, the identification number of each triangular patch, and the identification number of the vertex constituting the patch. To do.

  In the position / orientation calculation step (step S403), a nearest point pair consisting of a set of points having the smallest distance is obtained from the three-dimensional point group of the object region and the points on the shape model. The obtained nearest neighbor point pairs are stored in the memory 102 as a list of nearest neighbor point pairs.

Specifically, sample points that are candidates for the nearest point pair are selected from a three-dimensional point group in the object region. The sample points may be selected for each pixel in the object region on the distance image, or may be selected by thinning out the image region for each predetermined number of pixels (for example, a plurality of pixels such as 5 pixels). good. Next, the nearest point on the shape model corresponding to the sample point p 1 ^→ is obtained. The minimum distance d between the sample point p ^→ and the triangular patch t composed of the three vertices r ₁ ^→ , r ₂ ^→ , r ₃ ^→ is expressed as (Equation 14).

However, u∈ [0,1], v∈ [0,1], w∈ [0,1]. If the shape model T is composed of N triangular patches t ₁ ,... T _N , the distance between the sample point p ^→ and the nearest point on the shape model T is expressed as (Equation 15). Is done.

  That is, in order to obtain the nearest point of the sample point, a set of coefficients (u, v, w) representing the minimum distance between the sample point and each triangular patch and the coordinates of the nearest point are calculated. Then, a search is made for a pair (u, v, w) of a triangular patch and a coefficient that minimizes the minimum distance from the sample point among all the triangular patches. As a result of the search processing for the nearest point pair described above, the three-dimensional coordinates of the sample point, the identification number of the triangular patch where the nearest point on the shape model exists, and the coefficients (u, v, w) representing the coordinates of the nearest point The set is recorded in the memory 102 as a list of all sample points.

Next, the position and orientation of the model is calculated so that the sum of the distances between the nearest point pairs registered in the nearest point pair list is minimized, and the calculation result is set as the position and orientation of the target object. The three-dimensional coordinates in the coordinate system of the imaging unit of the distance image measuring device can be converted into the three-dimensional coordinates (x, y, z) in the coordinate system of the target object by the position and orientation s of the target object. Here, the degree of freedom of the position and orientation of the measurement target object is 6 degrees of freedom. That is, s is a six-dimensional vector, and includes three elements that represent the position of the measurement target object and three elements that represent the posture. It is assumed that a point in the imaging unit coordinate system is converted to a point (x ₀ , y ₀ , z ₀ ) in the target object coordinate system based on the approximate position and orientation of the target object calculated in step S403. (X, y, z) changes depending on the position and orientation of the target object, and can be approximated as (Equation 16) by the first-order Taylor expansion in the vicinity of (x ₀ , y ₀ , z ₀ ).

The equation in the target object coordinate system of the triangular patch surface where the geometric model side nearest point of the nearest neighbor point pair stored in the memory 102 exists is expressed as ax + by + cz = e (a ² + b ² + c ² = 1, a , b, c, e are constants). Assume that (x, y, z) transformed by the correct s satisfies the plane equation ax + by + cz = e (a ² + b ² + c ² = 1). Substituting (Equation 16) into the plane equation yields (Equation 17).

However, q = ax ₀ + by ₀ + cz ₀ (constant).

Since (Equation 17) holds for all nearest point pairs, a linear simultaneous equation regarding Δs _i as shown in (Equation 18) holds.

  Here, (Equation 18) is expressed as (Equation 19).

Based on (Equation 19), Δs is obtained using the generalized matrix (J ^T · J) ⁻¹ · J ^T of the matrix J. However, since there are many outliers due to noise included in the measurement point group of the distance image, a robust estimation method as described below is used. In general, the error e−q is large in the measurement point cloud data that is an outlier. Therefore, the degree of contribution to the simultaneous equations of (Equation 6) and (Equation 7) increases, and the accuracy of Δs obtained as a result decreases. Therefore, a small weight is given to data having a large error e−q, and a large weight is given to data having a small error e−q. The weight can be given by a Tukey function as shown in (Equation 20), for example.

c is a constant. Note that the function that gives the weight need not be a Tukey function. For example, any function that gives a small weight to data with a large error and gives a large weight to data with a small error may be used. Let w _i be the weight corresponding to each data. Here, a weight matrix W is defined as in (Equation 21).

The weight matrix W is a square matrix of “0” except for the diagonal component, and the weight w _i is placed in the diagonal component. N _c is the number of nearest point pairs (not including corresponding point pairs on the missing model region). Using this weight matrix W, (Equation 19) is transformed into (Equation 22).

  The correction value Δs is obtained by solving (Equation 22) as shown in (Equation 23).

  The approximate value of the model position / orientation is corrected with the correction value Δs calculated in this way, and the result is stored in the memory 102 or output to an external device via the data input / output unit 109. The above-described position and orientation optimization calculation method is based on the Gauss-Newton method, but is not limited thereto, and the Levenberg-Marquardt method, the steepest descent method, or the like may be used.

  In step S404, a convergence determination is performed. If the convergence has been achieved (S404-Yes), the position / orientation estimation process is terminated. If not converged (S404-No), the process returns to step S402, and the same process is repeated. In the convergence determination, it is determined that the convergence has occurred when the correction value Δs is approximately 0, or when the difference before and after correction of the square sum of the error vector E is approximately 0.

  The object region update processing is the same as that in the first embodiment, but the evaluation value for determining whether or not to add a partial region is not limited to the average distance represented by (Equation 10). For example, RMS error (the square root of a value obtained by dividing the sum of squares of the elements of the error vector by the number of sample points) can be used. The error vector related to the object area has been calculated in step S403. Further, the error vector related to the candidate partial region to be added to the object region can be calculated in the same manner as in the case of the object region described above.

  According to the present embodiment, by limiting the distance image data to be subjected to the position / orientation estimation process to the area of the target object, the processing speed can be improved and the memory consumption can be suppressed.

(Other examples)
The present invention can also be realized by executing the following processing. That is, software (program) that realizes the functions of the above-described embodiments is supplied to a system or apparatus via a network or various storage media, and a computer (or CPU, MPU, or the like) of the system or apparatus reads the program. It is a process to be executed.

Claims (8)

Obtaining means for obtaining a distance image of the target object imaged by the imaging means;
A dividing unit that extracts an edge from the acquired distance image and divides the distance image into partial region groups surrounded by the edge;
From the divided partial area group, based on the distance value of the distance image, search for a partial area that is considered to be closest to the imaging means, and setting means for setting the partial area as an object area;
Estimating means for estimating the position and orientation of the target object by fitting the surface of the object model obtained by modeling the target object to the set three-dimensional point group of the object region;
Compare the information indicating the distance between the object model and the object region, and the information indicating the distance between the partial region group not set as the object region and the object model in the partial region group. According to the result, a determination means for determining whether to add a new partial area to the object area,
An update unit for adding a new partial region to the object region when the determination unit determines to add a new partial region to the object region;
When the update is performed by the update unit, the estimation unit includes a control unit that estimates the position and orientation of the target object using a three-dimensional point group of the updated object region. Position and orientation measurement device.   The position / orientation measurement apparatus according to claim 1, wherein the determination unit selects and compares partial areas that are not set as the object area in the partial area group in ascending order of distance values.   When the information indicating the distance between the object model and the partial area that is not set as the object area by the setting unit in the partial area group is smaller than the information indicating the distance between the object model and the object area, The position / orientation measurement apparatus according to claim 2, wherein when the determination unit determines, the update unit adds a new partial region to the object region in ascending order of the distance value. The updating means includes
Whether the information indicating the distance between the object region after adding the partial region and the object model increases with respect to the information indicating the distance between the object region before adding the partial region and the object model An increase determination means for determining
An adding means for adding a new partial area in ascending order of the distance value with respect to the object area when it is determined by the increase determining means to increase;
The position / orientation measurement apparatus according to claim 2, further comprising: The updating means includes
When the information indicating the distance between the object region after adding the partial region and the information indicating the distance between the object model and the object model before the partial region is added increases, the increase determination is performed. The position / orientation measurement apparatus according to claim 4, further comprising a deletion unit that deletes the added partial region from the object region when determined by the unit.   The estimation means applies the object model to the three-dimensional point group of the object region so as to minimize the sum of the distances between the three-dimensional point group of the object region and the surface of the object model, The position and orientation measurement apparatus according to claim 1, wherein the position and orientation of the target object is estimated. An acquisition step in which the acquisition unit acquires a distance image of the target object imaged by the imaging unit;
A dividing step of extracting an edge from the acquired distance image and dividing the distance image into a partial region group surrounded by the edge;
A setting step in which a setting unit searches a partial region considered to be closest to the imaging unit based on a distance value of the distance image from the divided partial region group, and sets the partial region as an object region; ,
An estimation step in which the estimation means estimates the position and orientation of the target object by fitting the surface of the object model obtained by modeling the target object to the set three-dimensional point group of the object region;
The determination unit compares information indicating a distance between the object model and the object region with information indicating a distance between the partial region group that is not set as the object region and the object model. A determination step of determining whether or not to add a new partial region to the object region based on the result of the comparison;
When it is determined in the determination step that a new partial region is added to the object region, the update unit includes an update step of adding a new partial region to the object region. In this case, in the estimation step, the position and orientation measurement method estimates the position and orientation of the target object using a three-dimensional point group of the updated object region.   A program for causing a computer to function as each unit of the position and orientation measurement apparatus according to any one of claims 1 to 6.

Images