Three-line structured light based pose measurement method for non-cooperative target of rod

Abstract

This invention relates to the field of machine vision measurement technology, and particularly to a method for measuring the pose of a non-cooperative target rod based on three-line structured light. The method involves calibrating a three-line laser using a laser tracker to obtain the corresponding optical plane equations for each of the three structured light lines; placing the rod within the camera's field of view and illuminating it with the three-line lasers, extracting the center coordinate set of each optical blade in the captured image; using the three optical plane equations and the corresponding center coordinate sets of the optical blades, calculating the overall centroid position of all optical blades and the centroid position of each individual optical blade; fitting the centroid positions of the three optical blades to obtain the axial direction vector of the rod; optimizing the overall centroid position and axial direction vector using machine learning, and converting the optimal centroid position and optimal direction vector into a six-degree-of-freedom pose of the rod. This invention offers high accuracy, a simple process, strong anti-interference capabilities, and is suitable for engineering applications.

Description

Technical Field

[0001] This invention belongs to the field of machine vision measurement technology, and particularly relates to a method for measuring the pose of non-cooperative targets on rods based on three-line structured light. Background Technology

[0002] In fields such as industrial manufacturing, aerospace engineering, and robotics, it is often necessary to measure the pose of rod-shaped targets. Many of these rods are non-cooperative targets, meaning that it is impossible to attach markers, QR codes, or other cooperative identifiers to their surfaces; measurement must rely solely on the target's inherent geometric features. Examples include the support rods for satellite solar panels in aerospace, steel pipes in industrial production, and rod-shaped workpieces manipulated by robotic arms. The accuracy and efficiency of pose measurement for these rods directly impact the reliability of subsequent operations.

[0003] Currently, pose measurement methods for non-cooperative targets such as rods are mainly divided into two categories: contact measurement and non-contact measurement. While contact measurement offers high accuracy, it is cumbersome, inefficient, cannot achieve real-time measurement, and is prone to damaging the target surface, making it unsuitable for fragile or high-precision rod targets. Non-contact measurement primarily includes visual measurement and lidar measurement, with visual measurement becoming a current research hotspot due to its advantages of being non-contact, efficient, and low-cost.

[0004] Among existing visual measurement methods, monocular vision measurement is limited by the inability to acquire depth information from a single image, resulting in low measurement accuracy and ambiguity in pose calculation. While binocular vision measurement can acquire depth information, it requires complex camera calibration and image matching processes, has weak anti-interference capabilities, and performs poorly in complex scenarios such as varying lighting and target occlusion. Line structured light vision measurement combines the advantages of visual measurement and laser measurement. By projecting structured light onto the target surface and extracting light blade features to achieve 3D reconstruction, it features high measurement accuracy and strong anti-interference capabilities, and has been widely used in the field of 3D measurement.

[0005] However, existing methods for non-cooperative target pose measurement of rods based on line structured light mostly employ single or two line structured lights, which suffer from problems such as unstable light knife feature extraction, low accuracy of direction vector fitting, and large pose calculation errors. A single line structured light cannot fully reflect the spatial attitude of the rod, while two line structured lights are easily affected by noise when fitting the direction vector, resulting in a large deviation in the initial attitude value, which in turn affects the accuracy of subsequent optimization. At the same time, existing methods do not fully utilize the geometric features of the rod to construct error equations during the pose optimization process, resulting in a large deviation between the optimized result and the actual pose, making it difficult to meet the requirements of high-precision measurement. In addition, existing line structured light calibration mostly relies on the PnP algorithm, which is greatly affected by the accuracy of target feature extraction and camera intrinsic parameter errors, resulting in limited calibration accuracy, which further restricts the improvement of overall pose measurement accuracy.

[0006] Therefore, developing a simple, interference-resistant, and highly accurate method for measuring the pose of non-cooperative targets on rods based on three-line structured light, which can effectively solve the above problems, has significant practical application value. Summary of the Invention

[0007] In view of this, the present invention aims to provide a method for measuring the pose of non-cooperative targets such as rods based on three-line structured light. By leveraging the synergistic effect of three-line structured light, the stability of optical blade feature extraction and the accuracy of direction vector fitting are improved. An error equation is constructed by combining the LM optimization algorithm and the rod radius features, thereby achieving high-precision pose measurement of non-cooperative targets such as rods. This invention features high accuracy, a simple process, and strong anti-interference capabilities, making it suitable for engineering applications.

