Inverse kinematics solution method, system and equipment for seven-degree-of-freedom manipulator based on numerical iteration

Abstract

The present invention discloses a method, system and device for solving the inverse kinematics of a seven-degree-of-freedom manipulator based on numerical iteration. The method comprises the following steps: S1, establishing a D-H kinematic model of the manipulator based on the joint configuration of the seven-degree-of-freedom manipulator; S2, calculating a Jacobian matrix according to a joint angle sequence; S3, introducing a weight matrix to suppress the joint velocity at the edge of the joint range; S4, introducing a damping factor to calculate an optimized Jacobian pseudo-inverse matrix; S5, calculating the posture error between the desired end pose and the current end pose of the manipulator end effector; S6, converting the posture error into a joint angle adjustment using the optimized Jacobian pseudo-inverse matrix to obtain a joint angle iteration formula; S7, determining the joint angles of each rotating joint of the manipulator based on the posture error using a numerical iteration method. The present invention can avoid the problem of instability in solving the traditional analytical method under singular configurations, thereby improving computational efficiency and numerical stability.

Description

Technical Field

[0001] The present invention belongs to the technical field of redundant manipulator kinematics analysis, and in particular relates to a method, system and device for solving inverse kinematics of a seven-degree-of-freedom manipulator based on numerical iteration. Background Art

[0002] When a robotic arm completes a given Cartesian space task, its kinematics algorithm must fully account for various constraints. Joint limits are a constant and essential constraint. If a robotic arm joint exceeds its operating range, it can not only cause errors in task execution but can also damage or overload mechanical components.

[0003] Traditional analytical solutions often suffer from unstable solutions, or even become unsolvable, when the manipulator approaches a singular configuration. Furthermore, analytical solutions lack stability and flexibility in determining the specific structure of the manipulator for different manipulator types. Consequently, when it comes to addressing complex tasks and the diverse range of manipulators, traditional analytical solutions have significant limitations and are unable to meet the demands of today's complex scenarios. Traditional numerical solutions, on the other hand, suffer from slow convergence and low computational efficiency. Summary of the Invention

[0004] The first purpose of the present invention is to overcome the shortcomings and deficiencies in the prior art and provide a method for solving the inverse kinematics of a seven-degree-of-freedom manipulator based on numerical iteration, which can avoid the instability problem of solving the traditional analytical method under singular configurations and improve the computational efficiency and numerical stability.

[0005] The second object of the present invention is to provide a seven-degree-of-freedom manipulator inverse kinematics solution system based on numerical iteration.

[0006] A third object of the present invention is to provide a computing device.

[0007] The object of the present invention is achieved by the following technical solution: a method for solving inverse kinematics of a seven-degree-of-freedom manipulator based on numerical iteration, comprising the steps of:

[0008] S1. Establish the DH kinematic model of the manipulator based on the joint configuration of the 7-DOF manipulator;

[0009] S2, calculate the Jacobian matrix J according to the joint angle sequence;

[0010] S3. Introduce a weight matrix to suppress the joint velocity at the edge of the joint range and calculate the least squares norm solution of the seven-degree-of-freedom manipulator;

[0011] S4, introducing the damping factor to calculate the optimized Jacobian pseudo-inverse matrix;

[0012] S5. Calculate the posture error between the desired end-effector posture and the current end-effector posture of the robot arm based on the DH kinematic model of the robot arm;

[0013] S6. Use the optimized Jacobian pseudo-inverse matrix to convert the posture error into the joint angle adjustment value and obtain the joint angle iteration formula;

[0014] S7. Based on the posture error, determine the joint angles of each rotational joint of the robotic arm using a numerical iteration method.

[0015] Preferably, in step S1, the DH kinematic model is:

[0016]

[0017] in, Represents the coordinate transformation matrix between adjacent rotational joints of a seven-degree-of-freedom manipulator, where i represents the joint number and i = [1, 2, 3, 4, 5, 6, 7]. is the forward kinematic solution of the manipulator, θ i represents the joint angle, Represents the posture information of the end of the robot arm, p=(p x ,p y ,p z ) T Indicates the position information of the end of the robot arm.

[0018] Preferably, step S2 specifically includes the steps of:

[0019] S21. Calculate the Jacobian matrix based on the current joint angle sequence:

[0020]

[0021] Where J is the Jacobian matrix, and the Cartesian space pose expression of the manipulator end coordinate system is: l = f(q), joint angle sequence

