JP2021012628A - Position and posture estimation device and position and posture estimation method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To more accurately estimate the position and orientation of a work even if a part of the work is obstructed by another object. An image acquisition unit 11 that acquires a visible image, a position image, and a normal image, a schematic estimation unit 12 that estimates the position and orientation of a work based on a visible image acquired by the image acquisition unit 11, and an image. After determining the effective pixels on the image based on the acquisition result by the acquisition unit 11 and the position and orientation estimated by the rough estimation unit 12, the normal vector of the three-dimensional model of the work projected on the image in the position and orientation. Is compared with the normal vector shown by the normal image, and the rotation correction unit 13 that corrects the amount of rotation in the position and orientation, the acquisition result by the image acquisition unit 11, and the position and orientation after correction by the rotation correction unit 13 After determining the effective pixels on the image based on, the position and orientation are compared with the three-dimensional position of the three-dimensional model of the work projected on the image in the position and orientation and the three-dimensional position indicated by the position image. A translation correction unit 14 for correcting the translation amount of the above is provided. [Selection diagram] Fig. 1

Description

The present invention relates to a position / orientation estimation device and a position / orientation estimation method for estimating the position / orientation of a work from an image.

In recent years, at production sites such as factories, automation by robots has been promoted in order to improve production efficiency. As part of this, bulk picking technology has been developed. The bulk picking technique is a technique of estimating the position and orientation of the workpieces stacked in the tray and grasping the workpieces by the robot hand based on the estimation result.

As a method of estimating the position and orientation of the work, the rough position and orientation of the work is estimated from the image on which the trays in which the works are stacked are projected, and the position and orientation are used as the initial values for the three-dimensional model of the work with respect to the image data. There is a method of updating the position and orientation by applying (see, for example, Patent Document 1).

Japanese Unexamined Patent Publication No. 2010-69542

J. Gall, and V. Lempitsky, “Class-specific hough forests for object detection”, Computer Vision and Pattern Recognition, 2009.

However, in the loosely stacked state, a part of the work area is often blocked by another object (work). In this case, the value of the distance image in the above-mentioned part of the region is an abnormal value that is not the original distance to the work. When the estimation is performed by the conventional method using a distance image containing an abnormal value, the collation score is low even in the true position and orientation, and the collation score is high even in the wrong position and orientation, so the position is accurate. It becomes difficult to search for a posture.

The present invention has been made to solve the above-mentioned problems, and compared to the conventional case, even if a part of the work is obstructed by another object, the position and orientation of the work can be estimated more accurately. It is an object of the present invention to provide a posture estimation device.

The position / orientation estimation device according to the present invention has a visible image, a position image showing the three-dimensional position of the object projected on the visible image for each pixel, and a normal vector of the object projected on the visible image for each pixel. The image acquisition unit that acquires the normal image shown in (1), the approximate estimation unit that estimates the position and orientation of the work based on the visible image acquired by the image acquisition unit, and the acquisition result by the image acquisition unit and the approximate estimation unit. After determining the valid pixels on the image based on the determined position and orientation, the normal vector of the three-dimensional model of the work projected on the image in the position and orientation is compared with the normal vector shown by the normal image. Then, after determining the effective pixels on the image based on the rotation correction unit that corrects the rotation amount of the position and orientation, the acquisition result by the image acquisition unit, and the corrected position and orientation by the rotation correction unit. It is provided with a translation correction unit that compares the three-dimensional position of the three-dimensional model of the work projected on the image in the position and orientation with the three-dimensional position indicated by the position image and corrects the translation amount in the position and orientation. It is characterized by.

According to the present invention, since the structure is as described above, the position and orientation of the work can be estimated more accurately than in the past even if a part of the work is blocked by another object.

