Arm type angle parameterized inverse kinematics solving method of redundant robot arm

Abstract

The present application belongs to the technical field of inverse kinematics of space redundant manipulator, and particularly relates to a kind of arm type angle parameterized inverse kinematics solving method of redundant manipulator, solve the technical problem of inverse kinematics result solving of SSRMS type manipulator.The speciality of the solving method is that it includes the following steps: step 1: establish the kinematics model of redundant manipulator, and under the given condition that the pose matrix of the end of redundant manipulator relative to base, calculate the value of the vector determined from the origin of No.1 coordinate system to the origin of No.6 coordinate system;Step 2: calculate the projection vector of the normal vector of current arm type face in SW direction;Step 3: calculate the unit normal vector of reference plane;Step 4: calculate the normal vector of current arm type face;Step 5: calculate the value of joint angle 2;Step 6: according to the calculated value of joint angle 2, the value of the remaining joint angle is solved according to the single joint locking method, and then 8 groups of inverse kinematics results are finally obtained, and the inverse kinematics solving is completed.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of inverse kinematics of space redundant manipulator, and particularly relates to a solving method for inverse kinematics of arm type angle parameterization of a redundant manipulator. BACKGROUND

[0002] Compared with a 6-DOF manipulator, a redundant manipulator has obvious advantages in operation flexibility, obstacle avoidance, singularity avoidance, fault tolerance and joint torque optimization. The Canadian 2, the European manipulator (ERA), the special-purpose dexterous manipulator (SPDM) installed on the international space station, and the core cabin manipulator (CMM) and the experiment cabin manipulator (EMM) of the Chinese space station are all redundant manipulators. For the convenience, a manipulator with similar joint arrangement is usually called an SSRMS type manipulator. Due to the existence of the bias, the SSRMS type manipulator can obtain a larger workspace while making the position and attitude decoupling more difficult to solve compared with the spherical wrist (SRS) configuration scheme often used by a 7-DOF anthropomorphic manipulator. SUMMARY

[0003] The application aims to provide a solving method for inverse kinematics of arm type angle parameterization of a redundant manipulator to solve the technical problem of solving the inverse kinematics of an SSRMS type manipulator. The arm type angle of the redundant manipulator in the application is different from the concept of the arm type angle of a previous spherical wrist type manipulator, and is defined in detail in the following technical solution part.

[0004] The application adopts the technical solution of a solving method for inverse kinematics of arm type angle parameterization of a redundant manipulator, and the speciality thereof lies in comprising the following steps:

[0005] Step 1: establishing a kinematics model of the redundant manipulator, and calculating the value of a vector determined from the origin of the 1st coordinate system to the origin of the 6th coordinate system under the condition that the pose matrix of the redundant manipulator end relative to the base is given; the vector determined from the origin of the 1st coordinate system to the origin of the 6th coordinate system is specifically: defining the origin of the 1st coordinate system as point S and the origin of the 6th coordinate system as point W, and defining the vector Vector P 16 as the vector determined from the origin of the 1st coordinate system to the origin of the 6th coordinate system;

[0006] Step 2: defining a current arm type surface, and calculating the projection vector of the normal vector of the current arm type surface in the SW direction;

[0007] Step 3: defining a reference plane, and calculating the unit normal vector of the reference plane;

[0008] Step 4: According to the current arm profile defined in step 2 and the reference plane defined in step 3, define the arm profile angle between the current arm profile and the reference plane; and calculate the normal vector of the current arm profile as a parameter;

[0009] Step 5: According to the normal vector value of the current arm profile calculated in step 4, calculate the value of joint angle 2;

[0010] Step 6: According to the value of joint angle 2 calculated in step 5, and according to the single joint locking method, the values of the remaining joint angles are calculated, and finally 8 sets of inverse kinematics results are obtained, and the inverse kinematics solving is completed.

[0011] Further, the step 1 specifically comprises the following steps:

[0012] Step 1.1: Establish the kinematics model of the redundant manipulator;

