Method and device for acquiring singular pose of Cartesian trajectory tracking of industrial mechanical arm

Abstract

The invention provides a singular pose obtaining method and device for Cartesian trajectory tracking of an industrial mechanical arm, the industrial mechanical arm comprises a joint 1 to a joint 6, the joint angle of each joint is theta 1 to theta 6, and the method comprises the steps that a forward kinematics equation for Cartesian trajectory tracking of the industrial mechanical arm is established based on a standard DH method; according to the forward kinematics equation, a basic analytic expression of the joint angle of each joint is determined, wherein the basic analytic expression comprises a first intermediate parameter and a second intermediate parameter related to connecting rod parameters of the industrial mechanical arm; solving theta2 to theta5 according to the basic analytic expression for the joints 2 to 5; and for the joint 1 or 6, the first intermediate parameter and the second intermediate parameter are adjusted based on the singular pose, a singular analytic expression is obtained, and theta1 or theta6 is solved according to the singular analytic expression. And a real inverse solution of the singular pose can be accurately obtained, so that pose errors caused by sudden change of a motion path at the tail end of the industrial mechanical arm when the industrial mechanical arm tracks and passes through the Cartesian trajectory of the singular pose are avoided.

Description

Technical Field

[0001] The present application belongs to the technical field of inverse kinematics analysis, and in particular, relates to a method and device for acquiring singular postures of Cartesian trajectory tracking of an industrial robot arm. Background Art

[0002] Singularity is an inherent characteristic of mechanical structure, which has a great impact on the continuous Cartesian trajectory tracking task of industrial robot arms. When the end of the industrial robot arm passes through a singular posture, there may be an infinite set of inverse solutions. The existing inverse solution methods can obtain one of the inverse solutions, but they are more suitable for point-to-point tracking tasks. Because this type of task does not require the motion trajectory of the end of the industrial robot arm between the starting point and the end point, it is only necessary to perform joint interpolation based on the obtained inverse solution to make the reference trajectory for joint control smooth and continuous. For Cartesian trajectory tracking tasks, the reference trajectory for joint space control must be composed of the inverse solution of each sampling point in the target Cartesian trajectory. However, it is difficult for the existing inverse solution methods to find the "true solution" that makes the joint trajectory continuous and smooth in the infinite set of inverse solutions. If the inverse solution of the singular posture deviates from the "true solution", it will cause a sudden change in the actual motion trajectory of the end of the industrial robot arm, which will reduce the accuracy of Cartesian trajectory tracking at the least, or cause the robot arm to collide with the surrounding environment at the worst. Summary of the invention

[0003] The main purpose of the embodiments of the present invention is to provide a method and device for obtaining singular postures of Cartesian trajectory tracking of an industrial robot arm, which can solve the problem that it is difficult for existing inverse solution methods to find a "real solution" that makes the joint trajectory continuous and smooth in an infinite set of inverse solutions, and can accurately obtain the real inverse solution of the singular posture, so that the end of the industrial robot arm will not have posture errors caused by sudden changes in the motion path when tracking the Cartesian trajectory passing through the singular posture.

[0004] In a first aspect, a method for acquiring a singular posture of a Cartesian trajectory tracking of an industrial robot arm is provided. The industrial robot arm includes joints 1 to 6, and the joint angles of each joint are θ1 to θ6. The method includes: establishing a forward kinematic equation for Cartesian trajectory tracking of the industrial robot arm based on a standard DH method; determining a basic analytical expression for the joint angles of each joint according to the forward kinematic equation, wherein the basic analytical expression includes a first intermediate parameter and a second intermediate parameter related to the connecting rod parameters of the industrial robot arm; for joints 2 to 5, solving the joint angles θ2 to θ5 according to the basic analytical expression; for joints 1 or 6, adjusting the first intermediate parameter and the second intermediate parameter based on the singular posture to obtain a singular analytical expression, and solving the joint angle θ1 or the joint angle θ6 according to the singular analytical expression.

[0005] In a possible implementation, the basic analytical expressions of the joint angles of each joint include:

[0006] θ1=arctan2(m1,n1)+arctan2(±1,0)

[0007] θ2=arctan2(m2,n2)

[0008] θ3=arctan2(±m3,n3)-arctan2(d4,a3)

[0009] θ6=arctan2(±1,0)+arctan2(m6,n6)

[0010] θ4=arctan2(0,-1)+arctan2(m4,n4)

[0011] θ5=arctan2(0,1)+arctan2(m5,n5)

[0012] or

[0013] θ4=arctan2(0,1)+arctan2(m4,n4)

[0014] θ5=arctan2(0,-1)+arctan2(m5,n5)

[0015] In the formula, m i and n i (i=1,2,3,4,5,6) are related to the connecting rod parameters of the industrial robot arm and are used to solve θ i The first intermediate parameter and the second intermediate parameter, a3 is the vertical distance between the axes of joint 3 and joint 4 of the industrial robot arm, and d4 is the vertical distance between the axis of joint 4 and the axis of joint 5.

[0016] In another possible implementation, adjusting the first intermediate parameter and the second intermediate parameter according to the singular posture to obtain a singular analytical expression includes:

[0017] For joints 1 and 6, the first intermediate parameter and the second intermediate parameter are updated to the k-order derivative form according to the singular posture, where k is a positive integer, and the singular analytical expression is obtained:

[0018]

[0019]

[0020] In the formula, and are the k-order derivatives of the first intermediate parameter m1 and the second intermediate parameter n1 with respect to time t in the basic analytical expression of the joint angle θ1, and They are respectively the k-order derivatives of the first intermediate parameter m6 and the second intermediate parameter n6 in the basic analytical expression of the joint angle θ6 with respect to time t.

[0021] In another possible implementation, solving the joint angle θ1 or the joint angle θ6 according to the singular analytical expression includes: for joint 1 or joint 6, according to the first intermediate parameter m i and the second intermediate parameter n i Determine whether to use the corresponding singular analytical expression to solve the joint angle θ1 or the joint angle θ6; if so, then calculate the first intermediate parameter m i and the second intermediate parameter n i Take k-order derivatives until m i (k) and n i (k) If they are not zero at the same time, m i (k) and n i (k) Substitute into the corresponding singular analytical expression and solve the joint angle θ1 or the joint angle θ6; otherwise, take k equal to 0 and solve the joint angle θ1 or the joint angle θ6 according to the corresponding basic analytical expression.

[0022] In another possible implementation, according to the first intermediate parameter m i and the second intermediate parameter n i Determining whether to use the corresponding singular analytical expression to solve the joint angle θ1 or the joint angle θ6 includes: determining whether the following inequality holds: ((m i (k) ) 2 +(n i (k) ) 2 ) 1 / 2 <ε, ε is the reference threshold; if the inequality holds, it is determined that the corresponding singular analytical expression is used to solve the joint angle θ1 or θ6; otherwise, the corresponding basic analytical expression is used to solve the joint angle θ1 or θ6.

[0023] In another possible implementation, the basic analytical formula for determining the joint angle of each joint according to the forward kinematics equation includes:

[0024] Multiply both sides of the forward kinematics equation by 0 The inverse matrix of T1(θ1) is multiplied on the right 5 T6(θ6) and 4 The inverse matrix of T5(θ5); then multiply the Rot(x4,-α4) matrix only on the right side of the equal sign of the forward kinematics equation; finally, two unequal homogeneous matrices T are obtained according to the left and right parts of the forward kinematics equation. L and T R:

[0025] T L = 1 T2(θ2) 2 T3(θ3) 3 T4(θ4)Rot(x4,-α4)

