Pose solving method, system, device and medium based on geometric constraints and degree of freedom limits

Abstract

The application discloses a pose solving method, system, device and medium based on geometric constraints and degree of freedom limitation, and the method comprises the following steps: S1, determining geometric constraints to be satisfied, degrees of freedom to be limited and an initial pose; S2, solving the pose according to the geometric constraints to be satisfied to obtain a pose transformation matrix satisfying the geometric constraints; S3, limiting the degrees of freedom of the obtained pose transformation matrix, so that the pose transformation matrix satisfies the degree of freedom limitation condition; S4, updating the current pose by using the pose transformation matrix obtained after the degree of freedom limitation; S5, performing geometric constraint checking on the pose result obtained after the pose is updated, if the constraint is not satisfied, jumping to step S2 and performing subsequent steps, if the constraint is satisfied, ending the iteration and outputting the pose result. The method can solve the pose of a workpiece based on geometric constraints under the condition of limited degrees of freedom.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of robot technology, and particularly relates to a pose solving method, system, device and medium based on geometric constraint and degree of freedom limitation. BACKGROUND

[0002] In robot engineering applications, pose solving calculation is often needed, such as in logistics sorting applications, the pose of a part needs to be identified and calculated for grasping and sorting, in automatic driving applications, the pose of the self needs to be determined according to collected environmental data to realize positioning and navigation, and in machining applications, the pose of a workpiece needs to be determined for automatic machining.

[0003] Workpiece positioning based on vision is to determine the three-dimensional pose of a workpiece to be machined by a vision sensor before machining, so as to generate a machining trajectory and further complete workpiece machining. Workpiece positioning is usually based on the six-point positioning principle, and the pose of the workpiece in different degrees of freedom needs to be determined step by step and the degrees of freedom need to be limited, so as to finally determine the pose of the workpiece. Existing workpiece pose solving is all completed based on vision, and the more common pose solving methods can be divided into three categories: pose estimation by epipolar geometry, PNP pose estimation and ICP method pose estimation. The pose solving method based on epipolar geometry is for matching between 2D-2D in two continuous frames; PNP is a method for solving 3D to 2D point motion estimation; ICP is a method for solving the pose estimation problem of multiple 3D to 3D points, and the method for solving ICP can be divided into linear and nonlinear. The problem in robot engineering belongs to the pose solving problem of multiple 3D to 3D points, and the existing pose solving methods such as ICP matching do not consider the geometric constraint and degree of freedom limitation in actual workpiece positioning, and distortion will occur when the actual machining process is simulated by using digital twinning technology. SUMMARY

[0004] The main purpose of the present application is to overcome the shortcomings and deficiencies of the prior art, and to provide a pose solving method, system, device and medium based on geometric constraint and degree of freedom limitation, which can solve the pose that meets the geometric constraint without changing in the specified degrees of freedom.

[0005] In order to achieve the above purpose, the present application adopts the following technical scheme:

[0006] The pose solving method based on geometric constraint and degree of freedom limitation comprises the following steps:

[0007] S1, determining the geometric constraint to be met, the degree of freedom to be limited and the initial pose;

[0008] S2, solving the pose according to the geometric constraint to be met to obtain a pose transformation matrix that meets the geometric constraint;

[0009] S3, limiting the degree of freedom of the obtained posture transformation matrix so that the posture transformation matrix meets the degree of freedom limitation condition;

[0010] S4. Update the current posture using the posture transformation matrix obtained after the degree of freedom is limited;

[0011] S5. Perform geometric constraint verification on the pose result obtained after the pose is updated. If the constraint is not satisfied, jump to step S2 and perform subsequent steps. If the constraint is satisfied, end the iteration and output the pose result.

[0012] The present invention also includes a posture solving system based on geometric constraints and degree of freedom limitations, the system applying the posture solving method provided by the present invention, the system comprising:

[0013] Input limitation module, posture transformation matrix solving module, degree of freedom limitation module, posture transformation matrix updating module and constraint verification module;

[0014] The input constraint module is used to input the geometric constraints to be satisfied, the degrees of freedom to be limited, and the initial pose;

