JP6180158B2 - Position / orientation measuring apparatus, control method and program for position / orientation measuring apparatus - Google Patents

Description

  The present invention relates to a position / orientation measurement apparatus, a control method for the position / orientation measurement apparatus, and a program, and more particularly to a technique for measuring the position / orientation of an object having a known three-dimensional shape.

  Along with the development of robot technology in recent years, robots are instead performing complicated tasks that have been performed by humans, such as assembly of industrial products. Such a robot performs assembly by gripping a part with an end effector such as a hand. For this assembly, it is necessary to measure the relative position and orientation between the part to be gripped and the robot hand.

  As a method of measuring the position and orientation, it is obtained by measuring characteristic data detected from a grayscale image or a color image (hereinafter collectively referred to as “two-dimensional image”) captured by a camera, or by measuring the image with a distance sensor. There is a method by model fitting that applies a three-dimensional shape model of the object to the distance data. In model fitting for a two-dimensional image, the position and orientation are measured so that a projected image obtained when a three-dimensional shape model is projected on the image based on the position and orientation of the object is applied to the detected feature. In model fitting for distance data, each point of the distance image representing the distance data is converted to a 3D point cloud with 3D coordinates, and the 3D shape model is applied to the 3D point cloud in 3D space. Measure posture.

  Further, Patent Document 1 discloses a technique for measuring a position and orientation by using measurement information obtained from a two-dimensional image and measurement information obtained from a distance image in combination. A distance image can be stably measured on a flat portion where an image feature is difficult to detect on a two-dimensional image. On the other hand, many features (edges) are detected on a two-dimensional image in a portion where the accuracy of distance measurement is low, such as a boundary between a background and an object or a change of surface. Therefore, the position and orientation can be measured with high accuracy by increasing the amount of information by combining the two in a complementary manner.

  As a method for obtaining distance data for use in model fitting for distance data, each pixel from an image obtained by projecting a plurality of measurement patterns onto a measurement target object (hereinafter referred to as “distance measurement image”). There is a method of generating a distance image by obtaining a distance value of. In such a distance image measurement method, a distance value is calculated based on the correspondence of pixels between a plurality of captured images.

Japanese Patent Application No. 2009-175387

R. Y. Tsai, “A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses”, IEEE Journal of Robotics and Automation, vol.RA-3, no.4, 1987. 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 Seiji Iguchi, Kosuke Sato, “Three-dimensional image measurement” (Akirado, 1990)

  However, in order to acquire a distance image, it is necessary to perform imaging by projecting a plurality of measurement patterns while the measurement target object is stationary. Therefore, the time required for the imaging process compared to imaging of a normal two-dimensional image Becomes longer by the required number of sheets. Therefore, in this method, if the geometric relationship between the imaging apparatus and the measurement target object changes during a plurality of imaging processes, the correct pixel correspondence cannot be obtained, and there is a problem in the accuracy of the obtained distance image. Sometimes.

  For this reason, it is difficult to obtain a reliable distance image when the imaging apparatus moves or when the measurement target moves on the belt conveyor. In such a situation, if position and orientation measurement is performed using the information of the distance image, the measurement accuracy is degraded.

  In view of the above-described problems, the present invention provides a position / orientation in model fitting to a distance image generated from a plurality of images even when there is a change in the geometric relationship between the imaging device and the measurement target between the plurality of images. The purpose is to measure with high accuracy.

The position and orientation measurement apparatus according to the present invention that achieves the above object is as follows.
Model holding means for holding model data of the target object;
Illumination means for irradiating the target object with a pattern for distance measurement;
An imaging unit that captures the target object that is not irradiated with the distance measurement pattern and acquires a two-dimensional image; and that captures the target object that is irradiated with the distance measurement pattern and acquires a distance measurement image; ,
A distance image processing means for generating a distance image from a distance measurement image group obtained by irradiating and imaging each of the plurality of distance measurement patterns;
Detecting means for detecting a change in a geometric relationship between the target object and the imaging means;
Position and orientation calculation means for calculating the position and orientation of the target object based on at least one of the two-dimensional image and the distance image and the model data in accordance with a detection result by the detection means;
It is characterized by providing.

  According to the present invention, in the method of acquiring the distance image from a plurality of images and measuring the position and orientation of the target object, even when the positional relationship between the imaging device and the target object changes during the acquisition of the distance measurement image. Measurement can be performed with high accuracy.

The figure which shows the structure of the position and orientation measuring apparatus which concerns on 1st Embodiment. The figure explaining the three-dimensional shape model which concerns on 1st Embodiment. The flowchart of the process which the position and orientation measurement apparatus which concerns on 1st Embodiment implements. The figure explaining the edge detection from an image. The figure explaining the relationship between the projection image of a line segment, and the detected edge. The figure explaining the code | cord pattern which an illumination part irradiates. The figure which shows the structure of the position and orientation measuring apparatus which concerns on 2nd Embodiment. The flowchart which shows the procedure of the process which the position and orientation measurement apparatus which concerns on 2nd Embodiment implements. The figure which shows the structure of the position and orientation measurement apparatus which concerns on 3rd Embodiment.

