A three-dimensional pose estimation method for complex workpiece assembly

Abstract

The application discloses a three-dimensional pose estimation method for complex workpiece assembly. Firstly, the scanning point cloud data of assembly workpieces A and B are obtained respectively, and the scanning point cloud data is compared with the standard model point cloud data respectively to determine the corresponding scanning point cloud data of the assembly surfaces of the assembly workpieces A and B. Then, the initial poses of the assembly workpieces A and B in the assembly state are preset, and the assembly workpieces A and B are moved to the initial assembly poses to construct the assembly error functions of the assembly workpieces A and B. Next, the Dog-Leg method is used to optimize the assembly error functions to obtain the optimized poses of the assembly workpieces A and B. Finally, the scanning point cloud data of the assembly workpieces A and B is updated according to the optimized poses, and the distance from each point in the updated assembly workpiece A to the nearest point on the assembly workpiece B is recalculated, so that the assembly estimation results of the assembly workpieces A and B are obtained. The method can provide data support for the workpiece assembly process and improve the workpiece assembly precision and efficiency.

Description

Technical Field

[0001] This invention mainly relates to the field of robot three-dimensional target measurement and detection methods, specifically to a three-dimensional pose estimation method for complex workpiece assembly. Background Technology

[0002] Intelligent manufacturing technology refers to the use of robots' intelligent sensing, human-computer interaction, decision-making, and execution technologies to achieve high-precision, high-intensity repetitive operations. It has broad application prospects in high-end precision manufacturing industries such as aerospace, marine engineering, rail transportation, and new energy, and can complete tasks such as complex workpiece identification, positioning and grasping, scanning and measurement, welding and grinding, drilling and riveting.

[0003] Complex workpieces refer to non-standardized, complex structural workpieces widely used in high-precision manufacturing industries such as aerospace, marine engineering, and rail transportation. Due to limitations in processing technology, material strength, and processes, the accuracy of complex workpieces, consisting of multi-dimensional assemblies, is related not only to the accuracy of individual components but also to the assembly conditions between them. The assembly process amplifies errors between assembly surfaces, which in turn affect the overall accuracy of the assembled product. These surface errors are typically small and cannot be effectively detected by 3D measurement of individual components. Therefore, a "trial assembly-repair" model is commonly used in assembly. This model only allows for 3D measurement after assembly is complete. Since assembly is often time-consuming and labor-intensive, disassembly and reassembly may be necessary if the dimensional accuracy of the assembled parts does not meet requirements, severely impacting assembly and manufacturing efficiency. Furthermore, existing methods describe assembly errors based on Euclidean distance differences. The physical constraints of Euclidean distance differences and assembly surfaces result in numerous local optima in the optimization space of the assembly error function, affecting the final calculation results. Summary of the Invention

[0004] The technical problem solved by this invention is to provide a three-dimensional pose estimation method for complex workpiece assembly. This method fully considers assembly requirements and physical space constraints. By merging and splicing the assembly surfaces of the workpiece, an assembly error function is constructed and optimized to estimate the assembly pose of the assembled workpiece and obtain the overall workpiece assembly accuracy data.

[0005] To address the aforementioned technical problems, this invention provides a three-dimensional pose estimation method for complex workpiece assembly, the method comprising:

[0006] S1. Obtain the scanned point cloud data of assembly workpieces A and B respectively using a 3D scanner. Compare the scanned point cloud data of assembly workpieces A and B with their respective standard model point cloud data to determine the scanned point cloud data corresponding to the assembly surfaces of assembly workpieces A and B.

[0007] S2. Preset the initial poses of assembly workpieces A and B in the assembly state, move assembly workpieces A and B to the initial poses, and construct the assembly error function of assembly workpieces A and B.

[0008] S3. Optimize the assembly error function using the Dog-Leg method to obtain the optimized poses of assembled workpieces A and B;

[0009] S4. Update the scanned point cloud data of assembly workpieces A and B according to the optimized pose, obtain the updated scanned point cloud data of assembly workpieces A and B, recalculate the distance from each point in the updated assembly workpiece A to the nearest point on assembly workpiece B, and thus obtain the assembly estimation results of assembly workpieces A and B.

[0010] Preferably, in S1, the scanned point cloud data of assembled workpieces A and B are compared with their respective standard model point cloud data to determine the scanned point cloud data corresponding to the assembly surfaces of assembled workpieces A and B, specifically including:

[0011] S11. Mark the corresponding model point cloud data on the assembly surface of each of the standard models of assembly workpieces A and B respectively.

[0012] S12. Based on the corresponding model point cloud data on the assembly surface, the standard model point cloud data of assembly workpieces A and B are registered with their respective scanned point cloud data using the ICP registration method.

[0013] S13. Establish a Kd-Tree and preset a distance threshold. Separate the scan point cloud data that does not exceed the preset distance threshold from the scan point cloud data of assembly workpieces A and B respectively, and use them as the corresponding scan point cloud data on their respective assembly surfaces.

