Series compound joint inverse kinematics solving method

Abstract

A series composite joint inverse kinematics solving method belongs to the technical field of robots, and comprises the following steps: for a mechanical arm, a conversion matrix corresponding to each single joint is constructed; for a series composite joint, an overall conversion matrix is constructed, which is a characteristic matrix of the series composite joint; a mapping relationship of the representation parameters of the series composite joint is obtained, and the representation parameters of the series composite joint are screened out, and the kinematics characteristics of the series composite joint can be analyzed through the parameters; based on the above three steps, the mapping relationship between the position vector and the attitude vector and each degree of freedom is constructed; the iteration sequence and the solving method are constructed, which comprises the construction of the iteration sequence, the setting of the initial value of the parameter, the solving of the numerical value of the composite joint conversion matrix, the iterative solving or iterative adjustment, and the cyclic iteration; for the translation parameters, the non-composite joint parameters and the composite joint parameters, corresponding solving or adjustment methods are respectively proposed. The present application is universal and feasible for the series composite joint.

Description

TECHNICAL FIELD

[0001] The application relates to a series composite joint inverse kinematics solving method and belongs to the technical field of robots. BACKGROUND

[0002] The series composite joint inverse kinematics solving belongs to the field of mechanical arms and robots. The series composite joint is a technology in the field of mechanical arm engineering, which involves combining multiple joints or motion units in a series to achieve more complex motion and higher flexibility. Inverse kinematics solving is the process of solving the angles of each joint when the position and attitude of the end of the mechanical arm are known. Accurate inverse kinematics solving of the series composite joint is crucial for fast and stable control and path planning. However, the series composite joint has multiple degrees of freedom, and the conversion relationship between parameters is nonlinear and complex. Traditional numerical and analytical inverse kinematics solving methods cannot be directly applied to the inverse kinematics solving of this type of structure. Therefore, a general kinematics solving method is urgently needed for the series composite joint.

[0003] For the inverse kinematics solving of the mechanical arm, the analytical method can obtain a closed-form solution with fast calculation speed, but it is not suitable for all types of mechanical arms. The traditional numerical method solves the problem through iterative approximation, has a wide range of applications, and there is room for optimization in calculation efficiency. For special structures, the traditional method also needs to be modified and improved accordingly. Patent ZL 201910548364.6 proposes a mechanical arm inverse kinematics solving method, which solves the inverse kinematics problem through iteration. Patent ZL 201910278057.0 proposes a numerical unique solution method for a six-degree-of-freedom mechanical arm, which overcomes the requirement that the Jacobian matrix must be full rank. The above patents are only applicable to mechanical arms with six or fewer degrees of freedom and cannot solve the inverse kinematics of mechanical arms including composite joints. Patent ZL 202310437038.4 proposes an inverse kinematics solving method that combines the pose segmentation method and algebraic method for a surgical manipulator. This type of structure also includes a continuous angle bending series composite joint, but this composite joint only includes one type of degree of freedom, i.e., deflection, and does not consider the composite joint type with alternating series of pitch and deflection. For the series composite joint, especially the composite joint type with alternating series of pitch and deflection, there is currently no general kinematics solving method.

[0004] In summary, although there are many studies related to kinematics, there is no general solving method for the series composite joint. Because the series composite joint has multiple degrees of freedom, it cannot be written in matrix form, and thus inverse kinematics solving cannot be performed through inverse matrix. SUMMARY

[0005] The technical problems solved by the present application are: overcoming the deficiencies of the prior art, providing a series composite joint inverse kinematics solving method, constructing the overall conversion matrix of the series composite joint, screening out the representation parameters that can represent the kinematic characteristics of the series composite joint, solving the composite joint conversion matrix value by constructing the iteration sequence, setting the initial value of the parameter, and proposing a mediation method for the composite joint parameters, and then iteratively solving or iteratively mediating to solve all the degree of freedom values, solving the problem of multiple dimensions, complexity and inability to be used for inverse kinematics solving of the composite joint matrix.

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

[0007] Constructing the conversion matrix between each single joint of the mechanical arm and the world coordinate system;

[0008] Constructing the conversion matrix of the overall series composite joint of the mechanical arm according to the conversion matrix corresponding to each joint, and taking the conversion matrix as the characteristic matrix of the series composite joint of the mechanical arm;

[0009] According to the mapping relationship of each parameter in the characteristic matrix of the series composite joint of the mechanical arm, screening the representation parameters of the series composite joint, and determining the kinematic characteristics of the series composite joint according to the representation parameters;

[0010] According to the characteristic matrix and the kinematic characteristics of the series composite joint of the mechanical arm, constructing the mapping relationship between the position vector and the attitude vector and each degree of freedom;

