A method for zero-position fitting and kinematic modeling of mechanism axes integrating measurement point position uncertainty

Abstract

The present invention discloses a method for zero-position fitting and kinematic modeling of mechanism axis integrating measurement point uncertainty, including measuring the coordinates of target points at multiple positions when each axis of the mechanism moves; calculating the length of the semi-major axis, moment of inertia and product of inertia of the point uncertainty ellipsoid; integrating the uncertainty ellipsoid to fit the direction vector of the linear axis of the mechanism in the zero-position state; integrating the uncertainty ellipsoid to fit the plane where the point rotation trajectory is located, measuring the projection of the point onto the plane, minimizing the sum of the squares of the difference between the square of the distance from each projection point to the center of the circle and the square of the radius of the arc, fitting to obtain the position and posture of the rotating axis in the zero-position state of the mechanism; the fourth-order homogeneous transformation matrix corresponding to the movement of each axis is combined with the composition form of the mechanism to establish the conversion matrix from the end coordinate system of the mechanism to its zero-position reference position and posture. It can effectively avoid the convergence problem of parameter identification in the traditional method when the kinematic parameter error is large, so that the kinematic calibration method is applicable to the situation where the position and posture of the mechanism axis are arbitrary.

Description

Technical Field

[0001] The invention belongs to the technical field of mechanism kinematic modeling, and in particular relates to a mechanism axis zero position fitting and kinematic modeling method integrating measurement point position uncertainty. Background Art

[0002] In recent years, with the development of digital measurement and control technology in the field of mechanical manufacturing, a large number of intelligent equipment such as CNC machine tools, industrial robots, and parallel posture adjustment mechanisms have been widely used in the processing and assembly of products. The spatial motion and positioning of the equipment's end-effector is based on the kinematic model of the mechanism that characterizes the mapping relationship between the displacement of each drive axis and the end-position. Its accuracy is an important guarantee for product manufacturing quality. The actual kinematic model of the equipment often deviates from the ideal design due to errors in component manufacturing and assembly. It is necessary to calibrate the kinematic model using measured data to improve the accuracy of the equipment's end-motion trajectory and positioning.

[0003] The traditional kinematic modeling and calibration method takes the DH parameters of each motion axis of the mechanism as the core, and identifies the parameter error through iterative identification of the measured terminal posture error based on the linear differential transformation relationship between the terminal posture and the DH parameters, thereby realizing the correction of the kinematic model and compensation of the terminal positioning accuracy. In this process, the differential relationship between the terminal posture and the mechanism parameters is an approximate linear relationship. When the actual value of the parameter deviates greatly from the nominal value, the complex nonlinear relationship between the terminal posture error and the mechanism parameter error will make it impossible for the iterative identification algorithm to converge from the nominal value of the parameter to the true value. At the same time, the process of establishing the kinematic model with DH parameters as the core is cumbersome, and it is necessary to establish a local coordinate system on each axis, and it is not applicable to the situation where the direction and position of each axis are arbitrary. In recent years, some scholars have established a fitting model for the terminal positioning error of the mechanism as it changes with spatial position based on assumptions such as spatial similarity. This model completely abandons the geometric relationship between the motion axes of the mechanism and treats the mapping relationship between the terminal posture error and the displacement of each drive axis as a "black box". Although this method can accurately predict the terminal error based on the displacement of the drive axis within a certain range, it cannot guarantee accurate prediction of the error in the entire working space of the mechanism due to the lack of reliable mechanism support. Summary of the invention

[0004] The technical problem to be solved by the present invention is to provide a method for zero-position fitting and kinematic modeling of mechanism axes that integrates the uncertainty of measurement points in order to address the deficiencies of the above-mentioned prior art. The target point of the laser tracker is fixed at any position at the end of the motion mechanism. When the displacement of other axes is in the zero position state, each linear axis and rotating axis of the mechanism is driven separately in turn to obtain the coordinates of the end target at multiple points when each axis moves separately. For each measurement point, an uncertainty ellipsoid model centered on it is constructed, and relevant parameters are calculated. For the linear axis, the direction vector of the linear axis is fitted with the goal of minimizing the sum of the squared mean values ​​of the distances from each point in the uncertainty ellipsoid to the straight line. For the rotating axis, a circular plane is first fitted with the goal of minimizing the sum of the squared mean values ​​of the distances from each point in the uncertainty ellipsoid to the plane, and then each measurement point is projected onto the plane. The center of the rotating trajectory is fitted in the plane with the goal of minimizing the sum of the squares of the distance from each projection point to the center of the circle and the square of the radius of gyration. The normal vector of the plane is the direction vector of the rotation axis, and the center of the trajectory represents the position of the axis on the axis. The homogeneous transformation matrix of each translational axis and rotational axis motion is constructed using the posture of each axis in the zero-position state. The matrices are multiplied in sequence according to the composition of the mechanism to finally obtain the representation of the end posture of the mechanism in the mechanism base coordinate system, and the kinematic modeling of the mechanism is completed. This can avoid the convergence problem of the traditional error identification strategy when the actual kinematic parameters deviate greatly from the nominal values, so that the kinematic model is suitable for the situation where the posture of each driving axis of the mechanism is arbitrary in actual situations. At the same time, it has a reliable geometric mechanism, which can realize the precise movement and positioning of the end execution unit of intelligent equipment in mechanical manufacturing scenarios with high precision and high quality requirements.

[0005] In order to achieve the above technical objectives, the technical solution adopted by the present invention is:

[0006] A method for zero-position fitting and kinematic modeling of mechanism axes integrating measurement point position uncertainty, comprising:

[0007] S1: Taking any position at the end of the mechanism as the target point, a laser tracker is used to measure the coordinates of the target point at multiple positions when each axis of the mechanism moves independently;

