Five-axis robot workpiece coordinate system automatic establishment method and device and readable medium

Abstract

The present application relates to the technical field of industrial robot precision machining, and specifically relates to a five-axis robot workpiece coordinate system automatic establishment method, device and readable medium, the method acquires bimodal data under a visual mode and a tactile mode; in the visual mode, a point cloud is matched by using an improved iterative closest point algorithm; for the visual mode and the tactile mode, a workpiece coordinate system is constructed by using an improved Gram-Schmidt orthogonalization method and a three-point method; then, an adaptive fusion decision model is constructed, a homogeneous transformation matrix under the visual mode, the tactile mode or a fusion mode is output, and finally, the workpiece coordinate system obtained after fusion decision is integrated into a five-axis robot kinematics transformation chain, workpiece coordinate system fast, high-precision automatic calibration under complex working conditions is realized, and problems such as low efficiency, poor adaptability, insufficient numerical stability of the algorithm and lack of deep integration with the five-axis robot kinematics transformation chain in the workpiece coordinate system calibration of the prior art are solved.

Description

Technical Field

[0001] This invention relates to the field of precision machining technology for industrial robots, specifically to a method, apparatus, and readable medium for automatically establishing a workpiece coordinate system for a five-axis robot. Background Technology

[0002] In the field of five-axis precision machining, accurate calibration of the workpiece coordinate system is a core prerequisite for ensuring machining accuracy and realizing the tool tip tracking (RTCP) function. Traditional manual teaching methods rely on operators manually touching workpiece feature points using edge finders or dial indicators. This is not only time-consuming and labor-intensive, with a single calibration often taking 30 to 60 minutes, severely restricting production cycle time, but also highly dependent on human experience for accuracy. Repeat positioning errors are usually above ±0.05mm, making it difficult to meet the stringent micron-level accuracy requirements of high-end manufacturing such as aerospace, and even more difficult to adapt to the challenges of random changes in workpiece position in flexible production.

[0003] Existing automated calibration technologies mostly employ a single vision or single tactile mode, resulting in significant application bottlenecks. Pure vision solutions often fail to extract features when faced with highly reflective, dark-colored, light-absorbing, or complex curved workpieces due to poor point cloud quality. Furthermore, the traditional Iterative Closest Point (ICP) algorithm is prone to getting stuck in local optima when the initial pose deviation is large, lacking effective outlier removal and normal vector constraint mechanisms. While pure tactile solutions offer higher accuracy, they rely on expensive force-controlled probes, resulting in low efficiency and the risk of scratching the workpiece surface through point-by-point contact. Moreover, relying solely on discrete point calculations cannot perceive the overall shape of the workpiece, and calibration failure occurs if feature point selection is interfered with by burrs, leading to insufficient system robustness.

[0004] Moreover, existing solutions often treat calibration results as static transformations, failing to fully consider the dynamic coupling effect of the rotation center in a five-axis system with an A-axis turntable, resulting in contour errors when the workpiece rotates with the axis. At the same time, the system cannot adaptively switch or fuse visual and tactile data according to real-time working conditions such as workpiece material and ambient lighting, and lacks a weighted fusion mechanism based on confidence assessment and a coordinate system construction algorithm with high numerical stability. This results in low calibration success rate and poor accuracy stability in complex industrial scenarios, making it difficult to achieve truly fully automated high-precision operations.

[0005] To address the problems of low manual efficiency, poor adaptability to single modes, insufficient numerical stability of algorithms, and lack of deep integration with the kinematic transformation chain of five-axis robots in the existing five-axis robot coordinate system calibration, this application proposes an automatic method, device, and readable medium for establishing the workpiece coordinate system of a five-axis robot. Summary of the Invention

[0006] The purpose of this application is to provide a method, device, and readable medium for automatically establishing the workpiece coordinate system of a five-axis robot in response to the aforementioned technical problems.

[0007] In a first aspect, the present invention provides a method for automatically establishing a workpiece coordinate system for a five-axis robot, preferably comprising the following steps:

[0008] Obtain the original target point cloud of the workpiece to be calibrated, the template point cloud of the standard model, and the coordinates of at least three non-collinear feature points on the workpiece to be calibrated.

[0009] An improved iterative nearest-point algorithm is used to calculate the initial pose transformation matrix of the workpiece coordinate system relative to the robot base coordinate system based on the original target point cloud and the template point cloud.

[0010] Based on the initial pose transformation matrix, the rotation matrix and translation vector of the workpiece coordinate system are calculated using the improved Gram-Schmidt orthogonalization method and the three-point method, thus obtaining the first workpiece coordinate system and its first homogeneous transformation matrix.

[0011] Based on the coordinates of feature points, the rotation matrix and translation vector of the workpiece coordinate system are calculated using the improved Gram-Schmidt orthogonalization method and the three-point method, thus obtaining the second workpiece coordinate system and its second homogeneous transformation matrix.

[0012] A fusion decision model is constructed. The fusion decision model is used to calculate the confidence level of the first workpiece coordinate system and the confidence level of the second workpiece coordinate system respectively. Based on the confidence level of the first workpiece coordinate system and the confidence level of the second workpiece coordinate system, the first homogeneous transformation matrix, the second homogeneous transformation matrix, or the fusion homogeneous transformation matrix is ​​calculated as the homogeneous transformation matrix after decision.

[0013] A kinematic transformation chain for a five-axis robot is constructed, and the homogeneous transformation matrix after decision is integrated into the kinematic transformation chain to calculate the pose of the tool center point in the machine coordinate system.

[0014] Preferably, an improved iterative nearest-point algorithm is used to calculate the initial pose transformation matrix of the workpiece coordinate system relative to the robot base coordinate system based on the original target point cloud and the template point cloud, specifically including:

[0015] A statistical outlier removal algorithm is used to remove outliers from the original target point cloud, resulting in a denoised target point cloud.

[0016] Principal component analysis was used to calculate the covariance matrix of the denoised target point cloud, and eigenvalue decomposition was performed on the covariance matrix to obtain the initial rotation matrix.

[0017] Define a registration error objective function to find the optimal rotation matrix and translation vector, as shown in the following equation:

[0018] ;

[0019] in, Let represent the objective function for registration error. Let represent the rotation matrix, whose initial value is the initial rotation matrix, and t represent the translation vector. This represents any point in the template point cloud. yes The corresponding point in the denoised target point cloud; is the weighting coefficient, with a value of 0 or 1; N represents the number of points in the target point cloud after denoising.

[0020] Using KD-Tree to find the nearest neighbor, specifically including: for a point ,exist Search for nearest neighbor ,in, Represents a template point cloud. This represents the denoised target point cloud. This indicates the search for a function that makes The point q that takes the minimum value, Represents the Euclidean norm;

[0021] Introduce a normal vector consistency constraint to calculate the angle between the normal vectors of point pairs. ,in, Point The normal vector, Point The normal vector, Point normal vector and point The angle between the normal vectors;

[0022] Traverse the template point cloud and the denoised target point cloud, if , Let the threshold of the angle between the normal vectors be denoted as . Remove point pairs and The template point cloud and the target point cloud after removal are obtained.

[0023] A decentralized covariance matrix is ​​constructed based on the removed template point cloud and the removed target point cloud, as shown in the following equation:

[0024] ;

[0025] Where H represents the decentralized covariance matrix; , , This represents the template point cloud after culling, where M represents the number of points in the template point cloud after culling. This represents the geometric centroid of the template point cloud after removal. Point Decentralized coordinates; , , This represents the target point cloud after removal. This represents the geometric centroid of the target point cloud after removal. Point The decentralized coordinates, where T represents the transpose;

[0026] For decentralized covariance matrix Perform singular value decomposition The rotation matrix is ​​solved as shown in the following equation:

[0027] ;

[0028] Where U and V represent the left diagonal matrix and the right diagonal matrix, respectively. It is a diagonal matrix composed of singular values;

[0029] like Then correct The third column of symbols yields a new rotation matrix, as shown in the following equation:

[0030] ;

[0031] in, Represents the determinant of a matrix;

[0032] The translation vector is calculated based on the rotation matrix, as shown in the following formula:

[0033] ;

[0034] The root mean square error of the current iteration is calculated as follows:

[0035] ;

[0036] Wherein, RMSE represents the root mean square error;

[0037] Determine whether the root mean square error of the current iteration has converged or the number of iterations is the maximum number of iterations. If the root mean square error of the current iteration has converged or the number of iterations is the maximum number of iterations, then use the rotation matrix and translation vector calculated in the current iteration as the optimal rotation matrix and translation vector; otherwise, return to calculate the registration error objective function and continue iterating until the root mean square error of the current iteration converges or the number of iterations is the maximum number of iterations.

[0038] The initial pose transformation matrix of the workpiece to be calibrated relative to the robot's base coordinate system is generated based on the optimal rotation matrix and translation vector, as shown in the following equation:

[0039] ;

[0040] in, This represents the initial pose transformation matrix of the workpiece to be calibrated relative to the robot's base coordinate system.

[0041] Preferably, the rotation matrix and translation vector of the workpiece coordinate system are calculated using the improved Gram-Schmidt orthogonalization method and the three-point method, specifically including:

[0042] Obtain three target points, which are represented as follows: , and ;

[0043] The unit vector of the X-axis in the workpiece coordinate system is calculated as follows:

[0044] ;

[0045] ;

[0046] in, Indicates the target point With the target point The direction vector of the connecting line, The unit vector representing the X-axis of the workpiece coordinate system;

[0047] The temporary vector is calculated as follows:

[0048] ;

[0049] in, Indicates the target point With the target point The direction vector of the connecting line is a temporary vector;

[0050] The Y-axis components of the workpiece coordinate system are calculated using the improved Gram-Schmidt formula and then normalized to obtain the unit vector of the Y-axis of the workpiece coordinate system, as shown in the following equation:

[0051] ;

[0052] ;

[0053] in, Represents the Y-axis component of the workpiece coordinate system; The unit vector representing the Y-axis of the workpiece coordinate system;

[0054] Execution direction verification, if Then let ;

[0055] The Z-axis unit vector of the workpiece coordinate system is calculated using the cross product, as shown in the following formula:

[0056] ;

[0057] ;

[0058] in, Represents the Z-axis component of the workpiece coordinate system; The unit vector representing the Z-axis of the workpiece coordinate system;

[0059] According to the target point The workpiece coordinate system is constructed using the X-axis unit vector, Y-axis unit vector, and Z-axis unit vector. The rotation matrix, translation vector, and homogeneous transformation matrix of the workpiece coordinate system are shown in the following equations:

[0060] ;

[0061] ;

[0062] ;

[0063] in, Represents the homogeneous transformation matrix; The coordinates are represented as , The coordinates are represented as , The coordinates are represented as , The coordinates are represented as ;

[0064] The determinant of the rotation matrix is ​​calculated as follows:

[0065] ;

[0066] like and ,in If a preset tolerance value is used, the workpiece coordinate system is determined to be a left-handed coordinate system, and a correction operation is performed, as shown in the following formula:

[0067] ;

[0068] Construct a new rotation matrix and homogeneous transformation matrix ;

[0069] Based on the initial pose transformation matrix, the rotation matrix and translation vector of the workpiece coordinate system are calculated using the improved Gram-Schmidt orthogonalization method and the three-point method, resulting in the first workpiece coordinate system and its first homogeneous transformation matrix, specifically including:

[0070] Obtain three reference points on the template point cloud. , and The three reference points are then mapped to the actual space of the workpiece to be calibrated based on the initial pose transformation matrix, resulting in the coordinates of the three first target points, as shown in the following formula:

[0071] ;

[0072] ;

[0073] ;

[0074] in, , and These represent three reference points. , and The three corresponding first target points after mapping Represents the initial pose transformation matrix The inverse matrix;

[0075] Substituting the coordinates of the three first target points into the steps of calculating the rotation matrix and translation vector of the workpiece coordinate system using the improved Gram-Schmidt orthogonalization method and the three-point method, we obtain the first workpiece coordinate system and the first homogeneous transformation matrix, which is expressed as follows: ;

[0076] Based on the coordinates of feature points, the rotation matrix and translation vector of the workpiece coordinate system are calculated using an improved Gram-Schmidt orthogonalization method and a three-point method, resulting in the second workpiece coordinate system and its second homogeneous transformation matrix, specifically including:

[0077] Three non-collinear feature points on the workpiece to be calibrated are used as the second target points.

[0078] Substituting the coordinates of the three second target points into the steps of calculating the rotation matrix and translation vector of the workpiece coordinate system using the improved Gram-Schmidt orthogonalization method and the three-point method, we obtain the second workpiece coordinate system and the second homogeneous transformation matrix. The second homogeneous transformation matrix is ​​expressed as follows: .

[0079] Preferably, the process of obtaining the coordinates of at least three non-collinear feature points on the workpiece to be calibrated specifically includes:

[0080] The robot's end effector sequentially touches at least three non-collinear feature points on the target surface for calibration.

[0081] For each feature point, perform n repeated touches, and record the pose of the robot flange in the robot base coordinate system and the transformation of the tool coordinate system relative to the robot flange at the u-th touch. And calculate the coordinates of the feature points, as shown in the following formula:

[0082] ;

[0083] in, Representing feature points The coordinates at the u-th touch; f represents the index of the feature point, and the number of indices is at least 3; This represents the change in the robot flange pose during the u-th touch. This indicates the transformation of the pre-calibrated tool coordinate system relative to the robot flange;

[0084] Calculate feature points after n touches The feature points are obtained by averaging the coordinates of the points. The average coordinates are shown in the following formula:

[0085] ;

[0086] in, Representing feature points The average coordinates.

[0087] As a preferred option, the processing steps of the fusion decision model specifically include:

[0088] The confidence level of the first workpiece coordinate system is calculated as follows:

[0089] ;

[0090] in, This represents the confidence level of the first workpiece coordinate system. , and All represent weights. , This represents the maximum value of the root mean square error during the iteration process. To score the quality of the point cloud, Rate the quality of light;

[0091] The confidence level of the second workpiece coordinate system is calculated as follows:

[0092] ;

[0093] in, This represents the confidence level of the second workpiece coordinate system. For robot positioning accuracy coefficients, For feature points The standard deviation of the coordinates; and All represent weights. ;

[0094] Set decision threshold Based on the confidence levels of the first and second workpiece coordinate systems, the first homogeneous transformation matrix, the second homogeneous transformation matrix, or a fused homogeneous transformation matrix is ​​selected. The decision conditions are as follows:

[0095] ;

[0096] Wherein, Mode represents the data acquisition mode, Visual represents the visual mode, the workpiece coordinate system constructed based on the data acquired in the visual mode is the first workpiece coordinate system, Touch represents the tactile mode, the workpiece coordinate system constructed based on the data acquired in the tactile mode is the second workpiece coordinate system, and Fusion represents the fusion mode.

[0097] If the vision mode is selected, the first homogeneous transformation matrix corresponding to the first workpiece coordinate system will be used as the homogeneous transformation matrix after the decision.

[0098] If the haptic mode is selected, the second homogeneous transformation matrix corresponding to the second workpiece coordinate system will be used as the homogeneous transformation matrix after the decision.

[0099] If the fusion mode is selected, the Kalman filter algorithm is used to calculate the fused homogeneous transformation matrix, and this matrix is ​​used as the post-decision homogeneous transformation matrix. The calculation of the fused homogeneous transformation matrix using the Kalman filter algorithm specifically includes:

[0100] The state is estimated by performing a homogeneous transformation matrix on the workpiece coordinate system, and the state vector is shown in the following equation:

[0101] ;

[0102] in, Represents the state vector. This indicates the position of the origin of the workpiece coordinate system in the machine tool coordinate system and the rotation angle around the coordinate axes of the machine tool coordinate system during the k-th measurement.

[0103] The prediction steps are as follows:

[0104] ;

[0105] ;

[0106] in, This represents the prior state estimate of the homogeneous transformation matrix of the current workpiece coordinate system. This represents the state estimate of the homogeneous transformation matrix of the previous workpiece coordinate system. The state prediction error covariance matrix represents the homogeneous transformation matrix of the current workpiece coordinate system. Let Q represent the state estimation error covariance matrix of the homogeneous transformation matrix of the previous workpiece coordinate system, and let Q represent the process noise covariance matrix.

[0107] The update steps are as follows:

[0108] ;

[0109] ;

[0110] ;

[0111] in, Indicates Kalman gain, Represents the transpose of the observation matrix. To observe the noise covariance matrix, This represents the state estimate of the homogeneous transformation matrix of the current workpiece coordinate system. These are the observed values ​​of the homogeneous transformation matrix of the current workpiece coordinate system. It is the state estimation error covariance matrix of the homogeneous transformation matrix of the current workpiece coordinate system after measurement and update, where I represents the identity matrix, and is the final output. The corresponding homogeneous transformation matrix is ​​used as the fused homogeneous transformation matrix. .

[0112] Preferably, a kinematic transformation chain for the five-axis robot is constructed, and the homogeneous transformation matrix after decision-making is integrated into the kinematic transformation chain to calculate the pose of the tool center point in the machine tool coordinate system, specifically including:

[0113] The kinematic transformation chain includes, in sequence, the machine tool coordinate system. A-axis coordinate system Workpiece coordinate system and tool coordinate system ;

[0114] The pose of the tool center point in the machine coordinate system is calculated based on the kinematic transformation chain, and the first pose expression is obtained as follows:

[0115] ;

[0116] in, This indicates the pose of the tool center point in the machine tool coordinate system. This represents the fixed transformation matrix from the A-axis coordinate system to the machine tool coordinate system. The transformation matrix represents the rotation angle α of the A-axis around the Z-axis of the A-axis coordinate system; This represents the transformation matrix of the workpiece coordinate system relative to the A-axis coordinate system when the A-axis is at zero position. , Represents the homogeneous transformation matrix after the decision; The transformation matrix represents the tool coordinate system relative to the workpiece coordinate system; the expression for the transformation matrix of the rotation of the A-axis about the Z-axis of the workpiece coordinate system is shown below:

[0117] .

[0118] Secondly, the present invention provides an automatic workpiece coordinate system establishment device for a five-axis robot, comprising:

[0119] The data acquisition module is configured to acquire the original target point cloud of the workpiece to be calibrated, the template point cloud of the standard model, and the coordinates of at least three non-collinear feature points on the workpiece to be calibrated.

[0120] The pose calculation module is configured to calculate the initial pose transformation matrix of the workpiece coordinate system relative to the robot base coordinate system based on the original target point cloud and the template point cloud using an improved iterative nearest point algorithm.

[0121] The first workpiece coordinate system construction module is configured to calculate the rotation matrix and translation vector of the workpiece coordinate system based on the initial pose transformation matrix using the improved Gram-Schmidt orthogonalization method and the three-point method, so as to obtain the first workpiece coordinate system and its first homogeneous transformation matrix.

[0122] The second workpiece coordinate system construction module is configured to use the improved Gram-Schmidt orthogonalization method and the three-point method to calculate the rotation matrix and translation vector of the workpiece coordinate system based on the coordinates of feature points, thereby obtaining the second workpiece coordinate system and its second homogeneous transformation matrix.

[0123] The fusion decision module is configured to construct a fusion decision model. The fusion decision model is used to calculate the confidence level of the first workpiece coordinate system and the confidence level of the second workpiece coordinate system respectively, and select the first homogeneous transformation matrix, the second homogeneous transformation matrix, or calculate the fusion homogeneous transformation matrix as the homogeneous transformation matrix after decision based on the confidence level of the first workpiece coordinate system and the confidence level of the second workpiece coordinate system.

[0124] The coordinate system integration module is configured to construct the kinematic transformation chain of the five-axis robot, integrating the post-decision homogeneous transformation matrix into the kinematic transformation chain to calculate the pose of the tool center point in the machine coordinate system.

[0125] Thirdly, the present invention provides an electronic device including one or more processors; and a memory for storing one or more programs, which, when executed by one or more processors, cause the one or more processors to implement the method described in any implementation of the first aspect.

[0126] Fourthly, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method as described in any of the implementations of the first aspect.

[0127] Fifthly, the present invention provides a computer program product, including a computer program that, when executed by a processor, implements the method as described in any of the implementations in the first aspect.

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

[0129] (1) The automatic establishment method for the workpiece coordinate system of the five-axis robot proposed in this application processes the data acquired in the vision mode and the tactile mode respectively and constructs the workpiece coordinate system. The confidence of the workpiece coordinate system in the two modes is calculated by the fusion decision model. Based on the confidence calculation result, the homogeneous transformation matrix after decision is output. Furthermore, the homogeneous transformation matrix after decision is integrated into the motion transformation chain of the five-axis robot, realizing the automated establishment process of the workpiece coordinate system, replacing manual teaching, greatly shortening the calibration time, forming a full-link intelligent closed loop from data acquisition, processing, fusion to integration, greatly improving the automation level and reliability of the system, and effectively solving the problems of low efficiency, poor accuracy and weak anti-interference of traditional calibration.

[0130] (2) The five-axis robot workpiece coordinate system automatic establishment method proposed in this application uses an improved iterative nearest point algorithm and a multiple repeated touch averaging strategy to process the data obtained by the visual mode and tactile mode in the process of establishing the workpiece coordinate system to improve the positioning accuracy. It also combines the improved Gram-Schmidt orthogonalization method and the three-point method to construct the workpiece coordinate system, ensuring the orthogonality and direction accuracy between the coordinate axes of the workpiece coordinate system and constructing a coordinate system with high numerical stability.

[0131] (3) The five-axis robot workpiece coordinate system automatic establishment method proposed in this application constructs an adaptive fusion decision model. The output of the fusion decision model is a homogeneous transformation matrix in visual mode, tactile mode or fusion mode. It can use vision to process complex curved surfaces and use tactile sense to overcome visual blind spots such as high reflectivity and dark colors, thus solving the problem of single mode failure under extreme working conditions. Attached Figure Description

[0132] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0133] Figure 1 This is a flowchart illustrating the automatic establishment method for the workpiece coordinate system of a five-axis robot according to an embodiment of this application.

[0134] Figure 2 This is a schematic diagram of a grinding system using a five-axis robot and an A-axis turntable in collaboration, as described in an embodiment of this application.

[0135] Figure 3 This is a schematic diagram of an automatic workpiece coordinate system establishment device for a five-axis robot, as described in an embodiment of this application.

[0136] Figure 4 This is a schematic diagram of the hardware structure of an electronic device provided in an embodiment of the present invention.

[0137] Reference numerals: 11. Five-axis robot; 111. Upper arm; 112. Forearm; 113. First wrist; 114. Second wrist; 115. Flange; 116. End effector; 117. Linear mechanism; 1101, J1 axis; 1102, J2 axis; 1103, J3 axis; 1104, J4 axis; 1105, J5 axis; 12, A-axis turntable; 13, workpiece; 14, 3D line scan camera. Detailed Implementation

[0138] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this invention, and not all of them. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention.

[0139] Figure 1 An embodiment of this application illustrates an automatic method for establishing a workpiece coordinate system for a five-axis robot, comprising the following steps:

[0140] S1, obtain the original target point cloud of the workpiece to be calibrated, the template point cloud of the standard model, and the coordinates of at least three non-collinear feature points on the workpiece to be calibrated.

[0141] Specifically, step S1 achieves multimodal data acquisition. The method proposed in this application activates a visual mode and / or a tactile mode according to a preset strategy or user instruction. In visual mode, dense point cloud data of the workpiece surface to be calibrated is acquired using a 3D sensor; in tactile mode, a contact probe or rigid tool is installed at the end effector of a five-axis robot. The robot's end effector tool is controlled to sequentially contact at least three preset non-collinear feature points on the workpiece, and the robot's pose data at the moment of contact is recorded.

[0142] In a specific embodiment, the process of obtaining the coordinates of at least three non-collinear feature points on the workpiece to be calibrated specifically includes:

[0143] The robot's end effector sequentially touches at least three non-collinear feature points on the target surface for calibration.

[0144] For each feature point, perform n repeated touches, and record the pose of the robot flange in the robot base coordinate system and the transformation of the tool coordinate system relative to the robot flange at the u-th touch. And calculate the coordinates of the feature points, as shown in the following formula:

[0145] ;

[0146] in, Representing feature points The coordinates at the u-th touch; f represents the index of the feature point, and the number of indices is at least 3; This represents the change in the robot flange pose during the u-th touch. This indicates the transformation of the pre-calibrated tool coordinate system relative to the robot flange;

[0147] Calculate feature points after n touches The feature points are obtained by averaging the coordinates of the points. The average coordinates are shown in the following formula:

[0148] ;

[0149] in, Representing feature points The average coordinates.

[0150] Specifically, the method proposed in this application performs multiple repeated touches on the feature points acquired in the tactile mode and takes the average value to eliminate random errors.

[0151] S2, based on the original target point cloud and template point cloud, uses an improved iterative nearest point algorithm to calculate the initial pose transformation matrix of the workpiece coordinate system relative to the robot base coordinate system.

[0152] In a specific embodiment, an improved iterative nearest-point algorithm is used to calculate the initial pose transformation matrix of the workpiece coordinate system relative to the robot base coordinate system based on the original target point cloud and the template point cloud. Specifically, this includes:

[0153] A statistical outlier removal algorithm is used to remove outliers from the original target point cloud, resulting in a denoised target point cloud.

[0154] Principal component analysis was used to calculate the covariance matrix of the denoised target point cloud, and eigenvalue decomposition was performed on the covariance matrix to obtain the initial rotation matrix.

[0155] Define a registration error objective function to find the optimal rotation matrix and translation vector, as shown in the following equation:

[0156] ;

[0157] in, Let represent the objective function for registration error. Let represent the rotation matrix, whose initial value is the initial rotation matrix, and t represent the translation vector. This represents any point in the template point cloud. yes The corresponding point in the denoised target point cloud; is the weighting coefficient, with a value of 0 or 1; N represents the number of points in the target point cloud after denoising.

[0158] Using KD-Tree to find the nearest neighbor, specifically including: for a point ,exist Search for nearest neighbor ,in, Represents a template point cloud. This represents the denoised target point cloud. This indicates the search for a function that makes The point q that takes the minimum value, Represents the Euclidean norm;

[0159] Introduce a normal vector consistency constraint to calculate the angle between the normal vectors of point pairs. ,in, Point The normal vector, Point The normal vector, Point normal vector and point The angle between the normal vectors;

[0160] Traverse the template point cloud and the denoised target point cloud, if , Let the threshold of the angle between the normal vectors be denoted as . Remove point pairs and The template point cloud and the target point cloud after removal are obtained.

[0161] A decentralized covariance matrix is ​​constructed based on the removed template point cloud and the removed target point cloud, as shown in the following equation:

[0162] ;

[0163] Where H represents the decentralized covariance matrix; , , This represents the template point cloud after culling, where M represents the number of points in the template point cloud after culling. This represents the geometric centroid of the template point cloud after removal. Point Decentralized coordinates; , , This represents the target point cloud after removal. This represents the geometric centroid of the target point cloud after removal. Point The decentralized coordinates, where T represents the transpose;

