Inverse kinematics solving method for series composite joints

Abstract

A series composite joint inverse kinematics solving method belongs to the technical field of robots, and comprises the following steps: aiming at a mechanical arm, constructing a conversion matrix corresponding to each single joint; for the series composite joints, an overall conversion matrix is constructed, and the matrix is a feature matrix of the series composite joints; the mapping relation of the characterization parameters of the series composite joints is obtained, the characterization parameters of the series composite joints are screened out, and the kinematic characteristics of the series composite joints can be analyzed through the parameters; based on the first three steps, a mapping relation between a position vector and each degree of freedom and a mapping relation between a posture vector and each degree of freedom are constructed; and constructing an iteration sequence and a solving mode, namely constructing the iteration sequence, setting initial values of parameters, solving numerical values of a composite joint conversion matrix, carrying out iterative solving or iterative mediation and carrying out loop iteration, and respectively providing corresponding solving or mediation methods for translation parameters, non-composite joint parameters and composite joint parameters. The method aims at series composite joints and has universality and feasibility.

Description

Technical Field

[0001] The invention relates to a method for solving inverse kinematics of a series composite joint, and belongs to the technical field of robots. Background Art

[0002] The inverse kinematics solution of serial compound joints belongs to the field of manipulators and robots. Serial compound joints are a technology in the field of manipulator engineering, which involves combining multiple joints or motion units in series to achieve more complex movements and higher flexibility. Inverse kinematics solution is the process of solving the angles of each joint when the position and posture of the end of the manipulator are known. Accurate inverse kinematics solution of serial compound joints is crucial to achieve fast and stable control and path planning. However, serial compound joints have multiple degrees of freedom, and the conversion relationship between each parameter is nonlinear and relatively complex. Traditional inverse kinematics solution methods such as numerical methods and analytical methods cannot be directly applied to the inverse kinematics solution of this type of structure. Therefore, it is urgent to propose a general kinematic solution method for serial compound joints.

[0003] For the inverse kinematics solution of the robot arm, the analytical method can obtain a closed-form solution with fast calculation speed, but it is not suitable for all types of robot arms. The traditional numerical method solves through iterative approximation, has a wide range of applications, and there is room for optimization of calculation efficiency. For special structures, the traditional method also needs to be modified and improved accordingly. Patent ZL 20191054836.6 proposes a method for solving the inverse kinematics of a robot arm, which solves the inverse kinematics problem through iteration. Patent ZL 201910278057.0 proposes a numerical unique solution for a six-degree-of-freedom robot arm, overcoming the requirement that the Jacobian matrix must be full rank. The above patent is only applicable to robots with less than or equal to six degrees of freedom, and cannot perform inverse kinematics solutions for robots with compound joints. Patent ZL 202310437038.4 proposes an inverse kinematics solution method that combines the position segmentation method with the algebraic method for a surgical actuator. This type of structure also includes a series of composite joints with continuous equal-angle bending, but the composite joint only includes the degree of freedom of deflection, and does not consider the composite joint type of pitch and deflection alternating in series. For composite joints in series, especially composite joints of pitch and deflection alternating in series, a general kinematic solution method has not yet been constructed.

[0004] In summary, although there are many studies related to kinematics, there is no general solution method for serial compound joints. This is because the serial compound joints have multiple degrees of freedom and cannot be written in matrix form, and thus cannot be solved by inverse matrix. Summary of the invention

[0005] The technical problem solved by the present invention is: to overcome the shortcomings of the prior art, to provide a method for solving the inverse kinematics of a series of composite joints, to construct an overall transformation matrix of the series of composite joints, to screen out characterization parameters that can characterize the kinematic characteristics of the series of composite joints, to construct an iteration sequence, to set initial parameter values, to solve the values ​​of the composite joint transformation matrix, and to propose a mediation method for the composite joint parameters, and then to solve the values ​​of all degrees of freedom through cyclic iterative solutions or iterative mediation, thereby solving the problem that the composite joint matrix is ​​multi-dimensional, complex, and cannot be used for inverse kinematics solutions.

[0006] The technical solution of the present invention is: a method for solving inverse kinematics of a series composite joint, comprising:

[0007] Construct the transformation matrix between each single joint of the robot and the world coordinate system;

[0008] According to the transformation matrix corresponding to each joint, the transformation matrix of the entire series compound joint of the robot arm is constructed and used as the characteristic matrix of the series compound joint of the robot arm;

[0009] According to the mapping relationship of each parameter in the characteristic matrix of the serial compound joint of the robot arm, the characterization parameters of the serial compound joint are selected, and the kinematic characteristics of the serial compound joint are determined according to the characterization parameters;