[0008] To achieve the above objectives, the technical solution created by this invention is implemented as follows: A method for measuring the pose of a non-cooperative target on a rod based on three-line structured light includes: S1: The three-line laser is calibrated using a laser tracker and camera to obtain the optical plane equations corresponding to the three line structured lights emitted by the three-line laser. S2: Place the rod within the camera's field of view and illuminate it with a three-line laser, so that the light blades of the three-line structured light are fully projected onto the surface of the rod; extract the set of center coordinates of each light blade in the image captured by the camera; S3: Using the three optical plane equations obtained in step S1 and the set of center coordinates of the corresponding optical cutters in step S2, calculate the overall center of gravity position of all optical cutters and the center of gravity position of each optical cutter; fit the center of gravity positions of the three optical cutters to obtain the axial direction vector of the rod. S4: Using the overall center of gravity position obtained in step S3 and the axis direction vector obtained in step S4 as the objects to be optimized, perform machine learning, and convert the obtained optimal center of gravity position and optimal direction vector into the six-degree-of-freedom pose of the rod.

[0009] Furthermore, step S1 includes: S11: Control the three-line laser to transmit three-line structured light to the white back plate, and control the camera to capture the three-line structured light beam completely projected onto the back plate surface to obtain a three-light beam image. S12: Use a laser tracker to measure the first spatial plane equation parameters of the current white backplate in the reference coordinate system in step S11, and combine the camera extrinsic parameter matrix to transform the first spatial plane equation parameters to the camera coordinate system to obtain the second spatial plane equation parameters. S13: Adjust the pose of the white backplate multiple times, and repeat steps S11~S12 to obtain the three-light knife image and the second spatial plane equation parameters corresponding to each pose of the white backplate. S14: Extract the set of center coordinates of each light blade in each three-light blade image obtained in step S13, and use the second spatial plane equation parameters obtained in step S13 to transform the set of center coordinates of the light blades to the camera coordinate system to obtain the three-dimensional center coordinate set of each light blade. S15: Fit all the three-dimensional center coordinate sets obtained in step S14 to the corresponding line structured light to obtain the light plane equation of each line structured light.

[0010] Furthermore, step S14, which involves extracting the set of center coordinates for each light blade in each three-light-blade image, includes: performing distortion correction on each three-light-blade image based on the camera's intrinsic parameter matrix and distortion coefficients; sequentially performing grayscale conversion, Gaussian filtering, and adaptive thresholding on the corrected three-light-blade images to separate the light blade region from the background region in each corrected three-light-blade image, thereby obtaining the corresponding binarized light blade image; and calculating the set of grayscale centroids for each light blade along its width direction in each binarized light blade image to obtain the set of center coordinates for each light blade.

[0011] Furthermore, step S3 includes: S31: Using three light plane equations and the camera's intrinsic parameter matrix, the set of center coordinates of the corresponding light blades is transformed into the camera coordinate system to obtain the three-dimensional center coordinate set of each light blade. S32: Summarize the set of three-dimensional center coordinates of all light blades obtained in step S31, and calculate the overall center of gravity of all light blades; calculate the center of gravity of each light blade based on the set of three-dimensional center coordinates of each light blade. S33: Based on the centroid position of each light cutter obtained in step S32, principal component analysis is used to fit the data to obtain the axial direction vector of the rod.

[0012] Furthermore, the process of obtaining the axial direction vector of the rod in step S33 includes: decentering the centroid position of each blade; calculating the covariance matrix of the three centroid points at this time, solving for the eigenvalues ​​and eigenvectors of the covariance matrix; and selecting the eigenvector with the largest eigenvalue as the principal component direction, which is the direction vector of the rod.

[0013] Furthermore, the machine learning process in step S4 includes: determining the axis representation of the member based on the object to be optimized; calculating the distance between each three-dimensional center coordinate in the set of three-dimensional center coordinates of all light cutters and the axis of the member based on the axis representation of the member; calculating the error between the distance and the radius of the member, and using the minimum sum of squares of all errors as the objective function, optimizing the object to be optimized using the LM optimization method to obtain the optimal centroid position and the optimal direction vector.