[0022] S22. Establish and solve the kinematic equations of the joint velocity and the end coordinate system velocity of the seven-degree-of-freedom manipulator:

[0023]

[0024] in, is the joint velocity, is the terminal coordinate system velocity and R m represents an m-dimensional vector, R represents the set of real numbers, and m represents the dimension of the vector space;

[0025] Then the minimum norm solution of the seven-degree-of-freedom manipulator is:

[0026]

[0027] Among them, the Jacobian pseudo-inverse matrix J + =J T (JJ T ) -1 , J T Represents the transpose of the Jacobian matrix.

[0028] Preferably, step S3 specifically includes the steps of:

[0029] S31, define the weighted Jacobian matrix J w and weighted joint velocities

[0030] J w =JW -1 / 2 , Formula (5),

[0031]

[0032] Where W is the weight matrix and W∈R h×h , R h×h represents an h×h matrix, and R represents a set of real numbers;

[0033] S32, J w and Replace J and The kinematic equations of joint velocity and end coordinate system velocity are redefined as:

[0034]

[0035] Then the weighted least squares norm solution of formula (2) is:

[0036]

[0037] Among them, W -1 is the inverse weight matrix, is the joint velocity;

[0038] S33, inverse the weight matrix W -1 Defined as:

[0039]

[0040] The weight factors are:

[0041]

[0042] Among them, i=1,2,3,4,5,6,7, the feasible interval of the joint is divided into three parts: negative damping interval Positive damping range and normal range set up

[0043] The function g(·) in formula (10) satisfies the relationship: g(x)=-(2x 3 +3x 2 ) 2 , 0≤x≤1, satisfying g(1)=1, g(0)=0, when the joint θ i When approaching the limit, Will approach 0, thus stopping the i-th joint. When the joint θ i Within the normal range, the i-th joint exits the constraint of joint velocity, so that it can quickly converge to the error range.

[0044] Preferably, step S4 specifically includes the steps of:

[0045] S41. Introducing the damping factor λ to avoid the singularity of the Jacobian matrix, equation (8) is transformed into:

[0046]

[0047] in, is the joint velocity, I m represents the m×m identity matrix;

[0048] S42, combined with formula (11) and formula (3) The optimized Jacobian pseudo-inverse matrix is:

[0049] J +′ =W -1 J T (JW -1 J T +λI m ) -1 , Formula (12),

[0050] S43. In a small time step △t, it is approximately considered that:

[0051]

[0052] Where △q is the change in joint angle within △t, and △l is the change in posture within △t. Combining formulas (11), (13) and (14), we can obtain:

[0053] Δq=W -1 J T (JW-1 J T +λI m ) -1 Δl, Formula (15),

[0054] Preferably, step S5 specifically includes the steps of:

[0055] S51. Calculate the desired end pose and current end pose of the end effector of the robotic arm respectively:

[0056]

[0057] Among them, T d is the desired end pose, T(q k ) is the current end pose, R d represents the desired end posture, p d represents the desired end position, q k represents the joint angle sequence of the current k-th iteration, R k represents the end pose of the current k-th iteration, p k Indicates the current end position;

[0058] S52. Calculate the pose error between the desired end pose and the current end pose:

[0059] △l=(TdT -1 (q k )-I) ∨ , formula (18),

[0060] Where I is the 4×4 identity matrix and ∨ is the anti-wedge operator.

[0061] Preferably, in step S6, the joint angle iteration formula is:

[0062] q k+1 =q k +Δq k , Formula (19),

[0063] Δq k =W -1 J T (JW -1 J T +λI m ) -1 (T d T -1 (q k )-I) ∨ , formula (20),

[0064] Among them, q k represents the joint angle sequence of the k+1th iteration, Δq kRepresents the joint angle change sequence, Δq k =[Δθ1,Δθ2,Δθ3,Δθ4,Δθ5,Δθ6,Δθ7].

[0065] Preferably, step S7 specifically includes the steps of:

[0066] S71. Calculate the infinite norm of the joint angle variation sequence ||Δq k || ∞ :

[0067]

[0068] in, Represents the maximum absolute value of each component of the joint angle change sequence;

[0069] S72. Determine the infinite norm of the joint angle variation sequence || Δq k || ∞ Is it less than the preset threshold?

[0070] If so, output the current joint angle sequence q k ;

[0071] If not, return to step S6 to calculate and update the joint angle sequence until the convergence condition is met.

[0072] The inverse kinematics solution system for a seven-degree-of-freedom manipulator based on numerical iteration includes:

[0073] A modeling module is used to establish a DH kinematic model of the robotic arm based on the joint configuration of the seven-degree-of-freedom robotic arm;

[0074] The first calculation module is used to calculate the Jacobian matrix J according to the joint angle sequence;

[0075] The second calculation module is used to introduce a weight matrix to suppress the joint velocity at the edge of the joint range and calculate the least squares norm solution of the seven-degree-of-freedom manipulator;

[0076] The third calculation module is used to introduce the damping factor to calculate the optimized Jacobian pseudo-inverse matrix;

[0077] a fourth calculation module, configured to calculate a posture error between a desired end-effector posture and a current end-effector posture of the robot arm based on a DH kinematic model of the robot arm;

[0078] The iterative calculation module is used to convert the posture error into the joint angle adjustment value using the optimized Jacobian pseudo-inverse matrix, obtain the joint angle iteration formula, and determine the joint angle of each rotating joint of the robotic arm using the numerical iteration method based on the posture error.

[0079] A computing device includes a processor and a memory for storing a program executable by the processor. When the processor executes the program stored in the memory, the above-mentioned numerical iteration-based inverse kinematics solution method for a seven-degree-of-freedom manipulator is implemented.

[0080] The present invention has the following advantages and effects compared to the prior art:

[0081] (1) The numerical iteration-based inverse kinematics solution method for a seven-DOF manipulator proposed in this invention avoids the problem of unstable solution of the manipulator when approaching singularity in the traditional analytical method by introducing the Jacobian pseudo-inverse matrix damping factor.

[0082] (2) By introducing a weight matrix, the present invention can flexibly deal with joint limit constraints, ensuring that the movement of the joints of the robot arm always remains within a safe range when performing complex tasks, avoiding mechanical damage caused by exceeding the limit; and ensuring that under the joint angle constraints of the robot arm in actual applications, an inverse kinematic solution that meets the requirements can be stably calculated, thereby improving the accuracy and stability of the solution.

[0083] (3) The present invention achieves rapid convergence through a numerical iteration method and is applicable to a variety of robotic arm configurations and operation scenarios. Compared with traditional analytical methods and numerical solution methods, the present invention has higher computational efficiency, better numerical stability and wider application applicability, and can optimize the posture and motion trajectory of the robotic arm under redundant degrees of freedom conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] Figure 1 The figure is a flow chart of the numerical iteration-based inverse kinematics solution method for a seven-degree-of-freedom manipulator according to the present invention.

[0085] Figure 2 This is a schematic structural diagram of a seven-degree-of-freedom robotic arm according to an embodiment of the present invention. DETAILED DESCRIPTION

[0086] The present invention will be further described in detail below with reference to the embodiments and drawings, but the embodiments of the present invention are not limited thereto.

[0087] Example 1

[0088] like Figure 1 The figure shows a flow chart of the inverse kinematics solution method for a seven-degree-of-freedom manipulator based on numerical iteration, including the following steps:

[0089] S1. Establish the DH kinematic model of the manipulator based on the joint configuration of the 7-DOF manipulator;

[0090] S2, calculate the Jacobian matrix J according to the joint angle sequence;

[0091] S3. Introduce a weight matrix to suppress the joint velocity at the edge of the joint range and calculate the least squares norm solution of the seven-degree-of-freedom manipulator;

[0092] S4, introducing the damping factor to calculate the optimized Jacobian pseudo-inverse matrix;

[0093] S5. Calculate the posture error between the desired end-effector posture and the current end-effector posture of the robot arm based on the DH kinematic model of the robot arm;

[0094] S6. Use the optimized Jacobian pseudo-inverse matrix to convert the posture error into the joint angle adjustment value and obtain the joint angle iteration formula;

[0095] S7. Based on the posture error, determine the joint angles of each rotational joint of the robotic arm using a numerical iteration method.

[0096] In step S1, the DH kinematic model is:

[0097]

[0098] in, Represents the coordinate transformation matrix between adjacent rotational joints of a seven-degree-of-freedom manipulator, where i represents the joint number and i = [1, 2, 3, 4, 5, 6, 7]. is the forward kinematic solution of the manipulator, which describes the position and posture of the end of the manipulator relative to the base coordinate system, θ i represents the joint angle, Represents the posture information of the end of the robot arm, p=(p x ,p y ,p z ) T Indicates the position information of the end of the robotic arm, n x ,n y ,n z Respectively represent the cosine value of the angle between the x-axis of the end coordinate system and the x0, y0, and z0 axes of the base coordinate system, o x ,o y ,o z Respectively represent the cosine value of the angle between the y-axis of the end coordinate system and the x0, y0, and z0 axes of the base coordinate system, a x ,a y ,a z They represent the cosine values ​​of the angles between the z-axis of the end coordinate system and the x0, y0, and z0 axes of the base coordinate system.