[0015] The pose transformation matrix solving module is used to solve the pose according to the geometric constraints to be satisfied and obtain the pose transformation matrix that satisfies the geometric constraints;

[0016] The degree of freedom limitation module is used to limit the degree of freedom of the obtained posture transformation matrix;

[0017] The pose transformation matrix update module is used to update the current pose according to the pose transformation matrix obtained after the degree of freedom is limited;

[0018] The constraint verification module performs geometric constraint verification on the pose results obtained after the pose is updated.

[0019] The present invention also includes a computer device, including a memory and a processor, the memory stores a computer program, and when the processor executes the computer program, it implements the posture solution method based on geometric constraints and degree of freedom limitations provided by the present invention.

[0020] The present invention also includes a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, the posture solving method based on geometric constraints and degree of freedom limitations provided by the present invention is implemented.

[0021] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0022] 1. The present invention takes into account the problem of degree of freedom limitation and realizes the solution, which has a wider range of applications compared with traditional posture solution methods; and the use of quaternions to deal with the problem of rotational degree of freedom limitation is more intuitive and efficient than the rotation matrix solution.

[0023] 2. The present invention does not need to scan the entire workpiece and reconstruct the complete workpiece point cloud during visual processing. It only needs to collect part of the point cloud to complete the reconstruction of several geometric features of the workpiece to complete positioning.

[0024] 3. Based on the six-point positioning principle, the workpiece's position in different degrees of freedom is determined step by step and then limited. The final position of the workpiece is closer to the positioning scheme in the actual production process. By changing the visual scanning scheme, the positioning reference can be changed during production without re-clamping, thus avoiding the clamping errors caused by multiple clamping.

[0025] 4. Compared with traditional positioning methods, it has stronger applicability. Traditional physical positioning elements need to contact the positioning base surface. When the shape of the workpiece is more complex, the positioning elements become difficult to design. Visual reconstruction has a higher degree of freedom and can reconstruct surfaces that cannot be directly contacted, thereby achieving positioning. At the same time, it solves the obstacle avoidance needs of physical positioning elements in the processing process. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Figure 1 It is a flow chart of the method of the present invention. DETAILED DESCRIPTION

[0027] The present invention will be described in further detail below with reference to the embodiments and drawings, but the embodiments of the present invention are not limited thereto.

[0028] Example

[0029] like Figure 1 As shown, the present invention, based on the pose solving method of geometric constraints and degree of freedom limitation, comprises the following steps:

[0030] S1. Determine the geometric constraints to be satisfied, the degrees of freedom to be limited, and the initial pose; wherein the geometric constraints include angle constraints and distance constraints;

[0031] The degrees of freedom include translational freedom along the x, y, and z axes and rotational freedom around the x, y, and z axes and their combinations; the initial pose T0 is initialized to the unit matrix.

[0032] S2. Solve the pose according to the geometric constraints to be satisfied, and obtain a pose transformation matrix that satisfies the geometric constraints. Specifically, the pose transformation matrix is ​​solved according to the geometric constraints to be satisfied, and the pose transformation matrix obtained makes the pose of the object before and after the transformation satisfy the specified geometric constraints. The pose transformation matrix is ​​expressed as:

[0033]

[0034] Among them, R is the rotation matrix, t is the translation vector, t x , ty , t z is the component of the translation vector t along the x, y, and z axes; r represents the element of the rotation matrix, and its subscript is the sequence number of the row and column, such as r 11 Represents a row and column of elements.

[0035] S3. Limit the degrees of freedom of the obtained posture transformation matrix so that the posture transformation matrix satisfies the degree of freedom limitation condition. Specifically, limit the degrees of freedom of the obtained posture transformation matrix. When the degree of freedom of translation is limited, the corresponding component of the translation vector is set to zero. For example, the translational degree of freedom along the x-axis is limited by the following formula:

[0036]

[0037] Among them, t′ is the translation vector after the degree of freedom is limited, t′ x , t′ y , t′ z are the components of t′ along the x, y, and z axes; similarly, the translation vectors that limit the translational freedom along the y and z axes are:

[0038]