[0014] Furthermore, step S4, which converts the optimal center of gravity position and optimal direction vector into the six-degree-of-freedom pose of the link, includes: based on the optimal center of gravity position and optimal direction vector, using a certain direction in the camera coordinate system as a reference, calculating the deviation parameters of the link in the other two directions in the camera coordinate system to obtain auxiliary vectors; using the optimal direction vector as the Y rotation vector, multiplying the auxiliary vector with the Y rotation vector to obtain the X rotation vector, and multiplying the X rotation vector with the Y rotation vector to obtain the Z rotation vector; concatenating the three rotation vectors to obtain the rotation matrix; establishing the single-axis rotation matrix between the link and each axis in the camera coordinate system, and the transformation relationship between the single-axis rotation matrix and the rotation matrix; using the rotation matrix, solving the transformation relationship to obtain the angle between the link and each axis in the camera coordinate system; the three angles of the link and the auxiliary vector constitute the six-degree-of-freedom pose of the link in the camera coordinate system.

[0015] Furthermore, the rotation matrix is: ; Where Rot represents the rotation matrix, vec x vec represents the X rotation vector. y vec represents the Y rotation vector. z This represents the Z-rotation vector.

[0016] Furthermore, the single-axis rotation matrix between the rod and the Z-axis in the camera coordinate system: ; Among them, R z The single-axis rotation matrix between the rod and the Z-axis is represented by α, where α represents the angle between the rod and the Z-axis in the camera coordinate system. The single-axis rotation matrix between the rod and the Y-axis in the camera coordinate system: ; Among them, R y β represents the single-axis rotation matrix between the rod and the Y-axis, and β represents the angle between the rod and the Y-axis in the camera coordinate system. The single-axis rotation matrix between the rod and the X-axis in the camera coordinate system: ; Among them, R x γ represents the single-axis rotation matrix between the rod and the X-axis, and γ represents the angle between the rod and the X-axis in the camera coordinate system. The transformation relationship is as follows: Rot=R z (α)×R y (β)×R z (γ); Here, Rot represents the rotation matrix.

[0017] Furthermore, based on the calculation rules of Euler angles ZYX, the transformation relationship is calculated according to the rotation matrix to obtain the angle between the rod and each axis in the camera coordinate system.

[0018] Compared with the prior art, the present invention can achieve the following beneficial effects: This invention presents a method for measuring the pose of non-cooperative targets using three-line structured light. By leveraging the synergistic effect of the three-line structured light, the stability of optical blade feature extraction and the accuracy of direction vector fitting are improved. An error equation is constructed using the LM optimization algorithm and the radius features of the target, achieving high-precision pose measurement of non-cooperative targets. This invention eliminates the need for cooperative markers on the target, simplifies the measurement process, enhances anti-interference capabilities, and provides high measurement accuracy. It is suitable for real-time pose measurement of non-cooperative targets using three-line structured light in industrial, aerospace, and other fields. Attached Figure Description

[0019] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments and descriptions of the invention are used to explain the invention and do not constitute an undue limitation of the invention. In the drawings: Figure 1 A flowchart illustrating the non-cooperative target pose measurement method for rods based on three-line structured light, as described in an embodiment of the present invention. Figure 2 A flowchart illustrating the non-cooperative target pose measurement method for rods based on three-line structured light, as described in an embodiment of the present invention. Figure 3 This is a schematic diagram illustrating the positional relationship between the rod and the camera as described in an embodiment of the present invention.

[0020] Explanation of reference numerals in the attached figures: 1. Camera; 2. Rod. Detailed Implementation

[0021] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and do not constitute a limitation thereof. It should be noted that, unless otherwise specified, the embodiments and features described herein can be combined with each other.

[0022] In the description of this invention, it should be understood that the terms "first," "second," etc., are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Therefore, a feature defined with "first," "second," etc., may explicitly or implicitly include one or more of that feature. In the description of this invention, unless otherwise stated, "a plurality of" means two or more.

[0023] The invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0024] like Figures 1 to 2 As shown in the embodiment of the present invention, the method for measuring the pose of a non-cooperative target on a rod based on three-line structured light includes: S1: The three-line laser is calibrated using a laser tracker and a camera to obtain the optical plane equations corresponding to the three line structured lights emitted by the three-line laser.

[0025] In some embodiments, step S1 includes: S11: Control the three-line laser to transmit three-line structured light onto the white backplate, and control the camera to capture the three-line structured light beam fully projected onto the backplate surface to obtain a three-light beam image.

[0026] In this embodiment of the invention, a flat white backplate is placed within a measurement area that is simultaneously within the shooting range of the camera and the illumination range of the three-line laser. The three-line laser is controlled to project three-line structured light onto the backplate, and the spatial pose of the white backplate is adjusted so that the three-line structured light beam can be completely projected onto the surface of the white backplate without overlap or obstruction.