[0099] Specifically, in this embodiment, first, according to Figure 2 The kinematic model of the seven-DOF manipulator is established based on its structure, joint types, and coordinate systems of each joint. Based on the improved DH parameter method, the coordinate system of each joint is defined and the DH parameters are calculated, as shown in Table 1:

[0100] The method for determining each joint coordinate system adopts the improved DH parameter method. Each joint rotates around the z-axis of the joint coordinate system. Specifically, Figure 2 As shown, z i The axis is along the axis of the i+1 joint, x i Along z i axis and z i-1 The common perpendicular to the axis points away from z i-1 The direction of the y axis i The axis is determined by the right-hand rectangular coordinate system, O i In this embodiment, the initial position of the first joint coordinate system is set on the base of the robot arm and the base coordinate system {O o :x0,y0,z0} coincide, the base coordinate system remains unchanged.

[0101] Table 1 Improved DH parameters of seven-DOF manipulator

[0102] Connecting rod <![CDATA[θ i (°)]]> <![CDATA[d i (m)]]> <![CDATA[a i-1 (m)]]> <![CDATA[α i-1 (°)]]> 1 <![CDATA[θ1]]> 0 0 0 2 <![CDATA[θ2]]> 0 0 -90 3 <![CDATA[θ3]]> <![CDATA[d3]]> 0 90 4 <![CDATA[θ4]]> 0 0 -90 5 <![CDATA[θ5]]> <![CDATA[d5]]> 0 90 6 <![CDATA[θ6]]> 0 0 -90 7 <![CDATA[θ7]]> 0 0 90

[0103] Based on the improved DH parameter method, the parameters of each structure in the embodiment are as follows:

[0104] d3=245mm,d5=255mm,

[0105] Then, the coordinate transformation matrix between adjacent joints is obtained by using the improved DH coordinate change as follows:

[0106]

[0107] Among them, a i-1 Indicates the connecting rod length, d i represents the connecting rod distance, α i-1 Represents the connecting rod torsion angle, and substitutes the DH parameter into the coordinate transformation matrix of the above formula to obtain They are as follows:

[0108]

[0109] According to the working range of the robot arm, this embodiment gives a set of initial joint angles:

[0110] θ1=10°,θ2=20°,θ3=30°,θ4=40°,θ5=50°,θ6=30°,θ7=10°, then multiply the coordinate transformation matrices between adjacent joints in sequence, and calculate the pose matrix of the end coordinate system relative to the base coordinate system through forward kinematics.

[0111]

[0112] Calculate the corresponding Jacobian matrix at this time:

[0113]

[0114] Let the expected end pose matrix

[0115] Get the current joint angle:

[0116] θ1=30°, θ2=29.9999°, θ3=30.0001°, θ4=30°, θ5=29.9999°, θ6=29.9999°,

[0117] θ7=29.9999°.

[0118] The numerical iteration-based inverse kinematics solution method for a seven-degree-of-freedom manipulator proposed in this invention avoids the problem of unstable solution of the manipulator when approaching singularity in traditional analytical methods by introducing the Jacobian pseudo-inverse matrix and the damping factor. It achieves rapid convergence through the numerical iteration method, thereby improving computational efficiency and numerical stability.

[0119] Step S2 specifically includes the following steps:

[0120] S21. Calculate the Jacobian matrix based on the current joint angle sequence:

[0121]

[0122] Where J is the Jacobian matrix, and the Cartesian space pose expression of the manipulator end coordinate system is: l=f(q), joint angle sequence q=(θ1,θ2,θ3,θ4,θ5,θ6,θ7),

[0123] S22. Establish and solve the kinematic equations of the joint velocity and the end coordinate system velocity of the seven-degree-of-freedom manipulator:

[0124]

[0125] in, is the joint velocity, is the terminal coordinate system velocity and R m represents an m-dimensional vector, R represents the set of real numbers, and m represents the dimension of the vector space;

[0126] Then the minimum norm solution of the seven-degree-of-freedom manipulator is:

[0127]

[0128] Among them, the Jacobian pseudo-inverse matrix J + =JT (JJ T ) -1 , J T Represents the transpose of the Jacobian matrix.

