Virtual multi-view six-degree-of-freedom pose measurement method, medium and system

Abstract

The invention belongs to the technical field of space geometric measurement, and particularly relates to a virtual multi-view six-degree-of-freedom pose measurement method, medium and system, and the method comprises the steps: obtaining the pose information of a cooperative target located in a combined view field through a camera and a prism which are coaxial; a virtual camera is formed by rotating the prism; establishing a coordinate system; target rotation representation; estimating a first rotation matrix; determining a visual axis and a visual axis included angle of the virtual camera; determining a second rotation matrix; establishing a rotation error cost function; representing a rotation matrix and a rotation error cost function through quaternions; fusing multi-rotation information; determining an emergent point and an emergent ray vector of the prism; representing a feature point coordinate of the cooperative target; determining an object space residual error; constructing an object space residual cost function; and translation information fusion. Compared with the prior art, the problem that an existing six-degree-of-freedom pose measurement device is easily influenced by noise is solved, and the method provided by the scheme realizes effective improvement of measurement precision, adaptability and stability.

Description

Technical Field

[0001] The present invention belongs to the technical field of spatial geometry measurement, and in particular relates to a virtual multi-viewpoint six-degree-of-freedom posture measurement method, medium and system. Background Art

[0002] With the development of intelligent manufacturing, the extraction of motion information of manufacturing equipment is a key feedback link of intelligent manufacturing. As the key perceiver of information extraction, the test system directly determines the working accuracy of the manufacturing system. Currently, the six-degree-of-freedom measurement devices used in industrial sites are usually difficult to operate, have complex components, occupy a large space, are not easy to carry, and have a high market price, making them difficult to be widely used in industrial production.

[0003] Chinese invention patent CN102636139A proposes a spatial six-degree-of-freedom dynamic measurement device and method. The device uses a combination of multiple wire-type displacement sensors to measure the six degrees of freedom of the target to be measured, but the measurement device uses contact measurement and is applicable to limited measurement occasions.

