Target pose tiny change estimation method for monocular vision of humanoid robot

Abstract

The invention discloses a target pose tiny change estimation method for monocular vision of a humanoid robot, and the method comprises the steps: S1, firstly, applying tiny and accurate displacement and inclination to an object, and obtaining images before and after the change of the object through a monocular camera; s2, feature point detection is carried out on each grid object image; s3, perspective projection transformation is carried out on known grid point coordinates and unknown grid point coordinates; s4, a multivariate nonlinear optimization problem is introduced into unknown grid point coordinates, high-precision parameters of six-degree-of-freedom change of an object can be obtained, an estimation algorithm is improved, the original point of attitude change is the gravity center of a point cloud, in addition, the convergence speed of iterative calculation is also increased, and the method has the advantages of being high in robustness and high in robustness. Experimental measurement is carried out by using a simple and cheap monocular optical system, and the displacement change of about 0.25 mm and the dip angle change of 0.1 degree can be measured with high precision.

Description

Technical Field

[0001] The present invention belongs to the field of optical measurement, and in particular relates to a method for estimating minute changes in target posture using monocular vision of a humanoid robot. Background Art

[0002] Displacement sensors, which measure changes in the position of an object, are widely used in various fields, including positioning of machine tools and semiconductor devices, vibration control of large structures, disaster prevention, and structural health monitoring. Various types of sensors have been developed, depending on the type and condition of the object being measured, the required measurement range and accuracy, and the response speed.

[0003] Displacement sensor measurement methods can be broadly categorized into contact and non-contact types. Contact sensors involve contact between the sensing terminals and the object. Examples of this type include electric micrometers, which convert minute displacements of the terminals into electrical quantities for measurement, and linear scales (also known as linear encoders), which use light or magnetism to read graduations (scale positions) along a straight line. Contact sensors offer advantages such as easy installation, high accuracy, and virtually no influence from external disturbances. However, their disadvantages include being limited to rigid objects, which can sometimes damage them, a narrow measurement range, and being unsuitable for dynamic measurements during high-speed motion.

[0004] Non-contact measuring instruments use magnetism or lasers to measure without touching the object. This type includes optical sensors based on triangulation, linear encoders that detect the displacement of a scale attached to the object optically or magnetically, and laser length measuring machines based on Michelson interferometers.

[0005] Non-contact sensors have advantages such as no damage to objects, dynamic measurement, and wide measurement range, but they also have disadvantages such as being affected by external interference, being restricted by the use environment, and requiring high precision in installation and operation.

[0006] When an object's displacement is generalized to a three-dimensional position and orientation, it is represented by six parameters: the parallel displacement (position parameter), represented by the x, y, and z components; three angles (orientation parameters), representing the direction and degree of tilt; and rotation about the plane. These are referred to as 6DOF parameters. Few contact sensors can measure 6DOF. Most sensors capable of 6DOF are non-contact, image-based sensors.

[0007] Image-based pose sensing concepts such as Figure 1 As shown in Figure 1, a pattern of known shape and precise size is placed on the target plane, and its image is captured by an image sensor (monocular camera) to measure the distance to the target plane (Case 1).

[0008] Instead of setting a known pattern, feature points are extracted from the texture on the target plane and the change in distance is estimated based on the change in the image at two different times (Case 2). This paper introduces the detailed algorithms for Case 1 and Case 2.

[0009] Image-based methods for measuring 6DOF of objects can be further divided into two main categories, depending on whether the imaging system used is stereo vision or monocular vision.

[0010] Therefore, image-based methods are the most promising approaches for measuring six degrees of freedom, and they also have the advantage of being contactless. To meet the requirements of compactness and low cost, a monocular vision system with only one fixed camera is advantageous. Summary of the Invention

[0011] The purpose of the present invention is to provide a method for estimating small changes in target pose based on monocular vision of a humanoid robot, which can realize robust recognition of continuous slope terrain in the detection area by a patroller and reduce the time and space complexity of the system recognition calculation.

[0012] To solve the above technical problems, the technical solution adopted by the present invention is: a method for estimating small changes in target pose using monocular vision of a humanoid robot, comprising the following steps:

[0013] Step S1: First, a small but precise displacement and tilt are applied to the object, and a monocular camera is used to obtain images of the object before and after the change.

[0014] Step S2: Perform feature point detection on each grid object image and detect corner points using the Harris algorithm;

[0015] Step S3: Then, perspective projection transformation is performed on two cases: known grid point coordinates and unknown grid point coordinates. The known grid point coordinates are introduced into the solution of nonlinear optimization problem, and the unknown grid point coordinates are introduced into the multivariate nonlinear optimization problem.

[0016] Step S4: unknown grid point coordinates are introduced into the multivariate nonlinear optimization problem;

[0017] Step S5: Numerically estimate the 6-DOF parameters as the optimal solution to the problem, and improve the estimation algorithm so that the origin of the posture change is the center of gravity of the point cloud.

[0018] Furthermore, in step S1, the measurement object of this method is a gridded quartz glass plate with a 30×30 grid and a grid spacing of 5 mm. A Raspberry Pi camera module is used. A z-axis stage and a tilt stage are used to capture images by applying small but precise displacements and tilts to the object. An optical table is fixed to an optical table surface, and a glass grid pattern placed on the optical table serves as the measurement object. A camera is fixed above the grid pattern. The optical table precisely shifts the grid object while simultaneously capturing images.

[0019] Furthermore, in step S2, considering the performance of the optical system, image information other than the grid pattern caused by the shooting environment, and the line thickness of the grid pattern, the parameters of the cornerHarris function of the OpenCV library are set to blocksize=10, ksize=11, and k=0.08.

[0020] Furthermore, in step S3, two cases are respectively described, one in which the position of the grid point cloud is known, and the other in which the position of the grid point cloud is unknown, and the number of solution parameters is different.

[0021] Furthermore, in step S4, the number of unknowns is represented as U, and the number of expressions is represented as C. We have U = 3m + 6(n-1), and C = 2mn. When n = 1, we have U = 3m and C = 2m. Since U > C, the solution cannot be found for any m. However, when n = 2, U = 3m + 6 and C = 4m. Therefore, U ≤ C can be found when m ≥ 6.

[0022] Furthermore, in step S5, the origin of the world coordinate system is used as the center point of the posture change. Therefore, if the posture changes slightly, the angle change is minimal when the origin is used as the center, making estimation difficult. In addition, the convergence rate of the iterative calculation is slowed. To solve this problem, this patent has made an improvement so that the origin of the posture change is the center of gravity g of the point cloud.

[0023] In summary, due to the adoption of the above technical solution, the beneficial effects of the present invention are:

[0024] This paper proposes an algorithm for estimating the 6DOF of an object based on a point cloud extracted from an image. This algorithm, based on solving a nonlinear optimization problem, demonstrates its solution using the L-BFGS method. The algorithm is then applied to real-world images for 6DOF measurement, and the accuracy of each measurement variable is verified separately. Unlike traditional methods, this method uses only a monocular camera to obtain highly accurate pose changes of the object, accurately measuring displacement changes of approximately 0.25 mm and tilt changes of 0.1 degrees.

[0025] 2. The method proposed in this paper can obtain high-precision parameters for the six degrees of freedom of an object. It also improves the estimation algorithm so that the origin of the pose change is the center of gravity of the point cloud. This solves the problem of difficulty in estimating angles when the origin is used as the center. Furthermore, the convergence speed of the iterative calculation is improved.

[0026] 3. The present invention uses a simple and inexpensive monocular optical system to perform experimental measurements, which can measure displacement changes of about 0.25 mm and tilt changes of 0.1 degrees with high precision. This accuracy confirms the effectiveness of the proposed method. Image-based methods are the most promising methods for measuring six degrees of freedom, and they also have the advantage of being non-contact. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] To facilitate understanding by those skilled in the art, the present invention is further described below with reference to the accompanying drawings.

[0028] Figure 1 A schematic diagram of a method for estimating slight pose changes based on a monocular camera provided by one embodiment of the present invention.

[0029] Figure 2 A schematic diagram of perspective projection transformation provided by an embodiment of the present invention.

