A motion control method for a five-axis linkage drilling and tapping center

Abstract

The present invention provides a five-axis linkage drilling and tapping center motion control method, which belongs to the field of numerical control system control robots. The five-axis linkage drilling and tapping center includes a slide and a manipulator horizontally movably arranged on the slide. The manipulator adopts a double-link double-swing head mechanism to form a five-axis linkage mechanism with the slide. The kinematic positive solution of the five-axis linkage mechanism is solved according to the kinematic model, including step S11, calculating the coordinate transformation matrix from the third joint to the sixth joint according to the formula; step S12, calculating the coordinate transformation matrix from the first joint to the third joint according to the formula; step S13, calculating the coordinate transformation matrix from the first joint to the third joint according to the formula, and then obtaining the total kinematic equation describing the spatial posture of the end of the manipulator. The present invention has lower cost and reduces the difficulty of mechanical design.

Description

Technical Field

[0001] The invention belongs to the field of numerical control system control robots, and in particular relates to a motion control method for a five-axis linkage drilling and tapping center. Background Art

[0002] Compared with traditional processing machines, processing robots have the advantages of high flexibility, high agility, low cost, and a larger processing space range, so they are more suitable for small and medium-sized enterprises with diversified processing needs and smaller revenue scales. With the development of robot technology, the field of robot drilling and tapping mainly uses six-joint robots. Six-joint robots are often accompanied by the moving axis of the handling robot during processing, which means that the six-joint robot drilling and tapping has eight motors. The first joint and the moving axis of the handling cross function, resulting in redundant axis waste, and the fifth joint must meet the requirement of no vertical component with the second and third joints, which makes the design difficult and the fifth joint has low rigidity. Summary of the invention

[0003] The purpose of the present invention is to provide a five-axis linkage drilling and tapping center motion control method, which has lower cost and reduces the difficulty of mechanical design.

[0004] The present invention is achieved through the following technical solutions:

[0005] A motion control method for a five-axis linkage drilling and tapping center, the five-axis linkage drilling and tapping center includes a slide and a manipulator horizontally movably arranged on the slide, the manipulator adopts a double-link double-swing head mechanism, and forms a five-axis linkage mechanism with the slide. The motion control method includes:

[0006] The kinematic model of the five-axis linkage mechanism is established using the DH method. The slide is the first joint, the double links are the second and third joints, the double swing heads are the fourth and fifth joints, and a non-rotatable fixed connecting rod connected to the fifth joint is set as the auxiliary sixth joint.

[0007] The end spatial posture of the five-axis linkage mechanism is obtained through kinematics positive solution, including the following steps:

[0008] Step S11: According to the formula Calculate the coordinate transformation matrix from the third joint to the sixth joint ,in, , represents the coordinate transformation matrix from the third joint to the fourth joint, represents the coordinate transformation matrix from the fourth joint to the fifth joint, Represents the coordinate transformation matrix from the fifth to the sixth joint;

[0009] Step S12: According to the formula Calculate the coordinate transformation matrix from the first joint to the third joint , where the second joint and the third joint are parallel, calculate the coordinate transformation matrix Apply the angle formula in the process, represents the coordinate transformation matrix from the first joint to the second joint, Represents the coordinate transformation matrix from the second joint to the third joint;

[0010] Step S13: According to the formula Calculate the coordinate transformation matrix from the first joint to the third joint , and then the total kinematic equation describing the spatial position of the manipulator end is obtained , Represents the coordinate transformation matrix from the base coordinate system to the first joint.

[0011] Furthermore, in step S11, the coordinate transformation matrix from the fourth joint to the sixth joint is Expressed as , then the coordinate transformation matrix from the third joint to the sixth joint is Expressed as ,in, , , Indicates i The current angles of the joint motors of the joints, d i Indicates i The length of the joint, i =1,2,…,6.

[0012] Furthermore, in step S12, the coordinate transformation matrix from the first joint to the third joint is Expressed as ,in, , , a 2 represents the joint rotation of the second joint.

[0013] Furthermore, in step S13, the coordinate transformation matrix from the first joint to the sixth joint is Expressed as , the overall kinematic equation is expressed as The manipulator spatial position includes the X component, Y component and Z component of the manipulator spatial position, as well as the manipulator posture. r 14 Equal, the Y component and r 24 Equal, the Z component is r 34 Equally, the posture of the manipulator is given by the matrix Find, among which, , , , , , , , , , , , , , , , , , , , , , , .