[0010] According to the characteristic matrix and kinematic characteristics of the manipulator's serial compound joints, the mapping relationship between the position vector and the posture vector and each degree of freedom is constructed;

[0011] An iteration sequence and a solution method are constructed to iteratively solve the mapping relationship between the position vector and the attitude vector and each degree of freedom.

[0012] Furthermore, the transformation matrix of the entire series composite joint of the robotic arm is: Where N is the number of yaw degrees of freedom and pitch degrees of freedom in the manipulator's serial compound joints. is the coordinate system {O 5-i}Relative to the coordinate system {O 5-(i-1)} homogeneous transformation matrix, i = 1, 2, ..., N.

[0013] Furthermore, the characteristic matrix of the series composite joint of the robot arm is Among them, q4 is the yaw degree of freedom in the series compound joint, q5 is the pitch degree of freedom in the series compound joint, and a jk is the parameter of the jth row and kth column in the transformation matrix, j is the row number variable, ranging from 1 to 4, k is the column number variable, ranging from 1 to 4, is the coordinate system {O 5-N} is the homogeneous transformation matrix relative to the coordinate system {O3}.

[0014] Furthermore, the fourth row parameter in the characteristic matrix is ​​a fixed value, a 41 =0, a 42 =0, a 43 =0, a 44 =1.

[0015] Furthermore, the mapping relationship between the position vector and the attitude vector and each degree of freedom is:

[0016] in, is the homogeneous transformation matrix of coordinate system {O3} relative to coordinate system {O1}, is the coordinate system {O3} relative to the coordinate system {O 5-N}, q1 is the overall translational degree of freedom, q2 is the yaw degree of freedom, q3 is the pitch degree of freedom, q4 is the yaw degree of freedom in the compound joint, q5 is the pitch degree of freedom in the compound joint, q6 is the torsion degree of freedom, n x is the component of the x-axis of the end link coordinate system in the x-axis direction of the natural coordinate system {O0}, n y is the component of the x-axis of the end link coordinate system in the y-axis direction of the natural coordinate system {O0}, n z is the component of the x-axis of the end link coordinate system in the z-axis direction of the natural coordinate system {O0}, o x is the component of the y-axis of the end link coordinate system in the x-axis direction of the natural coordinate system {O0}, o y is the component of the y-axis of the end link coordinate system in the y-axis direction of the natural coordinate system {O0}, o z is the component of the y-axis of the end link coordinate system in the z-axis direction of the natural coordinate system {O0}, a x is the component of the z-axis of the end link coordinate system in the x-axis direction of the natural coordinate system {O0}, a y is the component of the z-axis of the end link coordinate system in the y-axis direction of the natural coordinate system {O0}, a z is the component of the z-axis of the end link coordinate system in the z-axis direction of the natural coordinate system {O0}, p x is the position coordinate of the origin of the end link coordinate system in the x-axis direction of the natural coordinate system {O0}, p y is the position coordinate of the origin of the end link coordinate system in the y-axis direction of the natural coordinate system {O0}, p z L is the position coordinate of the origin of the end link coordinate system in the z-axis direction of the natural coordinate system {O0}, 67 =L6+L7 is the coordinate system {O 5-N}The distance from the origin to the origin of the coordinate system {O7}.

[0017] Furthermore, the construction iteration sequence and solution method include three methods, namely:

[0018] Method 1: According to a 21 Solve the equation for q2, according to a 11 Solve the equation for q3, according to a 31 Equation adjusts q5 according to a 22 Solve equation q6, according to a 12 Equation adjusts q4, after the loop iterates a preset number of times, according to a 14 Solve the equation for q1;

[0019] Method 2: According to a 21 Solve the equation for q2, according to a 11 Solve the equation for q3, according to a 24 Solve the equation for q1, according to a 22 Solve equation q6, according to a 12 Equation adjusts q4 according to a 34 The equation adjusts q5, and after the preset number of loop iterations, according to a 14 Solve the equation for q1;

[0020] Method three, according to a 14 Solve the equation for q1, according to a 24 Solve the equation for q2, according to a 34 Solve the equation for q3, according to a 31 Equation adjusts q4 according to a 21 Equation adjusts q5 according to a 22 The equation is solved for q6, and after a preset number of iterations, the final solution with degrees of freedom is obtained.

[0021] Furthermore, the theoretical values ​​of the terminal position and posture are obtained based on the solved degrees of freedom, and the preset number of times is determined based on the error between the theoretical value and the expected value of the terminal posture being less than a specified value.

