A continuous robot inverse kinematics combination algorithm

Abstract

The application discloses a continuous robot inverse kinematics combination algorithm, and an inverse kinematics solving method comprises the following steps: 1, according to the single joint arm segment parameter, establishing the robot link coordinate system and the tool coordinate system; 2, based on the constant curvature method, according to the geometric relationship, establishing the transformation between the joint rotation angle and the end posture angle; 3, according to the given end posture matrix T en d, solving the end posture angle analytical solution and the inverse kinematics analytical solution; 4, according to the obtained end posture angle analytical solution and inverse kinematics analytical solution and the established transformation between the joint rotation angle and the end posture angle, calculating the inverse kinematics approximate solution; 5, taking the obtained inverse kinematics approximate solution as the initial value, using the numerical iteration method based on the motion Jacobian matrix, calculating the inverse kinematics accurate solution. The inverse kinematics combination algorithm is clear and intuitive, has the advantages of high calculation precision and fast operation speed, and reduces the error generated by the traditional inverse kinematics solving method based on the constant curvature method.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot kinematics, in particular to a continuous robot inverse kinematics combination algorithm. BACKGROUND

[0002] Continuous robot kinematics plays an important role in motion control, trajectory planning, workspace, etc. Continuous robot kinematics mainly includes forward kinematics and inverse kinematics, and inverse kinematics is relatively complex and is the basis of motion control and trajectory planning. The research on continuous robot kinematics is also mainly focused on the solution of inverse kinematics. At present, the modeling methods of continuous robot kinematics mainly include constant curvature method and non-constant curvature method. At present, the inverse kinematics of continuous robot based on constant curvature method mainly adopts Jacobian pseudo-inverse method, spine line method and geometric method, etc. However, these methods still have some shortcomings. With the increase of joint units, the calculation error increases, the calculation load increases, the efficiency decreases, and it cannot meet the requirements of motion control, etc. SUMMARY

[0003] The present application aims to provide a continuous robot inverse kinematics combination algorithm to solve the problems in the background technology.

[0004] To achieve the above purpose, the present application provides the following technical scheme: a continuous robot inverse kinematics combination algorithm, the inverse kinematics solving method comprising the following steps:

[0005] 1. Establishing the continuous robot link coordinate systems {X 2i-2 -Y 2i-2 -Z 2i-2}, {X 2i-1 -Y 2i-1 -Z 2i-1} and {X 2i -Y 2i -Z 2i} and the tool coordinate system {X w -Y w -Z w} according to the single-joint arm segment parameters;

[0006] 2. Based on the constant curvature method, the transformation between the joint rotation angle and the end attitude angle is established according to the geometric relationship;

[0007] 3. According to the given continuous robot end position and posture matrix T end , the end attitude angle analytical solution and the continuous robot inverse kinematics analytical solution are solved;

[0008] 4. Calculate the approximate inverse kinematics solution of the serial robot using the analytical solution of the end-effector orientation angle obtained in step 3, the analytical inverse kinematics solution of the serial robot and the transformation between joint angle and end-effector orientation angle established in step 2.

[0009] 5. Calculate the exact inverse kinematics solution of the serial robot using the approximate inverse kinematics solution of the serial robot obtained in step 4 as initial value and a numerical iterative method based on the kinematic Jacobian matrix.

[0010] Preferably, the transformation of the link coordinate system {X 2i-2 -Y 2i-2 -Z 2i-2} to the link coordinate system {X 2i-1 -Y 2i-1 -Z 2i-1} in step 1 is:

[0011] 1) translation along the Z 2i-2 axis by L, such that the X 2i-2 axis coincides with the X 2i-1 axis;

[0012] 2) rotation around the X 2i-1 axis by θ 2i-1 , such that the Y 2i-2 axis coincides with the Y 2i-1 axis;

[0013] The transformation of the link coordinate system {X 2i-1 -Y 2i-1 -Z 2i-1} to the link coordinate system {X 2i -Y 2i -Z 2i} is:

[0014] 1) translation along the Z 2i-1 axis by l c , such that the Y 2i-1 axis coincides with the Y 2i axis;

[0015] 2) rotation around the Y 2i axis by θ 2i , such that the X 2i-1 axis coincides with the X 2i axis;

[0016] The transformation of the link coordinate system {X 2i -Y 2i -Z 2i} to the tool coordinate system {X w -Y w -Z w} is:

[0017] translation along the Z 2i axis by L, such that the X2i axis and Y 2i axes respectively coincide with X w axis and Y w axes coincide respectively;

[0018] where θ 2i-1 , θ 2i are respectively called joint angles of the 2i-1th joint and the 2ith joint, l c , L are respectively called offset cross axis length and joint block length, where according to the constant curvature method, each odd joint angle is θ x , and each even joint angle is θ y ;

[0019] Define the homogeneous coordinate transformation matrix as:

[0020]

[0021] where variables c1=cosθ x , s1=sinθ x , c2=cosθ y , s2=sinθ y ;

[0022] Based on the link homogeneous transformation matrix, define the forward kinematics of the serial robot as:

[0023]

[0024] where k is the number of joint units, represents the pose matrix of the end of the serial robot in the base coordinate system {X0-Y0-Z0}; p=[p x p y p z ] T represents the position vector of the end link coordinate system relative to the base coordinate system X0-Y0-Z0, n=[n x n y n z ] T , o=[o x o y o z ] T , a=[a x a y a z ] T represents the attitude vector of the end link coordinate system relative to the base coordinate system {X0-Y0-Z0};

[0025] Define the end twist angle θ and the bending angle γ as the end attitude angle, where the twist angle θ is the vector a=[a x a y az ] T The angle between the projection onto the X0Y0-plane and the positive direction of the X0 axis is positive when counterclockwise; the curvature angle γ is the vector a = [a x a y a z ] T The angle between the positive direction of the Z0 axis and the Z0 axis.

[0026] Preferably, the iterative formula for the numerical iterative method based on the kinematic Jacobian matrix in step 5 is:

[0027] δq=J -1 error, q i =q i-1 +δq

[0028] in, The actual pose of the end effector With target pose The error vector between them, J is the kinematic Jacobian matrix, q = [θ x θ y ] represents the joint rotation angle, δq = [dθ] x dθ y [This represents the joint rotation angle increment;]

[0029] The iteration ends when the absolute value of the error vector is less than the given threshold ||error||≤ε or the maximum number of iterations is reached, and the final inverse kinematic solution is output. ε represents the threshold for the absolute value of the error vector.

[0030] Preferably, the transformation between the joint rotation angle and the end effector attitude angle in step 2 is as follows:

[0031] Ignoring the lengths of the joint links and the cross axis, the end-effector attitude angles of each joint segment are obtained using the constant curvature method:

[0032] θ s =θ,

[0033] Where, θ s γ s This is called the joint segment attitude angle. Based on the calculated joint segment attitude angle, the end-effector attitude vector 'a' for each joint segment can be obtained. s =[a sx a sy a sz ] T :

[0034] Among them, a sx =sin(γ) s cos(θ) s ), a sy =sin(γ) s sin(θ)s ), a sz = cos(gamma s ).

[0035] The expression of the continuous robot joint rotation angle is as follows:

[0036] theta x = atan2(-a sy , a sz )

[0037]

[0038] The specific expression of the continuous robot end pose angle analytical solution and inverse kinematics analytical solution solved in step 3 is as follows:

[0039] theta = atan2(a y , a x )

[0040]

[0041] wherein,

[0042] According to the continuous robot end pose matrix T end and the expression of the continuous robot end pose angle analytical solution and inverse kinematics analytical solution, a group of solutions theta x and theta y of the continuous robot inverse kinematics are obtained.

[0043] Compared with the prior art, the present application has the beneficial effects that:

[0044] 1. The inverse kinematics combination algorithm established in the present application is clear and intuitive, the end pose angle is first obtained through the end pose matrix, then the transformation relationship between the end pose angle and the joint rotation angle is established based on the constant curvature method, the error of the obtained joint rotation angle is large, which is used as the initial value of iteration, and finally the accurate solution of the inverse kinematics is obtained, and the modeling process is clear and understandable.

[0045] 2. The inverse kinematics combination algorithm of the present application is solved by using the constant curvature method and the numerical iteration method, has the advantages of high calculation precision and fast operation speed, and reduces the error generated by the traditional inverse kinematics solving method based on the constant curvature method. BRIEF DESCRIPTION OF DRAWINGS

[0046] Fig. 1 It is a continuous robot inverse kinematics combination algorithm flowchart of the present application;

[0047] Fig. 2The structure of the joint unit and the link coordinate system in the application;

[0048] Fig. 3 The structure of the joint and the link coordinate system in the application. DETAILED DESCRIPTION

[0049] The technical solutions in the embodiments of the application will be apparently and completely described in combination with the drawings in the embodiments of the application. Obviously, the described embodiments are only part of the embodiments of the application, rather than all the embodiments. Based on the embodiments in the application, all other embodiments obtained by a person of ordinary skill in the art without creative labor fall within the protection scope of the application.

[0050] Please refer to Figs. 1-3 The application provides a technical solution: a continuous robot inverse kinematics combination algorithm. The continuous robot selected in the application adopts the technical solution disclosed in the application patent with the application number 202111215181.6. The joint unit is composed of a plurality of joints. Each joint includes a biased cross joint shaft and two adjacent joint links on the sides. The biased cross joint shaft includes a biased central shaft and four joint shafts. The inverse kinematics solving method includes the following steps:

[0051] 1. Establishing the link coordinate systems {X 2i-2 -Y 2i-2 -Z 2i-2}, {X 2i-1 -Y 2i-1 -Z 2i-1}, {X 2i -Y 2i -Z 2i} and the tool coordinate system {X w -Y w -Z w} according to the single-joint arm segment parameters of the robot;

[0052] 2. Based on the constant curvature method, establishing the transformation between the joint rotation angle and the end posture angle according to the geometric relationship;

[0053] 3. According to the given continuous robot end posture matrix T end , solving the end posture angle analytical solution and the continuous robot inverse kinematics analytical solution;

[0054] 4. According to the end posture angle analytical solution and the continuous robot inverse kinematics analytical solution obtained in step 3, and the transformation between the joint rotation angle and the end posture angle established in step 2, calculating the continuous robot inverse kinematics approximate solution;

[0055] 5. Using the approximate solution of the inverse kinematics of the serial robot as the initial value, the accurate solution of the inverse kinematics of the serial robot is calculated by using the numerical iterative method based on the kinematic Jacobian matrix.

[0056] The transformation of the link coordinate system {X 2i-2 -Y 2i-2 -Z 2i-2} to the link coordinate system {X 2i-1 -Y 2i-1 -Z 2i-1} is:

[0057] 1) Translate along the Z 2i-2 axis by L, so that the X 2i-2 axis coincides with the X 2i-1 axis;

[0058] 2) Rotate about the X 2i-1 axis by θ 2i-1 , so that the Y 2i-2 axis coincides with the Y 2i-1 axis;

[0059] The transformation of the link coordinate system {X 2i-1 -Y 2i-1 -Z 2i-1} to the link coordinate system {X 2i -Y 2i -Z 2i} is:

[0060] 1) Translate along the Z 2i-1 axis by l c , so that the Y 2i-1 axis coincides with the Y 2i axis;

[0061] 2) Rotate about the Y 2i axis by θ 2i , so that the X 2i-1 axis coincides with the X 2i axis;

[0062] The transformation of the link coordinate system {X 2i -Y 2i -Z 2i} to the tool coordinate system {X w -Y w -Z w} is:

[0063] Translate along the Z 2i axis by L, so that the X 2i axis and the Y 2i axis respectively coincide with the X w axis and the Y w axis;

[0064] wherein θ 2i-1 , θ2i The joint angles are denoted as the 2i-1th joint and the 2ith joint, respectively, l c , L are denoted as the offset cross-axis length and the joint block length, respectively, where according to the constant curvature method, each odd joint angle is θ x , and each even joint angle is θ y .

[0065] The homogeneous coordinate transformation matrix is defined as:

[0066]

[0067] where variables c1 = cos θ x , s1 = sin θ x , c2 = cos θ y , and s2 = sin θ y .

[0068] Based on the link homogeneous transformation matrix, the forward kinematics of the serial robot is defined as:

[0069]

[0070] where k is the number of joint units, represents the pose matrix of the end of the serial robot in the base coordinate system {X0-Y0-Z0}; p = [p x p y p z ] T represents the position vector of the end link coordinate system relative to the base coordinate system X0-Y0-Z0, n = [n x n y n z ] T , o = [o x o y o z ] T , a = [a x a y a z ] T represents the attitude vector of the end link coordinate system relative to the base coordinate system {X0-Y0-Z0};

[0071] The end twist angle θ and the bending angle γ are defined as the end attitude angles, where the twist angle θ is the included angle between the projection of the vector a = [a x a y a z ] T in the X0Y0-plane and the positive direction of the X0-axis, and is positive counterclockwise; the bending angle γ is the included angle between the projection of the vector a = [a x a y a z ]T The angle between the positive direction of the Z0 axis and the Z0 axis.

[0072] The iterative formula for the numerical iterative method based on the kinematic Jacobian matrix in step 5 is as follows:

[0073] δq=J -1 error, q i =q i-1 +δq

[0074] in,

[0075] The actual pose of the end effector With target pose The error vector between them, J is the kinematic Jacobian matrix, q = [θ x θ y ] represents the joint rotation angle, δq = [dθ] x dθ y [This represents the joint rotation angle increment;]

[0076] The iteration ends when the absolute value of the error vector is less than the given threshold ||error||≤ε or the maximum number of iterations is reached, and the final inverse kinematic solution is output. ε represents the threshold for the absolute value of the error vector.

[0077] The transformation between joint rotation angle and end-effector attitude angle in step 2:

[0078] Ignoring the lengths of the joint links and the cross axis, the end-effector attitude angles of each joint segment are obtained using the constant curvature method:

[0079] θ s =θ,

[0080] Where, θ s γ s This is called the joint segment attitude angle. Based on the calculated joint segment attitude angle, the end-effector attitude vector 'a' for each joint segment can be obtained. s =[a sx a sy a sz ] T :

[0081] Among them, a sx =sin(γ) s cos(θ) s ), a sy =sin(γ) s sin(θ) s ), a sz =cos(γ) s ).

[0082] The expression for the joint rotation angle of a continuous robot is as follows:

[0083] θ x =atan2(-a sy ,a sz )

[0084]

[0085] The specific expressions for the analytical solutions of the continuous robot end-effector attitude angles and inverse kinematics obtained in step 3 are as follows:

[0086] θ=atan2(a y ,a x )

[0087]

[0088] in,

[0089] Based on the pose matrix T of the continuous robot end effector end Furthermore, the expressions for solving the analytical solutions of the end-effector attitude angles and inverse kinematics of the continuous robot are derived, resulting in one set of solutions θ for the inverse kinematics of the continuous robot. x and θ y .

[0090] The following are specific examples:

[0091] First, based on the inverse kinematics solution method, a link coordinate system for a continuous robot is established, such as... Fig. 2 As shown in Table 1, given the corresponding single-joint arm parameters of the robot, and the given target joint rotation angle as q = [θ], x θ y ] = [-7°6°], and the end-effector pose matrix is ​​calculated according to the robot forward kinematics formula described in step 1) above:

[0092]

[0093] Table 1 describes the parameters of a single articulated arm segment of a continuous robot.

[0094]

[0095] Secondly, the analytical solution for the end-effector attitude angle is obtained, and the geometric transformation between the end-effector attitude angle and the joint rotation angle is established. The approximate inverse kinematics solution for the continuous robot is then calculated. The obtained analytical solution and approximate inverse kinematics solution for the end-effector attitude angle are as follows:

[0096] θ=47.0430°, γ=92.0770°

[0097] q=[θ x θy ] = [-6.7658° 6.2601°]

[0098] Finally, the inverse kinematics of the serial robot is solved, given a threshold value ε = 0.0001, the computation time is 0.02 seconds, and the obtained solution is q = [θ x θ y ] = [-7.0003° 6.0001°], the relative error is less than 0.0001 compared to the given target joint angles.

[0099] It is to be understood that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting; it is also possible in the present application that units and parameters are chosen and used differently from how they are explained herein, unless otherwise explained or limited in the appended claims. Moreover, it is understood that the terms "comprise", "comprising", "include", and / or "including", as well as variations thereof, do not specify the presence of the stated features, integers, steps, operations, elements, and / or components thereof, and that the

[0100] While the embodiments of the application have been illustrated and described, it will be understood by those skilled in the art that various changes, modifications, substitutions, and alterations can be made therein without departing from the spirit and scope of the application, which is defined by the appended claims and their equivalents.

Claims

1. A continuous robot inverse kinematics combination algorithm, characterized in that, The inverse kinematics solving method comprises the following steps: Step 1, establish the link coordinate system {X 2i-2 -Y 2i-2 -Z 2i-2} of the robot according to the single joint arm segment parameters 2i-1 -Y 2i-1 -Z 2i-1} and {X 2i -Y 2i -Z 2i} and the tool coordinate system {X w -Y w -Z w}; Step 2, based on the constant curvature method, a transformation between joint rotation angle and end pose angle is established according to geometric relationship; Step 3, solving the end pose angle analytical solution and the serial robot inverse kinematics analytical solution according to the given serial robot end pose matrix T end , solving the end pose angle analytical solution and the serial robot inverse kinematics analytical solution according to the given serial robot end pose matrix T Step 4, according to the end pose angle analytical solution obtained in step 3 and the continuous robot inverse kinematics analytical solution and the transformation between joint rotation angle and end pose angle established in step 2, the continuous robot inverse kinematics approximate solution is calculated; Step 5, taking the continuous robot inverse kinematics approximate solution obtained in step 4 as an initial value, a numerical iteration method based on kinematics Jacobian matrix is used to calculate the continuous robot inverse kinematics accurate solution.