[0014] Preferably, the assembly error function for assembling workpieces A and B is constructed in S2, specifically including:

[0015] S21. Calculate the contact area between assembled workpieces A and B in their initial positions.

[0016] S22. Calculate the overlap volume of assembled workpieces A and B in their initial poses.

[0017] S23. Construct assembly error functions for assembled workpieces A and B by using contact area and overlap volume.

[0018] Preferably, the assembly error function in S23 is specifically formulated as follows:

[0019]

[0020] In the formula, F is the assembly error, μ and β are proportional parameters, R is the rotation matrix from assembly workpiece A to B, t is the translation matrix from assembly workpiece A to B, N is the total number of scanned point cloud data corresponding to the assembly surface of assembly workpiece A, and a i Let v be the contact area between the i-th point in the scanned point cloud data corresponding to the assembly surface of workpiece A and the nearest point in the scanned point cloud data corresponding to the assembly surface of workpiece B. i The overlap volume between the i-th point in the scanned point cloud data corresponding to the assembly surface of workpiece A and the nearest point in the scanned point cloud data corresponding to the assembly surface of workpiece B.

[0021] Preferably, S3 specifically includes:

[0022] S31. Construct a quadratic model function, preset the first iteration termination threshold ε1 and the second iteration termination threshold ε2, initialize the confidence region radius Δ0, and assemble the poses φ of workpieces A and B. 0 The similarity ρ0 between the assembly error function and the quadratic model function and the DL optimization step size δ0;

[0023] S32. Calculate the optimal step size parameter g for the k-th iteration. k And confidence region radius Δ k ;

[0024] S33. Optimize the step size parameter g for the k-th iteration. k The first iteration termination threshold ε1 or the confidence region radius Δ of the k-th iteration. k Compare with the second iteration termination threshold ε2, if g k <ε1 or Δ k <ε2‖φ k-1 If the first step is not executed, proceed to step S34; otherwise, proceed to step S35.

[0025] S34. End the DL optimization process and output the poses φ of the currently assembled workpieces A and B. k ;

[0026] S35. Calculate the DL optimization step size δ after the k-th iteration. k ;

[0027] S36. Calculate the similarity ρ between the assembly error function and the quadratic model function after the k-th iteration. k ;

[0028] S37. Update the parameters of the confidence region radius, the poses of assembled workpieces A and B, the similarity between the assembly error function and the quadratic model function, and the DL optimization step size. Return to step S32 and perform the (k+1)th iteration optimization.

[0029] Preferably, the quadratic model function in S31 is specifically:

[0030]

[0031] In the formula, ∈ k Let J be the assembly surface error in the k-th iteration. k Let δ be the Jacobian matrix of the k-th iteration. k This is the step size for DL ​​optimization after the k-th iteration.

[0032] Preferably, in S32, the optimization step size parameter g for the k-th iteration is calculated. k The specific formula is as follows:

[0033] g k =J k T ∈ k

[0034] In the formula, g k Let be the optimization step size parameter for the k-th iteration, ∈ k Let J be the assembly surface error in the k-th iteration. k Let be the Jacobian matrix for the k-th iteration;

[0035] Calculate the confidence region radius Δ for the k-th iteration. k The specific formula is as follows:

[0036]

[0037] In the formula, Δ k Let Δ be the radius of the confidence region in the k-th iteration. max ρ is the radius of the historical maximum confidence region. k-1 Let be the similarity between the assembly error function and the quadratic model function after the (k-1)th iteration.

[0038] Preferably, in S34, the pose φ of the currently assembled workpieces A and B is output. k The specific formula is as follows:

[0039]

[0040] In the formula, φ k Let φ be the pose of assembled workpieces A and B after the k-th iteration. k-1 Let δ be the pose of assembled workpieces A and B after the (k-1)th iteration. k-1 This is the step size for DL ​​optimization after the (k-1)th iteration.

[0041] Preferably, S35 specifically comprises:

[0042] S351. Calculate the step size δ using the Gauss-Newton method. sd-k and steepest descent step size δ gn-k ;

[0043] S352, Calculate the step size δ of the Gauss-Newton method.sd-k Compared with the confidence region radius, if the Gauss-Newton method step size δ sd-k If it is outside the confidence region radius, then the step size δ is determined using the Gauss-Newton method. sd-k Calculate the DL optimization step size δ k ;

[0044] S353, if the step size δ of Gauss-Newton's method sd-k Located within the confidence region radius, and with the steepest descent step size δ gn-k If it is also within the confidence region radius, then the steepest descent method with a step size δ is used. gn-k Calculate the DL optimization step size δ k ;

[0045] S354. In other cases, the step size δ is determined using the Gauss-Newton method. sd-k and steepest descent step size δ gn-k Calculate the DL optimization step size δ k .

[0046] Preferably, in S36, the similarity ρ between the assembly error function and the quadratic model function after the k-th iteration is calculated. k The specific formula is as follows:

[0047]