[0022] Further, the iterative solution of the mapping relationship between the position vector and the attitude vector and each degree of freedom includes:

[0023] pass Solve for the translation parameter q1;

[0024] By (L 67 n x -p x +q1)sinq2+(p y -L 67 n y )cosq2+L2sinq2=a 24 Solve for the non-compound joint parameter q2, and similarly solve for q3 and q6;

[0025] By x cosq2sinq3+ny sinq2sinq3+n z cosq3=a 31 Solve for the compound joint parameter q4, and solve for q4 in the same way;

[0026] Wherein, L2 is the distance between the origin of the coordinate system {O1} and the origin of the coordinate system {O2}, and L3 is the distance between the origin of the coordinate system {O2} and the origin of the coordinate system {O3}.

[0027] A computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the method for solving the inverse kinematics of a serial compound joint are implemented.

[0028] A device for solving inverse kinematics of a serial compound joint comprises a memory, a processor and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the steps of a method for solving inverse kinematics of a serial compound joint are implemented.

[0029] The advantages of the present invention compared with the prior art are:

[0030] (1) The present invention proposes a method for solving the inverse kinematics of a series of composite joints. For a plurality of composite joints with repeated series pitch degrees of freedom and yaw degrees of freedom, a composite matrix is ​​constructed, and the series of composite joints are characterized by parameters in the matrix, and the kinematic characteristics of the series of composite joints can be analyzed by the parameters;

[0031] (2) The present invention proposes a novel iterative solution method by analyzing the characterization parameters, and solves the values ​​of all degrees of freedom through several cycles of iterative solution or iterative mediation, thereby solving the problem that the composite joint matrix has multiple dimensions and is complex and cannot be used for inverse kinematics solution.

[0032] (3) The present invention constructs the mapping relationship between the position vector and the attitude vector and each degree of freedom. Through this formula including three 4×4 matrices, 16 equations can be obtained. By properly applying some of the formulas, the values ​​of all degrees of freedom can be iteratively solved;

[0033] (4) The present invention speeds up the solution process by planning the solution sequence. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Various other advantages and benefits will become apparent to those of ordinary skill in the art by reading the detailed description of the preferred embodiments below. The accompanying drawings are only for the purpose of illustrating the preferred embodiments and are not to be considered as limiting the present invention. Moreover, the same reference symbols are used throughout the accompanying drawings to represent the same components. In the accompanying drawings:

[0035] Figure 1 It is a flow chart of a method for solving inverse kinematics of serial compound joints;

[0036] Figure 2 is an example diagram of a robotic arm including a series of composite joints;

[0037] Figure 3 The surface diagram representing the variation of the parameters of the composite joint matrix with the degree of freedom of the composite joint;

[0038] Figure 4 is the equipotential variation diagram of the composite joint matrix representation parameters with the composite joint degrees of freedom, where (a) is a 12 The equipotential change diagram with q5, (b) is a 13 The equipotential change diagram with q4, (c) is a 21 The equipotential change diagram with q5, (d) is a 24 The equipotential change diagram with q5, (e) is a 31 The equipotential change diagram with q4, (f) is a 34 The equipotential variation diagram with q4;

[0039] Figure 5 It is a two-dimensional diagram of the composite joint matrix characterization parameters changing with the composite joint degrees of freedom;

[0040] Figure 6 Flowchart for parameter adjustment of composite joints;

[0041] Figure 7 This is a diagram showing the iterative solution process for the joint degrees of freedom. DETAILED DESCRIPTION

[0042] In order to better understand the above technical scheme, the technical scheme of the present invention is described in detail below through the accompanying drawings and specific embodiments. It should be understood that the embodiments of the present invention and the specific features in the embodiments are detailed descriptions of the technical scheme of the present invention, rather than limitations on the technical scheme of the present invention. In the absence of conflict, the embodiments of the present invention and the technical features in the embodiments can be combined with each other.

[0043] The following is a further detailed description of a method for solving inverse kinematics of a serial compound joint provided by an embodiment of the present invention in conjunction with the drawings of the specification. The specific implementation method may include: The present invention is an inverse kinematics solution method formulated for serial compound joints, which combines the posture segmentation method with the numerical solution method to efficiently solve inverse kinematics. Compared with other methods related to the inverse kinematics solution of a six-degree-of-freedom manipulator, improvements have been made in many aspects, and the effectiveness and practicality of the present invention have been verified through simulation.