[0011] Constructing the iteration sequence and the solving mode, and iteratively solving the mapping relationship between the position vector and the attitude vector and each degree of freedom.

[0012] Further, the conversion matrix of the overall series composite joint of the mechanical arm is ; wherein, is the number of yaw degrees of freedom and the number of pitch degrees of freedom in the series composite joint of the mechanical arm, is the homogeneous transformation matrix of the coordinate system {O 5-i} relative to the coordinate system {O 5-(i-1)}. .

[0013] Further, the characteristic matrix of the series composite joint of the mechanical arm is ; wherein, is the yaw degree of freedom in the series composite joint, is the pitch degree of freedom in the series composite joint, is the parameter in the first j row and the first k column of the conversion matrix, is a row variable, ranging from 1 to 4, is a column variable, ranging from 1 to 4, is the homogeneous transformation matrix of the coordinate system {O3} relative to the coordinate system {O1}, 5-N is the homogeneous transformation matrix of the coordinate system {O3} relative to the coordinate system {O1},

[0014] Further, the fourth row parameter in the feature matrix is a constant value, a 41 = 0, a 42 = 0, a 43 = 0, a 44 = 1.

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

[0016] ; wherein, is the homogeneous transformation matrix of the coordinate system {O3} relative to the coordinate system {O1}, is the homogeneous transformation matrix of the coordinate system {O3} relative to the coordinate system {O1}, 5-N is the homogeneous transformation matrix of the coordinate system {O3} relative to the coordinate system {O1}, q 1 is the overall translational degree of freedom, q 2 is the yaw degree of freedom, q 3 is the pitch degree of freedom, q 4 is the yaw degree of freedom in the compound joint, q 5 is the pitch degree of freedom in the compound joint, q 6 is the torsion degree of freedom, n x is the component of the z-axis of the end link coordinate system in the direction of the z-axis of the natural coordinate system {O0}, x is the component of the x-axis of the end link coordinate system in the direction of the x-axis of the natural coordinate system {O0}, x is the component of the y-axis of the end link coordinate system in the direction of the y-axis of the natural coordinate system {O0}, n y is the component of the x-axis of the end link coordinate system in the direction of the x-axis of the natural coordinate system {O0}, x is the component of the y-axis of the end link coordinate system in the direction of the y-axis of the natural coordinate system {O0}, y is the component of the z-axis of the end link coordinate system in the direction of the z-axis of the natural coordinate system {O0}, n z is the component of the y-axis of the end link coordinate system in the direction of the y-axis of the natural coordinate system {O0}, x is the component of the z-axis of the end link coordinate system in the direction of the z-axis of the natural coordinate system {O0}, z is the component of the x-axis of the end link coordinate system in the direction of the x-axis of the natural coordinate system {O0}, o x is the component of the z-axis of the end link coordinate system in the direction of the z-axis of the natural coordinate system {O0}, y is the component of the x-axis of the end link coordinate system in the direction of the x-axis of the natural coordinate system {O0}, x is the component of the y-axis of the end link coordinate system in the direction of the y-axis of the natural coordinate system {O0}, o y is the component of the y-axis of the end link coordinate system in the direction of the y-axis of the natural coordinate system {O0}, y is the component of the x-axis of the end link coordinate system in the direction of the x-axis of the natural coordinate system {O0}, y is the component of the z-axis of the end link coordinate system in the direction of the z-axis of the natural coordinate system {O0}, o z is the component of the z-axis of the end link coordinate system in the direction of the z-axis of the natural coordinate system {O0},y the component of the axis in the natural coordinate system {O0}, z a x the position coordinates of the origin of the end-link coordinate system in the natural coordinate system {O0} in the axis direction, z the component of the axis in the natural coordinate system {O0}, x the component of the axis in the natural coordinate system {O0}, a y the position coordinates of the origin of the end-link coordinate system in the natural coordinate system {O0} in the axis direction, z the component of the axis in the natural coordinate system {O0}, y the component of the axis in the natural coordinate system {O0}, a z the position coordinates of the origin of the end-link coordinate system in the natural coordinate system {O0} in the axis direction, z the component of the axis in the natural coordinate system {O0}, z the component of the axis in the natural coordinate system {O0}, p x the position coordinates of the origin of the end-link coordinate system in the natural coordinate system {O0} in the axis direction, x the position coordinates of the origin of the end-link coordinate system in the natural coordinate system {O0} in the axis direction, p y the position coordinates of the origin of the end-link coordinate system in the natural coordinate system {O0} in the axis direction, y the position coordinates of the origin of the end-link coordinate system in the natural coordinate system {O0} in the axis direction, p z the position coordinates of the origin of the end-link coordinate system in the natural coordinate system {O0} in the axis direction, z the position coordinates of the origin of the end-link coordinate system in the natural coordinate system {O0} in the axis direction, L 67 L 6 L 7 the distance from the origin of the coordinate system {O 5-N} to the origin of the coordinate system {O7}.