(First embodiment)
In the first embodiment, a dynamic change on the imaging region is detected before and after the distance measurement image acquisition. After that, it is determined whether to use the distance image for position and orientation measurement according to the detection result. In the present embodiment, “model fitting using two-dimensional image information” when there is a dynamic change in the imaging region, and “model fitting using distance image information (three-dimensional point group)” when there is no dynamic change. ”Is selected to measure the position and orientation of the object. In this embodiment, it is assumed that the approximate position and orientation of the object are known.

  FIG. 1 shows a configuration of a position / orientation measurement apparatus 1 according to the present embodiment. The position / orientation measurement apparatus 1 includes an imaging unit 110, an illumination unit 120, a two-dimensional image processing unit 130, a distance image processing unit 140, a motion detection unit 150, a mode selection / switching unit 160, and a two-dimensional image feature. The detection unit 170, the approximate position / orientation input unit 180, the three-dimensional shape model holding unit 190, and the position / orientation calculation unit 200 are provided. Hereinafter, each part which comprises the position and orientation measuring apparatus 1 is demonstrated.

  The imaging unit 110 is a camera that captures a two-dimensional image. In the present embodiment, the imaging unit 110 captures an image under normal illumination (hereinafter referred to as a “two-dimensional image”) and an image in a state where a pattern for distance measurement is irradiated (hereinafter referred to as “distance measurement”). The image ")" is also taken. In the present embodiment, it is assumed that N distance measurement images are captured for distance image acquisition, and one distance image is generated based on a series of these distance measurement image groups. In addition, in order to detect a change in the geometric relationship between the imaging region and the imaging device during the imaging of the distance measurement image, a two-dimensional image under normal illumination one by one before and after the imaging of the N distance measurement images. Image. Here, a two-dimensional image acquired before the distance measurement image is captured is a two-dimensional image A, and a two-dimensional image acquired after the distance measurement image is captured is a two-dimensional image B. The two-dimensional image captured by the imaging unit 110 is input to the two-dimensional image processing unit 130, and the distance measurement image is input to the distance image processing unit 140. Internal parameters such as the focal length, principal point position, and lens distortion parameter of the camera are calibrated in advance by the method disclosed in Non-Patent Document 1 and Non-Patent Document 2, for example.

  The illumination unit 120 irradiates a measurement target object with a pattern for distance measurement. Here, a liquid crystal projector is used as the illumination unit 120. A liquid crystal projector can irradiate an arbitrary two-dimensional pattern. The internal parameters of the liquid crystal projector and the external parameters for the camera necessary for distance measurement are calibrated in advance by a method disclosed in Non-Patent Document 3, for example. However, the illumination unit 120 is not limited to a liquid crystal projector, and may be any illumination device as long as it is an illumination device that irradiates a distance measurement pattern. For example, a projector using DMD (digital mirror device) or LCOS (liquid crystal on silicon) may be used. In this embodiment, distance measurement is performed by a spatial encoding method.

  In the spatial encoding method, a plurality of code patterns as shown in FIG. 6 (patterns 1 to 6 in the example of FIG. 6) are projected onto the measurement target object, and each pixel of the captured image and the irradiated pattern are projected. Are associated with each other and distance measurement is performed in units of pixels. However, the distance measurement method is not limited to this, and may be an optical cutting method in which distance measurement is performed by projecting slit light whose direction has been changed in time series.

  The two-dimensional image processing unit 130 acquires a two-dimensional image captured by the imaging unit 110 and inputs it to the motion detection unit 150 and the two-dimensional image feature detection unit 170.