[0044] The method of the present invention aims at the problem that the serial compound joint has many degrees of freedom, the motion conversion matrix is ​​complex and cannot be solved. The drive of the distal continuum segment and the proximal orthogonal joint combination is decoupled, divided into a position module and a posture module, and the trust region algorithm is used to directly find the optimal solution to achieve the iterative solution of the inverse kinematics of the serial compound joint. The flow chart of the method of the present invention is as follows Figure 1 As shown, specifically: first construct the DH coordinate system and the basic kinematic model of the multi-DOF surgical actuator (step 1). (Step 2). (Step 3). (Step 4). (Step 6). The present invention is applicable to a six-DOF robotic arm, and the improved method is also applicable to a robotic arm with seven or more DOFs. All the degrees of freedom of the robotic arm can be divided into two sections, which are respectively allocated to the two modules of the position model and the posture model. In the present invention, the process and principle of the present invention are described by taking the surgical actuator as an example. The specific contents of each step are described below:

[0045] The present invention Figure 2 The process and principle of the present invention are described by taking a mechanical arm including a series composite joint as an example. The specific steps are as follows.

[0046] Step 1: Single joint transformation matrix.

[0047] Figure 2 The types of degrees of freedom included in the robot arm shown are as follows: overall translational degree of freedom q1, yaw degree of freedom q2, pitch degree of freedom q3, a series compound joint with two degrees of freedom q4 and q5, a torsion degree of freedom q6, and a clamp opening and closing degree of freedom q7. The series compound joint consists of six torsion degrees of freedom and six rotational degrees of freedom, of which the yaw degree of freedom q 4-i and pitch freedom q 5-i Staggered distribution, including a total of 2×N small joints. Yaw degree of freedom q in series compound joints 4-i The following relationship is satisfied: 4-1 =q 4-2 =……=q 4-i =……=q 4-N =q4. Pitch degree of freedom q in a series compound joint 5-i The following relationship is satisfied: 5-1 =q 5-2 =……=q 5-i =……=q 5-N =q5.

[0048] By constructing the world coordinate system and the coordinate system corresponding to each joint, the DH parameters shown in Table 1 are constructed according to the degree of freedom of each joint.

[0049] Table 1 DH parameters

[0050]

[0051] Each coordinate system can be transformed through a transformation matrix. The transformation matrix between the body coordinate systems of two adjacent joints is the same as a i ,θ i , β i d i Related, coordinate system {O i-1}Along X i-1 Axis translation a i , and then around Z i-1 Axis rotation β i , and finally around Y i Axis rotation θ i To coordinate system {O i}. i is the coordinate system {O i-1}Origin to coordinate system {O i} Origin along X i-1 Measured distance; β i Y i-1 To Y i Along Z i-1 Angle of rotation; θ i For X i-1 To X i Along Y i Angle of rotation; d i For X i-1 To X i Along Z i Measure the distance.

[0052] The transformation matrix between the corresponding adjacent coordinate systems in the table As shown below:

[0053]

[0054] Substituting the DH parameters into the above formula, the transformation matrix corresponding to each single joint can be obtained. For example, the coordinate system corresponding to the i-th yaw single joint in the series compound joint is {O 4-i}, the transformation matrix corresponding to the single joint is as follows:

[0055]

[0056] In formula (1), when i=1, for The follow-up is similar.

[0057] The coordinate system corresponding to the i-th pitch single joint in the series compound joint is {O 5-i}, the transformation matrix corresponding to the single joint is as follows:

[0058]

[0059] Step 2: Concatenate the composite joint global transformation matrix.

[0060] Yaw freedom q 4-i and pitch freedom q 5-i Staggered distribution, only one yaw degree of freedom q is considered 4-i and pitch freedom q 5-i The corresponding transformation matrix As shown below:

[0061]

[0062] Substituting formula (1) and formula (2) into the equation, we can get:

[0063]

[0064] In this example, the series compound joint has N pairs of joints with staggered yaw and pitch distribution, so the transformation matrix of the series compound joint is As shown below:

[0065]

[0066] Right now:

[0067]

[0068] Transformation Matrix is the characteristic matrix of the serial compound joint. To facilitate the analysis of the characteristics of the serial compound joint, the transformation matrix Expressed in the following ways:

[0069]

[0070] The parameter a in the jth row and kth column of the transformation matrix jk Related to q4 and q5, if the conversion relationship is expressed in the form of an equation, the corresponding equation contains many sine and cosine functions, and the equation is complex and has N-th power functions. The traditional numerical solution cannot solve the inverse kinematics of the series compound joint.

[0071] Step 3: Concatenate the mapping relationship of the composite joint representation parameters.