[0014] Furthermore, the posture of the end of the manipulator is represented by angle C, angle B, and angle A, wherein angle C is the angle of rotation of the manipulator around the Z component, angle B is the angle of rotation of the manipulator around the Y component, and angle A is the angle of rotation of the manipulator around the X component. Angle C is represented by The non-constant angle is determined by the matching relationship. In actual application, "XYZ-BA" is output to avoid wasting the number of equipment axes and programming names.

[0015] Furthermore, the motion control method further includes obtaining the joint target angle corresponding to the spatial target point of the five-axis linkage mechanism by kinematic inverse solution, including the following steps:

[0016] Step S21: Obtain the total kinematic equation of the manipulator's spatial posture according to the current posture XYZ-CBA of the manipulator end. , and then get ,in, , , , , , , , , , , , , C Indicates the angle of rotation of the robot around the Z coordinate axis. B Indicates the angle of rotation of the robot around the Y coordinate axis, A Indicates the angle of rotation of the robot around the X axis. l 1 indicates the distance the manipulator moves horizontally on the slide;

[0017] Step S22: Avoid l 1 Related impact items, according to the formula get and , record the C value at the previous motion point, substitute it into the joint position obtained by the previous kinematic inverse solution or the known current joint position to calculate the new C value, so as to obtain a When the CBA value of the target point near the space is 0°, we get and ;

[0018] Step S23: Formula and Add the squares of the right sides and use the auxiliary angle formula to get , then the joint zero point of the third joint is obtained The first solution And the second solution , in the first solution and the second solution, take the solution that is closer to the current angle value of the third joint of the manipulator as The only solution of ;

[0019] Step S24: Substitute into the formula and , using the trigonometric auxiliary angle formula, we can get the simultaneous formula group , solving the simultaneous formulas yields ,in, , ;

[0020] Step S25: and Get the coordinate transformation matrix from the third joint to the sixth joint ,when c When the absolute value of 5 is 1, that is i 5 When 0° or ±180°, i 4No change, i 5. Determine the unique solution based on the current position of the fifth joint;

[0021] when c When the absolute value of 5 is not 1, that is s 5 is not 0, according to the formula The relationship between the third row and the third column and the first row and the first column in can be solved to get i 4. Substitution i 4 can be found s The value of 5 is obtained from this i 5;

[0022] Step S26: Substitute into the formula In p y To seekl 1.

[0023] Furthermore, in the kinematic model, the transformation of two adjacent joint coordinate systems is described by a relative moving coordinate system and adopts the right multiplication rule.

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

[0025] 1. The five-axis linkage drilling and tapping center of the present invention includes a slide and a manipulator horizontally movably arranged on the slide. The manipulator adopts a double-link double-swing head mechanism to form a five-axis linkage mechanism with the slide. In motion control, the DH method is used to establish a kinematic model. The slide is the first joint, the double links are the second joint and the third joint, the double swing heads are the fourth joint and the fifth joint, and an auxiliary sixth joint is set. In the kinematic forward solution, first according to the formula Calculate the coordinate transformation matrix from the third joint to the sixth joint , then according to the formula Calculate the coordinate transformation matrix from the first joint to the third joint Finally, according to the formula Calculate the coordinate transformation matrix from the first joint to the third joint , and then the total kinematic equation describing the manipulator's spatial position is obtained In this way, the control of the five-axis linkage machine tool mechanism is realized, and a double-link double-swing head manipulator is adopted, which has lower cost. The manipulator is slidably set on the slide, and a design deviation is allowed between the fifth joint and the fourth joint, so that the problems of high machining precision requirements and insufficient rigidity in the prior art are improved, thereby reducing the difficulty of mechanical design.

[0026] 2. In the kinematic inverse solution of the present invention, the C value at the previous motion point is recorded and substituted into the joint position obtained by the previous kinematic inverse solution or the known current joint position to obtain a new C value, so as to obtain a The CBA value of the target point in space when it is 0° is obtained. In this way, the result of the iterative solution with large amount of calculation in the prior art is achieved with smaller amount of calculation, so that the control algorithm can run more stably, and the number of programming variables is 5, which does not cause waste of programming axes. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] The present invention is further described in detail below with reference to the accompanying drawings.

