Method for Measuring 6D Pose of Workpiece Based on Monocular Vision and Feature Markers

Abstract

The present invention discloses a method for measuring the 6D pose of a workpiece based on monocular vision and feature markers, comprising the following steps: driving a monocular RGB camera through OpenCV to obtain real-time images. The computer preprocesses the images, extracts the region where the feature markers are located, and detects the pixel coordinates of the inner corner points on the feature markers. Calibrating the monocular RGB camera to obtain the internal parameter matrix and distortion matrix of the camera. Calculating the rotation matrix and displacement matrix between the feature markers and the monocular RGB camera through the PnP algorithm. Determining the pose and height of the workpiece using the obtained internal parameter matrix, distortion matrix, rotation matrix, and displacement matrix. Compared with the existing workpiece 6D pose measurement technologies, the present invention has the characteristics of simple structure, low operation difficulty, and low computer hardware requirements. At the same time, it can also detect the height defects of the workpiece. The present invention only requires an ordinary monocular RGB camera and feature markers to quickly and accurately detect the 6D pose and height defects of the workpiece.

Description

Technical Field

[0001] The present invention relates to the technical field of workpiece pose recognition, and in particular to a method for measuring the 6D pose of a workpiece based on monocular vision and feature markers, especially a method that can quickly measure the pose change and height defect of a workpiece on a detection table only with an ordinary monocular RGB camera. Background Art

[0002] The 6D pose refers to the three-dimensional pose of an object, that is, the position and pose of the object based on the plane vector and the rotation vector. At present, 6D pose estimation is related to many industrial fields, and many pose estimation applications have been developed, such as autonomous driving, robot automatic grasping, etc. On the production line, intelligent robots use 6D pose technology to identify and grasp objects. In the current popular field of autonomous driving, autonomous vehicles use 6D pose to identify roads and different obstacles.

[0003] At present, 6D pose estimation of an object can be realized based on RGB-D camera data. However, due to the limitations of the RGB-D camera itself and environmental factors, not only is the computational complexity high, but it can only be applied to some specific environments. And the method for 6D pose estimation based on a monocular RGB camera still has problems such as poor detection accuracy and is difficult to apply.

[0004] For the pose measurement of workpieces, the best current solution is to use an attitude sensor. An attitude sensor is a high-performance three-dimensional motion attitude measurement system based on micro-electro-mechanical system (MEMS). It usually includes a three-axis accelerometer, a three-axis gyroscope, a three-axis electronic compass, etc. Due to its complex structure and high price, it is difficult to apply it to the production line because one sensor is required to measure the 6D pose change of one workpiece. For workpieces on the production line, this solution is obviously uneconomical. Summary of the Invention

[0005] The technical problem to be solved by the embodiments of the present invention is to provide a method for measuring the 6D pose of a workpiece based on monocular vision and feature markers, which can solve the problems of high price and inconvenient operation of existing workpiece attitude measurement instruments. The present invention has a simple structure, good real-time performance, high measurement accuracy, and can also detect whether the height of the workpiece meets the standard, and is particularly suitable for detecting the pose and height of workpieces on a production line.

[0006] To solve the above technical problems, the embodiments of the present invention provide a method for measuring the 6D pose of a workpiece based on monocular vision and feature markers, including the following steps:

[0007] S1: Set the reference feature markers and workpiece feature markers of the workpiece detection transfer table;

[0008] S2: Calibrate the monocular RGB camera;

[0009] S3: Establish a world coordinate system at the reference feature markers and a camera coordinate system at the camera;

[0010] S4: Obtain the real-time image of the workpiece through the monocular RGB camera and preprocess the image;

[0011] S5: Detect and extract the regions where the feature markers are located in the preprocessed image, obtain all the regions containing feature markers in the image, and obtain the pixel coordinates of all the inner corner points on the feature markers through the inner corner point detection algorithm;

[0012] S6: Calculate the rotation matrix R and displacement matrix T of the reference feature markers and workpiece feature markers relative to the camera based on the camera pinhole imaging model;

[0013] S7: Calculate the workpiece height according to the rotation matrix R and displacement matrix T of the reference feature markers and workpiece feature markers relative to the camera;

[0014] S8: According to the principle of invariant camera pose and use the rotation matrix to solve the workpiece pose.

[0015] Among them, S2 includes obtaining the internal parameter matrix K and distortion coefficient D of the monocular RGB camera using the Zhang Zhengyou calibration method.

[0016] Among them, S3 includes the steps: World coordinate system Take the first inner corner point at the upper left corner of the feature marker as the origin, the horizontal right direction as the X-axis, the vertical downward direction as the Y-axis, and determine the Z-axis based on the right-hand rule. Camera coordinate system A three-dimensional rectangular coordinate system established with the focus center of the camera as the origin and the optical axis as the Z-axis.

[0017] Among them, in S5, the feature marker uses a black and white checkerboard calibration plate with 8 rows and 9 columns, and there are a total of 56 groups of inner corner points for detection.

[0018] Among them, in S6, it includes using the PnP reprojection iterative algorithm, substituting 56 groups of inner corner points, and obtaining the rotation matrix R and displacement matrix T between the feature marker and the camera.