[0072] In step 2, the overall transformation matrix of the series compound joint is sorted out, which is the characteristic matrix of the series compound joint. The characteristic matrix of the series compound joint includes 16 parameters, among which the parameters in the 4th row are fixed values, and the specific values ​​are as follows: a 41 =0, a 42 =0, a 43 =0, a 44=1. From the remaining 12 parameters, the characterization parameters of the series compound joint can be screened out, and the kinematic characteristics of the series compound joint can be analyzed through this parameter. Parameter a can be analyzed through simulation software ij The relationship between the degrees of freedom of the composite joint q4 and q5 is as follows: Figure 3 The variation surface shown is similar to Figure 4 The equipotential variation diagram is shown.

[0073] It can be seen from the figure that parameter a 11 、a 14 、a 22 、a 23 、a 32 、a 33 There are convex points in the surface graph of the changes of q4 and q5. It is not suitable to solve the degree of freedom parameters q4 and q5 through the above matrix parameters because there are multiple solutions. Figure 4 The parameter a shown ij It can be seen from the equipotential change diagram that parameter a 13 、a 31 、a 34 The change direction of each equipotential line is close to the vertical line. When the parameter value is determined, the change range of the degree of freedom q4 is small. 33 Although the direction of change of each equipotential line is also close to the vertical line, when the parameter value is determined, two equipotential lines symmetrical along the middle vertical line will appear, and the degree of freedom q4 has two ranges of change. Figure 4 The parameter a shown ij It can be seen from the equipotential change diagram that parameter a 12 、a 21 、a 24 The change direction of each equipotential line is close to the horizontal line. When the parameter value is determined, the change range of the degree of freedom q5 is small. 22 Although the change direction of each equipotential line is also close to the horizontal line, when the parameter value is determined, two equipotential lines symmetrical along the middle horizontal line will appear, and the degree of freedom q5 has two change ranges.

[0074] According to the analysis results of the equipotential change diagram, we can get Figure 5 The composite joint matrix shown in the figure represents the two-dimensional variation of the parameters with the composite joint degrees of freedom. The six figures are parameter a 12 Variation of degree of freedom q5, parameter a 13 Variation of degree of freedom q4, parameter a 21 Variation of degree of freedom q5, parameter a 24 Variation of degree of freedom q5, parameter a 31 Variation of degree of freedom q4, parameter a 34The graph of the change with the degree of freedom q4. From the above two-dimensional graph, we can see that the matrix parameter a 13 、a 31 、a 34 Suitable for solving the degree of freedom parameter q4 and matrix parameter a 12 、a 21 、a 24 Suitable for solving the degree of freedom parameter q5.

[0075] Step 4: Mapping relationship between position vector and attitude vector and each degree of freedom.

[0076] Through steps 1, 2, and 3, a transformation matrix T represented by each degree of freedom can be constructed. Then, the end position vector [p x ,p y ,p z ] and the posture vector [n x ,n y ,n z ] and the mapping relationship between each transformation matrix T:

[0077]

[0078] The vector [o x ,o y ,o z ] and the vector [a x ,a y ,a z ] are the unit matrices along the y-axis and z-axis directions, respectively, representing the directions of the y-axis and z-axis of the coordinate system of the end of the robot.

[0079] Both sides of the equal sign are multiplied by the inverse matrix of the transformation matrix related to q1, and formula (4) can be rearranged into:

[0080]

[0081] Both sides of the equal sign are multiplied by the inverse matrix of the transformation matrix related to q6, let L 67 =L6+L7, formula (5) can be rearranged into:

[0082]

[0083] After finishing, we can get:

[0084]

[0085] Solving the transformation matrix The inverse matrix of , we can get:

[0086]

[0087] Multiply both sides of the equal sign by the transformation matrix first The inverse matrix of The representation matrix of , formula (6) can be organized into:

[0088]

[0089] Formula (7) represents the mapping relationship between the position vector and the attitude vector and each degree of freedom. Through this formula consisting of three 4×4 matrices, 16 equations can be obtained. By properly applying some formulas, the values ​​of all degrees of freedom can be iteratively solved.

[0090] Step 5: Construct the iteration sequence and solution method.

[0091] Through step 3, the variation law of the characterization parameters of the series composite joint is analyzed, and the mapping relationship between the key characterization parameters and the two composite joint degrees of freedom is extracted. Through step 4, the mapping relationship between the position vector and the attitude vector and each degree of freedom is constructed. In this step, an iterative solution method is constructed, and the solution speed is accelerated by planning the solution sequence. Using the position vector and the attitude vector as input, the solution process is simplified, and the desired solution can be solved through several iterations.

[0092] Step 5.1: Build the iteration sequence.