[0048] In the formula, ρ k Let φ be the similarity between the assembly error function and the quadratic model function after the k-th iteration. k Let δ be the pose of assembled workpieces A and B after the k-th iteration. k This is the step size for DL ​​optimization after the k-th iteration.

[0049] The aforementioned three-dimensional pose estimation method for complex workpiece assembly first acquires scanned point cloud data of workpieces A and B respectively, and compares the scanned point cloud data with their respective standard model point cloud data to determine the scanned point cloud data corresponding to the assembly surfaces of workpieces A and B. Then, it presets the initial poses of workpieces A and B in the assembly state and moves them to the initial poses, constructing assembly error functions for workpieces A and B. Next, it optimizes the assembly error functions using the Dog-Leg method to obtain the optimized poses of workpieces A and B. Finally, it updates the scanned point cloud data of workpieces A and B according to the optimized poses and recalculates the distance from each point in workpiece A to the nearest point on workpiece B, thereby obtaining the assembly estimation results for workpieces A and B. In the assembly of complex components, this method fully considers the assembly requirements and physical constraints. By simulating the assembly process of parts, it constructs an assembly error function based on contact area and overlap volume, and uses a nonlinear optimization algorithm to solve it. It estimates the assembly pose of each part after final assembly, obtains the error distribution of the final product, and judges the assembly quality of the finished product. This method can reduce the probability of assembly failure, simplify the original "trial assembly-repair" assembly mode, and greatly improve the workpiece assembly accuracy and assembly efficiency. Attached Figure Description

[0050] Figure 1 This is a flowchart of a three-dimensional pose estimation method for complex workpiece assembly in one embodiment of the present invention;

[0051] Figure 2 This is a flowchart illustrating the optimization of the assembly error function using the Dog-Leg method in one embodiment of the present invention;

[0052] Figure 3 This is a flowchart of calculating the DL optimization step size in one embodiment of the present invention. Detailed Implementation

[0053] To enable those skilled in the art to better understand the technical solution of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings.

[0054] A three-dimensional pose estimation method for complex workpiece assembly, specifically including:

[0055] S1. Obtain the scanned point cloud data of assembly workpieces A and B respectively using a 3D scanner. Compare the scanned point cloud data of assembly workpieces A and B with their respective standard model point cloud data to determine the scanned point cloud data corresponding to the assembly surfaces of assembly workpieces A and B.

[0056] S2. Preset the initial poses of assembly workpieces A and B in the assembly state, move assembly workpieces A and B to the initial poses, and construct the assembly error function of assembly workpieces A and B.

[0057] S3. Optimize the assembly error function using the Dog-Leg method to obtain the optimized poses of assembled workpieces A and B;

[0058] S4. Update the scanned point cloud data of assembly workpieces A and B according to the optimized pose, obtain the updated scanned point cloud data of assembly workpieces A and B, recalculate the distance from each point in the updated assembly workpiece A to the nearest point on assembly workpiece B, and thus obtain the assembly estimation results of assembly workpieces A and B.

[0059] Specifically, see Figure 1 , Figure 1 This is a flowchart of a three-dimensional pose estimation method for complex workpiece assembly in one embodiment of the present invention.

[0060] A 3D pose estimation method for complex workpiece assembly is proposed. First, scanned point cloud data of workpieces A and B are acquired using a 3D scanner. These scanned point cloud data are then compared with their respective standard model point cloud data to determine the scanned point cloud data corresponding to the assembly surfaces of workpieces A and B. Next, the initial poses of workpieces A and B in their assembled state are preset, and workpieces A and B are moved to these initial poses. To ensure rapid convergence of the assembly poses during calculation, the initial assembly poses are generally as close as possible to the assembly surfaces of workpieces A and B, and the workpiece poses are made substantially consistent with the actual assembly poses. An assembly error function for workpieces A and B is then constructed. Next, the assembly error function is optimized using the Dog-Leg method to obtain the optimized poses of workpieces A and B. Finally, the scanned point cloud data of workpieces A and B are updated based on the optimized poses to obtain the updated scanned point cloud data. The distance from each point in workpiece A to the nearest point on workpiece B is recalculated, thus yielding the assembly estimation results for workpieces A and B.

[0061] In one embodiment, step S1 compares the scanned point cloud data of assembled workpieces A and B with their respective standard model point cloud data to determine the scanned point cloud data corresponding to the assembly surfaces of assembled workpieces A and B, specifically including:

[0062] S11. Mark the corresponding model point cloud data on the assembly surface of each of the standard models of assembly workpieces A and B respectively.

[0063] S12. Based on the corresponding model point cloud data on the assembly surface, the standard model point cloud data of assembly workpieces A and B are registered with their respective scanned point cloud data using the ICP registration method.

[0064] S13. Establish a Kd-Tree and preset a distance threshold. Separate the scan point cloud data that does not exceed the preset distance threshold from the scan point cloud data of assembly workpieces A and B respectively, and use them as the corresponding scan point cloud data on their respective assembly surfaces.