The distance image processing unit 140 acquires the distance measurement image captured by the imaging unit 110 and estimates the position and orientation so that the three-dimensional shape model (surface) is applied to the three-dimensional point group generated from the distance image. Then, perform the necessary processing. Specifically, one distance image is created from N distance measurement image groups captured by the imaging unit 110. Details of the distance image creation processing will be described later. Further, the created distance image is input to the position / orientation calculation unit 200. The motion detection unit 150 determines whether or not there is a change in the geometric relationship between the measurement target object and the imaging apparatus before and after acquiring the distance measurement image group. Is detected. Specifically, the two-dimensional image A captured by the imaging unit 110 before acquiring the distance measurement image and the two-dimensional image B captured after acquiring the distance measurement image are compared, and the geometry during the distance measurement image acquisition is compared. Detect changes in relationships. In the present embodiment, a region of interest is set from the two-dimensional image A assuming that the approximate position and orientation of the measurement target object are known in advance, and the region is compared with the same region of the two-dimensional image B. Detects a change in the geometric relationship within.

  First, a region of interest is set in the two-dimensional image A. The region of interest is set by, for example, projecting the measurement target object on the image plane of the two-dimensional image A based on the approximate position and orientation input from the approximate position and orientation input unit 180, and pixels belonging to the projection area plane of the measurement target object. Is the region of interest. However, the method of specifying the region of interest is not limited to this. For example, the Bounding Box of the area where the measurement target object is projected on the image plane may be used as the area of interest, or may be an area designated in advance by hand. Or the whole image of the two-dimensional image A may be sufficient.

  Next, a change in geometric relationship between the measurement target object and the imaging device is detected using the two-dimensional image A and the two-dimensional image B in the set region of interest. For example, a difference image in a region of interest between the two-dimensional image A and the two-dimensional image B is used for detecting the geometric relationship change. The pixel value of the two-dimensional image A at the pixel (i, j) is A (i, j) and the pixel value of the two-dimensional image B is B (i, j), and the difference C (i , J) = | A (i, j) −B (i, j) | If the pixel value at this time is 0 to 255 and the average value of all the pixel values of the difference image in the region of interest is equal to or greater than a threshold value (for example, 10), it is determined that the geometric region has changed in the region of interest. However, the detection method of the geometric relationship change is not limited to this. For example, the optical flows of the two-dimensional image A and the two-dimensional image B are obtained, and when the average value of the vector lengths of the corresponding optical flows obtained is equal to or greater than a threshold value (for example, 5 pixels), it is determined that the region of interest has changed. To do. In addition, any method may be used as long as it can detect a change in geometric relationship between the imaging apparatus and the measurement target object that would reduce the distance value calculation system.

  The mode selection / switching unit 160 selects and switches the mode in which the position / orientation calculation unit 200 calculates the position / orientation of the measurement target object according to the detection result of the change in the geometric relationship by the motion detection unit 150. In the present embodiment, a “first mode for applying a three-dimensional shape model using a distance image” and a “second mode for applying a three-dimensional shape model using a two-dimensional image” are provided. If a change in geometric relationship is detected on the region of interest as a result of detection by the motion detection unit 150, the measurement of the distance image is unreliable, and “a second model that applies a three-dimensional shape model using a two-dimensional image is used. Use mode. On the other hand, if a change in geometric relationship is not detected on the region of interest as a result of detection by the motion detection unit 150, the measurement result of the distance image is trusted, and a “three-dimensional shape model is obtained using the distance image. The first mode to be applied is used.

  The two-dimensional image feature detection unit 170 detects an image feature from the two-dimensional image input from the two-dimensional image processing unit 130. In this embodiment, an edge is detected as an image feature. The approximate position and orientation input unit 180 inputs approximate values of the position and orientation of the measurement target object with respect to the position and orientation measurement apparatus 1 to the motion detection unit 150 and the position and orientation calculation unit 200. The position / orientation measurement apparatus 1 is defined with a three-dimensional coordinate system (reference coordinate system) that is a reference for measuring position and orientation. The position and orientation of the measurement target object with respect to the position and orientation measurement apparatus 1 represent the position and orientation of the measurement target object in the reference coordinate system. In this embodiment, a coordinate system in which the camera center is the origin and the optical axis of the camera is the z axis is a reference coordinate system.

  In the present embodiment, the position / orientation measurement apparatus 1 uses the measurement value of the previous time (previous time) as the approximate position and orientation, assuming that the measurement is continuously performed in the time axis direction. However, the method for inputting the approximate values of the position and orientation is not limited to this. For example, the position and orientation change amount may be estimated based on the past position and orientation measurement results, and the current position and orientation may be predicted from the past position and orientation and the estimated change amount. In addition, when the approximate position or orientation where the measurement target object is placed is known in advance, the value is used as an approximate value.

  The three-dimensional shape model holding unit 190 holds three-dimensional shape model data of a measurement target object that is a target for measuring the position and orientation. In this embodiment, an object is described as a three-dimensional shape model composed of line segments and surfaces. In the position / orientation calculation unit 200, the position and orientation are estimated so that the line segment in the three-dimensional shape model is applied to the edge on the two-dimensional image and the surface is applied to the three-dimensional point group obtained from the distance image. To do.

  FIG. 2 is a diagram illustrating a three-dimensional shape model in the present embodiment. A three-dimensional shape model is defined by a set of points and a set of line segments formed by connecting the points. As shown in FIG. 2A, the three-dimensional shape model of the measurement target object 10 is composed of 14 points P1 to P14. As shown in FIG. 2B, the three-dimensional shape model of the measurement target object 10 includes line segments L1 to L16. As shown in FIG. 2C, the points P1 to P14 are represented by three-dimensional coordinate values. Moreover, as shown in FIG.2 (d), the line segment L1-line segment L16 is represented by ID of the point which comprises a line segment. Further, the three-dimensional shape model holds surface information. Each surface is represented by an ID of a point constituting each surface. In the three-dimensional shape model shown in FIG. 2, information on six surfaces constituting a rectangular parallelepiped is stored.

  The position / orientation calculation unit 200 calculates the position / orientation of the measurement target object based on the mode selected and switched by the mode selection / switching unit 160. When the first mode is selected, the 3D shape model held in the 3D shape model holding unit 190 is applied to the 3D point group input from the distance image processing unit 140. By these fittings, the position and orientation of the object are measured. On the other hand, when the second mode is selected, the 3D shape model held in the 3D shape model holding unit 190 is applied to the image feature detected and input by the 2D image feature detection unit 170. The position and orientation of the measurement target object are measured by this fitting.

  Next, with reference to the flowchart of FIG. 3, the procedure of the process which the position / orientation measuring apparatus 1 which concerns on 1st Embodiment which has the above-mentioned structure implements is demonstrated.

(Step S3010)
The approximate position and orientation input unit 180 inputs an approximate value of the position and orientation of the measurement target object with respect to the position and orientation measurement apparatus 1 (camera) to the motion detection unit 150 and the position and orientation calculation unit 200. As described above, in this embodiment, the position and orientation measured at the previous time is used.

(Step S3020)
The imaging unit 110 captures a two-dimensional image A of the measurement target object under normal illumination. Here, the normal illumination may be illumination such as a fluorescent lamp arranged in a general environment, or illumination of the same color (for example, white) by the illumination unit 120. That is, any illumination other than a special pattern illumination for distance measurement and the like may be used. The 2D image A captured in this step is input to the 2D image processing unit 130. Note that the image acquired here is used in step S3080 to detect the image feature used by the two-dimensional image feature detection unit 170 when calculating the position and orientation of the position and orientation calculation unit 200, and at the same time, motion detection is performed in step S3050. It is also used when the unit 150 detects a dynamic change in the imaging region.