[0027] S12: Using a laser tracker, measure the first spatial plane equation parameters of the current white backplate in the reference coordinate system in step S11, and combine them with the camera extrinsic parameter matrix to transform the first spatial plane equation parameters to the camera coordinate system, obtaining the second spatial plane equation parameters. In this embodiment of the invention, the relationship between the second spatial plane equation parameters and the first spatial plane equation parameters is as follows: ; in, Denotes the parameters of the second space plane equation. T1 represents the parameters of the first spatial plane equation, and T2 represents the camera extrinsic parameter matrix, where A, B, C, and D represent the intrinsic parameters of the plane equation.

[0028] S13: Adjust the pose of the white backplate multiple times, and repeat steps S11-S12 to obtain the three-light blade image and the second spatial plane equation parameters corresponding to each white backplate pose. In this embodiment of the invention, the pose of the white backplate is adjusted 20 times, and steps S11-S12 are repeated to ensure that the 20 three-light blade images can cover all areas of the measurement range, and that the position of the light blade in the three-light blade images is evenly distributed; the obtained 20 three-light blade images are associated and stored with the corresponding second spatial plane equation parameters in the camera coordinate system.

[0029] S14: Extract the set of center coordinates of each light blade in each three-light blade image obtained in step S13, and use the second spatial plane equation parameters obtained in step S13 to transform the set of center coordinates of the light blades to the camera coordinate system to obtain the three-dimensional center coordinate set of each light blade.

[0030] It should be noted that a laser scalpel is a very narrow band of light left on a plane when a laser irradiates it (generally 10μm to several hundredμm). Therefore, the set of center coordinates here is essentially the center line of the laser scalpel along its length, that is, the set of center coordinates is the set of coordinates of all points on the center line of the laser scalpel along its length.

[0031] In some embodiments, step S14, which involves extracting the set of center coordinates of each blade in each three-blade image, includes: Distortion correction is performed on each three-light knife image based on the camera's intrinsic parameter matrix and distortion coefficients; The corrected three-light scalpel image is sequentially grayscaled, Gaussian filtered and adaptive thresholded segmented to separate the scalpel region from the background region in each corrected three-light scalpel image, thus obtaining the corresponding binarized scalpel image. In each binarized light blade image, along the width direction of each light blade, the gray-level centroid set of each light blade is calculated to obtain the center coordinate set of each light blade.

[0032] In this invention, by combining pre-calibrated camera intrinsic parameters and second spatial plane equation parameters (A', B', C', D'), the set of center coordinates of all light slashes is transformed into a three-dimensional center coordinate set in the camera coordinate system. Specifically, the three-dimensional center coordinate set of the light slashes must first satisfy the second spatial plane equation of the camera coordinate system, that is: A'X c +B'Y c +C'Z c +D'=0; Among them, (X) c ,Y c Z c () represents the three-dimensional center coordinates of the laser cutter; At the same time, the three-dimensional center coordinates (X) of the light scalpel c ,Y c Z c Satisfying the pinhole imaging model of the camera: ; Among them, f x f y c x and c y This represents the camera intrinsic calibration parameters, and (u,v) represents the center coordinates of the laser cutter. By combining the equation of the second spatial plane with the pinhole imaging model, we obtain the three-dimensional center coordinates (X) of the light scalpel. c ,Y c Z c ): ; Where, k x =X c / Z c k y =Y c / Z c .

[0033] S15: Fit all the three-dimensional center coordinate sets obtained in step S14 to the corresponding line structured light to obtain the light plane equation of each line structured light.

[0034] In this embodiment of the invention, the least squares method is used to fit the set of all three-dimensional center coordinates corresponding to each line structured light to obtain the light plane equations of the three line structured lights, as follows: A1X+B1Y+C1Z+D1=0; A2X+B2Y+C2Z+D2=0; A3X+B3Y+C3Z+D3=0; In the three optical plane equations above, A, B, C, and D represent the parameters in the optical plane equations of the three line structures. This completes the planar calibration of the three-line structured light.

[0035] S2: As Figure 3 As shown, the rod 2 is placed within the shooting range of the camera 1, and the rod 2 is illuminated by a three-line laser, so that the light blades of the three-line structured light are completely projected onto the surface of the rod; the set of center coordinates of each light blade in the image captured by the camera 1 is extracted.