[0164] For decentralized covariance matrix Perform singular value decomposition The rotation matrix is ​​solved as shown in the following equation:

[0165] ;

[0166] Where U and V represent the left diagonal matrix and the right diagonal matrix, respectively. It is a diagonal matrix composed of singular values;

[0167] like Then correct The third column of symbols yields a new rotation matrix, as shown in the following equation:

[0168] ;

[0169] in, Represents the determinant of a matrix;

[0170] The translation vector is calculated based on the rotation matrix, as shown in the following formula:

[0171] ;

[0172] The root mean square error of the current iteration is calculated as follows:

[0173] ;

[0174] Wherein, RMSE represents the root mean square error;

[0175] Determine whether the root mean square error of the current iteration has converged or the number of iterations is the maximum number of iterations. If the root mean square error of the current iteration has converged or the number of iterations is the maximum number of iterations, then use the rotation matrix and translation vector calculated in the current iteration as the optimal rotation matrix and translation vector; otherwise, return to calculate the registration error objective function and continue iterating until the root mean square error of the current iteration converges or the number of iterations is the maximum number of iterations.

[0176] The initial pose transformation matrix of the workpiece to be calibrated relative to the robot's base coordinate system is generated based on the optimal rotation matrix and translation vector, as shown in the following equation:

[0177] ;

[0178] in, This represents the initial pose transformation matrix of the workpiece to be calibrated relative to the robot's base coordinate system.

[0179] Specifically, an improved Iterative Nearest Point (ICP) algorithm is executed on the original target point cloud and template point cloud acquired in visual mode: First, the original target point cloud is denoised by statistical outlier removal (SOR) filtering; second, principal component analysis (PCA) is used for coarse registration to obtain the initial rotation matrix; then, a KD-Tree is constructed to accelerate the nearest neighbor search, and normal vector consistency constraints are introduced to eliminate incorrect matching point pairs; finally, the optimal rotation matrix and translation vector are solved based on singular value decomposition (SVD), and the algorithm is iterated through multiple scales until convergence.

[0180] In the embodiments of this application, a statistical outlier removal algorithm is used to remove outliers from the original target point cloud to obtain a denoised target point cloud, specifically including:

[0181] For the original target point cloud any point in Calculate the average Euclidean distance from it to its K nearest neighbors, as shown in the following formula:

[0182] ;

[0183] in, Point The average Euclidean distance to the K nearest neighbors. Represents the original target point cloud any point, Represents the original target point cloud The number of points; express The j-th nearest neighbor. Assume the average distance between all points follows a Gaussian distribution. The global mean μ and standard deviation σ are calculated as follows:

[0184] ;

[0185] .

[0186] Furthermore, a threshold multiplier Mul is set, typically ranging from 2.5 to 3.5, for the original target point cloud. any point in Perform a rejection judgment; if the point satisfy Then retain the point Otherwise, remove the point. For the original target point cloud After all points are removed, the denoised target point cloud is obtained. .

[0187] Principal component analysis (PCA) is used to calculate the covariance matrix of the denoised target point cloud, and eigenvalue decomposition is performed on the covariance matrix to obtain the initial rotation matrix, specifically including:

[0188] Calculate the denoised target point cloud geometric centroid The calculation process for the covariance matrix is ​​as follows:

[0189] ;

[0190] ;

[0191] in, Represents the target point cloud after denoising. any point in it; Represents the target point cloud after denoising. The number of midpoints; Represents the target point cloud after denoising. The covariance matrix, This represents the summation operation. Perform eigenvalue decomposition to obtain eigenvalues. and its corresponding unit eigenvector Construct the initial rotation matrix. As shown in the following formula:

[0192] ;

[0193] like Then Reverse the direction to ensure the right hand is properly secured. Utilize Will Rotate to match the template point cloud Roughly aligned.

[0194] In the embodiments of this application, the registration error objective function is calculated. Weighting coefficients The default value is 1, but it may become 0 after filtering and elimination operations based on the normal vector consistency constraint.

[0195] S3. Based on the initial pose transformation matrix, the rotation matrix and translation vector of the workpiece coordinate system are calculated using the improved Gram-Schmidt orthogonalization method and the three-point method, thus obtaining the first workpiece coordinate system and its first homogeneous transformation matrix.

[0196] S4. Based on the coordinates of the feature points, the rotation matrix and translation vector of the workpiece coordinate system are calculated using the improved Gram-Schmidt orthogonalization method and the three-point method, thus obtaining the second workpiece coordinate system and its second homogeneous transformation matrix.

[0197] In a specific embodiment, the rotation matrix and translation vector of the workpiece coordinate system are calculated using an improved Gram-Schmidt orthogonalization method and a three-point method, specifically including:

[0198] Obtain three target points, which are represented as follows: , and ;

[0199] The unit vector of the X-axis in the workpiece coordinate system is calculated as follows:

[0200] ;

[0201] ;

[0202] in, Indicates the target point With the target point The direction vector of the connecting line, The unit vector representing the X-axis of the workpiece coordinate system;

[0203] The temporary vector is calculated as follows:

[0204] ;

[0205] in, Indicates the target point With the target point The direction vector of the connecting line is a temporary vector;

[0206] The Y-axis components of the workpiece coordinate system are calculated using the improved Gram-Schmidt formula and then normalized to obtain the unit vector of the Y-axis of the workpiece coordinate system, as shown in the following equation:

[0207] ;

[0208] ;

[0209] in, Represents the Y-axis component of the workpiece coordinate system; The unit vector representing the Y-axis of the workpiece coordinate system;

[0210] Execution direction verification, if Then let ;

[0211] The Z-axis unit vector of the workpiece coordinate system is calculated using the cross product, as shown in the following formula:

[0212] ;

[0213] ;

[0214] in, Represents the Z-axis component of the workpiece coordinate system; The unit vector representing the Z-axis of the workpiece coordinate system;

[0215] According to the target point The workpiece coordinate system is constructed using the X-axis unit vector, Y-axis unit vector, and Z-axis unit vector. The rotation matrix, translation vector, and homogeneous transformation matrix of the workpiece coordinate system are shown in the following equations:

[0216] ;

[0217] ;

[0218] ;

[0219] in, Represents the homogeneous transformation matrix; The coordinates are represented as , The coordinates are represented as , The coordinates are represented as , The coordinates are represented as ;

[0220] The determinant of the rotation matrix is ​​calculated as follows:

[0221] ;

[0222] like and ,in If a preset tolerance value is used, the workpiece coordinate system is determined to be a left-handed coordinate system, and a correction operation is performed, as shown in the following formula:

[0223] ;

[0224] Construct a new rotation matrix and homogeneous transformation matrix ;

[0225] Based on the initial pose transformation matrix, the rotation matrix and translation vector of the workpiece coordinate system are calculated using the improved Gram-Schmidt orthogonalization method and the three-point method, resulting in the first workpiece coordinate system and its first homogeneous transformation matrix, specifically including:

[0226] Obtain three reference points on the template point cloud. , and The three reference points are then mapped to the actual space of the workpiece to be calibrated based on the initial pose transformation matrix, resulting in the coordinates of the three first target points, as shown in the following formula:

[0227] ;

[0228] ;

[0229] ;

[0230] in, , and These represent three reference points. , and The three corresponding first target points after mapping Represents the initial pose transformation matrix The inverse matrix;

[0231] Substituting the coordinates of the three first target points into the steps of calculating the rotation matrix and translation vector of the workpiece coordinate system using the improved Gram-Schmidt orthogonalization method and the three-point method, we obtain the first workpiece coordinate system and the first homogeneous transformation matrix, which is expressed as follows: ;

[0232] Based on the coordinates of feature points, the rotation matrix and translation vector of the workpiece coordinate system are calculated using an improved Gram-Schmidt orthogonalization method and a three-point method, resulting in the second workpiece coordinate system and its second homogeneous transformation matrix, specifically including:

[0233] Three non-collinear feature points on the workpiece to be calibrated are used as the second target points.

[0234] Substituting the coordinates of the three second target points into the steps of calculating the rotation matrix and translation vector of the workpiece coordinate system using the improved Gram-Schmidt orthogonalization method and the three-point method, we obtain the second workpiece coordinate system and the second homogeneous transformation matrix. The second homogeneous transformation matrix is ​​expressed as follows: .

[0235] Specifically, the method proposed in this application employs an improved Gram-Schmidt orthogonalization method to construct a first workpiece coordinate system and a second workpiece coordinate system from multimodal data acquired in visual and tactile modes, respectively. The improved Gram-Schmidt orthogonalization method specifically includes: calculating the X-axis unit vector, projecting and normalizing the Y-axis component using the MGS algorithm, and calculating the Z-axis unit vector through the cross product; simultaneously, performing right-handed coordinate system verification; if a negative determinant is detected, the constructed workpiece coordinate system is a left-handed system; automatically flipping the Z-axis direction to ensure compliance with the right-hand rule; and generating a homogeneous transformation matrix for the workpiece coordinate system, thereby constructing a robust workpiece coordinate system. In the embodiments of this application, the improved Gram-Schmidt orthogonalization algorithm replaces the classical method, effectively suppressing the accumulation of rounding errors when feature points are nearly collinear, ensuring the orthogonality and directional accuracy between the coordinate axes of the workpiece coordinate system, and constructing a coordinate system with high numerical stability.