[0030] Figure 3 A schematic diagram of the rotation and translation of a coordinate system provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0031] The following will clearly and completely describe the technical solutions of the present invention with reference to the embodiments. Obviously, the embodiments described are only some of the embodiments of the present invention, rather than all of them. The embodiments of the present invention and all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0032] The present invention provides a method for estimating slight changes in target posture using monocular vision of a humanoid robot, comprising the following steps:

[0033] Step 1: This method uses a gridded quartz glass plate with a 30×30 grid and a 5mm grid spacing. A Raspberry Pi camera module, a z-axis stage, and a tilt stage are used to capture images by applying small but precise displacements and tilts to the object. An optical table is fixed to the surface of the optical table, and a glass grid pattern placed on the table serves as the measurement object. A camera is fixed above the grid pattern. The optical table precisely shifts the grid pattern while simultaneously capturing images.

[0034] The grid plates were placed about 70 mm away from the camera, facing each other, and a Z-stage was used to apply precise displacements of -2, -1, -0.5, -0.25, -0.1, +0.1, +0.25, +0.5, +1, and +2 mm in the Z-axis direction.

[0035] Each image is subjected to grid point detection using a grid pattern, and displacement is estimated using this patented method. Among the feature points detected in the resulting image, a 9×9 grid point grid close to the image center is used for estimation to avoid distortion at the image edges.

[0036] Feature extraction

[0037] Harris corner detection in OpenCV (version 2.4.9.1) is used to detect grid points.

[0038] The following is a brief introduction to the principle of the algorithm. In order to detect corners, the first step is to find the difference in pixel values of a given pixel position (u, v) in all directions.

[0039] This can be expressed by the following equation.

[0040]

[0041] Where ω(x,y) is a window function that represents the weight of each pixel.

[0042] For inflection point detection, we obtain the coordinates (u, v) that maximize E(u, v). In equation (1), we maximize I(x, y). The following equation is derived from equation (1).

[0043]

[0044] Where M is a matrix defined as follows:

[0045]

[0046] In addition, I x and I y denote the gradients in the x and y directions respectively and are calculated using a Sobel filter with the following weighting factors.

[0047]

[0048] Note that these are the weight coefficients for the 8-neighborhood case. This method uses a Sobel filter with 11×11 weight coefficients.

[0049] Through the above process, a score R defined by the following formula is calculated to determine whether the corner is included in the search window.

[0050] R=det(M)-k(trace(M)) 2 ;

[0051] Where det(M) = λ1λ2, trace(M) = λ1+λ2, λ1, λ2 are the eigenvalues of M.

[0052] These eigenvalues determine whether the region of interest is a corner, an edge, or a flat area; if R is large, that is, λ1 and λ2 are both large, then the region is considered to be a corner. In this method, considering the performance of the optical system, the image information outside the grid pattern caused by the shooting environment, and the line thickness of the grid pattern, the parameters of the cornerHarris function of the OpenCV library are set to blocksize = 10, ksize = 11, and k = 0.08. Among them, blocksize is the size of the adjacent area considered in corner detection, and ksize is the number of pixels in the gradient I. x and I y The kernel size of the gradient operator of the Sobel function is , and k is the threshold for corner detection. Since the size of the adjacent region considered for corner detection is set to 10, when detecting grid points, multiple points are detected as corners at each grid point. Therefore, in this method, we calculate the average pixel coordinates of a group of pixels near a grid point, round them to the nearest integer, and use them as the pixel coordinates of the grid point.

[0053] Perspective projection transformation

[0054] like Figure 2 As shown, establish an XYZ axis (world coordinate system) with the center of the camera lens as the origin O and the optical axis as the Z axis (the positive direction of the imaged object). The X axis and Y axis should coincide with the column direction and row direction of the camera image, respectively. Note that this coordinate system is a right-handed coordinate system. Let the focal length OC of the camera be represented by f. Let (u, v) represent the position of a point on the image captured by the camera (image coordinates: units are pixels), and a×a represent the pixel spacing. Project the point at position (x, y, z) in the world coordinates to position (u, v) in the image coordinates. In the perspective projection model, the relationship between the world coordinates and the image coordinates is expressed as

[0055]

[0056] Where (c u ,c v ) is the image coordinate of point C (usually the center of the image) where the optical axis passes through the camera image. The above equation can be rewritten as

[0057]

[0058] Where κ = f / a;