[0008] S2: Using the data measured in S1 and the conversion between the measurement coordinate system and the mechanism base coordinate system, the conversion between the local coordinate system representing the uncertainty ellipsoid posture and the mechanism base coordinate system is performed to calculate the length of the semi-major axis of the ellipsoid, the moment of inertia, the product of inertia and the coordinate integration results;

[0009] S3: With the goal of minimizing the sum of the mean squares of the distances from each point in the ellipsoid domain to the straight line, the direction vector of the straight axis in the zero position state of the mechanism is obtained by fitting using the semi-major axis length, moment of inertia and product of inertia results of the ellipsoid calculated in S2;

[0010] S4: With the goal of minimizing the sum of the mean squares of the distances from each point to the plane in the ellipsoid domain, the plane where the point rotation trajectory is located is fitted using the length of the semi-major axis, moment of inertia, product of inertia and coordinate integration results calculated by S2, and the position and posture of the rotation axis in the zero position state of the mechanism is fitted by minimizing the sum of the squares of the projection of the measured point to the plane and the difference between the square of the distance from each projection point to the center of the circle and the square of the radius of the arc;

[0011] S5: Based on the direction vector of the linear axis and the posture of the rotation axis in the zero-position state of the mechanism, the coordinate transformation of the point along any direction in space and the rotation around any axis is performed to obtain the fourth-order homogeneous transformation matrix corresponding to the motion of the rotation axis and the linear axis. Combined with the composition of the mechanism, the transformation matrix from the end coordinate system of the mechanism to its zero-position reference posture is established to complete the establishment of the kinematic model.

[0012] To optimize the above technical solutions, the specific measures taken also include:

[0013] The above-mentioned S1 includes: fixing a target point at any position at the end of the mechanism, driving each axis of the mechanism to move separately in turn, when each axis moves, the other axes are in a zero position state, and using a laser tracker to collect the coordinates of the target point at multiple measurement positions when each axis moves separately, wherein at least two measurement point coordinate data are collected when the mechanism moves along the linear axis, and at least three measurement point coordinate data are collected when the mechanism rotates around the axis.

[0014] In the above S2, the transformation relationship from the laser tracker measurement coordinate system to the mechanism base coordinate system is calibrated based on the multi-point registration algorithm, and all target points are transformed from the measurement coordinate system to the base coordinate system to obtain the measurement coordinates of the target points at multiple positions at the end of the mechanism in the base coordinate system. The error model of the tracker spherical coordinate measurement system is used to calculate the three semi-major axis lengths of the uncertainty ellipsoid of each point and the transformation matrix from the local coordinate system to the base coordinate system that characterizes the ellipsoid's position and posture, and further calculate the moment of inertia and product of inertia of each ellipsoid relative to the coordinate plane of the base coordinate system.

[0015] The calculation formulas for the above three semi-axis lengths are:

[0016]

[0017] Where U ui ,U vi ,U wi are the semi-axis lengths along the three coordinate axes of the local coordinate system;

[0018] σ r ,σ α and σ β are the distance measurement variance and angle measurement variance of the tracker, respectively, which are the inherent properties of the instrument;

[0019] ri and β i They are respectively the distance from the target point to the origin of the measurement coordinate system and the z-axis angle obtained directly by the tracker.

[0020] The calculation formula of the transformation matrix from the local coordinate system to the base coordinate system is:

[0021]

[0022] Where x mi ,y mi ,z mi It is the three-axis coordinate of the target point at the end of the mechanism measured at the i-th measurement point in the laser tracker measurement coordinate system;

[0023] It is the fourth-order homogeneous transformation matrix from the measurement coordinate system to the mechanism base coordinate system, which is obtained by calculating the measurement coordinates of multiple reference points and their given reference coordinates in the base coordinate system through a multi-point registration algorithm;

[0024] is the transformation matrix from the local coordinate system of the ellipsoid of the i-th measurement point to the measurement coordinate system.

[0025] The above transformation matrix from the local coordinate system to the base coordinate system The moment of inertia, product of inertia and coordinate integral of the uncertainty ellipsoid of the i-th measurement point relative to the coordinate plane of the base coordinate system are obtained as follows:

[0026]

[0027]

[0028] In the formula, I (i)xx ,I (i)yy ,I (i)zz are the moments of inertia of the ellipsoid about the coordinate planes yoz, xoz and xoy of the base coordinate system, respectively, (i)xy ,I (i)xz ,I (i)yz is the product of inertia of the ellipsoid with respect to the coordinate planes yoz, xoz and xoy of the base coordinate system, They are the coordinate integrals of the ellipsoid relative to the base coordinate system. They are used to calculate the triple integral of the square of the distance from the point to the line and the plane when fitting the linear axis and the rotation axis in S3 and S4. li and I pi ; U ui ,U vi ,U wi are the semi-axis lengths of the ellipsoid of the i-th measuring point along the three coordinate axes of the local coordinate system; r i11 ~r i33 is the transformation matrix The rotation matrix element in , t i1 ~t i3 is the translation vector element, namely:

[0029]

[0030] The above S3 includes:

[0031] The mean square of the distance from a point to a straight line in the ellipsoid domain is represented as the ratio of the triple integral of the square of the distance from the point to the straight line on the ellipsoid domain to the volume of the ellipsoid domain, and its expression is:

[0032]

[0033] Where U ui ,U vi ,U wi are the semi-axis lengths of the uncertain ellipsoid of the i-th measuring point along the three coordinate axes of the local coordinate system;

[0034] p l =[x l ,y l ,z l ] T It is the position vector of the foot of the perpendicular from the origin of the base coordinate system to the straight line in the base coordinate system;

[0035] s l =[a l ,b l ,c l ] T is the unit vector along the straight line in the base coordinate system;