[0017] Further, the construction iteration sequence and solving method include three methods, which are as follows:

[0018] Method 1, solving the equation according to a 21 Method 2, solving the equation according to q 2 a 11 Method 3, adjusting the equation according to q 3 a 31 Method 5, solving the equation according to q 5 a 22 Method 6, solving the equation according to q 6 a 12 Method 7, adjusting the equation according to q 4 a 14 Method 8, solving the equation according to q 1

[0019] Method 2, solving the equation according to​​a 21 solving the equations q 2. according to a 11 solving the equations q 3. according to a 24 solving the equations q 1. according to a 22 solving the equations q 6. according to a 12 adjusting the equations q 4. according to a 34 adjusting the equations q 5. according to a 14 solving the equations q 1;

[0020] 3. according to a 14 solving the equations q 1. according to a 24 solving the equations q 2. according to a 34 solving the equations q 3. according to a 31 adjusting the equations q 4. according to a 21 adjusting the equations q 5. according to a 22 solving the equations q 6. solving the final solution of the degrees of freedom after the preset number of iterations.

[0021] Further, the theoretical values of the end position and attitude are obtained according to the solved degrees of freedom, and the preset number is determined according to the error between the theoretical values of the end position and attitude and the expected values being less than a specified value.

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

[0023] by solving the translation parameters q 1;

[0024] by solving the non-complex joint parameters q 2. solving q 3 and q 6;

[0025] By solving the composite joint parameters Solving composite joint parameters q 4, and solve in the same way q 4;

[0026] wherein, is the distance between the origin of the coordinate system {O1} and the origin of the coordinate system {O2}, 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, the computer readable storage medium stores a computer program, the computer program is executed by a processor to realize the steps of the series composite joint inverse kinematics solving method.

[0028] A series composite joint inverse kinematics solving device, comprising a memory, a processor and a computer program stored in the memory and executable on the processor, the processor executes the computer program to realize the steps of the series composite joint inverse kinematics solving method.

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

[0030] (1) The present application proposes a series composite joint inverse kinematics solving method. For a plurality of repeated series of composite joints with pitch and yaw degrees of freedom, a composite matrix is constructed, and the parameters in the matrix are used to represent the series composite joint. The kinematic characteristics of the series composite joint can be analyzed through the parameters;

[0031] (2) The present application proposes a new iterative solving method by analyzing the characteristic parameters. Through several cycles of iterative solving or iterative adjustment, the numerical values of all degrees of freedom are solved, solving the problem of multiple dimensions, complexity and inability to be used for inverse kinematics solving of the composite joint matrix.

[0032] (3) The present application constructs the mapping relationship between the position vector and the attitude vector and each degree of freedom. Through this formula composed of three 4x4 matrices, 16 equations can be obtained. By reasonably using part of the formula, the numerical values of all degrees of freedom can be iteratively solved;

[0033] (4) The present application speeds up the solving speed by planning the solving sequence. BRIEF DESCRIPTION OF DRAWINGS

[0034] Various other advantages and benefits will become apparent to those of ordinary skill in the art upon reading the following detailed description of the preferred embodiments. The drawings are for purposes of illustration only and are not intended to limit the present application thereto. Moreover, the use of the same reference symbols in different drawings indicates similar or identical components. In the drawings:

[0035] Figure 1 A flow chart of a serial compound joint inverse kinematics solving method;

[0036] Figure 2 An example diagram of a mechanical arm including a serial compound joint;

[0037] Figure 3 A surface diagram of a compound joint matrix representation parameter change with a compound joint degree of freedom;

[0038] Figure 4 A potential change diagram of a compound joint matrix representation parameter change with a compound joint degree of freedom;

[0039] Figure 5 A two-dimensional change diagram of a compound joint matrix representation parameter change with a compound joint degree of freedom;

[0040] Figure 6 A compound joint parameter adjustment flow chart;

[0041] Figure 7 A joint degree of freedom iterative solving result process diagram. DETAILED DESCRIPTION

[0042] In order to better understand the above technical solutions, the following will be described in detail by the accompanying drawings and specific embodiments. It should be understood that the embodiments and specific features in the embodiments are detailed descriptions of the technical solutions of the present application, and are not limitations of the technical solutions of the present application. In the case of no conflict, the technical features in the embodiments and the embodiments can be combined with each other.