[0019] Among them, the derivation formulas of the rotation matrix R and displacement matrix T are:

[0020]

[0021] In the formula , , is the coordinate of the interior corner point in the world coordinate system, which is a known quantity. , , are the coordinates of the interior corner point in the camera coordinate system, which are unknown quantities.

[0022] Among them, the formula for calculating the height of the workpiece is:

[0023]

[0024] Among them , are the rotation matrix and displacement matrix between the reference feature marker and the camera, , are the rotation matrix and displacement matrix between the workpiece feature marker and the camera, , , are respectively the coordinates of the reference feature marker, the coordinates of the workpiece feature marker and the coordinates of the camera in the world coordinate system. Since the world coordinate system is established based on the reference feature marker, there are:

[0025]

[0026]

[0027] In the formula , , are known, is the height of the workpiece.

[0028] Among them, the said S8 includes the steps:

[0029] According to the principle of invariant camera pose, it can be known that:

[0030]

[0031]

[0032] Among them represents the pose of the reference feature marker, represents the pose of the monocular RGB camera, represents the pose of the workpiece feature marker. Then the rotation matrix from the reference feature marker to the workpiece feature marker, and finally the workpiece pose is solved through the rotation matrix.

[0033] Implementing the embodiments of the present invention has the following beneficial effects: The present invention rigidly connects the workpiece to be measured with a specific marker for identification, uses the PnP principle to calculate the displacement matrix T and rotation matrix R between the object to be measured and the camera, thereby estimating the camera pose. Finally, using the camera as a reference object, the pose change and height change of the workpiece on the assembly line relative to the reference feature marker are calculated. Compared with the existing 6D pose measurement technology for workpieces, the present invention has the characteristics of simple structure, low operation difficulty, and low computer hardware requirements. At the same time, it can also detect the height defects of the workpiece. The present invention only requires an ordinary monocular RGB camera and feature markers to quickly and accurately detect the 6D pose and height defects of the workpiece. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 is a schematic flowchart of the method of the present invention;

[0035] Figure 2 is a schematic diagram of the 6D pose measurement system for workpieces of the present invention;

[0036] Figure 3 is a schematic diagram of the feature marker and the world coordinate system of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0037] To make the objectives, technical solutions, and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings.

[0038] As Figure 1 , 2 shown, the method for measuring the 6D pose of a workpiece based on monocular vision and feature markers according to the embodiments of the present invention sets the reference feature marker and the workpiece feature marker of the workpiece detection conveyor table and is implemented through the following steps.

[0039] First, calibrate the monocular RGB camera to obtain the internal parameter matrix K and distortion coefficient D of the camera.

[0040] Establish a world coordinate system at the feature marker and a camera coordinate system at the monocular RGB camera.

[0041] World coordinate system Take the first inner corner point at the upper left corner of the feature marker as the origin, the horizontal right direction as the X-axis, the vertical downward direction as the Y-axis, and determine the Z-axis based on the right-hand rule. Camera coordinate system A three-dimensional rectangular coordinate system established with the focus center of the camera as the origin and the optical axis as the Z-axis.

[0042] Then rigidly connect the feature marker with the workpiece to be measured, and send the workpiece to the detection area of the monocular RGB camera through the conveyor belt.

[0043] Obtain the real-time image of the workpiece to be measured on the assembly line through a monocular RGB camera, and preprocess the image, including image resolution adjustment, image encoding format adjustment, and image noise elimination. Perform Gaussian filtering on the real-time image to reduce noise. The monocular RGB camera adopted in the present invention has a resolution of 2560×1440, a video encoding format of Motion JPEG, and uses a Gaussian filter to reduce image noise. Its kernel is as follows:

[0044]

[0045] Detect and extract the area where the feature markers are located in the preprocessed image, obtain all the areas containing feature markers in the image (including the reference feature marker area and the workpiece feature marker area), and obtain the pixel coordinates of all the interior corner points on the feature markers through the interior corner point detection algorithm.

[0046] As Figure 3 shown, the feature marker uses a black-and-white checkerboard calibration board with 8 rows and 9 columns, and there are a total of 56 groups of interior corner points available for detection. Using the PnP reprojection iterative algorithm, substitute the 56 groups of interior corner points to obtain the rotation matrix R and displacement matrix T between the feature marker and the camera.

[0047] Refine all the interior corner point pixel coordinates to the sub-pixel level, and use the find4QuadCornerSubpix function in OpenCV to obtain more accurate interior corner point pixel coordinates.

[0048] Based on the camera pinhole imaging model, calculate the rotation matrix R and displacement matrix T of the reference feature marker and the workpiece feature marker relative to the camera. The derivation formula is as follows:

[0049]

[0050] In the formula , , are the coordinates of the interior corner points in the world coordinate system, which are known quantities. , , are the coordinates of the interior corner points in the camera coordinate system, which are unknown quantities. If we want to solve for R and T, we need more than 4 groups of interior corner points. In fact, there are a total of 56 groups of 2D-3D corresponding interior corner points, so it is an overdetermined equation, and the least squares method is used to obtain the closest solution.