[0093] There are many ways to iterate the solution order, such as the three listed below.

[0094] Method 1: According to a 21 Solve the equation for q2, according to a 11 Solve the equation for q3, according to a 31 Equation adjusts q5 according to a 22 Solve equation q6, according to a 12 Equation adjusts q4, and after the above process is iterated w times, according to a 14 Solve the equation for q1.

[0095] Method 2: According to a 21 Solve the equation for q2, according to a 11 Solve the equation for q3, according to a 24 Solve the equation for q1, according to a 22 Solve equation q6, according to a 12 Equation adjusts q4 according to a 34 After the above process is iterated w times, according to a 14 Solve the equation for q1.

[0096] Method 3: According to a 14 Solve the equation for q1, according to a 24 Solve the equation for q2, according to a 34Solve the equation for q3, according to a 31 Equation adjusts q4 according to a 21 Equation adjusts q5 according to a 22 Solve the equation q6. After iterating the above process w times, the final solution of the degrees of freedom can be obtained.

[0097] Based on the conclusions obtained in step 3, it can be seen that in methods 1 and 2, the methods for solving the composite joint degree of freedom parameters q4 and q5 have multiple solutions and slow convergence speed. Therefore, based on the mapping relationship of the characterization parameters of the series composite joint, method 3 is formulated as the iterative solution sequence, and each step corresponds to a solution module.

[0098] Step 5.2: Set the initial values ​​of the parameters.

[0099] Before iteration, it is necessary to define the initial values ​​of each parameter. The solution result at the previous moment can be used as the initial value of each parameter, or the initial value of each parameter can be set to zero.

[0100] Step 5.3: Solve the composite joint transformation matrix.

[0101] Enter the values ​​corresponding to the current parameters and substitute them into formula (3) in step 2 to solve the transformation matrix corresponding to a single yaw and pitch composite joint. Then solve the transformation matrix of the entire series compound joint Solve for the parameter a in the matrix ij The numerical value of .

[0102] Step 5.4: Iterative solution or iterative mediation.

[0103] Each parameter is iteratively optimized in turn according to the iteration order, which can be divided into three categories: (1) solving translation parameters; (2) solving non-compound joint parameters; and (3) adjusting compound joint parameters.

[0104] (1) Method for solving translation parameters.

[0105] According to a 14 Solving the equation for q1 is to solve the parameter corresponding to the translation. According to formula (7), 16 equations can be obtained, among which a 14 The corresponding equation is as follows:

[0106] (p x -L 67 n x -q1)cosq2cosq3+(p y -L 67 n y )sinq2cosq3+(L 67 n z -p z)sinq3-(L2cosq2+L3)cosq3=a 14 After finishing, we can get:

[0107]

[0108] According to the fixed parameter L i , the parameter a obtained in step 5.3 14 , parameter initial value q i , end position parameter p i , terminal posture parameter n i , solve the value of parameter q1 corresponding to this iteration.

[0109] (2) Methods for solving non-compound joint parameters.

[0110] According to a 24 Solving the equation for q2 is to solve the parameters corresponding to the non-compound joint. According to formula (7), we can get a 24 The corresponding equation is as follows:

[0111] (L 67 n x -p x +q1)sinq2+(p y -L 67 n y )cosq2+L2sinq2=a 24

[0112] After finishing, we can get:

[0113] (L 67 n x -p x +q1+L2)sinq2+(p y -L 67 n y )cosq2=a 24

[0114] The above formula is the format of Asinx+Bcosx=C in sine and cosine. A, B, and C are specific values ​​that can be solved, and x is the unknown quantity to be solved. In this step, x is the degree of freedom parameter q2.

[0115] C=a 24 .

[0116] The present invention proposes a universal sine and cosine solution method, the specific process of which includes: coefficient normalization; coefficient sign judgment; sine and cosine equation angle solution.

[0117] The coefficients are normalized as follows:

[0118]

[0119] Coefficient sign judgment. If A≥0, no modification is required. If A0, then:

[0120]

[0121] The solution formula for the unknown quantity x in the sine and cosine equations is as follows:

[0122] x = arcsin(C)-arctan(B / A)

[0123] Through the above method, the value of q2 in this iteration process can be solved. 34 Solve equation q3, according to a 22 The iterative solution method corresponding to solving equation q6 is also a method for solving non-compound joint parameters.

[0124] (3) Methods for adjusting composite joint parameters.

[0125] According to a 31 The equation adjusting q4 is to adjust the parameters corresponding to the composite joint. According to formula (7), we can get a 31 The corresponding equation is as follows:

[0126] n x cosq2sinq3+n y sinq2sinq3+n z cosq3=a 31