[0043] The following will be further described in detail by the accompanying drawings and specific embodiments. The specific implementation manner can include: the present application is an inverse kinematics solving method for serial compound joints, which combines the pose segmentation method with the numerical solution method to efficiently solve the inverse kinematics. Compared with other methods related to the inverse kinematics solving of the six-degree-of-freedom mechanical arm, the present application is improved in many aspects, and the effectiveness and practicability of the present application are verified by simulation.

[0044] The method of the present application is aimed at the problem that the serial compound joint has multiple degrees of freedom, the motion conversion matrix is complex, and cannot be solved. The driving decoupling of the combination of the distal continuum segment and the proximal orthogonal joint is divided into a position module and an attitude module, and the confidence domain algorithm is used to directly obtain the optimal solution, so as to realize the iterative solving of the inverse kinematics of the serial compound joint. The flow chart of the method of the present application is as follows: Figure 1As shown, specifically: first, the DH coordinate system is constructed, and the kinematics basic model of the multi-degree-of-freedom surgical executor is constructed (step 1). (Step 2). (Step 3). (Step 4). (Step 6). The present application is applicable to a six-degree-of-freedom mechanical arm, and the improved method is also applicable to a seven-degree-of-freedom or more degree-of-freedom mechanical arm, which can divide all degrees of freedom of the mechanical arm into two sections and distribute them into the two modules of the position model and the attitude model. In the present application, the surgical executor is taken as an example to describe the process and principle of the present application. The specific content in each step is described as follows:

[0045] The present application is based on Figure 2 A mechanical arm including a series composite joint as shown, the process and principle of the present application are described. The specific steps are as follows.

[0046] Step 1: Single-joint transformation matrix.

[0047] Figure 2 The mechanical arm shown includes the following types of degrees of freedom: overall translational degrees of freedom q 1, yaw degree of freedom q 2, pitch degree of freedom q 3, having two degrees of freedom q 4 and q 5, series composite joint, torsional degree of freedom q 6, clip opening and closing degree of freedom q 7. The series composite joint therein is composed of six torsional degrees of freedom and six rotational degrees of freedom, wherein the yaw degree of freedom q 4-i and the pitch degree of freedom q 5-i are staggered, and a total of 2× N small joints are included. The yaw degree of freedom q 4-i in the series composite joint satisfies the following relationship: q 4-1 = q 4-2 = …… = q 4-i = …… = q 4-N = q 4. The pitch degree of freedom q 5-i in the series composite joint satisfies the following relationship: q 5-1 = q 5-2 = …… = q 5-i = …… = q 5-N = q 5.

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

[0049] Table 1 DH Parameters

[0050]