[0059] In general, the camera's internal parameters κ,c u ,c v It needs to be determined through camera calibration experiments. Currently, only the parameter values of the camera in the experiment are used: 3280×2464 pixels), a=1.12μm, f=3.04mm, κ=3.04 / 1.12×103, (c u ,c v )=(1647.5,1255.5)(center of image).

[0060] The Cartesian coordinate system fixed on the object to be measured is called the object coordinate system. The basis vectors on the X-, Y-, and Z-axes of the object system are represented by X B ,Y B ,Z B The position coordinates of a point in the object system are represented by r B , expressed as r in world coordinates I ( Figure 3 ). The relationship between these two coordinates is expressed by the following formula.

[0061]

[0062] in is the world coordinate of the origin of the target system, which represents the translation of the target system. (X B Y B Z B ) is a B ,Y B ,Z B is a column vector rotation matrix that represents the rotation of the target system. The rotation matrix is composed of the rotation matrix R around the Z axis z (ψ), the rotation matrix R of angle θ around the Y axis y (θ), the rotation matrix R of angle φ around the X axis x (φ) can be written as (rotate in the order of Z axis, Y axis, and X axis)

[0063] (X B Y B Z B )=R z (ψ)R y (θ)R x (φ);

[0064] In the formula

[0065]

[0066] 6-DOF pose estimation algorithm

[0067] Case 1: Imaging mode with known geometry

[0068] The position coordinates of the known point cloud in the target system are expressed as r i =(x i ,y i, z i ) T (i=1,2,…,m), where m is the number of points and the superscript T is the transpose of the matrix / vector. Let p i =(u i ,v i ) T The measured coordinates of the point cloud projected onto the image. It can be calculated by the coordinate system rotation, translation and perspective projection model, and is obtained by φ,θ,ψ,c x ,c y ,c z ,r i Therefore, the function p can be expressed as The specific calculation process of function p is as follows:

[0069]

[0070] Since the number of unknowns is 6 and the number of equations is 2m, it can be solved when m ≥ 3. Case 1 gives the three-dimensional coordinates r of the point cloud i and image coordinates p i Let e i residual

[0071]

[0072] L is the mean square of the residuals:

[0073]

[0074] Then the parameters phi,θ,ψ,c x ,c y ,c z Estimate the optimal solution that minimizes L. Because this minimization problem is nonlinear, it requires iterative calculations to find the solution. Here, we use the limited memory Broyden-Fletcher-Goldfarb-Shanno (L-BFGS) method, which is a quasi-Newton method that can be used for large-scale problems with high computational speed.

[0075] The calculation of the L-BFGS method requires calculating the gradient of L, without calculating the Jacobian matrix. The gradient can be derived as follows. First, perform partial differentiation on Equation (16) to obtain the partial derivative of φ as follows:

[0076]

[0077]

[0078] The same calculation can be performed for the partial derivatives of θ and ψ. x The partial derivatives of are as follows:

[0079]

[0080] This gives the following formula:

[0081]

[0082] Similarly, for c y ,c z , we can get the following equation.

[0083]

[0084] Case 2: For imaging modalities with unknown geometry

[0085] Object System i (i = 1, 2, ..., m) unknown coordinates, when the object's position and orientation change, take n images. In this case, the following equation holds for j = 1, 2, ..., n.

[0086]

[0087]

[0088] The first image (j=1) can be used as a reference, where the target system is equal to the reference system. Therefore, φ1,θ1,ψ1,c x,1 ,c y,1 ,c z,1 are all zero and known.

[0089] The number of unknowns is U, and the number of expressions is C. We have U = 3m + 6(n-1), and C = 2mn. When n = 1, we have U = 3m and C = 2m. Since U > C, the solution cannot be found for any m. However, when n = 2, U = 3m + 6 and C = 4m, so U ≤ C can be found for m ≥ 6.

[0090] As in case 1, consider the residual The square mean L minimization problem,

[0091]

[0092] The gradient of L can be calculated as follows (only including index j). First, j The partial derivatives are as follows:

[0093]

[0094] θ j , ψ j The same calculations were performed for some of the derivations of . In addition, x,j The partial derivative of can be obtained:

[0095]

[0096]