[0039]

[0040] When limiting the rotational degrees of freedom, first convert the rotation matrix to a quaternion:

[0041]

[0042] Among them, q x ,q y ,q z are the three imaginary parts of the quaternion, q w is the real part of the quaternion, tr(R) represents the trace of the matrix ${R}$;

[0043] Then let the corresponding component of quaternion q be 0, and get quaternion q′, q′ x ,q′ y and q′ z are the three imaginary parts of the quaternion q′, q′ w is the real part; when the z component is set to 0, it is specifically:

[0044]

[0045] Similarly, when the x and y components are 0, q' is:

[0046]

[0047]

[0048] After normalization, it is converted back to the rotation matrix:

[0049]

[0050] Among them, R′ is the rotation matrix that limits the degree of freedom of rotation around the z axis;

[0051] The final pose transformation matrix T′ after limiting the degree of freedom is:

[0052]

[0053] S4. Update the current posture using the posture transformation matrix obtained after the degree of freedom is limited. The specific expression of updating the current posture is:

[0054] T (n+1) =T′·T (n)

[0055] Among them, T (n+1) For the current generation result, T (n) Results for the previous generation.

[0056] The ultimate desired result of the pose update is to constrain all degrees of freedom of the target workpiece so that the geometric elements of the workpiece meet specific angle constraints or distance constraints. Each updated pose only constrains certain degrees of freedom of the workpiece and is modified based on the currently limited degrees of freedom so that it only transforms on specific degrees of freedom.

[0057] The geometric constraint methods corresponding to different positioning elements are composed of four geometric constraints: point on line, point on surface, line-line parallel, and line-surface parallel. Through iteration, the degrees of freedom are restricted in stages to meet part of the four geometric constraints. After multiple iterations, all degrees of freedom are restricted.

[0058] In plane positioning, when a support pin is used to position the plane, the geometric element ve extracted from the actual workpiece source Is a point that locates the geometric element ve corresponding to the base surface on the twin model target is a plane; the geometric constraint is the point-on-surface constraint, that is, solving the transformation matrix T so that the point ve source Kneading dough target Coincidence; the mathematical process is essentially to move a point so that it coincides with a point on the target surface; if point P is a point on the surface, vector n is the normal vector of the surface, and point A is the point outside the surface that you want to coincide with, then calculate the vector:

[0059] t=PA

[0060] The coincidence condition can be satisfied by translating point A along the direction of vector t. At this time, the pose transformation matrix T is:

[0061]

[0062] The solution of point-on-line constraints is the same;

[0063] The solution to the line-line parallel constraint is:

[0064] Given lines l and k, points P and A on these two lines, and direction vectors n and m, first translate vector m so that P and A coincide. m′ is the translated vector, T trans To solve the pose transformation matrix when solving point-surface constraints:

[0065] m′=T trans m

[0066] Decompose m' into components m' parallel to n || and the component m′ perpendicular to n ⊥ :

[0067] m′ || =(m′ T n)n

[0068] m′ ⊥ =m′-m′ ||

[0069] Next, by rotating m′ and m′ || Parallel, rotation axis u and rotation angle θ are calculated by the following formulas:

[0070] u=m′×m′ ||

[0071] θ=arccos(m′ T m′ || )

[0072] Convert the rotation axis and rotation angle into the rotation matrix R, and further obtain the pose matrix T rotation , and call the rotated vector m″:

[0073]

[0074] Finally, translate m″ so that its starting point coincides with point A, and the final posture transformation matrix T is:

[0075]

[0076] The solution method for line-plane parallelism constraint is similar to line-line parallelism.

[0077] S5. Perform geometric constraint verification on the pose result obtained after the pose is updated. If the constraint is not satisfied, jump to step S2 and perform subsequent steps. If the constraint is satisfied, end the iteration and output the pose result.

[0078] The geometric constraint verification is specifically to determine whether the distance or angle between two geometric elements is less than a preset threshold. If it is less than, the geometric constraint is considered to be satisfied and the iteration ends; otherwise, the constraint is not satisfied and the iteration continues.