[0236] S5. Construct a fusion decision model. The fusion decision model is used to calculate the confidence level of the first workpiece coordinate system and the confidence level of the second workpiece coordinate system respectively. Based on the confidence level of the first workpiece coordinate system and the confidence level of the second workpiece coordinate system, select the first homogeneous transformation matrix, the second homogeneous transformation matrix, or calculate the fusion homogeneous transformation matrix as the homogeneous transformation matrix after decision.

[0237] In a specific embodiment, the processing procedure of the fusion decision model specifically includes:

[0238] The confidence level of the first workpiece coordinate system is calculated as follows:

[0239] ;

[0240] in, This represents the confidence level of the first workpiece coordinate system. , and All represent weights. , This represents the maximum value of the root mean square error during the iteration process. To score the quality of the point cloud, Rate the quality of light;

[0241] The confidence level of the second workpiece coordinate system is calculated as follows:

[0242] ;

[0243] in, This represents the confidence level of the second workpiece coordinate system. For robot positioning accuracy coefficients, For feature points The standard deviation of the coordinates; and All represent weights. ;

[0244] Set decision threshold Based on the confidence levels of the first and second workpiece coordinate systems, the first homogeneous transformation matrix, the second homogeneous transformation matrix, or a fused homogeneous transformation matrix is ​​selected. The decision conditions are as follows:

[0245] ;

[0246] Wherein, Mode represents the data acquisition mode, Visual represents the visual mode, the workpiece coordinate system constructed based on the data acquired in the visual mode is the first workpiece coordinate system, Touch represents the tactile mode, the workpiece coordinate system constructed based on the data acquired in the tactile mode is the second workpiece coordinate system, and Fusion represents the fusion mode.

[0247] If the vision mode is selected, the first homogeneous transformation matrix corresponding to the first workpiece coordinate system will be used as the homogeneous transformation matrix after the decision.

[0248] If the haptic mode is selected, the second homogeneous transformation matrix corresponding to the second workpiece coordinate system will be used as the homogeneous transformation matrix after the decision.

[0249] If the fusion mode is selected, the Kalman filter algorithm is used to calculate the fused homogeneous transformation matrix, and this matrix is ​​used as the post-decision homogeneous transformation matrix. The calculation of the fused homogeneous transformation matrix using the Kalman filter algorithm specifically includes:

[0250] The state is estimated by performing a homogeneous transformation matrix on the workpiece coordinate system, and the state vector is shown in the following equation:

[0251] ;

[0252] in, Represents the state vector. This indicates the position of the origin of the workpiece coordinate system in the machine tool coordinate system and the rotation angle around the coordinate axes of the machine tool coordinate system when the workpiece coordinate system is established for the kth time.

[0253] The prediction steps are as follows:

[0254] ;

[0255] ;

[0256] in, This represents the prior state estimate of the homogeneous transformation matrix of the current workpiece coordinate system. This represents the state estimate of the homogeneous transformation matrix of the previous workpiece coordinate system. The state prediction error covariance matrix represents the homogeneous transformation matrix of the current workpiece coordinate system. Let Q represent the state estimation error covariance matrix of the homogeneous transformation matrix of the previous workpiece coordinate system, and let Q represent the process noise covariance matrix.

[0257] The update steps are as follows:

[0258] ;

[0259] ;

[0260] ;

[0261] in, Indicates Kalman gain, Represents the transpose of the observation matrix. To observe the noise covariance matrix, This represents the state estimate of the homogeneous transformation matrix of the current workpiece coordinate system. These are the observed values ​​of the homogeneous transformation matrix of the current workpiece coordinate system. It is the state estimation error covariance matrix of the homogeneous transformation matrix of the current workpiece coordinate system after measurement and update, where I represents the identity matrix, and is the final output. The corresponding homogeneous transformation matrix is ​​used as the fused homogeneous transformation matrix. .

[0262] Specifically, the rotation sequence of the workpiece coordinate system origin around the machine tool coordinate system axes adopts the ZYX Euler angle convention. When the process noise Q→∞, the Kalman filter degenerates into a weighted average. Specifically, in fusion mode, dynamic weights are calculated. As shown in the following formula:

[0263] ;

[0264] Constructing the correction matrix The fusion homogeneous transformation matrix is ​​calculated as shown in the following equation: ; ;

[0265] in, Represents the correction matrix. Let the first homogeneous transformation matrix be represented. Let I denote the second homogeneous transformation matrix, and let I denote the identity matrix.

[0266] In the embodiments of this application, due to the influence of robot body errors and environmental thermal errors, the homogeneous transformation matrix obtained by executing steps S1 to S5 in a single execution may have slight jitter. Therefore, the automatic workpiece coordinate system establishment stage will repeatedly execute steps S1 to S5 to continuously calculate the homogeneous transformation matrix after the decision multiple times. Specifically, when performing the automatic workpiece coordinate system establishment task, the homogeneous transformation matrix after the decision of the workpiece coordinate system is continuously calculated at a preset sampling frequency, for example, at a frequency of 10 times per second or 20 times per second. One homogeneous transformation matrix after the decision is one frame of data. The comprehensive accuracy index of the homogeneous transformation matrix after the decision is dynamically calculated. The formula for calculating the comprehensive accuracy of the homogeneous transformation matrix after the decision of the g-th frame is as follows:

[0267] ;

[0268] in, Indicates the overall accuracy of the homogeneous transformation matrix after the decision; This indicates visual error, which originates from errors caused by camera calibration or point cloud processing. This indicates that the error in the robot body comes from mechanical errors such as joint clearance of the robotic arm and inaccurate link parameters. This indicates algorithm error, which stems from numerical computation errors, such as residual errors caused by the incomplete convergence of the improved iterative closest point algorithm. This indicates environmental thermal error, which is caused by the expansion of metal due to the heat generated by the operation of various machine structures in the grinding system.

[0269] like ,in The required accuracy of the workpiece coordinate system calibration indicates that the homogeneous transformation matrix after the decision in the g-th frame is unreliable, necessitating the activation of a compensation mechanism. Specifically, the compensation mechanism is a moving average filter compensation, and the compensation formula is shown below:

[0270] ;

[0271] in, Let represent the homogeneous transformation matrix of the g-th frame after compensation and decision-making; This represents the homogeneous transformation matrix after decision-making in the past l'-th frame; L represents the size of the preset sliding window. For example, with a sliding window L of 5, to verify the stability of the workpiece coordinate system establishment result, steps S1 to S5 are repeated 5 times to obtain 5 slightly different homogeneous transformation matrices after decision-making. When the overall accuracy of the homogeneous transformation matrix after decision-making in the g-th frame is greater than the accuracy required for workpiece coordinate system calibration, moving average filtering compensation is activated. The average value of the homogeneous transformation matrices after decision-making in the most recent 5 established workpiece coordinate systems is taken as the compensated homogeneous transformation matrix after decision-making in the g-th frame, preventing the robot from jittering due to sudden changes in the workpiece coordinate system. The compensated homogeneous transformation matrix after decision-making in the g-th frame is written into the robot controller as a new, more stable workpiece coordinate system, realizing the dynamic integration of the workpiece coordinate system into the motion model of the five-axis robot.

[0272] The proposed method employs adaptive fusion decision-making. It calculates the confidence level of the visual mode in real time based on registration error, point cloud quality, and illumination quality, and calculates the confidence level of the tactile mode in real time based on robot positioning accuracy and the repeatability standard deviation of feature points. Depending on the difference in confidence levels between the visual and tactile modes, it intelligently selects a single high-confidence mode for output, or activates a shared control strategy when their confidence levels are close. Kalman filtering is used to fuse the observations of the two modes, thereby outputting the optimal coordinate system transformation matrix. The fusion decision-making mechanism based on visual and tactile modes proposed in this application can utilize vision to process complex curved surfaces and leverage touch to overcome visual blind spots such as high reflectivity and dark colors, solving the problem of single-modal failure under extreme conditions. Furthermore, the proposed method achieves self-evaluation and error compensation in the calibration process through confidence scoring and Kalman filtering fusion, significantly improving the reliability of workpiece coordinate system establishment.

[0273] S6. Construct the kinematic transformation chain of the five-axis robot and integrate the homogeneous transformation matrix after decision into the kinematic transformation chain to calculate the pose of the tool center point in the machine coordinate system.

[0274] In a specific embodiment, a kinematic transformation chain for the five-axis robot is constructed, and the homogeneous transformation matrix after decision-making is integrated into the kinematic transformation chain to calculate the pose of the tool center point in the machine tool coordinate system. Specifically, this includes:

[0275] The kinematic transformation chain includes, in sequence, the machine tool coordinate system. A-axis coordinate system Workpiece coordinate system and tool coordinate system ;

[0276] The pose of the tool center point in the machine coordinate system is calculated based on the kinematic transformation chain, and the first pose expression is obtained as follows:

[0277] ;