[0036] To elaborate further, the camera coordinate system and the link coordinate system are shown in Figure 3. The camera coordinate system is defined with the optical center of camera 1 as its origin. The Z-axis of the camera coordinate system is set along the optical axis of camera 1, pointing towards the target being measured (i.e., link 2). The X-axis of the camera coordinate system is horizontal to the right and perpendicular to the Z-axis, while the Y-axis is vertically downwards, forming a right-handed coordinate system with the X and Z axes. The link coordinate system is defined with the intersection of the axis of link 2 and the XOZ plane (y=0 plane) of the camera coordinate system as its origin. The Y-axis of the link coordinate system is defined downwards along the axis. The X, Y, and Z axes of the link coordinate system are orthogonal and form a right-handed rectangular coordinate system, used to describe the attitude of link 2 itself. Its spatial attitude is characterized by the rotation matrix and translation vector in the camera coordinate system, ultimately yielding 6 degrees of freedom pose parameters through transformation.

[0037] In this embodiment of the invention, the process of obtaining the center coordinate set of each light blade is the same as the process of obtaining the center coordinate set of each light blade in step S1 (i.e., step S14), and will not be repeated here. The center coordinate sets L1, L2, and L3 of each light blade are denoted as follows: L1=[(u 11 ,v 11 ),(u 12 ,v 12 ),...,(u 1m ,v 1m )]; L2=[(u 21 ,v 21 ),(u 22 ,v 22 ),...,(u 2p ,v 2p )]; L3=[(u 31 ,v 31 ),(u 32 ,v 32 ),...,(u 3q ,v 3q )]; Among them, (u 1m ,v 1m (u) represents the m-th center coordinate in the set of center coordinates L1. 2p ,v 2p ) represents the p-th center coordinate in the center coordinate set L2, (u 3q ,v 3q ) represents the q-th center coordinate in the center coordinate set L3.

[0038] S3: Using the three optical plane equations obtained in step S1 and the set of center coordinates of the corresponding optical cutters in step S2, calculate the overall center of gravity position of all optical cutters and the center of gravity position of each optical cutter; fit the center of gravity positions of the three optical cutters to obtain the axial direction vector of the rod.

[0039] In some embodiments, step S3 includes: S31: Using three light plane equations and the camera's intrinsic parameter matrix, the set of center coordinates of the corresponding light blades is transformed into the camera coordinate system to obtain the three-dimensional center coordinate set of each light blade.

[0040] In this embodiment of the invention, the coordinate transformation process is the same as the coordinate transformation process in step S14, except that the second spatial plane equation in step S14 is replaced with the light plane equation, resulting in the three-dimensional center coordinate set P1, P2, and P3 of the three light blades, which are denoted as follows: P1=[p 11 ,p12 ,...,p 1m ]; P2=[p 21 ,p 22 ,...,p 2p ]; P3=[p 31 ,p 32 ,...,p 3q ]; Where, p 1m Let p represent the m-th three-dimensional center coordinate in the set P1 of the three-dimensional center coordinates of the first light blade. 2p This represents the p-th three-dimensional center coordinate in the set P2 of the three-dimensional center coordinates of the second light blade. 3q Let q be the 3D center coordinate of the third light blade in the set of 3D center coordinates P3.

[0041] S32: Summarize the set of three-dimensional center coordinates of all light scalpels obtained in step S31, and calculate the overall center of gravity position of all light scalpels; calculate the center of gravity position of each light scalpel based on the set of three-dimensional center coordinates of each light scalpel.