[0129] Specifically, in this embodiment, the partial derivative of the end-arm posture is obtained to obtain a 6×7 Jacobian matrix, as shown in formula (2). The Jacobian matrix is ​​a 6×7 matrix, not a square matrix, and cannot be directly inverted. Its pseudo-inverse matrix J is used. + =J T (JJ T ) -1 Substitute for the inverse of the Jacobi.

[0130] Step S3 specifically includes the following steps:

[0131] S31, define the weighted Jacobian matrix J w and weighted joint velocities

[0132]

[0133] Where W is a diagonal and positive definite weight matrix and W∈R h×h , R h×h represents an h×h matrix, and R represents a set of real numbers;

[0134] S32, J w and Replace J and The kinematic equations of joint velocity and end coordinate system velocity are redefined as:

[0135]

[0136] Then the weighted least squares norm solution of formula (2) is:

[0137]

[0138] Among them, W -1 is the inverse weight matrix, is the joint velocity;

[0139] S33, inverse the weight matrix W -1 Defined as:

[0140]

[0141] The weight factors are:

[0142]

[0143] Among them, i=1,2,3,4,5,6,7, the feasible interval of the joint is divided into three parts: negative damping interval Positive damping range and normal range set up

[0144] The function f(·) in formula (10) satisfies the relationship: g(x) = -(2x 3 +3x 2 ) 2 , 0≤x≤1, satisfying g(1)=1, g(0)=0, when the joint θ i When approaching the limit, Will approach 0, thus stopping the i-th joint. When the joint θ i Within the normal range, the i-th joint exits the constraint of joint velocity, so that it can quickly converge to the error range.

[0145] Specifically, the Jacobian matrix is ​​a matrix used in robotic arm kinematics to describe the relationship between joint velocities and the end-of-arm coordinate system velocities. To prevent exceeding joint limits, weights are used to suppress joint velocities to achieve joint limit avoidance.

[0146] In this example, for the current joint angle sequence θ1, θ2, θ3, θ4, θ5, θ6, θ7, the feasible interval of each joint angle is determined, the corresponding weight factor is calculated according to formula (10), and then the weight inverse matrix is ​​calculated according to formula (9). By introducing the weight matrix, the present invention can flexibly deal with joint limit constraints, ensuring that the movement of the joints of the manipulator is always kept within a safe range when performing complex tasks, avoiding mechanical damage caused by exceeding the limit; ensuring that under the joint angle constraints of the manipulator in actual applications, the inverse kinematics solution that meets the requirements can be stably calculated, thereby improving the accuracy and stability of the solution.

[0147] Step S4 specifically includes the following steps:

[0148] S41. Introducing the damping factor λ to avoid the singularity of the Jacobian matrix, equation (8) is transformed into:

[0149]

[0150] in, is the joint velocity, I m represents the m×m identity matrix;

[0151] S42, combined with formula (11) and formula (3) The optimized Jacobian pseudo-inverse matrix is:

[0152] J+′ =W -1 J T (JW -1 J T +λI m ) -1 , Formula (12),

[0153] S43. In a small time step △t, it is approximately considered that:

[0154]

[0155] Where △q is the change in joint angle within △t, and △l is the change in posture within △t. Combining formulas (11), (13) and (14), we can obtain:

[0156] Δq=W -1 J T (JW -1 J T +λI m ) -1 Δl, Formula (15),

[0157] Specifically, in the inverse kinematics solution, the Jacobian matrix needs to be inverted. However, when the manipulator is in a singular configuration, the determinant of the Jacobian matrix approaches or is zero, making it irreversible. In this embodiment, to overcome the numerical issues of the singular configuration, a damping factor λ is added, resulting in Formula (11).

[0158] In this example, I in formula (15) m Take a 6×6 identity matrix to ensure JW -1 J T +λI m The damping factor λ is obtained by adding a small perturbation term λI to the Jacobian matrix. m , increase the diagonal elements of the matrix so that the matrix JW -1 J T The inverse calculation is more stable, and even when the determinant approaches zero, it does not lead to numerical divergence. In non-singular configurations, the damping factor is very small and therefore has little impact on the calculation results. However, in near-singular configurations, the damping factor plays a key role in ensuring the stability of the solution.

[0159] Step S5 specifically includes the following steps:

[0160] S51. Calculate the desired end pose and current end pose of the end effector of the robotic arm respectively:

[0161]