[0013] The kinematics model of the redundant manipulator is shown in the following formula (1) and formula (2):

[0014] 0 T7= 0 T1 1 T2… 6 T7=f(Θ) (1);

[0015]

[0016] In formula (1) and formula (2): 0 T7 represents the pose matrix of the end of the redundant manipulator relative to the base; 0 T1 represents the pose matrix of the first coordinate system relative to the zero coordinate system; 1 T2 represents the pose matrix of the second coordinate system relative to the first coordinate system; 6 T7 represents the pose matrix of the seventh coordinate system relative to the sixth coordinate system; Θ=[θ1 θ2 θ3 θ4 θ5 θ6 θ7] represents the 7 joint angles of the redundant manipulator; n=[n x ,n y ,n z ] T 、o=[o x ,o y ,o z ] T 、a=[a x ,a y ,a z ] T Respectively represent the unit vectors of the x, y, z axes of the end coordinate system in the base coordinate system {x0, y0, z0}; p=[p x ,p y ,pz ] T This is the position vector of the origin of the terminal coordinate system relative to the base coordinate system;

[0017] Step 1.2: Calculate the vector P that extends from the origin of coordinate system 1 to the origin of coordinate system 6. 16 The value;

[0018] The pose matrix of the redundant robotic arm end effector relative to the base 0 Under the conditions given by T7, according to equation (2) in step 1.1, we obtain the following equation (3):

[0019]

[0020] In equation (3): d7 represents the link offset between coordinate system 6 and coordinate system 7; d1 represents the link offset between coordinate system 0 and coordinate system 1.

[0021] Furthermore, step 2 specifically includes:

[0022] Construct SS′, which satisfies that SS′ is parallel to z4 and SS′ = d3 + 0.5d4; construct WW′, which satisfies that WW′ is parallel to z4 and WW′ = d5 + 0.5d4; SW and S′W′ intersect at point O; z4 represents the unit vector of the z-axis of coordinate system 4 in the base coordinate system; d3 represents the link offset between coordinate systems 2 and 3; d4 represents the link offset between coordinate systems 3 and 4; d5 represents the link offset between coordinate systems 4 and 5; define the origin of coordinate system 4 as point E; the center point of one end face of the fourth link coincides with point E, and the center point of the other end face is point O4′; point E′ is the midpoint of EO4′; define plane S′E′W′ as the current arm profile;

[0023] Draw a vector through the point O. satisfy Parallel to and equal to z4, the vector Let it be e o Then vector e o Let be the normal vector of the current arm shape, and the following equation (4) holds:

[0024] P 16 ·e o =SS′+W′W=d3+d4+d5 (4);

[0025] Draw RD perpendicular to SW through point R at point D, and vector... Let it be e d Then e dThe normal vector of the current arm surface is projected in the SW direction to obtain a projection vector e d As shown in the following formula (5) and formula (6):

[0026]

[0027]

[0028] Further, the step 3 is specifically:

[0029] The vector e The plane formed by z1 is the reference plane, and z1 is a unit vector of the z-axis of the first coordinate system in the base coordinate system;

[0030] The unit normal vector of the reference plane is e The vector e is e r0 ; the vector z1 =

[001] T The unit normal vector of the reference plane can be obtained as shown in the following formula (7):

[0031]

[0032] Further, the step 4 is specifically:

[0033] The vector e is e r The unit normal vector e r0 of the reference plane in step 3 is rotated around the vector P 16 by ψ to coincide with the vector e r ; ψ is defined as the arm angle between the current arm surface and the reference plane;

[0034] According to the Rodrigues rotation formula, the following formula (8) is obtained:

[0035]

[0036] The normal vector of the current arm surface, i.e., the vector e o is shown in the following formula (9):

[0037] e o = e d + e r (9).

[0038] Further, the step 5 is specifically:

[0039] According to the relationship between the DH coordinate systems of the redundant robot arm and the kinematics equation, the following formula (10) is obtained: 0 R3 is shown in the following formula (10):

[0040]