[0079] In another embodiment, a posture solving system based on geometric constraints and degree of freedom limitations is provided. The system applies the posture solving method of the above embodiment, and the system includes:

[0080] Input limitation module, posture transformation matrix solving module, degree of freedom limitation module, posture transformation matrix updating module and constraint verification module;

[0081] The input constraint module is used to input the geometric constraints to be satisfied, the degrees of freedom to be limited, and the initial pose;

[0082] The pose transformation matrix solving module is used to solve the pose according to the geometric constraints to be satisfied and obtain the pose transformation matrix that satisfies the geometric constraints;

[0083] The degree of freedom limitation module is used to limit the degree of freedom of the obtained posture transformation matrix;

[0084] The pose transformation matrix update module is used to update the current pose according to the pose transformation matrix obtained after the degree of freedom is limited;

[0085] The constraint verification module performs geometric constraint verification on the pose results obtained after the pose is updated.

[0086] In another embodiment, a computer device is provided, including a memory and a processor, wherein the memory stores a computer program, and the processor implements the posture solution method of the above embodiment when executing the computer program.

[0087] In another embodiment, a computer-readable storage medium is provided, which stores a computer program. When the computer program is executed by a processor, the posture solving method of the above embodiment is implemented.

[0088] It should also be noted that, in this specification, terms such as "comprises", "includes" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or apparatus comprising a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or apparatus. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of other identical elements in the process, method, article or apparatus comprising the element.

[0089] The above description of the disclosed embodiments is intended to enable one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention is not limited to the embodiments shown herein but is intended to conform to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A pose solving method based on geometric constraints and degree of freedom limitations, characterized by: The following steps are involved: S1. Determine the geometric constraints to be satisfied, the degrees of freedom to be limited, and the initial pose; S2. Solve the pose according to the geometric constraints to be satisfied, and obtain a pose transformation matrix that satisfies the geometric constraints. Specifically, the pose transformation matrix is ​​solved according to the geometric constraints to be satisfied, and the pose transformation matrix obtained makes the pose of the object before and after the transformation satisfy the specified geometric constraints. The pose transformation matrix is ​​expressed as: Among them, R is the rotation matrix, t is the translation vector, t x , t y , t z are the components of the translation vector t along the x, y, and z axes; r represents the elements of the rotation matrix, and its subscripts are the row and column numbers; S3. Limit the degrees of freedom of the obtained posture transformation matrix so that the posture transformation matrix meets the degree of freedom limitation conditions; limit the degrees of freedom of the obtained posture transformation matrix. When the degree of freedom is limited to the translation, set the corresponding component of the translation vector to zero, and limit the translational freedom along the x-axis by the following formula: Among them, t′ is the translation vector after the degree of freedom is limited, t′ x , t′ y , t′ z are the components of t′ along the x, y, and z axes; similarly, the translation vectors that limit the translational freedom along the y and z axes are: When limiting the rotational degrees of freedom, first convert the rotation matrix to a quaternion: Among them, q x ,q y ,q z are the three imaginary parts of the quaternion, q w is the real part of the quaternion, tr(R) represents the trace of the matrix R; Then let the corresponding component of quaternion q be 0, and get quaternion q′, q′ x ,q′ y and q′ z are the three imaginary parts of the quaternion q′, q′ w is the real part; when the z component is set to 0, it is specifically: Similarly, when the x and y components are 0, q' is: After normalization, it is converted back to the rotation matrix: Among them, R ′ That is, the rotation matrix that limits the degree of freedom of rotation around the z-axis; The final pose transformation matrix T after limiting the degree of freedom ′ That is: S4. Update the current posture using the posture transformation matrix obtained after the degree of freedom is limited; S5. Perform geometric constraint verification on the pose result obtained after the pose is updated. If the constraint is not satisfied, jump to step S2 and perform subsequent steps. If the constraint is satisfied, end the iteration and output the pose result.

2. The pose solving method based on geometric constraints and degree of freedom limitation according to claim 1, characterized in that: In step S1, the geometric constraints include angle constraints and distance constraints; The degrees of freedom include translational freedom along the x, y, and z axes and rotational freedom around the x, y, and z axes and their combinations; The initial pose T0 is initialized to the identity matrix.