[0097] From the previous equation,

[0098]

[0099] C y,j ,c z,j The same calculation is performed on the partial derivative of , and the following formula is obtained.

[0100]

[0101] In case 2, we also need to i To perform partial differentiation, we use the fact that x i The only item is

[0102]

[0103] improve

[0104] In the first stage of Case 2, the origin of the world coordinate system is used as the center point of the pose change. Therefore, if the pose changes slightly, the angle changes very little when the origin is used as the center, making estimation difficult. In addition, the convergence speed of the iterative calculation is slowed down.

[0105] In order to solve this problem, this patent has made improvements so that the origin of the posture change is the center of gravity g of the point cloud.

[0106]

[0107]

[0108] Note that the calculation of the center of gravity also includes unknowns. The calculation of the gradient in case 2 only changes in the following part.

[0109]

[0110] The preferred embodiments of the present invention disclosed above are intended only to help illustrate the present invention. These preferred embodiments do not exhaustively describe all details, nor do they limit the present invention to specific embodiments. Obviously, many modifications and variations are possible based on the contents of this specification. These embodiments are selected and described in detail in this specification to better explain the principles and practical applications of the present invention, thereby enabling those skilled in the art to better understand and utilize the present invention. The present invention is limited only by the claims and their full scope and equivalents.

Claims

1. A method for estimating small changes in target pose using monocular vision of a humanoid robot, characterized in that: The following steps are involved: Step S1: First, a small but precise displacement and tilt are applied to the object, and a monocular camera is used to obtain images of the object before and after the change. Step S2: Perform feature point detection on each grid object image and detect corner points using the Harris algorithm; Step S3: Then, perspective projection transformation is performed on two cases: known grid point coordinates and unknown grid point coordinates. The known grid point coordinates are introduced into the solution of nonlinear optimization problem, and the unknown grid point coordinates are introduced into the multivariate nonlinear optimization problem. Step S4: unknown grid point coordinates are introduced into the multivariate nonlinear optimization problem; Step S5: Numerically estimate the 6-DOF parameters as the optimal solution to the problem, and improve the estimation algorithm so that the origin of the posture change is the center of gravity of the point cloud.

2. The method for estimating slight changes in target pose using monocular vision of a humanoid robot according to claim 1, characterized in that: In step S1, the measurement object is a gridded quartz glass plate with a 30×30 grid and a 5mm grid spacing. A Raspberry Pi camera module is used. A z-axis stage and a tilt stage are used to capture images by applying small but precise displacements and tilts to the object. An optical table is fixed to the optical table surface, and a glass grid pattern placed on the table serves as the measurement object. A camera is fixed above the grid pattern. The optical table precisely shifts the grid object while simultaneously capturing images.

3. The method for estimating small changes in target pose using monocular vision of a humanoid robot according to claim 1, characterized in that: In step S2, considering the performance of the optical system, image information other than the grid pattern caused by the shooting environment, and the line thickness of the grid pattern, the parameters of the cornerHarris function of the OpenCV library are set to blocksize=10, ksize=11, and k=0.

08.

4. The method for estimating small changes in target pose using monocular vision of a humanoid robot according to claim 1, wherein: In step S3, two cases are respectively described, one in which the position of the grid point cloud is known, and the other in which the position of the grid point cloud is unknown, and the number of solution parameters is different.

5. The method for estimating slight changes in target pose using monocular vision of a humanoid robot according to claim 1, characterized in that: In step S4, the number of unknowns is represented by U, and the number of expressions is represented by C. We have U = 3m + 6(n-1), and C = 2mn. When n = 1, we have U = 3m and C = 2m. Since U > C, the solution cannot be found for any m. However, when n = 2, U = 3m + 6 and C = 4m. Therefore, U ≤ C can be found when m ≥ 6.

6. The method for estimating small changes in target pose using monocular vision of a humanoid robot according to claim 1, characterized in that: In step S5, the origin of the world coordinate system is used as the center of the pose change. Therefore, if the pose changes slightly, the angle changes very little when the origin is used as the center, making estimation difficult. Furthermore, the convergence rate of the iterative calculation slows down. To address this issue, this patent improves the pose change so that the origin is the center of gravity g of the point cloud.

Images