[0028] Figure 1 It is a structural schematic diagram of the five-axis linkage mechanism of the present invention.

[0029] Figure 2 It is a schematic diagram of the kinematic model of the five-axis linkage mechanism of the present invention.

[0030] Among them, 1. slide; 2. manipulator; 3. first joint; 4. second joint; 5. third joint; 6. fourth joint; 7. fifth joint. DETAILED DESCRIPTION

[0031] like Figure 1 and Figure 2 As shown, a motion control method for a five-axis linkage drilling and tapping center is shown, wherein the five-axis linkage drilling and tapping center comprises a slide 1 and a manipulator 2 horizontally movably arranged on the slide 1, wherein the manipulator 2 adopts a double-link double-swing head mechanism to form a five-axis linkage mechanism with the slide 1, and the motion control method comprises:

[0032] The kinematic model of the five-axis linkage mechanism is established by the DH method. The slide 1 is the first joint 3, the double links are the second joint 4 and the third joint 5, the double swing heads are the fourth joint 6 and the fifth joint 7, and a non-rotatable fixed connecting rod connected to the fifth joint is set as the auxiliary sixth joint.

[0033] In the kinematic model, the DH parameters are shown in Table 1, where: i Indicates the joint number, a i-1 For the i Joints and i -1 The vertical distance of the joint rod, For the i Joints and i -1 torsion angle of the joint member, d i For the i The length of the joint, i i For the i The current angles of the joint motors of each joint;

[0034] Table 1

[0035]

[0036] The transformation of two adjacent joint coordinate systems is described by the relative moving coordinate system and the right multiplication rule is adopted. The specific transformation matrix is ​​expressed as , the kinematic equation is expressed as .

[0037] The motion control method includes obtaining the end spatial posture of the five-axis linkage mechanism by positive solution, including the following steps:

[0038] Step S11, obtain the change matrix of each joint from the transformation matrix of two adjacent joint coordinate systems:

[0039] , and then the total transformation matrix is ,in, , , Indicates i The current angles of the joint motors of the joints, d i Indicates i The length of the joint, i =1,2,…,6;

[0040] According to the formula Calculate the coordinate transformation matrix from the third joint 5 to the sixth joint ,in, , represents the coordinate transformation matrix from the third joint 5 to the fourth joint 6, represents the coordinate transformation matrix from the fourth joint 6 to the fifth joint 7, Represents the coordinate transformation matrix from the fifth to the sixth joint;

[0041] More specifically, the coordinate transformation matrix from the fourth joint 6 to the sixth joint is Expressed as , then the coordinate transformation matrix from the third joint 5 to the sixth joint is Expressed as ;

[0042] Step S12: According to the formula Calculate the coordinate transformation matrix from the first joint 3 to the third joint 5 , where the second joint 4 and the third joint 5 are parallel, calculate the coordinate transformation matrix Apply the angle formula in the process, represents the coordinate transformation matrix from the first joint 3 to the second joint 4, Represents the coordinate transformation matrix from the second joint 4 to the third joint 5;

[0043] Specifically, the coordinate transformation matrix from the first joint 3 to the third joint 5 is Expressed as ,in, , , a 2 represents the joint rotation angle of the second joint;

[0044] Step S13: According to the formula Calculate the coordinate transformation matrix from the first joint 3 to the sixth joint , and then the total kinematic equation describing the spatial position of the end of manipulator 2 is obtained , Represents the coordinate transformation matrix from the base coordinate system to the first joint, where the base coordinate system is a fixed coordinate system at a fixed position on the machine tool;

[0045] Specifically, the coordinate transformation matrix from the first joint 3 to the sixth joint is Expressed as , the overall kinematic equation is expressed as ,in, , , , , , , , , , , , , , , , , , , , , , , .