[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 to be measured, but it is easily affected by noise such as light, vibration, and impact, and cannot meet the complex and changeable industrial field measurement needs.

[0005] Therefore, it is necessary to propose a posture measurement system and method that can overcome the fact that the measuring device is susceptible to noise such as light, vibration, and impact. Summary of the invention

[0006] The purpose of the present invention is to provide a virtual multi-viewpoint six-degree-of-freedom posture measurement method, medium and system in order to solve at least one of the above problems, so as to solve the problem that the six-degree-of-freedom posture measurement device in the prior art is easily affected by noise such as light, vibration and impact. The virtual multi-viewpoint six-degree-of-freedom posture measurement method proposed in this scheme achieves an effective improvement in measurement accuracy, adaptability and stability.

[0007] The purpose of the present invention is achieved through the following technical solutions:

[0008] The first aspect of the present invention discloses a virtual multi-viewpoint six-degree-of-freedom posture measurement method, which obtains the posture information of a cooperative target by combining a single camera and a prism; the camera and the prism are coaxially arranged, and the cooperative target is located in the combined field of view of the camera and the prism;

[0009] The method comprises the following steps:

[0010] A virtual camera is formed by rotating the prism;

[0011] S1: Establish coordinate system: Construct camera coordinate system O r -X r Y r Z r , virtual camera coordinate system O v -X v Y v Z v , prism coordinate system O p -X p Y p Z p and cooperative target coordinate system O t -X t Y t Z t ;

[0012] S2: Target rotation representation: represents the rotation matrix R of the cooperative target coordinate system relative to the camera coordinate system;

[0013] S3: Capture the cooperative target through the camera, and estimate the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system through the pose solving algorithm

[0014] S4: determining the virtual camera visual axis and the angle ψ between the camera visual axis and the virtual camera visual axis based on the parameters of the prism and the rotation angle of the prism;

[0015] S5: Determine the rotation matrix of the virtual camera coordinate system relative to the camera coordinate system based on the prism parameters, the prism rotation angle and the angle ψ

[0016] S6: Establishing rotation error cost function;

[0017] S7: Quaternion representation of rotation matrix;

[0018] S8: quaternion cost function of rotation error;

[0019] S9: Fusion of multiple rotation information to obtain the optimal rotation matrix R;

[0020] S10: Determine the exit point and exit light vector of the prism;

[0021] S11: represents the coordinates of the feature points of the cooperative target in the prism coordinate system;

[0022] S12: Determine the object-space residual between the feature point and the outgoing light;

[0023] S13: construct object-side residual cost function;

[0024] S14: Fusion of translation information to obtain the optimal translation matrix t.

[0025] Preferably, the cross-section of the prism is wedge-shaped.

[0026] Preferably, the parameters of the camera match the parameters of the prism.

[0027] Preferably, the parameters of the camera and the parameters of the prism are matched by the following formula:

[0028]

[0029] Where α is the prism wedge angle, n is the prism refractive index, and are half of the horizontal and vertical physical size of the camera image plane, f is the focal length of the camera, γ c1 is the camera field of view.

[0030] Preferably, the posture solution algorithms include LHM, EPnP, RPnP, DLS, OPnP, ASPnP, SDP, PPnP and EPPnP.

[0031] Preferably, in step S6, the error of the rotation matrix is ​​characterized by the least squares norm to establish a rotation error cost function:

[0032]

[0033] in, is the rotation matrix of the virtual camera coordinate system relative to the camera coordinate system at the i-th viewing angle, is the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system at the i-th perspective.

[0034] Preferably, in step S8, the quaternion cost function of the rotation error is as follows:

[0035]

[0036] In the formula, is the quaternion of the rotation matrix, S is the quaternion form group of the rotation matrix R, is the rotation matrix of the virtual camera coordinate system relative to the camera coordinate system at the i-th viewing angle, is the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system at the i-th perspective.

[0037] Preferably, in step S10, the outgoing light vector of the prism is determined according to the law of refraction, and the exit point of the prism is determined according to the spatial geometric relationship.

[0038] Preferably, in step S14, the translation matrix t is a function of the rotation matrix R:

[0039]

[0040] Where n is the refractive index of the prism, L i,j is the outgoing light vector, P w,i is the characteristic point of the cooperative target, K i,j For the launch point.

[0041] A second aspect of the present invention discloses a computer-readable storage medium, wherein the computer-readable storage medium includes a stored computer program, wherein when the computer program is running, the device where the computer-readable storage medium is located is controlled to execute any of the above methods.

[0042] The third aspect of the present invention discloses a virtual multi-viewpoint six-degree-of-freedom posture measurement system for obtaining the posture information of a cooperative target. The system includes a camera, a prism, a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor. When the processor executes the computer program, any of the methods described above is implemented; the cross-section of the prism is wedge-shaped, the camera and the prism are arranged coaxially, and the cooperative target is located in the combined field of view of the camera and the prism.

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

[0044] (1) By fusing multi-viewpoint pose information, the impact of noise such as illumination and vibration on measurement can be reduced, achieving higher measurement accuracy and robustness.

[0045] (2) By combining cameras with prisms, multi-camera and multi-angle observation and measurement can be achieved with only a single camera, which has the advantages of compact structure, low cost, flexible movement, and easy control. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 It is a schematic diagram of the virtual multi-viewpoint six-degree-of-freedom posture measurement method of the present invention;

[0047] Figure 2It is a diagram showing the principle of posture calculation in the virtual multi-viewpoint six-degree-of-freedom posture measurement method of the present invention;

[0048] Figure 3 It is a schematic diagram of displacement solution in the virtual multi-viewpoint six-degree-of-freedom posture measurement method of the present invention;

[0049] In the figure: 1- camera; 2- prism; 3- cooperative target; 4- virtual camera. DETAILED DESCRIPTION

[0050] The present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. This embodiment is implemented based on the technical solution of the present invention, and provides a detailed implementation method and specific operation process, but the protection scope of the present invention is not limited to the following embodiments.

[0051] The following unfinished matters can all adopt existing technologies.

[0052] Example

[0053] like Figure 1-3 As shown, the present application proposes a virtual multi-viewpoint six-degree-of-freedom posture measurement system, the measurement system includes a camera 1, a prism 2 and a cooperative target 3, the visual axis of the camera 1 is coaxially arranged with the prism 2, and the cooperative target 3 is set in the combined field of view of the camera 1 and the prism 2. To ensure that the camera 1 and the prism 2 can work normally and form an image after being combined, the parameters of the camera 1 and the parameters of the prism 2 need to match.

[0054] Preferably, the cross section of the prism is wedge-shaped;

[0055] Preferably, the parameters of the camera and the parameters of the prism can be matched by the following formula:

[0056]

[0057] Where α is the prism wedge angle, n is the prism refractive index, and are half of the horizontal and vertical physical size of the camera image plane, f is the focal length of the camera, γ c1 is the camera field of view.

[0058] Measurement methods, such as Figure 1-3 , the steps are as follows:

[0059] In this method, the camera visual axis is adjusted by rotating the prism to form a virtual camera with equivalent observation effect;

[0060] S1. Coordinate system establishment. Construct camera 1 coordinate system O r -X r Y r Z r , virtual camera 4 coordinate system O v -X vY v Z v , prism 2 coordinate system O p -X p Y p Z p and cooperative target 3 coordinate system O t -X t Y t Z t ;

[0061] S2. Target rotation representation. At any viewing angle, the rotation matrix of the cooperative target coordinate system relative to the camera coordinate system is expressed as:

[0062]

[0063] In the formula, is the rotation matrix of the virtual camera coordinate system relative to the camera coordinate system, is the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system.

[0064] S3. The camera captures the cooperative target and uses the pose solving algorithm to estimate the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system

[0065] S4. Virtual camera visual axis L v Sure:

[0066]

[0067] in, θ is the angle of rotation of the prism.

[0068] According to the geometric relationship, the angle θ of the prism rotation is the virtual camera visual axis L v With camera boresight L c The angle ψ is:

[0069]

[0070] Since the rotation axis of the prism must be perpendicular to L v With L c The vector L of the plane m ,but:

[0071]

[0072] S5. Rotation matrix of virtual camera coordinate system relative to camera coordinate system Sure:

[0073]

[0074] Where I is the third-order unit matrix, A p and Bp They are:

[0075]

[0076]

[0077] S6. Establishment of rotation error cost function. The least square norm is used to characterize the error of the rotation matrix, that is:

[0078]

[0079] in, is the rotation matrix of the virtual camera coordinate system relative to the camera coordinate system at the i-th viewing angle, is the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system at the i-th perspective.

[0080] S7. Quaternion representation of rotation matrix. Any quaternion can be represented as The rotation matrix is ​​represented by a quaternion:

[0081]

[0082] In the formula, I 3×3 is a 3×3 identity matrix, ρ is [q0,q1,q2], and ρ× is an antisymmetric matrix:

[0083]

[0084] S8. Quaternion cost function of rotation error. Combining S3 and S4, further deduction can be obtained:

[0085]

[0086] In the formula, S is the quaternion form group of the rotation matrix R.

[0087] S9. Multi-rotation information fusion. Further simplification and derivation, the rotation error cost function can be expressed as:

[0088]

[0089] In the formula, B b The eigenvector corresponding to the maximum eigenvalue of is the optimal solution of multi-rotation information fusion.

[0090] S10. Determine the exit point and exit light vector in the prism coordinate system. The imaging light vector of the jth image point at the i-th viewing angle is:

[0091]

[0092] Among them, (u i,j ,v i,j ) is the image coordinate of the jth image point under the ith viewing angle, and f is the focal length of the camera.

[0093] According to the law of refraction, the outgoing light vector of the prism is:

[0094]

[0095] Among them, N1 and N2 are the normal vectors of the two planes of the prism respectively.

[0096] Combined with the spatial geometric relationship, the emission point K i,j It can be expressed as:

[0097]

[0098] Where D1 is the distance between the camera and the prism, D2 is the center thickness of the prism, and θ is the rotation angle of the prism.

[0099] S11. Cooperation feature point representation. Feature point P of cooperation target w,i The coordinates in the prism coordinate system can be expressed as:

[0100] p pc,i =RP w,i +tK i,j

[0101] Where t is the translation matrix of the cooperative target coordinate system relative to the camera coordinate system, P w,i is the coordinate of the feature point in the cooperative target coordinate system.

[0102] S12. Determine the object-space residual. Due to the existence of noise, there is an object-space residual between the feature point and the outgoing light. The object-space residual of the i-th feature point can be expressed as:

[0103] e i (R,t)=(IL i,j )(RP w,i +tK i,j )

[0104] Where I is the unit matrix.

[0105] S13. Object-space residual cost function. Taking into account the object-space residuals of all feature points, the object-space residual cost function can be expressed as:

[0106]

[0107] S14. Translation information fusion. When the rotation matrix R is known, the translation matrix t has an optimal solution:

[0108]

[0109] T(R,t) is the transformation matrix of the cooperative target coordinate system relative to the camera coordinate system.

[0110] Preferably, The solution algorithm is any one of LHM, EPnP, RPnP, DLS, OPnP, ASPnP, SDP, PPnP and EPPnP.

[0111] The above description of the embodiments is to facilitate the understanding and use of the invention by those skilled in the art. It is obvious that those skilled in the art can easily make various modifications to these embodiments and apply the general principles described herein to other embodiments without creative work. Therefore, the present invention is not limited to the above embodiments, and 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 present invention should be within the scope of protection of the present invention.

Claims

1. A virtual multi-viewpoint six-degree-of-freedom posture measurement method, characterized in that: The position information of the cooperative target is obtained by combining a single camera and a prism; the camera and the prism are coaxially arranged, and the cooperative target is located in the combined field of view of the camera and the prism; The method comprises the following steps: A virtual camera is formed by rotating the prism; S1: Establish coordinate system: Construct camera coordinate system O r -X r Y r Z r , virtual camera coordinate system O v -X v Y v Z v , prism coordinate system O p -X p Y p Z p and cooperative target coordinate system O t -X t Y t Z t ; S2: Target rotation representation: represents the rotation matrix R of the cooperative target coordinate system relative to the camera coordinate system; S3: Capture the cooperative target through the camera, and estimate the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system through the pose solving algorithm S4: determining the virtual camera visual axis and the angle ψ between the camera visual axis and the virtual camera visual axis based on the parameters of the prism and the rotation angle of the prism; S5: Determine the rotation matrix of the virtual camera coordinate system relative to the camera coordinate system based on the prism parameters, the prism rotation angle and the angle ψ S6: Establishing rotation error cost function; S7: Quaternion representation of rotation matrix; S8: quaternion cost function of rotation error; S9: Fusion of multiple rotation information to obtain the optimal rotation matrix R; S10: Determine the exit point and exit light vector of the prism; S11: represents the coordinates of the feature points of the cooperative target in the prism coordinate system; S12: Determine the object-space residual between the feature point and the outgoing light; S13: construct object-side residual cost function; S14: Fusion of translation information to obtain the optimal translation matrix t.

2. A virtual multi-viewpoint six-degree-of-freedom posture measurement method according to claim 1, characterized in that: The parameters of the camera match those of the prism.

3. A virtual multi-viewpoint six-degree-of-freedom posture measurement method according to claim 2, characterized in that: The camera parameters and prism parameters are matched as follows: Where α is the prism wedge angle, n is the prism refractive index, and are half of the horizontal and vertical physical size of the camera image plane, f is the focal length of the camera, γ c1 is the camera field of view.

4. A virtual multi-viewpoint six-degree-of-freedom posture measurement method according to claim 1, characterized in that: The posture solution algorithms include LHM, EPnP, RPnP, DLS, OPnP, ASPnP, SDP, PPnP and EPPnP.

5. A virtual multi-viewpoint six-degree-of-freedom posture measurement method according to claim 1, characterized in that: In step S6, the error of the rotation matrix is ​​characterized by the least squares norm to establish a rotation error cost function: in, is the rotation matrix of the virtual camera coordinate system relative to the camera coordinate system at the i-th viewing angle, is the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system at the i-th perspective.

6. A virtual multi-viewpoint six-degree-of-freedom posture measurement method according to claim 1, characterized in that: In step S8, the quaternion cost function of the rotation error is as follows: In the formula, is the quaternion of the rotation matrix, S is the quaternion form group of the rotation matrix R, is the rotation matrix of the virtual camera coordinate system relative to the camera coordinate system at the i-th viewing angle, is the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system at the i-th perspective.

7. A virtual multi-viewpoint six-degree-of-freedom posture measurement method according to claim 1, characterized in that: In step S10, the outgoing light vector of the prism is determined according to the law of refraction, and the outgoing point of the prism is determined according to the spatial geometric relationship.

8. A virtual multi-viewpoint six-degree-of-freedom posture measurement method according to claim 1, characterized in that: In step S14, the translation matrix t is a function of the rotation matrix R: Where n is the refractive index of the prism, L i,j is the outgoing light vector, P w,i is the coordinate of the feature point in the cooperative target coordinate system, K i,j For the launch point.

9. A computer-readable storage medium, characterized in that: The computer-readable storage medium includes a stored computer program, wherein when the computer program is running, the device where the computer-readable storage medium is located is controlled to execute any one of the methods according to claims 1 to 8.

10. A virtual multi-viewpoint six-degree-of-freedom posture measurement system for obtaining posture information of a cooperative target, characterized in that: The system includes a camera, a prism, a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, and the processor implements the method according to any one of claims 1 to 8 when executing the computer program; the cross-section of the prism is wedge-shaped, the camera and the prism are coaxially arranged, and the cooperative target is located in the combined field of view of the camera and the prism.

Images