(Step S3030)
The imaging unit 110 captures an image for distance measurement. In the present embodiment, it is assumed that distance measurement is performed by the spatial encoding method, and thus it is necessary to capture a plurality of images irradiated with different code patterns. The illumination unit 120 irradiates the measurement target object with the code pattern shown in FIG.

  FIG. 6 shows a 6-bit gray code pattern. The illumination unit 120 irradiates each code pattern in order from the pattern 1, and the imaging unit 110 captures an image for distance measurement every time each code pattern is irradiated. The captured distance measurement image is input to the distance image processing unit 140. In this process, the code pattern is changed until the image when all the code patterns are irradiated is captured, and the measurement is performed by irradiating the measurement object with the code pattern. In the example of FIG. 6, since a 6-bit code pattern is used, a total of six distance measurement images are acquired.

(Step S3040)
The imaging unit 110 captures a two-dimensional image B of the measurement target object under normal illumination. The process itself in this step is the same as the process in step S3020. The two-dimensional image B acquired here is used when the motion detection unit 150 detects a dynamic change of the imaging region from the comparison with the two-dimensional image A in step S3050.

(Step S3050)
The motion detection unit 150 performs motion detection in the image area. Specifically, a change in the geometric relationship in the imaging region is detected between two images, the two-dimensional image A acquired in step S3020 and the two-dimensional image B acquired in step S3040. In the present embodiment, the difference image in the region of interest between the two-dimensional image A and the two-dimensional image B is obtained, and the absolute value of the average pixel value is a threshold value (for example, 10) assuming that the pixel value of the difference image is 0 to 255. ) If this is the case, it is determined that there is movement in the region of interest.

(Step S3060)
The mode selection / switching unit 160 selects and switches the mode in which the position / orientation calculation unit 200 measures the position / orientation based on the detection result of the dynamic change of the imaging region in step S3050. The first mode is a “mode for performing model fitting using measurement information (three-dimensional point cloud) obtained from distance data”. The second mode is a “mode for performing model fitting using measurement information obtained from a two-dimensional image”.

  If a change in geometric relationship is detected in the region of interest in step S3050, the second mode is selected assuming that distance measurement has not been accurately performed, and the flow proceeds to step S3080. In this case, since the position and orientation are measured in the second mode without using distance data with low measurement accuracy, measurement can be performed with high accuracy. On the other hand, if no change in the geometric relationship is detected in the region of interest in step S3050, the first mode is selected assuming that the distance measurement has been accurately performed, and the process proceeds to step S3070.

(Step S3070)
The distance image processing unit 140 generates a distance image from the distance measurement image group captured in step S3030. First, binarization processing is performed on each distance measurement image to binarize each pixel. From the binarization result, each pixel is assigned an n-bit code (the number of bits is the number of code patterns to be used, which is 6 bits in this embodiment). This code is converted into a binary code and associated with the image coordinates of the illumination pattern, and a depth image is generated by calculating a depth value for each pixel based on the correspondence and the above-described configuration parameters. By multiplying the line-of-sight vector corresponding to the pixel position by the depth value for each pixel of the distance image, the pixel is converted into three-dimensional point group data having three-dimensional coordinates in the reference coordinate system.

(Step S3080)
The two-dimensional image feature detection unit 170 detects image features on the two-dimensional image input in step S3020. In this embodiment, an edge is detected as an image feature. The edge is an extreme value of the density gradient.

  FIG. 4A and FIG. 4B are diagrams for explaining edge detection in the present embodiment. First, using the approximate position and orientation of the measurement target object input in step S3010 and the calibrated internal parameters of the two-dimensional image processing unit 130, a projected image on the image of each line segment constituting the three-dimensional shape model is obtained. calculate. The projected image of the line segment is also a line segment on the image. Next, as shown in FIG. 4A, control points are set on the projected line segments so as to be equally spaced on the image, and the normal direction of the projected line segment is set for each control point. One-dimensional edge detection is performed. Since the edge is detected as an extreme value of the density gradient of the pixel value, a plurality of edges may be detected as shown in FIG. In the present embodiment, the detected edge having the largest density gradient is set as the corresponding edge.

(Step S3090)
The position / orientation calculation unit 200 calculates the position / orientation of the measurement target object using either the first mode or the second mode switched in step S3060. Hereinafter, the procedure for calculating the position and orientation of the measurement target object in the first mode and the procedure for calculating the position and orientation of the measurement target object in the second mode will be described in order.

(First mode)
The position / orientation calculation unit 200 formulates an observation equation for calculating the position and orientation based on the correspondence between the three-dimensional point cloud data obtained in step S3070 and the model surface. First, each point of the point cloud data is associated with the model surface. Here, a three-dimensional shape model or point cloud data is coordinate-transformed based on approximate values of position and orientation, and each point in the point cloud data is associated with a model surface having the smallest distance in the three-dimensional space.

The three-dimensional coordinates of the point group in the reference coordinate system are converted into the three-dimensional coordinates (x, y, z) in the coordinate system of the measurement target object by the position and orientation s of the measurement target object. It is assumed that a certain point in the point cloud data is converted into measurement target object coordinates (x ₀ , y ₀ , z ₀ ) by the approximate values of the position and orientation. (X, y, z) changes depending on the position and orientation of the object to be measured, and can be approximated by the first-order Taylor expansion in the vicinity of (x ₀ , y ₀ , z ₀ ) as shown in Equation (1). .

