A method for solving the inverse kinematics of a robotic arm with autonomous planning of redundant parameters

Abstract

This invention relates to a method for solving the inverse kinematics of a robotic arm based on autonomous planning of redundant parameters, comprising: Step 1, the rotation axes of joints 1 to 3 of the robotic arm intersect at point Ps and are designated as the shoulder joint; the rotation center of joint 4 is Pe and is designated as the elbow joint; the rotation axes of joints 5 to 7 intersect at point Pw and are designated as the wrist joint; Step 2, the "shoulder-elbow-wrist" structural parameters are determined as: l bs l se l ew l wt Step 3: Solve for θ in the base coordinate system to find the vector of line PsPw. 1_0 θ 2_0 θ 3_0 θ 4_0 Step 4: Define the angle between the PsPePw plane and the reference plane as the redundancy angle; establish the functional relationship between θ1, θ2, θ3 and θ5, θ6, θ7 and the redundancy angle; Step 5: Solve for the initial inverse joint combination; Step 6: Solve for the optimal redundancy angle; Step 7: Substitute the optimal redundancy angle into the relational expression to finally solve for θ1, θ2, θ3 and θ5, θ6, θ7; Step 8: Use the calculated θ1~θ7 as the desired trajectory. This invention improves the control accuracy of the robotic arm and the operating efficiency of the controller.

Description

Technical Field

[0001] This invention relates to the field of robotics, specifically to a method for solving the inverse kinematics of a robotic arm with autonomous planning of redundant parameters. Background Technology

[0002] Among collaborative robots, the seven-degree-of-freedom anthropomorphic robotic arm is a common structure. Such robotic arms are widely used in flexible industrial production lines, electronic component assembly, medical and service industries. Compared with the traditional six-degree-of-freedom robotic arm, the seven-degree-of-freedom anthropomorphic robotic arm has a larger and more flexible workspace, making it particularly suitable for use in work environments that require human interaction.

[0003] Inverse kinematics planning for robotic arms is fundamental to robotic arm trajectory planning and control. Traditional inverse kinematics planning methods for industrial robotic arms cannot be directly applied to seven-DOF anthropomorphic robotic arms because mapping the six-DOF operational space trajectory to the seven-DOF trajectory presents a problem. Current solution methods mostly employ numerical methods, which solve the problem by finding the Jacobian pseudo-inverse in each iteration cycle. While numerical methods can handle inverse kinematics problems for robotic arms with redundant degrees of freedom, they suffer from large errors and low computational efficiency. Applying such methods to seven-DOF robotic arm trajectory planning not only compromises control accuracy but also reduces controller efficiency and even the real-time performance of the robotic arm's response. Summary of the Invention

[0004] The purpose of this invention is to provide a method for solving the inverse kinematics of a robotic arm with autonomous planning of redundant parameters. By defining a reference plane and redundant angles, the mapping relationship between the six degrees of freedom in space and the seven degrees of freedom of the robotic arm joints is realized, thereby improving the control accuracy of the robotic arm and the operating efficiency of the controller.

[0005] The objective of this invention is achieved through the following technical solution:

[0006] A method for solving the inverse kinematics of a robotic arm with autonomous planning of redundant parameters includes the following steps:

[0007] Step 1: Establish the robotic arm model and define the joints of the robotic arm from the base to the end effector as joint 1 to joint 7, θ i (i = 1-7) is the rotation angle of the i-th joint. The robotic arm is configured with a "shoulder-elbow-wrist" structure, where the rotation axes of joints 1 to 3 intersect at a point Ps and are configured as shoulder joints, the rotation center of joint 4 is Pe and is configured as elbow joints, and the rotation axes of joints 5 to 7 intersect at a point Pw and are configured as wrist joints.

[0008] Step 2: Determine the motion parameters of the "shoulder-elbow-wrist" structure as follows:bs l se l ew l wt , where: l bs l represents the distance from the origin of the base coordinate system to point Ps. se l represents the distance from point Ps to point Pe. ew l represents the distance from point Pe to point Pw. wt This represents the distance from Pw to the end effector of the robotic arm, based on the desired position of the end effector. and posture Solve for the vector of line segment PsPw in the base coordinate system.

[0009]

[0010] Step 3: Solve for the rotation angle θ of joints 1 to 4 in the base coordinate system. 1_0 θ 2_0 θ 3_0 θ 4_0 and θ 1_0 θ 2_0 θ 3_0 θ 4_0 A defined plane is defined as a reference plane;

[0011] Step 4: Define the angle between the arbitrary spatial plane formed by the three points Ps, Pe, and Pw at the centers of the shoulder, elbow, and wrist joints and the reference plane defined in Step 3 as the redundant angle. Establish shoulder joint angles θ1, θ2, θ3 and wrist joint angles θ5, θ6, θ7, and the redundant angles. Functional relationship Where θ = [θ1, θ2, θ3, θ5, θ6, θ7] T ;