[0127] By symbol a 31 (q2,q3) represents the value a obtained by solving the above equation 31 .

[0128] According to the method in step 5.3, input the current values ​​of q4 and q5, and the parameter a can be obtained 31 , the a solved this time 31 By symbol a 31 (q4,q5) represents.

[0129] a 31 The value of (q4,q5) and a 31 The difference in the values ​​of (q2,q3) is defined as e 31 .

[0130] e 31 =a 31 (q2,q3)-a 31 (q4,q5)

[0131] Define the maximum value q of the solution parameter before solving 4,max , minimum value q4,min and the initial current value q 4,new . Use Figure 6 The binary adjustment method shown in the figure, let i and j in the figure be 3 and 1 respectively, according to a 31 The numerical adjustment parameter q4 of (q2,q3) makes a 31 The value of (q4,q5) and a 31 The difference e between the values ​​of (q2,q3) 31 Less than the specified value δ 31 .

[0132] Through the above method, the composite joint parameter q4 in this iteration process can be adjusted. 21 The adjustment method corresponding to equation adjustment q5 is also the same compound joint parameter adjustment method.

[0133] Step 5.5: Loop iteration.

[0134] After iterating steps 5.3 to 5.4 w times, the final solution of all degrees of freedom can be obtained. The theoretical values ​​of the end position and posture can be obtained based on the solved degrees of freedom, and the value of w can be determined based on the error between the theoretical value and the expected value of the end posture being less than the specified value.

[0135] Finally, the numerical values ​​of all degrees of freedom are solved through cyclic iteration, which solves the problem that the composite joint matrix has many dimensions, is complex, and cannot be used for inverse kinematics solution.

[0136] Example:

[0137] In the embodiment, a robot arm including N=5 yaw and pitch staggered series compound joints is taken as an example. Assume that the 7 degrees of freedom in the robot arm are: q1=30mm, q2=15°, q4=5°、 q6=0°, q7=0°. Through forward kinematics, the end positions of the robot arm can be solved as follows: x =94.10, The terminal postures are n x =0.93, n y =0.25, n z =0.26.

[0138] Input the above-mentioned robot arm end position and end posture, adopt the method of the present invention, take w=10 to get Figure 7 The iterative solution result process diagram of each joint degree of freedom is shown. It can be seen from the figure that when w=4, q5 is equal to the final solution, when w=5, q4 is equal to the final solution, when w=6, q3 is equal to the final solution, and when w=7, q1 and q2 are equal to the final solution, which verifies the effectiveness of the present invention.

[0139] The present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores computer instructions, and when the computer instructions are executed on a computer, the computer executes Figure 1 The method described.

[0140] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage and optical storage, etc.) containing computer-usable program code.

[0141] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0142] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0143] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0144] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

[0145] The contents not described in detail in the specification of the present invention belong to the common knowledge of those skilled in the art.

Claims

1. A method for solving inverse kinematics of a series composite joint, characterized in that: include: Construct the transformation matrix between each single joint of the robot and the world coordinate system; The transformation matrix of the entire series composite joint of the robot arm is constructed according to the transformation matrix corresponding to each joint, and is used as the characteristic matrix of the series composite joint of the robot arm; According to the mapping relationship of each parameter in the characteristic matrix of the serial compound joint of the robot arm, the characterization parameters of the serial compound joint are selected, and the kinematic characteristics of the serial compound joint are determined according to the characterization parameters; According to the characteristic matrix and kinematic characteristics of the manipulator's serial compound joints, the mapping relationship between the position vector and the posture vector and each degree of freedom is constructed; An iteration sequence and a solution method are constructed to iteratively solve the mapping relationship between the position vector and the attitude vector and each degree of freedom.

2. A method for solving inverse kinematics of a series composite joint according to claim 1, characterized in that: The transformation matrix of the entire series composite joint of the robot arm is: Where N is the number of yaw degrees of freedom and pitch degrees of freedom in the manipulator's serial compound joints. is the coordinate system {O 5-i }Relative to the coordinate system {O 5-(i-1) } homogeneous transformation matrix, i = 1, 2, ..., N.

3. The method for solving inverse kinematics of a series composite joint according to claim 1, characterized in that: The characteristic matrix of the manipulator series composite joint is: Among them, q4 is the yaw degree of freedom in the series compound joint, q5 is the pitch degree of freedom in the series compound joint, and a jk is the parameter of the jth row and kth column in the transformation matrix, j is the row number variable, ranging from 1 to 4, k is the column number variable, ranging from 1 to 4, is the coordinate system {O 5-N } is the homogeneous transformation matrix relative to the coordinate system {O3}.