It is a figure which shows the structural example of the position | posture estimation apparatus which concerns on Embodiment 1. FIG. It is a figure which shows the structural example of the rotation correction part in Embodiment 1. It is a figure which shows the structural example of the translation correction part in Embodiment 1. It is a flowchart which shows the operation example of the position | posture estimation apparatus which concerns on Embodiment 1. FIG. 5A to 5C are diagrams showing an example of an image group acquired by the image acquisition unit in the first embodiment, FIG. 5A is a diagram showing an example of a visible image, and FIG. 5B is an example of a position image. FIG. 5C is a diagram showing an example of a normal image. 6A and 6B are diagrams showing an operation example of the schematic estimation unit according to the first embodiment. It is a flowchart which shows the operation example of the rotation correction part in Embodiment 1. 8A to 8C are diagrams showing an example of a projection image group generated by the projection unit in the first embodiment, FIG. 8A is a diagram showing an example of a projection label image, and FIG. 8B is a projection position image. It is a figure which shows an example, and FIG. 8C is a figure which shows an example of a projection normal line image. It is a flowchart which shows the operation example of the translation correction part in Embodiment 1.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
Embodiment 1.
FIG. 1 is a diagram showing a configuration example of the position / posture estimation device 1 according to the first embodiment.
The position / orientation estimation device 1 estimates the position / orientation of the work. As shown in FIG. 1, the position / orientation estimation device 1 includes an image acquisition unit 11, a schematic estimation unit 12, a rotation correction unit 13, and a translation correction unit 14. The position / orientation estimation device 1 is realized by a processing circuit such as a system LSI (Large Scale Integration), a CPU (Central Processing Unit) that executes a program stored in a memory or the like, or the like.

The image acquisition unit 11 acquires an image group. The image group is a group of images necessary for estimating the position and orientation of the work. The image group includes a visible image, a position image, and a normal image. The position image is an image showing the three-dimensional position of the object projected on the visible image for each pixel. The normal image is an image showing the normal vector of the object projected on the visible image for each pixel.

The rough estimation unit 12 estimates the position and orientation of the work based on the visible image acquired by the image acquisition unit 11. The estimation by the rough estimation unit 12 is a rough estimation of the position and orientation using a conventionally known method.

The rotation correction unit 13 determines valid pixels on the image based on the image group acquired by the image acquisition unit 11 and the position / orientation estimated by the rough estimation unit 12, and then projects the image in the position / orientation. The normal vector of the three-dimensional model of the work is compared with the normal vector shown by the normal image, and the amount of rotation in the position and orientation is corrected. The details of the rotation correction unit 13 will be described later.

The translation correction unit 14 determines valid pixels on the image based on the image group acquired by the image acquisition unit 11 and the position and orientation after correction by the rotation correction unit 13, and then projects the image in the position and orientation. The translation amount of the position and orientation is corrected by comparing the three-dimensional position of the three-dimensional model of the work and the three-dimensional position indicated by the position image. The details of the translation correction unit 14 will be described later.

Next, a configuration example of the rotation correction unit 13 will be described with reference to FIG.
As shown in FIG. 2, the rotation correction unit 13 has a projection unit (first projection unit) 131, a pixel determination unit (first pixel determination unit) 132, and a rotation update unit 133.

The projection unit 131 generates a projected image group by projecting a three-dimensional model of the work onto an image in the position and orientation estimated by the rough estimation unit 12. The projected image group includes a projected label image, a projected position image, and a projected normal image. The projected label image is an image showing identification information of each plane of the three-dimensional model of the work projected on the image in the position and orientation estimated by the approximate estimation unit 12 for each pixel. The projected position image is an image showing the three-dimensional position of the three-dimensional model of the work projected on the image in the position and orientation estimated by the approximate estimation unit 12 for each pixel. The projection normal image is an image showing the normal vector of the three-dimensional model of the work projected on the image in the position and orientation estimated by the rough estimation unit 12 for each pixel.

The pixel determination unit 132 determines as a valid pixel a pixel belonging to an effective plane of the projected label image based on the image group acquired by the image acquisition unit 11 and the projected image group generated by the projection unit 131. ..

The rotation update unit 133 includes a normal vector indicated by the projection normal image generated by the projection unit 131 and a normal vector indicated by the normal image acquired by the image acquisition unit 11 based on the determination result by the pixel determination unit 132. By generating a rotation matrix based on the singular value decomposition from the difference between the above and the above, the rotation amount of the position and orientation estimated by the approximate estimation unit 12 is updated.

Next, a configuration example of the translation correction unit 14 will be described with reference to FIG.
As shown in FIG. 3, the translation correction unit 14 has a projection unit (second projection unit) 141, a pixel determination unit (second pixel determination unit) 142, and a translation update unit 143.