[0012] Step 5: Assume redundant angles The value is 0, according to the above. Solve the initial inverse joint combination;

[0013] Step Six: Based on the initial inverse solution joint combination and redundant angles The solution involves determining the rate of change and the expected trajectory of the robotic arm's end effector, as well as finding the optimal redundancy angle.

[0014] Step 7: Optimal Redundancy Angle Substitution The final solution from the relational formula yields the shoulder joint angles θ1, θ2, θ3 and the wrist joint angles θ5, θ6, θ7.

[0015] Step 8: Send the calculated rotation angles θ1 to θ7 of the 7 joints as the planned desired trajectory to each joint of the robotic arm for control, where θ4 is obtained from step 3.

[0016] In step three, first determine the length l of the line connecting the center points of the shoulder and elbow. se The length l of the line connecting the center points of the elbow and wrist ew and the length of the line connecting the center points of the shoulder and wrist Solve for θ using the triangle formed 4_0 :

[0017]

[0018] Then, according to the following formula (3), the vector of the straight line segment of the shoulder-wrist center point Ps Pw in the joint 3-coordinate system is solved.

[0019]

[0020] Combined with the vector from step two Simultaneously, the joint angle θ is defined as fixed. 3_0 =0, solve for the angle θ of joint 1 in the base coordinate system at this time. 1_0 and joint 2 angle θ 2_0 ,in:

[0021] θ 2_0 The solution is as follows:

[0022]

[0023]

[0024] In equation (4) above, Indicates when θ 3_0 When θ = 0, the angle θ of joint 1 1_0 The rotation matrix of joint 1 relative to the base is determined. Indicates when θ 3_0 When θ = 0, the angle θ of joint 2 2_0 The rotation matrix of joint 2 relative to joint 1 is determined. This represents the rotation matrix of the joint 3 coordinate system relative to the joint 2 coordinate system. Additionally, in equation (4) above:

[0025] a = l se +l ew *cos(θ 4_0 )

[0026] b = -l ew *sin(θ 4_0 )

[0027]

[0028] In the above formula (5) In the base coordinate system The z-component of a vector;

[0029] θ 1_0 The solution is as follows:

[0030]

[0031] and They represent the coordinates in the base coordinate system. The x-axis and y-axis components of the vector, and in equation (6) above:

[0032] m = sin(θ) 2_0 )*(l se +l ew *cos(θ 4_0 ))