4. A method for solving inverse kinematics of a series composite joint according to claim 3, characterized in that: The fourth row parameter in the characteristic matrix is ​​a fixed value, a 41 =0, a 42 =0, a 43 =0, a 44 =1.

5. The method for solving inverse kinematics of a series composite joint according to claim 3, characterized in that: The mapping relationship between the position vector and attitude vector and each degree of freedom is: in, is the homogeneous transformation matrix of coordinate system {O3} relative to coordinate system {O1}, is the coordinate system {O3} relative to the coordinate system {O 5-N }, q1 is the overall translational degree of freedom, q2 is the yaw degree of freedom, q3 is the pitch degree of freedom, q4 is the yaw degree of freedom in the compound joint, q5 is the pitch degree of freedom in the compound joint, q6 is the torsion degree of freedom, n x is the component of the x-axis of the end link coordinate system in the x-axis direction of the natural coordinate system {O0}, n y is the component of the x-axis of the end link coordinate system in the y-axis direction of the natural coordinate system {O0}, n z is the component of the x-axis of the end link coordinate system in the z-axis direction of the natural coordinate system {O0}, o x is the component of the y-axis of the end link coordinate system in the x-axis direction of the natural coordinate system {O0}, o y is the component of the y-axis of the end link coordinate system in the y-axis direction of the natural coordinate system {O0}, o z is the component of the y-axis of the end link coordinate system in the z-axis direction of the natural coordinate system {O0}, a x is the component of the z-axis of the end link coordinate system in the x-axis direction of the natural coordinate system {O0}, a y is the component of the z-axis of the end link coordinate system in the y-axis direction of the natural coordinate system {O0}, a z is the component of the z-axis of the end link coordinate system in the z-axis direction of the natural coordinate system {O0}, p x is the position coordinate of the origin of the end link coordinate system in the x-axis direction of the natural coordinate system {O0}, p y is the position coordinate of the origin of the end link coordinate system in the y-axis direction of the natural coordinate system {O0}, p z L is the position coordinate of the origin of the end link coordinate system in the z-axis direction of the natural coordinate system {O0}, 67 =L6+L7 is the coordinate system {O 5-N }The distance from the origin to the origin of the coordinate system {O7}.

6. A method for solving inverse kinematics of a series composite joint according to claim 5, characterized in that: The construction iteration sequence and solution method include three methods, namely: Method 1: According to a 21 Solve the equation for q2, according to a 11 Solve the equation for q3, according to a 31 Equation adjusts q5 according to a 22 Solve equation q6, according to a 12 Equation adjusts q4, after the loop iterates a preset number of times, according to a 14 Solve the equation for q1; Method 2: According to a 21 Solve the equation for q2, according to a 11 Solve the equation for q3, according to a 24 Solve the equation for q1, according to a 22 Solve equation q6, according to a 12 Equation adjusts q4 according to a 34 The equation adjusts q5, and after the preset number of loop iterations, according to a 14 Solve the equation for q1; Method three, according to a 14 Solve the equation for q1, according to a 24 Solve the equation for q2, according to a 34 Solve the equation for q3, according to a 31 Equation adjusts q4 according to a 21 Equation adjusts q5 according to a 22 The equation is solved for q6, and after a preset number of iterations, the final solution with degrees of freedom is obtained.

7. A method for solving inverse kinematics of a series composite joint according to claim 6, characterized in that: The theoretical values ​​of the terminal position and posture are obtained based on the solved degrees of freedom, and the preset number of times is determined based on the error between the theoretical value and the expected value of the terminal posture being less than the specified value.

8. The method for solving inverse kinematics of a series composite joint according to claim 5, characterized in that: The iterative solution of the mapping relationship between the position vector and the attitude vector and each degree of freedom includes: pass Solve for the translation parameter q1; By (L 67 n x -p x +q1)sinq2+(p y -L 67 n y )cosq2+L2sinq2=a 24 Solve for the non-compound joint parameter q2, and similarly solve for q3 and q6; By x cosq2sinq3+n y sinq2sinq3+n z cosq3=a 31 Solve for the compound joint parameter q4, and solve for q4 in the same way; Wherein, L2 is the distance between the origin of the coordinate system {O1} and the origin of the coordinate system {O2}, and L3 is the distance between the origin of the coordinate system {O2} and the origin of the coordinate system {O3}.

9. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 8 are implemented.

10. A device for solving inverse kinematics of a serial composite joint, 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 computer program, the steps of the method according to any one of claims 1 to 8 are implemented.

Images