[0162] Among them, T dis the desired end pose, T(q k ) is the current end pose, R d represents the desired end posture, p d represents the desired end position, q k represents the joint angle sequence of the current k-th iteration, R k represents the end pose of the current k-th iteration, p k Indicates the current end position;

[0163] S52. Calculate the pose error between the desired end pose and the current end pose:

[0164] △l=(T d T -1 (q k )-I) ∨ , formula (18),

[0165] Where I is the 4×4 identity matrix, ∨ is the anti-wedge operator.

[0166] Specifically, in this embodiment, the desired end position p of the end coordinate system is d and the current end position p k The deviation between them is expressed as: Δp = p d -p k =(x,y,z) T .

[0167] In formula (18), the posture of the end coordinate system in Cartesian space is represented by a 3×3 orthogonal rotation matrix. d With T -1 (q k ) to calculate the relative transformation from the current end pose to the desired end pose, and subtract a 4×4 identity matrix to extract the rotation part. Use the inverse wedge operator ∨ Extract the rotation information from the rotation matrix and convert it into vector form.

[0168] The dewedge operator is a mathematical operator used to convert an antisymmetric matrix into a vector form. It is used to extract joint velocity or rotation axis information from the transformation matrix, for example:

[0169] For an antisymmetric matrix By using the inverse wedge operator, the matrix is ​​restored to the vector A = (a1, a2, a3) T ,

[0170] for

[0171] Definition B ∨ =(T d T -1(q k )-I) ∨ =(x,y,z,a1,a2,a3) T .

[0172] In step S6, the joint angle iteration formula is:

[0173] q k+1 =q k +Δq k , Formula (19),

[0174] Δq k =W -1 J T (JW -1 J T +λI m ) -1 (T d T -1 (q k )-I) ∨ , formula (20),

[0175] Among them, q k represents the joint angle sequence of the k+1th iteration, Δq k Represents the joint angle change sequence, Δq k =[Δθ1,Δθ2,Δθ3,Δθ4,Δθ5,Δθ6,Δθ7].

[0176] Specifically, in formula (18), the posture error △l between the desired end posture and the current end posture also represents the posture change within △t. Substituting formula (18) into formula (15) yields formula (20), and the joint angle iteration formula is obtained.

[0177] Step S7 specifically includes the following steps:

[0178] S71. Calculate the infinite norm of the joint angle variation sequence ||Δq k || ∞ :

[0179]

[0180] in, Represents the maximum absolute value of each component of the joint angle change sequence;

[0181] S72. Determine the infinite norm of the joint angle variation sequence || Δq k || ∞ Is it less than the preset threshold?

[0182] If so, output the current joint angle sequence q k ;

[0183] If not, return to step S6 to calculate and update the joint angle sequence until the convergence condition is met.

[0184] Specifically, the infinite norm of the vector of the obtained joint angle change is used as an evaluation index. When the infinite norm is less than the preset threshold, it can reflect that the gap between the current posture and the expected posture is small enough, and it can be considered that the current joint angle meets the expected accuracy corresponding to the preset threshold, so the current joint angle sequence is output as the solution. Otherwise, return to step S6 to calculate the next set of iterative q k+1 , until the convergence condition is met.

[0185] The present invention achieves rapid convergence through a numerical iterative method and is applicable to a variety of robotic arm configurations and operation scenarios. Compared with traditional analytical and numerical solution methods, the present invention has higher computational efficiency, better numerical stability and wider application applicability, and can optimize the posture and motion trajectory of the robotic arm under redundant degrees of freedom conditions.

[0186] Example 2

[0187] The inverse kinematics solution system for a seven-degree-of-freedom manipulator based on numerical iteration includes:

[0188] A modeling module is used to establish a DH kinematic model of the robotic arm based on the joint configuration of the seven-degree-of-freedom robotic arm;

[0189] The first calculation module is used to calculate the Jacobian matrix J according to the joint angle sequence;

[0190] The second calculation module is used to introduce a weight matrix to suppress the joint velocity at the edge of the joint range and calculate the least squares norm solution of the seven-degree-of-freedom manipulator;

[0191] The third calculation module is used to introduce the damping factor to calculate the optimized Jacobian pseudo-inverse matrix;

[0192] a fourth calculation module, configured to calculate a posture error between a desired end-effector posture and a current end-effector posture of the robot arm based on a DH kinematic model of the robot arm;