[0278] in, This indicates the pose of the tool center point in the machine tool coordinate system. This represents the fixed transformation matrix from the A-axis coordinate system to the machine tool coordinate system. The transformation matrix represents the rotation angle α of the A-axis around the Z-axis of the A-axis coordinate system; This represents the transformation matrix of the workpiece coordinate system relative to the A-axis coordinate system when the A-axis is at zero position. , Represents the homogeneous transformation matrix after the decision; The transformation matrix represents the tool coordinate system relative to the workpiece coordinate system; the expression for the transformation matrix of the rotation of the A-axis about the Z-axis of the workpiece coordinate system is shown below:

[0279] .

[0280] For details, please refer to Figure 2 This application applies to a grinding system employing a five-axis robot 11 and an A-axis rotary table 12 working collaboratively. The five-axis robot 11 is fixed to the machine tool base, while the A-axis rotary table 12 is independent of the five-axis robot 11. The workpiece 13 is clamped on the A-axis rotary table 12, which is independent of the five-axis robot 11. The five-axis robot 11 includes a robot base, The linear mechanism 117, the upper arm 111, the lower arm 112, the first wrist 113, the second wrist 114, the flange 115, and the end effector 116. The linear mechanism 117 is mounted on the robot base and controls the entire machine to move along the axis. Linear motion of the axis; the J1 axis 1101 is connected to the J1 axis pitch mechanism. The linear mechanism 117 connects to the upper arm 111 to achieve shoulder pitch; the J2 axis 1102 connects to the upper arm 111 and forearm 112 via the J2 axis elbow mechanism to achieve elbow flexion; the J3 axis 1103 connects to the forearm 112 and first wrist 113 via the J3 axis rotation mechanism to achieve end-effector rotation; the J4 axis 1104 connects to the first wrist 113 and second wrist 114 via the J4 axis tilting mechanism to achieve end-effector pitch and tilt; and the J5 axis 1105 connects to the second wrist 114 and flange 115 via the J5 axis rotation mechanism to achieve tool rotation around its own axis. Figure 2 In the middle, the end effector 116 is a calibration needle.

[0281] The A-axis turntable 12 includes a turntable base, an A-axis rotation mechanism, and a workpiece clamping seat. The A-axis turntable 12 is positioned in front of the robot's workspace. Its A-axis rotation mechanism controls the rotation of the workpiece clamping seat, causing the workpiece 13 to rotate within a ±180° range around the A-axis. The workpiece 13 is fixed to the A-axis turntable 12 by the workpiece clamping seat and rotates synchronously with the A-axis. The orientation of the workpiece 13 is changed by rotating the A-axis turntable 12 around its own axis.

[0282] The 3D line scan camera 14 includes a camera body, a lens assembly, a line laser emitter, and a camera bracket. The 3D line scan camera 14 is fixedly mounted above and to the side of the A-axis turntable 12, with its field of view facing the center area of ​​the turntable. The 3D line scan camera 14 acquires 3D point cloud data of the workpiece 13 through line laser projection, and collects real-time surface morphology information of the workpiece 13.

[0283] The grinding wheel assembly (or other replaceable end effector) includes a tool mounting flange, a spindle, and cutting components, wherein the cutting components include a grinding wheel, a needle tip, and a milling cutter. The grinding wheel assembly is mounted to the end effector of a five-axis robot 11 via flange 115, and its pose in three-dimensional space is controlled by the five-axis robot 11.

[0284] refer to Figure 2 For a grinding system including an A-axis rotary table 12 and a five-axis robot 11, a transformation relationship is established from the machine tool coordinate system to the A-axis coordinate system, then to the workpiece coordinate system rotating with the A-axis, and finally to the tool coordinate system. The kinematic transformation chain sequentially includes the machine tool coordinate system... A-axis coordinate system Workpiece coordinate system and tool coordinate system .

[0285] Expanding the first pose expression using matrix multiplication is shown in the following equation:

[0286] ;

[0287] in, , and Representing the A-axis coordinate system Relative to the machine tool coordinate system The rotation matrix and translation vector; , Representing the workpiece coordinate system around the A-axis coordinate system Z-axis rotation angle The rotation matrix, The translational component of rotational motion is ideally zero. , and Representing the workpiece coordinate system Relative to the A-axis coordinate system The rotation matrix and translation vector; , and Representing the tool coordinate system Relative to the workpiece coordinate system The rotation matrix and translation vector.

[0288] When the A-axis rotation angle is α, the pose of the tool center point (TCP) in the machine coordinate system is calculated in real time. The first pose expression encompasses the cascade operation of the A-axis rotation matrix Rot(Z,α) and the calibrated zero-position transformation matrix. The five-axis robot and the A-axis turntable coordinate their movements via G-code. The kinematic transformation chain unifies the coordinates of the grinding wheel at the end of the five-axis robot 11 with the coordinates of the workpiece 13 on the A-axis turntable 12, achieving precise spatial positioning and angle matching. The method proposed in this application eliminates machining contour errors caused by neglecting the coupling of the rotation center of the A-axis turntable 12 by establishing a complete kinematic transformation chain model that includes the dynamic rotation of the A-axis. It is particularly suitable for high-precision five-axis machining centers with a 5+1 structure.

[0289] The proposed method for automatically establishing the workpiece coordinate system of a five-axis robot establishes the workpiece coordinate system through steps S1 to S6 and integrates it into the motion transformation chain of the five-axis robot. This achieves an automated workpiece coordinate system establishment process, replacing manual teaching, significantly shortening calibration time, and forming a complete intelligent closed loop from data acquisition, processing, fusion to integration, greatly improving the automation level and reliability of the system. Specifically, in the process of establishing the workpiece coordinate system, an improved iterative nearest-point algorithm and a strategy of repeated touch and averaging are combined to achieve a dual improvement in calibration efficiency and accuracy, constructing a workpiece coordinate system with high numerical stability. By constructing an adaptive fusion decision model, the output of which is a homogeneous transformation matrix in visual mode, tactile mode, or fusion mode, the problems of low efficiency, poor accuracy, and weak anti-interference in traditional calibration are effectively solved. Applying the method proposed in this application, the calibration time can be shortened to 3-5 minutes, and the repeatability positioning accuracy can reach within ±0.02mm, significantly improving the intelligence level and operational accuracy of the grinding system in which the five-axis robot and the A-axis turntable work together.

[0290] The steps S1-S6 above do not necessarily represent the order of the steps, but are represented by step symbols. The order of the steps can be adjusted.

[0291] Further reference Figure 3 As an implementation of the methods shown in the above figures, this application provides an embodiment of a five-axis robot workpiece coordinate system automatic establishment device, which is similar to... Figure 1 Corresponding to the method embodiments shown, this device can be specifically applied to various electronic devices.

[0292] This application provides an automatic workpiece coordinate system establishment device for a five-axis robot, comprising:

[0293] Data acquisition module 1 is configured to acquire the original target point cloud of the workpiece to be calibrated, the template point cloud of the standard model, and the coordinates of at least three non-collinear feature points on the workpiece to be calibrated.

[0294] Pose calculation module 2 is configured to calculate the initial pose transformation matrix of the workpiece coordinate system relative to the robot base coordinate system based on the original target point cloud and the template point cloud using an improved iterative nearest point algorithm.

[0295] The first workpiece coordinate system construction module 3 is configured to calculate the rotation matrix and translation vector of the workpiece coordinate system based on the initial pose transformation matrix using the improved Gram-Schmidt orthogonalization method and the three-point method, so as to obtain the first workpiece coordinate system and its first homogeneous transformation matrix.

[0296] The second workpiece coordinate system construction module 4 is configured to use the improved Gram-Schmidt orthogonalization method and the three-point method to calculate the rotation matrix and translation vector of the workpiece coordinate system based on the coordinates of feature points, so as to obtain the second workpiece coordinate system and its second homogeneous transformation matrix.

[0297] The fusion decision module 5 is configured to construct a fusion decision model. The fusion decision model is used to calculate the confidence level of the first workpiece coordinate system and the confidence level of the second workpiece coordinate system respectively, and select the first homogeneous transformation matrix, the second homogeneous transformation matrix or calculate the fusion homogeneous transformation matrix as the homogeneous transformation matrix after decision based on the confidence level of the first workpiece coordinate system and the confidence level of the second workpiece coordinate system.

[0298] The coordinate system integration module 6 is configured to construct the kinematic transformation chain of the five-axis robot, integrating the post-decision homogeneous transformation matrix into the kinematic transformation chain to calculate the pose of the tool center point in the machine coordinate system.

[0299] Figure 4 This is a schematic diagram of the hardware structure of an electronic device provided in an embodiment of the present invention. For example... Figure 4 As shown, the electronic device in this embodiment includes a processor 401 and a memory 402; wherein the memory 402 is used to store computer execution instructions; and the processor 401 is used to execute the computer execution instructions stored in the memory to implement the various steps performed by the electronic device in the above embodiment. For details, please refer to the relevant descriptions in the foregoing method embodiments.

[0300] Alternatively, the memory 402 can be either standalone or integrated with the processor 401.

[0301] When the memory 402 is set up independently, the electronic device also includes a bus 403 for connecting the memory 402 and the processor 401.

[0302] This invention also provides a computer storage medium storing computer execution instructions, which, when executed by processor 401, implement the above method.