[0036] Ω i represents the uncertainty ellipsoid domain of the i-th measurement point;

[0037] p(x,y,z) is the position vector of any point in the ellipsoid domain in the base coordinate system;

[0038] Will I li The expression is expanded and sorted:

[0039]

[0040]

[0041] Where k 1i ~k 6i is a fixed parameter calculated based on the ellipsoidal moment of inertia and product of inertia of S2; x ci ,y ci ,z ci is the three-axis coordinate of the i-th measurement point in the base coordinate system, according to the fourth-order homogeneous transformation matrix from the measurement coordinate system of S2 to the mechanism base coordinate systemb m T to x mi ,y mi ,z mi Transform to get, x mi ,y mi ,z mi It is the three-axis coordinate of the target point at the end of the mechanism measured at the i-th measurement point in the laser tracker measurement coordinate system;

[0042] Then the objective function with the goal of minimizing the sum of the mean squares of the distances from each point to the straight line in the ellipsoid domain is obtained as follows:

[0043]

[0044]

[0045] The Lagrange multiplier method is used to transform the above objective function and construct the Lagrange function L(a l ,b l ,c l ,x l ,y l ,z l ,λ1,λ2):

[0046]

[0047] Let the Lagrangian function be l ,b l ,c l ,x l ,y l ,z l , the first-order partial derivative of λ1,λ2 is equal to zero:

[0048]

[0049] Use Newton's iteration method to numerically solve the above equation:

[0050]

[0051] Then when the parameter x of the kth step is given (k) , the parameter x to be solved in step k+1 (k+1) for:

[0052]

[0053] When ‖F(x)‖ is less than a given threshold, the iteration is stopped, thereby obtaining an approximate solution to the system of equations and finding the approximate stationary point of the Lagrangian function L;

[0054] By comparing the size of the original objective function corresponding to multiple sets of solutions, the final optimization parameters are selected to obtain the direction vector s of the linear axis in the zero position state of the mechanism. l =[a l ,b l ,c l ] T and the foot of the perpendicular from the origin to the line p l =[x l ,y l ,z l ] T , will s l =[a l ,b l ,c l ] T Input as S5.

[0055] In the above S4, the mean square of the distance from the point to the plane in the ellipsoid domain is represented as the ratio of the triple integral of the square of the distance from the point to the plane on the ellipsoid domain to the volume of the ellipsoid domain, and its expression is:

[0056]

[0057] Where n=[n1,n2,n3] T is the unit direction vector of the plane to be fitted, and d is the parameter representing the position of the plane;

[0058] Will I pi The expression is expanded and sorted:

[0059]

[0060]

[0061] Where k 1i ~k 6i is a fixed parameter calculated based on the ellipsoidal moment of inertia and product of inertia of S2, k 7i ~k 10i It is a fixed parameter calculated by integrating the coordinates of the ellipsoid of S2 relative to the base coordinate system;

[0062] Then, the fitting objective function is obtained, which aims to minimize the sum of the square mean of the distance from each point to the plane in the ellipsoid domain:

[0063]

[0064]

[0065] Solve the above fitting objective function to obtain the fitting plane n1x+n2y+n3z+d=0. Project the original coordinates of multiple measurement points onto this plane. For the coordinates of the i-th measurement point (x ci,y ci ,z ci ), the coordinates of its projection point in the plane n1x+n2y+n3z+d=0 for:

[0066]

[0067] The optimization goal is to minimize the sum of the squares of the differences between the squares of the distances from each projection point to the center of the circle and the square of the arc radius, and fit the center position vector p of the rotation trajectory arc in the plane. r =[x r ,y r ,z r ] T :

[0068]

[0069] The above formula is about x r ,y r ,z r The analytical solution is:

[0070]

[0071]

[0072]

[0073] When the mechanism is in the zero position, the unit direction vector n of each rotation axis in the base coordinate system is [n1, n2, n3] T and the center position vector p of the arc of the rotating trajectory r =[x r ,y r ,z r ] T As the position and posture of the rotation axis in the zero position state of the mechanism, input S5.

[0074] In the above S5, the fourth-order homogeneous transformation matrices corresponding to the linear axis and the rotation axis motion are:

[0075]

[0076]

[0077] Where t is the linear displacement of the linear axis relative to the zero position, and θ is the angular displacement of the rotation axis relative to the zero position;

[0078] (a l ,b l ,c l ) is the unit direction vector coordinate of the linear axis in the base coordinate system when the mechanism is in the zero position state;

[0079] (n1,n2,n3), (x r ,y r ,z r ) are the unit direction vectors of each rotation axis and the center position vector coordinates of the rotation trajectory in the base coordinate system when the mechanism is in the zero position state.

[0080] In the above S5, the transformation matrix of the terminal coordinate system relative to its zero reference is It is obtained by multiplying the fourth-order homogeneous transformation matrices corresponding to the motion of the rotary axis and the linear axis in sequence according to the composition of the mechanism.

[0081] The present invention has the following beneficial effects:

[0082] The present invention abandons the traditional theoretical model of mechanism kinematics → error estimation → position compensation precision improvement strategy, and uses multiple measured spatial point data to directly fit the zero-position posture of each linear axis and rotary axis of the motion mechanism, effectively avoiding the problem that when the nominal parameters deviate greatly from the actual parameters in the traditional calibration process, the complex nonlinear kinematic relationship of the mechanism makes it impossible for the parameter identification algorithm to converge from the nominal value to the true value, making the kinematic calibration / modeling method applicable to the situation where the spatial posture of the mechanism axis is arbitrary. When fitting the axis posture, the present invention incorporates the uncertainty ellipsoid of the point measurement into the optimization process, which effectively improves the fitting accuracy of the axis under the random noise of the measurement point compared to the least squares method based on single-point fitting.