The projection unit 141 generates a projected image group by projecting a three-dimensional model of the work onto an image in the position and orientation corrected by the rotation correction unit 13. The projected image group includes a projected label image, a projected position image, and a projected normal image. The projected label image is an image showing the identification information of each plane of the three-dimensional model of the work projected on the image in the position and orientation corrected by the rotation correction unit 13 for each pixel. The projected position image is an image showing the three-dimensional position of the three-dimensional model of the work projected on the image in the position and orientation corrected by the rotation correction unit 13 for each pixel. The projection normal image is an image showing the normal vector of the three-dimensional model of the work projected on the image in the position and orientation corrected by the rotation correction unit 13 for each pixel.

The pixel determination unit 142 determines the pixels belonging to the effective plane of the projected label image as effective pixels based on the captured image group acquired by the image acquisition unit 11 and the image group generated by the projection unit 141. ..

The translation update unit 143 calculates the equation of each plane of the object from the position image acquired by the image acquisition unit 11 based on the determination result by the pixel determination unit 142, and each of the projection position images generated by the projection unit 141. Based on the positional relationship between the pixel and the corresponding plane, the translation amount of the corrected position and orientation by the rotation correction unit 13 is updated by the minimum square method.

Next, an operation example of the position / orientation estimation device 1 according to the first embodiment shown in FIG. 1 will be described with reference to FIG.
When estimating the position and orientation of the work, it is assumed that the surface of the work is composed of planes, and among those planes, three or more planes whose normal vectors are independent of each other can be detected on the image, and each of them. When the equation of a plane can be specified, the position and orientation of the work can be uniquely specified. The equation of a plane can be specified from the result of the rough estimation by using the least squares method or the like from the pixels included in the region of the plane.

Conventionally, as much image data as possible on the surface of the work is taken in and the position and orientation are estimated, and in that, the image data and the coordinate-converted three-dimensional model are compared for each pixel. On the other hand, for pixels with a discrepancy in the comparison result, it is difficult to determine whether or not the discrepancy is due to an abnormality such as occlusion, so the estimation result is greatly affected by the occlusion.

The position / orientation estimation device 1 according to the first embodiment also compares the image data with the coordinate-converted three-dimensional model as in the conventional case. On the other hand, in the position / orientation estimation device 1 according to the first embodiment, unlike the conventional case, the purpose of the comparison is to specify the equation of a plane. Therefore, the pixels having a deviation in the comparison result are ignored, and the pixels having a small deviation are ignored. Use only to identify the equation of a plane. As a result, the position / orientation estimation device 1 according to the first embodiment can obtain a stable estimation result with less influence due to shielding.

However, the shape of the work targeted by the position / orientation estimation device 1 according to the first embodiment needs to satisfy some conditions. That is, the surface of the work is composed of planes, and three or more planes whose normal vectors are independent of each other can be detected on the image in any posture.
Although the position / orientation estimation device 1 according to the first embodiment is subject to the above-mentioned restrictions, it is only necessary to specify the equations of the planes constituting the surface of the work. A robust position and orientation estimation result can be obtained.

As described above, the position / orientation estimation device 1 according to the first embodiment estimates the position / orientation of the work from the visible image, determines the effective pixels on the image, and then rotates the position / orientation based on the normal image. By correcting the translation amount of the position and orientation based on the position image after determining the effective pixels on the image, the position and orientation of the work required to grip the work using a robot hand or the like can be obtained. presume.

Here, the position and orientation are composed of six numerical values [θ _x , θ _y , θ _z , t _x , _ty , t _z ] of the amount of rotation [radian] and the amount of translation [mm] in the three-dimensional space. From the value of this position and orientation, a coordinate transformation matrix in the three-dimensional space can be generated as shown in the following equation (1). In equation (1), M represents a coordinate transformation matrix.

On the contrary, as shown in the following equation (2), each element of the position and orientation can be obtained from the elements of the coordinate transformation matrix.

