A six-degree-of-freedom measurement system and measurement method

Abstract

This invention relates to a six-degree-of-freedom (6DOF) measurement system and method. The 6DOF measurement system includes a camera, a prism, and a target; the optical axis of the prism is coaxial with the line of sight of the camera; the target is mounted on the target and positioned within the combined field of view of the camera and prism; the target includes a mounting base and a main shaft mounted on the mounting base, with a fixing plate on the main shaft; an annular sleeve is fixedly connected to the fixing plate, and a motor is mounted outside the annular sleeve; the main shaft of the motor passes through the annular sleeve and is connected to a transmission component, which has rollers connected to a slider. Driven by the motor, the slider can slide radially from the center to the edge of the annular sleeve via the rollers; a main marker is mounted on the top of the main shaft, and an auxiliary marker is mounted on the top of the slider. Compared with the prior art, this invention can achieve six-degree-of-freedom pose measurement of a target under uncalibrated conditions, improving the adaptability of the measurement system.

Description

Technical Field

[0001] This invention belongs to the field of spatial geometry measurement technology, and in particular relates to a six-degree-of-freedom measurement system and measurement method. Background Technology

[0002] With the development of intelligent manufacturing, the extraction of motion condition information from manufacturing equipment is a crucial feedback link in intelligent manufacturing. As a key sensor in information extraction, the testing system directly determines the working accuracy of the manufacturing system. Currently, six-degree-of-freedom measurement devices used in industrial settings are typically complex in composition, require large configuration space, and involve complex system calibration, making them difficult to widely apply in industrial production.

[0003] Chinese patent CN107246866A discloses a high-precision six-degree-of-freedom measurement system and method, which uses two tilt sensors and a visual imaging system to measure the pose of the target. The two tilt sensors need to be fixed on the target along with the feature point target. The measurement device affects the operation of the target to some extent, and the system is complex to install and costly.

[0004] Prior art (G. Schweighofer and A. Pinz, “Globally optimal o(n) solution to the pnp problem for general camera models.” in BMVC, 2008, pp. 1-10; Zheng Y, Sugimoto S, Okutomi M. ASPnP: An Accurate and Scalable Solution to the Perspective-n-Point Problem[J]. Ice Transactions on Information & Systems, 2013, E96. D(7): 1525-1535.) proposed a six-degree-of-freedom visual measurement method, which can realize non-contact measurement of the target under test, but cannot meet the industrial field measurement needs under calibration conditions.

[0005] Therefore, a new six-degree-of-freedom measurement system and method still need to be developed to achieve six-degree-of-freedom pose measurement of targets under uncalibrated conditions. Summary of the Invention

[0006] The purpose of this invention is to provide a six-degree-of-freedom measurement system and method to achieve six-degree-of-freedom pose measurement of a target under uncalibrated conditions.

[0007] The objective of this invention can be achieved through the following technical solutions:

[0008] A six-degree-of-freedom measurement system includes a camera, a prism, and a target; the optical axis of the prism is coaxial with the line of sight of the camera; the target is mounted on the target to be measured and is positioned within the combined field of view of the camera and the prism.

[0009] The target includes a mounting base and a main shaft mounted on the mounting base, and a fixing plate is provided on the main shaft; an annular sleeve is fixedly connected to the fixing plate, and a motor is installed outside the annular sleeve.

[0010] The main shaft of the motor passes through the annular sleeve and is connected to the transmission component. The transmission component is equipped with rollers, and the rollers are connected to sliders. Driven by the motor, the sliders can slide radially from the center of the annular sleeve to the edge through the rollers.

[0011] A main marker is installed on the top of the spindle, and an auxiliary marker is installed on the top of the slider.

[0012] Furthermore, the transmission component includes a driving bevel gear connected to the motor spindle and a driven bevel gear meshing with the driving bevel gear, the driven bevel gear being fixedly connected to the drive sleeve.

[0013] Furthermore, the drive sleeve has a spiral groove, one end of the roller is disposed in the spiral groove, and the other end of the roller is connected to the slider.

