Multi-degree-of-freedom forming equipment pose sensing data fusion method considering error sensitivity

Abstract

The invention relates to a multi-degree-of-freedom forming equipment pose sensing data fusion method considering error sensitivity, multi-degree-of-freedom forming equipment realizes multi-degree-of-freedom motion through coupling motion of a plurality of motion branch chains, and a grating ruler is mounted on a driving branch chain of the multi-degree-of-freedom forming equipment; comprising the following steps: S1, calculating the pose of the multi-degree-of-freedom forming equipment based on a grating ruler; s2, obtaining the error sensitivity of calculating the pose based on a grating ruler; s3, calculating the pose of the multi-degree-of-freedom forming equipment based on monocular vision; s4, obtaining the error sensitivity of the pose calculated based on monocular vision; and S5, considering the error sensitivity of the two pose calculation methods in the step S3 and the step S4, and performing pose data fusion. According to the invention, the pose data collected by various sensors can be fused, the sensing precision of the equipment under different poses is improved, and the situation that the sensing pose error is relatively large under some special poses is avoided, so that the requirement that the multi-degree-of-freedom forming equipment realizes complex motion under a heavy load working condition is met.

Description

Technical Field

[0001] The present invention relates to the field of multi-degree-of-freedom forming, and more specifically, to a method for fusing pose perception data of a multi-degree-of-freedom forming equipment considering error sensitivity. Background Art

[0002] Multi-degree-of-freedom forming is an advanced local incremental metal forming technology. The upper die swings around the axis of the machine tool and its own main axis at a certain swing angle, while the lower die feeds upward at a certain speed, forcing the blank to undergo local near-net plastic deformation until the target shape of the complex part is achieved. Compared with the traditional single-degree-of-freedom integral forming process, it has technical advantages such as smaller forming force, higher part dimensional accuracy, lower vibration and noise, and good flexibility.

[0003] The heavy-duty multi-degree-of-freedom forming machine tool for multi-degree-of-freedom forming realizes the multi-degree-of-freedom movement of the equipment through the coupled movement of multiple motion chains. The pose of the multi-degree-of-freedom forming equipment is usually calculated based on the grating scale information installed on the active chain. This method is effective for parallel machine tools under light load conditions. However, under heavy load conditions, the parallel machine tool may produce excessive deformation in some special poses, resulting in inaccurate pose calculation. At the same time, a single data source is vulnerable to the influence of the working environment, leading to large deviations in the sensed data. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a method for fusing pose perception data of a multi-degree-of-freedom forming equipment considering error sensitivity, which can fuse the pose data collected by multiple sensors, improve the perception accuracy of the equipment in different poses, and avoid large errors in the sensed pose in some special poses to meet the requirements of the multi-degree-of-freedom forming equipment for realizing complex movements under heavy load conditions.

[0005] The technical solution adopted by the present invention to solve its technical problems is to construct a method for fusing pose perception data of a multi-degree-of-freedom forming equipment considering error sensitivity. The multi-degree-of-freedom forming equipment realizes multi-degree-of-freedom movement through the coupled movement of multiple motion chains. A grating scale is installed on the active chain of the multi-degree-of-freedom forming equipment. The data fusion method includes the following steps:

[0006] S1. Calculate the pose of the multi-degree-of-freedom forming equipment based on the grating scale;

[0007] S2. Obtain the error sensitivity of the pose calculated based on the grating scale;

[0008] S3. Calculate the pose of the multi-degree-of-freedom forming equipment based on monocular vision;

[0009] S4. Obtain the error sensitivity of the pose calculated based on monocular vision;

[0010] S5. Consider the error sensitivity of the two pose calculation methods in steps S3 and S4, and perform pose data fusion.

[0011] According to the above solution, in the step S1, the method for calculating the pose of the multi-degree-of-freedom forming equipment based on the grating scale includes establishing two coordinate systems, namely the moving coordinate system and the fixed coordinate system:

[0012] Construct a fixed coordinate system A-XYZ(S A ) on the upper surface of the static platform of the multi-degree-of-freedom forming equipment. Its origin A 0 is located at the geometric center of the static platform, and the Z-axis is upward and perpendicular to the static platform, that is, perpendicular to the sliding direction of the slider; construct a moving platform coordinate system B-XYZ(S B ) on the moving platform of the spatial parallel equipment. Its origin B 0 is at the geometric center of the circumcircle of the moving platform, and the Z-axis is upward and perpendicular to the surface of the moving platform; when the moving platform is in the initial position, that is, the equilibrium position, the X, Y, and Z directions of the moving coordinate system are the same as those of the fixed coordinate system, and the Z-axis of the moving coordinate system passes through the origin A 0 of the static coordinate system; the six spherical hinge points of the static platform are represented by A i (i = 1, 2, 3, 4, 5, 6), and the spherical hinge points of the moving platform are represented by B i . θ Ai (i = 1-6) represents the distribution angle of the spherical hinge points on the static platform, and θ Bi (i = 1-6) represents the distribution angle of the spherical hinge points on the moving platform.