Ax + by + cz = e (a ² + b ² + c ² = 1, a, b) for the surface of the 3D geometric model associated with a point in the point cloud data , c, e are constants). It is assumed that (x, y, z) transformed by the correct position and orientation s satisfies the plane equation ax + by + cz = e. Substituting equation (1) into the plane equation yields equation (2).

However, q = ax ₀ + by ₀ + cz ₀ (constant). Since the observation equation of equation (2) can be established for all point cloud data associated with each other, the following linear simultaneous equations relating to Δs _i can be established.

  Here, Expression (3) is expressed as Expression (4).

The correction value Δs of the position and orientation is obtained by multiplying both sides of the formula (4) by the generalized inverse matrix (J ^T · J) ⁻¹ · J ^T of the matrix J, and the position using the correction value Δs is obtained. Then, the position and orientation s of the measurement target object are calculated by correcting the approximate value of the orientation. In the embodiment described above, the position and orientation are corrected only once. However, the correction of the position and orientation is repeated by repeating the process of this step based on the approximate values of the corrected position and orientation. May be.

  Next, a procedure for calculating the position and orientation of the measurement target object in the second mode will be described.

(Second mode)
The position / orientation calculation unit 200 obtains the correspondence between the edge detected in step S3080 and the model-side edge. As described in the description of step S3080, the edge detection method in the present embodiment is a top-down method, and is a method of searching for a corresponding edge from the model side. Therefore, detection and association are performed at a time. . Next, based on the correspondence between the control points on the line segment of the model and the edges on the image, an observation equation is calculated for calculating the position and orientation. Here, the position and orientation of the measurement target object are represented by a six-dimensional vector s.

FIG. 5 is a diagram illustrating the relationship between the projected image of the line segment and the detected edge. In FIG. 5, the horizontal and vertical directions of the image are the u axis and the v axis, respectively. The position on the image of a certain control point is (u ₀ , v ₀ ), and the inclination on the image of the line segment to which the control point belongs is expressed as the inclination θ with respect to the u axis. The inclination θ is calculated as the inclination of a straight line connecting the coordinates of both ends on the image by projecting the three-dimensional coordinates of both ends of the line segment on the image based on the approximate value of s (s ₀ ). The normal vector on the image of the line segment is (sin θ, −cos θ). Also, the image coordinates of the corresponding point (edge) of the control point is (u ′, v ′). Here, a point (u, v) on a straight line (broken line in FIG. 5) passing through the point (u ′, v ′) and having an inclination of θ is

(Θ is a constant). Here, d = u′sinθ−v′cosθ (constant). The position of the control point on the image changes depending on the position and orientation of the measurement target object. The degree of freedom of the position and orientation of the measurement target object is 6 degrees of freedom. That is, s is a 6-dimensional vector, and includes three elements representing the position of the measurement target object and three elements representing the posture. The three elements representing the posture are represented by, for example, a representation by Euler angles or a three-dimensional vector in which the direction represents the rotation axis passing through the origin and the norm represents the rotation angle. The image coordinates (u, v) of the control point that changes depending on the position and orientation can be approximated as in Equation (6) by the first-order Taylor expansion in the vicinity of (u ₀ , v ₀ ). However, Δs _i (i = 1, 2,..., 6) represents a minute change of each component of s.

  Assuming that there is not much difference between the approximate value of the position and orientation and the actual position and orientation, it can be assumed that the position on the image of the control point obtained by the correct s is on the straight line represented by Equation (5). By substituting u and v approximated by equation (6) into equation (5), an observation equation, equation (7) is obtained.

However, r = u ₀ sin θ−v ₀ cos θ (constant). Observation equation of equation (7) it is possible to stand type for all of the control points correspondence is performed, it is possible to make a system of linear equations relating Delta] s _i as the following equation (8).

  By calculating the position and orientation correction values and correcting the approximate values of the position and orientation by the same method as the equations (3) and (4) for the simultaneous equations, the position and orientation s of the measurement target object are corrected. Is calculated.

  As described above, in the present embodiment, a change in the relative geometric relationship between the measurement target object and the imaging device is detected from two two-dimensional images captured before and after capturing the distance measurement image group, and the detection result is obtained. The position and orientation measurement method was selected accordingly. In situations where the distance image is not accurately captured, the distance image is not used during position / orientation measurement, but the position / orientation measurement is performed using the information of the two-dimensional image, enabling highly accurate measurement. ing.

[Modification]
In the first embodiment, one of “model fitting using distance image information (three-dimensional point cloud)” and “model fitting using two-dimensional image information (image feature)” is selected and the object is selected. Position and orientation measurement was performed. As a modified example, when a change in geometric relationship is not detected during the acquisition of the distance measurement image group, “two-dimensional image information (image feature) and distance image information (three-dimensional point group) are used together. The position and orientation of the object may be measured by a “third mode in which model fitting is performed”. In this case, by using both the information on the distance image and the information on the two-dimensional image, it is possible to perform more accurate position and orientation measurement. Note that the basic configuration and the flow of processing are the same as those in the first embodiment, and a detailed description thereof will be omitted. In this modified example, “a second mode in which model fitting is performed using two-dimensional image information (image features)” and “two-dimensional image information (image features) and distance image information (three-dimensional point cloud) are used together. And a third mode in which model fitting is performed.

  The mode selection / switching unit 160 selects the second mode when the motion detection unit 150 detects a change in the geometric relationship between the imaging device and the measurement target object, and selects the third mode when the change is not detected. The position / orientation calculation unit 200 performs the same processing as in the first embodiment when the second mode is selected. When the third mode is selected, the three-dimensional shape model is retained for the three-dimensional point group input from the distance image processing unit 140 and the image feature detected and input by the two-dimensional image feature detection unit 170. Each of the three-dimensional shape models held in the unit 190 is applied.

  Next, a procedure for calculating the position and orientation of the measurement target object in the third mode will be described.