[0083] The mechanism motion axis calibration and modeling method proposed in the present invention can be used for high-precision motion control of CNC machine tools, industrial robots, and flexible attitude adjustment mechanisms. The spatial straight line and circle fitting optimization strategy integrating the point uncertainty ellipsoid also has broad potential application value for measurement scenarios based on spherical coordinate systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] Figure 1 It is a schematic diagram of the process of the present invention;

[0085] Figure 2 It is a geometrical schematic diagram of the ellipsoid local coordinate system, the measurement coordinate system and the base coordinate system in the present invention;

[0086] Figure 3 It is a schematic diagram of the linear axis fitting of the fused uncertainty ellipsoid;

[0087] Figure 4 It is a schematic diagram of the rotation axis fitting of the fused uncertainty ellipsoid;

[0088] Figure 5 The PPP-RRR six-axis serial mechanism and the corresponding kinematic model;

[0089] Figure 6 To integrate the simulation effect of spatial straight line and circle fitting algorithm of uncertain ellipsoid;

[0090] Figure 7 Error comparison between the fitting algorithm for the fused uncertain ellipsoid and the fitting algorithm based on multi-point least squares distance. DETAILED DESCRIPTION

[0091] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0092] Although the steps in the present invention are arranged with numbers, they are not used to limit the order of the steps. Unless the order of the steps is clearly stated or the execution of a certain step requires other steps as a basis, the relative order of the steps can be adjusted. It is understood that the term "and / or" used in this article involves and covers any and all possible combinations of one or more of the associated listed items.

[0093] The present invention provides a mechanism axis zero-position fitting and kinematic modeling method that integrates the uncertainty of the measurement point position, improves the positioning accuracy of the motion mechanism, avoids the nonlinear convergence problem of the parameter identification algorithm under large errors in the traditional accuracy compensation method, and makes the kinematic model applicable to any axis in space. Figure 1 As shown, the method of the present invention comprises:

[0094] S1: Taking any position at the end of the mechanism as the target point, a laser tracker is used to measure the coordinates of the target point at multiple positions when each axis of the mechanism moves independently, to achieve point data collection:

[0095] A target point is fixed at any position at the end of the mechanism, and each axis of the mechanism is driven to move in turn. When each axis moves, the other axes are in the zero position state. A laser tracker is used to collect the coordinates of the target point at multiple positions when each axis moves independently. At least two points need to be collected when the mechanism moves along the linear axis, and at least three points need to be collected when it rotates around the axis.

[0096] S2: Construct an uncertainty ellipsoid model centered on each measurement point and calculate related parameters;

[0097] Based on the multi-point registration algorithm, the conversion relationship from the laser tracker measurement coordinate system to the mechanism base coordinate system is calibrated, and all target points are converted from the measurement coordinate system to the base coordinate system to realize the construction of the measurement point uncertainty ellipsoid model. Using the error model of the tracker spherical coordinate measurement system, the three semi-major axis lengths of each point uncertainty ellipsoid and the conversion matrix from the local coordinate system to the base coordinate system that characterizes the ellipsoid posture are calculated, and the inertia moment, inertia product and coordinate integral of each ellipsoid relative to the coordinate plane of the base coordinate system are further calculated. Specifically:

[0098] 1. Definition of the local coordinate system of the ellipsoid and calculation of the semi-major axis

[0099] like Figure 2 As shown, the i-th measurement point p mi The local coordinate system of the uncertainty ellipsoid is defined as {u i ,v i ,w i}, whose origin is the measurement point and the center of the ellipsoid. i It is along p mi The unit vector in the direction of the position vector in the measurement coordinate system, v i Located at i In the plane formed by the z-axis of the measurement coordinate system and w i Orthogonal, u i By v i With w i According to the measurement system error model, the calculation formula for the lengths of the three semi-major axes of the ellipsoid is:

[0100]

[0101] Where U ui ,U vi ,U wi That is, the lengths of the three semi-axes, which are respectively along the three coordinate axes of the local coordinate system. r ,σ α and σ β are the distance measurement variance and angle measurement variance of the tracker, which are the inherent properties of the instrument obtained through a large number of experiments. i and β i are the distance from the target point to the origin of the measurement coordinate system and the z-axis angle directly obtained by the tracker. According to the above uncertainty ellipsoid model and local coordinate system definition, using Figure 2 The geometric relationship shown can be used to calculate the local coordinate system of the ellipsoid {p mi ;u i ,v i ,w i} to the mechanism base coordinate system {o g ;x g ,y g,z g The transformation matrix formula of} is:

[0102]

[0103] Where x mi ,y mi ,z mi are the three coordinates of the i-th measurement point in the laser tracker measurement coordinate system; It is the fourth-order homogeneous transformation matrix from the measurement coordinate system to the mechanism base coordinate system, which is obtained by calculating the measurement coordinates of multiple reference points and their given reference coordinates in the base coordinate system through a multi-point registration algorithm. This has been extensively described in existing literature, so this article will not go into detail here. is the transformation matrix from the local coordinate system of the ellipsoid of the i-th measurement point to the measurement coordinate system, which can be calculated based on x mi ,y mi ,z mi The calculation is as shown in formula (2.2).

[0104] 2. Calculation of the moment of inertia, product of inertia and coordinate integral of the ellipsoid relative to the base coordinate system

[0105] For the uncertainty ellipsoid of the i-th measuring point, its moment of inertia, product of inertia and coordinate integral expressions relative to the coordinate plane of the base coordinate system are:

[0106]

[0107] At the same time, the moment of inertia, product of inertia and coordinate integral of the ellipsoid relative to the coordinate plane of the local coordinate system are:

[0108]

[0109] According to the transformation matrix from the local coordinate system to the base coordinate system The moment of inertia, product of inertia and coordinate integral in the base coordinate system can be expressed as a linear combination of the moment of inertia, product of inertia and coordinate integral in the local coordinate system. Assume that the transformation matrix calculated above is for:

[0110]

[0111] Then, by combining equations (2.3), (2.4) and (2.5), the moment of inertia, product of inertia and coordinate integral of the ellipsoid in the base coordinate system can be calculated as:

[0112] The integral calculation result of formula (2.6) is the input parameter for the linear axis and rotation axis fitting in the subsequent steps S3 and S4.

[0113] S3: For the linear axis, the direction vector of the linear axis is fitted with the goal of minimizing the sum of the squared mean values ​​of the distances from each point in the uncertain ellipsoid to the straight line;

[0114] According to the uncertainty ellipsoids of multiple measurement points in the base coordinate system, the direction of each linear axis in the zero position state is fitted to achieve linear axis direction vector fitting, including minimizing the sum of the square mean of the distance from each point to the straight line in the ellipsoid domain as the goal, using the semi-major axis length, moment of inertia and product of inertia results calculated by S2 to fit the direction vector of the linear axis in the zero position state of the mechanism, as shown in Figure 3 As shown, the details are as follows:

[0115] 1. Establishment of optimization objective function

[0116] With the goal of minimizing the sum of the mean squares of the distances from points to the straight line in each ellipsoid domain, the direction vector of the straight line axis is fitted using the semi-major axis length, moment of inertia, and product of inertia of each ellipsoid calculated by S2. The mean square of the distance from a point to a straight line in the ellipsoid domain is represented by the ratio of the triple integral of the square of the distance from the point to the straight line in the ellipsoid domain to the volume of the ellipsoid domain, and its expression is:

[0117]

[0118] Where p l =[x l ,y l ,z l ] T is the position vector of the foot of the perpendicular from the origin of the base coordinate system to the straight line in the base coordinate system, s l =[a l ,b l ,c l ] T is the unit vector along the straight line in the base coordinate system, such as Figure 3 As shown. i represents the uncertainty ellipsoid domain of the i-th measurement point, and p(x, y, z) is the position vector of any point in the ellipsoid domain in the base coordinate system. li Expand and organize:

[0119]

[0120]

[0121] Where k 1i ~k 6i is a fixed parameter calculated based on the ellipsoidal moment of inertia and product of inertia of S2; x ci ,y ci ,z ci is the three coordinates of the i-th measurement point in the base coordinate system, according to the transformation matrix of S2 x mi ,y mi ,z mi Then the objective function expression of linear axis fitting optimization is:

[0122]

[0123] 2. Solving the optimization goal:

[0124] Formula (3.3) is a cubic polynomial nonlinear programming problem with quadratic constraints. Here, the Lagrange multiplier method is used to transform it. Construct the Lagrange function L(a l ,b l ,c l ,x l ,y l ,z l ,λ1,λ2):

[0125]

[0126] According to the conditional extreme value principle, the necessary condition for the objective function to reach a minimum value at a certain point is that the point is a stationary point of the Lagrangian function. l ,b l ,c l ,x l ,y l ,z l , the first-order partial derivative of λ1,λ2 is equal to zero:

[0127]

[0128] Thus, the optimization problem of the original straight line fitting is transformed into the problem of solving an eight-variable quadratic nonlinear equation system shown in equation (3.5). Here, the Newton iteration method is further used to numerically solve the above equation. Let:

[0129]

[0130] Then when the parameter x of the kth step is given (k) , the parameter x to be solved in step k+1 (k+1) for:

[0131]

[0132] When ‖F(x)‖ is less than a given threshold, the iteration is stopped, thus obtaining an approximate solution to the equation group (3.5) and finding the approximate stationary point of the Lagrangian function L. By comparing the objective function size of equation (3.3) corresponding to multiple groups of solutions, the final optimization parameters are selected to obtain the direction vector s of the fitting line l =[a l ,bl ,c l ] T and the foot of the perpendicular from the origin to the line p l =[x l ,y l ,z l ] T When the mechanism is in the zero position, the direction vector s of each linear axis in the base coordinate system is l As parameters for kinematic modeling in the subsequent step S5.

[0133] S4. Rotation axis pose fitting:

[0134] The direction and position of each rotation axis in the zero position state are fitted according to the uncertain ellipsoids of multiple measurement points in the base coordinate system.

[0135] For the rotation axis, first, the goal is to minimize the sum of the mean squares of the distances from each point to the plane in each ellipsoid domain. The semi-major axis, moment of inertia, and product of inertia of the ellipsoid calculated by S2 are used to fit the circular plane where the point rotation trajectory is located. The normal vector of the plane is the direction of the axis. Then each measurement point is projected into the plane, and the center of the rotation trajectory is fitted in the plane with the goal of minimizing the sum of the squares of the difference between the distance from each projection point to the center of the circle and the radius of gyration. The normal vector of the plane is the direction vector of the rotation axis. The center of the trajectory is on the axis, that is, a point on the axis, which characterizes the spatial position of the axis; the details are as follows:

[0136] 1. Circular plane fitting

[0137] First, the goal is to minimize the sum of the squared mean distances from each point to the plane in each ellipsoid domain. Using the semi-major axis, moment of inertia, and product of inertia calculated by S2, the plane where the point rotation trajectory is located is fitted. The normal vector of the plane is the direction of the rotation axis, as shown in Figure 4 The mean square of the distance from a point to a plane in each ellipsoid domain is represented by the ratio of the triple integral of the square of the distance from the point to the plane on the ellipsoid domain to the volume of the ellipsoid domain, and its expression is:

[0138]