[0013] According to the above solution, in the step S1, the method for calculating the pose of the multi-degree-of-freedom forming equipment based on the grating scale further includes:

[0014] Given that the slider position is S i , the installation angle of the static platform hinge point is θ Ai , the radius of the moving platform is R B , and the installation angle of the moving platform hinge point is θ Bi , then the coordinate vector of the spherical hinge point A i on the slider of the static platform in the fixed system S A is:

[0015] a i = [X Ai Y Ai Z Ai T = [S i cosθ Ai S i sinθ Ai 0] T (1)

[0016] In the formula, S​i is the distance from the center of the slider of the static platform to the center of the static platform;

[0017] Furthermore, the coordinate vectors of each hinge point B of the moving platform i in the moving coordinate system S B are determined by Equation (2):

[0018] b i = [X Bi Y Bi Z Bi T = [R B cosθ Bi R B sinθ Bi 0] T (2)

[0019] wherein, R B is the distance from the center of the slider of the moving platform to the center of the moving platform;

[0020] x, y, z represent the translational motions of the moving platform in three directions, and α, β, γ represent the rotational angles of the moving platform in three directions; during the multi-degree-of-freedom forming process, the motion equation of the upper die is:

[0021]

[0022] wherein, is the swing angle of the upper die motion, ω is the rotational speed during the upper die motion, k is the feed rate during the upper die motion, and h is the feed distance during the upper die motion;

[0023] Thus, the rotation matrix R of S B relative to S A is determined by Equation (4):

[0024]

[0025] Furthermore, the position vector i of B A in the global coordinate system S A b i is determined by Equation (5):

[0026] A b i = Rb i + t (5)

[0027] wherein, t is the position vector of the origin of coordinates of S B relative to S A ; t is determined by Equation (6):

[0028] t = [x, y, z] T (6)​

[0029] According to the closed-loop vector relationship, it can be obtained that:

[0030] l i = Rb i + t - a i (7)

[0031] Taking the pose q = [x y z α β γ] of the moving platform T as the independent variable, we get:

[0032] |R(q)b i + t(q) - a i | 2 - l 2 = 0 (8)

[0033] In the formula, b i is a constant value. Substituting the slider position S measured by the grating ruler i into a i , a i is also a constant value. The unknown quantities are the pose quantities of the moving platform represented by R and t in the formula. Solving the above non-linear equations can obtain the pose calculated based on the grating ruler.

[0034] Transforming (8) into F(q) = 0, the pose q at time k can be obtained. k The Taylor formula at the point of the pose is:

[0035] F(q) = F(q k )+(q - q k )J(q k )+ R k (9)

[0036] In the formula, J(q k ) is the Jacobian matrix of the non-linear equations F(q), denoted as:

[0037]

[0038] The iterative formula of Newton's method for solving F(q) = 0 is:

[0039]

[0040] According to the above scheme, the step S2 includes the following steps:

[0041] Transforming equation (8) into the form of equation (11):

[0042]

[0043] The derivative of the pose with respect to the input represents the error sensitivity under the pose calculation method based on the grating ruler measurement. The calculation formula is as follows:

[0044]

[0045] wherein

[0046]

[0047] For a certain moment k, the maximum value of the input is selected to represent the error sensitivity of this pose, expressed as:

[0048]

[0049] According to the above scheme, step S3 includes the following steps:

[0050] Set the two-dimensional coordinates of the cooperation target in the camera as u i (i = 1...n), and finally set the position vector of the cooperation target as p i B ;

[0051] Based on the existing measurement points, four virtual points are calculated and determined by Equation (14):

[0052]

[0053] wherein, λ j is the A of the matrix Τ A eigenvalue, v j is the matrix A Τ A eigenvector; the coordinate of the cooperation target in the end coordinate system is represented by the virtual control point as Equation (15):

[0054]

[0055] The coefficient α can be obtained from Equation (15) ij as Equation (16):

[0056]

[0057] The virtual control point represents the coordinate of the cooperation target in the camera coordinate system can be represented by Equation (17):

[0058]

[0059] wherein represents the coordinate of the control point in the camera coordinate system;

[0060] According to the pinhole imaging model, it can be obtained that:

[0061]

[0062] wherein, fu , f v is the focal length, u c , v c is the optical center coordinate, u i = [u i , v i , 1] T ,

[0063] Expanding equation (18), we get:

[0064]

[0065] The matrix form is represented by equation (20):

[0066]

[0067] The solution C of equation (20) is in the right null space of M, and we get:

[0068]

[0069] where v i represents the eigenvector corresponding to the zero eigenvalue of matrix M Τ ; for the pinhole imaging model, N is usually taken as 1, and we get equation (22):

[0070] C = βv (22)

[0071] From the fact that the distance between virtual control points does not change with the change of the reference coordinate system, we get:

[0072]

[0073] β can be represented by equation (24):

[0074]

[0075] The coordinates of the four control points in the camera coordinate system can be obtained

[0076] Use ICP to solve the pose relationship between the moving platform coordinate system and the camera coordinate system C T B ; the attitude of the camera coordinate system relative to the static platform coordinate system is fixed and is represented as A T C , and the pose of the moving coordinate system relative to the static coordinate system can be determined by equation (25):

[0077] A T B = A T C · C T B (25)

[0078] Each pose component can be determined by Equation (26):

[0079]

[0080] wherein,

[0081] According to the above solution, step S4 includes the following steps:

[0082] The derivative of the pose q = [α, β, γ, x, y, z] with respect to the input u i is expressed as:

[0083]

[0084] Select the maximum value of all two-dimensional coordinates to represent the pose error sensitivity at this moment. Its value represents the credibility of the pose data based on monocular vision measurement, denoted as the maximum error sensitivity. The calculation formula is as follows:

[0085]

[0086] According to the above solution, step S5 includes the following steps:

[0087] The error sensitivity based on grating scale measurement is represented by the vector m s and the error sensitivity based on monocular vision measurement is represented by the vector m c and its covariance matrix is represented as Q s and Q c ;

[0088] At time k, the equipment pose is set as q k = [x, y, z, α, β, γ] Τ , and its first-order derivative and second-order derivative with respect to time are set as state variables, i.e., X k = [q k q k ′q k ″] T . Each state variable is independent. The state equations of the two measurement methods are:

[0089]

[0090] wherein, is the state noise vector, assumed to be Gaussian noise with zero mean and covariance, and F is the state transition matrix;

[0091] Assume the sampling time is Δt. Then the relationship between the poses at time k - 1 and time k is expressed by the Taylor expansion:

[0092]

[0093] From this, the state transition matrix F is represented by Equation (31):

[0094]

[0095] To achieve the fusion of two types of data, two sub-filters are required, and the measurement equations of each sub-filter are as follows:

[0096]

[0097] where q s , q c are the measured pose values of sub-filters 1 and 2, obtained from the grating scale position and monocular vision measurement, is the measurement matrix, is the error sensitivity of different measurement methods, which represents the credibility of the measurement data;

[0098] The two sub-filters respectively receive the pose data and measurement noise from the grating scale measurement subsystem and the monocular vision measurement subsystem, and obtain the state vector and the state covariance matrix P 1 , P 2 ; The main filter first performs a time update to obtain the state vector and the state covariance matrix P m , and then the main filter combines all the obtained state vectors and state covariance matrices to obtain the state vector of the global optimal estimate and the state covariance matrix P g .

[0099] According to the above scheme, the method by which the main filter combines all the obtained state vectors and state covariance matrices to obtain the state vector of the global optimal estimate and the state covariance matrix P g includes:

[0100] (1) The initial values of the three filters are obtained from the optimal estimate at the previous moment:

[0101]

[0102] where λ i is the distribution coefficient, λ i > 0 and ∑λ i = 1;

[0103] (2) The state vectors and the state covariance matrices P 1 , P 2 , P m of the three filters are as follows:

[0104]

[0105] wherein, j = 1, 2 represents sub - filters 1 and 2;

[0106] (3) Optimal estimation of equipment pose:

[0107]

[0108] Implementing the multi - degree - of - freedom forming equipment pose perception data fusion method considering error sensitivity of the present invention has the following beneficial effects:

[0109] 1. The present invention proposes a multi - degree - of - freedom forming equipment pose perception data fusion method considering error sensitivity. First, the equipment pose is calculated based on the grating scale data on the active link of the equipment. Then, an error sensitivity model for the pose calculated based on the grating scale is established. Next, the equipment pose is calculated based on monocular vision and an error sensitivity model for the pose calculated based on monocular vision is established. Finally, considering the error sensitivities of both pose calculation methods simultaneously, the calculation results are fused to obtain pose data with higher accuracy and stronger robustness. Through this method, the pose data of the equipment can be calculated, thereby guiding the development and control of the equipment.

[0110] 2. The method proposed by the present invention can fuse the calculation results of two different poses, avoid excessive pose measurement error values caused by a single data source affected by the environment, and propose an improved federated Kalman filtering method considering the error sensitivity of the measurement method, thereby realizing real - time monitoring of the equipment under different working conditions and laying a foundation for the subsequent real - time control of the equipment. BRIEF DESCRIPTION OF THE DRAWINGS

[0111] The present invention will be further described below in conjunction with the drawings and embodiments. In the drawings:

[0112] Figure 1 is a schematic diagram of the multi - degree - of - freedom forming equipment coordinate system and the distribution of spherical hinge points;

[0113] Figure 2 is a position vector diagram of the multi - degree - of - freedom forming equipment link;

[0114] Figure 3 is a schematic diagram of the monocular vision measurement system;

[0115] Figure 4 is a schematic diagram of the image processing process;

[0116] Figure 5 is a structural block diagram of the Kalman filtering method;

[0117] Figure 6is the error sensitivity of the pose of the multi-degree-of-freedom forming equipment to each input slider;

[0118] Figure 7 is the maximum error sensitivity of the pose of the multi-degree-of-freedom forming equipment to the input slider;

[0119] Figure 8 is the error sensitivity of the pose of the multi-degree-of-freedom forming equipment to the coordinates of each cooperation target;

[0120] Figure 9 is the maximum error sensitivity of the pose of the multi-degree-of-freedom forming equipment to the coordinates of the cooperation target;

[0121] Figure 10 is the schematic diagram of the experimental platform;

[0122] Figure 11 is the comparison diagram of the pose errors of the moving platform under each sensor and different fusion algorithms. Specific implementation manner

[0123] For a clearer understanding of the technical features, objectives, and effects of the present invention, the specific implementation manner of the present invention will now be described in detail with reference to the accompanying drawings.

[0124] The multi-degree-of-freedom forming equipment pose perception data fusion method considering error sensitivity of the present invention includes the following steps:

[0125] S1. Calculate the pose of the multi-degree-of-freedom forming equipment based on the grating ruler;

[0126] As Figure 1 shown, establish two coordinate systems, namely the moving coordinate system and the fixed coordinate system. On the upper surface of the static platform of the multi-degree-of-freedom forming equipment, establish a fixed coordinate system A-XYZ (S A ), its origin A 0 is located at the geometric center of the static platform, and the Z-axis is upward and perpendicular to the static platform, that is, perpendicular to the sliding direction of the slider. On the moving platform of the spatial parallel equipment, establish a moving platform coordinate system B-XYZ (S B ), its origin B 0 is at the geometric center of the circumcircle of the moving platform, and the Z-axis is upward and perpendicular to the surface of the moving platform. When the moving platform is in the initial position, that is, the equilibrium position, the X, Y, and Z directions of the moving coordinate system are the same as those of the fixed coordinate system, and the Z-axis of the moving coordinate system passes through the origin A 0 of the static coordinate system. The six spherical hinge points of the static platform are represented by A i (i = 1, 2, 3, 4, 5, 6), and the spherical hinge points of the moving platform are represented by B i . θ Ai (i = 1-6) represents the distribution angle of the spherical hinge points on the static platform, and θ Bi (i = 1-6) represents the distribution angle of the spherical hinge points on the moving platform.

[0127] The known slider position is S i , and the installation angle of the hinge point on the static platform is θ Ai , the radius of the moving platform is R B , and the installation angle of the hinge point on the moving platform is θ Bi , then the spherical hinge point A on the slider of the static platform i in the fixed coordinate system S A has the coordinate vector as follows:

[0128] a i = [X Ai Y Ai Z Ai T = [S i cosθ Ai S i sinθ Ai 0] T (1)

[0129] In the formula, S i is the distance from the center of the slider on the static platform to the center of the static platform.

[0130] Furthermore, the coordinate vectors of each hinge point B on the moving platform i in the moving coordinate system S B are determined by Equation (2):

[0131] b i = [X Bi Y Bi Z Bi T = [R B cosθ Bi R B sinθ Bi 0] T (2)

[0132] In the formula, R B is the distance from the center of the slider on the moving platform to the center of the moving platform.

[0133] The motion of the equipped moving platform can be represented by six parameters. x, y, and z represent the translational motion of the moving platform in three directions, and α, β, and γ represent the rotational angles of the moving platform in three directions. In the multi-degree-of-freedom forming process, the motion equation of the upper die is:

[0134]

[0135] In the formula, is the swing angle of the upper die motion, ω is the rotational speed during the upper die motion, k is the feed rate during the upper die motion, and h is the feed distance during the upper die motion.

[0136] Thus, S​​B Relative to S A The rotation matrix R is determined by Equation (4):

[0137]

[0138] Furthermore, B i The position vector A in the global coordinate system S A b i is determined by Equation (5):

[0139] A b i = Rb i + t (5)

[0140] where t is the position vector of the origin of coordinates of S B relative to S A t can be determined by Equation (6):

[0141] t = [x, y, z] T (6)

[0142] The position vector diagram of the i-th branch chain is as shown in Figure 2 According to the closed-loop vector relationship, we can obtain:

[0143] l i = Rb i + t - a i (7)

[0144] Regarding the pose q = [x y z α β γ] T of the moving platform as the independent variable, we can obtain:

[0145] |R(q)b i + t(q) - a i | 2 - l 2 = 0 (8)

[0146] where b i is a fixed value. Substituting the slider position S i measured by the grating scale into a i , a i is also a fixed value. The unknown quantities are the pose quantities of the moving platform represented by R and t in the formula. Solving the above non-linear equations, the pose calculated based on the grating scale can be obtained.

[0147] Transforming (8) into F(q) = 0, the pose q k at time k can be obtained. The Taylor formula at the point is:

[0148] F(q) = F(q k ) + (q - q k)J(q k )+R k (9)

[0149] Wherein, J(q k ) is the Jacobian matrix of the non - linear equation set F(q), denoted as:

[0150]

[0151] According to the Newton - Raphson method, the iterative formula of the Newton method for solving F(q)=0 is:

[0152]

[0153] For the parallel mechanism of the present invention, the initial value can be determined according to the working space of the moving platform. The range of the initial value from the reasonable solution is small, and the Newton method can quickly converge to the reasonable solution.

[0154] S2. Calculate the error sensitivity of the pose based on the grating scale;

[0155] The detection error of the grating scale for the slider will have different effects on the poses of the multi - degree - of - freedom envelope forming equipment at different positions of the slider. To study its differential motion, it is first necessary to convert Equation (8) into the form of Equation (11):

[0156]

[0157] The derivative of the pose with respect to the input can represent the error sensitivity under the pose calculation method based on the grating scale measurement. The calculation formula is as follows:

[0158]

[0159] Wherein,

[0160]

[0161] For a certain moment k, the error sensitivity represents the credibility of the measurement data and is an expected value. Therefore, we choose the maximum value of the six inputs to represent the error sensitivity of this pose, denoted as:

[0162]

[0163] S3. Calculate the pose of the multi - degree - of - freedom forming equipment based on monocular vision;

[0164] The present invention adopts a monocular vision measurement system based on cooperative targets. The hardware part of the system mainly consists of an industrial camera and a cooperative target. In the measurement system, as Figure 5As shown, the cooperation target is attached to the end of the multi-degree-of-freedom forming equipment and moves with the moving platform. The industrial camera is fixed on the machine tool to collect images of the cooperation target. Based on the captured images, the pose of the multi-degree-of-freedom forming equipment is solved by the EPNP method.

[0165] As Figure 6 shown, the image is processed. To eliminate noise and highlight the features of the cooperation target, the image is first denoised. In this work, the camera has a large field of view and there are obvious local gray level changes. This may cause some features to be in areas with low gray level values. Direct processing will result in the loss of information in these areas. To solve this problem, feature enhancement is performed to increase the intensity of the feature signal and improve the signal-to-noise ratio. After preprocessing the image, adaptive threshold segmentation is performed on the image, and then the Canny operator is used for edge detection. The detected edges are tracked to extract the edge point set. Ellipse fitting is performed on the edge point set to extract the ellipse center, and the ellipse center represents the two-dimensional image coordinates of the cooperation target. The EPnP algorithm is used for pose estimation of the end relative to the camera. This method only needs to optimize 4 control points, has a fast calculation speed and high accuracy. As Figure 5 shown, the two-dimensional coordinates of the cooperation target in the camera are set as u i (i = 1…n), and finally the position vector of the cooperation target is set as p i B .

[0166] First, based on the existing measurement points, 4 virtual points are calculated and determined by Equation (14):

[0167]

[0168] where λ j is the eigenvalue of matrix A Τ A, v j is the eigenvector of matrix A Τ A. The cooperation target coordinates in the end coordinate system are represented by virtual control points as Equation (15):

[0169]

[0170] The coefficient α ij can be obtained from Equation (15) as Equation (16):

[0171]

[0172] The virtual control points represent the coordinates of the cooperation target in the camera coordinate system which can be represented by Equation (17):

[0173]

[0174] In the formula, represents the coordinates of the control points in the camera coordinate system.

[0175] According to the pinhole imaging model, it can be obtained that:

[0176]

[0177] In the formula, f u , f v is the focal length, u c , v c is the optical center coordinate, u i = [u i , v i , 1] T ,

[0178] Expanding formula (18), it can be obtained that:

[0179]

[0180] Written in matrix form, it is represented by formula (20):

[0181]

[0182] The solution C of formula (20) is in the right null space of M, and it can be obtained that:

[0183]

[0184] In the formula, v i represents the eigenvector corresponding to the zero eigenvalue of matrix M Τ M. For the pinhole imaging model, N is usually taken as 1, and formula (22) can be obtained:

[0185] C = βv (22)

[0186] From the fact that the distance between virtual control points does not change with the change of the reference coordinate system, it can be obtained that:

[0187]

[0188] β can be represented by formula (24):

[0189]

[0190] The coordinates of the four control points in the camera coordinate system can be obtained

[0191] The above steps transform the PnP problem into a 3D-3D pose estimation problem. Subsequently, the pose relationship between the moving platform coordinate system and the camera coordinate system is solved using ICP (Iterative Nearest Point)C T B The attitude of the camera coordinate system relative to the static platform coordinate system is fixed and is represented as A T C , and the pose of the moving coordinate system relative to the static coordinate system can be determined by Equation (25):

[0192] A T B = A T C · C T B (25)

[0193] Each pose component can be determined by Equation (26):

[0194]

[0195] In the formula,

[0196] S4. Error sensitivity of pose calculation based on monocular vision;

[0197] The visual estimation algorithm for the pose of the moving platform, with the input being the two-dimensional coordinates u i of the cooperative target. Compared with the true coordinates, there may be errors in these two-dimensional coordinates. Under different equipment poses, the same coordinate error will lead to different end-effector pose errors after being processed by the pose estimation algorithm.

[0198] Therefore, the derivative of the pose q = [α, β, γ, x, y, z] with respect to the input u i can be expressed as:

[0199]

[0200] Analogous to the measurement method based on the grating scale, at this time we select the maximum value of all two-dimensional coordinates to represent the pose error sensitivity at this moment. Its value represents the credibility of the pose data measured based on monocular vision, denoted as the maximum error sensitivity, and the calculation formula is as follows:

[0201]

[0202] S5. Pose data fusion based on error sensitivity;

[0203] As described above, two pose measurement methods are discussed, namely the grating scale-based measurement method and the monocular vision-based measurement method. In order to obtain highly reliable and accurate results, it is necessary to fuse the two data. The Kalman filtering method is a good method for fusing data from different sources. However, the Kalman filtering method is based on the assumption that different data sources have the same error sensitivity. However, for a multi-degree-of-freedom forming equipment, the error sensitivities of the two measurement methods also change with the change of the pose of the moving platform. Therefore, the present invention proposes an enhanced Kalman filtering method that takes into account the measurement error sensitivities under different poses.

[0204] The error sensitivity based on the grating scale measurement is represented by the vector m s and the error sensitivity based on the monocular vision measurement is represented by the vector m c , and its covariance matrix can be expressed as Q s and Q c .

[0205] At time k, the pose of the equipment is set as q k = [x, y, z, α, β, γ] Τ , and its first and second derivatives with respect to time are set as state variables, that is, X k = [q k q k 'q k '' T . Each state variable is independent, and the state equations of the two measurement methods are:

[0206]

[0207] where is the state noise vector, assumed to be Gaussian noise with zero mean and covariance. F is the state transition matrix.

[0208] Assuming the sampling time is Δt, the relationship between the poses at time k - 1 and time k can be expressed by the Taylor expansion:

[0209]

[0210] Thus, the state transition matrix F can be expressed by Equation (31):

[0211]

[0212] In order to achieve the fusion of the two data, two sub-filters are required, and the measurement equation of each sub-filter is:

[0213]

[0214] where q s , q care the measured pose values of sub - filters 1 and 2, obtained from the grating scale position and monocular vision measurement. is the measurement matrix. is the error sensitivity of different measurement methods, which represents the credibility of measurement data.

[0215] Figure 9 represents the overall process of the proposed enhanced Kalman filtering method. Sub - filters 1 and 2 respectively receive the pose data and measurement noise from the grating scale measurement subsystem and the monocular vision measurement subsystem, and obtain the state vector and the state covariance matrix P 1 , P 2 . The main filter first performs a time update to obtain the state vector and the state covariance matrix P m , then the main filter combines all the obtained state vectors and state covariance matrices to obtain the state vector of the globally optimal estimate and the state covariance matrix P g .

[0216] The specific implementation steps are as follows:

[0217] (1) The initial values of the three filters are obtained from the optimal estimate of the previous moment:

[0218]

[0219] where λ i is the distribution coefficient, λ i > 0 and Σλ i = 1.

[0220] (2) The state vectors and the state covariance matrices P 1 , P 2 , P m of the three filters:

[0221]

[0222] where j = 1, 2 represents sub - filters 1 and 2, and the main filter only has the first two steps.

[0223] (3) The optimal estimate of the equipment pose:

[0224]

[0225] Thus, we obtain the optimal fusion solution based on the two - pose data.

[0226] Embodiment

[0227] Based on the formula (12), formula (13) and the equipment configuration parameters given in Table 1, the error sensitivities and the maximum error sensitivity of each slider for pose calculation based on the grating scale can be calculated, as Figure 6 , Figure 7 shown. According to formula (27) and formula (28), the error sensitivities and the maximum error sensitivity of each cooperative target for pose calculation based on monocular vision can be calculated, as Figure 8 , Figure 9 shown.

[0228] Table 1 Equipment configuration parameters

[0229]

[0230] Based on formula (10), formula (26) and formula (35) and the Figure 10 experimental platform shown, the pose errors of the moving platform under each sensor and different fusion algorithms in a complete motion cycle are obtained as Figure 11 shown. The X-axis represents the motion time of the equipment in one cycle, the Y-axis represents the error scale, black represents the error measured by the grating scale, blue represents the error measured by monocular vision, green represents the error of the traditional federated Kalman filtering method, and red represents the error of the improved federated Kalman filtering method considering error sensitivity. Generally speaking, compared with the traditional federated Kalman filtering method, the average root mean square error of the position error of the equipment in the x, y, and z directions is reduced from 0.569 mm to 0.293 mm, a reduction of 48%, and the average maximum error is reduced from 2.09 mm to 0.79 mm, a reduction of 62%. For the angle error of the equipment, the average root mean square error of α, β, and γ is reduced from 0.251° to 0.144°, a reduction of 42%, and the average maximum error is reduced from 0.7° to 0.42°, a reduction of 40%. In each direction, the average error of the improved federated Kalman filtering method is closer to zero. Therefore, the improved federated Kalman filtering method proposed by the present invention has higher measurement accuracy and better stability.

[0231] The embodiments of the present invention have been described above in conjunction with the accompanying drawings. However, the present invention is not limited to the above specific embodiments. The above specific embodiments are merely illustrative and not restrictive. Under the inspiration of the present invention, those of ordinary skill in the art can also make many forms without departing from the purpose and scope protected by the claims of the present invention. These all belong to the protection scope of the present invention.

Claims

1. A method for fusion of posture perception data of multi-degree-of-freedom forming equipment considering error sensitivity, wherein the multi-degree-of-freedom forming equipment realizes multi-degree-of-freedom motion by coupling motion of multiple motion branches, and a grating ruler is installed on the active branch of the multi-degree-of-freedom forming equipment, characterized in that: The following steps are involved: S1. Calculate the position and posture of multi-degree-of-freedom forming equipment based on the grating ruler; S2, obtaining the error sensitivity of the calculated posture based on the grating ruler; S3, calculate the posture of multi-degree-of-freedom forming equipment based on monocular vision; S4, obtaining the error sensitivity of the pose calculated based on monocular vision; S5. Considering the error sensitivity of the two pose calculation methods in steps S3 and S4, pose data fusion is performed.

2. The multi-degree-of-freedom forming equipment posture perception data fusion method considering error sensitivity according to claim 1 is characterized in that: In the step S1, the method for calculating the position and posture of the multi-degree-of-freedom forming equipment based on the grating ruler includes establishing two coordinate systems, namely a moving coordinate system and a fixed coordinate system: A fixed coordinate system A-XYZ (S) is constructed on the static platform surface of the multi-degree-of-freedom forming equipment. A ), whose origin A0 is located at the geometric center of the static platform, and the Z axis is perpendicular to the static platform, that is, perpendicular to the sliding direction of the slider; the dynamic platform coordinate system B-XYZ (S B ), its origin B0 is at the geometric center of the circumscribed circle of the moving platform, and the Z axis is perpendicular to the surface of the moving platform; when the moving platform is in the initial position, that is, the equilibrium position, the moving coordinate system is consistent with the X, Y, and Z directions of the fixed coordinate system, and the Z axis of the moving coordinate system passes through the origin A0 of the static coordinate system; the six spherical joints of the static platform are A i (i=1,2,3,4,5,6), the ball joint of the moving platform is represented by B i Indicates. θ Ai (i=1-6) represents the distribution angle of the spherical joint points on the static platform, θ Bi (i=1-6) represents the distribution angle of the spherical joints on the moving platform.

3. The multi-degree-of-freedom forming equipment posture perception data fusion method considering error sensitivity according to claim 2 is characterized in that: In the step S1, the method for calculating the posture of the multi-degree-of-freedom forming equipment based on the grating ruler also includes: The slider position is known to be S i , the installation angle of the static platform hinge is θ Ai , the radius of the moving platform is R B , the installation angle of the moving platform hinge point θ Bi , then the ball joint point A on the static platform slider i In the fixed system S A The coordinate vector in is: a i =[X Ai Y Ai Z Ai ] T =[S i cosθ Ai S i sinθ Ai 0] T (1) In the formula, S i is the distance from the center of the static platform slider to the center of the static platform; Then, each hinge point B of the moving platform i In the dynamic system S B The coordinate vector in is determined by formula (2): b i =[X Bi Y Bi Z Bi ] T =[R B cosθ Bi R B sinθ Bi 0] T (2) In the formula, R B is the distance from the center of the moving platform slider to the center of the moving platform; x, y, z represent the translational motion of the moving platform in three directions, α, β, γ represent the rotation angles of the moving platform in three directions; in the multi-degree-of-freedom forming process, the motion equation of the upper die is: In the formula, is the swing angle of the upper die movement, ω is the rotation speed of the upper die during the movement, k is the feed rate during the movement of the upper die, and h is the feed distance during the movement of the upper die; Thus S B Relative to S A The rotation matrix R is determined by formula (4): Furthermore, B i In the global coordinate system S A Position vector A b i Determined by formula (5): A b i =Rb i +t (5) Where t is S B The coordinate origin is relative to S A The position vector of ; t is determined by formula (6): t=[x,y,z] T (6) According to the closed-loop vector relationship, we can get: l i =Rb i +t-a i (7) The dynamic platform posture q = [xy zαβγ] T As an independent variable, we get: |R(q)b i +t(q)-a i | 2 -l 2 =0 (8) In the formula, b i As a fixed value, the grating ruler is used to measure the slider position S i Substitute a i , a i is also a constant. The unknown quantity is the posture of the moving platform represented by R and t in the formula. By solving the above nonlinear equations, the posture calculated based on the grating ruler can be obtained. Transforming (8) to F(q) = 0, we can obtain the pose q at time k k The point Taylor formula is: F(q)=F(q k )+(q-q k )J(q k )+R k (9) In the formula, J(q k ) is the Jacobian matrix of the nonlinear equation system F(q), denoted as: The Newton method iteration formula for solving F(q)=0 is:

4. The multi-degree-of-freedom forming equipment posture perception data fusion method considering error sensitivity according to claim 3 is characterized in that: The step S2 comprises the following steps: Convert equation (8) into equation (11): f i (q,S i )=l i 2 -|R(α,β,γ)b i +t(x,y,z)-a i (S i ) 2 =0 (11) The derivative of the pose to the input represents the error sensitivity based on the grating ruler measurement pose calculation method. The calculation formula is as follows: In the formula, For a certain moment k, the maximum value of the input is selected to represent the error sensitivity of this pose, expressed as:

5. The multi-degree-of-freedom forming equipment posture perception data fusion method considering error sensitivity according to claim 4 is characterized in that: The step S3 comprises the following steps: Set the 2D coordinate of the cooperation target in the camera to u i (i=1…n), and finally the position vector of the collaborative target is set to p i B ; Based on the existing measurement points, four virtual points are calculated and determined by formula (14): In the formula, λ j A is the matrix Τ A eigenvalue, v j is the matrix A Τ The eigenvector of A; The coordinates of the cooperative target in the terminal coordinate system are expressed by virtual control points as shown in formula (15): From formula (15), we can get the coefficient α ij As shown in formula (16): The virtual control point represents the coordinates of the cooperative target in the camera coordinate system It can be expressed by formula (17): In the formula, Represents the coordinates of the control point in the camera coordinate system; According to the pinhole imaging model, it can be concluded that: In the formula, f u ,f v is the focal length, u c ,v c is the optical center coordinate, u i =[u i ,v i ,1] T , Expanding formula (18), we can obtain: The matrix form is expressed by formula (20): The solution C of formula (20) is in the right null space of M, and we can get: In the formula, v i Represents the matrix M Τ M is the eigenvector of zero eigenvalue; for the pinhole imaging model, N is usually 1, and equation (22) can be obtained: C=βv (22) Since the distance between virtual control points does not change with the reference coordinate system, we can get: β can be expressed by formula (24): The coordinates of the four control points in the camera coordinate system are obtained Use ICP to solve the pose relationship between the moving platform coordinate system and the camera coordinate system C T B ; The posture of the camera coordinate system relative to the static platform coordinate system is fixed and is expressed as A T C , the position and posture of the moving coordinate system relative to the static coordinate system can be determined by formula (25): A T B = A T C · C T B (25) Each pose component can be determined by formula (26): In the formula, 6. The multi-degree-of-freedom forming equipment posture perception data fusion method considering error sensitivity according to claim 5 is characterized in that: The step S4 comprises the following steps: Pose q = [α, β, γ, x, y, z] for input u i The derivative of is expressed as: The maximum value of all two-dimensional coordinates is selected to represent the posture error sensitivity at this moment. Its value represents the credibility of the posture data based on monocular vision measurement, which is recorded as the maximum error sensitivity. The calculation formula is as follows:

7. The multi-degree-of-freedom forming equipment posture perception data fusion method considering error sensitivity according to claim 6 is characterized in that: The step S5 comprises the following steps: The error sensitivity based on the grating ruler measurement is expressed as vector m s The error sensitivity based on monocular vision measurement is represented by vector m c Represents, and its covariance matrix is ​​represented by Q s and Q c ; At time k, the equipment posture is set to q k =[x,y,z,α,β,γ] Τ , its first-order derivative and second-order derivative with respect to time are set as state variables, that is, X k =[q k q k ′q k ″] T Each state variable is independent, and the state equations for the two measurement methods are: In the formula, is the state noise vector, assumed to be a Gaussian noise with a mean of zero and covariance, and F is the state transfer matrix; Assuming the sampling time is Δt, the relationship between the posture at time k-1 and at time k is expressed by Taylor expansion: Therefore, the state transfer matrix F is expressed by formula (31): In order to achieve the fusion of the two types of data, two sub-filters are required, and the measurement equation of each sub-filter is: In the formula, q s ,q c is the measured value of the pose of sub-filters 1 and 2, obtained by the grating ruler position and monocular vision measurement, is the measurement matrix, is the error sensitivity of different measurement methods, which represents the reliability of the measurement data; The two sub-filters receive the position data and measurement noise based on the grating ruler measurement subsystem and the monocular vision measurement subsystem respectively, and obtain the state vector and the state covariance matrix P 1 ,P 2 ; The main filter first performs a time update to obtain the state vector and the state covariance matrix P m , and then the main filter combines all the state vectors and state covariance matrices to obtain the global optimal estimated state vector and the state covariance matrix P g .

8. The method for fusion of multi-degree-of-freedom forming equipment posture perception data considering error sensitivity according to claim 7 is characterized in that: The main filter combines all the state vectors and state covariance matrices to obtain the globally optimal estimated state vector and the state covariance matrix P g The methods include: (1) The initial values ​​of the three filters are obtained from the optimal estimate at the previous moment: In the formula, λ i is the distribution coefficient, λ i >0 and ∑λ i =1; (2) Three filter state vectors and the state covariance matrix P 1 ,P 2 ,P m : In the formula, represents sub-filters 1 and 2; (3) Optimal estimation of equipment posture:

Images