Further, as in the following equation (3), any point on the three-dimensional space is coordinate-converted to a point on the three-dimensional space by using the elements of the coordinate transformation matrix. In equation (3), (x, y, z) indicates an arbitrary point in the three-dimensional space before coordinate transformation, and (x', y', z') is coordinate transformation by (m _{11 to} m ₃₄ ). It shows a point on the later three-dimensional space.

Further, as in the following equation (4), any point in the three-dimensional space is converted into image coordinates when projected onto the image by the calibration coefficient peculiar to the camera. In equation (4), (c _{11 to} c ₃₄ ) indicate the calibration coefficient, and (X, Y) indicates the image coordinates after conversion by (c _{11 to} c ₃₄ ).

In the operation example of the position / orientation estimation device 1 according to the first embodiment shown in FIG. 1, as shown in FIG. 4, the image acquisition unit 11 first acquires an image group (visible image, position image, and normal image). (Step ST401). The image group will be described with reference to FIG.

As shown in FIG. 5A, the visible image is an image on which an object is projected. The pixel data of the visible image is composed of one integer. The visible image can be acquired by imaging with a normal camera.

As shown in FIG. 5B, the position image is an image showing the three-dimensional position of the object projected on the visible image for each pixel. In the position image, each pixel shows the coordinates in the three-dimensional space, and the pixel data is composed of three floating point numbers. In the position image shown in FIG. 5B, an image of the X coordinate at the three-dimensional position, an image of the Y coordinate at the three-dimensional position, and an image of the Z coordinate at the three-dimensional position are shown in order from the top. Examples of the position image acquisition method include a spatial code method, a stereo matching method, a TOF (Time-Of-Flight) method, and the like. The spatial coding method is a method of calculating a distance from a plurality of images in which a plurality of stripes having different pitches are projected on a measurement target. The stereo matching method is a method of calculating a distance from images captured by a plurality of cameras by the principle of triangulation. The TOF method is a method of calculating a distance using the flight time of light.

As shown in FIG. 5C, the normal image is an image showing the normal vector of the object projected on the visible image for each pixel. In the normal image, the image data is composed of three floating point numbers. In the normal image shown in FIG. 5C, an image of the X-axis component in the normal vector, an image of the Y-axis component in the normal vector, and an image of the Z-axis component in the normal vector are shown in order from the top. Examples of the method for acquiring the normal image include a method using the least squares method using the surrounding pixel data in the position image, the eigenspace method, and the like.

Next, the rough estimation unit 12 estimates the position and orientation of the work based on the visible image acquired by the image acquisition unit 11 (step ST402). Examples of the estimation method that can be used by the rough estimation unit 12 include various conventionally known methods such as a method using template matching and a method using boosting. In the following, the schematic estimation unit 12 shows a method using the Hough Forest disclosed in Non-Patent Document 1. In this method, a rough estimation model trained in advance is used.

As shown in FIG. 6A, in the schematic estimation model (model indicated by reference numeral 602), when an image (fragment image) showing a fragment of the work shown by reference numeral 601 is input, the fragment image is expressed as "what posture the work is in." Which part of the image is taken when it is in? ”Is estimated, and the posture and position of the work are output. The posture of the work is the posture of the work in the learning image (the image indicated by reference numeral 603) closest to the fragment image. The position of the work is a movement amount in pixel units (movement amount indicated by reference numeral 604) from the center of the fragment image to the center of the work.

In the actual estimation by the rough estimation unit 12, first, a voting image is prepared for each learned posture. The voting image has the same size as the visible image, and each pixel value of the voting image represents the number of votes obtained as a "candidate that is the center position of the work" and is initialized with 0.

As shown in FIG. 6B, when the visible image (image shown by reference numeral 605) is input, the schematic estimation unit 12 cuts out a fragment image centered on the pixel for all the pixels and inputs it to the schematic estimation model. .. The rough estimation model outputs the posture closest to the learned posture and the amount of movement from the pixel to the center of the work, and the rough estimation unit 12 estimates the estimated posture in the voting image (image shown by reference numeral 606). 1 is added to the pixels corresponding to the center position of the work.

After the above processing is completed, the rough estimation unit 12 selects the posture and position having the largest number of votes among all the voting images, and from this information, the position posture (P ⁰ = [θ _x ⁰ , θ _y). ⁰ , θ _z ⁰ , t _x ⁰ , _ty ⁰ , t _z ⁰ ]) is obtained. The image shown by reference numeral 607 is an image showing the estimation result by the rough estimation unit 12. In this image, a circle is drawn at a position estimated by the rough estimation unit 12 on the visible image. Hereinafter, the position / posture estimated by the rough estimation unit 12 will be referred to as a rough position / posture.