[0042] In this embodiment of the invention, the average values ​​of the x, y, and z coordinates in the three-dimensional center coordinate set of all light blades are calculated to obtain the overall centroid position (x0, y0, z0), i.e.: ; ; ; Where i represents the index number, (p 1ix ,p 1iy ,p 1iz () represents the i-th three-dimensional center coordinate in the set P1 of the three-dimensional center coordinates of the first light blade, (p 2ix ,p 2iy ,p 2iz () represents the i-th three-dimensional center coordinate in the set P2 of the three-dimensional center coordinates of the second light blade, (p 3ix ,p 3iy ,p 3iz ) represents the i-th three-dimensional center coordinate in the three-dimensional center coordinate set P3 of the third light blade; Calculate the average value of the three-dimensional center coordinate set of each light blade to obtain the centroid position of each light blade.

[0043] S33: Based on the centroid position of each light cutter obtained in step S32, principal component analysis is used to fit the data to obtain the axial direction vector of the rod.

[0044] In some embodiments, the process of obtaining the direction vector of the rod in step S33 includes: decentering the centroid position of each light cutter; calculating the covariance matrix of the three centroid points at this time, solving for the eigenvalues ​​and eigenvectors of the covariance matrix; and selecting the eigenvector with the largest eigenvalue as the principal component direction, which is the axial direction vector of the rod.

[0045] S4: Using the overall center of gravity position obtained in step S3 and the direction vector obtained in step S4 as the objects to be optimized, perform machine learning, and convert the obtained optimal center of gravity position and optimal direction vector into the six-degree-of-freedom pose of the rod.

[0046] In some embodiments, the machine learning process in step S4 includes: Based on the object to be optimized, determine the axial characterization of the member; Based on the axis representation of the rod, calculate the distance between each three-dimensional center coordinate in the set of three-dimensional center coordinates of all light cutters and the axis of the rod; The real 3D coordinate point set recovered by the optical blade corresponds to the sampling points projected onto the surface of the rod by the line structured light. Based on this point set, the spatial perpendicular distance from each point to the fitted rod axis is calculated. Ideally, this distance is equal to the standard radius of the rod. Therefore, this invention uses the deviation between the perpendicular distance from each point to the axis and the standard radius of the rod as the fitting error, and constructs an objective function that minimizes the sum of squared errors. That is, the distance from each point to the axis and the rod radius are calculated as errors. At this time, the error between the distance and the rod radius is calculated, and the objective function is to minimize the sum of squared errors. The LM optimization method is used to optimize the object to be optimized to obtain the optimal centroid position and the optimal direction vector.

[0047] In this embodiment of the invention, the object to be optimized is represented as [x0,y0,z0,a,b,c], where (a,b,c) represents the axial direction vector of the rod. The axial line of the rod is then represented by the following spatial straight line equation: ; The following formula is used to calculate the three-dimensional center coordinate p of each of the three-dimensional center coordinate sets of all light scalpels. i The distance d between the rod axis and the rod axis i : ; Where G0 represents the overall center of gravity position (x0, y0, z0), and V represents the axial direction vector of the member; The LM optimization method is used to optimize the object by minimizing the sum of squares of all errors. During the optimization process, the threshold for the number of iterations is 100, and the threshold for the error of the objective function value is 10. 6 That is, when the number of iterations reaches 100, or the objective function value is less than 10. 6 When the iteration stops, the optimal centroid position (x) is finally obtained. best ,y best ,z best ) and optimal direction vector (a best ,b best ,c best ).

[0048] In some embodiments, the process of converting the optimal center of gravity position and optimal orientation vector into a six-degree-of-freedom pose of the link includes: S411: Based on the optimal center of gravity position and the optimal direction vector, and taking a certain direction in the camera coordinate system as a reference, calculate the deviation parameters of the rod in the other two directions in the camera coordinate system to obtain the auxiliary vector.

[0049] In this embodiment of the invention, the y-direction in the camera coordinate system is used as the reference (i.e., the y-direction is 0). The deviation parameters of the rod in the other two directions in the camera coordinate system are calculated using the following formula to obtain the auxiliary vector (x). R ,y R ,z R ): ; Where, x R y R z R It also represents the deviation parameters of the rod in the three directions in the camera coordinate system. This auxiliary vector is the direction vector of the line connecting the origin of the rod coordinate system and the origin of the camera coordinate system.

[0050] S412: Using the optimal direction vector in step S411 as the Y rotation vector, multiply the auxiliary vector with the Y rotation vector to obtain the X rotation vector, and multiply the X rotation vector with the Y rotation vector to obtain the Z rotation vector.

[0051] In this embodiment of the invention, step S412 specifically includes: The auxiliary vector is normalized using the following formula: ; Among them, vec tmp This represents the normalized auxiliary vector; Y rotation vector vec y That is, the optimal direction vector (a) best ,b best ,c best ), and normalize the auxiliary vector vec tmp With Y rotation vector vec y Multiplying them together yields the X rotation vector vec. x Rotate the X-axis vector vec x With Y rotation vector vecy Multiplying them together yields the Z-rotation vector vec. z .

[0052] S413: Concatenate the three rotation vectors obtained in step S412 to obtain the rotation matrix, as shown below: ; Here, Rot represents the rotation matrix.