[0193] The iterative calculation module is used to convert the posture error into the joint angle adjustment value using the optimized Jacobian pseudo-inverse matrix, obtain the joint angle iteration formula, and determine the joint angle of each rotating joint of the robotic arm using the numerical iteration method based on the posture error.

[0194] Example 3

[0195] A computing device includes a processor and a memory for storing a program executable by the processor. When the processor executes the program stored in the memory, the method for solving inverse kinematics of a seven-degree-of-freedom manipulator based on numerical iteration in Example 1 is implemented as follows:

[0196] S1. Establish the DH kinematic model of the manipulator based on the joint configuration of the 7-DOF manipulator;

[0197] S2, calculate the Jacobian matrix J according to the joint angle sequence;

[0198] S3. Introduce a weight matrix to suppress the joint velocity at the edge of the joint range and calculate the least squares norm solution of the seven-degree-of-freedom manipulator;

[0199] S4, introducing the damping factor to calculate the optimized Jacobian pseudo-inverse matrix;

[0200] S5. Calculate the posture error between the desired end-effector posture and the current end-effector posture of the robot arm based on the DH kinematic model of the robot arm;

[0201] S6. Use the optimized Jacobian pseudo-inverse matrix to convert the posture error into the joint angle adjustment value and obtain the joint angle iteration formula;

[0202] S7. Based on the posture error, determine the joint angles of each rotational joint of the robotic arm using a numerical iteration method.

[0203] In the above process, the specific processing process is as described in Example 1 and will not be repeated here.

[0204] In this embodiment, the computing device may be a terminal device such as a desktop computer, a laptop computer, a PDA handheld terminal, or a tablet computer.

[0205] The above embodiments are preferred implementations of the present invention and are not intended to limit the present invention. Any other changes or other equivalent replacements that do not deviate from the technical solutions of the present invention are included in the protection scope of the present invention.

Claims

1. A numerical iteration-based inverse kinematics solution method for a seven-degree-of-freedom manipulator, characterized in that: Including steps: S1. Establish the DH kinematic model of the manipulator based on the joint configuration of the 7-DOF manipulator; S2, calculate the Jacobian matrix J according to the joint angle sequence; S3. Introduce a weight matrix to suppress the joint velocity at the edge of the joint range and calculate the least squares norm solution of the seven-degree-of-freedom manipulator; S4, introducing the damping factor to calculate the optimized Jacobian pseudo-inverse matrix; S5. Calculate the posture error between the desired end-effector posture and the current end-effector posture of the robot arm based on the DH kinematic model of the robot arm; Step S5 specifically includes the following steps: S51. Calculate the desired end pose and current end pose of the end effector of the robotic arm respectively: Among them, T d is the desired end pose, T(q k ) is the current end pose, R d represents the desired end posture, p d represents the desired end position, q k represents the joint angle sequence of the current k-th iteration, R k represents the end pose of the current k-th iteration, p k Indicates the current end position; S52. Calculate the pose error between the desired end pose and the current end pose: △l=(T d T -1 (q k )-I) ∨ , Where I is the 4×4 identity matrix, ∨ is the anti-wedge operator; S6. Use the optimized Jacobian pseudo-inverse matrix to convert the posture error into a joint angle adjustment value to obtain a joint angle iteration formula; in step S6, the joint angle iteration formula is: q k+1 =q k +Δq k , Among them, q k represents the joint angle sequence of the k+1th iteration, Δq k Represents the joint angle change sequence, Δq k =[Δθ1,Δθ2,Δθ3,Δθ4,Δθ5,Δθ6,Δθ7]; S7. Based on the posture error, determine the joint angles of each rotational joint of the robotic arm using a numerical iteration method; Step S7 specifically includes the following steps: S71. Calculate the infinite norm of the joint angle variation sequence ||Δq k || ∞ : in, Represents the maximum absolute value of each component of the joint angle change sequence; S72. Determine the infinite norm of the joint angle variation sequence || Δq k || ∞ Is it less than the preset threshold? If so, output the current joint angle sequence q k ; If not, return to step S6 to calculate and update the joint angle sequence until the convergence condition is met.

2. The method for solving inverse kinematics of a seven-degree-of-freedom manipulator based on numerical iteration according to claim 1, characterized in that: In step S1, the DH kinematic model is: in, Represents the coordinate transformation matrix between adjacent rotational joints of a seven-degree-of-freedom manipulator, where i represents the joint number and i = [1, 2, 3, 4, 5, 6, 7]. is the forward kinematic solution of the manipulator, θ i represents the joint angle, Represents the posture information of the end of the robot arm, p=(p x ,p y ,p z ) T Indicates the position information of the end of the robot arm.