[0046] The position and posture of the end of the manipulator 2 in space include the X component, Y component and Z component of the spatial position of the end of the manipulator 2, and the posture of the end of the manipulator 2. r 14 Equal, the Y component and r 24 Equal, the Z component is r 34 Equally, the posture of manipulator 2 is given by the matrix The posture of the end of the manipulator 2 is represented by angle C, angle B, and angle A. Angle C is the angle of rotation of the manipulator 2 around the Z component, angle B is the angle of rotation of the manipulator 2 around the Y component, and angle A is the angle of rotation of the manipulator 2 around the X component. When solving the posture CBA, when the angle of the sixth joint changes, it only affects the C angle. In the application scenarios of drilling or spindle grinding, the sixth joint is the spindle motor, not the robot joint motor, so the sixth joint is always 0°. At this time, angle C is calculated by It is determined by the matching relationship, and its non-constant angle, and the change of C angle does not affect the tool vector value. In the actual application process, "XYZ-BA" is output to avoid wasting the number of equipment axes and programming names, affecting actual use.

[0047] The motion control method also includes solving the joint target angle corresponding to the spatial target point of the motion five-axis linkage mechanism according to the kinematic inverse solution, including the following steps:

[0048] Step S21: Obtain the total kinematic equation of the spatial posture of the manipulator 2 according to the known current posture XYZ-CBA of the end of the manipulator 2 ,Will Moving to the left side of the equation, we get ,in, , , , , , , , , , , , , C Indicates the angle of rotation of robot 2 around the Z coordinate axis, B Indicates the angle of rotation of robot 2 around the Y coordinate axis, A Indicates the angle of rotation of robot 2 around the X coordinate axis, l 1 represents the horizontal movement distance of manipulator 2 on slide 1;

[0049] Step S22: l 1 Only for p y have an impact and Related, need to be solved last, that is, avoid l 1 Related impact items, according to the formula get and , because the rotation matrix inversely solved during the movement of the robot The result is no longer 0°, but The affected part Since there is no given C value, in order to make the inverse solution feasible, it is necessary to record the C value at the previous motion point and substitute it into the joint position obtained by the previous kinematic inverse solution or the known current joint position to obtain a new C value. When the CBA value of the target point near the space is 0°, we get and The spatial target point is the point formed by XYZ-BA after substituting the inverse solution. At this time, the unknown moving target angle But given the target XYZ-BA, we need to find the moving target angle ;

[0050] Step S23, formula and In the equation, the left side of the equal sign is a known value. and Add the squares of the right sides and use the auxiliary angle formula to get , then the joint zero point of the third joint is obtained The first solution And the second solution In the first solution and the second solution, the solution that is closer to the current angle value of the third joint 5 of the manipulator 2 is taken as The only solution of ;

[0051] Step S24: Substitute into the formula and , using the trigonometric auxiliary angle formula, we can get the simultaneous formula group , solving the simultaneous formulas yields ,in, , ;

[0052] Step S25: and Get the coordinate transformation matrix from the third joint 5 to the sixth joint ,when c When the absolute value of 5 is 1, that is i 5 When 0° or ±180°, i 4No change, i 5. Determine the unique solution based on the current position of the fifth joint, that is, the angle that is closer to the current position of the fifth joint is i Solution for 5;

[0053] when c When the absolute value of 5 is not 1, that is s 5 is not 0, according to the formula The relationship between the third row and the third column and the first row and the first column in can be solved to get i 4. Substitution i 4 can be found s The value of 5 is obtained from this i 5;

[0054] Step S26: Substitute into the formula In p y To seek l 1.

[0055] The above description is only a preferred embodiment of the present invention, and therefore cannot be used to limit the scope of implementation of the present invention. That is, equivalent changes and modifications made according to the application scope of the present invention and the contents of the specification should still fall within the scope of the present invention.

Claims