[0014] The present invention also provides a six-degree-of-freedom pose measurement method using the above-described measurement system, comprising the following steps:

[0015] S1: Set the camera and prism in front of the target to be measured, and install the target on the target to be measured. The target is within the combined field of view of the camera and prism.

[0016] S2: Constructing the real camera coordinate system O c -X c Y c Z c Virtual camera coordinate system O vc -X vc Y vc Z vc Prism coordinate system O p -X p Y p Z p and world coordinate system O w -X w Y w Z w ;

[0017] S3: Set the prism rotation angle to the initial position, and the camera captures the image point of the marker;

[0018] S4: Determine the relationship between the marker points and virtual image points in space, construct the collinearity equation of the object and image, and preliminarily determine the focal length f of the virtual camera;

[0019] S5: Determine the rotation matrix R of the world coordinate system relative to the virtual camera based on the focal length f and using a pose estimation method. vw Translation matrix t vw And initially extract the distortion coefficients of the virtual camera. and After rotating the prism n times, a cost function is constructed, maximum likelihood estimation is performed, and the optimal value is obtained.

[0020] S6: Determine the rotation and translation relationships of the virtual camera relative to the real camera, and determine the pose of the target under test in the real camera coordinate system.

[0021] Furthermore, in step S4, the specific relationships between the spatial markers and the virtual image points are as follows:

[0022]