Next, the rotation correction unit 13 determines the effective pixels on the image, and then determines the position / orientation (approximately position / orientation) estimated by the approximate estimation unit 12 based on the normal image acquired by the image acquisition unit 11. The amount of rotation is corrected (step ST403). The rotation correction unit 13 determines the correction amount of the rotation amount by comparing the normal vector of the three-dimensional model of the work projected on the image in the approximate position and orientation with the normal vector indicated by the normal image. Hereinafter, the position / posture after correction by the rotation correction unit 13 will be referred to as an intermediate position / posture.

That is, the rotation correction unit 13 corrects the rotation amount of the approximate position / orientation (P ⁰ = [θ _x ⁰ , θ _y ⁰ , θ _z ⁰ , t _x ⁰ , _ty ⁰ , t _z ⁰ ]) to correct the intermediate position. The posture (P ¹ = [θ _x ¹ , θ _y ¹ , θ _z ¹ , t _x ¹ , t _y ¹ , t _z ¹ ]) is obtained.

Next, the translation correction unit 14 determines the effective pixels on the image, and then, based on the position image acquired by the image acquisition unit 11, the translation correction unit 14 is among the position orientations (intermediate position postures) corrected by the rotation correction unit 13. The translation amount of is corrected (step ST404). The translation correction unit 14 determines the correction amount of the translation amount by comparing the three-dimensional position of the three-dimensional model of the work projected on the image in the intermediate position posture with the three-dimensional position indicated by the position image. Hereinafter, the position / posture after correction by the translation correction unit 14 will be referred to as a detailed position / posture.

That is, the translation correction unit 14 corrects the translation amount of the intermediate position posture (P ¹ = [θ _x ¹ , θ _y ¹ , θ _z ¹ , t _x ¹ , t _y ¹ , t _z ¹ ]) and corrects the detailed position. The posture (P ² = [θ _x ² , θ _y ² , θ _z ² , t _x ² , _ty ² , t _z ² ]) is obtained.

Next, an operation example of the rotation correction unit 13 according to the first embodiment shown in FIG. 2 will be described with reference to FIG. 7.
In the operation example of the rotation correction unit 13 in the first embodiment shown in FIG. 2, first, as shown in FIG. 7, the projection unit 131 has a three-dimensional structure on the image in the position and orientation estimated by the approximate estimation unit 12. The model is projected to generate a projected image group (projected label image, projected position image, and projected normal line image) (step ST701).

Each point on the surface of the 3D model of the work generated from the information of the CAD (Computer-Aided Design) file holds the position in the 3D space, the normal direction of the surface, the identification information of the plane to which it belongs, and the like. ing. First, the projection unit 131 generates a coordinate transformation matrix from the position / orientation estimated by the rough estimation unit 12 according to the equation (1). Next, the projection unit 131 performs coordinate conversion in the three-dimensional space for each point on the surface in the three-dimensional model of the work by the equation (3). Next, the projection unit 131 specifies the image coordinates to be projected by the equation (4), and projects various information of each point onto the image. As a result, the projection unit 131 generates a projection label image, a projection position image, and a projection normal image, as shown in FIG.

As shown in FIG. 8A, in the projection label image, the pixel data is composed of one integer value, and the identification number of the plane to which the pixel belongs is stored. If the pixel value is negative, it indicates that the pixel does not belong to any plane.

The properties of the pixel data of the projected position image are the same as those of the position image. In the projected position image shown in FIG. 8B, an image of the X coordinate at the three-dimensional position, an image of the Y coordinate at the three-dimensional position, and an image of the Z coordinate at the three-dimensional position are shown in order from the top.

The properties of the pixel data of the projected normal image are the same as those of the normal image. In the projection normal image shown in FIG. 8C, an image of the X-axis component in the normal vector, an image of the Y-axis component in the normal vector, and an image of the Z-axis component in the normal vector are shown in order from the top.