[0051] Coordinate systems can be transformed using transformation matrices. The transformation matrix between the body coordinate systems of two adjacent joints is... a i , θ i , β i , d i Related, coordinate system {O i-1} Along X i-1 Axis translation a i Then circle around Z i-1 Axis rotation β i Finally, around Y i Axis rotation θ i To coordinate system {O i}.in a i For coordinate system {O i-1}Origin to coordinate system {O i Origin along X i-1 The measured distance; β i For 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 The distance measured.

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

[0053]

[0054] Substituting the DH parameters into the above equation, we can obtain the transformation matrix corresponding to each single joint. For example, the first joint in a series composite joint... i The coordinate system corresponding to each yaw joint is {O}4-i The conversion matrix corresponding to the single joint is as follows:

[0055] (1)

[0056] In formula (1), when i = 1, = 0, and the subsequent is the same.

[0057] The coordinate system of the first i pitch single joint in the series composite joint is {O 5-i The conversion matrix corresponding to the single joint is as follows:

[0058] (2)

[0059] Step 2: Overall conversion matrix of the series composite joint.

[0060] The yaw degree of freedom q 4-i is staggered with the pitch degree of freedom q 5-i , and only one yaw degree of freedom q 4-i is considered. q 5-i The conversion matrix corresponding to the yaw and pitch joint is as follows:

[0061]

[0062] By substituting formula (1) and formula (2), the following can be obtained:

[0063] (3)

[0064] The series composite joint in this example has N a joint staggered in yaw and pitch, so the conversion matrix of the series composite joint is as follows:

[0065]

[0066] That is:

[0067]

[0068] The conversion matrix is the characteristic matrix of the series composite joint. In order to analyze the characteristics of the series composite joint, the conversion matrix is expressed in the following way:

[0069]

[0070] The first in the transformation matrix j Line number k Column parameters a jk With q4 and q 5. Relatedly, if the transformation relationship is expressed in equation form, the corresponding equation contains many sine and cosine functions, and the equation is complex, possessing... N Power functions. Traditional numerical methods cannot solve the inverse kinematics of cascaded complex joints.

[0071] Step 3: Mapping relationship of characterization parameters of tandem composite joints.

[0072] In step 2, the overall transformation matrix of the tandem compound joint was compiled, which is the characteristic matrix of the tandem compound joint. The characteristic matrix of the tandem compound joint includes 16 parameters, of which the parameters in the 4th row are constants, with the specific values ​​as follows: a 41 =0、 a 42 =0、 a 43 =0、 a 44 =1. From the remaining 12 parameters, characterization parameters for the tandem complex joint can be selected, and the kinematic characteristics of the tandem complex joint can be analyzed using these parameters. These parameters can be analyzed using simulation software. a ij With composite joint degrees of freedom q 4 and q The relationship between 5 is as follows: Figure 3 The variation surface plot shown is Figure 4 The diagram shows the equipotential changes.

[0073] The parameters can be seen from the graph. a 11 , a 14 , a 22 , a 23 , a 32 , a 33 Follow q 4 and q The surface plot of 5 contains convex points, making it unsuitable for solving the degree-of-freedom parameters using the aforementioned matrix parameters. q 4 and q 5. Because there are multiple solutions. From Figure 4 The parameters shown a ij The isopotential transformation diagram shows that the parameters a 13, a 31 , a 34 The direction of change of each equipotential line is close to a vertical line. Given a fixed parameter value, the degrees of freedom... q The range of variation for parameter 4 is small. a 33 Although the direction of change of each equipotential line is close to that of a vertical line, when the parameter values ​​are fixed, two equipotential lines will appear that are symmetrical along the middle vertical line, increasing the degrees of freedom. q 4 has two ranges of variation. From Figure 4 The parameters shown a ij The isopotential transformation diagram shows that the parameters a 12 , a 21 , a 24 The direction of change of each equipotential line is close to that of a horizontal line. Given a fixed parameter value, the degrees of freedom... q The range of variation for 5 is small. (Parameter) a 22 Although the direction of change of each equipotential line is close to that of a horizontal line, when the parameter values ​​are fixed, two equipotential lines will appear that are symmetrical along the middle horizontal line, increasing the degrees of freedom. q 5. There are two ranges of variation.

[0074] Based on the analysis results of the equipotential transformation diagram, we obtain Figure 5 The diagram shows the two-dimensional variation of the composite joint matrix characterization parameters with the degrees of freedom of the composite joint. The six figures represent the parameters... a 12 With degrees of freedom q 5. Changes and parameters a 13 With degrees of freedom q 4. Changes and parameters a 21 With degrees of freedom q 5. Changes and parameters a 24 With degrees of freedom q 5. Changes and parameters a 31 With degrees of freedom q 4. Changes and parameters a 34 With degrees of freedom q 4. The variation diagram shows the matrix parameters. a 13 , a 31 , a 34Suitable for solving the degree of freedom parameters q 4, matrix parameters a 12 , a 21 , a 24 Suitable for solving the degree of freedom parameters q 5.

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

[0076] Through step 1, step 2, step 3, the conversion matrix represented by each degree of freedom can be constructed T . Further, the end position vector of the actuator p x , p y , p z and the attitude vector n x , n y , n z and each conversion matrix T The mapping relationship between them is:

[0077] (4)

[0078] The vector [o x , o y , o z ] and the vector a x , a y , a z ] are unit matrices along the y axis and z axis direction, representing the direction of the mechanical arm end coordinate system y axis and z axis.

[0079] Both sides of the equation are multiplied by the inverse matrix of the conversion matrix related to q 1 in the front, and formula (4) can be arranged as:

[0080] (5)

[0081] Both sides of the equation are multiplied by the inverse matrix of the conversion matrix related to q 6 in the back, let L 67 = L 6+L 7, formula (5) can be arranged as:

[0082]

[0083] The arrangement can be obtained:

[0084] (6)

[0085] Solve the inverse matrix of the conversion matrix , can be obtained:

[0086]

[0087] Both sides of the equation are multiplied by the inverse matrix of the conversion matrix in the front, and the characteristic matrix of is brought in, and formula (6) can be arranged as:

[0088] (7)

[0089] Formula (7) represents the mapping relationship between the position vector and the attitude vector and each degree of freedom. Through this formula composed of three 4x4 matrices, 16 equations can be obtained. By reasonably using part of the formula, the numerical value of all degrees of freedom can be iteratively solved.

[0090] Step 5: Construct the iteration order and solving method.

[0091] Through step 3, the change rule of the serial compound joint representation parameter is analyzed, and the mapping relationship between the key representation parameter and the two compound 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, the iterative solving method is constructed, and the solving speed is accelerated by planning the solving order. Using the position vector and the attitude vector as input, the solving process is simplified, and the expected solution can be solved through several iterations.

[0092] Step 5.1: Construct the iteration order.

[0093] There are many ways to solve the iteration order, for example, the following three ways are listed.

[0094] Method one: according to a 21 equation to solve q 2, according to a 11 equation to solve q 3, according to a 31 equation to adjust q 5, according to a 22 equation to solve q 6, according toa 12 Equation adjustment q 4. Iterating the above process w times, the final solution of the degrees of freedom of the compound joint can be obtained. a 14 Equation solving q 1.

[0095] Method two: according to a 21 Equation solving q 2, according to a 11 Equation solving q 3, according to a 24 Equation solving q 1, according to a 22 Equation solving q 6, according to a 12 Equation adjustment q 4, according to a 34 Equation adjustment q 5, iterating the above process w times, the final solution of the degrees of freedom of the compound joint can be obtained. a 14 Equation solving q 1.

[0096] Method three: according to a 14 Equation solving q 1, according to a 24 Equation solving q 2, according to a 34 Equation solving q 3, according to a 31 Equation adjustment q 4, according to a 21 Equation adjustment q 5, according to a 22 Equation solving q 6, iterating the above process w times, the final solution of the degrees of freedom of the compound joint can be obtained.

[0097] Based on the conclusion obtained in step 3, it can be seen that in method one and method two, the degrees of freedom parameters of the compound joint q 4 and qThe method of 5 has multiple solutions and slow convergence speed. Therefore, based on the mapping relationship of the serial compound joint representation parameters, method three is formulated as an iterative solution sequence, and each step corresponds to a solution module.

[0098] Step 5.2: Set the initial value of the parameter.

[0099] Before iteration, the initial value of each parameter needs to be defined. The solution result of the previous time 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 compound joint transformation matrix value.

[0101] Input the value corresponding to each parameter, and bring it into formula (3) in step 2 to solve the transformation matrix corresponding to the single yaw and pitch compound joint , and further solve the transformation matrix of the entire serial compound joint , and solve the value of the parameter a ij in the matrix.

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

[0103] According to the iteration sequence, each parameter is iteratively optimized in turn, which is divided into three categories: (1) solving the translation parameter; (2) solving the non-compound joint parameter; (3) adjusting the compound joint parameter.

[0104] (1) Method for solving the translation parameter.

[0105] According to the equation a 14 , the parameter corresponding to the translation is solved as q 1. According to formula (7), 16 equations can be obtained, in which a 14 The corresponding equations are as follows:

[0106]

[0107] After sorting, we get:

[0108]

[0109] According to the fixed parameters L i , the parameters obtained in step 5.3 a 14 , the initial value of the parameter q i , the end position parameter p i , and the end attitude parameter n i, solve the parameters corresponding to this iteration q 1.

[0110] (2) the method for solving the non-composite joint parameters.

[0111] According to a 24 Solve q 2, that is, to solve the parameters corresponding to the non-composite joint. According to formula (7), we can get a 24 The corresponding equation is as follows:

[0112]

[0113] After sorting, we get:

[0114]

[0115] The above formula is the format. Among them, A , B , C are specific values that can be solved, x is the unknown quantity to be solved. In this step, x is the degree of freedom parameter q 2, A = L 67 n x - p x + q 1+ L 2, B = p y - L 67 n y , C = a 24 .

[0116] The present application proposes a general method for solving the cosine, and the specific process includes: coefficient normalization; coefficient sign judgment; cosine equation angle solving.

[0117] The coefficient normalization method is as follows:

[0118]

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

[0120]

[0121] Unknown quantity in the cosine equation x The solution formula is as follows:

[0122]

[0123] Through the above method, the value of q 2 in this iteration process can be solved. According to a 34 Solve the equation q 3, solve the equation according to a 22 Solve the equation q 6 The corresponding iterative solution method is also a method for solving non-composite joint parameters.

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

[0125] According to a 31 Adjust the equation q 4, which is the corresponding parameter of the adjusted composite joint. According to formula (7), we can get a 31 The corresponding equation is shown below:

[0126]

[0127] Through the symbol a 31 ( q 2, q 3) represents the value a 31 solved by the above formula.

[0128] According to the method in step 5.3, input the current values of q 4 and q 5, the parameter a 31 solved this time can be obtained. The value of a 31 solved this time is represented by the symbol a 31 ( q 4, q 5).

[0129] a 31 The difference between the value of q 4, q 5 and the value of a 31 ( q 2, q 3) is defined as e31 .

[0130]

[0131] Define the maximum value of the solution parameters before solving. q 4,max Minimum value q 4,min and initial current value q 4,new Adopting, for example Figure 6 The dichotomy adjustment method shown in the figure is to make the figure... i and j They are 3 and 1 respectively, according to a 31 ( q 2, q 3) Numerical adjustment parameters q 4, making a 31 ( q 4, q The value of 5) and a 31 ( q 2, q 3) The numerical difference e 31 Less than the specified value .

[0132] The composite joint parameters in this iteration process can be adjusted using the methods described above. q 4. According to a 21 Equation Adjustment q The adjustment method corresponding to 5 is also the same composite joint parameter adjustment method.

[0133] Step 5.5: Iterate.

[0134] Repeat steps 5.3 to 5.4. w After this, the final solution for all degrees of freedom can be obtained. Based on the solved degrees of freedom, the theoretical values ​​for the end effector position and attitude can be obtained. The final position and attitude can be determined based on the error between the theoretical and expected values ​​of the end effector pose being less than a specified value. w The value.

[0135] Finally, by iteratively solving for all degrees of freedom, the problem of complex joint matrices having multiple dimensions and being unsuitable for inverse kinematics solutions was solved.

[0136] Example:

[0137] In the embodiment, a method including a NTake a robotic arm with 5 tandem joints arranged in an alternating pattern of yaw and pitch as an example. Assume the 7 degrees of freedom in this robotic arm are as follows: q 1=30mm q 2=15° q 3 = -15° q 4=5° q 5 = -5° q 6=0°、 q 7 = 0°. The end effector positions of the robotic arm can be determined using forward kinematics: p x =94.10、 p y =-4.40、 p z =-7.22, end attitudes are respectively n x =0.93、 n y =0.25、 n z =0.26.

[0138] Input the above-mentioned end-effector position and end-effector posture, and use the method in this invention to obtain... w =10 Figure 7 The diagram shows the iterative solution process for each joint degree of freedom. It can be seen from the diagram that when... w =4 q 5 equals the final solution, when w =5 o'clock q 4 equals the final solution, when w =6 o'clock q 3 equals the final solution, when w =7 o'clock q 1 and q 2 equals the final solution, verifying the effectiveness of the invention.

[0139] This invention provides a computer-readable storage medium storing computer instructions that, when executed on a computer, cause the computer to perform... Figure 1 The method described.

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

[0141] The computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart block or blocks. Figure 1 one or more flow or blocks Figure 1 one or more flow or blocks

[0142] These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the function specified in the flowchart block or blocks. Figure 1 one or more flow or blocks Figure 1 one or more flow or blocks

[0143] The computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart block or blocks. Figure 1 one or more flow or blocks Figure 1 one or more flow or blocks

[0144] Obviously, numerous modifications and variations of the present application are possible in light of the above teachings. It is therefore to be understood that within the scope of the

[0145] Those skilled in the art will appreciate that the application described herein is susceptible to variations and modifications other than those specifically described. It is to be understood that the application includes all such variations and modifications which fall within the spirit and scope of the present application.

Claims

1. A method for solving inverse kinematics of a serial compound joint, characterized in that, The method comprises the following steps: constructing a transformation matrix between each single joint of the mechanical arm and a world coordinate system; constructing a transformation matrix of the whole series compound joint of the mechanical arm according to the transformation matrix corresponding to each joint, and taking the transformation matrix as a characteristic matrix of the series compound joint of the mechanical arm; screening a representation parameter of the series compound joint according to a mapping relationship of each parameter in the characteristic matrix of the series compound joint of the mechanical arm, and determining kinematic characteristics of the series compound joint according to the representation parameter; constructing a mapping relationship between the position vector and the attitude vector and each degree of freedom according to the characteristic matrix and the kinematic characteristics of the series compound joint of the mechanical arm; constructing an iteration order and a solving mode, and iteratively solving the mapping relationship between the position vector and the attitude vector and each degree of freedom; The characteristic matrix of the serial compound joint of the mechanical arm is ; wherein, is a yaw degree of freedom in the serial compound joint, is a pitch degree of freedom in the serial compound joint, is a parameter in the conversion matrix in the first j row and the first k column, is a row variable, ranging from 1 to 4, is a column variable, ranging from 1 to 4, is a homogeneous transformation matrix of the coordinate system {O 5-N} relative to the coordinate system {O3}; the mapping relationship between the position vector and the attitude vector and each degree of freedom is ; wherein is the homogeneous transformation matrix of the coordinate system {03} with respect to the coordinate system {01}, is the homogeneous transformation matrix of the coordinate system {03} with respect to the coordinate system {02}, 5-N q 1 is the overall translational degree of freedom, q 2 is the yaw degree of freedom, q 3 is the pitch degree of freedom, q 4 is the yaw degree of freedom in the compound joint, q 5 is the pitch degree of freedom in the compound joint, q 6 is the twist degree of freedom, n x is the component of the x axis of the end link coordinate system in the direction of the x axis of the natural coordinate system {00}, n y is the component of the x axis of the end link coordinate system in the direction of the y axis of the natural coordinate system {00}, n z is the component of the x axis of the end link coordinate system in the direction of the z axis of the natural coordinate system {00}, o x is the component of the y axis of the end link coordinate system in the direction of the x axis of the natural coordinate system {00}, o y is the component of the y axis of the end link coordinate system in the direction of the y axis of the natural coordinate system {00}, o z is the component of the y axis of the end link coordinate system in the direction of the z axis of the natural coordinate system {00}, a x is the component of the z axis of the end link coordinate system in the direction of the x axis of the natural coordinate system {00}, a y is the component of the z axis of the end link coordinate system in the direction of the y axis of the natural coordinate system {00}, a z is the component of the z axis of the end link coordinate system in the direction of the z axis of the natural coordinate system {00}, p x ​position coordinates in the axis direction of the natural coordinate system {O0} at the origin of the end link coordinate system x position coordinates in the axis direction of the natural coordinate system {O0} at the origin of the end link coordinate system p y position coordinates in the axis direction of the natural coordinate system {O0} at the origin of the end link coordinate system y position coordinates in the axis direction of the natural coordinate system {O0} at the origin of the end link coordinate system p z position coordinates in the axis direction of the natural coordinate system {O0} at the origin of the end link coordinate system z position coordinates in the axis direction of the natural coordinate system {O0} at the origin of the end link coordinate system L 67 = L 6+ L 7, distance from the origin of the coordinate system {O 5-N} to the origin of the coordinate system {O7} the iteratively solving the mapping relationship between the position vector and the attitude vector and each degree of freedom comprises: By Solving for translation parameters q 1; By solving non-composite joint parameters q 2, similarly solve q 3 and q 6; By Solving complex joint parameters q 4, and similarly solving q 4; wherein, is the distance between the origin of the coordinate system {O1} and the origin of the coordinate system {O2}, is the distance between the origin of the coordinate system {O2} and the origin of the coordinate system {O3}.

2. The method according to claim 1, wherein, The conversion matrix of the whole series compound joint of the mechanical arm is ; wherein, is the number of yaw degrees of freedom and the number of pitch degrees of freedom in the series compound joint of the mechanical arm, is the homogeneous transformation matrix of the coordinate system {O 5-i} relative to the coordinate system {O 5-(i-1)}, .

3. The method of solving inverse kinematics of a serial compound joint according to claim 1, wherein, the fourth row parameter in the feature matrix is a constant value, a 41 = 0, a 42 = 0, a 43 = 0, a 44 = 1.

4. The method of solving inverse kinematics of a serial compound joint according to claim 1, wherein, the construction of the iteration order and the solving mode comprises three modes, which are respectively: Mode 1, according to a 21 Equation solving q 2, according to a 11 Equation solving q 3, according to a 31 Equation solving q 5, according to a 22 Equation solving q 6, according to a 12 Equation solving q 4, after a preset number of loop iterations, according to a 14 Equation solving q 1; Method two, according to a 21 Equation solving q 2, according to a 11 Equation solving q 3, according to a 24 Equation solving q 1, according to a 22 Equation solving q 6, according to a 12 Equation solving q 4, according to a 34 Equation solving q 5, after a preset number of loop iterations, according to a 14 Equation solving q 1; Method three, according to a 14 Equation solving q 1, according to a 24 Equation solving q 2, according to a 34 Equation solving q 3, according to a 31 Equation solving q 4, according to a 21 Equation solving q 5, according to a 22 Equation solving q 6, after a preset number of loop iterations, the final solution of the individual degree of freedom is solved.

5. The method according to claim 4, wherein, determining the preset number of times according to the error between the theoretical value of the end position and attitude and the expected value being less than a specified value.

6. A computer-readable storage medium storing a computer program, the computer program comprising instructions that, when executed by a computer, cause the computer to perform the method of any one of claims 1-5. The computer program is executed by the processor to realize the steps of the method according to any one of claims 1-5.

7. A device for forward kinematics solving of a series compound joint, comprising a memory, a processor, and a computer program stored in the memory and capable of running on the processor, characterized in that: The processor executes the computer program to realize the steps of the method according to any one of claims 1-5.

Images