[0026] T R =( 0 T1(θ1) -1 0 T6( 4 T5(θ5) 5 T6(θ6) -1

[0027] In the formula, i-1 T i is the homogeneous transformation matrix of adjacent links i-1 and i, 0 T6 is the pose matrix of the end of the industrial robot arm relative to the base, Rot(x,α) represents the rotation homogeneous matrix obtained by rotating around the x-axis by an angle of α, x4 and α4 are the local coordinate system and connecting rod parameters established in the robot arm by the standard DH method; according to the homogeneous matrix T L and T R A plurality of equations for solving each joint angle are obtained; and a basic analytical expression of the joint angle is obtained according to the plurality of equations.

[0028] In another possible implementation, the multiple equations for solving the joint angles include:

[0029] T L (:,4)=T R (:,4)

[0030] T L (:,3)·T R (:,3)=0

[0031] T L (:,2) T ·T R (:,3)=1,-180°<α4<0°

[0032] T L (:,3) T ·T R (:,2)=-1,-180°<α4<0°

[0033] or

[0034] T L (:,2) T ·T R (:,3)=-1,0°<α4<180°

[0035] T l (:,3) T ·T R (:,2)=1,0°<α4<180°

[0036] Where T(:,i) represents the i-th column vector of matrix T.

[0037] In a second aspect, a singular posture acquisition device for Cartesian trajectory tracking of an industrial robot arm is provided. The industrial robot arm includes joints 1 to 6, and the joint angles of each joint are θ1 to θ6. The device includes: an equation establishment unit, used to establish forward kinematics equations for Cartesian trajectory tracking of the industrial robot arm based on the standard DH method; a basic analytical unit, used to determine the basic analytical expression of the joint angles of each joint according to the forward kinematics equation, the basic analytical expression including a first intermediate parameter and a second intermediate parameter related to the connecting rod parameters of the industrial robot arm; a first posture acquisition unit, used to solve the joint angles θ2 to θ5 for joints 2 to 5 according to the basic analytical expression; a second posture acquisition unit, used to adjust the first intermediate parameter and the second intermediate parameter based on the singular posture for joint 1 or joint 6, to obtain a singular analytical expression, and solve the joint angle θ1 or the joint angle θ6 according to the singular analytical expression.

[0038] In a third aspect, an electronic device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, a method for acquiring singular postures of Cartesian trajectory tracking of an industrial robot arm as provided in the first aspect is implemented.

[0039] In a fourth aspect, a non-transitory computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, a method for acquiring singular postures of Cartesian trajectory tracking of an industrial robot arm as provided in the first aspect is implemented. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in describing the embodiments of the present application are briefly introduced below.

[0041] Figure 1 A flowchart of a method for obtaining singular postures of an industrial robot arm Cartesian trajectory tracking provided by an embodiment of the present invention;

[0042] Figure 2 A schematic diagram of the connecting rod coordinate system of the ABBIRB6700 industrial robot arm according to an embodiment of the present invention;

[0043] Figure 3 Schematic diagram of the result of using the MoveJ instruction to track a parabolic trajectory in the prior art, wherein Figure (a) is a global diagram and Figure (b) is a local enlarged diagram of circle A;

[0044] Figure 4 It is a schematic diagram of the result of using the MoveL instruction to track the parabolic trajectory in the prior art;

[0045] Figure 5 A schematic diagram of a complete path of motion of the end of an ABB robot arm generated by a singular posture acquisition method for Cartesian trajectory tracking of an industrial robot arm according to an embodiment of the present invention;

[0046] Figure 6 A structural diagram of a singular posture acquisition device for Cartesian trajectory tracking of an industrial robot arm provided by an embodiment of the present invention;

[0047] Figure 7 A schematic diagram of the physical structure of an electronic device provided by the present invention. DETAILED DESCRIPTION

[0048] The embodiments of the present application are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar modules or modules with the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present application, and cannot be interpreted as limiting the present invention.

[0049] It will be understood by those skilled in the art that, unless expressly stated, the singular forms "a", "an", "said" and "the" used herein may also include plural forms. It should be further understood that the term "comprising" used in the specification of the present application refers to the presence of the features, integers, steps, operations, modules and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, modules, components and / or groups thereof. It should be understood that when we refer to a module as being "connected" or "coupled" to another module, it may be directly connected or coupled to the other modules, or there may be intermediate modules. In addition, the "connection" or "coupling" used herein may include wireless connection or wireless coupling. The term "and / or" used herein includes all or any modules and all combinations of one or more associated listed items.

[0050] In order to make the objectives, technical solutions and advantages of the present application clearer, the implementation method of the present application will be further described in detail below in conjunction with the accompanying drawings.

[0051] The technical solution of the present application and how to solve the above-mentioned technical problems are described in detail below with specific embodiments. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be repeated in some embodiments. The embodiments of the present application will be described below in conjunction with the accompanying drawings.

[0052] like Figure 1 The flowchart of a method for obtaining singular postures of an industrial robot arm Cartesian trajectory tracking provided by an embodiment of the present invention is applied to a server. The industrial robot arm includes joints 1 to 6, and the joint angles of each joint are θ1 to θ6. The method includes:

[0053] Step S11, establishing the forward kinematics equation of the Cartesian trajectory tracking of the industrial robot arm based on the standard DH method.

[0054] Step S12, determining a basic analytical expression of the joint angle of each joint according to the forward kinematics equation, wherein the basic analytical expression includes a first intermediate parameter and a second intermediate parameter related to the connecting rod parameters of the industrial robot arm.

[0055] Step S13, for joints 2 to 5, solve the joint angles θ2 to θ5 according to the basic analytical formula.

[0056] Step S14, for joint 1 or joint 6, adjust the first intermediate parameter and the second intermediate parameter based on the singular posture, obtain a singular analytical expression, and solve the joint angle θ1 or the joint angle θ6 according to the singular analytical expression.

[0057] The embodiment of the present invention introduces singular posture adjustment into the basic analytical expression of the joint angle θ1 or the joint angle θ6, and then obtains the singular posture, so as to accurately obtain the true inverse solution of the singular posture, so that when the industrial robot arm passes through the singular posture of the shoulder or the singular posture of the wrist, the joint trajectory is smooth and continuous at the singular point, and no posture error caused by sudden changes in the motion path occurs.

[0058] In the embodiment of the present invention, the ABBIRB6700 industrial robot arm is used as the research object. The last three joints of the industrial robot arm form a ball wrist structure, and their axes intersect at one point, which has an analytical solution. This type of industrial robot arm with an analytical solution has been widely used in various occasions. Figure 2 is the link coordinate system of the ABBIRB6700 industrial robot arm, and Table 1 is its DH parameters.

[0059] Table 1

[0060]

[0061] In step S11, the forward kinematics equation of the industrial robot arm is established based on the standard DH method as follows:

[0062]

[0063] Where n is the normal vector, o is the orientation vector, a is the approach vector, and p is the position vector. In the Cartesian trajectory tracking task, n, o, a, and p are all functions of time t.

[0064] The specific establishment process is as follows:

[0065] The homogeneous transformation matrix of a single joint is expressed as:

[0066]

[0067] θ i ——x i-1 Axis and x i Between the axes about z i-1 The angle of

[0068] d i ——x i-1 The axis is along z i-1 Axis to x i The distance between the axes;

[0069] a i ——Along x i-1 Axis, z i-1 Axis and z i The distance between the axes;

[0070] α i ——z i-1 Axis and z i The axis about x i-1 Angle.

[0071] Multiply all homogeneous transformation matrices in sequence to obtain the forward kinematics formula of the wrist center relative to the base.

[0072] 0 T6= 0 T1(θ1) 1 T2(θ2) 2 T3(θ3) 3 T4(θ4) 4 T5(θ5) 5 T6(θ6) (2)

[0073] When the Cartesian trajectory of the target is given, the expression of the wrist center can be written as:

[0074]

[0075] Multiply both sides of the forward kinematics equation by 0 The inverse matrix of T1(θ1) is multiplied on the right 5 T6(θ6) and 4 The inverse matrix of T5(θ5); then multiply the Rot(x4,-α4) matrix only on the right side of the equal sign of the forward kinematics equation; finally, two unequal homogeneous matrices T are obtained according to the left and right parts of the forward kinematics equation. L and T R :

[0076] T L = 1 T2(θ2) 2 T3(θ3) 3 T4(θ4)Rot(x4,-α4)

[0077] T R =( 0 T1(θ1) -1 0 T6( 4 T5(θ5) 5 T6(θ6) -1

[0078] In the formula, i-1 T i is the homogeneous transformation matrix of adjacent links i-1 and i, 0 T6 is the pose matrix of the end of the industrial robot arm relative to the base, Rot(x4,α4) represents the rotation homogeneous matrix obtained by rotating the x4 axis by an angle of α4, x4 and α4 are the local coordinate system and connecting rod parameters established in the robot arm by the standard DH method; according to the homogeneous matrix T L and T R A plurality of equations for solving each joint angle are obtained; and a basic analytical expression of the joint angle is obtained according to the plurality of equations.

[0079] The basic analytical formula for determining the joint angles of each joint according to the forward kinematics equation is as follows:

[0080] θ1=arctan2(m1,n1)+arctan2(±1,0)

[0081] θ2=arctan2(m2,n2)

[0082] θ3=arctan2(±m3,n3)-arctan2(d4,a3)

[0083] θ6=arctan2(±1,0)+arctan2(m6,n6)

[0084] θ4=arctan2(0,-1)+arctan2(m4,n4)

[0085] θ5=arctan2(0,1)+arctan2(m5,n5)

[0086] or

[0087] θ4=arctan2(0,1)+arctan2(m4,n4)

[0088] θ5=arctan2(0,-1)+arctan2(m5,n5)

[0089] In the formula, m i and n i (i=1,2,3,4,5,6) are related to the connecting rod parameters of the industrial robot arm and are used to solve θ i The first intermediate parameter and the second intermediate parameter, a3 is the vertical distance between the axes of joint 3 and joint 4 of the industrial robot arm, and d4 is the vertical distance between the axis of joint 4 and the axis of joint 5.

[0090] The specific solution process is as follows. Formula (2) can be transformed into:

[0091] T L = 1 T2(θ2) 2 T3(θ3) 3 T4(θ4)Rot(x4,-90°)

[0092] T R =( 0 T1(θ1) -1 0 T6( 4 T5(θ5) 5 T6(θ6) -1 (4)

[0093] Based on formula (4), according to the homogeneous matrix T L and T R Get several equations for solving for each joint angle:

[0094] T L (:,4)=T R (:,4) (5)

[0095] T L (:,3)·T R (:,3)=0 (6)

[0096] T L (:,2) T ·T R (:,3)=1,-180°<α4<0°

[0097] T L (:,3) T ·T R (:,2)=-1,-180°<α4<0°

[0098] or

[0099] T L (:,2) T·T R (:,3)=-1,0°<α4<180° (7)

[0100] T L (:,3) T ·T R (:,2)=1,0°<α4<180° (8)

[0101] Where T(:,i) represents the i-th column vector of matrix T.

[0102] The basic analytical formula for the joint angle is obtained based on multiple equations. Specifically, using equations (5) and (6), four equations containing only θ1, θ2, θ3, and θ6 are obtained:

[0103]

[0104]

[0105]

[0106]

[0107] Where: p m1 =[p x p y -a1] T , p x , p y , p z They are the three elements of the position vector p.

[0108] x m1 =[c1 s1 1] T

[0109] p m2 =[1 d1 -1] T

[0110] x m2 =[0 1 p z ] T

[0111] p m3 =[-p x p y 0] T

[0112] x m3 =[s1 c1 0] T

[0113] z L =[c1c 23 s1c 23 -s23 ] T

[0114] s i and c i Respectively represent sinθ i and cosθ i ,s ij and c ij Respectively represent sin(θ i +θ j ) and cos(θ i +θ j ).

[0115] Since equation (11) only contains the joint variable θ1, the basic analytical formula of the joint angle θ1 can be directly obtained:

[0116] θ1=arctan2(m1,n1)+arctan2(±1,0) (13)

[0117] In the formula, m1 = -p x , n1=p y .

[0118] The square sum of equation (9) and equation (10) gives the equation:

[0119]

[0120] The basic analytical formula of the joint angle θ3 is obtained from formula (14):

[0121] θ3=arctan2(k2m3,n3)-arctan2(d4,a3) (15)

[0122] in,

[0123] After obtaining θ3, we can regard equations (9) and (10) as binary linear equations about s2 and c2, and obtain the basic analytical formula of the joint angle θ2:

[0124]

[0125] In the formula, h m1 =d4c3+a3s3,h m2 =a3c3-d4s3+a2.

[0126] After obtaining the joint angles θ1, θ2, and θ3, the joint angles θ1, θ2, and θ3 are substituted into formula (12) to obtain the basic analytical formula for the joint angle θ6:

[0127] θ6=arctan2(k3,0)+arctan2(m6,n6) (17)

[0128] in, k3=±1.

[0129] From formula (7), we get the equation:

[0130]

[0131] Among them, h n1 =[s1 -c1 0] T ,h n2 =[-c1s 23 -s1s 23 -c 23 ] T ,h n3 =oc6+ns6.

[0132] After determining θ1, θ2, θ3 and θ6, the basic analytical formula of the joint angle θ4 is obtained by equation (7):

[0133] θ4=arctan2(0,-1)+arctan2(m4,n4) (19)

[0134] in,

[0135] The basic analytical formula of the joint angle θ5 can be determined by formula (8):

[0136] θ5=arctan2(0,1)+arctan2(m5,n5) (20)

[0137] in,

[0138] The above formula is the basic analytical expression of θ1, θ2, θ3, θ4, θ5 and θ6. When the industrial robot arm passes through the singular posture of the shoulder, the first intermediate parameter m1 and the second intermediate parameter n1 of the joint angle θ1 are both zero. In the computer, arctan2(0,0) defaults to zero, resulting in the joint angle θ1 can only be determined as a fixed value when the shoulder is in a singular posture, which makes the joint trajectory unsmooth at the singular point, that is, there is a mutation. Similarly, when the industrial robot arm passes through the singular posture of the wrist, the basic analytical expression of the joint angle θ6 also has this situation.

[0139] In the embodiment of the present invention, in order to prevent the industrial robot arm from experiencing path mutation when passing through a singular posture while tracking a Cartesian trajectory, m can be simultaneously obtained in the basic analytical expressions of the joint angles θ1 and θ6. i and n i The k-th derivative m with respect to time t i(k) and n i (k) , until m i (k) and n i (k) When k in the upper right corner is equal to zero, it means no derivative is required. i (k) and n i (k) When both are zero, it means that the industrial robot arm passes through a singular posture of the shoulder or wrist.

[0140] In step S14, optionally, for joint 1 or joint 6, the first intermediate parameter and the second intermediate parameter are updated to a k-order derivative form according to the singular posture, where k is a positive integer, to obtain a singular analytical expression:

[0141]

[0142]

[0143] In the formula, and are the k-order derivatives of the first intermediate parameter m1 and the second intermediate parameter n1 with respect to time t in the basic analytical expression of the joint angle θ1, and They are respectively the k-order derivatives of the first intermediate parameter m6 and the second intermediate parameter n6 in the basic analytical expression of the joint angle θ6 with respect to time t.

[0144] In actual calculation, the following conditions can be used to determine whether to use the singular analytical expressions of joint angles θ1 and θ6. For joint 1 or joint 6, according to the first intermediate parameter m i and the second intermediate parameter n i Determine whether to use the corresponding singular analytical expression to solve the joint angle θ1 or the joint angle θ6. Specifically, determine whether the following inequality holds: ((m i (k) ) 2 +(n i (k) ) 2 ) 1 / 2 <ε, ε is the reference threshold, which can be set as needed, preferably less than 10 -8 If the inequality holds, the corresponding singular analytical expression is used to solve the joint angle θ1 or θ6. Otherwise, the corresponding basic analytical expression is used to solve the joint angle θ1 or θ6.

[0145] If the corresponding singular analytical expression is used to solve the joint angle θ1 or the joint angle θ6, then the first intermediate parameter m i and the second intermediate parameter ni Take k-order derivatives until m i (k) and n i (k) If they are not zero at the same time, m i (k) and n i (k) Substitute into the corresponding singular analytical expression and solve for the joint angle θ1 or the joint angle θ6. Otherwise, take k equal to 0 and do not change the first intermediate parameter m. i and the second intermediate parameter n i Perform differentiation and directly solve the joint angle θ1 or the joint angle θ6 according to the corresponding basic analytical expression.

[0146] In this way, when the industrial robot arm passes through a singular shoulder posture, the k-order derivative of the first intermediate parameter m1 and the second intermediate parameter n1 in the basic analytical expression of the joint angle θ1 with respect to time t can be calculated to obtain an analytical solution that ensures smooth and continuous joint trajectory; when the industrial robot arm passes through a singular wrist posture, the k-order derivative of the first intermediate parameter m6 and the second intermediate parameter n6 in the basic analytical expression of the joint angle θ6 with respect to time t can be calculated to obtain an analytical solution that ensures smooth and continuous joint trajectory.

[0147] The embodiments of the present invention can accurately obtain the true inverse solution of the singular posture when the end of an industrial robot arm passes through a singular posture in Cartesian trajectory tracking, compared with the basic analytical solution and the minimum damped square method, thereby solving the problem that it is difficult for existing inverse solution methods to find the "true solution" that makes the joint trajectory continuous and smooth in an infinite set of inverse solutions. This ensures that when the industrial robot arm tracks the Cartesian trajectory motion through a singular posture, the joint trajectory used for servo control is smooth and continuous, and the end does not have a posture error caused by a sudden change in the motion path.

[0148] In order to verify the effectiveness of the singular pose acquisition method for Cartesian trajectory tracking of an industrial robot arm according to an embodiment of the present invention, a simulation of the industrial robot arm is performed in RobotStudio to verify offline whether the new analytical solution can be implemented in actual operations. Figure 2 This is the 3D model of the ABB IRB6700 industrial robot arm in RobotStudio.

[0149] In practice, tools are installed at the end of industrial robot arms, and their singular postures are mainly determined by the wrist center. Therefore, the posture of the tool relative to the base can be converted to the wrist center posture using the following formula:

[0150] T e =T w T we (twenty one)

[0151] Where, T w is the wrist center pose matrix of the robot arm, T eis the end pose matrix of the robot arm, T we is the homogeneous transformation matrix of the end position relative to the wrist center position.

[0152]

[0153] In addition, when the ABBIRB6700 robot arm is at zero position, the axes of joint 4 and joint 6 are collinear, and the position offset by 90° in the positive direction of joint 5 in Table 1 is the zero position. Therefore, when the value of the joint angle θ5 is obtained using the method of the embodiment of the present invention, 90° needs to be added to this value to make the end motion of the ABBIRB6700 robot arm consistent with the expected trajectory.

[0154] During the simulation experiment, the target Cartesian trajectory equation of the industrial robot arm wrist center is:

[0155] p x =625-26.5t 2

[0156]

[0157] p z =2605 (23)

[0158] In the formula, a=0.002734069064, b=-1.811320754717, t=0:0.05:5.5.

[0159] The position and posture of each point on the wrist-center trajectory remain unchanged, and the posture of the initial point is:

[0160]

[0161] On the above wrist-center trajectory, the robotic arm passes through the wrist singularity and shoulder singularity postures respectively. Figure 3 and 4 The results of using the MoveJ instruction and the MoveL instruction to track a parabola are shown respectively. Figure 3 middle, Figure 3 (a) is the global graph, Figure 3 (b) is a partial enlarged view of circle A. When the wrist center of the ABB robot arm moves to the singular position of the shoulder, its motion path undergoes an obvious mutation, and stops moving because joint 4 exceeds the range of motion. Under the MoveJ instruction, although the robot arm passes the singular position of the wrist smoothly, the end path also undergoes a mutation, as shown in Figure 2. Figure 3 (b) is shown in circle A. Figure 4 In the simulation, when the end of the ABB robot arm moved to a strange wrist position, the system issued a warning and terminated the movement. Figure 5The complete path of the end motion of the ABB robot arm generated by the method of the embodiment of the present invention is smooth and continuous without mutations. It shows that the singular posture acquisition method of the industrial robot arm Cartesian trajectory tracking of the embodiment of the present invention obtains the "true solution" in the singular posture of the wrist and the singular posture of the shoulder.

[0162] The embodiment of the present invention establishes a forward kinematics equation for Cartesian trajectory tracking of an industrial robot arm based on a standard DH method; a basic analytical expression for the joint angle of each joint is determined according to the forward kinematics equation, wherein the basic analytical expression includes a first intermediate parameter and a second intermediate parameter related to the connecting rod parameters of the industrial robot arm; for joints 2 to 5, the joint angles θ2 to θ5 are solved according to the basic analytical expression; for joints 1 or 6, the first intermediate parameter and the second intermediate parameter are adjusted based on the singular posture to obtain a singular analytical expression, and the joint angle θ1 or the joint angle θ6 is solved according to the singular analytical expression, so that the true inverse solution of the singular posture can be accurately obtained, so that when the industrial robot arm tracks the Cartesian trajectory motion through the singular posture, the end thereof does not have a posture error caused by a sudden change in the motion path.

[0163] like Figure 6 The figure shows a structural diagram of a singular posture acquisition device for Cartesian trajectory tracking of an industrial robot arm provided by an embodiment of the present invention, which is applied to a server. The industrial robot arm includes joints 1 to 6, and the joint angles of each joint are θ1 to θ6. The device includes:

[0164] An equation building unit 601 is used to build a forward kinematics equation for Cartesian trajectory tracking of an industrial robot arm based on a standard DH method;

[0165] A basic analytical unit 602, used to determine a basic analytical expression of the joint angle of each joint according to the forward kinematics equation, wherein the basic analytical expression includes a first intermediate parameter and a second intermediate parameter related to the connecting rod parameters of the industrial robot arm;

[0166] The first pose acquisition unit 603 is used to solve the joint angles θ2 to θ5 according to the basic analytical formula for joints 2 to 5;

[0167] The second posture acquisition unit 604 is used to adjust the first intermediate parameter and the second intermediate parameter based on the singular posture for joint 1 or joint 6, obtain the singular analytical expression, and solve the joint angle θ1 or the joint angle θ6 according to the singular analytical expression.

[0168] The device of the above embodiment is applied to the corresponding method in the above embodiment, and has the beneficial effects of the corresponding method embodiment, which will not be described in detail here.

[0169] Figure 7 An example of a physical structure diagram of an electronic device is shown in FIG. Figure 7As shown, the electronic device may include: a processor (processor) 701, a communication interface (Communications Interface) 702, a memory (memory) 703 and a communication bus 704, wherein the processor, the communication interface, and the memory complete mutual communication through the communication bus. The processor can call the logic instructions in the memory to execute a singular posture acquisition method for Cartesian trajectory tracking of an industrial robot arm, the method comprising: establishing a forward kinematics equation for Cartesian trajectory tracking of an industrial robot arm based on a standard DH method; determining a basic analytical expression of the joint angle of each joint according to the forward kinematics equation, the basic analytical expression including a first intermediate parameter and a second intermediate parameter related to the connecting rod parameters of the industrial robot arm; for joints 2 to 5, solving the joint angles θ2 to θ5 according to the basic analytical expression; for joints 1 or 6, adjusting the first intermediate parameter and the second intermediate parameter based on the singular posture, obtaining a singular analytical expression, and solving the joint angle θ1 or the joint angle θ6 according to the singular analytical expression.

[0170] In addition, the logic instructions in the above-mentioned memory can be implemented in the form of software functional units and can be stored in a computer-readable storage medium when sold or used as an independent product. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product, which is stored in a storage medium and includes several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0171] On the other hand, an embodiment of the present invention also provides a computer program product, which includes a computer program stored on a non-transitory computer-readable storage medium, and the computer program includes program instructions. When the program instructions are executed by a computer, the computer can execute a method for acquiring a singular posture of Cartesian trajectory tracking of an industrial robot arm provided by the above-mentioned method embodiments, the method including: establishing a forward kinematic equation for Cartesian trajectory tracking of an industrial robot arm based on the standard DH method; determining a basic analytical expression for the joint angle of each joint according to the forward kinematic equation, the basic analytical expression including a first intermediate parameter and a second intermediate parameter related to the connecting rod parameters of the industrial robot arm; for joints 2 to 5, solving the joint angles θ2 to θ5 according to the basic analytical expression; for joints 1 or 6, adjusting the first intermediate parameter and the second intermediate parameter based on the singular posture to obtain a singular analytical expression, and solving the joint angle θ1 or the joint angle θ6 according to the singular analytical expression.

[0172] On the other hand, an embodiment of the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, it is implemented to execute a method for acquiring a singular posture of Cartesian trajectory tracking of an industrial robot arm provided in the above-mentioned embodiments, the method comprising: establishing a forward kinematic equation for Cartesian trajectory tracking of an industrial robot arm based on the standard DH method; determining a basic analytical expression for the joint angle of each joint according to the forward kinematic equation, the basic analytical expression including a first intermediate parameter and a second intermediate parameter related to the connecting rod parameters of the industrial robot arm; for joints 2 to 5, solving the joint angles θ2 to θ5 according to the basic analytical expression; for joints 1 or 6, adjusting the first intermediate parameter and the second intermediate parameter based on the singular posture to obtain a singular analytical expression, and solving the joint angle θ1 or the joint angle θ6 according to the singular analytical expression.

[0173] It should be understood that, although the steps in the flowchart of the accompanying drawings are displayed in sequence as indicated by the arrows, these steps are not necessarily executed in sequence in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least a part of the steps in the flowchart of the accompanying drawings may include multiple sub-steps or multiple stages, and these sub-steps or stages are not necessarily executed at the same time, but can be executed at different times, and their execution order is not necessarily sequential, but can be executed in turn or alternately with other steps or at least a part of the sub-steps or stages of other steps.

[0174] The above is only a partial implementation of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A method for obtaining singular postures of industrial robot arm Cartesian trajectory tracking, characterized in that: The industrial robot arm includes joints 1 to 6, and the joint angles of the joints are θ1 to θ6. The method includes: The forward kinematics equations for Cartesian trajectory tracking of industrial robot arms are established based on the standard DH method; Determine a basic analytical expression of the joint angle of each joint according to the forward kinematics equation, wherein the basic analytical expression includes a first intermediate parameter and a second intermediate parameter related to a connecting rod parameter of the industrial robot arm; For joints 2 to 5, the joint angles θ2 to θ5 are solved according to the basic analytical formula; For joint 1 or joint 6, the first intermediate parameter and the second intermediate parameter are adjusted based on the singular posture, a singular analytical expression is obtained, and the joint angle θ1 or the joint angle θ6 is solved according to the singular analytical expression.

2. The method according to claim 1, characterized in that The basic analytical expressions of the joint angles of each joint include: θ1=arctan2(m1,n1)+arctan2(±1,0) θ2=arctan2(m2,n2) θ3=arctan2(±m3,n3)-arctan2(d4,a3) θ6=arctan2(±1,0)+arctan2(m6,n6) θ4=arctan2(0,-1)+arctan2(m4,n4) θ5=arctan2(0,1)+arctan2(m5,n5) or θ4=arctan2(0,1)+arctan2(m4,n4) θ5=arctan2(0,-1)+arctan2(m5,n5) In the formula, m i and n i (i=1,2,3,4,5,6) are respectively related to the connecting rod parameters of the industrial robot arm and are used to solve the joint angle θ i The first intermediate parameter and the second intermediate parameter, a3 is the vertical distance between the axes of joint 3 and joint 4 of the industrial robot arm, and d4 is the vertical distance between the axis of joint 4 and the axis of joint 5.

3. The method according to claim 2, characterized in that The step of adjusting the first intermediate parameter and the second intermediate parameter according to the singular posture to obtain a singular analytical expression includes: For joint 1 and joint 6, the first intermediate parameter and the second intermediate parameter are updated to the k-order derivative form according to the singular posture, where k is a positive integer, to obtain a singular analytical expression: In the formula, and are the k-order derivatives of the first intermediate parameter m1 and the second intermediate parameter n1 with respect to time t in the basic analytical expression of the joint angle θ1, and They are respectively the k-order derivatives of the first intermediate parameter m6 and the second intermediate parameter n6 in the basic analytical expression of the joint angle θ6 with respect to time t.

4. The method according to claim 3, characterized in that Solving the joint angle θ1 or the joint angle θ6 according to the singular analytical expression includes: For joint 1 or joint 6, according to the first intermediate parameter m i and the second intermediate parameter n i Determine whether to use the corresponding singular analytical expression to solve the joint angle θ1 or the joint angle θ6; If m i (k) and n i (k) At the same time, if they are zero, then the first intermediate parameter m i and the second intermediate parameter n i Take k-order derivatives until m i (k) and n i (k) If they are not zero at the same time, m i (k) and n i (k) Substitute into the corresponding singular analytical expression and solve for the joint angle θ1 or the joint angle θ6; Otherwise, k is taken to be equal to 0, and the joint angle θ1 or the joint angle θ6 is solved according to the corresponding basic analytical expression.

5. The method according to claim 4, characterized in that According to the first intermediate parameter m i and the second intermediate parameter n i Determining whether to use the corresponding singular analytical expression to solve the joint angle θ1 or the joint angle θ6 includes: Determine whether the following inequality holds: ((m i (k) ) 2 +(n i (k) ) 2 ) 1 / 2 <ε, ε is the reference threshold; If the inequality holds, it is determined to use the corresponding singular analytical expression to solve the joint angle θ1 or θ6; Otherwise, the corresponding basic analytical expression is used to solve the joint angle θ1 or θ6.

6. The method according to claim 1, characterized in that The basic analytical formula for determining the joint angle of each joint according to the forward kinematics equation includes: Multiply both sides of the forward kinematics equation by 0 The inverse matrix of T1(θ1) is multiplied on the right 5 T6(θ6) and 4 The inverse matrix of T5(θ5); then multiply the Rot(x4,-α4) matrix only on the right side of the equal sign of the forward kinematics equation; finally, two unequal homogeneous matrices T are obtained according to the left and right parts of the forward kinematics equation. L and T R : T L = 1 T2(θ2) 2 T3(θ3) 3 T4(θ4)Rot(x4,-α4) T R =( 0 T1(θ1)) -10 T6( 4 T5(θ5) 5 T6(θ6)) -1 In the formula, i-1 T i is the homogeneous transformation matrix of adjacent links i-1 and i, 0 T6 is the pose matrix of the end of the industrial robot arm relative to the base, Rot(x4,α4) represents the rotation homogeneous matrix obtained by rotating the x4 axis by an angle of α4, x4 and α4 are the local coordinate system and link parameters established in the robot arm by the standard DH method; According to the homogeneous matrix T L and T R Obtain multiple equations for solving for each joint angle; A basic analytical expression of the joint angle is obtained according to the multiple equations.

7. The method according to claim 6, characterized in that The multiple equations for solving the joint angles include: T L (:,4)=T R (:,4) T L (:,3)·T R (:,3)=0 T L (:,2) T ·T R (:,3)=1,-180°<α4<0° T L (:,3) T ·T R (:,2)=-1,-180°<α4<0° or T L (:,2) T ·T R (:,3)=-1,0°<α4<180° T L (:,3) T ·T R (:,2)=1,0°<α4<180° Where T(:,i) represents the i-th column vector of matrix T.

8. A singular posture acquisition device for Cartesian trajectory tracking of an industrial robot arm, characterized in that: The industrial robot arm includes joints 1 to 6, and the joint angles of the joints are θ1 to θ6. The device includes: Equation building unit, used to build forward kinematics equations for Cartesian trajectory tracking of industrial robot arms based on the standard DH method; A basic analytical unit, used for determining a basic analytical expression of the joint angle of each joint according to the forward kinematics equation, wherein the basic analytical expression includes a first intermediate parameter and a second intermediate parameter related to a connecting rod parameter of the industrial robot arm; A first posture acquisition unit, for solving joint angles θ2 to θ5 according to the basic analytical formula for joints 2 to 5; The second posture acquisition unit is used to adjust the first intermediate parameter and the second intermediate parameter based on the singular posture for joint 1 or joint 6, obtain a singular analytical expression, and solve the joint angle θ1 or the joint angle θ6 according to the singular analytical expression.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, a method for acquiring singular postures of Cartesian trajectory tracking of an industrial robot arm is implemented as described in any one of claims 1-7.

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, a method for acquiring singular postures of Cartesian trajectory tracking of an industrial robot arm is implemented as described in any one of claims 1 to 7.

Images