Next, the pixel determination unit 132 uses pixels belonging to an effective plane of the projected label image as effective pixels based on the image group acquired by the image acquisition unit 11 and the projected image group generated by the projection unit 131. Determine (step ST702).

At this time, the pixel determination unit 132 first determines the plane to which each pixel belongs from the pixel value of the projected label image. Here, when the pixel value is negative, the pixel determination unit 132 determines that the pixel is invalid.
Next, the pixel determination unit 132 determines that the pixel is an invalid pixel when there is a pixel in which the difference between the three-dimensional position indicated by the position image and the three-dimensional position indicated by the projected position image is equal to or greater than the threshold value. Then, the pixel determination unit 132 sets the pixel value of the invalid pixel in the projected label image to a negative value.
Further, the pixel determination unit 132 determines that the pixel is an invalid pixel when there is a pixel in which the inner product of the normal vector indicated by the normal image and the normal vector indicated by the projection normal image is equal to or less than a threshold value. Then, the pixel determination unit 132 sets the pixel value of the invalid pixel in the projected label image to a negative value.
Then, in the projected label image, the pixels that are not determined to be invalid pixels by the processing by the pixel determination unit 132 become effective pixels.

In the above, the pixel determination unit 132 makes both determinations of the pixel determination using the three-dimensional position and the pixel determination using the normal vector. However, the pixel determination unit 132 may perform only one of the above pixel determinations.

Next, the rotation update unit 133 updates the rotation amount (step ST703). The rotation update unit 133 updates the rotation amount by generating a rotation matrix based on the singular value decomposition from the difference between the normal vector shown by the projection normal image and the normal vector shown by the normal image.

The rotation update unit 133 evaluates the difference between the normal vector shown by the projected normal image and the normal vector shown by the normal image due to the approximate position and orientation, generates a coordinate transformation matrix that corrects the difference, and formulates an equation. According to (2), the intermediate position / orientation is calculated from the coordinate transformation matrix.

Here, in order to obtain the amount of rotation to be corrected, the rotation update unit 133 obtains the matrix shown in the following equation (5) by using the information of all the pixels having non-negative values in the projected label image. _{Here, (p i ', q i} ', r i ') is the normal vector of the i-th pixel in the normal image, (p, q, r) is normal to the i th pixel in the projected normal vector image Represents a vector.

Next, the rotation update unit 133 performs singular value decomposition on the matrix generated by the equation (5) to obtain a rotation matrix according to the following equations (6) and (7). In equation (7), R represents a rotation matrix.

Next, as shown in the following equation (8), the rotation update unit 133 generates a coordinate conversion matrix based on the correction amount of the rotation amount from the rotation matrix obtained by the equation (7). In equation (8), ΔM ¹⁰ indicates a coordinate transformation matrix based on the correction of the amount of rotation. Then, as shown in the following equation (9), the rotation update unit 133 multiplies the coordinate transformation matrix generated by the equation (8) with the coordinate transformation matrix based on the approximate position / orientation, thereby performing the coordinate transformation matrix based on the intermediate position / orientation. To update. In equation (9), M ⁰ indicates a coordinate transformation matrix based on the approximate position and orientation, and M ¹ indicates a coordinate transformation matrix based on the intermediate position orientation.

After that, the rotation update unit 133 determines the output intermediate position posture (P ¹ = [θ _x ¹ , θ _y ¹ , θ _z ¹ , t _x ¹ , _ty ¹ , t _z ¹ ]) according to the intermediate position posture. Obtained from the coordinate transformation matrix by equation (2).

Next, an operation example of the translation correction unit 14 according to the first embodiment shown in FIG. 3 will be described with reference to FIG.
In the operation example of the translation correction unit 14 in the first embodiment shown in FIG. 3, as shown in FIG. 9, first, the projection unit 141 is three-dimensionally displayed on the image in the position and orientation corrected by the rotation correction unit 13. The model is projected to generate a projected image group (projected label image, projected position image, and projected normal line image) (step ST901).

Next, the pixel determination unit 142 sets pixels belonging to an effective plane of the projected label image as effective pixels based on the image group acquired by the image acquisition unit 11 and the projected image group generated by the projection unit 141. Determine (step ST902).

The processing by the projection unit 141 in step ST901 and the processing by the pixel determination unit 142 in step ST902 are the same as the processing by the projection unit 131 in step ST701 and the processing by the pixel determination unit 132 in step ST702, although the data used are different. ..

Next, the translation update unit 143 updates the translation amount (step ST903). The translation update unit 143 calculates the equation of each plane of the object from the position image, and updates the translation amount by using the least squares method from the positional relationship between each pixel in the projected position image and the corresponding plane.

The translational update unit 143 evaluates the difference between the three-dimensional position indicated by the projected position image due to the intermediate position orientation and the three-dimensional position indicated by the position image, generates a coordinate conversion matrix that corrects the difference, and formulates (2). ) To calculate the detailed position and orientation from the coordinate conversion matrix.

Here, even though a certain observation point in the three-dimensional space is originally at a point on a known plane represented by ax + by + cz + d = 0, there is an unknown error (Δx, Δy, Δz) in the original coordinates. ) Is superposed as coordinates (x, y, z). Here, the coordinates (x + Δx, y + Δy, z + Δz) of the original observation point cannot be specified yet at this point, but at least they are points on a plane, so that the condition of the following equation (10) is satisfied.