(Third mode)
The position / orientation calculation unit 200 establishes Expression (3) in the same manner as in the first mode, from the correspondence between the distance image point group obtained in step S3070 and the model plane. Further, from the correspondence between the two-dimensional image edge obtained in step S3080 and the model-side edge, Expression (8) is established as in the second mode. Here, since the equations (3) and (8) are equations about the minute change Δs _i (i = 1, 2,..., 6) of each component of s, Δs like the equation (9) A linear system for _i can be established.

By solving the simultaneous equations, the correction value Delta] s _i of the position and orientation can be obtained. To calculate the final position and orientation by correcting the approximate value of the position and orientation using the correction value Delta] s _i. By the way, the value on the right side of Equation (9) is the signed distance on the image for the edge, and the signed distance in the three-dimensional space for the point group, so the scales do not match. Therefore, the error in the three-dimensional space is approximated by multiplying the error on the image by the depth of the edge. Specifically, the scale is unified to the distance in the three-dimensional space by multiplying both sides of the equation for the edge by the depth of the edge. Since depth information cannot be obtained from a two-dimensional image, the depth of an edge needs to be obtained by some approximate method. Here, the depth of each control point calculated by the approximate values of the position and orientation is converted into an error in a three-dimensional space by multiplying the error on the image. At this time, the simultaneous equations to be solved are as shown in Equation (10).

Here, z ₁ , z ₂ ... Represent the depth of each edge. Instead of the depths z ₁ , z ₂ ..., A scaling factor may be multiplied so as to be the length of a perpendicular drawn from a control vector in a three-dimensional space with respect to a line-of-sight vector passing through an edge on the image plane. By calculating the position and orientation correction values and correcting the approximate values of the position and orientation for the simultaneous equations in the same manner as in equations (3) and (4), the position and orientation s of the measurement target object are corrected. Is calculated.

(Second Embodiment)
In the first embodiment, acquisition of all distance measurement images is performed as a series of processes, and changes in the geometric relationship between the imaging device and the measurement target before and after that are detected to determine whether to use a distance image. did.

  On the other hand, in the second embodiment, in acquiring N distance measurement images, a change in geometric relationship is detected every time one distance measurement image is acquired. When a change is detected, the distance measurement is not performed thereafter, and the mode is switched to the first mode in which the position and orientation is measured using only the two-dimensional image.

  FIG. 7 shows a configuration of the position / orientation measurement apparatus 2 according to the present embodiment. The basic configuration is the same as the configuration of the position measuring apparatus 1 shown in FIG. The position / orientation measurement apparatus 2 includes an imaging unit 710, an illumination unit 720, a two-dimensional image processing unit 730, a distance image processing unit 740, a motion detection unit 750, a mode selection / switching unit 760, and a two-dimensional image feature. A detection unit 770, an approximate position / orientation input unit 780, a three-dimensional shape model holding unit 790, and a position / orientation calculation unit 800 are provided. The difference from the first embodiment is in the processing of the imaging unit 710, the processing of the motion detection unit 750, and the processing of the mode selection / switching unit 760. The imaging unit 710 acquires a distance measurement image necessary for generating a distance image. However, instead of performing acquisition of all images as a continuous process, the motion detection unit 750 performs motion detection each time a distance measurement image is acquired. Whether the mode selection / switching unit 760 switches the mode to be used based on the result of the motion detection that is performed every time one image for distance measurement is acquired, and whether to capture the image for distance measurement thereafter. Determine whether or not.

  Next, with reference to a flowchart of FIG. 8, a procedure of processing performed by the position / orientation measurement apparatus 2 according to the second embodiment having the above-described configuration will be described. However, the processes in steps S8010, S8020, S8080, S8090, and S8100 are the same as the processes in steps S3010, S3020, S3070, S3080, and S3090 in the first embodiment, and thus the description thereof is omitted. .

(Step S8030)
The imaging unit 710 captures one image for distance measurement. In this embodiment, every time a distance measurement image is acquired, motion detection is performed one by one. Accordingly, only one image irradiated with one code pattern is picked up in order from the pattern 1 among the code patterns shown in FIG. Whether or not to capture an image irradiated with the next code pattern is determined by subsequent processing.

(Step S8040)
The imaging unit 710 captures a two-dimensional image B of the measurement target object under normal illumination. The process itself is the same as that in step S8020. In the present embodiment, each time a distance measurement image is captured, the two-dimensional image B is acquired and updated. The two-dimensional image B acquired here is used when the motion detection unit 750 detects a dynamic change of the imaging region from the comparison with the two-dimensional image A in step S8050.