[0041] In formula (10), i represents 1 or 2; 0 R3 represents the posture of the third connecting rod relative to the base;c i =cos(θ i ), s i =sin(θ i );

[0042] As known from the redundant manipulator, vector z3 and vector e o are equal, thus, formula (11) can be obtained from formula (10):

[0043] z3=e o =[c1s2 s1s2 -c2] T (11);

[0044] Define e o (j) represents the value of the jth element of vector e o , then the value of the joint angle 2 is shown in formula (12) as follows:

[0045] θ2=±arccos(e o (3)) (12)。

[0046] Further, the redundant manipulator refers to an SSRMS type manipulator.

[0047] The beneficial effects of the present application are:

[0048] (1) The present application gives a new definition of the arm type angle parameter according to the self-motion characteristics of the SSRMS type manipulator. Unlike the spherical wrist type manipulator, the SSRMS type manipulator has a bias at its shoulder, elbow and wrist, and the inverse kinematics solution thereof is more difficult. The inverse kinematics solution method of the arm type angle parameterization proposed in the present application can directly obtain the analytical solution of the inverse kinematics, effectively solving the technical problem of solving the inverse kinematics result of the biased SSRMS type redundant manipulator, and has high calculation efficiency and absolute precision compared with other numerical methods.

[0049] (2) The arm type angle parameter given in the present application can intuitively describe the overall configuration of the manipulator in the working space, has obvious geometric meaning, is suitable for arm type control, and can also realize obstacle avoidance and other special problem processing by controlling the arm type angle parameter. Compared with other existing methods, the method of the present application well solves the real-time motion planning and control problem of the SSRMS type manipulator. BRIEF DESCRIPTION OF DRAWINGS

[0050] Figure 1 is the DH coordinate system diagram of the SSRMS type manipulator in the embodiment of the present application;

[0051] Figure 2 is a schematic diagram of the current arm profile and arm angle in the embodiment of the application. DETAILED DESCRIPTION

[0052] The application will be described in detail below with reference to the drawings and specific embodiments.

[0053] The arm angle parameterization inverse kinematics solving method of the redundant robot arm includes the following steps:

[0054] Step 1: establish the kinematics model of the redundant robot arm, and under the given condition of the pose matrix of the end of the redundant robot arm relative to the base, calculate the value of the vector determined from the origin of the No. 1 coordinate system to the origin of the No. 6 coordinate system; the above-mentioned vector determined from the origin of the No. 1 coordinate system to the origin of the No. 6 coordinate system is specifically as follows: Figure 2 , the origin of the No. 1 coordinate system is defined as point S, the origin of the No. 6 coordinate system is defined as point W, and the vector is defined as The vector P 16 is the vector determined from the origin of the No. 1 coordinate system to the origin of the No. 6 coordinate system;

[0055] Step 2: define the current arm profile, and calculate the projection vector of the normal vector of the current arm profile in the SW direction;

[0056] Step 3: define the reference plane, and calculate the unit normal vector of the reference plane;

[0057] Step 4: according to the current arm profile defined in step 2 and the reference plane defined in step 3, define the arm angle between the current arm profile and the reference plane; and take the above-mentioned arm angle as a parameter to calculate the normal vector of the current arm profile;

[0058] Step 5: according to the value of the normal vector of the current arm profile calculated in step 4, calculate the value of joint angle 2;

[0059] Step 6: according to the value of joint angle 2 calculated in step 5, the values of the remaining joint angles are solved according to the single-joint locking method, and then 8 sets of inverse kinematics results are finally obtained, and the inverse kinematics solving is completed.

[0060] In the embodiment, the above-mentioned step 1 specifically includes the following steps:

[0061] Step 1.1: establish the kinematics model of the redundant robot arm;

[0062] In the embodiment, the redundant robot arm is an SSRMS type robot arm, the DH coordinate system diagram of the SSRMS type robot arm is as shown in Figure 1 , and the DH parameters are shown in Table 1 below:

[0063] Table 1: DH parameters of SSRMS type robot arm

[0064]