1. A five-axis linkage drilling and tapping center motion control method, characterized in that: The five-axis linkage drilling and tapping center includes a slide and a manipulator horizontally arranged on the slide. The manipulator adopts a double-link double-swing head mechanism to form a five-axis linkage mechanism with the slide. The motion control method includes: The kinematic model of the five-axis linkage mechanism is established using the DH method. The slide is the first joint, the double links are the second and third joints, the double swing heads are the fourth and fifth joints, and a non-rotatable fixed connecting rod connected to the fifth joint is set as the auxiliary sixth joint. The end spatial posture of the five-axis linkage mechanism is obtained through kinematics positive solution, including the following steps: Step S11: According to the formula Calculate the coordinate transformation matrix from the third joint to the sixth joint ,in, , represents the coordinate transformation matrix from the third joint to the fourth joint, represents the coordinate transformation matrix from the fourth joint to the fifth joint, Represents the coordinate transformation matrix from the fifth to the sixth joint; Step S12: According to the formula Calculate the coordinate transformation matrix from the first joint to the third joint , where the second joint and the third joint are parallel, calculate the coordinate transformation matrix Apply the angle formula in the process, represents the coordinate transformation matrix from the first joint to the second joint, Represents the coordinate transformation matrix from the second joint to the third joint; Step S13: According to the formula Calculate the coordinate transformation matrix from the first joint to the third joint , and then the total kinematic equation describing the spatial position of the manipulator end is obtained , represents the coordinate transformation matrix from the base coordinate system to the first joint; in step S11, the coordinate transformation matrix from the fourth joint to the sixth joint Expressed as , then the coordinate transformation matrix from the third joint to the sixth joint is Expressed as ,in, , , Indicates i The current angles of the joint motors of the joints, d i Indicates i The length of the joint, i =1,2,…,6; In step S12, the coordinate transformation matrix from the first joint to the third joint Expressed as ,in, , , a 2 represents the joint rotation angle of the second joint; in step S13, the coordinate transformation matrix from the first joint to the sixth joint Expressed as , the overall kinematic equation is expressed as The manipulator spatial position includes the X component, Y component and Z component of the manipulator spatial position, as well as the manipulator posture. r 14 Equal, the Y component and r 24 Equal, the Z component is r 34 Equally, the posture of the manipulator is given by the matrix Find, among which, , , , , , , , , , , , , , , , , , , , , , , ,in, l 1 indicates the distance the manipulator moves horizontally on the slide.

2. A five-axis linkage drilling and tapping center motion control method according to claim 1, characterized in that: The posture of the end of the manipulator is represented by angle C, angle B, and angle A. Angle C is the angle of rotation of the manipulator around the Z component, angle B is the angle of rotation of the manipulator around the Y component, and angle A is the angle of rotation of the manipulator around the X component. Angle C is represented by The non-constant angle is determined by the matching relationship. In actual application, "XYZ-BA" is output to avoid wasting the number of equipment axes and programming names.

3. A five-axis linkage drilling and tapping center motion control method according to any one of claims 1 to 2, characterized in that: The motion control method further includes obtaining the joint target angle corresponding to the spatial target point of the five-axis linkage mechanism through kinematic inverse solution, including the following steps: Step S21: Obtain the total kinematic equation of the manipulator's spatial posture according to the current posture XYZ-CBA of the manipulator end. , and then get ,in, , , , , , , , , , , , , C Indicates the angle of rotation of the robot around the Z coordinate axis. B Indicates the angle of rotation of the robot around the Y coordinate axis, A Indicates the angle of rotation of the robot around the X axis. l 1 indicates the distance the manipulator moves horizontally on the slide; Step S22: Avoid l 1 Related impact items, according to the formula get and , record the C value at the previous motion point, substitute it into the joint position obtained by the previous kinematic inverse solution or the known current joint position to find a new C value, so as to obtain a When the CBA value of the target point near the space is 0°, we get and ; Step S23: Formula and Add the squares of the right sides and use the auxiliary angle formula to get , then the joint zero point of the third joint is obtained The first solution And the second solution , in the first solution and the second solution, take the solution that is closer to the current angle value of the third joint of the manipulator as The only solution of ; Step S24: Substitute into the formula and , using the trigonometric auxiliary angle formula, we can get the simultaneous formula group , solving the simultaneous formulas yields ,in, , ; Step S25: and Get the coordinate transformation matrix from the third joint to the sixth joint ,when c When the absolute value of 5 is 1, that is θ 5 When 0° or ±180°, θ 4No change, θ 5. Determine the unique solution based on the current position of the fifth joint; when c When the absolute value of 5 is not 1, that is s 5 is not 0, according to the formula The relationship between the third row and the third column and the first row and the first column in can be solved to get θ 4. Substitution θ 4 can be found s The value of 5 is obtained from this θ 5; Step S26: Substitute into the formula In p y To seek l 1.

4. A five-axis linkage drilling and tapping center motion control method according to any one of claims 1 to 2, characterized in that: In the kinematic model, the transformation of two adjacent joint coordinate systems is described by a relative moving coordinate system and adopts the right multiplication rule.

Images