(Step S8050)
The motion detection unit 750 detects motion in the image area. Specifically, a change in the geometric relationship in the imaging region is detected between the two images of the two-dimensional image A acquired in step S8020 and the two-dimensional image B acquired in step S8040. Since the content of the process is the same as the process of step S3050, description thereof is omitted.

  However, in order to detect a change in geometric relationship at shorter time intervals, motion detection may be performed using two-dimensional images acquired before and after acquiring a distance measurement image. Specifically, only when the first distance measurement image is captured, motion detection is performed using the two-dimensional image A and the two-dimensional image B, and thereafter, the previous distance measurement image is acquired. The motion detection may be performed using the two-dimensional image B and the two-dimensional image B ′ as the two-dimensional image B ′ captured later.

(Step S8060)
The mode selection / switching unit 760 determines whether or not to capture the image for distance measurement after this based on the detection result of the dynamic change of the imaging region in step S8050, and the position / orientation calculation unit 800 determines the position / orientation. Select and switch the mode for measurement. Each mode is the same as in the first embodiment. First, the initial mode is the first mode. As a result of step S8050, if a certain amount or more of movement is observed in the image area, it is determined that distance measurement is not accurately performed at that time.

  In this case, the distance measurement image is not acquired thereafter. Therefore, the next code pattern irradiation or imaging associated with imaging is not performed. At the same time, the first mode is switched to the second mode, and the process proceeds to step S8090. On the other hand, if the observed dynamic change is less than a certain amount, the process proceeds to step S8070 while maintaining the first mode. In the present embodiment, the third mode may be used instead of the first mode, as in the modification of the first embodiment.

(Step S8070)
The distance image processing unit 740 determines whether or not all the distance measurement images have been captured by capturing the images when all the code patterns are irradiated. If it is determined that all the distance measurement images have been captured (S8070; YES), the process proceeds to step S8080. On the other hand, if it is determined that all the distance measurement images have not yet been captured (S8070; NO), the process returns to step S8030, the code pattern is changed, and the measurement object is irradiated with the next code pattern. I do.

  As described above, in the second embodiment, every time a distance measurement image is acquired, a change in geometric relationship is detected. Since the subsequent distance measurement image is not captured when it is determined that the imaging accuracy of the distance measurement image has decreased, the processing of the entire position and orientation measurement is made efficient. In addition, since the position and orientation are measured without using the distance image with reduced accuracy, the measurement can be highly accurate.

(Third embodiment)
In the first embodiment and the second embodiment, whether a change in the geometric relationship between the imaging device and the measurement target is detected based on the two-dimensional images captured before and after the distance measurement image is captured, and the distance image is used. Judgment was made.

  On the other hand, in the third embodiment, a sensor for detecting movement is attached to the imaging device, and the value of the sensor is referred to when acquiring the distance measurement image, so that the imaging device and the measurement target are Detect changes in geometric relationships. FIG. 9 shows the configuration of the position / orientation measurement apparatus 3 according to the present embodiment. The basic configuration is the same as the configuration of the position measuring apparatus 1 shown in FIG. The position / orientation measurement apparatus 3 includes an imaging unit 910, an illumination unit 920, a two-dimensional image processing unit 930, a distance image processing unit 940, a motion detection unit 950, a mode selection / switching unit 960, and a two-dimensional image feature. A detection unit 970, an approximate position and orientation input unit 980, a three-dimensional shape model holding unit 990, and a position and orientation calculation unit 1000 are provided. The difference from the first embodiment is in the processing of the imaging unit 910 and the processing of the motion detection unit 950.

  The imaging unit 910 inputs a distance measurement image and inputs the time t at which the imaging was performed to the motion detection unit 950.

  The motion detection unit 950 detects motion by determining whether or not the geometric relationship between the imaging device and the measurement target has changed at time t input by the imaging unit 910. For detecting the change in the geometric relationship, for example, a gyro sensor is attached to the imaging apparatus, and the output value of the sensor is used. Note that when the imaging device is attached to a robot (hand), a gyro sensor may be attached to the robot instead of the imaging device. The three-axis angular velocities output by the gyro sensor at time t are θx, θy, and θz, respectively. If any one of these values is equal to or greater than a threshold value (for example, 1 deg / s), the imaging device and the measurement target It is determined that the geometric relationship with the object has changed. Alternatively, the sensor may be attached not only to the imaging apparatus but also to an imaging area (for example, a table on which a measurement target object is placed), and both output values may be used. If at least one of the three-axis angular velocity components output by the sensors attached to both the imaging device and the imaging region is greater than the threshold at time t, the geometric relationship between the imaging device and the measurement target is Judge that it has changed.

  However, the method for detecting a change in geometric relationship is not limited to this. For example, even if an acceleration sensor is used instead of the gyro sensor, the motion can be detected by the same method. Alternatively, the movement may be detected by comparing the position and orientation at two times using a position and orientation sensor. In the case of using the position and orientation sensor, the position and orientation of the imaging device at two times are acquired respectively at time t1 before acquisition of the distance measurement image and t2 after acquisition, and imaging at two times is performed. A change in geometric relationship is detected by comparing the position and orientation of the apparatus.

  For example, assume that the position and orientation at time t1 are T1 and R1, and the position and orientation at time t2 are T2 and R2. At this time, assuming that the difference between T1 and T2 is ΔT and the difference between postures R1 and R2 is Δθ, either ΔT or Δθ is equal to or greater than a threshold (eg, Δt: 1 cm, Δθ: 3 degrees). If there is, it is determined that the geometric relationship between the imaging device and the measurement target object has changed. In addition, any method may be used as long as it can detect a change in the geometric relationship between the imaging apparatus and the measurement target object during acquisition of a plurality of distance measurement images.

  Note that the position and orientation measurement processing procedure in the present embodiment is the same as the processing procedure described in the flowchart of FIG. However, in this embodiment, in S1020 and S1040, the sensor value is acquired as described above simultaneously with the acquisition of the two-dimensional image. In S1050, a change in geometric relationship is detected from the acquired sensor value. Since other processes are the same as those in the first embodiment, description thereof is omitted.

  Similarly to the second embodiment, every time one image for distance measurement is acquired, a change in the geometric relationship between the imaging device and the measurement target object is detected by the method described above, and the change is detected. The mode may be switched at the time, and the acquisition of the distance measurement image thereafter may be stopped.

  As described above, in the third embodiment, when a change in geometric relationship is detected by using information of an image sensor or a sensor attached to an imaging target and the distance image is not reliable, the distance image is displayed. By measuring the position and orientation without using it, the position and orientation can be measured with high accuracy.

  As described above, the mode for performing position and orientation measurement is not exclusively switched, but may be continuously switched according to the situation. In that case, the mode is continuously switched by manipulating the scaling coefficient of Expression (10) described in the third mode in the first embodiment.

  The mode may be switched using a user interface (input reception unit) that can be operated by the user. The user interface may be configured with a GUI menu displayed on the display screen or a hardware switch.

  Further, the system may be configured to suggest which mode is desirable on the display screen, instead of automatically switching according to the situation of measuring the target object. In this case, the user can select a mode using the user interface.

  In addition, the user may be presented with which mode the system is currently operating. The presentation may be displayed on a display screen, or may be presented in hardware using a lamp or LED.