[0023] Where, α ij Let the coordinates be the homogeneous barycenter coordinates. Let f be the coordinates of the i-th feature point in the camera coordinate system. x f y The equivalent focal length in both the horizontal and vertical directions; Virtual control point The coordinates in the camera coordinate system, (u i v i () is a virtual control point The image coordinates, where s is the non-perpendicularity factor between the horizontal and vertical axes of the image, (C x C y () is the principal point of the camera.

[0024] Furthermore, f x f y The equivalent focal length in both the horizontal and vertical directions is determined by the pixel aspect ratio and is close to 1. Therefore, f can be approximated as... x =f y =f.

[0025] Furthermore, the scale factor of the i-th feature point is A system of 2n linear equations can be obtained by using n control points:

[0026]

[0027] make If j = 1, 2, 3, 4, and X can be linearly expressed using the fundamental solution set of this system of equations, then X = βξ, ξ = [η1, η2, η3, η4]. T η j =[η j1 ,η j2 ,ηj3 ] T ;

[0028] Furthermore, the focal length f of the virtual camera is determined according to the following formula:

[0029]

[0030] in, B = β 2 Using the least squares method, we can obtain A and B, and further obtain...

[0031] Furthermore, in step S4, the imaging ray f(τ) can be determined by the following formula:

[0032] f(τ)=K i,j +τL i,j

[0033] Where τ is a parameter;

[0034] From the i-th viewpoint, the projected vector ray from the j-th marker point intersects with the exit point K of the prism. i,j It can be obtained through the following formula:

[0035]

[0036] Where D is the distance from the optical center of the camera to the prism plane, and t p Let t0 and t1 be the thickness of the prism, θ be the prism rotation angle, and α be the prism wedge angle.

[0037] From the i-th viewpoint, the projected vector ray L of the j-th marker point i,j It can be obtained through the following formula:

[0038]

[0039]

[0040] Where, n r The refractive index of the prism, Let N1 and N2 be the normal vectors of the first and second planes of the prism when the prism is rotated to the i-th position, and let N1 and N2 be the normal vectors of the first and second planes of the prism, respectively.

[0041] The pose estimation method uses one of the following algorithms: Orthogonal Iterative Method (LHM), Precise and Efficient Pose Estimation Method (EPnP), Robust Pose Estimation Method (RPnP), Direct Least Squares Method (DLS), Optimal Pose Estimation Method (OPnP), Precise Variable Scale Pose Estimation Method (ASPnP), Global Optimal Pose Estimation Method (SDP), and Fast Pose Estimation Method (EPPnP).

[0042] Furthermore, in step S5, the distortion coefficients of the virtual camera are initially extracted. and The specific steps are as follows:

[0043] Construct a system of linear equations by combining all cooperative feature points:

[0044]

[0045] Among them, (u di ,v di () represents the coordinates of the i-th distorted image point. and Here are the coefficients of the fitting terms for dx and dy, where f is the focal length and R0 is the focal length. vw and t vw These are the rotation and translation matrices of the world coordinate system relative to the virtual camera, R. vw (1,:) represents R vw The first line, R vw (2,:) represents R vw The second line, R vw (3,:) represents R vw The third line, (t x ,t y ,t z ) for t vw The coordinates are obtained by solving the system of equations using the least squares method. and

[0046] Furthermore, the specific steps of maximum likelihood estimation are as follows:

[0047] Rotate the prism n times to acquire n cooperative target images. Each image has s feature points. The cost function is constructed with the objective of minimizing the reprojection error of each feature point:

[0048]

[0049] Where, p ij For the j-th feature point in the i-th image, It is the projection point of the j-th feature point in the i-th image onto the image plane;

[0050] With R vw t vw f, C x C y , and Using the initial value as an initial value, the Levenberg-Marquardt algorithm is used to obtain the optimal value.

[0051] Furthermore, in step S6, the virtual camera rotates R relative to the real camera.cv for:

[0052] R cv =A p +(IA p cosρ+B p sinρ

[0053] Where ρ is the pitch angle of the line of sight, I is the third-order identity matrix, and A p and B p They are respectively:

[0054]

[0055] in, The azimuth angle of the line of sight;

[0056] Further, in step S6, the translation t of the virtual camera relative to the real camera is obtained. cv The specific steps are as follows:

[0057] Construct O vc Error cost function relative to all imaging rays:

[0058]

[0059] Where γ is and The angle between them;

[0060] The cost function is solved using the Levenberg-Marquardt algorithm, yielding O0. vc In O c -X c Y c Z c The coordinates below, i.e., t cv ;

[0061] Furthermore, in step S6, the pose of the target object in the real camera coordinate system can be expressed as:

[0062]

[0063] In the formula, This is the transformation matrix between the virtual camera coordinate system and the real camera coordinate system. It is the transformation matrix of the target coordinate system relative to the virtual camera coordinate system, that is, the pose of the cooperative target in the virtual camera coordinate system.

[0064] Furthermore, step S6 includes the following:

[0065] S7: Improved accuracy by establishing a function to minimize the sum of squared residuals in the object space:

[0066]

[0067] Using R and t determined in step S6 and f determined in step S5 as initial values, the Levenberg-Marquardt algorithm is used to solve the nonlinear equation system.

[0068] Furthermore, step S7 also includes:

[0069] S8: Robustness improvement, rotating prism adjusts camera line of view, multiple observations of control points obtain multiple sets of R and t, multiple sets of rotation and translation information are fused, and the problem of multiple rotation fusion is transformed into:

[0070]

[0071] in Given the rotation R obtained in step S6 during the i-th observation, the optimal solution to the problem is:

[0072] R = UDV T

[0073] Where U and V satisfy M = UΣV T M is defined as D = dig(1, 1, det(UV) T The mean of the translation and focal length obtained from multiple observations is used as the final translation and focal length estimation results:

[0074]

[0075] in, The translation obtained from step S6 during the i-th observation. The focal length is obtained from step S5 during the i-th observation.

[0076] Compared with the prior art, the present invention has the following beneficial effects:

[0077] (1) The present invention designs a test system consisting of a single camera, a prism and a target. When using it, the target only needs to be set on the corresponding object to be measured to achieve the measurement. At the same time, the present invention makes full use of multi-view prior information to realize target pose estimation under calibration conditions and completes the six-degree-of-freedom pose measurement of the target under calibration conditions.

[0078] (2) This invention uses a rotation prism-based attitude averaging method and an object residual calculation method for measurement calculation, which can reduce the influence of noise such as light and vibration on the measurement and has high measurement accuracy and robustness.

[0079] (3) The present invention can achieve synchronous adjustment of the x and y directions of the marker point with only a single power source. The target can be configured in space to change the distribution of the marker point in space and adapt to measurement scenarios with limited space.

[0080] (4) The present invention combines a camera and a prism, and a single camera can realize multi-camera multi-view observation and measurement. It has the advantages of compact configuration, convenient and flexible control, accurate line of sight pointing, and adaptive adjustment of observation angle. Attached Figure Description

[0081] Figure 1 This is a schematic diagram of the present invention.

[0082] Figure 2 This is a front cross-sectional view of the target of the present invention.

[0083] Figure 3 A schematic diagram illustrating the principle of virtual camera pose calculation.

[0084] Figure 4 This is a diagram illustrating the principle of pose calculation for a virtual camera relative to a real camera.

[0085] Explanation of markings in the diagram:

[0086] 1-Camera, 2-Prism, 3-Target, 31-Mounting base, 32-Main spindle, 321-Main marker point, 33-Fixing plate, 34-Annular sleeve, 35-Driver component, 36-Transmission component, 361-Driving bevel gear, 362-Driven bevel gear, 363-Drive sleeve, 37-Roller, 38-Slider, 381-Auxiliary marker point. Detailed Implementation

[0087] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. This embodiment is implemented based on the technical solution of the present invention, providing detailed implementation methods and specific operation processes. However, the scope of protection of the present invention is not limited to the following embodiments. Component models, material names, connection structures, control methods, algorithms, and other features not explicitly stated in the present invention are considered common technical features disclosed in the prior art.

[0088] Example 1:

[0089] This embodiment provides a six-degree-of-freedom measurement system, which includes a camera 1, a prism 2, and a target 3. The optical axis of the prism 2 is coaxial with the line of sight of the camera 1, and the target 3 is mounted on the target to be measured and is positioned within the combined field of view of the camera 1 and the prism 2.

[0090] The target 3 includes a mounting base 31 and a main shaft 32 mounted on the mounting base 31. A fixing plate 33 is provided on the main shaft 32. An annular sleeve 34 is fixedly connected to the fixing plate 33, and a driving component 35 is mounted outside the annular sleeve 34. In this embodiment, the driving component 35 is a motor. The main shaft of the driving component 35 passes through the annular sleeve 34 and is connected to a transmission component 36. The transmission component 36 is provided with a roller 37. Specifically, the transmission component 36 includes a driving bevel gear 361 connected to the motor main shaft and a driven bevel gear 362 meshing with the driving bevel gear 361. The driven bevel gear 362 is fixedly connected to a driving sleeve 363. The driving sleeve 363 has a helical groove, one end of the roller 37 is disposed within the helical groove, and the other end of the roller 37 is connected to a slider 38. A main marker point 321 is mounted on the top of the main shaft 32, and an auxiliary marker point 381 is mounted on the top of the slider 38. Driven by the motor, the slider 38 can slide radially from the center of the annular sleeve 34 to the edge via the roller 37.

[0091] Example 2:

[0092] This embodiment provides a six-degree-of-freedom measurement system and a six-degree-of-freedom pose measurement method using the system.

[0093] like Figure 1 As shown, the six-degree-of-freedom measurement system includes a camera 1, a prism 2, and a target 3. The line of sight of the camera 1 is arranged coaxially with the optical axis of the prism 2, and the target 3 is set within the combined field of view of the camera 1 and the prism 2.

[0094] like Figure 2 As shown, the target 3 includes a mounting base 31, a main shaft 32, a fixing plate 33, an annular sleeve 34, a motor, a driving bevel gear 361, a driven bevel gear 362, a drive sleeve 363, a roller 37, a slider 38, a main marker point 321, and an auxiliary marker point 381.

[0095] The main shaft 32 is mounted on the mounting base 31. The main shaft 32 passes through the fixed plate 33, the driven bevel gear 362, and the drive sleeve 363 in sequence. The main marker point 321 is installed at the top of the main shaft 32. The fixed plate 33 is fixedly connected to the main shaft 32 and to the annular sleeve 34. The motor is fixed on the annular sleeve 34. The motor's main shaft passes through the annular sleeve 34 and is fixed to the driving bevel gear 361. The driving bevel gear 361 meshes with the driven bevel gear 362. The driven bevel gear 362 is fixedly connected to the drive sleeve 363. The drive sleeve 363 is provided with a spiral groove. One end of the roller 37 is set in the spiral groove, and the other end is fixedly connected to the slider 38. Multiple sliders 38 are slidably connected to the annular sleeve 34 and can slide radially from the center of the annular sleeve 34 to the edge.

[0096] The specific steps of the six-degree-of-freedom pose measurement method in this embodiment are as follows:

[0097] S1. The measuring device is set in front of the target to be measured, the target 3 is fixed on the target to be measured, and the target 3 is within the combined field of view of the camera 1 and the prism 2;

[0098] S2. Construct the real camera coordinate system O c -X c Y c Z c Virtual camera coordinate system O vc -X vc Y vc Z vc Prism coordinate system O p -X p Y p Z p and world coordinate system O w -X w Y w Z w .

[0099] S3. Set the prism rotation angle to the initial position, and camera 1 captures the image of the marker point;

[0100] S4. Determine the relationship between spatial points and virtual image points. For example... Figure 3 As shown, based on geometric relationships, any landmark point in space can be linearly represented by four non-coplanar virtual control points. Combined with the perspective projection model, the following relationship exists between any spatial point and the image point:

[0101]

[0102] Where, α ij Let the coordinates be the homogeneous barycenter coordinates. Let f be the coordinates of the i-th feature point in the camera coordinate system. x f y The equivalent focal length, measured horizontally and vertically, is determined by the pixel aspect ratio and is close to 1. Therefore, f can be approximated as... x =f y =f. Virtual control point The coordinates in the camera coordinate system, (u i v i () is a virtual control point The image coordinates. S is set to 0. (C x C y (0, 0).

[0103] S5. Construct the collinearity equation between the object and the image. The scale factor of the i-th feature point is... A system of 2n linear equations can be obtained by using n control points:

[0104]

[0105] make For j = 1, 2, 3, 4, X can be linearly represented by the fundamental solution set of this system of equations, i.e. β i ξ is an unknown coefficient. i Let X be the eigenvector corresponding to the N zero eigenvalues. Since the imaging model is a perspective projection model, then N = 1, and X = βξ, ξ = [η1, η2, η3, η4]. T ,

[0106] S6. The virtual camera focal length is initially determined. This is based on the moment preservation property of the two coordinate systems under Euclidean transformation. but,

[0107]

[0108] in, B = β 2 Using the least squares method, we can obtain A and B, and further obtain...

[0109] S7. Determine the rotation matrix R of the world coordinate system relative to the virtual camera. vw Translation matrix t vw Using the focal length f determined in step S6, the pose estimation method is used to determine R. vw and t vw ;

[0110] The imaging ray f(τ) mentioned in step S7 can be determined by the following formula:

[0111] f(τ)=K i,j +τL i,j

[0112] Where τ is a parameter. According to geometric relationships, from the i-th viewpoint, the projected vector ray from the j-th marker point intersects with the exit point K of the prism. i,j It can be obtained through the following formula:

[0113]

[0114] Where D is the distance from the optical center of the camera to the prism plane, and t p Let be the thickness of the prism, t0 and t1 be parameters, θ be the prism rotation angle, and α be the prism wedge angle. The projected vector ray L from the j-th marker point at the i-th viewpoint... i,j It can be obtained through the following formula:

[0115]

[0116]

[0117] Where, nr is the refractive index of the prism. Let N1 and N2 be the normal vectors of the first and second planes of the prism when the prism is rotated to the i-th position, and let N1 and N2 be the normal vectors of the first and second planes of the prism, respectively.

[0118] Among them, R is determined vw and t vw The pose calculation algorithm used is any one of LHM, EPnP, RPnP, DLS, OPnP, ASPnP, SDP, PPnP and EPPnP.

[0119] S8. Preliminary extraction of distortion coefficients from the virtual camera. Constructing a system of linear equations using all cooperating feature points:

[0120]

[0121] Among them, (u di ,v di () represents the coordinates of the i-th distorted image point. and Here are the coefficients of the fitting terms for dx and dy, where f is the focal length and R0 is the focal length. vw and t vw These are the rotation and translation matrices of the world coordinate system relative to the virtual camera, R. vw (1,:) represents R vw The first line, R vw (2,:) represents R vw The second line, R vw (3,:) represents R vw The third line, (t x ,t y ,t z ) for t vw The coordinates are obtained by solving the system of equations using the least squares method. and

[0122] S9. Maximum Likelihood Estimation. Rotate the prism n times to acquire n cooperative target images, each image having s feature points. Construct a cost function to minimize the reprojection error of each feature point:

[0123]

[0124] Where, p ij For the j-th feature point in the i-th image, It is the projection point of the j-th feature point in the i-th image onto the image plane. R is determined by steps S6, S7, and S9. vw t vw f, C x C y , and Using the initial value as an initial value, the Levenberg-Marquardt algorithm is used to obtain the optimal value;

[0125] S10. The rotational relationship between the virtual camera and the real camera is determined. For example... Figure 4 As shown, according to Rodrigues' rotation formula, the view axis rotation transformation matrix, i.e., the rotation R of the virtual camera relative to the real camera, is... cv :

[0126] R cv =A p +(IA p cosρ+B p sinρ

[0127] Where ρ is the pitch angle of the line of sight, I is the third-order identity matrix, and A p and B p They are respectively:

[0128]

[0129] in, The azimuth angle of the line of sight;

[0130] S11. The translation relationship between the virtual camera and the real camera is determined. For example... Figure 4 As shown, the translation t of the virtual camera's optical center relative to the real camera's optical center cv That is, the optical center O of the virtual camera vc Coordinates in the real camera coordinate system. Construct O vc Error cost function relative to all imaging rays:

[0131]

[0132] Where γ is and The angle between them. The Levenberg-Marquardt algorithm is used to solve this cost function, obtaining O. vc In O c -X c Y c Z c The coordinates below, i.e., t cv ;

[0133] S12. Determine the target pose. For example... Figure 3 and 4 As shown, the pose of the target in the real camera coordinate system can be represented as:

[0134]

[0135] In the formula, This is the transformation matrix between the virtual camera coordinate system and the real camera coordinate system. This is the transformation matrix between the target coordinate system and the virtual camera coordinate system, i.e., the pose of the cooperative target in the virtual camera coordinate system;

[0136] S13. Improved accuracy. Due to the influence of noise, there is a certain residual error in the object space.

[0137] e ij =f(τ)-(R·p w +t)

[0138] Considering all feature points from all viewpoints, a function to minimize the sum of squared residuals in the object space is established:

[0139]

[0140] Using the initial values ​​of R and t determined in step S12 and f determined in step S7, the Levenberg-Marquardt algorithm is used to solve the nonlinear equation system, thereby improving the accuracy of R and t.

[0141] S14. Robustness Improvement. The camera's line of sight is adjusted by rotating the prism. Multiple observations of the control points are performed, and multiple sets of R and t are obtained using steps S3-S13. These multiple sets of rotation and translation information are fused, and the problem of multi-set rotation fusion is transformed into: (This is achieved by minimizing the chord distance norm.)

[0142]

[0143] in, Let be the rotation obtained in step S12 during the i-th observation. The optimal solution to this problem is:

[0144] R = UDV T

[0145] Where U and V satisfy M = UΣV T M is defined as The mean of translation and focal length obtained from multiple observations is used as the final translation and focal length estimation result:

[0146]

[0147] in, The translation obtained from step S12 during the i-th observation, The focal length is obtained from step S7 during the i-th observation.

[0148] The above description of the embodiments is provided to enable those skilled in the art to understand and use the invention. It will be apparent to those skilled in the art that various modifications can be made to these embodiments, and the general principles described herein can be applied to other embodiments without inventive effort. Therefore, the present invention is not limited to the above embodiments, and any improvements and modifications made by those skilled in the art based on the disclosure of the present invention without departing from the scope of the invention should be within the protection scope of the present invention.

Claims

1. A six-degree-of-freedom pose measurement method, characterized in that, A six-degree-of-freedom measurement system was used. The six-degree-of-freedom measurement system includes a camera (1), a prism (2), and a target (3); the optical axis of the prism (2) is coaxial with the line of sight of the camera (1); the target (3) is mounted on the target to be measured and is set within the combined field of view of the camera (1) and the prism (2); The target (3) includes a mounting base (31) and a main shaft (32) mounted on the mounting base (31). A fixing plate (33) is provided on the main shaft (32). An annular sleeve (34) is fixedly connected to the fixing plate (33). A driving component (35) is installed outside the annular sleeve (34). The main shaft of the driving component (35) passes through the annular sleeve (34) and is connected to a transmission component (36). A roller (37) is provided on the transmission component (36). A slider (38) is connected to the roller (37). Under the drive of the driving component (35), the slider (38) can slide radially from the center of the annular sleeve (34) to the edge through the roller (37). A main marker point (321) is installed on the top of the main shaft (32), and an auxiliary marker point (381) is installed on the top of the slider (38). The six-degree-of-freedom pose measurement method specifically includes the following steps: S1: Set the camera (1) and prism (2) in front of the target to be measured, and install the target (3) on the target to be measured. The target (3) is within the combined field of view of the camera (1) and prism (2). S2: Constructing a Real Camera Coordinate System O c -X c Y c Z c Virtual camera coordinate system O vc -X vc Y vc Z vc Prism coordinate system O p -X p Y p Z p and world coordinate system O w -X w Y w Z w ; S3: Rotate the prism (2) to the initial position and the camera (1) captures the image of the marker point; S4: Determine the relationship between the marker points in space and the virtual image points, construct the collinearity equation between the object and the image, and preliminarily determine the focal length of the virtual camera. f ; S5: Based on focal length f The rotation matrix of the world coordinate system relative to the virtual camera is determined using a pose estimation method. R vw Translation matrix t vw And initially extract the distortion coefficients of the virtual camera. and After rotating the prism n times (2), a cost function is constructed for maximum likelihood estimation, and the optimal value is obtained. S6: Determine the rotation and translation relationships of the virtual camera relative to the real camera, and determine the pose of the target under test in the real camera coordinate system.

2. The six-degree-of-freedom pose measurement method according to claim 1, characterized in that, The transmission component (36) includes a driving bevel gear (361) connected to the main shaft of the motor (35) and a driven bevel gear (362) meshing with the driving bevel gear (361). The driven bevel gear (362) is fixedly connected to the drive sleeve (363).

3. The six-degree-of-freedom pose measurement method according to claim 2, characterized in that, The drive sleeve (363) is provided with a spiral groove, one end of the roller (37) is located in the spiral groove, and the other end of the roller (37) is connected to the slider (38).

4. The six-degree-of-freedom pose measurement method according to claim 1, characterized in that, In step S4, the specific relationships between the spatial markers and the virtual image points are as follows: in, Let the coordinates be the homogeneous barycenter coordinates. For the first i The coordinates of each feature point in the camera coordinate system f x , f y The equivalent focal length in both the horizontal and vertical directions; Virtual control point Cc j The coordinates in the camera coordinate system, ( u i , v i () is a virtual control point Cc j Image coordinates, s The factor representing the non-perpendicularity of the horizontal and vertical axes of the image. C x , C y () is the principal point of the camera; Among them, the first i The scaling factor for each feature point is: ,pass n Two control points can be obtained n A system of linear equations: make , X If the system of equations can be expressed linearly using the fundamental solution set, then... , , ; The focal length of the virtual camera f Determined according to the following formula: in, , The least squares method can be used to obtain A and B Further, we can obtain .

5. The six-degree-of-freedom pose measurement method according to claim 1, characterized in that, In step S4, the imaging ray... Determined by the following formula: in, For parameters; No. i From the perspective of the first j The projected vector light from each marker point interacts with the exit point of the prism. K i,j It can be obtained through the following formula: in, D Let be the distance from the optical center of the camera to the plane of the prism. t p Let be the thickness of the prism. t 0 , t 1 For parameters, For the prism corner, α The wedge angle of the prism; No. i From the perspective of the first j Projecting vector light at each marker point L i,j It can be obtained through the following formula: in, n r The refractive index of the prism, , ( uj i , vi j ) is the prism rotated to the th i At the position, the first j A single point, and These are the normal vectors of the first and second planes of the prism, respectively; The pose estimation method uses an algorithm that includes one of LHM, EPnP, RPnP, DLS, OPnP, ASPnP, SDP, PPnP, or EPPnP.

6. The six-degree-of-freedom pose measurement method according to claim 1, characterized in that, In step S5, the distortion coefficients of the virtual camera are initially extracted. and The specific steps are as follows: Construct a system of linear equations by combining all cooperative feature points: in,( u di , v di ) is the first i Coordinates of the distorted image point and for dx and dy The corresponding coefficients of the fitted term, f Focal length R vw and t vw These are the rotation and translation matrices of the world coordinate system relative to the virtual camera, respectively. for The first line, for The second line, for The third line, for t vw The coordinates are obtained by solving the system of equations using the least squares method. and ; The specific steps of maximum likelihood estimation are as follows: Rotation n Secondary prism, collecting data n Sub-cooperative target images, each image has s For each feature point, a cost function is constructed with the objective of minimizing the reprojection error of each feature point: in, p ij For the first i The sub-image j The actual image points of each feature point It is the first i The sub-image j The projection points of each feature point onto the image plane; by R vw , t vw , f , C x , C y , and Using the initial value as an initial value, the Levenberg-Marquardt algorithm is used to obtain the optimal value.

7. The six-degree-of-freedom pose measurement method according to claim 1, characterized in that, In step S6, the virtual camera rotates relative to the real camera. R cv for: in, ρ The pitch angle of the line of sight. I It is a third-order identity matrix. A p and B p They are respectively: ， in,( u x , u y , u z )=(-sin φ cos φ ,0), φ The azimuth angle of the line of sight; Obtain the translation of the virtual camera relative to the real camera t cv The specific steps are as follows: Build O vc Error cost function relative to all imaging rays: in, γ for and The angle between them; The cost function is solved using the Levenberg-Marquardt algorithm to obtain... O vc exist O c -X c Y c Z c The coordinates below, i.e. t cv ; The pose of the target object in the real camera coordinate system can be represented as: In the formula, This is the transformation matrix between the virtual camera coordinate system and the real camera coordinate system. It is the transformation matrix of the target coordinate system relative to the virtual camera coordinate system, that is, the pose of the cooperative target in the virtual camera coordinate system.

8. The six-degree-of-freedom pose measurement method according to claim 7, characterized in that, Step S6 is followed by: S7: Improved accuracy by establishing a function to minimize the sum of squared residuals in the object space: As determined in step S6 R and t And determined in step S5 f Using the initial values, the Levenberg-Marquardt algorithm is used to solve the nonlinear equation system.

9. A six-degree-of-freedom pose measurement method according to claim 8, characterized in that, Step S7 is followed by: S8: Robustness improvement, rotating prism (2) to adjust camera (1) line of sight, multiple observations of control points to obtain multiple sets of data. R and t By fusing multiple sets of rotation and translation information and minimizing the chord distance norm, the problem of fusing multiple sets of rotation information is transformed into: in In the first i The rotation obtained in step S6 during the next observation R The optimal solution to the problem is: in, U and V satisfy M=UΣV T , M Defined as D= dig (1,1, det ( UV T The mean of translation and focal length obtained from multiple observations is used as the final translation and focal length estimation result: in, For the first i The translation obtained in step S6 during the next observation For the first i The focal length obtained in step S5 during the next observation.

Images