[0139] Where n=[n1,n2,n3] T is the unit normal vector of the plane to be fitted, and d is the parameter that characterizes the position of the plane. pi Expand and organize:

[0140]

[0141]

[0142] Where k 1i ~k 6iThe definition of is the same as that in the square integral of the distance from a point to a straight line in equation (3.2), k 7i ~k 10i It is a fixed parameter calculated by the coordinate integral of the ellipsoid of S2 relative to the base coordinate system. Therefore, the plane fitting optimization objective expression is:

[0143]

[0144] The above equation is a quadratic polynomial optimization problem with quadratic constraints. Its solution is the same as the processing method of S3 for the straight line fitting optimization problem, which will not be repeated here. Thus, the equation of the fitting plane n1x+n2y+n3z+d=0 can be obtained.

[0145] 2. Rotation trajectory center fitting

[0146] Then, each measurement point is projected into the plane, and the least square optimization objective is constructed according to each projection point to fit the arc of the rotation trajectory in the plane. The center of the arc is a point on the axis, so the spatial position of the axis is obtained, such as Figure 4 As shown. For the i-th measurement point (x ci ,y ci ,z ci ), the coordinates of its projection point in the plane (x' ci ,y' ci ,z' ci )for:

[0147]

[0148] After obtaining the projection coordinates of each measurement point, the center point p of the rotation trajectory arc is fitted in the plane. r =[x r ,y r ,z r ] T The optimization goal of the center of the circle is to minimize the sum of the squares of the difference between the squares of the distances from each projection point to the center of the circle and the square of the arc radius:

[0149]

[0150] The above formula exists with respect to x r ,y r ,z r The analytical solution is:

[0151]

[0152]

[0153]

[0154] From equation (4.6), we can directly obtain the analytical solution of the center of the fitting circle according to the projection point. The normal vector of the fitting plane n = [n1, n2, n3] T That is, the unit direction vector of the rotation axis, the center p r =[x r ,y r ,z r ] T That is, a point on the axis represents the position of the axis. When the mechanism is in the zero position state, the unit direction vector n of each rotation axis and the center position vector p of the rotation trajectory in the base coordinate system are r As the kinematic modeling parameters of the subsequent S5.

[0155] S5. Construction of kinematic model of mechanism based on arbitrary fitting axis in space:

[0156] Based on the direction vector of the linear axis and the position of the rotation axis in the zero position state of the mechanism, the coordinate transformation formula of the point translation in any direction in space and rotation around any axis is used to construct the fourth-order homogeneous transformation matrix expression corresponding to the motion of each rotation axis and translation axis. According to the composition form of the mechanism, the matrices corresponding to each axis are multiplied to establish the transformation matrix from the end coordinate system of the mechanism to the base coordinate system, thereby completing the establishment of the kinematic model of the mechanism.

[0157] When the mechanism is in the zero position, the direction vector in the base coordinate system is s l =[a l ,b l ,c l ] T The translation axis of , assuming that its linear displacement relative to the zero position is t, then the corresponding fourth-order homogeneous transformation matrix is:

[0158]

[0159] When the mechanism is in the zero position, the direction vector in the base coordinate system is n = [n1, n2, n3] T , the fitted center position vector is p r =[x r ,y r ,z r ] T The rotation axis of , assuming that its angular displacement relative to the zero position is θ, then the corresponding fourth-order homogeneous transformation matrix is:

[0160]

[0161] Based on equations (5.1) and (5.2), the transformation matrix of the end coordinate system relative to its zero position during the movement of the mechanism is It is obtained by multiplying the homogeneous transformation matrices of each axis in sequence. Figure 5As shown in the figure, taking the PPP-RRR six-axis mechanism with three linear axes and three rotary axes connected in series as an example, its end coordinate system {o e ;x e ,y e ,z e The transformation matrix of the zero-position {o0; x0, y0, z0} is obtained by right-multiplying the transformation matrices of the six axes in sequence:

[0162]

[0163] Where T (1)t ,T (2)t ,T (3)t are the homogeneous transformation matrices of the first three linear axes, T (4)r ,T (5)r ,T (6)r are the homogeneous transformation matrices of the last three rotation axes respectively.

[0164] Simulation verification of spatial straight line and circle fitting by integrating uncertainty ellipsoid:

[0165] In order to verify the advantages of the spatial straight line and circle fitting algorithm of the fused uncertainty ellipsoid in steps S3 and S4 in improving accuracy compared with the traditional fitting algorithm based on multi-point least squares distance, a straight line and a circle are used as fitting objects in space, and several true value points are generated on them. Then, the actual measured sampling points corresponding to each true value point are randomly generated according to the error model of formula (2.1) by the Monte Carlo method to simulate measurement noise. The sampling points are fitted using the algorithm of the fused uncertainty model of the present invention and the conventional algorithm based on multi-point least squares distance, and the fitting effect is as follows: Figure 6 shown.

[0166] In order to quantitatively compare the fitting accuracy of the two algorithms, the square of the modulus of the difference between the unit direction vector of the fitted line and the unit direction vector of the original line is used as the evaluation basis for line fitting; the square of the modulus of the six-dimensional vector error constructed by the unit normal vector of the circular plane and the coordinates of the arc center is used as the evaluation basis for spatial circle fitting. The comparison results of the two algorithms are shown in Figure 2. Figure 7 shown.

[0167] like Figure 7 As shown, the fitting algorithm of the present invention is more accurate than the traditional least squares fitting method in the spatial straight line and circle fitting simulation, which can verify that the linear axis and rotation axis fitting algorithm of the mechanism integrating the point uncertainty ellipsoid proposed in this patent has theoretical feasibility and accuracy advantages.

[0168] It will be apparent to those skilled in the art that the invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other specific forms without departing from the spirit or essential features of the invention. Therefore, the embodiments should be considered exemplary and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description, and it is intended that all variations falling within the meaning and scope of the equivalent elements of the claims be included in the invention. Any reference numeral in a claim should not be considered as limiting the claim to which it relates.

[0169] In addition, it should be understood that although the present specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation modes that can be understood by those skilled in the art.

Claims

1. A method for zero-position fitting and kinematic modeling of mechanism axes integrating measurement point position uncertainty, characterized in that: include: S1: Taking any position at the end of the mechanism as the target point, a laser tracker is used to measure the coordinates of the target point at multiple positions when each axis of the mechanism moves independently; S2: Based on the data measured in S1 and the transformation between the measurement coordinate system and the mechanism base coordinate system, the transformation between the local coordinate system representing the uncertainty ellipsoid posture and the mechanism base coordinate system, the length of the semi-major axis of the ellipsoid, the moment of inertia, the product of inertia and the coordinate integration results are calculated; S3: With the goal of minimizing the sum of the mean squares of the distances from each point in the ellipsoid domain to the straight line, the direction vector of the straight axis in the zero position state of the mechanism is obtained by fitting using the semi-major axis length, moment of inertia and product of inertia results of the ellipsoid calculated in S2; S4: With the goal of minimizing the sum of the mean squares of the distances from each point to the plane in the ellipsoid domain, the plane where the point rotation trajectory is located is fitted using the length of the semi-major axis, moment of inertia, product of inertia and coordinate integration results calculated by S2, and the position and posture of the rotation axis in the zero position state of the mechanism is fitted by minimizing the sum of the squares of the projection of the measured point to the plane and the difference between the square of the distance from each projection point to the center of the circle and the square of the radius of the arc; S5: Based on the direction vector of the linear axis and the posture of the rotation axis in the zero-position state of the mechanism, the coordinate transformation of the point along any direction in space and the rotation around any axis is performed to obtain the fourth-order homogeneous transformation matrix corresponding to the motion of the rotation axis and the linear axis. Combined with the composition of the mechanism, the transformation matrix from the end coordinate system of the mechanism to its zero-position reference posture is established to complete the establishment of the kinematic model.

2. The method for zero-position fitting and kinematic modeling of mechanism axes integrating measurement point position uncertainty according to claim 1 is characterized in that: The S1 includes: fixing a target point at any position at the end of the mechanism, driving each axis of the mechanism to move separately in turn, when each axis moves, the other axes are in a zero position state, using a laser tracker to collect the coordinates of the target point at multiple measurement positions when each axis moves separately, wherein at least two measurement point coordinate data are collected when the mechanism moves along the linear axis, and at least three measurement point coordinate data are collected when the mechanism rotates around the rotation axis.

3. The method for zero-position fitting and kinematic modeling of mechanism axis integrating measurement point position uncertainty according to claim 1 is characterized in that: In S2, the transformation relationship from the laser tracker measurement coordinate system to the mechanism base coordinate system is calibrated based on the multi-point registration algorithm, all target points are transformed from the measurement coordinate system to the base coordinate system, the measurement coordinates of the target points at multiple positions at the end of the mechanism in the base coordinate system are obtained, the three semi-major axis lengths of the uncertainty ellipsoid of each point and the transformation matrix from the local coordinate system to the base coordinate system representing the ellipsoid posture are calculated, and the moment of inertia and product of inertia of each ellipsoid relative to the coordinate plane of the base coordinate system are further calculated.

4. The method for zero-position fitting and kinematic modeling of mechanism axis integrating measurement point position uncertainty according to claim 3 is characterized in that: The calculation formulas for the three semi-major axis lengths are: Where U ui ,U vi ,U wi are the lengths of the semi-major axes along the three coordinate axes of the local coordinate system; σ r ,σ α and σ β are the distance measurement variance and angle measurement variance of the tracker, respectively, which are the inherent properties of the instrument; r i and β i They are respectively the distance from the target point to the origin of the measurement coordinate system and the z-axis angle obtained directly by the tracker.

5. The method for zero-position fitting and kinematic modeling of mechanism axes integrating measurement point position uncertainty according to claim 3 is characterized in that: The calculation formula of the transformation matrix from the local coordinate system to the base coordinate system is: Where x mi ,y mi ,z mi It is the three-axis coordinate of the target point at the end of the mechanism measured at the i-th measurement point in the laser tracker measurement coordinate system; It is the fourth-order homogeneous transformation matrix from the measurement coordinate system to the mechanism base coordinate system, which is obtained by calculating the measurement coordinates of multiple reference points and their given reference coordinates in the base coordinate system through a multi-point registration algorithm; is the transformation matrix from the local coordinate system of the ellipsoid of the i-th measurement point to the measurement coordinate system.

6. The method for zero-position fitting and kinematic modeling of mechanism axes integrating measurement point position uncertainty according to claim 3 is characterized in that: Transformation matrix from local coordinate system to base coordinate system sib T, the moment of inertia, product of inertia and coordinate integral of the uncertainty ellipsoid of the i-th measurement point relative to the coordinate plane of the base coordinate system are as follows: In the formula, I (i)xx ,I (i)yy ,I (i)zz are the moments of inertia of the ellipsoid about the coordinate planes yoz, xoz and xoy of the base coordinate system, respectively, (i)xy ,I (i)xz ,I (i)yz is the product of inertia of the ellipsoid with respect to the coordinate planes yoz, xoz and xoy of the base coordinate system, I (i)x ,I (i)y ,I (i)z are the coordinate integrals of the ellipsoid relative to the base coordinate systems yoz, xoz and xoy; U ui ,U vi ,U wi are the semi-major axis lengths of the ellipsoid of the i-th measuring point along the three coordinate axes of the local coordinate system; r i11 ~r i33 is the transformation matrix sib The rotation matrix elements in T, t i1 ~t i3 is the translation vector element, namely:

7. The method for zero-position fitting and kinematic modeling of mechanism axes integrating measurement point position uncertainty according to claim 1 is characterized in that: The S3 includes: The mean square of the distance from a point to a straight line in the ellipsoid domain is represented as the ratio of the triple integral of the square of the distance from the point to the straight line on the ellipsoid domain to the volume of the ellipsoid domain, and its expression is: Where U ui ,U vi ,U wi are the semi-major axis lengths of the uncertain ellipsoid of the i-th measuring point along the three coordinate axes of the local coordinate system; p l =[x l ,y l ,z l ] T It is the position vector of the foot of the perpendicular from the origin of the base coordinate system to the straight line in the base coordinate system; s l =[a l ,b l ,c l ] T is the unit vector along the straight line in the base coordinate system; Ω i represents the uncertainty ellipsoid domain of the i-th measurement point; p(x,y,z) is the position vector of any point in the ellipsoid domain in the base coordinate system; Will I li The expression is expanded and sorted: Where k 1i ~k 6i is a fixed parameter calculated based on the ellipsoidal moment of inertia and product of inertia of S2; x ci ,y ci ,z ci is the three-axis coordinate of the i-th measurement point in the base coordinate system, according to the fourth-order homogeneous transformation matrix from the measurement coordinate system of S2 to the mechanism base coordinate system x mi ,y mi ,z mi Transform to get, x mi ,y mi ,z mi It is the three-axis coordinate of the target point at the end of the mechanism measured at the i-th measurement point in the laser tracker measurement coordinate system; Then the objective function with the goal of minimizing the sum of the mean squares of the distances from each point to the straight line in the ellipsoid domain is obtained as follows: The Lagrange multiplier method is used to transform the above objective function and construct the Lagrange function L(a l ,b l ,c l ,x l ,y l ,z l ,λ1,λ2): Let the Lagrangian function be l ,b l ,c l ,x l ,y l ,z l ,λ1,λ2 first-order partial derivatives are equal to zero, so we get the following equations: Use Newton's iteration method to numerically solve the above equation: Then when the parameter x of the kth step is given (k) , the parameter x to be solved in step k+1 (k+1) for: When ‖F(x)‖ is less than a given threshold, the iteration is stopped, thereby obtaining an approximate solution to the system of equations and finding the approximate stationary point of the Lagrangian function L; By comparing the size of the original objective function corresponding to multiple sets of solutions, the final optimization parameters are selected to obtain the direction vector s of the linear axis in the zero position state of the mechanism. l =[a l ,b l ,c l ] T and the foot of the perpendicular from the origin to the line p l =[x l ,y l ,z l ] T , will s l =[a l ,b l ,c l ] T Input as S5.

8. The method for zero-position fitting and kinematic modeling of mechanism axes integrating measurement point position uncertainty according to claim 1 is characterized in that: In S4, the mean square of the distance from the point to the plane in the ellipsoid domain is represented as the ratio of the triple integral of the square of the distance from the point to the plane on the ellipsoid domain to the volume of the ellipsoid domain, and its expression is: Where n=[n1,n2,n3] T is the unit direction vector of the plane to be fitted, and d is the parameter representing the position of the plane; Will I pi The expression is expanded and sorted: Where k 1i ~k 6i is a fixed parameter calculated based on the ellipsoidal moment of inertia and product of inertia of S2, k 7i ~k 10i It is a fixed parameter calculated by integrating the coordinates of the ellipsoid of S2 relative to the base coordinate system; Then, the fitting objective function is obtained, which aims to minimize the sum of the mean squares of the distances from points to the plane in each ellipsoid domain: Solve the above fitting objective function to obtain the fitting plane n1x+n2y+n3z+d=0. Project the original coordinates of multiple measurement points onto this plane. For the i-th measurement point, the coordinates (x ci ,y ci ,z ci ), the coordinates of its projection point (x' ci ,y' ci ,z' ci )for: The optimization goal is to minimize the sum of the squares of the differences between the squares of the distances from each projection point to the center of the circle and the square of the arc radius, and fit the center position vector p of the rotation trajectory arc in the plane. r =[x r ,y r ,z r ] T : The above formula is about x r ,y r ,z r The analytical solution is: When the mechanism is in the zero position, the unit direction vector n of each rotation axis in the base coordinate system is [n1, n2, n3] T and the center position vector p of the arc of the rotating trajectory r =[x r ,y r ,z r ] T As the position and posture of the rotation axis in the zero position state of the mechanism, input S5.

9. The method for zero-position fitting and kinematic modeling of mechanism axes integrating measurement point position uncertainty according to claim 1, characterized in that: In S5, the fourth-order homogeneous transformation matrices corresponding to the motion of the linear axis and the rotation axis are respectively: Where t is the linear displacement of the linear axis relative to the zero position, and θ is the angular displacement of the rotation axis relative to the zero position; (a l ,b l ,c l ) is the unit direction vector coordinate of the linear axis in the base coordinate system when the mechanism is in the zero position state; (n1,n2,n3), (x r ,y r ,z r ) are the unit direction vector of the rotation axis and the center position vector coordinates of the rotation trajectory in the base coordinate system when the mechanism is in the zero position state.

10. The method for zero-position fitting and kinematic modeling of mechanism axes integrating measurement point position uncertainty according to claim 1, characterized in that: In S5, the transformation matrix of the terminal coordinate system relative to its zero reference is It is obtained by multiplying the fourth-order homogeneous transformation matrices corresponding to the motion of the rotary axis and the linear axis in sequence according to the composition of the mechanism.

Images