[0033] n = l ew *sin(θ 4_0 )*cos(θ 2_0 (7);

[0034] θ 1_0 θ 2_0 θ 4_0 After solving, the value at this point is determined by θ. 1_0 θ 2_0 θ 3_0 θ 4_0 The defined plane containing the shoulder-elbow-wrist axis is defined as the reference plane, where θ 3_0 =0.

[0035] In step three, θ 4_0 After solving, there are two solutions, positive and negative. When the trajectory points of the robotic arm's end effector are discrete, θ 4_0 There are actually two sets of solutions, with the sign of the solution chosen manually. When the trajectory points of the robotic arm's end effector are continuous, θ 4_0 The sign is determined based on the joint angle θ4 of the previous cycle.

[0036] In step three, θ 2_0 After solving, there are two solutions, positive and negative. When the trajectory points of the robotic arm's end effector are discrete, θ 2_0 There are actually two sets of solutions. The positive and negative signs are selected manually. When the trajectory points of the robotic arm end-effector are continuous, there is actually one set of solutions, which is selected according to the principle of minimum displacement. That is, the set of solutions in which the difference in the rotation angle between the two planning cycles is within the threshold range is selected.

[0037] In step four, The expression is as follows:

[0038] θ1, θ2, and θ3 are expressed as:

[0039]

[0040]

[0041]

[0042] In equation (8) above, parameter a sij b sij c sij Matrix A s B s C s The value in the i-th row and j-th column, where i, j = 1…3, A s B s C s Determined by the following formula (9):

[0043]

[0044]

[0045]

[0046]

[0047]

[0048] θ5, θ6, and θ7 are expressed as:

[0049]

[0050]

[0051]

[0052] In equation (10) above, parameter a wij b wij c wij Represent matrix A respectively w B w and C w The value in the i-th row and j-th column, where i and j = 1…3, A w B w and C w Determined by the following formula (11):

[0053]

[0054]

[0055]

[0056]

[0057] In step four, the solution for θ2 has two cases: positive and negative. When the end-effector's operational space trajectory is discrete, the sign is manually selected. When the end-effector's operational space trajectory is continuous, the sign of θ2 is determined based on the angle value of the previous planning cycle.

[0058] In step four, the solution for θ6 has two cases: positive and negative. When the end-effector's operational space trajectory is discrete, the sign is manually selected. When the end-effector's operational space trajectory is continuous, the sign of θ6 is determined based on the angle value of the previous planning cycle.

[0059] In step five, according to A total of four different joint solutions were obtained. When the trajectory points of the robotic arm's end-effector are discrete, there are actually four solutions, which are selected manually. When the trajectory points of the robotic arm's end-effector are continuous, there is actually one solution. In this case, the solution is selected according to the principle of minimum displacement, that is, the solution with the difference in rotation angle within the two planning cycles within the threshold range is selected as the initial inverse solution joint combination.

[0060] In step six, the planned trajectory of the robotic arm's end effector passes through A(x) in the base coordinate system. a y a , z a B(x) b y b , z b ), C(x) c y c , z c Three points form a trajectory plane; the optimal redundancy angle. The solution is as follows:

[0061] Step A: Solve for the normal vector of the trajectory plane:

[0062]

[0063]

[0064]

[0065] In the above formula (12), This represents the vector from point A to point B. This represents the vector from point A to point C. The normal vector representing the trajectory plane;

[0066] Step B: Solve for the angle between the shoulder-elbow-wrist center plane PsPePw formed by the initial inverse joint combination determined in Step 5 and the trajectory plane:

[0067]

[0068]

[0069] In the above formula (13), Let α represent the normal vector of the PsPePw plane, and α be the angle between the trajectory plane and the PsPePw plane.

[0070] Step C: When α = 0 calculated in step B, the redundancy angle at this point is 0, which is the optimal redundancy angle. When α is not equal to 0, the rate of change of the redundant angle is... change And continuously repeat the calculation of the following formula (14) to make the angle α between the PsPePw plane and the trajectory plane. i+1 Continuously approaching 0 degrees:

[0071]

[0072]

[0073]

[0074] Until α i+1 When the extreme value is reached, the optimal redundancy angle is obtained.

[0075] The advantages and positive effects of this invention are as follows:

[0076] This invention configures the joints of a seven-DOF robotic arm into a shoulder-elbow-wrist structure. First, the angles of joints 1-4 are solved. Then, the shoulder-elbow-wrist plane determined by the angles of joints 1-4 is defined as a reference plane. Furthermore, the angle between any spatial plane formed by the centers of the shoulder-elbow-wrist joints and the reference plane is defined as a redundant angle. Then establish redundant angles Functional relationship with shoulder joint angles θ1, θ2, θ3 and wrist joint angles θ5, θ6, θ7 And assume Get 0 The initial inverse joint combination is determined, and then the system uses the initial inverse joint combination and the set redundancy angles. The robot arm autonomously solves for the optimal redundancy angle based on the rate of change and the expected trajectory of the end effector. Substitute the optimal redundant angle This allows the rotation angles of each joint to be obtained as the planned desired trajectory for control, realizing the mapping relationship between the six degrees of freedom in space and the seven degrees of freedom in the joints, and improving the control accuracy of the robotic arm and the operating efficiency of the controller. Attached Figure Description

[0077] Figure 1 This is a schematic diagram of the process of the present invention.

[0078] Figure 2 This is a schematic diagram of the joint of the SRS-type seven-degree-of-freedom robotic arm that is the subject of this invention.

[0079] Figure 3 for Figure 2 A schematic diagram of the coordinate system model of each joint of the robotic arm.

[0080] Figure 4 For the present invention Figure 2 The diagram shows the SRS-type robotic arm with a "shoulder-elbow-wrist" joint configuration.

[0081] Figure 5 This is a schematic diagram of the robotic arm's operational space trajectory according to the present invention.

[0082] Figure 6 This is a schematic diagram of the joint trajectories obtained during the simulation calculation of this invention. Detailed Implementation

[0083] The invention will now be described in further detail with reference to the accompanying drawings.

[0084] like Figures 1-6 As shown, this invention utilizes the method of defining a reference plane and redundant angles to realize the mapping relationship between the six degrees of freedom in space and the seven degrees of freedom of the robotic arm joints. Thus, based on the obtained rotation angles of the seven joints, the desired trajectory of each joint of the robotic arm is obtained, realizing trajectory planning and subsequent control of each joint of the robotic arm.

[0085] The method of the present invention includes the following steps:

[0086] Step 1: Configure the seven-DOF robotic arm of the SRS configuration to have a "shoulder-elbow-wrist" structure.

[0087] like Figure 2 As shown, based on the relative positional relationship between each link and its adjacent link in the seven-DOF robotic arm of SRS configuration, joints 1 to 7 are defined sequentially from the robotic arm base to the end effector. θ i (i = 1-7) represents the rotation angle of the i-th joint. The robotic arm is modeled using the general Denabit-Hartenberg (DH) parametric modeling method, which requires only four parameters, and as follows... Figure 3As shown, a coordinate system is defined on each link to measure the parameters of each link. The coordinate system at point 0 is the base coordinate system of the robot arm, the origins of coordinate systems 1 and 2 coincide, the origins of coordinate systems 3 and 4 coincide, the origins of coordinate systems 5 and 6 coincide, and 7 is the tool coordinate system at the end of the robot arm.

[0088] like Figure 2 and Figure 4 As shown, since the rotation axes of joints 1 to 3 intersect at point Ps, and the rotation axes of joints 5 to 7 intersect at point Pw, the seven-DOF robotic arm of the SRS configuration can be configured with a "shoulder-elbow-wrist" structure. Joints 1 to 3 are configured as shoulder joints, which include three rotational degrees of freedom and whose rotation centers coincide at point Ps. Joint 4 is configured as an elbow joint, which includes one rotational degree of freedom and whose rotation center is Pe. Joints 5 to 7 are configured as wrist joints, which include three rotational degrees of freedom and whose rotation centers coincide at point Pw. Points Ps, Pe, and Pw are also the origins of the coordinate systems of the shoulder, elbow, and wrist, respectively.

[0089] Step Two: As Figure 4 As shown, the motion parameters of the "shoulder-elbow-wrist" model in step one are simplified into four groups: l bs l se l ew l wt , where: l bs l represents the distance from the origin of the base coordinate system to point Ps. se l represents the distance from point Ps to point Pe. ew l represents the distance from point Pe to point Pw. wt This indicates that Pw is at the end of the robotic arm ( Figure 3 The distance from the origin of the tool coordinate system can be used to determine the desired position of the robotic arm's end effector. and posture Find the vector of the straight line segment connecting the shoulder-wrist center points Ps and Pw in the base coordinate system.

[0090]

[0091] In equation (1) above, the superscript "0" represents a vector in the base coordinate system. This represents the distance from the origin to Ps in the base coordinate system, and the superscript "7" indicates the vector in the tool coordinate system. This represents the distance from point Pw in the tool coordinate system to the end of the robotic arm tool.

[0092] Step 3: Solve for the rotation angle θ of joints 1 to 4 in the base coordinate system. 1_0 θ 2_0 θ 3_0 θ 4_0 and θ1_0 θ 2_0 θ 3_0 θ 4_0 A defined plane is defined as a reference plane.

[0093] In Cartesian space, the description of any pose contains six variables: displacement along the X-axis, displacement along the Y-axis, displacement along the Z-axis, rotation about the X-axis, rotation about the Y-axis, and rotation about the Z-axis. Therefore, to realize the inverse kinematics solution from the pose in Cartesian space to the joint space angles of the robot arm, the robot arm theoretically needs to have six active degrees of freedom. However, the number of active degrees of freedom of a seven-degree-of-freedom robot arm is greater than the number required for inverse kinematics. Therefore, performing inverse kinematics calculations on a seven-degree-of-freedom robot arm can theoretically yield an infinite number of joint solutions, making direct solution quite difficult.

[0094] Therefore, in order to simplify the inverse kinematics calculation of a seven-degree-of-freedom robotic arm, this invention assumes θ 3_0 =0, thus reducing the seven-DOF robotic arm to six-DOF, and consequently obtaining a set of reference joint solutions: θ 1_0 θ 2_0 θ 3_0 θ 4_0 The details are as follows:

[0095] like Figure 4 As shown, the length of the upper arm is based on the line connecting the center points of the shoulder and elbow. se The length of the line connecting the center points of the elbow and wrist is the length of the forearm. ew And the length of the straight line segment from the shoulder-wrist center point Ps to Pw Given the triangle formed, solve for the elbow joint (joint 4) angle θ. 4_0 :

[0096]

[0097] In equation (2) above, since θ 4_0 Solving using the cosine function formula, there are two solutions, one positive and one negative. When the trajectory points of the robotic arm's end effector are discrete, there are indeed two sets of solutions, with the sign chosen manually. When the trajectory points of the robotic arm's end effector are continuous, there is one set of solutions, selected according to the minimum displacement principle. This means that the difference in rotation angle between two consecutive planning cycles does not change drastically (within the threshold range). In this case, the solution is determined based on the joint angle of the previous cycle. When the joint angle θ4 > 0 in the previous cycle, θ... 4_0 >0, otherwise θ 4_0 <0, θ 4_0 The case of 0 does not exist; it is outside the workspace.

[0098] Then, according to the following formula (3), the vector of the straight line segment Ps Pw connecting the shoulder and wrist center points in the joint 3-coordinate system is obtained.

[0099]

[0100] In equation (3) above, This represents the rotation matrix of the joint 4 coordinate system relative to the joint 3 coordinate system. Let PsPe represent the vector of the line segment from the shoulder-elbow center point in the 3-coordinate system of the joint. Let PePw represent the line segment vector of the elbow-wrist center point in the joint 4-coordinate system.

[0101] Then combine the shoulder-wrist center point vector in the base coordinate system Meanwhile, as mentioned above, if the angle of joint 3 is fixed, θ 3_0 =0, solve for the joint angle θ in the base coordinate system at this time. 1_0 and joint 2 angle θ 2_0 ,in:

[0102] θ 2_0 The solution is as follows:

[0103]

[0104]

[0105] In equation (4) above, Indicates when θ 3_0 When θ = 0, the angle θ between joints 1 and 2 is... 1_0 The rotation matrix of joint 1 relative to the base is determined. Indicates when θ 3_0 When θ = 0, the angle θ between joints 2 2_0 The rotation matrix of joint 2 relative to joint 1 is determined. This represents the rotation matrix of the joint 3 coordinate system relative to the joint 2 coordinate system. Additionally, in equation (4) above:

[0106] a = l se +l ew *cos(θ 4_0 )

[0107] b = -l ew *sin(θ 4_0 )

[0108]

[0109] In the above formula (5) In the base coordinate system The z-component of a vector.

[0110] θ 1_0 The solution is as follows:

[0111]

[0112] In the above formula (6), and They represent the coordinates in the base coordinate system. The x-axis and y-axis components of the vector, and in equation (6) above:

[0113] m = sin(θ) 2_0 )*(l se +l ew *cos(θ 4_0 ))

[0114] n = l ew *sin(θ 4_0 )*cos(θ 2_0 (7);

[0115] According to the formulas obtained from (4) to (7) above, θ 2_0 Similarly, there are two possible outcomes, positive and negative. When the trajectory points of the robotic arm's end effector are discrete, there are indeed two sets of solutions, with the sign chosen manually. When the trajectory points of the robotic arm's end effector are continuous, there is one set of solutions, selected according to the minimum displacement principle. That is, the solution where the difference in angle between two consecutive planning cycles does not change significantly (within a threshold range) is selected. θ 2_0 The case of 0 does not exist; it is outside the workspace.

[0116] When θ 1_0 θ 2_0 θ 3_0 (equal to 0), θ 4_0 After solving, the value at this point is determined by θ. 1_0 θ 2_0 θ 3_0 θ 4_0 The plane containing the shoulder-elbow-wrist is defined as the reference plane.

[0117] Step 4: Define the angle between the arbitrary spatial plane formed by the three points PsPePw (the center of the shoulder-elbow-wrist joint) and the reference plane defined in Step 3 as the redundant angle. Establish six angles θ1, θ2, θ3, θ5, θ6, θ7 for the shoulder and wrist joints, and the aforementioned redundant angles. Functional relationship

[0118] like Figure 4As shown, for a seven-DOF robotic arm with an SRS configuration, the reference plane determined in step three can rotate around a straight line from the origin of the shoulder joint coordinate system to the origin of the wrist joint coordinate system, that is, around the straight line PsPw. Therefore, the angle between any spatial plane formed by the three points PsPePw and the reference plane can be defined as a redundancy angle.

[0119] Based on Rodrigues' rotation formula, six angles θ1, θ2, θ3, θ5, θ6, and θ7 of the shoulder and wrist joints, along with a redundant angle, can be established. Functional relationship Where θ = [θ1, θ2, θ3, θ5, θ6, θ7] T , in addition θ1, θ2, θ3, θ5, θ6, θ7 and These refer to the joint rotation angle, which are scalars without a reference coordinate system.

[0120] The specific expression is as follows:

[0121] θ1, θ2, and θ3 are expressed as follows:

[0122]

[0123]

[0124]

[0125] In equation (8) above, parameter a sij b sij c sij Matrix A s B s C s The value in the i-th row and j-th column, where i and j = 1…3. A s B s C s Determined by the following formula (9):

[0126]

[0127]

[0128]

[0129]

[0130]

[0131] In the above formula (9), This represents the rotation matrix from the joint 3 coordinate system to the base coordinate system. express A skew-symmetric matrix of vectors.

[0132] θ5, θ6, and θ7 are expressed as follows:

[0133]

[0134]

[0135]

[0136] In equation (10) above, parameter a wij b wij c wij Represent matrix A respectively w B w and C w The value in the i-th row and j-th column, where i and j = 1…3. A w B w and C w Determined by the following formula (11):

[0137]

[0138]

[0139]

[0140]

[0141] In the above formula (11), This represents the rotation matrix from the 7-joint coordinate system to the 4-joint coordinate system.

[0142] The above matrix A s B s C s and matrix A w B w and C w The introduction of is for the sake of formula simplicity.

[0143] From equations (8) and (10) above, it can be seen that since θ2 and θ6 are both obtained from the inverse cosine function, there are two sets of solutions respectively. That is, θ2 has two solutions: θ2>0 and θ2<0. The case of θ2=0 does not exist, as it exceeds the workspace. When the end of the robotic arm is a discrete trajectory, the sign is manually selected. When the end of the robotic arm is a continuous trajectory, the sign is determined according to the angle value of the previous planning cycle. When the angle of the previous cycle is greater than 0, θ2>0, and vice versa. Similarly, θ6 also has two sets of solutions: θ6>0 and θ6<0. θ6=0 is a singular point and is excluded in the trajectory planning. When the end of the robotic arm is a discrete trajectory, the sign is manually selected. When the end of the robotic arm is a continuous trajectory, the sign is determined according to the angle value of the previous cycle.

[0144] Step 5: Assume redundant angles The value is 0, according to the above. Solving for joints θ1, θ2, θ3, θ5, θ6, and θ7 yields four different combinations of joint solutions. The reason for this is that, since the rotation axes of joints 1 to 3 in the SRS-type seven-DOF machine intersect at a single point, forming a spherical wrist structure, the same pose can be obtained by rotating the angles of joints 1 to 3. Another set of solutions is [θ1, θ2, θ3] and [θ1+π, -θ2, θ3+π]. Similarly, the rotation axes of joints 5 to 7 intersect at a single point, which is also a spherical wrist structure. Therefore, by flipping the angles of joints 5 to 7, the same pose can also be obtained. Another set of solutions is [θ5, θ6, θ7] and [θ5+π, -θ6, θ7+π]. Therefore, in summary, assuming redundant angles... When it is 0, according to Solving for joint solutions θ1, θ2, θ3, θ5, θ6, and θ7 yields a total of four different combinations of joint solutions, including:

[0145] θ a =[θ1, θ2, θ3, θ5, θ6, θ7] T

[0146] θ b =[θ1+π, -θ2, θ3+π, θ5, θ6, θ7] T

[0147] θ c =[θ1, θ2, θ3, θ5+π, -θ6, θ7+π] T

[0148] θ d =[θ1+π, -θ2, θ3+π, θ5+π, -θ6, θ7+π] T .

[0149] When the trajectory points of the robotic arm's end-effector are discrete, there are actually 4 sets of solutions, which are selected manually. When the trajectory points of the robotic arm's end-effector are continuous, there is actually 1 set of solutions. In this case, the solution is selected according to the principle of minimum displacement, that is, the set in which the difference in the rotation angle between the two planning cycles does not change significantly (within the threshold range) is selected as the initial inverse solution joint combination.

[0150] Step Six: Based on the initial inverse kinematics joint combination determined in Step Five and the redundancy angles set by the system... Based on the rate of change and the expected trajectory of the robotic arm's end effector, the system autonomously solves for the appropriate optimal redundancy angle.

[0151] This invention transforms a multivariate optimization problem into a univariate optimization problem, that is, based on a set redundancy angle. Based on the rate of change and the expected trajectory planning route, the system autonomously solves for the appropriate optimal redundancy angle. This ensures that the seven-degree-of-freedom robotic arm can safely and smoothly execute the expected trajectory.

[0152] Redundancy angle in this step The solution process is as follows:

[0153] like Figure 5 As shown, the end effector coordinate system of the robotic arm maintains its orientation, and the planned trajectory is expected to pass through point A(x) relative to the base coordinate system. a y a , z a B(x) b y b , z b ), C(x) c y c , z c Three points form a circular arc-shaped trajectory plane. The appropriate redundant angle can be solved using the following procedure.

[0154] Step A: Solve for the normal vector of the plane of the circular arc trajectory:

[0155]

[0156]

[0157]

[0158] In the above formula (12), This represents the vector from point A to point B. This represents the vector from point A to point C. This represents the normal vector of the trajectory plane.

[0159] Step B: Solve for the angle between the shoulder-elbow-wrist center plane PsPePw formed by the initial inverse joint combination determined in Step 5 and the trajectory plane:

[0160]

[0161]

[0162] In the above formula (13), The normal vector representing the plane of the circular arc trajectory. Let α represent the normal vector of the plane PsPePw, and α be the angle between the two planes.

[0163] Step C: When α = 0 calculated in step B, the redundancy angle at this point is 0, which is the optimal redundancy angle. The initial inverse joint combination determined in step five is the solution. When α is not equal to 0, the system control changes at a certain rate of redundancy angle. change Then change the redundant angle Substitution The modified set of joint solutions is obtained from the relation, and a new PsPePw plane and its normal vector are reconstructed. That is, by repeatedly calculating the following formula (14), the included angle α between the two planes is made so that... i+1 Approaching 0 degrees:

[0164]

[0165]

[0166]

[0167] Until α i+1 When the extreme value is reached, the optimal redundancy angle is obtained.

[0168] This step allows for the selection of different redundancy angle change rates and predetermined trajectory planning routes based on different work requirements, thereby ensuring the flexibility of the seven degrees of freedom motion and enabling the robotic arm to safely and smoothly execute the expected trajectory.

[0169] Step 7: Calculate the optimal redundancy angle obtained in Step 6. Substitution The relational calculations are then used to finally determine the shoulder joint angles θ1, θ2, θ3 and the wrist joint angles θ5, θ6, θ7.

[0170] Step 8: Send the 7 joint rotation angles θ1 to θ7 obtained in Step 3 and Step 7 as the planned desired trajectory to each joint of the robotic arm for control, where θ4 is obtained from Step 3.

[0171] The entire calculation process of this invention is applied to robot trajectory planning and control methods. Each control cycle performs a complete calculation of the entire process, and then the calculated rotation angles θ1 to θ7 of the 7 joints are sent as the planned desired trajectory to the drive devices of each joint of the robotic arm. Each drive device drives the robotic arm to move to the desired planned trajectory.

[0172] The operation of this invention can be further illustrated by the following simulation.

[0173] Simulation content:

[0174] like Figure 4 As shown, the kinematic parameters during simulation are:

[0175] l bs =317mm, l se =460mm, l ew =510mm, l wt =70mm:

[0176] The initial position angles of joints 1-7 are [0 0.7854 0 1.5708 0 -0.7854 0];

[0177] The positions of the three selected points A, B, and C on the end effector's motion trajectory in the end-effector coordinate system are [0, 0, 0], respectively. T [0, -0.2, 0.2] T [0, 0, 0.4] T The robotic arm's posture remains unchanged, and the trajectory in the operating space is ( Figure 5 Each sampling point in (as shown) is processed through steps one through seven above to obtain the joint angle trajectory (as shown). Figure 6 As shown in the figure, the system controls the movement of the robotic arm according to the solution, which meets the requirements.

Claims

1. A method for solving the inverse kinematics of a robotic arm with autonomous planning of redundant parameters, characterized in that: Includes the following steps: Step 1: Create a robotic arm model and define the joints of the robotic arm, from the base to the end effector, as joint 1 to joint 7. (i=1-7) represents the rotation angle of the i-th joint. The robotic arm is configured with a "shoulder-elbow-wrist" structure, where the rotation axes of joints 1 to 3 intersect at a point Ps and are configured as shoulder joints, the rotation center of joint 4 is Pe and is configured as elbow joints, and the rotation axes of joints 5 to 7 intersect at a point Pw and are configured as wrist joints. Step 2: Determine the motion parameters of the "shoulder-elbow-wrist" structure as follows: , , , ,in: This represents the distance from the origin of the base coordinate system to point Ps. This represents the distance from point Ps to point Pe. This represents the distance from point Pe to point Pw. This represents the distance from Pw to the end effector of the robotic arm, based on the desired position of the end effector. and posture Solve for the vector of line segment PsPw in the base coordinate system. : （1）； Step 3: Solve for the rotation angles of joints 1 to 4 in the base coordinate system. , , , and will , , , A defined plane is defined as a reference plane; In this step, first determine the length of the line connecting the center points of the shoulder and elbow. Length of the line connecting the center points of the elbow and wrist and the length of the line connecting the center points of the shoulder and wrist Solving the triangle formed : （2）； Then, according to the following formula (3), the vector of the straight line segment of the shoulder-wrist center point Ps Pw in the joint 3-coordinate system is solved. : （3）； Combined with the vector from step two Meanwhile, the three angles of the joint are defined to be fixed. =0, solve for the angle of joint 1 in the base coordinate system at this time. and joint 2 angle ,in: The solution is as follows: （4）； In the above formula (4), Indicates when When =0, it is determined by the angle of joint 1. The rotation matrix of joint 1 relative to the base is determined. Indicates when When =0, the angle of joint 2 The rotation matrix of joint 2 relative to joint 1 is determined. This represents the rotation matrix of the joint 3 coordinate system relative to the joint 2 coordinate system. Additionally, in equation (4) above: （5）； In the above formula (5) In the base coordinate system The z-component of a vector; The solution is as follows: （6）； and They represent the coordinates in the base coordinate system. The x-axis and y-axis components of the vector, and in equation (6) above: （7）； , , After solving, the current state will be determined by... , , , The defined plane containing the shoulder-elbow-wrist axis is defined as the reference plane, where =0; Step 4: Define the angle between the arbitrary spatial plane formed by the three points Ps, Pe, and Pw at the centers of the shoulder, elbow, and wrist joints and the reference plane defined in Step 3 as the redundant angle. Establish shoulder joint angle and wrist joint angle With the redundant angle Functional relationship ,in ; Step 5: Assume redundant angles It is 0, according to the above. Solve the initial inverse joint combination; Step Six: Based on the initial inverse solution joint combination and redundant angles The solution involves determining the rate of change and the expected trajectory of the robotic arm's end effector, as well as finding the optimal redundancy angle. ; In this step, the planned trajectory of the robotic arm's end effector passes through the base coordinate system. , , Three points form a trajectory plane; optimal redundancy angle. The solution is as follows: Step A: Solve for the normal vector of the trajectory plane: （12）； In the above formula (12), This represents the vector from point A to point B. This represents the vector from point A to point C. The normal vector representing the trajectory plane; Step B: Solve for the angle between the shoulder-elbow-wrist center plane PsPePw formed by the initial inverse joint combination determined in Step 5 and the trajectory plane: （13）； In the above formula (13), This represents the normal vector of the PsPePw plane. The angle between the trajectory plane and the PsPePw plane; Step C: When the calculation in step B is... When the redundancy angle is 0, the redundancy angle at this point is the optimal redundancy angle. ,when When not equal to 0, the rate of change of redundant angle is used. change And continuously repeat the calculation of the following formula (14) to make the angle between the PsPePw plane and the trajectory plane... Continuously approaching 0 degrees: （14）； until When the extreme value is reached, the optimal redundancy angle is obtained. ; Step 7: Optimal Redundancy Angle Substitution The shoulder joint angle is finally solved from the relation. and wrist joint angle ; Step 8: Calculate the rotation angles of the 7 joints. The planned desired trajectory is sent to each joint of the robotic arm for control, whereby... Obtained from step three.

2. The method for solving the inverse kinematics of a robotic arm based on autonomous planning of redundant parameters according to claim 1, characterized in that: In step three, The solution yields both positive and negative results when the trajectory points in the robotic arm's end effector space are discrete. There are actually two sets of solutions, with the sign of the solution chosen manually. This applies when the trajectory points in the robotic arm's end effector are continuous. Based on the joint angle of the previous cycle Determine the sign (positive or negative).

3. The method for solving the inverse kinematics of a robotic arm based on autonomous planning of redundant parameters according to claim 1, characterized in that: In step three, The solution yields both positive and negative results when the trajectory points in the robotic arm's end effector space are discrete. There are actually two sets of solutions. The positive and negative signs are selected manually. When the trajectory points of the robotic arm end-effector are continuous, there is actually one set of solutions, which is selected according to the principle of minimum displacement. That is, the set of solutions in which the difference in the rotation angle between the two planning cycles is within the threshold range is selected.

4. The method for solving the inverse kinematics of a robotic arm based on autonomous planning of redundant parameters according to claim 1, characterized in that: In step four, The expression is as follows: Expressed as: （8）； In equation (8) above, the parameter Each is a matrix The value in the i-th row and j-th column, where i, j = 1…3. Determined by the following formula (9): （9）； Expressed as: （10）； In equation (10) above, the parameter Represent matrices respectively The value in the i-th row and j-th column, where i and j = 1...

3. Determined by the following formula (11): （11）。 5. The method for solving the inverse kinematics of a robotic arm based on autonomous planning of redundant parameters according to claim 4, characterized in that: In step four, The solution involves two cases: positive and negative. When the trajectory of the robotic arm's end effector is discrete, the sign is manually chosen. When the trajectory of the robotic arm's end effector is continuous, the sign is determined by the operator. The sign is determined based on the angle value of the previous planning cycle.

6. The method for solving the inverse kinematics of a robotic arm based on autonomous planning of redundant parameters according to claim 4, characterized in that: In step four, The solution involves two cases: positive and negative. When the trajectory of the robotic arm's end effector is discrete, the sign is manually chosen. When the trajectory of the robotic arm's end effector is continuous, the sign is determined by the operator. The sign is determined based on the angle value of the previous planning cycle.

7. The method for solving the inverse kinematics of a robotic arm based on autonomous planning of redundant parameters according to claim 1, characterized in that: In step five, according to A total of four different joint solutions were obtained. When the trajectory points of the robotic arm's end-effector are discrete, there are actually four solutions, which are selected manually. When the trajectory points of the robotic arm's end-effector are continuous, there is actually one solution. In this case, the solution is selected according to the principle of minimum displacement, that is, the solution with the difference in rotation angle within the two planning cycles within the threshold range is selected as the initial inverse solution joint combination.

Images