3. The method for solving inverse kinematics of a seven-degree-of-freedom manipulator based on numerical iteration according to claim 2, characterized in that: Step S2 specifically includes the following steps: S21. Calculate the Jacobian matrix based on the current joint angle sequence: Where J is the Jacobian matrix, and the Cartesian space pose expression of the manipulator end coordinate system is: l=f(q), joint angle sequence q=(θ1,θ2,θ3,θ4,θ5,θ6,θ7), S22. Establish and solve the kinematic equations of the joint velocity and the end coordinate system velocity of the seven-degree-of-freedom manipulator: in, is the joint velocity, is the terminal coordinate system velocity and R m represents an m-dimensional vector, R represents the set of real numbers, and m represents the dimension of the vector space; Then the minimum norm solution of the seven-degree-of-freedom manipulator is: Among them, the Jacobian pseudo-inverse matrix J + =J T (JJ T ) -1 , J T Represents the transpose of the Jacobian matrix.

4. The method for solving inverse kinematics of a seven-degree-of-freedom manipulator based on numerical iteration according to claim 3, characterized in that: Step S3 specifically includes the following steps: S31, define the weighted Jacobian matrix J w and weighted joint velocities J w =JW -1 / 2 , Formula (5), Where W is the weight matrix and W∈R h×h , R h×h represents an h×h matrix, and R represents a set of real numbers; S32, J w and Replace J and The kinematic equations of joint velocity and end coordinate system velocity are redefined as: Then the weighted least squares norm solution of formula (2) is: Among them, W -1 is the inverse weight matrix, is the joint velocity; S33, inverse the weight matrix W -1 Defined as: The weight factors are: Among them, i=1,2,3,4,5,6,7, the feasible interval of the joint is divided into three parts: negative damping interval Positive damping range and normal range set up The function g(·) in formula (10) satisfies the relationship: g(x)=-(2x 3 +3x 2 ) 2 , 0≤x≤1, satisfying g(1)=1, g(0)=0, when the joint θ i When approaching the limit, Will approach 0, thus stopping the i-th joint. i Within the normal range, the i-th joint exits the constraint of joint velocity, so that it can quickly converge to the error range.

5. The method for solving inverse kinematics of a seven-degree-of-freedom manipulator based on numerical iteration according to claim 4, characterized in that: Step S4 specifically includes the following steps: S41. Introducing the damping factor λ to avoid the singularity of the Jacobian matrix, equation (8) is transformed into: in, is the joint velocity, I m represents the m×m identity matrix; S42, combined with formula (11) and formula (3) The optimized Jacobian pseudo-inverse matrix is: J +′ = W -1 J T (JW -1 J T + λI m ) -1 , Equation (12), S43. In a small time step △t, it is approximately considered that: Where △q is the change in joint angle within △t, and △l is the change in posture within △t. Combining formulas (11), (13) and (14), we can obtain: Δq = W -1 J T (JW -1 J T + λI m ) -1 Δl, Equation (15) Δq k =W -1 J T (JW -1 J T +λI m ) -1 (T d T -1 (q k )-I) ∨ 。 6. A seven-degree-of-freedom manipulator inverse kinematics solution system based on numerical iteration, applied to the method of claim 1, characterized in that: include: A modeling module is used to establish a DH kinematic model of the robotic arm based on the joint configuration of the seven-degree-of-freedom robotic arm; The first calculation module is used to calculate the Jacobian matrix J according to the joint angle sequence; The second calculation module is used to introduce a weight matrix to suppress the joint velocity at the edge of the joint range and calculate the least squares norm solution of the seven-degree-of-freedom manipulator; The third calculation module is used to introduce the damping factor to calculate the optimized Jacobian pseudo-inverse matrix; a fourth calculation module, configured to calculate a posture error between a desired end-effector posture and a current end-effector posture of the robot arm based on a DH kinematic model of the robot arm; The iterative calculation module is used to convert the posture error into the joint angle adjustment value using the optimized Jacobian pseudo-inverse matrix, obtain the joint angle iteration formula, and determine the joint angle of each rotating joint of the robotic arm using the numerical iteration method based on the posture error.

7. A computing device, characterized in that It comprises a processor and a memory for storing a program executable by the processor. When the processor executes the program stored in the memory, the inverse kinematics solution method of a seven-degree-of-freedom manipulator based on numerical iteration as described in any one of claims 1 to 5 is implemented.

Images