[0303] This invention also provides a computer program product, including a computer program, which, when executed by a processor 401, implements the above-described method.

[0304] In the embodiments provided by this invention, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative; for instance, the division of modules is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple modules may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be indirect coupling or communication connection through some interfaces, devices, or modules, and may be electrical, mechanical, or other forms.

[0305] The modules described as separate components may or may not be physically separate. The components shown as modules may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to implement the solution of this embodiment according to actual needs.

[0306] Furthermore, the functional modules in the various embodiments of the present invention can be integrated into one processing unit, or each module can exist physically separately, or two or more modules can be integrated into one unit. The unit composed of the above modules can be implemented in hardware or in the form of hardware plus software functional units.

[0307] The integrated modules implemented as software functional modules described above can be stored in a computer-readable storage medium. These software functional modules, stored in a storage medium, include several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) or processor 401 to execute some steps of the methods of the various embodiments of this application.

[0308] It should be understood that the processor 401 described above can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), etc. The general-purpose processor can be a microprocessor, or the processor 401 can be any conventional processor 401. The steps of the method disclosed in this invention can be directly manifested as the hardware processor 401 executing the steps, or as a combination of hardware and software modules within the processor 401 executing the steps.

[0309] The memory 402 may include high-speed RAM memory, and may also include non-volatile memory (NVM), such as at least one disk storage device, and may also be a USB flash drive, portable hard drive, read-only memory, disk or optical disc, etc.

[0310] Bus 403 can be an Industry Standard Architecture (ISA), a Peripheral Component Interconnect (PCI) bus, or an Extended Industry Standard Architecture (EISA) bus, etc. Bus 403 can be divided into address bus, data bus, control bus, etc. For ease of illustration, the bus 403 in the accompanying drawings of this application is not limited to only one bus 403 or one type of bus 403.

[0311] The aforementioned storage medium can be implemented from any type of volatile or non-volatile storage device or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic storage, flash memory, magnetic disk, or optical disk. The storage medium can be any available medium accessible to general-purpose or special-purpose computers.

[0312] An exemplary storage medium is coupled to processor 401, enabling processor 401 to read information from and write information to the storage medium. Alternatively, the storage medium can be an integral part of processor 401. Processor 401 and storage medium can reside in application-specific integrated circuits (ASICs). Alternatively, processor 401 and storage medium can exist as discrete components in an electronic device or host device.

[0313] Those skilled in the art will understand that all or part of the steps of the above-described method embodiments can be implemented by hardware related to program instructions. The aforementioned program can be stored in a computer-readable storage medium. When executed, the program performs the steps of the above-described method embodiments; and the aforementioned storage medium includes various media capable of storing program code, such as ROM, RAM, magnetic disks, or optical disks.

[0314] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for automatically establishing a workpiece coordinate system for a five-axis robot, characterized in that, Includes the following steps: Obtain the original target point cloud of the workpiece to be calibrated, the template point cloud of the standard model, and the coordinates of at least three non-collinear feature points on the workpiece to be calibrated. Based on the original target point cloud and the template point cloud, an improved iterative nearest point algorithm is used to calculate the initial pose transformation matrix of the workpiece coordinate system relative to the robot base coordinate system. Based on the initial pose transformation matrix, the rotation matrix and translation vector of the workpiece coordinate system are calculated using the improved Gram-Schmidt orthogonalization method and the three-point method, to obtain the first workpiece coordinate system and its first homogeneous transformation matrix. Based on the coordinates of the feature points, the rotation matrix and translation vector of the workpiece coordinate system are calculated using the improved Gram-Schmidt orthogonalization method and the three-point method, to obtain the second workpiece coordinate system and its second homogeneous transformation matrix. A fusion decision model is constructed, which is used to calculate the confidence level of the first workpiece coordinate system and the confidence level of the second workpiece coordinate system respectively, and select a first homogeneous transformation matrix, a second homogeneous transformation matrix, or calculate a fusion homogeneous transformation matrix as the homogeneous transformation matrix after decision based on the confidence level of the first workpiece coordinate system and the confidence level of the second workpiece coordinate system. A kinematic transformation chain for a five-axis robot is constructed, and the post-decision homogeneous transformation matrix is ​​integrated into the kinematic transformation chain to calculate the pose of the tool center point in the machine tool coordinate system.

2. The method for automatically establishing the workpiece coordinate system of a five-axis robot according to claim 1, characterized in that, The calculation of the initial pose transformation matrix of the workpiece coordinate system relative to the robot base coordinate system using an improved iterative nearest-point algorithm based on the original target point cloud and the template point cloud specifically includes: The original target point cloud is de-denoised by using a statistical outlier removal algorithm. The covariance matrix of the denoised target point cloud is calculated using principal component analysis, and the covariance matrix is ​​decomposed into eigenvalues ​​to obtain the initial rotation matrix. Define a registration error objective function to find the optimal rotation matrix and translation vector, as shown in the following equation: ； in, Let represent the objective function for registration error. Let represent the rotation matrix, whose initial value is the initial rotation matrix, and t represent the translation vector. This represents any point in the template point cloud. yes The corresponding point in the denoised target point cloud; is the weighting coefficient, with a value of 0 or 1; N represents the number of points in the target point cloud after denoising; Using KD-Tree to find the nearest neighbor, specifically including: for a point ,exist Search for nearest neighbor ,in, Represents a template point cloud. This represents the denoised target point cloud. This indicates the search for a function that makes The point q that takes the minimum value, Represents the Euclidean norm; Introduce a normal vector consistency constraint to calculate the angle between the normal vectors of point pairs. ,in, Point The normal vector, Point The normal vector, Point normal vector and point The angle between the normal vectors; Traverse the template point cloud and the denoised target point cloud, if , Let the threshold of the angle between the normal vectors be denoted as . Remove point pairs and The template point cloud and the target point cloud after removal are obtained. A decentralized covariance matrix is ​​constructed based on the removed template point cloud and the removed target point cloud, as shown in the following equation: ； Where H represents the decentralized covariance matrix; , , This represents the template point cloud after culling, where M represents the number of points in the template point cloud after culling. This represents the geometric centroid of the template point cloud after removal. Point Decentralized coordinates; , , This represents the target point cloud after removal. This represents the geometric centroid of the target point cloud after removal. Point The decentralized coordinates, where T represents the transpose; For the decentralized covariance matrix Perform singular value decomposition The rotation matrix is ​​solved as shown in the following equation: ； Where U and V represent the left diagonal matrix and the right diagonal matrix, respectively. It is a diagonal matrix composed of singular values; like Then correct The third column of symbols yields a new rotation matrix, as shown in the following equation: ； in, Represents the determinant of a matrix; The translation vector is calculated based on the rotation matrix, as shown in the following formula: ； The root mean square error of the current iteration is calculated as follows: ； Wherein, RMSE represents the root mean square error; Determine whether the root mean square error of the current iteration has converged or the number of iterations is the maximum number of iterations. If the root mean square error of the current iteration has converged or the number of iterations is the maximum number of iterations, then use the rotation matrix and translation vector calculated in the current iteration as the optimal rotation matrix and translation vector; otherwise, return to calculate the registration error objective function and continue iterating until the root mean square error of the current iteration converges or the number of iterations is the maximum number of iterations. The initial pose transformation matrix of the workpiece to be calibrated relative to the robot's base coordinate system is generated based on the optimal rotation matrix and translation vector, as shown in the following equation: ； in, This represents the initial pose transformation matrix of the workpiece to be calibrated relative to the robot's base coordinate system.