[0065] Specifically, firstly, the corresponding model point cloud data on the assembly surface are marked on the standard models of assembly workpieces A and B, respectively. Then, the standard model point cloud data of assembly workpieces A and B are registered with the scanned point cloud data using the ICP registration method. Next, a Kd-Tree is established with a preset distance threshold, and scanned point cloud data not exceeding the distance threshold is separated from the scanned point cloud data of assembly workpieces A and B, respectively, as the corresponding scanned point cloud data on their respective assembly surfaces. Through separation, scanned point cloud data of non-assembly areas on the assembled parts can be removed, reducing the amount of data processing. In addition, the distance threshold can be appropriately increased to avoid missing scanned point cloud data belonging to the assembly surface.

[0066] In one embodiment, S2 constructs the assembly error function for assembling workpieces A and B, specifically including:

[0067] S21. Calculate the contact area between assembled workpieces A and B in their initial positions.

[0068] S22. Calculate the overlap volume of assembled workpieces A and B in their initial poses.

[0069] S23. Construct assembly error functions for assembled workpieces A and B by using contact area and overlap volume.

[0070] In one embodiment, the assembly error function in S23 is specifically formulated as follows:

[0071]

[0072] In the formula, F is the assembly error, μ and β are proportional parameters, R is the rotation matrix from assembly workpiece A to B, t is the translation matrix from assembly workpiece A to B, N is the total number of scanned point cloud data corresponding to the assembly surface of assembly workpiece A, and a i Let v be the contact area between the i-th point in the scanned point cloud data corresponding to the assembly surface of workpiece A and the nearest point in the scanned point cloud data corresponding to the assembly surface of workpiece B. i The overlap volume between the i-th point in the scanned point cloud data corresponding to the assembly surface of workpiece A and the nearest point in the scanned point cloud data corresponding to the assembly surface of workpiece B.

[0073] Specifically, the assembly error function F(R,t) is constructed using the contact area and overlap volume of the two assembled workpieces during assembly, as follows:

[0074] 1) Calculate the contact area between workpieces A and B in their initial assembly positions:

[0075] The point cloud data of assembly workpieces A and B are acquired using a 3D scanner. The point cloud data of assembly workpieces A and B are then compared with the point cloud data of their respective standard models to determine the point cloud data X = {x1, x2, ..., x...} corresponding to the assembly surfaces of assembly workpieces A and B. N} and Y = {y1, y2, ..., y M};

[0076] Calculate the i-th data point x in the scanned point cloud data corresponding to the assembly surface of assembly workpiece A. i (i = 1, 2, ..., N) is the nearest point y in the scanned point cloud data corresponding to the assembly surface of assembly workpiece B. j Distances to (j = 1, 2, ..., M):

[0077] d i =||Rx i +ty j || 2

[0078] In the formula, R is the rotation matrix from assembly workpiece A to B, and t is the translation matrix from assembly workpiece A to B.

[0079] Calculate point x in the scanned point cloud data corresponding to the assembly surface of assembly workpiece A. i Density of scanned point cloud data in a neighborhood with radius r (i = 1, 2, ..., N):

[0080] dens(i)=s i / n i

[0081] Among them, s i =πr 2

[0082] In the formula, dens(i) represents the i-th data point x on the assembly surface of the assembled workpiece A. i The density of scanned point cloud data within a neighborhood with radius r, s i Let x be the i-th data point on the assembly surface of workpiece A. i The neighborhood area, i = 1, 2, ..., N, r is the neighborhood radius, n i Let x be the i-th data point on the assembly surface of workpiece A. i The number of scanned point cloud data within a neighborhood with radius r.

[0083] Calculate the contact area between workpieces A and B during assembly:

[0084] a i =dens(i)1(d i <τ)

[0085] In the formula, 1(d i <τ) is a binary function, τ is the assembly gap threshold, that is, the surface with a distance less than τ is the contact surface, a i This refers to the contact area of ​​the two surfaces of workpieces A and B to be assembled.

[0086] The i-th data point x on the assembly surface of workpiece A. i The nearest point y on the assembly surface of workpiece B j distance d i Mapping to binary output 0 or 1 ensures that the assembly estimation result basically matches the actual assembly situation: that is, when d i When <τ, 1(d i <τ)=1, when d i When ≥τ, 1(d i <τ)=0.

[0087] 2) Calculate the overlap volume of the assembled workpieces A and B in their initial assembly pose:

[0088] Overlapping volume v i This describes the phenomenon of one object "embedding" another during data stitching and merging. The main reason is that the error function uses data points on the assembly surface as the smallest computational unit, rather than the assembly surface itself. This makes the stitching and merging of 3D scan data only related to the gradient direction of the error function. Overlap volume v i The volume of the area enclosed by the assembly surfaces of workpieces A and B, and the overlapping volume v in the actual assembly result. i It is 0.

[0089] Overlapping volume v i It is the i-th data point x on the assembly surface of workpiece A. i The average area occupied by each point in the neighborhood of the current data point x multiplied by the area occupied by the current data point x. i The nearest point y on the assembly surface of workpiece B j distance, For data point x i Normal vector:

[0090]

[0091] Where, dens(i) = s i / n i

[0092] s i =πr 2

[0093] In the formula, v i Let x be the i-th data point on the assembly surface of workpiece A. iThe overlapping volume of the nearest point corresponding to the assembly surface of workpiece B during assembly. For data point x i The normal vector, dens(i) is the data point x on the assembly surface of the assembled workpiece A. i The density of scanned point cloud data within a neighborhood with radius r, n i Let x be the i-th data point on the assembly surface of workpiece A. i The number of scanned point cloud data within a neighborhood with radius r. This is a binary function that stores the i-th data point x on the assembly surface of workpiece A. i The endpoint is the nearest point y on the assembly surface of workpiece B. j The vector starting from point x, and the vector connecting the data points x i Domain Normal Vector The product is mapped to a binary output of 0 or 1 to ensure that the assembly estimation result is basically consistent with the actual assembly situation.

[0094] 3) Construct the assembly error functions for the assembled workpieces A and B:

[0095] Assembly simulation optimization calculation involves determining the assembly contact area 'a'. i Maximum, assembly overlap volume v i The pose at its minimum (closest to 0) is taken as the final assembly pose for workpieces A and B. Considering the assembly contact area a... i and assembly overlap volume v i The trend is opposite; the contact area a during assembly is calculated using the composite natural function exp(x) + exp(-x). i and overlapping volume v i merge:

[0096]

[0097] In the formula, F is the assembly error, μ and β are proportional parameters, R is the assembly rotation matrix, and t is the assembly translation matrix.

[0098] In one embodiment, S3 specifically includes:

[0099] S31. Construct a quadratic model function, preset the first iteration termination threshold ε1 and the second iteration termination threshold ε2, initialize the confidence region radius Δ0, and assemble the poses φ of workpieces A and B. 0 The similarity ρ0 between the assembly error function and the quadratic model function and the DL optimization step size δ0;

[0100] S32. Calculate the optimal step size parameter g for the k-th iteration. k And confidence region radius Δ k ;

[0101] S33. Optimize the step size parameter g for the k-th iteration. k The first iteration termination threshold ε1 or the confidence region radius Δ of the k-th iteration. k Compare with the second iteration termination threshold ε2, if g k <ε1 or Δ k <ε2‖φ k-1 If the first step is not executed, proceed to step S34; otherwise, proceed to step S35.

[0102] S34. End the DL optimization process and output the poses φ of the currently assembled workpieces A and B. k ;

[0103] S35. Calculate the DL optimization step size δ after the k-th iteration. k ;

[0104] S36. Calculate the similarity ρ between the assembly error function and the quadratic model function after the k-th iteration. k ;

[0105] S37. Update the parameters of the confidence region radius, the poses of assembled workpieces A and B, the similarity between the assembly error function and the quadratic model function, and the DL optimization step size. Return to step S32 and perform the (k+1)th iteration optimization.

[0106] In one embodiment, the quadratic model function in S31 is specifically:

[0107]

[0108] In the formula, ∈ k Let J be the assembly surface error in the k-th iteration. k Let δ be the Jacobian matrix of the k-th iteration. k This is the step size for DL ​​optimization after the k-th iteration.

[0109] In one embodiment, the optimization step size parameter g for the k-th iteration is calculated in S32. k The specific formula is as follows:

[0110]

[0111] In the formula, g k Let be the optimization step size parameter for the k-th iteration, ∈ k Let J be the assembly surface error in the k-th iteration. k It is a Jacobian matrix;

[0112] Calculate the confidence region radius Δ for the k-th iteration. k The specific formula is as follows:

[0113]

[0114] In the formula, Δk Let Δ be the radius of the confidence region in the k-th iteration. max ρ is the radius of the historical maximum confidence region. k-1 Let be the similarity between the assembly error function and the quadratic model function after the (k-1)th iteration.

[0115] In one embodiment, S34 outputs the pose φ of the currently assembled workpieces A and B. k The specific formula is as follows:

[0116]

[0117] In the formula, φ k Let φ be the pose of assembled workpieces A and B after the k-th iteration. k-1 Let δ be the pose of assembled workpieces A and B after the (k-1)th iteration. k-1 This is the step size for DL ​​optimization after the (k-1)th iteration.

[0118] Specifically, the assembly error function is optimized using the Dog-Leg method, which combines Gauss-Newton optimization and steepest descent methods, and utilizes the confidence region method to precisely control the iterative process of the assembly error function. In the confidence region method, the information of the assembly error function is reconstructed into a quadratic model function. This quadratic model function is essentially the same as the assembly error function in the neighborhood of a certain data point. By replacing the rotation matrix R and translation matrix t of the assembly error function F(R,t) with the pose φ of the assembled workpieces A and B, the iterative optimization problem of the assembly error function F(R,t) is transformed into the minimization problem of the quadratic model function L(δ).