[0065] In table 1: a i denotes the angle of rotation about the x i-1 axis from z i-1 to z i ; a i denotes the distance along the x i-1 axis from z i-1 to z i ; a i denotes the angle of rotation about the z i-1 axis from x i-1 to x i ; d i denotes the distance along the z i-1 axis from x i-1 to x i ;

[0066] The kinematic model of the redundant robot arm is shown in the following equations (1) and (2):

[0067] 0 T7= T1T2T3T4T5T6T7 0 T1 1 T2… 6 T7= f (Θ) (1) ;

[0068]

[0069] In equations (1) and (2): 0 T7denotes the pose matrix of the end of the redundant robot arm relative to the base; 0 T1denotes the pose matrix of the 1st coordinate system relative to the 0th coordinate system; 1 T2denotes the pose matrix of the 2nd coordinate system relative to the 1st coordinate system; 6 T7denotes the pose matrix of the 7th coordinate system relative to the 6th coordinate system; Θ = [θ1 θ2 θ3 θ4 θ5 θ6 θ7] denotes the 7 joint angles of the redundant robot arm; n = [n x ,n y ,n z ] T , o = [o x ,o y ,o z ] T , a = [a x ,a y ,a z ] T denote the unit vectors of the x, y, z axes of the end coordinate system represented in the base coordinate system {x0, y0, z0}, respectively; p = [px ,p y ,p z ] T is a position vector of the origin of the terminal coordinate system relative to the base coordinate system;

[0070] Step 1.2: Calculate the value of the vector P determined from the origin of the No. 1 coordinate system to the origin of the No. 6 coordinate system 16

[0071] Under the condition that the pose matrix of the redundant robot arm end relative to the base 0 T7 is given, according to formula (2) in step 1.1, the following formula (3) is obtained:

[0072]

[0073] In formula (3), d7 represents the link offset distance between the No. 6 coordinate system and the No. 7 coordinate system; and d1 represents the link offset distance between the No. 0 coordinate system and the No. 1 coordinate system.

[0074] In the embodiment, the above step 2 is specifically:

[0075] Referring to Figure 2 , draw SS' which satisfies that SS' is parallel to z4 and SS' = d3 + 0.5d4; draw WW' which satisfies that WW' is parallel to z4 and WW' = d5 + 0.5d4; SW intersects with S'W' at point O; the above z4 represents the unit vector of the z-axis of the No. 4 coordinate system in the base coordinate system; the above d3 represents the link offset distance between the No. 2 coordinate system and the No. 3 coordinate system; the above d4 represents the link offset distance between the No. 3 coordinate system and the No. 4 coordinate system; the above d5 represents the link offset distance between the No. 4 coordinate system and the No. 5 coordinate system; define the origin of the No. 4 coordinate system as point E; the center point of one end face of the fourth link coincides with point E, and the center point of the other end face is O4'; point E' is the midpoint of EO4'; define the plane S'E'W' as the above current arm profile;

[0076] Referring to Figure 2 , draw the vector which satisfies that is parallel to z4 and equal, and record the vector as e o , then the vector e o is the normal vector of the above current arm profile, and the following formula (4) is established:

[0077] P 16 ·e o = SS' + W'W = d3 + d4 + d5 (4);

[0078] Referring to Figure 2 ​Draw RD perpendicular to SW through point R at point D. Vector Let it be e d Then e d That is, the projection vector of the normal vector of the current arm shape onto the SW direction, then e can be obtained. d As shown in equations (5) and (6) below:

[0079]

[0080]

[0081] In this embodiment, step 3 above specifically includes:

[0082] See Figure 2 Define vector The plane formed by z1 and z2 is the aforementioned reference plane, where z1 is the unit vector of the z-axis of coordinate system 1 in the base coordinate system.

[0083] Draw the unit normal vector of the reference plane through point D mentioned in step 2. Let vector For e r0 Vector z1 = [0 0 1] T Then, the unit normal vector of the above reference plane can be obtained as shown in equation (7):

[0084]

[0085] In this embodiment, step 4 above specifically refers to:

[0086] See Figure 2 Let vector For e r In step 3, the unit normal vector e of the reference plane r0 According to the right-hand screw rule, around vector P 16 After rotation ψ and vector e r Coincident; Define ψ as the arm profile angle between the current arm profile surface and the reference plane;

[0087] According to Rodriguez's rotation formula, the following equation (8) can be obtained:

[0088]

[0089] Then the normal vector of the current arm shape, i.e., vector e o As shown in equation (9):

[0090] e o =e d +e r (9).

[0091] In the embodiment, the step 5 is specifically:

[0092] According to the relationship between the DH coordinate systems of the redundant manipulator and the kinematic equation 0 R3 is shown in the following formula (10):

[0093]

[0094] In formula (10): 0 R3 represents the posture of the third connecting rod relative to the base; c i =cos(θ i ), s i =sin(θ i ), wherein i is 1 or 2;

[0095] According to the redundant manipulator, the vector z3 and the vector e o are equal, and therefore, formula (11) can be obtained from the above formula (10):

[0096] z3=e o =[c1s2 s1s2 -c2] T (11);

[0097] Define e o (j) represents the value of the jth element of the vector e o , and the value of the above joint angle 2 is shown in the following formula (12):

[0098] θ2=±arccos(e o (3)) (12).

[0099] In the present application, according to the self-motion characteristics of the SSRMS type manipulator, a new arm type angle is defined, and in the case of given end pose of the redundant manipulator, the inverse kinematics result under the end pose can be obtained according to the arm type angle parameter.

[0100] It can be seen from the above solving process that the method proposed in the present application is an analytical method, which has the advantages of simplicity and practicality, obvious geometric meaning, and is suitable for arm type control. The range of the arm type angle parameter can be controlled to realize obstacle avoidance and other special problems, and the real-time motion planning and control problem of the SSRMS type manipulator is well solved, which has important significance.

Claims

1. A method for parametric inverse kinematics solution of the arm profile angle of a redundant robotic arm, characterized in that, Includes the following steps: Step 1: Establish the kinematic model of the redundant robotic arm, and given the pose matrix of the end effector of the redundant robotic arm relative to the base, calculate the value of the vector determined from the origin of coordinate system 1 to the origin of coordinate system 6; specifically, the vector determined from the origin of coordinate system 1 to the origin of coordinate system 6 is defined as follows: the origin of coordinate system 1 is defined as point S, the origin of coordinate system 6 is defined as point W, and the vector is defined... Vector P 16 That is, the vector defined from the origin of coordinate system 1 to the origin of coordinate system 6; Step 2: Define the current arm profile and calculate the projection vector of the normal vector of the current arm profile onto the SW direction; Step 3: Define a reference plane and calculate its unit normal vector; Step 4: Based on the current arm profile defined in Step 2 and the reference plane defined in Step 3, define the arm profile angle between the current arm profile and the reference plane; and use the arm profile angle as a parameter to calculate the normal vector of the current arm profile. Step 5: Calculate the value of joint angle 2 based on the normal vector value of the current arm profile obtained in Step 4; Step 6: Based on the value of joint angle 2 calculated in Step 5, calculate the values ​​of the remaining joint angles using the single joint locking method, and finally obtain 8 sets of inverse kinematics results, thus completing the inverse kinematics solution; Step 2 specifically involves: Construct SS′, which satisfies that SS′ is parallel to z4 and SS′ = d3 + 0.5d4; construct WW′, which satisfies that WW′ is parallel to z4 and WW′ = d5 + 0.5d4; SW and S′W′ intersect at point O; z4 represents the unit vector of the z-axis of coordinate system 4 in the base coordinate system; d3 represents the link offset between coordinate systems 2 and 3; d4 represents the link offset between coordinate systems 3 and 4; d5 represents the link offset between coordinate systems 4 and 5; define the origin of coordinate system 4 as point E; the center point of one end face of the fourth link coincides with point E, and the center point of the other end face is point O4′; point E′ is the midpoint of EO4′; define plane S′E′W′ as the current arm profile; Draw a vector through the point O. satisfy Parallel to and equal to z4, the vector Let it be e o Then vector e o Let be the normal vector of the current arm shape, and the following equation (4) holds: P 16 ·e o =SS′+W′W=d3+d4+d5 (4); Draw RD perpendicular to SW through point R at point D, and vector... Let it be e d Then e d That is, the projection vector of the normal vector of the current arm surface onto the SW direction, then e can be obtained. d As shown in equations (5) and (6) below: Step 3 specifically involves: Define vector The plane formed by z1 and z2 is the reference plane, where z1 is the unit vector of the z-axis of coordinate system 1 in the base coordinate system; Draw the unit normal vector of the reference plane through point D mentioned in step 2. Let vector For e r0 Vector z1 = [001] T Then, the unit normal vector of the reference plane can be obtained as shown in equation (7): Step 4 specifically involves: Let vector For e r In step 3, the unit normal vector e of the reference plane r0 According to the right-hand screw rule, around vector P 16 After rotation ψ and vector e r Coincident; Define ψ as the arm profile angle between the current arm profile surface and the reference plane; According to Rodriguez's rotation formula, the following equation (8) can be obtained: Then the normal vector of the current arm shape, i.e., vector e o As shown in equation (9): And o =and d +e r (9)。 2. The method for parametric inverse kinematics solution of the arm shape angle of a redundant robotic arm according to claim 1, characterized in that, Step 1 specifically includes the following steps: Step 1.1: Establish the kinematic model of the redundant robotic arm; The kinematic model of the redundant robotic arm is shown in equations (1) and (2) below: 0 T7= 0 T1 1 T2… 6 T7=f(Θ) (1); In equations (1) and (2): 0 T7 represents the pose matrix of the redundant robotic arm end effector relative to the base; 0 T1 represents the pose matrix of coordinate system 1 relative to coordinate system 0; 1 T2 represents the pose matrix of coordinate system 2 relative to coordinate system 1; 6 T7 represents the pose matrix of coordinate system 7 relative to coordinate system 6; Θ = [θ1 θ2 θ3 θ4 θ5 θ6 θ7] represents the 7 joint angles of the redundant robotic arm; n = [n x ,n y ,n z ] T o = [o x ,o y ,o z ] T a = [a x ,a y ,a z ] T Let x, y, and z represent the unit vectors of the x, y, and z axes of the terminal coordinate system in the base coordinate system {x0, y0, z0}, respectively; p = [p x ,p y ,p z ] T This is the position vector of the origin of the terminal coordinate system relative to the base coordinate system; Step 1.2: Calculate the vector P that extends from the origin of coordinate system 1 to the origin of coordinate system 6. 16 The value; The pose matrix of the redundant robotic arm end effector relative to the base 0 Under the conditions given by T7, according to equation (2) in step 1.1, we obtain the following equation (3): In equation (3): d7 represents the link offset between coordinate system 6 and coordinate system 7; d1 represents the link offset between coordinate system 0 and coordinate system 1.

3. The method for parametric inverse kinematics solution of the arm shape angle of a redundant robotic arm according to claim 1, characterized in that, Step 5 specifically involves: Based on the relationship between the redundant robotic arm's DH coordinate system and the kinematic equations, it can be known that... 0 R3 is shown in equation (10) below: In formula (10): 0 R3 indicates the attitude of the third link relative to the base; c i =cos(θ) i ), s i =sin(θ) i ), where i takes the value 1 or 2; As can be seen from the redundant robotic arm, vector z3 and vector e o Equal to each other, therefore, from the above equation (10), we can obtain the following equation (11): z3=e o =[c1s2 s1s2 -c2] T (11); Define e o (j) represents vector e o The value of the j-th element is given by the following formula (12): θ2=±arccos(e o (3))(12)。 4. The method for parametric inverse kinematics solving of the arm shape angle of a redundant robotic arm according to any one of claims 1 to 3, characterized in that: The redundant robotic arm refers to the SSRMS type robotic arm.

Images