[0053] S414: Establish the transformation relationship between the single-axis rotation matrix between the rod and each axis in the camera coordinate system and the rotation matrix obtained in step S413. Specifically, the single-axis rotation matrix between the rod and the Z-axis in the camera coordinate system is as follows: ; Among them, R z The single-axis rotation matrix between the rod and the Z-axis is represented by α, where α represents the angle between the rod and the Z-axis in the camera coordinate system. The single-axis rotation matrix between the rod and the Y-axis in the camera coordinate system: ; Among them, R y β represents the single-axis rotation matrix between the rod and the Y-axis, and β represents the angle between the rod and the Y-axis in the camera coordinate system. The single-axis rotation matrix between the rod and the X-axis in the camera coordinate system: ; Among them, R x γ represents the single-axis rotation matrix between the rod and the X-axis, and γ represents the angle between the rod and the X-axis in the camera coordinate system. The transformation relationship is as follows: Rot=R z (α)×R y (β)×R z (γ), that is: .

[0054] S415: Using the rotation matrix obtained in step S413, the transformation relationship constructed in step S414 is solved to obtain the angle between the rod and each axis in the camera coordinate system. In some embodiments, according to the Euler angle ZYX solution rules, the transformation relationship is solved based on the rotation matrix to obtain the angle between the rod and each axis in the camera coordinate system.

[0055] In this invention, the angles (α, β, γ) between the rod and each axis in the camera coordinate system are as follows: .

[0056] S416: The three included angles of the rod obtained in step S415, and the auxiliary vector obtained in step S411, are the six-degree-of-freedom pose of the rod in the camera coordinate system.

[0057] It should be understood that the various forms of processes shown above can be used to reorder, add, or delete steps. For example, the steps described in this invention disclosure can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution disclosed in this invention can be achieved, and this is not limited herein.

[0058] The specific embodiments described above do not constitute a limitation on the scope of protection of this invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A method for measuring the pose of a non-cooperative target on a rod based on three-line structured light, characterized in that, include: S1: The three-line laser is calibrated using a laser tracker and camera to obtain the optical plane equations corresponding to the three line structured lights emitted by the three-line laser. S2: Place the rod within the camera's field of view and illuminate it with a three-line laser, so that the light blades of the three-line structured light are fully projected onto the surface of the rod; extract the set of center coordinates of each light blade in the image captured by the camera; S3: Using the three optical plane equations obtained in step S1 and the set of center coordinates of the corresponding optical blades in step S2, calculate the overall center of gravity position of all optical blades and the center of gravity position of each optical blade. By fitting the centroid positions of the three light cutters, the axial direction vector of the rod is obtained; Step S3 includes: S31: Using three light plane equations and the camera's intrinsic parameter matrix, the set of center coordinates of the corresponding light blades is transformed into the camera coordinate system to obtain the three-dimensional center coordinate set of each light blade. S32: Summarize the set of three-dimensional center coordinates of all light scalpels obtained in step S31, and calculate the overall center of gravity position of all light scalpels; calculate the center of gravity position of each light scalpel based on the set of three-dimensional center coordinates of each light scalpel. S33: Based on the centroid position of each light cutter obtained in step S32, principal component analysis is used to fit the data to obtain the axial direction vector of the rod. S4: Using the overall center of gravity position and the axis direction vector obtained in step S3 as the objects to be optimized, perform machine learning, and convert the obtained optimal center of gravity position and optimal direction vector into the six-degree-of-freedom pose of the rod. The machine learning process in step S4 includes: determining the axis representation of the member based on the object to be optimized; calculating the distance between the axis of the member and the axis of the member in the three-dimensional center coordinate set of all light cutters based on the axis representation of the member; calculating the error between the distance and the radius of the member, and using the minimum sum of squares of all errors as the objective function, optimizing the object to be optimized using the LM optimization method to obtain the optimal centroid position and the optimal direction vector.