[0119] The quadratic model function is as follows:

[0120]

[0121] In the formula, ∈ k Let J be the assembly surface error in the k-th iteration. k Let δ be the Jacobian matrix of the k-th iteration. k This is the step size for DL ​​optimization after the k-th iteration.

[0122] The aforementioned quadratic model function can accurately represent the assembly error function within a neighborhood of radius Δ. Therefore, the iterative optimization problem of the assembly error function can be transformed into a minimization problem of the quadratic model function within the confidence region radius Δ.

[0123] min δ L(δ), subject to‖δ‖≤Δ

[0124] The confidence region radius Δ is crucial for iterative computation: if the confidence region is too large, the quadratic model function may differ significantly from the assembly error function, causing the minimum of the quadratic model function to be far from the minimum of the assembly error function in that region; if the confidence region is too small, the calculated candidate optimization step size δ (i.e., the DL optimization step size, also called the iterative optimization vector) may be insufficient to bring the current optimization point close to the minimum of the assembly error function. In practice, the choice of the confidence region radius is based on the degree of similarity (closeness) of the quadratic model function to the assembly error function in the previous iteration: if the quadratic model function is reliable, i.e., accurately predicts the changing trend of the assembly error function, the confidence region radius Δ is increased, and a longer optimization step size is attempted; if the quadratic model function cannot predict the assembly error function in the current confidence region, the confidence region radius Δ is decreased, and the solution is solved again in a smaller confidence region.

[0125] See Figure 2 , Figure 2 This is a flowchart illustrating the optimization of the assembly error function using the Dog-Leg method in one embodiment of the present invention.

[0126] The assembly error function is optimized using the DL (Dog-Leg) method. The specific optimization process is as follows:

[0127] 1) Replace the rotation matrix R and translation matrix t in the assembly error function with the poses φ of the assembled workpieces A and B, construct a quadratic model function L(δ), preset the first iteration termination threshold ε1 and the second iteration termination threshold ε2, and initialize relevant parameters, including: confidence region radius Δ0, and poses φ of the assembled workpieces A and B. 0 The similarity ρ0 between the assembly error function and the quadratic model function and the DL optimization step size δ0;

[0128] 2) Calculate the optimal step size parameter g for the k-th iteration. k The specific formula is as follows:

[0129]

[0130] In the formula, J k Let be the Jacobian matrix of the k-th iteration, ∈ k The assembly surface error in the kth iteration is the error at all points x on the assembly surface of workpiece A. i Point y on the assembly surface of workpiece B j The sum of distances.

[0131] Calculate the confidence region radius Δ for the k-th iteration. k The specific formula is as follows:

[0132]

[0133] Based on historical experience, a similarity threshold range is set, and a judgment is made: after the (k-1)th iteration, the similarity ρ between the assembly error function and the quadratic model function is... k-1 When the similarity is greater than or equal to 0.75, during the k-th iteration, the confidence region radius is the minimum of the historical maximum confidence region radius and twice the current confidence region radius; after the (k-1)-th iteration, the similarity ρ... k-1 When the similarity ρ is less than 0.25, the confidence region radius is taken as 0.25 times the current confidence region radius during the k-th iteration; after the (k-1)-th iteration, the similarity ρ... k-1 In cases outside the two ranges mentioned above, the confidence region radius remains the same as the current confidence region radius during the k-th iteration.

[0134] 3) The optimization step size parameter g of the kth iteration k The first iteration termination threshold ε1 or the confidence region radius Δ of the k-th iteration. k Compare with the second iteration termination threshold ε2, if g k <ε1 or Δ k <ε2‖φ k-1 If the condition is met, proceed to step 4); otherwise, proceed to step 5.

[0135] 4) End the DL optimization process and output the current pose φ of assembled workpieces A and B. k The specific formula is as follows:

[0136]

[0137] 5) Calculate the DL optimization step size δ after the k-th iteration. k , that is, the vector optimized in the kth iteration;

[0138] 6) Calculate the similarity ρ between the assembly error function F and the quadratic model function L after the k-th iteration. k ;

[0139] 7) Based on similarity ρ k Calculate the confidence region radius for the (k+1)th iteration, and optimize the step size δ using deep learning. k and pose φ k Calculate the poses of workpieces A and B in the (k+1)th assembly step, return to step 2), and perform the (k+1)th iteration calculation.

[0140] In one embodiment, S35 specifically refers to:

[0141] S351. Calculate the step size δ using the Gauss-Newton method. sd-k and steepest descent step size δ gn-k ;

[0142] S352, Calculate the step size δ of the Gauss-Newton method. sd-kCompared with the confidence region radius, if the Gauss-Newton method step size δ sd-k If it is outside the confidence region radius, then the step size δ is determined using the Gauss-Newton method. sd-k Calculate the DL optimization step size δ k ;

[0143] S353, if the step size δ of Gauss-Newton's method sd-k Located within the confidence region radius, and with the steepest descent step size δ gn-k If it is also within the confidence region radius, then the steepest descent method with a step size δ is used. gn-k Calculate the DL optimization step size δ k ;