3. The method for automatically establishing the workpiece coordinate system of a five-axis robot according to claim 2, characterized in that, The method of calculating the rotation matrix and translation vector of the workpiece coordinate system using the improved Gram-Schmidt orthogonalization method and the three-point method specifically includes: Three target points are obtained, and the three target points are respectively represented as follows: , and ; The unit vector of the X-axis in the workpiece coordinate system is calculated as follows: ； ； in, Indicates the target point With the target point The direction vector of the connecting line, The unit vector representing the X-axis of the workpiece coordinate system; The temporary vector is calculated as follows: ； in, Indicates the target point With the target point The direction vector of the connecting line is a temporary vector; The Y-axis components of the workpiece coordinate system are calculated using the improved Gram-Schmidt formula and then normalized to obtain the unit vector of the Y-axis of the workpiece coordinate system, as shown in the following equation: ； ； in, Represents the Y-axis component of the workpiece coordinate system; The unit vector representing the Y-axis of the workpiece coordinate system; Execution direction verification, if Then let ; The Z-axis unit vector of the workpiece coordinate system is calculated using the cross product, as shown in the following formula: ； ； in, Represents the Z-axis component of the workpiece coordinate system; The unit vector representing the Z-axis of the workpiece coordinate system; According to the target point The workpiece coordinate system is constructed using the X-axis unit vector, Y-axis unit vector, and Z-axis unit vector. The rotation matrix, translation vector, and homogeneous transformation matrix of the workpiece coordinate system are shown in the following equations: ； ； ； in, Represents the homogeneous transformation matrix; The coordinates are represented as , The coordinates are represented as , The coordinates are represented as , The coordinates are represented as ; The determinant of the rotation matrix is ​​calculated as follows: ； like and ,in If a preset tolerance value is used, the workpiece coordinate system is determined to be a left-handed coordinate system, and a correction operation is performed, as shown in the following formula: ； Construct a new rotation matrix and homogeneous transformation matrix ; The step of calculating the rotation matrix and translation vector of the workpiece coordinate system based on the initial pose transformation matrix using the improved Gram-Schmidt orthogonalization method and the three-point method to obtain the first workpiece coordinate system and its first homogeneous transformation matrix specifically includes: Obtain three reference points on the template point cloud. , and The three reference points are mapped to the actual space of the workpiece to be calibrated according to the initial pose transformation matrix, and the coordinates of the three first target points are obtained, as shown in the following formula: ； ； ； in, , and These represent three reference points. , and The three corresponding first target points after mapping Represents the initial pose transformation matrix The inverse matrix; Substituting the coordinates of the three first target points into the step of calculating the rotation matrix and translation vector of the workpiece coordinate system using the improved Gram-Schmidt orthogonalization method and the three-point method, the first workpiece coordinate system and the first homogeneous transformation matrix are obtained. The first homogeneous transformation matrix is ​​expressed as follows: ; The coordinates based on the feature points are used to calculate the rotation matrix and translation vector of the workpiece coordinate system using an improved Gram-Schmidt orthogonalization method and a three-point method, resulting in a second workpiece coordinate system and its second homogeneous transformation matrix, specifically including: Three non-collinear feature points on the workpiece to be calibrated are used as the second target points. Substituting the coordinates of the three second target points into the step of calculating the rotation matrix and translation vector of the workpiece coordinate system using the improved Gram-Schmidt orthogonalization method and the three-point method, the second workpiece coordinate system and the second homogeneous transformation matrix are obtained. The second homogeneous transformation matrix is ​​expressed as follows: .

4. The method for automatically establishing the workpiece coordinate system of a five-axis robot according to claim 1, characterized in that, The process of obtaining the coordinates of at least three non-collinear feature points on the workpiece to be calibrated specifically includes: The robot's end effector sequentially touches at least three pre-set non-collinear feature points on the workpiece to be calibrated; For each feature point, perform n repeated touches, and record the pose of the robot flange in the robot base coordinate system and the transformation of the tool coordinate system relative to the robot flange at the u-th touch. The coordinates of the feature points are calculated as follows: ； in, Representing feature points The coordinates at the u-th touch; f represents the index of the feature point, and the number of indices is at least 3; This represents the change in the robot flange pose during the u-th touch. This indicates the transformation of the pre-calibrated tool coordinate system relative to the robot flange; Calculate feature points after n touches The feature point is obtained by averaging the coordinates of the coordinates. The average coordinates are shown in the following formula: ； in, Representing feature points The average coordinates.

5. The method for automatically establishing the workpiece coordinate system of a five-axis robot according to claim 1, characterized in that, The processing steps of the fusion decision model specifically include: The confidence level of the first workpiece coordinate system is calculated as follows: ； in, This represents the confidence level of the first workpiece coordinate system. , and All represent weights. , This represents the maximum value of the root mean square error during the iteration process. To score the quality of the point cloud, Rate the quality of light; The confidence level of the second workpiece coordinate system is calculated as follows: ； in, This represents the confidence level of the second workpiece coordinate system. The robot positioning accuracy coefficient. For feature points The standard deviation of the coordinates; and All represent weights. ; Set decision threshold Based on the confidence levels of the first workpiece coordinate system and the second workpiece coordinate system, a first homogeneous transformation matrix, a second homogeneous transformation matrix, or a fused homogeneous transformation matrix is ​​selected. The decision conditions are as follows: ； Wherein, Mode represents the data acquisition mode, Visual represents the visual mode, the workpiece coordinate system constructed based on the data acquired in the visual mode is the first workpiece coordinate system, Touch represents the tactile mode, the workpiece coordinate system constructed based on the data acquired in the tactile mode is the second workpiece coordinate system, and Fusion represents the fusion mode. If the visual mode is selected, the first homogeneous transformation matrix corresponding to the first workpiece coordinate system is used as the homogeneous transformation matrix after the decision. If the haptic mode is selected, the second homogeneous transformation matrix corresponding to the second workpiece coordinate system will be used as the homogeneous transformation matrix after the decision. If the fusion mode is selected, the Kalman filter algorithm is used to calculate the fusion homogeneous transformation matrix, and this fusion homogeneous transformation matrix is ​​used as the post-decision homogeneous transformation matrix. The calculation of the fusion homogeneous transformation matrix using the Kalman filter algorithm specifically includes: The state is estimated by performing a homogeneous transformation matrix on the workpiece coordinate system, and the state vector is shown in the following equation: ； in, Represents the state vector. This indicates the position of the origin of the workpiece coordinate system in the machine tool coordinate system and the rotation angle around the coordinate axes of the machine tool coordinate system during the k-th measurement. The prediction steps are as follows: ； ； in, This represents the prior state estimate of the homogeneous transformation matrix of the current workpiece coordinate system. This represents the state estimate of the homogeneous transformation matrix of the previous workpiece coordinate system. The state prediction error covariance matrix represents the homogeneous transformation matrix of the current workpiece coordinate system. Let Q represent the state estimation error covariance matrix of the homogeneous transformation matrix of the previous workpiece coordinate system, and let Q represent the process noise covariance matrix. The update steps are as follows: ； ； ； in, Indicates Kalman gain, Represents the transpose of the observation matrix. To observe the noise covariance matrix, This represents the state estimate of the homogeneous transformation matrix of the current workpiece coordinate system. These are the observed values ​​of the homogeneous transformation matrix of the current workpiece coordinate system. It is the state estimation error covariance matrix of the homogeneous transformation matrix of the current workpiece coordinate system after measurement and update, where I represents the identity matrix, and is the final output. The corresponding homogeneous transformation matrix is ​​used as the fused homogeneous transformation matrix. .

6. The method for automatically establishing the workpiece coordinate system of a five-axis robot according to claim 5, characterized in that, The construction of the kinematic transformation chain for the five-axis robot integrates the post-decision homogeneous transformation matrix into the kinematic transformation chain to calculate the pose of the tool center point in the machine tool coordinate system, specifically including: The kinematic transformation chain includes, in sequence, the machine tool coordinate system. A-axis coordinate system Workpiece coordinate system and tool coordinate system ; The pose of the tool center point in the machine coordinate system is calculated based on the kinematic transformation chain, and the first pose expression is obtained as follows: ； in, This indicates the pose of the tool center point in the machine tool coordinate system. This represents the fixed transformation matrix from the A-axis coordinate system to the machine tool coordinate system. The transformation matrix represents the rotation angle α of the A-axis around the Z-axis of the A-axis coordinate system; This represents the transformation matrix of the workpiece coordinate system relative to the A-axis coordinate system when the A-axis is at zero position. , Represents the homogeneous transformation matrix after the decision; The transformation matrix represents the tool coordinate system relative to the workpiece coordinate system; the expression for the transformation matrix of the rotation of the A-axis about the Z-axis of the workpiece coordinate system is shown below: 。 7. An automatic workpiece coordinate system establishment device for a five-axis robot, characterized in that, include: The data acquisition module is configured to acquire the original target point cloud of the workpiece to be calibrated, the template point cloud of the standard model, and the coordinates of at least three non-collinear feature points on the workpiece to be calibrated. The pose calculation module is configured to calculate the initial pose transformation matrix of the workpiece coordinate system relative to the robot base coordinate system based on the original target point cloud and the template point cloud using an improved iterative nearest point algorithm. The first workpiece coordinate system construction module is configured to calculate the rotation matrix and translation vector of the workpiece coordinate system based on the initial pose transformation matrix using the improved Gram-Schmidt orthogonalization method and the three-point method, so as to obtain the first workpiece coordinate system and its first homogeneous transformation matrix. The second workpiece coordinate system construction module is configured to calculate the rotation matrix and translation vector of the workpiece coordinate system based on the coordinates of the feature points using the improved Gram-Schmidt orthogonalization method and the three-point method, thereby obtaining the second workpiece coordinate system and its second homogeneous transformation matrix. The fusion decision module is configured to construct a fusion decision model, which is used to calculate the confidence level of the first workpiece coordinate system and the confidence level of the second workpiece coordinate system respectively, and select a first homogeneous transformation matrix, a second homogeneous transformation matrix, or calculate a fusion homogeneous transformation matrix as the homogeneous transformation matrix after decision based on the confidence level of the first workpiece coordinate system and the confidence level of the second workpiece coordinate system. The coordinate system integration module is configured to construct the kinematic transformation chain of the five-axis robot, and integrate the post-decision homogeneous transformation matrix into the kinematic transformation chain to calculate the pose of the tool center point in the machine coordinate system.

8. An electronic device, characterized in that, include: One or more processors; Memory, used to store one or more programs. When the one or more programs are executed by the one or more processors, the one or more processors implement the method as described in any one of claims 1-6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the method as described in any one of claims 1-6.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by a processor, it implements the method as described in any one of claims 1-6.

Images