3. The pose solving method based on geometric constraints and degree of freedom limitation according to claim 1, characterized in that: In step S4, updating the current posture is specifically expressed as: T (n+1) =T′·T (n) Among them, T (n+1) For the current generation result, T (n) Results for the previous generation.

4. The method for solving the pose based on geometric constraints and degree of freedom limitations according to claim 3, characterized in that: The ultimate desired result of the pose update is to constrain all degrees of freedom of the target workpiece so that the geometric elements of the workpiece meet specific angle constraints or distance constraints. Each updated pose only constrains certain degrees of freedom of the workpiece and is modified based on the currently limited degrees of freedom so that it only transforms on specific degrees of freedom. The geometric constraint methods corresponding to different positioning elements are composed of four geometric constraints: point on line, point on surface, line-line parallel, and line-surface parallel. Through iteration, the degrees of freedom are restricted in stages to meet part of the four geometric constraints. After multiple iterations, all degrees of freedom are restricted. In plane positioning, when a support pin is used to position the plane, the geometric element ve extracted from the actual workpiece source Is a point that locates the geometric element ve corresponding to the base surface on the twin model target is a plane; the geometric constraint is the point-on-surface constraint, that is, solving the transformation matrix T so that the point ve source Kneading dough target Coincidence; the mathematical process is essentially to move a point so that it coincides with a point on the target surface; if point P is a point on the surface, vector n is the normal vector of the surface, and point A is the point outside the surface that you want to coincide with, then calculate the vector: t=PA The coincidence condition is satisfied by translating point A along the direction of vector t. At this time, the pose transformation matrix T is: The solution of point-on-line constraints is similar; The solution to the line-line parallel constraint is: Given lines l and k, points P and A on these two lines, and direction vectors n and m, first translate vector m so that P and A coincide. m′ is the translated vector, T trans To solve the pose transformation matrix when solving point-surface constraints: m ′ =T trans m Decompose m' into components m' parallel to n || and the component m′ perpendicular to n ⊥ : m′ || =(m′ T n)n m′ ⊥ =m′-m′ || Next, by rotating m′ and m′ || Parallel, rotation axis u and rotation angle θ are calculated by the following formulas: u=m′×m′ || θ=arccos(m′ T m′ || ) Convert the rotation axis and rotation angle into the rotation matrix R, and further obtain the pose matrix T rotation , and call the rotated vector m″: Finally, translate m″ so that its starting point coincides with point A, and the final posture transformation matrix T is: The solution method for line-plane parallelism constraint is similar to line-line parallelism.

5. The pose solving method based on geometric constraints and degree of freedom limitation according to claim 1, characterized in that: In step S5, the geometric constraint check is performed on the posture result obtained after the posture is updated. Specifically, it is determined whether the distance or angle between two geometric elements is less than a preset threshold. If it is less than a threshold, it is considered that the geometric constraint is met and the iteration ends; otherwise, the constraint is not met and the iteration continues.

6. A pose solving system based on geometric constraints and degree of freedom limitations, characterized by: The method for solving the posture according to any one of claims 1 to 5 is applied, and the system includes: Input limitation module, posture transformation matrix solving module, degree of freedom limitation module, posture transformation matrix updating module and constraint verification module; The input constraint module is used to input the geometric constraints to be satisfied, the degrees of freedom to be limited, and the initial pose; The pose transformation matrix solving module is used to solve the pose according to the geometric constraints to be satisfied and obtain the pose transformation matrix that satisfies the geometric constraints; The degree of freedom limitation module is used to limit the degree of freedom of the obtained posture transformation matrix; The pose transformation matrix update module is used to update the current pose according to the pose transformation matrix obtained after the degree of freedom is limited; The constraint verification module performs geometric constraint verification on the pose results obtained after the pose is updated.

7. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, it implements the posture solving method based on geometric constraints and degree of freedom limitations as described in any one of claims 1 to 5.

8. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the posture solving method based on geometric constraints and degree of freedom limitations described in any one of claims 1 to 5 is implemented.

Images