[0144] S354. In other cases, the step size δ is determined using the Gauss-Newton method. sd-k and steepest descent step size δ gn-k Calculate the DL optimization step size δ k .

[0145] Specifically, the step size δ of the Gauss-Newton method in the k-th iteration sd-k The calculation formula is:

[0146]

[0147] In the formula, δ sd-k Let g be the step size of the Gauss-Newton method in the k-th iteration. k Let be the optimization step size parameter for the k-th iteration, ∈ k Let be the assembly surface error in the k-th iteration.

[0148] The steepest descent step size δ in the kth iteration gn-k The calculation can be performed using the following formula:

[0149]

[0150] Set the scaling parameter α, calculate the Cauchy point, which is the minimum value along the direction of the fastest decay of the current optimization quantity. Use the Cauchy point to adjust the Gauss-Newton method step size δ. sd and steepest descent step size δ gn The synthetic DL optimization step size is a monotonically increasing function of the scaling parameter α in the k-th iteration.

[0151]

[0152] The current confidence region radius is Δ k The step size for the k-th iteration of the DL optimization is: if the step size of the Gauss-Newton method is δ sd-k If it is outside the confidence region radius, then the step size δ is determined using the Gauss-Newton method. sd-k Calculate the DL optimization step size δ kIf the step size of Gauss-Newton's method is δ sd-k Located within the confidence region radius, and with the steepest descent step size δ gn-k If it is also within the confidence region radius, then the steepest descent method with a step size δ is used. gn-k Calculate the DL optimization step size δ k In other cases, the step size δ is determined using the Gauss-Newton method. sd-k and steepest descent step size δ gn-k Calculate the DL optimization step size δ k The specific formula is as follows:

[0153]

[0154] In the formula, δ k For the k-th iteration of DL optimization step size, Δ k Let δ be the radius of the confidence region in the k-th iteration. gn-k The step size of the steepest descent method in the kth iteration, δ sd-k Let be the step size of the Gauss-Newton method in the k-th iteration.

[0155] In one embodiment, in S36, the similarity ρ between the assembly error function and the quadratic model function after the k-th iteration is calculated. k The specific formula is as follows:

[0156]

[0157] In the formula, ρ k Let φ be the similarity between the assembly error function and the quadratic model function after the k-th iteration. k Let δ be the pose of assembled workpieces A and B after the k-th iteration. k This is the step size for DL ​​optimization after the k-th iteration.

[0158] Specifically, based on the pose φ of assembled workpieces A and B after the kth iteration... k and DL optimization step size δ k To calculate the similarity between the assembly error function F(R,t) and the quadratic model function L(δ).

[0159] The aforementioned method for 3D pose estimation of complex workpiece assembly first acquires scanned point cloud data of workpieces A and B, respectively, and compares the scanned point cloud data with their respective standard model point cloud data to determine the scanned point cloud data corresponding to the assembly surfaces of workpieces A and B. Then, it presets the initial poses of workpieces A and B in the assembly state and moves them to the initial poses, constructing assembly error functions for workpieces A and B. Next, it optimizes the assembly error functions using the Dog-Leg method to obtain the optimized poses of workpieces A and B. Finally, it updates the scanned point cloud data of workpieces A and B based on the optimized poses and recalculates the distance from each point in workpiece A to the nearest point on workpiece B, thereby obtaining the assembly estimation results for workpieces A and B. In the assembly of complex components, this method fully considers the assembly requirements and physical constraints. By simulating the assembly process of parts, it constructs an assembly error function based on contact area and overlap volume, and uses a nonlinear optimization algorithm to solve it. It estimates the assembly pose of each part after final assembly, obtains the error distribution of the final product, and judges the assembly quality of the finished product. This method can reduce the probability of assembly failure, simplify the original "trial assembly-repair" assembly mode, and greatly improve the workpiece assembly accuracy and assembly efficiency.

[0160] The above provides a detailed description of a three-dimensional pose estimation method for complex workpiece assembly provided by this invention. Specific examples have been used to illustrate the principles and implementation methods of this invention. The descriptions of the embodiments above are merely for the purpose of helping to understand the core ideas of this invention. It should be noted that those skilled in the art can make various improvements and modifications to this invention without departing from its principles, and these improvements and modifications also fall within the protection scope of the claims of this invention.

Claims