2. The algorithm for inverse kinematics of a serial robot according to claim 1, wherein: The transformation of the link coordinate system {X 2i-2 -Y 2i-2 -Z 2i-2} to the link coordinate system {X 2i-1 -Y 2i-1 -Z 2i-1} is: 1) Along Z 2i-2 Translate the axis by L, so that X 2i-2 Axis and X 2i-1 Axis coincidence; 2) about X 2i-1 axis by θ 2i-1 , Y 2i-2 axis coincides with Y 2i-1 axis; The transformation from the body coordinate system {X 2i-1 -Y 2i-1 -Z 2i-1} to the connecting rod coordinate system {X 2i -Y 2i -Z 2i} is: 1) Translate along Z axis l 2i-1 c 2i-1 2i axis coincides with Y axis;​​​ 2) around Y 2i axis by θ 2i so that X 2i-1 axis coincides with X 2i axis The transformation of the connecting rod coordinate system {X 2i -Y 2i -Z 2i} to the tool coordinate system {X w -Y w -Z w} is: along the Z 2i axis, translating L, so that the X 2i axis and the Y 2i axis respectively coincide with the X w axis and the Y w axis. where θ 2i-1 , θ 2i are the joint angles of the 2i-1th and 2ihjoints, respectively, and l c , L are the lengths of the offset cross shaft and the joint block, respectively, where according to the method of constant curvature, each odd joint angle is θ x and each even joint angle is θ y ; A homogeneous coordinate transformation matrix is defined as follows: where the variables c1 = cos θ x s1 = sin θ x c2 = cos θ y s2 = sin θ y ; Based on the link homogeneous transformation matrix, the continuous robot forward kinematics is defined as follows: Wherein, k is the number of joint units, represents the pose matrix of the serial robot end in the base coordinate system {X0-Y0-Z0}; p = [p x p y p z ] T represents the position vector of the end link coordinate system relative to the base coordinate system X0-Y0-Z0, n = [n x n y n z ] T , o = [o x o y o z ] T , a = [a x a y a z ] T represents the attitude vector of the end link coordinate system relative to the base coordinate system {X0-Y0-Z0}; The end twist angle θ and the bend angle γ are defined as end pose angles, where the twist angle θ is the angle between the vector a = [a x a y a z ] T The angle between the projection of a onto the X0Y0-plane and the positive direction of the X0-axis, positive counterclockwise; the bend angle γ is the angle between the vector a = [a x a y a z ] T The angle between a and the positive direction of the Z0-axis.

3. The closed-form robot inverse kinematics algorithm of claim 2, wherein: The iteration formula of the numerical iteration method based on kinematics Jacobian matrix in step 5 is as follows: δq = J -1 error, q i = q i-1 + δq wherein, is the error vector between the end-effector actual pose and the target pose, J is the kinematic Jacobian matrix, q = [0 x 0 y ] is the joint angles, and x d0 y d0 s is the joint angles increment; When the absolute value of the error vector is less than a given threshold value ||error||≤ε or the maximum iteration number is reached, the iteration is ended, and the final kinematics inverse solution is output, and ε represents the threshold value of the absolute value of the error vector.

4. The closed-form robot inverse kinematics algorithm of claim 3, wherein: The transformation between joint rotation angle and end pose angle in step 2 is as follows: Ignoring the length of the joint link and the cross shaft, the end pose angle of each joint segment is obtained according to the constant curvature method as follows: θ s = θ, where θ s , γ s are called joint segment posture angles, and according to the joint segment posture angles, the end posture vector a s of each joint segment can be obtained as a sx = [a sy a sz ] T : among them, a sx =sin(γ s )cos(θ s ),a sy =sin(γ s )sin(θ s ),a sz =cos(γ s ) The expression of the continuous robot joint rotation angle is as follows: θ x = atan2(-a sy , a sz ) 5. A closed-form robotic inverse kinematics algorithm according to claim 4, wherein: The specific expression of the continuous robot end pose angle analytical solution and the inverse kinematics analytical solution solved in step 3 is as follows: The specific expression of the continuous robot end pose angle analytical solution and the inverse kinematics analytical solution solved in step 3 is as follows: θ = atan2(a y ,a x ) wherein According to the continuous robot end position matrix T end And the expression of the analytical solution of the continuous robot end attitude angle and the inverse kinematics analytical solution, a set of solutions θ x And θ y Of the inverse kinematics of the continuous robot are obtained.

Images