(Other embodiments)
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 (10)

Model holding means for holding model data of the target object;
Illumination means for irradiating the target object with a pattern for distance measurement;
An imaging unit that captures the target object that is not irradiated with the distance measurement pattern and acquires a two-dimensional image; and that captures the target object that is irradiated with the distance measurement pattern and acquires a distance measurement image; ,
A distance image processing means for generating a distance image from a distance measurement image group obtained by irradiating and imaging each of the plurality of distance measurement patterns;
Detecting means for detecting a change in a geometric relationship between the target object and the imaging means;
Position and orientation calculation means for calculating the position and orientation of the target object based on at least one of the two-dimensional image and the distance image and the model data in accordance with a detection result by the detection means;
A position and orientation measurement apparatus comprising: The position / orientation calculation means includes:
When the change is detected by the detection means, the position and orientation of the target object is calculated based on the two-dimensional image and the model data,
The position / orientation measurement apparatus according to claim 1, wherein when the change is not detected by the detection unit, the position / orientation of the target object is calculated based on the distance image and the model data.   The position and orientation calculation means calculates the position and orientation of the target object based on the two-dimensional image, the distance image, and the model data when the change is not detected by the detection means. The position and orientation measurement apparatus according to claim 2.   The detection unit detects a change in the geometric relationship based on a plurality of two-dimensional images acquired by the imaging unit before and after the distance measurement image group is acquired by the imaging unit. The position / orientation measurement apparatus according to claim 1.   The detecting means changes the geometric relationship based on a plurality of two-dimensional images acquired by the imaging means before and after acquiring one distance measurement image from the distance measurement image group by the imaging means. The position / orientation measurement apparatus according to claim 1, wherein the position / orientation measurement apparatus detects the position / orientation measurement apparatus. A sensor for detecting movement of the imaging means relative to the target object;
The position / orientation measurement apparatus according to claim 1, wherein the detection unit detects a change in the geometric relationship with reference to the sensor. An input receiving means for receiving input from the user;
The position / orientation calculation means is responsive to an input from the input receiving means,
A first mode for calculating the position and orientation based on the distance image and the model data;
A second mode for calculating the position and orientation based on the two-dimensional image and the model data;
A third mode for calculating the position and orientation based on the two-dimensional image, the distance image, and the model data;
The position / orientation measurement apparatus according to claim 1, wherein the position / orientation of the target object is calculated by switching between the two. The position and orientation calculation means is
A first mode for calculating the position and orientation based on the distance image and the model data;
A second mode for calculating the position and orientation based on the two-dimensional image and the model data;
A third mode for calculating the position and orientation based on the two-dimensional image, the distance image, and the model data;
The position / orientation measurement apparatus according to claim 1, further comprising a presentation unit that presents to a user which mode the position / orientation of the target object is calculated. A position comprising model holding means for holding model data of a target object, illumination means for irradiating the target object with a distance measurement pattern, imaging means, distance image processing means, detection means, and position and orientation calculation means A control method for an attitude measurement device,
The imaging means captures the target object that is not irradiated with the distance measurement pattern to obtain a two-dimensional image, captures the target object irradiated with the distance measurement pattern, and generates a distance measurement image. An imaging process to be acquired;
A distance image processing step in which the distance image processing unit generates a distance image from a group of distance measurement images captured by irradiation with each of the plurality of distance measurement patterns;
A detecting step in which the detecting means detects a change in a geometric relationship between the target object and the imaging means;
A position and orientation calculation step in which the position and orientation calculation means calculates the position and orientation of the target object based on at least one of the two-dimensional image and the distance image and the model data according to the detection result of the detection step. When,
A control method for a position and orientation measurement apparatus, comprising:   A program for causing a computer to function as each unit of the position and orientation measurement apparatus according to claim 1.

Images