1. A method for three-dimensional pose estimation of complex workpiece assembly, characterized in that, The method comprises: S1, acquiring scanning point cloud data of assembly workpieces A and B respectively by a three-dimensional scanner, 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 corresponding to assembly surfaces of the assembly workpieces A and B; S2, presetting an initial pose of the assembly workpieces A and B in an assembly state, moving the assembly workpieces A and B to the initial pose, and constructing an assembly error function of the assembly workpieces A and B; S3, optimizing the assembly error function by a Dog-Leg method to obtain an optimized pose of the assembly workpieces A and B; S4, updating the scanning point cloud data of the assembly workpieces A and B according to the optimized pose to obtain updated scanning point cloud data of the assembly workpieces A and B, and recalculating distances from each point in the updated assembly workpiece A to the nearest point in the assembly workpiece B, thereby obtaining assembly estimation results of the assembly workpieces A and B; The S2 of constructing the assembly error function of the assembly workpieces A and B specifically comprises: S21, calculating a contact area of the assembly workpieces A and B in the initial pose; S22, calculating an overlapping volume of the assembly workpieces A and B in the initial pose; S23, constructing the assembly error function of the assembly workpieces A and B by the contact area and the overlapping volume, and a specific formula of the assembly error function is: In the formula, To account for assembly errors, and For proportional parameters, The rotation matrix for assembling workpieces A to B, The translation matrix from workpiece A to B is given. This represents the total number of scanned point cloud data corresponding to the assembly surface of workpiece A. The first point cloud data corresponding to the assembly surface of workpiece A The contact area between a point and the nearest point in the scanned point cloud data corresponding to the assembly surface of assembly workpiece B. The first point cloud data corresponding to the assembly surface of workpiece A The overlap volume between a point and the nearest point in the scanned point cloud data corresponding to the assembly surface of assembly workpiece B.

2. The method of claim 1, wherein, The S1 of comparing the scanning point cloud data of the assembly workpieces A and B with respective standard model point cloud data to determine scanning point cloud data corresponding to assembly surfaces of the assembly workpieces A and B specifically comprises: S11, marking corresponding model point cloud data on respective assembly surfaces on standard models of the assembly workpieces A and B; S12, adopting an ICP registration method to register the standard model point cloud data of the assembly workpieces A and B with respective scanning point cloud data according to the corresponding model point cloud data on the assembly surfaces; S13, establishing a Kd-Tree and presetting a distance threshold, and separating scanning point cloud data not exceeding the preset distance threshold from the scanning point cloud data of the assembly workpieces A and B as corresponding scanning point cloud data on respective assembly surfaces.

3. The method of claim 2, wherein, The S3 specifically comprises: S31, constructing a quadratic model function, presetting a first iteration termination threshold value and a second iteration termination threshold value , initializing a confidence domain radius , assembling poses of workpieces A and B , similarity of an assembly error function and the quadratic model function and a DL optimization step size ; S32, compute the first optimization step length parameter of the second iteration and the confidence region radius ; S33, comparing the optimization step parameter of the first iteration with a first iteration termination threshold value S34, performing a second iteration S35, performing a third iteration S36, comparing the optimization step parameter of the second iteration with a second iteration termination threshold value S37, performing a fourth iteration S38, comparing the optimization step parameter of the third iteration with a third iteration termination threshold value S39, performing a fifth iteration S40, comparing the optimization step parameter of the fourth iteration with a fourth iteration termination threshold value S41, performing a sixth iteration S34, end the DL optimization process, output the current pose of the assembled workpieces A and B ; S35, calculate the first iteration of the DL optimization step size DL optimization step size after the second iteration ; S36、calculating the first similarity of the assembly error function and the quadratic model function after the second iteration ; S37, update the parameters of 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 DL optimization step size, return to step S32, and perform the second iteration optimization. second iteration optimization.

4. The method of claim 3, wherein, The quadratic model function in the S31 is specifically: wherein is the assembly face error for the th iteration, is the Jacobian matrix for the th iteration, is the DL optimization step size after the th iteration.

5. The method of claim 4, wherein, The optimization step length parameter of the S32th iteration is calculated The specific formula is:​ wherein is the optimization step size parameter for the is the assembly face error for the is the Jacobian matrix for the The confidence region radius of the nth iteration is calculated wherein the specific formula is​ wherein is the confidence region radius for the is the historical maximum confidence region radius, is the similarity of the assembly error function and the quadratic model function after the th iteration.

6. The method of claim 5, wherein, The S34 outputs the poses of the currently assembled workpieces A and B The specific formula is: wherein is the first iteration, poses of workpieces A and B after the first iteration, is the second iteration, poses of workpieces A and B after the second iteration, is the third iteration, DL optimization step after the third iteration.

7. The method of claim 6, wherein, The S35 is specifically: S351, compute Gauss-Newton step size and steepest descent step size ; S352, compare the Gauss-Newton step size to the trust region radius, and if the Gauss-Newton step size is outside the trust region radius, then compute the DL optimization step size ;​ S353、If Gauss-Newton step is within the trust region radius and the steepest descent step is also within the trust region radius, then take the steepest descent step Compute the DL optimization step ; S354、In other cases, the Gauss-Newton method step size and the steepest descent method step size DL optimization step size is calculated .

8. The method of claim 7, wherein, The S36 calculates the first Similarity of the assembly error function and the quadratic model function after the second iteration The specific formula is: wherein is the first iteration, is the similarity of the assembly error function and the quadratic model function after the is the first iteration, is the pose of the workpieces A and B after the is the first iteration, is the DL optimization step size after the

Images