[0051] The formula for calculating the workpiece height based on the rotation matrix R and displacement matrix T of the reference feature marker and the workpiece feature marker relative to the camera is:

[0052]

[0053] Where , The rotation matrix and displacement matrix between the reference feature marker and the camera. and The rotation matrix and displacement matrix between the workpiece feature marker and the camera. and and are respectively the coordinates of the reference feature marker, the coordinates of the workpiece feature marker, and the coordinates of the camera in the world coordinate system. Since the world coordinate system is established based on the reference feature marker, there are:

[0054]

[0055] So there are:

[0056]

[0057] where is the height difference from the reference feature marker to the workpiece feature marker. Also, since the workpiece feature marker is rigidly connected to the workpiece to be measured at the top, this height difference is the height of the workpiece to be measured.

[0058] When there is a rotational change in the workpiece, assume the vector perpendicular to the reference feature marker and pointing outwards represents the attitude of the reference feature marker, represents the attitude of the monocular RGB camera, represents the attitude of the workpiece feature marker (i.e., the attitude of the workpiece), then there are:

[0059]

[0060]

[0061] Then the rotation matrix from the reference feature marker to the workpiece feature marker .

[0062] Finally, through the rotation matrix the attitude change of the workpiece to be measured around the world coordinate system is obtained.

[0063] As the workpiece on the production line moves, continue to detect the 6D pose and height of the next workpiece to be measured, and screen out the defective workpieces.

[0064] The above-disclosed is only a preferred embodiment of the present invention. Of course, the scope of the rights of the present invention cannot be limited by this. Therefore, equivalent changes made according to the claims of the present invention still fall within the scope covered by the present invention.

Claims

1. A method based on monocular vision and feature markers for measuring the 6D pose of a workpiece, characterized in that It includes the following steps: S1: Set the reference feature mark and the workpiece feature mark of the workpiece detection transfer table; S2: Calibrate the monocular RGB camera; S3: Establish a world coordinate system at the reference feature mark and a camera coordinate system at the camera; S4: Obtain the real-time image of the workpiece through the monocular RGB camera and preprocess the image; S5: Detect and extract the area where the feature mark is located in the preprocessed image, obtain all areas containing feature marks in the image, and obtain the pixel coordinates of all inner corner points on the feature mark through the inner corner point detection algorithm; S6: Calculate the rotation matrix R and the displacement matrix T of the reference feature mark and the workpiece feature mark relative to the camera based on the camera pinhole imaging model; S7: Calculate the workpiece height according to the rotation matrix R and the displacement matrix T of the reference feature mark and the workpiece feature mark relative to the camera; The derivation formulas of the rotation matrix R and the displacement matrix T are: wherein , , are the coordinates of the interior corner points in the world coordinate system, which are known quantities, , , are the coordinates of the interior corner points in the camera coordinate system, which are unknown quantities; The formula for calculating the workpiece height is: wherein and are the rotation matrix and displacement matrix between the reference feature marker and the camera, and are the rotation matrix and displacement matrix between the workpiece feature marker and the camera, and and are the coordinates of the reference feature marker, the coordinates of the workpiece feature marker, and the coordinates of the camera in the world coordinate system respectively. Since the world coordinate system is established based on the reference feature marker, there are: wherein , , are known, is the height of the workpiece; S8: According to the principle of constant camera pose, use the rotation matrix to solve the workpiece pose.

2. The method for measuring the 6D pose of a workpiece based on monocular vision and feature markers according to claim 1, characterized in that, S2 includes obtaining the internal parameter matrix K and the distortion coefficient D of the monocular RGB camera using the Zhang Zhengyou calibration method.

3. The method for measuring the 6D pose of a workpiece based on monocular vision and feature markers according to claim 2, wherein, The S3 includes the steps: world coordinate system Taking the first inner corner point at the upper left corner of the feature marker as the origin, with the horizontal right direction as the X-axis, the vertical downward direction as the Y-axis, and determining the Z-axis based on the right-hand rule, camera coordinate system A three-dimensional rectangular coordinate system established with the focus center of the camera as the origin and the optical axis as the Z-axis.

4. The method for measuring the 6D pose of a workpiece based on monocular vision and feature markers according to claim 3, wherein In S5, the feature mark uses a black and white checkerboard calibration plate with 8 rows and 9 columns, and there are a total of 56 groups of inner corner points for detection.

5. The method for measuring the 6D pose of a workpiece based on monocular vision and feature markers according to claim 4, wherein In S6, it includes using the PnP reprojection iterative algorithm, substituting 56 groups of inner corner points, and obtaining the rotation matrix R and the displacement matrix T between the feature mark and the camera.

6. The method for measuring the 6D pose of a workpiece based on monocular vision and feature markers according to claim 1, characterized in that, S8 includes the steps: According to the principle of constant camera pose, it can be known that: where represents the pose of the reference feature marker, represents the pose of the monocular RGB camera, represents the pose of the workpiece feature marker, then the rotation matrix from the reference feature marker to the workpiece feature marker is , and finally the workpiece pose is calculated through the rotation matrix.

Images