Replacing the above situation with the translation update unit 143, the known plane corresponds to each plane calculated by the projection label image and the position image, and the observation point corresponds to each pixel data in the projection position image.

Here, when the equation (10) is applied to all the valid pixels in the projected label image, a plurality of equations are generated as in the equation (11). Here, _{N i} denotes the number of effective _pixels, shows the (x _i, y i, _{z i)} denotes the observed coordinates of the i-th pixel, s (i) is a plan number i-th pixel belongs shows _{(a s, b s, c} s, d s) showed a coefficient of s-th plane, (Δx, Δy, Δz) is unknown translation amount error.

When equation (11) is arranged by plane and expressed as a matrix, it is expressed by equation (12). In equation (12), N _s indicates the number of observed planes. The unknown translational amount error can be obtained by the following equation (13) using the least squares method.

As shown in the following equation (14), the detailed position and orientation (P ² = [θ _x ² , θ _y ² , θ _z ² , t _x ² , _ty ² , t _z ² ]) is the intermediate position and orientation (P ^1). It is obtained by modifying the translation amount of = [θ _x ¹ , θ _y ¹ , θ _z ¹ , t _x ¹ , t _y ¹ , t _z ¹ ]).

Detailed position and orientation (P ²⁾ is the final output of the position and orientation estimation apparatus 1.
When this detailed position / orientation is used in the bulk picking system, the position / orientation estimation device 1 notifies the bulk picking system of the detailed position / orientation as the position / orientation of the work, and the bulk picking system is based on this position / orientation. The robot hand is activated to grip the work.

As described above, according to the first embodiment, the position / orientation estimation device 1 is used for the visible image, the position image showing the three-dimensional position of the object projected on the visible image for each pixel, and the visible image. An image acquisition unit 11 that acquires a normal image showing the normal vector of the projected object for each pixel, and a rough estimation unit 12 that estimates the position and orientation of the work based on the visible image acquired by the image acquisition unit 11. After determining the effective pixels on the image based on the acquisition result by the image acquisition unit 11 and the position and orientation estimated by the rough estimation unit 12, the three-dimensional model of the work projected on the image in the position and orientation. The rotation correction unit 13 that compares the normal vector and the normal vector indicated by the normal image and corrects the amount of rotation in the position and orientation, the acquisition result by the image acquisition unit 11, and the correction by the rotation correction unit 13. After determining the effective pixels on the image based on the position and orientation of, the three-dimensional position of the three-dimensional model of the work projected on the image in the position and orientation is compared with the three-dimensional position indicated by the position image. A translation correction unit 14 for correcting the translation amount in the position and orientation is provided. As a result, the position / orientation estimation device 1 according to the first embodiment can estimate the position / orientation of the work more accurately than before, even if a part of the work is blocked by another object.

In the present invention, within the scope of the invention, it is possible to modify any component of the embodiment or omit any component of the embodiment.

1 Position / orientation estimation device 11 Image acquisition unit 12 Approximate estimation unit 13 Rotation correction unit 14 Translation correction unit 131 Projection unit (first projection unit)
132 Pixel determination unit (first pixel determination unit)
133 Rotation update unit 141 Projection unit (second projection unit)
142 Pixel determination unit (second pixel determination unit)
143 Translation Update

Claims (4)

Image acquisition to acquire a visible image, a position image showing the three-dimensional position of the object projected on the visible image for each pixel, and a normal image showing the normal vector of the object projected on the visible image for each pixel. Department and
A rough estimation unit that estimates the position and orientation of the work based on the visible image acquired by the image acquisition unit, and
After determining the effective pixels on the image based on the acquisition result by the image acquisition unit and the position and orientation estimated by the rough estimation unit, the normal of the three-dimensional model of the work projected on the image in the position and orientation. A rotation correction unit that compares the vector with the normal vector shown by the normal image and corrects the amount of rotation in the position and orientation.
After determining the effective pixels on the image based on the acquisition result by the image acquisition unit and the position and orientation after correction by the rotation correction unit, the three-dimensional model of the work projected on the image in the position and orientation is three-dimensional. A position / orientation estimation device including a translation correction unit that compares a position with a three-dimensional position indicated by a position image and corrects a translation amount in the position / orientation. The rotation correction part
A projected label image to which identification information of the plane to which each pixel belongs is given when a three-dimensional model of the work is projected on the image with the position and orientation estimated by the rough estimation unit, and when projected on the image with the position and orientation. A projected position image showing the 3D position of the 3D model of the work for each pixel, and a projection normal image showing the normal vector of the 3D model of the work for each pixel when projected onto the image in the position and orientation. The first projection unit to be generated and
Based on the acquisition result by the image acquisition unit and the generation result by the first projection unit, the first pixel determination unit that determines the pixel belonging to the effective plane of the projection label image as an effective pixel,
Based on the determination result by the first pixel determination unit, the normal vector indicated by the projection normal image generated by the first projection unit and the normal vector indicated by the normal image acquired by the image acquisition unit The first aspect of claim 1, wherein the rotation matrix is generated from the difference based on the singular value decomposition to have a rotation update unit that updates the rotation amount of the position and orientation estimated by the rough estimation unit. Position / orientation estimation device. The translation correction part
A projected label image to which identification information of the plane to which each pixel belongs is given when a three-dimensional model of the work is projected on the image in the position and orientation corrected by the rotation correction unit, and when projected on the image in the position and orientation. A projected position image showing the 3D position of the 3D model of the work for each pixel, and a projection normal image showing the normal vector of the 3D model of the work for each pixel when projected on the image in the position and orientation. The second projection unit to be generated and
Based on the acquisition result by the image acquisition unit and the generation result by the second projection unit, the second pixel determination unit that determines the pixel belonging to the effective plane of the projection label image as an effective pixel,
Based on the determination result by the second pixel determination unit, the equation of each plane of the object is calculated from the position image acquired by the image acquisition unit, and each pixel in the projection position image generated by the second projection unit is used. The first or second aspect of claim 1 or 2, characterized in that it has a translational update unit that updates the translation amount of the position and orientation corrected by the rotation correction unit by the least squares method from the positional relationship with the corresponding plane. Position / orientation estimation device. Image acquisition to acquire a visible image, a position image showing the three-dimensional position of the object projected on the visible image for each pixel, and a normal image showing the normal vector of the object projected on the visible image for each pixel. Steps and
A rough estimation step for estimating the position and orientation of the work based on the visible image acquired in the image acquisition step, and a rough estimation step.
After determining the effective pixels on the image based on the acquisition result in the image acquisition step and the position / orientation estimated in the outline estimation step, the normal vector of the three-dimensional model of the work projected on the image in the position / orientation. And the normal vector shown in the normal image, and the rotation correction step to correct the rotation amount in the position and orientation, and
After determining the effective pixels on the image based on the acquisition result in the image acquisition step and the corrected position and orientation in the rotation correction step, the three-dimensional model of the work projected on the image in the position and orientation is three-dimensional. A position / orientation estimation method having a translation correction step of comparing a position with a three-dimensional position indicated by a position image and correcting a translation amount in the position / posture.

Images