CN115952672A - Three-dimensional pose estimation method for complex workpiece assembly - Google Patents
Three-dimensional pose estimation method for complex workpiece assembly Download PDFInfo
- Publication number
- CN115952672A CN115952672A CN202211736662.6A CN202211736662A CN115952672A CN 115952672 A CN115952672 A CN 115952672A CN 202211736662 A CN202211736662 A CN 202211736662A CN 115952672 A CN115952672 A CN 115952672A
- Authority
- CN
- China
- Prior art keywords
- assembly
- workpieces
- cloud data
- point cloud
- workpiece
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/30—Computing systems specially adapted for manufacturing
Landscapes
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Abstract
The invention discloses a three-dimensional pose estimation method for complex workpiece assembly, which comprises the steps of firstly respectively obtaining scanning point cloud data of an assembly workpiece A and an assembly workpiece B, respectively comparing the scanning point cloud data with respective standard model point cloud data, and determining the scanning point cloud data corresponding to an assembly surface of the assembly workpiece A and the assembly surface of the assembly workpiece B; then presetting initial poses of the assembly workpieces A and B in an assembly state, moving the initial poses to the assembly state, and constructing assembly error functions of the assembly workpieces A and B; optimizing an assembly error function by a Dog-Leg method to obtain the optimized poses of the assembly workpieces A and B; and finally, respectively updating the scanning point cloud data of the assembly workpieces A and B according to the optimized pose, and recalculating the distance from each point in the updated assembly workpiece A to the nearest point on the assembly workpiece B, thereby obtaining the assembly estimation results of the assembly workpieces A and B. The method can provide data support for the workpiece assembly process, and improve the assembly precision and efficiency of the workpiece.
Description
Technical Field
The invention mainly relates to the field of robot three-dimensional target measurement and detection methods, in particular to a three-dimensional pose estimation method for complex workpiece assembly.
Background
The intelligent manufacturing technology is that high-precision and high-strength repeated operation is realized through intelligent sensing, man-machine interaction, decision making and execution technology of the robot, has wide application prospect in high-end precision manufacturing industries such as aerospace, ocean engineering, rail transit, new energy and the like, and can complete operation tasks such as complex workpiece identification, positioning and grabbing, scanning measurement, welding and polishing, drilling and riveting and the like.
The complex workpiece refers to a non-standardized complex structure workpiece and is widely applied to high-end precision manufacturing industries such as aerospace, ocean engineering, rail transit and the like. Due to the reasons of processing technology, material strength, process limitation and the like, the precision of the complex workpiece multi-dimensional combined assembly is closely related to the precision of single parts and assembly conditions among the parts. Because the error between the assembly face can be enlargied to spare part assembling process, the assembly face error can influence the whole precision of assembly finished product to a certain extent between the spare part, and this kind of assembly face error is usually less moreover, can't effectively detect this kind of error to the three-dimensional measurement of single assembly spare part. Therefore, a trial assembly-repair assembly mode is usually adopted in the assembly process, and three-dimensional measurement can be performed only after the assembly process is completed, and because the assembly process is time-consuming and labor-consuming, if the dimensional accuracy of the completed combined assembly part does not meet the requirement, the assembled assembly part can be disassembled and reassembled, and the assembly and manufacturing efficiency is seriously influenced. In addition, the description of the assembly error in the existing method is based on the Euclidean distance difference, and the optimization space of the assembly error function has a large number of local optimal values due to the Euclidean distance difference and the physical constraint of the assembly surface, so that the final calculation result is influenced.
Disclosure of Invention
The invention provides a three-dimensional pose estimation method for complex workpiece assembly, which fully considers assembly requirements and physical space constraints, constructs an assembly error function and optimizes and solves the assembly error function through merging and splicing the assembly surfaces of workpieces, estimates the assembly pose of the assembled workpiece and obtains integral workpiece assembly precision data.
In order to solve the technical problem, the invention provides a three-dimensional pose estimation method for complex workpiece assembly, which comprises the following steps:
s1, respectively acquiring scanning point cloud data of assembly workpieces A and B through a three-dimensional scanner, respectively comparing the scanning point cloud data of the assembly workpieces A and B with respective standard model point cloud data, and determining the scanning point cloud data corresponding to assembly surfaces of the assembly workpieces A and B;
s2, presetting initial poses of the assembly workpieces A and B in an assembly state, moving the assembly workpieces A and B to the initial poses, and constructing assembly error functions of the assembly workpieces A and B;
s3, optimizing an assembly error function by a Dog-Leg method to obtain the optimized poses of the assembly workpieces A and B;
and S4, respectively updating the scanning point cloud data of the assembly workpieces A and B according to the optimized poses to obtain the updated scanning point cloud data of the assembly workpieces A and B, and recalculating the distance from each point in the updated assembly workpieces A to the nearest point on the assembly workpieces B to obtain the assembly estimation results of the assembly workpieces A and B.
Preferably, in S1, the scanning point cloud data of the assembly workpieces a and B are respectively compared with the respective standard model point cloud data, and the scanning point cloud data corresponding to the assembly surfaces of the assembly workpieces a and B are determined, which specifically includes:
s11, respectively marking model point cloud data corresponding to the assembly surfaces on the standard models of the assembly workpieces A and B;
s12, respectively registering standard model point cloud data of the assembly workpieces A and B with respective scanning point cloud data by adopting an ICP (inductively coupled plasma) registration method according to corresponding model point cloud data on the assembly surface;
s13, establishing a Kd-Tree, presetting a distance threshold, and respectively separating scanning point cloud data which do not exceed the preset distance threshold from the scanning point cloud data of the assembly workpieces A and B to serve as corresponding scanning point cloud data on respective assembly surfaces.
Preferably, the constructing of the assembly error function of the assembly workpieces a and B in S2 specifically includes:
s21, calculating the contact area of the assembly workpieces A and B in the initial pose;
s22, calculating the overlapping volume of the assembled workpieces A and B in the initial pose;
and S23, constructing an assembly error function of the assembly workpieces A and B through the contact area and the overlapping volume.
Preferably, the assembly error function in S23 is specifically formulated as:
wherein F is assembly error, mu and beta are proportional parameters, R is a rotation matrix of the assembly workpieces A to B, t is a translation matrix of the assembly workpieces A to B, N is the total number of scanning point cloud data corresponding to the assembly surface of the assembly workpiece A, and a i The contact area between the ith point in the scanning point cloud data corresponding to the assembly surface for assembling the workpiece A and the nearest point in the scanning point cloud data corresponding to the assembly surface for assembling the workpiece B, v i The overlapping volume of the ith point in the scanning point cloud data corresponding to the assembling surface of the assembling workpiece A and the nearest point in the scanning point cloud data corresponding to the assembling surface of the assembling workpiece B is obtained.
Preferably, S3 specifically includes:
s31, constructing a quadratic model function, and presetting a first iteration termination threshold epsilon 1 And a second iteration termination threshold ε 2 Initializing confidence field halfDiameter delta 0 Pose phi of assembly work pieces A and B 0 Similarity rho of the fitting error function and the quadratic model function 0 And DL optimization step delta 0 ；
S32, calculating an optimization step length parameter g of the kth iteration k And confidence domain radius Δ k ；
S33, optimizing step length parameter g of the kth iteration k And the first iteration end threshold epsilon 1 Or the confidence domain radius delta of the kth iteration k And a second iteration end threshold epsilon 2 For comparison, if g k ＜ε 1 Or Δ k ＜ε 2 ‖φ k-1 Iiperforming step S34, otherwise, performing step S35;
s34, finishing the DL optimization process, and outputting the poses phi of the workpieces A and B currently assembled k ；
S35, calculating DL optimization step length delta after k iteration k ；
S36, calculating the similarity rho between the assembly error function and the quadratic model function after the kth iteration k ；
And S37, updating the confidence domain radius, the poses of the assembled workpieces A and B, the similarity of the assembly error function and the quadratic model function and the parameters of the DL optimization step length, returning to the step S32, and performing the (k + 1) th iterative optimization.
Preferably, the quadratic model function in S31 is specifically:
wherein, is E k Assembly face error for the kth iteration, J k Jacobian matrix, δ, for the k-th iteration k The step size is optimized for the DL after the kth iteration.
Preferably, an optimization step size parameter g for the kth iteration is calculated in S32 k The concrete formula is as follows:
g k ＝J k T ∈ k
in the formula, g k For the optimization step-size parameter of the kth iteration, ∈ k Assembly face error for the kth iteration, J k A Jacobian matrix for the kth iteration;
computing confidence domain radius delta for the kth iteration k The concrete formula is as follows:
in the formula,. DELTA. k Radius of confidence domain, Δ, for the kth iteration max For the historical maximum confidence domain radius, p k-1 And (4) the similarity of the assembly error function and the quadratic model function after the (k-1) th iteration.
Preferably, the poses φ of the currently assembled workpieces A and B are output in S34 k The concrete formula is as follows:
in the formula, phi k Pose, phi, of the assembled workpieces A and B after the kth iteration k-1 Pose, delta, of assembled workpieces A and B after the k-1 iteration k-1 The step size is optimized for the DL after the k-1 iteration.
Preferably, S35 is specifically:
s351, calculating the step length delta of the Gauss-Newton method sd-k And steepest descent method step size delta gn-k ；
S352, dividing the step length delta of the Gauss Newton method sd-k Comparing with the radius of confidence domain, and determining the step length delta by Gaussian-Newton method sd-k Outside the radius of the confidence domain, the step length delta is obtained by the Gauss-Newton method sd-k Calculating DL optimization step δ k ；
S353, determining the step length delta of the Gaussian Newton method sd-k Within the radius of a confidence domain and the steepest descent method step length delta gn-k Also within the radius of the confidence domain, the step size delta is calculated by the steepest descent method gn-k Calculating DL optimization step δ k ；
S354, otherwise, the step length delta is obtained by the Gauss-Newton method sd-k And steepest descent method step size delta gn-k Computing DLOptimizing the step size delta k 。
Preferably, the similarity ρ of the assembly error function and the quadratic model function after the kth iteration is calculated in S36 k The concrete formula is as follows:
in the formula, ρ k For the similarity of the fitting error function and the quadratic model function after the kth iteration, phi k Pose, delta, of the assembled workpieces A and B after the kth iteration k The step size is optimized for the DL after the kth iteration.
Firstly, respectively acquiring scanning point cloud data of an assembly workpiece A and an assembly workpiece B, respectively comparing the scanning point cloud data with respective standard model point cloud data, and determining scanning point cloud data corresponding to an assembly surface of the assembly workpiece A and the assembly surface of the assembly workpiece B; then presetting initial poses of the assembly workpieces A and B in an assembly state, moving the assembly workpieces A and B to the initial poses, and constructing assembly error functions of the assembly workpieces A and B; optimizing an assembly error function by a Dog-Leg method to obtain the optimized poses of the assembly workpieces A and B; and finally, respectively updating the scanning point cloud data of the assembly workpieces A and B according to the optimized pose, and recalculating the distance from each point in the updated assembly workpiece A to the nearest point on the assembly workpiece B, thereby obtaining the assembly estimation results of the assembly workpieces A and B. In the assembling process of the complex parts, the method fully considers the assembling requirement and physical limitation, constructs an assembling error function based on the contact area and the overlapping volume by simulating the assembling process of the parts, adopts a nonlinear optimization algorithm to solve, estimates the assembling pose of each part after final assembly, obtains the error distribution condition of a final finished product, and judges the assembling quality of the finished product.
Drawings
FIG. 1 is a flow chart of a method for estimating a three-dimensional pose of a complex workpiece assembly according to an embodiment of the invention;
FIG. 2 is a flow chart of optimizing an assembly error function using a Dog-Leg method according to an embodiment of the present invention;
fig. 3 is a flow chart of calculating DL optimization step size in an embodiment of the present invention.
Detailed Description
In order to make the technical solutions of the present invention better understood, the present invention is further described in detail below with reference to the accompanying drawings.
A three-dimensional pose estimation method for complex workpiece assembly specifically comprises the following steps:
s1, respectively acquiring scanning point cloud data of assembly workpieces A and B through a three-dimensional scanner, respectively comparing the scanning point cloud data of the assembly workpieces A and B with respective standard model point cloud data, and determining the scanning point cloud data corresponding to assembly surfaces of the assembly workpieces A and B;
s2, presetting initial poses of the assembly workpieces A and B in an assembly state, moving the assembly workpieces A and B to the initial poses, and constructing an assembly error function of the assembly workpieces A and B;
s3, optimizing an assembly error function by a Dog-Leg method to obtain the optimized poses of the assembly workpieces A and B;
and S4, respectively updating the scanning point cloud data of the assembly workpieces A and B according to the optimized poses to obtain the updated scanning point cloud data of the assembly workpieces A and B, and recalculating the distance from each point in the updated assembly workpieces A to the nearest point on the assembly workpieces B to obtain the assembly estimation results of the assembly workpieces A and B.
Specifically, referring to fig. 1, fig. 1 is a flowchart of a three-dimensional pose estimation method for complex workpiece assembly according to an embodiment of the present invention.
Firstly, respectively acquiring scanning point cloud data of an assembly workpiece A and an assembly workpiece B through a three-dimensional scanner, respectively comparing the scanning point cloud data of the assembly workpiece A and the assembly workpiece B with respective standard model point cloud data, and determining the scanning point cloud data corresponding to an assembly surface of the assembly workpiece A and the assembly workpiece B; then presetting initial poses of the assembly workpieces A and B in an assembly state, moving the assembly workpieces A and B to the initial poses, in order to enable the assembly poses to be rapidly converged in the calculation process, enabling the initial poses of the workpiece assembly to be generally close to assembly surfaces of the two assembly workpieces A and B as much as possible, enabling the poses of the assembly workpieces A and B to be basically consistent with the actual assembly poses, and constructing assembly error functions of the assembly workpieces A and B; optimizing the assembly error function by a Dog-Leg method to obtain the optimized poses of the assembly workpieces A and B; and finally, respectively updating the scanning point cloud data of the assembly workpieces A and B according to the optimized poses to obtain the updated scanning point cloud data of the assembly workpieces A and B, and recalculating the distance from each point in the updated assembly workpiece A to the nearest point on the assembly workpiece B to obtain the assembly estimation results of the assembly workpieces A and B.
In one embodiment, in S1, the scanning point cloud data of the assembly workpieces a and B are respectively compared with the respective standard model point cloud data, and the scanning point cloud data corresponding to the assembly surfaces of the assembly workpieces a and B are determined, which specifically includes:
s11, respectively marking model point cloud data corresponding to the assembly surfaces on the standard models of the assembly workpieces A and B;
s12, respectively registering standard model point cloud data of the assembly workpieces A and B with respective scanning point cloud data by adopting an ICP (inductively coupled plasma) registration method according to corresponding model point cloud data on the assembly surface;
s13, establishing a Kd-Tree, presetting a distance threshold, and respectively separating scanning point cloud data which do not exceed the preset distance threshold from the scanning point cloud data of the assembly workpieces A and B to serve as corresponding scanning point cloud data on respective assembly surfaces.
Specifically, respectively marking model point cloud data corresponding to an assembly surface on respective standard models of an assembly workpiece A and an assembly workpiece B; and then respectively registering standard model point cloud data and scanning point cloud data of the assembly workpieces A and B by adopting an ICP (inductively coupled plasma) registration method, establishing Kd-Tree and presetting a distance threshold, and respectively separating the scanning point cloud data which does not exceed the distance threshold from the scanning point cloud data of the assembly workpieces A and B as the corresponding scanning point cloud data on the respective assembly surfaces. Through separation, the scanning point cloud data of a non-assembly area on the assembly part can be removed, the data processing amount is reduced, and in addition, the distance threshold value can be properly increased to avoid missing the scanning point cloud data belonging to the assembly surface.
In one embodiment, the constructing of the assembly error function of the assembly workpieces a and B in S2 specifically includes:
s21, calculating the contact area of the assembled workpieces A and B in the initial pose;
s22, calculating the overlapping volume of the assembly workpieces A and B in the initial pose;
and S23, constructing an assembly error function of the assembly workpieces A and B through the contact area and the overlapping volume.
In one embodiment, the assembly error function in S23 is specifically formulated as:
wherein F is assembly error, mu and beta are proportional parameters, R is a rotation matrix of the assembly workpieces A to B, t is a translation matrix of the assembly workpieces A to B, N is the total number of scanning point cloud data corresponding to the assembly surface of the assembly workpiece A, and a i The contact area v of the ith point in the scanning point cloud data corresponding to the assembly surface for assembling the workpiece A and the nearest point in the scanning point cloud data corresponding to the assembly surface for assembling the workpiece B i The overlapping volume of the ith point in the scanning point cloud data corresponding to the assembling surface of the assembling workpiece A and the nearest point in the scanning point cloud data corresponding to the assembling surface of the assembling workpiece B is obtained.
Specifically, the fitting error function F (R, t) is constructed using the contact area and the overlapping volume of two fitting workpieces at the time of fitting, as follows:
1) Calculating the contact area of the assembly workpieces A and B in the initial assembly pose:
respectively acquiring scanning point cloud data of assembly workpieces A and B through a three-dimensional scanner, respectively comparing the scanning point cloud data of the assembly workpieces A and B with respective standard model point cloud data, and determining scanning point cloud data X = { X } corresponding to assembly surfaces of the assembly workpieces A and B 1 ,x 2 ,...,x N } and Y = -y 1 ,y 2 ,...,y M }；
Calculating the ith data point x in the scanning point cloud data corresponding to the assembly surface of the assembly workpiece A i (i =1, 2.. An, N) to the closest point y in the scanning point cloud data corresponding to the assembly surface of the assembly workpiece B j (j =1, 2.., M) distance:
d i ＝||Rx i +t-y j || 2
wherein R is a rotation matrix of the assembly workpieces A to B, and t is a translation matrix of the assembly workpieces A to B.
Calculating the point x in the scanning point cloud data corresponding to the assembly surface of the assembly workpiece A i (i =1, 2.. Ang., N) density of scan point cloud data within a neighborhood with r as a radius:
dens(i)＝s i /n i
wherein s is i ＝πr 2
Wherein dens (i) is the i-th data point x on the mounting surface of the mounting workpiece A i Density of scan point cloud data in neighborhood with r as radius, s i For mounting the ith data point x on the mounting surface of the workpiece A i N, r is the neighborhood radius, N i For the ith data point x on the mounting surface of the mounting part A i And taking r as the number of the scanning point cloud data in the neighborhood of the radius.
Calculating the contact area of the assembly workpieces A and B during assembly:
a i ＝dens(i)1(d i ＜τ)
in the formula, 1 (d) i < τ) is a binary function, τ is the fitting gap threshold, i.e. the surface at a distance less than τ is the contact surface, a i The contact area of two surfaces to be assembled for assembling the workpieces A and B.
The ith data point x on the mounting surface of the workpiece A to be mounted i Closest point y to the mounting surface of the mounting workpiece B j Distance d of i Mapping to binary output 0 or 1 to ensure that the fitting estimation result basically conforms to the actual fitting condition: when d is i < tau, 1 (d) i < τ) =1, when d i When t is greater than or equal to 1 (d) i ＜τ)＝0。
2) Calculating the overlapping volume of the assembly workpieces A and B in an assembly initial pose:
overlapping volumes v i The phenomenon that one object is embedded into another object in the data splicing and merging process is described, and the main reason is that an error function takes a data point on an assembly surface as a minimum calculation unit instead of the assembly surface. This allows the stitching combination of the three-dimensional scan data to be dependent only on the gradient direction of the error function. Overlapping volume v i Volume of area jointly enclosed by mounting surfaces for mounting workpieces A and B, overlap volume v in actual mounting result i Is 0.
Overlapping volumes v i Is the ith data point x on the assembly surface of the assembly workpiece A i The area occupied by each average point in the neighborhood of (a) is multiplied by the current data point x i With the closest point y on the mounting face of the mounting-workpiece B j The distance of (a) to (b),
is a data point x i The normal vector of (c):
wherein dens (i) = s i /n i
s i ＝πr 2
In the formula, v i For mounting the ith data point x on the mounting surface of the workpiece A i And the overlapping volume of the corresponding closest point on the fitting face of the fitting workpiece B at the time of fitting,
is a data point x i Is a data point x on the mounting surface of the mounting workpiece A i Density of scan point cloud data in neighborhood with r as radius, n i For mounting the ith data point x on the mounting surface of the workpiece A i The number of scan point cloud data in the neighborhood taking r as the radius->
For binary function, the ith data point x on the assembly surface of the assembly workpiece A i As an end point, a corresponding closest point y on the mounting face of the mounting workpiece B j Vector as starting point, and data point x i Location field normal vector>
The product of (d) is mapped to the binary output 0 or 1 to ensure that the fit estimate substantially matches the actual fit condition.
3) Constructing an assembly error function of the assembly workpieces A and B:
the optimization calculation of the assembly simulation is to calculate the contact area a of the assembly i Maximum, assembly overlap volume v i The pose at the minimum (close to 0) is taken as the final assembly pose of the assembly workpieces a and B. Taking into account the fitting contact area a i And assembling the overlapping volume v i The variation trend is opposite, and the contact area a of the assembly is formed by using a composite natural function exp (x) + exp (-x) i And an overlapping volume v i Merging:
wherein F is an assembly error, mu and beta are proportional parameters, R is an assembly rotation matrix, and t is an assembly translation matrix.
In one embodiment, S3 specifically includes:
s31, constructing a quadratic model function, and presetting a first iteration termination threshold epsilon 1 And a second iteration termination threshold ε 2 Initialization confidence field radius Δ 0 Pose phi of the assembled workpieces A and B 0 Similarity rho of the fitting error function and the quadratic model function 0 And DL optimization step delta 0 ；
S32, calculating an optimization step length parameter g of the kth iteration k And confidence domain radius Δ k ；
S33, optimizing step length parameter g of the kth iteration k And the first iteration end threshold epsilon 1 Or the confidence domain radius delta of the kth iteration k And a second iteration end threshold epsilon 2 For comparison, if g k ＜ε 1 Or Δ k ＜ε 2 ‖φ k-1 Iiperforming step S34, otherwise, performing step S35;
s34, finishing the DL optimization process, and outputting the poses phi of the workpieces A and B currently assembled k ；
S35, calculating DL optimization step length delta after k iteration k ；
S36, calculating the similarity rho between the assembly error function and the quadratic model function after the kth iteration k ；
And S37, updating the confidence domain radius, the poses of the assembled workpieces A and B, the similarity of the assembly error function and the quadratic model function and the parameters of the DL optimization step length, returning to the step S32, and performing the (k + 1) th iterative optimization.
In one embodiment, the quadratic model function in S31 is specifically:
wherein is e k Assembly plane error for the kth iteration, J k Is the Jacobian matrix of the k iteration, δ k Step size is optimized for the DL after the kth iteration.
In one embodiment, the optimization step size parameter g for the k-th iteration is calculated in S32 k The concrete formula is as follows:
in the formula, g k For the optimization step-size parameter of the kth iteration, ∈ k Assembly face error for the kth iteration, J k Is a Jacobian matrix;
computing confidence domain radius delta for the kth iteration k The concrete formula is as follows:
in the formula,. DELTA. k Radius of confidence domain, Δ, for the kth iteration max For the historical maximum confidence domain radius, p k-1 And (4) the similarity of the assembly error function and the quadratic model function after the (k-1) th iteration.
In one embodiment, the pose φ of the currently assembled workpieces A and B is output in S34 k The concrete formula is as follows:
in the formula, phi k Pose, phi, of the assembled workpieces A and B after the kth iteration k-1 Pose, delta, of the assembled workpieces A and B after the k-1 iteration k-1 The step size is optimized for the DL after the k-1 iteration.
Specifically, the assembly error function is optimized through a Dog-Leg method, the Dog-Leg method combines a Gauss-Newton optimization method and a steepest descent method, and the iterative process of the assembly error function is accurately controlled through a confidence domain method. In the confidence domain method, the information of the assembly error function is reconstructed into a quadratic model function, the situation of the quadratic model function in a certain data point neighborhood is basically the same as that of the assembly error function, and the iterative optimization problem of the assembly error function F (R, t) is converted into the minimization problem of the quadratic model function L (delta) by replacing the rotation matrix R and the translation matrix t of the assembly error function F (R, t) with the poses phi of the assembly workpieces A and B.
The quadratic model function is specifically:
wherein is e k Assembly face error for the kth iteration, J k Jacobian matrix, δ, for the k-th iteration k The step size is optimized for the DL after the kth iteration.
The above quadratic model function can accurately represent the assembly error function in the neighborhood of radius Δ, so the iterative optimization problem of the assembly error function can be converted into the minimization problem of the quadratic model function in the radius Δ of the confidence domain:
min δ L(δ),subject to‖δ‖≤Δ
the confidence domain radius Δ is crucial to the iterative computation: if the region of the confidence domain is too large, the quadratic model function may differ too much from the assembly error function, resulting in a minimum value of the quadratic model function that may be far from the minimum value of the assembly error function in the region; if the region of the confidence domain is too small, the calculated candidate optimization step δ (i.e., DL optimization step, also called iterative optimization vector) may not be sufficient to bring the current optimization point close to the minimum point of the assembly error function. In practice, the choice of the confidence domain radius is based on the degree to which the quadratic model function approximates the assembly error function in the previous iteration (degree of similarity): if the quadratic model function is reliable, namely the variation trend of the assembly error function is accurately predicted, the radius delta of the confidence domain is increased, and a longer optimization step length is tried; if the quadratic model function cannot predict the assembly error function over the current confidence domain, the confidence domain radius Δ is reduced and solved again over the smaller confidence domain.
Referring to fig. 2, fig. 2 is a flowchart illustrating an optimization of the assembly error function by using a Dog-Leg method according to an embodiment of the present invention.
And optimizing the assembly error function by adopting a DL (Dog-Leg) method, wherein the specific optimization process is as follows:
1) Replacing the rotation matrix R and the translation matrix t in the assembly error function with the poses phi of the assembly workpieces A and B, constructing a quadratic model function L (delta), and presetting a first iteration termination threshold epsilon 1 And a second iteration termination threshold ε 2 Initializing relevant parameters, including: confidence domain radius Δ 0 Pose phi of assembly work pieces A and B 0 Similarity rho of the fitting error function and the quadratic model function 0 And DL optimization step delta 0 ；
2) Calculating an optimized step size parameter g for the kth iteration k The concrete formula is as follows:
in the formula, J k Jacobian matrix for the kth iteration, ∈ k Assembly plane error for the kth iteration, i.e. all points x on the assembly plane of the assembly workpiece a i To a point y on the mounting face of the mounting workpiece B j The sum of the distances of (a).
Computing confidence domain radius delta for the kth iteration k The concrete formula is as follows:
setting a similarity threshold range according to historical experience, and judging: after the k-1 iteration, assembling the similarity rho of the error function and the quadratic model function k-1 When the radius is more than or equal to 0.75, when the kth iterative computation is carried out, the confidence domain radius is the minimum value of the historical maximum confidence domain radius and 2 times of the current confidence domain radius; after the k-1 iteration, the similarity rho k-1 When the radius is less than 0.25, the radius of the confidence domain is 0.25 times of the radius of the current confidence domain when the kth iterative computation is carried out; after the k-1 iteration, the similarity rho k-1 And under other conditions outside the two value ranges, when the kth iterative computation is carried out, the confidence domain radius is still the current confidence domain radius.
3) Optimizing step length parameter g of the kth iteration k With a first iteration end threshold epsilon 1 Or the confidence domain radius delta of the kth iteration k And a second iteration end threshold epsilon 2 For comparison, if g k ＜ε 1 Or Δ k ＜ε 2 ‖φ k-1 Iid, performing step 4), otherwise performing step 5);
4) Ending the DL optimization process, and outputting the current poses phi of the assembly workpieces A and B k The concrete formula is as follows:
5) Calculating DL optimization step length delta after k iteration k I.e. the kth iterative optimization vector;
6) Calculating the similarity rho between the assembly error function F and the quadratic model function L after the kth iteration k ；
7) According to the similarity rho k Calculating the confidence domain radius of the (k + 1) th time, and optimizing the step length delta according to DL k And pose phi k And (5) calculating the poses of the assembled workpieces A and B at the (k + 1) th time, returning to the step 2), and performing iterative calculation at the (k + 1) th time.
In one embodiment, S35 specifically is:
s351, calculating the step length delta of the Gauss-Newton method sd-k And steepest descent method step size delta gn-k ；
S352, dividing the step length delta of the Gauss Newton method sd-k Comparing with the radius of confidence domain, and determining the step length delta by Gaussian-Newton method sd-k Outside the radius of the confidence domain, the step length delta is obtained by the Gauss-Newton method sd-k Calculating DL optimization step δ k ；
S353, determining the step length delta of the Gaussian Newton method sd-k Within the radius of a confidence domain and the steepest descent method step length delta gn-k Also within the radius of the confidence domain, the step size delta is determined by the steepest descent method gn-k Calculating DL optimization step δ k ；
S354, otherwise, the step length delta is obtained through the Gauss-Newton method sd-k And steepest descent method step size delta gn-k Calculating DL optimization step δ k 。
In particular, the Gauss-Newton method step size delta of the kth iteration sd-k The calculation formula of (c) is:
in the formula, delta sd-k Gauss-Newton's step size, g, for the kth iteration k For the optimization step-size parameter of the kth iteration, ∈ k Is the assembly face error for the kth iteration.
Steepest descent method step length delta of kth iteration gn-k Can be measured by the following meterThe calculation formula is used for calculating:
setting a proportion parameter alpha, calculating a Cauchy Point (Cauchy Point), wherein the Cauchy Point is the minimum value in the direction of the fastest attenuation of the current optimized quantity, and the Gaussian Newton method step length delta is calculated by the Cauchy Point sd And steepest descent method step size delta gn Synthesizing a DL optimization step length, wherein the DL optimization step length of the kth iteration is a monotone increasing function of a proportional parameter alpha:
current confidence domain radius of Δ k The DL optimization step size of the kth iteration of (a) is: step length delta of gauss-newton method sd-k Outside the radius of the confidence domain, the step length delta is obtained by the Gauss-Newton method sd-k Calculating DL optimization step δ k (ii) a Step length delta of Gauss-Newton method sd-k Within the radius of a confidence domain and the steepest descent method step length delta gn-k Also within the radius of the confidence domain, the step size delta is calculated by the steepest descent method gn-k Calculating DL optimization step δ k (ii) a Otherwise, the step size delta is determined by the Gauss-Newton method sd-k And steepest descent method step size delta gn-k Calculating DL optimization step δ k . The concrete formula is as follows:
in the formula, delta k Optimizing the step size, Δ, for the kth iteration DL k Radius of confidence domain, δ, for the kth iteration gn-k Steepest descent method step size, δ, of the kth iteration sd-k The step size of the k iteration gauss newton method.
In one embodiment, the similarity ρ of the fitting error function and the quadratic model function after the kth iteration is calculated in S36 k The concrete formula is as follows:
in the formula, ρ k Is the similarity of the assembly error function and the quadratic model function after the kth iteration, phi k Pose, delta, of the assembled workpieces A and B after the kth iteration k The step size is optimized for the DL after the kth iteration.
Specifically, according to the poses φ of the assembly workpieces A and B after the kth iteration k And DL optimization step delta k The similarity of the fitting error function F (R, t) and the quadratic model function L (δ) is calculated.
Firstly, respectively acquiring scanning point cloud data of an assembly workpiece A and an assembly workpiece B, respectively comparing the scanning point cloud data with respective standard model point cloud data, and determining scanning point cloud data corresponding to an assembly surface of the assembly workpiece A and the assembly surface of the assembly workpiece B; then presetting initial poses of the assembly workpieces A and B in an assembly state, moving the initial poses to the initial poses, and constructing assembly error functions of the assembly workpieces A and B; optimizing an assembly error function by a Dog-Leg method to obtain the optimized poses of the assembly workpieces A and B; and finally, respectively updating the scanning point cloud data of the assembly workpieces A and B according to the optimized pose, and recalculating the distance from each point in the updated assembly workpiece A to the nearest point on the assembly workpiece B, thereby obtaining the assembly estimation results of the assembly workpieces A and B. In the assembling process of the complex parts, the method fully considers the assembling requirement and physical limitation, constructs an assembling error function based on the contact area and the overlapping volume by simulating the assembling process of the parts, adopts a nonlinear optimization algorithm to solve, estimates the assembling pose of each part after final assembly, obtains the error distribution condition of a final finished product, and judges the assembling quality of the finished product.
The three-dimensional pose estimation method for assembling the complex workpiece provided by the invention is described in detail above. The principles and embodiments of the present invention are explained herein using specific examples, which are presented only to assist in understanding the core concepts of the present invention. It should be noted that, for those skilled in the art, it is possible to make various improvements and modifications to the present invention without departing from the principle of the present invention, and those improvements and modifications also fall within the scope of the claims of the present invention.
Claims (10)
1. A method for estimating the three-dimensional pose of an assembly of complex workpieces, the method comprising:
s1, respectively acquiring scanning point cloud data of assembly workpieces A and B through a three-dimensional scanner, respectively comparing the scanning point cloud data of the assembly workpieces A and B with respective standard model point cloud data, and determining the scanning point cloud data corresponding to assembly surfaces of the assembly workpieces A and B;
s2, presetting initial poses of the assembly workpieces A and B in an assembly state, moving the assembly workpieces A and B to the initial poses, and constructing assembly error functions of the assembly workpieces A and B;
s3, optimizing the assembly error function through a Dog-Leg method to obtain the optimized poses of the assembly workpieces A and B;
and S4, respectively updating the scanning point cloud data of the assembly workpieces A and B according to the optimized poses to obtain the updated scanning point cloud data of the assembly workpieces A and B, and recalculating the distance from each point in the updated assembly workpieces A to the nearest point on the assembly workpieces B to obtain the assembly estimation results of the assembly workpieces A and B.
2. The method according to claim 1, wherein the step S1 of comparing the scanning point cloud data of the assembled workpieces a and B with respective standard model point cloud data to determine the scanning point cloud data corresponding to the assembled surfaces of the assembled workpieces a and B comprises:
s11, respectively marking model point cloud data corresponding to the assembly surfaces on the standard models of the assembly workpieces A and B;
s12, respectively registering standard model point cloud data of the assembly workpieces A and B and respective scanning point cloud data by adopting an ICP (inductively coupled plasma) registration method according to the corresponding model point cloud data on the assembly surface;
s13, establishing a Kd-Tree, presetting a distance threshold, and respectively separating scanning point cloud data which do not exceed the preset distance threshold from the scanning point cloud data of the assembly workpieces A and B to serve as corresponding scanning point cloud data on respective assembly surfaces.
3. The three-dimensional pose estimation method for complex workpiece assembly according to claim 1, wherein the constructing of the assembly error function of the assembled workpieces a and B in S2 specifically comprises:
s21, calculating the contact area of the assembly workpieces A and B in the initial pose;
s22, calculating the overlapped volume of the assembly workpieces A and B in the initial pose;
and S23, constructing an assembly error function of the assembly workpieces A and B through the contact area and the overlapping volume.
4. The three-dimensional pose estimation method for complex workpiece assembly according to claim 3, wherein the assembly error function in S23 is specifically formulated as:
wherein F is assembly error, mu and beta are proportional parameters, R is a rotation matrix of the assembly workpieces A to B, t is a translation matrix of the assembly workpieces A to B, N is the total number of scanning point cloud data corresponding to the assembly surface of the assembly workpiece A, and a i The contact area v of the ith point in the scanning point cloud data corresponding to the assembly surface for assembling the workpiece A and the nearest point in the scanning point cloud data corresponding to the assembly surface for assembling the workpiece B i In the scanning point cloud data corresponding to the assembly surface for assembling the workpiece A, the ith point in the scanning point cloud data corresponding to the assembly surface for assembling the workpiece BOverlapping volumes of closest points.
5. The three-dimensional pose estimation method for complex workpiece assembly according to claim 4, wherein the S3 specifically comprises:
s31, constructing a quadratic model function, and presetting a first iteration termination threshold epsilon 1 And a second iteration termination threshold ε 2 Initialization confidence field radius Δ 0 Pose phi of the assembled workpieces A and B 0 Similarity ρ of fitting error function and quadratic model function 0 And DL optimization step delta 0 ；
S32, calculating an optimization step length parameter g of the kth iteration k And confidence domain radius Δ k ；
S33, optimizing a step length parameter g of the kth iteration k And the first iteration end threshold epsilon 1 Or the confidence domain radius delta of the kth iteration k And a second iteration end threshold epsilon 2 For comparison, if g k ＜ε 1 Or Δ k ＜ε 2 ‖φ k-1 Iiperforming step S34, otherwise, performing step S35;
s34, finishing the DL optimization process, and outputting the poses phi of the workpieces A and B currently assembled k ；
S35, calculating DL optimization step length delta after k iteration k ；
S36, calculating the similarity rho between the assembly error function and the quadratic model function after the kth iteration k ；
And S37, updating the confidence domain radius, the poses of the assembly workpieces A and B, the similarity of an assembly error function and a quadratic model function and the parameters of a DL optimization step length, returning to the step S32, and performing iterative optimization for the (k + 1) th time.
6. The three-dimensional pose estimation method for complex workpiece assembly according to claim 5, wherein the quadratic model function in S31 is specifically:
wherein is e k Assembly face error for the kth iteration, J k Jacobian matrix, δ, for the k-th iteration k The step size is optimized for the DL after the kth iteration.
7. The method for estimating the three-dimensional pose of assembly of complex workpieces according to claim 6, wherein the step size parameter g for optimization of the k-th iteration is calculated in S32 k The concrete formula is as follows:
in the formula, g k An optimized step size parameter for the kth iteration, ∈ k Assembly face error for the kth iteration, J k A Jacobian matrix for the kth iteration;
computing confidence domain radius delta for the kth iteration k The concrete formula is as follows:
in the formula,. DELTA. k Radius of confidence domain, Δ, for the kth iteration max For the historical maximum confidence domain radius, p k-1 And (4) the similarity of the assembly error function and the quadratic model function after the (k-1) th iteration.
8. The method for estimating three-dimensional pose for assembly of complex workpieces according to claim 7, wherein pose φ of currently-assembled workpieces A and B is output in S34 k The concrete formula is as follows:
in the formula, phi k Pose, phi, of the assembled workpieces A and B after the kth iteration k-1 For assembling after the k-1 iterationPose of pieces A and B, δ k-1 The step size is optimized for the DL after the k-1 iteration.
9. The three-dimensional pose estimation method for complex workpiece assembly according to claim 8, wherein S35 is specifically:
s351, calculating the step length delta of the Gauss-Newton method sd-k And steepest descent method step size delta gn-k ；
S352, dividing the step length delta of the Gauss Newton method sd-k Comparing with the radius of confidence domain, and determining the step length delta by Gaussian-Newton method sd-k Outside the radius of the confidence domain, the step length delta is obtained by the Gauss-Newton method sd-k Calculating DL optimization step δ k ；
S353, determining the step length delta of Gaussian Newton method sd-k Within the radius of a confidence domain and the steepest descent method step length delta gn-k Also within the radius of the confidence domain, the step size delta is determined by the steepest descent method gn-k Calculating DL optimization step δ k ；
S354, otherwise, the step length delta is obtained by the Gauss-Newton method sd-k And steepest descent method step size delta gn-k Calculating DL optimization step δ k 。
10. The method for estimating three-dimensional pose of assembly of complex workpieces according to claim 9, wherein the similarity p between the assembly error function after the kth iteration and the quadratic model function is calculated in S36 k The concrete formula is as follows:
in the formula, ρ k For the similarity of the fitting error function and the quadratic model function after the kth iteration, phi k Pose, δ, of assembled workpieces A and B after the kth iteration k The step size is optimized for the DL after the kth iteration.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211736662.6A CN115952672A (en) | 2022-12-31 | 2022-12-31 | Three-dimensional pose estimation method for complex workpiece assembly |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211736662.6A CN115952672A (en) | 2022-12-31 | 2022-12-31 | Three-dimensional pose estimation method for complex workpiece assembly |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115952672A true CN115952672A (en) | 2023-04-11 |
Family
ID=87297087
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211736662.6A Pending CN115952672A (en) | 2022-12-31 | 2022-12-31 | Three-dimensional pose estimation method for complex workpiece assembly |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115952672A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116697914A (en) * | 2023-08-04 | 2023-09-05 | 南京航空航天大学 | Real-time measurement method for assembly gap based on digital twinning |
-
2022
- 2022-12-31 CN CN202211736662.6A patent/CN115952672A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116697914A (en) * | 2023-08-04 | 2023-09-05 | 南京航空航天大学 | Real-time measurement method for assembly gap based on digital twinning |
| CN116697914B (en) * | 2023-08-04 | 2023-10-17 | 南京航空航天大学 | Real-time measurement method for assembly gap based on digital twinning |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Torr et al. | MLESAC: A new robust estimator with application to estimating image geometry | |
| CN110069041B (en) | Workpiece machining method and system based on-machine measurement | |
| CN115952672A (en) | Three-dimensional pose estimation method for complex workpiece assembly | |
| CN109147040B (en) | Template-based human point cloud hole repairing method | |
| CN113077553B (en) | Three-dimensional model segmentation method based on surface attribute | |
| CN113538486B (en) | Method for improving identification and positioning accuracy of automobile sheet metal workpiece | |
| Radvar-Esfahlan et al. | Nonrigid geometric metrology using generalized numerical inspection fixtures | |
| JP7134016B2 (en) | Information processing device, information processing method | |
| CN110222454B (en) | Process design method for milling precision consistency | |
| CN107272597B (en) | A kind of nurbs curve interpolation based on advance and retreat method quickly pre-reads processing method | |
| Chojnacki et al. | From FNS to HEIV: A link between two vision parameter estimation methods | |
| CN114488943B (en) | Random multi-area efficient polishing path planning method oriented to matched working conditions | |
| Chen et al. | An automatic registration approach to laser point sets based on multidiscriminant parameter extraction | |
| CN113313200B (en) | Point cloud precision matching method based on normal constraint | |
| CN118081767A (en) | Automatic programming system and method for post-processing machining of casting robot | |
| CN112802077A (en) | High-precision point cloud normal calculation method | |
| Phong et al. | Optimal estimation of object pose from a single perspective view | |
| Shin et al. | An efficient algorithm for measurement and comparison of large-scale hull pieces in the line-heating process | |
| CN113570147A (en) | Method and device for quickly predicting multi-working-condition machining error of thin-wall part | |
| Chen et al. | Point set registration for assembly feature pose estimation using simulated annealing nested Gauss-Newton optimization | |
| CN115147471A (en) | Laser point cloud automatic registration method based on curvature density characteristics | |
| CN111460974B (en) | Scattered point cloud data global feature extraction method based on optimization | |
| Xiong et al. | Local deformable template matching in robotic deburring | |
| Li et al. | An allowance optimal distribution method based on improved iterative closest point algorithm | |
| Winter et al. | NURBS-based shape and parameter optimization of structural components with an adaptive amount of control points |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |