Seven-degree-of-freedom mechanical arm inverse kinematics solving method, system and equipment based on numerical iteration

Abstract

The invention discloses a seven-degree-of-freedom mechanical arm inverse kinematics solving method, system and device based on numerical iteration. The method comprises the steps that S1, a D-H kinematics model of a mechanical arm is established based on a joint configuration of the seven-degree-of-freedom mechanical arm; s2, calculating a Jacobian matrix according to the joint angle sequence; s3, introducing a weight matrix to suppress the joint speed at the edge of the joint range; s4, introducing a damping factor, and calculating to obtain an optimized Jacobian pseudo-inverse matrix; s5, the pose error between the expected tail end pose and the current tail end pose of the tail end actuator of the mechanical arm is calculated; s6, converting the pose error into a joint angle adjustment amount by using the optimized Jacobian pseudo-inverse matrix to obtain a joint angle iterative formula; and S7, based on the pose error, the joint angle of each rotating joint of the mechanical arm is determined through a numerical iteration method. According to the method, the problem of unstable solution of a traditional analytical method under the singular configuration can be avoided, and the calculation efficiency and the numerical stability are improved.

Description

Technical Field

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

[0002] When the robot arm completes a given Cartesian space task, the kinematic algorithm must fully consider various constraints, among which joint limit constraints are always present and must be considered. If the robot arm joint exceeds its operating range, it may not only cause task execution errors, but may even cause damage or overload of mechanical components.

[0003] Traditional analytical solutions usually become unstable or even unable to solve when the manipulator approaches a singular configuration. In addition, for different types of manipulators, the analytical solution lacks stability and flexibility in solving the specific structural process of the manipulator. Therefore, when dealing with complex task requirements and the diversity of manipulators, the traditional analytical solution has obvious limitations and cannot meet the application requirements in current complex scenarios. Traditional numerical solution methods have slow convergence speed 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 to provide a numerical solution method for a seven-degree-of-freedom robotic arm based on numerical iteration, which can avoid the instability problem of solving the traditional analytical method under singular configurations and improve the calculation efficiency and numerical stability.

[0005] The second object of the present invention is to provide a numerical solution system for a seven-degree-of-freedom robotic arm 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 numerical solution method for a seven-degree-of-freedom manipulator based on numerical iteration, comprising the steps of:

[0008] S1. Establish the DH kinematic model of the robot arm based on the joint configuration of the seven-degree-of-freedom robot arm;

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

[0010] S3, introducing a weight matrix to suppress the joint velocity at the edge of the joint range, and calculating 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. Based on the DH kinematic model of the robot arm, calculate the posture error between the desired end posture of the end effector of the robot arm and the current end posture;

[0013] S6. Use the optimized Jacobian pseudo-inverse matrix to convert the posture error into the joint angle adjustment amount to 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, i represents the serial number of the joint, and i = [1, 2, 3, 4, 5, 6, 7], is the forward kinematic solution of the robot arm, θ 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] Among them, J is the Jacobian matrix, and the Cartesian space pose expression of the robot end coordinate system is: l=f(q), joint angle sequence q=(θ1,θ2,θ3,θ4,θ5,θ6,θ7),

[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 a 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 Redefine the kinematic equations of joint velocity and end coordinate system velocity 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, satisfies 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.

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

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

[0046]

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

[0048] S42, combining 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]

[0053] Among them, △q is the change of joint angle within △t, and △l is the change of posture within △t. Combining formulas (11), (13) and (14), we can get:

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

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

[0056] S51, respectively calculating the desired end position and current end position of the end effector of the robot arm:

[0057]

[0058]

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

[0060] S52, calculating the posture error between the desired end posture and the current end posture:

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

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

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

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

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

[0066] 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].

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

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

[0069]

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

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

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

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

[0074] The numerical solution system of the seven-degree-of-freedom manipulator based on numerical iteration includes:

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

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

[0077] The second calculation module is used to introduce a weight matrix to suppress the joint speed at the edge of the joint range and calculate the minimum two-norm solution of the seven-degree-of-freedom manipulator;

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

[0079] A fourth calculation module is used to calculate a posture error between a desired end posture and a current end posture of an end effector of the robotic arm based on a DH kinematic model of the robotic arm;

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

[0081] A computing device 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 above-mentioned method for numerically solving a seven-degree-of-freedom manipulator based on numerical iteration is implemented.

[0082] Compared with the prior art, the present invention has the following advantages and effects:

[0083] (1) The numerical solution method for the seven-degree-of-freedom manipulator based on numerical iteration proposed in the present 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.

[0084] (2) By introducing a weight matrix, the present invention can flexibly respond to 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; ensuring that in actual applications, under the joint angle constraints of the robot arm, an inverse kinematic solution that meets the requirements can be stably calculated, thereby improving the accuracy and stability of the solution.

[0085] (3) The present invention achieves rapid convergence through a numerical iteration method and is applicable to a variety of robot 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 robot arm under redundant degrees of freedom conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0086] Figure 1 It is a schematic flow chart of the numerical solution method of the seven-degree-of-freedom manipulator based on numerical iteration of the present invention.

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

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

[0089] Example 1

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

[0091] S1. Establish the DH kinematic model of the robot arm based on the joint configuration of the seven-degree-of-freedom robot arm;

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

[0093] S3, introducing a weight matrix to suppress the joint velocity at the edge of the joint range, and calculating the least squares norm solution of the seven-degree-of-freedom manipulator;

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

[0095] S5. Based on the DH kinematic model of the robot arm, calculate the posture error between the desired end posture of the end effector of the robot arm and the current end posture;

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

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

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

[0099]

[0100] in, represents the coordinate transformation matrix between adjacent rotational joints of a seven-degree-of-freedom manipulator, i represents the serial number of the joint, and i = [1, 2, 3, 4, 5, 6, 7], is the forward kinematic solution of the manipulator, describing 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 terminal 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 terminal coordinate system and the x0, y0, and z0 axes of the base coordinate system.

[0101] Specifically, in this embodiment, firstly according to Figure 2 The kinematic model is established based on the structure, joint types and coordinate systems of each joint of the seven-degree-of-freedom robot. According to the improved DH parameter method, the coordinate system of each joint is defined and the DH parameters are calculated, as shown in Table 1:

[0102] The method for determining each joint coordinate system adopts the improved DH parameter method. Each joint giant 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+1th joint, x i Along z i Axis and z i-1 The common perpendicular line of the axis points away from z i-1 The direction of the axis, y i The axis is determined by the right-hand rectangular coordinate system. i is the origin of the i-th coordinate system; 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.

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

[0104]

[0105]

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

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

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

[0109]

[0110] Among them, a i-1 Indicates the connecting rod length, d i represents the connecting rod distance, α i-1 represents the torsion angle of the connecting rod. Substituting the DH parameters into the coordinate transformation matrix of the above formula, we get They are as follows:

[0111]

[0112]

[0113]

[0114]

[0115]

[0116]

[0117]

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

[0119] θ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 position matrix of the end coordinate system relative to the base coordinate system through forward kinematics.

[0120]

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

[0122]

[0123] Let the desired end pose matrix

[0124] Find the current joint angle:

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

[0126] θ7=29.9999°.

[0127] The numerical solution method for a seven-degree-of-freedom robotic arm based on numerical iteration proposed in the present invention avoids the problem of unstable solution of the robotic arm when it is close to singularity in the traditional analytical method by introducing the Jacobian pseudo-inverse matrix and the damping factor, achieves rapid convergence through the numerical iteration method, and improves the computational efficiency and numerical stability.

[0128] Step S2 specifically includes the following steps:

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

[0130]

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

[0132]

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

[0134]

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

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

[0137]

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

[0139] 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 the inverse of the Jacobian.

[0140] Step S3 specifically includes the following steps:

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

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

[0143]

[0144] 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;

[0145] S32, J w and Replace J and Redefine the kinematic equations of joint velocity and end coordinate system velocity as:

[0146]

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

[0148]

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

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

[0151]

[0152] The weight factors are:

[0153]

[0154] 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

[0155] The function f(·) in formula (10) satisfies the relationship: g(x) = -(2x 3 +3x 2 ) 2 , 0≤x≤1, satisfies 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.

[0156] Specifically, the Jacobian matrix is ​​a matrix used in the kinematics of the robot arm to describe the relationship between the joint velocity and the end coordinate system velocity. In order to avoid exceeding the joint limit, the joint velocity is suppressed by weights to achieve joint limit obstacle avoidance.

[0157] 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, ensure that the movement of the joints of the robot arm is always kept within a safe range when performing complex tasks, and avoid mechanical damage caused by exceeding the limit; ensure that under the joint angle constraints of the robot arm in actual applications, the inverse kinematics solution that meets the requirements can be stably calculated, thereby improving the accuracy and stability of the solution.

[0158] Step S4 specifically includes the following steps:

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

[0160]

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

[0162] S42, combining formula (11) and formula (3) The optimized Jacobian pseudo-inverse matrix is:

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

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

[0165]

[0166]

[0167] Among them, △q is the change of joint angle within △t, and △l is the change of posture within △t. Combining formulas (11), (13) and (14), we can get:

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

[0169] Specifically, in the process of solving inverse kinematics, the Jacobian matrix needs to be inverted, but when the manipulator is in a singular configuration, the determinant of the Jacobian matrix is ​​close to zero or is zero, resulting in its irreversibility. In this embodiment, in order to overcome the numerical problem of the singular configuration, a damping factor λ is added to obtain formula (11).

[0170] 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 , add the diagonal elements of the matrix so that the matrix JW -1 J T The inverse calculation of is more stable, and will not lead to numerical divergence even if the determinant approaches zero. In non-singular configurations, the value of the damping factor is very small, so the impact on the calculation results is negligible; in the case of near-singularity, the damping factor plays a key role in ensuring the stability of the solution.

[0171] Step S5 specifically includes the following steps:

[0172] S51, respectively calculate the desired end position and current end position of the end effector of the robot arm:

[0173]

[0174]

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

[0176] S52, calculating the posture error between the desired end posture and the current end posture:

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

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

[0179] 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 .

[0180] In formula (18), the posture of the terminal 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 anti-wedge operator ∨ Extract the rotation information from the rotation matrix and convert it into vector form.

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

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

[0183] for

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

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

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

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

[0188] 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].

[0189] Specifically, in formula (18), the posture error △l between the expected end posture and the current end posture also represents the posture change within △t. Substituting formula (18) into formula (15) obtains formula (20), and obtains the joint angle iteration formula.

[0190] Step S7 specifically includes the following steps:

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

[0192]

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

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

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

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

[0197] 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.

[0198] The present invention achieves rapid convergence through a numerical iteration method and is suitable for 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.

[0199] Example 2

[0200] The numerical solution system of the seven-degree-of-freedom manipulator based on numerical iteration includes:

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

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

[0203] The second calculation module is used to introduce a weight matrix to suppress the joint speed at the edge of the joint range and calculate the minimum two-norm solution of the seven-degree-of-freedom manipulator;

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

[0205] A fourth calculation module is used to calculate a posture error between a desired end posture and a current end posture of an end effector of the robotic arm based on a DH kinematic model of the robotic arm;

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

[0207] Example 3

[0208] 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 numerically solving a seven-degree-of-freedom manipulator based on numerical iteration in Embodiment 1 is implemented as follows:

[0209] S1. Establish the DH kinematic model of the robot arm based on the joint configuration of the seven-degree-of-freedom robot arm;

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

[0211] S3, introducing a weight matrix to suppress the joint velocity at the edge of the joint range, and calculating the least squares norm solution of the seven-degree-of-freedom manipulator;

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

[0213] S5. Based on the DH kinematic model of the robot arm, calculate the posture error between the desired end posture of the end effector of the robot arm and the current end posture;

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

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

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

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

[0218] 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 replacement methods 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 solution method for a seven-degree-of-freedom manipulator based on numerical iteration, characterized in that: Includes steps: S1. Establish the DH kinematic model of the robot arm based on the joint configuration of the seven-degree-of-freedom robot arm; S2, calculate the Jacobian matrix J according to the joint angle sequence; S3, introducing a weight matrix to suppress the joint velocity at the edge of the joint range, and calculating 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. Based on the DH kinematic model of the robot arm, calculate the posture error between the desired end posture of the end effector of the robot arm and the current end posture; S6. Use the optimized Jacobian pseudo-inverse matrix to convert the posture error into the joint angle adjustment amount to obtain the joint angle iteration formula; S7. Based on the posture error, determine the joint angles of each rotational joint of the robotic arm using a numerical iteration method.

2. The method for numerically solving 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, i represents the serial number of the joint, and i = [1, 2, 3, 4, 5, 6, 7], is the forward kinematic solution of the robot arm, θ 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 numerically solving 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: Among them, J is the Jacobian matrix, and the Cartesian space pose expression of the robot 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 a 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 numerically solving a seven-degree-of-freedom manipulator based on numerical iteration according to claim 3 is 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 Redefine the kinematic equations of joint velocity and end coordinate system velocity 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, satisfies 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 numerically solving 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. Introduce the damping factor λ to avoid the singularity of the Jacobian matrix, then equation (8) is transformed into: in, is the joint velocity, I m represents the m×m identity matrix; S42, combining 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: Among them, △q is the change of joint angle within △t, and △l is the change of posture within △t. Combining formulas (11), (13) and (14), we can get: Δq = W -1 J T (JW -1 J T + λI m ) -1 Δl, Equation (15).

6. The method for numerically solving a seven-degree-of-freedom manipulator based on numerical iteration according to claim 5, characterized in that: Step S5 specifically includes the following steps: S51, respectively calculate the desired end position and current end position of the end effector of the robot arm: Among them, T d is the desired terminal posture, T(q k ) is the current end position, R d represents the desired end posture, p d represents the desired end position, q k represents the joint angle sequence of the current kth iteration, R k represents the end posture of the current k-th iteration, p k Indicates the current end position; S52, calculating the posture error between the desired end posture and the current end posture: △l=(T d T -1 (q k )-I) ∨ , Equation (18), Where I is the 4×4 identity matrix, ∨ is the anti-wedge operator.

7. The method for numerically solving a seven-degree-of-freedom manipulator based on numerical iteration according to claim 6, characterized in that: In step S6, the joint angle iteration formula is: q k+1 = q k + Δq k , Equation (19), Δq k = W -1 J T (JW -1 J T + λI m ) -1 (T d T -1 (q k ) - I) ∨ , Equation (20), Among them, q k represents the joint angle sequence of the k+1th iteration, Δq k represents the sequence of joint angle changes, Δq k =[Δθ1,Δθ2,Δθ3,Δθ4,Δθ5,Δθ6,Δθ7].

8. The method for numerically solving a seven-degree-of-freedom manipulator based on numerical iteration according to claim 1, characterized in that: 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.

9. A numerical solution system for a seven-degree-of-freedom manipulator based on numerical iteration, characterized in that: include: A modeling module, used to establish a DH kinematic model of the robot arm based on the joint configuration of the seven-degree-of-freedom robot arm; A 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 speed at the edge of the joint range and calculate the minimum two-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 is used to calculate a posture error between a desired end posture and a current end posture of an end effector of the robotic arm based on a DH kinematic model of the robotic arm; The iterative calculation module is used to convert the posture error into the joint angle adjustment amount using the optimized Jacobian pseudo-inverse matrix, obtain the joint angle iteration formula, and based on the posture error, determine the joint angle of each rotating joint of the robot arm using a numerical iteration method.

10. 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, it implements the numerical solution method for a seven-degree-of-freedom manipulator based on numerical iteration as described in any one of claims 1 to 8.

Images