2. The method for measuring the pose of a non-cooperative target on a rod based on three-line structured light according to claim 1, characterized in that, Step S1 includes: S11: Control the three-line laser to transmit three-line structured light to the white back plate, and control the camera to capture the three-line structured light beam completely projected onto the back plate surface to obtain a three-light beam image. S12: Use a laser tracker to measure the first spatial plane equation parameters of the current white backplate in the reference coordinate system in step S11, and combine the camera extrinsic parameter matrix to transform the first spatial plane equation parameters to the camera coordinate system to obtain the second spatial plane equation parameters. S13: Adjust the pose of the white backplate multiple times, and repeat steps S11~S12 to obtain the three-light knife image and the second spatial plane equation parameters corresponding to each pose of the white backplate. S14: Extract the set of center coordinates of each light blade in each three-light blade image obtained in step S13, and use the second spatial plane equation parameters obtained in step S13 to transform the set of center coordinates of the light blades to the camera coordinate system to obtain the three-dimensional center coordinate set of each light blade. S15: Fit all the three-dimensional center coordinate sets obtained in step S14 to the corresponding line structured light to obtain the light plane equation of each line structured light.

3. The method for measuring the pose of a non-cooperative target on a rod based on three-line structured light according to claim 2, characterized in that, Step S14 involves extracting the set of center coordinates of each beam in each three-beam image, including: Based on the camera's intrinsic parameter matrix and distortion coefficients, distortion correction is performed on each three-light knife image; The corrected three-light scalpel image is sequentially grayscaled, Gaussian filtered and adaptive thresholded to separate the scalpel region from the background region in each corrected three-light scalpel image, and the corresponding binarized scalpel image is obtained. In each binarized light blade image, along the width direction of each light blade, the gray-level centroid set of each light blade is calculated to obtain the center coordinate set of each light blade.

4. The method for measuring the pose of a non-cooperative target on a rod based on three-line structured light according to claim 1, characterized in that, The process of obtaining the axial direction vector of the rod in step S33 includes: The center of gravity of each light blade is decentralized; Calculate the covariance matrix of the three centroids at this point, and solve for the eigenvalues ​​and eigenvectors of the covariance matrix; The eigenvector with the largest eigenvalue is selected as the principal component direction, which is the direction vector of the rod.

5. The method for measuring the pose of a non-cooperative target on a rod based on three-line structured light according to claim 1, characterized in that, Step S4, which converts the optimal center of gravity position and optimal direction vector into the six-degree-of-freedom pose of the link, includes: Based on the optimal center of gravity position and the optimal direction vector, and taking a certain direction in the camera coordinate system as a reference, calculate the deviation parameters of the rod in the other two directions in the camera coordinate system to obtain the auxiliary vector; Using the optimal direction vector as the Y rotation vector, multiply the auxiliary vector by the Y rotation vector to obtain the X rotation vector, and multiply the X rotation vector by the Y rotation vector to obtain the Z rotation vector; Concatenate the three rotation vectors to obtain the rotation matrix; Establish the single-axis rotation matrix between the rod and each axis in the camera coordinate system, and the transformation relationship between the rotation matrix and the rotation matrix; By using the rotation matrix, the transformation relationship is solved to obtain the angle between the rod and each axis in the camera coordinate system; The three included angles of the rod and the auxiliary vector constitute the six-degree-of-freedom pose of the rod in the camera coordinate system.

6. The method for measuring the pose of a non-cooperative target on a rod based on three-line structured light according to claim 5, characterized in that, The rotation matrix is: ； Where Rot represents the rotation matrix, vec x vec represents the X rotation vector. y vec represents the Y rotation vector. z This represents the Z-rotation vector.

7. The method for measuring the pose of a non-cooperative target on a rod based on three-line structured light according to claim 5, characterized in that, The single-axis rotation matrix between the rod and the Z-axis in the camera coordinate system: ； Among them, R z The single-axis rotation matrix between the rod and the Z-axis is represented by α, where α represents the angle between the rod and the Z-axis in the camera coordinate system. The single-axis rotation matrix between the rod and the Y-axis in the camera coordinate system: ； Among them, R y β represents the single-axis rotation matrix between the rod and the Y-axis, and β represents the angle between the rod and the Y-axis in the camera coordinate system. The single-axis rotation matrix between the rod and the X-axis in the camera coordinate system: ； Among them, R x γ represents the single-axis rotation matrix between the rod and the X-axis, and γ represents the angle between the rod and the X-axis in the camera coordinate system. The transformation relationship is as follows: Rot=R z (a)×R y (b)×R z (c); Here, Rot represents the rotation matrix.

8. The method for measuring the pose of a non-cooperative target on a rod based on three-line structured light according to claim 5, characterized in that, Based on the Euler angle ZYX calculation rules, the transformation relationship is calculated using the rotation matrix to obtain the angle between the rod and each axis in the camera coordinate system.

Images